Full state estimation and vision process are working.

7 files changed, 68 insertions, 22 deletions

diff --git a/src/camera.cpp b/src/camera.cpp
index 2040641..8d0be89 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -143,6 +143,16 @@ Camera::K (  ) const
     return _K ;
 }		/* -----  end of method Camera::K  ----- */
 
+cv::Matx33d
+Camera::K (int x) const
+{
+    cv::Matx33d k;
+    k << _K(0,0), _K(0,1), _K(0,2),
+         _K(1,0), _K(1,1), _K(1,2),
+         _K(2,0), _K(2,1), _K(2,2);
+    return k;
+}
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Camera
@@ -156,6 +166,12 @@ Camera::d ( ) const
     return _d;
 }		/* -----  end of method Camera::d  ----- */
 
+cv::Vec4d
+Camera::d ( int x ) const
+{
+    return cv::Vec4d( _d(0), _d(1), _d(2), _d(3));
+}
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Camera
diff --git a/src/camera.h b/src/camera.h
index 51ba9a6..78808ad 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -9,7 +9,7 @@
 #include "types.h"
 #define BINNING 0.5 // set the binning factor
 #define YAWCORRECT 2.0
-//#define DOYAWCORRECT
+#define DOYAWCORRECT
 using Eigen::Matrix;
 using Eigen::Vector4d;
 using Eigen::Vector3d;
@@ -32,7 +32,9 @@ class Camera
 
         /* ====================  ACCESSORS     ======================================= */
         Vector4d d() const;
+        cv::Vec4d d( int x ) const;
         Matrix<double,3,3> K() const;
+        cv::Matx33d K(int x) const;
         Matrix<double,4,4> K4() const;
         Vector3d principalPoint() const;
         Matrix<double,3,3> Rc2b() const;
diff --git a/src/main.h b/src/main.h
index fb11776..54433a4 100644
--- a/src/main.h
+++ b/src/main.h
@@ -37,7 +37,7 @@
 #define ACCBIASY 0.01465135
 #define ACCBIASZ -1*-0.00709229
 
-#define CANOECENTER 0.88            /* center of gravity of canoe  */
+#define CANOECENTER 0.92            /* center of gravity of canoe  */
 #define CANOEHEIGHT -0.40
 //#define CANOEHEIGHT -0.75
 #define DOWNSAMPLE 1            /*  */
