Skip to content

Re-visiting Logmap #2106

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 3 commits into
base: develop
Choose a base branch
from
Draft

Re-visiting Logmap #2106

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
698cf76
PR comments
dellaert Apr 22, 2025
c5d1da8
Repro figure from #1233
dellaert Apr 22, 2025
eff21cc
Call Quaternion::Logmap
dellaert Apr 28, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion gtsam/geometry/Pose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -319,7 +319,7 @@ Matrix6 Pose3::LogmapDerivative(const Vector6& xi) {
Matrix3 H_t_w;
local.applyLeftJacobian(v, H_t_w);

// Multiply with R^T to account for NavState::Create Jacobian.
// Multiply with R^T to account for Pose3::Create Jacobian.
const Matrix3 R = local.expmap();
const Matrix3 Qt = R.transpose() * H_t_w;

Expand Down
40 changes: 13 additions & 27 deletions gtsam/geometry/SO3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/Quaternion.h>

#include <Eigen/SVD>
#include <cmath>
Expand Down Expand Up @@ -313,52 +314,38 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);

const double tr = R.trace();
Vector3 omega;


// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (tr + 1.0 < 1e-3) {
if (tr + 1.0 < so3::kNearZeroThresholdSq) {
Quaternion q;
if (R33 > R22 && R33 > R11) {
// R33 is the largest diagonal, a=3, b=1, c=2
const double W = R21 - R12;
const double Q1 = 2.0 + 2.0 * R33;
const double Q2 = R31 + R13;
const double Q3 = R23 + R32;
const double r = sqrt(Q1);
const double one_over_r = 1 / r;
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
const double sgn_w = W < 0 ? -1.0 : 1.0;
const double mag = M_PI - (2 * sgn_w * W) / norm;
const double scale = 0.5 * one_over_r * mag;
omega = sgn_w * scale * Vector3(Q2, Q3, Q1);
q = Quaternion(W, Q2, Q3, Q1);
} else if (R22 > R11) {
// R22 is the largest diagonal, a=2, b=3, c=1
const double W = R13 - R31;
const double Q1 = 2.0 + 2.0 * R22;
const double Q2 = R23 + R32;
const double Q3 = R12 + R21;
const double r = sqrt(Q1);
const double one_over_r = 1 / r;
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
const double sgn_w = W < 0 ? -1.0 : 1.0;
const double mag = M_PI - (2 * sgn_w * W) / norm;
const double scale = 0.5 * one_over_r * mag;
omega = sgn_w * scale * Vector3(Q3, Q1, Q2);
q = Quaternion(W, Q3, Q1, Q2);
} else {
// R11 is the largest diagonal, a=1, b=2, c=3
const double W = R32 - R23;
const double Q1 = 2.0 + 2.0 * R11;
const double Q2 = R12 + R21;
const double Q3 = R31 + R13;
const double r = sqrt(Q1);
const double one_over_r = 1 / r;
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
const double sgn_w = W < 0 ? -1.0 : 1.0;
const double mag = M_PI - (2 * sgn_w * W) / norm;
const double scale = 0.5 * one_over_r * mag;
omega = sgn_w * scale * Vector3(Q1, Q2, Q3);
q = Quaternion(W, Q1, Q2, Q3);
}
// normalize quaternion
q.normalize();
return traits<Quaternion>::Logmap(q, H);
} else {
Vector3 omega;
double magnitude;
const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
if (tr_3 < -so3::kNearZeroThresholdSq) {
Expand All @@ -372,10 +359,9 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
magnitude = 0.5 - tr_3 * so3::one_12th + tr_3 * tr_3 * so3::one_60th;
}
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
if (H) *H = LogmapDerivative(omega);
return omega;
}

if (H) *H = LogmapDerivative(omega);
return omega;
}

//******************************************************************************
Expand Down
Loading
Loading