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

Merged in feature/heterogeneousSmartFactorNoise (pull request #271) 

Feature/heterogeneoussmartfactornoise

Approved-by: Chris Beall
Approved-by: Jing Dong
release/4.3a0
Luca Carlone 2017-03-12 05:50:08 +00:00 committed by Chris Beall
parent bd67779f74 2a6d372b0a
commit fbb9d3bdda
10 changed files with 603 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/CameraSet.h
View File

@ -56,7 +56,11 @@ protected:
// Project and fill error vector
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
b.segment<ZDim>(row) = traits<Z>::Local(measured[i], predicted[i]);
Vector bi = traits<Z>::Local(measured[i], predicted[i]);
if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan)
bi(1) = 0;
}
b.segment<ZDim>(row) = bi;
}
return b;
}

5
gtsam/geometry/StereoCamera.cpp
View File

@ -39,7 +39,10 @@ namespace gtsam {
const Point3 q = leftCamPose_.transform_to(point);
if ( q.z() <= 0 ) throw StereoCheiralityException();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0)
throw StereoCheiralityException();
#endif
// get calibration
const Cal3_S2Stereo& K = *K_;

17
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject)
CHECK(assert_equal(expected2,actual2,1e-7));
}
/* ************************************************************************* */
TEST( StereoCamera, projectCheirality)
{
// create a Stereo camera
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam(Pose3(), K);
// point behind the camera
Point3 p(0, 0, -5);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException);
#else // otherwise project should not throw the exception
StereoPoint2 expected = StereoPoint2(320, 470, 240);
CHECK(assert_equal(expected,stereoCam.project2(p),1e-7));
#endif
}
/* ************************************************************************* */
TEST( StereoCamera, backproject_case1)
{

10
gtsam/slam/SmartFactorBase.h
View File

@ -202,7 +202,7 @@ public:
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
if(body_P_sensor_){
if(body_P_sensor_ && Fs){
for(size_t i=0; i < Fs->size(); i++){
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
Matrix J(6, 6);
@ -210,9 +210,17 @@ public:
Fs->at(i) = Fs->at(i) * J;
}
}
correctForMissingMeasurements(cameras, ue, Fs, E);
return ue;
}
/**
* This corrects the Jacobians for the case in which some pixel measurement is missing (nan)
* In practice, this does not do anything in the monocular case, but it is implemented in the stereo version
*/
virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const {}
/**
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
* Noise model applied

134
gtsam/slam/SmartFactorParams.h Normal file
View File

@ -0,0 +1,134 @@
/* ----------------------------------------------------------------------------
* 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 SmartFactorParams
* @brief Collect common parameters for SmartProjection and SmartStereoProjection factors
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/triangulation.h>
namespace gtsam {
/**
* SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors
*/
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/*
* Parameters for the smart (stereo) projection factors
*/
struct GTSAM_EXPORT SmartProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
// Constructor
SmartProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false, double retriangulationTh = 1e-5) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartProjectionParams() {
}
void print(const std::string& str = "") const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
// get class variables
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
double getRetriangulationThreshold() const {
return retriangulationThreshold;
}
// set class variables
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRetriangulationThreshold(double retriangulationTh) {
retriangulationThreshold = retriangulationTh;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(linearizationMode);
ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
ar & BOOST_SERIALIZATION_NVP(triangulation);
ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
ar & BOOST_SERIALIZATION_NVP(throwCheirality);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
}
};
} // \ namespace gtsam

104
gtsam/slam/SmartProjectionFactor.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/inference/Symbol.h>
@ -31,109 +32,6 @@
namespace gtsam {
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/*
* Parameters for the smart projection factors
*/
struct GTSAM_EXPORT SmartProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
// Constructor
SmartProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false, double retriangulationTh = 1e-5) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
double getRetriangulationThreshold() const {
return retriangulationThreshold;
}
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setRetriangulationThreshold(double retriangulationTh) {
retriangulationThreshold = retriangulationTh;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(linearizationMode);
ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
ar & BOOST_SERIALIZATION_NVP(triangulation);
ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
ar & BOOST_SERIALIZATION_NVP(throwCheirality);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
}
};
/**
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
* This factor operates with monocular cameras, where a camera is expected to

7
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -1011,6 +1011,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) {
// this test considers a condition in which the cheirality constraint is triggered
using namespace vanillaPose;
vector<Key> views;
@ -1083,8 +1084,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
// Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY)
// rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
EXPECT(assert_equal(Pose3(values.at<Pose3>(x3).rotation(),
Point3(0,0,1)), result.at<Pose3>(x3)));
#else
// if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation
// with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose)
EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-3));
#endif
}
/* *************************************************************************/

214
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
@ -35,91 +36,10 @@
namespace gtsam {
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/*
* Parameters for the smart stereo projection factors
*/
struct GTSAM_EXPORT SmartStereoProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
/// Constructor
SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
1e-5), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartStereoProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
};
/*
* Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
*/
typedef SmartProjectionParams SmartStereoProjectionParams;
/**
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
@ -155,14 +75,19 @@ public:
/// Vector of cameras
typedef CameraSet<StereoCamera> Cameras;
/// Vector of monocular cameras (stereo treated as 2 monocular)
typedef PinholeCamera<Cal3_S2> MonoCamera;
typedef CameraSet<MonoCamera> MonoCameras;
typedef std::vector<Point2> MonoMeasurements;
/**
* Constructor
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) :
Base(sharedNoiseModel), //
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const boost::optional<Pose3> body_P_sensor = boost::none) :
Base(sharedNoiseModel, body_P_sensor), //
params_(params), //
result_(TriangulationResult::Degenerate()) {
}
@ -240,75 +165,28 @@ public:
size_t m = cameras.size();
bool retriangulate = decideIfTriangulate(cameras);
// if(!retriangulate)
// std::cout << "retriangulate = false" << std::endl;
//
// bool retriangulate = true;
if (retriangulate) {
// std::cout << "Retriangulate " << std::endl;
std::vector<Point3> reprojections;
reprojections.reserve(m);
for(size_t i = 0; i < m; i++) {
reprojections.push_back(cameras[i].backproject(measured_[i]));
// triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
MonoCameras monoCameras;
MonoMeasurements monoMeasured;
for(size_t i = 0; i < m; i++) {
const Pose3 leftPose = cameras[i].pose();
const Cal3_S2 monoCal = cameras[i].calibration().calibration();
const MonoCamera leftCamera_i(leftPose,monoCal);
const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
const Pose3 rightPose = leftPose.compose( left_Pose_right );
const MonoCamera rightCamera_i(rightPose,monoCal);
const StereoPoint2 zi = measured_[i];
monoCameras.push_back(leftCamera_i);
monoMeasured.push_back(Point2(zi.uL(),zi.v()));
if(!std::isnan(zi.uR())){ // if right point is valid
monoCameras.push_back(rightCamera_i);
monoMeasured.push_back(Point2(zi.uR(),zi.v()));
}
Point3 pw_sum(0,0,0);
for(const Point3& pw: reprojections) {
pw_sum = pw_sum + pw;
}
// average reprojected landmark
Point3 pw_avg = pw_sum / double(m);
double totalReprojError = 0;
// check if it lies in front of all cameras
for(size_t i = 0; i < m; i++) {
const Pose3& pose = cameras[i].pose();
const Point3& pl = pose.transform_to(pw_avg);
if (pl.z() <= 0) {
result_ = TriangulationResult::BehindCamera();
return result_;
}
// check landmark distance
if (params_.triangulation.landmarkDistanceThreshold > 0 &&
pl.norm() > params_.triangulation.landmarkDistanceThreshold) {
result_ = TriangulationResult::FarPoint();
return result_;
}
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) {
const StereoPoint2& zi = measured_[i];
StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi);
totalReprojError += reprojectionError.vector().norm();
}
} // for
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) {
result_ = TriangulationResult::Outlier();
return result_;
}
if(params_.triangulation.enableEPI) {
try {
pw_avg = triangulateNonlinear(cameras, measured_, pw_avg);
} catch(StereoCheiralityException& e) {
if(params_.verboseCheirality)
std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl;
if(params_.throwCheirality)
throw;
result_ = TriangulationResult::BehindCamera();
return TriangulationResult::BehindCamera();
}
}
result_ = TriangulationResult(pw_avg);
} // if retriangulate
}
if (retriangulate)
result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
params_.triangulation);
return result_;
}
/// triangulate
@ -570,6 +448,32 @@ public:
}
}
/**
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
*/
virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const
{
// when using stereo cameras, some of the measurements might be missing:
for(size_t i=0; i < cameras.size(); i++){
const StereoPoint2& z = measured_.at(i);
if(std::isnan(z.uR())) // if the right pixel is invalid
{
if(Fs){ // delete influence of right point on jacobian Fs
MatrixZD& Fi = Fs->at(i);
for(size_t ii=0; ii<Dim; ii++)
Fi(1,ii) = 0.0;
}
if(E) // delete influence of right point on jacobian E
E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
// set the corresponding entry of vector ue to zero
ue(ZDim * i + 1) = 0.0;
}
}
}
/** return the landmark */
TriangulationResult point() const {
return result_;

14
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -66,9 +66,9 @@ public:
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) :
Base(sharedNoiseModel, params) {
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const boost::optional<Pose3> body_P_sensor = boost::none) :
Base(sharedNoiseModel, params, body_P_sensor) {
}
/** Virtual destructor */
@ -102,7 +102,7 @@ public:
/**
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param K the (known) camera calibration (same for all measurements)
*/
@ -161,7 +161,11 @@ public:
Base::Cameras cameras;
size_t i=0;
for(const Key& k: this->keys_) {
const Pose3& pose = values.at<Pose3>(k);
Pose3 pose = values.at<Pose3>(k);
if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}

368
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -18,7 +18,7 @@
* @date Sept 2013
*/
// TODO #include <gtsam/slam/tests/smartFactorScenarios.h>
#include <gtsam/slam/tests/smartFactorScenarios.h>
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>
@ -33,8 +33,6 @@ using namespace boost::assign;
using namespace gtsam;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w = 640, h = 480;
static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
@ -62,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
static double missing_uR = std::numeric_limits<double>::quiet_NaN();
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -79,6 +79,35 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
LevenbergMarquardtParams lm_params;
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, params) {
SmartStereoProjectionParams p;
// check default values and "get"
EXPECT(p.getLinearizationMode() == HESSIAN);
EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY);
EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9);
EXPECT(p.getVerboseCheirality() == false);
EXPECT(p.getThrowCheirality() == false);
// check "set"
p.setLinearizationMode(JACOBIAN_SVD);
p.setDegeneracyMode(ZERO_ON_DEGENERACY);
p.setRankTolerance(100);
p.setEnableEPI(true);
p.setLandmarkDistanceThreshold(200);
p.setDynamicOutlierRejectionThreshold(3);
p.setRetriangulationThreshold(1e-2);
EXPECT(p.getLinearizationMode() == JACOBIAN_SVD);
EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY);
EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5);
EXPECT(p.getTriangulationParameters().enableEPI == true);
EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5);
EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5);
EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
@ -151,6 +180,60 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
//EXPECT(assert_equal(zero(4),actual,1e-8));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) {
// 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),
Point3(0, 0, 1));
StereoCamera 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));
StereoCamera level_camera_right(level_pose_right, K2);
// landmark ~5 meters in front of camera
Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate
StereoPoint2 level_uv = level_camera.project(landmark);
StereoPoint2 level_uv_right = level_camera_right.project(landmark);
StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v());
Values values;
values.insert(x1, level_pose);
values.insert(x2, level_pose_right);
SmartStereoProjectionPoseFactor factor1(model);
factor1.add(level_uv, x1, K2);
factor1.add(level_uv_right_missing, x2, K2);
double actualError = factor1.error(values);
double expectedError = 0.0;
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing:
SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
double actualError2 = factor1.totalReprojectionError(cameras);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
CameraSet<StereoCamera> cams;
cams += level_camera;
cams += level_camera_right;
TriangulationResult result = factor1.triangulateSafe(cams);
CHECK(result);
EXPECT(assert_equal(landmark, *result, 1e-7));
// TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing:
SmartStereoProjectionPoseFactor factor2(model);
StereoPoint2 level_uv_missing(level_uv.uL(),missing_uR,level_uv.v());
factor2.add(level_uv_missing, x1, K2);
factor2.add(level_uv_right_missing, x2, K2);
result = factor2.triangulateSafe(cams);
CHECK(result);
EXPECT(assert_equal(landmark, *result, 1e-7));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, noisy ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
@ -248,8 +331,6 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
@ -273,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7);
EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error
// get triangulated landmarks from smart factors
Point3 landmark1_smart = *smartFactor1->point();
@ -335,7 +416,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality));
// cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7);
EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7);
LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
Values result2 = optimizer2.optimize();
@ -344,7 +425,192 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl;
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
// camera has some displacement
Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.01, 0., -0.05), Point3(0.1, 0, 0.1));
// 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), Point3(0, 0, 1));
StereoCamera cam1(pose1.compose(body_P_sensor), K2);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2.compose(body_P_sensor), K2);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera cam3(pose3.compose(body_P_sensor), 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);
// 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
SmartStereoProjectionParams smart_params;
smart_params.triangulation.enableEPI = true;
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
smartFactor1->add(measurements_l1, views, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
smartFactor2->add(measurements_l2, views, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
smartFactor3->add(measurements_l3, views, 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), 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),
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);
EXPECT(
assert_equal(
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error
Values result;
gttic_(SmartStereoProjectionPoseFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
gttoc_(SmartStereoProjectionPoseFactor);
tictoc_finishedIteration_();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
// result.print("results of 3 camera, 3 landmark optimization \n");
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
// make a realistic calibration matrix
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam1(cameraPose1, *K); // with camera poses
SimpleCamera cam2(cameraPose2, *K);
SimpleCamera cam3(cameraPose3, *K);
// create arbitrary body_Pose_sensor (transforms from sensor to body)
Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
// These are the poses we want to estimate, from camera measurements
Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(5, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN)
vector<StereoPoint2> measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo;
for(size_t k=0; k<measurements_cam1.size();k++)
measurements_cam1_stereo.push_back(StereoPoint2(measurements_cam1[k].x() , missing_uR , measurements_cam1[k].y()));
for(size_t k=0; k<measurements_cam2.size();k++)
measurements_cam2_stereo.push_back(StereoPoint2(measurements_cam2[k].x() , missing_uR , measurements_cam2[k].y()));
for(size_t k=0; k<measurements_cam3.size();k++)
measurements_cam3_stereo.push_back(StereoPoint2(measurements_cam3[k].x() , missing_uR , measurements_cam3[k].y()));
SmartStereoProjectionParams params;
params.setRankTolerance(1.0);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
Cal3_S2Stereo::shared_ptr Kmono(new Cal3_S2Stereo(fov,w,h,b));
SmartStereoProjectionPoseFactor smartFactor1(model, params, sensor_to_body);
smartFactor1.add(measurements_cam1_stereo, views, Kmono);
SmartStereoProjectionPoseFactor smartFactor2(model, params, sensor_to_body);
smartFactor2.add(measurements_cam2_stereo, views, Kmono);
SmartStereoProjectionPoseFactor smartFactor3(model, params, sensor_to_body);
smartFactor3.add(measurements_cam3_stereo, views, Kmono);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// Put all factors in factor graph, adding priors
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
// Check errors at ground truth poses
Values gtValues;
gtValues.insert(x1, bodyPose1);
gtValues.insert(x2, bodyPose2);
gtValues.insert(x3, bodyPose3);
double actualError = graph.error(gtValues);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
Values values;
values.insert(x1, bodyPose1);
values.insert(x2, bodyPose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, bodyPose3*noise_pose);
LevenbergMarquardtParams lmParams;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
@ -411,6 +677,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
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), Point3(0, 0, 1));
StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2, K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera 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);
// 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
// DELETE SOME MEASUREMENTS
StereoPoint2 sp = measurements_cam1[1];
measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v());
sp = measurements_cam2[2];
measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v());
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, 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), 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),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3 * noise_pose);
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-7));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
@ -562,7 +900,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
// dynamic outlier rejection is off
EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9);
// Factors 1-3 should have valid point, factor 4 should not
EXPECT(smartFactor1->point());
@ -1039,7 +1377,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
vector<Key> views;
views.push_back(x1);
@ -1072,6 +1410,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
values);
// check that it is non degenerate
EXPECT(smartFactor->isValid());
Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues;
@ -1082,6 +1423,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues);
// check that it is non degenerate
EXPECT(smartFactor->isValid());
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(
assert_equal(hessianFactor->information(),
@ -1098,10 +1442,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactor->linearize(tranValues);
// Hessian is invariant to rotations and translations in the nondegenerate case
// Hessian is invariant to rotations and translations in the degenerate case
EXPECT(
assert_equal(hessianFactor->information(),
#ifdef GTSAM_USE_EIGEN_MKL
hessianFactorRotTran->information(), 1e-5));
#else
hessianFactorRotTran->information(), 1e-6));
#endif
}
/* ************************************************************************* */