基于D435i的点云重建

Task: 采用D435i采集深度图和RGB图像，进行点云重建和聚类。

1）解析Bag数据：

import os
import cv2
import numpy as np
import rosbag
from cv_bridge import CvBridge
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2import pcl
from pcl import pcl_visualization
import pyrealsense2 as rsimport numpy as np
from sklearn.cluster import KMeans
from sklearn.preprocessing import StandardScaler
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from skimage.segmentation import chan_vese
import time
import copyif __name__ == "__main__":   visual = pcl_visualization.CloudViewing()pc = rs.pointcloud()points = rs.points()bag = rosbag.Bag(file_path, "r")bag_data = bag.read_messages()info = bag.get_type_and_topic_info()print(info)bridge = CvBridge()depth = None; points_tp = None; cv_img10 = Nonecap = cv2.VideoWriter(video_output, cv2.VideoWriter_fourcc(*'MP4V'), 20.0, (1280*2+3, 720))nn = 0for topic, msg, t in bag_data:print(topic)if topic == "/cam_1/aligned_depth_to_color/image_raw/compressed":depth = bridge.compressed_imgmsg_to_cv2(msg)show_img(depth, "Depth Image")elif topic == "/cam_1/color/image_raw/compressed":cv_img = bridge.compressed_imgmsg_to_cv2(msg)show_img(cv_img, "Cam1")elif topic == "/cam_2/aligned_depth_to_color/image_raw/compressed":depth_10 = bridge.compressed_imgmsg_to_cv2(msg)elif topic == "/cam_2/color/image_raw/compressed":cv_img10 = bridge.compressed_imgmsg_to_cv2(msg)show_img(cv_img10, "Cam2")seg_color, add_image = visual_pcl(visual, depth_10, cv_img10)save_seg_video(cap, cv_img10, add_image)# if nn == 2499:    # 保存某一帧的数据和结果#     seg, seg_color, add_image = visual_pcl(visual, depth_10, cv_img10)# #     cv2.imwrite("./save_data/test_rgb.png", cv_img10)# #     cv2.imwrite("./save_data/test_depth.png", depth_10)# #     xyz_sand = depth2xyz(depth_10, depth_camera_metrix)# #     xyz_sand = xyz_sand.reshape(-1, 3)# #     with open("./save_data/points.txt", "w") as f:# #         for i in range(len(xyz_sand)):# #             strings = str(xyz_sand[i][0]) + "," + str(xyz_sand[i][1]) + "," + str(xyz_sand[i][2]) +"\n"# #             f.write(strings)elif topic == "/cam_3/color/image_raw/compressed":cv_img = bridge.compressed_imgmsg_to_cv2(msg)show_img(cv_img, "Cam3")nn = nn + 1print(nn)cap.release()print("end")

2）分割图像：

# 图像显示
def show_img(img, window_name):if SHOW_IMAGE:cv2.imshow(window_name, img)cv2.waitKey(3)# 寻找最大连通域
def find_max_region(mask_sel):contours, hierarchy = cv2.findContours(mask_sel,cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)#找到最大区域并填充 area = []for j in range(len(contours)):area.append(cv2.contourArea(contours[j]))max_idx = np.argmax(area)for k in range(len(contours)):if k != max_idx:cv2.fillPoly(mask_sel, [contours[k]], 0)# elif k == max_idx:#     cv2.fillPoly(mask_sel, [contours[k]], 255)return mask_sel# 填充孔洞
def FillHole(img):# 复制 im_in 图像im_floodfill = img.copy()# Mask 用于 floodFill，官方要求长宽+2h, w = img.shape[:2]mask = np.zeros((h+2, w+2), np.uint8)# floodFill函数中的seedPoint必须是背景isbreak = FalseseedPoint = Nonefor i in range(im_floodfill.shape[0] - 2):for j in range(im_floodfill.shape[1] - 2):if(im_floodfill[i][j]==0):seedPoint=(i,j)isbreak = Truebreakif(isbreak):break# 得到im_floodfilltry:if seedPoint is not None:cv2.floodFill(im_floodfill, mask, seedPoint, 255)# 得到im_floodfill的逆im_floodfill_invim_floodfill_inv = cv2.bitwise_not(im_floodfill)# 把im_in、im_floodfill_inv这两幅图像结合起来得到前景im_out = img | im_floodfill_invreturn im_outelse:return imgexcept:return imgdef segmentation_rgb(img):origin_img = imgshow_img(origin_img, "Origin R image")img = cv2.GaussianBlur(img, (5, 5), 1)h, w, c = img.shaperet3, segr = cv2.threshold(img[:, :, 2], 0, 255, cv2.THRESH_OTSU)show_img(segr, "Origin Seg R")kernel = np.ones((5, 5), np.uint8)segr = cv2.morphologyEx(segr, cv2.MORPH_OPEN, kernel, iterations=2)show_img(segr, "Seg R Open Operation")segr = cv2.dilate(segr, kernel, iterations=3)show_img(segr, "Seg R Dilate Operation")segr = FillHole(segr)show_img(segr, "Seg R Fill Hole")img[:, :, 2][segr > 0] = 0# 再次分割ret3, segrnew = cv2.threshold(img[:, :, 2], 0, 255, cv2.THRESH_OTSU)segrnew[segr == 255] = 0show_img(segrnew, "Re-seg R SEG")show_img(img[:, :, 0], "B")show_img(img[:, :, 2], "R")show_img(img[:, :, 1], "G")segr = find_max_region(segrnew)mm = segr[190:, :]tp = FillHole(mm)segr[190:, :] = tpshow_img(segr, "Re R SEG - find max region and fill hole")add_image = show_mask_on_image(origin_img, segr)dp_car = copy.copy(depth)dp_sand = copy.copy(depth)dp_car[segr == 255] = 0dp_sand[segr == 0] = 0show_img(dp_car, "Depth - Car")show_img(dp_sand, "Depth - Sand")return segr, add_image

