class mp2p_icp::LocalVelocityBuffer

Overview

Holds a short time window of local velocities (vw,vy,vz) and (wx,wy,wz), from external estimators or an IMU. This is used to compute the local velocity of the sensor at each point in time for precise scan deformation/deskew.

Its state can be (de)serialized to/from YAML.

#include <LocalVelocityBuffer.h>

class LocalVelocityBuffer
{
public:
    // typedefs

    typedef double TimeStamp;
    typedef mrpt::math::TVector3D LinearVelocity;
    typedef mrpt::math::TVector3D AngularVelocity;
    typedef std::map<double, mrpt::poses::CPose3D> Trajectory;

    // structs

    struct Parameters;

    // fields

    Parameters parameters;

    // methods

    void add_linear_velocity(const TimeStamp& time, const LinearVelocity& v_vehicle);
    void add_angular_velocity(const TimeStamp& time, const AngularVelocity& w_vehicle);
    void clear();
    mrpt::containers::yaml toYAML() const;
    void fromYAML(const mrpt::containers::yaml& y);
    const auto& get_linear_velocities() const;
    const auto& get_angular_velocities() const;
    void set_reference_zero_time(const TimeStamp& time);
    TimeStamp get_reference_zero_time() const;
    Trajectory reconstruct_poses_around_reference_time(double half_time_span) const;
};

Methods

void add_linear_velocity(const TimeStamp& time, const LinearVelocity& v_vehicle)

Add a new linear velocity entry (in the vehicle frame of reference)

void add_angular_velocity(const TimeStamp& time, const AngularVelocity& w_vehicle)

Add a new angular velocity entry (in the vehicle frame of reference)

void clear()

reset the buffer, clearing all entries

mrpt::containers::yaml toYAML() const

Stores the current state and parameters to a YAML dictionary.

void fromYAML(const mrpt::containers::yaml& y)

Restores the current state and parameters from a YAML dictionary.

const auto& get_linear_velocities() const

Get the current linear velocities map (in the vehicle frame of reference)

const auto& get_angular_velocities() const

Get the current angular velocities map (in the vehicle frame of reference)

void set_reference_zero_time(const TimeStamp& time)

Set the reference time for lidar scans:

TimeStamp get_reference_zero_time() const

Get the reference time for lidar scans:

Trajectory reconstruct_poses_around_reference_time(double half_time_span) const

Build the reconstructed vehicle trajectory in the time range ref_time ± half_time_span.

The closest velocity entry timestamp to the reference time is searched and used as if it was exactly the reference time. In the returned trajectory, t=0 is the reference time.

Returns:

The generated trajectory, or an empty one if there is no enough data to build it.