/* ************************************************************************* */ TEST( dataSet, openGL2gtsam) { Vector3 rotVec(0.2, 0.7, 1.1); Rot3 R = Rot3::Expmap(rotVec); Point3 t = Point3(0.0,0.0,0.0); Pose3 poseGTSAM = Pose3(R,t); Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z()); Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns! Rot3 cRw( r1.x(), r2.x(), r3.x(), -r1.y(), -r2.y(), -r3.y(), -r1.z(), -r2.z(), -r3.z()); Rot3 wRc = cRw.inverse(); Pose3 actual = Pose3(wRc,t); EXPECT(assert_equal(expected,actual)); }