A CObservation
-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LIDAR scanners.
A scan comprises one or more "velodyne packets" (refer to Velodyne user manual).
Axes convention for point cloud (x,y,z) coordinates:
If it can be assumed that the sensor moves SLOWLY through the environment (i.e. its pose can be approximated to be the same since the beginning to the end of one complete scan) then this observation can be converted / loaded into the following other classes:
Otherwise, the following API exists for accurate reconstruction of the sensor path in SE(3) over time:
Note that this object has two timestamp fields:
Definition at line 56 of file CObservationVelodyneScan.h.
#include <mrpt/obs/CObservationVelodyneScan.h>
Classes | |
struct | laser_return_t |
struct | raw_block_t |
Raw Velodyne data block. More... | |
struct | TGeneratePointCloudParameters |
struct | TGeneratePointCloudSE3Results |
Results for generatePointCloudAlongSE3Trajectory() More... | |
struct | TPointCloud |
See point_cloud and scan_packets. More... | |
struct | TVelodynePositionPacket |
Payload of one POSITION packet. More... | |
struct | TVelodyneRawPacket |
One unit of data from the scanner (the payload of one UDP DATA packet) More... | |
Public Member Functions | |
CObservationVelodyneScan () | |
virtual | ~CObservationVelodyneScan () |
void | getSensorPose (mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE |
A general method to retrieve the sensor pose on the robot. | |
void | setSensorPose (const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE |
A general method to change the sensor pose on the robot. | |
void | getDescriptionAsText (std::ostream &o) const MRPT_OVERRIDE |
Build a detailed, multi-line textual description of the observation contents and dump it to the output stream. | |
template<class METRICMAP > | |
bool | insertObservationInto (METRICMAP *theMap, const mrpt::poses::CPose3D *robotPose=NULL) const |
This method is equivalent to: | |
void | getSensorPose (mrpt::math::TPose3D &out_sensorPose) const |
A general method to retrieve the sensor pose on the robot. | |
void | setSensorPose (const mrpt::math::TPose3D &newSensorPose) |
A general method to change the sensor pose on the robot. | |
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 Attributes | |
Raw scan fixed parameters | |
static const int | SIZE_BLOCK = 100 |
static const int | RAW_SCAN_SIZE = 3 |
static const int | SCANS_PER_BLOCK = 32 |
static const int | BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE) |
static const float | ROTATION_RESOLUTION |
static const uint16_t | ROTATION_MAX_UNITS = 36000 |
hundredths of degrees | |
static const float | DISTANCE_MAX |
meters | |
static const float | DISTANCE_RESOLUTION |
meters | |
static const float | DISTANCE_MAX_UNITS |
static const uint16_t | UPPER_BANK = 0xeeff |
Blocks 0-31. | |
static const uint16_t | LOWER_BANK = 0xddff |
Blocks 32-63. | |
static const int | PACKET_SIZE = 1206 |
static const int | POS_PACKET_SIZE = 512 |
static const int | BLOCKS_PER_PACKET = 12 |
static const int | PACKET_STATUS_SIZE = 4 |
static const int | SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET) |
static const uint8_t | RETMODE_STRONGEST = 0x37 |
static const uint8_t | RETMODE_LAST = 0x38 |
static const uint8_t | RETMODE_DUAL = 0x39 |
RTTI stuff <br> | |
static const mrpt::utils::TRuntimeClassId | classCObservation |
Protected Member Functions | |
void | swap (CObservation &o) |
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. | |
CSerializable virtual methods | |
void | writeToStream (mrpt::utils::CStream &out, int *getVersion) const MRPT_OVERRIDE |
void | readFromStream (mrpt::utils::CStream &in, int version) MRPT_OVERRIDE |
RTTI stuff <br> | |
typedef CObservationVelodyneScanPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CObservationVelodyneScan |
static mrpt::utils::TRuntimeClassId | classCObservationVelodyneScan |
static const mrpt::utils::TRuntimeClassId * | classinfo |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const MRPT_OVERRIDE |
virtual mrpt::utils::CObject * | duplicate () const MRPT_OVERRIDE |
static mrpt::utils::CObject * | CreateObject () |
static CObservationVelodyneScanPtr | Create () |
Related to conversion to 3D point cloud | |
static const TGeneratePointCloudParameters | defaultPointCloudParams |
void | generatePointCloud (const TGeneratePointCloudParameters ¶ms=defaultPointCloudParams) |
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud where it is stored in local coordinates wrt the sensor (neither the vehicle nor the world). | |
void | generatePointCloudAlongSE3Trajectory (const mrpt::poses::CPose3DInterpolator &vehicle_path, std::vector< mrpt::math::TPointXYZIu8 > &out_points, TGeneratePointCloudSE3Results &results_stats, const TGeneratePointCloudParameters ¶ms=defaultPointCloudParams) |
An alternative to generatePointCloud() for cases where the motion of the sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored. | |
Scan data | |
double | minRange |
double | maxRange |
The maximum range allowed by the device, in meters (e.g. 100m). Stored here by the driver while capturing based on the sensor model. | |
mrpt::poses::CPose3D | sensorPose |
The 6D pose of the sensor on the robot/vehicle frame of reference. | |
std::vector< TVelodyneRawPacket > | scan_packets |
The main content of this object: raw data packet from the LIDAR. | |
mrpt::obs::VelodyneCalibration | calibration |
The calibration data for the LIDAR device. See mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::VelodyneCalibration for details. | |
mrpt::system::TTimeStamp | originalReceivedTimestamp |
The local computer-based timestamp based on the reception of the message in the computer. | |
bool | has_satellite_timestamp |
If true, CObservation::timestamp has been generated from accurate satellite clock. Otherwise, no GPS data is available and timestamps are based on the local computer clock. | |
TPointCloud | point_cloud |
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor, not the vehicle) with intensity (graylevel) information. | |
mrpt::system::TTimeStamp | getOriginalReceivedTimeStamp () const MRPT_OVERRIDE |
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. | |
Data common to any observation | |
mrpt::system::TTimeStamp | timestamp |
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based timestamp of the sensor reading. | |
std::string | sensorLabel |
An arbitrary label that can be used to identify the sensor. | |
mrpt::system::TTimeStamp | getTimeStamp () const |
Returns CObservation::timestamp for all kind of observations. | |
A typedef for the associated smart pointer
Definition at line 59 of file CObservationVelodyneScan.h.
mrpt::obs::CObservationVelodyneScan::CObservationVelodyneScan | ( | ) |
|
virtual |
|
staticprotected |
|
static |
|
static |
|
virtual |
void mrpt::obs::CObservationVelodyneScan::generatePointCloud | ( | const TGeneratePointCloudParameters & | params = defaultPointCloudParams | ) |
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud where it is stored in local coordinates wrt the sensor (neither the vehicle nor the world).
So, this method does not take into account the possible motion of the sensor through the world as it collects LIDAR scans. For high dynamics, see the more costly API generatePointCloudAlongSE3Trajectory()
void mrpt::obs::CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory | ( | const mrpt::poses::CPose3DInterpolator & | vehicle_path, |
std::vector< mrpt::math::TPointXYZIu8 > & | out_points, | ||
TGeneratePointCloudSE3Results & | results_stats, | ||
const TGeneratePointCloudParameters & | params = defaultPointCloudParams |
||
) |
An alternative to generatePointCloud() for cases where the motion of the sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored.
[in] | vehicle_path | Timestamped sequence of known poses for the VEHICLE. Recall that the sensor has a relative pose wrt the vehicle according to CObservationVelodyneScan::getSensorPose() & CObservationVelodyneScan::setSensorPose() |
[out] | out_points | The generated points, in the same coordinates frame than vehicle_path. Points are APPENDED to the list, so prevous contents are kept. |
[out] | results_stats | Stats |
[in] | params | Filtering and other parameters |
|
virtual |
Build a detailed, multi-line textual description of the observation contents and dump it to the output stream.
Reimplemented from mrpt::obs::CObservation.
|
virtual |
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.
Reimplemented from mrpt::obs::CObservation.
|
virtual |
Reimplemented from mrpt::obs::CObservation.
|
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.
|
inlinevirtual |
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::obs::CObservation.
Definition at line 223 of file CObservationVelodyneScan.h.
|
inlineinherited |
Returns CObservation::timestamp for all kind of observations.
Definition at line 63 of file obs/CObservation.h.
|
inlineinherited |
This method is equivalent to:
theMap | The map where this observation is to be inserted: the map will be updated. |
robotPose | The pose of the robot base for this observation, relative to the target metric map. Set to NULL (default) to use (0,0,0deg) |
Definition at line 83 of file obs/CObservation.h.
|
inlinevirtualinherited |
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 in mrpt::obs::CObservation3DRangeScan.
Definition at line 125 of file obs/CObservation.h.
|
protected |
|
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.
|
inlinevirtual |
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::obs::CObservation.
Definition at line 224 of file CObservationVelodyneScan.h.
|
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.
|
inlinevirtualinherited |
Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect).
Reimplemented in mrpt::obs::CObservation3DRangeScan.
Definition at line 129 of file obs/CObservation.h.
|
protected |
|
staticprotected |
Definition at line 59 of file CObservationVelodyneScan.h.
|
static |
Definition at line 70 of file CObservationVelodyneScan.h.
|
static |
Definition at line 84 of file CObservationVelodyneScan.h.
mrpt::obs::VelodyneCalibration mrpt::obs::CObservationVelodyneScan::calibration |
The calibration data for the LIDAR device. See mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::VelodyneCalibration for details.
Definition at line 133 of file CObservationVelodyneScan.h.
|
staticinherited |
Definition at line 50 of file obs/CObservation.h.
|
static |
Definition at line 59 of file CObservationVelodyneScan.h.
|
static |
Definition at line 59 of file CObservationVelodyneScan.h.
|
static |
Definition at line 189 of file CObservationVelodyneScan.h.
|
static |
meters
Definition at line 75 of file CObservationVelodyneScan.h.
|
static |
Definition at line 77 of file CObservationVelodyneScan.h.
|
static |
meters
Definition at line 76 of file CObservationVelodyneScan.h.
bool mrpt::obs::CObservationVelodyneScan::has_satellite_timestamp |
If true, CObservation::timestamp has been generated from accurate satellite clock. Otherwise, no GPS data is available and timestamps are based on the local computer clock.
Definition at line 135 of file CObservationVelodyneScan.h.
|
static |
Blocks 32-63.
Definition at line 80 of file CObservationVelodyneScan.h.
double mrpt::obs::CObservationVelodyneScan::maxRange |
The maximum range allowed by the device, in meters (e.g. 100m). Stored here by the driver while capturing based on the sensor model.
Definition at line 130 of file CObservationVelodyneScan.h.
double mrpt::obs::CObservationVelodyneScan::minRange |
Definition at line 130 of file CObservationVelodyneScan.h.
mrpt::system::TTimeStamp mrpt::obs::CObservationVelodyneScan::originalReceivedTimestamp |
The local computer-based timestamp based on the reception of the message in the computer.
Definition at line 134 of file CObservationVelodyneScan.h.
|
static |
Definition at line 82 of file CObservationVelodyneScan.h.
|
static |
Definition at line 85 of file CObservationVelodyneScan.h.
TPointCloud mrpt::obs::CObservationVelodyneScan::point_cloud |
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor, not the vehicle) with intensity (graylevel) information.
See axes convention in mrpt::obs::CObservationVelodyneScan
Definition at line 165 of file CObservationVelodyneScan.h.
|
static |
Definition at line 83 of file CObservationVelodyneScan.h.
|
static |
Definition at line 68 of file CObservationVelodyneScan.h.
|
static |
Definition at line 90 of file CObservationVelodyneScan.h.
|
static |
Definition at line 89 of file CObservationVelodyneScan.h.
|
static |
Definition at line 88 of file CObservationVelodyneScan.h.
|
static |
hundredths of degrees
Definition at line 73 of file CObservationVelodyneScan.h.
|
static |
Definition at line 72 of file CObservationVelodyneScan.h.
std::vector<TVelodyneRawPacket> mrpt::obs::CObservationVelodyneScan::scan_packets |
The main content of this object: raw data packet from the LIDAR.
Definition at line 132 of file CObservationVelodyneScan.h.
|
static |
Definition at line 69 of file CObservationVelodyneScan.h.
|
static |
Definition at line 86 of file CObservationVelodyneScan.h.
|
inherited |
An arbitrary label that can be used to identify the sensor.
Definition at line 60 of file obs/CObservation.h.
mrpt::poses::CPose3D mrpt::obs::CObservationVelodyneScan::sensorPose |
The 6D pose of the sensor on the robot/vehicle frame of reference.
Definition at line 131 of file CObservationVelodyneScan.h.
|
static |
Definition at line 67 of file CObservationVelodyneScan.h.
|
inherited |
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based timestamp of the sensor reading.
Definition at line 59 of file obs/CObservation.h.
|
static |
Blocks 0-31.
Definition at line 79 of file CObservationVelodyneScan.h.
Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 14:27:49 UTC 2023 |