class mp2p_icp_filters::FilterDeskew

Overview

Builds a new layer with a deskewed (motion compensated) version of an input pointcloud from a moving LIDAR, where points are time-stamped.

Important notes:

• The time field of each point is assumed to be in seconds since the reference time point, which can be the start or middle point of the scan. This can be controlled by adding a FilterAdjustTimestamps before this block.

• The input layer must contain a point cloud in the format mrpt::maps::CPointsMapXYZIRT so timestamps are present.

• If the input layer is of a different type, or the time field is missing, an exception will be thrown by default, unless the option silently_ignore_no_timestamps is set to true, in which case the input cloud will be just moved forward to the output.

#include <FilterDeskew.h>

class FilterDeskew: public mp2p_icp_filters::FilterBase
{
public:
    // fields

    std::string input_pointcloud_layer = mp2p_icp::metric_map_t::PT_LAYER_RAW;
    bool in_place = false;
    std::string output_pointcloud_layer;
    std::string output_layer_class = "mrpt::maps::CPointsMapXYZI";
    bool silently_ignore_no_timestamps = false;
    MotionCompensationMethod method = MotionCompensationMethod::Linear;
    std::optional<mrpt::math::TTwist3D> twist;
    bool points_already_global = false;
    mrpt::math::TPose3D robot_pose;
    mrpt::math::TVector3D bias_acc {0, 0, 0};
    mrpt::math::TVector3D bias_gyro {0, 0, 0};
    mrpt::math::TVector3D gravity_vector {0, 0, -9.81};

    // methods

    virtual void initialize_filter(const mrpt::containers::yaml& c);
    virtual void filter(mp2p_icp::metric_map_t& inOut) const;
};

Inherited Members

public:
    // methods

    virtual void filter(mp2p_icp::metric_map_t& inOut) const = 0;
    FilterBase& operator = (const FilterBase&);
    FilterBase& operator = (FilterBase&&);

Fields

std::string input_pointcloud_layer = mp2p_icp::metric_map_t::PT_LAYER_RAW

An input layer, from which to read input points Points must be already in the vehicle frame.

bool in_place = false

If set to true to deskew directly on the same input layer, so no new map is created

std::string output_pointcloud_layer

The output point cloud layer name, required unless in_place is true

std::string output_layer_class = "mrpt::maps::CPointsMapXYZI"

The class name for output layer if it does not exist and needs to be created

bool silently_ignore_no_timestamps = false

Whether to skip throwing an exception if the input layer does not contain timestamps.

MotionCompensationMethod method = MotionCompensationMethod::Linear

If enabled (true), the constant twist field is ignored and the precise twist trajectory is retrieved from the LocalVelocityBuffer from the ParameterSource attached to this block.

std::optional<mrpt::math::TTwist3D> twist

The velocity (linear and angular) of the vehicle in the local vehicle frame. See FilterDeskew::initialize for an example of how to define it via dynamic variables. This will be used only for method=MotionCompensationMethod::Linear; otherwise, it can be left as an empty std::optional.

bool points_already_global = false

If input points are already in global coordinates (as in the context of sm2mm) set to true and then fill in robot_pose

mrpt::math::TPose3D robot_pose

Only used when points_already_global is true

mrpt::math::TVector3D bias_acc {0, 0, 0}

Accelerometer bias (in sensor frame coordinates)

mrpt::math::TVector3D bias_gyro {0, 0, 0}

Gyroscope bias (in sensor frame coordinates)

mrpt::math::TVector3D gravity_vector {0, 0, -9.81}

Gravity vector (in global frame)

Methods

virtual void initialize_filter(const mrpt::containers::yaml& c)

Parameters:

params:
  input_pointcloud_layer: 'raw'
  output_pointcloud_layer: 'deskewed'
  method: 'MotionCompensationMethod::Linear'
  #in_place: true   # Set to true for the output layer to be the same input layer
  # silently_ignore_no_timestamps: false
  # These (vx,...,wz) are variable names that must be defined via the
  # mp2p_icp::Parameterizable API to update them dynamically.
  twist: [vx,vy,vz,wx,wy,wz]

virtual void filter(mp2p_icp::metric_map_t& inOut) const

See docs above for FilterBase.