9#ifndef CObservation3DRangeScan_H
10#define CObservation3DRangeScan_H
33 template <
class POINTMAP>
179 template <class POINTMAP>
180 inline
void project3DPointsFromDepthImageInto(
181 POINTMAP & dest_pointcloud,
182 const
bool takeIntoAccountSensorPoseOnRobot,
183 const
mrpt::poses::CPose3D *robotPoseInTheWorld=NULL,
184 const
bool PROJ3D_USE_LUT=true)
186 detail::project3DPointsFromDepthImageInto<POINTMAP>(*
this,dest_pointcloud,takeIntoAccountSensorPoseOnRobot,robotPoseInTheWorld,PROJ3D_USE_LUT);
193 this->project3DPointsFromDepthImageInto(*
this,
false,NULL,PROJ3D_USE_LUT);
226 const std::string & sensorLabel,
229 const double oversampling_ratio = 1.2 );
246 points3D_getExternalStorageFileAbsolutePath(tmp);
268 rangeImage_getExternalStorageFileAbsolutePath(tmp);
313 std::map<uint32_t,std::string>::const_iterator it = pixelLabelNames.find(label_idx);
314 if (it==pixelLabelNames.end())
throw std::runtime_error(
"Error: label index has no defined name");
317 void setLabelName(
unsigned int label_idx,
const std::string &name) { pixelLabelNames[label_idx]=name; }
321 std::map<uint32_t,std::string>::const_iterator it;
322 for ( it = pixelLabelNames.begin() ; it != pixelLabelNames.end(); it++ )
323 if ( it->second == name )
329 virtual void setSize(
const int NROWS,
const int NCOLS) =0;
354 BITFIELD_BYTES (BITFIELD_BYTES_)
365 virtual void Print( std::ostream& )
const =0;
369 template <
unsigned int BYTES_REQUIRED_>
373 BYTES_REQUIRED = BYTES_REQUIRED_
388 pixelLabels = TPixelLabelMatrix::Zero(NROWS,NCOLS);
391 pixelLabels(row,col) |=
static_cast<bitmask_t>(1) << label_idx;
395 labels = pixelLabels(row,col);
399 pixelLabels(row,col) &= ~(
static_cast<bitmask_t>(1) << label_idx);
402 pixelLabels(row,col) = 0;
405 return (pixelLabels(row,col) & (
static_cast<bitmask_t>(1) << label_idx)) != 0;
419 pixelLabels.resize(nR,nC);
422 in >> pixelLabels.coeffRef(r,c);
424 in >> pixelLabelNames;
434 out << pixelLabels.coeff(r,c);
436 out << pixelLabelNames;
443 out <<
"Number of rows: " << nR << std::endl;
444 out <<
"Number of cols: " << nC << std::endl;
445 out <<
"Matrix of labels: " << std::endl;
449 out << pixelLabels.coeff(r,c) <<
" ";
455 out <<
"Label indices and names: " << std::endl;
456 std::map<uint32_t,std::string>::const_iterator it;
457 for ( it = pixelLabelNames.begin(); it != pixelLabelNames.end(); it++)
458 out << it->first <<
" " << it->second << std::endl;
510 const double camera_offset = 0.01 );
554 static const int HAS_RGB = 0;
555 static const int HAS_RGBf = 0;
556 static const int HAS_RGBu8 = 0;
569 template <
typename T>
570 inline void getPointXYZ(
const size_t idx, T &x,T &y, T &z)
const {
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
This class is a "CSerializable" wrapper for "CMatrixFloat".
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement,...
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
bool m_points3D_external_stored
If set to true, m_points3D_external_file is valid.
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer.
void points3D_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
TPixelLabelInfoPtr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
std::vector< uint16_t > points3D_idxs_x
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
bool hasConfidenceImage
true means the field confidenceImage contains valid data
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
float maxRange
The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
bool hasIntensityImage
true means the field intensityImage contains valid data
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
bool points3D_isExternallyStored() const
std::string m_points3D_external_file
3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
std::string m_rangeImage_external_file
rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
virtual void load() const MRPT_OVERRIDE
Makes sure all images and other fields which may be externally stored are loaded in memory.
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
bool hasPoints3D
true means the field points3D contains valid data.
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,...
void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path) const
std::vector< float > points3D_x
static double recoverCameraCalibrationParameters(const CObservation3DRangeScan &in_obs, mrpt::utils::TCamera &out_camParams, const double camera_offset=0.01)
A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a ran...
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
CObservation3DRangeScan()
Default constructor.
mrpt::utils::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
std::vector< float > points3D_y
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
std::string points3D_getExternalStorageFileAbsolutePath() const
void convertTo2DScan(mrpt::obs::CObservation2DRangeScan &out_scan2d, const std::string &sensorLabel, const double angle_sup=mrpt::utils::DEG2RAD(5), const double angle_inf=mrpt::utils::DEG2RAD(5), const double oversampling_ratio=1.2)
Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV...
bool hasRangeImage
true means the field rangeImage contains valid data
std::vector< float > points3D_z
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera.
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)
TIntensityChannelID
Enum type for intensityImageChannel.
@ CH_IR
Infrarred (IR) channel.
@ CH_VISIBLE
Grayscale or RGB visible channel of the camera sensor.
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
virtual ~CObservation3DRangeScan()
Destructor.
void rangeImage_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
void rangeImage_setSize(const int HEIGHT, const int WIDTH)
Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the ...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
bool rangeImage_isExternallyStored() const
void rangeImage_forceResetExternalStorage()
Forces marking this observation as non-externally stored - it doesn't anything else apart from reseti...
void getZoneAsObs(CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2)
void points3D_getExternalStorageFileAbsolutePath(std::string &out_path) const
stlplus::smart_ptr< TPixelLabelInfoBase > TPixelLabelInfoPtr
Used in CObservation3DRangeScan::pixelLabels.
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
std::string rangeImage_getExternalStorageFile() const
std::string rangeImage_getExternalStorageFileAbsolutePath() const
std::string points3D_getExternalStorageFile() const
Declares a class that represents any robot's observation.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
A class for storing images as grayscale or RGB bitmaps.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
mrpt::obs::CObservation3DRangeScan & m_obj
void setPointXYZ(const size_t idx, const coords_t x, const coords_t y, const coords_t z)
Set XYZ coordinates of i'th point.
PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj)
Constructor (accept a const ref for convenience)
size_t size() const
Get number of points.
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
float coords_t
The type of each point XYZ coordinates.
void resize(const size_t N)
Set number of points (to uninitialized values)
An adapter to different kinds of point cloud object.
Structure to hold the parameters of a pinhole camera model.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
A helper base class for those PointCloudAdapter<> which do not handle RGB data; it declares needed in...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
void project3DPointsFromDepthImageInto(CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const bool takeIntoAccountSensorPoseOnRobot, const mrpt::poses::CPose3D *robotPoseInTheWorld, const bool PROJ3D_USE_LUT)
This namespace contains representation of robot actions and observations.
double DEG2RAD(const double x)
Degrees to radians.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Look-up-table struct for project3DPointsFromDepthImageInto()
mrpt::utils::TCamera prev_camParams
mrpt::math::CVectorFloat Kys
Virtual interface to all pixel-label information structs.
virtual void getLabels(const int row, const int col, uint8_t &labels)=0
std::map< uint32_t, std::string > TMapLabelID2Name
virtual ~TPixelLabelInfoBase()
virtual void internal_readFromStream(mrpt::utils::CStream &in)=0
virtual void unsetAll(const int row, const int col, uint8_t label_idx)=0
Removes all categories for pixel(row,col)
TMapLabelID2Name pixelLabelNames
The 'semantic' or human-friendly name of the i'th bit in pixelLabels(r,c) can be found in pixelLabelN...
virtual void internal_writeToStream(mrpt::utils::CStream &out) const =0
const uint8_t BITFIELD_BYTES
Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS bits.
virtual void Print(std::ostream &) const =0
void writeToStream(mrpt::utils::CStream &out) const
virtual bool checkLabel(const int row, const int col, uint8_t label_idx) const =0
Checks whether pixel(row,col) has been clasified into category label_idx, which may be in the range 0...
const std::string & getLabelName(unsigned int label_idx) const
int checkLabelNameExistence(const std::string &name) const
Check the existence of a label by returning its associated index.
virtual void setLabel(const int row, const int col, uint8_t label_idx)=0
Mark the pixel(row,col) as classified in the category label_idx, which may be in the range 0 to MAX_N...
friend std::ostream & operator<<(std::ostream &out, const TPixelLabelInfoBase &obj)
std stream interface
virtual void setSize(const int NROWS, const int NCOLS)=0
Resizes the matrix pixelLabels to the given size, setting all bitfields to zero (that is,...
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::utils::CStream &in)
virtual void unsetLabel(const int row, const int col, uint8_t label_idx)=0
For the pixel(row,col), removes its classification into the category label_idx, which may be in the r...
TPixelLabelInfoBase(unsigned int BITFIELD_BYTES_)
void setLabelName(unsigned int label_idx, const std::string &name)
void setLabel(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE
Mark the pixel(row,col) as classified in the category label_idx, which may be in the range 0 to MAX_N...
void unsetAll(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE
Removes all categories for pixel(row,col)
Eigen::Matrix< bitmask_t, Eigen::Dynamic, Eigen::Dynamic > TPixelLabelMatrix
Each pixel may be assigned between 0 and MAX_NUM_LABELS-1 'labels' by setting to 1 the corresponding ...
mrpt::utils::uint_select_by_bytecount< BYTES_REQUIRED >::type bitmask_t
Automatically-determined integer type of the proper size such that all labels fit as one bit (max: 64...
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
bool checkLabel(const int row, const int col, uint8_t label_idx) const MRPT_OVERRIDE
Checks whether pixel(row,col) has been clasified into category label_idx, which may be in the range 0...
TPixelLabelMatrix pixelLabels
void Print(std::ostream &out) const MRPT_OVERRIDE
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
void unsetLabel(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE
For the pixel(row,col), removes its classification into the category label_idx, which may be in the r...
void getLabels(const int row, const int col, uint8_t &labels) MRPT_OVERRIDE
void setSize(const int NROWS, const int NCOLS) MRPT_OVERRIDE
Resizes the matrix pixelLabels to the given size, setting all bitfields to zero (that is,...
static void fill(bimap< enum_t, std::string > &m_map)
mrpt::obs::CObservation3DRangeScan::TIntensityChannelID enum_t
Only specializations of this class are defined for each enum type of interest.
Usage: uint_select_by_bytecount<N>::type var; allows defining var as a unsigned integer with,...