PCLVelodyne高清晰度激光(HDL)抓取器

没有获取数据的仪器,只是把代码看了看,没有运行,有条件的可以尝试。
代码里有C++11新特性的知识,看不懂的可以自行百度。
代码如下:

#include <string>
#include <iostream>
#include <iomanip>

#include <pcl/io/hdl_grabber.h>
#include <pcl/console/parse.h>
#include <pcl/common/time.h>

class SimpleHDLGrabber 
{
  public:
    std::string calibrationFile, pcapFile;

    SimpleHDLGrabber (std::string& calibFile, std::string& pcapFile) 
      : calibrationFile (calibFile)
      , pcapFile (pcapFile) 
    {
    }

    void 
    sectorScan (
        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&, 
        float,
        float) 
    {
      static unsigned count = 0;
      static double last = pcl::getTime ();
      if (++count == 30) 
      {
        double now = pcl::getTime();
        std::cout << "got sector scan.  Avg Framerate " << double(count) / double(now - last) << " Hz" << std::endl;
        count = 0;
        last = now;
      }
    }

    void 
    sweepScan (
        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >& sweep)
    {
      static unsigned count = 0;
      static double last = pcl::getTime();

      if (sweep->header.seq == 0) {
        pcl::uint64_t stamp;
        stamp = sweep->header.stamp;
        time_t systemTime = static_cast<time_t>(((stamp & 0xffffffff00000000l) >> 32) & 0x00000000ffffffff);
        pcl::uint32_t usec = static_cast<pcl::uint32_t>(stamp & 0x00000000ffffffff);
        std::cout << std::hex << stamp << "  " << ctime(&systemTime) << " usec: " << usec << std::endl;
      }

      if (++count == 30) 
      {
        double now = pcl::getTime ();
        std::cout << "got sweep.  Avg Framerate " << double(count) / double(now - last) << " Hz" << std::endl;
        count = 0;
        last = now;
      }
    }

    void 
    run () 
    {
      pcl::HDLGrabber interface (calibrationFile, pcapFile);
      // make callback function from member function
      boost::function<void(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&, float, float)> f =
          boost::bind(&SimpleHDLGrabber::sectorScan, this, _1, _2, _3);

      // connect callback function for desired signal. In this case its a sector with XYZ and intensity information
      //boost::signals2::connection c = interface.registerCallback(f);

      // Register a callback function that gets complete 360 degree sweeps.
      boost::function<void(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&)> f2 = boost::bind(
          &SimpleHDLGrabber::sweepScan, this, _1);
      boost::signals2::connection c2 = interface.registerCallback(f2);

      //interface.filterPackets(boost::asio::ip::address_v4::from_string("192.168.18.38"));

      // start receiving point clouds
      interface.start ();

      std::cout << "<Esc>, \'q\', \'Q\': quit the program" << std::endl;
      char key;
      do 
      {
        key = static_cast<char> (getchar ());
      } while (key != 27 && key != 'q' && key != 'Q');

      // stop the grabber
      interface.stop ();
    }
};

int 
main (int argc, char **argv) 
{
	std::string hdlCalibration, pcapFile;

	pcl::console::parse_argument (argc, argv, "-calibrationFile", hdlCalibration);
	pcl::console::parse_argument (argc, argv, "-pcapFile", pcapFile);

	SimpleHDLGrabber grabber (hdlCalibration, pcapFile);
	grabber.run ();
	return (0);
}

下面的代码是可视化获取数据:

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/time.h> //fps calculations
#include <pcl/io/hdl_grabber.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/boost.h>
#include <pcl/visualization/mouse_event.h>
#include <vector>
#include <string>
#include <boost/algorithm/string.hpp>
#include <typeinfo>

using namespace std;
using namespace pcl;
using namespace pcl::console;
using namespace pcl::visualization;

#define SHOW_FPS 0
#if SHOW_FPS
#define FPS_CALC(_WHAT_) \
do \
{ \
    static unsigned count = 0;\
    static double last = getTime ();\
    double now = getTime (); \
    ++count; \
    if (now - last >= 1.0) \
    { \
      std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
      count = 0; \
      last = now; \
    } \
}while(false)
#else
#define FPS_CALC(_WHAT_) \
do \
{ \
}while(false)
#endif

//
template<typename PointType>
class SimpleHDLViewer
{
  public:
    typedef PointCloud<PointType> Cloud;
    typedef typename Cloud::ConstPtr CloudConstPtr;

    SimpleHDLViewer (Grabber& grabber,
                     PointCloudColorHandler<PointType> &handler) 
      : cloud_viewer_ (new PCLVisualizer ("PCL HDL Cloud"))
      , grabber_ (grabber)
      , handler_ (handler)
    {
    }

    void 
    cloud_callback (const CloudConstPtr& cloud)
    {
      FPS_CALC ("cloud callback");
      boost::mutex::scoped_lock lock (cloud_mutex_);
      cloud_ = cloud;
      //std::cout << cloud->points[0] << " " << cloud->size () << std::endl;
    }

    void 
    cloud_callback (const CloudConstPtr& cloud,
                    float /*startAngle*/,
                    float /*endAngle*/)
    {
      FPS_CALC ("cloud callback");
      boost::mutex::scoped_lock lock (cloud_mutex_);
      cloud_ = cloud;
    }

    void 
    keyboard_callback (const KeyboardEvent& event,
                       void* /*cookie*/)
    {
      if (event.keyUp ())
      {
        return;
      }
    }

    void 
    mouse_callback (const MouseEvent& mouse_event,
                    void* /*cookie*/)
    {
      if (mouse_event.getType () == MouseEvent::MouseButtonPress && 
          mouse_event.getButton () == MouseEvent::LeftButton)
      {
        cout << mouse_event.getX () << " , " << mouse_event.getY () << endl;
      }
    }

    void 
    run ()
    {
      cloud_viewer_->addCoordinateSystem (3.0, "global");
      cloud_viewer_->setBackgroundColor (0, 0, 0);
      cloud_viewer_->initCameraParameters ();
      cloud_viewer_->setCameraPosition (0.0, 0.0, 30.0, 0.0, 1.0, 0.0, 0);
      cloud_viewer_->setCameraClipDistances (0.0, 50.0);
      //cloud_viewer_->registerMouseCallback(&SimpleHDLViewer::mouse_callback, *this);
      //cloud_viewer_->registerKeyboardCallback (&SimpleHDLViewer::keyboard_callback, *this);

      //boost::function<void(const CloudConstPtr&, float, float)> cloud_cb = boost::bind(&SimpleHDLViewer::cloud_callback, this, _1, _2, _3);
      boost::function<void (const CloudConstPtr&)> cloud_cb = boost::bind (
          &SimpleHDLViewer::cloud_callback, this, _1);
      boost::signals2::connection cloud_connection = grabber_.registerCallback (
          cloud_cb);

      grabber_.start ();

      while (!cloud_viewer_->wasStopped ())
      {
        CloudConstPtr cloud;

        // See if we can get a cloud
        if (cloud_mutex_.try_lock ())
        {
          cloud_.swap (cloud);
          cloud_mutex_.unlock ();
        }

        if (cloud)
        {
          FPS_CALC("drawing cloud");
          handler_.setInputCloud (cloud);
          if (!cloud_viewer_->updatePointCloud (cloud, handler_, "HDL"))
            cloud_viewer_->addPointCloud (cloud, handler_, "HDL");

          cloud_viewer_->spinOnce ();
        }

        if (!grabber_.isRunning ())
          cloud_viewer_->spin ();

        boost::this_thread::sleep (boost::posix_time::microseconds (100));
      }

      grabber_.stop ();

      cloud_connection.disconnect ();
    }

    boost::shared_ptr<PCLVisualizer> cloud_viewer_;
    boost::shared_ptr<ImageViewer> image_viewer_;

    Grabber& grabber_;
    boost::mutex cloud_mutex_;
    boost::mutex image_mutex_;

    CloudConstPtr cloud_;
    PointCloudColorHandler<PointType> &handler_;
};

void
usage (char ** argv)
{
  cout << "usage: " << argv[0]
      << " [-hdlCalibration <path-to-calibration-file>] [-pcapFile <path-to-pcap-file>] [-h | --help] [-format XYZ(default)|XYZI|XYZRGB]"
      << endl;
  cout << argv[0] << " -h | --help : shows this help" << endl;
  return;
}

int 
main (int argc, char ** argv)
{
  std::string hdlCalibration, pcapFile, format ("XYZ");

  if (find_switch (argc, argv, "-h") || 
      find_switch (argc, argv, "--help"))
  {
    usage (argv);
    return (0);
  }

  parse_argument (argc, argv, "-calibrationFile", hdlCalibration);
  parse_argument (argc, argv, "-pcapFile", pcapFile);
  parse_argument (argc, argv, "-format", format);

  HDLGrabber grabber (hdlCalibration, pcapFile);

  cout << "viewer format:" << format << endl;
  if (boost::iequals (format, std::string ("XYZ")))
  {
    std::vector<double> fcolor (3); fcolor[0] = fcolor[1] = fcolor[2] = 255.0;
    pcl::console::parse_3x_arguments (argc, argv, "-fc", fcolor[0], fcolor[1], fcolor[2]);
    PointCloudColorHandlerCustom<PointXYZ> color_handler (fcolor[0], fcolor[1], fcolor[2]);

    SimpleHDLViewer<PointXYZ> v (grabber, color_handler);
    v.run ();
  }
  else if (boost::iequals (format, std::string ("XYZI")))
  {
    PointCloudColorHandlerGenericField<PointXYZI> color_handler ("intensity");

    SimpleHDLViewer<PointXYZI> v (grabber, color_handler);
    v.run ();
  }
  else if (boost::iequals (format, std::string ("XYZRGB")))
  {
    PointCloudColorHandlerRGBField<PointXYZRGBA> color_handler;

    SimpleHDLViewer<PointXYZRGBA> v (grabber, color_handler);
    v.run ();
  }
  return (0);
}

来源:PCL官方示例

上一篇:应用TYPE-C外围电源管理IC IM2605


下一篇:可持久化线段树(主席树) - AcWing - 255 - 第k小数