diff options
Diffstat (limited to 'src/feature.cpp')
| -rw-r--r-- | src/feature.cpp | 34 |
1 files changed, 32 insertions, 2 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index 2afd5a0..fd4a85f 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -25,13 +25,18 @@ * Description: constructor *-------------------------------------------------------------------------------------- */ -Feature::Feature (const Vector3d &pib, const Vector3d &xbw, const Quaterniond q ) /* constructor */ +Feature::Feature ( int id, const Vector3d &xib, const Vector3d &xpb, + const Vector3d &xbw, const Quaterniond &q ) /* constructor */ { - X = pib; + _id = id; + X = x2p(xib); + X[2] = 1./20.; xb0w = xbw; q0 = q; } /* ----- end of method Feature::Feature (constructor) ----- */ +Feature::Feature () {;} + /* *-------------------------------------------------------------------------------------- @@ -368,3 +373,28 @@ Feature::Hy ( const Vector3d &pos, const Quaterniond &q ) return H ; } /* ----- end of method Feature::Hy ----- */ +int +Feature::id ( ) +{ + return _id; +} /* ----- end of method Feature::id ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Feature + * Method: Feature :: P0 + * Description: Returns the initial P matrix. + *-------------------------------------------------------------------------------------- + */ +Matrix<double,3,3> +Feature::P0 ( ) +{ + Matrix<double,3,3> P; + double p0 = 1e-4; + double p1 = 1e-2; + P << p0, 0., 0., + 0., p0, 0., + 0., 0., p1; + return P; +} /* ----- end of method Feature::P0 ----- */ + |