9 #ifndef CObservation3DRangeScan_H
10 #define CObservation3DRangeScan_H
33 template <
class POINTMAP>
117 bool m_points3D_external_stored;
118 std::
string m_points3D_external_file;
120 bool m_rangeImage_external_stored;
121 std::
string m_rangeImage_external_file;
125 virtual ~CObservation3DRangeScan( );
134 virtual
void load() const;
138 virtual
void unload();
178 template <class POINTMAP>
180 POINTMAP & dest_pointcloud,
181 const
bool takeIntoAccountSensorPoseOnRobot,
182 const
mrpt::poses::CPose3D *robotPoseInTheWorld=NULL,
183 const
bool PROJ3D_USE_LUT=true)
185 detail::project3DPointsFromDepthImageInto<POINTMAP>(*
this,dest_pointcloud,takeIntoAccountSensorPoseOnRobot,robotPoseInTheWorld,PROJ3D_USE_LUT);
223 void convertTo2DScan(
225 const std::string & sensorLabel,
228 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_
387 void setSize(
const int NROWS,
const int NCOLS) {
388 pixelLabels = TPixelLabelMatrix::Zero(NROWS,NCOLS);
390 void setLabel(
const int row,
const int col, uint8_t label_idx) {
391 pixelLabels(row,col) |=
static_cast<bitmask_t
>(1) << label_idx;
393 void getLabels(
const int row,
const int col, uint8_t &labels )
395 labels = pixelLabels(row,col);
398 void unsetLabel(
const int row,
const int col, uint8_t label_idx) {
399 pixelLabels(row,col) &= ~(
static_cast<bitmask_t
>(1) << label_idx);
401 void unsetAll(
const int row,
const int col, uint8_t label_idx) {
402 pixelLabels(row,col) = 0;
404 bool checkLabel(
const int row,
const int col, uint8_t label_idx)
const {
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;
438 virtual void Print( std::ostream& out )
const
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 virtual void getDescriptionAsText(std::ostream &o)
const;
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) {
void unsetAll(const int row, const int col, uint8_t label_idx)
Removes all categories for pixel(row,col)
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const
A general method to retrieve the sensor pose on the robot.
bool checkLabel(const int row, const int col, uint8_t label_idx) const
Checks whether pixel(row,col) has been clasified into category label_idx, which may be in the range 0...
float coords_t
The type of each point XYZ coordinates.
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.
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
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...
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
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::type var; allows defining var as a unsigned integer with...
PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj)
Constructor (accept a const ref for convenience)
bool hasIntensityImage
true means the field intensityImage contains valid data
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 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.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
#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
If hasPoints3D=true, the Y coordinates of the 3D point cloud detected by the camera.
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.
void getLabels(const int row, const int col, uint8_t &labels)
void setSize(const int NROWS, const int NCOLS)
Resizes the matrix pixelLabels to the given size, setting all bitfields to zero (that is...
A bidirectional version of std::map, declared as bimap and which actually contains two std...
This namespace contains algorithms for SLAM, localization, map building, representation of robot's ac...
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)
Grayscale or RGB visible channel of the camera sensor.
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...
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)
TIntensityChannelID
Enum type for intensityImageChannel.
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
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.
virtual void internal_readFromStream(mrpt::utils::CStream &in)
std::map< uint32_t, std::string > TMapLabelID2Name
mrpt::obs::CObservation3DRangeScan::TIntensityChannelID enum_t
mrpt::obs::CObservation3DRangeScan & m_obj
std::vector< float > points3D_x
If hasPoints3D=true, the X coordinates of the 3D point cloud detected by the camera.
std::string points3D_getExternalStorageFile() const
bool rangeImage_isExternallyStored() const
virtual void Print(std::ostream &) const =0
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
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...
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose)
A general method to change the sensor pose on the robot.
virtual void Print(std::ostream &out) const
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.
This class is a "CSerializable" wrapper for "CMatrixFloat".
void unsetLabel(const int row, const int col, uint8_t label_idx)
For the pixel(row,col), removes its classification into the category label_idx, which may be in the r...
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_)
virtual void internal_writeToStream(mrpt::utils::CStream &out) const
TPixelLabelMatrix pixelLabels
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
Virtual interface to all pixel-label information structs.
void setLabel(const int row, const int col, uint8_t label_idx)
Mark the pixel(row,col) as classified in the category label_idx, which may be in the range 0 to MAX_N...
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