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_pos_vel.h"
00038 #include <assert.h>
00039 #include <vector>
00040 #include <std_msgs/Float64.h>
00041 #include "people_tracking_filter/rgb.h"
00042
00043
00044 using namespace MatrixWrapper;
00045 using namespace BFL;
00046 using namespace tf;
00047
00048 static const unsigned int NUM_CONDARG = 1;
00049
00050
00051 MCPdfPosVel::MCPdfPosVel(unsigned int num_samples)
00052 : MCPdf<StatePosVel> (num_samples, NUM_CONDARG)
00053 {}
00054
00055 MCPdfPosVel::~MCPdfPosVel() {}
00056
00057
00058 WeightedSample<StatePosVel>
00059 MCPdfPosVel::SampleGet(unsigned int particle) const
00060 {
00061 assert((int)particle >= 0 && particle < _listOfSamples.size());
00062 return _listOfSamples[particle];
00063 }
00064
00065
00066 StatePosVel MCPdfPosVel::ExpectedValueGet() const
00067 {
00068 tf::Vector3 pos(0, 0, 0);
00069 tf::Vector3 vel(0, 0, 0);
00070 double current_weight;
00071 std::vector<WeightedSample<StatePosVel> >::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().pos_ * current_weight);
00076 vel += (it_los->ValueGet().vel_ * current_weight);
00077 }
00078 return StatePosVel(pos, vel);
00079 }
00080
00081
00083 void MCPdfPosVel::getParticleCloud(const tf::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().pos_;
00088 Vector3 M = _listOfSamples[0].ValueGet().pos_;
00089
00090
00091 for (unsigned int s = 0; s < num_samples; s++)
00092 {
00093 Vector3 v = _listOfSamples[s].ValueGet().pos_;
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 = getHistogramPos(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
00111 vector<geometry_msgs::Point32> points(total);
00112 vector<float> weights(total);
00113 sensor_msgs::ChannelFloat32 channel;
00114 for (unsigned int r = 1; r <= row; r++)
00115 for (unsigned int c = 1; c <= col; c++)
00116 if (hist(r, c) > threshold)
00117 {
00118 for (unsigned int i = 0; i < 3; i++)
00119 points[t].x = m[0] + (step[0] * r);
00120 points[t].y = m[1] + (step[1] * c);
00121 points[t].z = m[2];
00122 weights[t] = rgb[999 - (int)trunc(max(0.0, min(999.0, hist(r, c) * 2 * total * total)))];
00123 t++;
00124 }
00125 cloud.header.frame_id = "odom_combined";
00126 cloud.points = points;
00127 channel.name = "rgb";
00128 channel.values = weights;
00129 cloud.channels.push_back(channel);
00130 }
00131
00132
00134 MatrixWrapper::Matrix MCPdfPosVel::getHistogramPos(const Vector3& m, const Vector3& M, const Vector3& step) const
00135 {
00136 return getHistogram(m, M, step, true);
00137 }
00138
00139
00141 MatrixWrapper::Matrix MCPdfPosVel::getHistogramVel(const Vector3& m, const Vector3& M, const Vector3& step) const
00142 {
00143 return getHistogram(m, M, step, false);
00144 }
00145
00146
00148 MatrixWrapper::Matrix MCPdfPosVel::getHistogram(const Vector3& m, const Vector3& M, const Vector3& step, bool pos_hist) const
00149 {
00150 unsigned int num_samples = _listOfSamples.size();
00151 unsigned int rows = round((M[0] - m[0]) / step[0]);
00152 unsigned int cols = round((M[1] - m[1]) / step[1]);
00153 Matrix hist(rows, cols);
00154 hist = 0;
00155
00156
00157 for (unsigned int i = 0; i < num_samples; i++)
00158 {
00159 Vector3 rel;
00160 if (pos_hist)
00161 rel = _listOfSamples[i].ValueGet().pos_ - m;
00162 else
00163 rel = _listOfSamples[i].ValueGet().vel_ - m;
00164
00165 unsigned int r = round(rel[0] / step[0]);
00166 unsigned int c = round(rel[1] / step[1]);
00167 if (r >= 1 && c >= 1 && r <= rows && c <= cols)
00168 hist(r, c) += _listOfSamples[i].WeightGet();
00169 }
00170
00171 return hist;
00172 }
00173
00174
00175
00176 unsigned int
00177 MCPdfPosVel::numParticlesGet() const
00178 {
00179 return _listOfSamples.size();
00180 }
00181
00182