From 64ef08ed07afe09cf92b9ca6cd8e41f89734302e Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 28 Mar 2017 15:11:33 -0500
Subject: Changes

---
 src/state.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index aaf9cb9..8ad1d9c 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -61,6 +61,7 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
     for (int i=0,row=0; i<z.size(); ++i) {
         switch ( z[i].z_type ) {
             int col;
+            Feature *fi;
             case REFLECTION:	
                 col = rowById(z[i].id);
                 if (col==-1) {
@@ -68,7 +69,6 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
                     exit(1);
                 }
 
-                Feature *fi;
                 fi = featureById(z[i].id);
 
                 h.block<6,9>(row,0) = fi->Hx(pos,q);
@@ -77,8 +77,13 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
                 break;
 
             case MONO:	
-                fprintf(stderr, "mono points not supported.\n");
-                exit(1);
+                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,9>(row,0) = fi->Hx(pos,q);
                 break;
 
             case HEIGHT:	
-- 
cgit v1.1