30 using namespace gtsam;
33 static double fov = 60;
34 static size_t w=640,
h=480;
47 TEST( ProjectionFactor, nonStandard ) {
52 TEST( ProjectionFactor, Constructor) {
62 TEST( ProjectionFactor, ConstructorWithTransform) {
73 TEST( ProjectionFactor, Equals ) {
84 TEST( ProjectionFactor, EqualsWithTransform ) {
96 TEST( ProjectionFactor, Error ) {
118 TEST( ProjectionFactor, ErrorWithTransform ) {
141 TEST( ProjectionFactor, Jacobian ) {
153 Matrix H1Actual, H2Actual;
157 Matrix H1Expected = (
Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
158 Matrix H2Expected = (
Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
166 TEST( ProjectionFactor, JacobianWithTransform ) {
179 Matrix H1Actual, H2Actual;
183 Matrix H1Expected = (
Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
184 Matrix H2Expected = (
Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();