深度图转点云图记录

深度图转点云图

包含了单应性矩阵的使用

单应性矩阵主要是负责像素坐标到世界坐标的转换

每个点云图都由三个维度组成xLine、yLine、zLine

那么Point[0]的坐标就是(xLine[0], yLine[1], zLine[2])

void LT360XWindow::onConverToPointMap()
{
    const QString xmlStr = "D:\\Document\\GtCode\\apps\\bin\\x64\\Release\\temp\\OIS-IMG\\OIS0%1.xml";
    const QString matStr = "D:\\Document\\GtCode\\apps\\bin\\x64\\Release\\temp\\OIS-IMG\\OIS0%1.mat";
    const QString pmpStr = "D:\\Document\\GtCode\\apps\\bin\\x64\\Release\\temp\\OIS-IMG\\OIS0%1.pmf";
    for (int i=1;i<=3;++i)
    {
        QString xmlFile = xmlStr.arg(i);
        cv::FileStorage cvFile;
        bool isOk = cvFile.open(xmlFile.toStdString(), cv::FileStorage::READ);
        if (!isOk)
        {
            OiWarning() << "Fail to open: " << xmlFile;
            continue;
        }
        cv::Mat curHomo;
        cvFile["Homo1"] >> curHomo;
        curHomo.at<double>(2, 2) = 1.0;
        cvFile.release();
        QString matFile = matStr.arg(i);
        Core::RangeMapPtr rmPtr = Core::RangeMap::create(matFile);
        int rowNum = rmPtr->rows();
        if (rowNum > 10)
        {
            rowNum = 10;
        }
        int colNum = rmPtr->cols();
        Core::PointMapPtr pmp = Core::PointMap::create(colNum, rowNum);
        pmp->resize(rowNum);
        for (int r = 0; r < rowNum; ++r)
        {
            float* rmLine = rmPtr->profile(r);
            float* xLine = pmp->xLine(r);
            float* yLine = pmp->yLine(r);
            float* zLine = pmp->zLine(r);
            for (int c = 0; c < colNum; ++c)
            {
                double rx = c;
                double ry = rmLine[c];
                float denum = curHomo.at<double>(2, 0) * rx + curHomo.at<double>(2, 1) * ry + curHomo.at<double>(2, 2);
                float x = (curHomo.at<double>(0, 0) * rx + curHomo.at<double>(0, 1) * ry + curHomo.at<double>(0, 2)) / denum;
                float y = (curHomo.at<double>(1, 0) * rx + curHomo.at<double>(1, 1) * ry + curHomo.at<double>(1, 2)) / denum;

                xLine[c] = x;
                yLine[c] = 0.1 * r;
                zLine[c] = y;
            }
        }
        QString pmpFile = pmpStr.arg(i);
        pmp->save(pmpFile);
    }
}