12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed bugs in new version since creator class is used: Get number of landmarks from creator object, 

replace ensureHasOrdering with direct COLAMD call. 
Current Smart Projection factor version works in batch mode
release/4.3a0
Zsolt Kira 2013-10-10 20:21:40 +00:00
parent e799c9ffa9
commit 1a8a670870
2 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp
View File

@ -325,7 +325,7 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap
//for (int i = 0; i < 3; i++) { //for (int i = 0; i < 3; i++) {
LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params); LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
// params = optimizer.ensureHasOrdering(params, graph); params.ordering = Ordering::COLAMD(graph);
gttic_(SmartProjectionFactorExample_kitti); gttic_(SmartProjectionFactorExample_kitti);
result = optimizer.optimize(); result = optimizer.optimize();
gttoc_(SmartProjectionFactorExample_kitti); gttoc_(SmartProjectionFactorExample_kitti);
@ -333,10 +333,13 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap
//} //}
//*ordering = params.ordering; //*ordering = params.ordering;
std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl; if (params.ordering) {
ordering = boost::make_shared<Ordering>(*(new Ordering())); std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl;
*ordering = *params.ordering; ordering = boost::make_shared<Ordering>(*(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) { if (useSmartProjectionFactor) {
smartCreator.add(L(l), X(r), Point2(uL,v), graphSmart); smartCreator.add(L(l), X(r), Point2(uL,v), graphSmart);
numLandmarks = smartCreator.getNumLandmarks();
// Add initial pose value if pose does not exist // Add initial pose value if pose does not exist
if (!graphSmartValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) { if (!graphSmartValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
@ -569,7 +573,7 @@ int main(int argc, char** argv) {
} }
if (1||debug) std::cout << "Optimizing landmark first " << ordering->size() << std::endl; 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) // Only process first N measurements (for development/debugging)
if ( (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks) ) { if ( (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks) ) {

7
gtsam_unstable/slam/SmartProjectionFactorsCreator.h
View File

@ -88,6 +88,9 @@ namespace gtsam {
} }
} }
unsigned int getTotalNumMeasurements() { return totalNumMeasurements; }
unsigned int getNumLandmarks() { return numLandmarks; }
protected: protected:
const SharedNoiseModel noise_; ///< noise model used const SharedNoiseModel noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor) ///< (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; SmartFactorToStateMap smartFactorStates;
SmartFactorMap smartFactors; SmartFactorMap smartFactors;
int totalNumMeasurements; unsigned int totalNumMeasurements;
int numLandmarks; unsigned int numLandmarks;
}; };