diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp index 69a48adee..2241e925c 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp @@ -325,7 +325,7 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap //for (int i = 0; i < 3; i++) { LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params); - // params = optimizer.ensureHasOrdering(params, graph); + params.ordering = Ordering::COLAMD(graph); gttic_(SmartProjectionFactorExample_kitti); result = optimizer.optimize(); gttoc_(SmartProjectionFactorExample_kitti); @@ -333,10 +333,13 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap //} //*ordering = params.ordering; - std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl; - ordering = boost::make_shared(*(new Ordering())); - *ordering = *params.ordering; - + if (params.ordering) { + std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl; + ordering = boost::make_shared(*(new Ordering())); + *ordering = *params.ordering; + } else { + std::cout << "WARNING: NULL ordering!" << std::endl; + } } } @@ -450,6 +453,7 @@ int main(int argc, char** argv) { if (useSmartProjectionFactor) { smartCreator.add(L(l), X(r), Point2(uL,v), graphSmart); + numLandmarks = smartCreator.getNumLandmarks(); // Add initial pose value if pose does not exist if (!graphSmartValues->exists(X(r)) && loadedValues->exists(X(r))) { @@ -569,7 +573,7 @@ int main(int argc, char** argv) { } if (1||debug) std::cout << "Optimizing landmark first " << ordering->size() << std::endl; - optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); + //optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); // Only process first N measurements (for development/debugging) if ( (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks) ) { diff --git a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h index 720e855dc..605fe758b 100644 --- a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h +++ b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h @@ -88,6 +88,9 @@ namespace gtsam { } } + unsigned int getTotalNumMeasurements() { return totalNumMeasurements; } + unsigned int getNumLandmarks() { return numLandmarks; } + protected: const SharedNoiseModel noise_; ///< noise model used ///< (important that the order is the same as the keys that we use to create the factor) @@ -102,8 +105,8 @@ namespace gtsam { SmartFactorToStateMap smartFactorStates; SmartFactorMap smartFactors; - int totalNumMeasurements; - int numLandmarks; + unsigned int totalNumMeasurements; + unsigned int numLandmarks; };