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

Added optional sensor to body transformation to the range factor (and unit tests)

release/4.3a0
Stephen Williams 2012-10-21 22:34:56 +00:00
parent 857b0d0d8c
commit 227f9c1620
2 changed files with 336 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/slam/RangeFactor.h
View File

@ -31,6 +31,7 @@ namespace gtsam {
private:
double measured_; /** measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
typedef RangeFactor<POSE, POINT> This;
typedef NoiseModelFactor2<POSE, POINT> Base;
@ -39,15 +40,15 @@ namespace gtsam {
typedef POINT Point;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT)
public:
RangeFactor() {} /* Default constructor */
RangeFactor(Key poseKey, Key pointKey, double measured,
const SharedNoiseModel& model) :
Base(model, poseKey, pointKey), measured_(measured) {
const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) {
}
virtual ~RangeFactor() {}
@ -58,8 +59,20 @@ namespace gtsam {
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */
Vector evaluateError(const Pose& pose, const Point& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = pose.range(point, H1, H2);
Vector evaluateError(const POSE& pose, const POINT& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double hx;
if(body_P_sensor_) {
if(H1) {
Matrix H0;
hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2);
*H1 = *H1 * H0;
} else {
hx = pose.compose(*body_P_sensor_).range(point, H1, H2);
}
} else {
hx = pose.range(point, H1, H2);
}
return Vector_(1, hx - measured_);
}
@ -71,12 +84,17 @@ namespace gtsam {
/** equals specialized to this factor */
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) && fabs(this->measured_ - e->measured_) < tol;
return e != NULL
&& Base::equals(*e, tol)
&& fabs(this->measured_ - 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_)));
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
}
@ -89,6 +107,7 @@ namespace gtsam {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // RangeFactor

311
gtsam/slam/tests/testRangeFactor.cpp Normal file
View File

@ -0,0 +1,311 @@
/* ----------------------------------------------------------------------------
* 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 testRangeFactor.cpp
* @brief Unit tests for RangeFactor Class
* @author Stephen Williams
* @date Oct 2012
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(1));
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
/* ************************************************************************* */
LieVector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
TEST( RangeFactor, Constructor) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
}
/* ************************************************************************* */
TEST( RangeFactor, ConstructorWithTransform) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D);
RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D);
}
/* ************************************************************************* */
TEST( RangeFactor, Equals ) {
// Create two identical factors and make sure they're equal
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model);
RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model);
CHECK(assert_equal(factor2D_1, factor2D_2));
RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model);
RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model);
CHECK(assert_equal(factor3D_1, factor3D_2));
}
/* ************************************************************************* */
TEST( RangeFactor, EqualsWithTransform ) {
// Create two identical factors and make sure they're equal
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D);
RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D);
CHECK(assert_equal(factor2D_1, factor2D_2));
RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D);
RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D);
CHECK(assert_equal(factor3D_1, factor3D_2));
}
/* ************************************************************************* */
TEST( RangeFactor, Error2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose2 pose(1.0, 2.0, 0.57);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( RangeFactor, Error2DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);
// Set the linearization point
Rot2 R(0.57);
Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
Pose2 pose(R, t);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( RangeFactor, Error3D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor3D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( RangeFactor, Error3DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
// Set the linearization point
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
Pose3 pose(R, t);
Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( RangeFactor, Jacobian2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose2 pose(1.0, 2.0, 0.57);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
TEST( RangeFactor, Jacobian2DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);
// Set the linearization point
Rot2 R(0.57);
Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
Pose2 pose(R, t);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
TEST( RangeFactor, Jacobian3D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor3D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
TEST( RangeFactor, Jacobian3DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
// Set the linearization point
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
Pose3 pose(R, t);
Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */