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;
}