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

Added option to increase initial error in SmartStereoProjectionFactorExample

release/4.3a0
Stephen Camp 2014-07-16 14:45:56 -04:00
parent d68e6b9add
commit e7c82652af
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -52,6 +52,8 @@ int main(int argc, char** argv){
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);
bool add_initial_noise = true;
string calibration_loc = findExampleDataFile("VO_calibration.txt"); string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
@ -75,6 +77,9 @@ int main(int argc, char** argv){
for (int i = 0; i < 16; i++) { for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i]; pose_file >> m.data()[i];
} }
if(add_initial_noise){
m(0,3) += (pose_id % 10)/5;
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
} }