SLAM数据处理小程序备忘

0.引言

备忘部分数据集相互处理以及近期用到的一些数据处理小程序.

1.程序运行时间计算

C++的库函数中，计算程序运行时间使用的知识点：

• clock_t

clock_t数据类型，long型，用来记录一段时间内的clocks数，即CPU的运行单元时间。

• clock()

返回类型clock_t，返回的是从程序开始，到调用用clock()函数这段时间的clocks。

• CLOCKS_PER_SEC

这些库函数、类型和常量都是定义在ctime库中的，头文件time.h。计算程序的运行时间，只需根据程序的入口点和出口点出计算clocks，再算差就可以。

int main() {
	clock_t start, end;
	start = clock();
	//测试对象
	end = clock();
	cout<<"Run time: "<<(double)(end - start) / CLOCKS_PER_SEC<<"S"<<endl;
	return 0;
    }

2.EuRoc2TUM

• TUM ground truth格式: timestamp tx ty tz qx qy qz qw
• EUROC ground truth格式: timestamp tx ty tz qw qx qy qz

Euroc2Tum.py

#!/usr/bin/python
# -*- coding: UTF-8 -*-
import csv
# 输入csv文件名称和输出txt文件名称
csv_file = raw_input('Enter the name of your input csv file: ')
txt_file = raw_input('Enter the name of your output txt file: ')
with open(txt_file, "w") as my_output_file:
    with open(csv_file, "r") as my_input_file:
        #逐行读取csv存入txt中
        for row in csv.reader(my_input_file):
            # 前8个数据是:timestamp tx ty tz qw qx qy qz
            row = row[0:8]
            # 时间戳单位处理
            temp1 = row[0][0:10] + '.' + row[0][10:16]
            row[0] = temp1
            # 互换 qw 和 qx
            temp2 = row[4]
            row[4] = row[7]
            row[7] = temp2
            my_output_file.write(" ".join(row)+'\n')
    my_output_file.close()

3.file2ROSBag

• ref.

将图片文件以及IMU转换为ROSBag.Transform2ROS.py

#import cv2
import time, sys, os
from ros import rosbag
import roslib
import rospy
roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image,Imu
from geometry_msgs.msg import Vector3
# import ImageFile
from PIL import ImageFile
from PIL import Image as ImagePIL


'''sort image name'''
def CompSortFileNamesNr(f):
    g = os.path.splitext(os.path.split(f)[1])[0]
    numbertext = ''.join(c for c in g if c.isdigit())
    return int(numbertext)

'''get image from dir'''
def ReadImages(dir):
    '''Generates a list of files from the directory'''
    print( "Searching directory %s" % dir )
    all = []
    left_files = []
    right_files = []
    if os.path.exists(dir):
        for path, names, files in os.walk(dir + '/cam0/data'):
            # for f in files:
            for f in sorted(files, key=CompSortFileNamesNr):
                if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg']:
                    if 'left' in f or 'left' in path or '/cam0/data' in f or '/cam0/data' in path :
                        left_files.append( os.path.join( path, f ) )
                    elif 'right' in f or 'right' in path or '/cam1/data' in f or '/cam1/data' in path :
                        right_files.append( os.path.join( path, f ) )
                    all.append( os.path.join( path, f ) )
        for path, names, files in os.walk(dir + '/cam1/data'):
            # for f in files:
            for f in sorted(files, key=CompSortFileNamesNr):
                if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg']:
                    if 'left' in f or 'left' in path or '/cam0/data' in f or '/cam0/data' in path :
                        left_files.append( os.path.join( path, f ) )
                    elif 'right' in f or 'right' in path or '/cam1/data' in f or '/cam1/data' in path :
                        right_files.append( os.path.join( path, f ) )
                    all.append( os.path.join( path, f ) )
    #return all, left_files, right_files
    return left_files, right_files


def ReadIMU(filename):
    '''return IMU data and timestamp of IMU'''
    file = open(filename,'r')

    all = file.readlines()[1:]
    timestamp = []
    imu_data = []
    for f in all:
        line = f.rstrip('\n').split(',')
        timestamp.append(line[0])
        imu_data.append(line[1:])
    return timestamp,imu_data


def CreateStereoBag(args):
    '''read image'''
    left_imgs, right_imgs = ReadImages(args[0])

    '''read time stamps'''
    imagetimestamps=[]
    file = open(args[1], 'r')
    all = file.readlines()[1:] # skip the first line.
    for f in all:
        imagetimestamps.append(f.rstrip('\n').split(',')[0])
    file.close()

    '''read imu timestamps and data'''
    imutimesteps,imudata = ReadIMU(args[2])

    '''Creates a bag file containing stereo image and imu pairs'''
    #if not os.path.exists(args[3]):
    #os.system(r'touch %s' % args[3])
    bag = rosbag.Bag(args[3], 'w')


    try:
        for i in range(len(imudata)):
            imu = Imu()
            angular_v = Vector3()
            linear_a = Vector3()
            angular_v.x = float(imudata[i][0])
            angular_v.y = float(imudata[i][1])
            angular_v.z = float(imudata[i][2])
            linear_a.x = float(imudata[i][3])
            linear_a.y = float(imudata[i][4])
            linear_a.z = float(imudata[i][5])
            #imuStamp = rospy.rostime.Time.from_sec(float( (float(imutimesteps[i]))/1e6 ))
            imuStamp = rospy.rostime.Time.from_sec(float(imutimesteps[i]))
            imu.header.stamp=imuStamp
            imu.angular_velocity = angular_v
            imu.linear_acceleration = linear_a

            bag.write("/imu0",imu,imuStamp)


        for i in range(len(left_imgs)):
            print("Adding %s" % left_imgs[i])
            fp_left = open( left_imgs[i], "r" )
            p_left = ImageFile.Parser()

            '''read image size'''
            imgpil = ImagePIL.open(left_imgs[0])
            width, height = imgpil.size

            while 1:
                s = fp_left.read(1024)
                if not s:
                    break
                p_left.feed(s)

            im_left = p_left.close()

            fp_right = open( right_imgs[i], "r" )
            print("Adding %s" % right_imgs[i])
            p_right = ImageFile.Parser()

            while 1:
                s = fp_right.read(1024)
                if not s:
                    break
                p_right.feed(s)

            im_right = p_right.close()

            # Stamp = roslib.rostime.Time.from_sec(time.time())
            # Stamp = rospy.rostime.Time.from_sec(float( (float(imagetimestamps[i]))/1e6 ))
            Stamp = rospy.rostime.Time.from_sec(float(imagetimestamps[i]))

            # left image
            Img_left = Image()
            Img_left.header.stamp = Stamp
            Img_left.width = width
            Img_left.height = height
            Img_left.header.frame_id = "camera/left"

            Img_left.encoding = "mono8"
            Img_left_data = [pix for pixdata in [im_left.getdata()] for pix in pixdata]
            Img_left.step = Img_left.width
            Img_left.data = Img_left_data

            Img_right = Image()
            Img_right.header.stamp = Stamp
            Img_right.width = width
            Img_right.height = height
            Img_right.header.frame_id = "camera/right"

            Img_right.encoding = "mono8"
            Img_right_data = [pix for pixdata in [im_right.getdata()] for pix in pixdata]
            Img_right.step = Img_right.width
            Img_right.data = Img_right_data

            bag.write('/cam0/image_raw', Img_left, Stamp)
            bag.write('/cam1/image_raw', Img_right, Stamp)
    finally:
        bag.close()


if __name__ == "__main__":
    if len(sys.argv) == 5:
        print(sys.argv)
        CreateStereoBag(sys.argv[1:])
    else:
        print( "Usage: img_file,img_timestamps_file, imu_file, bagname")
        print( "Example: python3 img2bag_Stereoimu.py /*/00  data.csv imu.csv img2bag_Stereoimu.bag")
#img_file,img_timestamps_file, imu_file, bagname

4.去除每行头尾的空格

在测试的时候，对文件数据集有严格要求，又不想重新改代码跑数据集结果，就直接对结果格式进行更改。

SpaceDelete.py

#!/usr/bin/python
# -*- coding: UTF-8 -*-
def trimLineSpace(finput, foutput):
    fInputHandle = open ( fInput )
    fOutputHandle = open (fOutput, 'w')
    lines = fInputHandle.readlines()

    for line in lines:
        fOutputHandle.write(line.strip()+'\r\n')
    fInputHandle.close()
    fOutputHandle.close()


if __name__ == '__main__':
    fInput = raw_input('Enter the name of your input txt file: ')
    fOutput = raw_input('Enter the name of your output txt file: ')
    trimLineSpace(fInput, fOutput)

5.Matrix Inverse

对标定结果有时候需要对矩阵求逆.
MatricxInverse.cpp

#include <iostream>
#include<iomanip>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>


int main( int argc, char** argv )
{

    Eigen::Matrix<double, 4, 4> matrix_44_cam0;
    Eigen::Matrix<double, 4, 4> matrix_44_cam1;
    //cammodel:mei
    
    matrix_44_cam0<< -0.9995407230847781, 0.029100449860456495,-0.008456164206050667, 0.04812531099830761,
						0.007419008250330716, -0.035565792521783365, -0.9993397984263814, -0.046268993994975235,
						-0.02938198787934812, -0.9989435610785118, 0.03533356149670343, -0.06808128621572819,
						0.0, 0.0, 0.0, 1.0;
	
	matrix_44_cam1<< -0.9995333551739931, 0.029563758443646823, -0.007684795462836215, -0.05277683771177886,
						0.008020445760736369, 0.01125165640719669, -0.9999045317818563, -0.04396772695601477,
						-0.029474469366199164, -0.9994995669907917, -0.011483520401031096, -0.0711950391086574,
						0.0, 0.0, 0.0, 1.0;
	

	//cammodel:pinhole
						
	//matrix_44_cam0<<-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392,
  	//					0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084,
  	//					- 0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297,
  	//					0.0, 0.0, 0.0, 1.0;
	
	//matrix_44_cam1<< -0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734,
  	//					0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924,
	//					-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751,
  	//					0.0, 0.0, 0.0, 1.0;
    //Eigen::Matrix<double, 4, 4> matrix_44_cam0_inv;
    //Eigen::Matrix<double, 4, 4> matrix_44_cam1_inv;
    //cout << "matrix_44_cam0_inv:  "<< endl<< setprecision(10)<<matrix_44_cam0.inverse() << endl;      
    //cout << "matrix_44_cam1_inv:  "<< endl<< setprecision(10)<<matrix_44_cam1.inverse() << endl; 


	Eigen::Matrix3d rotation_matrix; 
    rotation_matrix <<  -9.9951465899298464e-01, 7.5842033363785165e-03,
						-3.0214670573904204e-02, 2.9940114644659861e-02,
						-3.4023430206013172e-02, -9.9897246995704592e-01,
						-8.6044170750674241e-03, -9.9939225835343004e-01,
						3.3779845322755464e-02;
    Eigen::Vector3d v;
    v <<  4.4511917113940799e-02, -7.3197096234105752e-02,-4.7972907300764499e-02;

	Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵　
    T.rotate ( rotation_matrix );                                        // 按照rotation_vector进行旋转
    T.pretranslate (v);               // 把平移向量
    cout << "转换矩阵 Transform matrix = \n" << T.matrix() <<endl;
      
    T2Rt();
    
    return 0;
}

MakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( MatricxInverse )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )

include_directories( "/usr/local/include/eigen3" )
add_executable( MatricxInverse MatricxInverse.cpp )

6.ROSbag按时间戳解压

# !/usr/bin/python
# -*- coding: UTF-8 -*-
import roslib  
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
path='/media/ipsg/data/cam-right/' #存放图片的位置
class ImageCreator():


    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('stereo_calib.bag', 'r') as bag:   #要读取的bag文件；
            for topic,msg,t in bag.read_messages():
                if topic == "/camera/infra2/image_rect_raw":  #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print e
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".jpg" #图像命名：时间戳.jpg
                        cv2.imwrite(path+image_name, cv_image)  #保存；


