12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added PriorFactor and BetweenFactor with optional sensor pose 

transformations. Ideally these should simply be the PriorFactor and
BetweenFactor, but more investigation is needed.
release/4.3a0
Stephen Williams 2013-05-08 13:23:56 +00:00
parent bbf9bad6d7
commit 9e39df6e88
4 changed files with 630 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
gtsam_unstable/slam/PoseBetweenFactor.h Normal file
View File

@ -0,0 +1,130 @@
/* ----------------------------------------------------------------------------
* 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 BetweenFactor.h
* @author Frank Dellaert, Viorela Ila
**/
#pragma once
#include <ostream>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Testable.h>
namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam POSE the Pose type
* @addtogroup SLAM
*/
template<class POSE>
class PoseBetweenFactor: public NoiseModelFactor2<POSE, POSE> {
private:
typedef PoseBetweenFactor<POSE> This;
typedef NoiseModelFactor2<POSE, POSE> Base;
POSE measured_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE)
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PoseBetweenFactor> shared_ptr;
/** default constructor - only use for serialization */
PoseBetweenFactor() {}
/** Constructor */
PoseBetweenFactor(Key key1, Key key2, const POSE& measured,
const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) {
}
virtual ~PoseBetweenFactor() {}
/// @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))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measured_.print(" measured: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol)
&& this->measured_.equals(e->measured_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const POSE& p1, const POSE& p2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if(body_P_sensor_) {
POSE hx;
if(H1 || H2) {
Matrix H3;
hx = p1.compose(*body_P_sensor_,H3).between(p2.compose(*body_P_sensor_), H1, H2); // h(x)
if(H1) (*H1) *= H3;
if(H2) (*H2) *= H3;
} else {
hx = p1.compose(*body_P_sensor_).between(p2.compose(*body_P_sensor_)); // h(x)
}
// manifold equivalent of h(x)-z -> log(z,h(x))
return measured_.localCoordinates(hx);
} else {
POSE hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
return measured_.localCoordinates(hx);
}
}
/** return the measured */
const POSE& measured() const {
return measured_;
}
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_);
}
}; // \class PoseBetweenFactor
} /// namespace gtsam

111
gtsam_unstable/slam/PosePriorFactor.h Normal file
View File

@ -0,0 +1,111 @@
/* ----------------------------------------------------------------------------
* 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 PosePriorFactor.h
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Testable.h>
namespace gtsam {
/**
* A class for a soft prior on any Value type
* @addtogroup SLAM
*/
template<class POSE>
class PosePriorFactor: public NoiseModelFactor1<POSE> {
private:
typedef PosePriorFactor<POSE> This;
typedef NoiseModelFactor1<POSE> Base;
POSE prior_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE)
public:
/// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PosePriorFactor<POSE> > shared_ptr;
/** default constructor - only use for serialization */
PosePriorFactor() {}
virtual ~PosePriorFactor() {}
/** Constructor */
PosePriorFactor(Key key, const POSE& prior, const SharedNoiseModel& model,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) {
}
/// @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))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
prior_.print(" prior mean: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol)
&& this->prior_.equals(e->prior_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const POSE& p, boost::optional<Matrix&> H = boost::none) const {
if(body_P_sensor_) {
// manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p.compose(*body_P_sensor_, H));
} else {
if(H) (*H) = eye(p.dim());
// manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p);
}
}
const POSE& prior() const { return prior_; }
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(prior_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};
} /// namespace gtsam

204
gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp Normal file
View File

@ -0,0 +1,204 @@
/* ----------------------------------------------------------------------------
* 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_unstable/slam/PoseBetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
typedef PoseBetweenFactor<Pose3> TestPoseBetweenFactor;
/* ************************************************************************* */
TEST( PoseBetweenFactor, Constructor) {
Key poseKey1(1);
Key poseKey2(2);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
}
/* ************************************************************************* */
TEST( PoseBetweenFactor, ConstructorWithTransform) {
Key poseKey1(1);
Key poseKey2(2);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
}
/* ************************************************************************* */
TEST( PoseBetweenFactor, Equals ) {
// Create two identical factors and make sure they're equal
Key poseKey1(1);
Key poseKey2(2);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model);
TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model);
CHECK(assert_equal(factor1, factor2));
// Create a third, different factor and check for inequality
Pose3 measurement2(Rot3::RzRyRx(0.20, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement2, model);
CHECK(assert_inequal(factor1, factor3));
}
/* ************************************************************************* */
TEST( PoseBetweenFactor, EqualsWithTransform ) {
// Create two identical factors and make sure they're equal
Key poseKey1(1);
Key poseKey2(2);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model, body_P_sensor);
TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model, body_P_sensor);
CHECK(assert_equal(factor1, factor2));
// Create a third, different factor and check for inequality
Pose3 body_P_sensor2(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.30, -0.10, 1.0));
TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement, model, body_P_sensor2);
CHECK(assert_inequal(factor1, factor3));
}
/* ************************************************************************* */
TEST( PoseBetweenFactor, Error ) {
// Create the measurement and linearization point
Pose3 measurement(Rot3::RzRyRx(0.15, 0.15, -0.20), Point3(+0.5, -1.0, +1.0));
Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
Pose3 pose2(Rot3::RzRyRx(0.15, 0.00, 0.20), Point3(-3.5, 6.0, -9.0));
// The expected error
Vector expectedError(6);
expectedError << -0.029839512616488,
0.013145599455949,
0.096971291682578,
-0.139187549519821,
-0.142346243063553,
-0.039088532100977;
// Create a factor and calculate the error
Key poseKey1(1);
Key poseKey2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
Vector actualError(factor.evaluateError(pose1, pose2));
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( PoseBetweenFactor, ErrorWithTransform ) {
// Create the measurement and linearization point
Pose3 measurement(Rot3::RzRyRx(-0.15, 0.10, 0.15), Point3(+1.25, -0.90, +.45));
Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
Pose3 pose2(Rot3::RzRyRx(0.15, 0.00, 0.20), Point3(-3.5, 6.0, -9.0));
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
// The expected error
Vector expectedError(6);
expectedError << 0.017337193670445,
0.022222830355243,
-0.012504190982804,
0.026413288603739,
0.005237695303536,
-0.000071612703633;
// Create a factor and calculate the error
Key poseKey1(1);
Key poseKey2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
Vector actualError(factor.evaluateError(pose1, pose2));
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( PoseBetweenFactor, Jacobian ) {
// Create a factor
Key poseKey1(1);
Key poseKey2(2);
Pose3 measurement(Rot3::RzRyRx(0.15, 0.15, -0.20), Point3(+0.5, -1.0, +1.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
// Create a linearization point at the zero-error point
Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
Pose3 pose2(Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
Point3(-3.37493895, 6.14660244, -8.93650986));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative
Matrix actualH1;
Matrix actualH2;
factor.evaluateError(pose1, pose2, actualH1, actualH2);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH2, actualH2, 1e-9));
}
/* ************************************************************************* */
TEST( PoseBetweenFactor, JacobianWithTransform ) {
// Create a factor
Key poseKey1(1);
Key poseKey2(2);
Pose3 measurement(Rot3::RzRyRx(-0.15, 0.10, 0.15), Point3(+1.25, -0.90, +.45));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
// Create a linearization point at the zero-error point
Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
Pose3 pose2(Rot3::RzRyRx(0.162672458989103, 0.013665177349534, 0.224649482928184),
Point3(-3.5257579, 6.02637531, -8.98382384));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative
Matrix actualH1;
Matrix actualH2;
Vector error = factor.evaluateError(pose1, pose2, actualH1, actualH2);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH2, actualH2, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

185
gtsam_unstable/slam/tests/testPosePriorFactor.cpp Normal file
View File

@ -0,0 +1,185 @@
/* ----------------------------------------------------------------------------
* 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_unstable/slam/PosePriorFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
typedef PosePriorFactor<Pose3> TestPosePriorFactor;
/* ************************************************************************* */
TEST( PosePriorFactor, Constructor) {
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPosePriorFactor factor(poseKey, measurement, model);
}
/* ************************************************************************* */
TEST( PosePriorFactor, ConstructorWithTransform) {
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
}
/* ************************************************************************* */
TEST( PosePriorFactor, Equals ) {
// Create two identical factors and make sure they're equal
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPosePriorFactor factor1(poseKey, measurement, model);
TestPosePriorFactor factor2(poseKey, measurement, model);
CHECK(assert_equal(factor1, factor2));
// Create a third, different factor and check for inequality
Pose3 measurement2(Rot3::RzRyRx(0.20, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
TestPosePriorFactor factor3(poseKey, measurement2, model);
CHECK(assert_inequal(factor1, factor3));
}
/* ************************************************************************* */
TEST( PosePriorFactor, EqualsWithTransform ) {
// Create two identical factors and make sure they're equal
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestPosePriorFactor factor1(poseKey, measurement, model, body_P_sensor);
TestPosePriorFactor factor2(poseKey, measurement, model, body_P_sensor);
CHECK(assert_equal(factor1, factor2));
// Create a third, different factor and check for inequality
Pose3 body_P_sensor2(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.30, -0.10, 1.0));
TestPosePriorFactor factor3(poseKey, measurement, model, body_P_sensor2);
CHECK(assert_inequal(factor1, factor3));
}
/* ************************************************************************* */
TEST( PosePriorFactor, Error ) {
// Create the measurement and linearization point
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
Pose3 pose(Rot3::RzRyRx(0.0, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
// The expected error
Vector expectedError(6);
expectedError << -0.184137861505414,
0.139419283914526,
-0.158399296722242,
0.740211733817804,
-1.198210282560946,
1.008156093015192;
// Create a factor and calculate the error
Key poseKey(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPosePriorFactor factor(poseKey, measurement, model);
Vector actualError(factor.evaluateError(pose));
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( PosePriorFactor, ErrorWithTransform ) {
// Create the measurement and linearization point
Pose3 measurement(Rot3::RzRyRx(-M_PI_2+0.15, -0.30, -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0));
Pose3 pose(Rot3::RzRyRx(0.0, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
// The expected error
Vector expectedError(6);
expectedError << -0.022712885347328,
0.193765110165872,
0.276418420819283,
1.497519863757366,
-0.549375791422721,
0.452761203187666;
// Create a factor and calculate the error
Key poseKey(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
Vector actualError(factor.evaluateError(pose));
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( PosePriorFactor, Jacobian ) {
// Create a factor
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
TestPosePriorFactor factor(poseKey, measurement, model);
// Create a linearization point at the zero-error point
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative
Matrix actualH1;
factor.evaluateError(pose, actualH1);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
}
/* ************************************************************************* */
TEST( PosePriorFactor, JacobianWithTransform ) {
// Create a factor
Key poseKey(1);
Pose3 measurement(Rot3::RzRyRx(-M_PI_2+0.15, -0.30, -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0));
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
// Create a linearization point at the zero-error point
Pose3 pose(Rot3::RzRyRx(-0.303202977317447, -0.143253159173011, 0.494633847678171),
Point3(-4.74767676, 7.67044942, -11.00985));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative
Matrix actualH1;
factor.evaluateError(pose, actualH1);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */