diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index f3b14f3c4..7dec508e0 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -174,6 +174,7 @@ namespace gtsam { Point3 zc = target-eye; zc = zc/zc.norm(); Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down + xc = xc/xc.norm(); Point3 yc = zc.cross(xc); Pose3 pose3(Rot3(xc,yc,zc), eye); return PinholeCamera(pose3, K); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index e918606a1..d0f961bfa 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -74,6 +74,16 @@ TEST( SimpleCamera, lookat) Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); CHECK(assert_equal( camera.pose(), expected)); + + + Point3 C2(30.0,0.0,10.0); + SimpleCamera camera2 = SimpleCamera::lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + + Matrix R = camera2.pose().rotation().matrix(); + Matrix I = trans(R)*R; + gtsam::print(I,"I="); + CHECK(assert_equal(I, eye(3))); + } /* ************************************************************************* */