41 #include <std_msgs/Float64.h> 52 MCPdfVector::MCPdfVector(
unsigned int num_samples)
59 WeightedSample<Vector3>
62 assert((
int)particle >= 0 && particle < _listOfSamples.size());
63 return _listOfSamples[particle];
70 double current_weight;
71 std::vector<WeightedSample<Vector3> >::const_iterator it_los;
72 for (it_los = _listOfSamples.begin() ; it_los != _listOfSamples.end() ; it_los++)
74 current_weight = it_los->WeightGet();
75 pos += (it_los->ValueGet() * current_weight);
85 unsigned int num_samples = _listOfSamples.size();
86 assert(num_samples > 0);
87 Vector3 m = _listOfSamples[0].ValueGet();
88 Vector3 M = _listOfSamples[0].ValueGet();
91 for (
unsigned int s = 0;
s < num_samples;
s++)
93 Vector3 v = _listOfSamples[
s].ValueGet();
94 for (
unsigned int i = 0; i < 3; i++)
96 if (v[i] < m[i]) m[i] = v[i];
97 if (v[i] > M[i]) M[i] = v[i];
103 unsigned int row = hist.rows();
104 unsigned int col = hist.columns();
105 unsigned int total = 0;
107 for (
unsigned int r = 1; r <= row; r++)
108 for (
unsigned int c = 1; c <= col; c++)
109 if (hist(r, c) > threshold) total++;
110 cout <<
"size total " << total << endl;
112 vector<geometry_msgs::Point32> points(total);
113 vector<float> weights(total);
114 sensor_msgs::ChannelFloat32 channel;
115 for (
unsigned int r = 1; r <= row; r++)
116 for (
unsigned int c = 1; c <= col; c++)
117 if (hist(r, c) > threshold)
119 for (
unsigned int i = 0; i < 3; i++)
120 points[t].
x = m[0] + (step[0] * r);
121 points[t].
y = m[1] + (step[1] * c);
123 weights[t] =
rgb[999 - (int)trunc(max(0.0,
min(999.0, hist(r, c) * 2 * total * total)))];
126 cout <<
"points size " << points.size() << endl;
127 cloud.header.frame_id =
"base_link";
128 cloud.points = points;
129 channel.name =
"rgb";
130 channel.values = weights;
131 cloud.channels.push_back(channel);
138 unsigned int num_samples = _listOfSamples.size();
139 unsigned int rows = round((M[0] - m[0]) / step[0]);
140 unsigned int cols = round((M[1] - m[1]) / step[1]);
141 Matrix hist(rows, cols);
145 for (
unsigned int i = 0; i < num_samples; i++)
147 Vector3 rel(_listOfSamples[i].ValueGet() - m);
148 unsigned int r = round(rel[0] / step[0]);
149 unsigned int c = round(rel[1] / step[1]);
150 if (r >= 1 && c >= 1 && r <= rows && c <= cols)
151 hist(r, c) += _listOfSamples[i].WeightGet();
162 return _listOfSamples.size();
virtual tf::Vector3 ExpectedValueGet() const
virtual WeightedSample< tf::Vector3 > SampleGet(unsigned int particle) const
static const unsigned int NUM_CONDARG
TFSIMD_FORCE_INLINE const tfScalar & z() const
void getParticleCloud(const tf::Vector3 &step, double threshold, sensor_msgs::PointCloud &cloud) const
Get evenly distributed particle cloud.
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual unsigned int numParticlesGet() const
virtual ~MCPdfVector()
Destructor.
MatrixWrapper::Matrix getHistogram(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const
Get pos histogram from certain area.