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) {
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) ;
return K_.uncalibrate(pn);
}
@ -331,7 +331,7 @@ namespace gtsam {
// world to camera coordinate
Matrix Hc1_rot /* 3*3 */, Hc2 /* 3*3 */ ;
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);
Hc1.block(0,0,3,3) = Hc1_rot;
@ -342,14 +342,14 @@ namespace gtsam {
// uncalibration
Matrix Hk, Hi; // 2*2
Matrix H23 = Matrix::Zero(3,2);
H23.block(0,0,2,2) = Matrix::Identity(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 (H2) *H2 = (tmp * Hc2).block(0,0,3,2);
if (H3) *H3 = Hk;
return pi;
}

19
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){
string filename = directory_ + "camera_poses.txt";
string filename = directory_ + "out_camera_poses.txt";
ofstream fout;
fout.open(filename.c_str());
fout.precision(20);
@ -194,6 +194,9 @@ void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared
ProjectionFactorMap::iterator pfit;
if (debug) graphValues->print("graphValues \n");
if (debug) std::cout << " # END VALUES: " << std::endl;
// Iterate through all landmarks
if (debug) std::cout << " PROJECTION FACTOR GROUPED: " << projectionFactors.size();
int numProjectionFactors = 0;
@ -216,7 +219,6 @@ void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared
// Iterate through poses
cameraPoses.push_back( loadedValues->at<Pose3>((*vfit)->key1() ) );
measured.push_back( (*vfit)->measured() );
}
// 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;
landmarkKeys.push_back( projectionFactorVector[0]->key2() );
}
if (1||debug) std::cout << " # PROJECTION FACTORS CALCULATED: " << numProjectionFactors;
if (1||debug) std::cout << " # PROJECTION FACTORS ADDED: " << numProjectionFactorsAdded;
@ -370,8 +371,8 @@ int main(int argc, char** argv) {
bool useLM = true;
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 KittiRankTolerance = 1;
double KittiLinThreshold = -1.0; // 0.005; //
double KittiRankTolerance = 1.0;
bool incrementalFlag = false;
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 Triangulation: " << useTriangulation << std::endl;
std::cout << "PARAM LM: " << useLM << std::endl;
std::cout << "PARAM KittiLinThreshold (negative is disabled): " << KittiLinThreshold << std::endl;
// Get home directory and dataset
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 (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
if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
@ -589,7 +593,7 @@ int main(int argc, char** argv) {
// Add projection factor to graph
graphProjection.push_back(projectionFactor);
} else {
}else {
// Alternatively: Triangulate similar to how SmartProjectionFactor does it
// 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
@ -608,7 +612,7 @@ int main(int argc, char** argv) {
cout << "Loading graph projection... " << graphProjection.size() << endl;
}
}
if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
// if we haven't optimized yet
@ -616,7 +620,6 @@ int main(int argc, char** argv) {
if (useSmartProjectionFactor == false && useTriangulation) {
addTriangulatedLandmarks(graphSmart, loadedValues, graphSmartValues, K, projectionFactors, cameraPoseKeys, landmarkKeys);
}
if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
else

7
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -190,6 +190,8 @@ namespace gtsam {
const std::vector<Point2> measured,
const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
const double rankTol,
const double linThreshold,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
@ -269,7 +271,7 @@ namespace gtsam {
Pose3 localCameraPose = firstCameraPose.between(cameraPoses[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
}
}
@ -386,7 +388,7 @@ namespace gtsam {
bool doLinearize;
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);
}
else{
@ -398,6 +400,7 @@ namespace gtsam {
}
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));
}