MRPT  2.0.4
List of all members | Classes | Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes
mrpt::obs::CObservation3DRangeScan Class Reference

Detailed Description

A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light depth RGBD sensor.

This kind of observations can carry one or more of these data fields:

The coordinates of the 3D point cloud are in meters with respect to the depth camera origin of coordinates (in SwissRanger, the front face of the camera: a small offset ~1cm in front of the physical focal point), with the +X axis pointing forward, +Y pointing left-hand and +Z pointing up. By convention, a 3D point with its coordinates set to (0,0,0), will be considered as invalid. The field CObservation3DRangeScan::relativePoseIntensityWRTDepth describes the change of coordinates from the depth camera to the intensity (RGB or grayscale) camera. In a SwissRanger camera both cameras coincide, so this pose is just a rotation (0,0,0,-90deg,0,-90deg). But in Microsoft Kinect there is also an offset, as shown in this figure:

In any case, check the field relativePoseIntensityWRTDepth, or the method doDepthAndIntensityCamerasCoincide() to determine if both frames of reference coincide, since even for Kinect cameras both can coincide if the images have been rectified.

The 2D images and matrices are stored as common images, with an up->down rows order and left->right, as usual. Optionally, the intensity and confidence channels can be set to delayed-load images for off-rawlog storage so it saves memory by having loaded in memory just the needed images. See the methods load() and unload(). Due to the intensive storage requirements of this kind of observations, this observation is the only one in MRPT for which it's recommended to always call "load()" and "unload()" before and after using the observation, ONLY when the observation was read from a rawlog dataset, in order to make sure that all the externally stored data fields are loaded and ready in memory.

Classes that grab observations of this type are:

There are two sets of calibration parameters (see mrpt::vision::checkerBoardStereoCalibration() or the ready-to-use GUI program kinect-calibrate):

In some cameras, like SwissRanger, both are the same. It is possible in Kinect to rectify the range images such both cameras seem to coincide and then both sets of camera parameters will be identical.

Range data can be interpreted in two different ways depending on the 3D camera (this field is already set to the correct setting when grabbing observations from an mrpt::hwdrivers sensor):

The "intensity" channel may come from different channels in sesnsors as Kinect. Look at field intensityImageChannel to find out if the image was grabbed from the visible (RGB) or IR channels.

3D point clouds can be generated at any moment after grabbing with CObservation3DRangeScan::unprojectInto(), provided the correct calibration parameters. Note that unprojectInto() will store the point cloud in sensor-centric local coordinates. Use unprojectInto() to directly obtain vehicle or world coordinates.

Example of how to assign labels to pixels (for object segmentation, semantic information, etc.):

// Assume obs of type CObservation3DRangeScan::Ptr
obs->pixelLabels =TPixelLabelInfo::Ptr( new
*CObservation3DRangeScan::TPixelLabelInfo<NUM_BYTES>() );
obs->pixelLabels->setSize(ROWS,COLS);
obs->pixelLabels->setLabel(col,row, label_idx); // label_idxs =
[0,2^NUM_BYTES-1]
//...
Note
Starting at serialization version 2 (MRPT 0.9.1+), the confidence channel is stored as an image instead of a matrix to optimize memory and disk space.
Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud and the rangeImage can both be stored externally to save rawlog space.
Starting at serialization version 5 (MRPT 0.9.5+), the new field range_is_depth
Starting at serialization version 6 (MRPT 0.9.5+), the new field intensityImageChannel
Starting at serialization version 7 (MRPT 1.3.1+), new fields for semantic labeling
Since MRPT 1.5.0, external files format can be selected at runtime with CObservation3DRangeScan::EXTERNALS_AS_TEXT
See also
mrpt::hwdrivers::CSwissRanger3DCamera, mrpt::hwdrivers::CKinect, CObservation

Definition at line 168 of file CObservation3DRangeScan.h.

#include <mrpt/obs/CObservation3DRangeScan.h>

Inheritance diagram for mrpt::obs::CObservation3DRangeScan:

Classes

struct  unproject_LUT_t
 Look-up-table struct for unprojectInto() More...
 

Public Member Functions

 CObservation3DRangeScan ()=default
 
 ~CObservation3DRangeScan () override
 
