rgbdslam流程简单实现

知识点总结:

关于rgbdslam

算法输入:last frame 的深度图和rgb图 和current frame rgb图

核心算法: solve pnp

算法输出: last frame到current frame姿态变化

 

 

 

代码实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import numpy as np
import time
import math
import cv2
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
rgb_paths=[]
depth_paths=[]
ground_datas=[]

Camera_fx=517.306408
Camera_fy= 516.469215
Camera_cx= 318.643040
Camera_cy= 255.313989
Camera_depth_scale=5000.000000

Camera_k1= 0.262383
Camera_k2= -0.953104
Camera_p1= -0.005358
Camera_p2= 0.002628
Camera_k3= 1.163314

def d_main():
    fig = plt.figure()
    #ax1 = plt.axes(projection='3d')
    ax1 = plt.gca(projection='3d')
    ts_in_world=[]
#@1  load rgbdimage
    image_dir="/home/tum/rgbd_dataset_freiburg1_desk/";
    # open txt and
    filename =image_dir+"associate_with_ground.txt";
    with open(filename, 'r') as file_to_read:
        while True:
            lines=file_to_read.readline()
            if not lines:
                break
                pass
            timestamp_rgb,rgb_path,timestamp_depth,depth_path,timestamp_ground,g_tx,g_ty,g_tz,g_qx,g_qy,g_qz,g_qw=[i for i in lines.split()]
            rgb_paths.append(rgb_path)
            g_data=np.array([g_tx,g_ty,g_tz,g_qw,g_qx,g_qy,g_qz],dtype=np.double)
            ground_datas.append(g_data)
            depth_paths.append(depth_path)
            pass
        pass
    pass
    print len(rgb_paths)
    print rgb_paths[0]
    # 定义当前帧和上一个帧
    orb = cv2.ORB_create(500)
    last_rgb=cv2.imread(image_dir+rgb_paths[0],0)
    last_depth=cv2.imread(image_dir+depth_paths[0],-1)
    bf = cv2.BFMatcher(cv2.NORM_HAMMING)
    #need to do 图像金字塔
