SLAM： 用测试数据集来测试VINS

目前编写双目VINS代码，发现双目VINS初始化后的尺度存在问题（理想化双目尺度应该为1），所以想着利用无误差的测试集来测试一下自己写的代码。我先在单目VINS下做了测试，下面是我利用github上贺所长的单目测试集在单目vins上测试的过程：

1.单目VINS测试数据集：https://github.com/HeYijia/vio_data_simulation.

（下载后通过mkdir build；cd build; cmake ..; make ; cd ../bin; ./data_gen 命令生成待读取数据，数据存储在bin/里面）

    （1）gener_alldata.cpp :

             A.  createpointslines():   通过这个函数取出设计好的房子图形中的map点，之后所有的图像都能观测到这些map点，且顺  序不变；

            B.  imudata  和 imudata_noise:   只用了imudata；

            C. camdata：  完全由imudata 和map点决定，所以没有误差；

     （2）imudata的生成：imu.cpp

             A. MotionModel:  直接给出imu的线加速度和角加速度 以及imu的R、t；（需要推导）

2.在VINS中新建两个节点分别读取feature数据 和 imu数据：

    （1）其中map点在各帧观测的归一化坐标存储在 vio_data_simulation/bin/keyframe/all_points_x.txt中

          （6维，map3维+1+归一化坐标点2维），

    （2）feature的时间 存储在vio_data_simulation/bin/cam_pose.txt中

           （14维，时间1维+q4维+t3维+陀螺仪加速度3维+线加速度3 维）， 

   （3）imu数据存储在vio_data_simulation/bin/imu_pose.txt（无噪声）

             （ 与cam_pose存储相同，只需要时间和gyro、acc 7维）。

3.节点发布：

      （1）feature发布：注意频率为30

    ros::init(argc, argv, "test_pub_feature");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

    pub_img = n.advertise("feature", 1000);
    test_s();// 读取feature 和 time 数据
    int count = 0;
    ros::Rate loop_rate(30);
    while(ros::ok())
    {
      pub_feature(all_points[count],cam_time[count]);
      ros::spinOnce();
      loop_rate.sleep();
      count ++;
    }

      （2）imu发布：注意频率为200(与上面类似)

4.参数修改：projection_parameters + cam to imu 的 R、t

5.运行结果：refine之前g=9.80418

      

6.完整代码：

test_pub_feature.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

bool PUBLISH = 1;
ros::Publisher pub_img,pub_match,pub_imu;
ros::Publisher pub_restart;
int image_num = 600;
int init_pub = 0;
/*
struct Imu_Data
{
  double time;
  Eigen::Vector3d angular_velocity;
  Eigen::Vector3d linear_acceleration;
};*/

void Load_Feature(std::string filename, std::vector &point)
{
  
  std::ifstream f;
  Eigen::Vector3d pw;
  Eigen::Vector2d pts;
  f.open(filename.c_str());
  
  if(!f.is_open())
    {
        std::cerr << " can't open LoadFeatures file "<>pw(0);
          ss>>pw(1);
          ss>>pw(2);
	  ss>>empty;
          ss>>pts(0);
          ss>>pts(1); 
	  point.push_back(pts);
      }
  }
}

void Load_camTime(std::string filename, std::vector &time)
{
  std::ifstream f;
  double per_time;
  f.open(filename.c_str());
  
  if(!f.is_open())
    {
        std::cerr << " can't open LoadcamTime file "<>per_time;
           ss>>q.w();
           ss>>q.x();
           ss>>q.y();
           ss>>q.z();
           ss>>t(0);
           ss>>t(1);
           ss>>t(2);
           ss>>gyro(0);
           ss>>gyro(1);
           ss>>gyro(2);
           ss>>acc(0);
           ss>>acc(1);
           ss>>acc(2);
	   time.push_back(per_time);
      }
  }
}
void pub_feature(const std::vector &point, const double per_time)
{
    sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
    sensor_msgs::ChannelFloat32 id_of_point;
    sensor_msgs::ChannelFloat32 u_of_point;
    sensor_msgs::ChannelFloat32 v_of_point;
    sensor_msgs::ChannelFloat32 velocity_x_of_point;
    sensor_msgs::ChannelFloat32 velocity_y_of_point;
    ros::Time feature_time(per_time);
    feature_points->header.stamp = feature_time;
    feature_points->header.frame_id = "world";

    for(int i=0; ipoints.push_back(p);
       id_of_point.values.push_back(i);
       u_of_point.values.push_back(0);
       v_of_point.values.push_back(0);
       velocity_x_of_point.values.push_back(0);
       velocity_y_of_point.values.push_back(0);
    }
    
    feature_points->channels.push_back(id_of_point);
    feature_points->channels.push_back(u_of_point);
    feature_points->channels.push_back(v_of_point);
    feature_points->channels.push_back(velocity_x_of_point);
    feature_points->channels.push_back(velocity_y_of_point);
    //ROS_INFO("publish Point %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
    pub_img.publish(feature_points);
}

std::vector cam_time;
std::vector> all_points;
void test_s()
{
  std::stringstream filename_camTime;
  filename_camTime<<"/home/lupingping/slam/catkin_ws/src/data_for_test/vio_data_simulation/bin/cam_pose.txt";

  Load_camTime(filename_camTime.str(), cam_time);
     for(int n = 0; n < image_num; n++)
     { 
      std::stringstream filename_feature;
      filename_feature<<"/home/lupingping/slam/catkin_ws/src/data_for_test/vio_data_simulation/bin/keyframe/all_points_"<< n <<".txt";
      std::vector point;
      Load_Feature(filename_feature.str(), point);
      all_points.push_back(point);
     }  
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_pub_feature");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

    pub_img = n.advertise("feature", 1000);
    test_s();
    int count = 0;
    ros::Rate loop_rate(30);
    while(ros::ok())
    {
      pub_feature(all_points[count],cam_time[count]);
      ros::spinOnce();
      loop_rate.sleep();
      count ++;
    }
}

test_pub_imu.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

bool PUBLISH = 1;
ros::Publisher pub_imu;

struct Imu_Data
{
  double time;
  Eigen::Vector3d angular_velocity;
  Eigen::Vector3d linear_acceleration;
};

void Load_imu(std::string filename, std::vector &imu_data)
{
  Imu_Data imu_temp;
  std::ifstream f;
  f.open(filename.c_str());
  
  if(!f.is_open())
    {
        std::cerr << " can't open Loadimu file "<>time;
          ss>>q.w();
          ss>>q.x();
          ss>>q.y();
          ss>>q.z();
          ss>>t(0);
          ss>>t(1);
          ss>>t(2);
          ss>>gyro(0);
          ss>>gyro(1);
          ss>>gyro(2);
          ss>>acc(0);
          ss>>acc(1);
          ss>>acc(2);

	   
	  imu_temp.time = time;
	  imu_temp.linear_acceleration = acc;
          imu_temp.angular_velocity = gyro;
          imu_data.push_back(imu_temp);
      }
  }
}

void pubImu(const Imu_Data & imu_data)
{
    sensor_msgs::ImuPtr imu(new sensor_msgs::Imu);
    geometry_msgs::Vector3 linear_acceleration;
    geometry_msgs::Vector3 angular_velocity;
    ros::Time imu_time(imu_data.time);
    imu->header.stamp = imu_time;
    linear_acceleration.x = imu_data.linear_acceleration(0);
    linear_acceleration.y = imu_data.linear_acceleration(1);
    linear_acceleration.z = imu_data.linear_acceleration(2);
    imu->linear_acceleration = linear_acceleration;
    angular_velocity.x = imu_data.angular_velocity(0);
    angular_velocity.y = imu_data.angular_velocity(1);
    angular_velocity.z = imu_data.angular_velocity(2);
    imu->angular_velocity = angular_velocity;
    pub_imu.publish(imu);
}

std::vector cam_time;
std::vector imu_data;
std::vector> all_points;
void test_s()
{
  std::stringstream filename_imu;
  filename_imu << "/home/lupingping/slam/catkin_ws/src/data_for_test/vio_data_simulation/bin/imu_pose.txt";
  Load_imu(filename_imu.str(),imu_data); 
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_pub_imu");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    
    pub_imu = n.advertise("imu",1000);
    
    test_s();
    int count = 0;
    ros::Rate loop_rate(200);
    while(ros::ok())
    {
      pubImu(imu_data[count]);
      ros::spinOnce();
      loop_rate.sleep();
      count ++;
    }
}