估计姿态

最新推荐文章于 2024-06-27 23:34:37 发布

deepeed121

最新推荐文章于 2024-06-27 23:34:37 发布

阅读量3k

点赞数 2

本文链接：https://blog.csdn.net/hk121/article/details/100062736

版权

什么是姿态估计？

在计算机视觉中目标位置指的是它相对于摄像头的方向和坐标。你可以改变这个位置通过改变物体位置（相对于摄像头）或者改变摄像头（相对于物体）的位置来实现。

本教程中描述的姿态估计问题通常在计算机视觉术语中称为透视N点问题或PNP。我们将在下面的章节中更详细地看到，在这个问题中，我们的目标是找到一个校准相机时的物体姿态，我们知道物体上n个三维点的位置和图像中相应的二维投影。

如何用数学表示相机的运动？

一个三维刚性物体相对于相机只有两种运动。

1.平移：将相机从当前的三维位置（x，y，z）移动到新的三维位置（x‘，y’，z‘），称为平移。正如你所看到的，平移有3个自由度——你可以在X、Y或Z方向移动。平移由矢量 t 表示，该矢量 t 等于（x’-x，y’-y，z’-z）。

2. 旋转：您可以围绕X，Y和Z轴旋转摄像机。因此，旋转也具有三个自由度。有许多表示旋转的方法。您可以使用欧拉角（pitch/roll/yaw滚动，俯仰和偏航），3乘以3的旋转矩阵或旋转方向（即轴）和角度来表示它。

所以，估计一个3D坐标意味着找到6个参数---3个平移参数和3个旋转参数。

姿态估计需要什么？

要计算图像中对象的3D姿势，您需要以下信息：

1. 几个点的2D坐标：您需要图像中几个点的2D（x，y）位置。对于人脸，您可以选择眼角，鼻尖，嘴角等。Dlib的面部地标探测器为我们提供了许多可供选择的点。在本教程中，我们将使用鼻尖，下巴，左眼的左角，右眼的右角，嘴的左角和嘴的右角。

2. 相同点的3D位置：您还需要2D特征点的3D位置。您可能认为需要照片中人物的3D模型才能获得3D位置。理想情况下是的，但在实践中，你不需要。通用3D模型就足够了。你从哪里获得头部的3D模型？嗯，你真的不需要一个完整的3D模型。您只需要在某个任意参考系中的几个点的3D位置。在本教程中，我们将使用以下3D点。

1鼻尖：（0.0,0.0,0.0）

2.下巴：（0.0，-330.0，-65.0）

3.左眼左角：（ - 225.0f，170.0f，-135.0）

4.右眼右角：（225.0,170.0，-135.0）

5.左嘴角：（ - 150.0，-150.0，-125.0）

6.右嘴角：（150.0，-150.0，-125.0）

注意，以上几点是在一些任意参考系/坐标系中。这称为世界坐标（在OpenCV文档中为a.k.a模型坐标）。

3.相机的内在参数。如前所述，在这个问题中，假设相机被校准。换句话说，您需要知道相机的焦距，图像中的光学中心和径向失真参数。所以你需要校准你的相机。当然，对于我们中间一些懒惰的家伙和女孩来说，这工作量太大。我可以提供计算的捷径吗？当然，我可以！我们可以估计一个大致精确的3D模型。我们可以通过图像的中心来近似光学中心，以图像像素的宽度近似焦距，并假设不存在径向畸变。great！不费吹灰之力！

姿态估计算法原理？

有几种姿态估计算法。第一个已知的算法可以追溯到1841年。这些算法的细节已经超过这篇文章不展开，这里只阐述他的大致思想：

这里有三个坐标系。上面显示的各种面部特征的3D坐标是建立在世界坐标系中。如果我们知道旋转和平移（即姿势），我们可以将世界坐标中的3D点变换为相机坐标中的3D点。而相机坐标中的3D点可以使用相机的固有参数（焦距，光学中心等）投影到图像平面（即图像坐标系）上。

我们来看看图像形成的方程，以了解上述坐标系的转换原理。在上图中，o是相机的中心，图中所示的平面是图像平面。我们感兴趣的是找出“什么样的方程可以将3D点P映射在图像平面上的p点”。

假设我们知道世界坐标中3D点P的位置（U，V，W）。假设我们知道旋转矩阵R（3x3矩阵）和平移t（3x1向量），他们都建立在相对于相机坐标系的世界坐标系中，我们可以使用以下公式计算摄像机坐标系中点P的位置（X，Y，Z）。

$\begin{align*} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} &= \mathbf{R} \begin{bmatrix}U \\ V \\ W \end{bmatrix} + \mathbf{t} \\ \Rightarrow \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} &= \left [ \mathbf{R} \enspace | \enspace \mathbf{t} \right ] \begin{bmatrix}U \\ V \\ W \\ 1 \end{bmatrix} \end{align*}$ （1）

以扩展形式，上述方程式（1）如下所示：

$\begin{align*} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} r_{00} & r_{01} & r_{02} & t_{x}\\ r_{10} & r_{11} & r_{12} & t_{y}\\ r_{20} & r_{21} & r_{22} & t_{z}\\ \end{bmatrix} \begin{bmatrix}U \\ V \\ W \\ 1 \end{bmatrix} \end{align*}$ （2）

如果你已经学习了线性代数，你会认识到，如果我们知道足够数量的点对应（即（X，Y，Z）和（U，V，W）），上面是一个线性方程组。 $r_{ij}$ 和 (t_x, t_y, t_z) 是未知数，而您可以轻松地解出未知数。

正如你将在下一节中看到的，我们知道（X，Y，Z）只是一个未知的规模，所以我们没有一个简单的线性系统。

直接的线性变换

我们确实知道3D模型上的许多点（即（U，V，W）），但是我们不知道（X，Y，Z）。我们只知道2D点的位置（即（x，y））。在没有径向变形的情况下，图像坐标中点p的坐标（x，y）由下式给出：

$\begin{align*} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} &= s \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \end{align*}$ （3）

其中， f_x 和 f_y 是x和y方向上的焦距， (c_x, c_y) 是光学中心。当涉及径向畸变时，事情变得更复杂，为了简单起见，我将不考虑径向畸变。

在方程式中的S呢？这是一个未知的比例因子。它存在于等式中，因为在任何图像中我们不知道图像的深度。如果将3D中的任何点P连接到相机的中心o，则线段Po与图像平面相交的点p是P的图像。注意，Po线段上的所有点和产生的图像都和P产生的图像相同。换句话说，使用上述等式，您只能获得（X，Y，Z）的某一个倍数s。

现在这个干扰了方程式（2），因为它不再是我们知道如何解决的好的线性方程。我们的方程看起来更像：

$\begin{align*} s \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} r_{00} & r_{01} & r_{02} & t_{x}\\ r_{10} & r_{11} & r_{12} & t_{y}\\ r_{20} & r_{21} & r_{22} & t_{z}\\ \end{bmatrix} \begin{bmatrix}U \\ V \\ W \\ 1 \end{bmatrix} \end{align*}$ （4）

联合（2）（3）可得：

（5)

幸运的是，可以使用一些代数魔法解决上述形式的方程(5)，此方法称为直接线性变换（DirectLinear Transform，DLT）的方法，只要您发现方程几乎是线性但是有一个未知比例的问题，您可以使用DLT。

上式(5)的求解可用DLT(Direct Linear Transform)算法结合最小二乘进行迭代求解，最小二乘的目标函数可为:

(6)

带^的变量为预测值，其余为测量值。
可是相机也很无奈，她不完美，总有点瑕疵，比如径向和切向畸变，那关系就要稍微复杂一些,此处不考虑。自此，上式（5）可以根据Levenberg-Marquardt 优化计算R和T。

Levenberg-Marquardt 优化

上述DLT解决方案不是很准确，原因如下。首先，旋转R具有三个自由度，但在DLT解决方案中使用的矩阵表示有9个数字。DLT解决方案中没有任何内容迫使估计的3×3矩阵成为旋转矩阵。更重要的是，DLT解决方案不会使正确的目标函数最小化。理想情况下，我们希望最大限度地减少以下描述的重投影误差(reprojection error)。（https://www.cnblogs.com/Jessica-jie/p/7242179.html）

如等式（2）和（3）所示，如果我们知道正确的姿势（R和t），我们可以通过将3D点投影到图像上来预测图像上3D面部点的2D位置。换句话说，如果我们知道R和t，我们可以在图像中找到每个3D点P对应的点p。

