25 using namespace gtsam;
33 StereoPoint2 a(1, 2, 3),
b(4, 5, 6),
c(5, 7, 9),
d(3, 3, 3);
60 Point3 one_meter_z(0, 0, 1);
61 Pose3 camPose3(unit, one_meter_z);
116 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
118 #else // otherwise project should not throw the exception
139 Rot3 R(0.589511291, -0.804859792, 0.0683931805,
140 -0.804435942, -0.592650676, -0.0405925523,
141 0.0732045588, -0.0310882277, -0.996832359);
142 Point3 t(53.5239823, 23.7866016, -4.42379876);
162 Point3 expected_point(1.2, 2.3, 4.5);
165 Matrix actual_jacobian_1, actual_jacobian_2;
166 Point3 actual_point = stereoCam2.
backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2);
178 Rot3 R(0.589511291, -0.804859792, 0.0683931805,
179 -0.804435942, -0.592650676, -0.0405925523,
180 0.0732045588, -0.0310882277, -0.996832359);
181 Point3 t(53.5239823, 23.7866016, -4.42379876);
186 Matrix actual_jacobian_1, actual_jacobian_2;
187 Point3 l =
camera.backproject2(
z, actual_jacobian_1, actual_jacobian_2);