估计姿态

 

什么是姿态估计?

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

本教程中描述的姿态估计问题通常在计算机视觉术语中称为透视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_xf_y是x和y方向上的焦距,(c_x, c_y) 是光学中心。当涉及径向畸变时,事情变得更复杂,为了简单起见,我将不考虑径向畸变。

在方程式中的S呢?这是一个未知的比例因子。它存在于等式中,因为在任何图像中我们不知道图像的深度。如果将3D中的任何点P连接到相机的中心o,则线段Po与图像平面相交的点pP的图像。注意,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)所示,如果我们知道正确的姿势(Rt),我们可以通过将3D点投影到图像上来预测图像上3D面部点的2D位置。换句话说,如果我们知道Rt,我们可以在图像中找到每个3D点P对应的p

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

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

 

OpenCV solvePnP

在OpenCV中,函数solvePnPsolvePnPRansac可用于估计姿态。

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

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

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

Pythoncv2.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_4k_5 ,k_6 ],[ s_1s_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++

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

70

71

#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

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

#!/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;
 
}

 

参考:

https://blog.csdn.net/u010128736/article/details/52850444

https://zhuanlan.zhihu.com/p/51208197

https://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/

https://blog.csdn.net/Fenglin6165/article/details/86467363

  • 2
    点赞
  • 23
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值