MRPT  1.9.9
CSwissRanger3DCamera.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 headers
11 
13 
14 #include <mrpt/system/filesystem.h>
16 
18 
19 #if MRPT_HAS_SWISSRANGE
20 #ifdef _WIN32
21 #define WIN32_LEAN_AND_MEAN
22 #include <windows.h>
23 #endif
24 
25 #include <libMesaSR.h>
26 
27 #ifdef MRPT_OS_LINUX
28 #include <stdio.h>
29 #include <termios.h>
30 #include <unistd.h>
31 
32 #include <asm/ioctls.h>
33 #include <linux/sockios.h>
34 #include <sys/select.h>
35 #endif
36 #endif
37 
38 using namespace mrpt::hwdrivers;
39 using namespace mrpt::system;
40 using namespace std;
41 
42 /*-------------------------------------------------------------
43  ctor
44  -------------------------------------------------------------*/
46  : m_sensorPoseOnRobot(),
47 
48  m_ip_address("192.168.2.14")
49 
50 {
51  m_sensorLabel = "3DCAM";
52 
53  // Default params: Obtained from a SR4000 with 0.004px avr reprojection
54  // error.
55  m_cameraParams.ncols = 176;
56  m_cameraParams.nrows = 144;
57  m_cameraParams.intrinsicParams(0, 0) = 262.9201; // fx
58  m_cameraParams.intrinsicParams(1, 1) = 262.9218; // fy
59  m_cameraParams.intrinsicParams(0, 2) = 87.99958; // cx
60  m_cameraParams.intrinsicParams(1, 2) = 68.99957; // cy
61  m_cameraParams.dist[0] = -8.258543e-01;
62  m_cameraParams.dist[1] = 6.561022e-01;
63  m_cameraParams.dist[2] = 2.699818e-06;
64  m_cameraParams.dist[3] = -3.263559e-05;
65  m_cameraParams.dist[4] = 0;
66 
67 #if !MRPT_HAS_SWISSRANGE
69  "MRPT was compiled without support for SwissRanger 3D cameras! Rebuild "
70  "it.");
71 #endif
72 }
73 
74 /*-------------------------------------------------------------
75  dtor
76  -------------------------------------------------------------*/
78 /*-------------------------------------------------------------
79  Modified A-law compression algorithm for uint16_t -> uint8_t
80  The original method uses signed int16_t. It's being tuned for
81  what we want here...
82  -------------------------------------------------------------*/
83 static char ALawCompressTable[128] = {
84  1, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5,
85  5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
86  6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 7, 7,
87  7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
88  7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
89  7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
90 
91 uint8_t table_16u_to_8u[0x10000];
92 bool table_16u_to_8u_init = false;
93 
94 unsigned char LinearToALawSample(uint16_t sample)
95 {
96  if (sample >= 0x200)
97  {
98  int exponent = ALawCompressTable[(sample >> 9) & 0x7F];
99  int mantissa = (sample >> (exponent + 3)) & 0x1F;
100  return ((exponent << 5) | mantissa);
101  }
102  else
103  {
104  return (sample >> 4);
105  }
106 }
107 
109 {
110  for (unsigned int i = 0; i < 0x10000; i++)
112 }
113 
114 /** This method can or cannot be implemented in the derived class, depending on
115  * the need for it.
116  * \exception This method must throw an exception with a descriptive message if
117  * some critical error is found.
118  */
120 {
121  if (!open()) THROW_EXCEPTION("Error opening SwissRanger 3D camera.");
122 }
123 
124 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
125  * \exception This method must throw an exception with a descriptive message if
126  * some critical error is found.
127  */
129 {
130  using namespace mrpt::obs;
131 
132  bool thereIs, hwError;
133 
135  std::make_shared<CObservation3DRangeScan>();
136 
137  getNextObservation(*newObs, thereIs, hwError);
138 
139  if (hwError)
140  {
141  m_state = ssError;
142  THROW_EXCEPTION("Couldn't communicate to the SwissRanger 3D camera!");
143  }
144 
145  if (thereIs)
146  {
147  m_state = ssWorking;
148 
149  appendObservation(newObs);
150  }
151 }
152 
153 /** Loads specific configuration for the device from a given source of
154  * configuration parameters, for example, an ".ini" file, loading from the
155  * section "[iniSection]" (see config::CConfigFileBase and derived classes)
156  * \exception This method must throw an exception with a descriptive message if
157  * some critical parameter is missing or has an invalid value.
158  */
160  const mrpt::config::CConfigFileBase& configSource,
161  const std::string& iniSection)
162 {
163  using mrpt::DEG2RAD;
164 
166  configSource.read_float(iniSection, "pose_x", 0),
167  configSource.read_float(iniSection, "pose_y", 0),
168  configSource.read_float(iniSection, "pose_z", 0),
169  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
170  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
171  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
172 
174  configSource.read_bool(iniSection, "preview_window", m_preview_window);
175 
176  m_save_3d = configSource.read_bool(iniSection, "save_3d", m_save_3d);
178  configSource.read_bool(iniSection, "save_range_img", m_save_range_img);
179  m_save_intensity_img = configSource.read_bool(
180  iniSection, "save_intensity_img", m_save_intensity_img);
181  m_save_confidence = configSource.read_bool(
182  iniSection, "save_confidence", m_save_confidence);
183 
184  m_enable_img_hist_equal = configSource.read_bool(
185  iniSection, "enable_img_hist_equal", m_enable_img_hist_equal);
186  m_enable_median_filter = configSource.read_bool(
187  iniSection, "enable_median_filter", m_enable_median_filter);
188  m_enable_mediancross_filter = configSource.read_bool(
189  iniSection, "enable_mediancross_filter", m_enable_mediancross_filter);
190  m_enable_conv_gray = configSource.read_bool(
191  iniSection, "enable_conv_gray", m_enable_conv_gray);
192  m_enable_denoise_anf = configSource.read_bool(
193  iniSection, "enable_denoise_anf", m_enable_denoise_anf);
194 
196  configSource.read_bool(iniSection, "open_from_usb", m_open_from_usb);
197  m_usb_serial =
198  configSource.read_uint64_t(iniSection, "usb_serial", m_usb_serial);
199  m_ip_address =
200  configSource.read_string(iniSection, "ip_address", m_ip_address);
201 
203  iniSection, "external_images_format", m_external_images_format));
205  iniSection, "external_images_jpeg_quality",
207 
208  try
209  {
210  m_cameraParams.loadFromConfigFile(iniSection, configSource);
211  }
212  catch (std::exception&)
213  {
214  // If there's some missing field, just keep the default values.
215  }
216 }
217 
218 bool CSwissRanger3DCamera::getMesaLibVersion(std::string& out_version) const
219 {
220 #if MRPT_HAS_SWISSRANGE
221  unsigned short version[4];
222  if (0 != SR_GetVersion(version)) return false;
223 
224  out_version =
225  format("%d.%d.%d.%d", version[3], version[2], version[1], version[0]);
226  return true;
227 #else
228  MRPT_UNUSED_PARAM(out_version);
229  return false;
230 #endif
231 }
232 
233 bool CSwissRanger3DCamera::isOpen() const { return m_cam != nullptr; }
235 {
236 #if MRPT_HAS_SWISSRANGE
237  if (isOpen()) close();
238 
239  SRCAM cam;
240  if (m_open_from_usb)
241  {
242  if (SR_OpenUSB(&cam, this->m_usb_serial) <= 0) return false;
243  }
244  else
245  {
246  if (SR_OpenETH(&cam, this->m_ip_address.c_str()) <= 0) return false;
247  }
248  m_cam = cam;
249 
250  // Initialization code:
251  m_rows = SR_GetRows(cam);
252  m_cols = SR_GetCols(cam);
253 
254  m_cam_serial_num = SR_ReadSerial(cam);
255 
256  // Deduce max range from frequency:
257  const ModulationFrq fr = SR_GetModulationFrequency(cam);
258  switch (fr)
259  {
260  case MF_40MHz:
261  m_maxRange = 3.75;
262  break;
263  case MF_30MHz:
264  m_maxRange = 5;
265  break;
266  case MF_21MHz:
267  m_maxRange = 7.14;
268  break;
269  case MF_20MHz:
270  m_maxRange = 7.5;
271  break;
272  case MF_19MHz:
273  m_maxRange = 7.89;
274  break;
275  case MF_60MHz:
276  m_maxRange = 2.5;
277  break;
278  case MF_15MHz:
279  m_maxRange = 10;
280  break;
281  case MF_10MHz:
282  m_maxRange = 15;
283  break;
284  case MF_29MHz:
285  m_maxRange = 5.17;
286  break;
287  case MF_31MHz:
288  m_maxRange = 4.84;
289  break;
290  case MF_14_5MHz:
291  m_maxRange = 10.34;
292  break;
293  case MF_15_5MHz:
294  m_maxRange = 9.68;
295  break;
296 
297  default:
298  m_maxRange = 5.0;
299  break;
300  }
301 
302  SR_SetTimeout(cam, 1000 /* ms */);
303 
305 
306  return true;
307 #else
308  return false;
309 #endif
310 }
311 
313 {
314 #if MRPT_HAS_SWISSRANGE
315  if (m_cam) SR_Close(SRCAM(m_cam));
316  m_cam = nullptr;
317 #endif
318 }
319 
321 {
322 #if MRPT_HAS_SWISSRANGE
323  if (!isOpen()) return;
324 
325  SR_SetMode(
326  SRCAM(m_cam),
327  AM_COR_FIX_PTRN | // turns on fix pattern noise correction <b>this
328  // should always be enabled for good distance
329  // measurement</b>
330  (m_enable_median_filter ? AM_MEDIAN : 0) |
331  (m_enable_conv_gray ? AM_CONV_GRAY : 0) |
332  (m_enable_denoise_anf ? AM_DENOISE_ANF : 0) |
333  (m_save_confidence ? AM_CONF_MAP : 0) |
334  (m_enable_mediancross_filter ? AM_MEDIANCROSS : 0));
335 #endif
336 }
337 
338 /** The main data retrieving function, to be called after calling loadConfig()
339  * and initialize().
340  * \param out_obs The output retrieved observation (only if
341  * there_is_obs=true).
342  * \param there_is_obs If set to false, there was no new observation.
343  * \param hardware_error True on hardware/comms error.
344  *
345  * \sa doProcess
346  */
348  mrpt::obs::CObservation3DRangeScan& m_out_obs, bool& there_is_obs,
349  bool& hardware_error)
350 {
351  there_is_obs = false;
352  hardware_error = false;
353 #if MRPT_HAS_SWISSRANGE
354 
355  int bytesRx = SR_Acquire(SRCAM(m_cam));
356  if (!bytesRx)
357  {
358  cerr << "[CSwissRanger3DCamera] Zero bytes read from the camera."
359  << endl;
360  hardware_error = true;
361  return;
362  }
363 
364  // Extract images:
365  ImgEntry* imgEntryArray;
366  const int nImgs = SR_GetImageList(SRCAM(m_cam), &imgEntryArray);
367 
368  if (!nImgs)
369  {
370  cerr << "[CSwissRanger3DCamera] Error: no images in image list."
371  << endl;
372  hardware_error = true;
373  return;
374  }
375 
376  // Initialize the output observation:
380  obs.maxRange = m_maxRange;
381  obs.stdError = 0.01f;
382 
383  // Process each of the images:
384  for (int i = 0; i < nImgs; i++)
385  {
386  const ImgEntry* img = imgEntryArray + i;
387  switch (img->imgType)
388  {
389  // Ranges:
390  case ImgEntry::IT_DISTANCE:
391  {
392  if (this->m_save_range_img)
393  {
394  ASSERT_(img->dataType == ImgEntry::DT_USHORT)
395  obs.hasRangeImage = true;
396  obs.range_is_depth = false;
397 
398  // Convert data from uint16_t to float ranges:
399  // (0x0000, 0xFFFF) -> (0m, 5m)
400  const float K = obs.maxRange / 0xFFFF;
401  obs.rangeImage.setSize(img->height, img->width);
402 
403  const uint16_t* data_ptr =
404  reinterpret_cast<const uint16_t*>(img->data);
405 
406  obs.rangeUnits = K;
407  for (size_t y = 0; y < img->height; y++)
408  for (size_t x = 0; x < img->width; x++)
409  obs.rangeImage(y, x) = (*data_ptr++);
410  }
411 
412  if (this->m_save_3d)
413  {
414  ASSERT_(img->dataType == ImgEntry::DT_USHORT)
415  obs.hasPoints3D = true;
416 
417  const size_t N = img->height * img->width;
418  obs.points3D_x.resize(N);
419  obs.points3D_y.resize(N);
420  obs.points3D_z.resize(N);
421 
422  // Swap XYZ order, so:
423  // SwissRange -> MRPT
424  // Z X
425  // X Y
426  // Y Z
427  SR_CoordTrfFlt(
428  SRCAM(m_cam),
429  &obs.points3D_y[0], // X
430  &obs.points3D_z[0], // Y
431  &obs.points3D_x[0], // Z
432  sizeof(float), sizeof(float), sizeof(float));
433  }
434  }
435  break;
436 
437  // Intensity:
438  case ImgEntry::IT_AMPLITUDE:
439  {
440  if (this->m_save_intensity_img)
441  {
442  ASSERT_(img->dataType == ImgEntry::DT_USHORT)
443  obs.hasIntensityImage = true;
444 
445  // Make sure the camera params are there:
446  m_cameraParams.scaleToResolution(img->width, img->height);
448 
449  // make sure the modified A-law Look Up Table is up-to-date:
451  {
452  table_16u_to_8u_init = true;
454  }
455 
456  obs.intensityImage.resize(img->width, img->height, 1, true);
457 
458  const uint16_t* data_ptr =
459  reinterpret_cast<const uint16_t*>(img->data);
460  for (size_t y = 0; y < img->height; y++)
461  {
462  uint8_t* row = obs.intensityImage(0, y, 0);
463  for (size_t x = 0; x < img->width; x++)
464  // Convert 16u -> 8u
465  (*row++) = table_16u_to_8u[*data_ptr++];
466  }
467 
469  obs.intensityImage.equalizeHistInPlace();
470 
471  // Save as external image file??
472  if (!m_path_for_external_images.empty())
473  {
474  const string filName =
476  format(
477  "_INT_%f.%s",
478  (double)timestampTotime_t(obs.timestamp),
479  m_external_images_format.c_str());
481  m_path_for_external_images + string("/") + filName,
483  obs.intensityImage.setExternalStorage(filName);
484  }
485  }
486  }
487  break;
488 
489  // Confidence:
490  case ImgEntry::IT_CONF_MAP:
491  {
492  if (this->m_save_confidence)
493  {
494  ASSERT_(img->dataType == ImgEntry::DT_USHORT)
495  obs.hasConfidenceImage = true;
496 
498  img->width, img->height, 1, true);
499 
500  const uint16_t* data_ptr =
501  reinterpret_cast<const uint16_t*>(img->data);
502  for (size_t y = 0; y < img->height; y++)
503  {
504  uint8_t* row = obs.confidenceImage(0, y, 0);
505  for (size_t x = 0; x < img->width; x++)
506  (*row++) = (*data_ptr++) >> 8; // Convert 16u -> 8u
507  }
508 
509  // Save as external image file??
510  if (!m_path_for_external_images.empty())
511  {
512  const string filName =
514  format(
515  "_CONF_%f.%s",
516  (double)timestampTotime_t(obs.timestamp),
517  m_external_images_format.c_str());
519  m_path_for_external_images + string("/") + filName,
521  obs.confidenceImage.setExternalStorage(filName);
522  }
523  }
524  }
525  break;
526 
527  default:
528  break;
529  }
530  }
531 
532  // Save the observation to the user's object:
533  m_out_obs.swap(obs);
534 
535  there_is_obs = true;
536 
537  // preview in real-time?
538  if (m_preview_window)
539  {
540  if (m_out_obs.hasRangeImage)
541  {
542  static int decim = 0;
543  if (++decim > 10)
544  {
545  decim = 0;
546  if (!m_win_range)
547  {
548  m_win_range =
549  mrpt::gui::CDisplayWindow::Create("Preview RANGE");
550  m_win_range->setPos(5, 5);
551  }
552 
553  mrpt::img::CImage img;
554  // Normalize the image
555  math::CMatrixFloat range2D = m_out_obs.rangeImage;
556  range2D *= 1.0 / m_maxRange;
557  img.setFromMatrix(range2D);
558  m_win_range->showImage(img);
559  }
560  }
561  if (m_out_obs.hasIntensityImage)
562  {
563  static int decim = 0;
564  if (++decim > 10)
565  {
566  decim = 0;
567  if (!m_win_int)
568  {
569  m_win_int =
570  mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
571  m_win_int->setPos(300, 5);
572  }
573  m_win_int->showImage(m_out_obs.intensityImage);
574  }
575  }
576  }
577  else
578  {
579  if (m_win_range) m_win_range.clear();
580  if (m_win_int) m_win_int.clear();
581  }
582 
583  return;
584 #else
585  MRPT_UNUSED_PARAM(m_out_obs);
586  MRPT_UNUSED_PARAM(there_is_obs);
587  MRPT_UNUSED_PARAM(hardware_error);
588 #endif
589 }
590 
591 /* -----------------------------------------------------
592  setPathForExternalImages
593 ----------------------------------------------------- */
595  const std::string& directory)
596 {
597  return;
598  // Ignore for now. It seems performance is better grabbing everything
599  // to a single big file than creating hundreds of smaller files per
600  // second...
601 
602  if (!mrpt::system::createDirectory(directory))
603  {
605  "Error: Cannot create the directory for externally saved images: "
606  "%s",
607  directory.c_str());
608  }
609  m_path_for_external_images = directory;
610 }
uint32_t nrows
Definition: TCamera.h:40
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
size_t m_rows
Size of camera images, set on open()
static char ALawCompressTable[128]
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void scaleToResolution(unsigned int new_ncols, unsigned int new_nrows)
Rescale all the parameters for a new camera resolution (it raises an exception if the aspect ratio is...
Definition: TCamera.cpp:203
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
std::string m_sensorLabel
See CGenericSensor.
A class for grabing "range images" from a MESA imaging SwissRanger 3D cameras (SR-2, SR-3000, SR-4k).
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.
bool table_16u_to_8u_init
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
void setExternalStorage(const std::string &fileName) noexcept
By using this method the image is marked as referenced to an external file, which will be loaded only...
Definition: CImage.cpp:1571
STL namespace.
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:329
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)...
unsigned char LinearToALawSample(uint16_t sample)
bool m_save_3d
Save the 3D point cloud (default: true)
void loadFromConfigFile(const std::string &section, const mrpt::config::CConfigFileBase &cfg)
Load all the params from a config source, in the format used in saveToConfigFile(), that is:
Definition: TCamera.cpp:165
bool m_save_confidence
Save the estimated confidence 2D image (default: false)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section) override
See the class documentation at the top for expected parameters.
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig()
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:50
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::gui::CDisplayWindow::Ptr m_win_range
This class allows loading and storing values and vectors of different types from a configuration text...
void resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
Definition: CImage.cpp:247
bool getMesaLibVersion(std::string &out_version) const
Get the version of the MESA library.
constexpr double DEG2RAD(const double x)
Degrees to radians.
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
double timestampTotime_t(const mrpt::system::TTimeStamp t) noexcept
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.h:105
bool m_save_intensity_img
Save the 2D intensity image (default: true)
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.
uint64_t read_uint64_t(const std::string &section, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
bool m_open_from_usb
true: USB, false: ETH
bool hasRangeImage
true means the field rangeImage contains valid data
void setFromMatrix(const MAT &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: img/CImage.h:844
static std::string & trim(std::string &s)
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)...
double m_maxRange
Max range, as deducted from the camera frequency.
bool m_save_range_img
Save the 2D range image (default: true)
mrpt::img::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
bool hasPoints3D
true means the field points3D contains valid data.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
bool m_preview_window
Show preview window while grabbing.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
~CSwissRanger3DCamera() override
Default ctor.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
uint8_t table_16u_to_8u[0x10000]
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
bool isOpen() const
whether the camera is open and comms work ok.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
unsigned int m_external_images_jpeg_quality
For JPEG images, the quality (default=95%).
void do_init_table_16u_to_8u()
bool hasIntensityImage
true means the field intensityImage contains valid data
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
unsigned int m_cam_serial_num
Serial number of the camera, set on open()
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
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().
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
std::string m_external_images_format
The extension ("jpg","gif","png",...) that determines the format of images saved externally.
std::string trim(const std::string &str)
Removes leading and trailing spaces.
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:330
std::string m_path_for_external_images
The path where to save off-rawlog images: empty means save images embedded in the rawlog...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
void * m_cam
opaque handler to SRCAM.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
float maxRange
The maximum range allowed by the device, in meters (e.g.
bool hasConfidenceImage
true means the field confidenceImage contains valid data
mrpt::gui::CDisplayWindow::Ptr m_win_int
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
bool open()
return false on error - Called automatically from initialize(), no need normally for the user to call...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at miƩ mar 25 23:05:41 CET 2020