added SmartHessianProjection factor: allows different calibrations and noises for the cameras
Luca Carlone
parent
5446fe1711
commit
44ae90f523
|
|
@ -0,0 +1,599 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Luca Carlone
|
||||
* @author Zsolt Kira
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <vector>
|
||||
#include <gtsam_unstable/geometry/triangulation.h>
|
||||
#include <boost/optional.hpp>
|
||||
//#include <boost/assign.hpp>
|
||||
|
||||
static bool isDebug=false;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// default threshold for selective relinearization
|
||||
static double defaultLinThreshold = -1; // 1e-7; // 0.01
|
||||
// default threshold for retriangulation
|
||||
static double defaultTriangThreshold = 1e-5;
|
||||
// default threshold for rank deficient triangulation
|
||||
static double defaultRankTolerance = 1; // this value may be scenario-dependent and has to be larger in presence of larger noise
|
||||
// if set to true will use the rotation-only version for degenerate cases
|
||||
static bool manageDegeneracy = true;
|
||||
|
||||
/**
|
||||
* Structure for storing some state memory, used to speed up optimization
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
class SmartProjectionHessianFactorState {
|
||||
public:
|
||||
|
||||
static int lastID;
|
||||
int ID;
|
||||
|
||||
SmartProjectionHessianFactorState() {
|
||||
ID = lastID++;
|
||||
calculatedHessian = false;
|
||||
}
|
||||
|
||||
// Linearization point
|
||||
Values values;
|
||||
std::vector<Pose3> cameraPosesLinearization;
|
||||
|
||||
// Triangulation at current linearization point
|
||||
Point3 point;
|
||||
std::vector<Pose3> cameraPosesTriangulation;
|
||||
bool degenerate;
|
||||
bool cheiralityException;
|
||||
|
||||
// Overall reprojection error
|
||||
double overallError;
|
||||
std::vector<Pose3> cameraPosesError;
|
||||
|
||||
// Hessian representation (after Schur complement)
|
||||
bool calculatedHessian;
|
||||
Matrix H;
|
||||
Vector gs_vector;
|
||||
std::vector<Matrix> Gs;
|
||||
std::vector<Vector> gs;
|
||||
double f;
|
||||
|
||||
// C = Hl'Hl
|
||||
// Cinv = inv(Hl'Hl)
|
||||
// Matrix3 Cinv;
|
||||
// E = Hx'Hl
|
||||
// w = Hl'b
|
||||
};
|
||||
|
||||
int SmartProjectionHessianFactorState::lastID = 0;
|
||||
|
||||
/**
|
||||
* The calibration is known here.
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
|
||||
class SmartProjectionHessianFactor: public NonlinearFactor {
|
||||
protected:
|
||||
|
||||
// Keep a copy of measurement and calibration for I/O
|
||||
std::vector<Point2> measured_; ///< 2D measurement for each of the m views
|
||||
std::vector< SharedNoiseModel > noise_; ///< noise model used
|
||||
///< (important that the order is the same as the keys that we use to create the factor)
|
||||
std::vector< boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
|
||||
|
||||
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
|
||||
|
||||
double rankTolerance; ///< threshold to decide whether triangulation is degenerate
|
||||
|
||||
double linearizationThreshold; ///< threshold to decide whether to re-linearize
|
||||
|
||||
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for each camera)
|
||||
|
||||
boost::shared_ptr<SmartProjectionHessianFactorState> state_;
|
||||
|
||||
// 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 SmartProjectionHessianFactor<POSE, LANDMARK, CALIBRATION> This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// shorthand for smart projection factor state variable
|
||||
typedef boost::shared_ptr<SmartProjectionHessianFactorState> SmartFactorStatePtr;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
|
||||
* @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements)
|
||||
* @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
|
||||
* @param K shared pointer to the constant calibration
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
*/
|
||||
SmartProjectionHessianFactor(
|
||||
const double rankTol = defaultRankTolerance,
|
||||
const double linThreshold = defaultLinThreshold,
|
||||
boost::optional<POSE> body_P_sensor = boost::none,
|
||||
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionHessianFactorState())) :
|
||||
retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol),
|
||||
linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor),
|
||||
state_(state), throwCheirality_(false), verboseCheirality_(false) {}
|
||||
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionHessianFactor() {}
|
||||
|
||||
/**
|
||||
* add a new measurement and pose key
|
||||
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKey is the index corresponding to the camera observing the same landmark
|
||||
*/
|
||||
void add(const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i,
|
||||
const boost::shared_ptr<CALIBRATION> K_i) {
|
||||
measured_.push_back(measured_i);
|
||||
keys_.push_back(poseKey_i);
|
||||
noise_.push_back(noise_i);
|
||||
K_all_.push_back(K_i);
|
||||
}
|
||||
|
||||
void add(std::vector< Point2 > measurements, std::vector< Key > poseKeys, std::vector< SharedNoiseModel > noises,
|
||||
std::vector< boost::shared_ptr<CALIBRATION> > Ks) {
|
||||
for(size_t i = 0; i < measurements.size(); i++) {
|
||||
measured_.push_back(measurements.at(i));
|
||||
keys_.push_back(poseKeys.at(i));
|
||||
noise_.push_back(noises.at(i));
|
||||
K_all_.push_back(Ks.at(i));
|
||||
}
|
||||
}
|
||||
|
||||
void add(std::vector< Point2 > measurements, std::vector< Key > poseKeys, const SharedNoiseModel noise,
|
||||
const boost::shared_ptr<CALIBRATION> K) {
|
||||
for(size_t i = 0; i < measurements.size(); i++) {
|
||||
measured_.push_back(measurements.at(i));
|
||||
keys_.push_back(poseKeys.at(i));
|
||||
noise_.push_back(noise);
|
||||
K_all_.push_back(K);
|
||||
}
|
||||
}
|
||||
|
||||
// This function checks if the new linearization point is the same as the one used for previous triangulation
|
||||
// (if not, a new triangulation is needed)
|
||||
static bool decideIfTriangulate(std::vector<Pose3> cameraPoses, std::vector<Pose3> oldPoses, double retriangulationThreshold) {
|
||||
// several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
|
||||
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
|
||||
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
|
||||
|
||||
// if we do not have a previous linearization point or the new linearization point includes more poses
|
||||
if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size()))
|
||||
return true;
|
||||
|
||||
for(size_t i = 0; i < cameraPoses.size(); i++) {
|
||||
if (!cameraPoses[i].equals(oldPoses[i], retriangulationThreshold)) {
|
||||
return true; // at least two poses are different, hence we retriangulate
|
||||
}
|
||||
}
|
||||
return false; // if we arrive to this point all poses are the same and we don't need re-triangulation
|
||||
}
|
||||
|
||||
// This function checks if the new linearization point is 'close' to the previous one used for linearization
|
||||
// (if not, a new linearization is needed)
|
||||
static bool decideIfLinearize(std::vector<Pose3> cameraPoses, std::vector<Pose3> oldPoses, double linearizationThreshold) {
|
||||
// "selective linearization"
|
||||
// The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
|
||||
// (we only care about the "rigidity" of the poses, not about their absolute pose)
|
||||
|
||||
// if we do not have a previous linearization point or the new linearization point includes more poses
|
||||
if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size()))
|
||||
return true;
|
||||
|
||||
Pose3 firstCameraPose;
|
||||
Pose3 firstCameraPoseOld;
|
||||
|
||||
for(size_t i = 0; i < cameraPoses.size(); i++) {
|
||||
|
||||
if(i==0){ // we store the initial pose, this is useful for selective re-linearization
|
||||
firstCameraPose = cameraPoses[i];
|
||||
firstCameraPoseOld = oldPoses[i];
|
||||
continue;
|
||||
}
|
||||
|
||||
// we compare the poses in the frame of the first pose
|
||||
Pose3 localCameraPose = firstCameraPose.between(cameraPoses[i]);
|
||||
Pose3 localCameraPoseOld = firstCameraPoseOld.between(oldPoses[i]);
|
||||
|
||||
if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold)) {
|
||||
return true; // at least two "relative" poses are different, hence we re-linerize
|
||||
}
|
||||
}
|
||||
return false; // if we arrive to this point all poses are the same and we don't need re-linerize
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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 << "SmartProjectionHessianFactor, z = \n ";
|
||||
BOOST_FOREACH(const Point2& p, measured_) {
|
||||
std::cout << "measurement, p = "<< p << std::endl;
|
||||
}
|
||||
BOOST_FOREACH(const SharedNoiseModel& noise_i, noise_) {
|
||||
noise_i->print("noise model = ");
|
||||
}
|
||||
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_all_, tol);
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
}
|
||||
|
||||
/// 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& values) const {
|
||||
|
||||
bool blockwise = false; // the full matrix version in faster
|
||||
int dim_landmark = 3; // for degenerate instances this will become 2 (direction-only information)
|
||||
|
||||
// Create structures for Hessian Factors
|
||||
unsigned int numKeys = keys_.size();
|
||||
if(isDebug) {std::cout<< " numKeys = "<< numKeys<<std::endl; }
|
||||
std::vector<Index> js;
|
||||
std::vector<Matrix> Gs(numKeys*(numKeys+1)/2);
|
||||
std::vector<Vector> gs(numKeys);
|
||||
double f=0;
|
||||
|
||||
// Collect all poses (Cameras)
|
||||
std::vector<Pose3> cameraPoses;
|
||||
BOOST_FOREACH(const Key& k, keys_) {
|
||||
Pose3 cameraPose;
|
||||
if(body_P_sensor_) { cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);}
|
||||
else { cameraPose = values.at<Pose3>(k);}
|
||||
cameraPoses.push_back(cameraPose);
|
||||
}
|
||||
|
||||
if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative
|
||||
state_->degenerate = true;
|
||||
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
|
||||
BOOST_FOREACH(Vector& v, gs) v = zero(6);
|
||||
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
|
||||
}
|
||||
|
||||
bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);
|
||||
|
||||
if(retriangulate) {// we store the current poses used for triangulation
|
||||
state_->cameraPosesTriangulation = cameraPoses;
|
||||
}
|
||||
|
||||
if (retriangulate) {
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
state_->point = triangulatePoint3(cameraPoses, measured_, *K_all_.at(0), rankTolerance);
|
||||
state_->degenerate = false;
|
||||
state_->cheiralityException = false;
|
||||
} catch( TriangulationUnderconstrainedException& e) {
|
||||
// if TriangulationUnderconstrainedException can be
|
||||
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
|
||||
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
|
||||
// in the second case we want to use a rotation-only smart factor
|
||||
//std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity
|
||||
state_->degenerate = true;
|
||||
state_->cheiralityException = false;
|
||||
} catch( TriangulationCheiralityException& e) {
|
||||
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
|
||||
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
|
||||
//std::cout << e.what() << std::end;
|
||||
state_->cheiralityException = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){
|
||||
// std::cout << "In linearize: exception" << std::endl;
|
||||
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
|
||||
BOOST_FOREACH(Vector& v, gs) v = zero(6);
|
||||
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
|
||||
}
|
||||
|
||||
if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors
|
||||
state_->degenerate = true;
|
||||
dim_landmark = 2;
|
||||
}
|
||||
|
||||
bool doLinearize;
|
||||
if (linearizationThreshold >= 0){//by convention if linearizationThreshold is negative we always relinearize
|
||||
// std::cout << "Temporary disabled" << std::endl;
|
||||
doLinearize = decideIfLinearize(cameraPoses, state_->cameraPosesLinearization, linearizationThreshold);
|
||||
}
|
||||
else{
|
||||
doLinearize = true;
|
||||
}
|
||||
|
||||
if (doLinearize) {
|
||||
state_->cameraPosesLinearization = cameraPoses;
|
||||
}
|
||||
|
||||
if(!doLinearize){ // return the previous Hessian factor
|
||||
// std::cout << "Using stored factors :) " << std::endl;
|
||||
return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
|
||||
}
|
||||
|
||||
if (blockwise == false){ // version with full matrix multiplication
|
||||
// ==========================================================================================================
|
||||
Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
|
||||
Matrix Hl2 = zeros(2 * numKeys, dim_landmark);
|
||||
Vector b2 = zero(2 * numKeys);
|
||||
|
||||
if(state_->degenerate){
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
|
||||
if(i==0){ // first pose
|
||||
state_->point = camera.backprojectPointAtInfinity(measured_.at(i));
|
||||
// 3D parametrization of point at infinity: [px py 1]
|
||||
// std::cout << "point_ " << state_->point<< std::endl;
|
||||
}
|
||||
Matrix Hxi, Hli;
|
||||
Vector bi = -( camera.projectPointAtInfinity(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
// std::cout << "Hxi \n" << Hxi<< std::endl;
|
||||
// std::cout << "Hli \n" << Hli<< std::endl;
|
||||
|
||||
noise_.at(i)-> WhitenSystem(Hxi, Hli, bi);
|
||||
f += bi.squaredNorm();
|
||||
|
||||
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
|
||||
Hl2.block( 2*i, 0, 2, 2 ) = Hli;
|
||||
|
||||
subInsert(b2,bi,2*i);
|
||||
}
|
||||
// std::cout << "Hx2 \n" << Hx2<< std::endl;
|
||||
// std::cout << "Hl2 \n" << Hl2<< std::endl;
|
||||
}
|
||||
else{
|
||||
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
|
||||
Matrix Hxi, Hli;
|
||||
|
||||
Vector bi;
|
||||
try {
|
||||
bi = -( camera.project(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
} catch ( CheiralityException& e) {
|
||||
std::cout << "Cheirality exception " << state_->ID << std::endl;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
noise_.at(i)-> WhitenSystem(Hxi, Hli, bi);
|
||||
f += bi.squaredNorm();
|
||||
|
||||
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
|
||||
Hl2.block( 2*i, 0, 2, 3 ) = Hli;
|
||||
|
||||
subInsert(b2,bi,2*i);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Shur complement trick
|
||||
Matrix H(6 * numKeys, 6 * numKeys);
|
||||
Matrix C2;
|
||||
Vector gs_vector;
|
||||
|
||||
C2.noalias() = (Hl2.transpose() * Hl2).inverse();
|
||||
H.noalias() = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2))));
|
||||
gs_vector.noalias() = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2))));
|
||||
|
||||
// Populate Gs and gs
|
||||
int GsCount2 = 0;
|
||||
for(size_t i1 = 0; i1 < numKeys; i1++) {
|
||||
gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6);
|
||||
|
||||
for(size_t i2 = 0; i2 < numKeys; i2++) {
|
||||
if (i2 >= i1) {
|
||||
Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6);
|
||||
GsCount2++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ==========================================================================================================
|
||||
if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables
|
||||
state_->calculatedHessian = true;
|
||||
state_->Gs = Gs;
|
||||
state_->gs = gs;
|
||||
state_->f = f;
|
||||
}
|
||||
|
||||
return HessianFactor::shared_ptr(new HessianFactor(keys_, 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_) {
|
||||
Pose3 cameraPose;
|
||||
if(body_P_sensor_) { cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);}
|
||||
else { cameraPose = values.at<Pose3>(k);}
|
||||
cameraPoses.push_back(cameraPose);
|
||||
|
||||
if(0&& isDebug) {cameraPose.print("cameraPose = "); }
|
||||
}
|
||||
|
||||
if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);
|
||||
|
||||
if(retriangulate) {// we store the current poses used for triangulation
|
||||
state_->cameraPosesTriangulation = cameraPoses;
|
||||
}
|
||||
|
||||
if (retriangulate) {
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
state_->point = triangulatePoint3(cameraPoses, measured_, *K_all_.at(0), rankTolerance);
|
||||
state_->degenerate = false;
|
||||
state_->cheiralityException = false;
|
||||
} catch( TriangulationUnderconstrainedException& e) {
|
||||
// if TriangulationUnderconstrainedException can be
|
||||
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
|
||||
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
|
||||
// in the second case we want to use a rotation-only smart factor
|
||||
//std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity
|
||||
state_->degenerate = true;
|
||||
state_->cheiralityException = false;
|
||||
} catch( TriangulationCheiralityException& e) {
|
||||
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
|
||||
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
|
||||
//std::cout << e.what() << std::end;
|
||||
state_->cheiralityException = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){
|
||||
// if we don't want to manage the exceptions we discard the factor
|
||||
// std::cout << "In error evaluation: exception" << std::endl;
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors
|
||||
state_->degenerate = true;
|
||||
}
|
||||
|
||||
if(state_->degenerate){
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
|
||||
if(i==0){ // first pose
|
||||
state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
|
||||
}
|
||||
Point2 reprojectionError(camera.projectPointAtInfinity(state_->point) - measured_.at(i));
|
||||
overallError += 0.5 * noise_.at(i)->distance( reprojectionError.vector() );
|
||||
//overallError += reprojectionError.vector().norm();
|
||||
}
|
||||
return overallError;
|
||||
}
|
||||
else{
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
|
||||
|
||||
try {
|
||||
Point2 reprojectionError(camera.project(state_->point) - measured_.at(i));
|
||||
//std::cout << "Reprojection error: " << reprojectionError << std::endl;
|
||||
overallError += 0.5 * noise_.at(i)->distance( reprojectionError.vector() );
|
||||
//overallError += reprojectionError.vector().norm();
|
||||
} catch ( CheiralityException& e) {
|
||||
std::cout << "Cheirality exception " << state_->ID << std::endl;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
return overallError;
|
||||
}
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/** return the measurements */
|
||||
const Vector& measured() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** return the noise model */
|
||||
const SharedNoiseModel& noise() const {
|
||||
return noise_;
|
||||
}
|
||||
|
||||
/** return the landmark */
|
||||
boost::optional<Point3> point() const {
|
||||
return state_->point;
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
inline const boost::shared_ptr<CALIBRATION> calibration() const {
|
||||
return K_all_;
|
||||
}
|
||||
|
||||
/** 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_all_);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
|
||||
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
|
||||
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -0,0 +1,844 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 TestSmartProjectionHessianFactor.cpp
|
||||
* @brief Unit tests for ProjectionFactor Class
|
||||
* @author Frank Dellaert
|
||||
* @date Nov 2009
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam/slam/PoseTranslationPrior.h>
|
||||
#include <gtsam_unstable/slam/SmartProjectionHessianFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.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 <boost/assign/std/vector.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
|
||||
static bool isDebugTest = false;
|
||||
|
||||
// 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));
|
||||
|
||||
static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
|
||||
static double rankTol = 1.0;
|
||||
static double linThreshold = -1.0;
|
||||
// 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;
|
||||
|
||||
// tests data
|
||||
Symbol x1('X', 1);
|
||||
Symbol x2('X', 2);
|
||||
Symbol x3('X', 3);
|
||||
|
||||
static Key poseKey1(x1);
|
||||
static Key poseKey2(x2);
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
|
||||
typedef SmartProjectionHessianFactor<Pose3, Point3> SmartFactor;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionHessianFactor, Constructor) {
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionHessianFactor, Constructor2) {
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionHessianFactor, Constructor3) {
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(measurement1, poseKey1, model, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionHessianFactor, Constructor4) {
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
factor1.add(measurement1, poseKey1, model, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionHessianFactor, ConstructorWithTransform) {
|
||||
SmartFactor factor1(rankTol, linThreshold, body_P_sensor1);
|
||||
factor1.add(measurement1, poseKey1, model, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionHessianFactor, Equals ) {
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(measurement1, poseKey1, model, K);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor());
|
||||
factor2->add(measurement1, poseKey1, model, K);
|
||||
|
||||
CHECK(assert_equal(*factor1, *factor2));
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, noiseless ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: noisy ****************************" << endl;
|
||||
|
||||
// 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, *K2);
|
||||
|
||||
// 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, *K2);
|
||||
|
||||
// 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 values;
|
||||
values.insert(x1, level_pose);
|
||||
values.insert(x2, level_pose_right);
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, x1, model, K);
|
||||
factor1->add(level_uv_right, x2, model, K);
|
||||
|
||||
double actualError = factor1->error(values);
|
||||
double expectedError = 0.0;
|
||||
DOUBLES_EQUAL(expectedError, actualError, 1e-7);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, noisy ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: noisy ****************************" << endl;
|
||||
|
||||
// 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, *K2);
|
||||
|
||||
// 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, *K2);
|
||||
|
||||
// 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 values;
|
||||
values.insert(x1, level_pose);
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
|
||||
values.insert(x2, level_pose_right.compose(noise_pose));
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, x1, model, K);
|
||||
factor1->add(level_uv_right, x2, model, K);
|
||||
|
||||
double actualError1= factor1->error(values);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor());
|
||||
vector<Point2> measurements;
|
||||
measurements.push_back(level_uv);
|
||||
measurements.push_back(level_uv_right);
|
||||
|
||||
std::vector< SharedNoiseModel > noises;
|
||||
noises.push_back(model);
|
||||
noises.push_back(model);
|
||||
|
||||
std::vector< boost::shared_ptr<Cal3_S2> > Ks; ///< shared pointer to calibration object (one for each camera)
|
||||
Ks.push_back(K);
|
||||
Ks.push_back(K);
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
factor2->add(measurements, views, noises, Ks);
|
||||
|
||||
double actualError2= factor2->error(values);
|
||||
|
||||
DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
|
||||
}
|
||||
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, 3poses_smart_projection_factor ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: 3 cams + 3 landmarks **********************" << endl;
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K2);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
|
||||
SimpleCamera cam2(pose2, *K2);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
|
||||
SimpleCamera cam3(pose3, *K2);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
Point2 cam3_uv1 = cam3.project(landmark1);
|
||||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
measurements_cam1.push_back(cam3_uv1);
|
||||
|
||||
Point2 cam1_uv2 = cam1.project(landmark2);
|
||||
Point2 cam2_uv2 = cam2.project(landmark2);
|
||||
Point2 cam3_uv2 = cam3.project(landmark2);
|
||||
measurements_cam2.push_back(cam1_uv2);
|
||||
measurements_cam2.push_back(cam2_uv2);
|
||||
measurements_cam2.push_back(cam3_uv2);
|
||||
|
||||
Point2 cam1_uv3 = cam1.project(landmark3);
|
||||
Point2 cam2_uv3 = cam2.project(landmark3);
|
||||
Point2 cam3_uv3 = cam3.project(landmark3);
|
||||
measurements_cam3.push_back(cam1_uv3);
|
||||
measurements_cam3.push_back(cam2_uv3);
|
||||
measurements_cam3.push_back(cam3_uv3);
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, model, K2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, model, K2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, model, K2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
|
||||
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose3
|
||||
values.insert(x3, pose3*noise_pose);
|
||||
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
gttic_(SmartProjectionHessianFactor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartProjectionHessianFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
// result.print("results of 3 camera, 3 landmark optimization \n");
|
||||
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
if(isDebugTest) tictoc_print_();
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, 3poses_iterative_smart_projection_factor ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: 3 cams + 3 landmarks **********************" << endl;
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
|
||||
SimpleCamera cam2(pose2, *K);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
|
||||
SimpleCamera cam3(pose3, *K);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
Point2 cam3_uv1 = cam3.project(landmark1);
|
||||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
measurements_cam1.push_back(cam3_uv1);
|
||||
|
||||
Point2 cam1_uv2 = cam1.project(landmark2);
|
||||
Point2 cam2_uv2 = cam2.project(landmark2);
|
||||
Point2 cam3_uv2 = cam3.project(landmark2);
|
||||
measurements_cam2.push_back(cam1_uv2);
|
||||
measurements_cam2.push_back(cam2_uv2);
|
||||
measurements_cam2.push_back(cam3_uv2);
|
||||
|
||||
Point2 cam1_uv3 = cam1.project(landmark3);
|
||||
Point2 cam2_uv3 = cam2.project(landmark3);
|
||||
Point2 cam3_uv3 = cam3.project(landmark3);
|
||||
measurements_cam3.push_back(cam1_uv3);
|
||||
measurements_cam3.push_back(cam2_uv3);
|
||||
measurements_cam3.push_back(cam3_uv3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(cam1_uv1, views[0], model, K);
|
||||
smartFactor1->add(cam2_uv1, views[1], model, K);
|
||||
smartFactor1->add(cam3_uv1, views[2], model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(cam1_uv2, views[0], model, K);
|
||||
smartFactor2->add(cam2_uv2, views[1], model, K);
|
||||
smartFactor2->add(cam3_uv2, views[2], model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(cam1_uv3, views[0], model, K);
|
||||
smartFactor3->add(cam2_uv3, views[1], model, K);
|
||||
smartFactor3->add(cam3_uv3, views[2], model, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
|
||||
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose3
|
||||
values.insert(x3, pose3*noise_pose);
|
||||
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
gttic_(SmartProjectionHessianFactor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartProjectionHessianFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
// result.print("results of 3 camera, 3 landmark optimization \n");
|
||||
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
if(isDebugTest) tictoc_print_();
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, 3poses_projection_factor ){
|
||||
// cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K2);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
|
||||
SimpleCamera cam2(pose2, *K2);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
|
||||
SimpleCamera cam3(pose3, *K2);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2));
|
||||
graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2));
|
||||
graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2));
|
||||
|
||||
graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2));
|
||||
graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2));
|
||||
graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2));
|
||||
|
||||
graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2));
|
||||
graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2));
|
||||
graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2));
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
|
||||
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
values.insert(x3, pose3* noise_pose);
|
||||
values.insert(L(1), landmark1);
|
||||
values.insert(L(2), landmark2);
|
||||
values.insert(L(3), landmark3);
|
||||
if(isDebugTest) values.at<Pose3>(x3).print("Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
if(isDebugTest) result.at<Pose3>(x3).print("Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, 3poses_2land_rotation_only_smart_projection_factor ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K2);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
|
||||
SimpleCamera cam2(pose2, *K2);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
|
||||
SimpleCamera cam3(pose3, *K2);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
Point2 cam3_uv1 = cam3.project(landmark1);
|
||||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
measurements_cam1.push_back(cam3_uv1);
|
||||
|
||||
Point2 cam1_uv2 = cam1.project(landmark2);
|
||||
Point2 cam2_uv2 = cam2.project(landmark2);
|
||||
Point2 cam3_uv2 = cam3.project(landmark2);
|
||||
measurements_cam2.push_back(cam1_uv2);
|
||||
measurements_cam2.push_back(cam2_uv2);
|
||||
measurements_cam2.push_back(cam3_uv2);
|
||||
|
||||
double rankTol = 50;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
|
||||
smartFactor1->add(measurements_cam1, views, model, K2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
|
||||
smartFactor2->add(measurements_cam2, views, model, K2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
|
||||
Point3 positionPrior = gtsam::Point3(0,0,1);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
|
||||
graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
|
||||
graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2*noise_pose);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose3
|
||||
values.insert(x3, pose3*noise_pose*noise_pose);
|
||||
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
|
||||
if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
gttic_(SmartProjectionHessianFactor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartProjectionHessianFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
// result.print("results of 3 camera, 3 landmark optimization \n");
|
||||
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
if(isDebugTest) tictoc_print_();
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, 3poses_rotation_only_smart_projection_factor ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
|
||||
SimpleCamera cam2(pose2, *K);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
|
||||
SimpleCamera cam3(pose3, *K);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
Point2 cam3_uv1 = cam3.project(landmark1);
|
||||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
measurements_cam1.push_back(cam3_uv1);
|
||||
|
||||
Point2 cam1_uv2 = cam1.project(landmark2);
|
||||
Point2 cam2_uv2 = cam2.project(landmark2);
|
||||
Point2 cam3_uv2 = cam3.project(landmark2);
|
||||
measurements_cam2.push_back(cam1_uv2);
|
||||
measurements_cam2.push_back(cam2_uv2);
|
||||
measurements_cam2.push_back(cam3_uv2);
|
||||
|
||||
Point2 cam1_uv3 = cam1.project(landmark3);
|
||||
Point2 cam2_uv3 = cam2.project(landmark3);
|
||||
Point2 cam3_uv3 = cam3.project(landmark3);
|
||||
measurements_cam3.push_back(cam1_uv3);
|
||||
measurements_cam3.push_back(cam2_uv3);
|
||||
measurements_cam3.push_back(cam3_uv3);
|
||||
|
||||
double rankTol = 10;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
|
||||
Point3 positionPrior = gtsam::Point3(0,0,1);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
|
||||
graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
|
||||
graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose3
|
||||
values.insert(x3, pose3*noise_pose);
|
||||
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
|
||||
if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
gttic_(SmartProjectionHessianFactor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartProjectionHessianFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
// result.print("results of 3 camera, 3 landmark optimization \n");
|
||||
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
if(isDebugTest) tictoc_print_();
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, Hessian ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: Hessian **********************" << endl;
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K2);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
|
||||
SimpleCamera cam2(pose2, *K2);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
vector<Point2> measurements_cam1;
|
||||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1,views, model, K2);
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
|
||||
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize(values);
|
||||
if(isDebugTest) hessianFactor->print("Hessian factor \n");
|
||||
|
||||
// compute triangulation from linearization point
|
||||
// compute reprojection errors (sum squared)
|
||||
// compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
|
||||
// check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
|
||||
}
|
||||
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, HessianWithRotation ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: rotated Hessian **********************" << endl;
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
|
||||
SimpleCamera cam2(pose2, *K);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
|
||||
SimpleCamera cam3(pose3, *K);
|
||||
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
Point2 cam3_uv1 = cam3.project(landmark1);
|
||||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
measurements_cam1.push_back(cam3_uv1);
|
||||
|
||||
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
|
||||
smartFactorInstance->add(cam1_uv1, views[0], model, K);
|
||||
smartFactorInstance->add(cam2_uv1, views[1], model, K);
|
||||
smartFactorInstance->add(cam3_uv1, views[2], model, K);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
values.insert(x3, pose3);
|
||||
|
||||
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactorInstance->linearize(values);
|
||||
// hessianFactor->print("Hessian factor \n");
|
||||
|
||||
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));
|
||||
|
||||
Values rotValues;
|
||||
rotValues.insert(x1, poseDrift.compose(pose1));
|
||||
rotValues.insert(x2, poseDrift.compose(pose2));
|
||||
rotValues.insert(x3, poseDrift.compose(pose3));
|
||||
|
||||
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactorInstance->linearize(rotValues);
|
||||
// hessianFactorRot->print("Hessian factor \n");
|
||||
|
||||
// Hessian is invariant to rotations in the nondegenerate case
|
||||
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );
|
||||
|
||||
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));
|
||||
|
||||
Values tranValues;
|
||||
tranValues.insert(x1, poseDrift2.compose(pose1));
|
||||
tranValues.insert(x2, poseDrift2.compose(pose2));
|
||||
tranValues.insert(x3, poseDrift2.compose(pose3));
|
||||
|
||||
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactorInstance->linearize(tranValues);
|
||||
|
||||
// Hessian is invariant to rotations and translations in the nondegenerate case
|
||||
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionHessianFactor, HessianWithRotationDegenerate ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: rotated Hessian (degenerate) **********************" << endl;
|
||||
|
||||
std::vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K2);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0));
|
||||
SimpleCamera cam2(pose2, *K2);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0));
|
||||
SimpleCamera cam3(pose3, *K2);
|
||||
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
Point2 cam3_uv1 = cam3.project(landmark1);
|
||||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
measurements_cam1.push_back(cam3_uv1);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor(new SmartFactor());
|
||||
smartFactor->add(cam1_uv1, views[0], model, K2);
|
||||
smartFactor->add(cam2_uv1, views[1], model, K2);
|
||||
smartFactor->add(cam3_uv1, views[2], model, K2);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
values.insert(x3, pose3);
|
||||
|
||||
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(values);
|
||||
if(isDebugTest) hessianFactor->print("Hessian factor \n");
|
||||
|
||||
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));
|
||||
|
||||
Values rotValues;
|
||||
rotValues.insert(x1, poseDrift.compose(pose1));
|
||||
rotValues.insert(x2, poseDrift.compose(pose2));
|
||||
rotValues.insert(x3, poseDrift.compose(pose3));
|
||||
|
||||
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(rotValues);
|
||||
if(isDebugTest) hessianFactorRot->print("Hessian factor \n");
|
||||
|
||||
// Hessian is invariant to rotations in the nondegenerate case
|
||||
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );
|
||||
|
||||
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));
|
||||
|
||||
Values tranValues;
|
||||
tranValues.insert(x1, poseDrift2.compose(pose1));
|
||||
tranValues.insert(x2, poseDrift2.compose(pose2));
|
||||
tranValues.insert(x3, poseDrift2.compose(pose3));
|
||||
|
||||
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactor->linearize(tranValues);
|
||||
|
||||
// Hessian is invariant to rotations and translations in the nondegenerate case
|
||||
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue