#include <svo/config.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/initialization.h>
#include <svo/feature_detection.h>
#include <vikit/math_utils.h>
#include <vikit/homography.h>
namespace svo {
namespace initialization {
InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
{
reset();
detectFeatures(frame_ref, px_ref_, f_ref_);//提取特征点
if(px_ref_.size() < 100)
{//少于100个特征初始化失败,所以初始化要寻找纹理丰富的地方
SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
return FAILURE;
}
frame_ref_ = frame_ref;//第一帧初始化成功则设为参考帧
px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());//当前帧的关键特征点
return SUCCESS;
}
InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
{
//光流法跟踪特征点
trackKlt(frame_ref_/*参考帧也就是第一帧 */, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");
if(disparities_.size() < Config::initMinTracked())
return FAILURE;
//可以把disparity当作视差,该视差为[dx,dy]的模
double disparity = vk::getMedian(disparities_);//返回模中位数,利用nth_element排序,小于n前面,大于n后面
SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
if(disparity < Config::initMinDisparity())
return NO_KEYFRAME;//移动视差小于阈值认为当前帧不是关键帧
//两帧的归一化至相机坐标系下的特征点对计算单应矩阵
computeHomography(
f_ref_, f_cur_,
frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
inliers_, xyz_in_cur_, T_cur_from_ref_);
SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");
if(inliers_.size() < Config::initMinInliers())//内点太少,小于阈值,返回追踪失败
{
SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
return FAILURE;
}
// Rescale the map such that the mean scene depth is equal to the specified scale
//平移向量translation()和深度的尺度保持一致
vector<double> depth_vec;
for(size_t i=0; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());//深度点
double scene_depth_median = vk::getMedian(depth_vec);//深度点中位数
double scale = Config::mapScale()/scene_depth_median;//scale
frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;//当前帧在世界坐标系下的位姿
frame_cur->T_f_w_.translation() =
/*注意-号:tcw平移向量*/ -frame_cur->T_f_w_.rotation_matrix()*/*twc */(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
// For each inlier create 3D point and add feature in both frames共视3D点
SE3 T_world_cur = frame_cur->T_f_w_.inverse();
for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
{
Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
{
Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);//当前帧3D点转到世界坐标系下
Point* new_point = new Point(pos);
//3D点关联当前帧
Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
frame_cur->addFeature(ftr_cur);
new_point->addFrameRef(ftr_cur);//添加当前帧到obs_
//3D点关联参考帧
Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
frame_ref_->addFeature(ftr_ref);
new_point->addFrameRef(ftr_ref);//添加参考帧到obs_
}
}
return SUCCESS;//初始化完成
}
void KltHomographyInit::reset()
{
px_cur_.clear();//清空当前特征点容器,如果要节约内存用swap
frame_ref_.reset();//share_ptr释放参考对象
}
void detectFeatures(
FramePtr frame,
vector<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec)
{
Features new_features;//features list
//FastDetector特征检测器
feature_detection::FastDetector detector(
frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels());
//特征检测函数
detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
// now for all maximum corners, initialize a new seed
px_vec.clear(); px_vec.reserve(new_features.size());
f_vec.clear(); f_vec.reserve(new_features.size());
std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
f_vec.push_back(ftr->f);//获取特征点像素坐标和特征点转至相机坐标系的归一化坐标
delete ftr;
});
}
void trackKlt(
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,/*输出跟踪到的特征点 */
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities)
{
const double klt_win_size = 30.0;//LK窗口大小
const int klt_max_iter = 30;//LK迭代次数
const double klt_eps = 0.001;
vector<uchar> status;
vector<float> error;
vector<float> min_eig_vec;
//调用opencv光流法
cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);
cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
px_ref, px_cur,
status, error,
cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);
vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
vector<Vector3d>::iterator f_ref_it = f_ref.begin();
f_cur.clear(); f_cur.reserve(px_cur.size());
disparities.clear(); disparities.reserve(px_cur.size());
for(size_t i=0; px_ref_it != px_ref.end(); ++i)
{
if(!status[i])//剔除LK失败的点
{
px_ref_it = px_ref.erase(px_ref_it);
px_cur_it = px_cur.erase(px_cur_it);
f_ref_it = f_ref.erase(f_ref_it);
continue;
}
f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y));//当前帧有效特征点归一化到相机坐标系下
//当前LK有效特征点和Ref特征点的Pixel向量模
disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm());
++px_ref_it;
++px_cur_it;
++f_ref_it;
}
}
void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref)
{
vector<Vector2d > uv_ref(f_ref.size());
vector<Vector2d > uv_cur(f_cur.size());
for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
{
uv_ref[i] = vk::project2d(f_ref[i]);//z=1
uv_cur[i] = vk::project2d(f_cur[i]);
}
//创建单应求解器
vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();//求解单应矩阵
vector<int> outliers;
//通过三角化得到深度点,3D投影到两帧得到重投影误差,依据误差判断内外点,具体间math_utils.cpp文件
vk::computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inliers, outliers);
//当前帧相对于参考帧的SE3
T_cur_from_ref = Homography.T_c2_from_c1;
}
} // namespace initialization
} // namespace svo