Declares a class derived from "CObservation" that encapsules a 3D range scan measurement (e.g.
from a time of flight range camera). 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. 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:
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 (in some cameras, like SwissRanger, both are the same):
3D point clouds can be generated at any moment after grabbing with CObservation3DRangeScan::project3DPointsFromDepthImage()
Definition at line 90 of file CObservation3DRangeScan.h.
#include <mrpt/slam/CObservation3DRangeScan.h>
Public Member Functions | |
CObservation3DRangeScan () | |
Default constructor. | |
virtual | ~CObservation3DRangeScan () |
Destructor. | |
void | project3DPointsFromDepthImage () |
Compute the 3D points coordinates from the depth image (rangeImage) and the depth camera camera parameters (cameraParams). | |
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. | |
bool | rangeImage_isExternallyStored () const |
std::string | rangeImage_getExternalStorageFile () const |
void | rangeImage_getExternalStorageFileAbsolutePath (std::string &out_path) const |
std::string | rangeImage_getExternalStorageFileAbsolutePath () 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. | |
void | getSensorPose (CPose3D &out_sensorPose) const |
A general method to retrieve the sensor pose on the robot. | |
void | setSensorPose (const CPose3D &newSensorPose) |
A general method to change the sensor pose on the robot. | |
void | swap (CObservation3DRangeScan &o) |
Very efficient method to swap the contents of two observations. | |
void | getZoneAsObs (CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2) |
Delayed-load manual control methods. | |
virtual void | load () const |
Makes sure all images and other fields which may be externally stored are loaded in memory. | |
virtual void | unload () |
Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect). | |
Static Public Member Functions | |
static double | recoverCameraCalibrationParameters (const CObservation3DRangeScan &in_obs, mrpt::utils::TCamera &out_camParams, const double camera_offset=0.01) |
A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a range (depth) image and the corresponding 3D point cloud. | |
Public Attributes | |
bool | hasPoints3D |
true means the field points3D contains valid data. | |
std::vector< float > | points3D_x |
If hasPoints3D=true, the X coordinates of the 3D point cloud detected by the camera. | |
std::vector< float > | points3D_y |
If hasPoints3D=true, the Y coordinates of the 3D point cloud detected by the camera. | |
std::vector< float > | points3D_z |
If hasPoints3D=true, the Z coordinates of the 3D point cloud detected by the camera. | |
bool | hasRangeImage |
true means the field rangeImage contains valid data | |
mrpt::math::CMatrix | rangeImage |
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters). | |
bool | hasIntensityImage |
true means the field intensityImage contains valid data | |
mrpt::utils::CImage | intensityImage |
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage". | |
bool | hasConfidenceImage |
true means the field confidenceImage contains valid data | |
mrpt::utils::CImage | confidenceImage |
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the capture drivers. | |
mrpt::utils::TCamera | cameraParams |
Projection parameters of the depth camera. | |
mrpt::utils::TCamera | cameraParamsIntensity |
Projection parameters of the intensity (graylevel or RGB) camera. | |
mrpt::poses::CPose3D | relativePoseIntensityWRTDepth |
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this observation). | |
float | maxRange |
The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...) | |
CPose3D | sensorPose |
The 6D pose of the sensor on the robot. | |
float | stdError |
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid. | |
Protected Attributes | |
bool | m_points3D_external_stored |
If set to true, m_points3D_external_file is valid. | |
std::string | m_points3D_external_file |
3D points are in CImage::IMAGES_PATH_BASE+<this_file_name> | |
bool | m_rangeImage_external_stored |
If set to true, m_rangeImage_external_file is valid. | |
std::string | m_rangeImage_external_file |
rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name> | |
RTTI stuff | |
typedef CObservation3DRangeScanPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CObservation3DRangeScan |
static mrpt::utils::TRuntimeClassId | classCObservation3DRangeScan |
static const mrpt::utils::TRuntimeClassId * | classinfo |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const |
Returns information about the class of an object in runtime. | |
virtual mrpt::utils::CObject * | duplicate () const |
Returns a copy of the object, indepently of its class. | |
static mrpt::utils::CObject * | CreateObject () |
static CObservation3DRangeScanPtr | Create () |
A typedef for the associated smart pointer
Definition at line 93 of file CObservation3DRangeScan.h.
mrpt::slam::CObservation3DRangeScan::CObservation3DRangeScan | ( | ) |
Default constructor.
virtual mrpt::slam::CObservation3DRangeScan::~CObservation3DRangeScan | ( | ) | [virtual] |
Destructor.
static const mrpt::utils::TRuntimeClassId* mrpt::slam::CObservation3DRangeScan::_GetBaseClass | ( | ) | [static, protected] |
Reimplemented from mrpt::slam::CObservation.
static CObservation3DRangeScanPtr mrpt::slam::CObservation3DRangeScan::Create | ( | ) | [static] |
static mrpt::utils::CObject* mrpt::slam::CObservation3DRangeScan::CreateObject | ( | ) | [static] |
virtual mrpt::utils::CObject* mrpt::slam::CObservation3DRangeScan::duplicate | ( | ) | const [virtual] |
Returns a copy of the object, indepently of its class.
Implements mrpt::utils::CObject.
virtual const mrpt::utils::TRuntimeClassId* mrpt::slam::CObservation3DRangeScan::GetRuntimeClass | ( | ) | const [virtual] |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::slam::CObservation.
void mrpt::slam::CObservation3DRangeScan::getSensorPose | ( | CPose3D & | out_sensorPose | ) | const [inline, virtual] |
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.
Implements mrpt::slam::CObservation.
Definition at line 187 of file CObservation3DRangeScan.h.
void mrpt::slam::CObservation3DRangeScan::getZoneAsObs | ( | CObservation3DRangeScan & | obs, |
const unsigned int & | r1, | ||
const unsigned int & | r2, | ||
const unsigned int & | c1, | ||
const unsigned int & | c2 | ||
) |
virtual void mrpt::slam::CObservation3DRangeScan::load | ( | ) | const [virtual] |
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.
Reimplemented from mrpt::slam::CObservation.
void mrpt::slam::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.
std::string mrpt::slam::CObservation3DRangeScan::points3D_getExternalStorageFile | ( | ) | const [inline] |
Definition at line 137 of file CObservation3DRangeScan.h.
void mrpt::slam::CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath | ( | std::string & | out_path | ) | const |
std::string mrpt::slam::CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath | ( | ) | const [inline] |
Definition at line 139 of file CObservation3DRangeScan.h.
bool mrpt::slam::CObservation3DRangeScan::points3D_isExternallyStored | ( | ) | const [inline] |
Definition at line 136 of file CObservation3DRangeScan.h.
void mrpt::slam::CObservation3DRangeScan::project3DPointsFromDepthImage | ( | ) |
Compute the 3D points coordinates from the depth image (rangeImage) and the depth camera camera parameters (cameraParams).
The formulas for the i'th point, with rangeImage pixel coordinates (r,c) are:
x(i) = rangeImage(r,c) y(i) = (r_cx - c) * x(i) / r_fy z(i) = (r_cy - r) * x(i) / r_fx
void mrpt::slam::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.
std::string mrpt::slam::CObservation3DRangeScan::rangeImage_getExternalStorageFile | ( | ) | const [inline] |
Definition at line 152 of file CObservation3DRangeScan.h.
std::string mrpt::slam::CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath | ( | ) | const [inline] |
Definition at line 154 of file CObservation3DRangeScan.h.
void mrpt::slam::CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath | ( | std::string & | out_path | ) | const |
bool mrpt::slam::CObservation3DRangeScan::rangeImage_isExternallyStored | ( | ) | const [inline] |
Definition at line 151 of file CObservation3DRangeScan.h.
static double mrpt::slam::CObservation3DRangeScan::recoverCameraCalibrationParameters | ( | const CObservation3DRangeScan & | in_obs, |
mrpt::utils::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.
camera_offset | The offset (in meters) in the +X direction of the point cloud. It's 1cm for SwissRanger SR4000. |
void mrpt::slam::CObservation3DRangeScan::setSensorPose | ( | const CPose3D & | newSensorPose | ) | [inline, virtual] |
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.
Implements mrpt::slam::CObservation.
Definition at line 193 of file CObservation3DRangeScan.h.
void mrpt::slam::CObservation3DRangeScan::swap | ( | CObservation3DRangeScan & | o | ) |
Very efficient method to swap the contents of two observations.
virtual void mrpt::slam::CObservation3DRangeScan::unload | ( | ) | [virtual] |
Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect).
Reimplemented from mrpt::slam::CObservation.
mrpt::utils::CLASSINIT mrpt::slam::CObservation3DRangeScan::_init_CObservation3DRangeScan [static, protected] |
Definition at line 93 of file CObservation3DRangeScan.h.
Projection parameters of the depth camera.
Definition at line 168 of file CObservation3DRangeScan.h.
Projection parameters of the intensity (graylevel or RGB) camera.
Definition at line 169 of file CObservation3DRangeScan.h.
mrpt::utils::TRuntimeClassId mrpt::slam::CObservation3DRangeScan::classCObservation3DRangeScan [static] |
Definition at line 93 of file CObservation3DRangeScan.h.
Definition at line 93 of file CObservation3DRangeScan.h.
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the capture drivers.
Definition at line 166 of file CObservation3DRangeScan.h.
true means the field confidenceImage contains valid data
Definition at line 165 of file CObservation3DRangeScan.h.
true means the field intensityImage contains valid data
Definition at line 162 of file CObservation3DRangeScan.h.
true means the field points3D contains valid data.
Definition at line 130 of file CObservation3DRangeScan.h.
true means the field rangeImage contains valid data
Definition at line 147 of file CObservation3DRangeScan.h.
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
Definition at line 163 of file CObservation3DRangeScan.h.
std::string mrpt::slam::CObservation3DRangeScan::m_points3D_external_file [protected] |
3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
Definition at line 97 of file CObservation3DRangeScan.h.
bool mrpt::slam::CObservation3DRangeScan::m_points3D_external_stored [protected] |
If set to true, m_points3D_external_file is valid.
Definition at line 96 of file CObservation3DRangeScan.h.
std::string mrpt::slam::CObservation3DRangeScan::m_rangeImage_external_file [protected] |
rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
Definition at line 100 of file CObservation3DRangeScan.h.
bool mrpt::slam::CObservation3DRangeScan::m_rangeImage_external_stored [protected] |
If set to true, m_rangeImage_external_file is valid.
Definition at line 99 of file CObservation3DRangeScan.h.
The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
Definition at line 178 of file CObservation3DRangeScan.h.
std::vector<float> mrpt::slam::CObservation3DRangeScan::points3D_x |
If hasPoints3D=true, the X coordinates of the 3D point cloud detected by the camera.
Definition at line 131 of file CObservation3DRangeScan.h.
std::vector<float> mrpt::slam::CObservation3DRangeScan::points3D_y |
If hasPoints3D=true, the Y coordinates of the 3D point cloud detected by the camera.
Definition at line 132 of file CObservation3DRangeScan.h.
std::vector<float> mrpt::slam::CObservation3DRangeScan::points3D_z |
If hasPoints3D=true, the Z coordinates of the 3D point cloud detected by the camera.
Definition at line 133 of file CObservation3DRangeScan.h.
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters).
Definition at line 148 of file CObservation3DRangeScan.h.
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,0,0,0) 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.
Definition at line 175 of file CObservation3DRangeScan.h.
The 6D pose of the sensor on the robot.
Definition at line 179 of file CObservation3DRangeScan.h.
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
Definition at line 180 of file CObservation3DRangeScan.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011 |