41 #include <std_msgs/Float64.h> 58 assert(static_cast<int>(particle) >= 0 && particle <
_listOfSamples.size());
65 double current_weight;
66 std::vector<WeightedSample<tf::Vector3> >::const_iterator it_los;
69 current_weight = it_los->WeightGet();
70 pos += (it_los->ValueGet() * current_weight);
80 assert(num_samples > 0);
85 for (
unsigned int s = 0;
s < num_samples;
s++)
88 for (
unsigned int i = 0; i < 3; i++)
90 if (v[i] < m[i]) m[i] = v[i];
91 if (v[i] > M[i]) M[i] = v[i];
97 unsigned int row = hist.rows();
98 unsigned int col = hist.columns();
99 unsigned int total = 0;
101 for (
unsigned int r = 1; r <= row; r++)
102 for (
unsigned int c = 1; c <= col; c++)
103 if (hist(r, c) > threshold) total++;
104 std::cout <<
"size total " << total << std::endl;
106 std::vector<geometry_msgs::Point32> points(total);
107 std::vector<float> weights(total);
108 sensor_msgs::ChannelFloat32 channel;
109 for (
unsigned int r = 1; r <= row; r++)
110 for (
unsigned int c = 1; c <= col; c++)
111 if (hist(r, c) > threshold)
113 for (
unsigned int i = 0; i < 3; i++)
114 points[t].
x = m[0] + (step[0] * r);
115 points[t].
y = m[1] + (step[1] * c);
117 weights[t] =
rgb[999 -
static_cast<int>(trunc(std::max(0.0, std::min(999.0, hist(r, c) * 2 * total * total))))];
120 std::cout <<
"points size " << points.size() << std::endl;
121 cloud.header.frame_id =
"base_link";
122 cloud.points = points;
123 channel.name =
"rgb";
124 channel.values = weights;
125 cloud.channels.push_back(channel);
134 unsigned int rows = round((M[0] - m[0]) / step[0]);
135 unsigned int cols = round((M[1] - m[1]) / step[1]);
136 Matrix hist(rows, cols);
140 for (
unsigned int i = 0; i < num_samples; i++)
143 unsigned int r = round(rel[0] / step[0]);
144 unsigned int c = round(rel[1] / step[1]);
145 if (r >= 1 && c >= 1 && r <= rows && c <= cols)
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE Vector3()
TFSIMD_FORCE_INLINE const tfScalar & y() const
virtual ~MCPdfVector()
Destructor.
virtual tf::Vector3 ExpectedValueGet() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
MatrixWrapper::Matrix getHistogram(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const
Get pos histogram from certain area.
vector< WeightedSample< tf::Vector3 > > _listOfSamples
MCPdfVector(unsigned int num_samples)
Constructor.
void getParticleCloud(const tf::Vector3 &step, double threshold, sensor_msgs::PointCloud &cloud) const
Get evenly distributed particle cloud.
virtual WeightedSample< tf::Vector3 > SampleGet(unsigned int particle) const
static const unsigned int NUM_CONDARG
virtual unsigned int numParticlesGet() const