diff options
Diffstat (limited to 'src/feature.cpp')
| -rw-r--r-- | src/feature.cpp | 8 |
1 files changed, 5 insertions, 3 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index caa56f0..a75fdf0 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -54,6 +54,8 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw, pib[1] = xib[2]; pib[2] = RHO_0; + X = pib; + X0 = X; xb0w = xbw; q0 = q; @@ -531,11 +533,11 @@ Feature::P0 ( measurement_type t ) double p0 = FEATURECOVX; double p1 = FEATURECOVY; double p2; - if (t==REFLECTION) { +#ifdef INITDEPTH p2 = FEATURECOVRHO; - } else { +#else p2 = FEATURECOVRHO_MONO; - } +#endif P << p0, 0., 0., 0., p1, 0., 0., 0., p2; |