/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // #include #include #include IMPLEMENTS_GENERIC_SENSOR(CSwissRanger3DCamera, mrpt::hwdrivers) #if MRPT_HAS_SWISSRANGE #ifdef _WIN32 #define WIN32_LEAN_AND_MEAN #include #endif #include #ifdef MRPT_OS_LINUX #include #include #include #include #include #include #endif #endif using namespace mrpt::hwdrivers; using namespace mrpt::system; using namespace std; /*------------------------------------------------------------- ctor -------------------------------------------------------------*/ CSwissRanger3DCamera::CSwissRanger3DCamera() : m_sensorPoseOnRobot(), m_ip_address("192.168.2.14") { m_sensorLabel = "3DCAM"; // Default params: Obtained from a SR4000 with 0.004px avr reprojection // error. m_cameraParams.ncols = 176; m_cameraParams.nrows = 144; m_cameraParams.intrinsicParams(0, 0) = 262.9201; // fx m_cameraParams.intrinsicParams(1, 1) = 262.9218; // fy m_cameraParams.intrinsicParams(0, 2) = 87.99958; // cx m_cameraParams.intrinsicParams(1, 2) = 68.99957; // cy m_cameraParams.dist[0] = -8.258543e-01; m_cameraParams.dist[1] = 6.561022e-01; m_cameraParams.dist[2] = 2.699818e-06; m_cameraParams.dist[3] = -3.263559e-05; m_cameraParams.dist[4] = 0; #if !MRPT_HAS_SWISSRANGE THROW_EXCEPTION( "MRPT was compiled without support for SwissRanger 3D cameras! Rebuild " "it."); #endif } /*------------------------------------------------------------- dtor -------------------------------------------------------------*/ CSwissRanger3DCamera::~CSwissRanger3DCamera() { this->close(); } /*------------------------------------------------------------- Modified A-law compression algorithm for uint16_t -> uint8_t The original method uses signed int16_t. It's being tuned for what we want here... -------------------------------------------------------------*/ static char ALawCompressTable[128] = { 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, 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, 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, 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}; uint8_t table_16u_to_8u[0x10000]; bool table_16u_to_8u_init = false; unsigned char LinearToALawSample(uint16_t sample) { if (sample >= 0x200) { int exponent = ALawCompressTable[(sample >> 9) & 0x7F]; int mantissa = (sample >> (exponent + 3)) & 0x1F; return ((exponent << 5) | mantissa); } else { return (sample >> 4); } } void do_init_table_16u_to_8u() { for (unsigned int i = 0; i < 0x10000; i++) table_16u_to_8u[i] = LinearToALawSample(i); } /** This method can or cannot be implemented in the derived class, depending on * the need for it. * \exception This method must throw an exception with a descriptive message if * some critical error is found. */ void CSwissRanger3DCamera::initialize() { if (!open()) THROW_EXCEPTION("Error opening SwissRanger 3D camera."); } /** This method will be invoked at a minimum rate of "process_rate" (Hz) * \exception This method must throw an exception with a descriptive message if * some critical error is found. */ void CSwissRanger3DCamera::doProcess() { using namespace mrpt::obs; bool thereIs, hwError; CObservation3DRangeScan::Ptr newObs = std::make_shared(); getNextObservation(*newObs, thereIs, hwError); if (hwError) { m_state = ssError; THROW_EXCEPTION("Couldn't communicate to the SwissRanger 3D camera!"); } if (thereIs) { m_state = ssWorking; appendObservation(newObs); } } /** 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) * \exception This method must throw an exception with a descriptive message if * some critical parameter is missing or has an invalid value. */ void CSwissRanger3DCamera::loadConfig_sensorSpecific( const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection) { using mrpt::DEG2RAD; m_sensorPoseOnRobot.setFromValues( configSource.read_float(iniSection, "pose_x", 0), configSource.read_float(iniSection, "pose_y", 0), configSource.read_float(iniSection, "pose_z", 0), DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)), DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)), DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0))); m_preview_window = configSource.read_bool(iniSection, "preview_window", m_preview_window); m_save_3d = configSource.read_bool(iniSection, "save_3d", m_save_3d); m_save_range_img = configSource.read_bool(iniSection, "save_range_img", m_save_range_img); m_save_intensity_img = configSource.read_bool(iniSection, "save_intensity_img", m_save_intensity_img); m_save_confidence = configSource.read_bool(iniSection, "save_confidence", m_save_confidence); m_enable_img_hist_equal = configSource.read_bool(iniSection, "enable_img_hist_equal", m_enable_img_hist_equal); m_enable_median_filter = configSource.read_bool(iniSection, "enable_median_filter", m_enable_median_filter); m_enable_mediancross_filter = configSource.read_bool(iniSection, "enable_mediancross_filter", m_enable_mediancross_filter); m_enable_conv_gray = configSource.read_bool(iniSection, "enable_conv_gray", m_enable_conv_gray); m_enable_denoise_anf = configSource.read_bool(iniSection, "enable_denoise_anf", m_enable_denoise_anf); m_open_from_usb = configSource.read_bool(iniSection, "open_from_usb", m_open_from_usb); m_usb_serial = configSource.read_uint64_t(iniSection, "usb_serial", m_usb_serial); m_ip_address = configSource.read_string(iniSection, "ip_address", m_ip_address); m_external_images_format = mrpt::system::trim( configSource.read_string(iniSection, "external_images_format", m_external_images_format)); m_external_images_jpeg_quality = configSource.read_int( iniSection, "external_images_jpeg_quality", m_external_images_jpeg_quality); try { m_cameraParams.loadFromConfigFile(iniSection, configSource); } catch (std::exception&) { // If there's some missing field, just keep the default values. } } bool CSwissRanger3DCamera::getMesaLibVersion([[maybe_unused]] std::string& out_version) const { #if MRPT_HAS_SWISSRANGE unsigned short version[4]; if (0 != SR_GetVersion(version)) return false; out_version = mrpt::format("%d.%d.%d.%d", version[3], version[2], version[1], version[0]); return true; #else return false; #endif } bool CSwissRanger3DCamera::isOpen() const { return m_cam != nullptr; } bool CSwissRanger3DCamera::open() { #if MRPT_HAS_SWISSRANGE if (isOpen()) close(); SRCAM cam; if (m_open_from_usb) { if (SR_OpenUSB(&cam, this->m_usb_serial) <= 0) return false; } else { if (SR_OpenETH(&cam, this->m_ip_address.c_str()) <= 0) return false; } m_cam = cam; // Initialization code: m_rows = SR_GetRows(cam); m_cols = SR_GetCols(cam); m_cam_serial_num = SR_ReadSerial(cam); // Deduce max range from frequency: const ModulationFrq fr = SR_GetModulationFrequency(cam); switch (fr) { case MF_40MHz: m_maxRange = 3.75; break; case MF_30MHz: m_maxRange = 5; break; case MF_21MHz: m_maxRange = 7.14; break; case MF_20MHz: m_maxRange = 7.5; break; case MF_19MHz: m_maxRange = 7.89; break; case MF_60MHz: m_maxRange = 2.5; break; case MF_15MHz: m_maxRange = 10; break; case MF_10MHz: m_maxRange = 15; break; case MF_29MHz: m_maxRange = 5.17; break; case MF_31MHz: m_maxRange = 4.84; break; case MF_14_5MHz: m_maxRange = 10.34; break; case MF_15_5MHz: m_maxRange = 9.68; break; default: m_maxRange = 5.0; break; } SR_SetTimeout(cam, 1000 /* ms */); internal_resendParamsToCamera(); return true; #else return false; #endif } void CSwissRanger3DCamera::close() { #if MRPT_HAS_SWISSRANGE if (m_cam) SR_Close(SRCAM(m_cam)); m_cam = nullptr; #endif } void CSwissRanger3DCamera::internal_resendParamsToCamera() const { #if MRPT_HAS_SWISSRANGE if (!isOpen()) return; SR_SetMode( SRCAM(m_cam), AM_COR_FIX_PTRN | // turns on fix pattern noise correction this // should always be enabled for good distance // measurement (m_enable_median_filter ? AM_MEDIAN : 0) | (m_enable_conv_gray ? AM_CONV_GRAY : 0) | (m_enable_denoise_anf ? AM_DENOISE_ANF : 0) | (m_save_confidence ? AM_CONF_MAP : 0) | (m_enable_mediancross_filter ? AM_MEDIANCROSS : 0)); #endif } /** The main data retrieving function, to be called after calling loadConfig() * and initialize(). * \param out_obs The output retrieved observation (only if * there_is_obs=true). * \param there_is_obs If set to false, there was no new observation. * \param hardware_error True on hardware/comms error. * * \sa doProcess */ void CSwissRanger3DCamera::getNextObservation( [[maybe_unused]] mrpt::obs::CObservation3DRangeScan& m_out_obs, [[maybe_unused]] bool& there_is_obs, [[maybe_unused]] bool& hardware_error) { there_is_obs = false; hardware_error = false; #if MRPT_HAS_SWISSRANGE int bytesRx = SR_Acquire(SRCAM(m_cam)); if (!bytesRx) { cerr << "[CSwissRanger3DCamera] Zero bytes read from the camera." << endl; hardware_error = true; return; } // Extract images: ImgEntry* imgEntryArray; const int nImgs = SR_GetImageList(SRCAM(m_cam), &imgEntryArray); if (!nImgs) { cerr << "[CSwissRanger3DCamera] Error: no images in image list." << endl; hardware_error = true; return; } // Initialize the output observation: obs::CObservation3DRangeScan obs; obs.sensorLabel = m_sensorLabel; obs.sensorPose = m_sensorPoseOnRobot; obs.maxRange = m_maxRange; obs.stdError = 0.01f; // Process each of the images: for (int i = 0; i < nImgs; i++) { const ImgEntry* img = imgEntryArray + i; switch (img->imgType) { // Ranges: case ImgEntry::IT_DISTANCE: { if (this->m_save_range_img) { ASSERT_(img->dataType == ImgEntry::DT_USHORT) obs.hasRangeImage = true; obs.range_is_depth = false; // Convert data from uint16_t to float ranges: // (0x0000, 0xFFFF) -> (0m, 5m) const float K = obs.maxRange / 0xFFFF; obs.rangeImage.setSize(img->height, img->width); const uint16_t* data_ptr = reinterpret_cast(img->data); obs.rangeUnits = K; for (size_t y = 0; y < img->height; y++) for (size_t x = 0; x < img->width; x++) obs.rangeImage(y, x) = (*data_ptr++); } if (this->m_save_3d) { ASSERT_(img->dataType == ImgEntry::DT_USHORT) obs.hasPoints3D = true; const size_t N = img->height * img->width; obs.points3D_x.resize(N); obs.points3D_y.resize(N); obs.points3D_z.resize(N); // Swap XYZ order, so: // SwissRange -> MRPT // Z X // X Y // Y Z SR_CoordTrfFlt( SRCAM(m_cam), &obs.points3D_y[0], // X &obs.points3D_z[0], // Y &obs.points3D_x[0], // Z sizeof(float), sizeof(float), sizeof(float)); } } break; // Intensity: case ImgEntry::IT_AMPLITUDE: { if (this->m_save_intensity_img) { ASSERT_(img->dataType == ImgEntry::DT_USHORT) obs.hasIntensityImage = true; // Make sure the camera params are there: m_cameraParams.scaleToResolution(img->width, img->height); obs.cameraParams = m_cameraParams; // make sure the modified A-law Look Up Table is up-to-date: if (!table_16u_to_8u_init) { table_16u_to_8u_init = true; do_init_table_16u_to_8u(); } obs.intensityImage.resize(img->width, img->height, 1, true); const uint16_t* data_ptr = reinterpret_cast(img->data); for (size_t y = 0; y < img->height; y++) { uint8_t* row = obs.intensityImage(0, y, 0); for (size_t x = 0; x < img->width; x++) // Convert 16u -> 8u (*row++) = table_16u_to_8u[*data_ptr++]; } if (m_enable_img_hist_equal) obs.intensityImage.equalizeHistInPlace(); // Save as external image file?? if (!m_path_for_external_images.empty()) { const string filName = fileNameStripInvalidChars(trim(m_sensorLabel)) + mrpt::format( "_INT_%f.%s", (double)mrpt::Clock::toDouble(obs.timestamp), m_external_images_format.c_str()); obs.intensityImage.saveToFile( m_path_for_external_images + string("/") + filName, m_external_images_jpeg_quality); obs.intensityImage.setExternalStorage(filName); } } } break; // Confidence: case ImgEntry::IT_CONF_MAP: { if (this->m_save_confidence) { ASSERT_(img->dataType == ImgEntry::DT_USHORT) obs.hasConfidenceImage = true; obs.confidenceImage.resize(img->width, img->height, 1, true); const uint16_t* data_ptr = reinterpret_cast(img->data); for (size_t y = 0; y < img->height; y++) { uint8_t* row = obs.confidenceImage(0, y, 0); for (size_t x = 0; x < img->width; x++) (*row++) = (*data_ptr++) >> 8; // Convert 16u -> 8u } // Save as external image file?? if (!m_path_for_external_images.empty()) { const string filName = fileNameStripInvalidChars(trim(m_sensorLabel)) + mrpt::format( "_CONF_%f.%s", (double)mrpt::Clock::toDouble(obs.timestamp), m_external_images_format.c_str()); obs.confidenceImage.saveToFile( m_path_for_external_images + string("/") + filName, m_external_images_jpeg_quality); obs.confidenceImage.setExternalStorage(filName); } } } break; default: break; } } // Save the observation to the user's object: m_out_obs.swap(obs); there_is_obs = true; // preview in real-time? if (m_preview_window) { if (m_out_obs.hasRangeImage) { static int decim = 0; if (++decim > 10) { decim = 0; if (!m_win_range) { m_win_range = mrpt::gui::CDisplayWindow::Create("Preview RANGE"); m_win_range->setPos(5, 5); } mrpt::img::CImage img; // Normalize the image math::CMatrixFloat range2D = m_out_obs.rangeImage; range2D *= 1.0 / m_maxRange; img.setFromMatrix(range2D); m_win_range->showImage(img); } } if (m_out_obs.hasIntensityImage) { static int decim = 0; if (++decim > 10) { decim = 0; if (!m_win_int) { m_win_int = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY"); m_win_int->setPos(300, 5); } m_win_int->showImage(m_out_obs.intensityImage); } } } else { if (m_win_range) m_win_range.clear(); if (m_win_int) m_win_int.clear(); } return; #endif } /* ----------------------------------------------------- setPathForExternalImages ----------------------------------------------------- */ void CSwissRanger3DCamera::setPathForExternalImages([[maybe_unused]] const std::string& directory) { return; // Ignore for now. It seems performance is better grabbing everything // to a single big file than creating hundreds of smaller files per // second... if (!mrpt::system::createDirectory(directory)) { THROW_EXCEPTION_FMT( "Error: Cannot create the directory for externally saved images: " "%s", directory.c_str()); } m_path_for_external_images = directory; }