PCL代码

1. 显示CloudViewer *

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sstream>

using namespace std;

int user_data = 0;

void viewOnOff(pcl::visualization::PCLVisualizer &view)
{
    view.setBackgroundColor(1.0, 0.5, 1.0);
    pcl::PointXYZ p;
    p.x = 1.0;
    p.y = 0;
    p.z = 0;
    view.addSphere(p, 0.1, "sphere", 0);
    std::cout << "only run once" << endl;
}

void viewMore(pcl::visualization::PCLVisualizer &view)
{
    static unsigned count = 0;
    stringstream ss, sid;
    sid << "text" << user_data;
    ss << "Once per viewer loop" << user_data << endl;
    view.removeShape("test", 0);
    view.addText(ss.str(), 200, 300, sid.str(), 0);
}

int main()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("/home/lzf/catkin_ws/src/learning_pcl/data/tutorials/ism_test_cat.pcd", *cloud);
    pcl::visualization::CloudViewer view("Cloud Viewer");
    view.showCloud(cloud);

    view.runOnVisualizationThreadOnce(viewOnOff);
    view.runOnVisualizationThread(viewMore);
    while (!view.wasStopped())
    {
        /* code */
        user_data ++;
    }
    return 0;
    
}

2. k-d树

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <vector>
#include <ctime>

using namespace std;

int main(int argc, char *argv[])
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  cloud->width = 1000;
  cloud->height = 1;
  cloud->points.resize(cloud->width * cloud->height);

  for (int i=0; i < cloud->size(); i++)
  {
    cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024.0f * rand()/ (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024.0f * rand()/ (RAND_MAX + 1.0f);
  }

  pcl::PointXYZ searchPoint;
  searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  kdtree.setInputCloud(cloud);
  int k = 10;
  vector<int> pcdIdx(k);
  vector<float> pcdDist(k);
  cout << "K nearest neighbor search at (" << searchPoint.x << "  " << searchPoint.y << "  " << searchPoint.z << ")" << endl;
  // if (kdtree.nearestKSearch(searchPoint, k, pcdIdx, pcdDist) > 0)
  // {
  //   for (int i=0; i < pcdIdx.size(); i++)
  //   {
  //     cout << "  " << cloud->points[pcdIdx[i]].x << "  " << cloud->points[pcdIdx[i]].y << "  " << cloud->points[pcdIdx[i]].z << "  dist: " << pcdDist[i] << endl;
  //   }
  // }

  vector<int> rpcdIdx(k);
  vector<float> rpcdDist(k);
  float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
  cout << "K radius search at (" << searchPoint.x << "  " << searchPoint.y << "  " << searchPoint.z << ")" <<  radius << endl;
  if (kdtree.radiusSearch(searchPoint, radius, rpcdIdx, rpcdDist) > 0)
  {
    for (int i=0; i < rpcdIdx.size(); i++)
    {
      cout << "  " << cloud->points[rpcdIdx[i]].x << "  " << cloud->points[rpcdIdx[i]].y << "  " << cloud->points[rpcdIdx[i]].z << "  dist: " << rpcdDist[i] << endl;
    }
  }
  else
  {
    cerr << "error to search" << endl;
  }

  pcl::visualization::CloudViewer viewer("PointCloud");
  viewer.showCloud(cloud);

  while (!viewer.wasStopped())
  {
    /* code */
    // cout << cloud->size() << endl;
  }
  return 0;
  
}