Kitti test not working
Luca Carlone
parent
d049dd38c6
commit
444df1958a
|
|
@ -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<SmartProjectionFactorState> 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<SmartProjectionFactorState> 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<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
|
||||
graphSmartValues->insert(X(r), loadedValues->at<Pose3>(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<ProjectionFactor::shared_ptr> 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<Point3>(L(l)) == boost::none) {
|
||||
Pose3 camera = loadedValues->at<Pose3>(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<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
|
||||
graphProjectionValues->insert(X(r), loadedValues->at<Pose3>(X(r)));
|
||||
cameraPoseKeys.push_back( X(r) );
|
||||
//numPoses++;
|
||||
if (!graphSmartValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
|
||||
graphSmartValues->insert(X(r), loadedValues->at<Pose3>(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<ProjectionFactor::shared_ptr> 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<Point3>(L(l)) == boost::none) {
|
||||
Pose3 camera = loadedValues->at<Pose3>(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<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
|
||||
graphProjectionValues->insert(X(r), loadedValues->at<Pose3>(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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue