RGBD点云降采样

RGBD点云降采样

rgbd数据如果想远程实时来查看的话，其对带宽的要求是相当高的。
为了解决这个问题，想到将采样的方案。

从rgbd数据上将采样，要处理好多3d数据，没多想
从源头上着手比较简单和直接
两步走：
1、深度图、彩色图降采样（进行缩放）；
2、重新生成rgbd点云；

看下面疗效
#512,424
#170,140
#160,132
#128,106
#96,79
#85,70
#64,53
#32,26

下面是实现图片降采样（放缩功能）的节点，
上马：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# rospy for the subscriber
import rospy
# ROS Image message
from sensor_msgs.msg import Image
# ROS Image message -> OpenCV2 image converter
from cv_bridge import CvBridge, CvBridgeError
# OpenCV2 for saving an image
import cv2


import numpy as np
import rospy
import sys
from sensor_msgs.msg import CameraInfo
class CameraMatrixObj():
    def __init__(self,width=64,height=53):

        self.k1=self.k2=self.k3=0
        self.p1=self.p2=0
        self.dx=0
        self.dy=0
        self.interpolation=cv2.INTER_NEAREST#cv2.INTER_LINEAR#cv2.INTER_LINEAR#
        self.border_mode=cv2.BORDER_REFLECT_101
        self.value=None
        self.width=width
        self.height=height
        self.fx = self.width
        self.fy = self.height
        self.cx = self.width * 0.5 + self.dx
        self.cy = self.height * 0.5 + self.dy
        self.camera_matrix =None
        self.count_camera_matrix (self.fx,self.fy,self.cx,self.cy)
        self.distortion=None
        self.count_distortion (self.k1, self.k2, self.p1, self.p2, self.k3)
        self.map1 = None
        self.map2 = None
        self.count_map12 (self.camera_matrix)

    def count_camera_matrix (self,fx,fy,cx,cy):

        self.fx = fx
        self.fy = fy
        self.cx = cx
        self.cy = cy
        self.camera_matrix= np.array([[self.fx, 0, self.cx], [0, self.fy, self.cy], [0, 0, 1]], dtype=np.float32)


    def count_distortion (self,k1, k2, p1, p2, k3):
        self.distortion= np.array([k1, k2, p1, p2, k3], dtype=np.float32)   

    def count_map12 (self,camera_matrix_src):
        map1, map2= cv2.initUndistortRectifyMap(camera_matrix_src, self.distortion, None, self.camera_matrix, (self.width, self.height), cv2.CV_16SC2)
        self.map1=map1
        self.map2=map2
        
    def product_img (self,img_src):
        img = cv2.remap(img_src, self.map1, self.map2, interpolation=self.interpolation, borderMode=self.border_mode, borderValue=self.value)
        return img

