Merge pull request #813 from borglab/feature/between-factor-tests

gtsam/slam/tests/testBetweenFactor.cpp

@@ -1,18 +1,19 @@
 /**
- * @file    testBetweenFactor.cpp
+ * @file testBetweenFactor.cpp
  * @brief
- * @author Duy-Nguyen Ta
+ * @author Duy-Nguyen Ta, Varun Agrawal
- * @date    Aug 2, 2013
+ * @date Aug 2, 2013
  */
 
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/slam/BetweenFactor.h>
 
-#include <boost/bind/bind.hpp>
-#include <CppUnitLite/TestHarness.h>
-
 using namespace boost::placeholders;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
@@ -48,28 +49,71 @@ TEST(BetweenFactor, Rot3) {
 }
 
 /* ************************************************************************* */
-/*
 // Constructor scalar
 TEST(BetweenFactor, ConstructorScalar) {
   SharedNoiseModel model;
-  double measured_value = 0.0;
+  double measured = 0.0;
-  BetweenFactor<double> factor(1, 2, measured_value, model);
+  BetweenFactor<double> factor(1, 2, measured, model);
 }
 
+/* ************************************************************************* */
 // Constructor vector3
 TEST(BetweenFactor, ConstructorVector3) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
-  Vector3 measured_value(1, 2, 3);
+  Vector3 measured(1, 2, 3);
-  BetweenFactor<Vector3> factor(1, 2, measured_value, model);
+  BetweenFactor<Vector3> factor(1, 2, measured, model);
 }
 
+/* ************************************************************************* */
 // Constructor dynamic sized vector
 TEST(BetweenFactor, ConstructorDynamicSizeVector) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
-  Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
+  Vector measured(5); measured << 1, 2, 3, 4, 5;
-  BetweenFactor<Vector> factor(1, 2, measured_value, model);
+  BetweenFactor<Vector> factor(1, 2, measured, model);
+}
+
+/* ************************************************************************* */
+TEST(BetweenFactor, Point3Jacobians) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  Point3 measured(1, 2, 3);
+  BetweenFactor<Point3> factor(1, 2, measured, model);
+  
+  Values values;
+  values.insert(1, Point3(0, 0, 0));
+  values.insert(2, Point3(1, 2, 3));
+  Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3));
+  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(BetweenFactor, Rot3Jacobians) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  Rot3 measured = Rot3::Ry(M_PI_2);
+  BetweenFactor<Rot3> factor(1, 2, measured, model);
+  
+  Values values;
+  values.insert(1, Rot3());
+  values.insert(2, Rot3::Ry(M_PI_2));
+  Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2));
+  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(BetweenFactor, Pose3Jacobians) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0);
+  Pose3 measured(Rot3(), Point3(1, 2, 3));
+  BetweenFactor<Pose3> factor(1, 2, measured, model);
+
+  Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3));
+  Values values;
+  values.insert(1, pose1);
+  values.insert(2, pose2);
+  Vector6 error = factor.evaluateError(pose1, pose2);
+  EXPECT(assert_equal<Vector6>(Vector6::Zero(), error, 1e-9));
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
-*/
 
 /* ************************************************************************* */
 int main() {