Main MRPT website > C++ reference for MRPT 1.5.6
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...
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
bool isValidParameter(const mrpt::utils::TCamera &param)
double DEG2RAD(const double x)
Degrees to radians.
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) method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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.
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
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.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
GLint GLvoid * img
Definition: glew.h:1290
GLdouble s
Definition: glew.h:1295
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.
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)...
uint32_t ncols
Definition: TCamera.h:53
bool hasRangeImage
true means the field rangeImage contains valid data
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void setFromMatrix(const Eigen::MatrixBase< Derived > &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: CImage.h:746
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().
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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 sectionExists(const std::string &section_name) const
Checks if a given section exists (name is case insensitive)
bool hasIntensityImage
true means the field intensityImage contains valid data
uint32_t nrows
Camera resolution.
Definition: TCamera.h:53
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
GLfloat param
Definition: glew.h:1435
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018