template<class POINTMAP >
void unprojectInto (POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams=T3DPointsProjectionParams(), const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
 Unprojects the RGB+D image pair into a 3D point cloud (with color if the target map supports it) and optionally at a given 3D pose. More...
 
void convertTo2DScan (mrpt::obs::CObservation2DRangeScan &out_scan2d, const T3DPointsTo2DScanParams &scanParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
 Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV. More...
 
void undistort ()
 Removes the distortion in both, depth and intensity images. More...
 
void getDescriptionAsText (std::ostream &o) const override
 Build a detailed, multi-line textual description of the observation contents and dump it to the output stream. More...
 
void swap (CObservation3DRangeScan &o)
 Very efficient method to swap the contents of two observations. More...
 
void getZoneAsObs (CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2)
 Extract a ROI of the 3D observation as a new one. More...
 
const unproject_LUT_tget_unproj_lut () const
 Gets (or generates upon first request) the 3D point cloud projection look-up-table for the current depth camera intrinsics & distortion parameters. More...
 
template<class METRICMAP >
bool insertObservationInto (METRICMAP *theMap, const mrpt::poses::CPose3D *robotPose=nullptr) const
 This method is equivalent to: More...
 
void getSensorPose (mrpt::math::TPose3D &out_sensorPose) const
 A general method to retrieve the sensor pose on the robot. More...
 
void setSensorPose (const mrpt::math::TPose3D &newSensorPose)
 A general method to change the sensor pose on the robot. More...
 
std::string getDescriptionAsTextValue () const
 Return by value version of getDescriptionAsText(std::ostream&) More...
 
virtual mxArraywriteToMatlab () const
 Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class. More...
 
Delayed-load manual control methods.
void load () const override
 Makes sure all images and other fields which may be externally stored are loaded in memory. More...
 
void unload () override
 Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect). More...
 
Point cloud external storage functions
bool points3D_isExternallyStored () const
 
std::string points3D_getExternalStorageFile () const
 
void points3D_getExternalStorageFileAbsolutePath (std::string &out_path) const
 
std::string points3D_getExternalStorageFileAbsolutePath () const
 
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. More...
 
void points3D_overrideExternalStoredFlag (bool isExternal)
 Users normally won't need to use this. More...
 
Range Matrix external storage functions
bool rangeImage_isExternallyStored () const
 
std::string rangeImage_getExternalStorageFile (const std::string &rangeImageLayer) const
 
void rangeImage_getExternalStorageFileAbsolutePath (std::string &out_path, const std::string &rangeImageLayer) const
 rangeImageLayer: Empty for the main rangeImage matrix, otherwise, a key of rangeImageOtherLayers More...
 
std::string rangeImage_getExternalStorageFileAbsolutePath (const std::string &rangeImageLayer) const
 
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. More...
 
void rangeImage_forceResetExternalStorage ()
 Forces marking this observation as non-externally stored - it doesn't anything else apart from reseting the corresponding flag (Users won't normally want to call this, it's only used from internal MRPT programs) More...
 
RTTI classes and functions for polymorphic hierarchies
mrpt::rtti::CObject::Ptr duplicateGetSmartPtr () const
 Makes a deep copy of the object and returns a smart pointer to it. More...
 

Static Public Member Functions

static void EXTERNALS_AS_TEXT (bool value)
 Whether external files (3D points, range and confidence) are to be saved as .txt text files (MATLAB compatible) or *.bin binary (faster). More...
 
static bool EXTERNALS_AS_TEXT ()
 
static double recoverCameraCalibrationParameters (const CObservation3DRangeScan &in_obs, mrpt::img::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 range (depth) image and the corresponding 3D point cloud. More...
 

Public Attributes

Confidence "channel"
bool hasConfidenceImage {false}
 true means the field confidenceImage contains valid data More...
 
mrpt::img::CImage confidenceImage
 If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the capture drivers. More...
 

Protected Member Functions

void swap (CObservation &o)
 Swap with another observation, ONLY the data defined here in the base class CObservation. More...
 
CSerializable virtual methods
uint8_t serializeGetVersion () const override
 Must return the current versioning number of the object. More...
 
void serializeTo (mrpt::serialization::CArchive &out) const override
 Pure virtual method for writing (serializing) to an abstract archive. More...
 
void serializeFrom (mrpt::serialization::CArchive &in, uint8_t serial_version) override
 Pure virtual method for reading (deserializing) from an abstract archive. More...
 
CSerializable virtual methods
virtual void serializeTo (CSchemeArchiveBase &out) const
 Virtual method for writing (serializing) to an abstract schema based archive. More...
 
virtual void serializeFrom (CSchemeArchiveBase &in)
 Virtual method for reading (deserializing) from an abstract schema based archive. More...
 

Protected Attributes

bool m_points3D_external_stored {false}
 If set to true, m_points3D_external_file is valid. More...
 
std::string m_points3D_external_file
 3D points are in CImage::getImagesPathBase()+<this_file_name> More...
 
bool m_rangeImage_external_stored {false}
 If set to true, m_rangeImage_external_file is valid. More...
 
std::string m_rangeImage_external_file
 rangeImage is in CImage::getImagesPathBase()+<this_file_name> More...
 

Data common to any observation

mrpt::system::TTimeStamp timestamp {mrpt::system::now()}
 The associated UTC time-stamp. More...
 
std::string sensorLabel
 An arbitrary label that can be used to identify the sensor. More...
 
mrpt::system::TTimeStamp getTimeStamp () const
 Returns CObservation::timestamp for all kind of observations. More...
 
virtual mrpt::system::TTimeStamp getOriginalReceivedTimeStamp () const
 By default, returns CObservation::timestamp but in sensors capable of satellite (or otherwise) accurate UTC timing of readings, this contains the computer-based timestamp of reception, which may be slightly different than timestamp. More...
 

RTTI stuff

using Ptr = std::shared_ptr< mrpt::obs ::CObservation3DRangeScan >
 
using ConstPtr = std::shared_ptr< const mrpt::obs ::CObservation3DRangeScan >
 
using UniquePtr = std::unique_ptr< mrpt::obs ::CObservation3DRangeScan >
 
using ConstUniquePtr = std::unique_ptr< const mrpt::obs ::CObservation3DRangeScan >
 
static const mrpt::rtti::TRuntimeClassId runtimeClassId
 
static constexpr const char * className = "mrpt::obs" "::" "CObservation3DRangeScan"
 
static const mrpt::rtti::TRuntimeClassId_GetBaseClass ()
 
static constexpr auto getClassName ()
 
static const mrpt::rtti::TRuntimeClassIdGetRuntimeClassIdStatic ()
 
static std::shared_ptr< CObjectCreateObject ()
 
template<typename... Args>
static Ptr Create (Args &&... args)
 
template<typename Alloc , typename... Args>
static Ptr CreateAlloc (const Alloc &alloc, Args &&... args)
 
template<typename... Args>
static UniquePtr CreateUnique (Args &&... args)
 
virtual const mrpt::rtti::TRuntimeClassIdGetRuntimeClass () const override
 Returns information about the class of an object in runtime. More...
 
virtual mrpt::rtti::CObjectclone () const override
 Returns a deep copy (clone) of the object, indepently of its class. More...
 

Point cloud

bool hasPoints3D {false}
 true means the field points3D contains valid data. More...
 
std::vector< float > points3D_x
 If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera. More...
 
std::vector< float > points3D_y
 
std::vector< float > points3D_z
 
std::vector< uint16_t > points3D_idxs_x
 If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z. More...
 
std::vector< uint16_t > points3D_idxs_y
 
void resizePoints3DVectors (const size_t nPoints)
 Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage of the internal memory pool. More...
 
size_t getScanSize () const
 Get the size of the scan pointcloud. More...
 

Range (depth) image

bool hasRangeImage {false}
 true means the field rangeImage contains valid data More...
 
mrpt::math::CMatrix_u16 rangeImage
 If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters). More...
 
std::map< std::string, mrpt::math::CMatrix_u16rangeImageOtherLayers
 Additional layer range/depth images. More...
 
float rangeUnits = 0.001f
 The conversion factor from integer units in rangeImage and actual distances in meters. More...
 
bool range_is_depth {true}
 true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in rangeImage are actual distances in 3D. More...
 
static mrpt::img::CImage rangeImageAsImage (const mrpt::math::CMatrix_u16 &ranges, float val_min, float val_max, float rangeUnits, const std::optional< mrpt::img::TColormap > color=std::nullopt)
 Static method to convert a range matrix into an image. More...
 
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 memory allocation. More...
 
mrpt::img::CImage rangeImage_getAsImage (const std::optional< mrpt::img::TColormap > color=std::nullopt, const std::optional< float > normMinRange=std::nullopt, const std::optional< float > normMaxRange=std::nullopt, const std::optional< std::string > additionalLayerName=std::nullopt) const
 Builds a visualization from the rangeImage. More...
 

Intensity (RGB) channels

enum  TIntensityChannelID { CH_VISIBLE = 0, CH_IR = 1 }
 Enum type for intensityImageChannel. More...
 
bool hasIntensityImage {false}
 true means the field intensityImage contains valid data More...
 
mrpt::img::CImage intensityImage
 If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage". More...
 
TIntensityChannelID intensityImageChannel {CH_VISIBLE}
 The source of the intensityImage; typically the visible channel. More...
 

Pixel-wise classification labels (for semantic labeling, etc.)

TPixelLabelInfoBase::Ptr pixelLabels
 All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelLabelInfo for details on the contents User is responsible of creating a new object of the desired data type. More...
 
bool hasPixelLabels () const
 Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer. More...
 

Sensor parameters

mrpt::img::TCamera cameraParams
 Projection parameters of the depth camera. More...
 
mrpt::img::TCamera cameraParamsIntensity
 Projection parameters of the intensity (graylevel or RGB) camera. More...
 
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
 Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this observation). More...
 
float maxRange {5.0f}
 The maximum range allowed by the device, in meters (e.g. More...
 
mrpt::poses::CPose3D sensorPose
 The 6D pose of the sensor on the robot. More...
 
float stdError {0.01f}
 The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid. More...
 
bool doDepthAndIntensityCamerasCoincide () const
 Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon) More...
 
void getSensorPose (mrpt::poses::CPose3D &out_sensorPose) const override
 A general method to retrieve the sensor pose on the robot. More...
 
void setSensorPose (const mrpt::poses::CPose3D &newSensorPose) override
 A general method to change the sensor pose on the robot. More...
 

Member Typedef Documentation

◆ ConstPtr

Definition at line 170 of file CObservation3DRangeScan.h.

◆ ConstUniquePtr

Definition at line 170 of file CObservation3DRangeScan.h.

◆ Ptr

A type for the associated smart pointer

Definition at line 170 of file CObservation3DRangeScan.h.

◆ UniquePtr

Definition at line 170 of file CObservation3DRangeScan.h.

Member Enumeration Documentation

◆ TIntensityChannelID

Enum type for intensityImageChannel.

Enumerator
CH_VISIBLE 

Grayscale or RGB visible channel of the camera sensor.

CH_IR 

Infrarred (IR) channel.

Definition at line 471 of file CObservation3DRangeScan.h.

Constructor & Destructor Documentation

◆ CObservation3DRangeScan()

mrpt::obs::CObservation3DRangeScan::CObservation3DRangeScan ( )
default

◆ ~CObservation3DRangeScan()

CObservation3DRangeScan::~CObservation3DRangeScan ( )
override

Definition at line 290 of file CObservation3DRangeScan.cpp.

References mempool_donate_range_matrix(), and mempool_donate_xyz_buffers().

Here is the call graph for this function:

Member Function Documentation

◆ _GetBaseClass()

static const mrpt::rtti::TRuntimeClassId* mrpt::obs::CObservation3DRangeScan::_GetBaseClass ( )
staticprotected

◆ clone()

virtual mrpt::rtti::CObject* mrpt::obs::CObservation3DRangeScan::clone ( ) const
overridevirtual

Returns a deep copy (clone) of the object, indepently of its class.

Implements mrpt::rtti::CObject.

◆ convertTo2DScan()

void CObservation3DRangeScan::convertTo2DScan ( mrpt::obs::CObservation2DRangeScan out_scan2d,
const T3DPointsTo2DScanParams scanParams,
const TRangeImageFilterParams filterParams = TRangeImageFilterParams() 
)

Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV.

The result is a 2D laser scan with more "rays" (N) than columns has the 3D observation (W), exactly: N = W * oversampling_ratio. This oversampling is required since laser scans sample the space at evenly-separated angles, while a range camera follows a tangent-like distribution. By oversampling we make sure we don't leave "gaps" unseen by the virtual "2D laser".

All obstacles within a frustum are considered and the minimum distance is kept in each direction. The horizontal FOV of the frustum is automatically computed from the intrinsic parameters, but the vertical FOV must be provided by the user, and can be set to be assymetric which may be useful depending on the zone of interest where to look for obstacles.

All spatial transformations are riguorosly taken into account in this class, using the depth camera intrinsic calibration parameters.

The timestamp of the new object is copied from the 3D object. Obviously, a requisite for calling this method is the 3D observation having range data, i.e. hasRangeImage must be true. It's not needed to have RGB data nor the raw 3D point clouds for this method to work.

If scanParams.use_origin_sensor_pose is true, the points will be projected to 3D and then reprojected as seen from a different sensorPose at the vehicle frame origin. Otherwise (the default), the output 2D observation will share the sensorPose of the input 3D scan (using a more efficient algorithm that avoids trigonometric functions).

Parameters
[out]out_scan2dThe resulting 2D equivalent scan.
See also
The example in https://www.mrpt.org/tutorials/mrpt-examples/example-kinect-to-2d-laser-demo/

Definition at line 1196 of file CObservation3DRangeScan.cpp.

References mrpt::obs::T3DPointsTo2DScanParams::angle_inf, mrpt::obs::T3DPointsTo2DScanParams::angle_sup, mrpt::obs::CObservation2DRangeScan::aperture, ASSERT_ABOVE_, ASSERT_EQUAL_, mrpt::math::CMatrixDynamic< T >::cols(), mrpt::opengl::CPointCloud::Create(), mrpt::d2f(), mrpt::obs::TRangeImageFilter::do_range_filter(), mrpt::obs::CObservation2DRangeScan::getScanRange(), mrpt::keep_min(), mrpt::obs::CObservation2DRangeScan::maxRange, mrpt::obs::T3DPointsTo2DScanParams::oversampling_ratio, mrpt::obs::TRangeImageFilterParams::rangeMask_max, mrpt::obs::TRangeImageFilterParams::rangeMask_min, mrpt::obs::CObservation2DRangeScan::resizeScan(), mrpt::obs::CObservation2DRangeScan::resizeScanAndAssign(), mrpt::obs::CObservation2DRangeScan::rightToLeft, mrpt::round(), mrpt::math::CMatrixDynamic< T >::rows(), mrpt::obs::CObservation::sensorLabel, mrpt::obs::CObservation2DRangeScan::sensorPose, mrpt::obs::CObservation2DRangeScan::setScanRange(), mrpt::obs::CObservation2DRangeScan::setScanRangeValidity(), mrpt::obs::T3DPointsProjectionParams::takeIntoAccountSensorPoseOnRobot, mrpt::obs::CObservation::timestamp, mrpt::obs::detail::unprojectInto(), mrpt::obs::T3DPointsTo2DScanParams::use_origin_sensor_pose, mrpt::obs::T3DPointsTo2DScanParams::z_max, and mrpt::obs::T3DPointsTo2DScanParams::z_min.

Here is the call graph for this function:

◆ Create()

template<typename... Args>
static Ptr mrpt::obs::CObservation3DRangeScan::Create ( Args &&...  args)
inlinestatic

Definition at line 170 of file CObservation3DRangeScan.h.

◆ CreateAlloc()

template<typename Alloc , typename... Args>
static Ptr mrpt::obs::CObservation3DRangeScan::CreateAlloc ( const Alloc &  alloc,
Args &&...  args 
)
inlinestatic

Definition at line 170 of file CObservation3DRangeScan.h.

◆ CreateObject()

static std::shared_ptr<CObject> mrpt::obs::CObservation3DRangeScan::CreateObject ( )
static

◆ CreateUnique()

template<typename... Args>
static UniquePtr mrpt::obs::CObservation3DRangeScan::CreateUnique ( Args &&...  args)
inlinestatic

Definition at line 170 of file CObservation3DRangeScan.h.

◆ doDepthAndIntensityCamerasCoincide()

bool CObservation3DRangeScan::doDepthAndIntensityCamerasCoincide ( ) const

Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)

See also
relativePoseIntensityWRTDepth

Definition at line 1182 of file CObservation3DRangeScan.cpp.

References mrpt::poses::CPose3D::getRotationMatrix().

Here is the call graph for this function:

◆ duplicateGetSmartPtr()

mrpt::rtti::CObject::Ptr CObject::duplicateGetSmartPtr ( ) const
inlineinherited

Makes a deep copy of the object and returns a smart pointer to it.

Definition at line 204 of file CObject.h.

References mrpt::rtti::CObject::clone().

Referenced by mrpt::obs::CRawlog::insert().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ EXTERNALS_AS_TEXT() [1/2]

void CObservation3DRangeScan::EXTERNALS_AS_TEXT ( bool  value)
static

Whether external files (3D points, range and confidence) are to be saved as .txt text files (MATLAB compatible) or *.bin binary (faster).

Loading always will determine the type by inspecting the file extension.

Note
Default=false

Definition at line 182 of file CObservation3DRangeScan.cpp.

References EXTERNALS_AS_TEXT_value.

◆ EXTERNALS_AS_TEXT() [2/2]

bool CObservation3DRangeScan::EXTERNALS_AS_TEXT ( )
static

Definition at line 186 of file CObservation3DRangeScan.cpp.

References EXTERNALS_AS_TEXT_value.

◆ get_unproj_lut()

const CObservation3DRangeScan::unproject_LUT_t & CObservation3DRangeScan::get_unproj_lut ( ) const

Gets (or generates upon first request) the 3D point cloud projection look-up-table for the current depth camera intrinsics & distortion parameters.

Returns a const reference to a global variable. Multithread safe.

See also
unprojectInto

Definition at line 80 of file CObservation3DRangeScan.cpp.

References ASSERT_EQUAL_, LUT_info::calib, mrpt::obs::CObservation3DRangeScan::unproject_LUT_t::Kxs, LUTs, LUTs_mtx, LUT_info::range_is_depth, LUT_info::sensorPose, and THROW_EXCEPTION.

Referenced by mrpt::obs::detail::range2XYZ_LUT().

Here is the caller graph for this function:

◆ getClassName()

static constexpr auto mrpt::obs::CObservation3DRangeScan::getClassName ( )
inlinestatic

Definition at line 170 of file CObservation3DRangeScan.h.

◆ getDescriptionAsText()

void CObservation3DRangeScan::getDescriptionAsText ( std::ostream &  o) const
overridevirtual

Build a detailed, multi-line textual description of the observation contents and dump it to the output stream.

Note
If overried by derived classes, call base CObservation::getDescriptionAsText() first to show common information.
This is the text that appears in RawLogViewer when selecting an object in the dataset

Reimplemented from mrpt::obs::CObservation.

Definition at line 1362 of file CObservation3DRangeScan.cpp.

References mrpt::config::CConfigFileMemory::getContent(), and mrpt::math::CMatrixFixed< T, ROWS, COLS >::size().

Here is the call graph for this function:

◆ getDescriptionAsTextValue()

std::string CObservation::getDescriptionAsTextValue ( ) const
inherited

Return by value version of getDescriptionAsText(std::ostream&)

Definition at line 59 of file CObservation.cpp.

◆ getOriginalReceivedTimeStamp()

virtual mrpt::system::TTimeStamp mrpt::obs::CObservation::getOriginalReceivedTimeStamp ( ) const
inlinevirtualinherited

By default, returns CObservation::timestamp but in sensors capable of satellite (or otherwise) accurate UTC timing of readings, this contains the computer-based timestamp of reception, which may be slightly different than timestamp.

See also
getTimeStamp()

Reimplemented in mrpt::obs::CObservationVelodyneScan, mrpt::obs::CObservationGPS, and mrpt::obs::CObservationRotatingScan.

Definition at line 71 of file CObservation.h.

References mrpt::obs::CObservation::timestamp.

◆ GetRuntimeClass()

virtual const mrpt::rtti::TRuntimeClassId* mrpt::obs::CObservation3DRangeScan::GetRuntimeClass ( ) const
overridevirtual

Returns information about the class of an object in runtime.

Reimplemented from mrpt::obs::CObservation.

◆ GetRuntimeClassIdStatic()

static const mrpt::rtti::TRuntimeClassId& mrpt::obs::CObservation3DRangeScan::GetRuntimeClassIdStatic ( )
static

◆ getScanSize()

size_t CObservation3DRangeScan::getScanSize ( ) const

Get the size of the scan pointcloud.

Note
Method is added for compatibility with its CObservation2DRangeScan counterpart

Definition at line 1140 of file CObservation3DRangeScan.cpp.

◆ getSensorPose() [1/2]

void CObservation::getSensorPose ( mrpt::math::TPose3D out_sensorPose) const
inherited

A general method to retrieve the sensor pose on the robot.

Note that most sensors will return a full (6D) CPose3D, but see the derived classes for more details or special cases.

See also
setSensorPose

Definition at line 24 of file CObservation.cpp.

References mrpt::poses::CPose3D::asTPose().

Here is the call graph for this function:

◆ getSensorPose() [2/2]

void mrpt::obs::CObservation3DRangeScan::getSensorPose ( mrpt::poses::CPose3D out_sensorPose) const
inlineoverridevirtual

A general method to retrieve the sensor pose on the robot.

Note that most sensors will return a full (6D) CPose3D, but see the derived classes for more details or special cases.

See also
setSensorPose

Implements mrpt::obs::CObservation.

Definition at line 552 of file CObservation3DRangeScan.h.

References sensorPose.

◆ getTimeStamp()

mrpt::system::TTimeStamp mrpt::obs::CObservation::getTimeStamp ( ) const
inlineinherited

Returns CObservation::timestamp for all kind of observations.

See also
getOriginalReceivedTimeStamp()

Definition at line 66 of file CObservation.h.

References mrpt::obs::CObservation::timestamp.

◆ getZoneAsObs()

void CObservation3DRangeScan::getZoneAsObs ( CObservation3DRangeScan obs,
const unsigned int &  r1,
const unsigned int &  r2,
const unsigned int &  c1,
const unsigned int &  c2 
)

Extract a ROI of the 3D observation as a new one.

Note
PixelLabels are not copied to the output subimage.

Definition at line 1023 of file CObservation3DRangeScan.cpp.

References mrpt::math::CMatrixDynamic< T >::asEigen(), ASSERT_, cameraParams, confidenceImage, hasConfidenceImage, hasIntensityImage, hasPoints3D, hasRangeImage, intensityImage, intensityImageChannel, maxRange, points3D_x, points3D_y, points3D_z, rangeImage, sensorPose, and stdError.

Here is the call graph for this function:

◆ hasPixelLabels()

bool mrpt::obs::CObservation3DRangeScan::hasPixelLabels ( ) const
inline

Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer.

To enhance a 3D point cloud with labeling info, just assign an appropiate object to pixelLabels

Definition at line 507 of file CObservation3DRangeScan.h.

References pixelLabels.

◆ insertObservationInto()

template<class METRICMAP >
bool mrpt::obs::CObservation::insertObservationInto ( METRICMAP *  theMap,
const mrpt::poses::CPose3D robotPose = nullptr 
) const
inlineinherited

This method is equivalent to:

map->insertObservation(this, robotPose)
Parameters
theMapThe map where this observation is to be inserted: the map will be updated.
robotPoseThe pose of the robot base for this observation, relative to the target metric map. Set to nullptr (default) to use (0,0,0deg)
Returns
Returns true if the map has been updated, or false if this observations has nothing to do with a metric map (for example, a sound observation).

See: Maps and observations compatibility matrix

See also
CMetricMap, CMetricMap::insertObservation

Definition at line 99 of file CObservation.h.

◆ load()

void CObservation3DRangeScan::load ( ) const
overridevirtual

Makes sure all images and other fields which may be externally stored are loaded in memory.

Note that for all CImages, calling load() is not required since the images will be automatically loaded upon first access, so load() shouldn't be needed to be called in normal cases by the user. If all the data were alredy loaded or this object has no externally stored data fields, calling this method has no effects.

See also
unload

Reimplemented from mrpt::obs::CObservation.

Definition at line 623 of file CObservation3DRangeScan.cpp.

References mrpt::serialization::archiveFrom(), ASSERT_EQUAL_, mrpt::math::CMatrixDynamic< T >::begin(), mrpt::math::CMatrixDynamic< T >::cols(), mrpt::math::CMatrixDynamic< T >::data(), mrpt::system::extractFileExtension(), mrpt::math::MatrixVectorBase< Scalar, Derived >::loadFromTextFile(), mrpt::system::os::memcpy(), mrpt::math::CMatrixDynamic< T >::rows(), mrpt::math::CMatrixDynamic< T >::size(), and mrpt::system::strCmpI().

Referenced by mrpt::maps::CColouredOctoMap::internal_insertObservation(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateState().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ points3D_convertToExternalStorage()

void CObservation3DRangeScan::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.

See also
EXTERNALS_AS_TEXT

Definition at line 764 of file CObservation3DRangeScan.cpp.

References mrpt::serialization::archiveFrom(), ASSERT_, EXTERNALS_AS_TEXT_value, mrpt::system::fileNameChangeExtension(), mrpt::math::MATRIX_FORMAT_FIXED, mrpt::math::MatrixVectorBase< Scalar, Derived >::saveToTextFile(), mrpt::math::CMatrixDynamic< T >::setCol(), and mrpt::vector_strong_clear().

Here is the call graph for this function:

◆ points3D_getExternalStorageFile()

std::string mrpt::obs::CObservation3DRangeScan::points3D_getExternalStorageFile ( ) const
inline

Definition at line 350 of file CObservation3DRangeScan.h.

References m_points3D_external_file.

◆ points3D_getExternalStorageFileAbsolutePath() [1/2]

void CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath ( std::string &  out_path) const

Definition at line 743 of file CObservation3DRangeScan.cpp.

References ASSERT_.

◆ points3D_getExternalStorageFileAbsolutePath() [2/2]

std::string mrpt::obs::CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath ( ) const
inline

Definition at line 356 of file CObservation3DRangeScan.h.

◆ points3D_isExternallyStored()

bool mrpt::obs::CObservation3DRangeScan::points3D_isExternallyStored ( ) const
inline

Definition at line 346 of file CObservation3DRangeScan.h.

References m_points3D_external_stored.

◆ points3D_overrideExternalStoredFlag()

void mrpt::obs::CObservation3DRangeScan::points3D_overrideExternalStoredFlag ( bool  isExternal)
inline

Users normally won't need to use this.

Definition at line 367 of file CObservation3DRangeScan.h.

References m_points3D_external_stored.

◆ rangeImage_convertToExternalStorage()

void CObservation3DRangeScan::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.

See also
EXTERNALS_AS_TEXT

Definition at line 814 of file CObservation3DRangeScan.cpp.

References mrpt::serialization::archiveFrom(), ASSERT_, mrpt::math::CMatrixDynamic< T >::begin(), mrpt::math::CMatrixDynamic< T >::cols(), mrpt::math::CMatrixDynamic< T >::data(), EXTERNALS_AS_TEXT_value, mrpt::system::fileNameChangeExtension(), mrpt::math::MATRIX_FORMAT_FIXED, mrpt::math::CMatrixDynamic< T >::rows(), mrpt::math::MatrixVectorBase< Scalar, Derived >::saveToTextFile(), and mrpt::math::CMatrixDynamic< T >::size().

Here is the call graph for this function:

◆ rangeImage_forceResetExternalStorage()

void mrpt::obs::CObservation3DRangeScan::rangeImage_forceResetExternalStorage ( )
inline

Forces marking this observation as non-externally stored - it doesn't anything else apart from reseting the corresponding flag (Users won't normally want to call this, it's only used from internal MRPT programs)

Definition at line 462 of file CObservation3DRangeScan.h.

References m_rangeImage_external_stored.

◆ rangeImage_getAsImage()

mrpt::img::CImage CObservation3DRangeScan::rangeImage_getAsImage ( const std::optional< mrpt::img::TColormap color = std::nullopt,
const std::optional< float >  normMinRange = std::nullopt,
const std::optional< float >  normMaxRange = std::nullopt,
const std::optional< std::string >  additionalLayerName = std::nullopt 
) const

Builds a visualization from the rangeImage.

The image is built with the given color map (default: grayscale) and such that the colormap range is mapped to ranges 0 meters to the field "maxRange" in this object, unless overriden with the optional parameters. Note that the usage of optional<> allows any parameter to be left to its default placing std::nullopt.

Parameters
additionalLayerNameIf empty string or not provided, the main rangeImage will be used; otherwise, the given range image layer.
See also
rangeImageAsImage

Definition at line 1615 of file CObservation3DRangeScan.cpp.

References ASSERT_, ASSERT_ABOVE_, hasRangeImage, maxRange, rangeImage, rangeImageAsImage(), rangeImageOtherLayers, and rangeUnits.

Referenced by mrpt::hwdrivers::CKinect::getNextObservation().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ rangeImage_getExternalStorageFile()

std::string CObservation3DRangeScan::rangeImage_getExternalStorageFile ( const std::string &  rangeImageLayer) const

Definition at line 710 of file CObservation3DRangeScan.cpp.

References mrpt::system::extractFileExtension(), and mrpt::system::fileNameChangeExtension().

Here is the call graph for this function:

◆ rangeImage_getExternalStorageFileAbsolutePath() [1/2]

void CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath ( std::string &  out_path,
const std::string &  rangeImageLayer 
) const

rangeImageLayer: Empty for the main rangeImage matrix, otherwise, a key of rangeImageOtherLayers

Definition at line 723 of file CObservation3DRangeScan.cpp.

References ASSERT_.

Referenced by rangeImage_getExternalStorageFileAbsolutePath().

Here is the caller graph for this function:

◆ rangeImage_getExternalStorageFileAbsolutePath() [2/2]

std::string mrpt::obs::CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath ( const std::string &  rangeImageLayer) const
inline

Definition at line 447 of file CObservation3DRangeScan.h.

References rangeImage_getExternalStorageFileAbsolutePath().

Here is the call graph for this function:

◆ rangeImage_isExternallyStored()

bool mrpt::obs::CObservation3DRangeScan::rangeImage_isExternallyStored ( ) const
inline

Definition at line 436 of file CObservation3DRangeScan.h.

References m_rangeImage_external_stored.

◆ rangeImage_setSize()

void CObservation3DRangeScan::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 memory allocation.

Definition at line 1148 of file CObservation3DRangeScan.cpp.

References mrpt::system::CGenericMemoryPool< DATA_PARAMS, POOLABLE_DATA >::getInstance(), CObservation3DRangeScan_Ranges_MemPoolParams::H, CObservation3DRangeScan_Ranges_MemPoolData::rangeImage, mrpt::system::CGenericMemoryPool< DATA_PARAMS, POOLABLE_DATA >::request_memory(), and CObservation3DRangeScan_Ranges_MemPoolParams::W.

Referenced by fillSampleObs().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ rangeImageAsImage()

mrpt::img::CImage CObservation3DRangeScan::rangeImageAsImage ( const mrpt::math::CMatrix_u16 ranges,
float  val_min,
float  val_max,
float  rangeUnits,
const std::optional< mrpt::img::TColormap color = std::nullopt 
)
static

Static method to convert a range matrix into an image.

If val_max is left to zero, the maximum range in the matrix will be automatically used.

See also
rangeImage_getAsImage

Definition at line 1561 of file CObservation3DRangeScan.cpp.

References ASSERT_ABOVE_, mrpt::img::CH_GRAY, mrpt::img::CH_RGB, mrpt::img::cmGRAYSCALE, mrpt::math::MatrixVectorBase< Scalar, Derived >::coeff(), mrpt::img::colormap(), mrpt::math::CMatrixDynamic< T >::cols(), G, mrpt::math::MatrixVectorBase< Scalar, Derived >::maxCoeff(), R, rangeUnits, mrpt::math::CMatrixDynamic< T >::rows(), and THROW_EXCEPTION.

Referenced by rangeImage_getAsImage().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ recoverCameraCalibrationParameters()

double CObservation3DRangeScan::recoverCameraCalibrationParameters ( const CObservation3DRangeScan obs,
mrpt::img::TCamera out_camParams,
const double  camera_offset = 0.01 
)
static

A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a range (depth) image and the corresponding 3D point cloud.

Parameters
camera_offsetThe offset (in meters) in the +X direction of the point cloud. It's 1cm for SwissRanger SR4000.
Returns
The final average reprojection error per pixel (typ <0.05 px)

Definition at line 967 of file CObservation3DRangeScan.cpp.

References ASSERT_, CALIB_DECIMAT, mrpt::obs::detail::cam2vec(), mrpt::math::CMatrixDynamic< T >::cols(), mrpt::obs::detail::cost_func(), mrpt::math::MatrixVectorBase< Scalar, Derived >::fill(), mrpt::img::TCamera::focalLengthMeters, hasPoints3D, hasRangeImage, mrpt::img::TCamera::intrinsicParams, mrpt::system::LVL_INFO, MRPT_END, MRPT_START, mrpt::img::TCamera::ncols, mrpt::img::TCamera::nrows, points3D_x, points3D_y, points3D_z, rangeImage, mrpt::math::CVectorDynamic< T >::resize(), mrpt::math::CMatrixDynamic< T >::rows(), mrpt::math::CVectorDynamic< T >::size(), mrpt::square(), and mrpt::obs::detail::vec2cam().

Here is the call graph for this function:

◆ resizePoints3DVectors()

void CObservation3DRangeScan::resizePoints3DVectors ( const size_t  WH)

Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage of the internal memory pool.

Definition at line 1085 of file CObservation3DRangeScan.cpp.

References mrpt::system::CGenericMemoryPool< DATA_PARAMS, POOLABLE_DATA >::getInstance(), CObservation3DRangeScan_Points_MemPoolData::idxs_x, CObservation3DRangeScan_Points_MemPoolData::idxs_y, CObservation3DRangeScan_Points_MemPoolData::pts_x, CObservation3DRangeScan_Points_MemPoolData::pts_y, CObservation3DRangeScan_Points_MemPoolData::pts_z, mrpt::system::CGenericMemoryPool< DATA_PARAMS, POOLABLE_DATA >::request_memory(), mrpt::vector_strong_clear(), and CObservation3DRangeScan_Points_MemPoolParams::WH.

Referenced by mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::resize(), and mrpt::obs::detail::unprojectInto().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ serializeFrom() [1/2]

virtual void mrpt::serialization::CSerializable::serializeFrom ( CSchemeArchiveBase in)
inlineprotectedvirtualinherited

Virtual method for reading (deserializing) from an abstract schema based archive.

Definition at line 74 of file CSerializable.h.

References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.

Here is the call graph for this function:

◆ serializeFrom() [2/2]

void CObservation3DRangeScan::serializeFrom ( mrpt::serialization::CArchive in,
uint8_t  serial_version 
)
overrideprotectedvirtual

Pure virtual method for reading (deserializing) from an abstract archive.

Users don't call this method directly. Instead, use stream >> object;.

Parameters
inThe input binary stream where the object data must read from.
versionThe version of the object stored in the stream: use this version number in your code to know how to read the incoming data.
Exceptions
std::exceptionOn any I/O error

Implements mrpt::serialization::CSerializable.

Definition at line 376 of file CObservation3DRangeScan.cpp.

References ASSERT_, MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION, mrpt::serialization::CArchive::ReadAs(), mrpt::serialization::CArchive::ReadBuffer(), mrpt::serialization::CArchive::ReadBufferFixEndianness(), mrpt::round(), and mrpt::math::CMatrixDynamic< T >::rows().

Here is the call graph for this function:

◆ serializeGetVersion()

uint8_t CObservation3DRangeScan::serializeGetVersion ( ) const
overrideprotectedvirtual

Must return the current versioning number of the object.

Start in zero for new classes, and increments each time there is a change in the stored format.

Implements mrpt::serialization::CSerializable.

Definition at line 298 of file CObservation3DRangeScan.cpp.

◆ serializeTo() [1/2]

virtual void mrpt::serialization::CSerializable::serializeTo ( CSchemeArchiveBase out) const
inlineprotectedvirtualinherited

Virtual method for writing (serializing) to an abstract schema based archive.

Definition at line 64 of file CSerializable.h.

References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.

Here is the call graph for this function:

◆ serializeTo() [2/2]

void CObservation3DRangeScan::serializeTo ( mrpt::serialization::CArchive out) const
overrideprotectedvirtual

Pure virtual method for writing (serializing) to an abstract archive.

Users don't call this method directly. Instead, use stream << object;.

Exceptions
std::exceptionOn any I/O error

Implements mrpt::serialization::CSerializable.

Definition at line 299 of file CObservation3DRangeScan.cpp.

References ASSERT_, and out.

◆ setSensorPose() [1/2]

void CObservation::setSensorPose ( const mrpt::math::TPose3D newSensorPose)
inherited

A general method to change the sensor pose on the robot.

Note that most sensors will use the full (6D) CPose3D, but see the derived classes for more details or special cases.

See also
getSensorPose

Definition at line 31 of file CObservation.cpp.

◆ setSensorPose() [2/2]

void mrpt::obs::CObservation3DRangeScan::setSensorPose ( const mrpt::poses::CPose3D newSensorPose)
inlineoverridevirtual

A general method to change the sensor pose on the robot.

Note that most sensors will use the full (6D) CPose3D, but see the derived classes for more details or special cases.

See also
getSensorPose

Implements mrpt::obs::CObservation.

Definition at line 557 of file CObservation3DRangeScan.h.

References sensorPose.

◆ swap() [1/2]

void CObservation::swap ( CObservation o)
protectedinherited

Swap with another observation, ONLY the data defined here in the base class CObservation.

It's protected since it'll be only called from child classes that should know what else to swap appart from these common data.

Definition at line 36 of file CObservation.cpp.

References mrpt::obs::CObservation::sensorLabel, and mrpt::obs::CObservation::timestamp.

Referenced by mrpt::obs::CObservationStereoImages::swap().

Here is the caller graph for this function:

◆ swap() [2/2]

void CObservation3DRangeScan::swap ( CObservation3DRangeScan o)

◆ undistort()

void CObservation3DRangeScan::undistort ( )

Removes the distortion in both, depth and intensity images.

Intrinsics (fx,fy,cx,cy) remains the same for each image after undistortion.

Definition at line 1490 of file CObservation3DRangeScan.cpp.

References cameraParams, cameraParamsIntensity, mrpt::math::CMatrixDynamic< T >::cols(), mrpt::math::CMatrixDynamic< T >::data(), mrpt::img::TCamera::dist, hasIntensityImage, intensityImage, mrpt::img::TCamera::intrinsicParams, rangeImage, rangeImageOtherLayers, mrpt::math::CMatrixDynamic< T >::rows(), THROW_EXCEPTION, and mrpt::img::CImage::undistort().

Here is the call graph for this function:

◆ unload()

void CObservation3DRangeScan::unload ( )
overridevirtual

Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect).

See also
load

Reimplemented from mrpt::obs::CObservation.

Definition at line 695 of file CObservation3DRangeScan.cpp.

References mrpt::vector_strong_clear().

Here is the call graph for this function:

◆ unprojectInto()

template<class POINTMAP >
void mrpt::obs::CObservation3DRangeScan::unprojectInto ( POINTMAP &  dest_pointcloud,
const T3DPointsProjectionParams projectParams = T3DPointsProjectionParams(),
const TRangeImageFilterParams filterParams = TRangeImageFilterParams() 
)
inline

Unprojects the RGB+D image pair into a 3D point cloud (with color if the target map supports it) and optionally at a given 3D pose.

The 3D point coordinates are computed from the depth image (rangeImage) and the depth camera camera parameters (cameraParams). There exist two set of formulas for projecting the i'th point, depending on the value of "range_is_depth". In all formulas below, "rangeImage" is the matrix of ranges and the pixel coordinates are (r,c).

1) [range_is_depth=true] With "range equals depth" or "Kinect-like depth mode": the range values are in fact distances along the "+X" axis, not real 3D ranges (this is the way Kinect reports ranges):

x(i) = rangeImage(r,c) * rangeUnits
y(i) = (r_cx - c) * x(i) / r_fx
z(i) = (r_cy - r) * x(i) / r_fy

2) [range_is_depth=false] With "normal ranges": range means distance in 3D. This must be set when processing data from the SwissRange 3D camera, among others.

Ky = (r_cx - c)/r_fx
Kz = (r_cy - r)/r_fy
x(i) = rangeImage(r,c) * rangeUnits / sqrt( 1 + Ky^2 + Kz^2 )
y(i) = Ky * x(i)
z(i) = Kz * x(i)

The color of each point is determined by projecting the 3D local point into the RGB image using cameraParamsIntensity.

By default the local (sensor-centric) coordinates of points are directly stored into the local map, but if indicated so in takeIntoAccountSensorPoseOnRobot the points are transformed with sensorPose. Furthermore, if provided, those coordinates are transformed with robotPoseInTheWorld

Note
For multi-return sensors, only the layer specified in T3DPointsProjectionParams::layer will be unprojected.
Template Parameters
POINTMAPSupported maps are all those covered by mrpt::opengl::PointCloudAdapter (mrpt::maps::CPointsMap and derived, mrpt::opengl::CPointCloudColoured, PCL point clouds,...)

Definition at line 257 of file CObservation3DRangeScan.h.

Referenced by mrpt::hwdrivers::CKinect::getNextObservation(), and TEST().

Here is the caller graph for this function:

◆ writeToMatlab()

virtual mxArray* mrpt::serialization::CSerializable::writeToMatlab ( ) const
inlinevirtualinherited

Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.

Returns
A new mxArray (caller is responsible of memory freeing) or nullptr is class does not support conversion to MATLAB.

Definition at line 90 of file CSerializable.h.

Member Data Documentation

◆ cameraParams

mrpt::img::TCamera mrpt::obs::CObservation3DRangeScan::cameraParams

◆ cameraParamsIntensity

mrpt::img::TCamera mrpt::obs::CObservation3DRangeScan::cameraParamsIntensity

◆ className

constexpr const char* mrpt::obs::CObservation3DRangeScan::className = "mrpt::obs" "::" "CObservation3DRangeScan"
static

Definition at line 170 of file CObservation3DRangeScan.h.

◆ confidenceImage

mrpt::img::CImage mrpt::obs::CObservation3DRangeScan::confidenceImage

◆ hasConfidenceImage

bool mrpt::obs::CObservation3DRangeScan::hasConfidenceImage {false}

true means the field confidenceImage contains valid data

Definition at line 494 of file CObservation3DRangeScan.h.

Referenced by mrpt::detectors::CFaceDetection::checkIfFacePlaneCov(), mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation(), getZoneAsObs(), and swap().

◆ hasIntensityImage

bool mrpt::obs::CObservation3DRangeScan::hasIntensityImage {false}

◆ hasPoints3D

bool mrpt::obs::CObservation3DRangeScan::hasPoints3D {false}

◆ hasRangeImage

bool mrpt::obs::CObservation3DRangeScan::hasRangeImage {false}

◆ intensityImage

mrpt::img::CImage mrpt::obs::CObservation3DRangeScan::intensityImage

◆ intensityImageChannel

TIntensityChannelID mrpt::obs::CObservation3DRangeScan::intensityImageChannel {CH_VISIBLE}

The source of the intensityImage; typically the visible channel.

See also
TIntensityChannelID

Definition at line 488 of file CObservation3DRangeScan.h.

Referenced by getZoneAsObs(), and swap().

◆ m_points3D_external_file

std::string mrpt::obs::CObservation3DRangeScan::m_points3D_external_file
protected

3D points are in CImage::getImagesPathBase()+<this_file_name>

Definition at line 176 of file CObservation3DRangeScan.h.

Referenced by points3D_getExternalStorageFile(), and swap().

◆ m_points3D_external_stored

bool mrpt::obs::CObservation3DRangeScan::m_points3D_external_stored {false}
protected

If set to true, m_points3D_external_file is valid.

Definition at line 174 of file CObservation3DRangeScan.h.

Referenced by points3D_isExternallyStored(), points3D_overrideExternalStoredFlag(), and swap().

◆ m_rangeImage_external_file

std::string mrpt::obs::CObservation3DRangeScan::m_rangeImage_external_file
protected

rangeImage is in CImage::getImagesPathBase()+<this_file_name>

Definition at line 181 of file CObservation3DRangeScan.h.

Referenced by swap().

◆ m_rangeImage_external_stored

bool mrpt::obs::CObservation3DRangeScan::m_rangeImage_external_stored {false}
protected

If set to true, m_rangeImage_external_file is valid.

Definition at line 179 of file CObservation3DRangeScan.h.

Referenced by rangeImage_forceResetExternalStorage(), rangeImage_isExternallyStored(), and swap().

◆ maxRange

float mrpt::obs::CObservation3DRangeScan::maxRange {5.0f}

The maximum range allowed by the device, in meters (e.g.

8.0m, 5.0m,...)

Definition at line 544 of file CObservation3DRangeScan.h.

Referenced by mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation(), getZoneAsObs(), mrpt::maps::COccupancyGridMap3D::internal_insertObservationScan3D(), rangeImage_getAsImage(), and swap().

◆ pixelLabels

TPixelLabelInfoBase::Ptr mrpt::obs::CObservation3DRangeScan::pixelLabels

All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelLabelInfo for details on the contents User is responsible of creating a new object of the desired data type.

It will be automatically (de)serialized no matter its specific type.

Definition at line 513 of file CObservation3DRangeScan.h.

Referenced by hasPixelLabels(), and swap().

◆ points3D_idxs_x

std::vector<uint16_t> mrpt::obs::CObservation3DRangeScan::points3D_idxs_x

If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z.

Definition at line 332 of file CObservation3DRangeScan.h.

Referenced by mempool_donate_xyz_buffers(), mrpt::obs::detail::range2XYZ_LUT(), and swap().

◆ points3D_idxs_y

std::vector<uint16_t> mrpt::obs::CObservation3DRangeScan::points3D_idxs_y

◆ points3D_x

std::vector<float> mrpt::obs::CObservation3DRangeScan::points3D_x

◆ points3D_y

std::vector<float> mrpt::obs::CObservation3DRangeScan::points3D_y

◆ points3D_z

std::vector<float> mrpt::obs::CObservation3DRangeScan::points3D_z

◆ range_is_depth

bool mrpt::obs::CObservation3DRangeScan::range_is_depth {true}

true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in rangeImage are actual distances in 3D.

Definition at line 399 of file CObservation3DRangeScan.h.

Referenced by mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation(), and TEST().

◆ rangeImage

mrpt::math::CMatrix_u16 mrpt::obs::CObservation3DRangeScan::rangeImage

If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters).

For sensors with multiple returns per pixels, this matrix holds the CLOSEST of all the returns.

See also
range_is_depth, rangeUnits, rangeImageOtherLayers

Definition at line 384 of file CObservation3DRangeScan.h.

Referenced by mrpt::obs::detail::cost_func(), fillSampleObs(), mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation(), mrpt::hwdrivers::CKinect::getNextObservation(), getZoneAsObs(), mempool_donate_range_matrix(), mrpt::obs::detail::range2XYZ_LUT(), rangeImage_getAsImage(), recoverCameraCalibrationParameters(), swap(), TEST(), undistort(), and mrpt::obs::detail::unprojectInto().

◆ rangeImageOtherLayers

std::map<std::string, mrpt::math::CMatrix_u16> mrpt::obs::CObservation3DRangeScan::rangeImageOtherLayers

Additional layer range/depth images.

Text labels are arbitrary and sensor-dependent, e.g. "LAST", "SECOND", "3rd", etc.

Definition at line 388 of file CObservation3DRangeScan.h.

Referenced by fillSampleObs(), mrpt::obs::detail::range2XYZ_LUT(), rangeImage_getAsImage(), swap(), undistort(), and mrpt::obs::detail::unprojectInto().

◆ rangeUnits

float mrpt::obs::CObservation3DRangeScan::rangeUnits = 0.001f

The conversion factor from integer units in rangeImage and actual distances in meters.

Default is 0.001 m, that is 1 millimeter.

See also
rangeImage

Definition at line 393 of file CObservation3DRangeScan.h.

Referenced by fillSampleObs(), mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation(), mrpt::obs::detail::range2XYZ_LUT(), rangeImage_getAsImage(), rangeImageAsImage(), and TEST().

◆ relativePoseIntensityWRTDepth

mrpt::poses::CPose3D mrpt::obs::CObservation3DRangeScan::relativePoseIntensityWRTDepth
Initial value:
= {
0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg}

Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this observation).

In a SwissRanger camera, this will be (0,0,0,-90deg,0,-90deg) since both cameras coincide. In a Kinect, this will include a small lateral displacement and a rotation, according to the drawing on the top of this page.

See also
doDepthAndIntensityCamerasCoincide

Definition at line 533 of file CObservation3DRangeScan.h.

Referenced by mrpt::hwdrivers::CKinect::getNextObservation(), mrpt::maps::detail::pointmap_traits< CColouredPointsMap >::internal_loadFromRangeScan3D_init(), mrpt::maps::detail::pointmap_traits< CColouredPointsMap >::internal_loadFromRangeScan3D_prepareOneRange(), and swap().

◆ runtimeClassId

const mrpt::rtti::TRuntimeClassId mrpt::obs::CObservation3DRangeScan::runtimeClassId
staticprotected

Definition at line 170 of file CObservation3DRangeScan.h.

◆ sensorLabel

std::string mrpt::obs::CObservation::sensorLabel
inherited

◆ sensorPose

mrpt::poses::CPose3D mrpt::obs::CObservation3DRangeScan::sensorPose

◆ stdError

float mrpt::obs::CObservation3DRangeScan::stdError {0.01f}

The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.

Definition at line 549 of file CObservation3DRangeScan.h.

Referenced by mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation(), getZoneAsObs(), and swap().

◆ timestamp

mrpt::system::TTimeStamp mrpt::obs::CObservation::timestamp {mrpt::system::now()}
inherited

The associated UTC time-stamp.

Where available, this should contain the accurate satellite-based timestamp of the sensor reading.

See also
getOriginalReceivedTimeStamp(), getTimeStamp()

Definition at line 60 of file CObservation.h.

Referenced by mrpt::ros1bridge::convert(), convertTo2DScan(), mrpt::hwdrivers::CSICKTim561Eth::decodeScan(), mrpt::hwdrivers::CLMS100Eth::decodeScan(), mrpt::detectors::CObjectDetection::detectObjects(), mrpt::hwdrivers::CRaePID::doProcess(), mrpt::hwdrivers::CGPSInterface::doProcess(), mrpt::hwdrivers::CRoboPeakLidar::doProcessSimple(), mrpt::hwdrivers::CSickLaserUSB::doProcessSimple(), mrpt::hwdrivers::CCANBusReader::doProcessSimple(), mrpt::hwdrivers::CSickLaserSerial::doProcessSimple(), mrpt::hwdrivers::CHokuyoURG::doProcessSimple(), mrpt::hwdrivers::CGPSInterface::flushParsedMessagesNow(), mrpt::ros1bridge::fromROS(), mrpt::obs::CObservationRotatingScan::fromScan2D(), mrpt::obs::CObservationRotatingScan::fromVelodyne(), mrpt::hwdrivers::CRaePID::getFullInfo(), mrpt::hwdrivers::CSwissRanger3DCamera::getNextObservation(), mrpt::hwdrivers::COpenNI2_RGBD360::getNextObservation(), mrpt::hwdrivers::CKinect::getNextObservation(), mrpt::hwdrivers::CBoardSonars::getObservation(), mrpt::hwdrivers::CWirelessPower::getObservation(), mrpt::hwdrivers::CImageGrabber_OpenCV::getObservation(), mrpt::hwdrivers::CEnoseModular::getObservation(), mrpt::hwdrivers::CBoardENoses::getObservation(), mrpt::hwdrivers::CImageGrabber_dc1394::getObservation(), mrpt::hwdrivers::CImageGrabber_FlyCapture2::getObservation(), mrpt::hwdrivers::CDUO3DCamera::getObservations(), mrpt::obs::CObservation::getOriginalReceivedTimeStamp(), mrpt::obs::CObservation::getTimeStamp(), mrpt::hwdrivers::CNationalInstrumentsDAQ::grabbing_thread(), mrpt::maps::CLandmarksMap::loadOccupancyFeaturesFrom2DRangeScan(), mrpt::maps::CLandmarksMap::loadSiftFeaturesFromImageObservation(), mrpt::maps::CLandmarksMap::loadSiftFeaturesFromStereoImageObservation(), mrpt::hwdrivers::CGPSInterface::parse_NMEA(), mrpt::apps::KFSLAMApp::Run_KF_SLAM(), mrpt::obs::CObservation6DFeatures::serializeFrom(), mrpt::obs::CObservationPointCloud::serializeFrom(), mrpt::obs::CObservation6DFeatures::serializeTo(), mrpt::obs::CObservationPointCloud::serializeTo(), mrpt::maps::CLandmarksMap::simulateBeaconReadings(), mrpt::maps::CLandmarksMap::simulateRangeBearingReadings(), mrpt::obs::CObservation::swap(), mrpt::ros1bridge::toROS(), and velodyne_scan_to_pointcloud().




Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020