geometry_msgs::PoseArray PFilterTest::getParticlePoseArray(int idx)
{
std::vector<cspace> particles;
pFilter_.getAllParticles(particles, idx);
// tf::Transform trans = pHandler.getTransformToPartFrame();
tf::Transform trans = trans_;
if (idx == 5) trans = trans1_;
#ifdef POINT_CLOUD
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
int numParticles = pFilter_.getNumParticles();
for (int j = 0; j < numParticles; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
// cspace particles_est_stat;
// cspace particles_est;
// pFilter_.estimateGaussian(particles_est, particles_est_stat);
geometry_msgs::PoseArray poseArray;
for(int i=0; i<50; i++){
tf::Pose pose = poseAt(particles[i]);
geometry_msgs::Pose pose_msg;
tf::poseTFToMsg(trans*pose, pose_msg);
poseArray.poses.push_back(pose_msg);
// ROS_INFO("Pose %d: %f, %f, %f", i, poseArray.poses[i].position.x,
// poseArray.poses[i].position.y,
// poseArray.poses[i].position.z);
}
return poseArray;
}
geometry_msgs::PoseArray PFilterTest::getParticlePoseArray()
{
particleFilter::cspace particles[NUM_PARTICLES];
pFilter_.getAllParticles(particles);
tf::Transform trans = plt.getTrans();
#ifdef POINT_CLOUD
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
for (int j = 0; j < pFilter_.numParticles; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
particleFilter::cspace particles_est_stat;
particleFilter::cspace particles_est;
pFilter_.estimatedDistribution(particles_est, particles_est_stat);
geometry_msgs::PoseArray poseArray;
for(int i=0; i<50; i++){
tf::Pose pose = poseAt(particles[i]);
geometry_msgs::Pose pose_msg;
tf::poseTFToMsg(trans*pose, pose_msg);
poseArray.poses.push_back(pose_msg);
// ROS_INFO("Pose %d: %f, %f, %f", i, poseArray.poses[i].position.x,
// poseArray.poses[i].position.y,
// poseArray.poses[i].position.z);
}
return poseArray;
}