Add unit tests where angle of (axis,angle) repr. exceeds 180 degrees by <0.5 deg

[johnwlambert]

Adds specific examples to stress-test the log map.

[johnwlambert]

@ProfFan thanks for the updates. just curious about a few things:
(1) why is atan2 expensive? i've always thought of that as a very cheap function
(2) the maxima software seems very cool that it can compute those taylor series symbolically. why are we confident that maxima is more accurate than wolfram's Taylor series?

[johnwlambert]

i think this part is starting to look a bit like copy/paste @ProfFan, should we make a separate subroutine to avoid that?

[ProfFan]

@ProfFan thanks for the updates. just curious about a few things:
(1) why is atan2 expensive? i've always thought of that as a very cheap function
(2) the maxima software seems very cool that it can compute those taylor series symbolically. why are we confident that maxima is more accurate than wolfram's Taylor series?

(1) I think atan2 is not very expensive, but I think at this small a rotation it's probably still more complex than Taylor series

(2) The Taylor series in the Maxima derivation are symbolic (exact). Also Mathematica cannot calculate the Taylor series if the function is a limit at the linearization point.

[johnwlambert]

@ProfFan thanks for that info. I think we can make this function about 15 lines shorter if we avoid any copy-paste. How does this look?

double compute_signed_angle_from_quaternion(const double qx, const double qy, const double qz, const double qw)
{
	const double r = sqrt(qx);
	const double one_over_r = 1 / r;
	const double norm = sqrt(qx*qx + qy*qy + qz*qz + qw*qw);
	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;
	signed_scale = sign_w * scale;
	return signed_scale;
}

if (R33 > R22 && R33 > R11) {
	// R33 is the largest diagonal, a=3, b=1, c=2
	const double qw = R21 - R12;
	const double qx = 2.0 + 2.0 * R33;
	const double qy = R31 + R13;
	const double qz = R23 + R32;
	const double signed_scale = compute_signed_angle_from_quaternion(qx, qy, qz, qw);
	omega = signed_scale * Vector3(qy, qz, qx);
} else if (R22 > R11) {
	// R22 is the largest diagonal, a=2, b=3, c=1
	const double qw = R13 - R31;
	const double qx = 2.0 + 2.0 * R22;
	const double qy = R23 + R32;
	const double qz = R12 + R21;
	const double signed_scale = compute_signed_angle_from_quaternion(qx, qy, qz, qw);
	omega = signed_scale * Vector3(qz, qx, qy);
} else {
	// R11 is the largest diagonal, a=1, b=2, c=3
	const double qw = R32 - R23;
	const double qx = 2.0 + 2.0 * R11;
	const double qy = R12 + R21;
	const double qz = R31 + R13;
	const double signed_scale = compute_signed_angle_from_quaternion(qx, qy, qz, qw);
	omega = signed_scale * Vector3(qx, qy, qz);
}

[ProfFan]

That adds one more function call. Maybe you can use inline for that function though. Also have to make sure that it's still correct. Otherwise looks good to me.

[johnwlambert]

this computation of r doesnt seem to match the math in https://github.com/borglab/gtsam/files/7333898/AxisAngleConversion.pdf right?

[ProfFan]

I have a new version

[ProfFan]

BTW, r matches the old formulation, the only change is in the computation of the Taylor series (different series)

[johnwlambert]

@ProfFan could you share the new version of the doc with me?

[ProfFan]

Check slack

[ProfFan]

[ProfFan]

@johnwlambert Merging, please test in GTSFM and report if you have further problems.