Angle gradients in normals

[mtb]

Hi everyone,

I'm writing a factor to define the relative angle between two planes by dotting their normals and computing the acos of that. My problem is, that this factor seems to run into problems when the graph gets too stiff and might cause the solver unexpectedly quit early on. If possible, I'd love some feedback on the error function and if it's all well defined.

I already know about the singularity when looking for a gradient in acos, so defining the error solely based on dot(normal_a, normal_b) would also be ok, although a little unintuitive.

I'd be happy to contribute should this all seem sane to you.

The factor, as imagined, takes two planes and should only be sensitive to the first two elements of their inner representation in derivative space (the two rotational components)

Vector OrientedPlane3RotationFactor::evaluateError(
const OrientedPlane3 &planeA, const OrientedPlane3 &planeB, boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const
{
Matrix12 planeA_H, planeB_H;

double predicted_dot = planeA.normal().dot(planeB.normal(), planeA_H, planeB_H);
double predicted_angle = std::acos(predicted_dot);
double sq_content = 1.0 - predicted_dot * predicted_dot;
double D_angle_D_dot = -1.0 / std::sqrt(std::max(sq_content, 1e-20));

Vector1 err(measured_angle_ - predicted_angle);

// Apply the chain rule to calculate the derivatives.
if (H1)
{
*H1 = Matrix13(-planeA_H[0, 0] * D_angle_D_dot, -planeA_H[0, 1] * D_angle_D_dot, 0.0);
}
if (H2)
{
*H2 = Matrix13(-planeB_H[0, 0] * D_angle_D_dot, -planeB_H[0, 1] * D_angle_D_dot, 0.0);
}

return err;

}