168 lines
6.9 KiB
C++
168 lines
6.9 KiB
C++
/**
|
|
* @file testRelativeElevationFactor.cpp
|
|
*
|
|
* @date Aug 17, 2012
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <gtsam_unstable/slam/RelativeElevationFactor.h>
|
|
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
|
|
using namespace std::placeholders;
|
|
using namespace gtsam;
|
|
|
|
SharedNoiseModel model1 = noiseModel::Unit::Create(1);
|
|
|
|
const double tol = 1e-5;
|
|
|
|
const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
|
|
const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
|
|
const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0));
|
|
const Point3 point1(3.0, 4.0, 2.0);
|
|
const gtsam::Key poseKey = 1, pointKey = 2;
|
|
|
|
/* ************************************************************************* */
|
|
Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
|
|
return factor.evaluateError(x, p);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testRelativeElevationFactor, level_zero_error ) {
|
|
// Zero error
|
|
double measured = 2.0;
|
|
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
|
Matrix actH1, actH2;
|
|
EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
|
|
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose1, point1, 1e-5);
|
|
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose1, point1, 1e-5);
|
|
EXPECT(assert_equal(expH1, actH1, tol));
|
|
EXPECT(assert_equal(expH2, actH2, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testRelativeElevationFactor, level_positive ) {
|
|
// Positive meas
|
|
double measured = 0.0;
|
|
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
|
Matrix actH1, actH2;
|
|
EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
|
|
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose1, point1, 1e-5);
|
|
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose1, point1, 1e-5);
|
|
EXPECT(assert_equal(expH1, actH1, tol));
|
|
EXPECT(assert_equal(expH2, actH2, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testRelativeElevationFactor, level_negative ) {
|
|
// Negative meas
|
|
double measured = -1.0;
|
|
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
|
Matrix actH1, actH2;
|
|
EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
|
|
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose1, point1, 1e-5);
|
|
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose1, point1, 1e-5);
|
|
EXPECT(assert_equal(expH1, actH1, tol));
|
|
EXPECT(assert_equal(expH2, actH2, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testRelativeElevationFactor, rotated_zero_error ) {
|
|
// Zero error
|
|
double measured = 2.0;
|
|
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
|
Matrix actH1, actH2;
|
|
EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
|
|
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose2, point1, 1e-5);
|
|
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose2, point1, 1e-5);
|
|
EXPECT(assert_equal(expH1, actH1, tol));
|
|
EXPECT(assert_equal(expH2, actH2, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testRelativeElevationFactor, rotated_positive ) {
|
|
// Positive meas
|
|
double measured = 0.0;
|
|
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
|
Matrix actH1, actH2;
|
|
EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
|
|
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose2, point1, 1e-5);
|
|
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose2, point1, 1e-5);
|
|
EXPECT(assert_equal(expH1, actH1, tol));
|
|
EXPECT(assert_equal(expH2, actH2, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testRelativeElevationFactor, rotated_negative1 ) {
|
|
// Negative meas
|
|
double measured = -1.0;
|
|
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
|
Matrix actH1, actH2;
|
|
EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
|
|
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose2, point1, 1e-5);
|
|
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose2, point1, 1e-5);
|
|
EXPECT(assert_equal(expH1, actH1, tol));
|
|
EXPECT(assert_equal(expH2, actH2, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testRelativeElevationFactor, rotated_negative2 ) {
|
|
// Negative meas
|
|
double measured = -1.0;
|
|
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
|
Matrix actH1, actH2;
|
|
EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2)));
|
|
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose3, point1, 1e-5);
|
|
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
|
|
std::bind(evalFactorError, factor, std::placeholders::_1,
|
|
std::placeholders::_2),
|
|
pose3, point1, 1e-5);
|
|
EXPECT(assert_equal(expH1, actH1, tol));
|
|
EXPECT(assert_equal(expH2, actH2, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|