单目相机测距

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>


Eigen::Matrix4f m_camera2robot;
Eigen::Matrix3f cameraIntrinsic;
cv::Mat distCoeffs;




void buildPlaneFromPoints(const Eigen::Vector3f& point1, const Eigen::Vector3f& point2, const Eigen::Vector3f& point3, Eigen::Vector4f& plane) {
    // Compute two vectors lying on the plane
    Eigen::Vector3f vector1 = point2 - point1;
    Eigen::Vector3f vector2 = point3 - point1;

    // Compute the normal vector of the plane (cross product of vector1 and vector2)
    Eigen::Vector3f normal_vector = vector1.cross(vector2);

    // Check if the normal vector is zero (which happens when the points are collinear or not unique)
    if (normal_vector.isZero()) {
        std::cout << "Error: The provided points do not form a valid plane (they might be collinear or identical)." << std::endl;
        plane.setZero();
        return;
    }

    // Normalize the normal vector
    normal_vector.normalize();

    // Compute the D coefficient of the plane equation using one of the points
    float D = -normal_vector.dot(point1);

    // Set the coefficients of the plane equation (Ax + By + Cz + D = 0)
    // Here we make sure that the plane is assigned correctly without causing alignment issues
    plane.head<3>() = normal_vector;
    plane[3] = D;
}

void tranformatePlane(const Eigen::Vector4f& planeInA, const Eigen::Matrix4f& T_A2B, Eigen::Vector4f& planeInB) {
    // Extract normal vector from plane
    Eigen::Vector3f normal_vector = (planeInA.head<3>()).normalized();

    // Find the index of the maximum absolute component in the normal vector
    int max_index = 0;
    for (int i = 1; i < 3; ++i) {
        if (std::abs(normal_vector(i)) > std::abs(normal_vector(max_index))) {
            max_index = i;
        }
    }
    Eigen::Vector3f pointA;
    Eigen::Vector3f pointB;
    Eigen::Vector3f pointC;
    pointA.setZero();
    pointB.setZero();
    pointC.setZero();
    pointA(max_index) = 1.0f;
    pointB((max_index + 1) % 3) = 1.0f;
    pointC((max_index + 2) % 3) = 1.0f;

    float scale_factor = -planeInA(3) / normal_vector(max_index);
    pointA = pointA * scale_factor;
    pointB(max_index) = -(pointB.dot(normal_vector) + planeInA(3)) / normal_vector(max_index);
    pointC(max_index) = -(pointC.dot(normal_vector) + planeInA(3)) / normal_vector(max_index);

    Eigen::Vector3f pointA2 = T_A2B.block<3, 3>(0, 0) * pointA + T_A2B.block<3, 1>(0, 3);
    Eigen::Vector3f pointB2 = T_A2B.block<3, 3>(0, 0) * pointB + T_A2B.block<3, 1>(0, 3);
    Eigen::Vector3f pointC2 = T_A2B.block<3, 3>(0, 0) * pointC + T_A2B.block<3, 1>(0, 3);

    buildPlaneFromPoints(pointA2, pointB2, pointC2, planeInB);
}
bool getLinePlaneCrossPt(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, const Eigen::Vector4f& plane, Eigen::Vector3f& crossPoint) {
    // 求分子P1D
    auto P1D = plane[0] * p1[0] + plane[1] * p1[1] + plane[2] * p1[2] + plane[3]; // ax1+by1+cz1+d//or采用向量点乘方式

    const Eigen::Vector3f P1P2 = p2 - p1;

    // 求分母P1D2
    auto P1D2 = plane[0] * P1P2[0] + plane[1] * P1P2[1] + plane[2] * P1P2[2];
    if (std::abs(P1D2) < FLT_EPSILON)
        return false; // 平行

    auto m = P1D / P1D2;
    crossPoint = p1 - m * P1P2; // 注意:此处是 负号- 同时求m时无绝对值符号

    return true;
}
template <typename PointT>
void transformPointCloud(const std::vector<PointT>& cloudin, std::vector<PointT>& cloudout, const Eigen::Matrix4f& T)
{
    // 为输出点云分配空间
    int num = cloudin.size();
    cloudout.resize(num);

    Eigen::Matrix3f rotate = T.topLeftCorner<3, 3>();
    Eigen::Vector3f translate = T.col(3).head<3>();
    // 进行点云变换
    for (int i = 0; i < num; ++i)
    {

        Eigen::Vector3f transformedPoint = translate + rotate * cloudin[i].template head<3>(); // 矩阵乘法
        cloudout[i] = cloudin[i];                                                              // 赋值其他信息
        cloudout[i].template head<3>() = transformedPoint;
    }
    return;
}

void getPointSetByPlane(const std::vector<cv::Point2f>& imagePoints, const Eigen::Matrix3f& cameraIntrinsic, const Eigen::Vector4f& plane, std::vector<Eigen::Vector3f>& crossPoints) {
    int num = imagePoints.size();
    crossPoints.resize(num);
    float fx = cameraIntrinsic(0, 0);
    float fy = cameraIntrinsic(1, 1);
    float cx = cameraIntrinsic(0, 2);
    float cy = cameraIntrinsic(1, 2);
    for (int i = 0; i < num; i++) {
        cv::Point2f pt2d = imagePoints[i];
        Eigen::Vector3f pt3d;
        pt3d(0) = (pt2d.x - cx) / fx;
        pt3d(1) = (pt2d.y - cy) / fy;
        pt3d(2) = 1;
        Eigen::Vector3f line_origin{ 0, 0, 0 };
        Eigen::Vector3f ptCross;

        if (getLinePlaneCrossPt(line_origin, pt3d, plane, ptCross)) {
            crossPoints[i] = ptCross;
        }
    }
}
void getUndistoredPixels(const std::vector<cv::Point2f>& input, std::vector<cv::Point2f>& output, cv::Mat& cameraMatrix, cv::Mat& distCoeffs) {
     cv::fisheye::undistortPoints(input, output, cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix); // 矫正角点
}

void restorePixels(const std::vector<cv::Point2f>& input, std::vector<cv::Point2f>& output, cv::Mat& cameraMatrix) {// 假设 fx, fy, cx, cy 是相机内参矩阵中的值
    float fx = cameraMatrix.at<float>(0, 0);  // 焦距 fx
    float fy = cameraMatrix.at<float>(1, 1);  // 焦距 fy
    float cx = cameraMatrix.at<float>(0, 2);  // 主点 cx
    float cy = cameraMatrix.at<float>(1, 2);  // 主点 cy
    std::vector<cv::Point2f> output_pixel;
    for (const auto& pt : input) {
        cv::Point2f pixel_point;
        pixel_point.x = pt.x * fx + cx;
        pixel_point.y = pt.y * fy + cy;
        output_pixel.push_back(pixel_point);
    }
    output = output_pixel;
    // 输出像素坐标结果
    for (const auto& pt : output) {
        std::cout << "Pixel Point: " << pt << std::endl;
    }
}


int main() {
    m_camera2robot << 3.39371385e-03, -3.47959429e-01, 9.37503338e-01, 4.27294761e-01,
        -9.99993742e-01, -1.97809911e-03, 2.88561685e-03, 4.05987427e-02,
        8.50516604e-04, -9.37507451e-01, -3.47963899e-01, 2.34021828e-01,
        0., 0., 0., 1.;

    cameraIntrinsic << 1.83869247e+02, 0., 1.58713028e+02, 0., 1.83787170e+02,
        1.33699142e+02, 0., 0., 1.;

    distCoeffs = (cv::Mat_<double>(4, 1) << -1.09969601e-01, 2.19760053e-02, -2.66296137e-02,
        1.13827791e-02);
    std::cout << m_camera2robot << std::endl;
    Eigen::Vector4f m_planeInRGB; 
    m_planeInRGB(3) = -1000; // 先默认机器平行地面
    Eigen::Vector4f planen0{ 0, 0, 1, 0 };
    tranformatePlane(planen0, m_camera2robot.inverse(), m_planeInRGB);
    std::cout << m_planeInRGB << std::endl;
    std::vector<cv::Point2f> imagePoints;
    cv::Point2f Point = { 160,70 };
    imagePoints.push_back(Point);
    cv::Mat cameraMatrix(3, 3, CV_32F, cameraIntrinsic.data());
    std::cout << cameraMatrix << std::endl;
    getUndistoredPixels(imagePoints, imagePoints, cameraMatrix,distCoeffs);
    restorePixels(imagePoints, imagePoints, cameraMatrix);
    std::vector<Eigen::Vector3f> Pts3d;
    getPointSetByPlane(imagePoints, cameraIntrinsic, m_planeInRGB, Pts3d);
    transformPointCloud(Pts3d, Pts3d, m_camera2robot);

    std::cout <<"Pts3d:"<< Pts3d[0] << std::endl;
}