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/3rdparty/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(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg)
43 
44 {
45  // Default label:
46  m_sensorLabel = "OPENNI2";
47 
48  // =========== Default params ===========
49  // ----- RGB -----
50  m_cameraParamsRGB.ncols = 0;
51  m_cameraParamsRGB.nrows = 0;
52 
53  m_cameraParamsRGB.cx(-1);
54  m_cameraParamsRGB.cy(-1);
55  m_cameraParamsRGB.fx(-1);
56  m_cameraParamsRGB.fy(-1);
57 
58  m_cameraParamsRGB.dist.fill(0);
59 
60  // ----- Depth -----
61  m_cameraParamsDepth.ncols = 0;
62  m_cameraParamsDepth.nrows = 0;
63 
64  m_cameraParamsDepth.cx(-1);
65  m_cameraParamsDepth.cy(-1);
66  m_cameraParamsDepth.fx(-1);
67  m_cameraParamsDepth.fy(-1);
68 
69  m_cameraParamsDepth.dist.fill(0);
70 }
71 
72 /*-------------------------------------------------------------
73 dtor
74 -------------------------------------------------------------*/
76 {
77 #if MRPT_HAS_OPENNI2
78  close(m_user_device_number);
79 #endif // MRPT_HAS_OPENNI2
80 }
81 
82 /** This method can or cannot be implemented in the derived class, depending on
83  * the need for it.
84  * \exception This method must throw an exception with a descriptive message if
85  * some critical error is found.
86  */
88 {
89 #if MRPT_HAS_OPENNI2
90  try
91  {
92  if (getConnectedDevices() <= 0)
93  { // Check and list the available devices. If there is at least one
94  // device connected, open the first in the list.
95  return;
96  }
97  {
98  if (m_serial_number != 0)
99  {
100  openDeviceBySerial(m_serial_number);
101  if (getDeviceIDFromSerialNum(
102  m_serial_number, m_user_device_number) == false)
103  {
105  "Failed to find sensor_id from serial number(%d).",
106  m_serial_number));
107  }
108  }
109  else
110  open(m_user_device_number);
111  }
112  if (isOpen(m_user_device_number) == false)
113  {
115  "Failed to open OpenNI2 device(%d).", m_user_device_number));
116  }
117  /* If camera parameter is not read from ini file, we get the parameters
118  * from OpenNI2. */
119  if (isValidParameter(m_cameraParamsDepth) == false)
120  {
121  if (getDepthSensorParam(
122  m_cameraParamsDepth, m_user_device_number) == false)
123  {
124  THROW_EXCEPTION("Failed to get Depth camera parameters.");
125  }
126  }
127  if (isValidParameter(m_cameraParamsRGB) == false)
128  {
129  if (getColorSensorParam(m_cameraParamsRGB, m_user_device_number) ==
130  false)
131  {
132  THROW_EXCEPTION("Failed to get RGB camera parameters.");
133  }
134  }
135  }
136  catch (std::logic_error& e)
137  {
138  throw(e);
139  }
140 #else
141  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
142 #endif // MRPT_HAS_OPENNI2
143 }
144 
145 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
146  * \exception This method must throw an exception with a descriptive message if
147  * some critical error is found.
148  */
150 {
151 #if MRPT_HAS_OPENNI2
152  // cout << "COpenNI2Sensor::doProcess...\n";
153 
154  bool thereIs, hwError;
155 
157  std::make_shared<CObservation3DRangeScan>();
158 
159  assert(getNumDevices() > 0);
160  getNextObservation(*newObs, thereIs, hwError);
161 
162  if (hwError)
163  {
164  m_state = ssError;
165  THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
166  }
167 
168  if (thereIs)
169  {
170  m_state = ssWorking;
171 
172  std::vector<mrpt::serialization::CSerializable::Ptr> objs;
173  if (m_grab_image || m_grab_depth || m_grab_3D_points)
174  objs.push_back(newObs);
175 
176  appendObservations(objs);
177  }
178 #else
179  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
180 #endif // MRPT_HAS_OPENNI2
181 }
182 
183 /** Loads specific configuration for the device from a given source of
184  * configuration parameters, for example, an ".ini" file, loading from the
185  * section "[iniSection]" (see config::CConfigFileBase and derived classes)
186  * \exception This method must throw an exception with a descriptive message if
187  * some critical parameter is missing or has an invalid value.
188  */
190  const mrpt::config::CConfigFileBase& configSource,
191  const std::string& iniSection)
192 {
193  cout << "COpenNI2Sensor::loadConfig_sensorSpecific...\n";
194 
195  m_sensorPoseOnRobot.setFromValues(
196  configSource.read_float(iniSection, "pose_x", 0),
197  configSource.read_float(iniSection, "pose_y", 0),
198  configSource.read_float(iniSection, "pose_z", 0),
199  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
200  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
201  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
202 
203  m_preview_window =
204  configSource.read_bool(iniSection, "preview_window", m_preview_window);
205 
206  m_width = configSource.read_int(iniSection, "width", 0);
207  m_height = configSource.read_int(iniSection, "height", 0);
208  m_fps = configSource.read_float(iniSection, "fps", 0);
209  std::cout << "width " << m_width << " height " << m_height << " fps "
210  << m_fps << endl;
211 
212  bool hasRightCameraSection =
213  configSource.sectionExists(iniSection + string("_RIGHT"));
214  bool hasLeftCameraSection =
215  configSource.sectionExists(iniSection + string("_LEFT"));
216  bool hasLeft2RightPose =
217  configSource.sectionExists(iniSection + string("_LEFT2RIGHT_POSE"));
218 
220 
221  try
222  {
223  sc.loadFromConfigFile(iniSection, configSource);
224  }
225  catch (const std::exception& e)
226  {
227  std::cout << "[COpenNI2Sensor::loadConfig_sensorSpecific] Warning: "
228  "Ignoring error loading calibration parameters:\n"
229  << e.what();
230  }
231  if (hasRightCameraSection)
232  {
233  m_cameraParamsRGB = sc.rightCamera;
234  }
235  if (hasLeftCameraSection)
236  {
237  m_cameraParamsDepth = sc.leftCamera;
238  }
239  if (hasLeft2RightPose)
240  {
242  0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg);
243  m_relativePoseIntensityWRTDepth =
244  twist +
246  }
247 
248  // Id:
249  m_user_device_number = configSource.read_int(
250  iniSection, "device_number", m_user_device_number);
251  // cout << "LOAD m_user_device_number " << m_user_device_number << endl;
252  m_serial_number =
253  configSource.read_int(iniSection, "serial_number", m_serial_number);
254 
255  m_grab_image =
256  configSource.read_bool(iniSection, "grab_image", m_grab_image);
257  m_grab_depth =
258  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
259  m_grab_3D_points =
260  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
261 
262  {
263  std::string s = configSource.read_string(
264  iniSection, "relativePoseIntensityWRTDepth", "");
265  if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
266  }
267 }
268 
269 /** The main data retrieving function, to be called after calling loadConfig()
270  * and initialize().
271  * \param out_obs The output retrieved observation (only if there_is_obs=true).
272  * \param there_is_obs If set to false, there was no new observation.
273  * \param hardware_error True on hardware/comms error.
274  *
275  * \sa doProcess
276  */
278  mrpt::obs::CObservation3DRangeScan& out_obs, bool& there_is_obs,
279  bool& hardware_error)
280 {
281 #if MRPT_HAS_OPENNI2
282  // cout << "COpenNI2Sensor::getNextObservation \n";
283 
284  // Read a frame (depth + rgb)
285  getNextFrameRGBD(
286  out_obs, there_is_obs, hardware_error, m_user_device_number);
287 
288  // Set common data into observation:
289  // --------------------------------------
290  out_obs.sensorLabel = m_sensorLabel;
291  out_obs.sensorPose = m_sensorPoseOnRobot;
292  out_obs.relativePoseIntensityWRTDepth = m_relativePoseIntensityWRTDepth;
293 
294  out_obs.cameraParams = m_cameraParamsDepth;
295  out_obs.cameraParamsIntensity = m_cameraParamsRGB;
296 
297  // 3D point cloud:
298  if (out_obs.hasRangeImage && m_grab_3D_points)
299  {
301 
302  if (!m_grab_depth)
303  {
304  out_obs.hasRangeImage = false;
305  out_obs.rangeImage.resize(0, 0);
306  }
307  }
308 
309  // preview in real-time?
310  if (m_preview_window)
311  {
312  if (out_obs.hasRangeImage)
313  {
314  if (++m_preview_decim_counter_range > m_preview_window_decimation)
315  {
316  m_preview_decim_counter_range = 0;
317  if (!m_win_range)
318  {
319  m_win_range =
320  mrpt::gui::CDisplayWindow::Create("Preview RANGE");
321  m_win_range->setPos(5, 5);
322  }
323 
324  // Normalize the image
326  img.setFromMatrix(out_obs.rangeImage);
327  CMatrixFloat r = out_obs.rangeImage;
328  r *= float(1.0 / this->m_maxRange);
329  m_win_range->showImage(img);
330  }
331  }
332  if (out_obs.hasIntensityImage)
333  {
334  if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
335  {
336  m_preview_decim_counter_rgb = 0;
337  if (!m_win_int)
338  {
339  m_win_int =
340  mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
341  m_win_int->setPos(300, 5);
342  }
343  m_win_int->showImage(out_obs.intensityImage);
344  }
345  }
346  }
347  else
348  {
349  if (m_win_range) m_win_range.reset();
350  if (m_win_int) m_win_int.reset();
351  }
352 
353 // cout << "COpenNI2Sensor::getNextObservation finish\n";
354 #else
355  MRPT_UNUSED_PARAM(out_obs);
356  MRPT_UNUSED_PARAM(there_is_obs);
357  MRPT_UNUSED_PARAM(hardware_error);
358  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
359 #endif // MRPT_HAS_OPENNI2
360 }
361 
362 /* -----------------------------------------------------
363 setPathForExternalImages
364 ----------------------------------------------------- */
366 {
367  MRPT_UNUSED_PARAM(directory);
368  // Ignore for now. It seems performance is better grabbing everything
369  // to a single big file than creating hundreds of smaller files per
370  // second...
371  return;
372 
373  // if (!mrpt::system::createDirectory( directory ))
374  // {
375  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
376  // externally
377  // saved images: %s",directory.c_str() );
378  // }
379  // m_path_for_external_images = directory;
380 }
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
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool isValidParameter(const mrpt::img::TCamera &param)
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 ...
constexpr double DEG2RAD(const double x)
Degrees to radians.
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
Parameters for the Brown-Conrady camera lens distortion 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:85
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: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: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019