MRPT  2.0.3
test.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/gui.h>
12 #include <mrpt/img/TColor.h>
19 #include <mrpt/system/CTicTac.h>
20 #include <iostream>
21 
22 using namespace mrpt;
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::math;
25 using namespace mrpt::gui;
26 using namespace mrpt::maps;
27 using namespace mrpt::obs;
28 using namespace mrpt::opengl;
29 using namespace mrpt::system;
30 using namespace mrpt::img;
31 using namespace std;
32 
33 // ------------------------------------------------------
34 // Test_SwissRanger
35 // ------------------------------------------------------
36 void Test_SwissRanger()
37 {
39 
40  // Set params:
41  cam.setOpenFromUSB(true);
42 
43  cam.setSave3D(true);
44  cam.setSaveRangeImage(true);
45  cam.setSaveIntensityImage(true);
46  cam.setSaveConfidenceImage(false);
47 
48  // cam.enablePreviewWindow(true);
49 
50  // Open:
51  cam.initialize();
52 
53  if (cam.isOpen())
54  cout << "[Test_SwissRanger] Camera open, serial #"
55  << cam.getCameraSerialNumber() << " resolution: " << cam.cols()
56  << "x" << cam.rows() << " max. range: " << cam.getMaxRange()
57  << endl;
58 
59  const double aspect_ratio = cam.rows() / double(cam.cols());
60 
61  {
62  std::string ver;
63  cam.getMesaLibVersion(ver);
64  cout << "[Test_SwissRanger] Version: " << ver << "\n";
65  }
66 
68  bool there_is_obs = true, hard_error;
69 
70  mrpt::gui::CDisplayWindow3D win3D("3D camera view", 800, 600);
71 
72  win3D.setCameraAzimuthDeg(140);
73  win3D.setCameraElevationDeg(20);
74  win3D.setCameraZoom(6.0);
75  win3D.setCameraPointingToPoint(2.5, 0, 0);
76 
77  // mrpt::gui::CDisplayWindow win2D("2D range image",200,200);
78  // mrpt::gui::CDisplayWindow winInt("Intensity range image",200,200);
79  // win2D.setPos(10,10);
80  // winInt.setPos(350,10);
81  // win3D.setPos(10,290);
82  // win3D.resize(400,200);
83 
84  // mrpt::opengl::CPointCloud::Ptr gl_points =
85  // mrpt::opengl::CPointCloud::Create();
88  gl_points->setPointSize(4.5);
89 
92  0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);
93  mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity =
95  0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);
96  mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity_rect =
98  0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);
99 
100  {
101  mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();
102 
103  // Create the Opengl object for the point cloud:
104  scene->insert(gl_points);
105  scene->insert(mrpt::opengl::CGridPlaneXY::Create());
107 
108  const int VW_WIDTH = 200;
109  const int VW_HEIGHT = 150;
110  const int VW_GAP = 10;
111 
112  // Create the Opengl objects for the planar images, as textured planes,
113  // each in a separate viewport:
114  win3D.addTextMessage(
115  30, -10 - 1 * (VW_GAP + VW_HEIGHT), "Range data", 1);
116  opengl::COpenGLViewport::Ptr viewRange =
117  scene->createViewport("view2d_range");
118  scene->insert(gl_img_range, "view2d_range");
119  viewRange->setViewportPosition(
120  5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
121  viewRange->setTransparent(true);
122  viewRange->getCamera().setOrthogonal(true);
123  viewRange->getCamera().setAzimuthDegrees(90);
124  viewRange->getCamera().setElevationDegrees(90);
125  viewRange->getCamera().setZoomDistance(1.0);
126 
127  win3D.addTextMessage(
128  30, -10 - 2 * (VW_GAP + VW_HEIGHT), "Intensity data", 2);
130  scene->createViewport("view2d_int");
131  scene->insert(gl_img_intensity, "view2d_int");
132  viewInt->setViewportPosition(
133  5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
134  viewInt->setTransparent(true);
135  viewInt->getCamera().setOrthogonal(true);
136  viewInt->getCamera().setAzimuthDegrees(90);
137  viewInt->getCamera().setElevationDegrees(90);
138  viewInt->getCamera().setZoomDistance(1.0);
139 
140  win3D.addTextMessage(
141  30, -10 - 3 * (VW_GAP + VW_HEIGHT), "Intensity data (undistorted)",
142  3);
143  opengl::COpenGLViewport::Ptr viewIntRect =
144  scene->createViewport("view2d_intrect");
145  scene->insert(gl_img_intensity_rect, "view2d_intrect");
146  viewIntRect->setViewportPosition(
147  5, -10 - 3 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
148  viewIntRect->setTransparent(true);
149  viewIntRect->getCamera().setOrthogonal(true);
150  viewIntRect->getCamera().setAzimuthDegrees(90);
151  viewIntRect->getCamera().setElevationDegrees(90);
152  viewIntRect->getCamera().setZoomDistance(1.0);
153 
154  win3D.unlockAccess3DScene();
155  win3D.repaint();
156  }
157 
158  CTicTac tictac;
159  size_t nImgs = 0;
160 
161  bool endLoop = false;
162 
163  while (there_is_obs && !endLoop && win3D.isOpen())
164  {
165  // Grab new observation from the camera:
166  cam.getNextObservation(obs, there_is_obs, hard_error);
167 
168  // Show ranges as 2D:
169  if (there_is_obs && obs.hasRangeImage)
170  {
172 
173  win3D.get3DSceneAndLock();
174  gl_img_range->assignImage(std::move(img));
175  win3D.unlockAccess3DScene();
176  }
177 
178  // Show intensity image:
179  if (there_is_obs && obs.hasIntensityImage)
180  {
181  win3D.get3DSceneAndLock();
182  gl_img_intensity->assignImage(obs.intensityImage);
183 
184  CImage undistortImg;
185  obs.intensityImage.undistort(undistortImg, obs.cameraParams);
186  gl_img_intensity_rect->assignImage(undistortImg);
187  win3D.unlockAccess3DScene();
188  }
189 
190  // Show 3D points:
191  if (there_is_obs && obs.hasPoints3D)
192  {
193  // mrpt::maps::CSimplePointsMap pntsMap;
194  CColouredPointsMap pntsMap;
195  pntsMap.colorScheme.scheme =
196  CColouredPointsMap::cmFromIntensityImage;
197  pntsMap.loadFromRangeScan(obs);
198 
199  win3D.get3DSceneAndLock();
200  gl_points->loadFromPointsMap(&pntsMap);
201  win3D.unlockAccess3DScene();
202  win3D.repaint();
203  }
204 
205  nImgs++;
206  if (nImgs > 10)
207  {
208  win3D.get3DSceneAndLock();
209  win3D.addTextMessage(
210  0.01, 0.01, format("%.02f Hz", nImgs / tictac.Tac()), 100);
211  win3D.unlockAccess3DScene();
212  nImgs = 0;
213  tictac.Tic();
214  }
215 
216  // Process possible keyboard commands:
217  // --------------------------------------
218  if (win3D.keyHit())
219  {
220  const int key = tolower(win3D.getPushedKey());
221  // cout << "key: " << key << endl;
222 
223  switch (key)
224  {
225  case 'h':
228  break;
229  case 'g':
230  cam.enableConvGray(!cam.isEnabledConvGray());
231  break;
232  case 'd':
234  break;
235  case 'f':
237  break;
238  case 27:
239  endLoop = true;
240  break;
241  }
242  }
243 
244  win3D.get3DSceneAndLock();
245  win3D.addTextMessage(
246  0.08, 0.02,
247  format(
248  "Keyboard switches: H (hist.equal: %s) | G (convGray: %s) | D "
249  "(denoise: %s) | F (medianFilter: %s)",
250  cam.isEnabledImageHistEqualization() ? "ON" : "OFF",
251  cam.isEnabledConvGray() ? "ON" : "OFF",
252  cam.isEnabledDenoiseANF() ? "ON" : "OFF",
253  cam.isEnabledMedianFilter() ? "ON" : "OFF"),
254  110);
255  win3D.unlockAccess3DScene();
256 
257  std::this_thread::sleep_for(1ms);
258  }
259 }
260 
261 int main(int argc, char** argv)
262 {
263  try
264  {
266  return 0;
267  }
268  catch (const std::exception& e)
269  {
270  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
271  return -1;
272  }
273  catch (...)
274  {
275  printf("Another exception!!");
276  return -1;
277  }
278 }
mrpt::hwdrivers::CSwissRanger3DCamera::isEnabledImageHistEqualization
bool isEnabledImageHistEqualization() const
Definition: CSwissRanger3DCamera.h:198
mrpt::obs::CObservation3DRangeScan::hasPoints3D
bool hasPoints3D
true means the field points3D contains valid data.
Definition: CObservation3DRangeScan.h:326
mrpt::maps::CColouredPointsMap
A map of 2D/3D points with individual colours (RGB).
Definition: CColouredPointsMap.h:29
mrpt::maps::CColouredPointsMap::colorScheme
TColourOptions colorScheme
The options employed when inserting laser scans in the map.
Definition: CColouredPointsMap.h:219
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::hwdrivers::CSwissRanger3DCamera::setSave3D
void setSave3D(bool save)
Definition: CSwissRanger3DCamera.h:187
mrpt::hwdrivers::CSwissRanger3DCamera::isEnabledMedianFilter
bool isEnabledMedianFilter() const
Definition: CSwissRanger3DCamera.h:208
mrpt::maps::CColouredPointsMap::TColourOptions::scheme
TColouringMethod scheme
Definition: CColouredPointsMap.h:213
mrpt::hwdrivers::CSwissRanger3DCamera::enableConvGray
void enableConvGray(bool enable)
Definition: CSwissRanger3DCamera.h:219
mrpt::opengl::CPointCloudColoured::Ptr
std::shared_ptr< mrpt::opengl ::CPointCloudColoured > Ptr
Definition: CPointCloudColoured.h:48
CPointCloudColoured.h
mrpt::opengl::CTexturedPlane::Ptr
std::shared_ptr< mrpt::opengl ::CTexturedPlane > Ptr
Definition: CTexturedPlane.h:22
mrpt::hwdrivers::CSwissRanger3DCamera::getMesaLibVersion
bool getMesaLibVersion(std::string &out_version) const
Get the version of the MESA library.
Definition: CSwissRanger3DCamera.cpp:218
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::CSwissRanger3DCamera::getMaxRange
double getMaxRange() const
Returns the maximum camera range, as deduced from its operating frequency.
Definition: CSwissRanger3DCamera.h:169
mrpt::hwdrivers::CSwissRanger3DCamera::setOpenFromUSB
void setOpenFromUSB(bool USB)
true: open from USB, false: open from ethernet.
Definition: CSwissRanger3DCamera.h:183
CObservation3DRangeScan.h
mrpt::hwdrivers::CSwissRanger3DCamera::rows
size_t rows() const
Get the row count in the camera images, loaded automatically upon camera open().
Definition: CSwissRanger3DCamera.h:160
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
stock_objects.h
mrpt::opengl::CTexturedPlane::Create
static Ptr Create(Args &&... args)
Definition: CTexturedPlane.h:22
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation3DRangeScan::cameraParams
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
Definition: CObservation3DRangeScan.h:521
mrpt::hwdrivers::CSwissRanger3DCamera::setSaveIntensityImage
void setSaveIntensityImage(bool save)
Definition: CSwissRanger3DCamera.h:189
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::img
Definition: CCanvas.h:16
mrpt::obs::CObservation3DRangeScan::intensityImage
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
Definition: CObservation3DRangeScan.h:484
mrpt::opengl::COpenGLViewport::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLViewport > Ptr
Definition: COpenGLViewport.h:65
mrpt::obs::CObservation3DRangeScan
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
Definition: CObservation3DRangeScan.h:168
mrpt::hwdrivers::CSwissRanger3DCamera::initialize
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig()
Definition: CSwissRanger3DCamera.cpp:119
mrpt::opengl::CPointCloudColoured::Create
static Ptr Create(Args &&... args)
Definition: CPointCloudColoured.h:48
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
mrpt::hwdrivers::CSwissRanger3DCamera::enableDenoiseANF
void enableDenoiseANF(bool enable)
Definition: CSwissRanger3DCamera.h:225
mrpt::hwdrivers::CSwissRanger3DCamera::isOpen
bool isOpen() const
whether the camera is open and comms work ok.
Definition: CSwissRanger3DCamera.cpp:233
mrpt::obs::CObservation3DRangeScan::hasIntensityImage
bool hasIntensityImage
true means the field intensityImage contains valid data
Definition: CObservation3DRangeScan.h:480
mrpt::obs::CObservation3DRangeScan::hasRangeImage
bool hasRangeImage
true means the field rangeImage contains valid data
Definition: CObservation3DRangeScan.h:376
mrpt::hwdrivers::CSwissRanger3DCamera::enableMedianFilter
void enableMedianFilter(bool enable)
Definition: CSwissRanger3DCamera.h:203
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
mrpt::hwdrivers::CSwissRanger3DCamera::setSaveConfidenceImage
void setSaveConfidenceImage(bool save)
Definition: CSwissRanger3DCamera.h:193
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::hwdrivers::CSwissRanger3DCamera::isEnabledConvGray
bool isEnabledConvGray() const
Definition: CSwissRanger3DCamera.h:224
mrpt::hwdrivers::CSwissRanger3DCamera::isEnabledDenoiseANF
bool isEnabledDenoiseANF() const
Definition: CSwissRanger3DCamera.h:230
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
mrpt::hwdrivers::CSwissRanger3DCamera::getCameraSerialNumber
unsigned int getCameraSerialNumber() const
Get the camera serial number, loaded automatically upon camera open().
Definition: CSwissRanger3DCamera.h:166
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
mrpt::img::CImage::undistort
void undistort(CImage &out_img, const mrpt::img::TCamera &cameraParams) const
Undistort the image according to some camera parameters, and returns an output undistorted image.
Definition: CImage.cpp:1685
mrpt::opengl::stock_objects::CornerXYZ
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
Definition: StockObjects.cpp:136
mrpt::hwdrivers::CSwissRanger3DCamera::enableImageHistEqualization
void enableImageHistEqualization(bool enable)
Definition: CSwissRanger3DCamera.h:194
CTicTac.h
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
mrpt::maps::CColouredPointsMap::loadFromRangeScan
void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr) override
See CPointsMap::loadFromRangeScan()
Definition: CColouredPointsMap.cpp:989
mrpt::hwdrivers::CSwissRanger3DCamera::setSaveRangeImage
void setSaveRangeImage(bool save)
Definition: CSwissRanger3DCamera.h:188
mrpt::hwdrivers::CSwissRanger3DCamera::cols
size_t cols() const
Get the col count in the camera images, loaded automatically upon camera open().
Definition: CSwissRanger3DCamera.h:163
mrpt::hwdrivers::CSwissRanger3DCamera
A class for grabing "range images" from a MESA imaging SwissRanger 3D cameras (SR-2,...
Definition: CSwissRanger3DCamera.h:109
mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation
void getNextObservation(mrpt::obs::CObservation3DRangeScan &out_obs, bool &there_is_obs, bool &hardware_error)
The main data retrieving function, to be called after calling loadConfig() and initialize().
Definition: CSwissRanger3DCamera.cpp:347
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
gui.h
CColouredPointsMap.h
mrpt::maps
Definition: CBeacon.h:21
mrpt::exception_to_str
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
Test_SwissRanger
void Test_SwissRanger()
Definition: vision_stereo_rectify/test.cpp:36
CGridPlaneXY.h
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
TColor.h
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
CTexturedPlane.h
mrpt::system
Definition: backtrace.h:14
mrpt::obs::CObservation3DRangeScan::rangeImage_getAsImage
mrpt::img::CImage rangeImage_getAsImage(const std::optional< mrpt::img::TColormap > color=std::nullopt, const std::optional< float > normMinRange=std::nullopt, const std::optional< float > normMaxRange=std::nullopt, const std::optional< std::string > additionalLayerName=std::nullopt) const
Builds a visualization from the rangeImage.
Definition: CObservation3DRangeScan.cpp:1615
CSwissRanger3DCamera.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020