diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index e8f285f90..b4e8d2e68 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -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; } diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp index 4b4598d0d..49228c8ce 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp @@ -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((*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 diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 7df60cedb..f50f0e764 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -190,6 +190,8 @@ namespace gtsam { const std::vector measured, const SharedNoiseModel& model, const boost::shared_ptr& K, + const double rankTol, + const double linThreshold, bool throwCheirality, bool verboseCheirality, boost::optional 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)); }