class mola::TrajectoryRebaker

Overview

Trajectory rebaker: corrects a chain of KF poses using per-KF pitch/roll rotations so that the resulting trajectory is gravity-aligned.

#include <TrajectoryRebaker.h>

class TrajectoryRebaker
{
public:
    // typedefs

    typedef uint64_t KeyFrameID;

    // structs

    struct Result;

    // methods

    static Result rebake(
        const std::map<KeyFrameID, mrpt::poses::CPose3D>& odom_poses,
        const std::map<KeyFrameID, mrpt::poses::CPose3D>& r_grav,
        KeyFrameID anchor_id
        );

    static Result rebake(
        const std::map<KeyFrameID, mrpt::poses::CPose3D>& odom_poses,
        const std::map<KeyFrameID, mrpt::poses::CPose3D>& r_grav
        );

    static mrpt::poses::CPose3D computePublishResidual(
        const mrpt::poses::CPose3D& tail_pose_before,
        const mrpt::poses::CPose3D& tail_pose_after
        );
};

Methods

static Result rebake(
    const std::map<KeyFrameID, mrpt::poses::CPose3D>& odom_poses,
    const std::map<KeyFrameID, mrpt::poses::CPose3D>& r_grav,
    KeyFrameID anchor_id
    )

Rebake a contiguous window of KF poses.

Processes KFs in ascending id order within the intersection of odom_poses and r_grav, starting at anchor_id.

The anchor KF’s POSITION is copied verbatim from odom_poses; only its rotation is updated: T_anchor_new.R = R_grav[anchor] * T_anchor_odom.R T_anchor_new.pos = T_anchor_odom.pos (frozen)

For every subsequent KF i (in ascending id order after anchor): Δpos_odom = T_i_odom.pos - T_{i-1}_odom.pos Δpos_new = (R_{i-1}_new * R_{i-1}_odom⁻¹) * Δpos_odom T_i_new.pos = T_{i-1}_new.pos + Δpos_new T_i_new.R = R_grav[i] * T_i_odom.R

KFs before anchor_id in odom_poses are skipped (their poses are left unchanged by the caller). KFs in odom_poses that have no entry in r_grav are passed through unchanged (identity correction applied).

Parameters:

odom_poses	Original LIO poses keyed by KF id (sorted map).
r_grav	Per-KF pitch/roll corrections from GravityMapAligner. KFs missing from this map get identity correction.
anchor_id	First KF id to process; its position is frozen.

Returns:

Result with corrected_poses for anchor..end of window.

static Result rebake(
    const std::map<KeyFrameID, mrpt::poses::CPose3D>& odom_poses,
    const std::map<KeyFrameID, mrpt::poses::CPose3D>& r_grav
    )

Convenience overload: anchor = first KF present in both maps.

static mrpt::poses::CPose3D computePublishResidual(
    const mrpt::poses::CPose3D& tail_pose_before,
    const mrpt::poses::CPose3D& tail_pose_after
    )

Compute the publish residual after a rebake.

Unconditionally returns the residual transform that, when LEFT-composed onto tail_pose_after, reproduces tail_pose_before :

residual + tail_pose_after == tail_pose_before

The caller left-composes this onto the live (post-rebake) pose between rebakes (pose_pub = residual + pose_stored) so the publish stream is continuous across the rebake event.

Callers are responsible for skipping the call when the tail keyframe is missing from the relevant maps; this function performs no such check.

Parameters:

tail_pose_before	Tail KF pose before the rebake.
tail_pose_after	Tail KF pose after the rebake.

Returns:

The residual tail_pose_before + (-tail_pose_after).