Projection factor which also estimates transform (calibration is fixed)
cbeall3
parent
67e0e71802
commit
e22aa34f1d
21
gtsam.h
21
gtsam.h
|
|
@ -2181,6 +2181,27 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_
|
||||||
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
|
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
|
||||||
|
|
||||||
|
|
||||||
|
#include <gtsam/slam/TransformProjectionFactor.h>
|
||||||
|
template<POSE, LANDMARK, CALIBRATION>
|
||||||
|
virtual class TransformProjectionFactor : gtsam::NoiseModelFactor {
|
||||||
|
TransformProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
||||||
|
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k);
|
||||||
|
|
||||||
|
TransformProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
||||||
|
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
|
||||||
|
|
||||||
|
gtsam::Point2 measured() const;
|
||||||
|
CALIBRATION* calibration() const;
|
||||||
|
bool verboseCheirality() const;
|
||||||
|
bool throwCheirality() const;
|
||||||
|
|
||||||
|
// enabling serialization functionality
|
||||||
|
void serialize() const;
|
||||||
|
};
|
||||||
|
typedef gtsam::TransformProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> TransformProjectionFactorCal3_S2;
|
||||||
|
typedef gtsam::TransformProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> TransformProjectionFactorCal3DS2;
|
||||||
|
|
||||||
|
|
||||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||||
template<CAMERA, LANDMARK>
|
template<CAMERA, LANDMARK>
|
||||||
virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,181 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 TransformProjectionFactor.h
|
||||||
|
* @brief Basic bearing factor from 2D measurement
|
||||||
|
* @author Chris Beall
|
||||||
|
* @author Richard Roberts
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Alex Cunningham
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||||
|
#include <gtsam/geometry/SimpleCamera.h>
|
||||||
|
#include <boost/optional.hpp>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
|
||||||
|
* i.e. the main building block for visual SLAM.
|
||||||
|
* @addtogroup SLAM
|
||||||
|
*/
|
||||||
|
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
|
||||||
|
class TransformProjectionFactor: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
|
||||||
|
protected:
|
||||||
|
|
||||||
|
// Keep a copy of measurement and calibration for I/O
|
||||||
|
Point2 measured_; ///< 2D measurement
|
||||||
|
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
|
||||||
|
|
||||||
|
// verbosity handling for Cheirality Exceptions
|
||||||
|
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
|
||||||
|
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// shorthand for base class type
|
||||||
|
typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
|
||||||
|
|
||||||
|
/// shorthand for this class
|
||||||
|
typedef TransformProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
|
||||||
|
|
||||||
|
/// shorthand for a smart pointer to a factor
|
||||||
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
|
|
||||||
|
/// Default constructor
|
||||||
|
TransformProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
* TODO: Mark argument order standard (keys, measurement, parameters)
|
||||||
|
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||||
|
* @param model is the standard deviation
|
||||||
|
* @param poseKey is the index of the camera
|
||||||
|
* @param pointKey is the index of the landmark
|
||||||
|
* @param K shared pointer to the constant calibration
|
||||||
|
*/
|
||||||
|
TransformProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
|
||||||
|
Key poseKey, Key transformKey, Key pointKey,
|
||||||
|
const boost::shared_ptr<CALIBRATION>& K) :
|
||||||
|
Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
|
||||||
|
throwCheirality_(false), verboseCheirality_(false) {}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructor with exception-handling flags
|
||||||
|
* TODO: Mark argument order standard (keys, measurement, parameters)
|
||||||
|
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||||
|
* @param model is the standard deviation
|
||||||
|
* @param poseKey is the index of the camera
|
||||||
|
* @param pointKey is the index of the landmark
|
||||||
|
* @param K shared pointer to the constant calibration
|
||||||
|
* @param throwCheirality determines whether Cheirality exceptions are rethrown
|
||||||
|
* @param verboseCheirality determines whether exceptions are printed for Cheirality
|
||||||
|
*/
|
||||||
|
TransformProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
|
||||||
|
Key poseKey, Key transformKey, Key pointKey,
|
||||||
|
const boost::shared_ptr<CALIBRATION>& K,
|
||||||
|
bool throwCheirality, bool verboseCheirality) :
|
||||||
|
Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
|
||||||
|
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
|
||||||
|
|
||||||
|
/** Virtual destructor */
|
||||||
|
virtual ~TransformProjectionFactor() {}
|
||||||
|
|
||||||
|
/// @return a deep copy of this factor
|
||||||
|
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||||
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||||
|
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* print
|
||||||
|
* @param s optional string naming the factor
|
||||||
|
* @param keyFormatter optional formatter useful for printing Symbols
|
||||||
|
*/
|
||||||
|
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||||
|
std::cout << s << "TransformProjectionFactor, z = ";
|
||||||
|
measured_.print();
|
||||||
|
Base::print("", keyFormatter);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// equals
|
||||||
|
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
|
||||||
|
const This *e = dynamic_cast<const This*>(&p);
|
||||||
|
return e
|
||||||
|
&& Base::equals(p, tol)
|
||||||
|
&& this->measured_.equals(e->measured_, tol)
|
||||||
|
&& this->K_->equals(*e->K_, tol);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Evaluate error h(x)-z and optionally derivatives
|
||||||
|
Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point,
|
||||||
|
boost::optional<Matrix&> H1 = boost::none,
|
||||||
|
boost::optional<Matrix&> H2 = boost::none,
|
||||||
|
boost::optional<Matrix&> H3 = boost::none) const {
|
||||||
|
try {
|
||||||
|
if(H1 || H2 || H3) {
|
||||||
|
gtsam::Matrix H0, H02;
|
||||||
|
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
|
||||||
|
Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
|
||||||
|
*H2 = *H1 * H02;
|
||||||
|
*H1 = *H1 * H0;
|
||||||
|
return reprojectionError.vector();
|
||||||
|
} else {
|
||||||
|
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
|
||||||
|
Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
|
||||||
|
return reprojectionError.vector();
|
||||||
|
}
|
||||||
|
} catch( CheiralityException& e) {
|
||||||
|
if (H1) *H1 = zeros(2,6);
|
||||||
|
if (H2) *H2 = zeros(2,6);
|
||||||
|
if (H3) *H3 = zeros(2,3);
|
||||||
|
if (verboseCheirality_)
|
||||||
|
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
|
||||||
|
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||||
|
if (throwCheirality_)
|
||||||
|
throw e;
|
||||||
|
}
|
||||||
|
return ones(2) * 2.0 * K_->fx();
|
||||||
|
}
|
||||||
|
|
||||||
|
/** return the measurement */
|
||||||
|
const Point2& measured() const {
|
||||||
|
return measured_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** return the calibration object */
|
||||||
|
inline const boost::shared_ptr<CALIBRATION> calibration() const {
|
||||||
|
return K_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** return verbosity */
|
||||||
|
inline bool verboseCheirality() const { return verboseCheirality_; }
|
||||||
|
|
||||||
|
/** return flag for throwing cheirality exceptions */
|
||||||
|
inline bool throwCheirality() const { return throwCheirality_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/// Serialization function
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
} // \ namespace gtsam
|
||||||
|
|
@ -0,0 +1,221 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 testProjectionFactor.cpp
|
||||||
|
* @brief Unit tests for ProjectionFactor Class
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @date Nov 2009
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
|
#include <gtsam/slam/TransformProjectionFactor.h>
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
#include <gtsam/geometry/Cal3DS2.h>
|
||||||
|
#include <gtsam/geometry/Cal3_S2.h>
|
||||||
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
#include <gtsam/geometry/Point3.h>
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <boost/bind.hpp>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
// make a realistic calibration matrix
|
||||||
|
static double fov = 60; // degrees
|
||||||
|
static size_t w=640,h=480;
|
||||||
|
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
|
||||||
|
|
||||||
|
// Create a noise model for the pixel error
|
||||||
|
static SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||||
|
|
||||||
|
// Convenience for named keys
|
||||||
|
using symbol_shorthand::X;
|
||||||
|
using symbol_shorthand::L;
|
||||||
|
using symbol_shorthand::T;
|
||||||
|
|
||||||
|
typedef TransformProjectionFactor<Pose3, Point3> TestProjectionFactor;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, nonStandard ) {
|
||||||
|
TransformProjectionFactor<Pose3, Point3, Cal3DS2> f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, Constructor) {
|
||||||
|
Key poseKey(X(1));
|
||||||
|
Key transformKey(T(1));
|
||||||
|
Key pointKey(L(1));
|
||||||
|
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
|
||||||
|
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, ConstructorWithTransform) {
|
||||||
|
Key poseKey(X(1));
|
||||||
|
Key transformKey(T(1));
|
||||||
|
Key pointKey(L(1));
|
||||||
|
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, Equals ) {
|
||||||
|
// Create two identical factors and make sure they're equal
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
|
||||||
|
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
|
||||||
|
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
|
||||||
|
|
||||||
|
CHECK(assert_equal(factor1, factor2));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, EqualsWithTransform ) {
|
||||||
|
// Create two identical factors and make sure they're equal
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||||
|
|
||||||
|
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
|
||||||
|
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
|
||||||
|
|
||||||
|
CHECK(assert_equal(factor1, factor2));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, Error ) {
|
||||||
|
// Create the factor with a measurement that is 3 pixels off in x
|
||||||
|
Key poseKey(X(1));
|
||||||
|
Key transformKey(T(1));
|
||||||
|
Key pointKey(L(1));
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||||
|
|
||||||
|
// Set the linearization point
|
||||||
|
Pose3 pose(Rot3(), Point3(0,0,-6));
|
||||||
|
Point3 point(0.0, 0.0, 0.0);
|
||||||
|
|
||||||
|
// Use the factor to calculate the error
|
||||||
|
Vector actualError(factor.evaluateError(pose, Pose3(), point));
|
||||||
|
|
||||||
|
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
|
||||||
|
Vector expectedError = (Vector(2) << -3.0, 0.0);
|
||||||
|
|
||||||
|
// Verify we get the expected error
|
||||||
|
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, ErrorWithTransform ) {
|
||||||
|
// Create the factor with a measurement that is 3 pixels off in x
|
||||||
|
Key poseKey(X(1));
|
||||||
|
Key transformKey(T(1));
|
||||||
|
Key pointKey(L(1));
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||||
|
TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K);
|
||||||
|
|
||||||
|
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
|
||||||
|
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
|
||||||
|
Point3 point(0.0, 0.0, 0.0);
|
||||||
|
|
||||||
|
// Use the factor to calculate the error
|
||||||
|
Vector actualError(factor.evaluateError(pose, transform, point));
|
||||||
|
|
||||||
|
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
|
||||||
|
Vector expectedError = (Vector(2) << -3.0, 0.0);
|
||||||
|
|
||||||
|
// Verify we get the expected error
|
||||||
|
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, Jacobian ) {
|
||||||
|
// Create the factor with a measurement that is 3 pixels off in x
|
||||||
|
Key poseKey(X(1));
|
||||||
|
Key transformKey(T(1));
|
||||||
|
Key pointKey(L(1));
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||||
|
|
||||||
|
// Set the linearization point
|
||||||
|
Pose3 pose(Rot3(), Point3(0,0,-6));
|
||||||
|
Point3 point(0.0, 0.0, 0.0);
|
||||||
|
|
||||||
|
// Use the factor to calculate the Jacobians
|
||||||
|
Matrix H1Actual, H2Actual, H3Actual;
|
||||||
|
factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual);
|
||||||
|
|
||||||
|
// The expected Jacobians
|
||||||
|
Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
|
||||||
|
Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
|
||||||
|
|
||||||
|
// Verify the Jacobians are correct
|
||||||
|
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
|
||||||
|
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
|
||||||
|
|
||||||
|
// Verify H2 with numerical derivative
|
||||||
|
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
|
||||||
|
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
|
||||||
|
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
|
||||||
|
boost::none, boost::none, boost::none)), pose, Pose3(), point);
|
||||||
|
|
||||||
|
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( ProjectionFactor, JacobianWithTransform ) {
|
||||||
|
// Create the factor with a measurement that is 3 pixels off in x
|
||||||
|
Key poseKey(X(1));
|
||||||
|
Key transformKey(T(1));
|
||||||
|
Key pointKey(L(1));
|
||||||
|
Point2 measurement(323.0, 240.0);
|
||||||
|
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||||
|
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||||
|
|
||||||
|
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
|
||||||
|
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
|
||||||
|
Point3 point(0.0, 0.0, 0.0);
|
||||||
|
|
||||||
|
// Use the factor to calculate the Jacobians
|
||||||
|
Matrix H1Actual, H2Actual, H3Actual;
|
||||||
|
factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual);
|
||||||
|
|
||||||
|
// The expected Jacobians
|
||||||
|
Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
|
||||||
|
Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
|
||||||
|
|
||||||
|
// Verify the Jacobians are correct
|
||||||
|
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
|
||||||
|
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
|
||||||
|
|
||||||
|
// Verify H2 with numerical derivative
|
||||||
|
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
|
||||||
|
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
|
||||||
|
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
|
||||||
|
boost::none, boost::none, boost::none)), pose, body_P_sensor, point);
|
||||||
|
|
||||||
|
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
Loading…
Reference in New Issue