← Back to branch summary

~cyphylab/cyphy/bonsai

« back to all changes in this revision

Viewing changes to people_mapping/trajectory_clustering/src/Cluster.cpp

  • Committer: liz.murphy at edu
  • Date: 2012-06-08 01:58:16 UTC
  • Revision ID: liz.murphy@qut.edu.au-20120608015816-obcb29a4ifaejgd2
Cleaned out people mapping

Show diffs side-by-side

added added

removed removed

Lines of Context:
people_mapping/trajectory_clustering/src/Cluster.cpp
1
 
#include "Cluster.h"
2
 
#include <Eigen/Dense>
3
 
#include "ros/ros.h"
4
 
#include <cmath>
5
 
#include <limits>
6
 
#include <algorithm>
7
 
 
8
 
Cluster::Cluster(Trajectory &traj, int parent)
9
 
{
10
 
    m_nID = s_nIDGenerator++;
11
 
    std::vector<double> xtmp;
12
 
    traj.getPoses(m_xPoses,m_yPoses);
13
 
    m_sigmaPoses.resize(m_xPoses.size(),s_defaultSigma);
14
 
    ROS_INFO("Created cluster from vector length %d, cluster length %d",traj.size(),(int)m_xPoses.size());
15
 
}
16
 
 
17
 
Cluster::Cluster(Trajectory &traj)
18
 
{
19
 
    m_nID = s_nIDGenerator++;
20
 
    std::vector<double> xtmp;
21
 
    traj.getPoses(m_xPoses,m_yPoses);
22
 
    m_sigmaPoses.resize(m_xPoses.size(),s_defaultSigma);
23
 
    ROS_INFO("Created cluster from vector length %d, cluster length %d",traj.size(),(int)m_xPoses.size());
24
 
}
25
 
 
26
 
bool Cluster::concatenate(Cluster &cluster)
27
 
{
28
 
    m_xPoses.insert(m_xPoses.end(),cluster.m_xPoses.begin(), cluster.m_xPoses.end());
29
 
    m_yPoses.insert(m_yPoses.end(),cluster.m_yPoses.begin(), cluster.m_yPoses.end());
30
 
    m_sigmaPoses.insert(m_sigmaPoses.end(),cluster.m_sigmaPoses.begin(), cluster.m_sigmaPoses.end());
31
 
    return true;
32
 
}
33
 
 
34
 
void Cluster::print()
35
 
{
36
 
    ROS_INFO("Cluster PRINT, cluster has %d elements", (int)m_xPoses.size());
37
 
    for(unsigned int i=0; i<m_xPoses.size(); i++)
38
 
        ROS_INFO("Pose %d, X pos %f, Y pose %f",i,m_xPoses[i],m_yPoses[i]);
39
 
}
40
 
 
41
 
double Cluster::windowDistance(double pX, double pY, int nInd, double delta)
42
 
{
43
 
    int indL = std::floor((1-delta)*nInd);
44
 
    int indH = std::ceil((1+delta)*nInd);
45
 
 
46
 
    if(indL < 0)
47
 
        indL = 0;
48
 
 
49
 
    if(indH >= (int)m_xPoses.size())
50
 
        indH = m_xPoses.size()-1;
51
 
 
52
 
    if(indH-indL == 0)
53
 
        indH = indL+2;
54
 
 
55
 
     Eigen::Map<Eigen::VectorXd> xs(&m_xPoses[indL],indH-indL);
56
 
     Eigen::Map<Eigen::VectorXd> ys(&m_yPoses[indL],indH-indL);
57
 
     Eigen::Map<Eigen::VectorXd> sigmas(&m_sigmaPoses[indL],indH-indL);
58
 
 
59
 
     Eigen::ArrayXd tPointx(xs.size());
60
 
     Eigen::ArrayXd tPointy(xs.size());
61
 
     Eigen::ArrayXd diffx(xs.size());
62
 
     Eigen::ArrayXd diffy(xs.size());
63
 
     Eigen::ArrayXd dist(xs.size());
64
 
     tPointx.setConstant(pX);
65
 
     tPointy.setConstant(pY); 
66
 
 
67
 
     diffx = tPointx-xs.array();
68
 
     diffy = tPointy-ys.array();
69
 
 
70
 
     dist = diffx*diffx + diffy*diffy;
71
 
     dist = dist.sqrt()/sigmas.array(); 
72
 
     return dist.minCoeff();
73
 
}
74
 
 
75
 
double Cluster::clusterDistance(Trajectory &traj)
76
 
{
77
 
    double theSum = 0.0;
78
 
    for(int i=0; i<traj.size(); i++)
79
 
    {
80
 
        int ind = findClosest(traj.getX(i),traj.getY(i));
81
 
        theSum=theSum+windowDistance(traj.getX(i),traj.getY(i),ind,s_dfDelta);
82
 
    }
83
 
    return theSum/traj.size();
84
 
}
85
 
 
86
 
double Cluster::clusterDistance(Trajectory &traj, int ind_lim)
87
 
{
88
 
    double theSum = 0.0;
89
 
    for(int i=0; i<ind_lim; i++)
90
 
    {
91
 
        int ind = findClosest(traj.getX(i),traj.getY(i));
92
 
        theSum=theSum+windowDistance(traj.getX(i),traj.getY(i),ind,s_dfDelta);
93
 
    }
94
 
    return theSum/ind_lim;
95
 
}
96
 
 
97
 
 
98
 
double Cluster::ccDistance(Cluster &cluster)
99
 
{
100
 
    double theSum = 0.0;
101
 
    for(int i=0; i<cluster.size(); i++)
102
 
    {
103
 
        int ind = findClosest(cluster.getX(i),cluster.getY(i));
104
 
        theSum=theSum+windowDistance(cluster.getX(i),cluster.getY(i),ind,s_dfDelta);
105
 
    }
106
 
    return theSum/cluster.size();
107
 
}
108
 
 
109
 
 
110
 
int Cluster::findClosest(double pX, double pY)
111
 
{ 
112
 
    Eigen::Map<Eigen::VectorXd> xs(&m_xPoses[0],m_xPoses.size());
113
 
    Eigen::Map<Eigen::VectorXd> ys(&m_yPoses[0],m_yPoses.size());
114
 
    Eigen::Map<Eigen::VectorXd> sigmas(&m_sigmaPoses[0],m_sigmaPoses.size());
115
 
    Eigen::ArrayXd tPointx(xs.size());
116
 
    Eigen::ArrayXd tPointy(xs.size());
117
 
    
118
 
    Eigen::ArrayXd diffx(xs.size());
119
 
    Eigen::ArrayXd diffy(xs.size());
120
 
    Eigen::ArrayXd dist(xs.size());
121
 
    
122
 
    tPointx.setConstant(pX);
123
 
    tPointy.setConstant(pY); 
124
 
 
125
 
    diffx = tPointx-xs.array();
126
 
    diffy = tPointy-ys.array();
127
 
 
128
 
    dist = diffx*diffx + diffy*diffy;
129
 
    dist = dist.sqrt()/sigmas.array(); 
130
 
    int coeff;
131
 
    dist.minCoeff(&coeff);
132
 
 
133
 
 
134
 
    return coeff;
135
 
}
136
 
 
137
 
int Cluster::findClosest(double pX, double pY, double &retdist)
138
 
{ 
139
 
    Eigen::Map<Eigen::VectorXd> xs(&m_xPoses[0],m_xPoses.size());
140
 
    Eigen::Map<Eigen::VectorXd> ys(&m_yPoses[0],m_yPoses.size());
141
 
    Eigen::Map<Eigen::VectorXd> sigmas(&m_sigmaPoses[0],m_sigmaPoses.size());
142
 
    Eigen::ArrayXd tPointx(xs.size());
143
 
    Eigen::ArrayXd tPointy(xs.size());
144
 
    
145
 
    Eigen::ArrayXd diffx(xs.size());
146
 
    Eigen::ArrayXd diffy(xs.size());
147
 
    Eigen::ArrayXd dist(xs.size());
148
 
    
149
 
    tPointx.setConstant(pX);
150
 
    tPointy.setConstant(pY); 
151
 
 
152
 
    diffx = tPointx-xs.array();
153
 
    diffy = tPointy-ys.array();
154
 
 
155
 
    dist = diffx*diffx + diffy*diffy;
156
 
    dist = dist.sqrt()/sigmas.array(); 
157
 
    int coeff;
158
 
    retdist=dist.minCoeff(&coeff);
159
 
 
160
 
    return coeff;
161
 
}
162
 
 
163
 
bool Cluster::split(Trajectory &traj, Cluster &newCluster, bool hasChild)
164
 
{
165
 
    I_DONT_TRUST_YOU=1;
166
 
    double dist;
167
 
    int traj_ind,ind;
168
 
    for(traj_ind=traj.size()-1; traj_ind>0; --traj_ind)
169
 
    {
170
 
        ind = findClosest(traj.getX(traj_ind),traj.getY(traj_ind),dist);
171
 
        ROS_INFO("At trajectory %d, closest on cluster is %d at distance %f", traj_ind, ind, dist);
172
 
        if(dist < 2)  // make this a parameter!!!
173
 
            break;  // found the closest within reasonable shot of the trajectory
174
 
    }
175
 
 
176
 
    I_DONT_TRUST_YOU=0;
177
 
    if(ind < 1 || ind > m_xPoses.size()-1)
178
 
    {
179
 
        ROS_INFO("Can't split, closest point is at %d on cluster of length %d",ind,m_xPoses.size());
180
 
        return false;
181
 
 
182
 
    }
183
 
    int iw = traj.findClosest(m_xPoses[ind],m_yPoses[ind]);
184
 
    ROS_INFO("Splitting current trajectory at %d, it has length %d",iw,traj.size());
185
 
    traj.splitAfter(iw);
186
 
    ROS_INFO("Now it has length %d",traj.size());
187
 
 
188
 
    if(!hasChild)  // only split off from stub if it hasn't already been done
189
 
    {
190
 
        newCluster.m_xPoses.resize(m_xPoses.size()-ind);
191
 
        newCluster.m_yPoses.resize(m_xPoses.size()-ind);
192
 
        newCluster.m_sigmaPoses.resize(m_xPoses.size()-ind);
193
 
        copy(m_xPoses.begin()+ind, m_xPoses.end(),newCluster.m_xPoses.begin());
194
 
        copy(m_yPoses.begin()+ind, m_yPoses.end(),newCluster.m_yPoses.begin());
195
 
        copy(m_sigmaPoses.begin()+ind, m_sigmaPoses.end(),newCluster.m_sigmaPoses.begin());
196
 
        if(newCluster.m_xPoses.size() > 1)
197
 
        {
198
 
            std::cout << "First part of split OK\n";
199
 
        }
200
 
        else
201
 
        {
202
 
            std::cout << "First part of split is JUNK!, has size " << newCluster.m_xPoses.size();
203
 
            return false;
204
 
        }
205
 
        m_xPoses.erase(m_xPoses.begin()+ind,m_xPoses.end());
206
 
        m_yPoses.erase(m_yPoses.begin()+ind,m_yPoses.end());
207
 
        m_sigmaPoses.erase(m_sigmaPoses.begin()+ind,m_sigmaPoses.end());
208
 
 
209
 
        if(m_xPoses.size() > 1)
210
 
        {
211
 
            std::cout << "Second part of split OK\n";
212
 
        }
213
 
        else
214
 
        {
215
 
            std::cout << "Second part of split is JUNK!, has size " << m_xPoses.size();
216
 
            return false;
217
 
        }
218
 
    }
219
 
 
220
 
    return true;
221
 
}
222
 
 
223
 
bool Cluster::update(double pX, double pY, int i)
224
 
{
225
 
    m_xPoses[i]=(1-s_dfAlpha)*m_xPoses[i]+s_dfAlpha*pX;
226
 
    m_yPoses[i]=(1-s_dfAlpha)*m_yPoses[i]+s_dfAlpha*pY;
227
 
    double xdiff = pX-m_xPoses[i];
228
 
    double ydiff = pY-m_yPoses[i];
229
 
    m_sigmaPoses[i]=(1-s_dfAlpha)*m_sigmaPoses[i]+s_dfAlpha*(xdiff*xdiff+ydiff*ydiff);
230
 
    return true;
231
 
}
232
 
 
233
 
bool Cluster::weightedAv(Cluster &c2)
234
 
{
235
 
    for(int i=0; i < c2.size(); i++)
236
 
    {
237
 
        int ind = findClosest(c2.getX(i),c2.getY(i));
238
 
        double sigma_sq_1 = m_sigmaPoses[ind]*m_sigmaPoses[ind]; 
239
 
        double sigma_sq_2 = c2.getSigma(i)*c2.getSigma(i);
240
 
        m_xPoses[ind] = (m_xPoses[ind]/sigma_sq_1+c2.getX(i)/sigma_sq_2)/(1.0/sigma_sq_1 + 1.0/sigma_sq_2);
241
 
        m_yPoses[ind] = m_yPoses[ind]/sigma_sq_1+c2.getY(i)/sigma_sq_2/(1.0/sigma_sq_1 + 1.0/sigma_sq_2);
242
 
        m_sigmaPoses[ind] = 1.0/(1.0/sigma_sq_1+1.0/sigma_sq_2);
243
 
    }
244
 
    return true;
245
 
}
246
 
 
247
 
double Cluster::startDist(double pX, double pY)
248
 
{
249
 
    double xdiff = pX-m_xPoses[0];
250
 
    double ydiff = pY-m_yPoses[0];
251
 
    return std::sqrt(xdiff*xdiff+ydiff*ydiff);
252
 
}
253
 
 
254
 
std::ostream& operator<<(std::ostream& output, Cluster& c)
255
 
{
256
 
    output << c.getID();
257
 
    return output;
258
 
}
259