23 #if MRPT_HAS_SWISSRANGE 24 #ifdef MRPT_OS_WINDOWS 25 #define WIN32_LEAN_AND_MEAN 29 #include <libMesaSR.h> 36 #include <linux/sockios.h> 37 #include <asm/ioctls.h> 38 #include <sys/select.h> 46 : m_sensorPoseOnRobot(),
48 m_save_range_img(true),
49 m_save_intensity_img(true),
50 m_save_confidence(false),
52 m_enable_img_hist_equal(false),
53 m_enable_median_filter(true),
54 m_enable_mediancross_filter(false),
55 m_enable_conv_gray(false),
56 m_enable_denoise_anf(true),
58 m_open_from_usb(true),
60 m_ip_address(
"192.168.2.14"),
65 m_preview_window(false)
83 #if !MRPT_HAS_SWISSRANGE 85 "MRPT was compiled without support for SwissRanger 3D cameras! Rebuild " 100 1, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5,
101 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
102 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 7, 7,
103 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
104 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
105 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
115 int mantissa = (sample >> (exponent + 3)) & 0x1F;
116 return ((exponent << 5) | mantissa);
120 return (sample >> 4);
126 for (
unsigned int i = 0; i < 0x10000; i++)
148 bool thereIs, hwError;
151 mrpt::make_aligned_shared<CObservation3DRangeScan>();
182 configSource.
read_float(iniSection,
"pose_x", 0),
183 configSource.
read_float(iniSection,
"pose_y", 0),
184 configSource.
read_float(iniSection,
"pose_z", 0),
222 iniSection,
"external_images_jpeg_quality",
229 catch (std::exception&)
237 #if MRPT_HAS_SWISSRANGE 238 unsigned short version[4];
239 if (0 != SR_GetVersion(version))
return false;
242 format(
"%d.%d.%d.%d", version[3], version[2], version[1], version[0]);
253 #if MRPT_HAS_SWISSRANGE 259 if (SR_OpenUSB(&cam, this->
m_usb_serial) <= 0)
return false;
263 if (SR_OpenETH(&cam, this->
m_ip_address.c_str()) <= 0)
return false;
274 const ModulationFrq fr = SR_GetModulationFrequency(cam);
319 SR_SetTimeout(cam, 1000 );
331 #if MRPT_HAS_SWISSRANGE 339 #if MRPT_HAS_SWISSRANGE 366 bool& hardware_error)
368 there_is_obs =
false;
369 hardware_error =
false;
370 #if MRPT_HAS_SWISSRANGE 372 int bytesRx = SR_Acquire(SRCAM(
m_cam));
375 cerr <<
"[CSwissRanger3DCamera] Zero bytes read from the camera." 377 hardware_error =
true;
382 ImgEntry* imgEntryArray;
383 const int nImgs = SR_GetImageList(SRCAM(
m_cam), &imgEntryArray);
387 cerr <<
"[CSwissRanger3DCamera] Error: no images in image list." 389 hardware_error =
true;
401 for (
int i = 0; i < nImgs; i++)
403 const ImgEntry*
img = imgEntryArray + i;
404 switch (
img->imgType)
407 case ImgEntry::IT_DISTANCE:
411 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
417 const float K = obs.
maxRange / 0xFFFF;
423 for (
size_t y = 0;
y <
img->height;
y++)
424 for (
size_t x = 0;
x <
img->width;
x++)
430 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
433 const size_t N =
img->height *
img->width;
448 sizeof(
float),
sizeof(
float),
sizeof(
float));
454 case ImgEntry::IT_AMPLITUDE:
458 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
476 for (
size_t y = 0;
y <
img->height;
y++)
479 for (
size_t x = 0;
x <
img->width;
x++)
490 const string filName =
506 case ImgEntry::IT_CONF_MAP:
510 ASSERT_(img->dataType == ImgEntry::DT_USHORT)
514 img->width,
img->height, 1,
true);
518 for (
size_t y = 0;
y <
img->height;
y++)
521 for (
size_t x = 0;
x <
img->width;
x++)
522 (*
row++) = (*data_ptr++) >> 8;
528 const string filName =
558 static int decim = 0;
565 mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
574 img.setFromMatrix(range2D);
580 static int decim = 0;
587 mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
588 "Preview INTENSITY");
623 "Error: Cannot create the directory for externally saved images: " void equalizeHistInPlace()
Equalize the image histogram, replacing the original image.
bool createDirectory(const std::string &dirName)
Creates a directory.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
size_t m_rows
Size of camera images, set on open()
unsigned __int16 uint16_t
static char ALawCompressTable[128]
void internal_resendParamsToCamera() const
virtual void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string §ion)
See the class documentation at the top for expected parameters.
double DEG2RAD(const double x)
Degrees to radians.
bool m_enable_denoise_anf
bool m_enable_mediancross_filter
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
mrpt::utils::TCamera m_cameraParams
A class for storing images as grayscale or RGB bitmaps.
std::string m_sensorLabel
See CGenericSensor.
~CSwissRanger3DCamera()
Default ctor.
#define THROW_EXCEPTION(msg)
void resize(unsigned int width, unsigned int height, TImageChannels nChannels, bool originTopLeft)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
A class for grabing "range images" from a MESA imaging SwissRanger 3D cameras (SR-2, SR-3000, SR-4k).
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
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...
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.
std::shared_ptr< CObservation3DRangeScan > Ptr
Contains classes for various device interfaces.
bool table_16u_to_8u_init
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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. ...
unsigned char LinearToALawSample(uint16_t sample)
bool m_enable_img_hist_equal
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig()
bool m_save_3d
Save the 3D point cloud (default: true)
bool m_save_confidence
Save the estimated confidence 2D image (default: false)
This class allows loading and storing values and vectors of different types from a configuration text...
std::vector< float > points3D_z
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.
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::poses::CPose3D m_sensorPoseOnRobot
mrpt::gui::CDisplayWindow::Ptr m_win_range
std::vector< float > points3D_y
bool m_enable_median_filter
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
bool getMesaLibVersion(std::string &out_version) const
Get the version of the MESA library.
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
bool m_save_intensity_img
Save the 2D intensity image (default: true)
This namespace contains representation of robot actions and observations.
uint32_t ncols
Camera resolution.
bool m_open_from_usb
true: USB, false: ETH
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
virtual void doProcess()
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
bool hasRangeImage
true means the field rangeImage contains valid data
double m_maxRange
Max range, as deducted from the camera frequency.
bool m_save_range_img
Save the 2D range image (default: true)
bool hasPoints3D
true means the field points3D contains valid data.
GLsizei const GLchar ** string
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:
void appendObservation(const mrpt::utils::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
bool m_preview_window
Show preview window while grabbing.
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...
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
uint8_t table_16u_to_8u[0x10000]
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
bool isOpen() const
whether the camera is open and comms work ok.
unsigned int m_external_images_jpeg_quality
For JPEG images, the quality (default=95%).
GLenum GLenum GLvoid * row
void do_init_table_16u_to_8u()
bool hasIntensityImage
true means the field intensityImage contains valid data
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...
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)...
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...
A matrix of dynamic size.
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)...
uint64_t read_uint64_t(const std::string §ion, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
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.
CSwissRanger3DCamera()
Default ctor.
mrpt::utils::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
unsigned char * get_unsafe(unsigned int col, unsigned int row, unsigned int channel=0) const
Access to pixels without checking boundaries - Use normally the () operator better, which checks the coordinates.
std::string m_path_for_external_images
The path where to save off-rawlog images: empty means save images embedded in the rawlog...
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
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.
double timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
void loadFromConfigFile(const std::string §ion, const mrpt::utils::CConfigFileBase &cfg)
Load all the params from a config source, in the format used in saveToConfigFile(), that is:
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
bool open()
return false on error - Called automatically from initialize(), no need normally for the user to call...