#@2  load rgbdimage
    t_in_world=np.array([0.0,0.0,0.0]).reshape(3,1)
    for i in range(1,len(rgb_paths)):
        #对上一帧提取orb描述子
        kp1, des1 = orb.detectAndCompute(last_rgb,None)
        im_path=image_dir+rgb_paths[i]
        im_rgb=cv2.imread(im_path,0)
        im_path=image_dir+depth_paths[i]
        im_depth=cv2.imread(im_path,-1)
        kp2, des2 = orb.detectAndCompute(im_rgb,None)
        matches = bf.knnMatch(des1, trainDescriptors = des2, k = 2)
        #good = [m for (m,n) in matches if m.distance < 0.55*n.distance]
        good = []
        last_mkpId=[]
        im_mkpId=[]
        for m,n in matches:
            if m.distance < 0.55*n.distance:
                #print "m.queryIdx"+str(m.queryIdx) # 上一帧图像的id
                #print "################"
                #print "m.imgIdx"+str(m.imgIdx)  #一直都是0
                #print "################"
                #print "m.trainIdx"+str(m.trainIdx) #当前帧图像的id
                #print "################"
                good.append([m])
                last_mkpId.append(m.queryIdx)
                im_mkpId.append(m.trainIdx)
        #遍历last_mkpid和im_mkpid,并且将对应kps重新构建组存储起来
        #求解出每个kp的3d坐标
        #对上一帧进行求解3d点的位置

        last_pts_in_camera=[]
        last_uv_in_camera=[]
        im_pts_in_camera=[]
        im_uv_in_camera=[]
        #得到last 和 im的3d点和对应的uv,来估计相机的姿态
        for i in range(len(last_mkpId)):
            uv_last=kp1[last_mkpId[i]].pt# 获得特征点的像素坐标,需要转化成3d坐标
            uv_im=kp2[im_mkpId[i]].pt
            uv_last_ar=np.array([0,0])
            uv_im_ar=np.array([0,0])
            last_pt_in_camera=np.array([0.0,0.0,0.0])
            im_pt_in_camera=np.array([0.0,0.0,0.0])
            uv_last_ar[0]=int(uv_last[0])
            uv_last_ar[1]=int(uv_last[1])
            uv_im_ar[0]=int(uv_im[0])
            uv_im_ar[1]=int(uv_im[1])
            # need to do orb特征没有均匀化
            #提取深度
            depth_last=last_depth.item(uv_last_ar[1],uv_last_ar[0])
            depth_im=im_depth.item(uv_im_ar[1],uv_im_ar[0])
            if depth_im ==0 or  depth_last ==0:
                continue
            #add last
            rgb_depth=depth_last/Camera_depth_scale
            rgb_x=(uv_last_ar[0]-Camera_cx)*rgb_depth/Camera_fx
            rgb_y=(uv_last_ar[1]-Camera_cy)*rgb_depth/Camera_fy
            last_pt_in_camera[0]=rgb_x
            last_pt_in_camera[1]=rgb_y
            last_pt_in_camera[2]=rgb_depth
            last_uv_in_camera.append(uv_last_ar)
            last_pts_in_camera.append(last_pt_in_camera)
            #add im
            rgb_depth=depth_im/Camera_depth_scale
            rgb_x=(uv_im_ar[0]-Camera_cx)*rgb_depth/Camera_fx
            rgb_y=(uv_im_ar[1]-Camera_cy)*rgb_depth/Camera_fy
            im_pt_in_camera[0]=rgb_x
            im_pt_in_camera[1]=rgb_y
            im_pt_in_camera[2]=rgb_depth
            im_uv_in_camera.append(uv_im_ar)
            im_pts_in_camera.append(im_pt_in_camera)
        
        #转化成为np.array
        last_uv_in_camera_ar=np.array(last_uv_in_camera,dtype=np.double).reshape(len(last_uv_in_camera),2)
        last_pts_in_camera_ar=np.array(last_pts_in_camera,dtype=np.double).reshape(len(last_uv_in_camera),3)
        im_pts_in_camera_ar=np.array(im_pts_in_camera,dtype=np.double).reshape(len(last_uv_in_camera),3)
        im_uv_in_camera_ar=np.array(im_uv_in_camera,dtype=np.double).reshape(len(last_uv_in_camera),2)
        camera_matrix=np.array(([Camera_fx, 0.0, Camera_cx],
        [0.0, Camera_fy, Camera_cy],
        [0.0, 0.0, 1.0]))
        dist_coeffs = np.array([Camera_k1,Camera_k2,Camera_p1,Camera_p2,Camera_k3]).reshape(5,1)  # Assuming no lens distortion
        #求解2d-2d 通过匹配到的特征点恢复出相机的姿态,对极几何
        #get基础矩阵 ->如何恢复出rt呢?
        fundamental_matrix, mask=cv2.findFundamentalMat(last_uv_in_camera_ar,im_uv_in_camera_ar)
        #print fundamental_matrix
        #get 本质矩阵
        essential_matrix,mask=cv2.findEssentialMat(last_uv_in_camera_ar,im_uv_in_camera_ar,camera_matrix)
        #print essential_matrix
        points, RR, t_in_camera, mask =cv2.recoverPose(essential_matrix,last_uv_in_camera_ar,im_uv_in_camera_ar)
        #通过求解pnp
        found, rvec, tvec = cv2.solvePnP(last_pts_in_camera_ar, im_uv_in_camera_ar, camera_matrix,dist_coeffs)
        rotM = cv2.Rodrigues(rvec)[0]
        t_in_world=t_in_world+tvec#利用pnp求解的姿态
        t_in_world=t_in_world+tvec
        ts_in_world.append(t_in_world)


        #可视化
        #cv2.drawMatchesKnn()使用的点对集good是一维的,(N,1);画出good中前几个点对连线
        #good = np.expand_dims(good,1)
        #print len(good)
        im_rgb_kp = cv2.drawKeypoints(last_rgb, kp1, None, color=(0,255,0), flags=0)
        #cv2.imshow("1",im_rgb_kp )
        #cv2.waitKey(10)
        im_rgb_kp = cv2.drawKeypoints(im_rgb, kp2, None, color=(0,255,0), flags=0)
        #cv2.imshow("2",im_rgb_kp )
        #cv2.waitKey(10)
        #img_match=cv2.drawMatchesKnn(last_rgb, kp1, im_rgb, kp2, good, None, flags=2)
        #cv2.imshow("3",img_match)
        #cv2.waitKey(1)
        #update
        last_rgb=im_rgb
        last_depth=im_depth
        # plot_ts_in_world=np.array(ts_in_world).reshape(len(ts_in_world),3)
        # plot_gt_in_word=np.array(ground_datas).reshape(len(ground_datas),7)
        # print plot_gt_in_word
        # ax1.plot(plot_ts_in_world[:,0],plot_ts_in_world[:,1],plot_ts_in_world[:,2],'red') 
        # ax1.plot(plot_ts_in_world[:,0],plot_ts_in_world[:,1],plot_ts_in_world[:,2],'ob',markersize=2)
        # ax1.plot(plot_gt_in_word[:,0],plot_gt_in_word[:,1],plot_gt_in_word[:,2],'og',markersize=4)
        # plt.pause(0.1)



    #进行绘图
    plot_ts_in_world=np.array(ts_in_world).reshape(len(ts_in_world),3)
    plot_gt_in_word=np.array(ground_datas).reshape(len(ground_datas),7)
    print plot_gt_in_word
    ax1.plot(plot_ts_in_world[:,0],plot_ts_in_world[:,1],plot_ts_in_world[:,2],'red') 
    ax1.plot(plot_ts_in_world[:,0],plot_ts_in_world[:,1],plot_ts_in_world[:,2],'ob',markersize=2)
    ax1.plot(plot_gt_in_word[:,0],plot_gt_in_word[:,1],plot_gt_in_word[:,2],'og',markersize=4)
    plt.show()



if __name__ == '__main__':   
    d_main()
    cv2.destroyAllWindows()
    pass

 

使用pnp的效果

3d-2d通过特征匹配求解pnp恢复相机运动

 

对比如果仅仅使用2d-2d通过特征匹配求解出本质矩阵的效果

 

2d-2d通过特征匹配求解本质矩阵恢复相机运动

 

 

总结:两种方法在求解相机姿态的过程中,求解出的r,t都存在突变值,因此效果都不好,其中pnp优于求解本质矩阵,在查阅相关资料的过程中,发现了gx写的rgbd-slam,可以借鉴当中normofTransform函数,即:Δt+min(2π−r,r)作为限制条件来减少突值

关于rgbd-slam,由于原始的代码版本基于opencv2.4.x系列,目前修改到了基于opencv3.x系列,链接如下:

代码仓库:https://gitee.com/davidhan008/rgbd-slam

相关解释:http://blog.sina.com.cn/s/blog_161aed33e0102ymkm.html

已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 编程工作室 设计师:CSDN官方博客 返回首页