Main MRPT website > C++ reference for MRPT 1.9.9
COpenNI2Sensor.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 
10 #include "hwdrivers-precomp.h" // Precompiled header
11 
16 #include <mrpt/img/TStereoCamera.h>
17 #include <mrpt/poses/CPose3DQuat.h>
18 
19 // Universal include for all versions of OpenCV
20 #include <mrpt/otherlibs/do_opencv_includes.h>
21 
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::system;
24 using namespace mrpt::obs;
25 using namespace mrpt::math;
26 using namespace std;
27 using mrpt::DEG2RAD;
28 
30 {
31  return param.ncols > 0 && param.nrows > 0;
32 }
33 
35 
36 /*-------------------------------------------------------------
37 ctor
38 -------------------------------------------------------------*/
40  : m_sensorPoseOnRobot(),
41  m_preview_window(false),
42  m_preview_window_decimation(1),
43  m_preview_decim_counter_range(0),
44  m_preview_decim_counter_rgb(0),
45 
46  m_relativePoseIntensityWRTDepth(
47  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90)),
48  m_user_device_number(0),
49  m_serial_number(0)
50 {
51  // Default label:
52  m_sensorLabel = "OPENNI2";
53 
54  // =========== Default params ===========
55  // ----- RGB -----
56  m_cameraParamsRGB.ncols = 0;
57  m_cameraParamsRGB.nrows = 0;
58 
59  m_cameraParamsRGB.cx(-1);
60  m_cameraParamsRGB.cy(-1);
61  m_cameraParamsRGB.fx(-1);
62  m_cameraParamsRGB.fy(-1);
63 
64  m_cameraParamsRGB.dist.fill(0);
65 
66  // ----- Depth -----
67  m_cameraParamsDepth.ncols = 0;
68  m_cameraParamsDepth.nrows = 0;
69 
70  m_cameraParamsDepth.cx(-1);
71  m_cameraParamsDepth.cy(-1);
72  m_cameraParamsDepth.fx(-1);
73  m_cameraParamsDepth.fy(-1);
74 
75  m_cameraParamsDepth.dist.fill(0);
76 }
77 
78 /*-------------------------------------------------------------
79 dtor
80 -------------------------------------------------------------*/
82 {
83 #if MRPT_HAS_OPENNI2
84  close(m_user_device_number);
85 #endif // MRPT_HAS_OPENNI2
86 }
87 
88 /** This method can or cannot be implemented in the derived class, depending on
89 * the need for it.
90 * \exception This method must throw an exception with a descriptive message if
91 * some critical error is found.
92 */
94 {
95 #if MRPT_HAS_OPENNI2
96  try
97  {
98  if (getConnectedDevices() <= 0)
99  { // Check and list the available devices. If there is at least one
100  // device connected, open the first in the list.
101  return;
102  }
103  {
104  if (m_serial_number != 0)
105  {
106  openDeviceBySerial(m_serial_number);
107  if (getDeviceIDFromSerialNum(
108  m_serial_number, m_user_device_number) == false)
109  {
111  mrpt::format(
112  "Failed to find sensor_id from serial number(%d).",
113  m_serial_number))
114  }
115  }
116  else
117  open(m_user_device_number);
118  }
119  if (isOpen(m_user_device_number) == false)
120  {
122  mrpt::format(
123  "Failed to open OpenNI2 device(%d).", m_user_device_number))
124  }
125  /* If camera parameter is not read from ini file, we get the parameters
126  * from OpenNI2. */
127  if (isValidParameter(m_cameraParamsDepth) == false)
128  {
129  if (getDepthSensorParam(
130  m_cameraParamsDepth, m_user_device_number) == false)
131  {
132  THROW_EXCEPTION("Failed to get Depth camera parameters.");
133  }
134  }
135  if (isValidParameter(m_cameraParamsRGB) == false)
136  {
137  if (getColorSensorParam(m_cameraParamsRGB, m_user_device_number) ==
138  false)
139  {
140  THROW_EXCEPTION("Failed to get RGB camera parameters.");
141  }
142  }
143  }
144  catch (std::logic_error& e)
145  {
146  throw(e);
147  }
148 #else
149  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
150 #endif // MRPT_HAS_OPENNI2
151 }
152 
153 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
154 * \exception This method must throw an exception with a descriptive message if
155 * some critical error is found.
156 */
158 {
159 #if MRPT_HAS_OPENNI2
160  // cout << "COpenNI2Sensor::doProcess...\n";
161 
162  bool thereIs, hwError;
163 
165  mrpt::make_aligned_shared<CObservation3DRangeScan>();
166 
167  assert(getNumDevices() > 0);
168  getNextObservation(*newObs, thereIs, hwError);
169 
170  if (hwError)
171  {
172  m_state = ssError;
173  THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
174  }
175 
176  if (thereIs)
177  {
178  m_state = ssWorking;
179 
180  std::vector<mrpt::serialization::CSerializable::Ptr> objs;
181  if (m_grab_image || m_grab_depth || m_grab_3D_points)
182  objs.push_back(newObs);
183 
184  appendObservations(objs);
185  }
186 #else
187  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
188 #endif // MRPT_HAS_OPENNI2
189 }
190 
191 /** Loads specific configuration for the device from a given source of
192 * configuration parameters, for example, an ".ini" file, loading from the
193 * section "[iniSection]" (see config::CConfigFileBase and derived classes)
194 * \exception This method must throw an exception with a descriptive message if
195 * some critical parameter is missing or has an invalid value.
196 */
198  const mrpt::config::CConfigFileBase& configSource,
199  const std::string& iniSection)
200 {
201  cout << "COpenNI2Sensor::loadConfig_sensorSpecific...\n";
202 
203  m_sensorPoseOnRobot.setFromValues(
204  configSource.read_float(iniSection, "pose_x", 0),
205  configSource.read_float(iniSection, "pose_y", 0),
206  configSource.read_float(iniSection, "pose_z", 0),
207  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
208  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
209  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
210 
211  m_preview_window =
212  configSource.read_bool(iniSection, "preview_window", m_preview_window);
213 
214  m_width = configSource.read_int(iniSection, "width", 0);
215  m_height = configSource.read_int(iniSection, "height", 0);
216  m_fps = configSource.read_float(iniSection, "fps", 0);
217  std::cout << "width " << m_width << " height " << m_height << " fps "
218  << m_fps << endl;
219 
220  bool hasRightCameraSection =
221  configSource.sectionExists(iniSection + string("_RIGHT"));
222  bool hasLeftCameraSection =
223  configSource.sectionExists(iniSection + string("_LEFT"));
224  bool hasLeft2RightPose =
225  configSource.sectionExists(iniSection + string("_LEFT2RIGHT_POSE"));
226 
228 
229  try
230  {
231  sc.loadFromConfigFile(iniSection, configSource);
232  }
233  catch (std::exception& e)
234  {
235  std::cout << "[COpenNI2Sensor::loadConfig_sensorSpecific] Warning: "
236  "Ignoring error loading calibration parameters:\n"
237  << e.what();
238  }
239  if (hasRightCameraSection)
240  {
241  m_cameraParamsRGB = sc.rightCamera;
242  }
243  if (hasLeftCameraSection)
244  {
245  m_cameraParamsDepth = sc.leftCamera;
246  }
247  if (hasLeft2RightPose)
248  {
249  const mrpt::poses::CPose3D twist(
250  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90));
251  m_relativePoseIntensityWRTDepth =
252  twist +
254  }
255 
256  // Id:
257  m_user_device_number = configSource.read_int(
258  iniSection, "device_number", m_user_device_number);
259  // cout << "LOAD m_user_device_number " << m_user_device_number << endl;
260  m_serial_number =
261  configSource.read_int(iniSection, "serial_number", m_serial_number);
262 
263  m_grab_image =
264  configSource.read_bool(iniSection, "grab_image", m_grab_image);
265  m_grab_depth =
266  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
267  m_grab_3D_points =
268  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
269 
270  {
271  std::string s = configSource.read_string(
272  iniSection, "relativePoseIntensityWRTDepth", "");
273  if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
274  }
275 }
276 
277 /** The main data retrieving function, to be called after calling loadConfig()
278 * and initialize().
279 * \param out_obs The output retrieved observation (only if there_is_obs=true).
280 * \param there_is_obs If set to false, there was no new observation.
281 * \param hardware_error True on hardware/comms error.
282 *
283 * \sa doProcess
284 */
286  mrpt::obs::CObservation3DRangeScan& out_obs, bool& there_is_obs,
287  bool& hardware_error)
288 {
289 #if MRPT_HAS_OPENNI2
290  // cout << "COpenNI2Sensor::getNextObservation \n";
291 
292  // Read a frame (depth + rgb)
293  getNextFrameRGBD(
294  out_obs, there_is_obs, hardware_error, m_user_device_number);
295 
296  // Set common data into observation:
297  // --------------------------------------
298  out_obs.sensorLabel = m_sensorLabel;
299  out_obs.sensorPose = m_sensorPoseOnRobot;
300  out_obs.relativePoseIntensityWRTDepth = m_relativePoseIntensityWRTDepth;
301 
302  out_obs.cameraParams = m_cameraParamsDepth;
303  out_obs.cameraParamsIntensity = m_cameraParamsRGB;
304 
305  // 3D point cloud:
306  if (out_obs.hasRangeImage && m_grab_3D_points)
307  {
309 
310  if (!m_grab_depth)
311  {
312  out_obs.hasRangeImage = false;
313  out_obs.rangeImage.resize(0, 0);
314  }
315  }
316 
317  // preview in real-time?
318  if (m_preview_window)
319  {
320  if (out_obs.hasRangeImage)
321  {
322  if (++m_preview_decim_counter_range > m_preview_window_decimation)
323  {
324  m_preview_decim_counter_range = 0;
325  if (!m_win_range)
326  {
327  m_win_range =
328  mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
329  "Preview RANGE");
330  m_win_range->setPos(5, 5);
331  }
332 
333  // Normalize the image
335  img.setFromMatrix(out_obs.rangeImage);
336  CMatrixFloat r =
337  out_obs.rangeImage * float(1.0 / this->m_maxRange);
338  m_win_range->showImage(img);
339  }
340  }
341  if (out_obs.hasIntensityImage)
342  {
343  if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
344  {
345  m_preview_decim_counter_rgb = 0;
346  if (!m_win_int)
347  {
348  m_win_int =
349  mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
350  "Preview INTENSITY");
351  m_win_int->setPos(300, 5);
352  }
353  m_win_int->showImage(out_obs.intensityImage);
354  }
355  }
356  }
357  else
358  {
359  if (m_win_range) m_win_range.reset();
360  if (m_win_int) m_win_int.reset();
361  }
362 
363 // cout << "COpenNI2Sensor::getNextObservation finish\n";
364 #else
365  MRPT_UNUSED_PARAM(out_obs);
366  MRPT_UNUSED_PARAM(there_is_obs);
367  MRPT_UNUSED_PARAM(hardware_error);
368  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
369 #endif // MRPT_HAS_OPENNI2
370 }
371 
372 /* -----------------------------------------------------
373 setPathForExternalImages
374 ----------------------------------------------------- */
376 {
377  MRPT_UNUSED_PARAM(directory);
378  // Ignore for now. It seems performance is better grabbing everything
379  // to a single big file than creating hundreds of smaller files per
380  // second...
381  return;
382 
383  // if (!mrpt::system::createDirectory( directory ))
384  // {
385  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
386  // externally
387  // saved images: %s",directory.c_str() )
388  // }
389  // m_path_for_external_images = directory;
390 }
virtual void doProcess()
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
bool isValidParameter(const mrpt::img::TCamera &param)
double DEG2RAD(const double x)
Degrees to radians.
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
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().
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
Contains classes for various device interfaces.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
STL namespace.
GLdouble s
Definition: glext.h:3676
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
GLint GLvoid * img
Definition: glext.h:3763
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
virtual void setPathForExternalImages(const std::string &directory)
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
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
bool hasRangeImage
true means the field rangeImage contains valid data
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:29
bool sectionExists(const std::string &section_name) const
Checks if a given section exists (name is case insensitive)
void loadFromConfigFile(const std::string &section, const mrpt::config::CConfigFileBase &cfg)
Load all the params from a config source, in the same format that used in saveToConfigFile().
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool hasIntensityImage
true means the field intensityImage contains valid data
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:30
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:33
virtual void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section)
Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see config::CConfigFileBase and derived classes)
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
GLfloat param
Definition: glext.h:3831
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



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