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

degenerate case in smart vision factor

release/4.3a0
Luca Carlone 2013-08-28 16:07:58 +00:00
parent 633220a9dd
commit 0422b9cfef
3 changed files with 160 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/PinholeCamera.h
View File

@ -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

51
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -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;
@ -353,11 +362,32 @@ namespace gtsam {
try {
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;
}
std::cout << "degenerate " << degenerate << std::endl;
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;
}
else{
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_);
@ -366,7 +396,8 @@ namespace gtsam {
overallError += noise_->distance( reprojectionError.vector() );
}
return overallError;
} else {
}
} else { // else of active flag
return 0.0;
}
}

104
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
View File

@ -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_();
}
/* ************************************************************************* */