From df5ecac604b396139c72eafc0ad806532cdcde67 Mon Sep 17 00:00:00 2001 From: David Date: Tue, 16 Jun 2020 11:30:44 +1000 Subject: [PATCH 1/6] Add unit test for oriented plane 3 factor jacobian --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 81f67f1ee..36ae57201 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -122,6 +122,42 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } +double randDouble(double max = 1) { + return static_cast(rand()) / RAND_MAX * max; +} + +TEST( OrientedPlane3Factor, Derivatives ) { + for (int i=0; i<100; i++) { + // Random measurement + OrientedPlane3 p(randDouble(), randDouble(), randDouble(), randDouble()); + + // Random linearisation point + OrientedPlane3 pLin(randDouble(), randDouble(), randDouble(), randDouble()); + gtsam::Point3 pointLin = gtsam::Point3(randDouble(100), randDouble(100), randDouble(100)); + gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(randDouble(2*M_PI), randDouble(2*M_PI), randDouble(2*M_PI)); + Pose3 poseLin(rotationLin, pointLin); + + // Factor + Key planeKey(1), poseKey(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); + + // Calculate numerical derivatives + boost::function f = boost::bind( + &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); + Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(poseLin, pLin, actualH1, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); + } +} + // ************************************************************************* TEST( OrientedPlane3DirectionPrior, Constructor ) { From 75eb859ee7dc2ea0ff44ef7b26431f115a0d2e7e Mon Sep 17 00:00:00 2001 From: David Date: Mon, 22 Jun 2020 14:39:56 +1000 Subject: [PATCH 2/6] Fix OrientedPlane3Factor jacobian using numericalDerivative --- gtsam/geometry/OrientedPlane3.h | 4 ++++ gtsam/slam/OrientedPlane3Factor.h | 24 ++++++++++++++++--- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 6 ++--- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 61d8a30d2..ad6053234 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -114,6 +114,10 @@ public: */ 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. * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * Unit3::localCoordinates. This one has correct derivatives. diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 1f56adc45..d1f2329c4 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include @@ -48,10 +49,27 @@ public: virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, - H2); 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(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); } ; diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 36ae57201..65491648f 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -83,7 +83,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away 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; Values new_values; @@ -153,8 +153,8 @@ TEST( OrientedPlane3Factor, Derivatives ) { factor.evaluateError(poseLin, pLin, actualH1, actualH2); // Verify we get the expected error - EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); - EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); + EXPECT(assert_equal(numericalH1, actualH1, 1e-7)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-7)); } } From 58a0f82cba0acb9e458ed8d09d5403c8904d7108 Mon Sep 17 00:00:00 2001 From: David Date: Tue, 23 Jun 2020 19:02:51 +1000 Subject: [PATCH 3/6] Address Frank's comments and clean up changes --- gtsam/geometry/OrientedPlane3.cpp | 10 +++- gtsam/geometry/OrientedPlane3.h | 4 +- gtsam/slam/OrientedPlane3Factor.cpp | 27 ++++++++++ gtsam/slam/OrientedPlane3Factor.h | 27 +--------- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 50 ++++++++----------- 5 files changed, 62 insertions(+), 56 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 5cfa80779..76a569ffa 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -20,6 +20,7 @@ #include #include #include +#include using namespace std; @@ -58,7 +59,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> } /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { +Vector3 OrientedPlane3::error(const OrientedPlane3& plane, + OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + // Numerically calculate the derivative since this function doesn't provide one. + auto f = boost::bind(&OrientedPlane3::Error, _1, _2); + if (H1) *H1 = numericalDerivative21(f, *this, plane); + if (H2) *H2 = numericalDerivative22(f, *this, plane); + Vector2 n_error = -n_.localCoordinates(plane.n_); return Vector3(n_error(0), n_error(1), d_ - plane.d_); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index ad6053234..e56e650ab 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -112,7 +112,9 @@ public: * The error is a norm 1 difference in tangent space. * @param the other plane */ - Vector3 error(const OrientedPlane3& plane) const; + Vector3 error(const OrientedPlane3& plane, + OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2 = boost::none) const; static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) { return plane1.error(plane2); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 56968301a..699148e92 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -19,6 +19,33 @@ void OrientedPlane3Factor::print(const string& s, this->noiseModel_->print(" noise model: "); } +//*************************************************************************** +Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, + const OrientedPlane3& plane, boost::optional H1, + boost::optional H2) const { + Vector err(3); + + if (H1 || H2) { + Matrix36 predicted_H_pose; + Matrix33 predicted_H_plane, error_H_predicted; + + OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose); + err << predicted_plane.error(measured_p_, error_H_predicted); + + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = error_H_predicted * predicted_H_pose; + } + if (H2) { + *H2 = error_H_predicted * predicted_H_plane; + } + } else { + OrientedPlane3 predicted_plane = plane.transform(pose); + err << predicted_plane.error(measured_p_); + } + return (err); +} + //*************************************************************************** void OrientedPlane3DirectionPrior::print(const string& s, const KeyFormatter& keyFormatter) const { diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index d1f2329c4..282873b4e 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,7 +7,6 @@ #pragma once -#include #include #include @@ -48,31 +47,7 @@ public: /// evaluateError virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - Vector err(3); - - 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(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); - } - ; + boost::none) const; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 65491648f..6744dcd50 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -122,40 +122,34 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } -double randDouble(double max = 1) { - return static_cast(rand()) / RAND_MAX * max; -} - TEST( OrientedPlane3Factor, Derivatives ) { - for (int i=0; i<100; i++) { - // Random measurement - OrientedPlane3 p(randDouble(), randDouble(), randDouble(), randDouble()); + // Measurement + OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); - // Random linearisation point - OrientedPlane3 pLin(randDouble(), randDouble(), randDouble(), randDouble()); - gtsam::Point3 pointLin = gtsam::Point3(randDouble(100), randDouble(100), randDouble(100)); - gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(randDouble(2*M_PI), randDouble(2*M_PI), randDouble(2*M_PI)); - Pose3 poseLin(rotationLin, pointLin); + // Linearisation point + OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7); + gtsam::Point3 pointLin = gtsam::Point3(1, 2, -4); + gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI); + Pose3 poseLin(rotationLin, pointLin); - // Factor - Key planeKey(1), poseKey(2); - SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); + // Factor + Key planeKey(1), poseKey(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); - // Calculate numerical derivatives - boost::function f = boost::bind( - &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); - Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); - Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); + // Calculate numerical derivatives + boost::function f = boost::bind( + &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); + Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); - // Use the factor to calculate the derivative - Matrix actualH1, actualH2; - factor.evaluateError(poseLin, pLin, actualH1, actualH2); + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(poseLin, pLin, actualH1, actualH2); - // Verify we get the expected error - EXPECT(assert_equal(numericalH1, actualH1, 1e-7)); - EXPECT(assert_equal(numericalH2, actualH2, 1e-7)); - } + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); } // ************************************************************************* From 795380d5d7c57f999a715f2c304e103d24dbfa3c Mon Sep 17 00:00:00 2001 From: David Date: Wed, 24 Jun 2020 09:32:24 +1000 Subject: [PATCH 4/6] Update style and switch to errorVector() --- gtsam/slam/OrientedPlane3Factor.cpp | 32 ++++++++++++----------------- 1 file changed, 13 insertions(+), 19 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 699148e92..fc8bb3436 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -23,27 +23,21 @@ void OrientedPlane3Factor::print(const string& s, Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1, boost::optional H2) const { - Vector err(3); + Matrix36 predicted_H_pose; + Matrix33 predicted_H_plane, error_H_predicted; + OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, + H1 ? &predicted_H_pose : nullptr); + Vector3 err = predicted_plane.errorVector(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); - if (H1 || H2) { - Matrix36 predicted_H_pose; - Matrix33 predicted_H_plane, error_H_predicted; - - OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose); - err << predicted_plane.error(measured_p_, error_H_predicted); - - // Apply the chain rule to calculate the derivatives. - if (H1) { - *H1 = error_H_predicted * predicted_H_pose; - } - if (H2) { - *H2 = error_H_predicted * predicted_H_plane; - } - } else { - OrientedPlane3 predicted_plane = plane.transform(pose); - err << predicted_plane.error(measured_p_); + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = error_H_predicted * predicted_H_pose; } - return (err); + if (H2) { + *H2 = error_H_predicted * predicted_H_plane; + } + + return err; } //*************************************************************************** From a897ee786328154abb98950c21c1a24b2175456f Mon Sep 17 00:00:00 2001 From: David Date: Wed, 15 Jul 2020 19:18:39 +1000 Subject: [PATCH 5/6] Fix all unit tests. Only remaining item is the analytical Jacobian for Unit3::localCoordinates. --- gtsam/geometry/OrientedPlane3.cpp | 13 +++++++++--- gtsam/geometry/tests/testOrientedPlane3.cpp | 23 ++++++++++++++++++++- gtsam/geometry/tests/testUnit3.cpp | 1 + gtsam/slam/OrientedPlane3Factor.cpp | 2 +- 4 files changed, 34 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 76a569ffa..274d8220c 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -63,11 +63,18 @@ Vector3 OrientedPlane3::error(const OrientedPlane3& plane, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // Numerically calculate the derivative since this function doesn't provide one. - auto f = boost::bind(&OrientedPlane3::Error, _1, _2); - if (H1) *H1 = numericalDerivative21(f, *this, plane); - if (H2) *H2 = numericalDerivative22(f, *this, plane); + const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); Vector2 n_error = -n_.localCoordinates(plane.n_); + + if (H1) { + *H1 = I_3x3; + H1->block<2,2>(0,0) = -numericalDerivative21(f, n_, plane.n_);; + } + if (H2) { + *H2 = -I_3x3; + H2->block<2,2>(0,0) = -numericalDerivative22(f, n_, plane.n_);; + } return Vector3(n_error(0), n_error(1), d_ - plane.d_); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index dc5851319..36e566dca 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -138,7 +138,7 @@ TEST(OrientedPlane3, localCoordinates_retract) { } //******************************************************************************* -TEST (OrientedPlane3, error2) { +TEST (OrientedPlane3, errorVector) { OrientedPlane3 plane1(-1, 0.1, 0.2, 5); OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); @@ -161,6 +161,27 @@ TEST (OrientedPlane3, error2) { EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } +//******************************************************************************* +TEST (OrientedPlane3, error) { + // Hard-coded regression values, to ensure the result doesn't change. + OrientedPlane3 plane1(-1, 0.1, 0.2, 5); + OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); + + // Test the jacobians of transform + Matrix33 actualH1, expectedH1, actualH2, expectedH2; + Vector3 actual = plane1.error(plane2, actualH1, actualH2); + + EXPECT(assert_equal((Vector) Z_3x1, plane1.error(plane1), 1e-8)); + EXPECT(assert_equal(Vector3(0.0678852, 0.0761865, -0.4), actual, 1e-5)); + + boost::function f = // + boost::bind(&OrientedPlane3::error, _1, _2, boost::none, boost::none); + expectedH1 = numericalDerivative21(f, plane1, plane2); + expectedH2 = numericalDerivative22(f, plane1, plane2); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); +} + //******************************************************************************* TEST (OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a25ab25c7..ada735ed4 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -492,6 +492,7 @@ TEST(actualH, Serialization) { EXPECT(serializationTestHelpers::equalsBinary(p)); } + /* ************************************************************************* */ int main() { srand(time(nullptr)); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index fc8bb3436..7fd225e60 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -27,7 +27,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, Matrix33 predicted_H_plane, error_H_predicted; OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr); - Vector3 err = predicted_plane.errorVector(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); + Vector3 err = predicted_plane.error(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); // Apply the chain rule to calculate the derivatives. if (H1) { From 534fa6bb31e9a00f76c160d47adbe4553439685b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 6 Dec 2020 18:20:53 -0500 Subject: [PATCH 6/6] formatting and small fixes --- gtsam/geometry/OrientedPlane3.cpp | 4 ++-- gtsam/geometry/OrientedPlane3.h | 9 ++++----- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 274d8220c..6af79b5d1 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -82,8 +82,8 @@ Vector3 OrientedPlane3::error(const OrientedPlane3& plane, Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { Matrix22 H_n_error_this, H_n_error_other; - Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0, - H2 ? &H_n_error_other : 0); + Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0, + H2 ? &H_n_error_other : 0); double d_error = d_ - other.d_; diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index e56e650ab..20fa7c112 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -53,8 +53,8 @@ public: } /// Construct from a Unit3 and a distance - OrientedPlane3(const Unit3& s, double d) : - n_(s), d_(d) { + OrientedPlane3(const Unit3& n, double d) : + n_(n), d_(d) { } /// Construct from a vector of plane coefficients @@ -64,8 +64,7 @@ public: /// Construct from four numbers of plane coeffcients (a, b, c, d) OrientedPlane3(double a, double b, double c, double d) { - Point3 p(a, b, c); - n_ = Unit3(p); + n_ = Unit3(a, b, c); d_ = d; } @@ -110,7 +109,7 @@ public: /** Computes the error between two planes. * The error is a norm 1 difference in tangent space. - * @param the other plane + * @param plane The other plane */ Vector3 error(const OrientedPlane3& plane, OptionalJacobian<3,3> H1 = boost::none,