原始的KF-GINS是基于读写文件实现的，在此基础上改进了ros版本，将原始数据文件转换为rosbag格式，并实现了rviz下的可视化结果显示，代码已共享至github
https://github.com/slender1031/kf-gins-ros
感谢武汉大学卫星导航定位技术研究中心多源智能导航实验室(i2Nav)牛小骥教授团队开源的KF-GINS软件平台
https://github.com/i2Nav-WHU/KF-GINS
ros版本KF-GINS（带有rviz可视化结果显示及文件生成）
1 数据格式转换
数据包含GNSS定位结果、IMU原始观测值、配置文件和真值
参考下面这位大佬的博客，在此代码的基础上做了修改，生成了含有gnss与imu数据的rosbag文件
自采数据转rosbag
https://github.com/zzzzyp-sgg/SLAM-Tool
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <fstream>
#include <sstream>
double dt=1.0/200; //imu数据采样间隔
/* imu数据转bag，bag包以IMU数据为基础(因为通常imu数据是时间最长的) */
void imu2bag(rosbag::Bag &bag, const std::string imuFile, const std::string outBag, int gpsWeek)
{
std::ifstream file(imuFile);
if (!file.is_open())
{
ROS_ERROR_STREAM("Failed to open file!");
return;
}
bag.open(outBag, rosbag::bagmode::Write);
std::string line;
while (std::getline(file, line))
{
// 将每行数据分割为各个字段
std::istringstream iss(line);
double time, gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z;
if (!(iss >> time >> gyro_x >> gyro_y >> gyro_z >> accel_x >> accel_y >> accel_z))
{
ROS_WARN_STREAM("Failed to parse line: " << line);
continue;
}
// 创建IMU消息
sensor_msgs::Imu imu_msg;
// 315964800是GPS起始时间和计算机起始时间的一个固定差值
time = time + 315964800 + 604800 * gpsWeek - 8 * 3600;
imu_msg.header.stamp = ros::Time(time);
imu_msg.angular_velocity.x = gyro_x/dt;
imu_msg.angular_velocity.y = gyro_y/dt;
imu_msg.angular_velocity.z = gyro_z/dt;
imu_msg.linear_acceleration.x = accel_x/dt;
imu_msg.linear_acceleration.y = accel_y/dt;
imu_msg.linear_acceleration.z = accel_z/dt;
// 写入ROSbag文件
bag.write("/imu/data", ros::Time(time), imu_msg);
}
bag.close();
file.close();
std::cout << "imu data convert finished!" << std::endl;
}
/* gnss数据转bag */
void gnss2bag(rosbag::Bag &bag, const std::string gnssFile, const std::string outBag, int gpsWeek)
{
std::ifstream file(gnssFile);
if (!file.is_open())
{
ROS_ERROR_STREAM("Failed to open file!");
return;
}
bag.open(outBag, rosbag::bagmode::Append);
std::string line;
while (std::getline(file, line))
{
std::istringstream iss(line);
double time, lat, lon, h, vn, ve, vd;
if (!(iss >> time >> lat >>lon >>h >>vn >>ve >>vd))
{
ROS_WARN_STREAM("Failed to parse line: " << line);
continue;
}
// 创建gnss消息
sensor_msgs::NavSatFix gnss_msg;
// 315964800是GPS起始时间和计算机起始时间的一个固定差值
time = time + 315964800 + 604800 * gpsWeek - 8 * 3600;
gnss_msg.header.stamp = ros::Time(time);
gnss_msg.latitude=lat;
gnss_msg.longitude=lon;
gnss_msg.altitude=h;
gnss_msg.position_covariance[0]=0.005*0.005;
gnss_msg.position_covariance[3]=0.004*0.004;
gnss_msg.position_covariance[6]=0.008*0.008;
// 写入ROSbag文件
bag.write("/gnss", ros::Time(time), gnss_msg);
}
bag.close();
file.close();
std::cout << "gnss data convert finished!" << std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "data_to_rosbag");
ros::NodeHandle nh;
// 创建rosbag文件(注意修改数据文件路径)
rosbag::Bag bag;
int gpsWeek = 2017;
std::string imuFile = "src/data2bag/Leador-A15.txt"; //imu数据文件
std::string gnssFile = "src/data2bag/GNSS-RTK.txt"; //gnss数据文件
std::string outBag ="src/data2bag/output.bag"; //生成的结果文件
imu2bag(bag, imuFile, outBag, gpsWeek); // imu转bag
gnss2bag(bag, gnssFile, outBag, gpsWeek); // gnss转bag
return 0;
}
运行：
rosrun data_convert data_convert_node
生成的rosbag：
2 KF-GINS-ROS
2.1 程序编译与运行
环境配置：
- Ubuntu18.04
- ros-melodic
- Eigen3
代码编译：
扫描二维码关注公众号，回复：
16762576 查看本文章
cd && mkdir /gins_ws/src
git clone https://github.com/slender1031/kf-gins-ros.git
cd ..
catkin_make
运行：
source devel/setup.bash
rosrun data_convert data_convert_node
rosrun gins gins_node [path to YAML]
2.2 rviz可视化效果
rosbag play output.bag
roslaunch gins gins_rviz.launch
这里订阅的是gnss更新时刻，ned系下的路径结果/gins_ned_path，理论上是1s一个，也可以订阅imu时刻
3 程序框架
整体解算流程和kf-gins是一致的，做imu状态预测，然后在gnss时刻做量测更新，改动的部分主要在于输入ros数据流、ros时间戳对齐、topic发布
3.1 ros数据流输入
主函数订阅topic，读入配置文件yaml
int main(int argc, char **argv)
{
ros::init(argc, argv, "gins");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
if(argc != 2)
{
printf("please intput: rosrun gins gins_node [config file] \n");
return 1;
}
string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
readParameters(config_file);
ROS_WARN("waiting for gnss and imu...");
ros::Subscriber sub_imu = n.subscribe(GNSS_TOPIC, 100, gnss_callback);
ros::Subscriber sub_gnss = n.subscribe(IMU_TOPIC, 1000, imu_callback);
pub_ins_odometry = n.advertise<nav_msgs::Odometry>("/insmech_odom", 1000);
pub_ins_path = n.advertise<nav_msgs::Path>("/insmech_path", 1000);
pub_gins_blh = n.advertise<sensor_msgs::NavSatFix>("/gins_blh", 1000);
pub_gins_ned = n.advertise<nav_msgs::Path>("/gins_ned_path", 1000);
std::thread measurement_process{
process};
ros::spin();
return 0;
}
imu和gnss消息的回调函数
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one();
}
void gnss_callback(const sensor_msgs::NavSatFixConstPtr &gnss_msg)
{
m_buf.lock();
gnss_buf.push(gnss_msg);
m_buf.unlock();
con.notify_one();
}
3.2 时间戳对齐
getMeasurements用来划分数据流，以下图中绿色框为一组数据，在最后两个imu时刻做内插，得到gnss时刻对应的观测数据，然后递推到gnss时刻
//IMU与GNSS的数据流做同步，以GNSS的时间间隔，确定一组imu_msg
bool getMeasurements(std::vector<sensor_msgs::ImuConstPtr> &imu_msg, sensor_msgs::NavSatFixConstPtr &gnss_msg)
{
//当前imu和gnss数据都为空
if(imu_buf.empty() || gnss_buf.empty()){
return false;
}
//imu最新的时刻仍然小于gnss最早时刻
if(imu_buf.back()->header.stamp.toSec()<gnss_buf.front()->header.stamp.toSec()){
return false;
}
double front_imu_ts=imu_buf.front()->header.stamp.toSec();
double front_gnss_ts=gnss_buf.front()->header.stamp.toSec();
//第一个imu时间戳大于gnss时间戳，就把gnss数据丢掉
while(!gnss_buf.empty() && front_imu_ts>front_gnss_ts )
{
ROS_WARN("throw gnss_msg, only should happen at the beginning");
gnss_buf.pop();
if(gnss_buf.empty()) return false;
front_gnss_ts=gnss_buf.front()->header.stamp.toSec();
}
gnss_msg=gnss_buf.front();
gnss_buf.pop();
//截取两个gnss时刻中间的imu，放入imu_buf，作为一组imu数据
while (!imu_buf.empty() && imu_buf.front()->header.stamp.toSec() < gnss_msg->header.stamp.toSec())
{
imu_msg.emplace_back(imu_buf.front());
imu_buf.pop();
}
//多取出一个gnss时刻之后的imu数据，用来做内插，对齐到gnss时刻
if(!imu_buf.empty()){
imu_msg.emplace_back(imu_buf.front());
imu_buf.pop();
}
if (imu_msg.empty()){
ROS_WARN("no imu between two GNSS");
}
return true;
}
3.3 结果发布
rviz显示和结果文件输出
void pubGINS(const GINSEngine &gins_engine, const std_msgs::Header &header)
{
sensor_msgs::NavSatFix gps_position;
gps_position.header=header;
gps_position.header.frame_id = "world";
Vector3d blh = gins_engine.pvacur.blh;
gps_position.latitude = blh[0];
gps_position.longitude = blh[1];
gps_position.altitude = blh[2];
for(int i=0; i<9; i++){
gps_position.position_covariance[i]=gins_engine.Cov_(i/3, i%3);
}
pub_gins_blh.publish(gps_position); //发布更新后的BLH坐标
//BLH转NED
if(!first_gins){
first_gins=true;
ned_first=blh;
}
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = header;
pose_stamped.header.frame_id = "world";
Vector3d ned=Earth::global2local(ned_first, blh);
pose_stamped.pose.position.x=ned(0);
pose_stamped.pose.position.y=ned(1);
pose_stamped.pose.position.z=ned(2);
Quaterniond Q=gins_engine.pvacur.qnb;
pose_stamped.pose.orientation.x=Q.x();
pose_stamped.pose.orientation.y=Q.y();
pose_stamped.pose.orientation.z=Q.z();
pose_stamped.pose.orientation.w=Q.w();
Vector3d vec=Q.toRotationMatrix().eulerAngles(2,1,0)*180/M_PI; // [yaw, pitch, roll]
Vector3d euler (vec[2], vec[1], vec[0]);
//Vector3d vec=quaternion2euler(Q)*180/M_PI;
printf("GINS time: %f, t: %f %f %f q: %f %f %f \n", header.stamp.toSec(), blh[0]*180/M_PI, blh[1]*180/M_PI, blh[2], euler(0), euler(1), euler(2));
gins_ned_path.header = header;
gins_ned_path.header.frame_id = "world";
gins_ned_path.poses.push_back(pose_stamped);
pub_gins_ned.publish(gins_ned_path); //发布更新后的ned坐标
}