双目相机标定结果应用——测三维坐标

// test_point.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 // #include "pch.h" #include <iostream> using namespace std; void Stereo1(double lx, double ly, double rx, double ry) //lx左相机图像x坐标 rx右相机图片x坐标 { double x = 0; double y = 0; double z = 0; //左相机参数 double m1[9] = { 1168.280526, 0 ,0, 0 ,1168.33285, 0, 634.7887136 ,344.7889216 ,1 }; //右相机参数 double m2[9] = { 1173.829963, 0 ,0, 0 ,1174.084943 ,0, 615.5921798 ,342.8361905 ,1 }; //旋转矩阵 double R[9] = { 0.978399143 ,- 0.007453385, 0.20659033, 0.009604345, 0.999909593, - 0.00941076, - 0.206501511, 0.011191644, 0.978382274, }; //平移矩阵 double T[3] = { -594.6585459,2.181549611, 65.35083548 }; double numerator = (m1[0] + m1[4]) / 2 * ((m2[0] + m2[4]) / 2 * T[0] - (rx - m2[6])*T[2]); double denominator1 = (rx - m2[6])*(R[6] * (lx - m1[6]) + R[7] * (ly - m1[7]) + R[8] * (m1[0] + m1[4]) / 2); double denominator2 = (m2[0] + m2[4]) / 2 * (R[0] * (lx - m1[6]) + R[1] * (ly - m1[7]) + R[2] * (m1[0] + m1[4]) / 2); z = numerator / (denominator1 - denominator2); x = z * (lx - m1[6]) / ((m1[0] + m1[4]) / 2); y = z * (ly - m1[7]) / ((m1[0] + m1[4]) / 2); printf("该点世界坐标为 %f\t %f\t %f\n", x, y, z); //ofs << x <<" "<< y <<" "<< z << endl; } int main() { double k1_l = -0.0340; double k2_l = -0.0900; double k1_r = -0.0354; double k2_r = -0.0957; float x1, y1, x2, y2; cout << "请输入左相机图片的x像素坐标： "; cin >> x1 ; cout << "请输入左相机图片的y像素坐标： "; cin >> y1; double r1, x_1_c, y_1_c; r1 = pow((pow((x1 - 6.375220530377004e+02),2) + pow((y1 - 3.486802530008395e+02), 2)),0.5); x_1_c = x1 * (1 + k1_l * pow(r1 , 2) + k2_l *pow( r1 , 4)); y_1_c = y1 * (1 + k1_l *pow( r1 , 2) + k2_l * pow(r1 , 4)); cout << "请输入右相机图片的x像素坐标： "; cin >> x2; cout << "请输入右相机图片的y像素坐标： "; cin >> y2; double r2, x_2_c, y_2_c; r2 = pow((pow((x1 - 6.202963972580242e+02),2) + pow((y1 - 3.473683137713877e+02),2)),0.5); x_2_c = x2 * (1 + k1_r * pow(r2,2) + k2_r * pow(r2,4)); y_2_c = y2 * (1 + k1_r * pow(r2,2)+ k2_r *pow(r2,4)); /*Stereo1(x_1_c, y_1_c, x_2_c, y_2_c);*/ Stereo1(x1, y1, x2, y2); }