3）深度图计算点云图：

# 由深度图，计算点云图
def depth2xyz(depth_map,depth_cam_matrix,flatten=False,depth_scale=1000):fx,fy = depth_cam_matrix[0,0],depth_cam_matrix[1,1]cx,cy = depth_cam_matrix[0,2],depth_cam_matrix[1,2]h,w=np.mgrid[0:depth_map.shape[0],0:depth_map.shape[1]]z=depth_map/depth_scalex=(w-cx)*z/fxy=(h-cy)*z/fyxyz=np.dstack((x,y,z)) if flatten==False else np.dstack((x,y,z)).reshape(-1,3)return xyz

4）点云重建和可视化：

def visual_pcl(visual, depth, color):t1 = time.time()seg_color, add_image = segmentation_rgb(color)print("Segmentation time: ", time.time() - t1, "s")if GET_POINT:seg_color[seg_color == 0] = 20seg_color = cv2.cvtColor(seg_color, cv2.COLOR_GRAY2BGR)depth_sand = deptht2 = time.time()xyz_sand = depth2xyz(depth_sand, depth_camera_metrix)print("Depth to point cloud time: ", time.time() - t2, "s")xyz_sand = xyz_sand.reshape(-1, 3)seg_color = seg_color.reshape(-1,3)color = color.reshape(-1,3)# seg_color[seg_color > 0] = 255    # 聚类前结果，修改颜色值即可# kmeans(xyz_sand, xyz_vehicle)     # 采用kmeans方法进行聚类cloud = pcl.PointCloud_PointXYZRGB()temp = np.zeros((len(xyz_sand), 4), np.float32)length = len(xyz_sand)for i in range(length):temp[i][0] = xyz_sand[i][0] * 1000.0temp[i][1] = xyz_sand[i][1] * 1000.0temp[i][2] = xyz_sand[i][2] * 1000.0# temp[i][3] = 65536.0 * color[i][2] + 256.0*color[i][1] + color[i][0]      # 用原图像的颜色信息渲染点云temp[i][3] = 65536.0 * seg_color[i][2] + 256.0*seg_color[i][1] + seg_color[i][0]cloud.from_array(temp)visual.ShowColorCloud(cloud, b"color")return seg_color, add_image

结果：

基于D435i的点云重建相关推荐

基于matlab的点云重建与三角剖分
稠密点云重建MVS——基于多视角深度图像
稠密点云重建MVS--基于多视角深度图像前言一.整体流程* 二.算法原理 1.全局视角图像序列选取方法 2.局部视角图像序列选取方法 2.深度与法向量优化算法基本概念极线搜索--查找图像中某个 ...
invalid floating point operation什么意思_Point-MVSNet：基于多视角的点云重建网络
Point-Based Multi-View Stereo Network是一篇点云重建领域的文章,其工作内容为通过输入多张不同角度的图片,提取不同的点云特征,再进行融合,从而生成最终的点云. 1.介 ...
【点云重采样Resampling】Python-pcl 基于多项式平滑点云及法线估计的曲面重建
1. 点云重采样基于多项式平滑点云及法线估计的曲面重建以实现重采样,可以使得点云数据更规整一些,没之前那么杂乱. set_Compute_Normals(True) 可以通过在最小二乘法中进行法线估 ...
基于正交投影的点云局部特征描述详解
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本次介绍一个发表于Pattern Recognition的经典三维点云描述子TOLDI,首先进行算法阐 ...
【三维深度学习】多视角场景点云重建模型PointMVS
PointMVS区别于先前利用cost volum来进行多视角处理的方法,直接基于点云的方式对场景进行处理.从过由粗到细的迭代优化过程,充分结合了几何先验信息和2D纹理信息来来增强点云特征,并利用图网 ...
2015-2020年各类国际会议与期刊基于图像的三维对象重建论文综述（7）——Datasets
7. Datasets 下面是一些目前主流的用于训练和评估基于深度学习的三维重建算法性能的数据集. "IMG":图像. "obj":objects. " ...
图形处理（八）点云重建（上）点云滤波、尖锐特征边增采样、移除离群点
之前帮导师搞过一个项目,涉及点云尖锐的特征边重建技术,很多文献看起来效果很好,然而都是坑爹的算法,鲁邦性很差,比如这篇paper<Feature Sensitive Surface Extrac ...
图形处理（九）点云重建（下）法矢求取、有向距离场等值面提取
一.相关理论在点云重建算法中,点云法矢的求取是非常重要的一步.之前导师让我做点云尖锐特征边重建时,发了一堆异向法矢求取的英文paper,当时我很迷糊,就问了老师,让我做点云重建,为什么发的文献资料都 ...

基于D435i的点云重建

基于D435i的点云重建相关推荐

最新文章

热门文章