Merge branch 'feature/heterogeneousSmartFactorNoise' into feature/improvementsIncrementalFilter
lcarlone
commit
fc799abad7
|
|
@ -2862,6 +2862,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testBatchFixedLagSmoother.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testBatchFixedLagSmoother.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,6 +35,16 @@
|
|||
|
||||
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
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Base class for smart factors
|
||||
* This base class has no internal point, but it has a measurement, noise model
|
||||
|
|
@ -202,7 +212,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 +220,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
|
||||
|
|
|
|||
|
|
@ -31,16 +31,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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -35,16 +35,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 stereo projection factors
|
||||
*/
|
||||
|
|
@ -119,8 +109,6 @@ enum DegeneracyMode {
|
|||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
|
||||
* This factor operates with StereoCamera. This factor requires that values
|
||||
|
|
@ -155,14 +143,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,77 +233,30 @@ 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);
|
||||
// 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++) {
|
||||
reprojections.push_back(cameras[i].backproject(measured_[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();
|
||||
if (retriangulate)
|
||||
result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
|
||||
params_.triangulation);
|
||||
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
|
||||
return result_;
|
||||
|
||||
}
|
||||
|
||||
/// triangulate
|
||||
bool triangulateForLinearize(const Cameras& cameras) const {
|
||||
triangulateSafe(cameras); // imperative, might reset result_
|
||||
|
|
@ -570,6 +516,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_;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
||||
|
|
@ -151,6 +151,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 +302,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 +325,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 +387,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 +396,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 +648,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 +871,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 +1348,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
||||
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(x1);
|
||||
|
|
@ -1072,6 +1381,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 +1394,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 +1413,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
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue