Added lots of tests for BetweenFactor
							parent
							
								
									cd3854a1f6
								
							
						
					
					
						commit
						2ecad47b9e
					
				|  | @ -1,18 +1,19 @@ | |||
| /**
 | ||||
|  * @file    testBetweenFactor.cpp | ||||
|  * @file testBetweenFactor.cpp | ||||
|  * @brief | ||||
|  * @author Duy-Nguyen Ta | ||||
|  * @date    Aug 2, 2013 | ||||
|  * @author Duy-Nguyen Ta, Varun Agrawal | ||||
|  * @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,7 +49,6 @@ TEST(BetweenFactor, Rot3) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| /*
 | ||||
| // Constructor scalar
 | ||||
| TEST(BetweenFactor, ConstructorScalar) { | ||||
|   SharedNoiseModel model; | ||||
|  | @ -56,6 +56,7 @@ TEST(BetweenFactor, ConstructorScalar) { | |||
|   BetweenFactor<double> factor(1, 2, measured_value, model); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Constructor vector3
 | ||||
| TEST(BetweenFactor, ConstructorVector3) { | ||||
|   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); | ||||
|  | @ -63,13 +64,55 @@ TEST(BetweenFactor, ConstructorVector3) { | |||
|   BetweenFactor<Vector3> factor(1, 2, measured_value, 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; | ||||
|   BetweenFactor<Vector> factor(1, 2, measured_value, model); | ||||
| } | ||||
| */ | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(BetweenFactor, Point3Jacobians) { | ||||
|   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); | ||||
|   Point3 measured_value(1, 2, 3); | ||||
|   BetweenFactor<Point3> factor(1, 2, measured_value, 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_value = Rot3::Ry(M_PI_2); | ||||
|   BetweenFactor<Rot3> factor(1, 2, measured_value, 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_value(Rot3(), Point3(1, 2, 3)); | ||||
|   BetweenFactor<Pose3> factor(1, 2, measured_value, model); | ||||
| 
 | ||||
|   Values values; | ||||
|   values.insert(1, Pose3()); | ||||
|   values.insert(2, Pose3(Rot3(), Point3(1, 2, 3))); | ||||
|   Vector3 error = factor.evaluateError(Pose3(), Pose3(Rot3(), Point3(1, 2, 3))); | ||||
|   EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9)); | ||||
|   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue