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

added comment and made variables const when possible

release/4.3a0
Luca 2016-07-31 18:47:30 -04:00
parent 55ccc66de2
commit 9336c0b8c0
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -233,15 +233,16 @@ public:
size_t m = cameras.size(); size_t m = cameras.size();
bool retriangulate = decideIfTriangulate(cameras); bool retriangulate = decideIfTriangulate(cameras);
// triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
MonoCameras monoCameras; MonoCameras monoCameras;
MonoMeasurements monoMeasured; MonoMeasurements monoMeasured;
for(size_t i = 0; i < m; i++) { for(size_t i = 0; i < m; i++) {
Pose3 leftPose = cameras[i].pose(); const Pose3 leftPose = cameras[i].pose();
Cal3_S2 monoCal = cameras[i].calibration().calibration(); const Cal3_S2 monoCal = cameras[i].calibration().calibration();
MonoCamera leftCamera_i(leftPose,monoCal); const MonoCamera leftCamera_i(leftPose,monoCal);
Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0)); const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
Pose3 rightPose = leftPose.compose( left_Pose_right ); const Pose3 rightPose = leftPose.compose( left_Pose_right );
MonoCamera rightCamera_i(rightPose,monoCal); const MonoCamera rightCamera_i(rightPose,monoCal);
const StereoPoint2 zi = measured_[i]; const StereoPoint2 zi = measured_[i];
monoCameras.push_back(leftCamera_i); monoCameras.push_back(leftCamera_i);
monoMeasured.push_back(Point2(zi.uL(),zi.v())); monoMeasured.push_back(Point2(zi.uL(),zi.v()));