pcl::PointCloud<pcl::PointXYZ>::Ptr ImageUtils::cvMatToPcl(cv::Mat &mat) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(
new pcl::PointCloud<pcl::PointXYZ>);
std::cout << "begin parsing file" << std::endl;
for (int ki = 0; ki < mat.rows; ki++) {
for (int kj = 0; kj < mat.cols; kj++) {
pcl::PointXYZ pointXYZ;
pointXYZ.x = mat.at<cv::Point3f>(ki, kj).x;
pointXYZ.y = mat.at<cv::Point3f>(ki, kj).y;
pointXYZ.z = mat.at<cv::Point3f>(ki, kj).z;
if(pointXYZ.z <= 0)
continue;
cloud->points.push_back(pointXYZ);
}
}
return cloud;
}