12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix a bad bug in PinholeCamera::lookat: normalize the result vector of cross product!!!

release/4.3a0
Duy-Nguyen Ta 2012-06-07 06:53:03 +00:00
parent b10f4d09e3
commit 43fb6b76e9
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/geometry/PinholeCamera.h
View File

@ -174,6 +174,7 @@ namespace gtsam {
Point3 zc = target-eye; Point3 zc = target-eye;
zc = zc/zc.norm(); zc = zc/zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
xc = xc/xc.norm();
Point3 yc = zc.cross(xc); Point3 yc = zc.cross(xc);
Pose3 pose3(Rot3(xc,yc,zc), eye); Pose3 pose3(Rot3(xc,yc,zc), eye);
return PinholeCamera(pose3, K); return PinholeCamera(pose3, K);

10
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -74,6 +74,16 @@ TEST( SimpleCamera, lookat)
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C); Pose3 expected(Rot3(xc,yc,zc),C);
CHECK(assert_equal( camera.pose(), expected)); 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)));
} }
/* ************************************************************************* */ /* ************************************************************************* */