InvDepthFactor3: Add unit test for Jacobians
parent
d0144441cb
commit
c435da87fc
|
@ -1,8 +1,18 @@
|
|||
/*
|
||||
* testInvDepthFactor.cpp
|
||||
/* ----------------------------------------------------------------------------
|
||||
* 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
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testInvDepthFactor3.cpp
|
||||
* @brief Unit tests inverse depth parametrization
|
||||
*
|
||||
* Created on: Apr 13, 2012
|
||||
* Author: cbeall3
|
||||
* @author cbeall3
|
||||
* @author Dominik Van Opdenbosch
|
||||
* @date Apr 13, 2012
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
@ -12,6 +22,7 @@
|
|||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <gtsam_unstable/slam/InvDepthFactor3.h>
|
||||
|
||||
|
@ -28,6 +39,11 @@ PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
|
|||
typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
|
||||
typedef NonlinearEquality<Pose3> PoseConstraint;
|
||||
|
||||
Matrix factorError(const Pose3& pose, const Vector5& point, double invDepth,
|
||||
const InverseDepthFactor& factor) {
|
||||
return factor.evaluateError(pose, point, invDepth);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, optimize) {
|
||||
|
||||
|
@ -92,6 +108,55 @@ TEST( InvDepthFactor, optimize) {
|
|||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, Jacobian3D ) {
|
||||
|
||||
// landmark 5 meters infront of camera (camera center at (0,0,1))
|
||||
Point3 landmark(5, 0, 1);
|
||||
|
||||
// get expected projection using pinhole camera
|
||||
Point2 expected_uv = level_camera.project(landmark);
|
||||
|
||||
// get expected landmark representation using backprojection
|
||||
double inv_depth;
|
||||
Vector5 inv_landmark;
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
|
||||
std::tie(inv_landmark, inv_depth) = inv_camera.backproject(expected_uv, 5);
|
||||
Vector5 expected_inv_landmark((Vector(5) << 0., 0., 1., 0., 0.).finished());
|
||||
|
||||
CHECK(assert_equal(expected_inv_landmark, inv_landmark, 1e-6));
|
||||
CHECK(assert_equal(inv_depth, 1./5, 1e-6));
|
||||
|
||||
Symbol poseKey('x',1);
|
||||
Symbol pointKey('l',1);
|
||||
Symbol invDepthKey('d',1);
|
||||
InverseDepthFactor factor(expected_uv, sigma, poseKey, pointKey, invDepthKey, K);
|
||||
|
||||
std::vector<Matrix> actualHs(3);
|
||||
factor.unwhitenedError({{poseKey, genericValue(level_pose)},
|
||||
{pointKey, genericValue(inv_landmark)},
|
||||
{invDepthKey,genericValue(inv_depth)}},
|
||||
actualHs);
|
||||
|
||||
const Matrix& H1Actual = actualHs.at(0);
|
||||
const Matrix& H2Actual = actualHs.at(1);
|
||||
const Matrix& H3Actual = actualHs.at(2);
|
||||
|
||||
// Use numerical derivatives to verify the Jacobians
|
||||
Matrix H1Expected, H2Expected, H3Expected;
|
||||
|
||||
std::function<Matrix(const Pose3 &, const Vector5 &, const double &)>
|
||||
func = std::bind(&factorError, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, factor);
|
||||
H1Expected = numericalDerivative31(func, level_pose, inv_landmark, inv_depth);
|
||||
H2Expected = numericalDerivative32(func, level_pose, inv_landmark, inv_depth);
|
||||
H3Expected = numericalDerivative33(func, level_pose, inv_landmark, inv_depth);
|
||||
|
||||
// Verify the Jacobians
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-6))
|
||||
CHECK(assert_equal(H2Expected, H2Actual, 1e-6))
|
||||
CHECK(assert_equal(H3Expected, H3Actual, 1e-6))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue