• 世界坐标系,相机坐标系和图像坐标系的转换(Python)
千次阅读
2020-08-18 08:54:54

添加链接描述转载

# -*- coding: utf-8 -*-
"""
# --------------------------------------------------------
# @Project: Integral-Human-Pose-Regression-for-3D-Human-Pose-Estimation
# @Author : panjq
# @E-mail : pan_jinquan@163.com
# @Date   : 2020-02-04 16:03:01
# --------------------------------------------------------
"""
import sys
import os

sys.path.append(os.getcwd())

import cv2
import numpy as np
from modules.utils_3d import vis_3d as vis
from utils import image_processing

human36m_camera_intrinsic = {
# R，旋转矩阵
"R": [[-0.91536173, 0.40180837, 0.02574754],
[0.05154812, 0.18037357, -0.98224649],
[-0.39931903, -0.89778361, -0.18581953]],
# t，平移向量
"T": [1841.10702775, 4955.28462345, 1563.4453959],
# 焦距，f/dx, f/dy
"f": [1145.04940459, 1143.78109572],
# principal point，主点，主轴与像平面的交点
"c": [512.54150496, 515.45148698]

}

kinect2_camera_intrinsic = {

# R，旋转矩阵
"R": [[0.999853, -0.00340388, 0.0167495],
[0.00300206, 0.999708, 0.0239986],
[-0.0168257, -0.0239459, 0.999571]],
# t，平移向量
"T": [15.2562, 70.2212, -10.9926],
# 焦距，f/dx, f/dy
"f": [367.535, 367.535],
# principal point，主点，主轴与像平面的交点
"c": [260.166, 205.197]

}

camera_intrinsic = human36m_camera_intrinsic
# camera_intrinsic = kinect2_camera_intrinsic

class CameraTools(object):

@staticmethod
def convert_wc_to_cc(joint_world):
"""
世界坐标系 -> 相机坐标系: R * (pt - T):
joint_cam = np.dot(R, (joint_world - T).T).T
:return:
"""
joint_world = np.asarray(joint_world)
R = np.asarray(camera_intrinsic["R"])
T = np.asarray(camera_intrinsic["T"])
joint_num = len(joint_world)
# 世界坐标系 -> 相机坐标系
# [R|t] world coords -> camera coords
# joint_cam = np.zeros((joint_num, 3))  # joint camera
# for i in range(joint_num):  # joint i
#     joint_cam[i] = np.dot(R, joint_world[i] - T)  # R * (pt - T)
# .T is 转置, T is translation mat
joint_cam = np.dot(R, (joint_world - T).T).T  # R * (pt - T)
return joint_cam

@staticmethod
def convert_cc_to_wc(joint_world):
"""
相机坐标系 -> 世界坐标系: inv(R) * pt +T
joint_cam = np.dot(inv(R), joint_world.T)+T
:return:
"""
joint_world = np.asarray(joint_world)
R = np.asarray(camera_intrinsic["R"])
T = np.asarray(camera_intrinsic["T"])
# 相机坐标系 -> 世界坐标系
joint_cam = np.dot(np.linalg.inv(R), joint_world.T).T + T
return joint_cam

@staticmethod
def __cam2pixel(cam_coord, f, c):
"""
相机坐标系 -> 像素坐标系: (f / dx) * (X / Z) = f * (X / Z) / dx
cx,ppx=260.166; cy,ppy=205.197; fx=367.535; fy=367.535
将从3D(X,Y,Z)映射到2D像素坐标P(u,v)计算公式为：
u = X * fx / Z + cx
v = Y * fy / Z + cy
D(v,u) = Z / Alpha
=====================================================
camera_matrix = [[428.30114, 0.,   316.41648],
[   0.,    427.00564, 218.34591],
[   0.,      0.,    1.]])
fx = camera_intrinsic[0, 0]
fy = camera_intrinsic[1, 1]
cx = camera_intrinsic[0, 2]
cy = camera_intrinsic[1, 2]
=====================================================
:param cam_coord:
:param f: [fx,fy]
:param c: [cx,cy]
:return:
"""
# 等价于：(f / dx) * (X / Z) = f * (X / Z) / dx
# 三角变换， / dx, + center_x
u = cam_coord[..., 0] / cam_coord[..., 2] * f[0] + c[0]
v = cam_coord[..., 1] / cam_coord[..., 2] * f[1] + c[1]
d = cam_coord[..., 2]
return u, v, d

@staticmethod
def convert_cc_to_ic(joint_cam):
"""
相机坐标系 -> 像素坐标系
:param joint_cam:
:return:
"""
# 相机坐标系 -> 像素坐标系，并 get relative depth
# Subtract center depth
# 选择 Pelvis骨盆 所在位置作为相机中心，后面用之求relative depth
root_idx = 0
center_cam = joint_cam[root_idx]  # (x,y,z) mm
joint_num = len(joint_cam)
f = camera_intrinsic["f"]
c = camera_intrinsic["c"]
# joint image_dict，像素坐标系，Depth 为相对深度 mm
joint_img = np.zeros((joint_num, 3))
joint_img[:, 0], joint_img[:, 1], joint_img[:, 2] = CameraTools.__cam2pixel(joint_cam, f, c)  # x,y
joint_img[:, 2] = joint_img[:, 2] - center_cam[2]  # z
return joint_img

def demo_for_human36m():
joint_world = [[-91.679, 154.404, 907.261],
[-223.23566, 163.80551, 890.5342],
[-188.4703, 14.077106, 475.1688],
[-261.84055, 186.55286, 61.438915],
[39.877888, 145.00247, 923.98785],
[-11.675994, 160.89919, 484.39148],
[-51.550297, 220.14624, 35.834396],
[-132.34781, 215.73018, 1128.8396],
[-97.1674, 202.34435, 1383.1466],
[-112.97073, 127.96946, 1477.4457],
[-120.03289, 190.96477, 1573.4],
[25.895456, 192.35947, 1296.1571],
[107.10581, 116.050285, 1040.5062],
[129.8381, -48.024918, 850.94806],
[-230.36955, 203.17923, 1311.9639],
[-315.40536, 164.55284, 1049.1747],
[-350.77136, 43.442127, 831.3473],
[-102.237045, 197.76935, 1304.0605]]
joint_world = np.asarray(joint_world)
# 关节点连接线
kps_lines = ((0, 7), (7, 8), (8, 9), (9, 10), (8, 11), (11, 12), (12, 13), (8, 14), (14, 15),
(15, 16), (0, 1), (1, 2), (2, 3), (0, 4), (4, 5), (5, 6))
# show in 世界坐标系
vis.vis_3d(joint_world, kps_lines, coordinate="WC", title="WC", set_lim=True, isshow=True)

kp_vis = CameraTools()

# show in 相机坐标系
joint_cam = kp_vis.convert_wc_to_cc(joint_world)
vis.vis_3d(joint_cam, kps_lines, coordinate="CC", title="CC", set_lim=True, isshow=True)
joint_img = kp_vis.convert_cc_to_ic(joint_cam)

joint_world1 = kp_vis.convert_cc_to_wc(joint_cam)
vis.vis_3d(joint_world1, kps_lines, coordinate="WC", title="WC", set_lim=True, isshow=True)

# show in 像素坐标系
kpt_2d = joint_img[:, 0:2]
image_path = "./data/s_01_act_02_subact_01_ca_02_000001.jpg"
image = image_processing.draw_key_point_in_image(image, key_points=[kpt_2d], pointline=kps_lines)
image_processing.cv_show_image("image_dict", image)

if __name__ == "__main__":
demo_for_human36m()


更多相关内容
• 本资源包含基于Matlab的imref2d函数实现...本资源配套CSDN博客“Matlab函数学习---imref2d函数(将二维图像转世界坐标)”，可前往查看具体原理和实现效果！！！ 希望对大家有帮助,好的话帮忙点个赞哦！感谢支持！！！
• 世界坐标系,相机坐标系和图像坐标系的转换(Python)1.世界坐标->相机坐标2.相机坐标系->图像坐标系此时投影点p的单位还是mm，并不是pixel，需要进一步转换到像素坐标系。3.图像坐标系与像素坐标系像素坐标系和...

世界坐标系,相机坐标系和图像坐标系的转换(Python)

1.世界坐标->相机坐标

2.相机坐标系->图像坐标系

此时投影点p的单位还是mm，并不是pixel，需要进一步转换到像素坐标系。

3.图像坐标系与像素坐标系

像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下：其中dx和dy表示每一列和每一行分别代表多少mm，即1pixel=dx mm

那么通过上面四个坐标系的转换就可以得到一个点从世界坐标系如何转换到像素坐标系的。

python代码shi实现:

# -*- coding: utf-8 -*-

"""

# --------------------------------------------------------

# @Project: prpject

# @Author : panjq

# @E-mail : pan_jinquan@163.com

# @Date : 2020-02-04 16:03:01

# --------------------------------------------------------

"""

import sys

import os

from tools import image_processing

sys.path.append(os.getcwd())

import numpy as np

from modules.utils_3d import vis

camera_intrinsic = {

# R，旋转矩阵

"R": [[-0.91536173, 0.40180837, 0.02574754],

[0.05154812, 0.18037357, -0.98224649],

[-0.39931903, -0.89778361, -0.18581953]],

# t，平移向量

"T": [1841.10702775, 4955.28462345, 1563.4453959],

# 焦距，f/dx, f/dy

"f": [1145.04940459, 1143.78109572],

# principal point，主点，主轴与像平面的交点

"c": [512.54150496, 515.45148698]

}

class Human36M(object):

@staticmethod

def convert_wc_to_cc(joint_world):

"""

世界坐标系 -> 相机坐标系: R * (pt - T)

:return:

"""

joint_world = np.asarray(joint_world)

R = np.asarray(camera_intrinsic["R"])

T = np.asarray(camera_intrinsic["T"])

joint_num = len(joint_world)

# 世界坐标系 -> 相机坐标系

# [R|t] world coords -> camera coords

joint_cam = np.zeros((joint_num, 3)) # joint camera

for i in range(joint_num): # joint i

joint_cam[i] = np.dot(R, joint_world[i] - T) # R * (pt - T)

return joint_cam

@staticmethod

def __cam2pixel(cam_coord, f, c):

"""

相机坐标系 -> 像素坐标系: (f / dx) * (X / Z) = f * (X / Z) / dx

cx,ppx=260.166; cy,ppy=205.197; fx=367.535; fy=367.535

将从3D(X,Y,Z)映射到2D像素坐标P(u,v)计算公式为：

u = X * fx / Z + cx

v = Y * fy / Z + cy

D(v,u) = Z / Alpha

=====================================================

camera_matrix = [[428.30114, 0., 316.41648],

[ 0., 427.00564, 218.34591],

[ 0., 0., 1.]])

fx = camera_intrinsic[0, 0]

fy = camera_intrinsic[1, 1]

cx = camera_intrinsic[0, 2]

cy = camera_intrinsic[1, 2]

=====================================================

:param cam_coord:

:param f: [fx,fy]

:param c: [cx,cy]

:return:

"""

# 等价于：(f / dx) * (X / Z) = f * (X / Z) / dx

# 三角变换， / dx, + center_x

u = cam_coord[..., 0] / cam_coord[..., 2] * f[0] + c[0]

v = cam_coord[..., 1] / cam_coord[..., 2] * f[1] + c[1]

d = cam_coord[..., 2]

return u, v, d

@staticmethod

def convert_cc_to_ic(joint_cam):

"""

相机坐标系 -> 像素坐标系

:param joint_cam:

:return:

"""

# 相机坐标系 -> 像素坐标系，并 get relative depth

# Subtract center depth

# 选择 Pelvis骨盆 所在位置作为相机中心，后面用之求relative depth

root_idx = 0

center_cam = joint_cam[root_idx] # (x,y,z) mm

joint_num = len(joint_cam)

f = camera_intrinsic["f"]

c = camera_intrinsic["c"]

# joint image，像素坐标系，Depth 为相对深度 mm

joint_img = np.zeros((joint_num, 3))

joint_img[:, 0], joint_img[:, 1], joint_img[:, 2] = Human36M.__cam2pixel(joint_cam, f, c) # x,y

joint_img[:, 2] = joint_img[:, 2] - center_cam[2] # z

return joint_img

if __name__ == "__main__":

joint_world = [[-91.679, 154.404, 907.261],

[-223.23566, 163.80551, 890.5342],

[-188.4703, 14.077106, 475.1688],

[-261.84055, 186.55286, 61.438915],

[39.877888, 145.00247, 923.98785],

[-11.675994, 160.89919, 484.39148],

[-51.550297, 220.14624, 35.834396],

[-132.34781, 215.73018, 1128.8396],

[-97.1674, 202.34435, 1383.1466],

[-112.97073, 127.96946, 1477.4457],

[-120.03289, 190.96477, 1573.4],

[25.895456, 192.35947, 1296.1571],

[107.10581, 116.050285, 1040.5062],

[129.8381, -48.024918, 850.94806],

[-230.36955, 203.17923, 1311.9639],

[-315.40536, 164.55284, 1049.1747],

[-350.77136, 43.442127, 831.3473],

[-102.237045, 197.76935, 1304.0605]]

joint_world = np.asarray(joint_world)

kps_lines = ((0, 7), (7, 8), (8, 9), (9, 10), (8, 11), (11, 12), (12, 13), (8, 14), (14, 15),

(15, 16), (0, 1), (1, 2), (2, 3), (0, 4), (4, 5), (5, 6))

# show in 世界坐标系

vis.vis_3d(joint_world, kps_lines, coordinate="WC", title="WC")

human36m = Human36M()

# show in 相机坐标系

joint_cam = human36m.convert_wc_to_cc(joint_world)

vis.vis_3d(joint_cam, kps_lines, coordinate="CC", title="CC")

joint_img = human36m.convert_cc_to_ic(joint_cam)

# show in 像素坐标系

kpt_2d = joint_img[:, 0:2]

image_path = "/media/dm/dm1/git/python-learning-notes/modules/utils_3d/s_01_act_02_subact_01_ca_02_000001.jpg"

image = image_processing.draw_key_point_in_image(image, key_points=[kpt_2d], pointline=kps_lines)

image_processing.cv_show_image("image", image)

展开全文
• 本文链接：<a href="https://blog.csdn.net/Kalenee/article/details/80659489">.../a> > 一、坐标变换详解 1.1 坐标关系 相机中有四个坐标系，分别为wor...

## 一、坐标变换详解

### 1.1 坐标关系

相机中有四个坐标系，分别为worldcameraimagepixel

• world为世界坐标系，可以任意指定$x_w$轴和$y_w$轴，为上图P点所在坐标系。
• camera为相机坐标系，原点位于小孔，z轴与光轴重合，轴和轴平行投影面，为上图坐标系$X_{c}Y_{c}Z_{c}$
• image为图像坐标系，原点位于光轴和投影面的交点，轴和轴平行投影面，为上图坐标系xy。
• pixel为像素坐标系，从小孔向投影面方向看，投影面的左上角为原点，uv轴和投影面两边重合，该坐标系与图像坐标系处在同一平面，但原点不同。

### 1.2 坐标转换

下式为像素坐标pixel与世界坐标world的转换公式，左侧第一个矩阵为相机内参数矩阵，第二个矩阵为相机外参数矩阵。假设图像坐标已知，同时相机内参数矩阵通过标定已获取，还需计算比例系数s和外参数矩阵。

$s\begin{bmatrix} u\\v\\1 \end{bmatrix} =\begin{bmatrix} f_x&0&c_x\\0&f_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12}&r_{13}&t_1 \\ r_{21} & r_{22}&r_{23}&t_2 \\ r_{31} & r_{32}&r_{33}&t_3 \end{bmatrix} \begin{bmatrix} x\\y\\z\\1 \end{bmatrix}$

• 比例系数s

转换公式可简化为：

$s\begin{bmatrix} u\\v\\1 \end{bmatrix} =M(R\begin{bmatrix} X\\Y\\Z_{const} \end{bmatrix}+t)$

M为相机内参数矩阵，R为旋转矩阵，t为平移矩阵，$Z_{const}$为世界坐标系高度，可设置为0。

通过矩阵变换可得下式：

$R^{-1}M^{-1}s\begin{bmatrix} u\\v\\1 \end{bmatrix} =\begin{bmatrix} X\\Y\\Z_{const} \end{bmatrix}+R^{-1}t$

求解出旋转矩阵和平移矩阵即可算得s。

• 外参数矩阵

Perspective-n-Point是通过n组给定点的世界坐标与像素坐标估计相机位置的方法。OpenCV内部提供的函数为solvePnP()，函数介绍如下：



bool solvePnP(InputArray objectPoints,

InputArray imagePoints,

InputArray cameraMatrix,

InputArray distCoeffs,

OutputArray rvec,

OutputArray tvec,

bool useExtrinsicGuess=
false,

int flags=ITERATIVE )


• objectPoints，输入世界坐标系中点的坐标；
• imagePoints，输入对应图像坐标系中点的坐标；
• cameraMatrix, 相机内参数矩阵；
• distCoeffs, 畸变系数；
• rvec, 旋转向量，需输入一个非空Mat，需要通过cv::Rodrigues转换为旋转矩阵；
• tvec, 平移向量，需输入一个非空Mat；
• useExtrinsicGuess, 默认为false，如果设置为true则输出输入的旋转矩阵和平移矩阵；
• flags，选择采用的算法；
• 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 .
• 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.
• 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”.

注意：solvePnP的参数rvec和tvec应该都是double类型的

## 二、程序实现

（1）计算参数s和旋转平移矩阵，需要输入一系列的世界坐标系的点及其对应的图像坐标系的点。



//输入参数

Mat cameraMatrix = Mat(
3,
3, CV_32FC1, Scalar::all(
0));
/* 摄像机内参数矩阵 */

Mat distCoeffs = Mat(
1,
5, CV_32FC1, Scalar::all(
0));
/* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */

double zConst =
0;
//实际坐标系的距离，若工作平面与相机距离固定可设置为0

//计算参数

double s;

Mat rotationMatrix = Mat (
3,
3, DataType<
double>::type);

Mat tvec = Mat (
3,
1, cv::DataType<
double>::type);

void calcParameters(vector<cv::Point2f> imagePoints, vector<cv::Point3f> objectPoints)

{

//计算旋转和平移

Mat rvec(3, 1, cv::DataType<double>::type);

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);

cv::Rodrigues(rvec, rotationMatrix);

}



（2）根据输入的图像坐标计算世界坐标。



Point3f getWorldPoints(Point2f inPoints)

{

//获取图像坐标

cv::Mat imagePoint = cv::Mat::ones(
3,
1, cv::DataType<
double>::type);
//u,v,1

imagePoint.at<
double>(
0,
0) = inPoints.x;

imagePoint.at<
double>(
1,
0) = inPoints.y;

//计算比例参数S

cv::Mat tempMat, tempMat2;

tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;

tempMat2 = rotationMatrix.inv() * tvec;

s = zConst + tempMat2.at<
double>(
2,
0);

s /= tempMat.at<
double>(
2,
0);

//计算世界坐标

Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);

Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));

return worldPoint;

}



参考

https://stackoverflow.com/questions/12299870/computing-x-y-coordinate-3d-from-image-point

https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html

《视觉SLAM十四讲》——相机与图像

展开全文
• 一、坐标关系 相机中有四个坐标系，分别为world，camera，image，pixel world为世界坐标系，可以任意指定xwx_wxw​轴和ywy_wyw​轴，为上图P点所在坐标系。...image为图像坐标系，原点位于光轴...

相机标定(一)——内参标定与程序实现

相机标定(二)——图像坐标与世界坐标转换

相机标定(三)——手眼标定

## 一、坐标关系

相机中有四个坐标系，分别为worldcameraimagepixel

• world为世界坐标系，可以任意指定 x w x_w 轴和 y w y_w 轴，为上图P点所在坐标系。
• camera为相机坐标系，原点位于小孔，z轴与光轴重合， x w x_w 轴和 y w y_w 轴平行投影面，为上图坐标系 X c Y c Z c X_cY_cZ_c
• image为图像坐标系，原点位于光轴和投影面的交点， x w x_w 轴和 y w y_w 轴平行投影面，为上图坐标系 X Y Z XYZ
• pixel为像素坐标系，从小孔向投影面方向看，投影面的左上角为原点， u v uv 轴和投影面两边重合，该坐标系与图像坐标系处在同一平面，但原点不同。

## 二、坐标变换

下式为像素坐标pixel与世界坐标world的变换公式，右侧第一个矩阵为相机内参数矩阵，第二个矩阵为相机外参数矩阵。
s [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] [ x y z 1 ] s\begin{bmatrix} u\\v\\1 \end{bmatrix} =\begin{bmatrix} f_x&0&c_x\\0&f_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12}&r_{13}&t_1 \\ r_{21} & r_{22}&r_{23}&t_2 \\ r_{31} & r_{32}&r_{33}&t_3 \end{bmatrix} \begin{bmatrix} x\\y\\z\\1 \end{bmatrix}

### 2.1 变换流程

P u v = K T P w P_{uv} = KTP_w

该方程右侧隐含了一次齐次坐标到非齐次坐标的转换

• K K 内参 T c a m e r a p i x e l T_{camera}^{pixel} ：像素坐标系相对于相机坐标系的变换（与相机和镜头有关）
• T T 外参 T w o r l d c a m e r a T_{world}^{camera} ：相机坐标系相对于世界坐标系的变换

顺序变换

• pixelcamera，使用内参变换

P c a m e r a ( 3 × 1 ) = T c a m e r a p i x e l − 1 ( 3 × 3 ) ∗ P p i x e l ( 3 × 1 ) ∗ d e p t h P_{camera}(3 \times 1) = {T_{camera}^{pixel}}^{-1}(3 \times 3)*P_{pixel}(3 \times 1)*depth

• cameraworld，使用外参变换

P w o r l d ( 4 × 1 ) = T w o r l d c a m e r a − 1 ( 4 × 4 ) ∗ P c a m e r a ( 4 × 1 ) P_{world} (4 \times 1)= {T_{world}^{camera}}^{-1}(4 \times 4)* P_{camera}(4 \times 1)

注意：两个变换之间的矩阵大小不同，需要分开计算，从pixelcamera获得的相机坐标为非齐次，需转换为齐次坐标再进行下一步变换。而在进行从cameraworld时，需将外参矩阵转换为齐次再进行计算。（齐次坐标的分析

直接变换
[ X Y Z ] = R − 1 ( M − 1 ∗ s ∗ [ u v 1 ] − t ) \begin{bmatrix} X\\Y\\Z \end{bmatrix}= R^{-1}(M^{-1}*s*\begin{bmatrix} u\\v\\1 \end{bmatrix} - t)
注意：直接变换是直接根据变换公式获得，实际上包含pixelcameracameraworld，实际上和顺序变换一样，通过顺序变换可以更清晰了解变换过程。

### 2.2 参数计算

• 内参：通过张正友标定获得

• 外参：通过PNP估计获得

• 深度s：深度s为目标点在相机坐标系Z方向的值

### 2.3 外参计算

• solvePnP函数

Perspective-n-Point是通过n组给定点的世界坐标与像素坐标估计相机位置的方法。OpenCV内部提供的函数为solvePnP()，函数介绍如下：

bool solvePnP(InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArray rvec,
OutputArray tvec,
bool useExtrinsicGuess=false,
int flags=ITERATIVE )

• objectPoints，输入世界坐标系中点的坐标；
• imagePoints，输入对应图像坐标系中点的坐标；
• cameraMatrix, 相机内参数矩阵；
• distCoeffs, 畸变系数；
• rvec, 旋转向量，需输入一个非空Mat，需要通过cv::Rodrigues转换为旋转矩阵；
• tvec, 平移向量，需输入一个非空Mat；
• useExtrinsicGuess, 默认为false，如果设置为true则输出输入的旋转矩阵和平移矩阵；
• flags，选择采用的算法；
• 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 .
• 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.
• 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”.

注意solvePnP()的参数rvectvec应该都是double类型的

• 程序实现
//输入参数
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 摄像机内参数矩阵 */
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
double zConst = 0;//实际坐标系的距离,若工作平面与相机距离固定可设置为0

//计算参数
double s;
Mat rotationMatrix = Mat (3, 3, DataType<double>::type);
Mat tvec = Mat (3, 1, DataType<double>::type);
void calcParameters(vector<cv::Point2f> imagePoints, vector<cv::Point3f> objectPoints)
{
//计算旋转和平移
Mat rvec(3, 1, cv::DataType<double>::type);
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Rodrigues(rvec, rotationMatrix);
}


### 2.4 深度计算

理想情况下，相机与目标平面平行（只有绕Z轴的旋转），但实际上相机与目标平面不会完全平行，存在绕X和Y轴的旋转，此时深度s并不是固定值 t 3 t_3 ，计算深度值为：
s = t 3 + r 31 ∗ x + r 32 ∗ y + r 33 ∗ z s = t_3 + r_{31} * x + r_{32} * y + r_{33} * z
若使用固定值进行变换会导致较大误差。解决方案如下：

• 计算多个点的深度值，拟合一个最优值
• 通过外参计算不同位置的深度（此处采用该方案）

注意：此处环境为固定单目与固定工作平面，不同情况下获得深度方法不同。

像素坐标pixel与世界坐标world转换公式可简化为
s [ u v 1 ] = M ( R [ X Y Z c o n s t ] + t ) s\begin{bmatrix} u\\v\\1 \end{bmatrix} =M(R\begin{bmatrix} X\\Y\\Z_{const} \end{bmatrix}+t)
M M 为相机内参数矩阵， R R 为旋转矩阵， t t 为平移矩阵， z c o n s t z_{const} 为目标点在世界坐标Z方向的值，此处为0。

变换可得
R − 1 M − 1 s [ u v 1 ] = [ X Y Z c o n s t ] + R − 1 t R^{-1}M^{-1}s\begin{bmatrix} u\\v\\1 \end{bmatrix} =\begin{bmatrix} X\\Y\\Z_{const} \end{bmatrix}+R^{-1}t
当相机内外参已知可计算获得 s s

## 三、程序实现

### 3.1 Matlab

clc;
clear;

% 内参
syms fx cx fy cy;
M = [fx,0,cx;
0,fy,cy;
0,0,1];

% 外参
%旋转矩阵
syms r11 r12 r13 r21 r22 r23 r31 r32 r33;
R = [r11,r12,r13;
r21,r22,r23;
r31,r32,r33];
%平移矩阵
syms t1 t2 t3;
t = [t1;
t2;
t3];
%外参矩阵
T = [R,t;
0,0,0,1];

% 图像坐标
syms u v;
imagePoint = [u;v;1];

% 计算深度
syms zConst;
rightMatrix = inv(R)*inv(M)*imagePoint;
leftMatrix = inv(R)*t;
s = (zConst + leftMatrix(3))/rightMatrix(3);

% 转换世界坐标方式一
worldPoint1 = inv(R) * (s*inv(M) * imagePoint - t);
simplify(worldPoint1)

% 转换世界坐标方式二
cameraPoint = inv(M)* imagePoint * s;% image->camrea
worldPoint2 = inv(T)* [cameraPoint;1];% camrea->world
worldPoint2 = [worldPoint2(1);worldPoint2(2);worldPoint2(3)];
simplify(worldPoint2)


### 3.2 C++

该程序参考《视觉SLAM十四讲》第九讲实践章：设计前端代码部分进行修改获得，去掉了李群库Sopuhus依赖，因该库在windows上调用较为麻烦，若在Linux建议采用Sopuhus

