目前編寫雙目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<sensor_msgs::PointCloud>("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 <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <message_filters/subscriber.h>
#include <ros/time.h>
#include <fstream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <vector>

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<Eigen::Vector2d> &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 "<<std::endl;
        return;
    }
     while (!f.eof()) 
  {
      std::string s;
      std::getline(f,s);
      if(! s.empty())
      {
          std::stringstream ss;
          ss << s;
          int empty;
          ss>>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<double> &time)
{
  std::ifstream f;
  double per_time;
  f.open(filename.c_str());
  
  if(!f.is_open())
    {
        std::cerr << " can't open LoadcamTime file "<<std::endl;
        return;
    }
   
  while (!f.eof()) 
  {
      std::string s;
      std::getline(f,s);
      if(! s.empty())
      {
          std::stringstream ss;
          ss << s;

           
           Eigen::Quaterniond q;
           Eigen::Vector3d t;
           Eigen::Vector3d gyro;
           Eigen::Vector3d acc;

           ss>>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<Eigen::Vector2d> &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; i<point.size(); i++)
    {
      geometry_msgs::Point32 p;
       p.x = point[i](0);
       p.y = point[i](1);
       p.z = 1;
       feature_points->points.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<double> cam_time;
std::vector<std::vector<Eigen::Vector2d>> 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<Eigen::Vector2d> 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<sensor_msgs::PointCloud>("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 <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <message_filters/subscriber.h>
#include <geometry_msgs/Vector3.h>
#include <ros/time.h>
#include <fstream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <vector>

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_Data imu_temp;
  std::ifstream f;
  f.open(filename.c_str());
  
  if(!f.is_open())
    {
        std::cerr << " can't open Loadimu file "<<std::endl;
        return;
    }
   
  while (!f.eof()) 
  {
      std::string s;
      std::getline(f,s);
      if(! s.empty())
      {
          std::stringstream ss;
          ss << s;
          double time;
          Eigen::Quaterniond q;
          Eigen::Vector3d t;
          Eigen::Vector3d gyro;
          Eigen::Vector3d acc;

          ss>>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<double> cam_time;
std::vector<Imu_Data> imu_data;
std::vector<std::vector<Eigen::Vector2d>> 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<sensor_msgs::Imu>("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 ++;
    }
}