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); }