diff options
author | Martin Miller | 2017-03-28 10:06:47 -0500 |
---|---|---|
committer | Martin Miller | 2017-03-28 10:06:47 -0500 |
commit | fb4b8328bfa5a8930b87ec8b7465b8032f4873c5 (patch) | |
tree | f1998951d3b9b10d2feec8423ebec1b85a184a48 /src/state.cpp | |
parent | 9533fbcda07254b65a53a9109555662d9a09086c (diff) | |
download | refslam-fb4b8328bfa5a8930b87ec8b7465b8032f4873c5.zip refslam-fb4b8328bfa5a8930b87ec8b7465b8032f4873c5.tar.gz |
move aboveWater
() back to main
Diffstat (limited to 'src/state.cpp')
-rw-r--r-- | src/state.cpp | 7 |
1 files changed, 4 insertions, 3 deletions
diff --git a/src/state.cpp b/src/state.cpp index aee0caa..aaf9cb9 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -260,9 +260,11 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni { std::vector<measurement_t> featuresToAdd; std::vector<measurement_t> featuresToUpdate; + double zmeas = body->ned()[2]; for (auto i=z.begin(); i!=z.end(); ++i) { if (i->z_type==HEIGHT) { featuresToUpdate.push_back(*i); + zmeas = i->height; } else if (exists(i->id)) { featuresToUpdate.push_back(*i); } else { @@ -280,16 +282,15 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni #endif /* ----- not FULLS ----- */ } - addFeatures( featuresToAdd, q); + addFeatures( featuresToAdd, q, zmeas); return ; } /* ----- end of method State::feature_update ----- */ void -State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) +State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double z) { // Add new features - double z = aboveWater(q); Vector3d pos = body->ned(); for (auto i=F.begin(); i!=F.end(); ++i) { if (features.size()>MAXFEATURES) break; |