雷达和RGB相机对齐的核心代码crop

代码来源：

https://github.com/agarwa65/lidar_camera_calibration

最核心的只是把校准函数calibrate的核心crop，去理解整个过程，其实和结构光相机与RGB相机对齐，没有区别

import numpy
import math
#from tf.transformations import euler_from_matrix #原函数
# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
# epsilon for testing whether a number is close to zero
_EPS = numpy.finfo(float).eps * 4.0
# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())def euler_from_matrix(matrix, axes='sxyz'):"""Return Euler angles from rotation matrix for specified axis sequence.axes : One of 24 axis sequences as string or encoded tupleNote that many Euler angle triplets can describe one matrix.>>> R0 = euler_matrix(1, 2, 3, 'syxz')>>> al, be, ga = euler_from_matrix(R0, 'syxz')>>> R1 = euler_matrix(al, be, ga, 'syxz')>>> numpy.allclose(R0, R1)True>>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)>>> for axes in _AXES2TUPLE.keys():...    R0 = euler_matrix(axes=axes, *angles)...    R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))...    if not numpy.allclose(R0, R1): print axes, "failed""""try:firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]except (AttributeError, KeyError):_ = _TUPLE2AXES[axes]firstaxis, parity, repetition, frame = axesi = firstaxisj = _NEXT_AXIS[i+parity]k = _NEXT_AXIS[i-parity+1]M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3]if repetition:sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])if sy > _EPS:ax = math.atan2( M[i, j],  M[i, k])ay = math.atan2( sy,       M[i, i])az = math.atan2( M[j, i], -M[k, i])else:ax = math.atan2(-M[j, k],  M[j, j])ay = math.atan2( sy,       M[i, i])az = 0.0else:cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])if cy > _EPS:ax = math.atan2( M[k, j],  M[k, k])ay = math.atan2(-M[k, i],  cy)az = math.atan2( M[j, i],  M[i, i])else:ax = math.atan2(-M[j, k],  M[j, j])ay = math.atan2(-M[k, i],  cy)az = 0.0if parity:ax, ay, az = -ax, -ay, -azif frame:ax, az = az, axreturn ax, ay, az# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------import cv2
import numpy as np
import os
import sysimport matplotlib.cm
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3Dfrom sensor_msgs.msg import Image, CameraInfo, PointCloud2
import image_geometry
CAMERA_MODEL = image_geometry.PinholeCameraModel()
camera_info = CameraInfo()
camera_info.height = 724
camera_info.width = 964
camera_info.K = [481.228482, 0.000000, 456.782531, \0.000000, 481.158298, 364.412635, \0.000000, 0.000000, 1.000000]camera_info.distortion_model = 'plumb_bob'
camera_info.D = [-0.195875, 0.065588, 0.003400, 0.000218, 0.000000]camera_info.R = [1.000000, 0.000000, 0.000000, \0.000000, 1.000000, 0.000000, \0.000000, 0.000000, 1.000000]camera_info.P = [422.024750, 0.000000, 460.332694, \0.000000, 0.000000, 429.271149, \368.531509, 0.000000, 0.000000, \0.000000, 1.000000, 0.000000]CAMERA_MODEL.fromCameraInfo(camera_info)# Rectify image
# CAMERA_MODEL.rectifyImage(img, img)# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# 校准过程calibratedef calibrate(points2D=None, points3D=None):'''Calibrate the 3D and image points using OpenCV PnP RANSACRequires minimum 5 point correspondences相关性 or Inputs:points2D - [numpy array] - (N, 2) array of image pointspoints3D - [numpy array] - (N, 3) array of 3D points'''# Check points shapeassert(points2D.shape[0] == points3D.shape[0])if not (points2D.shape[0] >= 5):print('PnP RANSAC Requires minimum 5 points')return# Obtain camera matrix and distortion coefficientscamera_matrix = CAMERA_MODEL.intrinsicMatrix()dist_coeffs = CAMERA_MODEL.distortionCoeffs()# Estimate extrinsicssuccess, rotation_vector, translation_vector, inliers = cv2.solvePnPRansac(points3D, points2D, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)# Compute re-projection error.points2D_reproj = cv2.projectPoints(points3D, rotation_vector,translation_vector, camera_matrix, dist_coeffs)[0].squeeze(1)assert(points2D_reproj.shape == points2D.shape)error = (points2D_reproj - points2D)[inliers]  # Compute error only over inliers.rmse = np.sqrt(np.mean(error[:, 0] ** 2 + error[:, 1] ** 2))print('Re-projection error before LM refinement (RMSE) in px: ' + str(rmse))# Refine estimate using LMif not success:print('Initial estimation unsuccessful, skipping refinement')elif not hasattr(cv2, 'solvePnPRefineLM'):print('solvePnPRefineLM requires OpenCV >= 4.1.1, skipping refinement')else:assert len(inliers) >= 3, 'LM refinement requires at least 3 inlier points'rotation_vector, translation_vector = cv2.solvePnPRefineLM(points3D[inliers],points2D[inliers], camera_matrix, dist_coeffs, rotation_vector, translation_vector)# Compute re-projection error.points2D_reproj = cv2.projectPoints(points3D, rotation_vector,translation_vector, camera_matrix, dist_coeffs)[0].squeeze(1)assert(points2D_reproj.shape == points2D.shape)error = (points2D_reproj - points2D)[inliers]  # Compute error only over inliers.rmse = np.sqrt(np.mean(error[:, 0] ** 2 + error[:, 1] ** 2))print('Re-projection error after LM refinement (RMSE) in px: ' + str(rmse))# Convert rotation vectorrotation_matrix = cv2.Rodrigues(rotation_vector)[0]euler = euler_from_matrix(rotation_matrix)# Save extrinsicsnp.savez(os.path.join("./result", 'extrinsics.npz'),euler=euler, R=rotation_matrix, T=translation_vector.T)# Display resultsprint('Euler angles (RPY):', euler)print('Rotation Matrix:', rotation_matrix)print('Translation Offsets:', translation_vector.T)# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# ----------------------------分界线----------------------------
# 这个是将雷达3D点通过外参矩阵旋转到标定相机的2D坐标系下投影后形成2D图像def project_point_cloud(points3D, img, config):'''Projects the point cloud on to the image plane using the extrinsicspoints3D - [numpy array] - (N, 3) array of 3D pointsimg - [numpy array] - (H, W， 3) array of 2D imageconfig 内部有外参，即[RT]'''rt_points3D = np.dot(points3D, config.T)rt_points3D = np.asarray(rt_points3D.tolist())# Group all beams together and pick the first 4 columns for X, Y, Z, intensity.OUSTER_LIDAR = Falseif OUSTER_LIDAR: points3D = points3D.reshape(-1, 9)[:, :4]# Filter points in front of camerainrange = np.where((points3D[:, 2] > 0) &(points3D[:, 2] < 6) &(np.abs(points3D[:, 0]) < 6) &(np.abs(points3D[:, 1]) < 6))max_intensity = np.max(points3D[:, -1])points3D = points3D[inrange[0]]# Color map for the pointscmap = matplotlib.cm.get_cmap('jet')colors = cmap(points3D[:, -1] / max_intensity) * 255# Project to 2D and filter points within image boundariespoints2D = [ CAMERA_MODEL.project3dToPixel(point) for point in points3D[:, :3] ]points2D = np.asarray(points2D)inrange = np.where((points2D[:, 0] >= 0) &(points2D[:, 1] >= 0) &(points2D[:, 0] < img.shape[1]) &(points2D[:, 1] < img.shape[0]))points2D = points2D[inrange[0]].round().astype('int')# Draw the projected 2D pointsfor i in range(len(points2D)):cv2.circle(img, tuple(points2D[i]), 2, tuple(colors[i]), -1)

