commit
						f5ff7aa49f
					
				|  | @ -18,7 +18,7 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <gtsam/geometry/OrientedPlane3.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| 
 | ||||
| #include <iostream> | ||||
| 
 | ||||
| using namespace std; | ||||
|  | @ -28,12 +28,13 @@ namespace gtsam { | |||
| /* ************************************************************************* */ | ||||
| void OrientedPlane3::print(const string& s) const { | ||||
|   Vector4 coeffs = planeCoefficients(); | ||||
|   cout << s << " : " << coeffs << endl; | ||||
|   cout << s << " : " << coeffs.transpose() << endl; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp, | ||||
|     OptionalJacobian<3, 6> Hr) const { | ||||
| OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, | ||||
|                                          OptionalJacobian<3, 3> Hp, | ||||
|                                          OptionalJacobian<3, 6> Hr) const { | ||||
|   Matrix23 D_rotated_plane; | ||||
|   Matrix22 D_rotated_pose; | ||||
|   Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); | ||||
|  | @ -42,15 +43,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> | |||
|   double pred_d = n_.unitVector().dot(xr.translation()) + d_; | ||||
| 
 | ||||
|   if (Hr) { | ||||
|     *Hr = Matrix::Zero(3,6); | ||||
|     Hr->setZero(); | ||||
|     Hr->block<2, 3>(0, 0) = D_rotated_plane; | ||||
|     Hr->block<1, 3>(2, 3) = unit_vec; | ||||
|   } | ||||
|   if (Hp) { | ||||
|     Vector2 hpp = n_.basis().transpose() * xr.translation(); | ||||
|     *Hp = Z_3x3; | ||||
|     Hp->setZero(); | ||||
|     Hp->block<2, 2>(0, 0) = D_rotated_pose; | ||||
|     Hp->block<1, 2>(2, 0) = hpp; | ||||
|     Hp->block<1, 2>(2, 0) = n_.basis().transpose() * xr.translation(); | ||||
|     (*Hp)(2, 2) = 1; | ||||
|   } | ||||
| 
 | ||||
|  | @ -58,25 +58,20 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { | ||||
|   Vector2 n_error = -n_.localCoordinates(plane.n_); | ||||
|   return Vector3(n_error(0), n_error(1), d_ - plane.d_); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, | ||||
| 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, | ||||
|   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_; | ||||
| 
 | ||||
|   if (H1) { | ||||
|     *H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1; | ||||
|     *H1 << H_n_error_this, Z_2x1, 0, 0, 1; | ||||
|   } | ||||
|   if (H2) { | ||||
|     *H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1; | ||||
|     *H2 << H_n_error_other, Z_2x1, 0, 0, -1; | ||||
|   } | ||||
| 
 | ||||
|   return Vector3(n_error(0), n_error(1), d_error); | ||||
|  | @ -84,11 +79,11 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| OrientedPlane3 OrientedPlane3::retract(const Vector3& v, | ||||
|                                        OptionalJacobian<3,3> H) const { | ||||
|                                        OptionalJacobian<3, 3> H) const { | ||||
|   Matrix22 H_n; | ||||
|   Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); | ||||
|   Unit3 n_retract(n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); | ||||
|   if (H) { | ||||
|     *H << H_n, Vector2::Zero(), 0, 0, 1; | ||||
|     *H << H_n, Z_2x1, 0, 0, 1; | ||||
|   } | ||||
|   return OrientedPlane3(n_retract, d_ + v(2)); | ||||
| } | ||||
|  | @ -100,4 +95,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { | |||
|   return Vector3(n_local(0), n_local(1), -d_local); | ||||
| } | ||||
| 
 | ||||
| } | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -20,22 +20,21 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/geometry/Unit3.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <string> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * @brief Represents an infinite plane in 3D, which is composed of a planar normal and its | ||||
|  *  perpendicular distance to the origin. | ||||
|  * Currently it provides a transform of the plane, and a norm 1 differencing of two planes. | ||||
|  * @brief Represents an infinite plane in 3D, which is composed of a planar | ||||
|  * normal and its perpendicular distance to the origin. | ||||
|  * Currently it provides a transform of the plane, and a norm 1 differencing of | ||||
|  * two planes. | ||||
|  * Refer to Trevor12iros for more math details. | ||||
|  */ | ||||
| class GTSAM_EXPORT OrientedPlane3 { | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   Unit3 n_;     ///< The direction of the planar normal
 | ||||
|   double d_;    ///< The perpendicular distance to this plane
 | ||||
| 
 | ||||
|  | @ -53,19 +52,17 @@ 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
 | ||||
|   OrientedPlane3(const Vector4& vec) : | ||||
|     n_(vec(0), vec(1), vec(2)), d_(vec(3)) { | ||||
|   } | ||||
|   explicit OrientedPlane3(const Vector4& vec) | ||||
|       : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {} | ||||
| 
 | ||||
|   /// 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; | ||||
|   } | ||||
| 
 | ||||
|  | @ -90,37 +87,18 @@ public: | |||
|    * @return the transformed plane | ||||
|    */ | ||||
|   OrientedPlane3 transform(const Pose3& xr, | ||||
|       OptionalJacobian<3, 3> Hp = boost::none, | ||||
|       OptionalJacobian<3, 6> Hr = boost::none) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * @deprecated the static method has wrong Jacobian order, | ||||
|    *    please use the member method transform() | ||||
|    * @param The raw plane | ||||
|    * @param xr a transformation in current coordiante | ||||
|    * @param Hr optional jacobian wrpt the pose transformation | ||||
|    * @param Hp optional Jacobian wrpt the destination plane | ||||
|    * @return the transformed plane | ||||
|    */ | ||||
|   static OrientedPlane3 Transform(const OrientedPlane3& plane, | ||||
|       const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, | ||||
|       OptionalJacobian<3, 3> Hp = boost::none) { | ||||
|       return plane.transform(xr, Hp, Hr); | ||||
|   } | ||||
| 
 | ||||
|   /** Computes the error between two planes.
 | ||||
|    *  The error is a norm 1 difference in tangent space. | ||||
|    * @param the other plane | ||||
|    */ | ||||
|   Vector3 error(const OrientedPlane3& plane) const; | ||||
|                            OptionalJacobian<3, 3> Hp = boost::none, | ||||
|                            OptionalJacobian<3, 6> Hr = boost::none) const; | ||||
| 
 | ||||
|   /** 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. | ||||
|    *  This uses Unit3::errorVector, as opposed to the other .error() in this | ||||
|    *  class, which uses Unit3::localCoordinates. This one has correct | ||||
|    *  derivatives. | ||||
|    *  NOTE(hayk): The derivatives are zero when normals are exactly orthogonal. | ||||
|    * @param the other plane | ||||
|    * @param other the other plane | ||||
|    */ | ||||
|   Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
 | ||||
|   Vector3 errorVector(const OrientedPlane3& other, | ||||
|                       OptionalJacobian<3, 3> H1 = boost::none, | ||||
|                       OptionalJacobian<3, 3> H2 = boost::none) const; | ||||
| 
 | ||||
|   /// Dimensionality of tangent space = 3 DOF
 | ||||
|  | @ -134,7 +112,8 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// The retract function
 | ||||
|   OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const; | ||||
|   OrientedPlane3 retract(const Vector3& v, | ||||
|                         OptionalJacobian<3, 3> H = boost::none) const; | ||||
| 
 | ||||
|   /// The local coordinates function
 | ||||
|   Vector3 localCoordinates(const OrientedPlane3& s) const; | ||||
|  | @ -166,5 +145,5 @@ template<> struct traits<const OrientedPlane3> : public internal::Manifold< | |||
| OrientedPlane3> { | ||||
| }; | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
| }  // namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -31,7 +31,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) | |||
| GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) | ||||
| 
 | ||||
| //*******************************************************************************
 | ||||
| TEST (OrientedPlane3, getMethods) { | ||||
| TEST(OrientedPlane3, getMethods) { | ||||
|   Vector4 c; | ||||
|   c << -1, 0, 0, 5; | ||||
|   OrientedPlane3 plane1(c); | ||||
|  | @ -50,44 +50,27 @@ TEST (OrientedPlane3, getMethods) { | |||
| 
 | ||||
| 
 | ||||
| //*******************************************************************************
 | ||||
| OrientedPlane3 Transform_(const OrientedPlane3& plane,  const Pose3& xr) { | ||||
|   return OrientedPlane3::Transform(plane, xr); | ||||
| } | ||||
| 
 | ||||
| OrientedPlane3 transform_(const OrientedPlane3& plane,  const Pose3& xr) { | ||||
|   return plane.transform(xr); | ||||
| } | ||||
| 
 | ||||
| TEST (OrientedPlane3, transform) { | ||||
| TEST(OrientedPlane3, transform) { | ||||
|   gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0), | ||||
|       gtsam::Point3(2.0, 3.0, 4.0)); | ||||
|                     gtsam::Point3(2.0, 3.0, 4.0)); | ||||
|   OrientedPlane3 plane(-1, 0, 0, 5); | ||||
|   OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); | ||||
|   OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose, | ||||
|       none, none); | ||||
|   OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none); | ||||
|   EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5)); | ||||
|   EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5)); | ||||
|   OrientedPlane3 transformedPlane = plane.transform(pose, none, none); | ||||
|   EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5)); | ||||
| 
 | ||||
|   // Test the jacobians of transform
 | ||||
|   Matrix actualH1, expectedH1, actualH2, expectedH2; | ||||
|   { | ||||
|     // because the Transform class uses a wrong order of Jacobians in interface
 | ||||
|     OrientedPlane3::Transform(plane, pose, actualH1, none); | ||||
|     expectedH1 = numericalDerivative22(Transform_, plane, pose); | ||||
|     EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); | ||||
|     OrientedPlane3::Transform(plane, pose, none, actualH2); | ||||
|     expectedH2 = numericalDerivative21(Transform_, plane, pose); | ||||
|     EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); | ||||
|   } | ||||
|   { | ||||
|     plane.transform(pose, actualH1, none); | ||||
|     expectedH1 = numericalDerivative21(transform_, plane, pose); | ||||
|     EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); | ||||
|     plane.transform(pose, none, actualH2); | ||||
|     expectedH2 = numericalDerivative22(Transform_, plane, pose); | ||||
|     EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); | ||||
|   } | ||||
|   expectedH1 = numericalDerivative21(transform_, plane, pose); | ||||
|   plane.transform(pose, actualH1, none); | ||||
|   EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); | ||||
| 
 | ||||
|   expectedH2 = numericalDerivative22(transform_, plane, pose); | ||||
|   plane.transform(pose, none, actualH2); | ||||
|   EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| //*******************************************************************************
 | ||||
|  | @ -109,7 +92,6 @@ inline static Vector randomVector(const Vector& minLimits, | |||
| 
 | ||||
| //*******************************************************************************
 | ||||
| TEST(OrientedPlane3, localCoordinates_retract) { | ||||
| 
 | ||||
|   size_t numIterations = 10000; | ||||
|   Vector4 minPlaneLimit, maxPlaneLimit; | ||||
|   minPlaneLimit << -1.0, -1.0, -1.0, 0.01; | ||||
|  | @ -119,7 +101,6 @@ TEST(OrientedPlane3, localCoordinates_retract) { | |||
|   minXiLimit << -M_PI, -M_PI, -10.0; | ||||
|   maxXiLimit << M_PI, M_PI, 10.0; | ||||
|   for (size_t i = 0; i < numIterations; i++) { | ||||
| 
 | ||||
|     // Create a Plane
 | ||||
|     OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit)); | ||||
|     Vector v12 = randomVector(minXiLimit, maxXiLimit); | ||||
|  | @ -138,22 +119,24 @@ 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); | ||||
| 
 | ||||
|   // Hard-coded regression values, to ensure the result doesn't change.
 | ||||
|   EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8)); | ||||
|   EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5)); | ||||
|   EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), | ||||
|                       plane1.errorVector(plane2), 1e-5)); | ||||
| 
 | ||||
|   // Test the jacobians of transform
 | ||||
|   Matrix33 actualH1, expectedH1, actualH2, expectedH2; | ||||
| 
 | ||||
|   Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2); | ||||
|   EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1]))); | ||||
|   EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), | ||||
|                       Vector2(actual[0], actual[1]))); | ||||
|   EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); | ||||
| 
 | ||||
|   boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = //
 | ||||
|   boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = | ||||
|       boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); | ||||
|   expectedH1 = numericalDerivative21(f, plane1, plane2); | ||||
|   expectedH2 = numericalDerivative22(f, plane1, plane2); | ||||
|  | @ -162,19 +145,19 @@ TEST (OrientedPlane3, error2) { | |||
| } | ||||
| 
 | ||||
| //*******************************************************************************
 | ||||
| TEST (OrientedPlane3, jacobian_retract) { | ||||
| TEST(OrientedPlane3, jacobian_retract) { | ||||
|   OrientedPlane3 plane(-1, 0.1, 0.2, 5); | ||||
|   Matrix33 H_actual; | ||||
|   boost::function<OrientedPlane3(const Vector3&)> f = | ||||
|       boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); | ||||
|   { | ||||
|       Vector3 v (-0.1, 0.2, 0.3); | ||||
|       Vector3 v(-0.1, 0.2, 0.3); | ||||
|       plane.retract(v, H_actual); | ||||
|       Matrix H_expected_numerical = numericalDerivative11(f, v); | ||||
|       EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5)); | ||||
|   } | ||||
|   { | ||||
|       Vector3 v (0, 0, 0); | ||||
|       Vector3 v(0, 0, 0); | ||||
|       plane.retract(v, H_actual); | ||||
|       Matrix H_expected_numerical = numericalDerivative11(f, v); | ||||
|       EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5)); | ||||
|  |  | |||
|  | @ -501,6 +501,7 @@ TEST(actualH, Serialization) { | |||
|   EXPECT(serializationTestHelpers::equalsBinary(p)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   srand(time(nullptr)); | ||||
|  |  | |||
|  | @ -2,7 +2,7 @@ | |||
|  * OrientedPlane3Factor.cpp | ||||
|  * | ||||
|  *  Created on: Jan 29, 2014 | ||||
|  *      Author: Natesh Srinivasan | ||||
|  *  Author: Natesh Srinivasan | ||||
|  */ | ||||
| 
 | ||||
| #include "OrientedPlane3Factor.h" | ||||
|  | @ -14,15 +14,42 @@ namespace gtsam { | |||
| //***************************************************************************
 | ||||
| void OrientedPlane3Factor::print(const string& s, | ||||
|     const KeyFormatter& keyFormatter) const { | ||||
|   cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n"; | ||||
|   cout << s << (s == "" ? "" : "\n"); | ||||
|   cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " | ||||
|        << keyFormatter(key2()) << ")\n"; | ||||
|   measured_p_.print("Measured Plane"); | ||||
|   this->noiseModel_->print("  noise model: "); | ||||
| } | ||||
| 
 | ||||
| //***************************************************************************
 | ||||
| Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, | ||||
|     const OrientedPlane3& plane, boost::optional<Matrix&> H1, | ||||
|     boost::optional<Matrix&> H2) const { | ||||
|   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); | ||||
| 
 | ||||
|   // 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; | ||||
|   } | ||||
| 
 | ||||
|   return err; | ||||
| } | ||||
| 
 | ||||
| //***************************************************************************
 | ||||
| void OrientedPlane3DirectionPrior::print(const string& s, | ||||
|     const KeyFormatter& keyFormatter) const { | ||||
|   cout << "Prior Factor on " << landmarkKey_ << "\n"; | ||||
|   cout << s << (s == "" ? "" : "\n"); | ||||
|   cout << s << "Prior Factor on " << keyFormatter(key()) << "\n"; | ||||
|   measured_p_.print("Measured Plane"); | ||||
|   this->noiseModel_->print("  noise model: "); | ||||
| } | ||||
|  | @ -36,26 +63,17 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, | |||
| } | ||||
| 
 | ||||
| //***************************************************************************
 | ||||
| 
 | ||||
| Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, | ||||
|     boost::optional<Matrix&> H) const { | ||||
| 
 | ||||
| Vector OrientedPlane3DirectionPrior::evaluateError( | ||||
|     const OrientedPlane3& plane, boost::optional<Matrix&> H) const { | ||||
|   Unit3 n_hat_p = measured_p_.normal(); | ||||
|   Unit3 n_hat_q = plane.normal(); | ||||
|   Matrix2 H_p; | ||||
|   Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr); | ||||
|   if (H) { | ||||
|     Matrix H_p; | ||||
|     Unit3 n_hat_p = measured_p_.normal(); | ||||
|     Unit3 n_hat_q = plane.normal(); | ||||
|     Vector e = n_hat_p.error(n_hat_q, H_p); | ||||
|     H->resize(2, 3); | ||||
|     H->block<2, 2>(0, 0) << H_p; | ||||
|     H->block<2, 1>(0, 2) << Z_2x1; | ||||
|     return e; | ||||
|   } else { | ||||
|     Unit3 n_hat_p = measured_p_.normal(); | ||||
|     Unit3 n_hat_q = plane.normal(); | ||||
|     Vector e = n_hat_p.error(n_hat_q); | ||||
|     return e; | ||||
|     *H << H_p, Z_2x1; | ||||
|   } | ||||
| 
 | ||||
| } | ||||
|   return e; | ||||
| } | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -16,66 +16,54 @@ namespace gtsam { | |||
|  * Factor to measure a planar landmark from a given pose | ||||
|  */ | ||||
| class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> { | ||||
| 
 | ||||
| protected: | ||||
|   Key poseKey_; | ||||
|   Key landmarkKey_; | ||||
|   Vector measured_coeffs_; | ||||
|  protected: | ||||
|   OrientedPlane3 measured_p_; | ||||
| 
 | ||||
|   typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|  public: | ||||
|   /// Constructor
 | ||||
|   OrientedPlane3Factor() { | ||||
|   } | ||||
|   ~OrientedPlane3Factor() override {} | ||||
| 
 | ||||
|   /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
 | ||||
|   OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel, | ||||
|       const Key& pose, const Key& landmark) : | ||||
|       Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_( | ||||
|           z) { | ||||
|     measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); | ||||
|   } | ||||
|   /** Constructor with measured plane (a,b,c,d) coefficients
 | ||||
|    * @param z measured plane (a,b,c,d) coefficients as 4D vector | ||||
|    * @param noiseModel noiseModel Gaussian noise model | ||||
|    * @param poseKey Key or symbol for unknown pose | ||||
|    * @param landmarkKey Key or symbol for unknown planar landmark | ||||
|    * @return the transformed plane | ||||
|    */ | ||||
|   OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel, | ||||
|                        Key poseKey, Key landmarkKey) | ||||
|       : Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {} | ||||
| 
 | ||||
|   /// print
 | ||||
|   void print(const std::string& s = "OrientedPlane3Factor", | ||||
|       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; | ||||
| 
 | ||||
|   /// evaluateError
 | ||||
|   Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, | ||||
|       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = | ||||
|           boost::none) const override { | ||||
|     OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, | ||||
|         H2); | ||||
|     Vector err(3); | ||||
|     err << predicted_plane.error(measured_p_); | ||||
|     return (err); | ||||
|   } | ||||
|   ; | ||||
|   Vector evaluateError( | ||||
|       const Pose3& pose, const OrientedPlane3& plane, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const override; | ||||
| }; | ||||
| 
 | ||||
| // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
 | ||||
| class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> { | ||||
| protected: | ||||
|   OrientedPlane3 measured_p_; /// measured plane parameters
 | ||||
|   Key landmarkKey_; | ||||
| class OrientedPlane3DirectionPrior : public NoiseModelFactor1<OrientedPlane3> { | ||||
|  protected: | ||||
|   OrientedPlane3 measured_p_;  /// measured plane parameters
 | ||||
|   typedef NoiseModelFactor1<OrientedPlane3> Base; | ||||
| public: | ||||
| 
 | ||||
|  public: | ||||
|   typedef OrientedPlane3DirectionPrior This; | ||||
|   /// Constructor
 | ||||
|   OrientedPlane3DirectionPrior() { | ||||
|   } | ||||
| 
 | ||||
|   /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
 | ||||
|   OrientedPlane3DirectionPrior(Key key, const Vector&z, | ||||
|       const SharedGaussian& noiseModel) : | ||||
|       Base(noiseModel, key), landmarkKey_(key) { | ||||
|     measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); | ||||
|   } | ||||
|   OrientedPlane3DirectionPrior(Key key, const Vector4& z, | ||||
|                                const SharedGaussian& noiseModel) | ||||
|       : Base(noiseModel, key), measured_p_(z) {} | ||||
| 
 | ||||
|   /// print
 | ||||
|   void print(const std::string& s = "OrientedPlane3DirectionPrior", | ||||
|  |  | |||
|  | @ -10,20 +10,24 @@ | |||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /*
 | ||||
|  * @file testOrientedPlane3.cpp | ||||
|  * @file testOrientedPlane3Factor.cpp | ||||
|  * @date Dec 19, 2012 | ||||
|  * @author Alex Trevor | ||||
|  * @brief Tests the OrientedPlane3Factor class | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/slam/OrientedPlane3Factor.h> | ||||
| #include <gtsam/nonlinear/ISAM2.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| 
 | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/nonlinear/ISAM2.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <boost/bind.hpp> | ||||
| 
 | ||||
| #include <boost/assign/std/vector.hpp> | ||||
| #include <boost/assign/std.hpp> | ||||
| #include <boost/bind.hpp> | ||||
| 
 | ||||
| using namespace boost::assign; | ||||
| using namespace gtsam; | ||||
|  | @ -32,46 +36,46 @@ using namespace std; | |||
| GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) | ||||
| GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) | ||||
| 
 | ||||
| using symbol_shorthand::P;  //< Planes
 | ||||
| using symbol_shorthand::X;  //< Pose3
 | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
| TEST (OrientedPlane3Factor, lm_translation_error) { | ||||
| TEST(OrientedPlane3Factor, lm_translation_error) { | ||||
|   // Tests one pose, two measurements of the landmark that differ in range only.
 | ||||
|   // Normal along -x, 3m away
 | ||||
|   Symbol lm_sym('p', 0); | ||||
|   OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); | ||||
| 
 | ||||
|   ISAM2 isam2; | ||||
|   Values new_values; | ||||
|   NonlinearFactorGraph new_graph; | ||||
|   NonlinearFactorGraph graph; | ||||
| 
 | ||||
|   // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
 | ||||
|   Symbol init_sym('x', 0); | ||||
|   // Init pose and prior.  Pose Prior is needed since a single plane measurement
 | ||||
|   // does not fully constrain the pose
 | ||||
|   Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); | ||||
|   Vector sigmas(6); | ||||
|   Vector6 sigmas; | ||||
|   sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; | ||||
|   new_graph.addPrior( | ||||
|       init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas)); | ||||
|   new_values.insert(init_sym, init_pose); | ||||
|   graph.addPrior(X(0), init_pose, noiseModel::Diagonal::Sigmas(sigmas)); | ||||
| 
 | ||||
|   // Add two landmark measurements, differing in range
 | ||||
|   new_values.insert(lm_sym, test_lm0); | ||||
|   Vector sigmas3(3); | ||||
|   sigmas3 << 0.1, 0.1, 0.1; | ||||
|   Vector test_meas0_mean(4); | ||||
|   test_meas0_mean << -1.0, 0.0, 0.0, 3.0; | ||||
|   OrientedPlane3Factor test_meas0(test_meas0_mean, | ||||
|       noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); | ||||
|   new_graph.add(test_meas0); | ||||
|   Vector test_meas1_mean(4); | ||||
|   test_meas1_mean << -1.0, 0.0, 0.0, 1.0; | ||||
|   OrientedPlane3Factor test_meas1(test_meas1_mean, | ||||
|       noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); | ||||
|   new_graph.add(test_meas1); | ||||
|   Vector3 sigmas3 {0.1, 0.1, 0.1}; | ||||
|   Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; | ||||
|   OrientedPlane3Factor factor0( | ||||
|       measurement0, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0)); | ||||
|   graph.add(factor0); | ||||
|   Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; | ||||
|   OrientedPlane3Factor factor1( | ||||
|       measurement1, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0)); | ||||
|   graph.add(factor1); | ||||
| 
 | ||||
|   // Initial Estimate
 | ||||
|   Values values; | ||||
|   values.insert(X(0), init_pose); | ||||
|   values.insert(P(0), test_lm0); | ||||
| 
 | ||||
|   // Optimize
 | ||||
|   ISAM2Result result = isam2.update(new_graph, new_values); | ||||
|   ISAM2 isam2; | ||||
|   ISAM2Result result = isam2.update(graph, values); | ||||
|   Values result_values = isam2.calculateEstimate(); | ||||
|   OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>( | ||||
|       lm_sym); | ||||
|   OrientedPlane3 optimized_plane_landmark = | ||||
|       result_values.at<OrientedPlane3>(P(0)); | ||||
| 
 | ||||
|   // Given two noisy measurements of equal weight, expect result between the two
 | ||||
|   OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); | ||||
|  | @ -79,48 +83,80 @@ TEST (OrientedPlane3Factor, lm_translation_error) { | |||
| } | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
| // TODO As described in PR #564 after correcting the derivatives in
 | ||||
| // OrientedPlane3Factor this test fails. It should be debugged and re-enabled.
 | ||||
| /*
 | ||||
| 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; | ||||
|   NonlinearFactorGraph new_graph; | ||||
|   NonlinearFactorGraph graph; | ||||
| 
 | ||||
|   // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
 | ||||
|   Symbol init_sym('x', 0); | ||||
|   Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); | ||||
|   new_graph.addPrior(init_sym, init_pose, | ||||
|   graph.addPrior(X(0), init_pose, | ||||
|       noiseModel::Diagonal::Sigmas( | ||||
|           (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); | ||||
|   new_values.insert(init_sym, init_pose); | ||||
| 
 | ||||
| //  // Add two landmark measurements, differing in angle
 | ||||
|   new_values.insert(lm_sym, test_lm0); | ||||
|   Vector test_meas0_mean(4); | ||||
|   test_meas0_mean << -1.0, 0.0, 0.0, 3.0; | ||||
|   OrientedPlane3Factor test_meas0(test_meas0_mean, | ||||
|       noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); | ||||
|   new_graph.add(test_meas0); | ||||
|   Vector test_meas1_mean(4); | ||||
|   test_meas1_mean << 0.0, -1.0, 0.0, 3.0; | ||||
|   OrientedPlane3Factor test_meas1(test_meas1_mean, | ||||
|       noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); | ||||
|   new_graph.add(test_meas1); | ||||
|   // Add two landmark measurements, differing in angle
 | ||||
|   Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; | ||||
|   OrientedPlane3Factor factor0(measurement0, | ||||
|       noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0)); | ||||
|   graph.add(factor0); | ||||
|   Vector4 measurement1 {0.0, -1.0, 0.0, 3.0}; | ||||
|   OrientedPlane3Factor factor1(measurement1, | ||||
|       noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0)); | ||||
|   graph.add(factor1); | ||||
| 
 | ||||
|   // Initial Estimate
 | ||||
|   Values values; | ||||
|   values.insert(X(0), init_pose); | ||||
|   values.insert(P(0), test_lm0); | ||||
| 
 | ||||
|   // Optimize
 | ||||
|   ISAM2Result result = isam2.update(new_graph, new_values); | ||||
|   ISAM2 isam2; | ||||
|   ISAM2Result result = isam2.update(graph, values); | ||||
|   Values result_values = isam2.calculateEstimate(); | ||||
|   OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>( | ||||
|       lm_sym); | ||||
|   auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0)); | ||||
| 
 | ||||
|   // Given two noisy measurements of equal weight, expect result between the two
 | ||||
|   OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, | ||||
|       0.0, 3.0); | ||||
|   EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); | ||||
| } | ||||
| */ | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
| TEST( OrientedPlane3Factor, Derivatives ) { | ||||
|   // Measurement
 | ||||
|   OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); | ||||
| 
 | ||||
|   // 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); | ||||
| 
 | ||||
|   // Calculate numerical derivatives
 | ||||
|   boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind( | ||||
|       &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); | ||||
|   Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin); | ||||
|   Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(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 ) { | ||||
|  | @ -129,7 +165,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { | |||
|   // If pitch and roll are zero for an aerospace frame,
 | ||||
|   // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
 | ||||
| 
 | ||||
|   Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin
 | ||||
|   Vector4 planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin
 | ||||
| 
 | ||||
|   // Factor
 | ||||
|   Key key(1); | ||||
|  | @ -137,9 +173,9 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { | |||
|   OrientedPlane3DirectionPrior factor(key, planeOrientation, model); | ||||
| 
 | ||||
|   // Create a linearization point at the zero-error point
 | ||||
|   Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); | ||||
|   Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0); | ||||
|   Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); | ||||
|   Vector4 theta1 {0.0, 0.02, -1.2, 10.0}; | ||||
|   Vector4 theta2 {0.0, 0.1, -0.8, 10.0}; | ||||
|   Vector4 theta3 {0.0, 0.2, -0.9, 10.0}; | ||||
| 
 | ||||
|   OrientedPlane3 T1(theta1); | ||||
|   OrientedPlane3 T2(theta2); | ||||
|  | @ -170,6 +206,59 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { | |||
|   EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Simplified version of the test by Marco Camurri to debug issue #561
 | ||||
| TEST(OrientedPlane3Factor, Issue561Simplified) { | ||||
|   // Typedefs
 | ||||
|   using Plane = OrientedPlane3; | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
| 
 | ||||
|   // Setup prior factors
 | ||||
|   // Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
 | ||||
|   Pose3 x0(Rot3::identity(), Vector3(10, -1, 1)); | ||||
|   auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); | ||||
|   graph.addPrior<Pose3>(X(0), x0, x0_noise); | ||||
| 
 | ||||
|   // Two horizontal planes with different heights, in the world frame.
 | ||||
|   const Plane p1(0,0,1,1), p2(0,0,1,2); | ||||
|   auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); | ||||
|   auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); | ||||
|   graph.addPrior<Plane>(P(1), p1, p1_noise); | ||||
|   graph.addPrior<Plane>(P(2), p2, p2_noise); | ||||
| 
 | ||||
|   // Plane factors
 | ||||
|   auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement
 | ||||
|   auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement
 | ||||
|   const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); | ||||
|   const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); | ||||
|   graph.emplace_shared<OrientedPlane3Factor>( | ||||
|       p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); | ||||
|   graph.emplace_shared<OrientedPlane3Factor>( | ||||
|       p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); | ||||
| 
 | ||||
|   // Initial values
 | ||||
|   // Just offset the initial pose by 1m. This is what we are trying to optimize.
 | ||||
|   Values initialEstimate; | ||||
|   Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); | ||||
|   initialEstimate.insert(P(1), p1); | ||||
|   initialEstimate.insert(P(2), p2); | ||||
|   initialEstimate.insert(X(0), x0_initial); | ||||
| 
 | ||||
|   // Optimize
 | ||||
|   try { | ||||
|     GaussNewtonOptimizer optimizer(graph, initialEstimate); | ||||
|     Values result = optimizer.optimize(); | ||||
|     EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); | ||||
|     EXPECT(x0.equals(result.at<Pose3>(X(0)))); | ||||
|     EXPECT(p1.equals(result.at<Plane>(P(1)))); | ||||
|     EXPECT(p2.equals(result.at<Plane>(P(2)))); | ||||
|   } catch (const IndeterminantLinearSystemException &e) { | ||||
|     std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; | ||||
|     EXPECT(false); // fail if this happens
 | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   srand(time(nullptr)); | ||||
|  |  | |||
|  | @ -0,0 +1,64 @@ | |||
| /*
 | ||||
|  * LocalOrientedPlane3Factor.cpp | ||||
|  * | ||||
|  *  Author: David Wisth | ||||
|  *  Created on: February 12, 2021 | ||||
|  */ | ||||
| 
 | ||||
| #include "LocalOrientedPlane3Factor.h" | ||||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| //***************************************************************************
 | ||||
| void LocalOrientedPlane3Factor::print(const string& s, | ||||
|     const KeyFormatter& keyFormatter) const { | ||||
|   cout << s << (s == "" ? "" : "\n"); | ||||
|   cout << "LocalOrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " | ||||
|        << keyFormatter(key2()) << ", " << keyFormatter(key3()) << ")\n"; | ||||
|   measured_p_.print("Measured Plane"); | ||||
|   this->noiseModel_->print("  noise model: "); | ||||
| } | ||||
| 
 | ||||
| //***************************************************************************
 | ||||
| Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi, | ||||
|     const Pose3& wTwa, const OrientedPlane3& a_plane, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, | ||||
|     boost::optional<Matrix&> H3) const { | ||||
| 
 | ||||
|   Matrix66 aTai_H_wTwa, aTai_H_wTwi; | ||||
|   Matrix36 predicted_H_aTai; | ||||
|   Matrix33 predicted_H_plane, error_H_predicted; | ||||
| 
 | ||||
|   // Find the relative transform from anchor to sensor frame.
 | ||||
|   const Pose3 aTai = wTwa.transformPoseTo(wTwi, | ||||
|       H2 ? &aTai_H_wTwa : nullptr, | ||||
|       H1 ? &aTai_H_wTwi : nullptr); | ||||
| 
 | ||||
|   // Transform the plane measurement into sensor frame.
 | ||||
|   const OrientedPlane3 i_plane = a_plane.transform(aTai, | ||||
|       H2 ? &predicted_H_plane : nullptr, | ||||
|       (H1 || H3) ? &predicted_H_aTai  : nullptr); | ||||
| 
 | ||||
|   // Calculate the error between measured and estimated planes in sensor frame.
 | ||||
|   const Vector3 err = measured_p_.errorVector(i_plane, | ||||
|     boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr); | ||||
| 
 | ||||
|   // Apply the chain rule to calculate the derivatives.
 | ||||
|   if (H1) { | ||||
|     *H1 = error_H_predicted * predicted_H_aTai * aTai_H_wTwi; | ||||
|   } | ||||
| 
 | ||||
|   if (H2) { | ||||
|     *H2 = error_H_predicted * predicted_H_aTai * aTai_H_wTwa; | ||||
|   } | ||||
| 
 | ||||
|   if (H3) { | ||||
|     *H3 = error_H_predicted * predicted_H_plane; | ||||
|   } | ||||
| 
 | ||||
|   return err; | ||||
| } | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  | @ -0,0 +1,92 @@ | |||
| /*
 | ||||
|  * @file LocalOrientedPlane3Factor.h | ||||
|  * @brief LocalOrientedPlane3 Factor class | ||||
|  * @author David Wisth | ||||
|  * @date February 12, 2021 | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/geometry/OrientedPlane3.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <string> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * Factor to measure a planar landmark from a given pose, with a given local | ||||
|  * linearization point. | ||||
|  * | ||||
|  * This factor is based on the relative plane factor formulation proposed in: | ||||
|  * Equation 25, | ||||
|  * M. Kaess, "Simultaneous Localization and Mapping with Infinite Planes", | ||||
|  * IEEE International Conference on Robotics and Automation, 2015. | ||||
|  *  | ||||
|  * | ||||
|  * The main purpose of this factor is to improve the numerical stability of the | ||||
|  * optimization, especially compared to gtsam::OrientedPlane3Factor. This | ||||
|  * is especially relevant when the sensor is far from the origin (and thus | ||||
|  * the derivatives associated to transforming the plane are large). | ||||
|  * | ||||
|  * x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e. | ||||
|  * a local linearisation point for the plane. The plane is representated and | ||||
|  * optimized in x1 frame in the optimization. | ||||
|  */ | ||||
| class LocalOrientedPlane3Factor: public NoiseModelFactor3<Pose3, Pose3, | ||||
|                                                           OrientedPlane3> { | ||||
| protected: | ||||
|   OrientedPlane3 measured_p_; | ||||
|   typedef NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> Base; | ||||
| public: | ||||
|   /// Constructor
 | ||||
|   LocalOrientedPlane3Factor() {} | ||||
| 
 | ||||
|   virtual ~LocalOrientedPlane3Factor() {} | ||||
| 
 | ||||
|   /** Constructor with measured plane (a,b,c,d) coefficients
 | ||||
|    * @param z measured plane (a,b,c,d) coefficients as 4D vector | ||||
|    * @param noiseModel noiseModel Gaussian noise model | ||||
|    * @param poseKey Key or symbol for unknown pose | ||||
|    * @param anchorPoseKey Key or symbol for the plane's linearization point, | ||||
|                           (called the "anchor pose"). | ||||
|    * @param landmarkKey Key or symbol for unknown planar landmark | ||||
|    * | ||||
|    * Note: The anchorPoseKey can simply be chosen as the first pose a plane | ||||
|    * is observed.   | ||||
|    */ | ||||
|   LocalOrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel, | ||||
|                             Key poseKey, Key anchorPoseKey, Key landmarkKey) | ||||
|       : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} | ||||
| 
 | ||||
|   LocalOrientedPlane3Factor(const OrientedPlane3& z, | ||||
|                             const SharedGaussian& noiseModel, | ||||
|                             Key poseKey, Key anchorPoseKey, Key landmarkKey) | ||||
|     : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} | ||||
| 
 | ||||
|   /// print
 | ||||
|   void print(const std::string& s = "LocalOrientedPlane3Factor", | ||||
|       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; | ||||
| 
 | ||||
|   /***
 | ||||
|     * Vector of errors | ||||
|     * @brief Error = measured_plane_.error(a_plane.transform(inv(wTwa) * wTwi)) | ||||
|     *  | ||||
|     * This is the error of the measured and predicted plane in the current | ||||
|     * sensor frame, i. The plane is represented in the anchor pose, a. | ||||
|     * | ||||
|     * @param wTwi The pose of the sensor in world coordinates | ||||
|     * @param wTwa The pose of the anchor frame in world coordinates | ||||
|     * @param a_plane The estimated plane in anchor frame. | ||||
|     * | ||||
|     * Note: The optimized plane is represented in anchor frame, a, not the | ||||
|     * world frame. | ||||
|     */ | ||||
|   Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa, | ||||
|       const OrientedPlane3& a_plane, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none) const override; | ||||
| }; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
| 
 | ||||
|  | @ -0,0 +1,243 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /*
 | ||||
|  * @file testLocalOrientedPlane3Factor.cpp | ||||
|  * @date Feb 12, 2021 | ||||
|  * @author David Wisth | ||||
|  * @brief Tests the LocalOrientedPlane3Factor class | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam_unstable/slam/LocalOrientedPlane3Factor.h> | ||||
| 
 | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/ISAM2.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <boost/bind.hpp> | ||||
| 
 | ||||
| using namespace gtsam; | ||||
| using namespace std; | ||||
| 
 | ||||
| GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) | ||||
| GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) | ||||
| 
 | ||||
| using symbol_shorthand::P;  //< Planes
 | ||||
| using symbol_shorthand::X;  //< Pose3
 | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
| TEST(LocalOrientedPlane3Factor, lm_translation_error) { | ||||
|   // Tests one pose, two measurements of the landmark that differ in range only.
 | ||||
|   // Normal along -x, 3m away
 | ||||
|   OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
| 
 | ||||
|   // Init pose and prior.  Pose Prior is needed since a single plane measurement
 | ||||
|   // does not fully constrain the pose
 | ||||
|   Pose3 init_pose = Pose3::identity(); | ||||
|   Pose3 anchor_pose = Pose3::identity(); | ||||
|   graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); | ||||
|   graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001)); | ||||
| 
 | ||||
|   // Add two landmark measurements, differing in range
 | ||||
|   Vector4 measurement0(-1.0, 0.0, 0.0, 3.0); | ||||
|   Vector4 measurement1(-1.0, 0.0, 0.0, 1.0); | ||||
|   LocalOrientedPlane3Factor factor0( | ||||
|       measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0)); | ||||
|   LocalOrientedPlane3Factor factor1( | ||||
|       measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0)); | ||||
|   graph.add(factor0); | ||||
|   graph.add(factor1); | ||||
| 
 | ||||
|   // Initial Estimate
 | ||||
|   Values values; | ||||
|   values.insert(X(0), init_pose); | ||||
|   values.insert(X(1), anchor_pose); | ||||
|   values.insert(P(0), test_lm0); | ||||
| 
 | ||||
|   // Optimize
 | ||||
|   ISAM2 isam2; | ||||
|   isam2.update(graph, values); | ||||
|   Values result_values = isam2.calculateEstimate(); | ||||
|   auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0)); | ||||
| 
 | ||||
|   // Given two noisy measurements of equal weight, expect result between the two
 | ||||
|   OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); | ||||
|   EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); | ||||
| } | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
| // TODO As described in PR #564 after correcting the derivatives in
 | ||||
| // OrientedPlane3Factor this test fails. It should be debugged and re-enabled.
 | ||||
| /*
 | ||||
| TEST (LocalOrientedPlane3Factor, lm_rotation_error) { | ||||
|   // Tests one pose, two measurements of the landmark that differ in angle only.
 | ||||
|   // Normal along -x, 3m away
 | ||||
|   OrientedPlane3 test_lm0(-1.0/sqrt(1.01), -0.1/sqrt(1.01), 0.0, 3.0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
| 
 | ||||
|   // Init pose and prior.  Pose Prior is needed since a single plane measurement
 | ||||
|   // does not fully constrain the pose
 | ||||
|   Pose3 init_pose = Pose3::identity(); | ||||
|   graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); | ||||
| 
 | ||||
|   // Add two landmark measurements, differing in angle
 | ||||
|   Vector4 measurement0(-1.0, 0.0, 0.0, 3.0); | ||||
|   Vector4 measurement1(0.0, -1.0, 0.0, 3.0); | ||||
|   LocalOrientedPlane3Factor factor0(measurement0, | ||||
|       noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); | ||||
|   LocalOrientedPlane3Factor factor1(measurement1, | ||||
|       noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); | ||||
|   graph.add(factor0); | ||||
|   graph.add(factor1); | ||||
| 
 | ||||
|   // Initial Estimate
 | ||||
|   Values values; | ||||
|   values.insert(X(0), init_pose); | ||||
|   values.insert(P(0), test_lm0); | ||||
| 
 | ||||
|   // Optimize
 | ||||
|   ISAM2 isam2; | ||||
|   isam2.update(graph, values); | ||||
|   Values result_values = isam2.calculateEstimate(); | ||||
|   isam2.getDelta().print(); | ||||
| 
 | ||||
|   auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0)); | ||||
| 
 | ||||
|   values.print(); | ||||
|   result_values.print(); | ||||
| 
 | ||||
|   // Given two noisy measurements of equal weight, expect result between the two
 | ||||
|   OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, | ||||
|       0.0, 3.0); | ||||
|   EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); | ||||
| } | ||||
| */ | ||||
| 
 | ||||
| // *************************************************************************
 | ||||
| TEST(LocalOrientedPlane3Factor, Derivatives) { | ||||
|   // Measurement
 | ||||
|   OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); | ||||
| 
 | ||||
|   // Linearisation point
 | ||||
|   OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7); | ||||
|   Pose3 poseLin(Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI), Point3(1, 2, -4)); | ||||
|   Pose3 anchorPoseLin(Rot3::RzRyRx(-0.1*M_PI, 0.2*M_PI, 1.0), Point3(-5, 0, 1)); | ||||
| 
 | ||||
|   // Factor
 | ||||
|   Key planeKey(1), poseKey(2), anchorPoseKey(3); | ||||
|   SharedGaussian noise = noiseModel::Isotropic::Sigma(3, 0.1); | ||||
|   LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey); | ||||
| 
 | ||||
|   // Calculate numerical derivatives
 | ||||
|   auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor, | ||||
|     _1, _2, _3, boost::none, boost::none, boost::none); | ||||
|   Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3, | ||||
|     OrientedPlane3>(f, poseLin, anchorPoseLin, pLin); | ||||
|   Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3, | ||||
|     OrientedPlane3>(f, poseLin, anchorPoseLin, pLin); | ||||
|   Matrix numericalH3 = numericalDerivative33<Vector3, Pose3, Pose3, | ||||
|     OrientedPlane3>(f, poseLin, anchorPoseLin, pLin); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative
 | ||||
|   Matrix actualH1, actualH2, actualH3; | ||||
|   factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2, | ||||
|       actualH3); | ||||
| 
 | ||||
|   // Verify we get the expected error
 | ||||
|   EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); | ||||
|   EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); | ||||
|   EXPECT(assert_equal(numericalH3, actualH3, 1e-8)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Simplified version of the test by Marco Camurri to debug issue #561
 | ||||
| //
 | ||||
| // This test provides an example of how LocalOrientedPlane3Factor works.
 | ||||
| // x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e.
 | ||||
| // a local linearisation point for the plane. The plane is representated and
 | ||||
| // optimized in x1 frame in the optimization. This greatly improves numerical
 | ||||
| // stability when the pose is far from the origin.
 | ||||
| //
 | ||||
| TEST(LocalOrientedPlane3Factor, Issue561Simplified) { | ||||
|   // Typedefs
 | ||||
|   using Plane = OrientedPlane3; | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
| 
 | ||||
|   // Setup prior factors
 | ||||
|   Pose3 x0(Rot3::identity(), Vector3(100, 30, 10));  // the "sensor pose"
 | ||||
|   Pose3 x1(Rot3::identity(), Vector3(90, 40,  5) );  // the "anchor pose"
 | ||||
| 
 | ||||
|   auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); | ||||
|   auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01); | ||||
|   graph.addPrior<Pose3>(X(0), x0, x0_noise); | ||||
|   graph.addPrior<Pose3>(X(1), x1, x1_noise); | ||||
| 
 | ||||
|   // Two horizontal planes with different heights, in the world frame.
 | ||||
|   const Plane p1(0, 0, 1, 1), p2(0, 0, 1, 2); | ||||
|   // Transform to x1, the "anchor frame" (i.e. local frame)
 | ||||
|   auto p1_in_x1 = p1.transform(x1); | ||||
|   auto p2_in_x1 = p2.transform(x1); | ||||
|   auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); | ||||
|   auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); | ||||
|   graph.addPrior<Plane>(P(1), p1_in_x1, p1_noise); | ||||
|   graph.addPrior<Plane>(P(2), p2_in_x1, p2_noise); | ||||
| 
 | ||||
|   // Add plane factors, with a local linearization point.
 | ||||
|   // transform p1 to pose x0 as a measurement
 | ||||
|   auto p1_measured_from_x0 = p1.transform(x0); | ||||
|   // transform p2 to pose x0 as a measurement
 | ||||
|   auto p2_measured_from_x0 = p2.transform(x0); | ||||
|   const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); | ||||
|   const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); | ||||
|   graph.emplace_shared<LocalOrientedPlane3Factor>( | ||||
|       p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), X(1), P(1)); | ||||
|   graph.emplace_shared<LocalOrientedPlane3Factor>( | ||||
|       p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), X(1), P(2)); | ||||
| 
 | ||||
|   // Initial values
 | ||||
|   // Just offset the initial pose by 1m. This is what we are trying to optimize.
 | ||||
|   Values initialEstimate; | ||||
|   Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0))); | ||||
|   initialEstimate.insert(P(1), p1_in_x1); | ||||
|   initialEstimate.insert(P(2), p2_in_x1); | ||||
|   initialEstimate.insert(X(0), x0_initial); | ||||
|   initialEstimate.insert(X(1), x1); | ||||
| 
 | ||||
|   // Optimize
 | ||||
|   try { | ||||
|     ISAM2 isam2; | ||||
|     isam2.update(graph, initialEstimate); | ||||
|     Values result = isam2.calculateEstimate(); | ||||
|     EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); | ||||
|     EXPECT(x0.equals(result.at<Pose3>(X(0)))); | ||||
|     EXPECT(p1_in_x1.equals(result.at<Plane>(P(1)))); | ||||
|     EXPECT(p2_in_x1.equals(result.at<Plane>(P(2)))); | ||||
|   } catch (const IndeterminantLinearSystemException &e) { | ||||
|     cerr << "CAPTURED THE EXCEPTION: " | ||||
|       << DefaultKeyFormatter(e.nearbyVariable()) << endl; | ||||
|     EXPECT(false);  // fail if this happens
 | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   srand(time(nullptr)); | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
		Loading…
	
		Reference in New Issue