summaryrefslogtreecommitdiff
path: root/src/state.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/state.cpp')
-rw-r--r--src/state.cpp11
1 files changed, 9 insertions, 2 deletions
diff --git a/src/state.cpp b/src/state.cpp
index a9a55ae..472ee66 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -290,7 +290,9 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
} else if (exists(i->id)) {
Feature *ft;
ft = featureById(i->id);
+#ifdef INLIERTEST
if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, INLIER_THRESHOLD))
+#endif
{
featuresToUpdate.push_back(*i);
}
@@ -319,13 +321,15 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
}
}
if (featuresToUpdate.size()>1) {
+#ifdef DORANSAC
ransacUpdate(featuresToUpdate,q);
- } else if (featuresToUpdate.size()>1) {
+#else /* ----- not DORANSAC ----- */
MatrixXd h;
h = H(q,featuresToUpdate);
MatrixXd S;
S = h*P*h.transpose() + R(featuresToUpdate);
kalmanUpdate(h,S,featuresToUpdate,q);
+#endif /* ----- not DORANSAC ----- */
}
addFeatures( featuresToAdd, q, zmeas);
@@ -405,8 +409,11 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
// Create feature
Feature *f;
if (i->z_type==REFLECTION) {
+#ifdef INITDEPTH
f = new Feature(i->id, i->source, i->reflection, pos, q, z);
- //f = new Feature(i->id, i->source, pos, q);
+#else /* ----- not INITDEPTH ----- */
+ f = new Feature(i->id, i->source, pos, q);
+#endif /* ----- not INITDEPTH ----- */
} else {
f = new Feature(i->id, i->source, pos, q);
}