Feature test code

1 files changed, 125 insertions, 0 deletions

diff --git a/tests/test_feature.cpp b/tests/test_feature.cpp
new file mode 100644
index 0000000..13b4e0a
--- /dev/null
+++ b/tests/test_feature.cpp
@@ -0,0 +1,125 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  test_feature.cpp
+ *
+ *    Description:  Test the feature class.
+ *
+ *        Version:  1.0
+ *        Created:  03/25/2017 12:43:53 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+
+
+#include "test_feature.h"
+
+
+/* 
+ * ===  FUNCTION  ======================================================================
+ *         Name:  test_findDepth
+ *  Description:  Test the Feature::findDepth() method.
+ * =====================================================================================
+ */
+bool
+test_findDepth ( Camera &cam )
+{
+    bool success;
+    UTM pos;
+    pos.northing = 4451490.899494356;
+    pos.easting = 381887.311030777;
+    pos.up = 206.256890383;
+    pos.zone_i = 16;
+    pos.zone_c = 'T';
+
+    // Quat for 16.bmp
+    attitude_t att{ -1.073641623878,2.685395530695,16.357370508605};
+
+    double roll, pitch, yaw;
+    roll = M_PI*att.roll/180.;
+    pitch = M_PI*att.pitch/180.;
+    yaw = M_PI*(att.yaw)/180.;
+    Matrix3d R;
+    Quaterniond q;
+    R = AngleAxisd(yaw, Vector3d::UnitZ())
+      * AngleAxisd(roll, Vector3d::UnitX())
+      *  AngleAxisd(pitch, Vector3d::UnitY());
+    q = Quaterniond(R);
+
+    Matrix<double,26,5> meas;
+    meas << 162,493,260,493,331,
+        66,370,229,369,352,
+        268,763,235,764,364,
+        414,554,250,554,337,
+        452,365,210,365,370,
+        580,353,239,353,344,
+        606,455,253,455,333,
+        680,748,237,747,357,
+        706,650,233,651,358,
+        724,671,199,668,400,
+        736,768,283,768,318,
+        774,368,159,369,370,
+        842,281,221,282,352,
+        882,748,249,748,349,
+        910,386,217,387,369,
+        916,713,210,713,383,
+        966,519,218,519,368,
+        1026,567,264,567,325,
+        1050,686,227,686,361,
+        1070,650,220,650,370,
+        1084,531,267,531,323,
+        1160,331,222,332,353,
+        1184,388,273,388,314,
+        1262,547,219,546,360,
+        1274,314,218,313,354,
+        1278,469,217,469,365;
+
+    double z;
+    z = -0.34;
+    z -= 0.6*3.43*sin(pitch);
+
+    Vector3d xbw;
+    xbw << pos.northing, pos.easting, -pos.up;
+
+    printf("id,utm_z,utm_c,utm-northing,utm-easting,altitude\n");
+    for (int i=0; i<meas.rows(); ++i) {
+        Matrix<double,1,5> row;
+        row = meas.block<1,5>(i,0);
+        int id = row[0];
+
+        Vector3d xsi,xri,xs,xr;
+        xsi << row[1], row[2], 1;
+        xri << row[3], row[4], 1;
+
+        xs = cam.img2body(xsi);
+        xr = cam.img2body(xri);
+
+        Feature f(id,xs,xr,xbw,q,z);
+        if (f.sane()) 
+        {
+            UTM utmi;
+            utmi = f.utm(xbw,q);
+            printf("%d,%d,%c,%f,%f,%f\n", id,utmi.zone_i, utmi.zone_c, utmi.northing, utmi.easting, utmi.up);
+        }
+    }
+
+    success = true;
+    return success;
+}		/* -----  end of function test_findDepth  ----- */
+
+int main(int argc, char **argv)
+{
+    Camera cam(argv[1]);
+    if (test_findDepth(cam)) {
+        ;//printf("PASSED\n");
+    } else {
+        printf("FAILED\n");
+    }
+	return 0;
+}
+