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