diff --git a/.cproject b/.cproject
index c593a801b..11d2ab33e 100644
--- a/.cproject
+++ b/.cproject
@@ -307,14 +307,6 @@
true
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -341,6 +333,7 @@
make
+
tests/testBayesTree.run
true
false
@@ -348,6 +341,7 @@
make
+
testBinaryBayesNet.run
true
false
@@ -395,6 +389,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -402,6 +397,7 @@
make
+
tests/testSymbolicFactor.run
true
false
@@ -409,6 +405,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -424,11 +421,20 @@
make
+
tests/testBayesTree
true
false
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -517,22 +523,6 @@
false
true
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
make
-j2
@@ -549,6 +539,22 @@
true
true
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
make
-j2
@@ -573,26 +579,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
+ -j2
+ clean
true
true
true
@@ -677,26 +683,26 @@
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
@@ -951,6 +957,7 @@
make
+
testGraph.run
true
false
@@ -958,6 +965,7 @@
make
+
testJunctionTree.run
true
false
@@ -965,6 +973,7 @@
make
+
testSymbolicBayesNetB.run
true
false
@@ -1092,6 +1101,7 @@
make
+
testErrors.run
true
false
@@ -1465,6 +1475,14 @@
true
true
+
+ make
+ -j5
+ testSimpleCamera.run
+ true
+ true
+ true
+
make
-j2
@@ -1547,7 +1565,6 @@
make
-
testSimulated2DOriented.run
true
false
@@ -1587,7 +1604,6 @@
make
-
testSimulated2D.run
true
false
@@ -1595,7 +1611,6 @@
make
-
testSimulated3D.run
true
false
@@ -1811,7 +1826,6 @@
make
-
tests/testGaussianISAM2
true
false
@@ -1833,102 +1847,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
-j2
@@ -2130,6 +2048,7 @@
cpack
+
-G DEB
true
false
@@ -2137,6 +2056,7 @@
cpack
+
-G RPM
true
false
@@ -2144,6 +2064,7 @@
cpack
+
-G TGZ
true
false
@@ -2151,6 +2072,7 @@
cpack
+
--config CPackSourceConfig.cmake
true
false
@@ -2244,42 +2166,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_gtsam
+ -j2
+ timeRot3.run
true
true
true
-
+
make
- -j5
- wrap
+ -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
@@ -2323,6 +2301,46 @@
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_gtsam
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap
+ true
+ true
+ true
+
diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp
new file mode 100644
index 000000000..d441716cb
--- /dev/null
+++ b/gtsam/geometry/SimpleCamera.cpp
@@ -0,0 +1,49 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SimpleCamera.cpp
+ * @brief A simple camera class with a Cal3_S2 calibration
+ * @date June 30, 2012
+ * @author Frank Dellaert
+ */
+
+#include
+#include
+
+namespace gtsam {
+
+ SimpleCamera simpleCamera(const Matrix& P) {
+
+ // P = [A|a] = s K cRw [I|-T], with s the unknown scale
+ Matrix A = P.topLeftCorner(3, 3);
+ Vector a = P.col(3);
+
+ // do RQ decomposition to get s*K and cRw angles
+ Matrix sK;
+ Vector xyz;
+ boost::tie(sK, xyz) = RQ(A);
+
+ // Recover scale factor s and K
+ double s = sK(2, 2);
+ Matrix K = sK / s;
+
+ // Recover cRw itself, and its inverse
+ Rot3 cRw = Rot3::RzRyRx(xyz);
+ Rot3 wRc = cRw.inverse();
+
+ // Now, recover T from a = - s K cRw T = - A T
+ Vector T = -(A.inverse() * a);
+ return SimpleCamera(Pose3(wRc, T),
+ Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
+ }
+
+}
diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index d3b0cdbdc..abe42f1a0 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -22,5 +22,10 @@
#include
namespace gtsam {
+
+ /// A simple camera class with a Cal3_S2 calibration
typedef PinholeCamera SimpleCamera;
+
+ /// Recover camera from 3*4 camera matrix
+ SimpleCamera simpleCamera(const Matrix& P);
}
diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp
index 6cc00de8a..a1647fa47 100644
--- a/gtsam/geometry/tests/testSimpleCamera.cpp
+++ b/gtsam/geometry/tests/testSimpleCamera.cpp
@@ -133,6 +133,26 @@ TEST( SimpleCamera, Dproject_point_pose)
CHECK(assert_equal(Dpoint, numerical_point,1e-7));
}
+/* ************************************************************************* */
+TEST( SimpleCamera, simpleCamera)
+{
+ Cal3_S2 K(468.2,427.2,91.2,300,200);
+ Rot3 R(
+ 0.41380,0.90915,0.04708,
+ -0.57338,0.22011,0.78917,
+ 0.70711,-0.35355,0.61237);
+ Point3 T(1000,2000,1500);
+ SimpleCamera expected(Pose3(R.inverse(),T),K);
+ // H&Z example, 2nd edition, page 163
+ Matrix P = Matrix_(3,4,
+ 3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6,
+ -1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5,
+ 7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2);
+ SimpleCamera actual = simpleCamera(P);
+ // Note precision of numbers given in book
+ CHECK(assert_equal(expected, actual,1e-1));
+}
+
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */