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
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.
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