Question

我正在尝试通过一系列图像来构建路面的俯视图，这些图像我都知道地面真相的构成。数据来自KITTI数据集，据我所知，相机与地面的距离为。我包括了数据集中的样本图像。

我根据方程式here计算真实世界的坐标（请参见下面的imageToWorldPoints函数），在该方程式中，我根据摄像机相对于地面的高度，投影矩阵和摄像机的姿势计算了比例。然后，我只需将它们放入0.1m分辨率的网格图中即可。

road markers in generated map are not parallel

我还不能嵌入图像，但是如您在上面的链接中所见，我生成的地图严重超出了“比例”。白色像素是道路标记。 Y较高（底部行）中的标记是更靠近相机的标记，它们是在较早的姿势中生成的。因此，您可以想象相机正在从图像底部向上移动。车道应该是平行的，但是在这里我变成了圆锥形。

我有2个问题：

参考系是相机在第一个序列中的姿态。我的假设是这种圆锥形的原因是由于参考框架的轴不平行于路面，导致对远处的点的比例尺的错误计算。这有效吗？
我注意到映射到鸟瞰图的另一种方法是将道路上的4个点映射到一个矩形，然后使用opencv的warpPerspective函数生成鸟瞰图。这和我上面的方法有什么区别？这样可以解决锥形行为吗？

我的代码

#include <string>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <fstream>
#include <map>
#include <numeric>

#define HEIGHT 1.65

using namespace Eigen;
using namespace std;

bool nextPose(ifstream &f, Eigen::MatrixXf& pose){
    bool hasPose = false;

    if(f.peek() != EOF) {
        hasPose = true;
        for (int i = 0; i < 12; ++i) {
            float v;
            f >> v;
            int y = i / 4;
            int x = i % 4;
            pose(y, x) = v;
        }
    }
    return hasPose;
}

MatrixXf getPMatrix(){
    MatrixXf P(3, 4);
    P << 7.070912000000e+02, 0.000000000000e+00,  6.018873000000e+02, 4.688783000000e+01,
        0.000000000000e+00, 7.070912000000e+02, 1.831104000000e+02, 1.178601000000e-01,
        0.000000000000e+00, 0.000000000000e+00, 1.000000000000e+00, 6.203223000000e-03;
    return P;
}


VectorXf imageToWorldPoints(float u, float v, const MatrixXf& pose, const MatrixXf& proj){
    Matrix4f pose2 = Matrix4f::Zero();
    pose2.topLeftCorner<3, 4>() = pose;
    pose2(3, 3) = 1;
    MatrixXf pose3 = pose2.inverse();


    Vector3f imagePoint(u, v, 1);

    MatrixXf R = pose3.topLeftCorner<3,3>();
    VectorXf t = pose3.col(3).head(3);

    MatrixXf lMatrix = R.inverse() * proj.topLeftCorner<3,3>().inverse() * imagePoint;
    MatrixXf rMatrix = R.inverse() * t;

    float s = (HEIGHT + rMatrix(1, 0)) / lMatrix(1, 0);
    VectorXf realWorld = R.inverse() * (s * proj.topLeftCorner<3,3>().inverse() * imagePoint - t);

    return realWorld;
}


typedef map<long, map<int, int> > CellAssign;

int main(){
    const int size = 1000;
    const float resolution = 0.1;
    const float  minX = -50;
    const float  minY = -50;

    string labelDir = "/home/cy/Desktop/Kitti_segmented/resize/results/";

    ifstream pose_file;
    pose_file.open("/ext/data/odometry/dataset/poses/04.txt");

    if (!pose_file){
        cerr << "Cannot open pose file" << endl;
    }

    MatrixXf proj = getPMatrix() ;

    bool hasPose = true;
    int frameNum = 0;

    map<pair<int, int>, CellAssign> patchMapping;

    while (hasPose) {
        MatrixXf pose(3,4);
        hasPose = nextPose(pose_file, pose);

        int pad = 6 - to_string(frameNum).length();
        stringstream filename;
        filename << labelDir << string(pad, '0') << frameNum << ".png";

        cv::Mat img = cv::imread(filename.str(), CV_LOAD_IMAGE_GRAYSCALE);
        cv::Size shape = img.size();   

        for (int row = 0; row < shape.height; row++){
            for (int col = 0; col < shape.width; col++){
                uint8_t label = img.at<uchar>(row, col);

//                if (label == 24){
                if (row > 250){
                    //convert u,v coordinate to world coordinate
                    VectorXf realWorld = imageToWorldPoints(col, row, pose, proj);

                    //calculate which cell inside map
                    int patchX = realWorld(0) / (resolution * size);
                    int patchY = realWorld(2) / (resolution * size);
                    int cellX = fmod(realWorld(0), resolution * size) / resolution;
                    int cellY = fmod(realWorld(2), resolution * size) / resolution;
                    cellX =(500 + cellX) % size;
//                    cellY =(500 + cellY) % size;

                    if (cellX < 0){
                        cout << "less 0" << endl;
                        continue;
                    }

                    pair<int, int> gp = {patchX, patchY};
                    long cellId = cellX * size + cellY;

                    patchMapping.insert({gp, CellAssign()});
                    patchMapping[gp].insert({cellId, map<int, int>()});
                    patchMapping[gp][cellId].insert({(int)label, 0});

                    patchMapping[gp][cellId][label]++;

                }
            }
        }


        //TODO: remove this
        frameNum++;
        if (frameNum > 50)
            break;
    }

    for (const auto &it: patchMapping){
        int x = it.first.first;
        int y = it.first.second;
        const CellAssign &ca = it.second;

        cv::Mat mapArea = cv::Mat::zeros(size, size, CV_8UC1);

        int maxPts = 0;

        for (const auto &it2: ca){
            long cellId = it2.first;
            const map<int, int> &pointMap = it2.second;

            int cellX = cellId % size;
            int cellY = cellId / size;

            int maxLabel = -1;
            int maxCount = -1;

            for(auto &maxIter: pointMap){
                if (maxCount < maxIter.second){
                    maxCount = maxIter.second;
                    maxLabel = maxIter.first;
                }
            }

            //uint8_t label = points[points.size()-1];
            if (maxLabel == 24) {
                mapArea.at<uchar>(cellX, cellY) = 255;
            } else {
                mapArea.at<uchar>(cellX, cellY) = 128;
            }
        }

        stringstream filename;

        filename << "/home/cy/Desktop/x" << x << "_y_" << y << ".png";
        cv::Mat mapAreaF = cv::Mat::zeros(size, size, CV_8UC1);
        cv::flip(mapArea, mapAreaF, 0);
        cv::imwrite(filename.str(), mapAreaF);
    }

    return 0;
}

从具有一系列姿势的图像计算自上而下/鸟瞰图

0 个答案: