Fix/plane factor

gtsam/geometry/OrientedPlane3.cpp

@@ -19,6 +19,7 @@
 
 #include <gtsam/geometry/OrientedPlane3.h>
 #include <gtsam/geometry/Point2.h>
+
 #include <iostream>
 
 using namespace std;
@@ -28,7 +29,7 @@ namespace gtsam {
 /* ************************************************************************* */
 void OrientedPlane3::print(const string& s) const {
   Vector4 coeffs = planeCoefficients();
-  cout << s << " : " << coeffs << endl;
+  cout << s << " : " << coeffs.transpose() << endl;
 }
 
 /* ************************************************************************* */
@@ -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;
   }
 

gtsam/geometry/OrientedPlane3.h

@@ -24,6 +24,9 @@
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Pose3.h>
 
+#include <algorithm>
+#include <string>
+
 namespace gtsam {
 
 /**
@@ -58,9 +61,8 @@ class GTSAM_EXPORT OrientedPlane3 {
   }
 
   /// 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) {

gtsam/slam/OrientedPlane3Factor.cpp

@@ -14,15 +14,15 @@ namespace gtsam {
 //***************************************************************************
 void OrientedPlane3Factor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
-  cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n";
+  cout << "OrientedPlane3Factor Factor on " << keyFormatter(landmarkKey_) << "\n";
   measured_p_.print("Measured Plane");
   this->noiseModel_->print("  noise model: ");
 }
 
 //***************************************************************************
 void OrientedPlane3DirectionPrior::print(const string& s,
     const KeyFormatter& keyFormatter) const {
-  cout << "Prior Factor on " << landmarkKey_ << "\n";
+  cout << "Prior Factor on " << keyFormatter(landmarkKey_) << "\n";
   measured_p_.print("Measured Plane");
   this->noiseModel_->print("  noise model: ");
 }

gtsam/slam/OrientedPlane3Factor.h

@@ -45,16 +45,13 @@ class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
       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 {
+    auto predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2);
+    return predicted_plane.error(measured_p_);
   }
-  ;
 };
 
 // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior

gtsam/slam/tests/testOrientedPlane3Factor.cpp

@@ -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;
@@ -170,6 +174,75 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
   EXPECT(assert_equal(expectedH3, actualH3, 1e-8));
 }
 
+/* ************************************************************************* */
+// Test by Marco Camurri to debug issue #561
+TEST(OrientedPlane3Factor, Issue561) {
+  // Typedefs
+  using symbol_shorthand::P;  //< Planes
+  using symbol_shorthand::X;  //< Pose3 (x,y,z,r,p,y)
+  using Plane = OrientedPlane3;
+
+  NonlinearFactorGraph graph;
+
+  // Setup prior factors
+  Pose3 x0_prior(
+      Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195,
+           0.117236322, -0.234214312, 0.0187322547, 0.972004505),
+      Vector3{-91.7500013, -0.47569366, -2.2});
+  auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
+  graph.addPrior<Pose3>(X(0), x0_prior, x0_noise);
+
+//   Plane p1_prior(0.211098835, 0.214292752, 0.95368543, 26.4269514);
+//   auto p1_noise =
+//       noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});
+//   graph.addPrior<Plane>(P(1), p1_prior, p1_noise);
+
+// ADDING THIS PRIOR MAKES OPTIMIZATION FAIL
+//   Plane p2_prior(0.301901811, 0.151751467, 0.941183717, 33.4388229);
+//   auto p2_noise =
+//       noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});
+//   graph.addPrior<Plane>(P(2), p2_prior, p2_noise);
+
+  // First plane factor
+  Plane p1_meas = Plane(0.0638967294, 0.0755284627, 0.995094297, 2.55956073);
+  const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.0451801);
+  graph.emplace_shared<OrientedPlane3Factor>(p1_meas.planeCoefficients(),
+                                             x0_p1_noise, X(0), P(1));
+
+  // Second plane factor
+  Plane p2_meas = Plane(0.104902077, -0.0275756528, 0.994100165, 1.32765088);
+  const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.0322889);
+  graph.emplace_shared<OrientedPlane3Factor>(p2_meas.planeCoefficients(),
+                                             x0_p2_noise, X(0), P(2));
+
+  // Optimize
+  try {
+    // Initial values
+    Values initialEstimate;
+    Plane p1(0.211098835, 0.214292752, 0.95368543, 26.4269514);
+    Plane p2(0.301901811, 0.151751467, 0.941183717, 33.4388229);
+    Pose3 x0(
+        Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195,
+             0.117236322, -0.234214312, 0.0187322547, 0.972004505),
+        Vector3{-91.7500013, -0.47569366, -2.2});
+    initialEstimate.insert(P(1), p1);
+    initialEstimate.insert(P(2), p2);
+    initialEstimate.insert(X(0), x0);
+
+    GaussNewtonParams params;
+    GTSAM_PRINT(graph);
+    Ordering ordering = list_of(P(1))(P(2))(X(0));  // make sure P1 eliminated first
+    params.setOrdering(ordering);
+    params.setLinearSolverType("SEQUENTIAL_QR");  // abundance of caution
+    params.setVerbosity("TERMINATION");  // show info about stopping conditions
+    GaussNewtonOptimizer optimizer(graph, initialEstimate);
+    Values result = optimizer.optimize();
+    EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
+  } catch (IndeterminantLinearSystemException e) {
+    std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl;
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(nullptr));