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

Incremental mode in Smart Factors (not working yet with standard ProjectionFactors)

release/4.3a0
Luca Carlone 2013-10-06 18:17:32 +00:00
parent f65740e79e
commit d049dd38c6
3 changed files with 21 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/PinholeCamera.h
View File

@ -323,7 +323,7 @@ namespace gtsam {
if (!H1 && !H2 && !H3) { if (!H1 && !H2 && !H3) {
const Point3 pc = pose_.rotation().unrotate(pw) ; const Point3 pc = pose_.rotation().unrotate(pw) ;
if ( pc.z() <= 0 ) throw CheiralityException(); // if ( pc.z() <= 0 ) throw CheiralityException(); // this does not make sense for point at infinity
const Point2 pn = projectPointAtInfinityToCamera(pc) ; const Point2 pn = projectPointAtInfinityToCamera(pc) ;
return K_.uncalibrate(pn); return K_.uncalibrate(pn);
} }
@ -331,7 +331,7 @@ namespace gtsam {
// world to camera coordinate // world to camera coordinate
Matrix Hc1_rot /* 3*3 */, Hc2 /* 3*3 */ ; Matrix Hc1_rot /* 3*3 */, Hc2 /* 3*3 */ ;
const Point3 pc = pose_.rotation().unrotate(pw, Hc1_rot, Hc2) ; const Point3 pc = pose_.rotation().unrotate(pw, Hc1_rot, Hc2) ;
if( pc.z() <= 0 ) throw CheiralityException(); // if( pc.z() <= 0 ) throw CheiralityException(); // this does not make sense for point at infinity
Matrix Hc1 = Matrix::Zero(3,6); Matrix Hc1 = Matrix::Zero(3,6);
Hc1.block(0,0,3,3) = Hc1_rot; Hc1.block(0,0,3,3) = Hc1_rot;
@ -342,14 +342,14 @@ namespace gtsam {
// uncalibration // uncalibration
Matrix Hk, Hi; // 2*2 Matrix Hk, Hi; // 2*2
Matrix H23 = Matrix::Zero(3,2); // Matrix H23 = Matrix::Zero(3,2);
H23.block(0,0,2,2) = Matrix::Identity(2,2); // H23.block(0,0,2,2) = Matrix::Identity(2,2);
const Point2 pi = K_.uncalibrate(pn, Hk, Hi); const Point2 pi = K_.uncalibrate(pn, Hk, Hi);
// chain the jacobian matrices // chain the jacobian matrices
const Matrix tmp = Hi*Hn; const Matrix tmp = Hi*Hn;
if (H1) *H1 = tmp * Hc1; if (H1) *H1 = tmp * Hc1;
if (H2) *H2 = tmp * Hc2 * H23; if (H2) *H2 = (tmp * Hc2).block(0,0,3,2);
if (H3) *H3 = Hk; if (H3) *H3 = Hk;
return pi; return pi;
} }

17
gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp
View File

@ -149,7 +149,7 @@ Cal3_S2::shared_ptr loadCalibration(const string& filename) {
} }
void writeValues(string directory_, const Values& values){ void writeValues(string directory_, const Values& values){
string filename = directory_ + "camera_poses.txt"; string filename = directory_ + "out_camera_poses.txt";
ofstream fout; ofstream fout;
fout.open(filename.c_str()); fout.open(filename.c_str());
fout.precision(20); fout.precision(20);
@ -194,6 +194,9 @@ void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared
ProjectionFactorMap::iterator pfit; ProjectionFactorMap::iterator pfit;
if (debug) graphValues->print("graphValues \n");
if (debug) std::cout << " # END VALUES: " << std::endl;
// Iterate through all landmarks // Iterate through all landmarks
if (debug) std::cout << " PROJECTION FACTOR GROUPED: " << projectionFactors.size(); if (debug) std::cout << " PROJECTION FACTOR GROUPED: " << projectionFactors.size();
int numProjectionFactors = 0; int numProjectionFactors = 0;
@ -216,7 +219,6 @@ void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared
// Iterate through poses // Iterate through poses
cameraPoses.push_back( loadedValues->at<Pose3>((*vfit)->key1() ) ); cameraPoses.push_back( loadedValues->at<Pose3>((*vfit)->key1() ) );
measured.push_back( (*vfit)->measured() ); measured.push_back( (*vfit)->measured() );
} }
// Triangulate landmark based on set of poses and measurements // Triangulate landmark based on set of poses and measurements
@ -263,7 +265,6 @@ void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared
graphValues->insert( projectionFactorVector[0]->key2(), point); // add point; graphValues->insert( projectionFactorVector[0]->key2(), point); // add point;
landmarkKeys.push_back( projectionFactorVector[0]->key2() ); landmarkKeys.push_back( projectionFactorVector[0]->key2() );
} }
if (1||debug) std::cout << " # PROJECTION FACTORS CALCULATED: " << numProjectionFactors; if (1||debug) std::cout << " # PROJECTION FACTORS CALCULATED: " << numProjectionFactors;
if (1||debug) std::cout << " # PROJECTION FACTORS ADDED: " << numProjectionFactorsAdded; if (1||debug) std::cout << " # PROJECTION FACTORS ADDED: " << numProjectionFactorsAdded;
@ -370,8 +371,8 @@ int main(int argc, char** argv) {
bool useLM = true; bool useLM = true;
int landmarkFirstOrderingMethod = 2; // 0 - COLAMD, 1 - landmark first, poses from smart factor, 2 - landmark first through constrained ordering int landmarkFirstOrderingMethod = 2; // 0 - COLAMD, 1 - landmark first, poses from smart factor, 2 - landmark first through constrained ordering
double KittiLinThreshold = -1; // 1e-7; // 0.01 double KittiLinThreshold = -1.0; // 0.005; //
double KittiRankTolerance = 1; double KittiRankTolerance = 1.0;
bool incrementalFlag = false; bool incrementalFlag = false;
int optSkip = 200; // we optimize the graph every optSkip poses int optSkip = 200; // we optimize the graph every optSkip poses
@ -379,6 +380,7 @@ int main(int argc, char** argv) {
std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl; std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl;
std::cout << "PARAM Triangulation: " << useTriangulation << std::endl; std::cout << "PARAM Triangulation: " << useTriangulation << std::endl;
std::cout << "PARAM LM: " << useLM << std::endl; std::cout << "PARAM LM: " << useLM << std::endl;
std::cout << "PARAM KittiLinThreshold (negative is disabled): " << KittiLinThreshold << std::endl;
// Get home directory and dataset // Get home directory and dataset
string HOME = getenv("HOME"); string HOME = getenv("HOME");
@ -446,6 +448,8 @@ int main(int argc, char** argv) {
if (debug) cout << "Adding triangulated landmarks, graph size after: " << graphProjection.size() << endl; if (debug) cout << "Adding triangulated landmarks, graph size after: " << graphProjection.size() << endl;
if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
// Optimize every optSkip poses if we want to do incremental inference
if (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0 ){
// optimize // optimize
if (useLM) if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
@ -589,7 +593,7 @@ int main(int argc, char** argv) {
// Add projection factor to graph // Add projection factor to graph
graphProjection.push_back(projectionFactor); graphProjection.push_back(projectionFactor);
} else { }else {
// Alternatively: Triangulate similar to how SmartProjectionFactor does it // Alternatively: Triangulate similar to how SmartProjectionFactor does it
// We only do this at the end, when all of the camera poses are available // We only do this at the end, when all of the camera poses are available
// Note we do not add anything to the graph until then, since in some cases // Note we do not add anything to the graph until then, since in some cases
@ -616,7 +620,6 @@ int main(int argc, char** argv) {
if (useSmartProjectionFactor == false && useTriangulation) { if (useSmartProjectionFactor == false && useTriangulation) {
addTriangulatedLandmarks(graphSmart, loadedValues, graphSmartValues, K, projectionFactors, cameraPoseKeys, landmarkKeys); addTriangulatedLandmarks(graphSmart, loadedValues, graphSmartValues, K, projectionFactors, cameraPoseKeys, landmarkKeys);
} }
if (useLM) if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
else else

7
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -190,6 +190,8 @@ namespace gtsam {
const std::vector<Point2> measured, const std::vector<Point2> measured,
const SharedNoiseModel& model, const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K, const boost::shared_ptr<CALIBRATION>& K,
const double rankTol,
const double linThreshold,
bool throwCheirality, bool verboseCheirality, bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none, boost::optional<POSE> body_P_sensor = boost::none,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
@ -269,7 +271,7 @@ namespace gtsam {
Pose3 localCameraPose = firstCameraPose.between(cameraPoses[i]); Pose3 localCameraPose = firstCameraPose.between(cameraPoses[i]);
Pose3 localCameraPoseOld = firstCameraPoseOld.between(oldPoses[i]); Pose3 localCameraPoseOld = firstCameraPoseOld.between(oldPoses[i]);
if (!cameraPoses[i].equals(oldPoses[i], linearizationThreshold)) { if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold)) {
return true; // at least two "relative" poses are different, hence we re-linerize return true; // at least two "relative" poses are different, hence we re-linerize
} }
} }
@ -386,7 +388,7 @@ namespace gtsam {
bool doLinearize; bool doLinearize;
if (linearizationThreshold >= 0){//by convention if linearizationThreshold is negative we always relinearize if (linearizationThreshold >= 0){//by convention if linearizationThreshold is negative we always relinearize
std::cout << "Temporary disabled" << std::endl; // std::cout << "Temporary disabled" << std::endl;
doLinearize = decideIfLinearize(cameraPoses, state_->cameraPosesLinearization, linearizationThreshold); doLinearize = decideIfLinearize(cameraPoses, state_->cameraPosesLinearization, linearizationThreshold);
} }
else{ else{
@ -398,6 +400,7 @@ namespace gtsam {
} }
if(!doLinearize){ // return the previous Hessian factor if(!doLinearize){ // return the previous Hessian factor
// std::cout << "Using stored factors :) " << std::endl;
return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f)); return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
} }