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

Working version of rotation only smart factor

release/4.3a0
Luca Carlone 2013-08-28 17:37:58 +00:00
parent df36ee643a
commit 141958123a
3 changed files with 118 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/PinholeCamera.h
View File

@ -329,10 +329,13 @@ namespace gtsam {
}
// world to camera coordinate
Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
const Point3 pc = pose_.rotation().unrotate(pw, Hc1, Hc2) ;
Matrix Hc1_rot /* 3*3 */, Hc2 /* 3*3 */ ;
const Point3 pc = pose_.rotation().unrotate(pw, Hc1_rot, Hc2) ;
if( pc.z() <= 0 ) throw CheiralityException();
Matrix Hc1 = Matrix::Zero(3,6);
Hc1.block(0,0,3,3) = Hc1_rot;
// camera to normalized image coordinate
Matrix Hn; // 2*3
const Point2 pn = projectPointAtInfinityToCamera(pc, Hn) ;

26
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -206,9 +206,13 @@ namespace gtsam {
//std::cout << e.what() << std::end;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
//return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
// TODO: this is a debug condition, should be removed the comment
}
degenerate = true; // TODO: this is a debug condition, should be removed
dim_landmark = 2; // TODO: this is a debug condition, should be removed the comment
if (blockwise){
// ==========================================================================================================
std::vector<Matrix> Hx(numKeys);
@ -265,7 +269,6 @@ namespace gtsam {
if (blockwise == false){ // version with full matrix multiplication
// ==========================================================================================================
Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
Matrix Hl2 = zeros(2 * numKeys, dim_landmark);
Vector b2 = zero(2 * numKeys);
@ -276,10 +279,12 @@ namespace gtsam {
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;
// std::cout << "point_ " << point_<< std::endl;
}
Matrix Hxi, Hli;
Vector bi = -( camera.projectPointAtInfinity(point_,Hxi,Hli) - measured_.at(i) ).vector();
// std::cout << "Hxi \n" << Hxi<< std::endl;
// std::cout << "Hli \n" << Hli<< std::endl;
noise_-> WhitenSystem(Hxi, Hli, bi);
f += bi.squaredNorm();
@ -289,6 +294,8 @@ namespace gtsam {
subInsert(b2,bi,2*i);
}
// std::cout << "Hx2 \n" << Hx2<< std::endl;
// std::cout << "Hl2 \n" << Hl2<< std::endl;
}
else{
std::cout << "non degenerate " << point_<< std::endl;
@ -308,14 +315,16 @@ namespace gtsam {
}
}
std::cout << "dim_landmark " << dim_landmark << std::endl;
// Shur complement trick
Matrix H(6 * numKeys, 6 * numKeys);
Matrix3 C2 = (Hl2.transpose() * Hl2).inverse();
std::cout << "Hl2.transpose() * Hl2 \n" << Hl2.transpose() * Hl2 << std::endl;
Matrix C2 = (Hl2.transpose() * Hl2).inverse();
std::cout << "C2 \n" << C2.size() << std::endl;
H = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2))));
Vector gs_vector = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2))));
// Populate Gs and gs
int GsCount2 = 0;
for(size_t i1 = 0; i1 < numKeys; i1++) {
@ -362,17 +371,17 @@ namespace gtsam {
try {
point_ = triangulatePoint3(cameraPoses, measured_, *K_);
} catch( TriangulationCheiralityException& e) {
std::cout << "TriangulationCheiralityException " << std::endl;
// 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;
// return 0.0; // TODO: this is a debug condition, should be removed the comment
} catch( TriangulationUnderconstrainedException& e) {
// point is triangulated at infinity
//std::cout << e.what() << std::endl;
degenerate = true;
}
std::cout << "degenerate " << degenerate << std::endl;
degenerate = true; // TODO: this is a debug condition, should be removed
if(degenerate){
for(size_t i = 0; i < measured_.size(); i++) {
@ -380,7 +389,6 @@ namespace gtsam {
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() );

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

@ -120,7 +120,7 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) {
}
/* ************************************************************************* */
/* *************************************************************************
TEST( SmartProjectionFactor, noisy ){
cout << " ************************ SmartProjectionFactor: noisy ****************************" << endl;
@ -311,7 +311,7 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
}
/* ************************************************************************* */
/* *************************************************************************
TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
@ -405,7 +405,7 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
}
/* ************************************************************************* */
/* *************************************************************************
TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
@ -510,7 +510,7 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){
}
/* ************************************************************************* */
/* *************************************************************************
TEST( SmartProjectionFactor, 3poses_projection_factor ){
// cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
@ -583,7 +583,94 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){
}
/* *************************************************************************
/* ************************************************************************* */
TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 2 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 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
SimpleCamera cam2(pose2, *K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
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);
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;
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));
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(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2*noise_pose);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, pose3*noise_pose*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_();
}
/* ************************************************************************* */
TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
@ -603,11 +690,11 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
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;
Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
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;
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
SimpleCamera cam3(pose3, *K);
// three landmarks ~5 meters infront of camera
@ -652,7 +739,7 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
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;
@ -677,11 +764,10 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
tictoc_print_();
}
/* ************************************************************************* */
/* *************************************************************************
TEST( SmartProjectionFactor, Hessian ){
cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl;