48 return originalReceivedTimestamp;
51 uint8_t CObservationVelodyneScan::serializeGetVersion()
const {
return 1; }
52 void CObservationVelodyneScan::serializeTo(
55 out << timestamp << sensorLabel;
56 out << minRange << maxRange << sensorPose;
58 if (!scan_packets.empty())
60 &scan_packets[0],
sizeof(scan_packets[0]) * scan_packets.size());
62 if (!calibration.laser_corrections.empty())
64 &calibration.laser_corrections[0],
65 sizeof(calibration.laser_corrections[0]) *
66 calibration.laser_corrections.size());
67 out << point_cloud.x << point_cloud.y << point_cloud.z
68 << point_cloud.intensity;
69 out << has_satellite_timestamp;
72 void CObservationVelodyneScan::serializeFrom(
80 in >> timestamp >> sensorLabel;
82 in >> minRange >> maxRange >> sensorPose;
86 scan_packets.resize(N);
89 &scan_packets[0],
sizeof(scan_packets[0]) * N);
94 calibration.laser_corrections.resize(N);
97 &calibration.laser_corrections[0],
98 sizeof(calibration.laser_corrections[0]) * N);
100 in >> point_cloud.x >> point_cloud.y >> point_cloud.z >>
101 point_cloud.intensity;
103 in >> has_satellite_timestamp;
105 has_satellite_timestamp =
106 (this->timestamp != this->originalReceivedTimestamp);
116 void CObservationVelodyneScan::getDescriptionAsText(std::ostream& o)
const 118 CObservation::getDescriptionAsText(o);
119 o <<
"Homogeneous matrix for the sensor 3D pose, relative to robot base:\n";
122 << sensorPose << endl;
123 o <<
format(
"Sensor min/max range: %.02f / %.02f m\n", minRange, maxRange);
124 o <<
"Raw packet count: " << scan_packets.size() <<
"\n";
145 virtual void add_point(
146 double pt_x,
double pt_y,
double pt_z,
uint8_t pt_intensity,
162 scan_props.
nRays = CObservationVelodyneScan::ROTATION_MAX_UNITS;
169 const int minAzimuth_int =
round(
params.minAzimuth_deg * 100);
170 const int maxAzimuth_int =
round(
params.maxAzimuth_deg * 100);
171 const float realMinDist =
173 const float realMaxDist =
175 const int16_t isolatedPointsFilterDistance_units =
176 params.isolatedPointsFilterDistance /
177 CObservationVelodyneScan::DISTANCE_RESOLUTION;
182 for (
size_t iPkt = 0; iPkt < scan.
scan_packets.size(); iPkt++)
193 (us_pkt_this >= us_pkt0)
194 ? (us_pkt_this - us_pkt0)
195 : (1000000UL * 3600UL + us_pkt_this - us_pkt0);
202 int median_azimuth_diff;
206 const int nBlocksPerAzimuth =
208 CObservationVelodyneScan::RETMODE_DUAL)
211 std::vector<int> diffs(
212 CObservationVelodyneScan::BLOCKS_PER_PACKET -
214 for (
int i = 0; i < CObservationVelodyneScan::BLOCKS_PER_PACKET -
218 int localDiff = (CObservationVelodyneScan::ROTATION_MAX_UNITS +
221 CObservationVelodyneScan::ROTATION_MAX_UNITS;
222 diffs[i] = localDiff;
226 diffs.begin() + CObservationVelodyneScan::BLOCKS_PER_PACKET / 2,
228 median_azimuth_diff =
229 diffs[CObservationVelodyneScan::BLOCKS_PER_PACKET / 2];
232 for (
int block = 0; block < CObservationVelodyneScan::BLOCKS_PER_PACKET;
236 if ((num_lasers != 64 &&
237 CObservationVelodyneScan::UPPER_BANK !=
240 CObservationVelodyneScan::UPPER_BANK &&
242 CObservationVelodyneScan::LOWER_BANK))
244 cerr <<
"[CObservationVelodyneScan] skipping invalid packet: " 246 << block <<
" header value is " 252 CObservationVelodyneScan::LOWER_BANK)
256 const bool block_is_dual_2nd_ranges =
258 CObservationVelodyneScan::RETMODE_DUAL &&
259 ((block & 0x01) != 0));
260 const bool block_is_dual_last_ranges =
262 CObservationVelodyneScan::RETMODE_DUAL &&
263 ((block & 0x01) == 0));
273 static_cast<uint8_t>(dsr + dsr_offset);
277 bool firingWithinBlock =
false;
278 if (num_lasers == 16)
283 firingWithinBlock =
true;
293 if (block_is_dual_2nd_ranges)
298 if (!
params.dualKeepStrongest)
continue;
300 if (block_is_dual_last_ranges && !
params.dualKeepLast)
continue;
305 CObservationVelodyneScan::DISTANCE_RESOLUTION +
307 if (distance < realMinDist || distance > realMaxDist)
continue;
310 if (
params.filterOutIsolatedPoints)
312 bool pass_filter =
true;
320 std::abs(dist_this - dist_prev) >
321 isolatedPointsFilterDistance_units)
329 std::abs(dist_this - dist_next) >
330 isolatedPointsFilterDistance_units)
333 if (!pass_filter)
continue;
338 double timestampadjustment =
340 double blockdsr0 = 0.0;
341 double nextblockdsr0 = 1.0;
348 CObservationVelodyneScan::RETMODE_DUAL)
351 block / 2, laserId, firingWithinBlock);
359 block, laserId, firingWithinBlock);
381 median_azimuth_diff * ((timestampadjustment - blockdsr0) /
382 (nextblockdsr0 - blockdsr0)));
384 const float azimuth_corrected_f =
385 azimuth_raw_f + azimuthadjustment;
386 const int azimuth_corrected =
387 ((int)
round(azimuth_corrected_f)) %
388 CObservationVelodyneScan::ROTATION_MAX_UNITS;
391 if (!((minAzimuth_int < maxAzimuth_int &&
392 azimuth_corrected >= minAzimuth_int &&
393 azimuth_corrected <= maxAzimuth_int) ||
394 (minAzimuth_int > maxAzimuth_int &&
395 (azimuth_corrected <= maxAzimuth_int ||
396 azimuth_corrected >= minAzimuth_int))))
405 float xy_distance =
distance * cos_vert_angle;
406 if (vert_offset) xy_distance += vert_offset * sin_vert_angle;
408 const int azimuth_corrected_for_lut =
410 (CObservationVelodyneScan::ROTATION_MAX_UNITS / 2)) %
411 CObservationVelodyneScan::ROTATION_MAX_UNITS;
412 const float cos_azimuth =
413 lut_sincos.
ccos[azimuth_corrected_for_lut];
414 const float sin_azimuth =
415 lut_sincos.
csin[azimuth_corrected_for_lut];
419 xy_distance * cos_azimuth +
420 horz_offset * sin_azimuth,
421 -(xy_distance * sin_azimuth -
422 horz_offset * cos_azimuth),
423 distance * sin_vert_angle + vert_offset);
425 bool add_point =
true;
432 if (
params.filterBynROI &&
438 if (!add_point)
continue;
444 azimuth_corrected_f);
451 void CObservationVelodyneScan::generatePointCloud(
458 PointCloudStorageWrapper_Inner(
461 : me_(me), params_(
p)
467 double pt_x,
double pt_y,
double pt_z,
uint8_t pt_intensity,
480 const int azimuth_corrected =
482 CObservationVelodyneScan::ROTATION_MAX_UNITS;
484 azimuth_corrected * ROTATION_RESOLUTION);
489 PointCloudStorageWrapper_Inner my_pc_wrap(*
this,
params);
494 void CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory(
496 std::vector<mrpt::math::TPointXYZIu8>& out_points,
503 scan_packets.size() * BLOCKS_PER_PACKET * SCANS_PER_BLOCK + 16);
509 std::vector<mrpt::math::TPointXYZIu8>& out_points_;
513 bool last_query_valid_;
515 PointCloudStorageWrapper_SE3_Interp(
518 std::vector<mrpt::math::TPointXYZIu8>& out_points,
521 vehicle_path_(vehicle_path),
522 out_points_(out_points),
523 results_stats_(results_stats),
525 last_query_valid_(
false)
529 double pt_x,
double pt_y,
double pt_z,
uint8_t pt_intensity,
534 if (last_query_tim_ != tim)
536 last_query_tim_ = tim;
537 vehicle_path_.
interpolate(tim, last_query_, last_query_valid_);
540 if (last_query_valid_)
546 global_sensor_pose.
composePoint(pt_x, pt_y, pt_z, gx, gy, gz);
547 out_points_.push_back(
555 PointCloudStorageWrapper_SE3_Interp my_pc_wrap(
556 *
this, vehicle_path, out_points, results_stats);
569 void CObservationVelodyneScan::TPointCloud::clear_deep()
572 std::vector<float> d;
576 std::vector<float> d;
580 std::vector<float> d;
584 std::vector<uint8_t> d;
588 std::vector<mrpt::system::TTimeStamp> d;
592 std::vector<float> d;
Results for generatePointCloudAlongSE3Trajectory()
size_t num_points
Number of points in the observation.
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
const TSinCosValues & getSinCosForScan(const CObservation2DRangeScan &scan) const
Return two vectors with the cos and the sin of the angles for each of the rays in a scan...
double verticalOffsetCorrection
raw_block_t blocks[BLOCKS_PER_PACKET]
const float HDR32_FIRING_TOFFSET
#define THROW_EXCEPTION(msg)
static void velodyne_scan_to_pointcloud(const CObservationVelodyneScan &scan, const CObservationVelodyneScan::TGeneratePointCloudParameters ¶ms, PointCloudStorageWrapper &out_pc)
#define ASSERT_BELOW_(__A, __B)
uint32_t gps_timestamp
us from top of hour
std::vector< mrpt::system::TTimeStamp > timestamp
Timestamp for each point (if generatePerPointTimestamp=true in TGeneratePointCloudParameters), or empty vector if not populated (default).
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
std::vector< float > azimuth
Original azimuth of each point (if generatePerPointAzimuth=true, empty otherwise ) ...
bool generatePerPointAzimuth
(Default:false) If true, populate the vector azimuth
mrpt::obs::VelodyneCalibration calibration
The calibration data for the LIDAR device.
void WriteAs(const TYPE_FROM_ACTUAL &value)
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
const float VLP16_DSR_TOFFSET
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::math::CVectorFloat ccos
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor...
double minRange
The maximum range allowed by the device, in meters (e.g.
A numeric matrix of compile-time fixed size.
Lightweight 3D point (float version).
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
const float HDR32_DSR_TOFFSET
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
std::vector< PerLaserCalib > laser_corrections
CSinCosLookUpTableFor2DScans velodyne_sincos_tables
This namespace contains representation of robot actions and observations.
size_t num_correctly_inserted_points
Number of points for which a valid interpolated SE(3) pose could be determined.
uint16_t header
Block id: UPPER_BANK or LOWER_BANK.
bool generatePerPointTimestamp
(Default:false) If true, populate the vector timestamp
std::vector< uint8_t > intensity
Color [0,255].
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
pose_t & interpolate(const mrpt::Clock::time_point &t, pose_t &out_interp, bool &out_valid_interp) const
Returns the pose at a given time, or interpolates using splines if there is not an exact match...
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
mrpt::math::CVectorFloat csin
laser_return_t laser_returns[SCANS_PER_BLOCK]
Virtual base class for "archives": classes abstracting I/O streams.
A pair of vectors with the cos and sin values.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
One unit of data from the scanner (the payload of one UDP DATA packet)
Declares a class that represents any robot's observation.
const float VLP16_BLOCK_TDURATION
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
double distanceCorrection
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
const int SCANS_PER_FIRING
double horizontalOffsetCorrection
void clear()
Sets all vectors to zero length.
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
mrpt::system::TTimeStamp timestampAdd(const mrpt::system::TTimeStamp tim, const double num_seconds)
Shifts a timestamp the given amount of seconds (>0: forwards in time, <0: backwards) ...
GLenum GLsizei GLenum format
uint16_t rotation
0-35999, divide by 100 to get degrees
std::vector< TVelodyneRawPacket > scan_packets
The main content of this object: raw data packet from the LIDAR.
double HDL32AdjustTimeStamp(int firingblock, int dsr)
[us]
virtual void add_point(double pt_x, double pt_y, double pt_z, uint8_t pt_intensity, const mrpt::system::TTimeStamp &tim, const float azimuth)=0
Process the insertion of a new (x,y,z) point to the cloud, in sensor-centric coordinates, with the exact timestamp of that LIDAR ray.
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.
unsigned __int32 uint32_t
Auxiliary class used to refactor CObservationVelodyneScan::generatePointCloud()
XYZ point (double) + Intensity(u8)
GLenum const GLfloat * params
void clear()
Clear the contents of this container.
double VLP16AdjustTimeStamp(int firingblock, int dsr, int firingwithinblock)
[us]
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
const float VLP16_FIRING_TOFFSET
int round(const T value)
Returns the closer integer (int) to x.