之前在rosNodeTest.cpp主程序中的setParameter函数开启了新线程processMeasurements，然后开启之前介绍过的sync_process()线程，在该线程中通过inputImage函数将同步的图像输入到估计器中，让我们来看一下inputImage函数
// 输入图像数据的函数
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
inputImageCnt++;// 增加输入图像计数
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;// 定义特征帧数据结构
TicToc featureTrackerTime;// 定义计时器用于跟踪特征提取时间
if(_img1.empty())// 如果只有一个图像（单目相机）
featureFrame = featureTracker.trackImage(t, _img);// 进行单目特征跟踪
else// 如果有两个图像（双目相机）
featureFrame = featureTracker.trackImage(t, _img, _img1);// 进行双目特征跟踪
//printf("featureTracker time: %f\n", featureTrackerTime.toc());
if (SHOW_TRACK)// 如果需要显示跟踪结果
{
cv::Mat imgTrack = featureTracker.getTrackImage();// 获取跟踪结果图像
pubTrackImage(imgTrack, t);// 发布跟踪结果图像
}
if(MULTIPLE_THREAD) // 如果使用多线程
{
if(inputImageCnt % 2 == 0)// 每隔两帧图像处理一次
{
mBuf.lock();// 加锁
featureBuf.push(make_pair(t, featureFrame)); // 将特征帧推入队列
mBuf.unlock(); // 解锁
}
}
else// 如果不使用多线程
{
mBuf.lock();// 加锁
featureBuf.push(make_pair(t, featureFrame));// 将特征帧推入队列
mBuf.unlock();// 解锁
TicToc processTime;// 定义计时器用于测量处理时间
processMeasurements();// 调用processMeasurements()函数来处理测量数据
printf("process time: %f\n", processTime.toc());// 打印处理时间
}
}该函数首先通过调用trackImage函数对输入的图像进行处理存进featureFrame，最后放进featureBuf然后调用processMeasurements()函数来处理数据。
我们接着看下trackImage函数吧
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
{
TicToc t_r;// 计时器，用于记录整个跟踪过程的时间
cur_time = _cur_time;// 更新当前时间
cur_img = _img;// 更新当前图像
row = cur_img.rows;// 获取图像的行数
col = cur_img.cols;// 获取图像的列数
cv::Mat rightImg = _img1;// 如果是双目相机，则获取右图像
/*
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
clahe->apply(cur_img, cur_img);
if(!rightImg.empty())
clahe->apply(rightImg, rightImg);
}
*/
cur_pts.clear();// 清空当前帧的特征点
// 如果有前一帧的特征点，则进行光流跟踪
if (prev_pts.size() > 0)
{
TicToc t_o;// 计时器，用于记录光流跟踪的时间
vector<uchar> status;// 用于存储光流跟踪状态
vector<float> err;// 用于存储光流跟踪误差
if(hasPrediction)// 如果有预测值
{
cur_pts = predict_pts;// 使用预测值作为当前帧的初始特征点
// 进行光流跟踪
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
int succ_num = 0;// 成功跟踪的特征点数量
for (size_t i = 0; i < status.size(); i++)
{
if (status[i])
succ_num++;
}
// 如果成功跟踪的特征点少于10个，则重新进行光流跟踪
if (succ_num < 10)
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
}
// 进行光流跟踪
else
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
// reverse check
// 反向检查光流跟踪结果，以验证光流跟踪的可靠性。
if(FLOW_BACK)
{
vector<uchar> reverse_status;// 定义用于存储反向跟踪状态的向量
vector<cv::Point2f> reverse_pts = prev_pts;// 复制前一帧的特征点到 reverse_pts 中
// 进行反向光流跟踪：从当前帧跟踪到前一帧
cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
//cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3);
// 遍历所有跟踪点，检查正向和反向跟踪的匹配情况
for(size_t i = 0; i < status.size(); i++)
{
// 如果正向和反向跟踪都成功，并且正向和反向的点距离小于等于0.5
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
{
// 保留这个点
status[i] = 1;
}
else
// 否则，丢弃这个点
status[i] = 0;
}
}
// 过滤掉不在图像边界内的特征点
for (int i = 0; i < int(cur_pts.size()); i++)
if (status[i] && !inBorder(cur_pts[i]))
status[i] = 0;
// 根据状态向量过滤特征点、ID和跟踪计数
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
// 输出光流跟踪的时间
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
//printf("track cnt %d\n", (int)ids.size());
}
// 增加每个特征点的跟踪计数
for (auto &n : track_cnt)
n++;
// 如果需要重新检测特征点
if (1)
{
//rejectWithF();
ROS_DEBUG("set mask begins");
TicToc t_m;// 计时器，用于记录设置掩码的时间
setMask();// 设置掩码
ROS_DEBUG("set mask costs %fms", t_m.toc());
ROS_DEBUG("detect feature begins");
TicToc t_t;// 计时器，用于记录特征检测的时间
int n_max_cnt = MAX_CNT - static_cast<int>(cur_pts.size());// 计算需要检测的新特征点数量
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
// 检测新特征点
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %f ms", t_t.toc());
// 将新检测到的特征点加入当前特征点列表
for (auto &p : n_pts)
{
cur_pts.push_back(p);
ids.push_back(n_id++);
track_cnt.push_back(1);
}
//printf("feature cnt after add %d\n", (int)ids.size());
}
// 将当前特征点去畸变
cur_un_pts = undistortedPts(cur_pts, m_camera[0]);
pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);
// 如果是双目相机，进行右图像的特征点跟踪
if(!_img1.empty() && stereo_cam)
{
ids_right.clear();
cur_right_pts.clear();
cur_un_right_pts.clear();
right_pts_velocity.clear();
cur_un_right_pts_map.clear();
if(!cur_pts.empty())
{
//printf("stereo image; track feature on right image\n");
vector<cv::Point2f> reverseLeftPts;
vector<uchar> status, statusRightLeft;
vector<float> err;
// cur left ---- cur right
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
// reverse check cur right ---- cur left
// 反向检查右图到左图光流跟踪
if(FLOW_BACK)
{
cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
for(size_t i = 0; i < status.size(); i++)
{
if(status[i] && statusRightLeft[i] && inBorder(cur_right_pts[i]) && distance(cur_pts[i], reverseLeftPts[i]) <= 0.5)
status[i] = 1;
else
status[i] = 0;
}
}
ids_right = ids;
reduceVector(cur_right_pts, status);
reduceVector(ids_right, status);
// only keep left-right pts
/*
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
reduceVector(cur_un_pts, status);
reduceVector(pts_velocity, status);
*/
cur_un_right_pts = undistortedPts(cur_right_pts, m_camera[1]);
right_pts_velocity = ptsVelocity(ids_right, cur_un_right_pts, cur_un_right_pts_map, prev_un_right_pts_map);
}
prev_un_right_pts_map = cur_un_right_pts_map;
}
// 如果需要显示跟踪结果
if(SHOW_TRACK)
drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);
// 更新前一帧的图像和特征点数据
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
prev_un_pts_map = cur_un_pts_map;
prev_time = cur_time;
hasPrediction = false;
// 更新前一帧的特征点映射
prevLeftPtsMap.clear();
for(size_t i = 0; i < cur_pts.size(); i++)
prevLeftPtsMap[ids[i]] = cur_pts[i];
// 构建特征帧数据结构
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
for (size_t i = 0; i < ids.size(); i++)
{
int feature_id = ids[i];
//去畸变坐标
double x, y ,z;
x = cur_un_pts[i].x;
y = cur_un_pts[i].y;
z = 1;
//像素坐标
double p_u, p_v;
p_u = cur_pts[i].x;
p_v = cur_pts[i].y;
//相机id
int camera_id = 0;
//速度
double velocity_x, velocity_y;
velocity_x = pts_velocity[i].x;
velocity_y = pts_velocity[i].y;
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
// 如果是双目相机，处理右图像的特征帧数据
if (!_img1.empty() && stereo_cam)
{
for (size_t i = 0; i < ids_right.size(); i++)
{
int feature_id = ids_right[i];
double x, y ,z;
x = cur_un_right_pts[i].x;
y = cur_un_right_pts[i].y;
z = 1;
double p_u, p_v;
p_u = cur_right_pts[i].x;
p_v = cur_right_pts[i].y;
int camera_id = 1;
double velocity_x, velocity_y;
velocity_x = right_pts_velocity[i].x;
velocity_y = right_pts_velocity[i].y;
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
}
//printf("feature track whole time %f\n", t_r.toc());
// 返回构建的特征帧数据
return featureFrame;
}
看详细注释的代码即可，简单来说就是通过opencv中的calcOpticalFlowPyrLK函数来实现光流跟踪，并进行反向检验，将跟踪到的特征点进行去畸变，最终将跟踪到的特征点的信息放进featureFrame中返回，如是双目，则右目同理。
其中，undistortedPts函数的作用是对特征点进行去畸变
//对一组二维图像点进行去畸变操作的函数
vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)
{
// 创建一个空的向量来存储去畸变后的点
vector<cv::Point2f> un_pts;
// 对于每个输入的二维图像点，执行以下操作
for (unsigned int i = 0; i < pts.size(); i++)
{
// 将二维图像点转换为 Eigen::Vector2d
Eigen::Vector2d a(pts[i].x, pts[i].y);
// 创建一个空的三维向量来存储去畸变后的点
Eigen::Vector3d b;
// 使用相机模型对二维点进行反投影和去畸变操作
cam->liftProjective(a, b);
// 将去畸变后的三维点转换回二维图像点，并将其存储在 un_pts 中
un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
}
// 返回去畸变后的二维图像点向量
return un_pts;
}ptsVelocity函数是计算当前帧相对于前一帧 特征点沿x,y方向的像素移动速度
//计算了特征点的速度
vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts,
map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)
{
// 存储特征点速度的向量
vector<cv::Point2f> pts_velocity;
// 清空当前ID-点映射
cur_id_pts.clear();
for (unsigned int i = 0; i < ids.size(); i++)
{
// 将特征点ID和对应的点插入到映射中
cur_id_pts.insert(make_pair(ids[i], pts[i]));
}
// caculate points velocity
// 计算特征点的速度
if (!prev_id_pts.empty())
{
// 计算当前帧和上一帧之间的时间差
double dt = cur_time - prev_time;
// 遍历所有特征点
for (unsigned int i = 0; i < pts.size(); i++)
{
// 根据特征点ID在上一帧中查找对应的点
std::map<int, cv::Point2f>::iterator it;
it = prev_id_pts.find(ids[i]);
// 如果找到了对应的点，计算速度
if (it != prev_id_pts.end())
{
// 计算速度分量
double v_x = (pts[i].x - it->second.x) / dt;
double v_y = (pts[i].y - it->second.y) / dt;
// 将速度添加到向量中
pts_velocity.push_back(cv::Point2f(v_x, v_y));
}
else
// 如果没有找到对应的点，将速度设为零向量
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
else
{
// 如果上一帧中的ID-点映射为空，则特征点的速度设为零向量
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
// 返回特征点的速度向量
return pts_velocity;
}drawTrack函数用来绘制特征点在图像上的轨迹
//用于在图像上绘制跟踪特征点的轨迹
void FeatureTracker::drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight,
vector<int> &curLeftIds,
vector<cv::Point2f> &curLeftPts,
vector<cv::Point2f> &curRightPts,
map<int, cv::Point2f> &prevLeftPtsMap)
{
//int rows = imLeft.rows;
// 获取左图像的列数
int cols = imLeft.cols;
// 如果右图像不为空且使用的是立体相机，将左右图像水平拼接
if (!imRight.empty() && stereo_cam)
cv::hconcat(imLeft, imRight, imTrack);
else
// 否则克隆左图像
imTrack = imLeft.clone();
// 将图像转换为彩色图像（灰度到RGB）
cv::cvtColor(imTrack, imTrack, CV_GRAY2RGB);
// 在图像上绘制左图像中的特征点
for (size_t j = 0; j < curLeftPts.size(); j++)
{
// 根据跟踪计数决定颜色
double len = std::min(1.0, 1.0 * track_cnt[j] / 20);
// 绘制特征点
cv::circle(imTrack, curLeftPts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
}
// 如果右图像不为空且使用的是立体相机，在图像上绘制右图像中的特征点
if (!imRight.empty() && stereo_cam)
{
for (size_t i = 0; i < curRightPts.size(); i++)
{
// 将右图像中的特征点坐标的x值加上左图像的列数
cv::Point2f rightPt = curRightPts[i];
rightPt.x += cols;
// 绘制特征点
cv::circle(imTrack, rightPt, 2, cv::Scalar(0, 255, 0), 2);
//cv::Point2f leftPt = curLeftPtsTrackRight[i];
//cv::line(imTrack, leftPt, rightPt, cv::Scalar(0, 255, 0), 1, 8, 0);
}
}
// 绘制左图像中特征点的轨迹
map<int, cv::Point2f>::iterator mapIt;
for (size_t i = 0; i < curLeftIds.size(); i++)
{
int id = curLeftIds[i];
// 查找特征点在前一帧中的位置
mapIt = prevLeftPtsMap.find(id);
if(mapIt != prevLeftPtsMap.end())
{
// 绘制从当前帧特征点到前一帧特征点的箭头
cv::arrowedLine(imTrack, curLeftPts[i], mapIt->second, cv::Scalar(0, 255, 0), 1, 8, 0, 0.2);
}
}
//draw prediction
/*
for(size_t i = 0; i < predict_pts_debug.size(); i++)
{
cv::circle(imTrack, predict_pts_debug[i], 2, cv::Scalar(0, 170, 255), 2);
}
*/
//printf("predict pts size %d \n", (int)predict_pts_debug.size());
//cv::Mat imCur2Compress;
//cv::resize(imCur2, imCur2Compress, cv::Size(cols, rows / 2));
}
setMask函数绘制特征点掩膜
个人理解是可以是特征点分布更加均匀，设置一个mask，用于控制特征点的分布，以避免特征点过于密集。它优先保留跟踪时间较长的特征点，同时在遮罩中标记特征点的位置，确保新特征点不会在这些区域内再次出现
//设置特征点的掩膜
void FeatureTracker::setMask()
{
// 创建一个与图像大小相同的遮罩，初始值为255（白色）
mask = cv::Mat(row, col, CV_8UC1, cv::Scalar(255));
// prefer to keep features that are tracked for long time
// 创建一个向量，用于存储特征点的跟踪计数、点坐标和ID
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
// 遍历当前帧的特征点，将跟踪计数、点坐标和ID存储到向量中
for (unsigned int i = 0; i < cur_pts.size(); i++)
cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i])));
// 按照特征点的跟踪计数排序，优先保留跟踪时间长的特征点
sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
{
return a.first > b.first;
});
// 清空当前帧的特征点、ID和跟踪计数
cur_pts.clear();
ids.clear();
track_cnt.clear();
// 遍历排序后的特征点
for (auto &it : cnt_pts_id)
{
// 如果掩膜中对应位置为白色，表示该特征点没有被遮挡
if (mask.at<uchar>(it.second.first) == 255)
{
// 将特征点、ID和跟踪计数添加到当前帧的特征点、ID和跟踪计数中
cur_pts.push_back(it.second.first);
ids.push_back(it.second.second);
track_cnt.push_back(it.first);
// 将特征点周围一定半径范围内的遮罩像素值设为黑色，表示该区域已经被遮挡
cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
}
}
}
扫一扫
1852
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
