TEST (Point3, distance) {
Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
Matrix H1, H2;
double d = P.distance(Q, H1, H2);
double expectedDistance = 55.542686;
Matrix numH1 = numericalDerivative21(testFunc, P, Q);
Matrix numH2 = numericalDerivative22(testFunc, P, Q);
DOUBLES_EQUAL(expectedDistance, d, 1e-5);
EXPECT(assert_equal(numH1, H1, 1e-8));
EXPECT(assert_equal(numH2, H2, 1e-8));
}
/* ************************************************************************* */
double PoseRTV::range(const PoseRTV& other,
OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const {
Matrix36 D_t1_pose, D_t2_other;
const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
Matrix13 D_d_t1, D_d_t2;
double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
return d;
}
/* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) {
return P.distance(Q);
}
