FAST_LIO

FAST_LIO copied to clipboard

大佬们好，我是一个刚接触slam、里程计等内容的小白，自己折腾不下去了，前来求助。 我按照提供的GitHub内容进行了Rosbag example的测试，rviz中的显示一切正常，跑不同的bag包有不同的rviz显示结果。下面这张图就是 Avia测试的rviz中的结果，使用的outdoor_Mainbuilding_10hz_2020-12-24-16-38-00.bag。

这应该可以说明，我的一系列环境配置都没问题。但是，我在rosbag play xxx 时，观察了里程计的数据输出（/Odometry话题），发现输出中只有 pose数据，而twist数据全为0。我也测试了其他的bag文件，也进行了 Velodyne的bag文件测试。尽管这些测试都一切正常（rviz中看着挺正常的），但是其/Odometry输出数据中，所有的twist数据全为零。这是为什么呢？twist下不应该有数据吗？

可以参考下面这个gif动画(有些模糊，见谅)。

您知道这是为什么吗？我也并未对clone下来的源码进行改动过。 （下面是自己尝试寻找问题原因的插曲...） 我也读了fastlio的两篇论文，把fastlio的大致流程弄明白了（应该...）。我发现两篇论文给的系统概览图中，都是Converged？都指向了Odometry，但是并未在论文中看见相关的描述（也可能是我没看到...）。然后，我尝试又从源代码中找找Odometry的输出，结果更是看不明白..... 不得已之下，只能向您请教一下了。

我没有输出twist，你可以自己根据需要添加。

请问该在哪里进行设置呢？ 🥺🥺我之前找来找去也没找到 （我啥也不太懂），可以给点提示吗？谢啦 (:з」∠)

我最近在laserMapping.cpp中进行改动，似乎把twist数据中的linear xyz速度输出添加上了，但是angular xyz还没添加上。不知道angular的数据在哪里？请问能给一点提示吗？ 添加linear xyz数据的代码改动如下：

// laserMapping.cpp 约580行处
// 自己加的函数
template<typename T>
void set_twiststamp(T & out)
{
    // 这三个 xyz的速度没问题
    out.twist.linear.x = state_point.vel(0);
    out.twist.linear.y = state_point.vel(1);
    out.twist.linear.z = state_point.vel(2);
    // 没找到 angular 数据储存在哪里
    // out.twist.angular.x = ;
    // out.twist.angular.y = ;
    // out.twist.angular.z = ;
}

void publish_odometry(const ros::Publisher & pubOdomAftMapped)
{
    odomAftMapped.header.frame_id = "camera_init";
    odomAftMapped.child_frame_id = "body";
    odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time);
    set_posestamp(odomAftMapped.pose);
    // 自己加的一个函数   publish_odometry()中仅这里有改动
    set_twiststamp(odomAftMapped.twist); 
    // 发布里程计数据
    pubOdomAftMapped.publish(odomAftMapped);
    auto P = kf.get_P();
    // 后面的内容并未改动，为节省篇幅，已经删去
}

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

您好，不知道您的问题解决了没有。我最近学习了fast-lio的源码，发现代码在设置状态变量的时候并没有将角速度作为状态变量设置，因此无法从state_point里获得。但是imu是可以直接测量角速度的，如果想将角速度整合进odom进行发布，或许可以尝试获取Measures结构体中或imu_buffer中存储的imu角速度。以使用Measures结构体为例，可以将您的set_twiststamp()函数修改如下：

template<typename T>
void set_twiststamp(T & out)
{
    out.twist.linear.x = state_point.vel(0);
    out.twist.linear.y = state_point.vel(1);
    out.twist.linear.z = state_point.vel(2);    
// Measures 结构体定义于common_lib.h文件中
    out.twist.angular.x = Measures.imu.back()->angular_velocity.x;
    out.twist.angular.y = Measures.imu.back()->angular_velocity.y;
    out.twist.angular.z = Measures.imu.back()->angular_velocity.z;
}