一、重定位帧的处理
在process( )函数中,在处理完IMU之后,接着就是对重定位帧的设置,代码片段如下:
//[4] 设置重定位帧setReloFrame( )
sensor_msgs::PointCloudConstPtr relo_msg = NULL;//去看传感器数据的definition
while (!relo_buf.empty())
{
relo_msg = relo_buf.front();// 返回队首元素的值
relo_buf.pop();//删除队列首元素
}
if (relo_msg != NULL)
{
vector<Vector3d> match_points;
double frame_stamp = relo_msg->header.stamp.toSec();
//遍历relo_msg中的points特征点
for (unsigned int i = 0; i < relo_msg->points.size(); i++)
{
Vector3d u_v_id;
u_v_id.x() = relo_msg->points[i].x;
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
//[重定位帧的平移向量T的x,y,z,旋转四元数w,x,y,z和索引值]
Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);//平移
Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);//四元数表示的旋转
Matrix3d relo_r = relo_q.toRotationMatrix();
int frame_index;
frame_index = relo_msg->channels[0].values[7];//索引
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}
relo_buf用来接收relo_msg,从relo_msg中取出点push到match_points,再取出帧的时间戳、帧的id、旋转以及平移量传递给估计器的setReloFrame( )函数。
void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
//[1]将传入的各参数进行赋值给重定位相关的变量
relo_frame_stamp = _frame_stamp;
relo_frame_index = _frame_index;
match_points.clear();
match_points = _match_points;
prev_relo_t = _relo_t;
prev_relo_r = _relo_r;
//【2】遍历滑动窗口,将当前传入的重定位帧的时间戳和滑动窗口中的进行对比
for(int i = 0; i < WINDOW_SIZE; i++)
{
if(relo_frame_stamp == Headers[i].stamp.toSec())
{
relo_frame_local_index = i;//记录滑窗中是那一帧重定位
relocalization_info = 1;//时间戳相等,重定位标志置1
for (int j = 0; j < SIZE_POSE; j++)//SIZE_POSE=7
relo_Pose[j] = para_Pose[i][j];// 把两个匹配的帧的位置和旋转四元数存储起来
}
}
}
二、processImage( )函数
先来看张图:
代码部分为:
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
//image的数据类型分别表示feature_id,camera_id,点的x,y,z坐标,u,v坐标,在x,y方向上的跟踪速度
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
//[1]根据视差来决定marg掉哪一帧,如果次新帧是关键帧,marg掉最老帧,如果次新帧不是关键帧,marg掉次新帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))//添加特征并检测视差