Main MRPT website > C++ reference for MRPT 1.5.5
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-2017, 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 
15 #include <mrpt/utils/CTimeLogger.h>
17 
18 // Universal include for all versions of OpenCV
19 #include <mrpt/otherlibs/do_opencv_includes.h>
20 
21 
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::system;
24 using namespace mrpt::synch;
25 using namespace mrpt::obs;
26 using namespace mrpt::math;
27 using namespace std;
29 
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(0,0,0, DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90)),
47  m_user_device_number(0),
48  m_serial_number(0)
49 {
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.zeros();
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.zeros();
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 the need for it.
89 * \exception This method must throw an exception with a descriptive message if some critical error is found.
90 */
92 {
93 #if MRPT_HAS_OPENNI2
94  try{
95  if(getConnectedDevices() <= 0){ // Check and list the available devices. If there is at least one 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(m_serial_number, m_user_device_number) == false){
103  THROW_EXCEPTION(mrpt::format("Failed to find sensor_id from serial number(%d).", m_serial_number))
104  }
105  }
106  else
107  open(m_user_device_number);
108  }
109  if(isOpen(m_user_device_number) == false){
110  THROW_EXCEPTION(mrpt::format("Failed to open OpenNI2 device(%d).", m_user_device_number))
111  }
112  /* If camera parameter is not read from ini file, we get the parameters from OpenNI2. */
113  if(isValidParameter(m_cameraParamsDepth) == false){
114  if(getDepthSensorParam(m_cameraParamsDepth, m_user_device_number) == false){
115  THROW_EXCEPTION("Failed to get Depth camera parameters.")
116  }
117  }
118  if(isValidParameter(m_cameraParamsRGB) == false){
119  if(getColorSensorParam(m_cameraParamsRGB, m_user_device_number) == false){
120  THROW_EXCEPTION("Failed to get RGB camera parameters.")
121  }
122  }
123  }catch(std::logic_error& e){
124  throw(e);
125  }
126 #else
127  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
128 #endif // MRPT_HAS_OPENNI2
129 }
130 
131 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
132 * \exception This method must throw an exception with a descriptive message if some critical error is found.
133 */
135 {
136 #if MRPT_HAS_OPENNI2
137 // cout << "COpenNI2Sensor::doProcess...\n";
138 
139  bool thereIs, hwError;
140 
141  CObservation3DRangeScanPtr newObs = CObservation3DRangeScan::Create();
142 
143  assert(getNumDevices() > 0);
144  getNextObservation( *newObs, thereIs, hwError );
145 
146  if (hwError)
147  {
148  m_state = ssError;
149  THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
150  }
151 
152  if (thereIs)
153  {
154  m_state = ssWorking;
155 
156  std::vector<mrpt::utils::CSerializablePtr> objs;
157  if (m_grab_image || m_grab_depth || m_grab_3D_points) objs.push_back(newObs);
158 
159  appendObservations( objs );
160  }
161 #else
162  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
163 #endif // MRPT_HAS_OPENNI2
164 }
165 
166 /** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes)
167 * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
168 */
170  const mrpt::utils::CConfigFileBase &configSource,
171  const std::string &iniSection )
172 {
173  cout << "COpenNI2Sensor::loadConfig_sensorSpecific...\n";
174 
175  m_sensorPoseOnRobot.setFromValues(
176  configSource.read_float(iniSection,"pose_x",0),
177  configSource.read_float(iniSection,"pose_y",0),
178  configSource.read_float(iniSection,"pose_z",0),
179  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
180  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
181  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
182  );
183 
184  m_preview_window = configSource.read_bool(iniSection,"preview_window",m_preview_window);
185 
186  m_width = configSource.read_int(iniSection,"width",0);
187  m_height = configSource.read_int(iniSection,"height",0);
188  m_fps = configSource.read_float(iniSection,"fps",0);
189  std::cout << "width " << m_width << " height " << m_height << " fps " << m_fps << endl;
190 
191  bool hasRightCameraSection = configSource.sectionExists(iniSection + string("_RIGHT"));
192  bool hasLeftCameraSection = configSource.sectionExists(iniSection + string("_LEFT"));
193  bool hasLeft2RightPose = configSource.sectionExists(iniSection + string("_LEFT2RIGHT_POSE"));
194 
196 
197  try {
198  sc.loadFromConfigFile(iniSection,configSource);
199  } catch (std::exception &e) {
200  std::cout << "[COpenNI2Sensor::loadConfig_sensorSpecific] Warning: Ignoring error loading calibration parameters:\n" << e.what();
201  }
202  if(hasRightCameraSection){
203  m_cameraParamsRGB = sc.rightCamera;
204  }
205  if(hasLeftCameraSection){
206  m_cameraParamsDepth = sc.leftCamera;
207  }
208  if(hasLeft2RightPose){
209  const mrpt::poses::CPose3D twist(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90));
210  m_relativePoseIntensityWRTDepth = twist + mrpt::poses::CPose3D(sc.rightCameraPose);
211  }
212 
213  // Id:
214  m_user_device_number = configSource.read_int(iniSection, "device_number", m_user_device_number);
215  //cout << "LOAD m_user_device_number " << m_user_device_number << endl;
216  m_serial_number = configSource.read_int(iniSection, "serial_number", m_serial_number);
217 
218  m_grab_image = configSource.read_bool(iniSection,"grab_image",m_grab_image);
219  m_grab_depth = configSource.read_bool(iniSection,"grab_depth",m_grab_depth);
220  m_grab_3D_points = configSource.read_bool(iniSection,"grab_3D_points",m_grab_3D_points);
221 
222  {
223  std::string s = configSource.read_string(iniSection,"relativePoseIntensityWRTDepth","");
224  if (!s.empty())
225  m_relativePoseIntensityWRTDepth.fromString(s);
226  }
227 
228 }
229 
230 
231 /** The main data retrieving function, to be called after calling loadConfig() and initialize().
232 * \param out_obs The output retrieved observation (only if there_is_obs=true).
233 * \param there_is_obs If set to false, there was no new observation.
234 * \param hardware_error True on hardware/comms error.
235 *
236 * \sa doProcess
237 */
240  bool &there_is_obs,
241  bool &hardware_error)
242 {
243 #if MRPT_HAS_OPENNI2
244 // cout << "COpenNI2Sensor::getNextObservation \n";
245 
246  // Read a frame (depth + rgb)
247  getNextFrameRGBD(out_obs, there_is_obs, hardware_error, m_user_device_number );
248 
249 
250  // Set common data into observation:
251  // --------------------------------------
252  out_obs.sensorLabel = m_sensorLabel;
253  out_obs.sensorPose = m_sensorPoseOnRobot;
254  out_obs.relativePoseIntensityWRTDepth = m_relativePoseIntensityWRTDepth;
255 
256  out_obs.cameraParams = m_cameraParamsDepth;
257  out_obs.cameraParamsIntensity = m_cameraParamsRGB;
258 
259  // 3D point cloud:
260  if ( out_obs.hasRangeImage && m_grab_3D_points )
261  {
263 
264  if ( !m_grab_depth )
265  {
266  out_obs.hasRangeImage = false;
267  out_obs.rangeImage.resize(0,0);
268  }
269 
270  }
271 
272  // preview in real-time?
273  if (m_preview_window)
274  {
275  if ( out_obs.hasRangeImage )
276  {
277  if (++m_preview_decim_counter_range>m_preview_window_decimation)
278  {
279  m_preview_decim_counter_range=0;
280  if (!m_win_range) { m_win_range = mrpt::gui::CDisplayWindow::Create("Preview RANGE"); m_win_range->setPos(5,5); }
281 
282  // Normalize the image
284  img.setFromMatrix(out_obs.rangeImage);
285  CMatrixFloat r = out_obs.rangeImage * float(1.0/this->m_maxRange);
286  m_win_range->showImage(img);
287  }
288  }
289  if ( out_obs.hasIntensityImage )
290  {
291  if (++m_preview_decim_counter_rgb>m_preview_window_decimation)
292  {
293  m_preview_decim_counter_rgb=0;
294  if (!m_win_int) { m_win_int = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY"); m_win_int->setPos(300,5); }
295  m_win_int->showImage(out_obs.intensityImage );
296  }
297  }
298  }
299  else
300  {
301  if (m_win_range) m_win_range.clear();
302  if (m_win_int) m_win_int.clear();
303  }
304 
305 // cout << "COpenNI2Sensor::getNextObservation finish\n";
306 #else
307  MRPT_UNUSED_PARAM(out_obs); MRPT_UNUSED_PARAM(there_is_obs); MRPT_UNUSED_PARAM(hardware_error);
308  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
309 #endif // MRPT_HAS_OPENNI2
310 }
311 
312 
313 /* -----------------------------------------------------
314 setPathForExternalImages
315 ----------------------------------------------------- */
317 {
318  MRPT_UNUSED_PARAM(directory);
319  // Ignore for now. It seems performance is better grabbing everything
320  // to a single big file than creating hundreds of smaller files per second...
321  return;
322 
323  // if (!mrpt::system::createDirectory( directory ))
324  // {
325  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for externally saved images: %s",directory.c_str() )
326  // }
327  // m_path_for_external_images = directory;
328 }
virtual void doProcess()
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool sectionExists(const std::string &section_name) const
Checks if a given section exists (name is case insensitive)
bool isValidParameter(const mrpt::utils::TCamera &param)
double DEG2RAD(const double x)
Degrees to radians.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
#define THROW_EXCEPTION(msg)
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
virtual void loadConfig_sensorSpecific(const mrpt::utils::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 utils::CConfigFileBase and derived classes)
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.
std::string read_string(const std::string &section, const std::string &name, const std::string &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:3602
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
GLint GLvoid * img
Definition: glext.h:3645
TCamera rightCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:29
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.
bool hasRangeImage
true means the field rangeImage contains valid data
This namespace provides multitask, synchronization utilities.
Definition: atomic_incr.h:29
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
#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.
static CDisplayWindowPtr Create()
void loadFromConfigFile(const std::string &section, const mrpt::utils::CConfigFileBase &cfg)
Load all the params from a config source, in the same format that used in saveToConfigFile().
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
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:72
bool hasIntensityImage
true means the field intensityImage contains valid data
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
GLfloat param
Definition: glext.h:3705
mrpt::poses::CPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:30
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019