三维重建(10)之由世界坐标反推图像坐标
世界坐标系、相机坐标系、像平面坐标系、图像坐标系 -> 可参考:三维重建(2)之相机成像+单应性变换+相机标定+立体校正
——————
简介:如果已知世界坐标中的的一组期望坐标点,想要得到其在相机参数(内参与外参)下的对应图像的坐标,那么需要从相机标定过程中推理出3D-2D
的映射关系
1.最小二乘求解单应性矩阵
单应性矩阵指的是由三维世界坐标到二维相机坐标的映射关系,推理过程如下图所示;
2.由世界坐标推理图像坐标
直接点,原理如下图所示:
3.Opencv函数实现
3.实现过程
- 最小二成法求解单应性矩阵的实现在此就不列了,因为还没有着手去实现,等后边如果用到的话再来补充一下
- 下边给出一下由三维世界坐标计算得到二维坐标的中间过程实现:
将三维坐标点转为矩阵形式
cv::Mat point2mat(const std::vector<Point3f>points)
{
int nums = points.size();
cv::Mat res(cv::Size(nums, 4), CV_64FC1,cv::Scalar::all(0));
for (int i = 0; i < nums; ++i)
{
res.at<double>(0, i) = points[i].x;
res.at<double>(1, i) = points[i].y;
res.at<double>(2, i) = points[i].z;
res.at<double>(3, i) = 1.;
}
return res;
}
由坐标矩阵转为二位点集格式
std::vector<cv::Point2f> Calibration::mat2point2f(const cv::Mat& mat)
{
CV_Assert(!mat.empty());
CV_Assert(mat.type() == CV_64FC1);
std::vector<cv::Point2f>points(mat.cols);
for (int i = 0; i < mat.cols; ++i)
{
cv::Point2f p = cv::Point2f(mat.at<double>(0, i), mat.at<double>(1, i));
points[i] = p;
}
return points;
}
反推相机坐标
/*
*@para ps:一组世界坐标
*@para return:双目相机对应图像的图像坐标
*/
std::vector<vector<cv::Point2f>>get_rect3(const std::vector<cv::Point3f>ps)
{
std::vector<vector<cv::Point2f>>points(2);
cv::Mat m1, m2, m3, m4, m5;
//读取相机内参
m1 = cal.point2mat(ps);
cv::Mat L_in = cameraMatrix1; //相机内参
cv::Mat R_in = cameraMatrix2;
cv::Mat Rc = camera.R; //左右相机的旋转和平移矩阵
cv::Mat tc = camera.T;
//1.转左相机坐标系,求解做相机坐标
m2 = trans; //世界坐标系到相机坐标系的变换矩阵
m3 = m2 * m1; //转像机坐标系
m4 = m3(Rect(0, 0, m3.cols, m3.rows - 1)).clone();
m5 = L_in * m4;
m5.row(0) /= m5.row(2);
m5.row(1) /= m5.row(2);
points[0] = cal.mat2point2f(m5);
//2.转右相机坐标系
cv::Mat m6, m7, m8;
hconcat(Rc, tc, m6);
m7 = m6 * m3;
m8 = R_in * m7;
m8.row(0) /= m8.row(2);
m8.row(1) /= m8.row(2);
points[1] = cal.mat2point2f(m8);
int row = 1536;
int col = 2048;
for (int i = 0; i < points.size(); ++i)
{
for (int j = 0; j< points[i].size(); ++j)
{
//判断是否有越界的点,如果有,将其规范在图像范围内
points[i][j].x = points[i][j].x < 0 ? 0 : points[i][j].x;
points[i][j].x = points[i][j].x > col ? col : points[i][j].x;
points[i][j].y = points[i][j].y < 0 ? 0 : points[i][j].y;
points[i][j].y = points[i][j].y > row ? row : points[i][j].y;
}
}
return points;
}
打印
void print_p(std::vector<cv::Point2f>& points)
{
std::cout << "cout:" << std::endl;
for (int i = 0; i < points.size(); ++i)
{
std::cout << points[i] << std::endl;
}
}