From 9639660685072bf99316ad97f3406b3e5aee561b Mon Sep 17 00:00:00 2001 From: Zsolt Kira Date: Fri, 11 Oct 2013 01:59:36 +0000 Subject: [PATCH] Added GenericProjectionFactorCreator. Both smart and generic projection factors now work (again) in batch mode. Incremental not tested yet. --- ...ProjectionFactorExample_kitti_nonbatch.cpp | 281 +++--------------- .../slam/GenericProjectionFactorsCreator.h | 230 ++++++++++++++ 2 files changed, 272 insertions(+), 239 deletions(-) create mode 100644 gtsam_unstable/slam/GenericProjectionFactorsCreator.h diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp index 2241e925c..a1f62e1a7 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp @@ -41,9 +41,8 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. #include -#include -#include #include +#include // Standard headers, added last, so we know headers above work on their own #include @@ -61,12 +60,8 @@ using symbol_shorthand::X; using symbol_shorthand::L; typedef PriorFactor Pose3Prior; -typedef SmartProjectionFactor SmartFactor; typedef SmartProjectionFactorsCreator SmartFactorsCreator; -typedef GenericProjectionFactor ProjectionFactor; -typedef FastMap > SmartFactorToStateMap; -typedef FastMap > SmartFactorMap; -typedef FastMap > > ProjectionFactorMap; +typedef GenericProjectionFactorsCreator ProjectionFactorsCreator; typedef FastMap OrderingMap; bool debug = false; @@ -151,6 +146,7 @@ Cal3_S2::shared_ptr loadCalibration(const string& filename) { return K; } +// Write key values to file void writeValues(string directory_, const Values& values){ string filename = directory_ + "out_camera_poses.txt"; ofstream fout; @@ -187,93 +183,6 @@ void writeValues(string directory_, const Values& values){ } } -void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr loadedValues, - gtsam::Values::shared_ptr graphValues, boost::shared_ptr K, ProjectionFactorMap &projectionFactors, - vector &cameraPoseKeys, vector &landmarkKeys) { - std::vector > projectionFactorVector; - std::vector >::iterator vfit; - Point3 point; - Pose3 cameraPose; - - 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; - int numProjectionFactorsAdded = 0; - int numFailures = 0; - for (pfit = projectionFactors.begin(); pfit != projectionFactors.end(); pfit++) { - - projectionFactorVector = (*pfit).second; - - std::vector cameraPoses; - std::vector measured; - - // Iterate through projection factors - for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { - numProjectionFactors++; - - if (debug) std::cout << "ProjectionFactor: " << std::endl; - if (debug) (*vfit)->print("ProjectionFactor"); - - // Iterate through poses - cameraPoses.push_back( loadedValues->at((*vfit)->key1() ) ); - measured.push_back( (*vfit)->measured() ); - } - - // Triangulate landmark based on set of poses and measurements - if (debug) std::cout << "Triangulating: " << std::endl; - try { - point = triangulatePoint3(cameraPoses, measured, *K); - if (debug) std::cout << "Triangulation succeeded: " << point << std::endl; - } catch( TriangulationUnderconstrainedException& e) { - if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl; - if (debug) { - BOOST_FOREACH(const Pose3& pose, cameraPoses) { - std::cout << " Pose: " << pose << std::endl; - } - } - numFailures++; - continue; - } catch( TriangulationCheiralityException& e) { - if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl; - if (debug) { - std::cout << "Triangulation failed because of cheirality exception" << std::endl; - BOOST_FOREACH(const Pose3& pose, cameraPoses) { - std::cout << " Pose: " << pose << std::endl; - } - } - numFailures++; - continue; - } - - // Add projection factors and pose values - for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { - numProjectionFactorsAdded++; - if (debug) std::cout << "Adding factor " << std::endl; - if (debug) (*vfit)->print("Projection Factor"); - graph.push_back( (*vfit) ); - - if (!graphValues->exists( (*vfit)->key1()) && loadedValues->exists((*vfit)->key1())) { - graphValues->insert((*vfit)->key1(), loadedValues->at((*vfit)->key1())); - cameraPoseKeys.push_back( (*vfit)->key1() ); - } - } - - // Add landmark value - if (debug) std::cout << "Adding value " << std::endl; - 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; - if (1||debug) std::cout << " # FAILURES: " << numFailures; -} - void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result, boost::shared_ptr &ordering) { // Optimization parameters LevenbergMarquardtParams params; @@ -368,14 +277,12 @@ void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr g // main int main(int argc, char** argv) { - unsigned int maxNumLandmarks = 389007; //100000000; // 309393 // (loop_closure_merged) //37106 //(reduced kitti); + unsigned int maxNumLandmarks = 10000; //389007; //100000000; // 309393 // (loop_closure_merged) //37106 //(reduced kitti); unsigned int maxNumPoses = 1e+6; // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used - bool useSmartProjectionFactor = true; - bool useTriangulation = true; + bool useSmartProjectionFactor = false; bool useLM = true; - int landmarkFirstOrderingMethod = 1; // 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering) double KittiLinThreshold = -1.0; // 0.005; // double KittiRankTolerance = 1.0; @@ -384,7 +291,6 @@ int main(int argc, char** argv) { int optSkip = 200; // we optimize the graph every optSkip poses 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; @@ -395,12 +301,9 @@ int main(int argc, char** argv) { //string input_dir = HOME + "/data/kitti_00_full_dirty/"; static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01))); - //static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9))); NonlinearFactorGraph graphSmart, graphProjection; // Load calibration - //Cal3_S2::shared_ptr K(new Cal3_S2(718.856, 718.856, 0.0, 607.1928, 185.2157)); boost::shared_ptr K = loadCalibration(input_dir+"calibration.txt"); K->print("Calibration"); @@ -412,12 +315,10 @@ int main(int argc, char** argv) { exit(1); } - // Load all values, add priors + // Load all values gtsam::Values::shared_ptr graphSmartValues(new gtsam::Values()); gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values()); gtsam::Values::shared_ptr loadedValues = loadPoseValues(input_dir+"camera_poses.txt"); - //graph.push_back(Pose3Prior(X(0),loadedValues->at(X(0)), prior_model)); - //graph.push_back(Pose3Prior(X(1),loadedValues->at(X(1)), prior_model)); // read all measurements tracked by VO stereo cout << "Loading stereo_factors.txt" << endl; @@ -427,15 +328,13 @@ int main(int argc, char** argv) { Key r, l; double uL, uR, v, x, y, z; std::vector landmarkKeys, cameraPoseKeys; - std::vector views; - std::vector measurements; Values values; - ProjectionFactorMap projectionFactors; Values result; - int totalNumMeasurements = 0; bool optimized = false; boost::shared_ptr ordering(new Ordering()); + bool breakingCondition; SmartFactorsCreator smartCreator(pixel_sigma, K, KittiRankTolerance, KittiLinThreshold); + ProjectionFactorsCreator projectionCreator(pixel_sigma, K); // main loop: reads measurements and adds factors (also performs optimization if desired) // r >> pose id @@ -464,159 +363,63 @@ int main(int argc, char** argv) { } else { // or STANDARD PROJECTION FACTORS - // Create projection factor - ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(Point2(uL,v), pixel_sigma, X(r), L(l), K)); - - // Check if landmark exists in mapping - ProjectionFactorMap::iterator pfit = projectionFactors.find(L(l)); - if (pfit != projectionFactors.end()) { - if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); - - // Add projection factor to list of projection factors associated with this landmark - (*pfit).second.push_back(projectionFactor); - - } else { - if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end()); - - // Create a new vector of projection factors - std::vector projectionFactorVector; - projectionFactorVector.push_back(projectionFactor); - - // Insert projection factor to NEW list of projection factors associated with this landmark - projectionFactors.insert( make_pair(L(l), projectionFactorVector) ); - - - optimized = false; // TODO - - - // Add projection factor to graph - //graphProjection.push_back(projectionFactor); - - // We have a new landmark - //numLandmarks++; - //landmarkKeys.push_back( L(l) ); - } - - if (!useTriangulation) { - cerr << "Deprecated use of -useTriangulation- flag" << endl; - } -// // Add landmark if triangulation is not being used to initialize them -// if (!useTriangulation) { -// // For projection factor, landmarks positions are used, but have to be transformed to world coordinates -// if (graphProjectionValues->exists(L(l)) == boost::none) { -// Pose3 camera = loadedValues->at(X(r)); -// Point3 worldPoint = camera.transform_from(Point3(x, y, z)); -// graphProjectionValues->insert(L(l), worldPoint); // add point; -// } -// -// // Add initial pose value if pose does not exist -// // Only do this if triangulation is not used. Otherwise, it depends what projection factors are added -// // based on triangulation success -// if (!graphProjectionValues->exists(X(r)) && loadedValues->exists(X(r))) { -// graphProjectionValues->insert(X(r), loadedValues->at(X(r))); -// cameraPoseKeys.push_back( X(r) ); -// //numPoses++; -// } -// -// // Add projection factor to graph -// graphProjection.push_back(projectionFactor); -// -// }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 -// // of triangulation failure we cannot add the landmark to the graph -// } - + projectionCreator.add(L(l), X(r), Point2(uL,v), graphProjection); + numLandmarks = projectionCreator.getNumLandmarks(); + optimized = false; } + breakingCondition = currentLandmark != l && (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks); + // Optimize if have a certain number of poses/landmarks, or we want to do incremental inference - if (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0) { + if (breakingCondition || (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0) ) { if (debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); if (debug) cout << "Adding triangulated landmarks, graph size: " << graphProjection.size() << endl; - //if (useSmartProjectionFactor == false && useTriangulation) { - // addTriangulatedLandmarks(graphProjection, loadedValues, graphProjectionValues, K, projectionFactors, cameraPoseKeys, landmarkKeys); - //} + if (useSmartProjectionFactor == false) { + projectionCreator.update(graphProjection, loadedValues, graphProjectionValues); + ordering = projectionCreator.getOrdering(); + } 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 ){ - if (useSmartProjectionFactor == false && useTriangulation) { - addTriangulatedLandmarks(graphSmart, loadedValues, graphSmartValues, K, projectionFactors, cameraPoseKeys, landmarkKeys); - } - if (useLM) - optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); - else - optimizeGraphISAM2(graphSmart, graphSmartValues, result); - - if(incrementalFlag) *graphSmartValues = result; // we use optimized solution as initial guess for the next one - - optimized = true; - if (1||debug) std::cout << "Landmark Keys: " << landmarkKeys.size() << " Pose Keys: " << cameraPoseKeys.size() << std::endl; - if (1||debug) std::cout << "Pose ordering: " << ordering->size() << std::endl; - - if (landmarkFirstOrderingMethod == 1) { - OrderingMap orderingMap; - // Add landmark keys first for ordering - BOOST_FOREACH(const Key& key, landmarkKeys) { - orderingMap.insert( make_pair(key, 1) ); - } - //Ordering::iterator oit; - BOOST_FOREACH(const Key& key, cameraPoseKeys) { - orderingMap.insert( make_pair(key, 2) ); - } - *ordering = graphProjection.orderingCOLAMDConstrained(orderingMap); - } - - if (1||debug) std::cout << "Optimizing landmark first " << ordering->size() << std::endl; - //optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); - - // Only process first N measurements (for development/debugging) - if ( (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks) ) { - if (debug) fprintf(stderr,"%d: BREAKING %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); - break; - } - if(!incrementalFlag) break; + if (useSmartProjectionFactor) { + if (useLM) + optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); + else + optimizeGraphISAM2(graphSmart, graphSmartValues, result); + } else { + if (useLM) + optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering); + else + optimizeGraphISAM2(graphSmart, graphSmartValues, result); } + if(incrementalFlag) *graphSmartValues = result; // we use optimized solution as initial guess for the next one + optimized = true; - - if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks); - if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; - - currentLandmark = l; - count++; - if(count==100000) { - cout << "Loading graph smart... " << graphSmart.size() << endl; - cout << "Loading graph projection... " << graphProjection.size() << endl; - } } - if(currentLandmark != l && (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks)){ // reached desired number of landmarks/poses + if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks); + if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; + + if (1||debug) fprintf(stderr,"%d: %d, %d > %d, %d > %d\n", count, currentLandmark != l, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); + if(breakingCondition){ // reached desired number of landmarks/poses break; } + currentLandmark = l; + count++; + if(count==100000) { + cout << "Loading graph smart... " << graphSmart.size() << endl; + cout << "Loading graph projection... " << graphProjection.size() << endl; + } + } // end of while if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); - // if we haven't optimized yet - if (!optimized) { - if (useSmartProjectionFactor == false && useTriangulation) { - addTriangulatedLandmarks(graphSmart, loadedValues, graphSmartValues, K, projectionFactors, cameraPoseKeys, landmarkKeys); - } - if (useLM) - optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); - else - optimizeGraphISAM2(graphSmart, graphSmartValues, result); - optimized = true; - } - if (useSmartProjectionFactor||debug) std::cout << "TOTAL NUM MEASUREMENTS " << totalNumMeasurements; - cout << "===================================================" << endl; //graphSmartValues->print("before optimization "); //result.print("results of kitti optimization "); diff --git a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h new file mode 100644 index 000000000..e31d513d9 --- /dev/null +++ b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h @@ -0,0 +1,230 @@ +/* + * GenericProjectionFactorsCreator.h + * + * Created on: Oct 10, 2013 + * Author: zkira + */ + +#ifndef GENERICPROJECTIONFACTORSCREATOR_H_ +#define GENERICPROJECTIONFACTORSCREATOR_H_ + +// Both relative poses and recovered trajectory poses will be stored as Pose3 objects +#include + +#include + +#include + +// Use a map to store landmark/smart factor pairs +#include +#include + + +#include + +//#include +//#include +//#include +//#include +//#include +#include + +namespace gtsam { + + typedef GenericProjectionFactor ProjectionFactor; + typedef FastMap > > ProjectionFactorMap; + typedef FastMap OrderingMap; + + template + class GenericProjectionFactorsCreator { + public: + GenericProjectionFactorsCreator(const SharedNoiseModel& model, + const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : + noise_(model), K_(K), body_P_sensor_(body_P_sensor), + orderingMethod(0), totalNumMeasurements(0), numLandmarks(0) { + ordering = boost::make_shared(*(new Ordering())); + }; + + void add(Key landmarkKey, + Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) { + bool debug = false; + + // Create projection factor + ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, noise_, poseKey, landmarkKey, K_)); + + // Check if landmark exists in mapping + ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey); + if (pfit != projectionFactors.end()) { + if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); + + // Add projection factor to list of projection factors associated with this landmark + (*pfit).second.push_back(projectionFactor); + + } else { + if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end()); + + // Create a new vector of projection factors + std::vector projectionFactorVector; + projectionFactorVector.push_back(projectionFactor); + + // Insert projection factor to NEW list of projection factors associated with this landmark + projectionFactors.insert( std::make_pair(landmarkKey, projectionFactorVector) ); + + // Add projection factor to graph + //graph.push_back(projectionFactor); + + // We have a new landmark + numLandmarks++; + landmarkKeys.push_back( landmarkKey ); + } + } + + void update(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr inputValues, gtsam::Values::shared_ptr outputValues) { + addTriangulatedLandmarks(graph, inputValues, outputValues); + updateOrdering(graph); + } + + unsigned int getTotalNumMeasurements() { return totalNumMeasurements; } + unsigned int getNumLandmarks() { return numLandmarks; } + unsigned int getNumPoses() { return cameraPoseKeys.size(); } + boost::shared_ptr getOrdering() { return ordering; } + + protected: + + void updateTriangulations() { + } + + void updateOrdering(NonlinearFactorGraph &graph) { + bool debug = false; + + if (1||debug) std::cout << "Landmark Keys: " << landmarkKeys.size() << " Pose Keys: " << cameraPoseKeys.size() << std::endl; + if (1||debug) std::cout << "Pose ordering: " << ordering->size() << std::endl; + + if (orderingMethod == 1) { + OrderingMap orderingMap; + // Add landmark keys first for ordering + BOOST_FOREACH(const Key& key, landmarkKeys) { + orderingMap.insert( std::make_pair(key, 1) ); + } + //Ordering::iterator oit; + BOOST_FOREACH(const Key& key, cameraPoseKeys) { + orderingMap.insert( std::make_pair(key, 2) ); + } + *ordering = graph.orderingCOLAMDConstrained(orderingMap); + } + + if (1||debug) std::cout << "Optimizing landmark first " << ordering->size() << std::endl; + } + + void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr loadedValues, + gtsam::Values::shared_ptr graphValues) { + + bool debug = false; + + std::vector > projectionFactorVector; + std::vector >::iterator vfit; + Point3 point; + Pose3 cameraPose; + + 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; + int numProjectionFactorsAdded = 0; + int numFailures = 0; + for (pfit = projectionFactors.begin(); pfit != projectionFactors.end(); pfit++) { + + projectionFactorVector = (*pfit).second; + + std::vector cameraPoses; + std::vector measured; + + // Iterate through projection factors + for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { + numProjectionFactors++; + + if (debug) std::cout << "ProjectionFactor: " << std::endl; + if (debug) (*vfit)->print("ProjectionFactor"); + + // Iterate through poses + cameraPoses.push_back( loadedValues->at((*vfit)->key1() ) ); + measured.push_back( (*vfit)->measured() ); + } + + // Triangulate landmark based on set of poses and measurements + if (debug) std::cout << "Triangulating: " << std::endl; + try { + point = triangulatePoint3(cameraPoses, measured, *K_); + if (debug) std::cout << "Triangulation succeeded: " << point << std::endl; + } catch( TriangulationUnderconstrainedException& e) { + if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl; + if (debug) { + BOOST_FOREACH(const Pose3& pose, cameraPoses) { + std::cout << " Pose: " << pose << std::endl; + } + } + numFailures++; + continue; + } catch( TriangulationCheiralityException& e) { + if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl; + if (debug) { + std::cout << "Triangulation failed because of cheirality exception" << std::endl; + BOOST_FOREACH(const Pose3& pose, cameraPoses) { + std::cout << " Pose: " << pose << std::endl; + } + } + numFailures++; + continue; + } + + // Add projection factors and pose values + for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { + numProjectionFactorsAdded++; + if (debug) std::cout << "Adding factor " << std::endl; + if (debug) (*vfit)->print("Projection Factor"); + graph.push_back( (*vfit) ); + + if (!graphValues->exists( (*vfit)->key1()) && loadedValues->exists((*vfit)->key1())) { + graphValues->insert((*vfit)->key1(), loadedValues->at((*vfit)->key1())); + cameraPoseKeys.push_back( (*vfit)->key1() ); + } + } + + // Add landmark value + if (debug) std::cout << "Adding value " << std::endl; + 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; + if (1||debug) std::cout << " # FAILURES: " << numFailures; + } + + const SharedNoiseModel noise_; ///< noise model used + ///< (important that the order is the same as the keys that we use to create the factor) + boost::shared_ptr K_; ///< shared pointer to calibration object + + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + std::vector cameraPoseKeys; + std::vector landmarkKeys; + ProjectionFactorMap projectionFactors; + boost::shared_ptr ordering; + // orderingMethod: 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering) + int orderingMethod; + + unsigned int totalNumMeasurements; + unsigned int numLandmarks; + unsigned int numPoses; + + }; + +} + +#endif /* SMARTPROJECTIONFACTORSCREATOR_H_ */