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 #ifndef JSK_PCL_ROS_ROS_COLLABORATIVE_PARTICLE_FILTER_H_
00038 #define JSK_PCL_ROS_ROS_COLLABORATIVE_PARTICLE_FILTER_H_
00039
00040 #include <pcl/tracking/particle_filter.h>
00041 #include <pcl/tracking/impl/particle_filter.hpp>
00042
00043 namespace pcl
00044 {
00045 namespace tracking
00046 {
00047
00048
00049
00050 template <typename PointInT, typename StateT>
00051 class ROSCollaborativeParticleFilterTracker: public ParticleFilterTracker<PointInT, StateT>
00052 {
00053 public:
00054 using Tracker<PointInT, StateT>::input_;
00055 using ParticleFilterTracker<PointInT, StateT>::particles_;
00056 using ParticleFilterTracker<PointInT, StateT>::changed_;
00057 typedef boost::shared_ptr<ROSCollaborativeParticleFilterTracker> Ptr;
00058 typedef typename Tracker<PointInT, StateT>::PointCloudIn PointCloudIn;
00059 typedef typename PointCloudIn::Ptr PointCloudInPtr;
00060 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00061
00062 typedef typename Tracker<PointInT, StateT>::PointCloudState PointCloudState;
00063 typedef typename PointCloudState::Ptr PointCloudStatePtr;
00064 typedef typename PointCloudState::ConstPtr PointCloudStateConstPtr;
00065
00066 typedef PointCoherence<PointInT> Coherence;
00067 typedef boost::shared_ptr< Coherence > CoherencePtr;
00068 typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
00069
00070 typedef PointCloudCoherence<PointInT> CloudCoherence;
00071 typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
00072 typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
00073
00074 typedef boost::function<StateT (const StateT&)> CustomSampleFunc;
00075 typedef boost::function<void (PointCloudInConstPtr, StateT&)> CustomLikelihoodFunc;
00076 ROSCollaborativeParticleFilterTracker():
00077 ParticleFilterTracker<PointInT, StateT>()
00078 {
00079 motion_ratio_ = 0.0;
00080 changed_ = true;
00081 }
00082
00083 void setParticles(PointCloudStatePtr particles)
00084 {
00085 particles_ = particles;
00086 }
00087
00088 void setCustomSampleFunc(CustomSampleFunc f)
00089 {
00090 custom_sample_func_ = f;
00091 }
00092
00093 void setLikelihoodFunc(CustomLikelihoodFunc f)
00094 {
00095 custom_likelihood_func_ = f;
00096 }
00097
00098 protected:
00099 bool initCompute()
00100 {
00101
00102 return true;
00103 }
00104
00105 void weight()
00106 {
00107 if (!particles_) {
00108 std::cerr << "no particles" << std::endl;
00109 }
00110 if (!input_) {
00111 std::cerr << "no input pointcloud" << std::endl;
00112 }
00113 for (size_t i = 0; i < particles_->points.size (); i++) {
00114 custom_likelihood_func_ (input_, particles_->points[i]);
00115 }
00116 normalizeWeight();
00117 }
00118
00119 void
00120 resample()
00121 {
00122 std::vector<int> a (particles_->points.size ());
00123 std::vector<double> q (particles_->points.size ());
00124 this->genAliasTable (a, q, particles_);
00125
00126
00127
00128 PointCloudStatePtr origparticles = particles_;
00129 particles_.reset(new PointCloudState());
00130 particles_->points.reserve(origparticles->points.size() + 1);
00131
00132
00133
00134
00135
00136 int motion_num = static_cast<int> (particles_->points.size ()) * static_cast<int> (motion_ratio_);
00137 for ( int i = 1; i < motion_num; i++ ) {
00138 int target_particle_index = sampleWithReplacement (a, q);
00139 StateT p = origparticles->points[target_particle_index];
00140 p = custom_sample_func_(p);
00141 p = p + motion_;
00142 particles_->points.push_back (p);
00143 }
00144
00145
00146 for ( int i = motion_num; i < particle_num_; i++ ) {
00147 int target_particle_index = sampleWithReplacement (a, q);
00148 StateT p = origparticles->points[target_particle_index];
00149
00150 p = custom_sample_func_(p);
00151 particles_->points.push_back (p);
00152 }
00153 }
00154
00155 bool initParticles(bool)
00156 {
00157
00158 return true;
00159 }
00160
00161 void computeTracking()
00162 {
00163
00164
00165 for (int i = 0; i < iteration_num_; i++) {
00166 resample();
00167 weight();
00168 update();
00169 }
00170 }
00171
00172 void normalizeWeight()
00173 {
00174 double n = 0.0;
00175 for (size_t i = 0; i < particles_->points.size(); i++) {
00176 n = particles_->points[i].weight + n;
00177 }
00178 if (n != 0.0) {
00179 for (size_t i = 0; i < particles_->points.size(); i++) {
00180 particles_->points[i].weight = particles_->points[i].weight / n;
00181 }
00182 }
00183 else {
00184 for (size_t i = 0; i < particles_->points.size(); i++) {
00185 particles_->points[i].weight = 1.0 / particles_->points.size();
00186 }
00187 }
00188 }
00189
00190 using ParticleFilterTracker<PointInT, StateT>::iteration_num_;
00191 using ParticleFilterTracker<PointInT, StateT>::update;
00192 using ParticleFilterTracker<PointInT, StateT>::representative_state_;
00193 using ParticleFilterTracker<PointInT, StateT>::motion_ratio_;
00194 using ParticleFilterTracker<PointInT, StateT>::particle_num_;
00195 using ParticleFilterTracker<PointInT, StateT>::motion_;
00196 using ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement;
00197 CustomSampleFunc custom_sample_func_;
00198 CustomLikelihoodFunc custom_likelihood_func_;
00199 private:
00200 };
00201 }
00202 }
00203
00204 #endif