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); }