用于同步加速度计和陀螺仪时间戳保存为txt文件

针对不同频率的加速度计和陀螺仪（对对频率高的降采样）

读取格式：

ACC.txt >> 时间戳 acc.x acc.y acc.z
GYR.txt >> 时间戳 gyr.x gyr.y gyr.z

输出格式:

IMU.txt >> 时间戳 gyr.x gyr.y gyr.z acc.x acc.y acc.z

参考VINS-Fusion时间戳同步、及深蓝VIO课程贺博代码

代码如下:

#include <cmath>
#include <fstream>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <unistd.h>
#include <queue>
#include <map>
#include <thread>
#include <mutex>
using namespace Eigen;
using namespace std;

//why  ：用于加速度计和陀螺仪时间戳同步
struct ACC_MSG
{
    double header;
    Eigen::Vector3f linear_acceleration;
};
typedef std::shared_ptr<ACC_MSG const> AccConstPtr;

struct GYR_MSG
{
    double header;
    Eigen::Vector3f angular_velocity;
};
typedef std::shared_ptr<GYR_MSG const> GyrConstPtr;
std::queue<AccConstPtr> acc_buf;
std::queue<GyrConstPtr> gyr_buf;

const int nDelayTimes = 2;
string sConfig_path = "/media/why/新加卷/corridor/corridor-1-2/"; // txt文件读取路径


void saveIMU(std::string filename, double time, Eigen::Vector3f &vGyr, Eigen::Vector3f &vAcc)
{
    std::ofstream save_imu;
    save_imu.setf(std::ios::fixed, std::ios::floatfield);
    save_imu.open(filename.c_str(), std::ios::app);
    save_imu << time << " "
             << vGyr.x() << " "
             << vGyr.y() << " "
             << vGyr.z() << " "
             << vAcc.x() << " "
             << vAcc.y() << " "
             << vAcc.z() << std::endl;
    save_imu.close();
}

void PubAccData()
{
	string sAcc_data_file = sConfig_path + "d400_accelerometer.txt";

	cout << "1.1 AccData start sAcc_data_file: " << sAcc_data_file << endl;

	ifstream fsAcc;
	fsAcc.open(sAcc_data_file.c_str());
	if (!fsAcc.is_open())
	{
		cerr << "Failed to open imu file! " << sAcc_data_file << endl;
		return;
	}

	std::string sAcc_line, sGyr_line;
	double dStampNSec = 0.0;
	Vector3f vAcc;
	while (std::getline(fsAcc, sAcc_line) && !sAcc_line.empty()) // read imu data
	{
		std::istringstream ssImuData(sAcc_line);
		ssImuData >> dStampNSec >> vAcc.x() >> vAcc.y() >> vAcc.z();
        shared_ptr<ACC_MSG> acc_msg(new ACC_MSG());

        acc_msg->header = dStampNSec;
        acc_msg->linear_acceleration = vAcc;
        acc_buf.push(acc_msg);
	}
	fsAcc.close();
}

void PubGyrData()
{
	string sGyr_data_file = sConfig_path + "d400_gyroscope.txt";
	cout << "1.2 GyrData start sGyr_data_file: " << sGyr_data_file << endl;

	ifstream fsGyr;
	fsGyr.open(sGyr_data_file.c_str());
	if (!fsGyr.is_open())
	{
		cerr << "Failed to open imu file! " << sGyr_data_file << endl;
		return;
	}

	std::string sGyr_line;
	double dStampNSec = 0.0;
	Vector3f vGyr;
	while (std::getline(fsGyr, sGyr_line) && !sGyr_line.empty()) // read imu data
	{
		std::istringstream ssImuData(sGyr_line);
		ssImuData >> dStampNSec >> vGyr.x() >> vGyr.y() >> vGyr.z();
        shared_ptr<GYR_MSG> gyr_msg(new GYR_MSG());

        gyr_msg->header = dStampNSec;
        gyr_msg->angular_velocity = vGyr;
        gyr_buf.push(gyr_msg);
	}
	fsGyr.close();
}

void sync_process()
{
    while (1)
    {
        Eigen::Vector3f acceleration;
        Eigen::Vector3f angular;
        double time = 0;
        if (!acc_buf.empty() && !gyr_buf.empty())
        {
            double time0 = acc_buf.front()->header;
            double time1 = gyr_buf.front()->header;
            time0 = floor(time0 * 100) * 0.01;
            time1 = floor(time1 * 100) * 0.01;

            if (time0 < time1)
            {
                acc_buf.pop();
                printf("throw acceleration\n");
            }
            else if (time0 > time1)
            {
                gyr_buf.pop();
                printf("throw angular\n");
            }
            else
            {
                time = gyr_buf.front()->header;
                //header = acc_buf.front()->header;
                acceleration = acc_buf.front()->linear_acceleration;
                acc_buf.pop();
                angular = gyr_buf.front()->angular_velocity;
                gyr_buf.pop();
                printf("find acceleration and angular\n");
                if (acceleration.size() != 0)
                {
                    saveIMU("imu.txt", time, angular, acceleration);
                }
            }
        }

    }
}

int main()
{
    PubAccData();
    PubGyrData();
    sync_process();
}