diff --git a/src/state.cpp b/src/state.cpp
index 70dc32c..32801f8 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -172,7 +172,7 @@ State::R ( const vector<measurement_t> &z )
     for (int i=0,row=0; i<z.size(); ++i) {
         Feature *fi;
         switch ( z[i].z_type ) {
-            case REFLECTION:	
+            case BOTH:	
                 fi = featureById(z[i].id);
                 r.block<6,6>(row,row) = fi->R();
                 row += 6;
@@ -184,6 +184,12 @@ State::R ( const vector<measurement_t> &z )
                 row += 4;
                 break;
 
+            case REFLECTION:
+                fi = featureById(z[i].id);
+                r.block<4,4>(row,row) = fi->R().block<4,4>(2,2);
+                row += 4;
+                break;
+
             case HEIGHT:	
                 r.block<1,1>(row,row) = body->R();
                 row += 1;
@@ -291,7 +297,7 @@ State::innovation( const vector<measurement_t> &z, const Quaterniond &q)
     for (int i=0,row=0; i<z.size(); ++i) {
         Feature *fi;
         measurement_type mt=z[i].z_type;
-        if (mt==REFLECTION) {
+        if (mt==BOTH) {
             fi = featureById(z[i].id);
             Matrix<double,6,1> hi, zi;
             hi = fi->h(body->ned(), q);
@@ -305,6 +311,13 @@ State::innovation( const vector<measurement_t> &z, const Quaterniond &q)
             zi = fi->measurement_vector(z[i].source);
             y.segment<4>(row) = zi-hi;
             row += 4;
+        } else if (mt==REFLECTION) {
+            fi = featureById(z[i].id);
+            Matrix<double,4,1> hi, zi;
+            hi = fi->h(body->ned(), q).segment<4>(2);
+            zi = fi->measurement_vector(z[i].source, z[i].reflection).segment<4>(2);
+            y.segment<4>(row) = zi-hi;
+            row += 4;
         } else if (mt==HEIGHT) {
             Matrix<double,1,1> zi, hi;
             zi[0] = z[i].height;
@@ -422,6 +435,8 @@ State::blockSI ( const vector<measurement_t> &z, const Quaterniond &q )
 #if STATESIZE==13
     Quaterniond q = body->qhat();
 #endif
+    fprintf(stderr, "blockSI not implemented\n");
+    exit(1);
     int rows = Hrows(z);
     MatrixXd SI;
     SI = MatrixXd::Zero(rows,rows);
@@ -498,7 +513,7 @@ State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z)
         if (std::find(blacklist.begin(),blacklist.end(), i->id)!=blacklist.end()) continue;
         // Create feature
         Feature *f;
-        if (i->z_type==REFLECTION) {
+        if (i->z_type==BOTH) {
 #ifdef  INITDEPTH
             f = new Feature(i->id, i->source, i->reflection, pos, q, z, i->patch);
 #else      /* -----  not INITDEPTH  ----- */
@@ -507,7 +522,7 @@ State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z)
         } else if (i->z_type==MONO) {
             f = new Feature(i->id, i->source, pos, q, i->patch);
         } else {
-            fprintf(stderr, "Measurement type: %d not known, quitting\n", i->z_type);
+            fprintf(stderr, "Measurement type: %d not supported by addFeatures, quitting\n", i->z_type);
             exit(1);
         }
         if (!f->sane()) {
@@ -1088,7 +1103,7 @@ State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam,
         z.id = (*i)->id();
         z.source = (*i)->p2x(pbs);
         z.reflection = (*i)->p2x(pbr);
-        z.z_type = MONO;
+        z.z_type = BOTH;
         z.patch = (*i)->warpedPatch(cam, q);
         z.refpatch = (*i)->reflectedPatch(cam, q);
 
@@ -1163,8 +1178,9 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
 #else
     featuresAsMeasurements(updatedFeatures, cam, q);
 #endif
-    viz.drawMeasurements(z, cam, cv::Scalar(0,0,100), cv::Scalar(0,0,0), false);
-    viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,0,0), true);
+    cerr << z.size() << endl;
+    viz.drawMeasurements(z, cam, cv::Scalar(0,0,200), cv::Scalar(0,0,200), false);
+    viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,200,0), true);
     viz.show();
 
     return ;
diff --git a/src/state.h b/src/state.h
index 08d71ee..3ec5f10 100644
--- a/src/state.h
+++ b/src/state.h
@@ -11,8 +11,8 @@
 #include "types.h"
 #include "vision.h"
 
-#define MAXFEATURES 40
-#define MINFEATURES 20
+#define MAXFEATURES 70
+#define MINFEATURES 30
 #define COVBIAS 2e-5
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define DORANSAC            /*  */
diff --git a/src/types.h b/src/types.h
index 955e3bf..ac28f6f 100644
--- a/src/types.h
+++ b/src/types.h
@@ -6,8 +6,8 @@
 #include <exception>
 #define MAXLINE 8192
 #define MAXFILENAME 1024
-#define STATESIZE 9            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
-#define INLIER_THRESHOLD 12.
+#define STATESIZE 13            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
+#define INLIER_THRESHOLD 24.
 #define PATCHSIZE 51            /* must be odd */
 using Eigen::Matrix;
 using Eigen::Matrix3d;
diff --git a/src/vision.cpp b/src/vision.cpp
index 0f89b35..4d4ba2e 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -41,19 +41,16 @@ Vision::Vision ()
 void
 Vision::open ( const char *fn, const Camera &cam )
 {
-    Mat bayered, unbayered ;
+    Mat bayered, unbayered, undistorted ;
     bayered = cv::imread(fn, -1);
     if (!bayered.data) {
         fprintf(stderr, "Could not read %s.\n", fn);
         exit(1);
     }
     cv::cvtColor(bayered, unbayered, CV_BayerBG2BGR ,3);
-    unbayered.convertTo(original, CV_32FC3);
-    Mat luv;
+    cv::undistort( unbayered, undistorted, cam.K(1), cam.d(1));
+    undistorted.convertTo(original, CV_32FC3);
     cv::cvtColor(original, gray, CV_BGR2GRAY);
-    //vector<Mat> luv_channels;
-    //cv::split(luv,luv_channels);
-    //gray = luv_channels[0];
     display = unbayered.clone();
     return ;
 }		/* -----  end of method Vision::open  ----- */
@@ -173,6 +170,7 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
     for (auto i=zin.begin(); i!=zin.end(); ++i) {
         measurement_t z;
         z = *i;
+        measurement_type mt = z.z_type;
         if (i->z_type==MONO || i->z_type==BOTH) {
             measurement_t zins, zouts;
             zins = *i;
@@ -180,6 +178,13 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
             measure(cam,zins, zouts);
             z.xcorrmax = zouts.xcorrmax;
             z.source = zouts.source;
+            if (z.xcorrmax<0.9) { //downgrade
+                if (mt==BOTH) {
+                    mt=REFLECTION;
+                } else if (mt==MONO) {
+                    continue;
+                }
+            }
         }
         if (i->z_type==REFLECTION || i->z_type==BOTH) {
             measurement_t zinr, zoutr;
@@ -187,9 +192,16 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
             zinr.z_type = REFLECTION;
             measure(cam, zinr, zoutr);
             z.reflection = zoutr.reflection;
+            if (zoutr.xcorrmax<0.4) { //downgrade
+                if (mt==BOTH) {
+                    mt = MONO;
+                } else if (mt==REFLECTION) {
+                    continue;
+                }
+            }
         }
-        if (z.xcorrmax>0.90)
-            zout.push_back(z);
+        z.z_type = BOTH;
+        zout.push_back(z);
     }
     return ;
 }		/* -----  end of method Vision::measurements  ----- */
@@ -287,7 +299,7 @@ Vision::searchRegion ( const Camera &cam, const measurement_t &z, cv::RotatedRec
     if (z.z_type==MONO) {
         xis = cam.body2img(z.source);
     } else {
-        xis = cam.body2img(z.source);
+        xis = cam.body2img(z.reflection);
     }
     //xis /= xis[2];
     cv::Point ps;