-
Notifications
You must be signed in to change notification settings - Fork 37
/
add_feature.cpp
91 lines (76 loc) · 1.79 KB
/
add_feature.cpp
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
#include "add_feature.h"
#include <math.h>
#include <vector>
#include <iostream>
using namespace std;
//
// add new features
//
void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
{
int lenz = z.cols();
MatrixXf xf(2,lenz);
vector<MatrixXf> Pf;
VectorXf xv = particle.xv();
//TODO: this doesn't match :(
//TODO: fix this!!!
#if 0
cout<<"xv in add_feature"<<endl;
cout<<xv<<endl;
cout<<"should be"<<endl;
cout<<"0.7648"<<endl;
cout<<"0.0221"<<endl;
cout<<"0.0050"<<endl;
cout<<endl;
#endif
float r,b,s,c;
MatrixXf Gz(2,2);
for (int i=0; i<lenz; i++) {
r = z(0,i);
b = z(1,i);
s = sin(xv(2)+b);
c = cos(xv(2)+b);
xf(0,i) = xv(0) + r*c;
xf(1,i) = xv(1) + r*s;
Gz <<c,-r*s,s,r*c;
Pf.push_back(Gz*R*Gz.transpose());
}
int lenx = particle.xf().cols();
vector<int> ii;
for (int i=lenx; i<lenx+lenz; i++) {
ii.push_back(i);
}
MatrixXf xfCopy = particle.xf();
vector<MatrixXf> pfCopy(particle.Pf());
for (unsigned c=0; c<ii.size(); c++) {
#if 0
cout<<"ii"<<endl;
cout<<ii[c]<<endl;
#endif
if (xfCopy.isZero()) {
xfCopy = xf;
} else {
for (unsigned r=0; r<xf.rows(); r++) {
xfCopy(r,ii[c]) = xf(r,c);
}
}
if (pfCopy.empty()) {
pfCopy = Pf;
} else {
pfCopy[ii[c]] = Pf[c];
}
}
#if 0
cout<<"xf in add_feature"<<endl;
cout<<xf<<endl;
cout<<"should be"<<endl;
cout<<"3.4011 25.7814"<<endl;
cout<<"-25.6172 3.6347"<<endl;
cout<<endl;
cout<<"xfCopy"<<endl;
cout<<xfCopy<<endl;
cout<<endl;
#endif
particle.setXf(xfCopy);
particle.setPf(pfCopy);
}