defproj_to_2d(k,pt_3d):## project 3d to 2d using the camera's intrisic matrix## [u,v,1] = (1/z)*(k*[x,y,z]^T) k = np.array(k).reshape((3,3)) px = np.dot(k, pt_3d.reshape((3,1))) px = (int(px[0,0]/pt_3d[2]),int(px[1,0]/pt_3d[2]))return px
import open3d as o3dself.pcd = o3d.geometry.PointCloud()defimg_callback(self,data):if self.is_k_empty ==False: self.height = data.height self.width = data.width np_cloud = np.zeros((self.height*self.width,3))# print(self.k)for iy inrange(self.height):for ix inrange(self.width): idx = iy*self.width+ix z = (data.data[idx*2+1]*256+data.data[idx*2])/1000.0if z!=0:## x, y are on the camera plane, z is the depth#np_cloud[idx][0] = z*(ix-self.k[2])/self.k[0] #x#np_cloud[idx][1] = z*(iy-self.k[5])/self.k[4] #y#np_cloud[idx][2] = z## same coordinate as `/camera/depth/image_rect_raw`## y (left & right), z (up & down) are on the camera plane, x is the depth np_cloud[idx][1] =-z*(ix-self.k[2])/self.k[0] np_cloud[idx][2] =-z*(iy-self.k[5])/self.k[4] np_cloud[idx][0] = z self.pcd.points = o3d.utility.Vector3dVector(np_cloud)## publish as a ROS message# header = Header()# header.stamp = rospy.Time.now()# header.frame_id = "camera_depth_frame"# fields = self.FIELDS_XYZ# pc2_data = pc2.create_cloud(header, fields, np.asarray(np_cloud))# self.pub.publish(pc2_data) self.is_data_updated =True