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