degenerate case in smart vision factor
Luca Carlone
parent
633220a9dd
commit
0422b9cfef
|
|
@ -255,7 +255,7 @@ namespace gtsam {
|
|||
* camera and returns a 2-dimensional point, no calibration applied
|
||||
* With optional 2by3 derivative
|
||||
*/
|
||||
inline static Point2 project_point_at_infinity_to_camera(const Point3& P,
|
||||
inline static Point2 projectPointAtInfinityToCamera(const Point3& P,
|
||||
boost::optional<Matrix&> H1 = boost::none){
|
||||
if (H1) {
|
||||
double d = 1.0 / P.z(), d2 = d * d;
|
||||
|
|
@ -316,7 +316,7 @@ namespace gtsam {
|
|||
* @param H2 is the jacobian w.r.t. point3
|
||||
* @param H3 is the jacobian w.r.t. calibration
|
||||
*/
|
||||
inline Point2 project_point_at_infinity(const Point3& pw,
|
||||
inline Point2 projectPointAtInfinity(const Point3& pw,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none) const {
|
||||
|
|
@ -324,7 +324,7 @@ namespace gtsam {
|
|||
if (!H1 && !H2 && !H3) {
|
||||
const Point3 pc = pose_.rotation().unrotate(pw) ;
|
||||
if ( pc.z() <= 0 ) throw CheiralityException();
|
||||
const Point2 pn = project_point_at_infinity_to_camera(pc) ;
|
||||
const Point2 pn = projectPointAtInfinityToCamera(pc) ;
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
|
|
@ -335,7 +335,7 @@ namespace gtsam {
|
|||
|
||||
// camera to normalized image coordinate
|
||||
Matrix Hn; // 2*3
|
||||
const Point2 pn = project_point_at_infinity_to_camera(pc, Hn) ;
|
||||
const Point2 pn = projectPointAtInfinityToCamera(pc, Hn) ;
|
||||
|
||||
// uncalibration
|
||||
Matrix Hk, Hi; // 2*2
|
||||
|
|
@ -386,6 +386,13 @@ namespace gtsam {
|
|||
return pose_.transform_from(pc);
|
||||
}
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
|
||||
inline Point3 backprojectPointAtInfinity(const Point2& p) const {
|
||||
const Point2 pn = K_.calibrate(p);
|
||||
const Point3 pc(pn.x(), pn.y(), 1.0);
|
||||
return pose_.rotation().rotate(pc);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate range to a landmark
|
||||
* @param point 3D location of landmark
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ namespace gtsam {
|
|||
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
|
||||
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
|
||||
boost::shared_ptr<SmartProjectionFactorState> state_;
|
||||
boost::optional<Point3> point_;
|
||||
mutable Point3 point_;
|
||||
|
||||
// verbosity handling for Cheirality Exceptions
|
||||
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
|
||||
|
|
@ -175,6 +175,8 @@ namespace gtsam {
|
|||
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
|
||||
|
||||
bool blockwise = false;
|
||||
bool degenerate = false;
|
||||
int dim_landmark = 3;
|
||||
|
||||
unsigned int numKeys = keys_.size();
|
||||
std::vector<Index> js;
|
||||
|
|
@ -194,6 +196,11 @@ namespace gtsam {
|
|||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
point_ = triangulatePoint3(cameraPoses, measured_, *K_);
|
||||
} catch( TriangulationUnderconstrainedException& e) {
|
||||
// point is triangulated at infinity
|
||||
//std::cout << e.what() << std::end;
|
||||
degenerate = true;
|
||||
dim_landmark = 2;
|
||||
} catch( TriangulationCheiralityException& e) {
|
||||
// point is behind one of the cameras, turn factor off by setting everything to 0
|
||||
//std::cout << e.what() << std::end;
|
||||
|
|
@ -202,12 +209,6 @@ namespace gtsam {
|
|||
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
|
||||
}
|
||||
|
||||
bool degenerate = true;
|
||||
|
||||
int dim_landmark = 2;
|
||||
|
||||
|
||||
|
||||
if (blockwise){
|
||||
// ==========================================================================================================
|
||||
std::vector<Matrix> Hx(numKeys);
|
||||
|
|
@ -273,8 +274,12 @@ namespace gtsam {
|
|||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||
if(i==0){ // first pose
|
||||
point_ = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
|
||||
std::cout << "point_ " << point_<< std::endl;
|
||||
}
|
||||
Matrix Hxi, Hli;
|
||||
Vector bi = -( camera.project(point,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
Vector bi = -( camera.projectPointAtInfinity(point_,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
|
||||
noise_-> WhitenSystem(Hxi, Hli, bi);
|
||||
f += bi.squaredNorm();
|
||||
|
|
@ -286,11 +291,12 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
else{
|
||||
std::cout << "non degenerate " << point_<< std::endl;
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||
Matrix Hxi, Hli;
|
||||
Vector bi = -( camera.project(point,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
Vector bi = -( camera.project(point_,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
|
||||
noise_-> WhitenSystem(Hxi, Hli, bi);
|
||||
f += bi.squaredNorm();
|
||||
|
|
@ -338,6 +344,9 @@ namespace gtsam {
|
|||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
double overallError=0;
|
||||
bool degenerate = false;
|
||||
|
||||
std::cout << "evaluating error in smart factor " << std::endl;
|
||||
|
||||
// Collect all poses (Cameras)
|
||||
std::vector<Pose3> cameraPoses;
|
||||
|
|
@ -351,22 +360,44 @@ namespace gtsam {
|
|||
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
point_ = triangulatePoint3(cameraPoses, measured_, *K_);
|
||||
point_ = triangulatePoint3(cameraPoses, measured_, *K_);
|
||||
} catch( TriangulationCheiralityException& e) {
|
||||
std::cout << "TriangulationCheiralityException " << std::endl;
|
||||
// point is behind one of the cameras, turn factor off by setting everything to 0
|
||||
//std::cout << e.what() << std::end;
|
||||
return 0.0;
|
||||
} catch( TriangulationUnderconstrainedException& e) {
|
||||
// point is triangulated at infinity
|
||||
//std::cout << e.what() << std::endl;
|
||||
degenerate = true;
|
||||
}
|
||||
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||
std::cout << "degenerate " << degenerate << std::endl;
|
||||
|
||||
Point2 reprojectionError(camera.project(point_) - measured_.at(i));
|
||||
overallError += noise_->distance( reprojectionError.vector() );
|
||||
if(degenerate){
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||
if(i==0){ // first pose
|
||||
point_ = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
|
||||
std::cout << "point_ " << point_<< std::endl;
|
||||
}
|
||||
Point2 reprojectionError(camera.projectPointAtInfinity(point_) - measured_.at(i));
|
||||
overallError += noise_->distance( reprojectionError.vector() );
|
||||
}
|
||||
return overallError;
|
||||
}
|
||||
return overallError;
|
||||
} else {
|
||||
else{
|
||||
for(size_t i = 0; i < measured_.size(); i++) {
|
||||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||
|
||||
Point2 reprojectionError(camera.project(point_) - measured_.at(i));
|
||||
overallError += noise_->distance( reprojectionError.vector() );
|
||||
}
|
||||
return overallError;
|
||||
}
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam/slam/PoseTranslationPrior.h>
|
||||
#include <gtsam_unstable/slam/SmartProjectionFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
|
@ -41,6 +42,7 @@
|
|||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
|
||||
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -85,7 +87,7 @@ TEST( SmartProjectionFactor, ConstructorWithTransform) {
|
|||
measurements.push_back(Point2(323.0, 240.0));
|
||||
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
|
||||
TestSmartProjectionFactor factor(measurements, model, views, K, boost::none, body_P_sensor);
|
||||
TestSmartProjectionFactor factor(measurements, model, views, K, body_P_sensor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -111,8 +113,8 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) {
|
|||
|
||||
std::vector<Key> views;
|
||||
views += X(1);
|
||||
TestSmartProjectionFactor factor1(measurements, model, views, K, boost::none, body_P_sensor);
|
||||
TestSmartProjectionFactor factor2(measurements, model, views, K, boost::none, body_P_sensor);
|
||||
TestSmartProjectionFactor factor1(measurements, model, views, K, body_P_sensor);
|
||||
TestSmartProjectionFactor factor2(measurements, model, views, K, body_P_sensor);
|
||||
|
||||
CHECK(assert_equal(factor1, factor2));
|
||||
}
|
||||
|
|
@ -581,6 +583,102 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){
|
|||
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
|
||||
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
|
||||
|
||||
Symbol x1('X', 1);
|
||||
Symbol x2('X', 2);
|
||||
Symbol x3('X', 3);
|
||||
|
||||
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
|
||||
|
||||
std::vector<Key> views;
|
||||
views += x1, x2, x3;
|
||||
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
|
||||
// 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 = Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)) * pose1;
|
||||
SimpleCamera cam2(pose2, *K);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)) * pose2;
|
||||
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 += cam1_uv1, cam2_uv1, cam3_uv1;
|
||||
|
||||
//
|
||||
Point2 cam1_uv2 = cam1.project(landmark2);
|
||||
Point2 cam2_uv2 = cam2.project(landmark2);
|
||||
Point2 cam3_uv2 = cam3.project(landmark2);
|
||||
measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2;
|
||||
|
||||
|
||||
Point2 cam1_uv3 = cam1.project(landmark3);
|
||||
Point2 cam2_uv3 = cam2.project(landmark3);
|
||||
Point2 cam3_uv3 = cam3.project(landmark3);
|
||||
measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3;
|
||||
|
||||
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K));
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K));
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, 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));
|
||||
graph.print();
|
||||
// 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);
|
||||
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
|
||||
params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
gttic_(SmartProjectionFactor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartProjectionFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
// result.print("results of 3 camera, 3 landmark optimization \n");
|
||||
result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
tictoc_print_();
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue