void MapModel::initGlobal(Particles& particles, double z, double roll, double pitch,
const Vector6d& initNoise,
UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal, double maxHeight, double minHeight){ //maxHeight added by LC
double sizeX,sizeY,sizeZ, minX, minY, minZ;
//LC: get total map size
m_map->getMetricSize(sizeX,sizeY,sizeZ);
//LC: get minimum values of map bounding box
m_map->getMetricMin(minX, minY, minZ);
double weight = 1.0 / particles.size();
Particles::iterator it = particles.begin();
while (true){
if (it == particles.end())
break;
// obtain a pose hypothesis:
double x = minX + sizeX * rngUniform();
double y = minY + sizeY * rngUniform();
std::vector<double> z_list;
getHeightlist(x, y, minHeight, maxHeight,z_list);
//LC: create a pose with random yaw orientation at (x,y) for every entry in the height list
for (unsigned zIdx = 0; zIdx < z_list.size(); zIdx++){
if (it == particles.end())
break;
// not needed => we already know that z contains valid poses
// distance map: used distance from obstacles:
//std::abs(node->getLogOdds()) < 0.1){
// if (!isOccupied(octomap::point3d(x, y, z[zIdx]))){
it->pose.getOrigin().setX(x);
it->pose.getOrigin().setY(y);
// TODO: sample z, roll, pitch
it->pose.getOrigin().setZ(z_list.at(zIdx) + z + rngNormal() * initNoise(2));
//LC: create yaw orientation from -PI to +PI
double yaw = rngUniform() * 2 * M_PI -M_PI;
it->pose.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
it->weight = weight;
it++;
}
}
}
void MapModel::initGlobal(Particles& particles,
const Vector6d& initPose, const Vector6d& initNoise,
UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal){
double sizeX,sizeY,sizeZ, minX, minY, minZ;
m_map->getMetricSize(sizeX,sizeY,sizeZ);
m_map->getMetricMin(minX, minY, minZ);
double weight = 1.0 / particles.size();
Particles::iterator it = particles.begin();
while (true){
if (it == particles.end())
break;
// obtain a pose hypothesis:
double x = minX + sizeX * rngUniform();
double y = minY + sizeY * rngUniform();
std::vector<double> z;
getHeightlist(x, y, 0.6,z);
for (unsigned zIdx = 0; zIdx < z.size(); zIdx++){
if (it == particles.end())
break;
// not needed => we already know that z contains valid poses
// distance map: used distance from obstacles:
//std::abs(node->getLogOdds()) < 0.1){
// if (!isOccupied(octomap::point3d(x, y, z[zIdx]))){
it->pose.getOrigin().setX(x);
it->pose.getOrigin().setY(y);
// TODO: sample z, roll, pitch around odom pose!
it->pose.getOrigin().setZ(z.at(zIdx) + initPose(2) + rngNormal() * initNoise(2));
double yaw = rngUniform() * 2 * M_PI -M_PI;
it->pose.setRotation(tf::createQuaternionFromYaw(yaw));
it->weight = weight;
it++;
}
}
}
