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

added intermediate camera variable for clarity

release/4.3a0
lcarlone 2021-11-07 14:56:06 -05:00
parent b1baf6c8b3
commit c105aa4e1e
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -295,11 +295,12 @@ class SmartProjectionPoseFactorRollingShutter
double interpolationFactor = alphas_[i]; double interpolationFactor = alphas_[i];
const Pose3& w_P_body = const Pose3& w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor); interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]];
const Pose3& body_P_cam = camera_i.pose();
const Pose3& w_P_cam = w_P_body.compose(body_P_cam); const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, cameras.emplace_back(w_P_cam,
make_shared<typename CAMERA::CalibrationType>( make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration())); camera_i.calibration()));
} }
return cameras; return cameras;
} }
@ -347,10 +348,10 @@ class SmartProjectionPoseFactorRollingShutter
auto w_P_body = auto w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor, interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]];
auto body_P_cam = camera_i.pose();
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
PinholeCamera<CALIBRATION> camera( PinholeCamera<CALIBRATION> camera(w_P_cam, camera_i.calibration());
w_P_cam, cameraRig_[cameraIds_[i]].calibration());
// get jacobians and error vector for current measurement // get jacobians and error vector for current measurement
Point2 reprojectionError_i = Point2 reprojectionError_i =