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

degenerate case in SmartProjectionFactor

release/4.3a0
Luca Carlone 2013-08-28 16:07:56 +00:00
parent 19ddf3f228
commit 633220a9dd
4 changed files with 102 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
gtsam/geometry/PinholeCamera.h
View File

@ -250,6 +250,20 @@ namespace gtsam {
return Point2(P.x() / P.z(), P.y() / P.z());
}
/**
* projects a 3-dimensional point at infinity (direction-only) in camera coordinates into the
* 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,
boost::optional<Matrix&> H1 = boost::none){
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
/// Project a point into the image and check depth
inline std::pair<Point2,bool> projectSafe(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw) ;
@ -296,6 +310,48 @@ namespace gtsam {
return pi;
}
/** project a point at infinity from world coordinate to the image
* @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf)
* @param H1 is the jacobian w.r.t. pose3
* @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,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
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) ;
return K_.uncalibrate(pn);
}
// world to camera coordinate
Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
const Point3 pc = pose_.rotation().unrotate(pw, Hc1, Hc2) ;
if( pc.z() <= 0 ) throw CheiralityException();
// camera to normalized image coordinate
Matrix Hn; // 2*3
const Point2 pn = project_point_at_infinity_to_camera(pc, Hn) ;
// uncalibration
Matrix Hk, Hi; // 2*2
Matrix H23 = Matrix::Zero(3,2);
H23.block(0,0,2,2) = Matrix::Identity(2,2);
const Point2 pi = K_.uncalibrate(pn, Hk, Hi);
// chain the jacobian matrices
const Matrix tmp = Hi*Hn;
if (H1) *H1 = tmp * Hc1;
if (H2) *H2 = tmp * Hc2 * H23;
if (H3) *H3 = Hk;
return pi;
}
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
* @param H1 is the jacobian w.r.t. [pose3 calibration]

2
gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp
View File

@ -285,7 +285,7 @@ int main(int argc, char** argv) {
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionFactorState> smartFactorState(new SmartProjectionFactorState());
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K, boost::none, boost::none, smartFactorState));
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K));
smartFactorStates.insert( make_pair(L(l), smartFactorState) );
smartFactors.insert( make_pair(L(l), smartFactor) );
graph.push_back(smartFactor);

59
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -45,9 +45,9 @@ namespace gtsam {
const SharedNoiseModel noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor)
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
boost::optional<Point3> point_;
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
boost::shared_ptr<SmartProjectionFactorState> state_;
boost::optional<Point3> point_;
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
@ -78,10 +78,9 @@ namespace gtsam {
*/
SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<LANDMARK> point = boost::none,
boost::optional<POSE> body_P_sensor = boost::none,
boost::shared_ptr<SmartProjectionFactorState> state = boost::shared_ptr<SmartProjectionFactorState>()) :
measured_(measured), noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor),
measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor),
state_(state), throwCheirality_(false), verboseCheirality_(false) {
keys_.assign(poseKeys.begin(), poseKeys.end());
}
@ -100,10 +99,9 @@ namespace gtsam {
SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<LANDMARK> point = boost::none,
boost::optional<POSE> body_P_sensor = boost::none,
boost::shared_ptr<SmartProjectionFactorState> state = boost::shared_ptr<SmartProjectionFactorState>()) :
measured_(measured), noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor),
measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor),
state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/**
@ -112,10 +110,9 @@ namespace gtsam {
* @param K shared pointer to the constant calibration
*/
SmartProjectionFactor(const SharedNoiseModel& model, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<LANDMARK> point = boost::none,
boost::optional<POSE> body_P_sensor = boost::none,
boost::shared_ptr<SmartProjectionFactorState> state = boost::shared_ptr<SmartProjectionFactorState>()) :
noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor), state_(state) {
noise_(model), K_(K), body_P_sensor_(body_P_sensor), state_(state) {
}
/** Virtual destructor */
@ -195,13 +192,8 @@ namespace gtsam {
}
// We triangulate the 3D position of the landmark
Point3 point;
try {
if (point_) {
point = *point_;
} else {
point = triangulatePoint3(cameraPoses, measured_, *K_);
}
point_ = triangulatePoint3(cameraPoses, measured_, *K_);
} catch( TriangulationCheiralityException& e) {
// point is behind one of the cameras, turn factor off by setting everything to 0
//std::cout << e.what() << std::end;
@ -210,6 +202,12 @@ 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);
@ -219,7 +217,7 @@ namespace gtsam {
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_);
b.at(i) = - ( camera.project(point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
b.at(i) = - ( camera.project(point_,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i));
f += b.at(i).squaredNorm();
}
@ -266,10 +264,28 @@ namespace gtsam {
if (blockwise == false){ // version with full matrix multiplication
// ==========================================================================================================
Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
Matrix Hl2 = zeros(2 * numKeys, 3);
Matrix Hl2 = zeros(2 * numKeys, dim_landmark);
Vector b2 = zero(2 * numKeys);
if(degenerate){
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();
noise_-> WhitenSystem(Hxi, Hli, bi);
f += bi.squaredNorm();
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
Hl2.block( 2*i, 0, 2, 2 ) = Hli;
subInsert(b2,bi,2*i);
}
}
else{
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_);
@ -283,7 +299,7 @@ namespace gtsam {
Hl2.block( 2*i, 0, 2, 3 ) = Hli;
subInsert(b2,bi,2*i);
}
}
// Shur complement trick
@ -334,13 +350,8 @@ namespace gtsam {
}
// We triangulate the 3D position of the landmark
Point3 point;
try {
if (point_) {
point = *point_;
} else {
point = triangulatePoint3(cameraPoses, measured_, *K_);
}
point_ = triangulatePoint3(cameraPoses, measured_, *K_);
} catch( TriangulationCheiralityException& e) {
// point is behind one of the cameras, turn factor off by setting everything to 0
//std::cout << e.what() << std::end;
@ -351,7 +362,7 @@ namespace gtsam {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point) - measured_.at(i));
Point2 reprojectionError(camera.project(point_) - measured_.at(i));
overallError += noise_->distance( reprojectionError.vector() );
}
return overallError;
@ -370,7 +381,7 @@ namespace gtsam {
return noise_;
}
/** return the noise landmark */
/** return the landmark */
boost::optional<Point3> point() const {
return point_;
}

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

@ -167,7 +167,7 @@ TEST( SmartProjectionFactor, noisy ){
// DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
/* ************************************************************************* */
/* *************************************************************************
TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl;