40 #include <std_msgs/Float64.h> 51 MCPdfPosVel::MCPdfPosVel(
unsigned int num_samples)
58 WeightedSample<StatePosVel>
61 assert((
int)particle >= 0 && particle < _listOfSamples.size());
62 return _listOfSamples[particle];
70 double current_weight;
71 std::vector<WeightedSample<StatePosVel> >::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().pos_ * current_weight);
76 vel += (it_los->ValueGet().vel_ * current_weight);
85 unsigned int num_samples = _listOfSamples.size();
86 assert(num_samples > 0);
87 Vector3 m = _listOfSamples[0].ValueGet().pos_;
88 Vector3 M = _listOfSamples[0].ValueGet().pos_;
91 for (
unsigned int s = 0;
s < num_samples;
s++)
93 Vector3 v = _listOfSamples[
s].ValueGet().pos_;
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++;
111 vector<geometry_msgs::Point32> points(total);
112 vector<float> weights(total);
113 sensor_msgs::ChannelFloat32 channel;
114 for (
unsigned int r = 1; r <= row; r++)
115 for (
unsigned int c = 1; c <= col; c++)
116 if (hist(r, c) > threshold)
118 for (
unsigned int i = 0; i < 3; i++)
119 points[t].
x = m[0] + (step[0] * r);
120 points[t].
y = m[1] + (step[1] * c);
122 weights[t] =
rgb[999 - (int)trunc(max(0.0,
min(999.0, hist(r, c) * 2 * total * total)))];
125 cloud.header.frame_id =
"odom_combined";
126 cloud.points = points;
127 channel.name =
"rgb";
128 channel.values = weights;
129 cloud.channels.push_back(channel);
150 unsigned int num_samples = _listOfSamples.size();
151 unsigned int rows = round((M[0] - m[0]) / step[0]);
152 unsigned int cols = round((M[1] - m[1]) / step[1]);
153 Matrix hist(rows, cols);
157 for (
unsigned int i = 0; i < num_samples; i++)
161 rel = _listOfSamples[i].ValueGet().pos_ - m;
163 rel = _listOfSamples[i].ValueGet().vel_ - m;
165 unsigned int r = round(rel[0] / step[0]);
166 unsigned int c = round(rel[1] / step[1]);
167 if (r >= 1 && c >= 1 && r <= rows && c <= cols)
168 hist(r, c) += _listOfSamples[i].WeightGet();
179 return _listOfSamples.size();
MatrixWrapper::Matrix getHistogramPos(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const
Get pos histogram from certain area.
Class representing state with pos and vel.
virtual ~MCPdfPosVel()
Destructor.
static const unsigned int NUM_CONDARG
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual StatePosVel ExpectedValueGet() const
void getParticleCloud(const tf::Vector3 &step, double threshold, sensor_msgs::PointCloud &cloud) const
Get evenly distributed particle cloud.
virtual unsigned int numParticlesGet() const
MatrixWrapper::Matrix getHistogram(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step, bool pos_hist) const
Get histogram from certain area.
virtual WeightedSample< StatePosVel > SampleGet(unsigned int particle) const
MatrixWrapper::Matrix getHistogramVel(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const
Get vel histogram from certain area.