PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
const PinholeCamera<CALIBRATION>& camera) {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 cameraPose = camera.pose();
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
typename gtsam::traits<CALIBRATION>::TangentVector d;
d.setRandom();
d *= 0.1;
CALIBRATION perturbedCalibration = camera.calibration().retract(d);
return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
}
