使用orbslam位姿进行map2dfusion的建图

 

 

https://blog.csdn.net/qq_60320488/article/details/132084670

 

一、map2dfusion所采用的数据集如下

npupilab/npu-dronemap-dataset: NPU Drone-Map Dataset (github.com)

其中map2dfusion的数据集中包含一组图片和两个配置文件

1.config.cfg

 

 

其中包含的内容：

plane为地面位姿

GPS.Origine为给定的初始GPS坐标

Camera.CameraType为采用的摄像头成像原理

Camera.Paraments为摄像头参数，前两个为图像的分辨率，后面四个分别为摄像机内参

TrajectoryFile为所存图像及其位姿的配置文件

2.trajectory.txt

其中包含8个数据：

第1位为图片的时间戳，也是图片名称，

第2~4位是摄像头的translation

第5~8位是摄像头的rotation，采用的四元数的格式

所以要使用orb-slam生成该格式的位姿数据，此处为了提高建图融合的效果，选取将所有帧及其位姿用于建图

二、在orb-slam的System.cc中添加如下函数，保存所有的轨迹。

 

void System::SaveTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
 
    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
 
    // Transform all keyframes so that the first keyframe is at the origin.
    // After a loop closure the first keyframe might not be at the origin.
    cv::Mat Two = vpKFs[0]->GetPoseInverse();
 
    ofstream f;
    f.open(filename.c_str());
    f << fixed;
 
    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
    // We need to get first the keyframe pose and then concatenate the relative transformation.
    // Frames not localized (tracking failure) are not saved.
 
    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
    // which is true when tracking failed (lbL).
    list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
    list<bool>::iterator lbL = mpTracker->mlbLost.begin();
    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
    {
        if(*lbL)
            continue;
 
        KeyFrame* pKF = *lRit;
 
        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
 
        // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
        while(pKF->isBad())
        {
            Trw = Trw*pKF->mTcp;
            pKF = pKF->GetParent();
        }
 
        Trw = Trw*pKF->GetPose()*Two;
 
        cv::Mat Tcw = (*lit)*Trw;
        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
 
        vector<float> q = Converter::toQuaternion(Rwc);
 
        f << setprecision(6) << *lT << " " <<  setprecision(9) << 100*twc.at<float>(0) << " " << -1*100*twc.at<float>(1) << " " << 100*twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
    }
    f.close();
    cout << endl << "trajectory saved!" << endl;
}

　　

在Examples\Monocular\的mono_tum.cc中将SaveKeyFrameTrajectoryTUM改为SaveTrajectoryTUM，重新编译即可。

三、重新配置orbslam中的yaml配置文件

新建一个TUMX.yaml文件，

 

fx,fy,cx,cy是摄像头内参，改为config.cfg中Camera.Paraments的后四位

k1，k2,k3,p1,p2为摄像头的畸变参数，这里全部设置为0

由于数据集的分辨率较高，考虑到性能限制，将fps设置为5

 

将nFeatures设置为2000

iniThFAST设置为10

minThFAST设置为3

四、在数据集中运行如下代码生成一个rgb文件用于slam

import numpy as np
import os
# 图片文件夹，后面的/不能省
from PIL import Image
 
 
img_path = 'D:/slam/phantom3-village-kfs-master/phantom3-village-kfs-master/'
 
# print(files)        # 当前路径下所有非目录子文件
# with open(img_path+"\opt_poses.txt") as f:
#     read_data = f.readlines()
# # print(read_data)
count = 0
for root, dirs, files in os.walk(img_path, topdown=False):
    print(root)     # 当前目录路径
    print(dirs)     # 当前目录下所有子目录
    with open("D:/slam/phantom3-village-kfs-master/phantom3-village-kfs-master"+"/rgb.txt", 'w') as g:
        # count = 0
        for i in files:
            # count = count +0.1
            # print(i[-3:-1]+i[-1])
            # if(i[-3:-1]+i[-1] == "jpg"):
            #     #print(count)
            #     #I = Image.open('./cadastre_gray/'+i)
            #     count = int(i[6:-1].split('.')[0])
            #     print(count)
            #     # print(read_data[count][0:7] +' '+ i)
            #     write_date[count] = time_list[count] +' '+ time_list[count]+'.jpg' +"\n"
            #     print(write_date[count])
            #     # print(read_data[count][0:7])
            #     #I.save('./picture/'+time_list[count]+'.jpg')
 
            # print(count)
            # print(i[0:9] + '.'+i[9] +i[11:17])
            #for i in range(10):
            #name = str(count)+".jpg"
            #g.writelines(str(count) + ' ' + name + '\n')
            print(i[0:-4])
            g.writelines(i[0:-4] + ' ' + i + '\n')
            # g.writelines(i[0:9] + '.'+i[9] +i[11:17]+ ' '+ i[0:9] + '.'+i[9] +i[11:17]+".jpg\n")
            #out.save('D:/slam/phantom3-village-kfs-master/phantom3-npu-master/phantom3-npu-master/npu/'+i)
            count = count + 0.01
        # for i in range(len(files)):
        #     print(write_date[i])
        #     # g.writelines(write_date[i])
        #     count=count+1
        # print(count)
# I = Image.open('./image.png')
# print(type(I))       #---><class 'PIL.JpegImagePlugin.JpegImageFile'>
# print(I.size)        #--->(1280, 720)
# I.show()
# I.save('./save.png')

　　

随后将生成的KeyFrameTrajectory作为trajectory.txt运行map2dfusion的进程即可

其中将产生的点云文件用于ransac平面拟合，得到的结果用于config的plane中，即可得到相对优秀的融合结果。

ransac平面拟合如下

 

import random
import numpy as np
from math import acos, sin, cos, fabs, sqrt, log
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import csv
def new_csv():
    with open("pcltest.csv", "w") as csvfile:
        writer = csv.writer(csvfile)
 
        # 先写入columns_name
        # writer.writerow(["a", "b", "c", "d", "message"])
        writer.writerows([[1, 2, 3, 4, ',']])
 
def getData(filepath, row_need=1000):
    """
    加载数据并且取其中的部分行
    """
    data = np.loadtxt(filepath, delimiter=",")
    row_total = data.shape[0]
    print(row_total)
    row_sequence = np.arange(row_total)
    np.random.shuffle(row_sequence)
 
    return data[row_sequence[0:row_need], :]
 
 
def solve_plane(A, B, C):
    """
    求解平面方程
    :param A: 点A
    :param B: 点B
    :param C: 点C
    :return: Point(平面上一点),Quaternion(平面四元数),Nx(平面的法向量)
    """
 
    # 两个常量
    N = np.array([0, 0, 1])
    Pi = 3.1415926535
 
    # 计算平面的单位法向量，即BC 与 BA的叉积
    Nx = np.cross(B - C, B - A)
    Nx = Nx / np.linalg.norm(Nx)
 
    # 计算单位旋转向量与旋转角（范围为0到Pi）
    Nv = np.cross(Nx, N)
    angle = acos(np.dot(Nx, N))
 
    # 考虑到两个向量夹角不大于Pi/2，这里需要处理一下
    if angle > Pi / 2.0:
        angle = Pi - angle
        Nv = -Nv
 
    # FIXME: 此处如何确定平面上的一个点？？？
    # Point = (A + B + C) / 3.0
    Point = B
    # 计算单位四元数
    Quaternion = np.append(Nv * sin(angle / 2), cos(angle / 2))
 
    # print("旋转向量:\t", Nv)
    # print("旋转角度:\t", angle)
    # print("对应四元数:\t", Quaternion)
 
    return Point, Quaternion, Nx
 
 
def solve_distance(M, P, N):
    """
    求解点M到平面(P,Q)的距离
    :param M: 点M
    :param P: 平面上一点
    :param N: 平面的法向量
    :return: 点到平面的距离
    """
 
    # 从四元数到法向量
    # A = 2 * Q[0] * Q[2] + 2 * Q[1] * Q[3]
    # B = 2 * Q[1] * Q[2] - 2 * Q[0] * Q[3]
    # C = -Q[0] ** 2 - Q[1] ** 2 + Q[2] ** 2 + Q[3] ** 2
    # D = -A * P[0] - B * P[1] - C * P[2]
 
    # 为了计算简便，直接使用求解出的法向量
    A = N[0]
    B = N[1]
    C = N[2]
    D = -A * P[0] - B * P[1] - C * P[2]
 
    return fabs(A * M[0] + B * M[1] + C * M[2] + D) / sqrt(A ** 2 + B ** 2 + C ** 2)
 
 
