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

Revert "Added C++ stereo visual odometry examples" 

This reverts commit db82bad668.
release/4.3a0
Stephen Camp 2014-05-28 23:49:10 -04:00
parent db82bad668
commit 89130f1626
3 changed files with 57 additions and 248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
examples/Data/dubrovnik-3-7-pre-rewritten.txt
View File

@ -1,80 +1,80 @@
3 7 19 3 7 19
0 0 -385.989990234375 387.1199951171875 0 0 -385.989990234375 387.1199951171875
1 0 -38.439998626708984 492.1199951171875 1 0 -38.439998626708984375 492.1199951171875
2 0 -667.91998291015625 123.11000061035156 2 0 -667.91998291015625 123.1100006103515625
0 1 383.8800048828125 -15.299989700317383 0 1 383.8800048828125 -15.299989700317382812
1 1 559.75 -106.15000152587891 1 1 559.75 -106.15000152587890625
0 2 591.54998779296875 136.44000244140625 0 2 591.54998779296875 136.44000244140625
1 2 863.8599853515625 -23.469970703125 1 2 863.8599853515625 -23.469970703125
2 2 494.72000122070312 112.51999664306641 2 2 494.720001220703125 112.51999664306640625
0 3 592.5 125.75 0 3 592.5 125.75
1 3 861.08001708984375 -35.219970703125 1 3 861.08001708984375 -35.219970703125
2 3 498.54000854492187 101.55999755859375 2 3 498.540008544921875 101.55999755859375
0 4 348.72000122070312 558.3800048828125 0 4 348.720001220703125 558.3800048828125
1 4 776.030029296875 483.52999877929687 1 4 776.030029296875 483.529998779296875
2 4 7.7800288200378418 326.35000610351562 2 4 7.7800288200378417969 326.350006103515625
0 5 14.010009765625 96.420013427734375 0 5 14.010009765625 96.420013427734375
1 5 207.1300048828125 118.36000061035156 1 5 207.1300048828125 118.3600006103515625
0 6 202.75999450683594 340.989990234375 0 6 202.7599945068359375 340.989990234375
1 6 543.18011474609375 294.80999755859375 1 6 543.18011474609375 294.80999755859375
2 6 -58.419979095458984 110.83000183105469 2 6 -58.419979095458984375 110.8300018310546875
0.29656188120312943 0.29656188120312942935
-0.03531835438428587 -0.035318354384285870207
0.31252101755032047 0.31252101755032046793
0.47230274932665989 0.47230274932665988752
-0.35723408637441134 -0.3572340863744113415
-2.0517704282499576 -2.0517704282499575896
1430.031982421875 1430.031982421875
-7.5572756941255648e-008 -7.5572756941255647689e-08
3.2377570134516087e-014 3.2377570134516087119e-14
0.28532097290364833 0.28532097381985194184
-0.27699838355720618 -0.27699838370789808817
0.048601170006498565 0.048601169984112867206
-1.2598695987678452 -1.2598695987143850861
-0.04906379852479037 -0.049063798188844320869
-1.9586867140054638 -1.9586867140445654023
1432.137451171875 1432.137451171875
-7.317191830225056e-008 -7.3171918302250560373e-08
3.1759419042137055e-014 3.1759419042137054801e-14
0.057491325683772541 0.057491325683772541433
0.34853090049579966 0.34853090049579965592
0.47985129303736057 0.47985129303736057116
8.196390428906339 8.1963904289063389541
6.5146840788718787 6.5146840788718787252
-3.8392804395897406 -3.8392804395897406344
1572.047119140625 1572.047119140625
-1.5962623223231276e-008 -1.5962623223231275915e-08
-1.6507904730136101e-014 -1.6507904730136101212e-14
-11.317351620610928 -11.317351620610928364
3.3594874875767187 3.3594874875767186673
-42.755222607849106 -42.755222607849105998
4.2648515634753199 4.2648515634753199066
-8.4629358700849355 -8.4629358700849355301
-22.252086323427271 -22.252086323427270997
10.996977688149537 10.996977688149536689
-9.2123370180278048 -9.2123370180278048025
-29.206739014051372 -29.206739014051372294
10.935342607054865 10.935342607054865383
-9.4338917557810742 -9.4338917557810741954
-29.1122639091755 -29.112263909175499776
15.71402493540176 15.714024935401759819
1.3745079651566265 1.3745079651566265433
-59.286834979937105 -59.286834979937104606
-1.3624227800805182 -1.3624227800805182031
-4.1979357415396095 -4.1979357415396094666
-21.034430148188399 -21.034430148188398846
6.7690173115899297 6.7690173115899296974
-4.7352452433700787 -4.7352452433700786827
-53.605307875695893 -53.605307875695892506

75
examples/StereoVOExample.cpp
View File

@ -1,75 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SteroVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
*/
/**
* A 3D stereo visual odometry example
* - robot starts at origin
* -moves forward 1 meter
* -takes stereo readings on three landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose = Pose3();
graph.push_back(NonlinearEquality<Pose3>(1, first_pose));
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
//create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate = Values();
//create and add iniital estimates
initial_estimate.insert(1, first_pose);
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
initial_estimate.insert(3, Point3(1, 1, 5));
initial_estimate.insert(4, Point3(-1, 1, 5));
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
result.print("Final result:\n");
return 0;
}

116
examples/StereoVOExample_large.cpp
View File

@ -1,116 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SteroVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
*/
/**
* A 3D stereo visual odometry example
* - robot starts at origin
* -moves forward, taking periodic stereo measurements
* -takes stereo readings of many landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
Values initial_estimate = Values();
vector<double> read_vector;
string read_string, parse_string;
string data_folder = "C:/Users/Stephen/Documents/Borg/gtsam/Examples/Data/";
string calibration_loc = data_folder + "VO_calibration.txt";
string pose_loc = data_folder + "VO_camera_poses_large.txt";
string factor_loc = data_folder + "VO_stereo_factors_large.txt";
//read camera calibration info from file
ifstream calibration_file(calibration_loc);
getline(calibration_file,read_string);
istringstream string_parser(read_string);
cout << "Reading calibration info" << endl;
while(string_parser){
if(!getline(string_parser,parse_string,' ')) break;
read_vector.push_back(atof(parse_string.c_str()));
}
//create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(read_vector[0],read_vector[1],read_vector[2],read_vector[3],read_vector[4],read_vector[5]));
ifstream pose_file(pose_loc);
cout << "Reading camera poses" << endl;
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
float x, l, uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc);
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3,Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if(!initial_estimate.exists(Symbol('l',l))){
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X,Y,Z));
initial_estimate.insert(Symbol('l',l),worldPoint);
}
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
first_pose.print("Check estimate poses:\n");
//constrain the first pose such that it cannot change from its original value during optimization
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to solve the initial factor graph estimate
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
return 0;
}