Main MRPT website > C++ reference for MRPT 1.9.9
test.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
13 #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)
104  cout << "Close the window to end.\n";
105  while (win.isOpen())
106  {
107  win.addTextMessage(
108  5, 5, format("%.02fFPS", win.getRenderingFPS()), TColorf(1, 1, 1),
109  "sans", 15, mrpt::opengl::FILL, 0);
110  win.addTextMessage(
111  5, 25, format(
112  "'r': Switch rectify (Now is: %s) | '+'/'-': Modify "
113  "alpha (Now is: %.02f)",
114  enable_rectify ? "ON" : "OFF", rectifyMap.getAlpha()),
115  TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 10);
116  win.addTextMessage(
117  5, 50,
119  "'s': Switch resize output to 320x240 (Now is: %s) | 'c': "
120  "Switch no-disparity (Now is: %s) | 'e': Switch epipolar lines",
121  rectifyMap.isEnabledResizeOutput() ? "ON" : "OFF",
122  rectifyMap.isEnabledBothCentersCoincide() ? "ON" : "OFF"),
123  TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 11);
124 
125  std::this_thread::sleep_for(1ms);
126 
127  // Grab new video frame:
128  CObservation::Ptr obs = cam->getNextFrame();
129  if (obs)
130  {
132  {
133  // Get the observation object:
135  std::dynamic_pointer_cast<CObservationStereoImages>(obs);
136 
137  // If the rectification maps are still not ready, prepare them
138  // now:
139  if (!rectifyMap.isSet())
140  {
141  timlog.enter("rectifyMap.setFromCamParams");
142  rectifyMap.setFromCamParams(*o);
143  timlog.leave("rectifyMap.setFromCamParams");
144 
145  /*mrpt::img::TStereoCamera params;
146  o->getStereoCameraParams(params);
147  cout << params.dumpAsText() << endl;*/
148  }
150  win.get3DSceneAndLock();
151 
152  if (enable_rectify)
153  {
154  // Rectify:
155  timlog.enter("rectifyMap.rectify()");
156 
157  rectifyMap.rectify(
158  o->imageLeft, o->imageRight, img_left_rectified,
159  img_right_rectified);
160 
161  timlog.leave("rectifyMap.rectify()");
162  }
163  else
164  {
165  // Don't rectify:
166  img_left_rectified = o->imageLeft;
167  img_right_rectified = o->imageRight;
168  }
169 
170  // Draw lines:
171  if (enable_draw_epipolar_lines)
172  {
173  const unsigned int LINES_SEP = 40;
174  const unsigned int w = img_left_rectified.getWidth();
175  const unsigned int h = img_left_rectified.getHeight();
176  for (unsigned int y = 0; y < h; y += LINES_SEP)
177  {
178  img_left_rectified.line(
179  0, y, w - 1, y, mrpt::img::TColor::red(), 2);
180  img_right_rectified.line(
181  0, y, w - 1, y, mrpt::img::TColor::red(), 2);
182  }
183  }
185  gl_views[0]->setImageView(img_left_rectified);
186  gl_views[1]->setImageView(img_right_rectified);
187 
188  win.addTextMessage(
189  150, 5, mrpt::system::timeToString(o->timestamp),
190  TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 2);
191 
192  win.unlockAccess3DScene();
193  win.repaint();
194  }
196  if (win.keyHit())
197  {
199  int key = win.getPushedKey(&kmods);
200 
201  if (key == MRPTK_ESCAPE) break;
202  if (key == 'r' || key == 'R') enable_rectify = !enable_rectify;
203  if (key == 'e' || key == 'E')
204  enable_draw_epipolar_lines = !enable_draw_epipolar_lines;
205  if (key == '+' || key == '-')
206  {
207  double alpha =
208  rectifyMap.getAlpha() + (key == '-' ? -0.1 : 0.1);
209  alpha = std::min(1., std::max(0., alpha));
210  rectifyMap.setAlpha(alpha);
211  }
212  if (key == 's' || key == 'S')
213  {
214  rectifyMap.enableResizeOutput(
215  !rectifyMap.isEnabledResizeOutput(), 320, 240);
216  }
217  if (key == 'c' || key == 'C')
218  {
219  rectifyMap.enableBothCentersCoincide(
220  !rectifyMap.isEnabledBothCentersCoincide());
221  }
222  }
223  }
224  }
225 }
226 
227 // ------------------------------------------------------
228 // MAIN
229 // ------------------------------------------------------
230 int main(int argc, char** argv)
231 {
232  try
233  {
234  TestStereoRectify(argc, argv);
235  return 0;
236  }
237  catch (std::exception& e)
238  {
239  std::cout << "MRPT exception caught: " << e.what() << std::endl;
240  return -1;
241  }
242  catch (...)
243  {
244  printf("Untyped exception!!");
245  return -1;
246  }
247 }
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:1296
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
#define min(a, b)
void TestStereoRectify(int argc, char **argv)
CCameraSensor::Ptr prepareVideoSourceFromUserSelection()
Show to the user a list of possible camera drivers and creates and open the selected camera...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
bool isEnabledResizeOutput() const
Returns whether resizing is enabled (default=false)
This class allows loading and storing values and vectors of different types from ".ini" files easily.
mrptKeyModifier
Definition: keycodes.h:159
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
void setFromCamParams(const mrpt::img::TStereoCamera &params)
Prepares the mapping from the intrinsic, distortion and relative pose parameters of a stereo camera...
std::string timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
Definition: datetime.cpp:338
STL namespace.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
bool isSet() const
Returns true if setFromCamParams() has been already called, false otherwise.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:864
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...
void setAlpha(double alpha)
Sets the alpha parameter which controls the zoom in/out of the rectified images, such that: ...
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
Use this class to rectify stereo images if the same distortion maps are reused over and over again...
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
double leave(const char *func_name)
End of a named section.
void enableBothCentersCoincide(bool enable=true)
If enabled (default=false), the principal points in both output images will coincide.
mrpt::gui::CDisplayWindow3D::Ptr win
static constexpr TColor red()
Predefined colors.
Definition: TColor.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLenum GLint GLint y
Definition: glext.h:3538
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:16
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
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...
renders glyphs as filled polygons
Definition: opengl_fonts.h:38
double getAlpha() const
Return the alpha parameter.
GLenum const GLfloat * params
Definition: glext.h:3534
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
void enter(const char *func_name)
Start of a named section.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019