116 lines
3.6 KiB
C++
116 lines
3.6 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------1------------------------------------------- */
|
|
|
|
/**
|
|
* @file testCustomChartExpression.cpp
|
|
* @date September 18, 2014
|
|
* @author Frank Dellaert
|
|
* @author Paul Furgale
|
|
* @brief unit tests for Block Automatic Differentiation
|
|
*/
|
|
|
|
#include <gtsam_unstable/nonlinear/expressionTesting.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <type_traits>
|
|
|
|
|
|
using namespace gtsam;
|
|
|
|
// A simple prototype custom chart that does two things:
|
|
// 1. it reduces the dimension of the variable being estimated
|
|
// 2. it scales the input to retract.
|
|
//
|
|
// The Jacobian of this chart is:
|
|
// [ 2 0 ]
|
|
// [ 0 3 ]
|
|
// [ 0 0 ]
|
|
struct ProjectionChart {
|
|
typedef Eigen::Vector3d type;
|
|
typedef Eigen::Vector2d vector;
|
|
static vector local(const type& origin, const type& other) {
|
|
return (other - origin).head<2>();
|
|
}
|
|
static type retract(const type& origin, const vector& d) {
|
|
return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0);
|
|
}
|
|
static int getDimension(const type& origin) {
|
|
return 2;
|
|
}
|
|
};
|
|
|
|
namespace gtsam {
|
|
namespace traits {
|
|
template<> struct is_chart<ProjectionChart> : public std::true_type {};
|
|
template<> struct dimension<ProjectionChart> : public std::integral_constant<int, 2> {};
|
|
} // namespace traits
|
|
} // namespace gtsam
|
|
|
|
TEST(ExpressionCustomChart, projection) {
|
|
// Create expression
|
|
Expression<Eigen::Vector3d> point(1);
|
|
|
|
Eigen::Vector3d pval(1.0, 2.0, 3.0);
|
|
Values standard;
|
|
standard.insert(1, pval);
|
|
|
|
Values custom;
|
|
custom.insert(1, pval, ProjectionChart());
|
|
|
|
Eigen::Vector3d pstandard = point.value(standard);
|
|
Eigen::Vector3d pcustom = point.value(custom);
|
|
|
|
// The values should be the same.
|
|
EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10));
|
|
|
|
|
|
EXPECT_LONGS_EQUAL(3, standard.at(1).dim());
|
|
VectorValues vstandard = standard.zeroVectors();
|
|
EXPECT_LONGS_EQUAL(3, vstandard.at(1).size());
|
|
|
|
|
|
EXPECT_LONGS_EQUAL(2, custom.at(1).dim());
|
|
VectorValues vcustom = custom.zeroVectors();
|
|
EXPECT_LONGS_EQUAL(2, vcustom.at(1).size());
|
|
|
|
ExpressionFactor<Eigen::Vector3d> f(noiseModel::Unit::Create(pval.size()), pval, point);
|
|
|
|
std::shared_ptr<GaussianFactor> gfstandard = f.linearize(standard);
|
|
std::shared_ptr<JacobianFactor> jfstandard = //
|
|
std::dynamic_pointer_cast<JacobianFactor>(gfstandard);
|
|
|
|
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
|
|
Jacobian Jstandard = jfstandard->jacobianUnweighted();
|
|
EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10));
|
|
|
|
std::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom);
|
|
std::shared_ptr<JacobianFactor> jfcustom = //
|
|
std::dynamic_pointer_cast<JacobianFactor>(gfcustom);
|
|
|
|
Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
|
|
expectedJacobian(0,0) = 2.0;
|
|
expectedJacobian(1,1) = 3.0;
|
|
Jacobian Jcustom = jfcustom->jacobianUnweighted();
|
|
EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10));
|
|
|
|
// Amazingly, the finite differences get the expected Jacobian right.
|
|
const double fd_step = 1e-5;
|
|
const double tolerance = 1e-5;
|
|
EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|
|
|