Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "people_tracking_filter/mcpdf_vector.h"
00038 #include "people_tracking_filter/state_pos_vel.h"
00039 #include <assert.h>
00040 #include <vector>
00041 #include <std_msgs/Float64.h>
00042 #include "people_tracking_filter/rgb.h"
00043
00044
00045 using namespace MatrixWrapper;
00046 using namespace BFL;
00047 using namespace tf;
00048
00049 static const unsigned int NUM_CONDARG = 1;
00050
00051
00052 MCPdfVector::MCPdfVector(unsigned int num_samples)
00053 : MCPdf<Vector3> (num_samples, NUM_CONDARG)
00054 {}
00055
00056 MCPdfVector::~MCPdfVector() {}
00057
00058
00059 WeightedSample<Vector3>
00060 MCPdfVector::SampleGet(unsigned int particle) const
00061 {
00062 assert((int)particle >= 0 && particle < _listOfSamples.size());
00063 return _listOfSamples[particle];
00064 }
00065
00066
00067 Vector3 MCPdfVector::ExpectedValueGet() const
00068 {
00069 Vector3 pos(0, 0, 0);
00070 double current_weight;
00071 std::vector<WeightedSample<Vector3> >::const_iterator it_los;
00072 for (it_los = _listOfSamples.begin() ; it_los != _listOfSamples.end() ; it_los++)
00073 {
00074 current_weight = it_los->WeightGet();
00075 pos += (it_los->ValueGet() * current_weight);
00076 }
00077
00078 return Vector3(pos);
00079 }
00080
00081
00083 void MCPdfVector::getParticleCloud(const Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const
00084 {
00085 unsigned int num_samples = _listOfSamples.size();
00086 assert(num_samples > 0);
00087 Vector3 m = _listOfSamples[0].ValueGet();
00088 Vector3 M = _listOfSamples[0].ValueGet();
00089
00090
00091 for (unsigned int s = 0; s < num_samples; s++)
00092 {
00093 Vector3 v = _listOfSamples[s].ValueGet();
00094 for (unsigned int i = 0; i < 3; i++)
00095 {
00096 if (v[i] < m[i]) m[i] = v[i];
00097 if (v[i] > M[i]) M[i] = v[i];
00098 }
00099 }
00100
00101
00102 Matrix hist = getHistogram(m, M, step);
00103 unsigned int row = hist.rows();
00104 unsigned int col = hist.columns();
00105 unsigned int total = 0;
00106 unsigned int t = 0;
00107 for (unsigned int r = 1; r <= row; r++)
00108 for (unsigned int c = 1; c <= col; c++)
00109 if (hist(r, c) > threshold) total++;
00110 cout << "size total " << total << endl;
00111
00112 vector<geometry_msgs::Point32> points(total);
00113 vector<float> weights(total);
00114 sensor_msgs::ChannelFloat32 channel;
00115 for (unsigned int r = 1; r <= row; r++)
00116 for (unsigned int c = 1; c <= col; c++)
00117 if (hist(r, c) > threshold)
00118 {
00119 for (unsigned int i = 0; i < 3; i++)
00120 points[t].x = m[0] + (step[0] * r);
00121 points[t].y = m[1] + (step[1] * c);
00122 points[t].z = m[2];
00123 weights[t] = rgb[999 - (int)trunc(max(0.0, min(999.0, hist(r, c) * 2 * total * total)))];
00124 t++;
00125 }
00126 cout << "points size " << points.size() << endl;
00127 cloud.header.frame_id = "base_link";
00128 cloud.points = points;
00129 channel.name = "rgb";
00130 channel.values = weights;
00131 cloud.channels.push_back(channel);
00132 }
00133
00134
00136 MatrixWrapper::Matrix MCPdfVector::getHistogram(const Vector3& m, const Vector3& M, const Vector3& step) const
00137 {
00138 unsigned int num_samples = _listOfSamples.size();
00139 unsigned int rows = round((M[0] - m[0]) / step[0]);
00140 unsigned int cols = round((M[1] - m[1]) / step[1]);
00141 Matrix hist(rows, cols);
00142 hist = 0;
00143
00144
00145 for (unsigned int i = 0; i < num_samples; i++)
00146 {
00147 Vector3 rel(_listOfSamples[i].ValueGet() - m);
00148 unsigned int r = round(rel[0] / step[0]);
00149 unsigned int c = round(rel[1] / step[1]);
00150 if (r >= 1 && c >= 1 && r <= rows && c <= cols)
00151 hist(r, c) += _listOfSamples[i].WeightGet();
00152 }
00153
00154 return hist;
00155 }
00156
00157
00158
00159 unsigned int
00160 MCPdfVector::numParticlesGet() const
00161 {
00162 return _listOfSamples.size();
00163 }
00164
00165