• camera.h
#ifndef CAMERA_H
#define CAMERA_H

#include <Eigen/Core>
#include <Eigen/Geometry>
using Eigen::Vector4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Quaterniond;
using Eigen::Matrix;

class Camera
{
public:
Camera();

// coordinate transform: world, camera, pixel
Vector3d world2camera( const Vector3d& p_w);
Vector3d camera2world( const Vector3d& p_c);
Vector2d camera2pixel( const Vector3d& p_c);
Vector3d pixel2camera( const Vector2d& p_p);
Vector3d pixel2world ( const Vector2d& p_p);
Vector2d world2pixel ( const Vector3d& p_w);

// set params
void setInternalParams(double fx, double cx, double fy, double cy);
void setExternalParams(Quaterniond Q, Vector3d t);
void setExternalParams(Matrix<double, 3, 3>  R, Vector3d t);

// cal depth
double calDepth(const Vector2d& p_p);

private:
// 内参
double fx_, fy_, cx_, cy_, depth_scale_;
Matrix<double, 3, 3> inMatrix_;

// 外参
Quaterniond Q_;
Matrix<double, 3, 3>  R_;
Vector3d t_;
Matrix<double, 4, 4> exMatrix_;
};

#endif // CAMERA_H

• camera.cpp
#include "camera.h"

Camera::Camera(){}

Vector3d Camera::world2camera ( const Vector3d& p_w)
{
Vector4d p_w_q{ p_w(0,0),p_w(1,0),p_w(2,0),1};
Vector4d p_c_q = exMatrix_ * p_w_q;
return Vector3d{p_c_q(0,0),p_c_q(1,0),p_c_q(2,0)};
}

Vector3d Camera::camera2world ( const Vector3d& p_c)
{
Vector4d p_c_q{ p_c(0,0),p_c(1,0),p_c(2,0),1 };
Vector4d p_w_q = exMatrix_.inverse() * p_c_q;
return Vector3d{ p_w_q(0,0),p_w_q(1,0),p_w_q(2,0) };
}

Vector2d Camera::camera2pixel ( const Vector3d& p_c )
{
return Vector2d (
fx_ * p_c ( 0,0 ) / p_c ( 2,0 ) + cx_,
fy_ * p_c ( 1,0 ) / p_c ( 2,0 ) + cy_
);
}

Vector3d Camera::pixel2camera ( const Vector2d& p_p)
{
double depth = calDepth(p_p);
return Vector3d (
( p_p ( 0,0 )-cx_ ) *depth/fx_,
( p_p ( 1,0 )-cy_ ) *depth/fy_,
depth
);
}

Vector2d Camera::world2pixel ( const Vector3d& p_w)
{
return camera2pixel ( world2camera(p_w) );
}

Vector3d Camera::pixel2world ( const Vector2d& p_p)
{
return camera2world ( pixel2camera ( p_p ));
}

double Camera::calDepth(const Vector2d& p_p)
{
Vector3d p_p_q{ p_p(0,0),p_p(1,0),1 };
Vector3d rightMatrix = R_.inverse() * inMatrix_.inverse() * p_p_q;
Vector3d leftMatrix = R_.inverse() * t_;
return leftMatrix(2,0)/rightMatrix(2,0);
}

void Camera::setInternalParams(double fx, double cx, double fy, double cy)
{
fx_ = fx;
cx_ = cx;
fy_ = fy;
cy_ = cy;

inMatrix_ << fx, 0, cx,
0, fy, cy,
0, 0, 1;
}

void Camera::setExternalParams(Quaterniond Q, Vector3d t)
{
Q_ = Q;
R_ = Q.normalized().toRotationMatrix();
setExternalParams(R_,t);
}

void Camera::setExternalParams(Matrix<double, 3, 3>  R, Vector3d t)
{
t_ = t;
R_ = R;

exMatrix_ << R_(0, 0), R_(0, 1), R_(0, 2), t(0,0),
R_(1, 0), R_(1, 1), R_(1, 2), t(1,0),
R_(2, 0), R_(2, 1), R_(2, 2), t(2,0),
0, 0, 0, 1;
}


## 参考

image coordinate to world coordinate opencv

Computing x,y coordinate (3D) from image point

单应矩阵

camera_calibration_and_3d

《视觉SLAM十四讲》—相机与图像+实践章：设计前端

展开全文
• 摘 要：摄像机标定是计算机视觉的一项基本任务。...图像平面；摄像机标定 1 引 言   摄像机作为一种视觉传感器使用越来越普遍，由于摄像机系统存在较大的成像误差，使用前要进行内参数的标定，以此提高成像精度
• 基于opencv的已知左右图像坐标求空间中三维坐标，以及已知空间坐标求其在左右图像中的二维坐标。
• 平移矩阵tvecs 根据coordinate_transform_formula.jpg可以推出从图像坐标世界坐标的转换公式 注：如果是使用张正友标定法，则world_point_z=0,相当于一个平面到另一个平面的投影，假设旋转矩阵R=[rx,ry,rz,t],...
• 详细介绍了摄像头坐标系，图像坐标系，世界坐标系的转换过程
• 本程序结合我的博客而仿真的，网址是：https://mp.csdn.net/postedit/82115829
• 而相机标定的输入就是相机所拍的多帧图片的角点坐标，以及标定板图像上所有角点的空间坐标(一般Z轴假设为Z=0)。相机标定后的输出就是相机的内外参数。 针对张正友标定相机的标定流程：1、获得多帧图片的角点坐标 2...
• 参考：1https://blog.csdn.net/chentravelling/article/details/53558096 2.https://blog.csdn.net/csxiaoshui/article/details/65446125 3.https://zhuanlan.zhihu.com/p/32030223 图像坐标系与世界坐...
• 直角坐标图像转换为极坐标
•  针对张正友标定相机的标定流程：1、获得多帧图片的角点坐标 2、获取标定板图像上所有角点的空间坐标 3、进行相机标定 4、对标定结果进行评价 5、再次利用标定板图像进行矫正 6、 图像坐标转世界坐标系 ...
• 目录一、各坐标系简要介绍二、坐标系转换2.1 世界坐标系转为相机坐标系2.2 相机坐标系转为物理图像坐标系2.3 物理图像坐标系转为图像坐标系2.4 总结2.5 为什么要使用齐次坐标参考文献 一、各坐标系简要介绍 首先...
• 实例1：计算二值分割图像的dice相似系数 实例2：计算多区域分割图像的dice相似系数
• set up 一个用于存放py源码(.py)、待转换三维坐标(.txt)__ps： 格式为（Xw，Yw，Zw）、转换二维坐标结果(.txt)的文件夹，大概就是像下边图片中的一样 随便一个可以run python文件的IDE 源码如下（需要更改自己...
• 世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换 图像处理、立体视觉等方向常常涉及到四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。例如下图： 构建世界坐标系只是为了更好的描述...
• 只能求取已知外参的世界坐标平面上的世界坐标，具体公式如图片所示！ PS：字丑请谅解！
• 世界坐标、相机坐标、图像坐标、像素坐标的原理、关系，并用matlab仿真 照相机是日常生活中最常见的。它能把三维的空间图片等比例缩小投影在照片上，称为一个二维图像。 以下我们就讲一讲原理，并相应的进行matlab...
• 原理下图表示了小孔成像模型(图片及公式参考opencv官方资料)这个图里涉及4个坐标系：世界坐标系：其坐标原点可视情况而定，可以表示空间的物体，单位为长度单位，比如mm，用矩阵表示；...图像物理坐标系(也叫...
• 新学期第一天开始写的这篇文章，看看我啥时候能把他发...相机焦距f，相机之间距离为b，右上角为目标位置P（x,y），目标的水平坐标为x，相机离目标垂直距离（所求目标距离相机的深度）为z。 如果要计算深度z，必须.
• 世界坐标系到成像坐标系的映射关系 透镜模型 近似关系（相似三角形） 新的改变 我们对Markdown编辑器进行了一些功能拓展与语法支持，除了标准的Markdown编辑器功能，我们增加了如下几点新功能，帮助你用它写博客： ...
• 关于opencv相机标定，包括标定完后，世界坐标到 图像坐标的转换，以评估图像的标定误差，这些网上有很多资源和源代码。 可是，相机标定完之后，我们想要的是，知道了图像坐标，想要得到它的世界坐标，或者我们已知...
• ================================================ ...================================================ 转换原理在文末 d_x和d_y表示1个像素有多少长度，即用传感器的尺寸除以像素数量 f表示焦距 ...
• 基于matlab，对拍摄图像进行三角形、方形、圆形块的识别，并且利用世界坐标变换，将图像上的坐标转换到真实世界中，并进行误差分析。
• 这个过程中会涉及到四个坐标系，即世界坐标系、相机坐标系、图像坐标系、像素坐标系。下面将详细介绍四个坐标系的函已并且记录其的转换过程。 图像中所涉及的四个坐标系及其含义： Ow - Xw - Yw - Zw ：世界坐标...
• 图像坐标系->像素坐标系一系列变换． 世界坐标系->相机坐标系 世界坐标系下的点可以通过旋转和平移转到相机坐标系下． 其中T的定义： 也就是我们说的相机的外参矩阵，OpenCV中可以通过SolvePnP等方法算...

...