37 #ifndef JSK_PCL_ROS_ROS_COLLABORATIVE_PARTICLE_FILTER_H_ 38 #define JSK_PCL_ROS_ROS_COLLABORATIVE_PARTICLE_FILTER_H_ 40 #include <pcl/tracking/particle_filter.h> 41 #include <pcl/tracking/impl/particle_filter.hpp> 50 template <
typename Po
intInT,
typename StateT>
54 using Tracker<PointInT, StateT>::input_;
55 using ParticleFilterTracker<PointInT, StateT>::particles_;
56 using ParticleFilterTracker<PointInT, StateT>::changed_;
58 typedef typename Tracker<PointInT, StateT>::PointCloudIn
PointCloudIn;
77 ParticleFilterTracker<PointInT, StateT>()
85 particles_ = particles;
108 std::cerr <<
"no particles" << std::endl;
111 std::cerr <<
"no input pointcloud" << std::endl;
115 #pragma omp parallel for 117 for (
size_t i = 0; i < particles_->points.size (); i++) {
126 std::vector<int>
a (particles_->points.size ());
127 std::vector<double>
q (particles_->points.size ());
128 this->genAliasTable (
a,
q, particles_);
132 PointCloudStatePtr origparticles = particles_;
134 particles_->points.reserve(origparticles->points.size() + 1);
140 int motion_num =
static_cast<int> (particles_->points.size ()) * static_cast<int> (motion_ratio_);
141 for (
int i = 1; i < motion_num; i++ ) {
142 int target_particle_index = sampleWithReplacement (
a,
q);
143 StateT
p = origparticles->points[target_particle_index];
146 particles_->points.push_back (p);
150 for (
int i = motion_num; i < particle_num_; i++ ) {
151 int target_particle_index = sampleWithReplacement (
a,
q);
152 StateT
p = origparticles->points[target_particle_index];
155 particles_->points.push_back (p);
169 for (
int i = 0; i < iteration_num_; i++) {
179 for (
size_t i = 0; i < particles_->points.size(); i++) {
180 n = particles_->points[i].weight + n;
183 for (
size_t i = 0; i < particles_->points.size(); i++) {
184 particles_->points[i].weight = particles_->points[i].weight / n;
188 for (
size_t i = 0; i < particles_->points.size(); i++) {
189 particles_->points[i].weight = 1.0 / particles_->points.size();
194 using ParticleFilterTracker<PointInT, StateT>::iteration_num_;
195 using ParticleFilterTracker<PointInT, StateT>::update;
196 using ParticleFilterTracker<PointInT, StateT>::representative_state_;
197 using ParticleFilterTracker<PointInT, StateT>::motion_ratio_;
198 using ParticleFilterTracker<PointInT, StateT>::particle_num_;
199 using ParticleFilterTracker<PointInT, StateT>::motion_;
200 using ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement;
CustomSampleFunc custom_sample_func_
PointCloudIn::ConstPtr PointCloudInConstPtr
boost::shared_ptr< const Coherence > CoherenceConstPtr
bool update(const T &new_val, T &my_val)
boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr
Tracker< PointInT, StateT >::PointCloudState PointCloudState
void setParticles(PointCloudStatePtr particles)
boost::function< StateT(const StateT &)> CustomSampleFunc
PointCloudState::Ptr PointCloudStatePtr
PointCoherence< PointInT > Coherence
PointCloudIn::Ptr PointCloudInPtr
void setCustomSampleFunc(CustomSampleFunc f)
PointCloudCoherence< PointInT > CloudCoherence
PointCloudState::ConstPtr PointCloudStateConstPtr
boost::shared_ptr< ROSCollaborativeParticleFilterTracker > Ptr
Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
CustomLikelihoodFunc custom_likelihood_func_
boost::shared_ptr< CloudCoherence > CloudCoherencePtr
boost::function< void(PointCloudInConstPtr, StateT &)> CustomLikelihoodFunc
void setLikelihoodFunc(CustomLikelihoodFunc f)
ROSCollaborativeParticleFilterTracker()
boost::shared_ptr< Coherence > CoherencePtr