Debian Codegraph @

libs/hwdrivers/src/CSwissRanger3DCamera.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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 //
12 #include <mrpt/hwdrivers/CSwissRanger3DCamera.h>
13 #include <mrpt/system/filesystem.h>
14 #include <mrpt/system/string_utils.h>
15
16 IMPLEMENTS_GENERIC_SENSOR(CSwissRanger3DCamera, mrpt::hwdrivers)
17
18 #if MRPT_HAS_SWISSRANGE
19 #ifdef _WIN32
20 #define WIN32_LEAN_AND_MEAN
21 #include <windows.h>
22 #endif
23
24 #include <libMesaSR.h>
25
26 #ifdef MRPT_OS_LINUX
27 #include <asm/ioctls.h>
28 #include <linux/sockios.h>
29 #include <stdio.h>
30 #include <sys/select.h>
31 #include <termios.h>
32 #include <unistd.h>
33 #endif
34 #endif
35
36 using namespace mrpt::hwdrivers;
37 using namespace mrpt::system;
38 using namespace std;
39
40 /*-------------------------------------------------------------
41 ctor
42 -------------------------------------------------------------*/
43 CSwissRanger3DCamera::CSwissRanger3DCamera() :
44 m_sensorPoseOnRobot(),
45
46 m_ip_address("192.168.2.14")
47
48 {
49 m_sensorLabel = "3DCAM";
50
51 // Default params: Obtained from a SR4000 with 0.004px avr reprojection
52 // error.
53 m_cameraParams.ncols = 176;
54 m_cameraParams.nrows = 144;
55 m_cameraParams.intrinsicParams(0, 0) = 262.9201; // fx
56 m_cameraParams.intrinsicParams(1, 1) = 262.9218; // fy
57 m_cameraParams.intrinsicParams(0, 2) = 87.99958; // cx
58 m_cameraParams.intrinsicParams(1, 2) = 68.99957; // cy
59 m_cameraParams.dist[0] = -8.258543e-01;
60 m_cameraParams.dist[1] = 6.561022e-01;
61 m_cameraParams.dist[2] = 2.699818e-06;
62 m_cameraParams.dist[3] = -3.263559e-05;
63 m_cameraParams.dist[4] = 0;
64
65 #if !MRPT_HAS_SWISSRANGE
66 THROW_EXCEPTION(
67 "MRPT was compiled without support for SwissRanger 3D cameras! Rebuild "
68 "it.");
69 #endif
70 }
71
72 /*-------------------------------------------------------------
73 dtor
74 -------------------------------------------------------------*/
75 CSwissRanger3DCamera::~CSwissRanger3DCamera() { this->close(); }
76 /*-------------------------------------------------------------
77 Modified A-law compression algorithm for uint16_t -> uint8_t
78 The original method uses signed int16_t. It's being tuned for
79 what we want here...
80 -------------------------------------------------------------*/
81 static char ALawCompressTable[128] = {
82 1, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
83 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
84 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
85 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
86
87 uint8_t table_16u_to_8u[0x10000];
88 bool table_16u_to_8u_init = false;
89
90 unsigned char LinearToALawSample(uint16_t sample)
91 {
92 if (sample >= 0x200)
93 {
94 int exponent = ALawCompressTable[(sample >> 9) & 0x7F];
95 int mantissa = (sample >> (exponent + 3)) & 0x1F;
96 return ((exponent << 5) | mantissa);
97 }
98 else
99 {
100 return (sample >> 4);
101 }
102 }
103
104 void do_init_table_16u_to_8u()
105 {
106 for (unsigned int i = 0; i < 0x10000; i++) table_16u_to_8u[i] = LinearToALawSample(i);
107 }
108
109 /** This method can or cannot be implemented in the derived class, depending on
110 * the need for it.
111 * \exception This method must throw an exception with a descriptive message if
112 * some critical error is found.
113 */
114 void CSwissRanger3DCamera::initialize()
115 {
116 if (!open()) THROW_EXCEPTION("Error opening SwissRanger 3D camera.");
117 }
118
119 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
120 * \exception This method must throw an exception with a descriptive message if
121 * some critical error is found.
122 */
123 void CSwissRanger3DCamera::doProcess()
124 {
125 using namespace mrpt::obs;
126
127 bool thereIs, hwError;
128
129 CObservation3DRangeScan::Ptr newObs = std::make_shared<CObservation3DRangeScan>();
130
131 getNextObservation(*newObs, thereIs, hwError);
132
133 if (hwError)
134 {
135 m_state = ssError;
136 THROW_EXCEPTION("Couldn't communicate to the SwissRanger 3D camera!");
137 }
138
139 if (thereIs)
140 {
141 m_state = ssWorking;
142
143 appendObservation(newObs);
144 }
145 }
146
147 /** Loads specific configuration for the device from a given source of
148 * configuration parameters, for example, an ".ini" file, loading from the
149 * section "[iniSection]" (see config::CConfigFileBase and derived classes)
150 * \exception This method must throw an exception with a descriptive message if
151 * some critical parameter is missing or has an invalid value.
152 */
153 void CSwissRanger3DCamera::loadConfig_sensorSpecific(
154 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
155 {
156 using mrpt::DEG2RAD;
157
158 m_sensorPoseOnRobot.setFromValues(
159 configSource.read_float(iniSection, "pose_x", 0),
160 configSource.read_float(iniSection, "pose_y", 0),
161 configSource.read_float(iniSection, "pose_z", 0),
162 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
163 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
164 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
165
166 m_preview_window = configSource.read_bool(iniSection, "preview_window", m_preview_window);
167
168 m_save_3d = configSource.read_bool(iniSection, "save_3d", m_save_3d);
169 m_save_range_img = configSource.read_bool(iniSection, "save_range_img", m_save_range_img);
170 m_save_intensity_img =
171 configSource.read_bool(iniSection, "save_intensity_img", m_save_intensity_img);
172 m_save_confidence = configSource.read_bool(iniSection, "save_confidence", m_save_confidence);
173
174 m_enable_img_hist_equal =
175 configSource.read_bool(iniSection, "enable_img_hist_equal", m_enable_img_hist_equal);
176 m_enable_median_filter =
177 configSource.read_bool(iniSection, "enable_median_filter", m_enable_median_filter);
178 m_enable_mediancross_filter =
179 configSource.read_bool(iniSection, "enable_mediancross_filter", m_enable_mediancross_filter);
180 m_enable_conv_gray = configSource.read_bool(iniSection, "enable_conv_gray", m_enable_conv_gray);
181 m_enable_denoise_anf =
182 configSource.read_bool(iniSection, "enable_denoise_anf", m_enable_denoise_anf);
183
184 m_open_from_usb = configSource.read_bool(iniSection, "open_from_usb", m_open_from_usb);
185 m_usb_serial = configSource.read_uint64_t(iniSection, "usb_serial", m_usb_serial);
186 m_ip_address = configSource.read_string(iniSection, "ip_address", m_ip_address);
187
188 m_external_images_format = mrpt::system::trim(
189 configSource.read_string(iniSection, "external_images_format", m_external_images_format));
190 m_external_images_jpeg_quality = configSource.read_int(
191 iniSection, "external_images_jpeg_quality", m_external_images_jpeg_quality);
192
193 try
194 {
195 m_cameraParams.loadFromConfigFile(iniSection, configSource);
196 }
197 catch (std::exception&)
198 {
199 // If there's some missing field, just keep the default values.
200 }
201 }
202
203 bool CSwissRanger3DCamera::getMesaLibVersion([[maybe_unused]] std::string& out_version) const
204 {
205 #if MRPT_HAS_SWISSRANGE
206 unsigned short version[4];
207 if (0 != SR_GetVersion(version)) return false;
208
209 out_version = mrpt::format("%d.%d.%d.%d", version[3], version[2], version[1], version[0]);
210 return true;
211 #else
212 return false;
213 #endif
214 }
215
216 bool CSwissRanger3DCamera::isOpen() const { return m_cam != nullptr; }
217 bool CSwissRanger3DCamera::open()
218 {
219 #if MRPT_HAS_SWISSRANGE
220 if (isOpen()) close();
221
222 SRCAM cam;
223 if (m_open_from_usb)
224 {
225 if (SR_OpenUSB(&cam, this->m_usb_serial) <= 0) return false;
226 }
227 else
228 {
229 if (SR_OpenETH(&cam, this->m_ip_address.c_str()) <= 0) return false;
230 }
231 m_cam = cam;
232
233 // Initialization code:
234 m_rows = SR_GetRows(cam);
235 m_cols = SR_GetCols(cam);
236
237 m_cam_serial_num = SR_ReadSerial(cam);
238
239 // Deduce max range from frequency:
240 const ModulationFrq fr = SR_GetModulationFrequency(cam);
241 switch (fr)
242 {
243 case MF_40MHz:
244 m_maxRange = 3.75;
245 break;
246 case MF_30MHz:
247 m_maxRange = 5;
248 break;
249 case MF_21MHz:
250 m_maxRange = 7.14;
251 break;
252 case MF_20MHz:
253 m_maxRange = 7.5;
254 break;
255 case MF_19MHz:
256 m_maxRange = 7.89;
257 break;
258 case MF_60MHz:
259 m_maxRange = 2.5;
260 break;
261 case MF_15MHz:
262 m_maxRange = 10;
263 break;
264 case MF_10MHz:
265 m_maxRange = 15;
266 break;
267 case MF_29MHz:
268 m_maxRange = 5.17;
269 break;
270 case MF_31MHz:
271 m_maxRange = 4.84;
272 break;
273 case MF_14_5MHz:
274 m_maxRange = 10.34;
275 break;
276 case MF_15_5MHz:
277 m_maxRange = 9.68;
278 break;
279
280 default:
281 m_maxRange = 5.0;
282 break;
283 }
284
285 SR_SetTimeout(cam, 1000 /* ms */);
286
287 internal_resendParamsToCamera();
288
289 return true;
290 #else
291 return false;
292 #endif
293 }
294
295 void CSwissRanger3DCamera::close()
296 {
297 #if MRPT_HAS_SWISSRANGE
298 if (m_cam) SR_Close(SRCAM(m_cam));
299 m_cam = nullptr;
300 #endif
301 }
302
303 void CSwissRanger3DCamera::internal_resendParamsToCamera() const
304 {
305 #if MRPT_HAS_SWISSRANGE
306 if (!isOpen()) return;
307
308 SR_SetMode(
309 SRCAM(m_cam),
310 AM_COR_FIX_PTRN | // turns on fix pattern noise correction <b>this
311 // should always be enabled for good distance
312 // measurement</b>
313 (m_enable_median_filter ? AM_MEDIAN : 0) | (m_enable_conv_gray ? AM_CONV_GRAY : 0) |
314 (m_enable_denoise_anf ? AM_DENOISE_ANF : 0) | (m_save_confidence ? AM_CONF_MAP : 0) |
315 (m_enable_mediancross_filter ? AM_MEDIANCROSS : 0));
316 #endif
317 }
318
319 /** The main data retrieving function, to be called after calling loadConfig()
320 * and initialize().
321 * \param out_obs The output retrieved observation (only if
322 * there_is_obs=true).
323 * \param there_is_obs If set to false, there was no new observation.
324 * \param hardware_error True on hardware/comms error.
325 *
326 * \sa doProcess
327 */
328 void CSwissRanger3DCamera::getNextObservation(
329 [[maybe_unused]] mrpt::obs::CObservation3DRangeScan& m_out_obs,
330 [[maybe_unused]] bool& there_is_obs,
331 [[maybe_unused]] bool& hardware_error)
332 {
333 there_is_obs = false;
334 hardware_error = false;
335 #if MRPT_HAS_SWISSRANGE
336
337 int bytesRx = SR_Acquire(SRCAM(m_cam));
338 if (!bytesRx)
339 {
340 cerr << "[CSwissRanger3DCamera] Zero bytes read from the camera." << endl;
341 hardware_error = true;
342 return;
343 }
344
345 // Extract images:
346 ImgEntry* imgEntryArray;
347 const int nImgs = SR_GetImageList(SRCAM(m_cam), &imgEntryArray);
348
349 if (!nImgs)
350 {
351 cerr << "[CSwissRanger3DCamera] Error: no images in image list." << endl;
352 hardware_error = true;
353 return;
354 }
355
356 // Initialize the output observation:
357 obs::CObservation3DRangeScan obs;
358 obs.sensorLabel = m_sensorLabel;
359 obs.sensorPose = m_sensorPoseOnRobot;
360 obs.maxRange = m_maxRange;
361 obs.stdError = 0.01f;
362
363 // Process each of the images:
364 for (int i = 0; i < nImgs; i++)
365 {
366 const ImgEntry* img = imgEntryArray + i;
367 switch (img->imgType)
368 {
369 // Ranges:
370 case ImgEntry::IT_DISTANCE:
371 {
372 if (this->m_save_range_img)
373 {
374 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
375 obs.hasRangeImage = true;
376 obs.range_is_depth = false;
377
378 // Convert data from uint16_t to float ranges:
379 // (0x0000, 0xFFFF) -> (0m, 5m)
380 const float K = obs.maxRange / 0xFFFF;
381 obs.rangeImage.setSize(img->height, img->width);
382
383 const uint16_t* data_ptr = reinterpret_cast<const uint16_t*>(img->data);
384
385 obs.rangeUnits = K;
386 for (size_t y = 0; y < img->height; y++)
387 for (size_t x = 0; x < img->width; x++) obs.rangeImage(y, x) = (*data_ptr++);
388 }
389
390 if (this->m_save_3d)
391 {
392 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
393 obs.hasPoints3D = true;
394
395 const size_t N = img->height * img->width;
396 obs.points3D_x.resize(N);
397 obs.points3D_y.resize(N);
398 obs.points3D_z.resize(N);
399
400 // Swap XYZ order, so:
401 // SwissRange -> MRPT
402 // Z X
403 // X Y
404 // Y Z
405 SR_CoordTrfFlt(
406 SRCAM(m_cam),
407 &obs.points3D_y[0], // X
408 &obs.points3D_z[0], // Y
409 &obs.points3D_x[0], // Z
410 sizeof(float), sizeof(float), sizeof(float));
411 }
412 }
413 break;
414
415 // Intensity:
416 case ImgEntry::IT_AMPLITUDE:
417 {
418 if (this->m_save_intensity_img)
419 {
420 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
421 obs.hasIntensityImage = true;
422
423 // Make sure the camera params are there:
424 m_cameraParams.scaleToResolution(img->width, img->height);
425 obs.cameraParams = m_cameraParams;
426
427 // make sure the modified A-law Look Up Table is up-to-date:
428 if (!table_16u_to_8u_init)
429 {
430 table_16u_to_8u_init = true;
431 do_init_table_16u_to_8u();
432 }
433
434 obs.intensityImage.resize(img->width, img->height, 1, true);
435
436 const uint16_t* data_ptr = reinterpret_cast<const uint16_t*>(img->data);
437 for (size_t y = 0; y < img->height; y++)
438 {
439 uint8_t* row = obs.intensityImage(0, y, 0);
440 for (size_t x = 0; x < img->width; x++)
441 // Convert 16u -> 8u
442 (*row++) = table_16u_to_8u[*data_ptr++];
443 }
444
445 if (m_enable_img_hist_equal) obs.intensityImage.equalizeHistInPlace();
446
447 // Save as external image file??
448 if (!m_path_for_external_images.empty())
449 {
450 const string filName = fileNameStripInvalidChars(trim(m_sensorLabel)) +
451 mrpt::format(
452 "_INT_%f.%s", (double)mrpt::Clock::toDouble(obs.timestamp),
453 m_external_images_format.c_str());
454 obs.intensityImage.saveToFile(
455 m_path_for_external_images + string("/") + filName, m_external_images_jpeg_quality);
456 obs.intensityImage.setExternalStorage(filName);
457 }
458 }
459 }
460 break;
461
462 // Confidence:
463 case ImgEntry::IT_CONF_MAP:
464 {
465 if (this->m_save_confidence)
466 {
467 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
468 obs.hasConfidenceImage = true;
469
470 obs.confidenceImage.resize(img->width, img->height, 1, true);
471
472 const uint16_t* data_ptr = reinterpret_cast<const uint16_t*>(img->data);
473 for (size_t y = 0; y < img->height; y++)
474 {
475 uint8_t* row = obs.confidenceImage(0, y, 0);
476 for (size_t x = 0; x < img->width; x++)
477 (*row++) = (*data_ptr++) >> 8; // Convert 16u -> 8u
478 }
479
480 // Save as external image file??
481 if (!m_path_for_external_images.empty())
482 {
483 const string filName = fileNameStripInvalidChars(trim(m_sensorLabel)) +
484 mrpt::format(
485 "_CONF_%f.%s", (double)mrpt::Clock::toDouble(obs.timestamp),
486 m_external_images_format.c_str());
487 obs.confidenceImage.saveToFile(
488 m_path_for_external_images + string("/") + filName, m_external_images_jpeg_quality);
489 obs.confidenceImage.setExternalStorage(filName);
490 }
491 }
492 }
493 break;
494
495 default:
496 break;
497 }
498 }
499
500 // Save the observation to the user's object:
501 m_out_obs.swap(obs);
502
503 there_is_obs = true;
504
505 // preview in real-time?
506 if (m_preview_window)
507 {
508 if (m_out_obs.hasRangeImage)
509 {
510 static int decim = 0;
511 if (++decim > 10)
512 {
513 decim = 0;
514 if (!m_win_range)
515 {
516 m_win_range = mrpt::gui::CDisplayWindow::Create("Preview RANGE");
517 m_win_range->setPos(5, 5);
518 }
519
520 mrpt::img::CImage img;
521 // Normalize the image
522 math::CMatrixFloat range2D = m_out_obs.rangeImage;
523 range2D *= 1.0 / m_maxRange;
524 img.setFromMatrix(range2D);
525 m_win_range->showImage(img);
526 }
527 }
528 if (m_out_obs.hasIntensityImage)
529 {
530 static int decim = 0;
531 if (++decim > 10)
532 {
533 decim = 0;
534 if (!m_win_int)
535 {
536 m_win_int = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
537 m_win_int->setPos(300, 5);
538 }
539 m_win_int->showImage(m_out_obs.intensityImage);
540 }
541 }
542 }
543 else
544 {
545 if (m_win_range) m_win_range.clear();
546 if (m_win_int) m_win_int.clear();
547 }
548
549 return;
550 #endif
551 }
552
553 /* -----------------------------------------------------
554 setPathForExternalImages
555 ----------------------------------------------------- */
556 void CSwissRanger3DCamera::setPathForExternalImages([[maybe_unused]] const std::string& directory)
557 {
558 return;
559 // Ignore for now. It seems performance is better grabbing everything
560 // to a single big file than creating hundreds of smaller files per
561 // second...
562
563 if (!mrpt::system::createDirectory(directory))
564 {
565 THROW_EXCEPTION_FMT(
566 "Error: Cannot create the directory for externally saved images: "
567 "%s",
568 directory.c_str());
569 }
570 m_path_for_external_images = directory;
571 }