Go to the documentation of this file.
19 #if MRPT_HAS_SWISSRANGE
21 #define WIN32_LEAN_AND_MEAN
25 #include <libMesaSR.h>
32 #include <asm/ioctls.h>
33 #include <linux/sockios.h>
34 #include <sys/select.h>
46 : m_sensorPoseOnRobot(),
48 m_ip_address(
"192.168.2.14")
67 #if !MRPT_HAS_SWISSRANGE
69 "MRPT was compiled without support for SwissRanger 3D cameras! Rebuild "
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};
99 int mantissa = (sample >> (exponent + 3)) & 0x1F;
100 return ((exponent << 5) | mantissa);
104 return (sample >> 4);
110 for (
unsigned int i = 0; i < 0x10000; i++)
132 bool thereIs, hwError;
135 std::make_shared<CObservation3DRangeScan>();
161 const std::string& iniSection)
166 configSource.
read_float(iniSection,
"pose_x", 0),
167 configSource.
read_float(iniSection,
"pose_y", 0),
168 configSource.
read_float(iniSection,
"pose_z", 0),
205 iniSection,
"external_images_jpeg_quality",
212 catch (std::exception&)
219 [maybe_unused]] std::string& out_version)
const
221 #if MRPT_HAS_SWISSRANGE
222 unsigned short version[4];
223 if (0 != SR_GetVersion(version))
return false;
226 format(
"%d.%d.%d.%d", version[3], version[2], version[1], version[0]);
236 #if MRPT_HAS_SWISSRANGE
242 if (SR_OpenUSB(&cam, this->
m_usb_serial) <= 0)
return false;
246 if (SR_OpenETH(&cam, this->
m_ip_address.c_str()) <= 0)
return false;
257 const ModulationFrq fr = SR_GetModulationFrequency(cam);
302 SR_SetTimeout(cam, 1000 );
314 #if MRPT_HAS_SWISSRANGE
322 #if MRPT_HAS_SWISSRANGE
349 [[maybe_unused]]
bool& there_is_obs, [[maybe_unused]]
bool& hardware_error)
351 there_is_obs =
false;
352 hardware_error =
false;
353 #if MRPT_HAS_SWISSRANGE
355 int bytesRx = SR_Acquire(SRCAM(
m_cam));
358 cerr <<
"[CSwissRanger3DCamera] Zero bytes read from the camera."
360 hardware_error =
true;
365 ImgEntry* imgEntryArray;
366 const int nImgs = SR_GetImageList(SRCAM(
m_cam), &imgEntryArray);
370 cerr <<
"[CSwissRanger3DCamera] Error: no images in image list."
372 hardware_error =
true;
384 for (
int i = 0; i < nImgs; i++)
386 const ImgEntry* img = imgEntryArray + i;
387 switch (img->imgType)
390 case ImgEntry::IT_DISTANCE:
394 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
400 const float K = obs.
maxRange / 0xFFFF;
403 const uint16_t* data_ptr =
404 reinterpret_cast<const uint16_t*
>(img->data);
407 for (
size_t y = 0; y < img->height; y++)
408 for (
size_t x = 0; x < img->width; x++)
414 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
417 const size_t N = img->height * img->width;
432 sizeof(
float),
sizeof(
float),
sizeof(
float));
438 case ImgEntry::IT_AMPLITUDE:
442 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
458 const uint16_t* data_ptr =
459 reinterpret_cast<const uint16_t*
>(img->data);
460 for (
size_t y = 0; y < img->height; y++)
463 for (
size_t x = 0; x < img->width; x++)
474 const string filName =
490 case ImgEntry::IT_CONF_MAP:
494 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
498 img->width, img->height, 1,
true);
500 const uint16_t* data_ptr =
501 reinterpret_cast<const uint16_t*
>(img->data);
502 for (
size_t y = 0; y < img->height; y++)
505 for (
size_t x = 0; x < img->width; x++)
506 (*row++) = (*data_ptr++) >> 8;
512 const string filName =
540 if (m_out_obs.hasRangeImage)
542 static int decim = 0;
561 if (m_out_obs.hasIntensityImage)
563 static int decim = 0;
573 m_win_int->showImage(m_out_obs.intensityImage);
591 [maybe_unused]]
const std::string& directory)
601 "Error: Cannot create the directory for externally saved images: "
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,...
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates):
mrpt::gui::CDisplayWindow::Ptr m_win_int
void * m_cam
opaque handler to SRCAM.
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
bool hasPoints3D
true means the field points3D contains valid data.
static std::string & trim(std::string &s)
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).
size_t m_rows
Size of camera images, set on open()
void loadFromConfigFile(const std::string §ion, const mrpt::config::CConfigFileBase &cfg)
Load all the params from a config source, in the format used in saveToConfigFile(),...
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string §ion) override
See the class documentation at the top for expected parameters.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
bool m_enable_mediancross_filter
std::string m_path_for_external_images
The path where to save off-rawlog images: empty means save images embedded in the rawlog.
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...
std::vector< float > points3D_z
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...
mrpt::gui::CDisplayWindow::Ptr m_win_range
static char ALawCompressTable[128]
uint32_t ncols
Camera resolution.
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...
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool getMesaLibVersion(std::string &out_version) const
Get the version of the MESA library.
Contains classes for various device interfaces.
mrpt::poses::CPose3D m_sensorPoseOnRobot
mrpt::img::TCamera m_cameraParams
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
bool m_save_intensity_img
Save the 2D intensity image (default: true)
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
std::vector< float > points3D_y
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
bool table_16u_to_8u_init
This namespace contains representation of robot actions and observations.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char.
bool m_save_confidence
Save the estimated confidence 2D image (default: false)
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
unsigned char LinearToALawSample(uint16_t sample)
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,...
bool m_enable_img_hist_equal
std::string m_external_images_format
The extension ("jpg","gif","png",...) that determines the format of images saved externally.
~CSwissRanger3DCamera() override
Default ctor.
bool m_save_3d
Save the 3D point cloud (default: true)
bool m_enable_median_filter
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig()
This class allows loading and storing values and vectors of different types from a configuration text...
bool m_preview_window
Show preview window while grabbing.
bool isOpen() const
whether the camera is open and comms work ok.
bool hasIntensityImage
true means the field intensityImage contains valid data
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera.
bool m_open_from_usb
true: USB, false: ETH
bool hasRangeImage
true means the field rangeImage contains valid data
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
bool m_save_range_img
Save the 2D range image (default: true)
double m_maxRange
Max range, as deducted from the camera frequency.
mrpt::img::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
std::string m_sensorLabel
See CGenericSensor.
uint64_t read_uint64_t(const std::string §ion, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
CSwissRanger3DCamera()
Default ctor.
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
constexpr double DEG2RAD(const double x)
Degrees to radians
A class for storing images as grayscale or RGB bitmaps.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
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).
unsigned int m_external_images_jpeg_quality
For JPEG images, the quality (default=95%).
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters).
float maxRange
The maximum range allowed by the device, in meters (e.g.
bool hasConfidenceImage
true means the field confidenceImage contains valid data
void do_init_table_16u_to_8u()
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...
A class for grabing "range images" from a MESA imaging SwissRanger 3D cameras (SR-2,...
uint8_t table_16u_to_8u[0x10000]
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().
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...
bool open()
return false on error - Called automatically from initialize(), no need normally for the user to call...
std::string trim(const std::string &str)
Removes leading and trailing spaces.
unsigned int m_cam_serial_num
Serial number of the camera, set on open()
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
bool createDirectory(const std::string &dirName)
Creates a directory.
std::shared_ptr< mrpt::obs ::CObservation3DRangeScan > Ptr
void internal_resendParamsToCamera() const
bool m_enable_denoise_anf
This template class provides the basic functionality for a general 2D any-size, resizable container o...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |