深度图转点云（Xtion/Kinect/OB等）

　　今天分享一个将深度图转点云的方法。
　　所谓的深度图一般指从深度相机中读取到的带有深度信息的一帧数据，一般为16bit的png/pgm格式。深度图不适合直观的去察看，点云的效果会更强，所以，一般我们都是将深度图转成点云再察看。

其实，像Xtion/Kinect等深度相机可以直接输出点云，当然，深度图的获取也不仅是以上几种设备，例如双目稠密重建算法也可以。我这里用到的是华硕的Xtion采集的数据。

基本原理

• 基本原理很简单，其实就是简单的相似三角形。先看下面示意图：
  
  如上图所示，将深度图上的m点转换成世界坐标的M点，也就是深度图转点云的子过程。我们假设深度图中心为O’(图中没有标注，失误），则可利用图中三角形OmO’和OMA相似，所以可得$\frac{OO'}{OA}=\frac{mO'}{MA}$，将这个相似关系反应到内存上，就是:
  $$u=f_x\frac{X}{Z}+c_x$$ $$v=f_x\frac{Y}{Z}+c_y$$
  其中m点坐标是（u，v），这个在图中也没有标注！～～
  其实，这也是内参矩阵的推导过程。 不过，要将像素坐标转换为齐次坐标。
  $$\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \frac{1}{Z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ \end{bmatrix}$$
  习惯性的将$\frac{1}{Z}$写到左边，则上式变为
  $$Z\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ \end{bmatrix}$$
  我们将矩阵
  $$\begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \\ \end{bmatrix}$$
  称做内参数矩阵（Camera Intrinsics）K。令P（X，Y，Z），则上式可写为：
  $$P_{uv} = KTP_w$$
  好，下面看代码。

转换关键代码

void depth2PointCloud(Mat imDepth, std::string filePath)
{
    //-- 结构光相机IR传感器内参，我这里用的是Xtion
    float fx = 525.0;
    float fy = 525.0;
    float cx = 319.5;
    float cy = 239.5;

    mypoint.clear();
    for(int i=0; i<imDepth.rows; i++) {
        for(int j=0; j<imDepth.cols; j++) {
            if( imDepth.at<unsigned short>(i,j)!=0 ) {
                unsigned short Zw = imDepth.at<unsigned short>(i, j);
                float Xw, Yw, Zw;
                Xw = Yw = Zw = 0;

                Xw = (j-cx) *  Zw / fx;
                Yw = (i-cy) * Zw / fy;
                Zw= (float)(Zw/1000.0);

                point myp;
                myp.x=Xw;
                myp.y=Yw;
                myp.z=Zw;
                mypoint.push_back(myp);
           }
        }
    }

    std::ofstream os(filePath.c_str());
    int num_p=mypoint.size();
    os<<"ply"<<"\n"<<"format ascii 1.0"<<"\n";
    os<<"element vertex "<<num_p<<"\n";
    os<<"property float x"<<"\n";
    os<<"property float y"<<"\n";
    os<<"property float z"<<"\n";
    //add************2017/6/28
    os<<"property uchar red"<<"\n";
    os<<"property uchar green"<<"\n";
    os<<"property uchar blue"<<"\n";
    //add************2017/6/28
    os<<"end_header"<<"\n";

    qDebug() << mypoint.size();

    for(int i=0;i<mypoint.size();i++)  {
//-- 这里可以增加RGB信息到ply文件中
//        os<<mypoint[i].x<<" "<<mypoint[i].y<<" "<<mypoint[i].z<<" "<< mypoint[i].r<<" "<<\
//            mypoint[i].g<<" "<<mypoint[i].b<<endl;
        os<<mypoint[i].x<<"     "<<mypoint[i].y<<"      "<<mypoint[i].z<< "\n";
   }
   os.close();
}

---

为其增加一点人性化的枝叶，即读取当前文件夹所有的深度图，自动转换。

void on_pb_depth2Pointcloud_clicked()
{
    QString Path = QFileDialog::getExistingDirectory(this, tr("选择文件夹"), QDir::currentPath().append("/img/"));

    /**************get file List****************/
    QDir *dir=new QDir(Path);
    QStringList filter;
    filter << "*.pgm" << "png";
    dir->setNameFilters(filter);
    QList<QFileInfo> *fileInfo=new QList<QFileInfo>(dir->entryInfoList(filter));

    if( fileInfo->isEmpty() ) {
        QMessageBox::warning(NULL, "Warning!!!", "Is there have no Image?");
        return;
    }

    //删除文件路径中的文件名
    QString rootPath = fileInfo->at(0).filePath();
    rootPath.replace(QRegExp(fileInfo->at(0).fileName()), "");

    int num = 0;
    foreach (QFileInfo tmp, *fileInfo) {
        //IMREAD_ANYDEPTH读取深度图必须添加此参数，否则不是按照16位读取
        Mat imDepth = imread(tmp.filePath().toStdString(), IMREAD_ANYDEPTH);
        qDebug() << tmp.filePath();

        QString tmp_rootPath = rootPath;
        tmp_rootPath += QString::number(num);
        tmp_rootPath += ".txt";
        depth2PointCloud(imDepth, tmp_rootPath.toStdString());
        num++;
    }
}