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

Changed to optimize entire graph instead of sub-graphs

release/4.3a0
Zsolt Kira 2013-08-08 14:20:08 +00:00
parent a82262cf99
commit 6dd67a13fd
1 changed files with 80 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp
View File

@ -38,7 +38,6 @@
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/slam/SmartProjectionFactor.h>
// Standard headers, added last, so we know headers above work on their own
@ -57,6 +56,27 @@ typedef PriorFactor<Pose3> Pose3Prior;
static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01)));
Values::shared_ptr loadPoseValues(const string& filename) {
Values::shared_ptr values(new Values());
// read in camera poses
string full_filename = filename;
ifstream fin;
fin.open(full_filename.c_str());
int pose_id;
while (fin >> pose_id) {
double pose_matrix[16];
for (int i = 0; i < 16; i++) {
fin >> pose_matrix[i];
}
values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)));
}
fin.close();
return values;
}
/* ************************************************************************* */
Values::shared_ptr loadPoseValues(const string& filename, list<Key> keys) {
Values::shared_ptr values(new Values());
@ -87,8 +107,12 @@ Values::shared_ptr loadPoseValues(const string& filename, list<Key> keys) {
// main
int main(int argc, char** argv) {
bool debug = false;
unsigned int minimumNumViews = 1;
string HOME = getenv("HOME");
string input_dir = HOME + "/data/kitti/loop_closures_merged/";
//string input_dir = HOME + "/data/kitti/loop_closures_merged/";
string input_dir = HOME + "/data/KITTI_00_200/";
Cal3_S2::shared_ptr K(new Cal3_S2(718.856, 718.856, 0.0, 607.1928, 185.2157));
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
@ -109,7 +133,7 @@ int main(int argc, char** argv) {
// read all measurements tracked by VO
cout << "Loading stereo_factors.txt" << endl;
int count = 0;
int currentLandmark = 0;
Key currentLandmark = 0;
int numLandmarks = 0;
Key r, l;
double uL, uR, v, x, y, z;
@ -117,88 +141,50 @@ int main(int argc, char** argv) {
std::vector<Key> views;
std::vector<Point2> measurements;
Values values;
bool updateGraph = false;
while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) {
cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl;
if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl;
if (currentLandmark != l && views.size() < 3) {
if (currentLandmark != l && views.size() < minimumNumViews) {
// New landmark. Not enough views for previous landmark so move on.
cout << "New landmark " << l << " with not enough view for previous one" << std::endl;
if (debug) cout << "New landmark " << l << " with not enough view for previous one" << std::endl;
currentLandmark = l;
views.clear();
measurements.clear();
} else if (currentLandmark != l) {
// New landmark. Add previous landmark and associated views to new factor
cout << "New landmark " << l << " with "<< views.size() << " views for previous landmark " << currentLandmark << std::endl;
if (debug) cout << "New landmark " << l << " with "<< views.size() << " views for previous landmark " << currentLandmark << std::endl;
cout << "Keys ";
if (debug) cout << "Keys ";
BOOST_FOREACH(const Key& k, views) {
allViews.push_back(k);
cout << k << " ";
if (debug) cout << k << " ";
}
cout << endl;
if (debug) cout << endl;
if (debug) {
cout << "Measurements ";
BOOST_FOREACH(const Point2& p, measurements) {
cout << p << " ";
}
cout << endl;
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K));
numLandmarks++;
graph.push_back(smartFactor);
if (numLandmarks > 4) {
updateGraph = true;
}
numLandmarks++;
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K));
graph.push_back(smartFactor);
currentLandmark = l;
views.clear();
measurements.clear();
} else {
// We have new view for current landmark, so add it to the list later
cout << "New view for landmark " << l << " (" << views.size() << " total)" << std::endl;
if (debug) cout << "New view for landmark " << l << " (" << views.size() << " total)" << std::endl;
}
// Add view for new landmark
views += X(r);
measurements += Point2(uL,v);
// Optimize
if (updateGraph) {
cout << "Optimizing... " << endl;
// Get all view in the graph and populate poses from VO output
// TODO: Handle loop closures properly
allViews.unique();
cout << "All Keys ";
values = *loadPoseValues(input_dir+"camera_poses.txt", allViews);
BOOST_FOREACH(const Key& k, allViews) {
cout << k << " ";
}
cout << endl;
// Optimize!
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionFactorExample_kitti);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionFactorExample_kitti);
tictoc_finishedIteration_();
values.print("before optimization ");
result.print("results of kitti optimization ");
tictoc_print_();
updateGraph = false;
values.clear();
allViews.clear();
graph = NonlinearFactorGraph();
}
count++;
if(count==100000) {
cout << "Loading graph... " << graph.size() << endl;
@ -207,6 +193,42 @@ int main(int argc, char** argv) {
}
cout << "Graph size: " << graph.size() << endl;
gtsam::Values::shared_ptr loaded_values = loadPoseValues(input_dir+"camera_poses.txt");
graph.add(Pose3Prior(X(0),loaded_values->at<Pose3>(X(0)), prior_model));
// Get all view in the graph and populate poses from VO output
// TODO: Handle loop closures properly
cout << "All Keys (" << allViews.size() << ")" << endl;
allViews.unique();
cout << "All Keys (" << allViews.size() << ")" << endl;
/*
values = *loadPoseValues(input_dir+"camera_poses.txt", allViews);
BOOST_FOREACH(const Key& k, allViews) {
if (debug) cout << k << " ";
}
cout << endl;
*/
cout << "Optimizing... " << endl;
// Optimize!
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionFactorExample_kitti);
LevenbergMarquardtOptimizer optimizer(graph, *loaded_values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionFactorExample_kitti);
tictoc_finishedIteration_();
cout << "===================================================" << endl;
values.print("before optimization ");
result.print("results of kitti optimization ");
tictoc_print_();
cout << "===================================================" << endl;
exit(0);
}