刚今天验收的实验,记录一下。
是比较基础的三维重建内容。
算是三维重建入门。
系统:windows
环境:visual studio 2013
语言:c++
相关:OpenCV 2、Kinect SDK 2.0、PCL库
内容:
使用Kinect 2.0拍摄获取深度图,将彩色图与深度图配准生成点云;
然后每次拍摄得到的点云用ICP算法进行融合,形成完整点云(每次拍摄仅做微小偏移);
之后稍微对点云做了些许处理;
还添加了回档的功能;
声明:
有挺多借鉴博客与参考资料的,太多懒得写,假装忘了~
原理:(以下不对变量作用作解释,具体可参照变量名猜测,完整代码最后给出)
流程图如下
1.关于彩色图与深度图的配准,官方文档给出了如下3个坐标系:
Kinect中总共有着3种坐标空间:
1.相机空间(Camera space):拥有三个坐标轴,假设kinect面朝正前方,那么X轴向左递增,Y轴向上递增,Z轴向前递增。
2.深度空间(Depth space):拥有三个坐标轴,其中x、y分别是深度图中像素的位置,z轴为像素的深度值。
3.色彩空间(Color space):拥有两个坐标轴,其中x、y分别是彩色图像中像素的位置。
由此,如果知道参数其实自己也能算,但是kinect事实上已经给出了函数。如下图所示
下图为单次生成的点云:
2.关于ICP算法点云配准:
它的本质思路如下:
1.计算两个点云之间的匹配关系;
2.计算旋转平移矩阵;
3.旋转平移源点云。
4.如果误差达到要求或者迭代次数够了,则退出,否则回到第一步。
具体实现可以参照原论文。
这里使用的是PCL库里自带的实现
接下来几幅图是点云融合过程(两个点云,慢慢融合)
融合效果:
3.点云处理,都是水水的实验性的处理,
1.第一种是简单的按照y轴进行颜色变换
2.第二种是根据高度生成水面
3.第三种是三角网格化
详情见代码
代码:
#ifndef KINECT_FXXL_H #define KINECT_FXXL_H #include <Kinect.h> #endif
#include <Kinect.h> #include "KinectFxxL.h"
#ifndef TEST_FXXL_H #define TEST_FXXL_H #include <iostream> #include <cstring> #include <cstdio> #include <algorithm> #include <cmath> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/opencv.hpp> using namespace std; using namespace cv; typedef unsigned short UINT16; void showImage(Mat tmpMat, string windowName = "tmpImage"); Mat convertDepthToMat(const UINT16* pBuffer, int width, int height); string convertIntToString(int num); #endif
#include <iostream> #include <cstring> #include <cstdio> #include <algorithm> #include <cmath> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/opencv.hpp> #include "OpenCVFxxL.h" #define pause system("pause") typedef unsigned short UINT16; using namespace std; using namespace cv; void showImage(Mat tmpMat, string windowName) { imshow(windowName, tmpMat); if (cvWaitKey(0) == 27) //ESC键值为27 return; } Mat convertDepthToMat(const UINT16* pBuffer, int width, int height) { Mat ret; uchar* pMat; ret = Mat(height, width, CV_8UC1); pMat = ret.data; //uchar类型的指针,指向Mat数据矩阵的首地址 for (int i = 0; i < width*height; i++) *(pMat + i) = *(pBuffer + i); return ret; } string convertIntToString(int num) { string ret = ""; if (num < 0) return puts("the function only fits positive int number"), ""; while (num) ret += (num % 10) + '0', num /= 10; reverse(ret.begin(), ret.end()); if (ret.size() == 0) ret += "0"; return ret; }
#ifndef PCL_FXXL_H #define PCL_FXXL_H #include <time.h> #include <stdlib.h> #include <map> #include <time.h> #include <iostream> #include <cstdio> #include <cmath> #include <algorithm> #include <thread> #include <mutex> #include <queue> #include <boost/make_shared.hpp> //boost指针相关头文件 #include <pcl/io/io.h> #include <pcl/visualization/cloud_viewer.h> #include <pcl/point_types.h> //点类型定义头文件 #include <pcl/point_cloud.h> //点云类定义头文件 #include <pcl/point_representation.h> //点表示相关的头文件 #include <pcl/io/pcd_io.h> //PCD文件打开存储类头文件 #include <pcl/filters/voxel_grid.h> //用于体素网格化的滤波类头文件 #include <pcl/filters/filter.h> //滤波相关头文件 #include <pcl/features/normal_3d.h> //法线特征头文件 #include <pcl/registration/icp.h> //ICP类相关头文件 #include <pcl/registration/icp_nl.h> //非线性ICP 相关头文件 #include <pcl/registration/transforms.h> //变换矩阵类头文件 #include <pcl/visualization/pcl_visualizer.h> //可视化类头文件 #include <pcl/kdtree/kdtree_flann.h> #include <pcl/surface/gp3.h> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/opencv.hpp> #include <cv.h> #include "OpenCVFxxL.h" #define shapeCloud(x) x->width=1,x->height=x->size() #define GAP_NORMAL 0.001 #define GAP_TRANSPARENT 0.005 #define random(x) (rand()%x) using namespace cv; using namespace std; using namespace pcl; using pcl::visualization::PointCloudColorHandlerGenericField; using pcl::visualization::PointCloudColorHandlerCustom; typedef PointXYZRGBA PointT; typedef PointCloud<PointT> PointCloudT; typedef PointXYZ PointX; typedef PointCloud<PointX> PointCloudX; typedef PointCloud<PointNormal> PointCloudWithNormals; extern boost::shared_ptr<pcl::visualization::PCLVisualizer> visualizer; extern bool iterationFlag_visualizer; void makeBall(PointCloudT::Ptr cloud, double x_p, double y_p, double z_p, int r_color, int g_color, int b_color, double radius, double gap = GAP_NORMAL); void makeWall(PointCloudT::Ptr cloud, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, int r_color, int g_color, int b_color, double gap = GAP_TRANSPARENT); void getTriangles(const PointCloudT::Ptr cloud, PolygonMesh &triangles); void getExCloud(const PointCloudT::Ptr cloud, PointCloudT::Ptr exCloud); void filterCloud(PointCloudX::Ptr cloud, double size = 0.05); void filterCloud(PointCloudT::Ptr cloud, double size = 0.05); void showPolygonMesh(PolygonMesh polygonMesh); void showPointCloudX(PointCloudX::Ptr cloud0); void showPointCloudT(PointCloudT::Ptr cloud0, PointCloudT::Ptr cloud1 = NULL); void showTransform(const PointCloudWithNormals::Ptr targetCloud, const PointCloudWithNormals::Ptr sourceCloud); void keyBoardEventOccurred(const pcl::visualization::KeyboardEvent& event, void *nothing); void initVisualizer(); void cloudMerge(const PointCloudT::Ptr cloudSrc, const PointCloudT::Ptr cloudTgt, PointCloudT::Ptr cloudRet, Eigen::Matrix4f &transformingMatrix, bool flag_slow, bool filterFlag = true); class PointRepresentationT :public PointRepresentation<PointNormal> { using PointRepresentation<PointNormal>::nr_dimensions_; public: PointRepresentationT(); virtual void copyToFloatArray(const PointNormal &p, float *out) const; }; #endif
#include <time.h> #include <stdlib.h> #include <map> #include <time.h> #include <iostream> #include <cstdio> #include <cmath> #include <algorithm> #include <thread> #include <mutex> #include <queue> #include <boost/make_shared.hpp> //boost指针相关头文件 #include <pcl/visualization/cloud_viewer.h> #include <pcl/point_types.h> //点类型定义头文件 #include <pcl/point_cloud.h> //点云类定义头文件 #include <pcl/point_representation.h> //点表示相关的头文件 #include <pcl/io/io.h> #include <pcl/io/pcd_io.h> //PCD文件打开存储类头文件 #include <pcl/filters/voxel_grid.h> //用于体素网格化的滤波类头文件 #include <pcl/filters/filter.h> //滤波相关头文件 #include <pcl/features/normal_3d.h> //法线特征头文件 #include <pcl/registration/icp.h> //ICP类相关头文件 #include <pcl/registration/icp_nl.h> //非线性ICP 相关头文件 #include <pcl/registration/transforms.h> //变换矩阵类头文件 #include <pcl/visualization/pcl_visualizer.h> //可视化类头文件 #include <pcl/kdtree/kdtree_flann.h> #include <pcl/surface/gp3.h> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/opencv.hpp> #include <cv.h> #include "OpenCVFxxL.h" #include "PCLFxxL.h" boost::shared_ptr<pcl::visualization::PCLVisualizer> visualizer(new pcl::visualization::PCLVisualizer("fxxl")); bool iterationFlag_visualizer; void makeBall(PointCloudT::Ptr cloud, double x_p, double y_p, double z_p, int r_color, int g_color, int b_color, double radius, double gap) { for (double x = x_p - radius; x <= x_p + radius; x += gap) { double tmp0 = sqrt(radius*radius - (x - x_p)*(x - x_p)); for (double y = y_p - tmp0; y <= y_p + tmp0; y += gap) { double z = z_p + sqrt(radius*radius - (x - x_p)*(x - x_p) - (y - y_p)*(y - y_p)); PointT tmpPoint; tmpPoint.x = x, tmpPoint.y = y, tmpPoint.z = z, tmpPoint.r = r_color, tmpPoint.g = g_color, tmpPoint.b = b_color; cloud->points.push_back(tmpPoint); } } cout << cloud->points.size() << endl; } void makeWall(PointCloudT::Ptr cloud, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, int r_color, int g_color, int b_color, double gap) { for (double x = min_x; x <= max_x; x += gap) for (double y = min_y; y <= max_y; y += gap) for (double z = min_z; z <= max_z; z += gap) { PointT tmpPoint; tmpPoint.x = x, tmpPoint.y = y, tmpPoint.z = z, tmpPoint.r = r_color, tmpPoint.g = g_color, tmpPoint.b = b_color; cloud->points.push_back(tmpPoint); } } void getTriangles(const PointCloudT::Ptr cloud, PolygonMesh &triangles) { PointCloudX::Ptr tmpCloud(new PointCloudX); tmpCloud->clear(); for (int i = 0; i < cloud->points.size(); i++) { PointX tmpPointX; tmpPointX.x = cloud->points[i].x, tmpPointX.y = cloud->points[i].y, tmpPointX.z = cloud->points[i].z; tmpCloud->points.push_back(tmpPointX); } shapeCloud(tmpCloud); filterCloud(tmpCloud, 0.04); NormalEstimation<PointX, Normal> normalEstimation; PointCloud<Normal>::Ptr normalCloud(new PointCloud<Normal>); search::KdTree<PointX>::Ptr kdTree(new search::KdTree<PointX>); kdTree->setInputCloud(tmpCloud); normalEstimation.setInputCloud(tmpCloud); normalEstimation.setSearchMethod(kdTree); normalEstimation.setKSearch(20); normalEstimation.compute(*normalCloud); PointCloud<PointNormal>::Ptr pointWithNormalCloud(new PointCloud<PointNormal>); pcl::concatenateFields(*tmpCloud, *normalCloud, *pointWithNormalCloud); search::KdTree<PointNormal>::Ptr kdTree2(new search::KdTree<PointNormal>); kdTree2->setInputCloud(pointWithNormalCloud); GreedyProjectionTriangulation<PointNormal> gp3; gp3.setSearchRadius(0.24); gp3.setMu(2.5); gp3.setMaximumNearestNeighbors(100); //邻近点阈值 gp3.setMaximumSurfaceAngle(M_PI / 4); //45度;两点的法向量角度差大于此值,这两点将不会连接成三角形 gp3.setMinimumAngle(M_PI / 18); //三角形的最小角度 gp3.setMaximumAngle(2 * M_PI / 3); gp3.setNormalConsistency(false); gp3.setInputCloud(pointWithNormalCloud); gp3.setSearchMethod(kdTree2); gp3.reconstruct(triangles); // vector<int> partIDs = gp3.getPartIDs(); // vector<int> states = gp3.getPointStates(); } void getExCloud(const PointCloudT::Ptr cloud, PointCloudT::Ptr exCloud) { string op; puts(" OwO input: 1.ex1: transform ex1; 2.ex2: transform ex2. OwO "); cin >> op; if (op.compare("ex1") == 0) { exCloud->points.clear(), *exCloud += *cloud, shapeCloud(exCloud); double max_x, min_x, max_y, min_y, max_z, min_z, colorStep_y, tmpColorValue; max_x = max_y = max_z = -1e9 - 44, min_x = min_y = min_z = 1e9 + 44; for (int i = 0; i < exCloud->points.size(); i++) max_y = max(max_y, 1.0*exCloud->points[i].y), min_y = min(min_y, 1.0*exCloud->points[i].y); colorStep_y = 255.0 / (max_y - min_y); for (int i = 0; i < exCloud->points.size(); i++) { tmpColorValue = (exCloud->points[i].y - min_y)*colorStep_y; exCloud->points[i].r = tmpColorValue, exCloud->points[i].g = tmpColorValue*2.0 / 3, exCloud->points[i].b = 0; } shapeCloud(exCloud); } else if (op.compare("ex2") == 0) { exCloud->clear(), *exCloud += *cloud, shapeCloud(exCloud); const int r_color0 = 12, g_color0 = 74, b_color0 = 82; double max_x, min_x, max_y, min_y, max_z, min_z, colorStep_z, tmpColorValue, y_surface; max_x = max_y = max_z = -1e9 - 44, min_x = min_y = min_z = 1e9 + 44; for (int i = 0; i < exCloud->points.size(); i++) { max_x = max(max_x, 1.0*exCloud->points[i].x), min_x = min(min_x, 1.0*exCloud->points[i].x); max_y = max(max_y, 1.0*exCloud->points[i].y), min_y = min(min_y, 1.0*exCloud->points[i].y); max_z = max(max_z, 1.0*exCloud->points[i].z), min_z = min(min_z, 1.0*exCloud->points[i].z); } y_surface = min_y + (max_y - min_y) / 3.0 * 2; makeWall(exCloud, min_x, min_x, min_y, y_surface, min_z, max_z, r_color0, g_color0, b_color0); makeWall(exCloud, max_x, max_x, min_y, y_surface, min_z, max_z, r_color0, g_color0, b_color0); makeWall(exCloud, min_x, max_x, min_y, min_y, min_z, max_z, r_color0, g_color0, b_color0); makeWall(exCloud, min_x, max_x, min_y, y_surface, min_z, min_z, r_color0, g_color0, b_color0); makeWall(exCloud, min_x, max_x, min_y, y_surface, max_z, max_z, r_color0, g_color0, b_color0); Mat imageMat_water = imread("res\water_surface.jpg"); // showImage(imageMat_water); double x_image = imageMat_water.rows - 4, z_image = imageMat_water.cols - 4, x_cloud = max_x - min_x, z_cloud = max_z - min_z; if (z_image / x_image > z_cloud / x_cloud) z_image = x_image * (z_cloud / x_cloud); else x_image = z_image / (z_cloud / x_cloud); double step_x = x_image / x_cloud, step_z = z_image / z_cloud; for (double x = min_x; x <= max_x; x += GAP_NORMAL) for (double z = min_z; z <= max_z; z += GAP_NORMAL) { PointT tmpPoint; int x_tmp = (int)(step_x*(x - min_x)), z_tmp = (int)(step_z*(z - min_z)); x_tmp = max(0, x_tmp), x_tmp = min(imageMat_water.rows - 1, x_tmp), z_tmp = max(0, z_tmp), z_tmp = min(imageMat_water.cols - 1, z_tmp); tmpPoint.x = x, tmpPoint.y = y_surface, tmpPoint.z = z; tmpPoint.r = imageMat_water.at<Vec3b>(x_tmp, z_tmp)[2]; tmpPoint.g = imageMat_water.at<Vec3b>(x_tmp, z_tmp)[1]; tmpPoint.b = imageMat_water.at<Vec3b>(x_tmp, z_tmp)[0]; exCloud->points.push_back(tmpPoint); } shapeCloud(exCloud); } else if (op.compare("ex3") == 0) { puts(" OAO invalid input, retry please OAO "); getExCloud(cloud, exCloud); /* srand((int)time(0)); const int r_color0 = 204, g_color0 = 207, b_color0 = 190, r_color1 = 94, g_color1 = 87, b_color1 = 46, r_color2 = 22, g_color2 = 35, b_color2 = 44, r_colorBas = 255, g_colorBas = 223, b_colorBas = 178; const int rateBound_color0 = 14, rateBound_color1 = 70, rateBound_color2 = 100,rateBound_colorBas=200; double max_x, min_x, max_y, min_y, max_z, min_z, colorStep_z, tmpColorValue, z_surface; max_x = max_y = max_z = -1e9 - 44, min_x = min_y = min_z = 1e9 + 44; for (int i = 0; i < exCloud->points.size(); i++) { max_x = max(max_x, 1.0*exCloud->points[i].x), min_x = min(min_x, 1.0*exCloud->points[i].x); max_y = max(max_y, 1.0*exCloud->points[i].y), min_y = min(min_y, 1.0*exCloud->points[i].y); max_z = max(max_z, 1.0*exCloud->points[i].z), min_z = min(min_z, 1.0*exCloud->points[i].z); } for (int i = 0; i < exCloud->points.size(); i++) exCloud->points[i].r = r_colorBas, exCloud->points[i].g = g_colorBas, exCloud->points[i].b = b_colorBas; int tmpSize_exCloud = exCloud->points.size(); for (int i = 0; i < tmpSize_exCloud; i++) makeBall(exCloud, exCloud->points[i].x, exCloud->points[i].y, exCloud->points[i].z, r_colorBas, g_colorBas, b_colorBas, random(100000) / 100000.0*0.02); map<double, bool> mp; mp.clear(); const double sed0 = 16123512.0, sed1 = 743243.0, sed2 = 6143134.0; const double x_sun=0, y_sun=max_y*1.74, z_sun=0; vector<double> energy; energy.clear(); double k0, k1, k2, ktmp, kdis; for (int i = 0; i < exCloud->points.size(); i++) { k0 = x_sun - exCloud->points[i].x, k1 = y_sun - exCloud->points[i].y, k2 = z_sun - exCloud->points[i].z; kdis = sqrt(k0*k0 + k1*k1 + k2*k2), k0 /= kdis, k1 /= kdis, k2 /= kdis; ktmp = sed0*(int)(k0 * 2000) + sed1*(int)(k1 * 2000) + sed2*(int)(k2 * 2000); if (mp[ktmp]) energy.push_back(0); else { exCloud->points[i].r = r_color0, exCloud->points[i].g = g_color0, exCloud->points[i].b = b_color0; energy.push_back(1 / (kdis*kdis*kdis)); } mp[ktmp] = 1; } shapeCloud(exCloud); */ } else { puts(" OAO invalid input, retry please OAO "); getExCloud(cloud, exCloud); } } void filterCloud(PointCloudX::Ptr cloud, double size) { VoxelGrid<PointX> voxelGrid; voxelGrid.setLeafSize(size, size, size); voxelGrid.setInputCloud(cloud), voxelGrid.filter(*cloud); printf("cloud size now : %d ", cloud->size()); } void filterCloud(PointCloudT::Ptr cloud, double size) { VoxelGrid<PointT> voxelGrid; voxelGrid.setLeafSize(size, size, size); voxelGrid.setInputCloud(cloud), voxelGrid.filter(*cloud); printf("cloud size now : %d ", cloud->size()); } void showPolygonMesh(PolygonMesh polygonMesh) { visualizer->addPolygonMesh(polygonMesh, "PolygonMesh0"); // visualizer->addCoordinateSystem(1.0); // visualizer->initCameraParameters(); iterationFlag_visualizer = false; while (!iterationFlag_visualizer) visualizer->spinOnce(); visualizer->removePolygonMesh("PolygonMesh0"); } void showPointCloudX(PointCloudX::Ptr cloud0) { visualizer->addPointCloud(cloud0, "Cloud0"); iterationFlag_visualizer = false; while (!iterationFlag_visualizer) visualizer->spinOnce(); visualizer->removePointCloud("Cloud0"); } void showPointCloudT(PointCloudT::Ptr cloud0, PointCloudT::Ptr cloud1) { visualizer->addPointCloud(cloud0, "Cloud0"); if (cloud1 != NULL) visualizer->addPointCloud(cloud1, "Cloud1"); iterationFlag_visualizer = false; while (!iterationFlag_visualizer) visualizer->spinOnce(); visualizer->removePointCloud("Cloud0"); if (cloud1 != NULL) visualizer->removePointCloud("Cloud1"); } void showTransform(const PointCloudWithNormals::Ptr targetCloud, const PointCloudWithNormals::Ptr sourceCloud) { PointCloudColorHandlerGenericField<PointNormal> colorHandler_target(targetCloud, "curvature"); if (!colorHandler_target.isCapable()) PCL_WARN("colorHandler_target error~"); PointCloudColorHandlerGenericField<PointNormal> colorHandler_source(sourceCloud, "curvature"); if (!colorHandler_source.isCapable()) PCL_WARN("colorHandler_source error~"); visualizer->addPointCloud(targetCloud, colorHandler_target, "targetCloud"); visualizer->addPointCloud(sourceCloud, colorHandler_source, "sourceCloud"); iterationFlag_visualizer = false; while (!iterationFlag_visualizer) visualizer->spinOnce(); visualizer->removePointCloud("targetCloud"); visualizer->removePointCloud("sourceCloud"); } void keyBoardEventOccurred(const pcl::visualization::KeyboardEvent& event, void *nothing) { if (event.getKeySym() == "space" && event.keyDown()) iterationFlag_visualizer = true; } void initVisualizer() { visualizer->registerKeyboardCallback(&keyBoardEventOccurred, (void*)NULL); visualizer->setBackgroundColor(246.0 / 255, 175.0 / 255, 245.0 / 255); // visualizer->setBackgroundColor(255.0 / 255, 255.0 / 255, 255.0 / 255); } // cloudRet 为 cloudTgt 反向转到与 cloudSrc 匹配后得到的点云 // transformingMatrix 为 cloudTgt 反向转到与 cloudSrc 匹配的时的矩阵 void cloudMerge(const PointCloudT::Ptr cloudSrc, const PointCloudT::Ptr cloudTgt, PointCloudT::Ptr cloudRet, Eigen::Matrix4f &transformingMatrix, bool flag_slow, bool filterFlag) { PointCloudT::Ptr src(new PointCloudT); PointCloudT::Ptr tgt(new PointCloudT); src->clear(), tgt->clear(); *src += *cloudSrc, *tgt += *cloudTgt; if (filterFlag) filterCloud(src), filterCloud(tgt); printf("final size: %d %d ", src->size(), tgt->size()); PointCloudWithNormals::Ptr src_pointsWithNormals(new PointCloudWithNormals); PointCloudWithNormals::Ptr tgt_pointsWithNormals(new PointCloudWithNormals); NormalEstimation<PointT, PointNormal> normalEstimation; search::KdTree<PointT>::Ptr kdtree(new search::KdTree<PointT>()); normalEstimation.setSearchMethod(kdtree); normalEstimation.setKSearch(30); normalEstimation.setInputCloud(src), normalEstimation.compute(*src_pointsWithNormals), copyPointCloud(*src, *src_pointsWithNormals); normalEstimation.setInputCloud(tgt), normalEstimation.compute(*tgt_pointsWithNormals), copyPointCloud(*tgt, *tgt_pointsWithNormals); //配准 IterativeClosestPointNonLinear<PointNormal, PointNormal> reg; reg.setTransformationEpsilon(1e-6); reg.setMaxCorrespondenceDistance(0.1); //对应点最大距离0.1m float tmpFloatValueArray[4] = { 1.0, 1.0, 1.0, 1.0 }; PointRepresentationT pointRepresentationT; pointRepresentationT.setRescaleValues(tmpFloatValueArray); reg.setPointRepresentation(boost::make_shared<const PointRepresentationT>(pointRepresentationT)); reg.setInputCloud(src_pointsWithNormals); reg.setInputTarget(tgt_pointsWithNormals); if (flag_slow) { string op; reg.setMaximumIterations(2); Eigen::Matrix4f fin, prev, inv; fin = Eigen::Matrix4f::Identity(); //单位矩阵 PointCloudWithNormals::Ptr tmpCloud = src_pointsWithNormals; for (int i = 0;; i++) { PCL_INFO("No. %d ", i); src_pointsWithNormals = tmpCloud, reg.setInputCloud(src_pointsWithNormals); reg.align(*tmpCloud); fin = reg.getFinalTransformation()*fin; if (i>0 && fabs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon()) reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001); prev = reg.getLastIncrementalTransformation(); showTransform(tgt_pointsWithNormals, src_pointsWithNormals); puts(" OwO input: 1.continue: continue to the next transformation; 2.break: stop transformation; OwO "); cin >> op; if (op.compare("continue") == 0) continue; else if (op.compare("break") == 0) break; else puts(" OAO invalid input, retry please OAO "); } inv = fin.inverse(); //从目标点云到源点云的变换 transformPointCloud(*cloudTgt, *cloudRet, inv); showPointCloudT(cloudSrc, cloudRet); transformingMatrix = inv; } else { int maximumIterations_input = 30; puts(" OwO input the maximum iterations (1 ~ 200), please OwO "); scanf("%d", &maximumIterations_input); maximumIterations_input = max(maximumIterations_input, 1), maximumIterations_input = min(maximumIterations_input, 30); reg.setMaximumIterations(maximumIterations_input); PointCloudWithNormals::Ptr tmpCloud = src_pointsWithNormals; Eigen::Matrix4f fin, inv; reg.align(*tmpCloud); fin = reg.getFinalTransformation(); inv = fin.inverse(); //从目标点云到源点云的变换 transformPointCloud(*cloudTgt, *cloudRet, inv); showPointCloudT(cloudSrc, cloudRet); transformingMatrix = inv; } } PointRepresentationT::PointRepresentationT() { nr_dimensions_ = 4; } void PointRepresentationT::copyToFloatArray(const PointNormal &p, float *out) const { out[0] = p.x, out[1] = p.y, out[2] = p.z, out[3] = p.curvature; }
#define _CRT_SECURE_NO_WARNINGS #define _SCL_SECURE_NO_WARNINGS #include <time.h> #include <stdlib.h> #include "kinect.h" #include <iostream> #include <cstdio> #include <cmath> #include <algorithm> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/opencv.hpp> #include <cv.h> #include <GLFWglfw3.h> #include <gl/glut.h> #include <thread> #include <mutex> #include <queue> #include <boost/make_shared.hpp> //boost指针相关头文件 #include <pcl/io/io.h> #include <pcl/visualization/cloud_viewer.h> #include <pcl/point_types.h> //点类型定义头文件 #include <pcl/point_cloud.h> //点云类定义头文件 #include <pcl/point_representation.h> //点表示相关的头文件 #include <pcl/io/pcd_io.h> //PCD文件打开存储类头文件 #include <pcl/filters/voxel_grid.h> //用于体素网格化的滤波类头文件 #include <pcl/filters/filter.h> //滤波相关头文件 #include <pcl/features/normal_3d.h> //法线特征头文件 #include <pcl/registration/icp.h> //ICP类相关头文件 #include <pcl/registration/icp_nl.h> //非线性ICP 相关头文件 #include <pcl/registration/transforms.h> //变换矩阵类头文件 #include <pcl/visualization/pcl_visualizer.h> //可视化类头文件 #include "OpenCVFxxL.h" #include "KinectFxxL.h" #include "PCLFxxL.h" #define check_hr(x) if(hr<0) return false #define pause system("pause") using namespace cv; using namespace std; using namespace pcl; using pcl::visualization::PointCloudColorHandlerGenericField; using pcl::visualization::PointCloudColorHandlerCustom; typedef PointXYZRGBA PointT; typedef PointCloud<PointT> PointCloudT; typedef PointXYZ PointX; typedef PointCloud<PointX> PointCloudX; typedef PointCloud<PointNormal> PointCloudWithNormals; const int n_depth = 424; const int m_depth = 512; const int n_color = 1080; const int m_color = 1920; HRESULT hr; IKinectSensor *pKinectSensor; ICoordinateMapper *pCoordinateMapper; IDepthFrameReader *pDepthFrameReader; IColorFrameReader *pColorFrameReader; IDepthFrame *pDepthFrame = NULL; IColorFrame *pColorFrame = NULL; IFrameDescription *pFrameDescription = NULL; IDepthFrameSource *pDepthFrameSource = NULL; IColorFrameSource *pColorFrameSource = NULL; USHORT depthMinReliableDistance, depthMaxReliableDistance; UINT bufferSize_depth, bufferSize_color; UINT16 *pBuffer_depth = NULL; uchar *pBuffer_color = NULL; ColorImageFormat imageFormat_color; int width_depth, height_depth, width_color, height_color; DepthSpacePoint *pDepthSpace = NULL; ColorSpacePoint *pColorSpace = NULL; CameraSpacePoint *pCameraSpace = NULL; UINT16 *image_depth; BYTE *image_color; Mat imageMat_depth, imageMat_color; PointCloudT::Ptr cloud(new PointCloudT), prevCloud(new PointCloudT), finCloud(new PointCloudT), tmpCloud(new PointCloudT), lastFinCloud(new PointCloudT), lastPrevCloud(new PointCloudT); int cnt_image; bool getDepthImage() { pDepthFrame = NULL; while (hr < 0 || pDepthFrame == NULL) hr = pDepthFrameReader->AcquireLatestFrame(&pDepthFrame); hr = pDepthFrame->get_FrameDescription(&pFrameDescription); check_hr(hr); pFrameDescription->get_Width(&width_depth); pFrameDescription->get_Height(&height_depth); hr = pDepthFrame->get_DepthMinReliableDistance(&depthMinReliableDistance); check_hr(hr); hr = pDepthFrame->get_DepthMaxReliableDistance(&depthMaxReliableDistance); check_hr(hr); pDepthFrame->AccessUnderlyingBuffer(&bufferSize_depth, &pBuffer_depth); imageMat_depth = convertDepthToMat(pBuffer_depth, width_depth, height_depth); hr = pDepthFrame->CopyFrameDataToArray(bufferSize_depth, image_depth); pDepthFrame->Release(); // equalizeHist(imageMat_depth, imageMat_depth); return true; } bool getColorImage() { pColorFrame = NULL; while (hr < 0 || pColorFrame == NULL) hr = pColorFrameReader->AcquireLatestFrame(&pColorFrame); hr = pColorFrame->get_FrameDescription(&pFrameDescription); check_hr(hr); pFrameDescription->get_Width(&width_color); pFrameDescription->get_Height(&height_color); imageMat_color = Mat(height_color, width_color, CV_8UC4); pBuffer_color = imageMat_color.data; bufferSize_color = width_color * height_color * 4; hr = pColorFrame->CopyConvertedFrameDataToArray(bufferSize_color, reinterpret_cast<BYTE*>(pBuffer_color), ColorImageFormat::ColorImageFormat_Bgra); hr = pColorFrame->CopyConvertedFrameDataToArray(bufferSize_color, image_color, ColorImageFormat::ColorImageFormat_Bgra); pColorFrame->Release(); return true; } bool getPoints(bool flag_slow) { hr = pCoordinateMapper->MapDepthFrameToColorSpace(n_depth*m_depth, image_depth, n_depth*m_depth, pColorSpace); check_hr(hr); hr = pCoordinateMapper->MapDepthFrameToCameraSpace(n_depth*m_depth, image_depth, n_depth*m_depth, pCameraSpace); check_hr(hr); CameraSpacePoint p_camera; ColorSpacePoint p_color; PointT p; int cnt = 0, px_color, py_color; for (int i = 0; i < n_depth*m_depth; i++) { p_camera = pCameraSpace[i]; p_color = pColorSpace[i]; px_color = (int)(p_color.X + 0.5), py_color = (int)(p_color.Y + 0.5); if (px_color >= 0 && px_color < m_color && py_color >= 0 && py_color < n_color) { cnt++; p.b = image_color[(py_color*m_color + px_color) * 4], p.g = image_color[(py_color*m_color + px_color) * 4 + 1], p.r = image_color[(py_color*m_color + px_color) * 4 + 2]; p.x = p_camera.X, p.y = p_camera.Y, p.z = p_camera.Z; cloud->points.push_back(p); } } printf("number of points: %d ", cnt); shapeCloud(cloud); showPointCloudT(cloud); if (cnt_image) { Eigen::Matrix4f transformingMatrix; cloudMerge(prevCloud, cloud, tmpCloud, transformingMatrix, flag_slow); cloud->clear(), *cloud += *tmpCloud, shapeCloud(cloud); } lastPrevCloud->points.clear(), *lastPrevCloud += *prevCloud, shapeCloud(lastPrevCloud); lastFinCloud->points.clear(), *lastFinCloud += *finCloud, shapeCloud(lastFinCloud); prevCloud->points.clear(), *prevCloud += *cloud, shapeCloud(prevCloud); *finCloud += *cloud, shapeCloud(finCloud); filterCloud(finCloud, 0.002), shapeCloud(finCloud); cloud->points.clear(); return true; } void init() { cnt_image = 0; finCloud->clear(); initVisualizer(); pColorSpace = new ColorSpacePoint[n_depth*m_depth]; pCameraSpace = new CameraSpacePoint[n_depth*m_depth]; image_color = new BYTE[n_color*m_color * 4]; image_depth = new UINT16[n_depth*m_depth]; } bool start() { String op; hr = GetDefaultKinectSensor(&pKinectSensor); check_hr(hr); hr = pKinectSensor->Open(); check_hr(hr); hr = pKinectSensor->get_CoordinateMapper(&pCoordinateMapper); check_hr(hr); hr = pKinectSensor->get_DepthFrameSource(&pDepthFrameSource); check_hr(hr); hr = pDepthFrameSource->OpenReader(&pDepthFrameReader); check_hr(hr); hr = pKinectSensor->get_ColorFrameSource(&pColorFrameSource); check_hr(hr); hr = pColorFrameSource->OpenReader(&pColorFrameReader); check_hr(hr); while (1) { bool flag_slow; puts(" OwO input: 1.work_slow: get next cloud and show every step in transform; 2.work: get next cloud; 3.exit: exit this program; 4.show: show final cloud; 5.show_ex: show final cloud with some change; 6.show_triangles: show the triangles; 7.back: back to the cloud. OwO "); cin >> op; if (op.compare("exit") == 0) return true; else if (op.compare("work") == 0 || op.compare("work_slow") == 0) { if (op.compare("work_slow") == 0) flag_slow = true; else flag_slow = false; if (getDepthImage() == false) return false; if (getColorImage() == false) return false; imwrite("tmp\" + convertIntToString(cnt_image) + "_image_depth.jpg", imageMat_depth); // showImage(imageMat_depth); imwrite("tmp\" + convertIntToString(cnt_image) + "_image_color.jpg", imageMat_color); // showImage(imageMat_color); if (!getPoints(flag_slow)) return puts(" OAO get points failed OAO "), false; cnt_image++; } else if (op.compare("show") == 0) showPointCloudT(finCloud); else if (op.compare("show_ex") == 0) { PointCloudT::Ptr exFinCloud(new PointCloudT); getExCloud(finCloud, exFinCloud); showPointCloudT(exFinCloud); } else if (op.compare("show_triangles") == 0) { PolygonMesh triangles; getTriangles(finCloud, triangles); showPolygonMesh(triangles); } else if (op.compare("back") == 0) { finCloud->clear(), *finCloud += *lastFinCloud, shapeCloud(finCloud); prevCloud->clear(), *prevCloud += *lastPrevCloud, shapeCloud(prevCloud); puts("OwO finished"); } else puts(" OAO invalid input, retry please OAO "); } return true; } int main() { init(); if (!start()) return puts(" OAO init failed OAO "), 0; puts(" OwO program ends OwO "); pause; return 0; }