Undid Logmap refactor:
there is some subtle bug with that lambda that I could not resolve withing reasonable time.release/4.3a0
parent
dd6b70e08c
commit
c4b2cc5b0f
|
@ -317,26 +317,46 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
|
|||
|
||||
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
// we do something special
|
||||
if (tr + 1.0 < so3::kNearPiThresholdSq) {
|
||||
// Used below
|
||||
auto omegaFromQ = [](double W, double r, double w1, double w2, double w3) {
|
||||
if (tr + 1.0 < 1e-3) {
|
||||
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(w1 * w1 + w2 * w2 + w3 * w3 + W * W);
|
||||
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;
|
||||
return sgn_w * scale * Vector3(w1, w2, w3);
|
||||
};
|
||||
|
||||
if (R33 > R22 && R33 > R11) { // R33 is the largest diagonal, a=3, b=1, c=2
|
||||
const double Q1 = 2.0 + 2.0 * R33;
|
||||
omega = omegaFromQ(R21 - R12, sqrt(Q1), R31 + R13, R23 + R32, Q1);
|
||||
} else if (R22 > R11) { // R22 is the largest diagonal, a=2, b=3, c=1
|
||||
omega = sgn_w * scale * Vector3(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;
|
||||
omega = omegaFromQ(R13 - R31, sqrt(Q1), R12 + R21, Q1, R23 + R32);
|
||||
} else { // R11 is the largest diagonal, a=1, b=2, c=3
|
||||
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);
|
||||
} 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;
|
||||
omega = omegaFromQ(R32 - R23, sqrt(Q1), Q1, R12 + R21, R31 + R13);
|
||||
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);
|
||||
}
|
||||
} else {
|
||||
double magnitude;
|
||||
|
|
Loading…
Reference in New Issue