雷达和RGB相机对齐的核心代码crop相关推荐

Unity学习日志_七行核心代码实现第三人称游戏的相机逻辑
七行核心代码实现第三人称游戏的相机逻辑: 使用到的一些知识: 欧拉角虚拟轴四元数计算向量计算代码实现: 其中RoundMovement方法为核心方法(): using UnityEngine; ...
图像超分综述:超长文一网打尽图像超分的前世今生 (附核心代码)
文章目录一.目的二.研究背景三.存在的问题四.研究现状五.各算法创新点及核心代码总结 SRCNN ESPCN VDSR DRCN DRRN EDSR SRGAN ESRGAN RDN WDS ...
第13章程序的动态加载和执行（三，核心代码）
这个核心代码也是本书唯一的一个核心代码,把这个读懂了,本书基本上通了,这个核心代码不难,只是前面知识的综合应用而已,所以用一到两个星期把这个三个程序读熟再进行下面的四章. 怎么样才算是读通了一个代码: ...
鱼眼图像自监督深度估计原理分析和Omnidet核心代码解读
作者丨苹果姐@知乎来源丨https://zhuanlan.zhihu.com/p/508090405 编辑丨3D视觉工坊在自动驾驶实际应用中,对相机传感器的要求之一是拥有尽可能大的视野范围,鱼眼相 ...
TFLite Objec Detection IOS 检测核心代码说明
文章目录 TFLite Objec Detection IOS 检测核心代码说明简要说明 Object Detection 执行 object detection 所使用的模型代码分析 Model ...
奥比中光相机的python采集代码
奥比中光相机的python采集代码前言一.驱动安装二.配置openni 三.采集代码前言在Windows系统下,使用python语言,采集奥比中光相机的拍摄的图像. 一.驱动安装在奥比中光 ...
Apollo control模块纵向控制原理及核心代码逐行解析
前言 2021/12/30 前段时间一直在看Apollo的控制代码,因为工作较忙,只能抽时间整理下代码笔记,可能稍显粗糙,部分图片手绘,作为日后调试之参照.以后有时间再优化排版,再把涉及到的其他概念补 ...
使用c++/winrt API获取RGB相机视频流
使用c++/winrt API获取RGB相机视频流 1.前提条件该示例使用c++/winrt进行开发,需要编译器支持c++17,本人使用Visual Studio2017,系统版本为Windows1 ...
使用二维激光雷达和cartographer_ros实现实时SLAM
在前面已经完成了cartographer_ros的安装和demo的运行了.接下来,就要放到机器人上,实时进行SLAM了. 前一篇内容的链接如下: Cartographer_ros的下载.配置及编译与问 ...

雷达和RGB相机对齐的核心代码crop

雷达和RGB相机对齐的核心代码crop相关推荐

最新文章

热门文章