使用opencv手写点云投影函数

zengy5

于 2024-05-31 16:39:13 发布

阅读量262

点赞数 4

文章标签： opencv 人工智能计算机视觉 c++ linux 笔记

本文链接：https://blog.csdn.net/weixin_48013375/article/details/139356058

版权

原理

给定内参矩阵 $K\in R^{3\times3}$ ，外参矩阵 $T=[R,t],R\in SO(3), t\in\mathbb{R}^3$ ，以及点云 $P$ ，投影到图像平面中
对于每个点 $p\in P$ ，其相机坐标系中的坐标计算公式为 $p_{2d} = K*[R|t]*\tilde{p}$ ，再进行归一化即可得到像素坐标

代码

#include <opencv/opencv.hpp>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr pcd(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::io::loadPCDFile<pcl::PointXYZ>("pointcloud.pcd", *pcd);
    cv::Mat R = (cv::Mat_<double>(3,3) << 0.0, -1.0, 0.0,
        0.0, 0.0, -1.0, 
        1.0, 0.0, 0.0);
        
    cv::Mat t = (cv::Mat_<double>(3,1) << 0,0,0);

    cv::Mat K = (cv::Mat_<double>(3,3) << 718.8560, 0.0, 607.1928,
        0.0, 718.8560, 185.2157,
        0.0, 0.0, 1.0);
    cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F);
    
    cv::Mat Rt;
    cv::hconcat(R,t,Rt);
    cv::Mat P = K * Rt;
    cv::Mat image = cv::Mat::zeros(height, width, CV_8UC3);

    for(const auto & point : pcd.points)
    {
        cv::Mat pointHomo = (cv::Mat_<double>(4,1) << point.x, point.y, point.z, 1.0);
        cv::Mat pixel_2d = P * pointHomo;
        cv::Point2d imagePoint(pixel_2d.at<double>(0, 0) / pixel_2d.at<double>(2, 0),
                           pixel_2d.at<double>(1, 0) / pixel_2d.at<double>(2, 0));

        if (imagePoint.x >= 0 && imagePoint.x < image.cols && imagePoint.y >= 0 && imagePoint.y < image.rows) 
        {
            cv::circle(image, imagePoint, 3, cv::Scalar(0, 0, 255), -1);
        }

    }

    cv::imshow("Projected Points", image);
    cv::waitKey(0);
	return 0;
}