def RANSAC(data):
    """
    使用RANSAC算法估算模型
    """
    # 数据规模
    SIZE = data.shape[0]
    # 迭代最大次数，每次得到更好的估计会优化iters的数值，默认10000
    iters = 10000
    # 数据和模型之间可接受的差值，默认0.25
    sigma = 0.15
    # 内点数目
    pretotal = 0
    # 希望的得到正确模型的概率，默认0.99
    Per = 0.999
    # 初始化一下
    P = np.array([])
    Q = np.array([])
    N = np.array([])
    for i in range(iters):
        # 随机在数据中选出三个点去求解模型
        sample_index = random.sample(range(SIZE), 3)
        P, Q, N = solve_plane(data[sample_index[0]], data[sample_index[1]], data[sample_index[2]])
 
        # 算出内点数目
        total_inlier = 0
        for index in range(SIZE):
            if solve_distance(data[index], P, N) < sigma:
                total_inlier = total_inlier + 1
 
        # 判断当前的模型是否比之前估算的模型好
        if total_inlier > pretotal:
            print(total_inlier / SIZE)
            iters = log(1 - Per) / log(1 - pow(abs(total_inlier / SIZE), 2))
            pretotal = total_inlier
 
        # 判断是否当前模型已经符合超过一半的点
        if total_inlier > SIZE / 2:
            break
    return P, Q, N
 
 
def draw(data, P, N):
    """
    画出散点图和平面
    :param data: 三维点
    :param N: 平面法向量
    """
    # 创建一个画布figure，然后在这个画布上加各种元素。
    fig = plt.figure()
    # 将画布作用于 Axes3D 对象上。
    ax = Axes3D(fig)
 
    # 画出散点图
    ax.scatter(data[0], data[1], data[2], c="gold")
 
    # 画出平面
    x = np.linspace(-30, 30, 10)
    y = np.linspace(-30, 30, 10)
    X, Y = np.meshgrid(x, y)
    Z = -(N[0] * X + N[1] * Y - (N[0] * P[0] + N[1] * P[1] + N[2] * P[2])) / N[2]
    ax.plot_surface(X, Y, Z)
 
    # 画出坐标轴
    ax.set_xlabel('X label')
    ax.set_ylabel('Y label')
    ax.set_zlabel('Z label')
 
    plt.show()
 
 
def test():
    A = np.random.randn(3)
    B = np.random.randn(3)
    C = np.random.randn(3)
 
    P, Q, N = solve_plane(A, B, C)
    print("Plane:\t", P, Q)
 
    D = np.random.randn(3)
    print("Point:\t", D)
 
    d = solve_distance(D, P, N)
    print("Distance:\t", d)
 
 
if __name__ == '__main__':
    data = getData("pcltest.csv")
    P, Q, N = RANSAC(data)
    print("Plane:\t", np.append(P, Q))
    draw(data.T, P, N)

　　

 

 

其他

1 平面参数

 

 

bool Map2DPrepare::prepare(const pi::SE3d& plane,const PinHoleParameters& camera,
                                        const std::deque<std::pair<cv::Mat,pi::SE3d> >& frames)
{
    if(frames.size()==0||camera.w<=0||camera.h<=0||camera.fx==0||camera.fy==0)
    {
        cerr<<"Map2D::prepare:Not valid prepare!\n";
        return false;
    }
    _camera=camera;_fxinv=1./camera.fx;_fyinv=1./camera.fy;
    _plane =plane;
    _frames=frames;
    for(std::deque<std::pair<cv::Mat,pi::SE3d> >::iterator it=_frames.begin();it!=_frames.end();it++)
    {
        pi::SE3d& pose=it->second;
        pose=plane.inverse()*pose;//plane coordinate
    }
    return true;
}