MRPT  1.9.9
COpenNI2Sensor.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 
10 #include "hwdrivers-precomp.h" // Precompiled header
11 
14 #include <mrpt/img/TStereoCamera.h>
16 #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 
42  m_relativePoseIntensityWRTDepth(
43  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90))
44 
45 {
46  // Default label:
47  m_sensorLabel = "OPENNI2";
48 
49  // =========== Default params ===========
50  // ----- RGB -----
51  m_cameraParamsRGB.ncols = 0;
52  m_cameraParamsRGB.nrows = 0;
53 
54  m_cameraParamsRGB.cx(-1);
55  m_cameraParamsRGB.cy(-1);
56  m_cameraParamsRGB.fx(-1);
57  m_cameraParamsRGB.fy(-1);
58 
59  m_cameraParamsRGB.dist.fill(0);
60 
61  // ----- Depth -----
62  m_cameraParamsDepth.ncols = 0;
63  m_cameraParamsDepth.nrows = 0;
64 
65  m_cameraParamsDepth.cx(-1);
66  m_cameraParamsDepth.cy(-1);
67  m_cameraParamsDepth.fx(-1);
68  m_cameraParamsDepth.fy(-1);
69 
70  m_cameraParamsDepth.dist.fill(0);
71 }
72 
73 /*-------------------------------------------------------------
74 dtor
75 -------------------------------------------------------------*/
77 {
78 #if MRPT_HAS_OPENNI2
79  close(m_user_device_number);
80 #endif // MRPT_HAS_OPENNI2
81 }
82 
83 /** This method can or cannot be implemented in the derived class, depending on
84  * the need for it.
85  * \exception This method must throw an exception with a descriptive message if
86  * some critical error is found.
87  */
89 {
90 #if MRPT_HAS_OPENNI2
91  try
92  {
93  if (getConnectedDevices() <= 0)
94  { // Check and list the available devices. If there is at least one
95  // device connected, open the first in the list.
96  return;
97  }
98  {
99  if (m_serial_number != 0)
100  {
101  openDeviceBySerial(m_serial_number);
102  if (getDeviceIDFromSerialNum(
103  m_serial_number, m_user_device_number) == false)
104  {
106  "Failed to find sensor_id from serial number(%d).",
107  m_serial_number));
108  }
109  }
110  else
111  open(m_user_device_number);
112  }
113  if (isOpen(m_user_device_number) == false)
114  {
116  "Failed to open OpenNI2 device(%d).", m_user_device_number));
117  }
118  /* If camera parameter is not read from ini file, we get the parameters
119  * from OpenNI2. */
120  if (isValidParameter(m_cameraParamsDepth) == false)
121  {
122  if (getDepthSensorParam(
123  m_cameraParamsDepth, m_user_device_number) == false)
124  {
125  THROW_EXCEPTION("Failed to get Depth camera parameters.");
126  }
127  }
128  if (isValidParameter(m_cameraParamsRGB) == false)
129  {
130  if (getColorSensorParam(m_cameraParamsRGB, m_user_device_number) ==
131  false)
132  {
133  THROW_EXCEPTION("Failed to get RGB camera parameters.");
134  }
135  }
136  }
137  catch (std::logic_error& e)
138  {
139  throw(e);
140  }
141 #else
142  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
143 #endif // MRPT_HAS_OPENNI2
144 }
145 
146 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
147  * \exception This method must throw an exception with a descriptive message if
148  * some critical error is found.
149  */
151 {
152 #if MRPT_HAS_OPENNI2
153  // cout << "COpenNI2Sensor::doProcess...\n";
154 
155  bool thereIs, hwError;
156 
158  std::make_shared<CObservation3DRangeScan>();
159 
160  assert(getNumDevices() > 0);
161  getNextObservation(*newObs, thereIs, hwError);
162 
163  if (hwError)
164  {
165  m_state = ssError;
166  THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
167  }
168 
169  if (thereIs)
170  {
171  m_state = ssWorking;
172 
173  std::vector<mrpt::serialization::CSerializable::Ptr> objs;
174  if (m_grab_image || m_grab_depth || m_grab_3D_points)
175  objs.push_back(newObs);
176 
177  appendObservations(objs);
178  }
179 #else
180  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
181 #endif // MRPT_HAS_OPENNI2
182 }
183 
184 /** Loads specific configuration for the device from a given source of
185  * configuration parameters, for example, an ".ini" file, loading from the
186  * section "[iniSection]" (see config::CConfigFileBase and derived classes)
187  * \exception This method must throw an exception with a descriptive message if
188  * some critical parameter is missing or has an invalid value.
189  */
191  const mrpt::config::CConfigFileBase& configSource,
192  const std::string& iniSection)
193 {
194  cout << "COpenNI2Sensor::loadConfig_sensorSpecific...\n";
195 
196  m_sensorPoseOnRobot.setFromValues(
197  configSource.read_float(iniSection, "pose_x", 0),
198  configSource.read_float(iniSection, "pose_y", 0),
199  configSource.read_float(iniSection, "pose_z", 0),
200  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
201  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
202  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
203 
204  m_preview_window =
205  configSource.read_bool(iniSection, "preview_window", m_preview_window);
206 
207  m_width = configSource.read_int(iniSection, "width", 0);
208  m_height = configSource.read_int(iniSection, "height", 0);
209  m_fps = configSource.read_float(iniSection, "fps", 0);
210  std::cout << "width " << m_width << " height " << m_height << " fps "
211  << m_fps << endl;
212 
213  bool hasRightCameraSection =
214  configSource.sectionExists(iniSection + string("_RIGHT"));
215  bool hasLeftCameraSection =
216  configSource.sectionExists(iniSection + string("_LEFT"));
217  bool hasLeft2RightPose =
218  configSource.sectionExists(iniSection + string("_LEFT2RIGHT_POSE"));
219 
221 
222  try
223  {
224  sc.loadFromConfigFile(iniSection, configSource);
225  }
226  catch (const std::exception& e)
227  {
228  std::cout << "[COpenNI2Sensor::loadConfig_sensorSpecific] Warning: "
229  "Ignoring error loading calibration parameters:\n"
230  << e.what();
231  }
232  if (hasRightCameraSection)
233  {
234  m_cameraParamsRGB = sc.rightCamera;
235  }
236  if (hasLeftCameraSection)
237  {
238  m_cameraParamsDepth = sc.leftCamera;
239  }
240  if (hasLeft2RightPose)
241  {
243  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90));
244  m_relativePoseIntensityWRTDepth =
245  twist +
247  }
248 
249  // Id:
250  m_user_device_number = configSource.read_int(
251  iniSection, "device_number", m_user_device_number);
252  // cout << "LOAD m_user_device_number " << m_user_device_number << endl;
253  m_serial_number =
254  configSource.read_int(iniSection, "serial_number", m_serial_number);
255 
256  m_grab_image =
257  configSource.read_bool(iniSection, "grab_image", m_grab_image);
258  m_grab_depth =
259  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
260  m_grab_3D_points =
261  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
262 
263  {
264  std::string s = configSource.read_string(
265  iniSection, "relativePoseIntensityWRTDepth", "");
266  if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
267  }
268 }
269 
270 /** The main data retrieving function, to be called after calling loadConfig()
271  * and initialize().
272  * \param out_obs The output retrieved observation (only if there_is_obs=true).
273  * \param there_is_obs If set to false, there was no new observation.
274  * \param hardware_error True on hardware/comms error.
275  *
276  * \sa doProcess
277  */
279  mrpt::obs::CObservation3DRangeScan& out_obs, bool& there_is_obs,
280  bool& hardware_error)
281 {
282 #if MRPT_HAS_OPENNI2
283  // cout << "COpenNI2Sensor::getNextObservation \n";
284 
285  // Read a frame (depth + rgb)
286  getNextFrameRGBD(
287  out_obs, there_is_obs, hardware_error, m_user_device_number);
288 
289  // Set common data into observation:
290  // --------------------------------------
291  out_obs.sensorLabel = m_sensorLabel;
292  out_obs.sensorPose = m_sensorPoseOnRobot;
293  out_obs.relativePoseIntensityWRTDepth = m_relativePoseIntensityWRTDepth;
294 
295  out_obs.cameraParams = m_cameraParamsDepth;
296  out_obs.cameraParamsIntensity = m_cameraParamsRGB;
297 
298  // 3D point cloud:
299  if (out_obs.hasRangeImage && m_grab_3D_points)
300  {
302 
303  if (!m_grab_depth)
304  {
305  out_obs.hasRangeImage = false;
306  out_obs.rangeImage.resize(0, 0);
307  }
308  }
309 
310  // preview in real-time?
311  if (m_preview_window)
312  {
313  if (out_obs.hasRangeImage)
314  {
315  if (++m_preview_decim_counter_range > m_preview_window_decimation)
316  {
317  m_preview_decim_counter_range = 0;
318  if (!m_win_range)
319  {
320  m_win_range =
321  mrpt::gui::CDisplayWindow::Create("Preview RANGE");
322  m_win_range->setPos(5, 5);
323  }
324 
325  // Normalize the image
327  img.setFromMatrix(out_obs.rangeImage);
328  CMatrixFloat r = out_obs.rangeImage;
329  r *= float(1.0 / this->m_maxRange);
330  m_win_range->showImage(img);
331  }
332  }
333  if (out_obs.hasIntensityImage)
334  {
335  if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
336  {
337  m_preview_decim_counter_rgb = 0;
338  if (!m_win_int)
339  {
340  m_win_int =
341  mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
342  m_win_int->setPos(300, 5);
343  }
344  m_win_int->showImage(out_obs.intensityImage);
345  }
346  }
347  }
348  else
349  {
350  if (m_win_range) m_win_range.reset();
351  if (m_win_int) m_win_int.reset();
352  }
353 
354 // cout << "COpenNI2Sensor::getNextObservation finish\n";
355 #else
356  MRPT_UNUSED_PARAM(out_obs);
357  MRPT_UNUSED_PARAM(there_is_obs);
358  MRPT_UNUSED_PARAM(hardware_error);
359  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
360 #endif // MRPT_HAS_OPENNI2
361 }
362 
363 /* -----------------------------------------------------
364 setPathForExternalImages
365 ----------------------------------------------------- */
367 {
368  MRPT_UNUSED_PARAM(directory);
369  // Ignore for now. It seems performance is better grabbing everything
370  // to a single big file than creating hundreds of smaller files per
371  // second...
372  return;
373 
374  // if (!mrpt::system::createDirectory( directory ))
375  // {
376  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
377  // externally
378  // saved images: %s",directory.c_str() );
379  // }
380  // m_path_for_external_images = directory;
381 }
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
void resize(size_t row, size_t col)
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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...
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
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
STL namespace.
GLdouble s
Definition: glext.h:3682
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
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:3769
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
This namespace contains representation of robot actions and observations.
bool hasRangeImage
true means the field rangeImage contains valid data
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:25
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:45
GLsizei const GLchar ** string
Definition: glext.h:4116
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void setPathForExternalImages(const std::string &directory) override
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
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:3711
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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:28
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:31
~COpenNI2Sensor() override
Default ctor.
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section) override
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)
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
GLfloat param
Definition: glext.h:3838
This template class provides the basic functionality for a general 2D any-size, resizable container o...
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
#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: 582f6a819 Mon Jul 22 16:41:38 2019 -0700 at mar jul 23 01:50:13 CEST 2019