Multi sensor fusion positioning - Summary of common auxiliary debugging tools

Multi sensor fusion positioning - Summary of common auxiliary debugging tools

Many debugging methods will be used in the debugging process of multi-sensor fusion positioning, such as open3d visualization and matplotlib waveform visualization. This chapter will be collected and continuously updated~

Summary of commissioning methods

1. mamtplotlib visual scan_context matrix

Storage scan_ The context matrix is in the txt file

#include <list>
#include <sstream>
#include <fstream>
#include <iomanip>
std::ofstream  sc_local_map;
std::ofstream  sc_cur_scan;
char sc_local_map_path[] = "/home/kaho/catkin2_ws/visual_sc_matrix/sc_matrix_local_map.txt";
char sc_cur_scan_path[] = "/home/kaho/catkin2_ws/visual_sc_matrix/sc_matrix_cur_scan.txt";

bool CreateFile(std::ofstream& ofs, std::string file_path) {
    ofs.open(file_path, std::ios::out);                          //  Use std::ios::out to achieve coverage
    if(!ofs)
    {
        std::cout << "open csv file error " << std::endl;
        return  false;
    }
    return true;
}

/* write2txt  */
void WriteText(std::ofstream& ofs, Eigen::MatrixXf   sc_matrix){
    for(int did = 0; did < sc_matrix.rows();  ++did){
        for(int sid=0; sid < sc_matrix.cols(); ++sid){
                ofs << std::fixed  << sc_matrix(did, sid)  << "\t";
        }ofs  << "\n";
    }
}

Call method:

    Eigen::MatrixXf   sc_matrix(query_scan_context.rows(), query_scan_context.cols());				// Save sc to sc_ In matrix
    for(int did = 0; did < query_scan_context.rows();  ++ did){
        for(int sid = 0; sid < query_scan_context.cols(); ++sid){
            sc_matrix(did,sid)  =   query_scan_context(did,sid);
        }
    }   
CreateFile(sc_local_map, sc_local_map_path);   //  create folder
    WriteText(sc_local_map, sc_matrix);             // Put SC_ matrxi_ local_ Write map into folder

python matplotlib reads sc_matrix and visualize the matrix

import matplotlib.pyplot as plt
import numpy as np

sc_matrix_cur_scan = np.loadtxt("sc_matrix_cur_scan.txt")
sc_matrix_lcoal_map = np.loadtxt("sc_matrix_local_map.txt")

def samplemat(mat):
    """Make a matrix with all zeros and increasing elements on the diagonal"""
    sector = mat.shape[0]   # x
    ring = mat.shape[1]     # y
    aa = np.zeros((sector,ring))
    for i in range(sector):
        for j in range(ring):
            if(mat[i,j]):
                aa[i,j]  = 1 #mat[i,j]
            else:
                aa[i, j] = mat[i,j]
    return aa

fig = plt.figure(figsize=(10,10))
ax1 =  fig.add_subplot(2,1,1)          # 2 * 1 distribution, 1st

# Display matrix
#ax1.matshow(samplemat(sc_matrix_lcoal_map))   #,cmap = plt.cm.gray
ax1.matshow(sc_matrix_lcoal_map)   #,cmap = plt.cm.gray
plt.title("sc_local_map");

ax2 =  fig.add_subplot(2,1,2)          # 2 * 1 distribution, 2nd
#ax2.matshow(samplemat(sc_matrix_cur_scan))   #,cmap = plt.cm.gray
 ax2.matshow(sc_matrix_cur_scan)   #,cmap = plt.cm.gray
plt.title("sc_current_scan ");

plt.show()

2. open3d visual point cloud

The storage point cloud is pcd

pcl::io::savePCDFileASCII("/home/kaho/catkin2_ws/visual_sc_matrix/pcd/cur_scan.pcd",*transformed_scan_.cloud_ptr );
//save pcd 
 pcl::io::savePCDFileASCII("/home/kaho/catkin2_ws/visual_sc_matrix/pcd/local_map.pcd",*local_map_ptr_ );

python open3d visualization

import open3d as o3d
import numpy as np
local_map_pcd = o3d.io.read_point_cloud("pcd/local_map.pcd")
cur_scan_pcd = o3d.io.read_point_cloud("pcd/cur_scan.pcd")

local_map_points = np.asarray(local_map_pcd.points)
cur_scan_points = np.asarray(cur_scan_pcd.points)


# print(local_map_pcd)#Number of output point cloud points
# print(cur_scan_pcd)#Number of output point cloud points
#
print(local_map_points)#3D coordinates of the output point
# print(cur_scan_points)#3D coordinates of the output point
#
# print('Give all points a uniform color, which is the value within the range of [0,1] in RGB space ')

local_map_pcd.paint_uniform_color([0.9, 0.9, 0.9])        # blue
cur_scan_pcd.paint_uniform_color([1, 0, 0])        # red


o3d.visualization.draw_geometries([local_map_pcd,cur_scan_pcd])

3. Store pose in Evo format

FILE:catkin_ws/src/imu_integration/src/evo_evaluate/evaluate.cpp

The evo tool is used for accuracy evaluation, and the evo format uses the TUM format, referring to the teaching assistant of eamo GitHub Writing method: in order to facilitate subsequent calls, the format of the saved file is written as ROS Node through subscription:

/sim/sensor/imu /pose/ground_truth two topics are stored in files of corresponding formats for evo evaluation.

For the use of evo, please refer to:

SLAM Trajectory Accuracy Evaluation Tool evo usage

evo evaluation TUM dataset

How to install evo, how to use evo, how to use the tutorial, SLAM trajectory accuracy evaluation tool, how to evaluate the Trajectory Accuracy generated by ORB-SLAM2, evaluate the trajectory accuracy of lidar SLAM and visual SLAM, and quantify the error of SLAM

3.1 evo TUM format storage

The commonly used evo KITTI pose format is known as timestamp. The accuracy is evaluated and compared through a fixed number of sequences. There is a timestamp in the format of the TUM data set. It will be more accurate if compared through the timestamp. Therefore, the stored evo data format is TUM

evo TUM requires that the pose storage format is timestamp x y z q_x q_y q_z q_w ; Refer to evo official file Formats for details

Storage writing method

/* write2txt   format  TUM*/
void WriteText(std::ofstream& ofs, pose data){
    ofs << std::fixed  << data.timestamp  << " " << data.pos.x() << " " <<  data.pos.y()  << " " <<   data.pos.z()  << " "
                                                                              <<  data.q.x()  << " "  <<  data.q.y()   <<" " <<  data.q.z()   << " "  <<  data.q.w()  <<  std::endl;
}

3.2 time alignment of tum timestamps

At the beginning, when timestamps are written in gt.txt and ins.txt, the timestamps are the timestamps of their respective ROS topic s. Run Evo_ During RPE segment evaluation, an error is reported, and the data with a timestamp difference of 0.01s in the two files cannot be found. After observation, it is because the estimated IMU inertia solution node subscribes to IMU first_ The SIM topic data is only solved, which will be a little slower than the groundtruth topic timestamp. Therefore, the respective time can be aligned by subtracting the data timestamp of the first frame from their current timestamp.

    if(flag_ins){
        stamp_ins_init  =  msg->header.stamp.toSec();   
        flag_ins = 0;
    }
    pose_ins.timestamp =  msg->header.stamp.toSec()  -   stamp_ins_init;  //Convert timestamp to floating point format

/******************************************************************************************************/
        if(flag_gt){
        stamp_gt_init = msg->header.stamp.toSec();   
        flag_gt = 0;
    }
        pose_gt.timestamp = msg->header.stamp.toSec() -  stamp_gt_init;

3.3 evo TUM ROS_ Complete implementation of node

FILE:catkin_ws/src/imu_integration/src/evo_evaluate/evaluate.cpp

#include <iostream>
#include <vector>
#include <string>
#include <list>
#include <sstream>
#include <fstream>
#include <iomanip>

/*ROS INCLUDE*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Dense>
#include <Eigen/Core>


using namespace std;

/*Define pose structure*/
struct pose
{
    double  timestamp;
    Eigen::Vector3d  pos ;
    Eigen::Quaterniond  q;
};


pose  pose_gt;                      //   GroundTruth  pose
pose  pose_ins;                    //    Estimate   pose

std::ofstream  gt;
std::ofstream  ins;

double  stamp_gt = 0;
double  stamp_ins = 0;

double  stamp_gt_init = 0;
double  stamp_ins_init = 0;

int  flag_gt = 1;
int  flag_ins = 1;

bool CreateFile(std::ofstream& ofs, std::string file_path) {
    ofs.open(file_path, std::ios::out);                          //  Use std::ios::out to achieve coverage
    if(!ofs)
    {
        std::cout << "open csv file error " << std::endl;
        return  false;
    }
    return true;
}
/* write2txt   format  TUM*/
void WriteText(std::ofstream& ofs, pose data){
    ofs << std::fixed  << data.timestamp  << " " << data.pos.x() << " " <<  data.pos.y()  << " " <<   data.pos.z()  << " "
                                                                              <<  data.q.x()  << " "  <<  data.q.y()   <<" " <<  data.q.z()   << " "  <<  data.q.w()  <<  std::endl;
}

void  insCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    if(flag_ins){
        stamp_ins_init  =  msg->header.stamp.toSec();   
        flag_ins = 0;
    }
    pose_ins.timestamp =  msg->header.stamp.toSec()  -   stamp_ins_init;  //Convert timestamp to floating point format
    /*update  position*/
     pose_ins.pos.x()  =  msg->pose.pose.position.x ;
     pose_ins.pos.y()  =  msg->pose.pose.position.y ;
     pose_ins.pos.z()  =  msg->pose.pose.position.z ;
     /*update  orientation*/
     pose_ins.q.w()  =  msg->pose.pose.orientation.w;
     pose_ins.q.x()  =  msg->pose.pose.orientation.x;
     pose_ins.q.y()  =  msg->pose.pose.orientation.y;
     pose_ins.q.z()  =  msg->pose.pose.orientation.z;
     /*write to txt, fomat TUM*/
     WriteText(ins,pose_ins);
}

void  gtCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    if(flag_gt){
        stamp_gt_init = msg->header.stamp.toSec();   
        flag_gt = 0;
    }
        pose_gt.timestamp = msg->header.stamp.toSec() -  stamp_gt_init;
        /*update  position*/
        pose_gt.pos.x()  =  msg->pose.pose.position.x ;
        pose_gt.pos.y()  =  msg->pose.pose.position.y ;
        pose_gt.pos.z()  =  msg->pose.pose.position.z ;
        /*update  orientation*/
        pose_gt.q.w()  =  msg->pose.pose.orientation.w;
        pose_gt.q.x()  =  msg->pose.pose.orientation.x;
        pose_gt.q.y()  =  msg->pose.pose.orientation.y;
        pose_gt.q.z()  =  msg->pose.pose.orientation.z;
        /*write to txt, fomat TUM*/
        WriteText(gt,pose_gt);
}

int main(int argc, char  **argv)
{
    char path_gt[] = "/home/kaho/catkin_ws/src/data/gnss_ins_sim/recorder_gnss_ins_sim/gt.txt";
    char path_ins[] = "/home/kaho/catkin_ws/src/data/gnss_ins_sim/recorder_gnss_ins_sim/ins.txt";

    CreateFile(gt,path_gt );
    CreateFile(ins,path_ins );
    // init  node
    ros::init(argc, argv, "evaluate_node");
    //  create nodehandle
    ros::NodeHandle nh;
    //  create  subscriber
    ros::Subscriber sub_ins  =  nh.subscribe("/pose/estimation", 10, insCallback);
    ros::Subscriber sub_gt    =  nh.subscribe("/pose/ground_truth",10, gtCallback);

    ros::Rate loop_rate(100);      //  frequence 100hz
    while (ros::ok())
    {
        ros::spinOnce();                        //  goto  callback function
        loop_rate.sleep();
    }
    gt.close();
    ins.close();
    return 0;
}

4.Mapviz makes GPS odometer visualization

Main reference Blogs:

Dai Kaige's Zhihu blog:

Inertial navigation system INS570D test

Mapviz for GPS + odometer + LiDAR visualization

4.1.mapviz installation

reference resources: Use Mapviz and zhongketuxin to draw the satellite map of the robot's GPS trajectory

4.2.mapviz calls the sky map api

Because the map loaded by mapviz by default is Stamen's map, which is a foreign website, the loading is very slow Dr. Dai Kai Under the guidance of, select WMTs API using day map to display slice map and display track on it

4.2.1 apply for personal key of tianmap API key

Need first Application day map API KEY Note that when applying, the selected application type is the server. After submitting, you can generate your own tk key.

Map API

4.2.2 map tile acquisition format

according to Map API Write your key in the format of

Format:

http://t0.tianditu.gov.cn/img_ W / WMTs? Service = WMTs & request = gettile & version = 1.0.0 & layer = img & style = Default & tilematrixset = W & format = tiles & tilematrix = {Z} & tilerow = {x} & tilecol = {y} & TK = your key

Note: as shown in the following figure, in mapviz, in tile_ In the map, the format of the added Base URL is

http://Tile. Stamen. COM / Terrain / {level} / {x} / {y}. Png # level x y respectively represents high longitude and latitude

Therefore, mapviz needs to change the z variable in the sky map api retrieval format to the level variable to recognize that the modified sky map api retrieval format is

http://t0.tianditu.gov.cn/img_ W / WMTs? Service = WMTs & request = gettile & version = 1.0.0 & layer = img & style = Default & tilematrixset = W & format = tiles & tilematrix = {level} & tilerow = {x} & tilecol = {y} & TK = your key

Vector basemap

Longitude and latitude projection

http://t0.tianditu.gov.cn/vec_c/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0

Spherical Mercator projection

http://t0.tianditu.gov.cn/vec_w/wmts?
SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0
Vector annotation

Longitude and latitude projection

http://t0.tianditu.gov.cn/cva_c/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0

Spherical Mercator projection

http://t0.tianditu.gov.cn/cva_w/wmts?
SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0
Image base map

Longitude and latitude projection

http://t0.tianditu.gov.cn/img_c/wmts?
SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0

Spherical Mercator projection

http://t0.tianditu.gov.cn/img_w/wmts?
SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0
Topographic shading

Longitude and latitude projection

http://t0.tianditu.gov.cn/ter_c/wmts?
SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0

Spherical Mercator projection

http://t0.tianditu.gov.cn/ter_w/wmts?
SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0
3D topographic image base map
https://[ t0-t7 ].tianditu.gov.cn/mapservice/swdx?
SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0

4.3. Use

4.3.1 add Custom WMTS Source

For example, using the image base map spherical Mercator projection, add the sky map URL + key to the Base URL, and select 99 for Max zoom to increase the resolution

http://t0.tianditu.gov.cn/img_w/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0

4.3.2 add navsat path display

Select the / navsat/fix topic and the appropriate color to draw the path on the satellite map

Note that tile should be added in mapviz first_ Add navsat(new display) to the map

4.3.3 playing data sets

3.3.4 final effect

As shown in the figure below, the red track is the track route map in the square

​ edited by kaho 12.5

Tags: C++ Back-end

Posted on Sun, 05 Dec 2021 14:09:51 -0500 by tvaughan77