CObservationVelodyneScan.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include "obs-precomp.h"  // Precompiled headers
 
#include <mrpt/obs/CObservationVelodyneScan.h>
#include <mrpt/poses/CPose3DInterpolator.h>
#include <mrpt/core/round.h>
#include <mrpt/serialization/CArchive.h>
#include <iostream>
 
using namespace std;
using namespace mrpt::obs;
 
// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CObservationVelodyneScan, CObservation, mrpt::obs)
 
CSinCosLookUpTableFor2DScans velodyne_sincos_tables;
 
const float CObservationVelodyneScan::ROTATION_RESOLUTION =
    0.01f; /**< degrees */
const float CObservationVelodyneScan::DISTANCE_MAX = 130.0f; /**< meters */
const float CObservationVelodyneScan::DISTANCE_RESOLUTION =
    0.002f; /**< meters */
const float CObservationVelodyneScan::DISTANCE_MAX_UNITS =
    (CObservationVelodyneScan::DISTANCE_MAX /
         CObservationVelodyneScan::DISTANCE_RESOLUTION +
     1.0f);
 
const int SCANS_PER_FIRING = 16;
 
const float VLP16_BLOCK_TDURATION = 110.592f;  // [us]
const float VLP16_DSR_TOFFSET = 2.304f;  // [us]
const float VLP16_FIRING_TOFFSET = 55.296f;  // [us]
 
const float HDR32_DSR_TOFFSET = 1.152f;  // [us]
const float HDR32_FIRING_TOFFSET = 46.08f;  // [us]
 
mrpt::system::TTimeStamp
    CObservationVelodyneScan::getOriginalReceivedTimeStamp() const
{
    return originalReceivedTimestamp;
}
 
uint8_t CObservationVelodyneScan::serializeGetVersion() const { return 1; }
void CObservationVelodyneScan::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    out << timestamp << sensorLabel;
    out << minRange << maxRange << sensorPose;
    out.WriteAs<uint32_t>(scan_packets.size());
    if (!scan_packets.empty())
        out.WriteBuffer(
            &scan_packets[0], sizeof(scan_packets[0]) * scan_packets.size());
    out.WriteAs<uint32_t>(calibration.laser_corrections.size());
    if (!calibration.laser_corrections.empty())
        out.WriteBuffer(
            &calibration.laser_corrections[0],
            sizeof(calibration.laser_corrections[0]) *
                calibration.laser_corrections.size());
    out << point_cloud.x << point_cloud.y << point_cloud.z
        << point_cloud.intensity;
    out << has_satellite_timestamp;  // v1
}
 
void CObservationVelodyneScan::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        case 1:
        {
            in >> timestamp >> sensorLabel;
 
            in >> minRange >> maxRange >> sensorPose;
            {
                uint32_t N;
                in >> N;
                scan_packets.resize(N);
                if (N)
                    in.ReadBuffer(
                        &scan_packets[0], sizeof(scan_packets[0]) * N);
            }
            {
                uint32_t N;
                in >> N;
                calibration.laser_corrections.resize(N);
                if (N)
                    in.ReadBuffer(
                        &calibration.laser_corrections[0],
                        sizeof(calibration.laser_corrections[0]) * N);
            }
            in >> point_cloud.x >> point_cloud.y >> point_cloud.z >>
                point_cloud.intensity;
            if (version >= 1)
                in >> has_satellite_timestamp;
            else
                has_satellite_timestamp =
                    (this->timestamp != this->originalReceivedTimestamp);
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
    };
 
    // m_cachedMap.clear();
}
 
void CObservationVelodyneScan::getDescriptionAsText(std::ostream& o) const
{
    CObservation::getDescriptionAsText(o);
    o << "Homogeneous matrix for the sensor 3D pose, relative to robot base:\n";
    o << sensorPose.getHomogeneousMatrixVal<mrpt::math::CMatrixDouble44>()
      << "\n"
      << sensorPose << endl;
    o << format("Sensor min/max range: %.02f / %.02f m\n", minRange, maxRange);
    o << "Raw packet count: " << scan_packets.size() << "\n";
}
 
/** [us] */
double HDL32AdjustTimeStamp(int firingblock, int dsr)
{
    return (firingblock * HDR32_FIRING_TOFFSET) + (dsr * HDR32_DSR_TOFFSET);
}
/** [us] */
double VLP16AdjustTimeStamp(int firingblock, int dsr, int firingwithinblock)
{
    return (firingblock * VLP16_BLOCK_TDURATION) + (dsr * VLP16_DSR_TOFFSET) +
           (firingwithinblock * VLP16_FIRING_TOFFSET);
}
 
/** Auxiliary class used to refactor
 * CObservationVelodyneScan::generatePointCloud() */
struct PointCloudStorageWrapper
{
    /** 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 */
    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;
};
 
static void velodyne_scan_to_pointcloud(
    const CObservationVelodyneScan& scan,
    const CObservationVelodyneScan::TGeneratePointCloudParameters& params,
    PointCloudStorageWrapper& out_pc)
{
    // Initially based on code from ROS velodyne & from
    // vtkVelodyneHDLReader::vtkInternal::ProcessHDLPacket().
    using mrpt::round;
 
    // Access to sin/cos table:
    mrpt::obs::T2DScanProperties scan_props;
    scan_props.aperture = 2 * M_PI;
    scan_props.nRays = CObservationVelodyneScan::ROTATION_MAX_UNITS;
    scan_props.rightToLeft = true;
    // The LUT contains sin/cos values for angles in this order: [180deg ... 0
    // deg ... -180 deg]
    const CSinCosLookUpTableFor2DScans::TSinCosValues& lut_sincos =
        velodyne_sincos_tables.getSinCosForScan(scan_props);
 
    const int minAzimuth_int = round(params.minAzimuth_deg * 100);
    const int maxAzimuth_int = round(params.maxAzimuth_deg * 100);
    const float realMinDist =
        std::max(static_cast<float>(scan.minRange), params.minDistance);
    const float realMaxDist =
        std::min(params.maxDistance, static_cast<float>(scan.maxRange));
    const int16_t isolatedPointsFilterDistance_units =
        params.isolatedPointsFilterDistance /
        CObservationVelodyneScan::DISTANCE_RESOLUTION;
 
    // This is: 16,32,64 depending on the LIDAR model
    const size_t num_lasers = scan.calibration.laser_corrections.size();
 
    for (size_t iPkt = 0; iPkt < scan.scan_packets.size(); iPkt++)
    {
        const CObservationVelodyneScan::TVelodyneRawPacket* raw =
            &scan.scan_packets[iPkt];
 
        mrpt::system::TTimeStamp pkt_tim;  // Find out timestamp of this pkt
        {
            const uint32_t us_pkt0 = scan.scan_packets[0].gps_timestamp;
            const uint32_t us_pkt_this = raw->gps_timestamp;
            // Handle the case of time counter reset by new hour 00:00:00
            const uint32_t us_ellapsed =
                (us_pkt_this >= us_pkt0)
                    ? (us_pkt_this - us_pkt0)
                    : (1000000UL * 3600UL + us_pkt_this - us_pkt0);
            pkt_tim =
                mrpt::system::timestampAdd(scan.timestamp, us_ellapsed * 1e-6);
        }
 
        // Take the median rotational speed as a good value for interpolating
        // the missing azimuths:
        int median_azimuth_diff;
        {
            // In dual return, the azimuth rate is actually twice this
            // estimation:
            const int nBlocksPerAzimuth =
                (raw->laser_return_mode ==
                 CObservationVelodyneScan::RETMODE_DUAL)
                    ? 2
                    : 1;
            std::vector<int> diffs(
                CObservationVelodyneScan::BLOCKS_PER_PACKET -
                nBlocksPerAzimuth);
            for (int i = 0; i < CObservationVelodyneScan::BLOCKS_PER_PACKET -
                                    nBlocksPerAzimuth;
                 ++i)
            {
                int localDiff = (CObservationVelodyneScan::ROTATION_MAX_UNITS +
                                 raw->blocks[i + nBlocksPerAzimuth].rotation -
                                 raw->blocks[i].rotation) %
                                CObservationVelodyneScan::ROTATION_MAX_UNITS;
                diffs[i] = localDiff;
            }
            std::nth_element(
                diffs.begin(),
                diffs.begin() + CObservationVelodyneScan::BLOCKS_PER_PACKET / 2,
                diffs.end());  // Calc median
            median_azimuth_diff =
                diffs[CObservationVelodyneScan::BLOCKS_PER_PACKET / 2];
        }
 
        for (int block = 0; block < CObservationVelodyneScan::BLOCKS_PER_PACKET;
             block++)  // Firings per packet
        {
            // ignore packets with mangled or otherwise different contents
            if ((num_lasers != 64 &&
                 CObservationVelodyneScan::UPPER_BANK !=
                     raw->blocks[block].header) ||
                (raw->blocks[block].header !=
                     CObservationVelodyneScan::UPPER_BANK &&
                 raw->blocks[block].header !=
                     CObservationVelodyneScan::LOWER_BANK))
            {
                cerr << "[CObservationVelodyneScan] skipping invalid packet: "
                        "block "
                     << block << " header value is "
                     << raw->blocks[block].header;
                continue;
            }
 
            const int dsr_offset = (raw->blocks[block].header ==
                                    CObservationVelodyneScan::LOWER_BANK)
                                       ? 32
                                       : 0;
            const float azimuth_raw_f = (float)(raw->blocks[block].rotation);
            const bool block_is_dual_2nd_ranges =
                (raw->laser_return_mode ==
                     CObservationVelodyneScan::RETMODE_DUAL &&
                 ((block & 0x01) != 0));
            const bool block_is_dual_last_ranges =
                (raw->laser_return_mode ==
                     CObservationVelodyneScan::RETMODE_DUAL &&
                 ((block & 0x01) == 0));
 
            for (int dsr = 0, k = 0; dsr < SCANS_PER_FIRING; dsr++, k++)
            {
                if (!raw->blocks[block]
                         .laser_returns[k]
                         .distance)  // Invalid return?
                    continue;
 
                const uint8_t rawLaserId =
                    static_cast<uint8_t>(dsr + dsr_offset);
                uint8_t laserId = rawLaserId;
 
                // Detect VLP-16 data and adjust laser id if necessary
                bool firingWithinBlock = false;
                if (num_lasers == 16)
                {
                    if (laserId >= 16)
                    {
                        laserId -= 16;
                        firingWithinBlock = true;
                    }
                }
 
                ASSERT_BELOW_(laserId, num_lasers);
                const mrpt::obs::VelodyneCalibration::PerLaserCalib& calib =
                    scan.calibration.laser_corrections[laserId];
 
                // In dual return, if the distance is equal in both ranges,
                // ignore one of them:
                if (block_is_dual_2nd_ranges)
                {
                    if (raw->blocks[block].laser_returns[k].distance ==
                        raw->blocks[block - 1].laser_returns[k].distance)
                        continue;  // duplicated point
                    if (!params.dualKeepStrongest) continue;
                }
                if (block_is_dual_last_ranges && !params.dualKeepLast) continue;
 
                // Return distance:
                const float distance =
                    raw->blocks[block].laser_returns[k].distance *
                        CObservationVelodyneScan::DISTANCE_RESOLUTION +
                    calib.distanceCorrection;
                if (distance < realMinDist || distance > realMaxDist) continue;
 
                // Isolated points filtering:
                if (params.filterOutIsolatedPoints)
                {
                    bool pass_filter = true;
                    const int16_t dist_this =
                        raw->blocks[block].laser_returns[k].distance;
                    if (k > 0)
                    {
                        const int16_t dist_prev =
                            raw->blocks[block].laser_returns[k - 1].distance;
                        if (!dist_prev ||
                            std::abs(dist_this - dist_prev) >
                                isolatedPointsFilterDistance_units)
                            pass_filter = false;
                    }
                    if (k < (SCANS_PER_FIRING - 1))
                    {
                        const int16_t dist_next =
                            raw->blocks[block].laser_returns[k + 1].distance;
                        if (!dist_next ||
                            std::abs(dist_this - dist_next) >
                                isolatedPointsFilterDistance_units)
                            pass_filter = false;
                    }
                    if (!pass_filter) continue;  // Filter out this point
                }
 
                // Azimuth correction: correct for the laser rotation as a
                // function of timing during the firings
                double timestampadjustment =
                    0.0;  // [us] since beginning of scan
                double blockdsr0 = 0.0;
                double nextblockdsr0 = 1.0;
                switch (num_lasers)
                {
                    // VLP-16
                    case 16:
                    {
                        if (raw->laser_return_mode ==
                            CObservationVelodyneScan::RETMODE_DUAL)
                        {
                            timestampadjustment = VLP16AdjustTimeStamp(
                                block / 2, laserId, firingWithinBlock);
                            nextblockdsr0 =
                                VLP16AdjustTimeStamp(block / 2 + 1, 0, 0);
                            blockdsr0 = VLP16AdjustTimeStamp(block / 2, 0, 0);
                        }
                        else
                        {
                            timestampadjustment = VLP16AdjustTimeStamp(
                                block, laserId, firingWithinBlock);
                            nextblockdsr0 =
                                VLP16AdjustTimeStamp(block + 1, 0, 0);
                            blockdsr0 = VLP16AdjustTimeStamp(block, 0, 0);
                        }
                    }
                    break;
                    // HDL-32:
                    case 32:
                        timestampadjustment = HDL32AdjustTimeStamp(block, dsr);
                        nextblockdsr0 = HDL32AdjustTimeStamp(block + 1, 0);
                        blockdsr0 = HDL32AdjustTimeStamp(block, 0);
                        break;
                    case 64:
                        break;
                    default:
                    {
                        THROW_EXCEPTION("Error: unhandled LIDAR model!");
                    }
                };
 
                const int azimuthadjustment = mrpt::round(
                    median_azimuth_diff * ((timestampadjustment - blockdsr0) /
                                           (nextblockdsr0 - blockdsr0)));
 
                const float azimuth_corrected_f =
                    azimuth_raw_f + azimuthadjustment;
                const int azimuth_corrected =
                    ((int)round(azimuth_corrected_f)) %
                    CObservationVelodyneScan::ROTATION_MAX_UNITS;
 
                // Filter by azimuth:
                if (!((minAzimuth_int < maxAzimuth_int &&
                       azimuth_corrected >= minAzimuth_int &&
                       azimuth_corrected <= maxAzimuth_int) ||
                      (minAzimuth_int > maxAzimuth_int &&
                       (azimuth_corrected <= maxAzimuth_int ||
                        azimuth_corrected >= minAzimuth_int))))
                    continue;
 
                // Vertical axis mis-alignment calibration:
                const float cos_vert_angle = calib.cosVertCorrection;
                const float sin_vert_angle = calib.sinVertCorrection;
                const float horz_offset = calib.horizontalOffsetCorrection;
                const float vert_offset = calib.verticalOffsetCorrection;
 
                float xy_distance = distance * cos_vert_angle;
                if (vert_offset) xy_distance += vert_offset * sin_vert_angle;
 
                const int azimuth_corrected_for_lut =
                    (azimuth_corrected +
                     (CObservationVelodyneScan::ROTATION_MAX_UNITS / 2)) %
                    CObservationVelodyneScan::ROTATION_MAX_UNITS;
                const float cos_azimuth =
                    lut_sincos.ccos[azimuth_corrected_for_lut];
                const float sin_azimuth =
                    lut_sincos.csin[azimuth_corrected_for_lut];
 
                // Compute raw position
                const mrpt::math::TPoint3Df pt(
                    xy_distance * cos_azimuth +
                        horz_offset * sin_azimuth,  // MRPT +X = Velodyne +Y
                    -(xy_distance * sin_azimuth -
                      horz_offset * cos_azimuth),  // MRPT +Y = Velodyne -X
                    distance * sin_vert_angle + vert_offset);
 
                bool add_point = true;
                if (params.filterByROI &&
                    (pt.x > params.ROI_x_max || pt.x < params.ROI_x_min ||
                     pt.y > params.ROI_y_max || pt.y < params.ROI_y_min ||
                     pt.z > params.ROI_z_max || pt.z < params.ROI_z_min))
                    add_point = false;
 
                if (params.filterBynROI &&
                    (pt.x <= params.nROI_x_max && pt.x >= params.nROI_x_min &&
                     pt.y <= params.nROI_y_max && pt.y >= params.nROI_y_min &&
                     pt.z <= params.nROI_z_max && pt.z >= params.nROI_z_min))
                    add_point = false;
 
                if (!add_point) continue;
 
                // Insert point:
                out_pc.add_point(
                    pt.x, pt.y, pt.z,
                    raw->blocks[block].laser_returns[k].intensity, pkt_tim,
                    azimuth_corrected_f);
 
            }  // end for k,dsr=[0,31]
        }  // end for each block [0,11]
    }  // end for each data packet
}
 
void CObservationVelodyneScan::generatePointCloud(
    const TGeneratePointCloudParameters& params)
{
    struct PointCloudStorageWrapper_Inner : public PointCloudStorageWrapper
    {
        CObservationVelodyneScan& me_;
        const TGeneratePointCloudParameters& params_;
        PointCloudStorageWrapper_Inner(
            CObservationVelodyneScan& me,
            const TGeneratePointCloudParameters& p)
            : me_(me), params_(p)
        {
            // Reset point cloud:
            me_.point_cloud.clear();
        }
        void add_point(
            double pt_x, double pt_y, double pt_z, uint8_t pt_intensity,
            const mrpt::system::TTimeStamp& tim, const float azimuth) override
        {
            me_.point_cloud.x.push_back(pt_x);
            me_.point_cloud.y.push_back(pt_y);
            me_.point_cloud.z.push_back(pt_z);
            me_.point_cloud.intensity.push_back(pt_intensity);
            if (params_.generatePerPointTimestamp)
            {
                me_.point_cloud.timestamp.push_back(tim);
            }
            if (params_.generatePerPointAzimuth)
            {
                const int azimuth_corrected =
                    ((int)round(azimuth)) %
                    CObservationVelodyneScan::ROTATION_MAX_UNITS;
                me_.point_cloud.azimuth.push_back(
                    azimuth_corrected * ROTATION_RESOLUTION);
            }
        }
    };
 
    PointCloudStorageWrapper_Inner my_pc_wrap(*this, params);
 
    velodyne_scan_to_pointcloud(*this, params, my_pc_wrap);
}
 
void CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory(
    const mrpt::poses::CPose3DInterpolator& vehicle_path,
    std::vector<mrpt::math::TPointXYZIu8>& out_points,
    TGeneratePointCloudSE3Results& results_stats,
    const TGeneratePointCloudParameters& params)
{
    // Pre-alloc mem:
    out_points.reserve(
        out_points.size() +
        scan_packets.size() * BLOCKS_PER_PACKET * SCANS_PER_BLOCK + 16);
 
    struct PointCloudStorageWrapper_SE3_Interp : public PointCloudStorageWrapper
    {
        CObservationVelodyneScan& me_;
        const mrpt::poses::CPose3DInterpolator& vehicle_path_;
        std::vector<mrpt::math::TPointXYZIu8>& out_points_;
        TGeneratePointCloudSE3Results& results_stats_;
        mrpt::system::TTimeStamp last_query_tim_;
        mrpt::poses::CPose3D last_query_;
        bool last_query_valid_;
 
        PointCloudStorageWrapper_SE3_Interp(
            CObservationVelodyneScan& me,
            const mrpt::poses::CPose3DInterpolator& vehicle_path,
            std::vector<mrpt::math::TPointXYZIu8>& out_points,
            TGeneratePointCloudSE3Results& results_stats)
            : me_(me),
              vehicle_path_(vehicle_path),
              out_points_(out_points),
              results_stats_(results_stats),
              last_query_tim_(INVALID_TIMESTAMP),
              last_query_valid_(false)
        {
        }
        void add_point(
            double pt_x, double pt_y, double pt_z, uint8_t pt_intensity,
            const mrpt::system::TTimeStamp& tim, const float azimuth) override
        {
            // Use a cache since it's expected that the same timestamp is
            // queried several times in a row:
            if (last_query_tim_ != tim)
            {
                last_query_tim_ = tim;
                vehicle_path_.interpolate(tim, last_query_, last_query_valid_);
            }
 
            if (last_query_valid_)
            {
                mrpt::poses::CPose3D global_sensor_pose(
                    mrpt::poses::UNINITIALIZED_POSE);
                global_sensor_pose.composeFrom(last_query_, me_.sensorPose);
                double gx, gy, gz;
                global_sensor_pose.composePoint(pt_x, pt_y, pt_z, gx, gy, gz);
                out_points_.push_back(
                    mrpt::math::TPointXYZIu8(gx, gy, gz, pt_intensity));
                ++results_stats_.num_correctly_inserted_points;
            }
            ++results_stats_.num_points;
        }
    };
 
    PointCloudStorageWrapper_SE3_Interp my_pc_wrap(
        *this, vehicle_path, out_points, results_stats);
    velodyne_scan_to_pointcloud(*this, params, my_pc_wrap);
}
 
void CObservationVelodyneScan::TPointCloud::clear()
{
    x.clear();
    y.clear();
    z.clear();
    intensity.clear();
    timestamp.clear();
    azimuth.clear();
}
void CObservationVelodyneScan::TPointCloud::clear_deep()
{
    {
        std::vector<float> d;
        x.swap(d);
    }
    {
        std::vector<float> d;
        y.swap(d);
    }
    {
        std::vector<float> d;
        z.swap(d);
    }
    {
        std::vector<uint8_t> d;
        intensity.swap(d);
    }
    {
        std::vector<mrpt::system::TTimeStamp> d;
        timestamp.swap(d);
    }
    {
        std::vector<float> d;
        azimuth.swap(d);
    }
}