pnp标定验证

pnp标定lidar-相机后，验证得到的旋转平移矩阵是否满足要求：
【标定程序参考文章末尾的链接】
获取同一时刻的图像与点云，提取点云中某一特征（如一个箱）的所有点云坐标，并保存到lidar_xyz.txt

输入图像：image.png
输入点云：lidar_xyz.txt
可以得到配准后的图像…

 //**************************************************************************
//             标定误差验证
//***************************************************************************
#include <iostream> 
#include <vector>         
#include <algorithm>     
#include <fstream>        
#include <iomanip>       
#include "opencv.hpp"  
#include "opencv2/core/core.hpp"  
#include "opencv2/highgui/highgui.hpp"   
#include <iostream>
using namespace std;
using namespace cv;


void readline(const char* strPath, int &line)
{
	ifstream is(strPath);
	char c;
	while (is.get(c))
	{
		if (c == '\n')
			line++;
	}
	is.close();
	cout << "line=" << line << endl;

}

int main()
{
    std::cout << "Hello World!\n";

	cv::Mat image = cv::imread("image.png");
	imshow("原始图像", image);

	int row = image.rows; //行
	int col = image.cols;
	cout << "row=" << row << endl;
	cout << "col=" << col << endl;
	Mat M= (Mat_<double>(3, 3) << 1120.793247366556, 0, 404.6240125739656, 0, 1126.958397947255, 248.9162767468943, 0, 0, 1);
	cout << "M=" << M << endl;
	Mat RT = (Mat_<double>(3, 4) << -0.0209971, - 0.999327, 0.0300679 , 0.01784, - 0.0238557 ,- 0.0295651, - 0.999278, - 0.0963614, 0.999495 ,- 0.0216992, - 0.0232189, 0.00614244 );
	cout << "RT=" << RT << endl;

	vector<double> V;
	vector<double>::iterator it;
	
	const char * filename = "lidar_xyz.txt";
	ifstream file(filename);

	double d;
	while (file >> d)
	{
		V.push_back(d);//将数据压入堆栈
	}
	file.close();
	int line = 1;
	readline(filename, line);
	double aa[100][3];
	if (line > 100) { line = 100; }
	for (it = V.begin(); it != V.end(); )
	{
		for (int i = 0; i < line; i++)//定义行循环
		{
			for (int j = 0; j < 3; j++)//定义列循环
			{
				aa[i][j] = *it;
				it++;
			}
			cout << "i=" << i<< endl;
		}
		break;
	}
	for (int i = 0; i < line; i++)
	//for (int i = 0; i < 1; i++)
	{ 
		Mat point = (Mat_<double>(4, 1) << aa[i][0], aa[i][1], aa[i][2], 1);
		cout << "point=" << aa[i][0]<<"  "<< aa[i][1]<<"  "<< aa[i][2] << endl;
		Mat uv(3, 1, CV_64F);
		uv = M * RT*point;
		if (uv.at<double>(2) == 0) 
		{
			cout << "uv.at<double>(2)=0" << endl;
			break;
		}
		double u = uv.at<double>(0) / uv.at<double>(2);
		double v = uv.at<double>(1) / uv.at<double>(2);
		int px = int(u + 0.5);
		int py = int(v + 0.5);
		cout << "(u,v)=" << px << "  "<< py << endl;
		//注意：image.at<Vec3b>(row,rol)  但是像素坐标为(x,y)，即(u,v),即(rol,row)
		if (0 <= px && px < col && 0 <= py && py < row)
		{
			image.at<Vec3b>(py, px)[0] = 0;
			image.at<Vec3b>(py, px)[1] = 0;
			image.at<Vec3b>(py, px)[2] = 255;
		}
	}
	imshow("配准图像", image);
	cv::waitKey();
}

PNP标定源码：https://blog.csdn.net/xx970829/article/details/121086958