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 "cob_people_tracking_filter/mcpdf_vector.h"
00038 #include "cob_people_tracking_filter/state_pos_vel.h"
00039 #include <assert.h>
00040 #include <vector>
00041 #include <std_msgs/Float64.h>
00042 #include "cob_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 current_weight = it_los->WeightGet();
00074 pos += (it_los->ValueGet() * current_weight);
00075 }
00076
00077 return Vector3(pos);
00078 }
00079
00080
00082 void MCPdfVector::getParticleCloud(const Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const
00083 {
00084 unsigned int num_samples = _listOfSamples.size();
00085 assert(num_samples > 0);
00086 Vector3 m = _listOfSamples[0].ValueGet();
00087 Vector3 M = _listOfSamples[0].ValueGet();
00088
00089
00090 for (unsigned int s=0; s<num_samples; s++){
00091 Vector3 v = _listOfSamples[s].ValueGet();
00092 for (unsigned int i=0; i<3; i++){
00093 if (v[i] < m[i]) m[i] = v[i];
00094 if (v[i] > M[i]) M[i] = v[i];
00095 }
00096 }
00097
00098
00099 Matrix hist = getHistogram(m, M, step);
00100 unsigned int row = hist.rows();
00101 unsigned int col = hist.columns();
00102 unsigned int total = 0;
00103 unsigned int t = 0;
00104 for (unsigned int r=1; r<= row; r++)
00105 for (unsigned int c=1; c<= col; c++)
00106 if (hist(r,c) > threshold) total++;
00107 cout << "size total " << total << endl;
00108
00109 vector<geometry_msgs::Point32> points(total);
00110 vector<float> weights(total);
00111 sensor_msgs::ChannelFloat32 channel;
00112 for (unsigned int r=1; r<= row; r++)
00113 for (unsigned int c=1; c<= col; c++)
00114 if (hist(r,c) > threshold){
00115 for (unsigned int i=0; i<3; i++)
00116 points[t].x = m[0] + (step[0] * r);
00117 points[t].y = m[1] + (step[1] * c);
00118 points[t].z = m[2];
00119 weights[t] = rgb[999-(int)trunc(max(0.0,min(999.0,hist(r,c)*2*total*total)))];
00120 t++;
00121 }
00122 cout << "points size " << points.size() << endl;
00123 cloud.header.frame_id = "base_link";
00124 cloud.points = points;
00125 channel.name = "rgb";
00126 channel.values = weights;
00127 cloud.channels.push_back(channel);
00128 }
00129
00130
00132 MatrixWrapper::Matrix MCPdfVector::getHistogram(const Vector3& m, const Vector3& M, const Vector3& step) const
00133 {
00134 unsigned int num_samples = _listOfSamples.size();
00135 unsigned int rows = round((M[0]-m[0])/step[0]);
00136 unsigned int cols = round((M[1]-m[1])/step[1]);
00137 Matrix hist(rows, cols);
00138 hist = 0;
00139
00140
00141 for (unsigned int i=0; i<num_samples; i++){
00142 Vector3 rel(_listOfSamples[i].ValueGet() - m);
00143 unsigned int r = round(rel[0] / step[0]);
00144 unsigned int c = round(rel[1] / step[1]);
00145 if (r >= 1 && c >= 1 && r <= rows && c <= cols)
00146 hist(r,c) += _listOfSamples[i].WeightGet();
00147 }
00148
00149 return hist;
00150 }
00151
00152
00153
00154 unsigned int
00155 MCPdfVector::numParticlesGet() const
00156 {
00157 return _listOfSamples.size();
00158 }
00159
00160