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++) {
LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
params = optimizer.ensureHasOrdering(params, graph);
// params = optimizer.ensureHasOrdering(params, graph);
gttic_(SmartProjectionFactorExample_kitti);
result = optimizer.optimize();
gttoc_(SmartProjectionFactorExample_kitti);
@ -500,7 +500,7 @@ int main(int argc, char** argv) {
// add factors
// SMART FACTORS ..
//if (useSmartProjectionFactor) {
if (useSmartProjectionFactor) {
// Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(L(l));
SmartFactorMap::iterator fit = smartFactors.find(L(l));
@ -541,7 +541,7 @@ int main(int argc, char** argv) {
optimized = false;
}
//} else {
} else {
// or STANDARD PROJECTION FACTORS
// Create projection factor
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
}
//}
}
if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks);
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 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);
}
/* *************************************************************************
/* **************************************************************************
TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl;