9 #ifndef CObservation3DRangeScan_H 10 #define CObservation3DRangeScan_H 38 T3DPointsProjectionParams() : takeIntoAccountSensorPoseOnRobot(false), robotPoseInTheWorld(NULL), PROJ3D_USE_LUT(true),USE_SSE2(true), MAKE_DENSE(true)
58 template <
class POINTMAP>
206 template <class POINTMAP>
208 detail::project3DPointsFromDepthImageInto<POINTMAP>(*
this,dest_pointcloud,projectParams, filterParams);
211 template <
class POINTMAP>
212 MRPT_DEPRECATED(
"DEPRECATED: Use the other method signature with structured parameters instead.")
214 POINTMAP & dest_pointcloud,
215 const
bool takeIntoAccountSensorPoseOnRobot,
216 const
mrpt::poses::CPose3D *robotPoseInTheWorld=NULL,
217 const
bool PROJ3D_USE_LUT=true,
218 const
mrpt::math::CMatrix * rangeMask_min = NULL
227 detail::project3DPointsFromDepthImageInto<POINTMAP>(*
this,dest_pointcloud,pp,fp);
268 MRPT_DEPRECATED(
"DEPRECATED: Use the other method signature with structured parameters instead.")
269 void convertTo2DScan(
271 const std::string & sensorLabel,
274 const double oversampling_ratio = 1.2,
292 void resizePoints3DVectors(
const size_t nPoints);
299 void points3D_getExternalStorageFileAbsolutePath(std::string &out_path)
const;
302 points3D_getExternalStorageFileAbsolutePath(tmp);
305 void points3D_convertToExternalStorage(
const std::string &fileName,
const std::string &use_this_base_dir );
314 void rangeImage_setSize(
const int HEIGHT,
const int WIDTH);
321 void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path)
const;
324 rangeImage_getExternalStorageFileAbsolutePath(tmp);
327 void rangeImage_convertToExternalStorage(
const std::string &fileName,
const std::string &use_this_base_dir );
370 if (it==pixelLabelNames.end())
throw std::runtime_error(
"Error: label index has no defined name");
373 void setLabelName(
unsigned int label_idx,
const std::string &name) { pixelLabelNames[label_idx]=name; }
378 for ( it = pixelLabelNames.begin() ; it != pixelLabelNames.end(); it++ )
379 if ( it->second == name )
385 virtual void setSize(
const int NROWS,
const int NCOLS) =0;
388 virtual void setLabel(
const int row,
const int col, uint8_t label_idx) =0;
389 virtual void getLabels(
const int row,
const int col, uint8_t &labels ) =0;
392 virtual void unsetLabel(
const int row,
const int col, uint8_t label_idx)=0;
394 virtual void unsetAll(
const int row,
const int col, uint8_t label_idx) =0;
397 virtual bool checkLabel(
const int row,
const int col, uint8_t label_idx)
const =0;
409 BITFIELD_BYTES (BITFIELD_BYTES_)
420 virtual void Print( std::ostream& )
const =0;
424 template <
unsigned int BYTES_REQUIRED_>
428 BYTES_REQUIRED = BYTES_REQUIRED_
443 pixelLabels = TPixelLabelMatrix::Zero(NROWS,NCOLS);
446 pixelLabels(row,col) |=
static_cast<bitmask_t
>(1) << label_idx;
450 labels = pixelLabels(row,col);
454 pixelLabels(row,col) &= ~(
static_cast<bitmask_t
>(1) << label_idx);
457 pixelLabels(row,col) = 0;
460 return (pixelLabels(row,col) & (static_cast<bitmask_t>(1) << label_idx)) != 0;
474 pixelLabels.resize(nR,nC);
475 for (uint32_t c=0;c<nC;c++)
476 for (uint32_t r=0;r<nR;r++)
477 in >> pixelLabels.coeffRef(r,c);
479 in >> pixelLabelNames;
484 const uint32_t nR=
static_cast<uint32_t
>(pixelLabels.rows());
485 const uint32_t nC=
static_cast<uint32_t
>(pixelLabels.cols());
487 for (uint32_t c=0;c<nC;c++)
488 for (uint32_t r=0;r<nR;r++)
489 out << pixelLabels.coeff(r,c);
491 out << pixelLabelNames;
496 const uint32_t nR=
static_cast<uint32_t
>(pixelLabels.rows());
497 const uint32_t nC=
static_cast<uint32_t
>(pixelLabels.cols());
498 out <<
"Number of rows: " << nR << std::endl;
499 out <<
"Number of cols: " << nC << std::endl;
500 out <<
"Matrix of labels: " << std::endl;
501 for (uint32_t c=0;c<nC;c++)
503 for (uint32_t r=0;r<nR;r++)
504 out << pixelLabels.coeff(r,c) <<
" ";
510 out <<
"Label indices and names: " << std::endl;
512 for ( it = pixelLabelNames.begin(); it != pixelLabelNames.end(); it++)
513 out << it->first <<
" " << it->second << std::endl;
538 bool doDepthAndIntensityCamerasCoincide()
const;
552 void getDescriptionAsText(std::ostream &o)
const MRPT_OVERRIDE;
556 void getZoneAsObs(
CObservation3DRangeScan &obs,
const unsigned int &r1,
const unsigned int &r2,
const unsigned int &c1,
const unsigned int &c2 );
562 static double recoverCameraCalibrationParameters(
565 const double camera_offset = 0.01 );
609 static const int HAS_RGB = 0;
610 static const int HAS_RGBf = 0;
611 static const int HAS_RGBu8 = 0;
624 template <
typename T>
631 inline void setPointXYZ(
const size_t idx,
const coords_t
x,
const coords_t y,
const coords_t
z) {
639 THROW_EXCEPTION(
"mrpt::obs::CObservation3DRangeScan requires needs to be dense");
bool m_points3D_external_stored
If set to true, m_points3D_external_file is valid.
std::string m_rangeImage_external_file
rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
std::string rangeImage_getExternalStorageFile() const
void setInvalidPoint(const size_t idx)
Set XYZ coordinates of i'th point.
const std::string & getLabelName(unsigned int label_idx) const
bool points3D_isExternallyStored() const
float coords_t
The type of each point XYZ coordinates.
TIntensityChannelID
Enum type for intensityImageChannel.
double DEG2RAD(const double x)
Degrees to radians.
const mrpt::poses::CPose3D * robotPoseInTheWorld
(Default: NULL) Read takeIntoAccountSensorPoseOnRobot
void resize(const size_t N)
Set number of points (to uninitialized values)
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.
stlplus::smart_ptr< TPixelLabelInfoBase > TPixelLabelInfoPtr
Used in CObservation3DRangeScan::pixelLabels.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
A helper base class for those PointCloudAdapter<> which do not handle RGB data; it declares needed in...
void setLabelName(unsigned int label_idx, const std::string &name)
TPixelLabelInfoBase(unsigned int BITFIELD_BYTES_)
A class for storing images as grayscale or RGB bitmaps.
TPixelLabelInfoPtr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
mrpt::math::CVectorFloat Kzs
#define THROW_EXCEPTION(msg)
std::string sensorLabel
The sensor label that will have the newly created observation.
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
std::string points3D_getExternalStorageFile() const
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...
virtual ~TPixelLabelInfoBase()
double oversampling_ratio
(Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::c...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
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.
const mrpt::math::CMatrix * rangeMask_min
(Default: NULL) If provided, each data range will be tested to be greater-than (rangeMask_min) or les...
Look-up-table struct for project3DPointsFromDepthImageInto()
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
Usage: uint_select_by_bytecount<N>::type var; allows defining var as a unsigned integer with...
PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj)
Constructor (accept a const ref for convenience)
Only specializations of this class are defined for each enum type of interest.
friend std::ostream & operator<<(std::ostream &out, const TPixelLabelInfoBase &obj)
std stream interface
const Scalar * const_iterator
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
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) ...
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
const uint8_t BITFIELD_BYTES
Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS bits.
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
std::string rangeImage_getExternalStorageFileAbsolutePath() const
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...
#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...
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
std::vector< float > points3D_y
std::vector< uint16_t > points3D_idxs_y
//!< If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z
bool rangeImage_isExternallyStored() const
TMapLabelID2Name pixelLabelNames
The 'semantic' or human-friendly name of the i'th bit in pixelLabels(r,c) can be found in pixelLabelN...
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
Used in CObservation3DRangeScan::convertTo2DScan()
int checkLabelNameExistence(const std::string &name) const
Check the existence of a label by returning its associated index.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
std::string m_points3D_external_file
3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
This namespace contains representation of robot actions and observations.
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
bool hasRangeImage
true means the field rangeImage contains valid data
static void fill(bimap< enum_t, std::string > &m_map)
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
void rangeImage_forceResetExternalStorage()
Forces marking this observation as non-externally stored - it doesn't anything else apart from reseti...
std::string points3D_getExternalStorageFileAbsolutePath() const
bool hasPoints3D
true means the field points3D contains valid data.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
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 ...
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
void Print(std::ostream &out) const MRPT_OVERRIDE
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
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...
static bool EXTERNALS_AS_TEXT
Whether external files (3D points, range and confidence) are to be saved as .txt text files (MATLAB c...
std::map< uint32_t, std::string > TMapLabelID2Name
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
#define MRPT_DEPRECATED(msg)
Usage: MRPT_DEPRECATED("Use XX instead") void myFunc(double);.
mrpt::obs::CObservation3DRangeScan::TIntensityChannelID enum_t
mrpt::obs::CObservation3DRangeScan & m_obj
std::vector< float > points3D_x
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer...
virtual void Print(std::ostream &) const =0
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...
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
mrpt::utils::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
T3DPointsProjectionParams()
An adapter to different kinds of point cloud object.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
bool MAKE_DENSE
(Default:true) set to false if you want to preserve the organization of the point cloud ...
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
This class is a "CSerializable" wrapper for "CMatrixFloat".
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
void getLabels(const int row, const int col, uint8_t &labels) MRPT_OVERRIDE
void unsetAll(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE
Removes all categories for pixel(row,col)
TPixelLabelMatrix pixelLabels
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
Virtual interface to all pixel-label information structs.
size_t size() const
Get number of points.
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...
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
Grayscale or RGB visible channel of the camera sensor.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated. Otherwise, points are transformed with sensorPose. Furthermore, if provided, those coordinates are transformed with robotPoseInTheWorld
Structure to hold the parameters of a pinhole camera model.
bool use_origin_sensor_pose
(Default:false) If false, the conversion will be such that the 2D observation pose on the robot coinc...
float maxRange
The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
mrpt::utils::TCamera prev_camParams
bool hasConfidenceImage
true means the field confidenceImage contains valid data