diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp index 49228c8ce..99afcd22b 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp @@ -322,7 +322,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 = optimizer.ensureHasOrdering(params, graph); gttic_(SmartProjectionFactorExample_kitti); result = optimizer.optimize(); gttoc_(SmartProjectionFactorExample_kitti); @@ -443,174 +443,176 @@ int main(int argc, char** argv) { 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 (useLM) - optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); - else - optimizeGraphISAM2(graphSmart, graphSmartValues, result); + // 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); + else + optimizeGraphISAM2(graphSmart, graphSmartValues, result); - if(incrementalFlag) *graphSmartValues = result; // we use optimized solution as initial guess for the next one + 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; + 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) { - // Add landmark keys first for ordering - BOOST_FOREACH(const Key& key, landmarkKeys) { - landmarkFirstOrdering->push_back(key); + 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) { + orderingMap.insert( make_pair(key, 2) ); + } + *landmarkFirstOrdering = graphProjection.orderingCOLAMDConstrained(orderingMap); } - // Add COLAMD on pose keys to ordering - //Ordering::iterator oit; - BOOST_FOREACH(const Key& key, *ordering) { - landmarkFirstOrdering->push_back(key); + if (1||debug) std::cout << "Optimizing landmark first " << landmarkFirstOrdering->size() << std::endl; + optimizeGraphLM(graphProjection, graphProjectionValues, result, landmarkFirstOrdering); + + + // 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; } - } 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) { - orderingMap.insert( make_pair(key, 2) ); - } - *landmarkFirstOrdering = graphProjection.orderingCOLAMDConstrained(orderingMap); + if(!incrementalFlag) break; } - if (1||debug) std::cout << "Optimizing landmark first " << landmarkFirstOrdering->size() << std::endl; - optimizeGraphLM(graphProjection, graphProjectionValues, result, landmarkFirstOrdering); + // 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(); - // 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; - } + } else { - // 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"); + if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end()); - // Add measurement to smart factor - (*fit).second->add(Point2(uL,v), X(r)); - totalNumMeasurements++; - if (debug) (*fit).second->print(); + views += X(r); + measurements += Point2(uL,v); - } else { + // 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++; - 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; + views.clear(); + measurements.clear(); } // 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++; + if (!graphSmartValues->exists(X(r)) && loadedValues->exists(X(r))) { + graphSmartValues->insert(X(r), loadedValues->at(X(r))); + numPoses++; + optimized = false; } - // Add projection factor to graph - graphProjection.push_back(projectionFactor); + } 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 + } - }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; - 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; + currentLandmark = l; + count++; + if(count==100000) { + cout << "Loading graph smart... " << graphSmart.size() << endl; + cout << "Loading graph projection... " << graphProjection.size() << endl; + } } + } if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 88ef7de1b..9262bb25f 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -169,7 +169,7 @@ TEST( SmartProjectionFactor, noisy ){ // DOUBLES_EQUAL(expectedError, actualError, 1e-7); } -/* ************************************************************************* +/* ************************************************************************** TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl; @@ -311,7 +311,7 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ } -/* ************************************************************************ */ +/* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;