diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index c661967c0..e2caca94f 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -25,45 +25,41 @@ */ #include -#include +#include +#include +#include #include #include -#include -#include -#include - -#include -#include #include +#include +#include +#include #include #include -#include -#include using namespace std; using namespace gtsam; int main(int argc, char** argv){ + Values initial_estimate; NonlinearFactorGraph graph; const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); - Values initial_estimate = Values(); - vector 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"; + string calibration_loc = findExampleDataFile("VO_calibration.txt"); + string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); + string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); //read camera calibration info from file - double fx,fy,s,u,v,b; + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + double fx, fy, s, u0, v0, b; ifstream calibration_file(calibration_loc); cout << "Reading calibration info" << endl; - calibration_file >> fx >> fy >> s >> u >> v >> b; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + //create stereo camera calibration object - const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u,v,b)); + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); ifstream pose_file(pose_loc); cout << "Reading camera poses" << endl; @@ -77,30 +73,36 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - double x, l, uL, uR, v, X, Y, Z; + // camera and landmark keys + size_t x, l; + + // pixel coordinates uL, uR, v (same for left/right images due to rectification) + // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + double 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(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(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); - } + graph.push_back( + GenericStereoFactor(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(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(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 + // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky + // QR is much slower than Cholesky, but numerically more stable graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); cout << "Optimizing" << endl; - //create Levenberg-Marquardt optimizer to solve the initial factor graph estimate + //create Levenberg-Marquardt optimizer to optimize the factor graph LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); Values result = optimizer.optimize(); @@ -109,4 +111,4 @@ int main(int argc, char** argv){ pose_values.print("Final camera poses:\n"); return 0; -} \ No newline at end of file +}