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 
15 #include <mrpt/system/filesystem.h> // for ASSERT_FILE_EXISTS_
17 #include <iostream>
18 
19 using namespace mrpt;
20 using namespace mrpt::opengl;
21 using namespace mrpt::gui;
22 using namespace mrpt::vision;
23 using namespace mrpt::system;
24 using namespace mrpt::img;
25 using namespace mrpt::obs;
26 using namespace mrpt::system;
27 using namespace mrpt::config;
28 using namespace mrpt::img;
29 using namespace std;
30 
31 // ------------------------------------------------------
32 // TestStereoRectify
33 // ------------------------------------------------------
34 void TestStereoRectify(int argc, char** argv)
35 {
36  CTimeLogger timlog;
38 
39  // Parse optional arguments:
40  if (argc != 1 && argc != 2)
41  {
42  cout
43  << "Usage:\n"
44  << argv[0]
45  << " ==> Run with default camera parameters (from rawlog file)\n"
46  << argv[0]
47  << "[params.cfg] ==> Load stereo camera parameters from cfg file\n";
48  }
49  if (argc == 2)
50  {
51  const string sCfgFile = argv[1];
53 
54  // Load params from file:
56  params.loadFromConfigFile(
57  "CAMERA_PARAMS", mrpt::config::CConfigFile(sCfgFile));
58 
59  // Prepare rectify map:
60  timlog.enter("rectifyMap.setFromCamParams");
61  rectifyMap.setFromCamParams(params);
62  timlog.leave("rectifyMap.setFromCamParams");
63  }
64 
65  // Show to the user a list of possible camera drivers and creates and open
66  // the selected camera.
67  cout << "Please, select the input stereo camera or rawlog file (with "
68  "stereo images)...\n";
69 
72  if (!cam) return;
73 
74  cout << "Video stream open OK\n";
75 
76  // Create 3D window:
77  CDisplayWindow3D win("Demo of stereo rectification", 1280, 600);
78 
79  // Create 2 viewports, one for each image:
80  std::vector<COpenGLViewport::Ptr> gl_views(2);
81  {
82  COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();
83  gl_views[0] = theScene->getViewport("main");
84  ASSERT_(gl_views[0]);
85  gl_views[1] = theScene->createViewport("right_image");
86 
87  // Assign sizes:
88  gl_views[0]->setViewportPosition(0, 0, .5, 1.);
89  gl_views[1]->setViewportPosition(.5, 0, .5, 1.);
90 
91  // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
92  win.unlockAccess3DScene();
93  }
94 
95  win.setPos(10, 10);
96  // win.addTextMessage(...
97 
98  bool enable_rectify = true;
99  bool enable_draw_epipolar_lines = true;
100  CImage img_left_rectified,
101  img_right_rectified; // Declared here to serve as a memory buffer
102  // (avoid deallocating/allocating)
103 
104  cout << "Close the window to end.\n";
105  while (win.isOpen())
106  {
107  win.addTextMessage(5, 5, format("%.02fFPS", win.getRenderingFPS()), 0);
108  win.addTextMessage(
109  5, 25,
111  "'r': Switch rectify (Now is: %s) | '+'/'-': Modify "
112  "alpha (Now is: %.02f)",
113  enable_rectify ? "ON" : "OFF", rectifyMap.getAlpha()),
114  10);
115  win.addTextMessage(
116  5, 50,
118  "'s': Switch resize output to 320x240 (Now is: %s) | 'c': "
119  "Switch no-disparity (Now is: %s) | 'e': Switch epipolar lines",
120  rectifyMap.isEnabledResizeOutput() ? "ON" : "OFF",
121  rectifyMap.isEnabledBothCentersCoincide() ? "ON" : "OFF"),
122  11);
123 
124  std::this_thread::sleep_for(1ms);
125 
126  // Grab new video frame:
127  CObservation::Ptr obs = cam->getNextFrame();
128  if (obs)
129  {
131  {
132  // Get the observation object:
134  std::dynamic_pointer_cast<CObservationStereoImages>(obs);
135 
136  // If the rectification maps are still not ready, prepare them
137  // now:
138  if (!rectifyMap.isSet())
139  {
140  timlog.enter("rectifyMap.setFromCamParams");
141  rectifyMap.setFromCamParams(*o);
142  timlog.leave("rectifyMap.setFromCamParams");
143 
144  /*mrpt::img::TStereoCamera params;
145  o->getStereoCameraParams(params);
146  cout << params.dumpAsText() << endl;*/
147  }
148 
149  win.get3DSceneAndLock();
150 
151  if (enable_rectify)
152  {
153  // Rectify:
154  timlog.enter("rectifyMap.rectify()");
155 
156  rectifyMap.rectify(
157  o->imageLeft, o->imageRight, img_left_rectified,
158  img_right_rectified);
159 
160  timlog.leave("rectifyMap.rectify()");
161  }
162  else
163  {
164  // Don't rectify:
165  img_left_rectified = o->imageLeft;
166  img_right_rectified = o->imageRight;
167  }
168 
169  // Draw lines:
170  if (enable_draw_epipolar_lines)
171  {
172  const unsigned int LINES_SEP = 40;
173  const unsigned int w = img_left_rectified.getWidth();
174  const unsigned int h = img_left_rectified.getHeight();
175  for (unsigned int y = 0; y < h; y += LINES_SEP)
176  {
177  img_left_rectified.line(
178  0, y, w - 1, y, mrpt::img::TColor::red(), 2);
179  img_right_rectified.line(
180  0, y, w - 1, y, mrpt::img::TColor::red(), 2);
181  }
182  }
183 
184  gl_views[0]->setImageView(img_left_rectified);
185  gl_views[1]->setImageView(img_right_rectified);
186 
187  win.addTextMessage(
188  150, 5, mrpt::system::timeToString(o->timestamp), 2);
189 
190  win.unlockAccess3DScene();
191  win.repaint();
192  }
193 
194  if (win.keyHit())
195  {
196  mrptKeyModifier kmods;
197  int key = win.getPushedKey(&kmods);
198 
199  if (key == MRPTK_ESCAPE) break;
200  if (key == 'r' || key == 'R') enable_rectify = !enable_rectify;
201  if (key == 'e' || key == 'E')
202  enable_draw_epipolar_lines = !enable_draw_epipolar_lines;
203  if (key == '+' || key == '-')
204  {
205  double alpha =
206  rectifyMap.getAlpha() + (key == '-' ? -0.1 : 0.1);
207  alpha = std::min(1., std::max(0., alpha));
208  rectifyMap.setAlpha(alpha);
209  }
210  if (key == 's' || key == 'S')
211  {
212  rectifyMap.enableResizeOutput(
213  !rectifyMap.isEnabledResizeOutput(), 320, 240);
214  }
215  if (key == 'c' || key == 'C')
216  {
217  rectifyMap.enableBothCentersCoincide(
219  }
220  }
221  }
222  }
223 }
224 
225 // ------------------------------------------------------
226 // MAIN
227 // ------------------------------------------------------
228 int main(int argc, char** argv)
229 {
230  try
231  {
233  return 0;
234  }
235  catch (const std::exception& e)
236  {
237  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
238  return -1;
239  }
240  catch (...)
241  {
242  printf("Untyped exception!!");
243  return -1;
244  }
245 }
mrpt::config
Definition: config/CConfigFile.h:14
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
filesystem.h
TestStereoRectify
void TestStereoRectify(int argc, char **argv)
Definition: vision_stereo_rectify/test.cpp:34
mrpt::img::CImage::getWidth
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
mrpt::img::TColor::red
static constexpr TColor red()
Predefined colors.
Definition: TColor.h:66
mrpt::system::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: system/CTimeLogger.h:50
mrpt::vision::CStereoRectifyMap::setAlpha
void setAlpha(double alpha)
Sets the alpha parameter which controls the zoom in/out of the rectified images, such that:
Definition: CStereoRectifyMap.cpp:77
mrpt::vision::CStereoRectifyMap::getAlpha
double getAlpha() const
Return the alpha parameter.
Definition: CStereoRectifyMap.h:140
mrpt::vision
Copyright (C) 2010 Hauke Strasdat Imperial College London Copyright (c) 2005-2020,...
Definition: bundle_adjustment.h:35
IS_CLASS
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
Definition: CObject.h:146
CConfigFile.h
mrpt::system::timeToString
std::string timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
Definition: datetime.cpp:244
CCameraSensor.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::vision::CStereoRectifyMap
Use this class to rectify stereo images if the same distortion maps are reused over and over again.
Definition: CStereoRectifyMap.h:74
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
CDisplayWindow3D.h
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
CStereoRectifyMap.h
mrpt::hwdrivers::prepareVideoSourceFromUserSelection
CCameraSensor::Ptr prepareVideoSourceFromUserSelection()
Show to the user a list of possible camera drivers and creates and open the selected camera.
Definition: CCameraSensor.cpp:1467
mrpt::vision::CStereoRectifyMap::isEnabledResizeOutput
bool isEnabledResizeOutput() const
Returns whether resizing is enabled (default=false)
Definition: CStereoRectifyMap.h:152
mrpt::img
Definition: CCanvas.h:16
mrpt::system::CTimeLogger::leave
double leave(const std::string_view &func_name) noexcept
End of a named section.
Definition: system/CTimeLogger.h:146
COpenGLScene.h
mrpt::img::TStereoCamera
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
mrpt::img::CImage::line
void line(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid) override
Draws a line.
Definition: CImage.cpp:1117
mrpt::system::CTimeLogger::enter
void enter(const std::string_view &func_name) noexcept
Start of a named section.
Definition: system/CTimeLogger.h:140
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
mrpt::vision::CStereoRectifyMap::isEnabledBothCentersCoincide
bool isEnabledBothCentersCoincide() const
Definition: CStereoRectifyMap.h:182
mrpt::vision::CStereoRectifyMap::enableResizeOutput
void enableResizeOutput(bool enable, unsigned int target_width=0, unsigned int target_height=0)
If enabled, the computed maps will rectify images to a size different than their original size.
Definition: CStereoRectifyMap.cpp:84
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
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::vision::CStereoRectifyMap::setFromCamParams
void setFromCamParams(const mrpt::img::TStereoCamera &params)
Prepares the mapping from the intrinsic, distortion and relative pose parameters of a stereo camera.
Definition: CStereoRectifyMap.cpp:104
mrpt::img::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
mrpt::vision::CStereoRectifyMap::rectify
void rectify(const mrpt::img::CImage &in_left_image, const mrpt::img::CImage &in_right_image, mrpt::img::CImage &out_left_image, mrpt::img::CImage &out_right_image) const
Rectify the input image pair and save the result in a different output images - setFromCamParams() mu...
Definition: CStereoRectifyMap.cpp:264
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
CTimeLogger.h
mrpt::gui::MRPTK_ESCAPE
@ MRPTK_ESCAPE
Definition: keycodes.h:27
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:31
mrpt::gui::mrptKeyModifier
mrptKeyModifier
Definition: keycodes.h:156
mrpt::vision::CStereoRectifyMap::isSet
bool isSet() const
Returns true if setFromCamParams() has been already called, false otherwise.
Definition: CStereoRectifyMap.h:86
mrpt::hwdrivers::CCameraSensor::Ptr
std::shared_ptr< CCameraSensor > Ptr
Definition: CCameraSensor.h:353
mrpt::vision::CStereoRectifyMap::enableBothCentersCoincide
void enableBothCentersCoincide(bool enable=true)
If enabled (default=false), the principal points in both output images will coincide.
Definition: CStereoRectifyMap.cpp:94
mrpt::obs::CObservationStereoImages::Ptr
std::shared_ptr< mrpt::obs ::CObservationStereoImages > Ptr
Definition: CObservationStereoImages.h:40
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
ASSERT_FILE_EXISTS_
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
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
mrpt::obs::CObservationStereoImages
Observation class for either a pair of left+right or left+disparity images from a stereo camera.
Definition: CObservationStereoImages.h:38
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020