激光雷达和RTK的标定（无人小车）

总结一下最近的标定工作，标定平台是实验室的无人小车，目标是实现激光雷达(lidar)和RTK的标定，也就是求解lidar到RTK的位姿变换矩阵。采用的代码是ETH的lidar align(https://github.com/ethz-asl/lidar_align)。对其代码的详细解读可以参考：https://zhuanlan.zhihu.com/p/256306094
需要对其读取RTK位姿信息的的代码做一定的修改。RTK所采用的ros消息类型为geometry_msgs/PoseStamped，修改代码如下，将其loader.cpp里的Loader::loadTformFromROSBag函数修改一下

    bool Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom) {
        rosbag::Bag bag;
        try {
            bag.open(bag_path, rosbag::bagmode::Read);
        } catch (rosbag::BagException e) {
            ROS_ERROR_STREAM("LOADING BAG FAILED: " << e.what());
            return false;
        }
         std::vector<std::string> types;
         types.push_back(std::string("geometry_msgs/PoseStamped"));
         rosbag::View view(bag, rosbag::TypeQuery(types));

         size_t tform_num = 0;
         for (const rosbag::MessageInstance& m : view) {
           std::cout << " Loading transform: \e[1m" << tform_num++
                     << "\e[0m from ros bag" << '\r' << std::flush;

           geometry_msgs::PoseStamped transform_msg =
               *(m.instantiate<geometry_msgs::PoseStamped>());

//             sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());

           Timestamp stamp = transform_msg.header.stamp.sec * 1000000ll +
                             transform_msg.header.stamp.nsec / 1000ll;

           Transform T(Transform::Translation(transform_msg.pose.position.x,
                                              transform_msg.pose.position.y,
                                              transform_msg.pose.position.z),
                       Transform::Rotation(transform_msg.pose.orientation.w,
                                           transform_msg.pose.orientation.x,
                                           transform_msg.pose.orientation.y,
                                           transform_msg.pose.orientation.z
                                           ));
           odom->addTransformData(stamp, T);
         }

        if (odom->empty()) {
            ROS_ERROR_STREAM("No odom messages found!");
            return false;
        }

        return true;
    }

录制的包的msg.fields由于pcl库版本的原因可能不含"time_offset_us"，然后就会从intensity中读取时间信息，但实际上代码中，并没有往点云的intensity中添加时间信息，时间戳都保存在private变量timestamps_us_中，而对intensity没有赋值（会赋随机值），从而导致代码运行出错，所以对loader.cpp中的函数void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,LoaderPointcloud* pointcloud)进行修改（增加一行即可）：

void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
                                    LoaderPointcloud* pointcloud) {
        bool has_timing = false;
        bool has_intensity = false;
        for (const sensor_msgs::PointField& field : msg.fields) {
            if (field.name == "time_offset_us") {
                has_timing = true;
            } else if (field.name == "intensity") {
                has_intensity = true;
            }
        }

//        std::cout<<has_intensity<<" "<<has_timing<<std::endl;

        if (has_timing) {
            pcl::fromROSMsg(msg, *pointcloud);
            return;
        } else if (has_intensity) {
            Pointcloud raw_pointcloud;
            pcl::fromROSMsg(msg, raw_pointcloud);

            for (const Point& raw_point : raw_pointcloud) {
                PointAllFields point;
                point.x = raw_point.x;
                point.y = raw_point.y;
                point.z = raw_point.z;
                point.intensity = raw_point.intensity;
                point.time_offset_us=0; #增加这一行即可

                if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
                    !std::isfinite(point.z) || !std::isfinite(point.intensity)) {
                    continue;
                }

                pointcloud->push_back(point);
            }
            pointcloud->header = raw_pointcloud.header;
        } else {
            pcl::PointCloud<pcl::PointXYZ> raw_pointcloud;
            pcl::fromROSMsg(msg, raw_pointcloud);

            for (const pcl::PointXYZ& raw_point : raw_pointcloud) {
                PointAllFields point;
                point.x = raw_point.x;
                point.y = raw_point.y;
                point.z = raw_point.z;

                if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
                    !std::isfinite(point.z)) {
                    continue;
                }

                pointcloud->push_back(point);
            }
            pointcloud->header = raw_pointcloud.header;
        }
    }

提醒

1、实验前可以先录一组数据，使用EVO(https://blog.csdn.net/weixin_47074246/article/details/109134740)画出RTK和lidar的轨迹，使用-a命令看是否可以对齐，这里注意不要使用-s命令，确保RTK和lidar的轨迹之间不存在尺度问题。
2、由于RTK在室内收不到信号，只能在室外采集数据，录rosbag的时候，尽量选择环境结构性强，最近物体或者墙面较多的地方，并且动态物体越少越好。
3、录rosbag的时候，尽量选择有上下坡，并且有较多转弯的地方，使其在yaw和pitch上有充分的运动变换，受限与无人小车的结构，很难在roll上有充分的运动。
4、针对自己的激光，本实验采用的是velodyne vpl16，可以适当修改sensor.h里的参数，实验结果显示将max_point_distance修改为30后取得了不错的效果。
5、优化过程中计算的error是累计误差，rosbag越长，对应的点云帧数也越多，error也相应的会变大，不具有很强的说服力，可以修改为平均误差，具体做法是将aligner.cpp中的Aligner::lidarOdomKNNError函数的返回值由原本的total_error改为total_error/base_pointcloud.size()。

程序运行结束会生成相应的txt和ply文件。
txt文件如下

ply文件可以通过pcl_ply2pcd 1.ply 1.pcd命令将对应的ply文件转化为pcd文件，然后使用pcl_viewer 1.pcd查看具体点云内容，实验点云图如下：

当误差较小并且点云匹配OK的情况下，我们认为标定结果可靠。