opencv - 激光雷达到图像投影问题
问题描述
我有如下所示的 3D 点，
objpts = np.float32([[-24.6608814740376,7.77957823997484,-0.3467245],
[-24.6472521660611,5.72834451906318,-0.3467245],
[-18.4698745259624, 5.76938976002516,-0.3467245],
[-18.483503833939, 7.82062348093682,-0.3467245],
[-24.6608814740376, 7.77957823997484, 2.4066965],
[-24.6472521660611, 5.72834451906318, 2.4066965],
[-18.4698745259624, 5.76938976002516, 2.4066965],
[-18.483503833939, 7.82062348093682, 2.4066965]])
JSON文件如下所示，
"camera_06":{ "extrinsics":{ "yaw_deg":-170.00001556208605, "R":[[-0.98851285476551365, 0.15101692369437689, -0.006018697634966764], [-0.15101790740705864, -0.98853098363718561, -0.00029331159425322094], [-0.0059939641079565127, 0.00061898884077741807, 0.999981844458732 ]], "x_m":-9.282084, "y_m":1.255527, "z_m":0.457751 }, "intrinsics":{ "intrinsicMatrix":{ "focalLengthX":968.749578, "focalLengthY":969.750192, "principalPointX":965.512978 , "principalPointY":651.582338 }, "intrinsicDistortion":{ "k1":-0.054333, "k2":0.025711, "k3":-0.027771, "k4":0.01073 } } },
以下是获取投影点的python代码。
import numpy as np
import cv2
objpts = np.float32([[-24.6608814740376,7.77957823997484,-0.3467245],
[-24.6472521660611,5.72834451906318,-0.3467245],
[-18.4698745259624, 5.76938976002516,-0.3467245],
[-18.483503833939, 7.82062348093682,-0.3467245],
[-24.6608814740376, 7.77957823997484, 2.4066965],
[-24.6472521660611, 5.72834451906318, 2.4066965],
[-18.4698745259624, 5.76938976002516, 2.4066965],
[-18.483503833939, 7.82062348093682, 2.4066965]])
obj = np.zeros((4, 1), np.float32)
obj[0] = -24.6608814740376,
obj[1] = 7.77957823997484,
obj[2] = -0.3467245,
obj[3] = 1
rmat = np.float32([[-0.98851285476551365, 0.15101692369437689, -0.006018697634966764 ],
[-0.15101790740705864, -0.98853098363718561, -0.00029331159425322094],
[-0.0059939641079565127, 0.00061898884077741807, 0.999981844458732],
[0, 0, 0]])
tVec = np.zeros((4, 1), np.float32)
tVec[0] = -9.282084
tVec[1] = 1.255527
tVec[2] = 0.457751
tVec[3] = 1
extrinsicMatrix = np.float32([[-0.98851285476551365, 0.15101692369437689,
-0.006018697634966764,-9.282084 ],
[-0.15101790740705864, -0.98853098363718561,
-0.00029331159425322094,1.255527],
[-0.0059939641079565127, 0.00061898884077741807,
0.999981844458732,0.457751],
[0, 0, 0 , 1]])
cx = 965.512978;
cy = 651.582338;
fx = 968.749578;
fy = 969.750192;
intrinsicMat = np.array([[fx, 0, cx, 0],[0,fy,cy, 0],[0,0,1, 0]])
rVec, jacb = cv2.Rodrigues(rmat)
print(tVec)
[[-9.282084]
[ 1.255527]
[ 0.457751]]
distCoeffs = np.zeros((4, 1), np.float32)
imagePoints = cv2.projectPoints(objpts, rVec, tVec, intrinsicMat, distCoeffs);
print(imagePoints)
(array([[[ 60753.293 , -9317.519 ]],
[[ 59867.54 , -1880.2196 ]],
[[ 43312.184 , -6485.8486 ]],
[[ 44441.234 , -15109.788 ]],
[[ 6185.1636 , -219.89659]],
[[ 6083.6655 , 431.09888]],
[[ 4163.2847 , 111.43414]],
[[ 4266.914 , -547.49945]]], dtype=float32), array([[-3.80793055e+06,
8.90772015e+05, 7.55035231e+02,
3.67417254e+03, 0.00000000e+00, -2.26756875e+05,
6.17164470e+01, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 2.34045232e+08, 9.16193393e+11,
-1.22924529e+06, 1.11720457e+07],
[ 6.34783692e+05, -1.47858683e+05, 9.62898830e+04,
0.00000000e+00, 3.67796756e+03, 3.78097678e+04,
0.00000000e+00, -1.02800710e+01, 0.00000000e+00,
1.00000000e+00, -3.90250391e+07, -1.52767406e+11,
4.00114993e+06, -1.23051497e+06],
[-3.74551768e+06, 5.75908217e+05, -5.04673084e+03,
3.69310101e+03, 0.00000000e+00, -2.24548369e+05,
6.08021196e+01, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 2.18156259e+08, 8.07988370e+11,
-3.07560176e+05, 1.07507075e+07],
[ 1.60788442e+05, -2.40801978e+04, 9.38000292e+04,
0.00000000e+00, 3.69691559e+03, 9.65182400e+03,
0.00000000e+00, -2.61077749e+00, 0.00000000e+00,
1.00000000e+00, -9.37707017e+06, -3.47299852e+10,
3.60489721e+06, -3.07877853e+05],
[-2.36300974e+06, 5.45800269e+05, 3.98631375e+03,
4.29959645e+03, 0.00000000e+00, -1.87947012e+05,
4.37127098e+01, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 8.32100044e+07, 1.63505300e+11,
-6.23349025e+05, 5.60574078e+06],
[ 3.98204652e+05, -9.11602910e+04, 8.37772535e+04,
0.00000000e+00, 4.30403747e+03, 3.16780235e+04,
0.00000000e+00, -7.36007149e+00, 0.00000000e+00,
1.00000000e+00, -1.40248491e+07, -2.75584308e+10,
2.01059564e+06, -6.23992878e+05],
[-2.43319999e+06, 8.18560750e+05, 1.11383957e+04,
4.27396205e+03, 0.00000000e+00, -1.91807656e+05,
4.48781842e+01, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 9.90469174e+07, 2.25649889e+11,
-1.41322371e+06, 6.10923997e+06],
[ 8.82146932e+05, -2.95925952e+05, 8.68453563e+04,
0.00000000e+00, 4.27837659e+03, 6.95365476e+04,
0.00000000e+00, -1.62530217e+01, 0.00000000e+00,
1.00000000e+00, -3.59077464e+07, -8.18054636e+10,
2.72163646e+06, -1.41468341e+06],
[-2.95871793e+04, 6.83990210e+03, 1.16674830e+03,
3.21093169e+02, 0.00000000e+00, -1.73005929e+03,
5.38802895e+00, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 1.55746298e+05, 4.64722838e+06,
-9.38141741e+03, 8.51532464e+04],
[ 4.78341278e+03, -1.66458552e+03, 8.22846079e+03,
0.00000000e+00, 3.21424824e+02, 2.88852700e+02,
0.00000000e+00, -8.98663322e-01, 0.00000000e+00,
1.00000000e+00, -2.60035820e+04, -7.75906625e+05,
3.05021766e+04, -9.39110740e+03],
[-2.88422454e+04, 4.39527690e+03, 5.29719956e+02,
3.21237056e+02, 0.00000000e+00, -1.69717771e+03,
5.28325634e+00, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 1.43126523e+05, 4.00246031e+06,
-2.32733747e+03, 8.11716101e+04],
[ 1.15450508e+03, -7.17625500e+02, 8.11457637e+03,
0.00000000e+00, 3.21568860e+02, 7.31122489e+01,
0.00000000e+00, -2.27361098e-01, 0.00000000e+00,
1.00000000e+00, -6.16570787e+03, -1.72420880e+05,
2.72188288e+04, -2.32974136e+03],
[-1.40371509e+04, 3.16081321e+03, 8.97560816e+02,
3.25227503e+02, 0.00000000e+00, -1.07355229e+03,
3.30092712e+00, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 3.58353999e+04, 4.01584599e+05,
-3.56230018e+03, 3.19673851e+04],
[ 2.21954862e+03, -1.05942075e+03, 6.23356449e+03,
0.00000000e+00, 3.25563428e+02, 1.81337936e+02,
0.00000000e+00, -5.56997256e-01, 0.00000000e+00,
1.00000000e+00, -6.05309826e+03, -6.78332332e+04,
1.14690975e+04, -3.56597966e+03],
[-1.45946686e+04, 4.77229813e+03, 1.54681618e+03,
3.25080020e+02, 0.00000000e+00, -1.10784006e+03,
3.40789958e+00, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00, 4.33892448e+04, 5.70250754e+05,
-8.16426776e+03, 3.52336492e+04],
[ 5.04188059e+03, -2.24973413e+03, 6.34860447e+03,
0.00000000e+00, 3.25415793e+02, 4.02371817e+02,
0.00000000e+00, -1.23648522e+00, 0.00000000e+00,
1.00000000e+00, -1.57591424e+04, -2.07117290e+05,
1.57104056e+04, -8.17270057e+03]]))
由于我没有得到正确的投影点，请帮助我解决问题。或者有没有其他方法可以实现这一点。
解决方案
推荐阅读
- c# - Unity - 如何在运动的第一帧设置变量？
- python - 函数列表如何在 Python 中工作？
- spring - spring安全授权错误401未授权
- prolog - 序言 | 添加几个值
- javascript - 使用 for 将整数转换为 8 位二进制
- php - 在 add.php 页面中添加数据库后表单验证不起作用（来自 net ninja 的项目）
- algorithm - 如果在二叉搜索树中找不到节点
- c - C - 为计算分配大矩阵的最有效方法是什么（基于计算时的时间效率）？
- google-apps-script - 为每个联赛重复国家名称
- python - 在 zip/列表理解中使用 namedtuple