时间戳

首页 > 解决方案 > 将检测到的 2D 地图调整为参考 2D 地图

c++ - 将检测到的 2D 地图调整为参考 2D 地图

问题描述

我有一张地图，其中一些参考位置对应于一些对象的中心（小十字），如下所示：
参考位置

我拍照是为了找到我的对象，但在图片中我有一些噪音，所以我不能总是找到所有的对象，它可能是这样的：
检测到的位置

从少数找到的位置，我需要知道图片中其他未找到的对象应该在哪里。在过去的几天里，我一直在阅读这方面的内容并进行试验，但我找不到合适的方法来做到这一点。在某些示例中，它们首先计算质心并将它们一起平移，然后旋转，其他一些示例使用最小二乘法最小化并从旋转开始。我不能使用 OpenCV 或任何其他 API，只能使用普通 C++。如果有帮助，我可以使用 Eigen 库。谁能给我一些指示？

编辑：
我已经解决了点之间的对应关系，图片与参考没有太大的不同，因此对于每个找到的位置，我都可以搜索其对应的参考。简而言之，我有一个带有参考点的二维矩阵和另一个带有找到点的二维矩阵。在找到的点矩阵中，未找到的点保存为 NaN 只是为了保持相同的矩阵大小，计算中不使用 NaN 点。

标签： c++algorithmmath2d

解决方案

由于您已经将这些点相互匹配，因此找到变换很简单：

Eigen::Affine2d findAffine(Eigen::Matrix2Xd const& refCloud, Eigen::Matrix2Xd const& targetCloud)
{
    // get translation
    auto refCom = centerOfMass(refCloud);
    auto refAtOrigin = refCloud.colwise() - refCom;

    auto targetCom = centerOfMass(targetCloud);
    auto targetAtOrigin = targetCloud.colwise() - targetCom;

    // get scale
    auto scale = targetAtOrigin.rowwise().norm().sum() / refAtOrigin.rowwise().norm().sum();

    // get rotation
    auto covMat = refAtOrigin * targetAtOrigin.transpose();
    auto svd = covMat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
    auto rot = svd.matrixV() * svd.matrixU().transpose();   

    // combine the transformations
    Eigen::Affine2d trans = Eigen::Affine2d::Identity();
    trans.translate(targetCom).scale(scale).rotate(rot).translate(-refCom);
    return trans;
}

refCloud是您的参考点集，targetCloud是您在图像中找到的点集。云与索引匹配很重要，因此refCloud[n]必须是 的对应点targetCloud[n]。这意味着您必须从矩阵中删除所有 NaN 并挑选参考点集中的对应项。

这是一个完整的例子。我正在使用 OpenCV 来绘制这些东西：

#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

#include <vector>
#include <iostream>

using Point = Eigen::Vector2d;

template <typename TMatrix>
Point centerOfMass(TMatrix const& points)
{
    return points.rowwise().sum() / points.cols();
}

Eigen::Affine2d findAffine(Eigen::Matrix2Xd const& refCloud, Eigen::Matrix2Xd const& targetCloud)
{
    // get translation
    auto refCom = centerOfMass(refCloud);
    auto refAtOrigin = refCloud.colwise() - refCom;

    auto targetCom = centerOfMass(targetCloud);
    auto targetAtOrigin = targetCloud.colwise() - targetCom;

    // get scale
    auto scale = targetAtOrigin.rowwise().norm().sum() / refAtOrigin.rowwise().norm().sum();

    // get rotation
    auto covMat = refAtOrigin * targetAtOrigin.transpose();
    auto svd = covMat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
    auto rot = svd.matrixV() * svd.matrixU().transpose();   

    // combine the transformations
    Eigen::Affine2d trans = Eigen::Affine2d::Identity();
    trans.translate(targetCom).scale(scale).rotate(rot).translate(-refCom);
    return trans;
}

void drawCloud(cv::Mat& img, Eigen::Matrix2Xd const& cloud, Point const& origin, Point const& scale, cv::Scalar const& color, int thickness = cv::FILLED)
{
    for (int c = 0; c < cloud.cols(); c++)
    {
        auto p = origin + cloud.col(c).cwiseProduct(scale);
        cv::circle(img, {int(p.x()), int(p.y())}, 5, color, thickness, cv::LINE_AA);
    }
}

int main()
{
    // generate sample reference
    std::vector<Point> points = {{4, 9}, {4, 4}, {6, 9}, {6, 4}, {8, 9}, {8, 4}, {10, 9}, {10, 4}, {12, 9}, {12, 4}};
    Eigen::Matrix2Xd fullRefCloud(2, points.size());
    for (int i = 0; i < points.size(); i++)
        fullRefCloud.col(i) = points[i];

    // generate sample target
    Eigen::Matrix2Xd refCloud = fullRefCloud.leftCols(fullRefCloud.cols() * 0.6);
    Eigen::Affine2d refTransformation = Eigen::Affine2d::Identity();
    refTransformation.translate(Point(8, -4)).rotate(4.3).translate(-centerOfMass(refCloud)).scale(1.5);
    Eigen::Matrix2Xd targetCloud = refTransformation * refCloud;

    // find the transformation
    auto transform = findAffine(refCloud, targetCloud);
    std::cout << "Original: \n" << refTransformation.matrix() << "\n\nComputed: \n" << transform.matrix() << "\n";

    // apply the computed transformation
    Eigen::Matrix2Xd queryCloud = fullRefCloud.rightCols(fullRefCloud.cols() - refCloud.cols());
    queryCloud = transform * queryCloud;

    // draw it
    Point scale = {15, 15}, origin = {100, 300};
    cv::Mat img(600, 600, CV_8UC3);
    cv::line(img, {0, int(origin.y())}, {800, int(origin.y())}, {});
    cv::line(img, {int(origin.x()), 0}, {int(origin.x()), 800}, {});    
    drawCloud(img, refCloud, origin, scale, {0, 255, 0});
    drawCloud(img, fullRefCloud, origin, scale, {255, 0, 0}, 1);
    drawCloud(img, targetCloud, origin, scale, {0, 0, 255});
    drawCloud(img, queryCloud, origin, scale, {255, 0, 255}, 1);

    cv::flip(img, img, 0);
    cv::imshow("img", img);
    cv::waitKey();

    return 0;
}

推荐阅读