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

Modified SmartStereoProjectionFactorExample to write optimized poses to file

release/4.3a0
Stephen Camp 2014-07-22 09:08:38 -04:00
parent d85c9986b3
commit a297434148
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -48,9 +48,12 @@ int main(int argc, char** argv){
typedef SmartStereoProjectionPoseFactor<Pose3, Point3, Cal3_S2Stereo> SmartFactor; typedef SmartStereoProjectionPoseFactor<Pose3, Point3, Cal3_S2Stereo> SmartFactor;
bool output_poses = true;
string poseOutput("../../../examples/data/optimized_poses.txt");
Values initial_estimate; Values initial_estimate;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
ofstream pose3Out;
bool add_initial_noise = true; bool add_initial_noise = true;
@ -126,5 +129,14 @@ int main(int argc, char** argv){
Values pose_values = result.filter<Pose3>(); Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n"); pose_values.print("Final camera poses:\n");
if(output_poses){
pose3Out.open(poseOutput.c_str(),ios::out);
for(int i = 1; i<=pose_values.size(); i++){
pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
" ", " ")) << endl;
}
cout << "Writing output" << endl;
}
return 0; return 0;
} }