|
公司项目,在做动态的3d重建,以第三人称显示,类似玩游戏一样。需要从pcap雷达数据的动态点云,实现重建并贴图。
现在在vs2013中遇到一个问题,搞了很久,都没有结果,希望有大佬帮忙看下,感激不尽!
代码如下:
#include <pcl/point_cloud.h>
#include <pcl/io/hdl_grabber.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>
#include <pcl/pcl_base.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/ply_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <string>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/grid_projection.h>
#include <pcl/point_traits.h>
using namespace std;
using namespace pcl::console;
using namespace pcl::visualization;
class SimpleHDLViewer
{
public:
typedef pcl::PointCloud<pcl::PointXYZI> Cloud;
//typedef typename Cloud::ConstPtr CloudConstPtr;
typedef Cloud::ConstPtr CloudConstPtr;
SimpleHDLViewer(pcl::Grabber& grabber,
pcl::visualization::PointCloudColorHandler<pcl::PointXYZI> &handler) :
cloud_viewer_ (new pcl::visualization::PCLVisualizer ("Serious?")),
grabber_ (grabber),
handler_ (handler)
{
}
void cloud_callback (const CloudConstPtr& cloud)
{
boost::mutex::scoped_lock lock (cloud_mutex_);
cloud_ = cloud;
}
void run ()
{
cloud_viewer_->addCoordinateSystem (3.0);
cloud_viewer_->setBackgroundColor (0, 0, 0);
cloud_viewer_->initCameraParameters ();
cloud_viewer_->setCameraPosition (0.0, 0.0, 30.0, 0.0, 10.0, 0.0, 0);
cloud_viewer_->setCameraClipDistances (0.0, 50.0);
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)
{
//////////////////////////////////////////////////////////////////////////
pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
tree->setInputCloud(cloud);
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(20);
n.compute(*normals);
//* normals should not contain the point normals + surface curvatures
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloud_with_normals); //////////////////////////////////////////////////////////////////
pcl::GridProjection<pcl::PointNormal> gp;
pcl::PolygonMesh mesh;
//gp.setInputCloud(cloud_with_normals);
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
//tree2->setInputCloud(cloud_with_normals);
gp.setSearchMethod(tree2);
gp.setResolution(0.2);
gp.setPaddingSize(2);
gp.setMaxBinarySearchLevel(8);
gp.reconstruct(mesh);
cloud_viewer_->addPolygonMesh(mesh);
//////////////////////////////////////////////////////////////////////////
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<pcl::visualization::PCLVisualizer> cloud_viewer_;
pcl::Grabber& grabber_;
boost::mutex cloud_mutex_;
CloudConstPtr cloud_;
//pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2;
pcl::visualization::PointCloudColorHandler<pcl::PointXYZI> &handler_;
};
int main (int argc, char ** argv)
{
std::string hdlCalibration, pcapFile;
//parse_argument (argc, argv, "-calibrationFile", hdlCalibration);
//parse_argument (argc, argv, "-pcapFile", pcapFile);
hdlCalibration = "";
pcapFile = "32e.pcap";
pcl::HDLGrabber grabber(hdlCalibration,pcapFile);
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> color_handler ("intensity");
// SimpleHDLViewer<PointXYZI> v (grabber, color_handler);
SimpleHDLViewer v(grabber, color_handler);
v.run ();
return (0);
}
|
|