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

Fix OrientedPlane3Factor jacobian using numericalDerivative

release/4.3a0
David 2020-06-22 14:39:56 +10:00
parent df5ecac604
commit 75eb859ee7
3 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/OrientedPlane3.h
View File

@ -114,6 +114,10 @@ public:
*/ */
Vector3 error(const OrientedPlane3& plane) const; Vector3 error(const OrientedPlane3& plane) const;
static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) {
return plane1.error(plane2);
}
/** Computes the error between the two planes, with derivatives. /** Computes the error between the two planes, with derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
* Unit3::localCoordinates. This one has correct derivatives. * Unit3::localCoordinates. This one has correct derivatives.

24
gtsam/slam/OrientedPlane3Factor.h
View File

@ -7,6 +7,7 @@
#pragma once #pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/OrientedPlane3.h> #include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
@ -48,10 +49,27 @@ public:
virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
H2);
Vector err(3); Vector err(3);
err << predicted_plane.error(measured_p_);
if (H1 || H2) {
Matrix H1_1, H2_1;
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1);
err << predicted_plane.error(measured_p_);
// Numerically calculate the derivative since this function doesn't provide one.
auto f = boost::bind(&OrientedPlane3::Error, _1, _2);
Matrix H1_2 = numericalDerivative21<Vector3, OrientedPlane3, OrientedPlane3>(f, predicted_plane, measured_p_);
// Apply the chain rule to calculate the derivatives.
if (H1) {
*H1 = H1_2 * H1_1;
}
if (H2) {
*H2 = H1_2 * H2_1;
}
} else {
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2);
err << predicted_plane.error(measured_p_);
}
return (err); return (err);
} }
; ;

6
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -83,7 +83,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only. // Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away // Normal along -x, 3m away
Symbol lm_sym('p', 0); Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0);
ISAM2 isam2; ISAM2 isam2;
Values new_values; Values new_values;
@ -153,8 +153,8 @@ TEST( OrientedPlane3Factor, Derivatives ) {
factor.evaluateError(poseLin, pLin, actualH1, actualH2); factor.evaluateError(poseLin, pLin, actualH1, actualH2);
// Verify we get the expected error // Verify we get the expected error
EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); EXPECT(assert_equal(numericalH1, actualH1, 1e-7));
EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); EXPECT(assert_equal(numericalH2, actualH2, 1e-7));
} }
} }