我们也知道2D面部特征点（使用Dlib或手动点击）。我们可以看看投影3D点和2D面部特征之间的距离。当估计的姿势是完美的，投影到图像平面上的3D点将几乎完美地与2D面部特征相匹配。当姿态估计不正确时，我们可以计算重投影误差量度——投影3D点与2D面部特征点之间的平方距离之和。

如前所述，可以使用DLT解决方案找到姿态的近似估计（R和t）。改善DLT解决方案的一个天真的方法是轻轻随意地改变姿势（R和t），并检查重新投射错误是否减少。如果是这样，我们可以接受新的姿势估计。我们可以一次又一次地保持扰乱R和t来找到更好的估计。虽然这个程序会奏效，但是会很慢。现在有基本的方法迭代地改变R和t的值，以使重新投射错误减少。一种这样的方法称为Levenberg-Marquardt优化。查看维基百科上的更多细节。

OpenCV solvePnP

在OpenCV中，函数solvePnP和solvePnPRansac可用于估计姿态。

solvePnP实现了几种用于姿态估计的算法，可以使用参数标志来选择。默认情况下，它使用标志SOLVEPNP_ITERATIVE，它本质上是DLT解决方案加上Levenberg-Marquardt优化。SOLVEPNP_P3P仅使用3点来计算姿势，只有在使用solvePnPRansac时才使用它。

在OpenCV 3中，引入了两种新的方法——SOLVEPNP_DLS和SOLVEPNP_UPNP。关于SOLVEPNP_UPNP的有趣之处在于它也试图估计摄像机的内部参数。

C++: bool solvePnP(InputArrayobjectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArraydistCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false,int flags=SOLVEPNP_ITERATIVE )

Python: cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]) → retval, rvec, tvec

Parameters:

objectPoints - 世界坐标空间中的对象点数组。我通常通过N个3D点的向量。您还可以传递大小为Nx3（或3xN）单通道矩阵，或Nx1（或1xN）3通道矩阵的Mat。我强烈推荐使用矢量。

imagePoints - 对应图像点的数组。你应该传递一个N个 2D点的向量。但您也可以通过2xN（或Nx2）1通道或1xN（或Nx1）2通道垫，其中N是点数。

cameraMatrix - 输入相机矩阵 $A = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$ 。注意，在某些情况下 f_x ， f_y ，可以通过像素的图像宽度来近似，并且 c_x 和 c_y 可以是图像中心的坐标。

distCoeffs - 4，5，8或12个元素的失真系数（ k_1 ， k_2 ， p_1 ， p_2 [ k_3 [ k_4 ， k_5 ， k_6 ]，[ s_1 ， s_2 ， s_3 ， s_4 ]]的输入向量。如果向量为NULL/empty，则假定零失真系数。除非您正在使用像变形巨大的Go-Pro像相机，否则我们可以将其设置为NULL。如果您正在使用高失真镜头，建议您进行全面的相机校准。

rvec - 输出旋转矢量。

tvec - 输出平移向量。

useExtrinsicGuess - 用于SOLVEPNP_ITERATIVE的参数。如果为真（1），则函数使用提供的rvec和tvec值作为旋转和平移向量的初始近似值，并进一步优化它们。

Flags - 解决 PnP 问题的方法：

SOLVEPNP_ITERATIVE迭代法基于Levenberg-Marquardt优化。在这种情况下，该功能可以找到这样一种姿态，使重投影误差最小化，即观察到的投影图像点与投影（使用projectPoints（））对象点之间的距离之间的平方和。

SOLVEPNP_P3P方法是基于X.S.的论文。.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”. “三点问题的完整解决方案分类”。在这种情况下，该功能只需要四个对象和图像点。

SOLVEPNP_EPNP方法由F.Moreno-Noguer，V.Lepetit和P.Fua在论文“EPnP：Efficient Perspective-n-Point Camera Pose Estimation”中引入。

以下标志仅适用于OpenCV 3

SOLVEPNP_DLS方法基于Joel A. Hesch和Stergios I. Roumeliotis的论文。Joel A. Hesch and Stergios I. Roumeliotis. “A Direct Least-Squares (DLS) Method for PnP”. “PnP的直接最小二乘法（DLS）方法”。

