MRPT  1.9.9
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-2019, 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)
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,
113  "'r': Switch rectify (Now is: %s) | '+'/'-': Modify "
114  "alpha (Now is: %.02f)",
115  enable_rectify ? "ON" : "OFF", rectifyMap.getAlpha()),
116  TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 10);
117  win.addTextMessage(
118  5, 50,
120  "'s': Switch resize output to 320x240 (Now is: %s) | 'c': "
121  "Switch no-disparity (Now is: %s) | 'e': Switch epipolar lines",
122  rectifyMap.isEnabledResizeOutput() ? "ON" : "OFF",
123  rectifyMap.isEnabledBothCentersCoincide() ? "ON" : "OFF"),
124  TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 11);
126  std::this_thread::sleep_for(1ms);
128  // Grab new video frame:
129  CObservation::Ptr obs = cam->getNextFrame();
130  if (obs)
131  {
133  {
134  // Get the observation object:
136  std::dynamic_pointer_cast<CObservationStereoImages>(obs);
137 
138  // If the rectification maps are still not ready, prepare them
139  // now:
140  if (!rectifyMap.isSet())
141  {
142  timlog.enter("rectifyMap.setFromCamParams");
143  rectifyMap.setFromCamParams(*o);
144  timlog.leave("rectifyMap.setFromCamParams");
146  /*mrpt::img::TStereoCamera params;
147  o->getStereoCameraParams(params);
148  cout << params.dumpAsText() << endl;*/
149  }
151  win.get3DSceneAndLock();
152 
153  if (enable_rectify)
154  {
155  // Rectify:
156  timlog.enter("rectifyMap.rectify()");
158  rectifyMap.rectify(
159  o->imageLeft, o->imageRight, img_left_rectified,
160  img_right_rectified);
161 
162  timlog.leave("rectifyMap.rectify()");
163  }
164  else
165  {
166  // Don't rectify:
167  img_left_rectified = o->imageLeft;
168  img_right_rectified = o->imageRight;
169  }
170 
171  // Draw lines:
172  if (enable_draw_epipolar_lines)
173  {
174  const unsigned int LINES_SEP = 40;
175  const unsigned int w = img_left_rectified.getWidth();
176  const unsigned int h = img_left_rectified.getHeight();
177  for (unsigned int y = 0; y < h; y += LINES_SEP)
178  {
179  img_left_rectified.line(
180  0, y, w - 1, y, mrpt::img::TColor::red(), 2);
181  img_right_rectified.line(
182  0, y, w - 1, y, mrpt::img::TColor::red(), 2);
183  }
184  }
185 
186  gl_views[0]->setImageView(img_left_rectified);
187  gl_views[1]->setImageView(img_right_rectified);
189  win.addTextMessage(
190  150, 5, mrpt::system::timeToString(o->timestamp),
191  TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 2);
193  win.unlockAccess3DScene();
194  win.repaint();
195  }
197  if (win.keyHit())
198  {
199  mrptKeyModifier kmods;
200  int key = win.getPushedKey(&kmods);
201 
202  if (key == MRPTK_ESCAPE) break;
203  if (key == 'r' || key == 'R') enable_rectify = !enable_rectify;
204  if (key == 'e' || key == 'E')
205  enable_draw_epipolar_lines = !enable_draw_epipolar_lines;
206  if (key == '+' || key == '-')
207  {
208  double alpha =
209  rectifyMap.getAlpha() + (key == '-' ? -0.1 : 0.1);
210  alpha = std::min(1., std::max(0., alpha));
211  rectifyMap.setAlpha(alpha);
212  }
213  if (key == 's' || key == 'S')
214  {
215  rectifyMap.enableResizeOutput(
216  !rectifyMap.isEnabledResizeOutput(), 320, 240);
217  }
218  if (key == 'c' || key == 'C')
219  {
220  rectifyMap.enableBothCentersCoincide(
221  !rectifyMap.isEnabledBothCentersCoincide());
222  }
223  }
224  }
225  }
226 }
228 // ------------------------------------------------------
229 // MAIN
230 // ------------------------------------------------------
231 int main(int argc, char** argv)
232 {
233  try
234  {
236  return 0;
237  }
238  catch (const std::exception& e)
239  {
240  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
241  return -1;
242  }
243  catch (...)
244  {
245  printf("Untyped exception!!");
246  return -1;
247  }
248 }
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:1113
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
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...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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:156
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:845
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:244
STL namespace.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
bool isSet() const
Returns true if setFromCamParams() has been already called, false otherwise.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:814
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.
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: CDifodo.h:17
#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
void enableBothCentersCoincide(bool enable=true)
If enabled (default=false), the principal points in both output images will coincide.
mrpt::gui::CDisplayWindow3D::Ptr win
void enter(const std::string_view &func_name) noexcept
Start of a named section.
static constexpr TColor red()
Predefined colors.
Definition: TColor.h:61
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const char * argv[]
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
double leave(const std::string_view &func_name) noexcept
End of a named section.
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
GLenum GLint GLint y
Definition: glext.h:3542
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
const int argc
#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:35
double getAlpha() const
Return the alpha parameter.
GLenum const GLfloat * params
Definition: glext.h:3538
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
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: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019