if __name__ == '__main__':

    #rospy.init_node(PKG)

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

同时保存时间戳：

# !/usr/bin/python
# -*- coding: UTF-8 -*-
import roslib  
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
path='../avm/landmark/' #存放图片的位置
txt_file = "../avm/timestamp.txt"
class ImageCreator():


    def __init__(self):
        self.bridge = CvBridge()

        with open(txt_file, "w") as my_output_file:
          with rosbag.Bag('mapping.bag', 'r') as bag:   #要读取的bag文件；
              for topic,msg,t in bag.read_messages():
                  if topic == "landmark_node_topic":  #图像的topic；
                          # print("ok!")
                          try:
                             cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                          except CvBridgeError as e:
                              print e
                          timestr = "%.6f" %  msg.header.stamp.to_sec()
                          #%.6f表示小数点后带有6位，可根据精确度需要修改；
                          image_name = timestr+ ".jpg" #图像命名：时间戳.jpg
                          print(image_name)
                          cv2.imwrite(path+image_name, cv_image)  #保存；
                          my_output_file.write("".join(timestr)+'\n')
          my_output_file.close()

if __name__ == '__main__':

    #rospy.init_node(PKG)

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

7.保存topic数据至TXT

rostopic echo /topicname | grep id: -w > txtname.txt

或者

rostopic echo -b rosbagname.bag -p /topicname > txtname.txt

eg:

rostopic echo -b mapping.bag -p /dr_odometry_topic > dr.txt

2dr.py

#!/usr/bin/python
# -*- coding: UTF-8 -*-
import csv
# 输入csv文件名称和输出txt文件名称
#csv_file = raw_input('Enter the name of your input csv file: ')
#txt_file = raw_input('Enter the name of your output txt file: ')
csv_file = "./dr.csv"
txt_file = "./dr.txt"
with open(txt_file, "w") as my_output_file:
    with open(csv_file, "r") as my_input_file:
        #逐行读取csv存入txt中
	lines = my_input_file.readlines()[1:]  # skip the first line.
        for row in csv.reader(lines):
            # 前11个数据
            row = [row[2],row[5],row[6],row[7],row[8],row[9],row[10],row[11]]
            #row = row[2] + row[5:11]
            print(row)
            # 时间戳单位处理
            temp1 = row[0][0:4] + '.' + row[0][4:9]
            print(temp1)
            row[0] = temp1
            # 互换 qw 和 qx
            # temp2 = row[4]
            # row[4] = row[7]
            # row[7] = temp2
            my_output_file.write(" ".join(row)+'\n')
    my_output_file.close()

2timestamp.pt

#!/usr/bin/python
# -*- coding: UTF-8 -*-
import csv
# 输入csv文件名称和输出txt文件名称
#csv_file = raw_input('Enter the name of your input csv file: ')
#txt_file = raw_input('Enter the name of your output txt file: ')
csv_file = "./dr.csv"
txt_file = "./timestamp.txt"
with open(txt_file, "w") as my_output_file:
    with open(csv_file, "r") as my_input_file:
        #逐行读取csv存入txt中
	lines = my_input_file.readlines()[1:]  # skip the first line.
        for row in csv.reader(lines):
            # 前1个数据
            row = str(row[2])
            #row = row[2] + row[5:11]
            print(row)
            # 时间戳单位处理
            row = row[0:4] + '.' + row[4:9]
            print(row)
            #row[0] = temp1
            # 互换 qw 和 qx
            # temp2 = row[4]
            # row[4] = row[7]
            # row[7] = temp2
            my_output_file.write("".join(row)+'\n')
    my_output_file.close()

2dr2timestamp.sh

rostopic echo -b mapping.bag -p /dr_odometry_topic > dr.csv

sleep 4s

python2 2dr.py

sleep 1s

python2 2timestamp.py