SOLVEPNP_UPNP方法基于A.Penate-Sanchez，J.Andrade-Cetto，M.Moreno-Noguer的论文。 “用于强大的相机姿态和焦距估计的穷尽线性化”。在这种情况下，假设两者都具有相同的值，函数估计参数f_x和f_y。然后用估计的焦距更新cameraMatrix。

OpenCV solvePnPRansac

solvePnPRansac与solvePnP非常相似，只是它使用随机样本一致性（RANSAC）来鲁棒估计姿势。

当您怀疑几个数据点非常嘈杂时，使用RANSAC非常有用。例如，考虑将线拟合到2D点的问题。使用线性最小二乘法可以解决这个问题，其中从拟合线的所有点的距离最小化。现在考虑一个非常糟糕的数据点。这一个数据点可以控制最小二乘解决方案，我们对该线的估计将是非常错误的。在RANSAC中，通过随机选择最小点数来估计参数。在线拟合问题中，我们从所有数据中随机选择两个点，并找到通过它们的线。距离线路足够近的其他数据点称为内联。通过随机选择两个点来获得线的几个估计，并且选择具有最大数目的线内值的线作为正确估计。

solvePnPRansac的使用如下所示，并解释了对于solpnPRansac特定的参数。

C++:void solvePnPRansac(InputArrayobjectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArraydistCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false,int iterationsCount=100, float reprojectionError=8.0, int minInliersCount=100,OutputArray inliers=noArray(), int flags=ITERATIVE )

iterationsCount - 选择最小点数和估计参数的次数。

reprojectionError - 如前所述，在RANSAC中，预测足够近的点被称为“内在”。该参数值是观测值和计算点投影之间的最大允许距离，以将其视为一个惰性。

minInliersCount - 内联数。如果在某个阶段的算法比minInliersCount发现更多的内核，它会完成。

inliers - 包含objectPoints和imagePoints中的内联索引的输出向量。

OpenCV POSIT

OpenCV用于姿态估计算法称为POSIT。它仍然存在于C的API（cvPosit）中，但不是C ++API的一部分。 POSIT假设一个缩放的正交相机模型，因此您不需要提供焦距估计。此功能现在已经过时了，我建议您使用solvePnp中实现的一种算法。

OpenCV姿势估计代码：C ++ / Python

在本节中，我在C ++和Python中共享了一个示例代码，用于单个图像中的头部姿态估计。您可以在这里下载图片headPose.jpg。

面部特征点的位置是硬编码（设置好的）的，如果要使用自己的图像，则需要更改矢量image_points（特征点，上面说的下巴、眼睛、鼻尖等）。

C++

#include <opencv2/opencv.hpp>

using namespace std;

using namespace cv;

int main(int argc, char **argv)

{

// Read input image

cv::Mat im = cv::imread("headPose.jpg");

// 2D image points. If you change the image, you need to change vector

std::vector<cv::Point2d> image_points;

image_points.push_back( cv::Point2d(359, 391) ); // Nose tip

image_points.push_back( cv::Point2d(399, 561) ); // Chin

image_points.push_back( cv::Point2d(337, 297) ); // Left eye left corner

image_points.push_back( cv::Point2d(513, 301) ); // Right eye right corner

image_points.push_back( cv::Point2d(345, 465) ); // Left Mouth corner

image_points.push_back( cv::Point2d(453, 469) ); // Right mouth corner

// 3D model points.

std::vector<cv::Point3d> model_points;

model_points.push_back(cv::Point3d(0.0f, 0.0f, 0.0f)); // Nose tip

model_points.push_back(cv::Point3d(0.0f, -330.0f, -65.0f)); // Chin

model_points.push_back(cv::Point3d(-225.0f, 170.0f, -135.0f)); // Left eye left corner

model_points.push_back(cv::Point3d(225.0f, 170.0f, -135.0f)); // Right eye right corner

model_points.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f)); // Left Mouth corner

model_points.push_back(cv::Point3d(150.0f, -150.0f, -125.0f)); // Right mouth corner

// Camera internals

double focal_length = im.cols; // Approximate focal length.

Point2d center = cv::Point2d(im.cols/2,im.rows/2);

cv::Mat camera_matrix = (cv::Mat_<double>(3,3) << focal_length, 0, center.x, 0 , focal_length, center.y, 0, 0, 1);

cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type); // Assuming no lens distortion

cout << "Camera Matrix " << endl << camera_matrix << endl ;

// Output rotation and translation

cv::Mat rotation_vector; // Rotation in axis-angle form

cv::Mat translation_vector;

// Solve for pose

cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);

// Project a 3D point (0, 0, 1000.0) onto the image plane.

// We use this to draw a line sticking out of the nose

vector<Point3d> nose_end_point3D;

vector<Point2d> nose_end_point2D;

nose_end_point3D.push_back(Point3d(0,0,1000.0));

projectPoints(nose_end_point3D, rotation_vector, translation_vector, camera_matrix, dist_coeffs, nose_end_point2D);

for(int i=0; i < image_points.size(); i++)

{

circle(im, image_points[i], 3, Scalar(0,0,255), -1);

}

cv::line(im,image_points[0], nose_end_point2D[0], cv::Scalar(255,0,0), 2);

cout << "Rotation Vector " << endl << rotation_vector << endl;

cout << "Translation Vector" << endl << translation_vector << endl;

cout << nose_end_point2D << endl;

// Display image.

cv::imshow("Output", im);

cv::waitKey(0);

}

Python

#!/usr/bin/env python

import cv2

import numpy as np

# Read Image

im = cv2.imread("headPose.jpg");

size = im.shape

#2D image points. If you change the image, you need to change vector

image_points = np.array([

(359, 391), # Nose tip

(399, 561), # Chin

(337, 297), # Left eye left corner

(513, 301), # Right eye right corne

(345, 465), # Left Mouth corner

(453, 469) # Right mouth corner

], dtype="double")

# 3D model points.

model_points = np.array([

(0.0, 0.0, 0.0), # Nose tip

(0.0, -330.0, -65.0), # Chin

(-225.0, 170.0, -135.0), # Left eye left corner

(225.0, 170.0, -135.0), # Right eye right corne

(-150.0, -150.0, -125.0), # Left Mouth corner

(150.0, -150.0, -125.0) # Right mouth corner

])

# Camera internals

focal_length = size[1]

center = (size[1]/2, size[0]/2)

camera_matrix = np.array(

[[focal_length, 0, center[0]],

[0, focal_length, center[1]],

[0, 0, 1]], dtype = "double"

)

print "Camera Matrix :\n {0}".format(camera_matrix)

dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion

(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.CV_ITERATIVE)

print "Rotation Vector:\n {0}".format(rotation_vector)

print "Translation Vector:\n {0}".format(translation_vector)

# Project a 3D point (0, 0, 1000.0) onto the image plane.

# We use this to draw a line sticking out of the nose

(nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs)

for p in image_points:

cv2.circle(im, (int(p[0]), int(p[1])), 3, (0,0,255), -1)

p1 = ( int(image_points[0][0]), int(image_points[0][1]))

p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))

cv2.line(im, p1, p2, (255,0,0), 2)

# Display image

cv2.imshow("Output", im)

cv2.waitKey(0)

使用Dlib实时姿态估计

这篇文章中包含的视频是使用我的dlib分支，可以免费为这个博客的订阅者使用。如果您已经订阅，请查看欢迎电子邮件链接到我的dlib fork，并查看此文件。

dlib/examples/webcam_head_pose.cpp

Dlib中获取的各个点

std::vectorget_2d_image_points(full_object_detection &d)
{
std::vector image_points;
image_points.push_back( cv::Point2d(d.part(30).x(), d.part(30).y() ) ); // Nose tip
image_points.push_back( cv::Point2d(d.part(8).x(), d.part(8).y() ) ); // Chin
image_points.push_back( cv::Point2d(d.part(36).x(), d.part(36).y() ) ); // Left eye left corner
image_points.push_back( cv::Point2d(d.part(45).x(), d.part(45).y() ) ); // Right eye right corner
image_points.push_back( cv::Point2d(d.part(48).x(), d.part(48).y() ) ); // Left Mouth corner
image_points.push_back( cv::Point2d(d.part(54).x(), d.part(54).y() ) ); // Right mouth corner
return image_points;
 
}

参考：