用VS+Opencv3.1从双目立体视差图中重建三维点云-CSDN博客

基本原理

详细原理请阅读这篇文章http://www.360doc.com/content/14/0205/15/10724725_349968116.shtml.

双目立体视觉几何原理图

基本流程

点云重建基本流程

代码

本代码运行需要在VS上配置好opencv3.1+openNI+PCL，opencv3.1的配置可以在网上找到很多资料，openNI和PCL的配置可以参看上一篇博文下http://blog.csdn.net/u014283958/article/details/52599457
下面是代码：

//by shuishui shiwenjun 20160926
#include <pcl/visualization/cloud_viewer.h>
#include <iostream> 
#include <pcl/io/io.h> 
#include <pcl/io/pcd_io.h> 
#include <opencv2/opencv.hpp> 
 
usingnamespace cv;
usingnamespacestd;
usingnamespace pcl;
 
int user_data;
//相机内参，根据输入改动
constdouble u0 = 1329.49 / 4;//由于后面resize成原图的1/4所以有些参数要缩小相同倍数
constdouble v0 = 954.485 / 4;
constdouble fx = 6872.874 / 4;
constdouble fy = 6872.874 / 4;
constdouble Tx = 174.724;
constdouble doffs = 293.97 / 4;
 
void viewerOneOff(visualization::PCLVisualizer& viewer)
{
    viewer.setBackgroundColor(0.0, 0.0, 0.0);
}
 
int main()
{
    PointCloud<PointXYZRGB>cloud_a;
   PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
    Mat color1 = imread("im0.png");
    Mat depth = imread("Sword1_perfect_d.png");
    Resize
    //color1.resize();
    Mat color;
    resize(color1, color,Size(color1.cols/4,color1.rows/4), 0, 0,CV_INTER_LINEAR);
    //imshow("h",color);
    //waitKey(0);
    int rowNumber = color.rows;
    int colNumber = color.cols;
    cloud_a.height = rowNumber;
    cloud_a.width = colNumber;
   cloud_a.points.resize(cloud_a.width * cloud_a.height);
    for (unsignedint u = 0; u < rowNumber; ++u)
    {
        for (unsignedint v = 0; v < colNumber; ++v)
        {
            /*unsigned intnum = rowNumber*colNumber-(u*colNumber + v)-1;*/
            unsignedint num = u*colNumber + v;
            double Xw = 0, Yw = 0, Zw = 0;
 
            Zw = fx*Tx / (((double)depth.at<Vec3b>(u, v)[0]) + doffs);
            Xw = (v+1 - u0) * Zw / fx;
            Yw = (u+1 - v0) * Zw / fy;
            cloud_a.points[num].b =color.at<Vec3b>(u, v)[0];
            cloud_a.points[num].g =color.at<Vec3b>(u, v)[1];
            cloud_a.points[num].r =color.at<Vec3b>(u, v)[2];
            cloud_a.points[num].x =Xw;
            cloud_a.points[num].y =Yw;
            cloud_a.points[num].z =Zw;
        }
    }
    *cloud = cloud_a;
    visualization::CloudViewerviewer("Cloud Viewer");
    viewer.showCloud(cloud);
   viewer.runOnVisualizationThreadOnce(viewerOneOff);
    while (!viewer.wasStopped())
    {
        user_data = 9;
    }
    return0;
}