点云_矩阵旋转后平移和平移后旋转_open3d读写PCD

矩阵运算

 变换矩阵是先旋转再平移
      P = R*p + t1
 先平移再旋转的话
      P= R(p+t2)=R*p +R*t2 
其中平移矩阵之间的关系是 t1 = R*t2

示例代码

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

import numpy as np
from time import time
  
if __name__ == "__main__":
    t_s = time()
    ##calibration
    R_calib_rad = [0,0,-1.16]
    T_calib = [9.114,10.93, -7.9]
    base_orig_point =np.array( 
                 [[ -32.2852 ,-1901.231 ,7.5923],[-32.9883 , -1901.567,7.0294],
                  [ -10.2388 ,-1901.685 ,7.0294],[-35.9165 , -1905.568,2.304 ],
                  [ -27.016 , -1902.966 , 2.7729 ]])   
    ## RT矩阵
    rotation_angle = (np.array(R_calib_rad)).tolist()
    pitch, roll, yaw =  tuple(rotation_angle)
    pitch_R = np.array([
            [np.cos(pitch), 0,  np.sin(pitch)],[0, 1,0],[-np.sin(pitch),0,  np.cos(pitch)]
        ])
    roll_R = np.array([
            [1,0,0],[0,np.cos(roll),  -np.sin(roll)],[0,np.sin(roll),   np.cos(roll)]
        ])
    yaw_R = np.array([
            [np.cos(yaw),  -np.sin(yaw), 0],[np.sin(yaw),  np.cos(yaw), 0],[0,0,1]
        ])
    ################method a  先平移再旋转
    rotation_matrix = np.matmul(pitch_R, np.matmul(roll_R, yaw_R))
    translate_matrix_a = np.hstack([np.eye(3),np.array(T_calib).reshape(-1,1)])
    translate_matrix = np.vstack([translate_matrix_a, [0,0,0,1]])

    A_orig_points = np.zeros((base_orig_point.shape[0],4))
    data_mid = np.vstack([base_orig_point[:,:3].T, np.ones((base_orig_point.shape[0]))])
    data_trans = np.matmul(translate_matrix, data_mid).T
    A_orig_points[:, :3] = data_trans[:, :3]
    data_trans = np.matmul(rotation_matrix, A_orig_points[:,:3].T).T
    A_orig_points[:, :3] = data_trans
    print(A_orig_points)

    ################method b   先旋转再平移 变换矩阵=先旋转再平移
    rotation_matrix = np.matmul(pitch_R, np.matmul(roll_R, yaw_R))
    translate_matrix_b = np.array(T_calib).reshape(-1,1)
	### t1 = R*t2
    rot_translate_matrix = np.dot(rotation_matrix,translate_matrix_b)
    print(rot_translate_matrix)
    TR_matrix = np.eye(4,dtype=np.float32)
    TR_matrix[:3,:3] = rotation_matrix
	## 注意数据类型
    TR_matrix[:3,3]= rot_translate_matrix[:,0]
    print(TR_matrix)
    # # # """points transform  """
    orig_points = np.zeros((base_orig_point.shape[0],4))
    orig_points[:,:3] = base_orig_point
    orig_points[:,3]=np.ones((base_orig_point.shape[0]))
    new_xyz_array = np.dot(TR_matrix, orig_points.T).T
    print(new_xyz_array)

读写PCD

1.open3d

open3d.io 
      open3d.io.read_point_cloud(filename[, format, …])  Function to read PointCloud from file
          open3d.cpu.pybind.geometry.PointCloud
		  
      class  open3d.geometry.PointCloud
open3d.t
    geometry  Tensor-based geometry defining module.
    io         Tensor-based input-output handling module.
	      open3d.cpu.pybind.t.geometry.PointCloud 
    pipelines Tensor-based geometry processing pipelines
	
	class  open3d.t.geometry.PointCloud
	         # Default attribute: "positions".
		   open3d.t.geometry.Geometry	

2.读写示例

##cloud point
pcd = o3d.t.io.read_point_cloud(exp_pcdfile)
exp_points = pcd.point
point_xyz = exp_points['positions'].numpy()
intensity = exp_points['intensity']

# ## trans and save
transformed_points = transform_TR(point_xyz,T_matrix,R_matrix)
result_pcd = o3d.t.geometry.PointCloud()
result_pcd.point['positions'] = o3d.core.Tensor(transformed_points[:, :3],dtype=o3d.core.Dtype.Float32)
result_pcd.point['intensity'] = o3d.core.Tensor(intensity.numpy(),dtype=o3d.core.Dtype.Float32)
o3d.t.io.write_point_cloud( save_file, result_pcd, write_ascii=False)

参考

http://www.open3d.org/docs/release/index.html

原文地址：http://www.cnblogs.com/ytwang/p/16870672.html