diff --git a/.cproject b/.cproject
index 4228b4ac0..ed6db2627 100644
--- a/.cproject
+++ b/.cproject
@@ -317,14 +317,6 @@
true
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -351,6 +343,7 @@
make
+
tests/testBayesTree.run
true
false
@@ -358,6 +351,7 @@
make
+
testBinaryBayesNet.run
true
false
@@ -405,6 +399,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -412,6 +407,7 @@
make
+
tests/testSymbolicFactor.run
true
false
@@ -419,6 +415,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -434,11 +431,20 @@
make
+
tests/testBayesTree
true
false
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -527,22 +533,6 @@
false
true
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
make
-j2
@@ -559,6 +549,22 @@
true
true
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
make
-j2
@@ -583,42 +589,26 @@
true
true
-
+
make
- -j5
- testValues.run
+ -j2
+ all
true
true
true
-
+
make
- -j5
- testOrdering.run
+ -j2
+ check
true
true
true
-
+
make
- -j5
- testKey.run
- true
- true
- true
-
-
- make
- -j5
- testLinearContainerFactor.run
- true
- true
- true
-
-
- make
- -j6 -j8
- testWhiteNoiseFactor.run
+ -j2
+ clean
true
true
true
@@ -663,26 +653,42 @@
true
true
-
+
make
- -j2
- all
+ -j5
+ testValues.run
true
true
true
-
+
make
- -j2
- check
+ -j5
+ testOrdering.run
true
true
true
-
+
make
- -j2
- clean
+ -j5
+ testKey.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testLinearContainerFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j6 -j8
+ testWhiteNoiseFactor.run
true
true
true
@@ -1073,6 +1079,7 @@
make
+
testGraph.run
true
false
@@ -1080,6 +1087,7 @@
make
+
testJunctionTree.run
true
false
@@ -1087,6 +1095,7 @@
make
+
testSymbolicBayesNetB.run
true
false
@@ -1254,6 +1263,7 @@
make
+
testErrors.run
true
false
@@ -1299,14 +1309,6 @@
true
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -1387,6 +1389,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -1611,6 +1621,30 @@
true
true
+
+ make
+ -j5
+ timeCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testPinholeCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ timePinholeCamera.run
+ true
+ true
+ true
+
make
-j2
@@ -1693,7 +1727,6 @@
make
-
testSimulated2DOriented.run
true
false
@@ -1733,7 +1766,6 @@
make
-
testSimulated2D.run
true
false
@@ -1741,7 +1773,6 @@
make
-
testSimulated3D.run
true
false
@@ -2013,7 +2044,6 @@
make
-
tests/testGaussianISAM2
true
false
@@ -2035,102 +2065,6 @@
true
true
-
- make
- -j2
- testRot3.run
- true
- true
- true
-
-
- make
- -j2
- testRot2.run
- true
- true
- true
-
-
- make
- -j2
- testPose3.run
- true
- true
- true
-
-
- make
- -j2
- timeRot3.run
- true
- true
- true
-
-
- make
- -j2
- testPose2.run
- true
- true
- true
-
-
- make
- -j2
- testCal3_S2.run
- true
- true
- true
-
-
- make
- -j2
- testSimpleCamera.run
- true
- true
- true
-
-
- make
- -j2
- testHomography2.run
- true
- true
- true
-
-
- make
- -j2
- testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPoint2.run
- true
- true
- true
-
make
-j3
@@ -2332,6 +2266,7 @@
cpack
+
-G DEB
true
false
@@ -2339,6 +2274,7 @@
cpack
+
-G RPM
true
false
@@ -2346,6 +2282,7 @@
cpack
+
-G TGZ
true
false
@@ -2353,6 +2290,7 @@
cpack
+
--config CPackSourceConfig.cmake
true
false
@@ -2518,34 +2456,98 @@
true
true
-
+
make
- -j5
- testSpirit.run
+ -j2
+ testRot3.run
true
true
true
-
+
make
- -j5
- testWrap.run
+ -j2
+ testRot2.run
true
true
true
-
+
make
- -j5
- check.wrap
+ -j2
+ testPose3.run
true
true
true
-
+
make
- -j5
- wrap
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
true
true
true
@@ -2589,6 +2591,38 @@
false
true
+
+ make
+ -j5
+ testSpirit.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testWrap.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.wrap
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap
+ true
+ true
+ true
+
diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp
new file mode 100644
index 000000000..3a8075c0c
--- /dev/null
+++ b/gtsam/geometry/tests/testPinholeCamera.cpp
@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testPinholeCamera.cpp
+ * @author Frank Dellaert
+ * @brief test PinholeCamera class
+ */
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+using namespace gtsam;
+
+static const Cal3_S2 K(625, 625, 0, 0, 0);
+
+static const Pose3 pose1(Matrix_(3,3,
+ 1., 0., 0.,
+ 0.,-1., 0.,
+ 0., 0.,-1.
+ ),
+ Point3(0,0,0.5));
+
+typedef PinholeCamera Camera;
+static const Camera camera(pose1, K);
+
+static const Point3 point1(-0.08,-0.08, 0.0);
+static const Point3 point2(-0.08, 0.08, 0.0);
+static const Point3 point3( 0.08, 0.08, 0.0);
+static const Point3 point4( 0.08,-0.08, 0.0);
+
+/* ************************************************************************* */
+TEST( PinholeCamera, constructor)
+{
+ CHECK(assert_equal( camera.calibration(), K));
+ CHECK(assert_equal( camera.pose(), pose1));
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, level2)
+{
+ // Create a level camera, looking in Y-direction
+ Pose2 pose2(0.4,0.3,M_PI/2.0);
+ Camera camera = Camera::Level(K, pose2, 0.1);
+
+ // expected
+ Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
+ Rot3 wRc(x,y,z);
+ Pose3 expected(wRc,Point3(0.4,0.3,0.1));
+ CHECK(assert_equal( camera.pose(), expected));
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, lookat)
+{
+ // Create a level camera, looking in Y-direction
+ Point3 C(10.0,0.0,0.0);
+ Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
+
+ // expected
+ 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);
+ Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
+
+ Matrix R = camera2.pose().rotation().matrix();
+ Matrix I = trans(R)*R;
+ CHECK(assert_equal(I, eye(3)));
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, project)
+{
+ CHECK(assert_equal( camera.project(point1), Point2(-100, 100) ));
+ CHECK(assert_equal( camera.project(point2), Point2(-100, -100) ));
+ CHECK(assert_equal( camera.project(point3), Point2( 100, -100) ));
+ CHECK(assert_equal( camera.project(point4), Point2( 100, 100) ));
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, backproject)
+{
+ CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
+ CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
+ CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
+ CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, backproject2)
+{
+ Point3 origin;
+ Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
+ Camera camera(Pose3(rot, origin), K);
+
+ Point3 actual = camera.backproject(Point2(), 1.);
+ Point3 expected(0., 1., 0.);
+ pair x = camera.projectSafe(expected);
+
+ CHECK(assert_equal(expected, actual));
+ CHECK(assert_equal(Point2(), x.first));
+ CHECK(x.second);
+}
+
+/* ************************************************************************* */
+static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& cal) {
+ return Camera(pose,cal).project(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, Dproject_point_pose)
+{
+ Matrix Dpose, Dpoint, Dcal;
+ Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
+ Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K);
+ Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K);
+ Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K);
+ CHECK(assert_equal(result, Point2(-100, 100) ));
+ CHECK(assert_equal(Dpose, numerical_pose, 1e-7));
+ CHECK(assert_equal(Dpoint, numerical_point,1e-7));
+ CHECK(assert_equal(Dcal, numerical_cal,1e-7));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
+
+