• 基于Kinect 2.0深度摄像头的三维重建


    刚今天验收的实验,记录一下。

    是比较基础的三维重建内容。

    算是三维重建入门。

    系统: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
    KinectFxxL.h
    #include <Kinect.h>
    #include "KinectFxxL.h"
    KinectFxxL.cpp
    #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
    OpenCVFxxL.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>  
    
    #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;
    }
    OpenCVFxxL.cpp
    #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
    PCLFxxL.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/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;
    }
    PCLFxxL.cpp
    #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;
    }
    main.cpp
  • 相关阅读:
    JVM性能调优监控工具jps、jstack、jmap、jhat、jstat使用详解
    JVM运行时内存结构
    nginx(Window下安装 & 配置文件参数说明 & 实例)
    Linux常用命令
    Redis(Windows安装方法与Java调用实例 & 配置文件参数说明 & Java使用Redis所用Jar包 & Redis与Memcached区别 & redis-cli.exe命令及示例)
    Caché数据库学习笔记(3)
    Caché数据库学习笔记(2)
    Caché数据库学习笔记(1)
    [读书笔记]机器学习:实用案例解析(6)
    [读书笔记]机器学习:实用案例解析(5)
  • 原文地址:https://www.cnblogs.com/FxxL/p/9147087.html
Copyright © 2020-2023  润新知