9 #ifndef CObservation3DRangeScan_H 10 #define CObservation3DRangeScan_H 33 template <
class POINTMAP>
118 bool m_points3D_external_stored;
119 std::
string m_points3D_external_file;
121 bool m_rangeImage_external_stored;
122 std::
string m_rangeImage_external_file;
126 virtual ~CObservation3DRangeScan( );
139 virtual
void unload() MRPT_OVERRIDE;
179 template <class POINTMAP>
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);
224 void convertTo2DScan(
226 const std::string & sensorLabel,
229 const double oversampling_ratio = 1.2 );
237 void resizePoints3DVectors(
const size_t nPoints);
243 void points3D_getExternalStorageFileAbsolutePath(std::string &out_path)
const;
246 points3D_getExternalStorageFileAbsolutePath(tmp);
249 void points3D_convertToExternalStorage(
const std::string &fileName,
const std::string &use_this_base_dir );
258 void rangeImage_setSize(
const int HEIGHT,
const int WIDTH);
265 void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path)
const;
268 rangeImage_getExternalStorageFileAbsolutePath(tmp);
271 void rangeImage_convertToExternalStorage(
const std::string &fileName,
const std::string &use_this_base_dir );
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; }
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;
332 virtual void setLabel(
const int row,
const int col, uint8_t label_idx) =0;
333 virtual void getLabels(
const int row,
const int col, uint8_t &labels ) =0;
336 virtual void unsetLabel(
const int row,
const int col, uint8_t label_idx)=0;
338 virtual void unsetAll(
const int row,
const int col, uint8_t label_idx) =0;
341 virtual bool checkLabel(
const int row,
const int col, uint8_t label_idx)
const =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);
420 for (uint32_t c=0;c<nC;c++)
421 for (uint32_t r=0;r<nR;r++)
422 in >> pixelLabels.coeffRef(r,c);
424 in >> pixelLabelNames;
429 const uint32_t nR=
static_cast<uint32_t
>(pixelLabels.rows());
430 const uint32_t nC=
static_cast<uint32_t
>(pixelLabels.cols());
432 for (uint32_t c=0;c<nC;c++)
433 for (uint32_t r=0;r<nR;r++)
434 out << pixelLabels.coeff(r,c);
436 out << pixelLabelNames;
441 const uint32_t nR=
static_cast<uint32_t
>(pixelLabels.rows());
442 const uint32_t nC=
static_cast<uint32_t
>(pixelLabels.cols());
443 out <<
"Number of rows: " << nR << std::endl;
444 out <<
"Number of cols: " << nC << std::endl;
445 out <<
"Matrix of labels: " << std::endl;
446 for (uint32_t c=0;c<nC;c++)
448 for (uint32_t r=0;r<nR;r++)
449 out << pixelLabels.coeff(r,c) <<
" ";
455 out <<
"Label indices and names: " << std::endl;
457 for ( it = pixelLabelNames.begin(); it != pixelLabelNames.end(); it++)
458 out << it->first <<
" " << it->second << std::endl;
483 bool doDepthAndIntensityCamerasCoincide()
const;
497 void getDescriptionAsText(std::ostream &o)
const MRPT_OVERRIDE;
501 void getZoneAsObs(
CObservation3DRangeScan &obs,
const unsigned int &r1,
const unsigned int &r2,
const unsigned int &c1,
const unsigned int &c2 );
507 static double recoverCameraCalibrationParameters(
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 {
576 inline void setPointXYZ(
const size_t idx,
const coords_t x,
const coords_t y,
const coords_t z) {
float coords_t
The type of each point XYZ coordinates.
TIntensityChannelID
Enum type for intensityImageChannel.
double DEG2RAD(const double x)
Degrees to radians.
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
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
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()
void project3DPointsFromDepthImageInto(CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const bool takeIntoAccountSensorPoseOnRobot, const mrpt::poses::CPose3D *robotPoseInTheWorld, const bool PROJ3D_USE_LUT)
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.
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
std::string rangeImage_getExternalStorageFileAbsolutePath() const
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...
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
std::string rangeImage_getExternalStorageFile() const
TMapLabelID2Name pixelLabelNames
The 'semantic' or human-friendly name of the i'th bit in pixelLabels(r,c) can be found in pixelLabelN...
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...
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...
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 ...
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer...
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
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...
bool points3D_isExternallyStored() const
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
size_t size() const
Get number of points.
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...
std::map< uint32_t, std::string > TMapLabelID2Name
bool hasIntensityImage
true means the field intensityImage contains valid data
mrpt::obs::CObservation3DRangeScan::TIntensityChannelID enum_t
mrpt::obs::CObservation3DRangeScan & m_obj
std::vector< float > points3D_x
std::string points3D_getExternalStorageFile() const
bool rangeImage_isExternallyStored() const
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...
mrpt::utils::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
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.
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.
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.
Structure to hold the parameters of a pinhole camera model.
const std::string & getLabelName(unsigned int label_idx) const
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
std::string points3D_getExternalStorageFileAbsolutePath() const