diff options
Diffstat (limited to 'src/state.cpp')
-rw-r--r-- | src/state.cpp | 3 |
1 files changed, 2 insertions, 1 deletions
diff --git a/src/state.cpp b/src/state.cpp index 4677595..94e8a5c 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -21,7 +21,7 @@ State::State ( ) { body = new Body; P = Matrix<double,Dynamic,Dynamic>::Zero(9,9); - P.block<3,3>(6,6) = 1e-6*Matrix3d::Identity(); + P.block<3,3>(6,6) = COVBIAS*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ @@ -421,6 +421,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double Feature *f; if (i->z_type==REFLECTION) { f = new Feature(i->id, i->source, i->reflection, pos, q, z); + //f = new Feature(i->id, i->source, pos, q); } else { f = new Feature(i->id, i->source, pos, q); } |