-
Notifications
You must be signed in to change notification settings - Fork 45
Open
Description
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);
};?
Reactions are currently unavailable
Metadata
Metadata
Assignees
Labels
No labels