Added and tested Cage Factor. Added Matlab Wrapper

gtsam.h

@@ -2149,6 +2149,10 @@ virtual class DroneDynamicsVelXYFactor : gtsam::NoiseModelFactor {
   DroneDynamicsVelXYFactor(size_t key1, size_t key2, size_t key3, Vector motors, Vector acc, const gtsam::noiseModel::Base* noiseModel);
 };
 
+#include <gtsam/slam/CageFactor.h>
+virtual class CageFactor : gtsam::NoiseModelFactor {
+  CageFactor(size_t key1, const gtsam::Pose3& pose, double cageBoundary, const gtsam::noiseModel::Base* noiseModel);
+};
 
 #include <gtsam/slam/BearingFactor.h>
 template<POSE, POINT, ROTATION>
@@ -2461,22 +2465,22 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
 
 
-  static void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+  static void predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
       const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis);
 
-  static gtsam::imuBias::ConstantBias PredictImuBias(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
+  static gtsam::imuBias::ConstantBias predictImuBias(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
       const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis);
 
-  static gtsam::LieVector PredictVelocity(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
+  static gtsam::LieVector predictVelocity(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
       const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis);
 
-  static gtsam::Pose3 PredictPose(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
+  static gtsam::Pose3 predictPose(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
       const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis);

gtsam/slam/CageFactor.h

@@ -0,0 +1,98 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 CageFactor.h
+ * @author Krunal Chande
+ * @date November 10, 2014
+ */
+
+#pragma once
+
+#include <boost/lexical_cast.hpp>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Factor to constrain position based on size of the accessible area
+ */
+
+class CageFactor: public NoiseModelFactor1<Pose3> {
+private:
+  Pose3 pose_;
+  double cageBoundary_;
+  typedef CageFactor This;
+  typedef NoiseModelFactor1<Pose3> Base;
+
+public:
+  CageFactor() {} /* Default Constructor*/
+  CageFactor(Key poseKey, const Pose3& pose, double cageBoundary, const SharedNoiseModel& model):
+    Base(model, poseKey), pose_(pose), cageBoundary_(cageBoundary){}
+  virtual ~CageFactor(){}
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** h(x) - z */
+  Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const {
+
+        double distance = pose.translation().dist(Point3(0,0,0));
+        if(distance > cageBoundary_){
+          double distance = pose.range(Point3(0,0,0), H);
+          return (gtsam::Vector(1) << distance - cageBoundary_);
+        } else {
+          if(H) *H = gtsam::zeros(1, Pose3::Dim());
+          return (gtsam::Vector(1) << 0.0);
+        }
+//    Point3 p2;
+//    double x = pose.x(), y = pose.y(), z = pose.z();
+//    if (x < 0) x = -x;
+//    if (y < 0) y = -y;
+//    if (z < 0) z = -z;
+//    double errorX = 100/(x-cageBoundary_), errorY = 100/(y-cageBoundary_), errorZ = 100/(z-cageBoundary_);
+//    if (H) *H = pose.translation().distance(p2, H);
+//    return (Vector(3)<<errorX, errorY, errorZ);
+  }
+
+  /** equals specialized to this factor */
+  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    const This *e = dynamic_cast<const This*> (&expected);
+    return e != NULL
+        && Base::equals(*e, tol)
+    ;
+  }
+
+  /** print contents */
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    std::cout << s << "Cage Factor, Cage Boundary = " << cageBoundary_ << " Pose: " << pose_ << std::endl;
+    Base::print("", keyFormatter);
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+        boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(cageBoundary_);
+    ar & BOOST_SERIALIZATION_NVP(pose_);
+  }
+
+}; // end CageFactor
+} // end namespace
+
+

gtsam/slam/tests/testCageFactor.cpp

@@ -0,0 +1,78 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testCageFactor.cpp
+ * @brief  Unit tests CageFactor class
+ * @author Krunal Chande
+ */
+
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/slam/CageFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <boost/bind.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+// Create a noise model
+static SharedNoiseModel model(noiseModel::Unit::Create(6));
+
+LieVector factorError(const Pose3& pose, const CageFactor& factor) {
+  return factor.evaluateError(pose);
+}
+
+
+/* ************************************************************************* */
+TEST(CageFactor, Inside) {
+  Key poseKey(1);
+  Pose3 pose(Rot3::ypr(0,0,0),Point3(0,0,0));
+  double cageBoundary = 10; // in m
+  CageFactor factor(poseKey, pose, cageBoundary, model);
+
+  // Set the linearization point
+  Pose3 poseLin;
+  Matrix H;
+  Vector actualError(factor.evaluateError(poseLin, H));
+  Vector expectedError = zero(1);
+  CHECK(assert_equal(expectedError, actualError, 1e-9));
+
+  // use numerical derivatives to calculate the jacobians
+  Matrix HExpected;
+  HExpected = numericalDerivative11<Pose3>(boost::bind(&factorError, _1, factor), pose);
+  CHECK(assert_equal(HExpected, H, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(CageFactor, Outside) {
+  Key poseKey(1);
+  Point3 translation = Point3(15,0,0);
+  Pose3 pose(Rot3::ypr(0,0,0),translation);
+  double cageBoundary = 10; // in m
+  CageFactor factor(poseKey, pose, cageBoundary, model);
+
+  // Set the linearization point
+  Pose3 poseLin;
+  Matrix H;
+  Vector actualError(factor.evaluateError(pose, H));
+  Vector expectedError(Vector(1)<<5);
+  CHECK(assert_equal(expectedError, actualError, 1e-9));
+
+  // use numerical derivatives to calculate the jacobians
+  Matrix HExpected;
+  HExpected = numericalDerivative11<Pose3>(boost::bind(&factorError, _1, factor), pose);
+  CHECK(assert_equal(HExpected, H, 1e-9));
+}
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */