diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp index 99afcd22b..69a48adee 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp @@ -43,6 +43,7 @@ #include #include #include +#include // Standard headers, added last, so we know headers above work on their own #include @@ -61,6 +62,7 @@ using symbol_shorthand::L; typedef PriorFactor Pose3Prior; typedef SmartProjectionFactor SmartFactor; +typedef SmartProjectionFactorsCreator SmartFactorsCreator; typedef GenericProjectionFactor ProjectionFactor; typedef FastMap > SmartFactorToStateMap; typedef FastMap > SmartFactorMap; @@ -72,10 +74,11 @@ bool debug = false; //// Helper functions taken from VO code // Loaded all pose values into list Values::shared_ptr loadPoseValues(const string& filename) { + Values::shared_ptr values(new Values()); bool addNoise = false; std::cout << "PARAM Noise: " << addNoise << std::endl; -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3)); // read in camera poses @@ -363,13 +366,13 @@ void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr g int main(int argc, char** argv) { unsigned int maxNumLandmarks = 389007; //100000000; // 309393 // (loop_closure_merged) //37106 //(reduced kitti); - unsigned int maxNumPoses = 45400; //3541 + unsigned int maxNumPoses = 1e+6; // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used bool useSmartProjectionFactor = true; bool useTriangulation = true; bool useLM = true; - int landmarkFirstOrderingMethod = 2; // 0 - COLAMD, 1 - landmark first, poses from smart factor, 2 - landmark first through constrained ordering + int landmarkFirstOrderingMethod = 1; // 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering) double KittiLinThreshold = -1.0; // 0.005; // double KittiRankTolerance = 1.0; @@ -420,37 +423,127 @@ int main(int argc, char** argv) { unsigned int numLandmarks = 0, numPoses = 0; Key r, l; double uL, uR, v, x, y, z; - std::vector views; std::vector landmarkKeys, cameraPoseKeys; + std::vector views; std::vector measurements; Values values; - SmartFactorToStateMap smartFactorStates; - SmartFactorMap smartFactors; ProjectionFactorMap projectionFactors; Values result; int totalNumMeasurements = 0; bool optimized = false; - boost::shared_ptr ordering, landmarkFirstOrdering(new Ordering()); + boost::shared_ptr ordering(new Ordering()); + SmartFactorsCreator smartCreator(pixel_sigma, K, KittiRankTolerance, KittiLinThreshold); // main loop: reads measurements and adds factors (also performs optimization if desired) + // r >> pose id + // l >> landmark id + // (uL >> uR) >> measurement (xaxis pixel measurement in left and right camera - since we do monocular, we only use uL) + // v >> measurement (yaxis pixel measurement) + // (x >> y >> z) 3D initialization for the landmark (not used in this code) while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) { if (debug) fprintf(stderr,"Landmark %ld\n", l); if (debug) fprintf(stderr,"Line %d: %d landmarks, (max landmarks %d), %d poses, max poses %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); + // 1: add values and factors to the graph + // 1.1: add factors + // SMART FACTORS .. + if (useSmartProjectionFactor) { + + smartCreator.add(L(l), X(r), Point2(uL,v), graphSmart); + + // Add initial pose value if pose does not exist + if (!graphSmartValues->exists(X(r)) && loadedValues->exists(X(r))) { + graphSmartValues->insert(X(r), loadedValues->at(X(r))); + numPoses++; + optimized = false; + } + + } 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 +// } + + } + // Optimize if have a certain number of poses/landmarks, or we want to do incremental inference - if (currentLandmark != l && (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks || (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0 )) ) { + if (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); + // addTriangulatedLandmarks(graphProjection, loadedValues, graphProjectionValues, K, projectionFactors, cameraPoseKeys, landmarkKeys); //} + 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 (useSmartProjectionFactor == false && useTriangulation) { + addTriangulatedLandmarks(graphSmart, loadedValues, graphSmartValues, K, projectionFactors, cameraPoseKeys, landmarkKeys); + } if (useLM) optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); else @@ -463,32 +556,20 @@ int main(int argc, char** argv) { if (1||debug) std::cout << "Pose ordering: " << ordering->size() << std::endl; if (landmarkFirstOrderingMethod == 1) { - // Add landmark keys first for ordering - BOOST_FOREACH(const Key& key, landmarkKeys) { - landmarkFirstOrdering->push_back(key); - } - - // Add COLAMD on pose keys to ordering - //Ordering::iterator oit; - BOOST_FOREACH(const Key& key, *ordering) { - landmarkFirstOrdering->push_back(key); - } - } else if (landmarkFirstOrderingMethod == 2) { 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, *ordering) { + BOOST_FOREACH(const Key& key, cameraPoseKeys) { orderingMap.insert( make_pair(key, 2) ); } - *landmarkFirstOrdering = graphProjection.orderingCOLAMDConstrained(orderingMap); + *ordering = graphProjection.orderingCOLAMDConstrained(orderingMap); } - if (1||debug) std::cout << "Optimizing landmark first " << landmarkFirstOrdering->size() << std::endl; - optimizeGraphLM(graphProjection, graphProjectionValues, result, landmarkFirstOrdering); - + 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) ) { @@ -498,109 +579,7 @@ int main(int argc, char** argv) { if(!incrementalFlag) break; } - // add factors - // SMART FACTORS .. - if (useSmartProjectionFactor) { - // Check if landmark exists in mapping - SmartFactorToStateMap::iterator fsit = smartFactorStates.find(L(l)); - SmartFactorMap::iterator fit = smartFactors.find(L(l)); - if (fsit != smartFactorStates.end() && fit != smartFactors.end()) { - if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); - // Add measurement to smart factor - (*fit).second->add(Point2(uL,v), X(r)); - totalNumMeasurements++; - if (debug) (*fit).second->print(); - - } else { - - if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end()); - - views += X(r); - measurements += Point2(uL,v); - - // This is a new landmark, create a new factor and add to mapping - boost::shared_ptr smartFactorState(new SmartProjectionFactorState()); - //SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K)); - SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K, KittiRankTolerance, KittiLinThreshold)); - smartFactorStates.insert( make_pair(L(l), smartFactorState) ); - smartFactors.insert( make_pair(L(l), smartFactor) ); - graphSmart.push_back(smartFactor); - numLandmarks++; - //landmarkKeys.push_back( L(l) ); - totalNumMeasurements++; - - views.clear(); - measurements.clear(); - } - - // Add initial pose value if pose does not exist - if (!graphSmartValues->exists(X(r)) && loadedValues->exists(X(r))) { - graphSmartValues->insert(X(r), loadedValues->at(X(r))); - numPoses++; - optimized = false; - } - - } 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) ); - - // Add projection factor to graph - //graphProjection.push_back(projectionFactor); - - // We have a new landmark - //numLandmarks++; - //landmarkKeys.push_back( L(l) ); - } - - // 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 - } - - } if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks); if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; @@ -613,7 +592,11 @@ int main(int argc, char** argv) { } } - } + if(currentLandmark != l && (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks)){ // reached desired number of landmarks/poses + break; + } + + } // end of while if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index f50f0e764..9ebec9c53 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -196,8 +196,8 @@ namespace gtsam { boost::optional body_P_sensor = boost::none, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : measured_(measured), noise_(model), K_(K), - retriangulationThreshold(defaultTriangThreshold), rankTolerance(defaultRankTolerance), - linearizationThreshold(defaultLinThreshold), body_P_sensor_(body_P_sensor), + retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol), + linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor), state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) { } diff --git a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h new file mode 100644 index 000000000..720e855dc --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h @@ -0,0 +1,112 @@ +/* + * SmartProjectionFactorsCreator.h + * + * Created on: Oct 8, 2013 + * Author: aspn + */ + +#ifndef SMARTPROJECTIONFACTORSCREATOR_H_ +#define SMARTPROJECTIONFACTORSCREATOR_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 SmartProjectionFactor SmartFactor; + typedef FastMap > SmartFactorToStateMap; + typedef FastMap > SmartFactorMap; + + template + class SmartProjectionFactorsCreator { + public: + SmartProjectionFactorsCreator(const SharedNoiseModel& model, + const boost::shared_ptr& K, + const double rankTol, + const double linThreshold, + boost::optional body_P_sensor = boost::none) : + noise_(model), K_(K), rankTolerance_(rankTol), + linearizationThreshold_(linThreshold), body_P_sensor_(body_P_sensor), + totalNumMeasurements(0), numLandmarks(0) {}; + + void add(Key landmarkKey, + Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) { + + std::vector views; + std::vector measurements; + + bool debug = false; + + // Check if landmark exists in mapping + SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey); + SmartFactorMap::iterator fit = smartFactors.find(landmarkKey); + if (fsit != smartFactorStates.end() && fit != smartFactors.end()) { + if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); + + // Add measurement to smart factor + (*fit).second->add(measurement, poseKey); + totalNumMeasurements++; + if (debug) (*fit).second->print(); + + } else { + + if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end()); + + views += poseKey; + measurements += measurement; + + // This is a new landmark, create a new factor and add to mapping + boost::shared_ptr smartFactorState(new SmartProjectionFactorState()); + SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, noise_, K_, rankTolerance_, linearizationThreshold_)); + smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) ); + smartFactors.insert( std::make_pair(landmarkKey, smartFactor) ); + graph.push_back(smartFactor); + + numLandmarks++; + //landmarkKeys.push_back( L(l) ); + totalNumMeasurements++; + + views.clear(); + measurements.clear(); + } + } + + protected: + 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 + + double rankTolerance_; ///< threshold to decide whether triangulation is degenerate + + double linearizationThreshold_; ///< threshold to decide whether to re-linearize + + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + SmartFactorToStateMap smartFactorStates; + SmartFactorMap smartFactors; + + int totalNumMeasurements; + int numLandmarks; + + }; + +} + +#endif /* SMARTPROJECTIONFACTORSCREATOR_H_ */