12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

support optional Jacobians for Point3::distance

release/4.3a0
thduynguyen 2014-09-26 17:20:13 -04:00
parent bc2e9959fa
commit e133e8c2e8
2 changed files with 28 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Point3.h
View File

@ -164,8 +164,17 @@ namespace gtsam {
Point3 operator / (double s) const;
/** distance between two points */
inline double distance(const Point3& p2) const {
return (p2 - *this).norm();
inline double distance(const Point3& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z())*(1./d);
}
if (H2) {
*H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z())*(1./d);
}
return d;
}
/** @deprecated The following function has been deprecated, use distance above */

17
gtsam/geometry/tests/testPoint3.cpp
View File

@ -88,6 +88,23 @@ TEST (Point3, normalize) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) {
return P.distance(Q);
}
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));
}
/* ************************************************************************* */
int main() {
TestResult tr;