1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
|
#ifndef feature_INC
#define feature_INC
#include <cv.h>
#include <Eigen/Dense>
#include <iostream>
#include "types.h"
#define FEATURE_NOISE 1e-3 /* Feature process noise */
#define VIEW_NOISE 5e-2 /* */
#define INITIAL_VIEW_NOISE 1e-1 /* */
#define REFLECTION_VIEW_NOISE 5e-2 /* */
#define FEATURECOVX .01 /* */
#define FEATURECOVY .01 /* */
#define FEATURECOVRHO 25e-4 /* */
#define FEATURECOVRHO_MONO 0.5 /* */
#define RHO_0 1./10. /* */
#define RANSAC_LI_THRESHOLD 4e-5 /* */
#define RANSAC_HI_THRESHOLD 5e-2 /* */
#define INITDEPTH
using cv::Mat;
using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;
using std::cout;
using std::cerr;
using std::endl;
/*
* =====================================================================================
* Class: Feature
* Description:
* =====================================================================================
*/
class Feature
{
public:
/* ==================== LIFECYCLE ======================================= */
Feature ( int id, const Vector3d &xs, const Vector3d &xr,
const Vector3d &xbw, const Quaterniond &q, double z, Mat &p);
Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
const Quaterniond &q, Mat &p);
/* ==================== ACCESSORS ======================================= */
int id();
Matrix<double,3,3> P0(measurement_type t);
Vector3d asVector();
UTM utm(Vector3d &xbw, Quaterniond &q);
Mat patch();
/* ==================== MUTATORS ======================================= */
void asVector(const Vector3d &m);
void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
void dx( const Vector3d &del);
/* ==================== OPERATORS ======================================= */
bool sane();
bool isInlier(const measurement_t &z, const Matrix<double,STATESIZE,STATESIZE> &Pxx,
const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy,
const Vector3d &pos, const Quaterniond &q, double thr);
Matrix<double,3,STATESIZE> Fx( double dt );
Matrix<double,6,STATESIZE> Hx( const Vector3d &pos, const Quaterniond &q);
Matrix<double,6,6> S ( const Matrix<double,STATESIZE,STATESIZE> &Pxx,
const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy,
const Vector3d &pos, const Quaterniond &q);
bool isRansacInlier(const measurement_t &z, const Vector3d &pos, const Quaterniond &q);
bool inFOV();
Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs,
const Vector3d &xr);
Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt);
Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
Matrix<double,3,6> L();
Matrix<double,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr);
Matrix<double,4,1> measurement_vector( const Vector3d &xs );
Matrix<double,3,3> Q(const double dt);
Matrix<double,6,6> R();
Vector3d p2x(const Vector3d &p);
Vector3d x2p(const Vector3d &x);
protected:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
private:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
int _id;
Mat _patch;
Quaterniond q0;
Vector3d X;
Vector3d X0;
Vector3d xb0w;
}; /* ----- end of class Feature ----- */
#endif /* ----- #ifndef feature_INC ----- */
|
