diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 11 |
1 files changed, 8 insertions, 3 deletions
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: |