1 files changed, 54 insertions, 0 deletions
diff --git a/tests/test_camera.cpp b/tests/test_camera.cpp
new file mode 100644
index 0000000..f864f20
--- /dev/null
+++ b/tests/test_camera.cpp
@@ -0,0 +1,54 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  test_camera.cpp
+ *
+ *    Description:  Tests the methods of the Camera class
+ *
+ *        Version:  1.0
+ *        Created:  03/27/2017 12:19:51 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+
+
+#include <iostream>
+#include "camera.h"
+#define TOL 1e-12
+
+using std::cout;
+using std::cerr;
+using std::endl;
+using namespace Eigen;
+
+int main(int argc, char **argv)
+{
+    Camera cam("left.yml");
+    MatrixXd k;
+    k = cam.K();
+    assert(k.cols()==3);
+    assert(k.rows()==3);
+
+    k = cam.K4();
+    assert(k.cols()==4);
+    assert(k.rows()==4);
+
+    Vector3d xi,xb;
+    // Feature 114 0000009362.bmp.txt
+    // from pyslam: [[ 0.9237706 ] [-0.36529118] [-0.11493579]]
+    Vector3d xb0;
+    xb0 <<  0.9237706, -0.36529118, -0.11493579;
+    xi << 236,216,1;
+    xb = cam.img2body(xi);
+    xb -= xb0;
+    assert(abs(xb[0])<TOL);
+    assert(abs(xb[1])<TOL);
+    assert(abs(xb[2])<TOL);
+	return 0;
+}
+