From 43fb6b76e98728fbdd0fb125a8c4dd9e52197093 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Thu, 7 Jun 2012 06:53:03 +0000 Subject: [PATCH] fix a bad bug in PinholeCamera::lookat: normalize the result vector of cross product!!! --- gtsam/geometry/PinholeCamera.h | 1 + gtsam/geometry/tests/testSimpleCamera.cpp | 10 ++++++++++ 2 files changed, 11 insertions(+) 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))); + } /* ************************************************************************* */