Beispiel #1
0
void visualize(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud, pcl::visualization::PCLVisualizer::Ptr pVisualizer)
{
    //init visualizer
	pVisualizer->setSize(640, 480);
	pVisualizer->setPosition(640, 0);
	pVisualizer->setBackgroundColor(0x00, 0x00, 0x00);
	pVisualizer->initCameraParameters();
	pVisualizer->addPointCloud(pCloud, "cloud");

	//reload visualizer content
	pVisualizer->spinOnce(1000000);
}
    virtual void processBufferElement(CloudConstPtr cloud) override {
        cloud_viewer_->spinOnce();

        if (!cloud_viewer_->updatePointCloud(cloud, "OpenNICloud")) {
            cloud_viewer_->setPosition(0, 0);
            cloud_viewer_->setSize(cloud->width, cloud->height);
            cloud_viewer_->addPointCloud(cloud, "OpenNICloud");
            cloud_viewer_->resetCameraViewpoint("OpenNICloud");
            cloud_viewer_->setCameraPosition(
                    0, 0, 0,        // Position
                    0, 0, 1,        // Viewpoint
                    0, -1, 0);    // Up
        }

        // TODO; Figure out how to achieve this
//        if(cloud_viewer_->wasStopped()) {
//            this->cloud_buffer_.reset(new Buffer<CloudConstPtr>());
//            this->stop();
//            cloud_viewer_->close();
//        }
    }
Beispiel #3
0
int main(int argc, char** argv)
{

  ros::init(argc,argv,"greedy_objrec_ransac_node");
  ros::NodeHandle nh;

  pcl::console::parse_argument(argc, argv, "--m", method_id);
  pcl::console::parse_argument(argc, argv, "--p", path);
  pcl::console::parse_argument(argc, argv, "--t", trial_id);
  pcl::console::parse_argument(argc, argv, "--link", link_mesh_name);
  pcl::console::parse_argument(argc, argv, "--node", node_mesh_name);
  pcl::console::parse_argument(argc, argv, "--link-width", linkWidth);
  pcl::console::parse_argument(argc, argv, "--node-width", nodeWidth);
  pcl::console::parse_argument(argc, argv, "--voxel-size", voxelSize);

  linkrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(linkWidth, voxelSize));
  noderec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));
  objrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));

  bool view_flag = false;
  if( pcl::console::find_switch(argc, argv, "-v") == true )
    view_flag = true;

  if( view_flag == true )
  {
    viewer = pcl::visualization::PCLVisualizer::Ptr (new pcl::visualization::PCLVisualizer());
    viewer->initCameraParameters();
    viewer->addCoordinateSystem(0.1);
    viewer->setSize(640, 480);
    viewer->setCameraPosition(0, 0, 0.1, 0, 0, 1, 0, -1, 0);
  }

  std::stringstream mm;
  mm << method_id;

  if( method_id >= 3 )
  {
    ROS_INFO("Loading link mesh from \"%s.obj\"...",link_mesh_name.c_str());
    link_mesh = LoadMesh(link_mesh_name+".obj", "link");
    ROS_INFO("Loading node mesh from \"%s.obj\"...",node_mesh_name.c_str());
    node_mesh = LoadMesh(node_mesh_name+".obj", "node");
    mesh_set.push_back(link_mesh);
    mesh_set.push_back(node_mesh);
  }

  switch(method_id)
  {
    case 0:
    case 1:
    case 2:
      objrec->AddModel(link_mesh_name, "link");
      objrec->AddModel(node_mesh_name, "node");
      break;
    case 3:
    case 4:
    case 5:
      linkrec->AddModel(link_mesh_name, "link");
      noderec->AddModel(node_mesh_name, "node");
      break;
    default:return 0;
  }

  if (method_id < 3) {
    pub_link = nh.advertise<geometry_msgs::PoseArray>("objects",1000);
  } else {
    pub_link = nh.advertise<geometry_msgs::PoseArray>("links",1000);
    pub_node = nh.advertise<geometry_msgs::PoseArray>("nodes",1000);
  }
  sub = nh.subscribe("points_in",1,callback);

  // send and recieve messages
  while (ros::ok()) {
    ros::spin();
  }

  return 0;
}