forked from vaiv/Random-Dot-Marker-SLAM
-
Notifications
You must be signed in to change notification settings - Fork 0
/
dot.h
110 lines (92 loc) · 2.96 KB
/
dot.h
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
106
107
108
109
110
#ifndef DOT_H
#define DOT_H
#include <opencv2/opencv.hpp>
#include <iostream>
struct DistComp
{
float distance;
cv::Point2f Center;
float angle;
DistComp(float _distance,cv::Point2f _Center)
{
distance = _distance;
Center = _Center;
}
int operator() (DistComp D)
{
return distance < D.distance;
}
int operator< (DistComp D)
{
return distance < D.distance;
}
};
struct Comparator
{
bool operator() (DistComp D, DistComp E)
{
return D.distance > E.distance;
}
};
struct angle_Comp
{
bool operator() (DistComp D, DistComp E)
{
return D.angle > E.angle;
}
};
class Dot
{
std::vector<DistComp> Neighbors;
float resol; //scaling parameter
float thresh1; //threshold for confidence of individual descriptor
float thresh2; //threshold for voting scheme
int nn_count; //count of nearest neighbors considered
int selector; //size of subset considered for a descriptor
std::vector<cv::Point2f> Centers; //Neighbor Centers
float tolerance; // tolerance factor for bruteforce descriptor matcher
long int idx; //Marker Id
cv::Point2f COM; //Centroid of the Neighbor-Point Locus
cv::Vec3f Circle; //circle inscribing the dot
cv::Mat frame;
std::vector< std::vector<int> > Descriptors;
cv::Point3f Point_3D;
////Non linear Motion model parameters: constant update interval assumption.
cv::Point2f prev_position;
cv::Point2f prev_pred_position;
cv::Vec2f prev_velocity;
cv::Point2f predicted_position;
float roi_radius;
cv::Vec2f curr_velocity;
cv::Vec2f acc;
float pred_error;
float prev_pred_error;
float pred_error_gradient;
float expansion_const;
void findNN();
void computeCOM();
void computeDescriptors(std::vector<DistComp> m_Neighbors);
double getMatch(std::vector<int> candidate);
float computeCrossRatio(std::vector<cv::Point2f> Vertices);
float computeCrossRatio_4Points(std::vector<cv::Point2f> Vertices);
float area(cv::Point2f pt1,cv::Point2f pt2, cv::Point2f pt3);
public:
Dot(cv::Vec3f Circle,cv::Mat frame,int nn_count,int selector,float thresh1,float thresh2,float resol);
cv::Point2f getCOM();
std::vector< std::vector<int> > getDescriptors();
void setCenters(std::vector<cv::Point2f> Centers);
void draw(cv::Mat& img);
void assignDescriptors();
void setId(long int id){this->idx = id;}
long int getId(){return idx;}
bool matchDescriptors(std::vector< std::vector<int> > poss_match);
cv::Point2f getCenter();
void set3DPos(cv::Point3f pt);
void get3DPos(cv::Point3f& pt);
cv::Point3f get3DPos();
////
void updateMotionModel(Dot Candidate);
void resetMotionModel();
bool withinROI(cv::Point2f Candidate);
};
#endif // DOT_H