realsenseD435i跑PL-VINS

修改配置文件

先把PL-VINS跑起来，然后仿照用D435i跑VINS-mono的步骤，修改配置文件。
把vinsmono的realsense_color.launch文件拷贝进PL-VINS/vins_estimator/launch目录中，记得添加线特征的节点，参考PL-VINS的plvins-show-linepoint.launch文件。
把VINS-Mono/config/realsense/realsense_color_config.yaml配置文件拷贝到PL-VINS的配置文件目录下。
同时，要修改这上述两个文件中的内容（主要是一些名称）和PL-VINS对应上。
最后roslaunch realsense_color.launch和D435i的rs_camera.launch 就可以了。

遇到问题及解决

问题描述

笔者可以正常跑起来，但在rviz中的可视化界面看不到提取的点线特征图像，显示No Image
在左侧Displays中的tracked image下拉的status项显示状态为warn，并提示no image received。而且似乎整个线特征提取的节点没有正常地提取线特征。

解决

阅读代码发现在image_node_b.cpp中直接硬编码读了eruoc的config，所以这个image_node_b这个节点整个就不对了。
解决该问题最方便的方法是改掉main中

m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(）

的参数，改为config/realsense/realsense_color_config.yaml，然后改话题名

message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/cam0/image_raw", 1000);
改为:
message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/camera/color/image_raw", 1000);

问题解决，可以正常显示tracked image，并且线特征提取的节点可以正常提取线特征。

更优雅的解决方案（暂未实现，仍然报错）

上面把程序写死了，如果想要切换运行方式还要改代码再编译，十分窘迫。
尝试修改launch文件和代码，更优雅地启动程序。
在image_node_b.cpp中添加如下代码，放在主函数之前

std::string IMAGE_TOPIC;
std::vector<std::string> CAM_NAMES;
template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
    T ans;
    if (n.getParam(name, ans))
    {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    }
    else
    {
        ROS_ERROR_STREAM("Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

void readParameters(ros::NodeHandle &n)
{
    std::string config_file;
    config_file = readParam<std::string>(n, "config_file");
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }

    fsSettings["image_topic"] >> IMAGE_TOPIC;
    CAM_NAMES.push_back(config_file);
    fsSettings.release();
}

并将主函数中读入参数的部分改为如下

  ros::init(argc, argv, "sync_control_node"); 
  ros::NodeHandle nh;
  readParameters(nh);
  m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(CAM_NAMES[0]);
  K_ = m_camera->initUndistortRectifyMap(undist_map1_,undist_map2_);  

  message_filters::Subscriber<sensor_msgs::PointCloud> point_feature_sub(nh, "/feature_tracker/feature",1000); 
  message_filters::Subscriber<sensor_msgs::PointCloud> line_feature_sub(nh, "/linefeature_tracker/linefeature", 1000);
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, IMAGE_TOPIC, 1000);

在realsense_color.launch中的image_node_b这个node下添加参数config_file

    <node name="image_node_b" pkg="image_node_b" type="image_node_b" output="log">
    	<param name="config_file" type="string" value="$(arg config_path)" />
    </node>   

在Nvidia NX上运行

按照之前readme的步骤改了之后，catkin_make报错

PL-VINS/feature_tracker/src/line_descriptor/lib/liblinedesc.so: error adding symbols: file in wrong format

这可能是cpu架构的问题导致的格式错误，用file命令查看此文件格式，显示为x86架构，而NX内核为arm架构。

解决方法

进入PL-VINS/feature_tracker/src/line_descriptor/目录下，重新cmake ，make问题解决。