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