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

Added SmartProjectionFactor (+unit tests)

release/4.3a0
Luca Carlone 2013-08-02 23:35:39 +00:00
parent 334d71ce51
commit d1419a3d11
2 changed files with 600 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

292
gtsam_unstable/slam/SmartProjectionFactor.h Normal file
View File

@ -0,0 +1,292 @@
/* ----------------------------------------------------------------------------
* 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 ProjectionFactor.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/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam_unstable/geometry/triangulation.h>
#include <boost/optional.hpp>
#include <boost/assign.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 SmartProjectionFactor: public NonlinearFactor {
protected:
// Keep a copy of measurement and calibration for I/O
std::vector<Point2> measured_; ///< 2D measurement for each of the n views
///< (important that the order is the same as the keys that we use to create the factor)
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
const SharedNoiseModel noise_; ///< noise model used
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
// 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 NonlinearFactor Base;
/// shorthand for this class
typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
SmartProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2n dimensional location of the n points in the n views (the measurements)
* @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
* @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
* @param K shared pointer to the constant calibration
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) :
measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {
keys_.assign(poseKeys.begin(), poseKeys.end());
}
/**
* 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 K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~SmartProjectionFactor() {}
/// @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 << "SmartProjectionFactor, z = ";
BOOST_FOREACH(const Point2& p, measured_) {
std::cout << "measurement, p = "<< p << std::endl;
}
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
bool areMeasurementsEqual = true;
for(size_t i = 0; i < measured_.size(); i++) {
if(this->measured_.at(i).equals(e->measured_.at(i), tol) == false)
areMeasurementsEqual = false;
break;
}
return e
&& Base::equals(p, tol)
&& areMeasurementsEqual
&& this->K_->equals(*e->K_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/// Evaluate error h(x)-z and optionally derivatives
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const{
Vector a;
return a;
// Point3 point = x.at<Point3>(*keys_.end());
//
// std::vector<KeyType>::iterator vit;
// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) {
// Key key = (*vit);
// Pose3 pose = x.at<Pose3>(key);
//
// if(body_P_sensor_) {
// if(H1) {
// gtsam::Matrix H0;
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// *H1 = *H1 * H0;
// return reprojectionError.vector();
// } else {
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// }
// } else {
// PinholeCamera<CALIBRATION> camera(pose, *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// }
// }
}
/// get the dimension of the factor (number of rows on linearization)
virtual size_t dim() const {
return 6*keys_.size();
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
// fill in the keys
std::vector<Index> js;
BOOST_FOREACH(const Key& k, keys_) {
js += ordering[k];
}
std::vector<Matrix> Gs;
std::vector<Vector> gs;
// Shur complement trick
// double e = u + b - z , e2 = e * e;
// double c = 2 * logSqrt2PI - log(p) + e2 * p;
// Vector g1 = Vector_(1, -e * p);
// Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2);
// Vector g3 = Vector_(1, -e * p);
// Matrix G11 = Matrix_(1, 1, p);
// Matrix G12 = Matrix_(1, 1, e);
// Matrix G13 = Matrix_(1, 1, p);
// Matrix G22 = Matrix_(1, 1, 0.5 / (p * p));
// Matrix G23 = Matrix_(1, 1, e);
// Matrix G33 = Matrix_(1, 1, p);
double f = 0;
return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
}
/**
* Calculate the error of the factor.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const Values& values) const {
if (this->active(values)) {
double overallError=0;
// Collect all poses (Cameras)
std::vector<Pose3> cameraPoses;
BOOST_FOREACH(const Key& k, keys_) {
if(body_P_sensor_)
cameraPoses.push_back(values.at<Pose3>(k).compose(*body_P_sensor_));
else
cameraPoses.push_back(values.at<Pose3>(k));
}
// We triangulate the 3D position of the landmark
boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_);
if(point)
{ // triangulation produced a good estimate of landmark position
std::cout << "point " << *point << std::endl;
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_);
std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl;
Point2 reprojectionError(camera.project(*point) - measured_.at(i));
std::cout << "reprojectionError " << reprojectionError << std::endl;
overallError += noise_->distance( reprojectionError.vector() );
std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl;
}
return sqrt(overallError);
}else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error
return 0.0;
}
} else {
return 0.0;
}
}
/** return the measurements */
const Vector& 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(body_P_sensor_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

308
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp Normal file
View File

@ -0,0 +1,308 @@
/* ----------------------------------------------------------------------------
* 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/nonlinear/ConcurrentBatchFilter.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam_unstable/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam_unstable/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <CppUnitLite/TestHarness.h>
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;
//typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
///* ************************************************************************* */
TEST( MultiProjectionFactor, noiseless ){
cout << " ************************ MultiProjectionFactor: noiseless ****************************" << endl;
Values theta;
NonlinearFactorGraph graph;
Symbol x1('X', 1);
Symbol x2('X', 2);
// Symbol x3('X', 3);
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
std::vector<Key> views;
views += x1, x2; //, x3;
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
// create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera level_camera_right(level_pose_right, *K);
// landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark);
Values value;
value.insert(x1, level_pose);
value.insert(x2, level_pose_right);
// poses += level_pose, level_pose_right;
vector<Point2> measurements;
measurements += level_uv, level_uv_right;
SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(measurements, noiseProjection, views, K);
double actualError = smartFactor.error(value);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
///* ************************************************************************* */
TEST( MultiProjectionFactor, noisy ){
cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl;
Values theta;
NonlinearFactorGraph graph;
Symbol x1('X', 1);
Symbol x2('X', 2);
// Symbol x3('X', 3);
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
std::vector<Key> views;
views += x1, x2; //, x3;
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
// create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera level_camera_right(level_pose_right, *K);
// landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate
Point2 pixelError(0.2,0.2);
Point2 level_uv = level_camera.project(landmark) + pixelError;
Point2 level_uv_right = level_camera_right.project(landmark);
Values value;
value.insert(x1, level_pose);
value.insert(x2, level_pose_right);
// poses += level_pose, level_pose_right;
vector<Point2> measurements;
measurements += level_uv, level_uv_right;
SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(measurements, noiseProjection, views, K);
double actualError = smartFactor.error(value);
double expectedError = sqrt(0.08);
// we do not expect to be able to predict the error, since the error on the pixel will change
// the triangulation of the landmark which is internal to the factor.
// DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
///* ************************************************************************* */
//TEST( ProjectionFactor, nonStandard ) {
// GenericProjectionFactor<Pose3, Point3, Cal3DS2> f;
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, Constructor) {
// Key poseKey(X(1));
// Key pointKey(L(1));
//
// Point2 measurement(323.0, 240.0);
//
// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, ConstructorWithTransform) {
// Key poseKey(X(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, pointKey, K, body_P_sensor);
//}
//
///* ************************************************************************* */
//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), L(1), K);
// TestProjectionFactor factor2(measurement, model, X(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), L(1), K, body_P_sensor);
// TestProjectionFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor);
//
// 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 pointKey(L(1));
// Point2 measurement(323.0, 240.0);
// TestProjectionFactor factor(measurement, model, poseKey, 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, 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 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, pointKey, K, body_P_sensor);
//
// // 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, 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 pointKey(L(1));
// Point2 measurement(323.0, 240.0);
// TestProjectionFactor factor(measurement, model, poseKey, 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;
// factor.evaluateError(pose, point, H1Actual, H2Actual);
//
// // 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 H2Expected = 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(H2Expected, H2Actual, 1e-3));
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, JacobianWithTransform ) {
// // Create the factor with a measurement that is 3 pixels off in x
// Key poseKey(X(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, pointKey, K, body_P_sensor);
//
// // 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;
// factor.evaluateError(pose, point, H1Actual, H2Actual);
//
// // 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 H2Expected = 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(H2Expected, H2Actual, 1e-3));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */