本文整理汇总了C++中ParticleSet::end方法的典型用法代码示例。如果您正苦于以下问题：C++ ParticleSet::end方法的具体用法？C++ ParticleSet::end怎么用？C++ ParticleSet::end使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ParticleSet的用法示例。
在下文中一共展示了ParticleSet::end方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: update
/**
* Updates the particle set according to the motion.
*
* @TODO Currently assumes odometry is how FakeOdometryModule creates it,
* ie (delta X, delta Y, etc...
* Verify when motion module is pulled
* @return the updated ParticleSet.
*/
void MotionSystem::update(ParticleSet& particles,
const messages::RobotLocation& deltaMotionInfo)
{
ParticleIt iter;
for(iter = particles.begin(); iter != particles.end(); iter++)
{
Particle* particle = &(*iter);
/** Should be used if odometry gives global **/
/** Should also be TESTED extensively **/
// float sinh, cosh;
// sincosf(deltaMotionInfo.h() - particle->getLocation().h(),
// &sinh, &cosh);
// float changeX = cosh * deltaMotionInfo.x() + sinh * deltaMotionInfo.y();
// float changeY = cosh * deltaMotionInfo.y() - sinh * deltaMotionInfo.x();
float changeX = deltaMotionInfo.x();
float changeY = deltaMotionInfo.y();
float changeH = deltaMotionInfo.h();
particle->shift(changeX, changeY, changeH);
randomlyShiftParticle(particle);
}
// std::cout << "\n\n Updated Particles w/ Motion \n";
}示例2: update
/**
* Updates the particle set according to the motion.
*
* @return the updated ParticleSet.
*/
void MotionSystem::update(ParticleSet& particles,
const messages::RobotLocation& odometry,
float error)
{
// Store the last odometry and set the current one
lastOdometry.set_x(curOdometry.x());
lastOdometry.set_y(curOdometry.y());
lastOdometry.set_h(curOdometry.h());
curOdometry.set_x(odometry.x());
curOdometry.set_y(odometry.y());
curOdometry.set_h(odometry.h());
// change in the robot frame
float dX_R = curOdometry.x() - lastOdometry.x();
float dY_R = curOdometry.y() - lastOdometry.y();
float dH_R = curOdometry.h() - lastOdometry.h();
if( (std::fabs(dX_R) > 3.f) || (std::fabs(dY_R) > 3.f) || (std::fabs(dH_R) > 0.35f) ) {
// Probably reset odometry somewhere, so skip frame
// std::cout << "std::fabs(dX_R) = " << std::fabs(dX_R) << " std::fabs(dY_R) = " << std::fabs(dY_R) << std::endl;
// std::cout << "curOdometry.x() = " << curOdometry.x() << " curOdometry.y() = " << curOdometry.y() << std::endl;
std::cout << "Odometry reset, motion frame skipped in loc" << std::endl;
return;
}
float dX, dY, dH;
ParticleIt iter;
for(iter = particles.begin(); iter != particles.end(); iter++)
{
Particle* particle = &(*iter);
// Rotate from the robot frame to the global to add the translation
float sinh, cosh;
sincosf(curOdometry.h() - particle->getLocation().h(),
&sinh, &cosh);
dX = (cosh*dX_R + sinh*dY_R) * FRICTION_FACTOR_X;
dY = (cosh*dY_R - sinh*dX_R) * FRICTION_FACTOR_Y;
dH = dH_R * FRICTION_FACTOR_H;
particle->shift(dX, dY, dH);
// noiseShiftWithOdo(particle, dX, dY, dH, error);
randomlyShiftParticle(particle, false);
}
}示例3: update
/**
* Updates the particle set according to the motion.
*
* @return the updated ParticleSet.
*/
void MotionSystem::update(ParticleSet& particles,
const messages::RobotLocation& odometry,
bool nearMid)
{
// Store the last odometry and set the current one
lastOdometry.set_x(curOdometry.x());
lastOdometry.set_y(curOdometry.y());
lastOdometry.set_h(curOdometry.h());
curOdometry.set_x(odometry.x());
curOdometry.set_y(odometry.y());
curOdometry.set_h(odometry.h());
// change in the robot frame
float dX_R = curOdometry.x() - lastOdometry.x();
float dY_R = curOdometry.y() - lastOdometry.y();
float dH_R = curOdometry.h() - lastOdometry.h();
if( (std::fabs(dX_R) > 3.f) || (std::fabs(dY_R) > 3.f) ) {
//Probably reset odometry somewhere so skip a frame
return;
}
float dX, dY, dH;
ParticleIt iter;
for(iter = particles.begin(); iter != particles.end(); iter++)
{
Particle* particle = &(*iter);
// Rotate from the robot frame to the global to add the translation
float sinh, cosh;
sincosf(curOdometry.h() - particle->getLocation().h(),
&sinh, &cosh);
dX = (cosh*dX_R + sinh*dY_R) * FRICTION_FACTOR_X;
dY = (cosh*dY_R - sinh*dX_R) * FRICTION_FACTOR_Y;
dH = dH_R * FRICTION_FACTOR_H; // just add the rotation
particle->shift(dX, dY, dH);
noiseShiftWithOdo(particle, dX, dY, dH);
//randomlyShiftParticle(particle, nearMid);
}
}