OpenCV位姿与投影变换

瞻邈

已于 2024-05-23 13:02:04 修改

阅读量1.2k

点赞数

分类专栏： # C++开源库文章标签： opencv 人工智能计算机视觉

于 2020-12-24 20:30:11 首次发布

本文链接：https://blog.csdn.net/xhtchina/article/details/111654381

版权

C++开源库专栏收录该内容

36 篇文章 15 订阅

订阅专栏

1. 旋转矩阵与旋转向量的相互转换

1.1. 罗德里格斯公式

$\boldsymbol{R}=\cos\theta\boldsymbol{I}+\left(1-\cos\theta\right)\boldsymbol{n}\boldsymbol{n}^T+\sin\theta\boldsymbol{n}^\wedge.$

符号^是向量到反对称的转换符。反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角θ，有：

$\begin{aligned}\operatorname{tr}\left(\boldsymbol{R}\right)&=\quad\cos\theta\mathrm{tr}\left(\boldsymbol{I}\right)+\left(1-\cos\theta\right)\mathrm{tr}\left(\boldsymbol{n}\boldsymbol{n}^{T}\right)+\sin\theta\mathrm{tr}(\boldsymbol{n}^{\wedge})\\&=\quad3\cos\theta+(1-\cos\theta)\\&=\quad1+2\cos\theta.\end{aligned}$

因此：

$\theta=\arccos(\frac{\mathrm{tr}(\boldsymbol{R})-1}{2}).$

关于转轴n，由于旋转轴上的向量在旋转后不发生改变，说明

$Rn=n.$

转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

1.2. 转换函数

void Rodrigues(const cv::Mat& src，cv::Mat dst，cv::Mat jacobian=0);

参数说明

src——为输入的旋转向量（3x1或者1x3）或者旋转矩阵（3x3）。该参数向量表示其旋转的角度，用向量长度表示。
dst——为输出的旋转矩阵（3x3）或者旋转向量（3x1或者1x3）。
jacobian——为可选的输出雅可比矩阵（3x9或者9x3），是输入与输出数组的偏导数。

2. 图像变换

2.1. 去畸变

2.1.1. 针孔相机

调用方法

std::vector<cv::Point2f> inputDistortedPoints = ...
std::vector<cv::Point2f> outputUndistortedPoints;
cv::Mat cameraMatrix = ...
cv::Mat distCoeffs = ...
 
cv::undistortPoints(inputDistortedPoints, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);

不要像下面这样调用，输出的点不正确

cv::undistortPoints(inputDistortedPoints, outputUndistortedPoints, cameraMatrix, distCoeffs）

2.1.2. 鱼眼相机

cv::fisheye::undistortPoints(inputDistortedPoints, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);

2.2. 投影

2.2.1. 针孔相机

函数

void cv::projectPoints(InputArray objectPoints,
                       InputArray rvec,
                       InputArray tvec,
                       InputArray cameraMatrix,
                       InputArray distCoeffs,
                       OutputArray imagePoints,
                       OutputArray	jacobian = noArray(),
                       double aspectRatio = 0)

Parameters

objectPoints	Array of object points expressed wrt. the world coordinate frame. A 3xN/Nx3 1-channel or 1xN/Nx1 3-channel (or vector<Point3f> ), where N is the number of points in the view.
rvec	The rotation vector (Rodrigues) that, together with tvec, performs a change of basis from world to camera coordinate system, see calibrateCamera for details.
tvec	The translation vector, see parameter description above.
cameraMatrix	Camera intrinsic matrix
distCoeffs	Input vector of distortion coefficients (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]]) of 4, 5, 8, 12 or 14 elements . If the vector is empty, the zero distortion coefficients are assumed.
imagePoints	Output array of image points, 1xN/Nx1 2-channel, or vector<Point2f> .
jacobian	Optional output 2Nx(10+<numDistCoeffs>) jacobian matrix of derivatives of image points with respect to components of the rotation vector, translation vector, focal lengths, coordinates of the principal point and the distortion coefficients. In the old interface different components of the jacobian are returned via different output parameters.
aspectRatio	Optional "fixed aspect ratio" parameter. If the parameter is not 0, the function assumes that the aspect ratio ( fx/fy) is fixed and correspondingly adjusts the jacobian matrix.

示例

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
 
#include <iostream>
#include <string>
 
using namespace std;
 
vector<cv::Point3f> Generate3DPoints();
 
int main(int argc, char* argv[])
{
    // Read 3D points
    vector<cv::Point3f> objectPoints = Generate3DPoints();
    vector<cv::Point2f> imagePoints;
 
    cv::Mat intrisicMat(3, 3, cv::DataType<float>::type); // Intrisic matrix
    intrisicMat.at<float>(0, 0) = 1.6415318549788924e+003;
    intrisicMat.at<float>(1, 0) = 0;
    intrisicMat.at<float>(2, 0) = 0;
 
    intrisicMat.at<float>(0, 1) = 0;
    intrisicMat.at<float>(1, 1) = 1.7067753507885654e+003;
    intrisicMat.at<float>(2, 1) = 0;
 
    intrisicMat.at<float>(0, 2) = 5.3262822453148601e+002;
    intrisicMat.at<float>(1, 2) = 3.8095355839052968e+002;
    intrisicMat.at<float>(2, 2) = 1;
 
    cv::Mat rVec(3, 1, cv::DataType<float>::type); // Rotation vector
    rVec.at<float>(0) = -3.9277902400761393e-002;
    rVec.at<float>(1) = 3.7803824407602084e-002;
    rVec.at<float>(2) = 2.6445674487856268e-002;
 
    cv::Mat tVec(3, 1, cv::DataType<float>::type); // Translation vector
    tVec.at<float>(0) = 2.1158489381208221e+000;
    tVec.at<float>(1) = -7.6847683212704716e+000;
    tVec.at<float>(2) = 2.6169795190294256e+001;
 
    cv::Mat distCoeffs(5, 1, cv::DataType<float>::type);   // Distortion vector
    distCoeffs.at<float>(0) = -7.9134632415085826e-001;
    distCoeffs.at<float>(1) = 1.5623584435644169e+000;
    distCoeffs.at<float>(2) = -3.3916502741726508e-002;
    distCoeffs.at<float>(3) = -1.3921577146136694e-002;
    distCoeffs.at<float>(4) = 1.1430734623697941e-002;
 
    cout << "Intrisic matrix: " << intrisicMat << endl << endl;
    cout << "Rotation vector: " << rVec << endl << endl;
    cout << "Translation vector: " << tVec << endl << endl;
    cout << "Distortion coef: " << distCoeffs << endl << endl;
 
    std::vector<cv::Point2f> projectedPoints;
    cv::projectPoints(objectPoints, rVec, tVec, intrisicMat, distCoeffs, projectedPoints);
 
    cout << "Press any key to exit.";
    cin.ignore();
    cin.get();
 
    return 0;
}
 
vector<cv::Point3f> Generate3DPoints()
{
    vector<cv::Point3f> points;
    points.push_back(cv::Point3f(.5, .5, -.5));
    points.push_back(cv::Point3f(.5, .5, .5));
    points.push_back(cv::Point3f(-.5, .5, .5));
    points.push_back(cv::Point3f(-.5, .5, -.5));
    points.push_back(cv::Point3f(.5, -.5, -.5));
    points.push_back(cv::Point3f(-.5, -.5, -.5));
    points.push_back(cv::Point3f(-.5, -.5, .5));
 
    for(unsigned int i = 0; i < points.size(); ++i)
    { 
        cout << points[i] << endl << endl;
    }
 
    return points;
}

2.2.2. 鱼眼相机

函数

// 提供了两个重载函数，经验上第一个容易崩溃，建议使用第二个
void cv::fisheye::projectPoints(InputArray objectPoints,
                                OutputArray	imagePoints,
                                InputArray rvec,
                                InputArray tvec,
                                InputArray K,
                                InputArray D,
                                double alpha = 0,
                                OutputArray	jacobian = noArray())

void cv::fisheye::projectPoints(InputArray objectPoints,
                                OutputArray	imagePoints,
                                const Affine3d&	affine,
                                InputArray K,
                                InputArray D,
                                double alpha = 0,
                                OutputArray	jacobian = noArray())

Parameters

objectPoints	Array of object points, 1xN/Nx1 3-channel (or vector<Point3f> ), where N is the number of points in the view.
imagePoints	Output array of image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, or vector<Point2f>.
affine	仿射变换，可以使用位姿矩阵进行初始化
K	Camera intrinsic matrix cameramatrixK.
D	Input vector of distortion coefficients (k1,k2,k3,k4).
alpha	The skew coefficient.
jacobian	Optional output 2Nx15 jacobian matrix of derivatives of image points with respect to components of the focal lengths, coordinates of the principal point, distortion coefficients, rotation vector, translation vector, and the skew. In the old interface different components of the jacobian are returned via different output parameters.