MRPT  2.0.5
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-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 
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  [[maybe_unused]] mrpt::obs::CObservation3DRangeScan& out_obs,
279  [[maybe_unused]] bool& there_is_obs, [[maybe_unused]] 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  {
300  out_obs.unprojectInto(out_obs);
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  m_win_range->showImage(out_obs.rangeImage_getAsImage());
325  }
326  }
327  if (out_obs.hasIntensityImage)
328  {
329  if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
330  {
331  m_preview_decim_counter_rgb = 0;
332  if (!m_win_int)
333  {
334  m_win_int =
335  mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
336  m_win_int->setPos(300, 5);
337  }
338  m_win_int->showImage(out_obs.intensityImage);
339  }
340  }
341  }
342  else
343  {
344  if (m_win_range) m_win_range.reset();
345  if (m_win_int) m_win_int.reset();
346  }
347 
348 // cout << "COpenNI2Sensor::getNextObservation finish\n";
349 #else
350  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
351 #endif // MRPT_HAS_OPENNI2
352 }
353 
354 /* -----------------------------------------------------
355 setPathForExternalImages
356 ----------------------------------------------------- */
358  [maybe_unused]] const std::string& directory)
359 {
360  // Ignore for now. It seems performance is better grabbing everything
361  // to a single big file than creating hundreds of smaller files per
362  // second...
363  return;
364 
365  // if (!mrpt::system::createDirectory( directory ))
366  // {
367  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
368  // externally
369  // saved images: %s",directory.c_str() );
370  // }
371  // m_path_for_external_images = directory;
372 }
uint32_t nrows
Definition: TCamera.h:40
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 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().
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
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.
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.
constexpr double DEG2RAD(const double x)
Degrees to radians.
This namespace contains representation of robot actions and observations.
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
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
#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)...
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
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.
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: eb5aebda1 Sat Jul 18 23:34:37 2020 +0200 at sáb jul 18 23:45:10 CEST 2020