Skip to content

Commit

Permalink
[CORE] Made the vpParticleFilter a template class to be able to use o…
Browse files Browse the repository at this point in the history
…ther things than a vpColVector for the measurements e.g. vpImage, vpMatrix, pcl::PointCloud ...
  • Loading branch information
rlagneau committed Jul 1, 2024
1 parent 9057c9f commit 9ac540b
Show file tree
Hide file tree
Showing 3 changed files with 399 additions and 414 deletions.
14 changes: 7 additions & 7 deletions example/particle-filter/pf-nonlinear-example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -568,26 +568,26 @@ int main(const int argc, const char *argv[])
//! [Constants_for_the_PF]

//! [Initial_estimates]
vpColVector X0(4); // The initial guess for the state
vpColVector X0(4U); // The initial guess for the state
X0[0] = radius; // wX = radius m
X0[1] = 0.; // wY = 0m
X0[2] = 0.95 * wZ; // Wrong estimation of the position along the z-axis: error of 5%
X0[3] = 0.95 * w * dt; // Wrong estimation of the pulsation: error of 5%
//! [Initial_estimates]

//! [Init_functions]
vpParticleFilter::vpProcessFunction processFunc = fx;
vpParticleFilter<vpColVector>::vpProcessFunction processFunc = fx;
vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaLikelihood, sigmaMeasurements, seed);
using std::placeholders::_1;
using std::placeholders::_2;
vpParticleFilter::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihoodParticle, &markerMeas, _1, _2);
vpParticleFilter::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter::simpleResamplingCheck;
vpParticleFilter::vpResamplingFunction resamplingFunc = vpParticleFilter::simpleImportanceResampling;
vpParticleFilter<vpColVector>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihoodParticle, &markerMeas, _1, _2);
vpParticleFilter<vpColVector>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<vpColVector>::simpleResamplingCheck;
vpParticleFilter<vpColVector>::vpResamplingFunction resamplingFunc = vpParticleFilter<vpColVector>::simpleImportanceResampling;
//! [Init_functions]

//! [Init_PF]
// Initialize the PF
vpParticleFilter filter(nbParticles, stdevsPF, seedPF, nbThread);
vpParticleFilter<vpColVector> filter(nbParticles, stdevsPF, seedPF, nbThread);
filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc);
//! [Init_PF]

Expand Down Expand Up @@ -628,7 +628,7 @@ int main(const int argc, const char *argv[])
//! [Init_simu]
// Initialize the simulation
vpObjectSimulator object(radius, w, phi, wZ);
vpColVector object_pos(4, 0.);
vpColVector object_pos(4U, 0.);
object_pos[3] = 1.;
//! [Init_simu]

Expand Down
Loading

0 comments on commit 9ac540b

Please sign in to comment.