fix MONO again

1 files changed, 18 insertions, 2 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 05c3b30..70dc32c 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -69,7 +69,7 @@ State::H ( const Quaterniond &q, const vector<measurement_t> &z )
         int col;
         Feature *fi;
         switch ( z[i].z_type ) {
-            case REFLECTION:	
+            case BOTH:	
                 col = rowById(z[i].id);
                 if (col==-1) {
                     fprintf(stderr, "Reflection feature %d not found, quitting.\n", z[i].id);
@@ -92,8 +92,20 @@ State::H ( const Quaterniond &q, const vector<measurement_t> &z )
                 fi = featureById( z[i].id );
                 h.block<4,STATESIZE>(row,0) = fi->Hx(pos,q).block<4,STATESIZE>(0,0);
                 h.block<4,3>(row,col) = fi->Hy(pos,q).block<4,3>(0,0);
+                row += 4;
                 break;
 
+            case REFLECTION:
+                col = rowById(z[i].id);
+                if (col==-1) {
+                    fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id);
+                    exit(1);
+                }
+                fi = featureById( z[i].id );
+                h.block<4,STATESIZE>(row,0) = fi->Hx(pos,q).block<4,STATESIZE>(2,0);
+                h.block<4,3>(row,col) = fi->Hy(pos,q).block<4,3>(2,0);
+                row += 4;
+
             case HEIGHT:	
                 h.block<1,STATESIZE>(row,0) = body->H();
                 row += 1;
@@ -120,10 +132,14 @@ State::Hrows ( const vector<measurement_t> &z)
     int rows = 0;
     for (auto i=z.begin(); i!=z.end(); ++i) {
         switch ( i->z_type ) {
-            case REFLECTION:	
+            case BOTH:
                 rows += 6;
                 break;
 
+            case REFLECTION:	
+                rows += 4;
+                break;
+
             case MONO:	
                 rows += 4;
                 break;