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

Initial implementation of selective relinearization

release/4.3a0
Luca Carlone 2013-09-20 20:19:58 +00:00
parent e1ef219916
commit 9f68c04792
2 changed files with 102 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

154
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -68,6 +68,10 @@ namespace gtsam {
std::vector<Matrix> Gs; std::vector<Matrix> Gs;
std::vector<Vector> gs; std::vector<Vector> gs;
Matrix Hx;
Matrix Hl;
Vector b;
// C = Hl'Hl // C = Hl'Hl
// Cinv = inv(Hl'Hl) // Cinv = inv(Hl'Hl)
@ -234,6 +238,9 @@ namespace gtsam {
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(numKeys);
double f=0; double f=0;
Vector changeLinPoses(numKeys*6);
Point3 changeLinPoint;
// Collect all poses (Cameras) // Collect all poses (Cameras)
bool valuesEqualRetriangulation = true; bool valuesEqualRetriangulation = true;
std::vector<Pose3> cameraPoses; std::vector<Pose3> cameraPoses;
@ -247,11 +254,17 @@ namespace gtsam {
cameraPose = values.at<Pose3>(k); cameraPose = values.at<Pose3>(k);
if (!state_->cameraPosesTriangulation.empty()) { if (!state_->cameraPosesTriangulation.empty()) {
// TODO: are you sure that when using "add" the number of poses will be ok? (old linearization point will contain 1 pose less)
if (!cameraPose.equals(state_->cameraPosesTriangulation[poseCount], retriangulationThreshold)) { if (!cameraPose.equals(state_->cameraPosesTriangulation[poseCount], retriangulationThreshold)) {
valuesEqualRetriangulation = false; valuesEqualRetriangulation = false;
subInsert(changeLinPoses, Vector::Zero(6), 6*poseCount);
}else{
Vector changeLinPoses_i = Pose3::Logmap(state_->cameraPosesTriangulation[poseCount].between(cameraPose));
subInsert(changeLinPoses, changeLinPoses_i, 6*poseCount);
} }
} else { } else {
valuesEqualRetriangulation = false; valuesEqualRetriangulation = false;
subInsert(changeLinPoses, Vector::Zero(6), 6*poseCount);
} }
cameraPoses.push_back(cameraPose); cameraPoses.push_back(cameraPose);
@ -267,7 +280,9 @@ namespace gtsam {
if (retriangulate) { if (retriangulate) {
// We triangulate the 3D position of the landmark // We triangulate the 3D position of the landmark
try { try {
state_->point = triangulatePoint3(cameraPoses, measured_, *K_); Point3 newPoint = triangulatePoint3(cameraPoses, measured_, *K_);
changeLinPoint = newPoint - state_->point; // TODO: implement this check for the degenerate case
state_->point = newPoint;
state_->degenerate = false; state_->degenerate = false;
state_->cheiralityException = false; state_->cheiralityException = false;
} catch( TriangulationUnderconstrainedException& e) { } catch( TriangulationUnderconstrainedException& e) {
@ -300,56 +315,58 @@ namespace gtsam {
if (blockwise){ if (blockwise){
// ========================================================================================================== // ==========================================================================================================
std::vector<Matrix> Hx(numKeys); std::cout << "Deprecated use of blockwise version. This is slower and no longer supported" << std::endl;
std::vector<Matrix> Hl(numKeys); blockwise = false;
std::vector<Vector> b(numKeys); // std::vector<Matrix> Hx(numKeys);
// std::vector<Matrix> Hl(numKeys);
for(size_t i = 0; i < measured_.size(); i++) { // std::vector<Vector> b(numKeys);
Pose3 pose = cameraPoses.at(i); //
PinholeCamera<CALIBRATION> camera(pose, *K_); // for(size_t i = 0; i < measured_.size(); i++) {
b.at(i) = - ( camera.project(state_->point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); // Pose3 pose = cameraPoses.at(i);
noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i)); // PinholeCamera<CALIBRATION> camera(pose, *K_);
f += b.at(i).squaredNorm(); // b.at(i) = - ( camera.project(state_->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();
// Shur complement trick // }
//
// Allocate m^2 matrix blocks // // Shur complement trick
std::vector< std::vector<Matrix> > Hxl(keys_.size(), std::vector<Matrix>( keys_.size())); //
// // Allocate m^2 matrix blocks
// Allocate inv(Hl'Hl) // std::vector< std::vector<Matrix> > Hxl(keys_.size(), std::vector<Matrix>( keys_.size()));
Matrix3 C = zeros(3,3); //
for(size_t i1 = 0; i1 < keys_.size(); i1++) { // // Allocate inv(Hl'Hl)
C.noalias() += Hl.at(i1).transpose() * Hl.at(i1); // Matrix3 C = zeros(3,3);
} // for(size_t i1 = 0; i1 < keys_.size(); i1++) {
// C.noalias() += Hl.at(i1).transpose() * Hl.at(i1);
Matrix3 Cinv = C.inverse(); // this is very important: without eval, because of eigen aliasing the results will be incorrect // }
//
// Calculate sub blocks // Matrix3 Cinv = C.inverse(); // this is very important: without eval, because of eigen aliasing the results will be incorrect
for(size_t i1 = 0; i1 < keys_.size(); i1++) { //
for(size_t i2 = 0; i2 < keys_.size(); i2++) { // // Calculate sub blocks
// we only need the upper triangular entries // for(size_t i1 = 0; i1 < keys_.size(); i1++) {
Hxl[i1][i2].noalias() = Hx.at(i1).transpose() * Hl.at(i1) * Cinv * Hl.at(i2).transpose(); // for(size_t i2 = 0; i2 < keys_.size(); i2++) {
} // // we only need the upper triangular entries
} // Hxl[i1][i2].noalias() = Hx.at(i1).transpose() * Hl.at(i1) * Cinv * Hl.at(i2).transpose();
// Populate Gs and gs // }
int GsCount = 0; // }
for(size_t i1 = 0; i1 < numKeys; i1++) { // // Populate Gs and gs
gs.at(i1).noalias() = Hx.at(i1).transpose() * b.at(i1); // int GsCount = 0;
// for(size_t i1 = 0; i1 < numKeys; i1++) {
for(size_t i2 = 0; i2 < numKeys; i2++) { // gs.at(i1).noalias() = Hx.at(i1).transpose() * b.at(i1);
gs.at(i1).noalias() -= Hxl[i1][i2] * b.at(i2); //
// for(size_t i2 = 0; i2 < numKeys; i2++) {
if (i2 == i1){ // gs.at(i1).noalias() -= Hxl[i1][i2] * b.at(i2);
Gs.at(GsCount).noalias() = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); //
GsCount++; // if (i2 == i1){
} // Gs.at(GsCount).noalias() = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
if (i2 > i1) { // GsCount++;
Gs.at(GsCount).noalias() = - Hxl[i1][i2] * Hx.at(i2); // }
GsCount++; // if (i2 > i1) {
} // Gs.at(GsCount).noalias() = - Hxl[i1][i2] * Hx.at(i2);
} // GsCount++;
} // }
// }
// }
} }
if (blockwise == false){ // version with full matrix multiplication if (blockwise == false){ // version with full matrix multiplication
@ -383,11 +400,41 @@ namespace gtsam {
// std::cout << "Hl2 \n" << Hl2<< std::endl; // std::cout << "Hl2 \n" << Hl2<< std::endl;
} }
else{ else{
for(size_t i = 0; i < measured_.size(); i++) { for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i); Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_); PinholeCamera<CALIBRATION> camera(pose, *K_);
Matrix Hxi, Hli; Matrix Hxi, Hli;
// Selective relinearization (check if new linearization is needed)
Vector repErr_i;
try {
repErr_i = - ( camera.project(state_->point) - measured_.at(i) ).vector();
} catch ( CheiralityException& e) {
std::cout << "Cheirality exception " << state_->ID << std::endl;
exit(EXIT_FAILURE);
}
noise_-> whitenInPlace(repErr_i);
f += repErr_i.squaredNorm();
Vector linRepErr;
linRepErr = state_->Hx * changeLinPoses + state_->Hl * changeLinPoint.vector() - state_->b;
double f_lin = linRepErr.squaredNorm();
// Relinearization check
if (state_->f - f_lin > 1e-7){
double rho = (state_->f - f) / (state_->f - f_lin);
if( fabs(rho) > 0.75 ){
return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
}
}
else{
return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
}
Vector bi; Vector bi;
try { try {
bi = -( camera.project(state_->point,Hxi,Hli) - measured_.at(i) ).vector(); bi = -( camera.project(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
@ -395,9 +442,7 @@ namespace gtsam {
std::cout << "Cheirality exception " << state_->ID << std::endl; std::cout << "Cheirality exception " << state_->ID << std::endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
noise_-> WhitenSystem(Hxi, Hli, bi); noise_-> WhitenSystem(Hxi, Hli, bi);
f += bi.squaredNorm();
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
Hl2.block( 2*i, 0, 2, 3 ) = Hli; Hl2.block( 2*i, 0, 2, 3 ) = Hli;
@ -406,6 +451,9 @@ namespace gtsam {
} }
} }
state_->Hx = Hx2;
state_->Hl = Hl2;
state_->b = b2;
// Shur complement trick // Shur complement trick
Matrix H(6 * numKeys, 6 * numKeys); Matrix H(6 * numKeys, 6 * numKeys);

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

@ -670,7 +670,7 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor
tictoc_print_(); tictoc_print_();
} }
/* ************************************************************************* */ /* *************************************************************************
TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;