先捋一下顺序
前边是从项目入口rosNodeTest.cpp的主线程中读取配置文件，然后设置估计器参数，订阅一系列回调函数，然后在sync_process线程中通过inputImage函数经trackImage函数完成对特征点进行lk光流追踪后构建featureFrame，并将其图像信息放入featureBuf后，最后调用processMeasurements()函数要对测量数据进行处理
processMeasurements()这个函数是用来处理测量数据的主要循环。它从 featureBuf 中获取特征数据，同时等待IMU数据（如果启用了IMU），然后对图像和IMU数据进行处理。
首先看一下源码：
//这个函数是用来处理测量数据的主要循环。它从 featureBuf 中获取特征数据，同时等待IMU数据（如果启用了IMU），然后对图像和IMU数据进行处理
void Estimator::processMeasurements()
{
//这里开始一个无限循环，一直等待处理测量数据。
while (1)
{
//printf("process measurments\n");
//定义了一个 feature 变量，其中包含时间戳和特征点数据的映射。
pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
//定义了用于存储IMU数据的向量。
vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
if(!featureBuf.empty())
{
//如果特征点缓冲区不为空，就从中获取下一个特征数据。
feature = featureBuf.front();
// 计算当前时间（特征数据的时间戳加上时间偏移）
curTime = feature.first + td;
// 等待IMU数据，直到IMU数据在当前时间戳可用
while(1)
{
// 如果不使用IMU或者IMU数据在当前时间戳处可用，则退出循环
if ((!USE_IMU || IMUAvailable(feature.first + td)))
break;
// 否则，等待IMU数据
else
{
printf("wait for imu ... \n");
// 如果没有启用多线程，直接返回
if (! MULTIPLE_THREAD)
return;
// 等待5毫秒
std::chrono::milliseconds dura(5);
std::this_thread::sleep_for(dura);
}
}
// 加锁，获取IMU数据并存储到accVector和gyrVector中
mBuf.lock();
if(USE_IMU)
getIMUInterval(prevTime, curTime, accVector, gyrVector);
// 从特征数据缓冲区中移除已处理的特征数据
featureBuf.pop();
mBuf.unlock();
// 如果使用IMU数据
if(USE_IMU)
{
// 如果尚未初始化第一个姿态，先初始化第一个IMU姿态
if(!initFirstPoseFlag)
initFirstIMUPose(accVector);
// 对每个时间间隔的IMU数据进行处理
for(size_t i = 0; i < accVector.size(); i++)
{
double dt;
// 计算时间间隔
if(i == 0)
dt = accVector[i].first - prevTime;// 第一个数据包的时间戳与上一个处理的时间戳之差
else if (i == accVector.size() - 1)
dt = curTime - accVector[i - 1].first;// 当前时间与最后一个数据包的时间戳之差
else
dt = accVector[i].first - accVector[i - 1].first;// 当前数据包的时间戳与前一个数据包的时间戳之差
// 调用processIMU处理IMU数据
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
}
}
// 加锁，调用processImage函数处理图像数据
mProcess.lock();
processImage(feature.second, feature.first);
prevTime = curTime;
// 打印统计信息
printStatistics(*this, 0);
// 设置ROS消息头部，并发布相关数据
std_msgs::Header header;
header.frame_id = "world";
header.stamp = ros::Time(feature.first);
pubOdometry(*this, header);
pubKeyPoses(*this, header);
pubCameraPose(*this, header);
pubPointCloud(*this, header);
pubKeyframe(*this);
pubTF(*this, header);
mProcess.unlock();
}
// 如果没有启用多线程，退出主循环
if (! MULTIPLE_THREAD)
break;
// 短暂休眠以减少处理器资源的占用
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}看这个函数，我们首先定义了一些变量，用于存储相应数据，然后如果featureBuf不为空，那么我们取出队列中第一个开始处理
我们等待IMU数据，通过IMUAvailable函数来判断IMU数据是否在当前时间戳可用，如果使用IMU，那么通过getIMUInterval函数将imu数据读取进开始设置的变量accVector, gyrVector中，然后没有初始化则调用initFirstIMUPose对imu信息进行初始化，最后计算完时间间隔后，调用processIMU函数来完成对IMU数据的处理，至此，有关imu数据的处理流程结束
接下来调用processImage函数实现对图像数据的处理（下节介绍）
最后，设置ros消息头，并发布相关数据
我们接下来看看与处理IMU数据相关的函数
首先是IMUAvailable，核心就是加速度计里有数据，并且当前时间在加速度计里最后一个数据的时间点前，那么认为当前时间点就是有IMU数据的
bool Estimator::IMUAvailable(double t)
{
//检查加速度计数据缓冲区 accBuf 是否不为空
//并且 检查给定的时间 t 是否在加速度计缓冲区中最后一个数据点的时间点前或刚好等于（确保此时间点有imu数据）。
if(!accBuf.empty() && t <= accBuf.back().first)
return true;
else
return false;
} getIMUInterval函数，该函数确保在给定的时间范围内（从 t0 到 t1）提取IMU数据，并将其存储在 accVector 和 gyrVector 中。如果在 t1 时刻没有足够的IMU数据，函数将返回 false。
//该函数确保在给定的时间范围内（从 t0 到 t1）提取IMU数据，并将其存储在 accVector 和 gyrVector 中。
bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector,
vector<pair<double, Eigen::Vector3d>> &gyrVector)
{
// 检查加速度计缓冲区是否为空
if(accBuf.empty())
{
// 如果为空，打印消息并返回 false
printf("not receive imu\n");
return false;
}
//printf("get imu from %f %f\n", t0, t1);
//printf("imu fornt time %f imu end time %f\n", accBuf.front().first, accBuf.back().first);
// 检查目标时间范围的结束时间 t1 是否小于或等于IMU缓冲区中最后一个数据点的时间
if(t1 <= accBuf.back().first)
{
// 如果 t1 小于或等于缓冲区中最后一个数据点的时间，执行以下操作
// 从缓冲区中移除时间小于或等于 t0 的所有数据点
while (accBuf.front().first <= t0)
{
accBuf.pop();
gyrBuf.pop();
}
// 将时间在 t0 和 t1 之间的IMU数据添加到输出向量中
while (accBuf.front().first < t1)
{
accVector.push_back(accBuf.front());
accBuf.pop();
gyrVector.push_back(gyrBuf.front());
gyrBuf.pop();
}
// 添加时间等于 t1 的IMU数据点到输出向量中
accVector.push_back(accBuf.front());
gyrVector.push_back(gyrBuf.front());
}
else
{
// 如果 t1 大于缓冲区中最后一个数据点的时间，打印消息并返回 false
printf("wait for imu\n");
return false;
}
// 返回 true 表示成功获取IMU数据
return true;
}initFirstIMUPose函数是初始化imu姿态
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)
{
// 打印初始化第一个IMU姿态的消息
printf("init first imu pose\n");
// 设置初始姿态标志为 true
initFirstPoseFlag = true;
//return;
// 初始化一个三维向量 averAcc，表示平均加速度，初始值为零向量。
Eigen::Vector3d averAcc(0, 0, 0);
// 获取加速度向量的大小并存储在变量 n 中。
int n = (int)accVector.size();
// 遍历 accVector，计算所有加速度数据的总和。
for(size_t i = 0; i < accVector.size(); i++)
{
averAcc = averAcc + accVector[i].second;
}
// 计算加速度向量的平均值
averAcc = averAcc / n;
// 打印平均加速度向量的值
printf("averge acc %f %f %f\n", averAcc.x(), averAcc.y(), averAcc.z());
// 调用 Utility 类的 g2R 函数，根据平均加速度向量计算初始旋转矩阵 R0
Matrix3d R0 = Utility::g2R(averAcc);
// 从旋转矩阵 R0 中提取偏航角 yaw
double yaw = Utility::R2ypr(R0).x();
// 根据 yaw 计算校正后的旋转矩阵 R0
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
// 将校正后的旋转矩阵 R0 赋值给姿态数组 Rs 的第一个元素
Rs[0] = R0;
// 打印初始旋转矩阵 R0
cout << "init R0 " << endl << Rs[0] << endl;
//Vs[0] = Vector3d(5, 0, 0);
}processIMU函数处理IMU数据，得到预积分值，让我们来看一下源码
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
// 如果是第一次处理IMU数据，初始化第一次的加速度和角速度
if (!first_imu)
{
//设置标志位 first_imu 为 true，表示已经处理了第一次IMU数据
first_imu = true;
//将当前线性加速度赋值给 acc_0
acc_0 = linear_acceleration;
//将当前角速度赋值给 gyr_0
gyr_0 = angular_velocity;
}
// 如果当前帧的预积分对象不存在，创建一个新的预积分对象
if (!pre_integrations[frame_count])
{
//创建新的预积分对象，初始化参数包括加速度、角速度、加速度偏差和角速度偏差
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
// 如果当前不是第一帧，进行预积分和状态更新
if (frame_count != 0)
{
//将当前时间间隔、线性加速度和角速度加入预积分对象
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
//临时预积分对象也加入相同的数据（非线性求解时需要）
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
//将时间间隔存入缓冲区
dt_buf[frame_count].push_back(dt);
//将线性加速度存入缓冲区。
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
//将角速度存入缓冲区
angular_velocity_buf[frame_count].push_back(angular_velocity);
//设置当前帧索引
int j = frame_count;
//计算去除重力后的未校正加速度 un_acc_0。
//将IMU测量的加速度数据转换到世界坐标系，并去除了重力的影响
//Rs[j]：当前帧的旋转矩阵，将IMU坐标系下的向量转换到世界坐标系下。
//acc_0：前一时刻的线性加速度（IMU测量值）。
//Bas[j]：当前帧的加速度计偏差
//g：重力向量
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
//计算未校正的角速度 un_gyr。
//gyr_0是前一时刻的角速度（通常为上一个IMU数据包的角速度）
//angular_velocity：当前时刻的角速度（当前IMU数据包的角速度）。
//0.5 * (gyr_0 + angular_velocity)：计算当前和前一时刻角速度的平均值。
//Bgs[j]：当前帧的陀螺仪偏差
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
//更新旋转矩阵 Rs[j]。
//un_gyr * dt得到一个微小旋转量的角度矢量，然后构造一个四元数Q，再转成旋转矩阵R
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
//计算当前加速度下的未校正加速度 un_acc_1。
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
//计算平均未校正加速度 un_acc
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
//更新位置 Ps[j]
//经典的运动学方程
//dt * Vs[j]：初始速度Vs[j]在时间 dt 内产生的位移
//0.5 * dt * dt * un_acc：加速度在时间 dt 内产生的位移。
//再加上初始位置，就得到更新后的位置
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
//更新速度 Vs[j]
Vs[j] += dt * un_acc;
}
//更新当前加速度。
acc_0 = linear_acceleration;
//更新当前角速度
gyr_0 = angular_velocity;
}
得到当前帧的预积分对象，并计算当前帧的位置和速度
对图像数据处理的函数见下节
扫一扫
420
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
