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

Fixed warnings

release/4.3a0
Frank 2015-10-19 15:01:48 -07:00
parent 1c83329b9b
commit 901fb56993
1 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -54,7 +54,7 @@ int main(int argc, char** argv){
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");
//read camera calibration info from file //read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
cout << "Reading calibration info" << endl; cout << "Reading calibration info" << endl;
@ -67,17 +67,17 @@ int main(int argc, char** argv){
cout << "Reading camera poses" << endl; cout << "Reading camera poses" << endl;
ifstream pose_file(pose_loc.c_str()); ifstream pose_file(pose_loc.c_str());
int i; int pose_index;
MatrixRowMajor m(4,4); MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses //read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> i) { while (pose_file >> pose_index) {
for (int i = 0; i < 16; i++) for (int i = 0; i < 16; i++)
pose_file >> m.data()[i]; pose_file >> m.data()[i];
initial_estimate.insert(i, Pose3(m)); initial_estimate.insert(pose_index, Pose3(m));
} }
// landmark keys // landmark keys
size_t l; size_t landmark_key;
// pixel coordinates uL, uR, v (same for left/right images due to rectification) // pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
@ -90,14 +90,14 @@ int main(int argc, char** argv){
SmartFactor::shared_ptr factor(new SmartFactor(model, K)); SmartFactor::shared_ptr factor(new SmartFactor(model, K));
size_t current_l = 3; // hardcoded landmark ID from first measurement size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { while (factor_file >> pose_index >> landmark_key >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) { if(current_l != landmark_key) {
graph.push_back(factor); graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(model, K)); factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
current_l = l; current_l = landmark_key;
} }
factor->add(Point2(uL,v), i); factor->add(Point2(uL,v), pose_index);
} }
Pose3 firstPose = initial_estimate.at<Pose3>(1); Pose3 firstPose = initial_estimate.at<Pose3>(1);