Added MultiProjectionFactor, invoving n views (camera poses) observing a single landmark: work in progress
Luca Carlone
parent
ba2ddb22d8
commit
ed79432a69
|
|
@ -0,0 +1,228 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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/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 MultiProjectionFactor: public NoiseModelFactor {
|
||||
protected:
|
||||
|
||||
// Keep a copy of measurement and calibration for I/O
|
||||
Vector measured_; ///< 2D measurement for each of the n views
|
||||
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
|
||||
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 NoiseModelFactor Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef MultiProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
MultiProjectionFactor() : 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 pointKey is the index of the landmark
|
||||
* @param K shared pointer to the constant calibration
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
*/
|
||||
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
|
||||
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
|
||||
boost::optional<POSE> body_P_sensor = boost::none) :
|
||||
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
|
||||
throwCheirality_(false), verboseCheirality_(false) {
|
||||
keys_.assign(poseKeys.begin(), poseKeys.end());
|
||||
keys_.push_back(pointKey);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
*/
|
||||
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
|
||||
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
|
||||
bool throwCheirality, bool verboseCheirality,
|
||||
boost::optional<POSE> body_P_sensor = boost::none) :
|
||||
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
|
||||
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~MultiProjectionFactor() {}
|
||||
|
||||
/// @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 << "MultiProjectionFactor, z = ";
|
||||
std::cout << measured_ << "measurements, z = ";
|
||||
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);
|
||||
return e
|
||||
&& Base::equals(p, tol)
|
||||
//&& this->measured_.equals(e->measured_, tol)
|
||||
&& 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();
|
||||
// }
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
|
||||
Vector evaluateError(const Pose3& pose, const Point3& point,
|
||||
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
||||
try {
|
||||
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();
|
||||
}
|
||||
} catch( CheiralityException& e) {
|
||||
if (H1) *H1 = zeros(2,6);
|
||||
if (H2) *H2 = 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 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
|
||||
|
|
@ -0,0 +1,235 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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/MultiProjectionFactor.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/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 <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, create ){
|
||||
Values theta;
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
Symbol x1('X', 1);
|
||||
Symbol x2('X', 2);
|
||||
Symbol x3('X', 3);
|
||||
|
||||
Symbol l1('l', 1);
|
||||
Vector n_measPixel(6); // Pixel measurements from 3 cameras observing landmark 1
|
||||
n_measPixel << 10, 10, 10, 10, 10, 10;
|
||||
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
|
||||
|
||||
FastSet<Key> views;
|
||||
views.insert(x1);
|
||||
views.insert(x2);
|
||||
views.insert(x3);
|
||||
|
||||
MultiProjectionFactor<Pose3, Point3> mpFactor(n_measPixel, noiseProjection, views, l1, K);
|
||||
graph.add(mpFactor);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
///* ************************************************************************* */
|
||||
//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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue