Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Head pose estimation #9

Open
ysh329 opened this issue Nov 11, 2018 · 9 comments
Open

Head pose estimation #9

ysh329 opened this issue Nov 11, 2018 · 9 comments

Comments

@ysh329
Copy link
Contributor

ysh329 commented Nov 11, 2018 •
edited

1. 什么是物体姿态和PNP问题

在计算机视觉中，物体的姿势指的是其相对于相机的相对取向和位置，一般用旋转矩阵、旋转向量、四元数或欧拉角表示（这四个量也可以互相转换）。一般而言，欧拉角可读性更好一些，也可以可视化（见下图，分别对应欧拉角的三个角度），所以常用欧拉角表示。欧拉角包含3个角度：pitch、yaw、roll，这三个角度也称为姿态角。

姿态角/欧拉角动图注解

pitch()：俯仰，将物体绕X轴旋转（localRotationX）
pitch
yaw()：航向，将物体绕Y轴旋转（localRotationY）
yaw
roll()：横滚，将物体绕Z轴旋转（localRotationZ）
roll

确定物体的姿态就是要计算其相对相机的欧拉角，刚提到也可以是旋转矩阵。旋转矩阵可以和欧拉角相互转换，一般而言，得到的欧拉角可以可视化到二维图像上，见下图。

image

姿势估计问题通常称为Perspective-n-Point问题或计算机视觉术语中的PNP。在该问题中，目标是在我们有校准相机时找到物体的姿势或运动（即找到欧拉角或旋转矩阵），还需要知道物体上的n个3D点的位置以及相应的相机3D到图像2D平面的投影。即，我们需要三个东西（后文会说到为什么需要这三样）：

image

  1. 物体（人脸）在相机3D世界中投影到二维平面的坐标。即图像上人脸的关键点，比方我们这里是五个关键点或者六个，那么就要给出这几个点在二维平面上的坐标。因为我们已有模型计算得到的人脸关键点信息（这个也可以用Dlib/OpenCV得到，Dlib提供计算人脸68个关键点的模型与接口），所以这个是已知的。
  2. 物体与2D平面相对应的3D世界坐标系位置。可能你会认为需要照片中人物的3D模型才能获得3D位置。理想情况下是的，但在实践中我们一是没有，二是通用3D模型就足够了。获得完整头部的3D模型，只需要知道在某个任意参考系中的几个点的3D位置。下面我们以鼻子为3D世界坐标系的原点位置（0，0，0）。得到以下人脸3D点（以下几个点是任意参考系/坐标系中，你还可以根据自己情况修改校准，这称为世界坐标，也即OpenCV docs中的模型坐标）：
    • 鼻尖：（0.0,0.0,0.0）
    • 下巴：（0.0，-330.0，-65.0）
    • 左眼左角：（ - 225.0f，170.0f，-135.0）
    • 右眼右角：（225.0,170.0，-135.0）
    • 口的左角：（ - 150.0，-150.0，-125.0）
    • 嘴角：（150.0，-150.0，-125.0）
  3. 物体在3D世界中对应相机中3D坐标系的位置，知道了3D世界中的坐标点通过相机参数可以算出，换句话说，这一步是要知道相机参数（相机焦距、图像中的光学中心、径向失真参数）。因为这不是软件层面的事情，所以我们这里假设相机被校准，使用校准相机的参数。我们通过图像的中心来近似光学中心，以像素的宽度近似焦距，并假设不存在径向畸变。

2. 如何在数学上表示物体相对相机的运动？

在此前，我们先看看怎么表示物体在3D世界坐标中的运动（即如何让3D世界坐标下的物体运动到相机3D世界中去）。3D刚性物体相对于相机的运动，仅具有两种运动：

  • 平移（Translation）：将相机从当前3D位置（X，Y，Z）移动到新的3D位置（X'，Y'，Z'）称为平移，平移具有3个自由度，即可以沿X，Y或Z方向移动。平移由向量t表示（X' - X，Y' - Y，Z' - Z）。
  • 旋转（Rotation）：可以旋转摄像头的X、Y和Z轴，即旋转也具有三个自由度。有许多表示旋转的方法，常见的如欧拉角（包含滚动角，俯仰角和偏航角，对应roll、pitch、和yaw），即一个3x3维度的旋转矩阵或者3x1维度的旋转向量，或者用一个旋转方向（即轴）和角度来定义。

因此，估计3D对象中某一个点的运动，意味着找到该点的平移与旋转的6个参数 （这里，假定旋转用1x3维度的旋转向量表示）：3个用于平移，3个用于旋转。那么我们就描述完了如何将3D世界中的坐标点转换到3D相机世界中去，这是第一个转换。

但需要注意的是，我们需要知道物体3D世界中的点，而且一个假定是面部特征的3d坐标是基于世界坐标。这部分内容就是将物体在3D世界中的点，平移旋转到3D相机世界坐标的方法，但我只知道相机参数，并不知道物体在3D相机世界中的坐标。

3. 姿势估计算法如何工作？

image

别急，你看上图里有三个坐标系（世界3D坐标World Coordinates、相机3D坐标Camera Coordinates、投影平面坐标即图像Image Plane）。前文中有一张图片显示就是面部特征在世界3D坐标中的样子，以及对应2D图像Image Plane上的样子。如果我们知道，从人脸在世界3D坐标中的位置，到人脸在相机3D世界的位置的旋转和平移（即姿势/物体运动）参数，那么就可知道人脸在相机3D坐标中的位置。

仍旧看上图，可以发现相机3D坐标Camera Coordinates与投影平面坐标即图像Image Plane，二者的X和Y是一样的。现在我们知道世界坐标中位置P的3D点为（U，V，W）。如果知道相对于相机坐标的世界坐标的旋转矩阵R（3×3矩阵）和平移b（3×1矢量），就可以使用以下等式计算相机坐标系（X，Y，Z）中点的位置P。

image

也可以写成扩展的形式：

image

如果我们知道足够数量的点对，即（X，Y，Z）和与之对应的（U，V，W），上面就是一个线性方程组，可以求解得到旋转R和平移b的值。但该方法也有缺点（具体见下文方法2），正如前面所说，描述物体运动需要旋转和平移，对应3个方向6个值，这个方法一个3D点的平移参数算出是3个值，但是旋转参数却有9个（假定我们这里不考虑使用3x3维度的旋转矩阵，而是想要使用3x1维度的旋转向量）。

3.1 方法1：直接线性变换（DLT）求解参数

在上面的方程中，我们有很多已知的3D世界坐标点，即（U，V，W），但不知道具体的3D相机坐标（X，Y，Z），但知道3D相机坐标中的X和Y，即2D图像上的关键点。在没有径向畸变（即z点的畸变）的情况下，2D图像坐标中P点即（x，y）有以下公式：

PS：下面公式描述了，将相机坐标（X，Y，Z）转换即投影为平面坐标（x，y，1）的过程，已知平面坐标和相机参数，通过下面公式算出相机坐标（X，Y，Z）：

image

其中，f_x和f_y是在x和y方向上的焦距，并且（c_x，c_y）是光学中心。当涉及径向变形时，事情变得稍微复杂一些，为了简单起见不考虑这一项。

s是一个我们未知的比例因子，表示深度信息。举个例子，若将3D世界坐标中某个点P与3D相机坐标中对应的点P连接起来，即光线，那么这个光线会与图像平面相交得到二维平面上的点P。这个s就是这个过程中变换的深度信息，但我们这里因为未知，统统将所有点s考虑为一样的。

PS：以下公式，将3D世界坐标（U，V，W）转换为3D相机坐标，已知3D世界坐标和上面公式算出的3D相机坐标，因而可以算出3x3的旋转矩阵和3x1的平移向量。

image

基于上面的两个公式，先用第一个公式计算出Z值，再用第二个公式计算出旋转矩阵，依次列出方程组，依次求解得到3D相机坐标的值Z、旋转R和平移T参数。这种方法称为直接线性变换（Direct Linear Transform，DLT）。然后再讲旋转矩阵转换为欧拉角。

下面是补充内容，将3x3维度的旋转矩阵转换为欧拉角。

旋转矩阵转换为欧拉角

我们可以将旋转矩阵转换为欧拉角，从而得到姿态。已知维度为3x3的旋转矩阵：
image
基于旋转矩阵计算欧拉角的公式为：
image
image
image
这里atan2就是arc tangent函数，调用该函数时会做象限的检查。该函数可在C或Matlab代码中找到。.
注：如果角度绕y轴是+ / -90°。在这种情况下，除了一个在角落较低的元素（不是1就是-1），其它所有元素都在在第一列和最后一行上都是0（因为cos(1)=0）。针对该问题，需要修正，即绕x轴旋转180°，并计算绕z轴的角度，可以用这个函数解决：atan2(r_12, -r_22)。
更具体参考：http://www.soi.city.ac.uk/~sbbh653/publications/euler.pdf。

3.2 方法2：Levenberg-Marquardt

实际中，方法1的直接线性变换（Direct Linear Transform，DLT）计算结果不准确，因为：

  1. 旋转R有3个自由度需要的只是3个数字，但DLT方法使用的矩阵有9个数字，且DLT方法中没有任何方法使3x3矩阵的意义是旋转矩阵（这里我有点懵，可能是说，3x3维度的这个矩阵其实际意义很难说带有旋转的意思在里面，因为没有最小化目标函数，这个算出的3x3的矩阵与旋转的参数意义不明确）；
  2. 直接线性变换（DLT）方法不会最小化正确的目标函数，我们希望最小化下面描述中的投影误差。

前文中的公式中，我们已经知道，若得到了正确的姿态R和t参数，就可以基于3D点信息以3D点投射到2D图像上的方式，来得到2D点信息。换句话说，知道了姿态参数R和t就可以知道每个3D点投影的2D点。

实际计算姿态前，我们已知2D点位置信息（比方用Dlib或手动选择点击得到关键点），假设也已知旋转R和平移t参数，那么基于2D点、R和t，我们可以算出2D点对应投影出的3D点（这个3D点是我们用参数R和t估算的），因为我们也知道真实的3D点，进而可以得到估算的3D点与真实的3D点二者的投影误差（re-projection error），即所有相对应点的距离平方和。

image
image

用前文中提到的两个公式（如上，再次列出），直接线性变换（DLT，direct linear transform）就是对姿态参数R和t的估计，想要改进一个最简单的方法就是修正R和t参数，看重投影误差有没有降低。如果降低，则接受新的R和t参数。但这个方法没有修正目标（每次加一点轻微扰动），而且很慢，一种名为LM的优化方法（Levenberg-Marquardt optimization），可以很好地解决这个问题，具体参考维基百科。

OpenCV的APIsolvePnP（PnP，Perspective-n-Point，即从3D点找到其对应的2D点），可以使用基于LM方法（Levenberg-Marquardt）进行姿态估计，只需要定义flag参数为CV_ITERATIVE即可（当然还有其他方法，见后文API说明），无论哪种方法的使用，都需要定义这三个参数：

  1. 3D坐标中的点objectPoints；
  2. 与3D点对应的2D坐标点imagePoints。如人脸关键点位置信息（五点如左右眼睛、嘴唇左右、鼻子）；
  3. 摄像机的畸变参数distCoeffs（一般是固定的，其中定义了焦距，光学中心等）。确保或者以该参数校准相机，不会有误差、错误的结果等。

有了上面三个参数交给LM优化算法，它就会使用相机的固有参数，结合世界坐标中的3D点与图像平面上的点信息，估计姿态的旋转R和平移t参数（即图像坐标系），反复迭代计算并寻找重投影误差最小时的旋转和平移参数，即我们最后期望的结果。

实际上，OpenCV有两个用来解决计算物体姿态函数接口：solvePnP、solvePnPRansac，第二个solvePnPRansac利用随机抽样一致算法（Random sample consensus，RANSAC）的思想，虽然计算出的姿态更加精确，但速度慢、没法实现实时系统，所以我们这里只关注第一个solvePnP函数，OpenCV提供其C++与Python接口，具体说明如下：

solvePnP

Finds an object pose from 3D-2D point correspondences.

  • C++: bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )
  • Python: cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]) → retval, rvec, tvec

Parameters:

  • objectPoints – Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector can be also passed here.
  • imagePoints – Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points.vector can be also passed here.
  • cameraMatrix – Input camera matrix  .
  • distCoeffs – Input vector of distortion coefficients  of 4, 5, or 8 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
  • rvec – Output rotation vector (see Rodrigues() ) that, together with tvec , brings points from the model coordinate system to the camera coordinate system.
  • tvec – Output translation vector.useExtrinsicGuess – If true (1), the function uses the provided rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them.
  • flags –Method for solving a PnP problem:
    1. CV_ITERATIVE Iterative method is based on Levenberg-Marquardt optimization. In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
    2. CV_P3P Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
    3. CV_EPNP Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”.
      The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients.
      Note: An example of how to use solvePNP for planar augmented reality can be found at opencv_source_code/samples/python2/plane_ar.py

4. Levenberg-Marquardt优化算法

【见后文】【见后文】【见后文】【见后文】【见后文】【见后文】
【】【】【】【】【】【】【】【】【】【】【】【】【】

5. 旋转参数转换为欧拉角

副标题：旋转向量（角轴）、四元数与欧拉角的转换

前面我们使用OpenCV的solvePnP接口，得到了旋转向量R和平移向量t。现在我们需要将旋转向量R转换为欧拉角。后面有与之对应实现的具体代码。这个代码的过程包含以下两个转换部分：

  1. 旋转向量rotation_vector ——> 四元数（quaterniond，即w,x,y,z）；
  2. 四元数（quaterniond，即w,x,y,z）——>对应的欧拉角（pitch、yaw、roll）。

注：更多旋转方面的转换公式见后面参考链接。

5.1 旋转向量（角轴）转为四元数

假设某个旋转是绕单位向量 n = [n_x, n_y, n_z]^T进行角度为\theta的旋转，则有四元数q：

  • q = [cos(\theta / 2), n_x sin(\theta / 2), n_y sin(\theta / 2), n_z sin(\theta / 2)] ^ T，
  • 即 q = [w, x, y, z]^T，
  • 其中有|q|^2 = w^2 + x^2 + y^2 + z^2 = 1

该转换公式中的\theta加上2pi，得到一个相同的旋转，其对应的四元数变成了-q，即任意的旋转都可以由两个互为相反数的四元数表示。

对应如下代码：

# calculate quaterniond from rotation vector
theta = cv2.norm(rotation_vector)
w = np.cos(theta / 2)
x = np.sin(theta / 2) * rotation_vector[0] / theta
y = np.sin(theta / 2) * rotation_vector[1] / theta
z = np.sin(theta / 2) * rotation_vector[2] / theta

5.2 四元数转为欧拉角

得到四元数后，再计算欧拉角，公式如下：

image

由于arctan和arcsin的取值范围在[−π/2, π/2]，之间只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数：

image

从四元数[w, x, y, z]转换为欧拉角的代码实现：

# quaterniond to eulerAngle
ysqr = y * y
xsqr = x * x
zsqr = z * z

# pitch (x-axis rotation)
t0 = 2.0 * (w * x + y * z)
t1 = 1.0 - 2.0 * (xsqr + ysqr)
pitch = math.atan2(t0, t1)
pitch = pitch * 180 / math.pi

# yaw (y-axis rotation)
t2 = 2.0 * (w * y - z * x)
t2 = 1.0 if t2 > 1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
yaw = math.asin(t2)
yaw = yaw * 180 / math.pi

# roll (z-axis rotation)
t3 = 2.0 * (w * z + x * y)
t4 = 1.0 - 2.0 * (ysqr + zsqr)
roll = math.atan2(t3, t4)
roll = roll * 180 / math.pi
if roll > 90:
	roll = (roll - 180) % 180
if roll < -90:
	roll = (roll + 180) % 180

计算得到的欧拉角是弧度，需要将弧度转换为角度，才是我们姿态估计时候的欧拉角，转换公式如下：

  • 角度转弧度：π/180×角度
  • 弧度变角度：180/π×弧度

6. 欧拉角与头部姿态

image

知道了欧拉角后，要确定头部姿态需要计算不同姿态对应的三个角度（pitch、yaw、roll）的值范围。常用的头部姿态有如下五个：左右摇头（水平45°倾斜，或者说是绕x轴旋转），左右转头（yaw，绕z轴旋转），抬头点头（一般看点头，不看抬头因为抬头和人脸正面差不多）。

这个参数需要调并校准，我在网上找了很多人脸图，然后对其旋转，得到3组图片：

  1. 从左到右水平转头的一系列图片；
  2. 从左到右摇头的一系列图片；
  3. 从上到下（抬头到点头）的一系列图片。

分别先跑网络得到关键点，观察计算出的角度，确认每张图片的角度范围。得出：

  1. 从左到右水平转头的一系列图片的欧拉角，即yaw，pitch，roll的角度变化范围；
  2. 从左到右摇头的一系列图片的欧拉角，即yaw，pitch，roll的角度变化范围；
  3. 从上到下（抬头到点头）的一系列图片的欧拉角，即yaw，pitch，roll的角度变化范围。

这个对应姿态的三个角度参数需要自己调整，从而判断当前的头部属于哪种姿态。

def get_pose(self, angle_dict):
	pose_dict = {'down':0,'front':0,\
				 'turn_left':0, 'turn_right':0,\
				 'shake_left':0, 'shake_right':0}
	# pitch
	if -180<angle_dict['pitch'] and angle_dict['pitch']<-100:
		pass #sose_dict['down'] = 1
	elif 100<angle_dict['pitch'] and angle_dict['pitch']<180:
		pose_dict['up'] = 1
	# yaw
	if -70<angle_dict['yaw'] and angle_dict['yaw']<-15:
		pose_dict['turn_left'] = 1
	elif 15<angle_dict['yaw'] and angle_dict['yaw']<70:
		pose_dict['turn_right'] = 1
	# roll
	if 10<angle_dict['roll']<70:
		pose_dict['shake_left'] = 1
	elif -70<angle_dict['roll']<-10:
		pose_dict["shake_right"] = 1
	# front
	if self.is_front(angle_dict):
		pose_dict["front"] = 1
	return pose_dict

def is_front(self, angle_dict):
	if abs(angle_dict['yaw'])<10 and\
	   abs(angle_dict['roll'])<10 and\
	   abs(angle_dict['pitch'])>160:
		return True
	return False
@ysh329
Copy link
Contributor Author

ysh329 commented Nov 11, 2018 •
edited

7. 头部姿态估计实现代码

具体代码说明，见前文内容。这部分是对前文内容基于LM方法的具体实现，LM方法调用OpenCV的solvePnP接口：

import cv2
import numpy as np
import math

    def get_angle(self, img, five_points_dict):
        img_size = img.shape
        if type(five_points_dict)==tuple or five_points_dict==None:
            print("[ERROR] no face found or many faces")
            return None
        image_points = np.array([five_points_dict['nose'],\
                                 five_points_dict['left_eye'],\
                                 five_points_dict['right_eye'],\
                                 five_points_dict['mouse_left'],\
                                 five_points_dict['mouse_right'],\
                                ])
        # 3D model points.
        model_points = np.array([
            (0.0, 0.0, 0.0),  # Nose tip
            (-165.0, 170.0, -135.0),  # Left eye left corner
            (165.0, 170.0, -135.0),  # Right eye right corner
            (-150.0, -150.0, -125.0),  # Left Mouth corner
            (150.0, -150.0, -125.0)  # Right mouth corner
        ])
        # Camera internals
        focal_length = img_size[1]
        center = (img_size[1] / 2, img_size[0] / 2)
        camera_matrix = np.array(
            [[focal_length, 0, center[0]],
             [0, focal_length, center[1]],
             [0, 0, 1]], dtype="double"
        )
        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)
        # calculate rotation angles
        theta = cv2.norm(rotation_vector)
        # transformed to quaterniond
        w = np.cos(theta / 2)
        x = np.sin(theta / 2) * rotation_vector[0] / theta
        y = np.sin(theta / 2) * rotation_vector[1] / theta
        z = np.sin(theta / 2) * rotation_vector[2] / theta
        # quaterniondToEulerAngle
        ysqr = y * y
        xsqr = x * x
        zsqr = z * z
        # pitch (x-axis rotation)
        t0 = 2.0 * (w * x + y * z)
        t1 = 1.0 - 2.0 * (xsqr + ysqr)
        pitch = math.atan2(t0, t1)
        pitch = pitch * 180 / math.pi
        # yaw (y-axis rotation)
        t2 = 2.0 * (w * y - z * x)
        t2 = 1.0 if t2 > 1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        yaw = math.asin(t2)
        yaw = yaw * 180 / math.pi
        # roll (z-axis rotation)
        t3 = 2.0 * (w * z + x * y)
        t4 = 1.0 - 2.0 * (ysqr + zsqr)
        roll = math.atan2(t3, t4)
        roll = roll * 180 / math.pi
        if roll > 90:
            roll = (roll - 180) % 180
        if roll < -90:
            roll = (roll + 180) % 180
        angle_dict = {"pitch": pitch,\
                              "yaw":   yaw,\
                              "roll":  roll}
        return angle_dict

8. 头部姿态估计的评价准则

image

@ysh329
Copy link
Contributor Author

ysh329 commented Nov 12, 2018 •
edited

9. 其它优秀方法

最新姿态估计研究进展，一般分为两种：

  • 自上而下：就是先检测包含人的框，即human proposal，然后对框子中的人进行姿态估计。一般RCNN（区域CNN就是这个思路）；
  • 自下而上：先检测keypoint，然后根据热力图、点与点之间连接的概率，根据图论知识，基于PAF（部分亲和字段）将关键点连接起来，将关键点分组到人。

9.1 CMU：openpose 研究多人的姿态估计

9.2 Google DeepGaze：

deepgaze 研究头部姿态和注意力方向。主要为头部姿态估计 先框出人脸区域，再进行姿态估计。

@ysh329
Copy link
Contributor Author

ysh329 commented Nov 16, 2018 •
edited

4. Levenberg-Marquardt优化算法

注：本节的整体脉络来自这篇博文：Levenberg-Marquardt算法浅谈 | liu14lang，在此我加以补充和扩写。

讲Levenberg-Marquardt算法前，先有必要讲一下牛顿法（Newton's method ）和高斯牛顿法（Quasi-Newton Methods）。

牛顿法（Newton's method）

牛顿法是一种在实数域和复数域上近似求解方程的方法。方法使用函数f(x)的泰勒级数（Taylor series，用无限项连加式——级数，来表示一个函数，这些相加的项由函数在某一点的导数求得）的前面几项来寻找方程f (x) = 0的根。牛顿法最大的特点就在于它的收敛速度很快（二次收敛）。

具体步骤

首先，选择一个接近函数f (x)零点的 x0，计算相应的f (x0)和切线斜率f ' (x0)（这里f ' 表示函数f 的导数）。然后我们计算穿过点(x0, f (x0)) 并且斜率为f '(x0)的直线和 x 轴的交点的x坐标，也就是求如下方程的解：
image
上面公式不好想的话，可以这么想：我们已知过点(x0, f(x0))、斜率为f'(x0)、但不知截距的直线，那么列直线方程可以求得截距，得到截距后就得到了该直线，然后再计算该直线和 x 轴的交点的 x 坐标。那就设y = 0，求 x即 可。这时便有该直线与 x 轴的交点坐标。
将上述公式左右两边同时除以f'(x0)，就得到x + f(x0)/f‘’(x0) - x0= 0，经过移动，即得到x = x0 - f(x0)/f'(x0)，即下面的公式（见下图）；
将新求得的点的 x 坐标命名为x1，通常x1会比x0更接近方程f (x) = 0的解。因此我们现在可以利用x1开始下一轮迭代。迭代公式可化简为如下所示：
image

  • 点斜式方程_百度百科
    已经有人证明了，若f ' 是连续的，且待求的零点x是孤立的，那么在零点x周围存在一个区域，只要初始值x0位于这个邻近区域内，那么牛顿法必定收敛。并且，若f ' (x)不为0，那么牛顿法将具有平方收敛的性能。粗略地说，这意味着每迭代一次，牛顿法结果的有效数字将增加一倍（见下图，牛顿法在二维情况下搜索路径的动态过程）。由于牛顿法是基于当前位置的切线来确定下一次的位置，所以牛顿法又被很形象地称为是“切线法”。
    newton

关于牛顿法和梯度下降法的效率对比

本质上看，牛顿法是二阶收敛，梯度下降是一阶收敛，所以牛顿法就更快。如果更通俗地说的话，比如你想找一条最短的路径走到一个盆地的最底部，梯度下降法每次只从你当前所处位置选一个坡度最大的方向走一步，牛顿法在选择方向时，不仅会考虑坡度是否够大，还会考虑你走了一步之后，坡度是否会变得更大。所以，可以说牛顿法比梯度下降法看得更远一点，能更快地走到最底部。（牛顿法目光更加长远，所以少走弯路；相对而言，梯度下降法只考虑了局部的最优，没有全局思想。）
根据wiki上的解释，从几何上说，牛顿法是用一个二次曲面去拟合你当前所处位置的局部曲面，而梯度下降法是用一个平面去拟合当前的局部曲面，通常情况下，二次曲面的拟合会比平面更好，所以牛顿法选择的下降路径会更符合真实的最优下降路径。
image
注：红色的牛顿法的迭代路径，绿色的是梯度下降法的迭代路径。

牛顿法的优缺点总结

  • 优点：二阶收敛，收敛速度快；
  • 缺点：牛顿法是一种迭代算法，每一步都需要求解目标函数的Hessian矩阵的逆矩阵，计算比较复杂。

【】【】【】【】【】【】【】【】【】【】【】【】【】

Newton method · Issue #22 · yuenshome/yuenshome.github.io
#22

@ysh329
Copy link
Contributor Author

ysh329 commented Nov 21, 2018

@ysh329
Copy link
Contributor Author

ysh329 commented Nov 22, 2018 •
edited

OpenCV

其是实现的solvePnP函数opencv/solvepnp.cpp at 8f15a609afc3c08ea0a5561ca26f1cf182414ca2 · opencv/opencv
如下：

bool solvePnP( InputArray _opoints, InputArray _ipoints,
               InputArray _cameraMatrix, InputArray _distCoeffs,
               OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
{
    CV_INSTRUMENT_REGION();

    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
    CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) )
               && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );

    Mat rvec, tvec;
    if( flags != SOLVEPNP_ITERATIVE )
        useExtrinsicGuess = false;

    if( useExtrinsicGuess )
    {
        int rtype = _rvec.type(), ttype = _tvec.type();
        Size rsize = _rvec.size(), tsize = _tvec.size();
        CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
                   (ttype == CV_32F || ttype == CV_64F) );
        CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
                   (tsize == Size(1, 3) || tsize == Size(3, 1)) );
    }
    else
    {    // 省略...................................    }
    rvec = _rvec.getMat();
    tvec = _tvec.getMat();

    Mat cameraMatrix0 = _cameraMatrix.getMat();
    Mat distCoeffs0 = _distCoeffs.getMat();
    Mat cameraMatrix = Mat_<double>(cameraMatrix0);
    Mat distCoeffs = Mat_<double>(distCoeffs0);
    bool result = false;

    if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
    {    // 省略...................................    }
    else if (flags == SOLVEPNP_P3P)
    {    // 省略...................................    }
    else if (flags == SOLVEPNP_AP3P)
    {    // 省略...................................    }
    else if (flags == SOLVEPNP_ITERATIVE)
    {
        CvMat c_objectPoints = cvMat(opoints), c_imagePoints = cvMat(ipoints);
        CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs);
        CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec);
        cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                                     (c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0,
                                     &c_rvec, &c_tvec, useExtrinsicGuess );
        result = true;
    }
    /*else if (flags == SOLVEPNP_DLS)
    {    // 省略...................................    }
    else if (flags == SOLVEPNP_UPNP)
    {    // 省略...................................    }*/
    else
        CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
    return result;
}

Levenberg-Marquardt优化算法的实现是其中的flags == SOLVEPNP_ITERATIVE的情况，该情况下调用了cvFindExtrinsicCameraParams2函数，该函数opencv/solvepnp.cpp at 8f15a609afc3c08ea0a5561ca26f1cf182414ca2 · opencv/opencv定义如下：

/* Finds extrinsic camera parameters from
   a few known corresponding point pairs and intrinsic parameters */
void cvFindExtrinsicCameraParams2( const CvMat* object_points,
                                const CvMat* image_points,
                                const CvMat* camera_matrix,
                                const CvMat* distortion_coeffs,
                                CvMat* rotation_vector,
                                CvMat* translation_vector,
                                int use_extrinsic_guess CV_DEFAULT(0) );

其实现的cvFindExtrinsicCameraParams2函数位于opencv/calibration.cpp at 8f15a609afc3c08ea0a5561ca26f1cf182414ca2 · opencv/opencv如下：

CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
                  const CvMat* imagePoints, const CvMat* A,
                  const CvMat* distCoeffs, CvMat* rvec, CvMat* tvec,
                  int useExtrinsicGuess )
{
    const int max_iter = 20;
    Ptr<CvMat> matM, _Mxy, _m, _mn, matL;

    int i, count;
    double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9];
    double MM[9], U[9], V[9], W[3];
    cv::Scalar Mc;
    double param[6];
    CvMat matA = cvMat( 3, 3, CV_64F, a );
    CvMat _Ar = cvMat( 3, 3, CV_64F, ar );
    CvMat matR = cvMat( 3, 3, CV_64F, R );
    CvMat _r = cvMat( 3, 1, CV_64F, param );
    CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
    CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val );
    CvMat _MM = cvMat( 3, 3, CV_64F, MM );
    CvMat matU = cvMat( 3, 3, CV_64F, U );
    CvMat matV = cvMat( 3, 3, CV_64F, V );
    CvMat matW = cvMat( 3, 1, CV_64F, W );
    CvMat _param = cvMat( 6, 1, CV_64F, param );
    CvMat _dpdr, _dpdt;

    CV_Assert( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) &&
        CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) );

    count = MAX(objectPoints->cols, objectPoints->rows);
    matM.reset(cvCreateMat( 1, count, CV_64FC3 ));
    _m.reset(cvCreateMat( 1, count, CV_64FC2 ));

    cvConvertPointsHomogeneous( objectPoints, matM );
    cvConvertPointsHomogeneous( imagePoints, _m );
    cvConvert( A, &matA );

    CV_Assert( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) &&
        (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 );

    CV_Assert( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) &&
        (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 );

    CV_Assert((count >= 4) || (count == 3 && useExtrinsicGuess)); // it is unsafe to call LM optimisation without an extrinsic guess in the case of 3 points. This is because there is no guarantee that it will converge on the correct solution.

    _mn.reset(cvCreateMat( 1, count, CV_64FC2 ));
    _Mxy.reset(cvCreateMat( 1, count, CV_64FC2 ));

    // normalize image points
    // (unapply the intrinsic matrix transformation and distortion)
    cvUndistortPoints( _m, _mn, &matA, distCoeffs, 0, &_Ar );

    if( useExtrinsicGuess )
    {
        CvMat _r_temp = cvMat(rvec->rows, rvec->cols,
            CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
        CvMat _t_temp = cvMat(tvec->rows, tvec->cols,
            CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3);
        cvConvert( rvec, &_r_temp );
        cvConvert( tvec, &_t_temp );
    }
    else
    {
        Mc = cvAvg(matM);
        cvReshape( matM, matM, 1, count );
        cvMulTransposed( matM, &_MM, 1, &_Mc );
        cvSVD( &_MM, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );

        // initialize extrinsic parameters
        if( W[2]/W[1] < 1e-3)
        {
            // a planar structure case (all M's lie in the same plane)
            double tt[3], h[9], h1_norm, h2_norm;
            CvMat* R_transform = &matV;
            CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
            CvMat matH = cvMat( 3, 3, CV_64F, h );
            CvMat _h1, _h2, _h3;

            if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
                cvSetIdentity( R_transform );

            if( cvDet(R_transform) < 0 )
                cvScale( R_transform, R_transform, -1 );

            cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );

            for( i = 0; i < count; i++ )
            {
                const double* Rp = R_transform->data.db;
                const double* Tp = T_transform.data.db;
                const double* src = matM->data.db + i*3;
                double* dst = _Mxy->data.db + i*2;

                dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
                dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
            }

            cvFindHomography( _Mxy, _mn, &matH );

            if( cvCheckArr(&matH, CV_CHECK_QUIET) )
            {
                cvGetCol( &matH, &_h1, 0 );
                _h2 = _h1; _h2.data.db++;
                _h3 = _h2; _h3.data.db++;
                h1_norm = std::sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
                h2_norm = std::sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);

                cvScale( &_h1, &_h1, 1./MAX(h1_norm, DBL_EPSILON) );
                cvScale( &_h2, &_h2, 1./MAX(h2_norm, DBL_EPSILON) );
                cvScale( &_h3, &_t, 2./MAX(h1_norm + h2_norm, DBL_EPSILON));
                cvCrossProduct( &_h1, &_h2, &_h3 );

                cvRodrigues2( &matH, &_r );
                cvRodrigues2( &_r, &matH );
                cvMatMulAdd( &matH, &T_transform, &_t, &_t );
                cvMatMul( &matH, R_transform, &matR );
            }
            else
            {
                cvSetIdentity( &matR );
                cvZero( &_t );
            }

            cvRodrigues2( &matR, &_r );
        }
        else
        {
            // non-planar structure. Use DLT method
            double* L;
            double LL[12*12], LW[12], LV[12*12], sc;
            CvMat _LL = cvMat( 12, 12, CV_64F, LL );
            CvMat _LW = cvMat( 12, 1, CV_64F, LW );
            CvMat _LV = cvMat( 12, 12, CV_64F, LV );
            CvMat _RRt, _RR, _tt;
            CvPoint3D64f* M = (CvPoint3D64f*)matM->data.db;
            CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db;

            matL.reset(cvCreateMat( 2*count, 12, CV_64F ));
            L = matL->data.db;

            for( i = 0; i < count; i++, L += 24 )
            {
                double x = -mn[i].x, y = -mn[i].y;
                L[0] = L[16] = M[i].x;
                L[1] = L[17] = M[i].y;
                L[2] = L[18] = M[i].z;
                L[3] = L[19] = 1.;
                L[4] = L[5] = L[6] = L[7] = 0.;
                L[12] = L[13] = L[14] = L[15] = 0.;
                L[8] = x*M[i].x;
                L[9] = x*M[i].y;
                L[10] = x*M[i].z;
                L[11] = x;
                L[20] = y*M[i].x;
                L[21] = y*M[i].y;
                L[22] = y*M[i].z;
                L[23] = y;
            }

            cvMulTransposed( matL, &_LL, 1 );
            cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
            _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
            cvGetCols( &_RRt, &_RR, 0, 3 );
            cvGetCol( &_RRt, &_tt, 3 );
            if( cvDet(&_RR) < 0 )
                cvScale( &_RRt, &_RRt, -1 );
            sc = cvNorm(&_RR);
            CV_Assert(fabs(sc) > DBL_EPSILON);
            cvSVD( &_RR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
            cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T );
            cvScale( &_tt, &_t, cvNorm(&matR)/sc );
            cvRodrigues2( &matR, &_r );
        }
    }

    cvReshape( matM, matM, 3, 1 );
    cvReshape( _mn, _mn, 2, 1 );

    // refine extrinsic parameters using iterative algorithm
    CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
    cvCopy( &_param, solver.param );

    for(;;)
    {
        CvMat *matJ = 0, *_err = 0;
        const CvMat *__param = 0;
        bool proceed = solver.update( __param, matJ, _err );
        cvCopy( __param, &_param );
        if( !proceed || !_err )
            break;
        cvReshape( _err, _err, 2, 1 );
        if( matJ )
        {
            cvGetCols( matJ, &_dpdr, 0, 3 );
            cvGetCols( matJ, &_dpdt, 3, 6 );
            cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
                              _err, &_dpdr, &_dpdt, 0, 0, 0 );
        }
        else
        {
            cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
                              _err, 0, 0, 0, 0, 0 );
        }
        cvSub(_err, _m, _err);
        cvReshape( _err, _err, 1, 2*count );
    }
    cvCopy( solver.param, &_param );

    _r = cvMat( rvec->rows, rvec->cols,
        CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
    _t = cvMat( tvec->rows, tvec->cols,
        CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 );

    cvConvert( &_r, rvec );
    cvConvert( &_t, tvec );
}

@ysh329
Copy link
Contributor Author

ysh329 commented Nov 26, 2018 •
edited

该函数代码中最关键的是函数solver，因为有一个参数max_iter被传入：

    // refine extrinsic parameters using iterative algorithm
    CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
    cvCopy( &_param, solver.param );

其定义如下：

@ysh329
Copy link
Contributor Author

ysh329 commented Nov 26, 2018 •
edited

CvLevMarq类的定义，位于opencv/calib3d_c.h at 8f15a609afc3c08ea0a5561ca26f1cf182414ca2 · opencv/opencv，其实现的构造、初始化、析构、参数更新等成员函数位于opencv/compat_ptsetreg.cpp at 8f15a609afc3c08ea0a5561ca26f1cf182414ca2 · opencv/opencv。

类CvLevMarq定义：

class CV_EXPORTS CvLevMarq
{
public:
    CvLevMarq();
    CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria=
              cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
              bool completeSymmFlag=false );
    ~CvLevMarq();
    void init( int nparams, int nerrs, CvTermCriteria criteria=
              cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
              bool completeSymmFlag=false );
    bool update( const CvMat*& param, CvMat*& J, CvMat*& err );
    bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm );

    void clear();
    void step();
    enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 };

    cv::Ptr<CvMat> mask;
    cv::Ptr<CvMat> prevParam;
    cv::Ptr<CvMat> param;
    cv::Ptr<CvMat> J;
    cv::Ptr<CvMat> err;
    cv::Ptr<CvMat> JtJ;
    cv::Ptr<CvMat> JtJN;
    cv::Ptr<CvMat> JtErr;
    cv::Ptr<CvMat> JtJV;
    cv::Ptr<CvMat> JtJW;
    double prevErrNorm, errNorm;
    int lambdaLg10;
    CvTermCriteria criteria;
    int state;
    int iters;
    bool completeSymmFlag;
    int solveMethod;
};

@ysh329
Copy link
Contributor Author

ysh329 commented Nov 26, 2018

在cvFindExtrinsicCameraParams2函数的实现中，初始化了CvLevMarq solver，在后面该实例会更新update，其实现如下：

bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
{
    matJ = _err = 0;

    assert( !err.empty() );
    if( state == DONE )
    {
        _param = param;
        return false;
    }

    if( state == STARTED )
    {
        _param = param;
        cvZero( J );
        cvZero( err );
        matJ = J;
        _err = err;
        state = CALC_J;
        return true;
    }

    if( state == CALC_J )
    {
        cvMulTransposed( J, JtJ, 1 );
        cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
        cvCopy( param, prevParam );
        step();
        if( iters == 0 )
            prevErrNorm = cvNorm(err, 0, CV_L2);
        _param = param;
        cvZero( err );
        _err = err;
        state = CHECK_ERR;
        return true;
    }

    assert( state == CHECK_ERR );
    errNorm = cvNorm( err, 0, CV_L2 );
    if( errNorm > prevErrNorm )
    {
        if( ++lambdaLg10 <= 16 )
        {
            step();
            _param = param;
            cvZero( err );
            _err = err;
            state = CHECK_ERR;
            return true;
        }
    }

    lambdaLg10 = MAX(lambdaLg10-1, -16);
    if( ++iters >= criteria.max_iter ||
        cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
    {
        _param = param;
        state = DONE;
        return true;
    }

    prevErrNorm = errNorm;
    _param = param;
    cvZero(J);
    matJ = J;
    _err = err;
    state = CALC_J;
    return true;
}

@zhaipro zhaipro mentioned this issue Jun 26, 2019
Open
@liser0908
Copy link

有没有定义 抬头低头的具体pitch值啊？

@henryouly henryouly mentioned this issue Jun 5, 2022
Merged
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@ysh329 @liser0908