Skip to content

SE3::exp() already does on-manifold integration? #44

@scott-robotics

Description

@scott-robotics

The exp() already applies the tangent on the manifold, as far I understand. So the following,

    auto motion_model = [](const Eigen::Vector2d &integrated_controls) {
        Sophus::SE3d::Tangent dx = Sophus::SE3d::Tangent::Zero();
        const double &displacement = integrated_controls(0);
        const double &theta = integrated_controls(1);
        dx(0) = displacement * std::sin(theta) / (theta + epsilon);
        dx(1) = displacement * (1.0 - std::cos(theta)) / (theta + epsilon);
        dx(5) = theta;
        return Sophus::SE3d::exp(dx);
    };

should just become:

    auto motion_model = [](const Eigen::Vector2d &integrated_controls) {
        Sophus::SE3d::Tangent dx = Sophus::SE3d::Tangent::Zero();
        dx(0) = integrated_controls(0);
        dx(5) = integrated_controls(1);
        return Sophus::SE3d::exp(dx);
    };

?

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions