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

Kitti test not working

release/4.3a0
Luca Carlone 2013-10-06 18:17:35 +00:00
parent d049dd38c6
commit 444df1958a
2 changed files with 145 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp
View File

@ -322,7 +322,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 = optimizer.ensureHasOrdering(params, graph);
gttic_(SmartProjectionFactorExample_kitti); gttic_(SmartProjectionFactorExample_kitti);
result = optimizer.optimize(); result = optimizer.optimize();
gttoc_(SmartProjectionFactorExample_kitti); gttoc_(SmartProjectionFactorExample_kitti);
@ -500,7 +500,7 @@ int main(int argc, char** argv) {
// add factors // add factors
// SMART FACTORS .. // SMART FACTORS ..
//if (useSmartProjectionFactor) { if (useSmartProjectionFactor) {
// Check if landmark exists in mapping // Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(L(l)); SmartFactorToStateMap::iterator fsit = smartFactorStates.find(L(l));
SmartFactorMap::iterator fit = smartFactors.find(L(l)); SmartFactorMap::iterator fit = smartFactors.find(L(l));
@ -541,7 +541,7 @@ int main(int argc, char** argv) {
optimized = false; optimized = false;
} }
//} else { } else {
// or STANDARD PROJECTION FACTORS // or STANDARD PROJECTION FACTORS
// Create projection factor // Create projection factor
ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(Point2(uL,v), pixel_sigma, X(r), L(l), K)); ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(Point2(uL,v), pixel_sigma, X(r), L(l), K));
@ -600,7 +600,7 @@ int main(int argc, char** argv) {
// of triangulation failure we cannot add the landmark to the graph // of triangulation failure we cannot add the landmark to the graph
} }
//} }
if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks); if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks);
if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl;
@ -613,6 +613,8 @@ int main(int argc, char** argv) {
} }
} }
}
if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
// if we haven't optimized yet // if we haven't optimized yet

2
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
View File

@ -169,7 +169,7 @@ TEST( SmartProjectionFactor, noisy ){
// DOUBLES_EQUAL(expectedError, actualError, 1e-7); // DOUBLES_EQUAL(expectedError, actualError, 1e-7);
} }
/* ************************************************************************* /* **************************************************************************
TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl; cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl;