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