'''
    k=0, dx=0, dy=0, interpolation=cv2.INTER_LINEAR, border_mode=cv2.BORDER_REFLECT_101, value=None
    fx = width
    fy = height

    cx = width * 0.5 + dx
    cy = height * 0.5 + dy

    camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float32)

    distortion = np.array([k, k, 0, 0, 0], dtype=np.float32)
    map1, map2 = cv2.initUndistortRectifyMap(camera_matrix, distortion, None, None, (width, height), cv2.CV_32FC1)
    img = cv2.remap(img, map1, map2, interpolation=interpolation, borderMode=border_mode, borderValue=value)
'''
class ImageProcess():
    def __init__(self,width=96,height=79,rate_hz=10,frame_id="kinect2_rgb_optical_frame",inputns="/kinect2/sd",outputns="/kinect2/mini"):
        self.image_output_size=(width,height)#170,140 #160,132 #128,106 #96,79  #85,70  #64,53 #32,26
        self.camera_mo_src=CameraMatrixObj(512,424)
        self.frame_id=frame_id
        self.camera_mo=CameraMatrixObj(self.image_output_size[0],self.image_output_size[1])
        self.camera_mo_isset=0
        # Instantiate CvBridge
        self.bridge = CvBridge()
        self.rate = rospy.Rate(rate_hz)
        #image_scale=0.2

        self.image_topic_color = inputns+"/image_color_rect"
        self.image_topic_depth = inputns+"/image_depth_rect"
        self.ci_topic = inputns+"/camera_info"
        self.image_pub_color = rospy.Publisher(outputns+"/image_color_rect",Image,queue_size=2)
        self.image_pub_depth = rospy.Publisher(outputns+"/image_depth_rect",Image,queue_size=2)
        self.ci_pub = rospy.Publisher(outputns+"/camera_info", CameraInfo, queue_size=1)
        self.image_pub_color_enable=0
        self.image_pub_depth_enable=0
        self.image_color=None
        self.image_depth=None

        rospy.Subscriber(self.image_topic_color, Image, self.image_callback_color)
        rospy.Subscriber(self.image_topic_depth, Image, self.image_callback_depth)
        rospy.Subscriber(self.ci_topic, CameraInfo, self.camera_info_callback, queue_size=1)

    def image_callback_color(self,msg):

        #self.image_color=msg
        #return
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except CvBridgeError, e:
            print(e)
        else:
            #(rows,cols,channels) = cv_image.shape
            #if cols > 60 and rows > 60 :
            #    cv2.circle(cv_image, (50,50), 10, 255)

            if self.image_pub_color_enable==0:
                self.image_pub_color_enable=1
                #image_color = cv2.resize(cv_image, None, fx = image_scale, fy = image_scale)
                self.image_color = cv2.resize(cv_image, self.image_output_size, interpolation =cv2.INTER_NEAREST)
                print "creat__color"
                #image_pub_color.publish(bridge.cv2_to_imgmsg(image_color, "bgr8"))



    def image_callback_depth(self,msg):

        #self.image_depth=msg
        #return
        try:
            print msg.encoding
            cv_image = self.bridge.imgmsg_to_cv2(msg, "16UC1")#msg.encoding
        except CvBridgeError, e:
            print(e)
        else:
            if self.image_pub_depth_enable==0:
                self.image_pub_depth_enable=1
                #image_depth = cv2.resize(cv_image, image_output_size, interpolation =cv2.INTER_NEAREST)
                self.image_depth=self.camera_mo.product_img(cv_image)
                #image_depth = cv2.resize(cv_image, None, fx = image_scale, fy = image_scale)
                print "creat__depth"
                #image_pub_depth.publish(bridge.cv2_to_imgmsg(image_depth, "16UC1"))
    def camera_info_callback(self, msg):

            p=float(self.image_output_size[0])/msg.width
            ci = CameraInfo()
            ci.header = msg.header
            ci.header.frame_id=self.frame_id
            ci.height = self.image_output_size[1]
            ci.width = self.image_output_size[0]
            ci.distortion_model =msg.distortion_model# "plumb_bob"

            ci.D = msg.D#[0.0, 0.0, 0.0, 0, 0]
            ci.K = [500.0, 0.0, msg.width/2, 0.0, 500.0, msg.height/2, 0.0, 0.0, 1.0]
            for i in range(len(msg.K)):
                if i<6:
                    ci.K[i]=msg.K[i]*p
                else:
                    ci.K[i]=msg.K[i]

            ci.R = msg.R # [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
            ci.P =  [500.0, 0.0, msg.width/2, 0.0, 0.0, 500.0, msg.height/2, 0.0,  0.0, 0.0, 1.0, 0.0]
            for i in range(len(msg.P)):
                if i<7:
                    ci.P[i]=msg.P[i]*p
                else:
                    ci.P[i]=msg.P[i]


            if self.camera_mo_isset==0:
                self.camera_mo_isset=1
                fx=msg.K[0]
                fy=msg.K[4]
                cx=msg.K[2]
                cy=msg.K[5]
                self.camera_mo_src.count_camera_matrix (fx,fy,cx,cy)
                k1=msg.D[0]
                k2=msg.D[1]
                p1=msg.D[2]
                p2=msg.D[3]
                k3=msg.D[4]
                self.camera_mo_src.count_distortion(k1, k2, p1, p2, k3)
                #camera_mo_src.count_map12()

                fx=ci.K[0]
                fy=ci.K[4]
                cx=ci.K[2]
                cy=ci.K[5]
                self.camera_mo.count_camera_matrix (fx,fy,cx,cy)
                k1=ci.D[0]
                k2=ci.D[1]
                p1=ci.D[2]
                p2=ci.D[3]
                k3=ci.D[4]
                self.camera_mo.count_distortion(k1, k2, p1, p2, k3)
                self.camera_mo.count_map12(self.camera_mo_src.camera_matrix)

            #print ci
            self.ci_pub.publish(ci)


    def run(self):

        c=0
        #rospy.spin()
        while not rospy.is_shutdown():
            #rospy.sleep(rospy.Duration(.1))
            c=c+1
            print c
            self.rate.sleep()

            if (self.image_color is not None and  self.image_depth is not None):


                #image_pub_color.publish(image_color)
                #image_pub_depth.publish(image_depth)
                #'''
                img_msg=self.bridge.cv2_to_imgmsg(self.image_color, "bgr8")
                img_msg.header.frame_id=self.frame_id
                t=rospy.Time.now()
                img_msg.header.stamp = t
                self.image_pub_color.publish(img_msg)
                img_msg=self.bridge.cv2_to_imgmsg(self.image_depth, "16UC1")
                img_msg.header.frame_id=self.frame_id
                img_msg.header.stamp = t
                self.image_pub_depth.publish(img_msg)
                self.image_pub_color_enable=0
                self.image_pub_depth_enable=0
                #'''

if __name__ == '__main__':

    rospy.init_node('kinect2_image_process')
    rate = rospy.get_param("~rate", 10)
    width_height = rospy.get_param("~width_height", "(96,79)")##170,140 #160,132 #128,106 #96,79  #85,70  #64,53 #32,26
    inputns = rospy.get_param("~inputns", "/kinect2/sd")
    outputns = rospy.get_param("~outputns", "/kinect2/mini")
    target_frame = rospy.get_param("~target_frame", "kinect2_ir_optical_frame")#
    try:
        wh=eval(width_height)
        print wh
    except:
        print "err ,wh = (96, 79)"
        wh = (96, 79)


    ip=ImageProcess(wh[0],wh[1],rate,target_frame,inputns,outputns)
    ip.run()