diff options
author | Martin Miller | 2017-03-28 15:11:33 -0500 |
---|---|---|
committer | Martin Miller | 2017-03-28 15:11:33 -0500 |
commit | 64ef08ed07afe09cf92b9ca6cd8e41f89734302e (patch) | |
tree | c89f152634102efbb48e05642fd7f77cea497f72 /src/state.cpp | |
parent | fb4b8328bfa5a8930b87ec8b7465b8032f4873c5 (diff) | |
download | refslam-64ef08ed07afe09cf92b9ca6cd8e41f89734302e.zip refslam-64ef08ed07afe09cf92b9ca6cd8e41f89734302e.tar.gz |
Changes
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: |