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-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)
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);
124  std::this_thread::sleep_for(1ms);
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  }
149  win.get3DSceneAndLock();
151  if (enable_rectify)
152  {
153  // Rectify:
154  timlog.enter("rectifyMap.rectify()");
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  }
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  }
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 }
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
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:849
mrpt::vision::TStereoCalibParams params
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.
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:818
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:66
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.
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
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
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...
double getAlpha() const
Return the alpha parameter.
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:148
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020