导出数据
slam 优化后的enu gnss首帧起点转化的真值的enu gnss的经纬高
是这连个计算均方根误差
运行脚本
#!/bin/sh #延迟5秒执行 sleep 1 echo "1 运行开始 vins_rviz" gnome-terminal -t "1-vins_rviz" -x bash -c "\ cd /home/dongdong/v2_Project/v3_SLAM/v5_VinsFusion/catkin ;\ source devel/setup.bash ;\ roslaunch vins vins_rviz.launch ;\ exit;exec bash;" sleep 3 echo "2 kitti_gps_test_my data" gnome-terminal -t "2-kitti_gps_test_mydata" -x bash -c "\ cd /home/dongdong/v2_Project/v3_SLAM/v5_VinsFusion/catkin;\ source devel/setup.bash;\ rosrun vins kitti_gps_test_my2 /media/dongdong/Elements/fjx/2TestData/2TestData/ehang3/;\ exit;exec bash;" #sleep 1 echo "3 global_fusion" gnome-terminal -t "3-global_fusion" -x bash -c "\ cd /home/dongdong/v2_Project/v3_SLAM/v5_VinsFusion/catkin ;\ source devel/setup.bash ;\ rosrun global_fusion global_fusion_node ;\ exit;exec bash;" sleep 1
相机内参改造
%YAML:1.0
#common parameters
imu: 0
num_of_cam: 2
imu_topic: ""
image0_topic: ""
image1_topic: ""
output_path: "/media/fbz/新加卷/data/EHang2022/v1解压后压缩包备份数据/3_for_testing_return_2022-05-25-17-18-43/vins_out/"
cam0_calib: "cam1.yaml"
cam1_calib: "cam2.yaml"
image_width: 720
image_height: 720
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
#T_cam_imu
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.00348999 ,-0.99999188 , 0.0020138, -0.02343326,
-0.9999876 ,-0.00349711 ,-0.00354508 ,0.01260535,
0.0035521 ,-0.0020014 , -0.99999169 ,-0.01791895,
0, 0, 0, 1]
#T_cam_imu
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.00446047, -0.99998687 , 0.00252315 ,-0.02420532,
-0.99996658, -0.00444307 , 0.00686255, -0.10818298,
-0.00685125 ,-0.00255368 ,-0.99997327 ,-0.01824371,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 0
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.08 # max solver itration time (s), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-4 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
核心 相机双目之间的内参
1 局部状态估计
1 输入畸变矫正
2 双目飞机容易丢失 只开启前90帧初始化
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include <iostream>
#include <stdio.h>
#include <cmath>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include "estimator/estimator.h"
#include "utility/visualization.h"
using namespace std;
using namespace Eigen;
Estimator estimator;
ros::Publisher pubGPS;
void SetImage(cv::Mat &source_img,cv::Mat &dst_img,cv::Mat &cameraMatrix,cv::Mat &distCoeffs){
// double fx= 355.0974745605948;
// double fy= 355.47832693317105;
// double cx= 357.7074039567714;
// double cy= 351.0244037313849;
// double k1= -0.023790306606729556;
// double k2= -0.0007571494794293715;
// double p1= 0.00016452517056601848;
// double p2= -0.0005743824914513448;
// cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
// cv::Mat distCoeffs = (cv::Mat_<float>(4, 1) << k1, k2, p1, p2);
cv::Mat dst_Left;
// 普通图像畸变矫正
//undistort(imLeft, dst_Left, cameraMatrix, distCoeffs);
cv::Size corrected_size(source_img.cols,source_img.rows);
cv::Mat mapx, mapy;
// 鱼眼图像畸变矫正
cv::fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Matx33d::eye(), cameraMatrix, corrected_size, CV_16SC2, mapx, mapy);
remap(source_img, dst_img, mapx, mapy, cv::INTER_LINEAR, cv::BORDER_TRANSPARENT);
// cv::imshow("leftImage", imLeft);
// cv::imshow("dst_Left", dst_Left);
// cv::waitKey(2);
//imLeft=dst_Left;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000);
// if(argc != 2)
// {
// printf("please intput: rosrun vins kitti_gps_test [config file] [data folder] \n"
// "for example: rosrun vins kitti_gps_test "
// "~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml "
// "/media/tony-ws1/disk_D/kitti/2011_10_03/2011_10_03_drive_0027_sync/ \n");
// return 1;
// }
string data_path = argv[1];
printf("data rource: %s\n", data_path.c_str());
string config_file = data_path+"/Vins_Fusion_Config/config.yaml";
printf("config_file: %s\n", config_file.c_str());
string imgpath = data_path+"/FHY_img/";
printf("imgpath file: %s\n",imgpath.c_str());
// 我们的数据 图像和GPS时间戳采集阶段对应好了
string img_timesharp_gps=data_path + "/FHY_config/FHY_gps.txt";
printf("img_timesharp_gps file: %s\n",img_timesharp_gps.c_str());
string dataPath=data_path;
// load image list
FILE* file;
file = std::fopen(img_timesharp_gps.c_str() , "r"); //.c_str()
if(file == NULL){
printf("cannot find file: %s \n", img_timesharp_gps.c_str());
ROS_BREAK();
return 0;
}
vector<string> imageTimeList;
vector<string> GPSTimeList;
vector<vector<string>> GPSDataList;
char timeshara_c[30];
char lat_c[30], lon_c[30], alt_c[30];
string timesharap_s;
string lat_s, lon_s, alt_s;
double timesharap_l;
double lat_l, lon_l, alt_l;
// 相机内参 畸变矫正用
// 左相机
double fx= 355.0974745605948;
double fy= 355.47832693317105;
double cx= 357.7074039567714;
double cy= 351.0244037313849;
double k1= -0.023790306606729556;
double k2= -0.0007571494794293715;
double p1= 0.00016452517056601848;
double p2= -0.0005743824914513448;
cv::Mat cameraMatrix1 = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
cv::Mat distCoeffs1 = (cv::Mat_<float>(4, 1) << k1, k2, p1, p2);
//右相机
fx= 354.57110572187014;
fy= 354.97464271252375;
cx= 384.86917126604794;
cy= 350.73170222126146;
k1= -0.025645869860201013;
k2= 0.0021983043217292503;
p1= -0.001180406728702895;
p2= -0.00021669642909000813;
cv::Mat cameraMatrix2 = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
cv::Mat distCoeffs2 = (cv::Mat_<float>(4, 1) << k1, k2, p1, p2);
// 按照string 读取出来 容易保存超高精度
while (fscanf(file, "%s %s %s %s", ×hara_c, &lat_c, &lon_c, &alt_c) != EOF)
////%lf之间应该有逗号，因为没有逗号只能读第一个数。用&是因为要把数存到对应数组元素的地址中去。\n是换行读取
//while (fscanf(file, "%lf %lf %lf %lf", ×harap_l, &lat_l, &lon_l, &alt_l) != EOF)
{
std::cout<< timeshara_c << " "<<lat_c<< " " << lon_c <<" "<<alt_c<<std::endl;
//std::cout<< timesharap_l << " "<<lat_l<< " " << lon_l <<" "<< alt_l<<std::endl;
timesharap_s=(string(timeshara_c)); //stod
lat_s=(string(lat_c));
lon_s=(string(lon_c));
alt_s=(string(alt_c));
imageTimeList.push_back(timesharap_s);
GPSTimeList.push_back(timesharap_s);
vector<string> gpsdata_i;
gpsdata_i.push_back(timesharap_s);
gpsdata_i.push_back(lat_s);
gpsdata_i.push_back(lon_s);
gpsdata_i.push_back(alt_s);
GPSDataList.push_back(gpsdata_i);
std::cout<< fixed << setprecision(10) <<timesharap_s << ' '<<lat_s<< " " << lon_s <<" "<<alt_s<<std::endl;
}
std::fclose(file);
readParameters(config_file);
estimator.setParameter();
registerPub(n);
FILE* outFile;
outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
if(outFile == NULL)
printf("Output path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());
string leftImagePath, rightImagePath;
cv::Mat imLeft, imRight;
double baseTime;
if(stod(imageTimeList[0]) < stod(GPSTimeList[0]))
baseTime = stod(imageTimeList[0]);
else
baseTime = stod(GPSTimeList[0]);
for (size_t i = 0; i < imageTimeList.size(); i++)
{
if(ros::ok())
{
//printf("base time is %f\n", baseTime);
printf("process image %d\n", (int)i);
// stringstream ss;
// ss << setfill('0') << setw(10) << i;
int begin_id=0;
if(i<begin_id){
continue;
}
leftImagePath = data_path+"/left/" + imageTimeList[i] + ".png";
rightImagePath = data_path+"/right/" + imageTimeList[i] + ".png";
printf("%s\n", leftImagePath.c_str() );
printf("%s\n", rightImagePath.c_str() );
imLeft = cv::imread(leftImagePath, cv::ImreadModes::IMREAD_GRAYSCALE);
if(imLeft.empty() )
//if(imLeft.empty() && imRight.empty()) //判断是否有数据
{
cout<<"Can not read this image !"<<endl;
continue;
}
if( i>begin_id+0 && i<begin_id+90 ){// 保证初始化
imRight = cv::imread(rightImagePath, cv::ImreadModes::IMREAD_GRAYSCALE);
if(imRight.empty() )
//if(imLeft.empty() && imRight.empty()) //判断是否有数据
{
cout<<"Can not read this image !"<<endl;
continue;
}
SetImage(imRight,imRight,cameraMatrix2,distCoeffs2); //右侧相机畸变矫正
}
else{
cv::Mat imRight_;
imRight=imRight_;
}
//=========================对原始图像畸变矫正==========================
cv::Mat dst_Left;
cv::Mat dst_Right;
SetImage(imLeft,imLeft,cameraMatrix1,distCoeffs1); //左相机畸变矫正
//SetImage(imRight,imRight,cameraMatrix2,distCoeffs2); //右侧相机畸变矫正
//imLeft=dst_Left;
//imRight=dst_Right;
//=========================对原始图像畸变矫正==========================
double imgTime = stod(imageTimeList[i]) - (baseTime);
// // load gps
// FILE* GPSFile;
// string GPSFilePath = dataPath + "oxts/data/" + ss.str() + ".txt";
// GPSFile = std::fopen(GPSFilePath.c_str() , "r");
// if(GPSFile == NULL){
// printf("cannot find file: %s\n", GPSFilePath.c_str());
// ROS_BREAK();
// return 0;
// }
double lat, lon, alt, roll, pitch, yaw;
double vn, ve, vf, vl, vu;
double ax, ay, az, af, al, au;
double wx, wy, wz, wf, wl, wu;
double pos_accuracy, vel_accuracy;
double navstat, numsats;
double velmode, orimode;
double gpstime=atof(GPSDataList[i][0].c_str()) ;
lat= atof(GPSDataList[i][1].c_str()) ;
lon= atof(GPSDataList[i][2].c_str()) ;
alt= atof(GPSDataList[i][3].c_str()) ;
// fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &lat, &lon, &alt, &roll, &pitch, &yaw);
// //printf("lat:%lf lon:%lf alt:%lf roll:%lf pitch:%lf yaw:%lf \n", lat, lon, alt, roll, pitch, yaw);
// fscanf(GPSFile, "%lf %lf %lf %lf %lf ", &vn, &ve, &vf, &vl, &vu);
// //printf("vn:%lf ve:%lf vf:%lf vl:%lf vu:%lf \n", vn, ve, vf, vl, vu);
// fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &ax, &ay, &az, &af, &al, &au);
// //printf("ax:%lf ay:%lf az:%lf af:%lf al:%lf au:%lf\n", ax, ay, az, af, al, au);
// fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &wx, &wy, &wz, &wf, &wl, &wu);
// //printf("wx:%lf wy:%lf wz:%lf wf:%lf wl:%lf wu:%lf\n", wx, wy, wz, wf, wl, wu);
// fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &pos_accuracy, &vel_accuracy, &navstat, &numsats, &velmode, &orimode);
// //printf("pos_accuracy:%lf vel_accuracy:%lf navstat:%lf numsats:%lf velmode:%lf orimode:%lf\n",
// // pos_accuracy, vel_accuracy, navstat, numsats, velmode, orimode);
// std::fclose(GPSFile);
// sensor_msgs::NavSatFix gps_position;
// gps_position.header.frame_id = "NED";
// gps_position.header.stamp = ros::Time(imgTime);
// gps_position.status.status = navstat;
// gps_position.status.service = numsats;
// gps_position.latitude = lat;
// gps_position.longitude = lon;
// gps_position.altitude = alt;
// gps_position.position_covariance[0] = pos_accuracy;
sensor_msgs::NavSatFix gps_position;
gps_position.header.frame_id = "NED";
gps_position.header.stamp = ros::Time(imgTime);
gps_position.status.status = 4; //工作状态 kkt正常时候是4
gps_position.status.service = 8; //卫星数目
gps_position.latitude = lat;
gps_position.longitude = lon;
gps_position.altitude = alt;
gps_position.position_covariance[0] = 0.138013155617496; // 速度误差因子 kkt正常数据找的一个 实际应该跟随gps改变 cereas 优化过程中判断条件是否终止
cout<<fixed << setprecision(7) <<"==="<<i << "=== "<<gps_position.header.stamp <<" "<< lat<<" "<<lon << " "<< alt<<endl;
//printf("pos_accuracy %f \n", pos_accuracy);
pubGPS.publish(gps_position);
estimator.inputImage(imgTime, imLeft, imRight);
Eigen::Matrix<double, 4, 4> pose;
estimator.getPoseInWorldFrame(pose);
if(outFile != NULL)
fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
pose(1,0), pose(1,1), pose(1,2),pose(1,3),
pose(2,0), pose(2,1), pose(2,2),pose(2,3));
// cv::imshow("leftImage", imLeft);
// cv::imshow("rightImage", imRight);
// cv::waitKey(2);
}
else
break;
}
if(outFile != NULL)
fclose (outFile);
return 0;
}
2 全局优化进程
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include "ros/ros.h"
#include "globalOpt.h"
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <stdio.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <fstream>
#include <queue>
#include <mutex>
GlobalOptimization globalEstimator;
ros::Publisher pub_global_odometry, pub_global_path, pub_car;
nav_msgs::Path *global_path;
double last_vio_t = -1;
std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
std::mutex m_buf;
void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car)
{
visualization_msgs::MarkerArray markerArray_msg;
visualization_msgs::Marker car_mesh;
car_mesh.header.stamp = ros::Time(t);
car_mesh.header.frame_id = "world";
car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
car_mesh.action = visualization_msgs::Marker::ADD;
car_mesh.id = 0;
car_mesh.mesh_resource = "package://global_fusion/models/car.dae";
Eigen::Matrix3d rot;
rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;
Eigen::Quaterniond Q;
Q = q_w_car * rot;
car_mesh.pose.position.x = t_w_car.x();
car_mesh.pose.position.y = t_w_car.y();
car_mesh.pose.position.z = t_w_car.z();
car_mesh.pose.orientation.w = Q.w();
car_mesh.pose.orientation.x = Q.x();
car_mesh.pose.orientation.y = Q.y();
car_mesh.pose.orientation.z = Q.z();
car_mesh.color.a = 1.0;
car_mesh.color.r = 1.0;
car_mesh.color.g = 0.0;
car_mesh.color.b = 0.0;
float major_scale = 2.0;
car_mesh.scale.x = major_scale;
car_mesh.scale.y = major_scale;
car_mesh.scale.z = major_scale;
markerArray_msg.markers.push_back(car_mesh);
pub_car.publish(markerArray_msg);
}
//GPS 消息订阅
//理解：GPS回调函数，简单，只是把GPS消息存储到了全局变量 gpsQueue 队列里面
void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
//printf("gps_callback! \n");
m_buf.lock();
gpsQueue.push(GPS_msg);
m_buf.unlock();
}
//订阅VIO信息
//(1) 获取 VIO位姿 localPoseMap[t] 和 世界坐标系下的globalPoseMap[t]
void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
//printf("vio_callback! \n");
double t = pose_msg->header.stamp.toSec();
last_vio_t = t;
Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
Eigen::Quaterniond vio_q;
vio_q.w() = pose_msg->pose.pose.orientation.w;
vio_q.x() = pose_msg->pose.pose.orientation.x;
vio_q.y() = pose_msg->pose.pose.orientation.y;
vio_q.z() = pose_msg->pose.pose.orientation.z;
globalEstimator.inputOdom(t, vio_t, vio_q);
m_buf.lock();
while(!gpsQueue.empty())
{
sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
double gps_t = GPS_msg->header.stamp.toSec();
printf("vio t: %f, gps t: %f \n", t, gps_t);
// 10ms sync tolerance
if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
{
//printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
//int numSats = GPS_msg->status.service;
double pos_accuracy = GPS_msg->position_covariance[0];
if(pos_accuracy <= 0)
pos_accuracy = 1;
//printf("receive covariance %lf \n", pos_accuracy);
//if(GPS_msg->status.status > 8)
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if(gps_t < t - 0.01)
gpsQueue.pop();
else if(gps_t > t + 0.01)
break;
}
m_buf.unlock();
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.getGlobalOdom(global_t, global_q);
nav_msgs::Odometry odometry;
odometry.header = pose_msg->header;
odometry.header.frame_id = "world";
odometry.child_frame_id = "world";
odometry.pose.pose.position.x = global_t.x();
odometry.pose.pose.position.y = global_t.y();
odometry.pose.pose.position.z = global_t.z();
odometry.pose.pose.orientation.x = global_q.x();
odometry.pose.pose.orientation.y = global_q.y();
odometry.pose.pose.orientation.z = global_q.z();
odometry.pose.pose.orientation.w = global_q.w();
pub_global_odometry.publish(odometry);
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);
/*******************
// write result to file
std::ofstream foutC(gpssavename, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << global_t.x() << ","
<< global_t.y() << ","
<< global_t.z() << ","
<< global_q.w() << ","
<< global_q.x() << ","
<< global_q.y() << ","
<< global_q.z() << endl;
foutC.close();
******************/
//###########################修改 #############################
// write result to file
std::string gpssavename="/home/dongdong/v2_Project/v3_SLAM/v5_VinsFusion/catkin/src/VINS-Fusion/outdata/vio_global.csv";
std::ofstream foutC(gpssavename, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << pose_msg->header.stamp.toSec() << " ";
foutC.precision(5);
foutC << global_t.x() << " "
<< global_t.y() << " "
<< global_t.z() << " "
<< global_q.w() << " "
<< global_q.x() << " "
<< global_q.y() << " "
<< global_q.z() << endl;
foutC.close();
//###########################修改 #############################
}
int main(int argc, char **argv)
{
//初始化
ros::init(argc, argv, "globalEstimator");
ros::NodeHandle n("~");
//全局优化
global_path = &globalEstimator.global_path;
//订阅GPS信息
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
//订阅VIO信息
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
//发布信息， //这些发布的数据，rviz 订阅然后显示，看一下 rviz的配置文件就知道了！
//配置文件在config里面的 vins_rviz_config.rviz
pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
ros::spin();
return 0;
}
全局优化节点保存数据
这里只能保存时间戳没有名字
globalOpt.cpp
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include "globalOpt.h"
#include "Factors.h"
#include <fstream>
GlobalOptimization::GlobalOptimization()
{
initGPS = false;
newGPS = false;
WGPS_T_WVIO = Eigen::Matrix4d::Identity();
threadOpt = std::thread(&GlobalOptimization::optimize, this);
}
GlobalOptimization::~GlobalOptimization()
{
threadOpt.detach();
}
void GlobalOptimization::GPS2XYZ(double latitude, double longitude, double altitude, double* xyz)
{
if(!initGPS)
{
geoConverter.Reset(latitude, longitude, altitude);
initGPS = true;
}
geoConverter.Forward(latitude, longitude, altitude, xyz[0], xyz[1], xyz[2]);
//printf("la: %f lo: %f al: %f\n", latitude, longitude, altitude);
//printf("gps x: %f y: %f z: %f\n", xyz[0], xyz[1], xyz[2]);
}
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
globalPoseMap[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(t);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = lastP.x();
pose_stamped.pose.position.y = lastP.y();
pose_stamped.pose.position.z = lastP.z();
pose_stamped.pose.orientation.x = lastQ.x();
pose_stamped.pose.orientation.y = lastQ.y();
pose_stamped.pose.orientation.z = lastQ.z();
pose_stamped.pose.orientation.w = lastQ.w();
global_path.header = pose_stamped.header;
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
}
void GlobalOptimization::getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ)
{
odomP = lastP;
odomQ = lastQ;
}
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz);
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy,latitude, longitude, altitude};
//printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);
GPSPositionMap[t] = tmp;
newGPS = true;
}
void GlobalOptimization::optimize()
{
while(true)
{
if(newGPS)
{
newGPS = false;
printf("global optimization\n");
TicToc globalOptimizationTime;
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//options.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//add param
mPoseMap.lock();
int length = localPoseMap.size();
// w^t_i w^q_i
double t_array[length][3];
double q_array[length][4];
map<double, vector<double>>::iterator iter;
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
t_array[i][0] = iter->second[0];
t_array[i][1] = iter->second[1];
t_array[i][2] = iter->second[2];
q_array[i][0] = iter->second[3];
q_array[i][1] = iter->second[4];
q_array[i][2] = iter->second[5];
q_array[i][3] = iter->second[6];
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
}
map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
int i = 0;
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
{
//vio factor
iterVIONext = iterVIO;
iterVIONext++;
if(iterVIONext != localPoseMap.end())
{
Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4],
iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4],
iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
Eigen::Matrix4d iTj = wTi.inverse() * wTj;
Eigen::Quaterniond iQj;
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);
ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
iQj.w(), iQj.x(), iQj.y(), iQj.z(),
0.1, 0.01);
problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
/*
double **para = new double *[4];
para[0] = q_array[i];
para[1] = t_array[i];
para[3] = q_array[i+1];
para[4] = t_array[i+1];
double *tmp_r = new double[6];
double **jaco = new double *[4];
jaco[0] = new double[6 * 4];
jaco[1] = new double[6 * 3];
jaco[2] = new double[6 * 4];
jaco[3] = new double[6 * 3];
vio_function->Evaluate(para, tmp_r, jaco);
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 1>>(tmp_r).transpose() << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[0]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[1]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[2]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[3]) << std::endl
<< std::endl;
*/
}
//gps factor
double t = iterVIO->first;
iterGPS = GPSPositionMap.find(t);
if (iterGPS != GPSPositionMap.end())
{
ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
iterGPS->second[2], iterGPS->second[3]);
//vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy,latitude, longitude, altitude};
// float latitude=iterGPS->second[4];
// float longitude=iterGPS->second[5];
// float altitude=iterGPS->second[6];
//printf("inverse weight %f \n", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
/*
double **para = new double *[1];
para[0] = t_array[i];
double *tmp_r = new double[3];
double **jaco = new double *[1];
jaco[0] = new double[3 * 3];
gps_function->Evaluate(para, tmp_r, jaco);
std::cout << Eigen::Map<Eigen::Matrix<double, 3, 1>>(tmp_r).transpose() << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(jaco[0]) << std::endl
<< std::endl;
*/
}
}
//mPoseMap.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
// update global pose
//mPoseMap.lock();
// save
iter = globalPoseMap.begin();
map<double, vector<double>> globalPoseMap_gps;
for (int i = 0; i < length; i++, iter++)
{
vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
iter->second = globalPose;
if(i == length - 1)
{
Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();
Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
double t = iter->first;
WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4],
localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();
WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);
WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4],
globalPose[5], globalPose[6]).toRotationMatrix();
WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
//vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy,latitude, longitude, altitude};
iterGPS = GPSPositionMap.find(t);
if (iterGPS != GPSPositionMap.end()){
//vector<double> globalPose_t{t_array[i][0], t_array[i][1], t_array[i][2]}; // 优化后的全局坐标xyz
//vector<double> globalPose_gps{iterGPS->second[0],iterGPS->second[1],iterGPS->second[2]}; //// 优化后的全局坐标xyz 优化前的坐标xyz
//vector<double> real_gps{iterGPS->second[4],iterGPS->second[5],iterGPS->second[6]}; //// 优化后的全局坐标xyz 优化前的坐标xyz
//vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy,latitude, longitude, altitude};
vector<double> globalPose_xyz_gps{t_array[i][0], t_array[i][1], t_array[i][2], iterGPS->second[0], iterGPS->second[1], iterGPS->second[2],iterGPS->second[4],iterGPS->second[5],iterGPS->second[6]};
globalPoseMap_gps.insert(pair<double, vector<double>>(t,globalPose_xyz_gps));
}
}
}
std::string gpssavename1="/home/dongdong/v2_Project/v3_SLAM/v5_VinsFusion/catkin/src/VINS-Fusion/outdata/vio_gps_global.csv";
std::ofstream fout_XYZGPS(gpssavename1, ios::app);
fout_XYZGPS.setf(ios::fixed, ios::floatfield);
fout_XYZGPS.precision(6);
map<double, vector<double>>::iterator iter_xyz_gps;
for (iter_xyz_gps = globalPoseMap_gps.begin(); iter_xyz_gps != globalPoseMap_gps.end(); iter_xyz_gps++)
{
double timesharp_=iter_xyz_gps->first;
vector<double> data_xyz_gps=iter_xyz_gps->second;
fout_XYZGPS << timesharp_ << " "
<< data_xyz_gps[0] << " "
<< data_xyz_gps[1] << " "
<< data_xyz_gps[2] << " "
<< data_xyz_gps[3] << " "
<< data_xyz_gps[4] << " "
<< data_xyz_gps[5] << " "
<< data_xyz_gps[6] << " "
<< data_xyz_gps[7] << " "
<< data_xyz_gps[8] << endl;
}
fout_XYZGPS.close();
updateGlobalPath();
//printf("global time %f \n", globalOptimizationTime.toc());
mPoseMap.unlock();
}
std::chrono::milliseconds dura(20);
std::this_thread::sleep_for(dura);
}
return;
}
void GlobalOptimization::updateGlobalPath()
{
global_path.poses.clear();
map<double, vector<double>>::iterator iter;
for (iter = globalPoseMap.begin(); iter != globalPoseMap.end(); iter++)
{
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(iter->first);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = iter->second[0];
pose_stamped.pose.position.y = iter->second[1];
pose_stamped.pose.position.z = iter->second[2];
pose_stamped.pose.orientation.w = iter->second[3];
pose_stamped.pose.orientation.x = iter->second[4];
pose_stamped.pose.orientation.y = iter->second[5];
pose_stamped.pose.orientation.z = iter->second[6];
global_path.poses.push_back(pose_stamped);
}
}
globalOpt.h
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#pragma once
#include <vector>
#include <map>
#include <iostream>
#include <mutex>
#include <thread>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <ceres/ceres.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include "LocalCartesian.hpp"
#include "tic_toc.h"
using namespace std;
class GlobalOptimization
{
public:
GlobalOptimization();
~GlobalOptimization();
void inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy);
void inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ);
void getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ);
nav_msgs::Path global_path;
private:
void GPS2XYZ(double latitude, double longitude, double altitude, double* xyz);
void optimize();
void updateGlobalPath();
// format t, tx,ty,tz,qw,qx,qy,qz
map<double, vector<double>> localPoseMap;
map<double, vector<double>> globalPoseMap;
map<double, vector<double>> GPSPositionMap;
bool initGPS;
bool newGPS;
GeographicLib::LocalCartesian geoConverter;
std::mutex mPoseMap;
Eigen::Matrix4d WGPS_T_WVIO;
Eigen::Vector3d lastP;
Eigen::Quaterniond lastQ;
std::thread threadOpt;
};
浙公网安备 33010602011771号