r3lives运行实践(自用)

自己做实验的记录，所以会有许多胡言乱语

r3lives跑作者上传数据重建出来的点云

获取每一帧的点云：

先将包中的custom_msg转换成pointcloud2的数据记录在另一个包里面

参考：大疆livox定制的格式CustomMsg格式转换pointcloud2-CSDN博客

source devel/setup.bash 
roslaunch livox_repub livox_repub.launch

roscore
rosbag record -a
rosbag play XX.bag
rosrun pcl_ros point_to_pcd input:=XX (topic)

完整的点云直接按作者的流程走就可以获得

source devel/setup.bash
roslaunch r3live r3live_bag.launch
rosbag play XX.bag

点数量有些多（3,748,915个点）

打印的log文件：

这里的角度是旋转向量而不是旋转矩阵

现在需要从中获取相机的位姿。lic_lio.log文件里面可以获取到雷达的位姿和相机到雷达的rt矩阵，但是这里的角度是旋转向量而不是旋转矩阵。这里涉及到了李代数，疯狂查补一波。推荐：简单易懂和直接明了

        Eigen::Vector3d rot_angle = Sophus::SO3d( Eigen::Quaterniond( g_lio_state.rot_end ) ).log();
        Eigen::Vector3d rot_ext_i2c_angle = Sophus::SO3d( Eigen::Quaterniond( g_lio_state.rot_ext_i2c ) ).log();

源代码是将旋转矩阵都变成了李代数，现在就要找找怎么把李代数变成旋转矩阵

源码里面计算的公式是：

    vec_3   pose_t = rot_mat * state.pos_ext_i2c + t_vec;
    mat_3_3 R_w2c = rot_mat * state.rot_ext_i2c;

更新一下自己的写的代码

#include <math.h>
#include <thread>
#include <fstream>
#include <string>
#include <sstream>
#include <iostream>
#include <vector>
#include <iterator>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/so3.hpp>
using namespace std;
using namespace Eigen;
 
int  main()
{
 
    std::string filePath = "/home/wxr/catkin_ws/lic_lio.txt";
    std::string outFilePath= "/home/wxr/catkin_ws/pose.txt";
 
    // 打开文件
    std::fstream fileStream;
    fileStream.open(filePath, std::ios::in);
 
    // 判断文件是否打开成功
    if (!fileStream.is_open())
    {
        std::cout << "无法打开文件 " << filePath << "!" << std::endl;
        
    }
    // 写入文件
    std::ofstream outFileStream;
    outFileStream.open(outFilePath, std::ios::out);
 
    if (!outFileStream.is_open())
    {
        std::cout << "无法打开文件 " << outFilePath << "!" << std::endl;
        
    }
    int i = 1;
    // 逐行读取文件内容
    std::string line;
    std::string content;
    while (std::getline(fileStream, line))
    {
    	
        std::istringstream iss(line);
        std::vector<double> data(std::istream_iterator<double>(iss), {});
        double time = data[0];
        Eigen::Vector3d radar_ang(data[1], data[2], data[3]);
        Eigen::Vector3d radar_pose(data[4], data[5], data[6]);
        Eigen::Vector3d rot_ext_ang(data[25], data[26], data[27]);
        Eigen::Vector3d pos_ext_i2c(data[28], data[29], data[30]);
 
        Eigen::Matrix3d rot_mat = Sophus::SO3d::exp(radar_ang).matrix();
        Eigen::Matrix3d rot_ext_mat = Sophus::SO3d::exp(rot_ext_ang).matrix();
        Eigen::Vector3d t_c2w = rot_mat * pos_ext_i2c + radar_pose;
        Eigen::Matrix3d R_c2w = rot_mat * rot_ext_mat;
        Eigen::Quaterniond q_rot;
        q_rot = Eigen::Quaterniond(R_c2w);
        // outFileStream <<"time:"<<std::endl;
        // outFileStream <<std:: fixed <<time+1629514998.21<<std::endl;
        //  outFileStream <<"rot:" <<std::endl;
        outFileStream <<std:: fixed << i<<" " <<q_rot.x() <<" " << q_rot.y() <<" " << q_rot.z() <<" " << q_rot.w()<<" "  << t_c2w[0]<<" "<< t_c2w[1]<<" "<< t_c2w[2]<<" "<< "1" <<" "<<time+1629514998.206884<<std::endl;
       outFileStream <<" " <<std::endl;
        //outFileStream << t_c2w <<std::endl;
        i++;
    }
 
 
    fileStream.close();
    outFileStream.close();
 
    return 0;
}

编译的时候疯狂报错：

In file included from 1.cpp:5:
/usr/local/include/sophus/so3.hpp:574:12: error: ‘std::optional’ has not been declared
  574 |       std::optional<Scalar> const& theta_o = std::nullopt) {
      |            ^~~~~~~~
/usr/local/include/sophus/so3.hpp:574:20: error: expected ‘,’ or ‘...’ before ‘<’ token
  574 |       std::optional<Scalar> const& theta_o = std::nullopt) {
      |                    ^

optiona是C++17的东西编译的时候需要指定一下版本即可

g++ img.cpp -o img -std=c++17

最后，我闪耀的数据就这样出来啦!

下一步就是变成3DGS可以用的格式，我打算用python写，c++还是太陌生了