重投影小结
目的是在一个立体视觉系统中，通过直接法（direct method）在当前帧（
cur_frame）中寻找与参考帧（
ref_frame）中给定特征（
ref_ftr）的匹配点。函数使用相机运动（
T_cur_ref）和特征深度的倒数估计（
d_estimate_inv）来指导搜索。
1. 功能总结：
-
计算极线：函数首先计算参考特征在当前帧中的极线起点和终点，这是通过将参考特征的三维坐标通过相机运动变换到当前帧，并根据最小和最大深度的倒数投影到图像平面上得到的。
-
计算仿射变换矩阵：接着计算一个仿射变换矩阵，用于将参考特征的图像区域变换到当前帧的图像尺度和方向。
-
特征预筛选：如果特征是边缘特征（edgelet），并且开启了边缘特征筛选选项，函数会检查特征的方向是否与极线方向一致，如果不一致，则拒绝匹配。
-
匹配准备：确定最佳的搜索级别，并使用仿射变换矩阵对参考特征的图像块进行变换。
-
直接搜索：如果极线长度太短，则在极线中点附近进行局部搜索以找到匹配点。
-
沿极线搜索：如果极线长度足够，函数会沿极线搜索最佳匹配点。
-
匹配结果处理：如果找到的匹配点质量足够好，函数会进行亚像素级的精细化处理，并计算匹配点的深度。
2. Matcher::findEpipolarMatchDirect接口描述
接口名称Matcher::findEpipolarMatchDirect
输入参数
ref_frame: 参考帧（const Frame&），包含参考特征所在的图像信息。cur_frame: 当前帧（const Frame&），包含需要搜索匹配的特征的图像信息。T_cur_ref: 从当前帧到参考帧的变换（const Transformation&），用于计算极线和特征的变换。ref_ftr: 参考特征（const FeatureWrapper&），需要在当前帧中找到匹配的特征。d_estimate_inv: 估计的深度倒数（const double），用于计算仿射变换矩阵。d_min_inv: 最小深度的倒数（const double），用于计算极线的起点。d_max_inv: 最大深度的倒数（const double），用于计算极线的终点。
输出参数
depth: 匹配特征的深度（double&），用于输出。
输出
MatchResult: 匹配结果，表示匹配是否成功以及成功的原因。
功能和目的
该函数的目的是在立体视觉系统中，通过直接法在当前帧中寻找与参考帧中给定特征的匹配点。它使用相机运动和特征深度的倒数估计来指导搜索，通过计算极线和仿射变换矩阵来优化匹配过程。最终，该函数能够提供匹配特征的深度信息。
算法流程及步骤性描述
-
初始化最佳ZMSSD值：
- 设置
zmssd_best为PatchScore::threshold()的值，这是用于后续匹配评分的阈值。 - 代码行：
int zmssd_best = PatchScore::threshold();
- 设置
-
计算极线端点：
- 通过相机运动
T_cur_ref变换参考特征ref_ftr.f，并乘以最小和最大深度倒数d_min_inv和d_max_inv，得到极线在图像平面上的起点A和终点B。 - 代码行：
const BearingVector A = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_min_inv;和const BearingVector B = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_max_inv; - 将三维点
A和B投影到图像平面上，得到像素坐标px_A和px_B。 - 代码行：
cur_frame.cam()->project3(A, &px_A);和cur_frame.cam()->project3(B, &px_B); - 计算极线方向向量
epi_image_。 - 代码行：
epi_image_ = px_A - px_B;
- 通过相机运动
-
计算仿射变换矩阵：
- 计算仿射变换矩阵
A_cur_ref_，用于将参考特征的图像区域变换到当前帧的图像尺度和方向。 - 代码行：
warp::getWarpMatrixAffine(...)
- 计算仿射变换矩阵
-
特征预筛选：
- 如果特征是边缘特征（edgelet），并且开启了边缘特征筛选选项，函数会检查特征的方向是否与极线方向一致，如果不一致，则拒绝匹配。
- 代码行：
if (isEdgelet(ref_ftr.type) && options_.epi_search_edgelet_filtering) {...}
-
匹配准备：
- 确定最佳的搜索级别，并使用仿射变换矩阵对参考特征的图像块进行变换。
- 代码行：
search_level_ = warp::getBestSearchLevel(A_cur_ref_, ref_frame.img_pyr_.size() - 1);和if (!warp::warpAffine(...)) return MatchResult::kFailWarp;
-
直接搜索：
- 如果极线长度太短，则在极线中点附近进行局部搜索以找到匹配点。
- 代码行：
if (epi_length_pyramid_ < 2.0) {...}
-
沿极线搜索：
- 如果极线长度足够，函数会沿极线搜索最佳匹配点。
- 代码行：
scanEpipolarLine(...)
-
匹配结果处理：
- 如果找到的匹配点质量足够好，函数会进行亚像素级的精细化处理，并计算匹配点的深度。
- 如果最佳匹配评分
zmssd_best小于阈值，返回成功匹配，并计算深度。 - 代码行：
if (zmssd_best < PatchScore::threshold()) {...}
-
返回匹配结果：
- 如果匹配失败，返回相应的失败原因。
- 代码行：
return MatchResult::kFailScore;
这个函数通过计算极线和仿射变换矩阵来优化匹配过程，提高了匹配的效率和准确性，并能够提供匹配特征的深度信息。
Matcher::MatchResult Matcher::findEpipolarMatchDirect(
const Frame& ref_frame,
const Frame& cur_frame,
const Transformation& T_cur_ref,
const FeatureWrapper& ref_ftr,
const double d_estimate_inv,
const double d_min_inv,
const double d_max_inv,
double& depth)
{
int zmssd_best = PatchScore::threshold();
// Compute start and end of epipolar line in old_kf for match search, on image plane
const BearingVector A = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_min_inv;
const BearingVector B = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_max_inv;
Eigen::Vector2d px_A, px_B;
cur_frame.cam()->project3(A, &px_A);
cur_frame.cam()->project3(B, &px_B);
epi_image_ = px_A - px_B;
//LOG(INFO) << "A:" << A;
//LOG(INFO) << "B:" << B;
//LOG(INFO) << "px_A:" << px_A;
//LOG(INFO) << "px_B:" << px_B;
// Compute affine warp matrix
warp::getWarpMatrixAffine(
ref_frame.cam_, cur_frame.cam_, ref_ftr.px, ref_ftr.f,
1.0 / std::max(0.000001, d_estimate_inv), T_cur_ref, ref_ftr.level, &A_cur_ref_);
// feature pre-selection
reject_ = false;
if (isEdgelet(ref_ftr.type) && options_.epi_search_edgelet_filtering)
{
const Eigen::Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
const double cosangle = fabs(grad_cur.dot(epi_image_.normalized()));
if (cosangle < options_.epi_search_edgelet_max_angle)
{
reject_ = true;
return MatchResult::kFailAngle;
}
}
//LOG(INFO) << "A_cur_ref_: " << A_cur_ref_;
// prepare for match
// - find best search level
// - warp the reference patch
search_level_ = warp::getBestSearchLevel(A_cur_ref_, ref_frame.img_pyr_.size() - 1);
// length and direction on SEARCH LEVEL
epi_length_pyramid_ = epi_image_.norm() / (1 << search_level_);
GradientVector epi_dir_image = epi_image_.normalized();
if (!warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
ref_ftr.level, search_level_, kHalfPatchSize + 1, patch_with_border_))
return MatchResult::kFailWarp;
patch_utils::createPatchFromPatchWithBorder(
patch_with_border_, kPatchSize, patch_);
// Case 1: direct search locally if the epipolar line is too short
if (epi_length_pyramid_ < 2.0)
{
px_cur_ = (px_A + px_B) / 2.0;
MatchResult res = findLocalMatch(cur_frame, epi_dir_image, search_level_, px_cur_);
if (res != MatchResult::kSuccess)
return res;
cur_frame.cam()->backProject3(px_cur_, &f_cur_);
f_cur_.normalize();
return matcher_utils::depthFromTriangulation(T_cur_ref, ref_ftr.f, f_cur_, &depth);
}
// Case 2: search along the epipolar line for the best match
PatchScore patch_score(patch_); // precompute for reference patch
BearingVector C = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_estimate_inv;
//LOG(INFO) << "C: " << C;
//LOG(INFO) << "px_cur_: " << std::setprecision(15) << px_cur_.transpose();
scanEpipolarLine(cur_frame, A, B, C, patch_score, search_level_, &px_cur_, &zmssd_best);
//LOG(INFO) << "zmssd_best: " << zmssd_best;
// check if the best match is good enough
if (zmssd_best < PatchScore::threshold())
{
if (options_.subpix_refinement)
{
MatchResult res = findLocalMatch(cur_frame, epi_dir_image, search_level_, px_cur_);
if (res != MatchResult::kSuccess)
return res;
}
//LOG(INFO) << "BACK PROJECT";
cur_frame.cam()->backProject3(px_cur_, &f_cur_);
f_cur_.normalize();
//LOG(INFO) << "f_cur_ NORM: " <<std::setprecision(15)<< f_cur_.x() <<" " << f_cur_.y() << " " << f_cur_.z();
//LOG(INFO) << "T_cur_ref: \n" << std::setprecision(15) << T_cur_ref;
//LOG(INFO) << "ref_ftr.f: " << std::setprecision(15) << ref_ftr.f.transpose();
return matcher_utils::depthFromTriangulation(T_cur_ref, ref_ftr.f, f_cur_, &depth);
}
else
return MatchResult::kFailScore;
}
3. Matcher::findMatchDirect接口描述
接口名称 Matcher::findMatchDirect
输入参数
ref_frame: 参考帧（const Frame&），包含参考特征所在的图像信息。cur_frame: 当前帧（const Frame&），包含需要搜索匹配的特征的图像信息。ref_ftr: 参考特征（const FeatureWrapper&），需要在当前帧中找到匹配的特征。ref_depth: 参考特征的深度值（const FloatType&）。px_cur: 当前帧中匹配点的像素坐标（Keypoint&），用于输出。
输出
MatchResult: 匹配结果，表示匹配是否成功以及成功的原因。
功能和目的
该函数的目的是在立体视觉系统中，通过直接法在当前帧中寻找与参考帧中给定特征的匹配点。它使用参考特征的深度信息和相机运动来指导搜索，通过计算仿射变换矩阵和特征对齐来优化匹配过程。
算法流程及步骤性描述
-
检查特征可见性：
- 检查参考特征的像素坐标是否在图像边界内，如果不在，则返回匹配失败。
- 代码行：
if (pxi[0] < boundary ...
-
计算仿射变换矩阵：
- 计算仿射变换矩阵
A_cur_ref_，用于将参考特征的图像区域变换到当前帧的图像尺度和方向。 - 代码行：
warp::getWarpMatrixAffine(...)
- 计算仿射变换矩阵
-
确定搜索级别：
- 确定最佳的搜索级别
search_level_。 - 代码行：
search_level_ = warp::getBestSearchLevel(A_cur_ref_, ref_frame.img_pyr_.size() - 1);
- 确定最佳的搜索级别
-
特征图像块变换：
- 如果使用仿射变换，将参考特征的图像块通过仿射变换矩阵变换到当前帧的尺度。
- 如果不使用仿射变换，使用像素级变换。
- 代码行：
if (options_.use_affine_warp_) {...} else {...}
-
创建特征块：
- 从变换后的图像块中创建特征块。
- 代码行：
patch_utils::createPatchFromPatchWithBorder(...)
-
特征对齐：
- 如果参考特征是边缘特征，使用一维对齐方法；否则，使用二维对齐方法。
- 代码行：
if (isEdgelet(ref_ftr.type)) {...} else {...}
-
检查对齐结果：
- 如果特征对齐成功，检查特征点移动的距离是否超过了最大允许值。
- 如果超过，返回匹配失败；否则，更新当前帧中匹配点的像素坐标，并返回匹配成功。
- 代码行：
if ((px_scaled - px_scaled_start).norm() > options_.max_patch_diff_ratio * kPatchSize) {...}
-
返回匹配结果：
- 如果特征对齐失败，返回匹配失败。
- 代码行：
return MatchResult::kFailAlignment;
这个函数通过计算仿射变换矩阵和特征对齐来优化匹配过程，提高了匹配的效率和准确性。
Matcher::MatchResult Matcher::findEpipolarMatchDirect(
const Frame& ref_frame,
const Frame& cur_frame,
const Transformation& T_cur_ref,
const FeatureWrapper& ref_ftr,
const double d_estimate_inv,
const double d_min_inv,
const double d_max_inv,
double& depth)
{
int zmssd_best = PatchScore::threshold();
// Compute start and end of epipolar line in old_kf for match search, on image plane
const BearingVector A = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_min_inv;
const BearingVector B = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_max_inv;
Eigen::Vector2d px_A, px_B;
cur_frame.cam()->project3(A, &px_A);
cur_frame.cam()->project3(B, &px_B);
epi_image_ = px_A - px_B;
//LOG(INFO) << "A:" << A;
//LOG(INFO) << "B:" << B;
//LOG(INFO) << "px_A:" << px_A;
//LOG(INFO) << "px_B:" << px_B;
// Compute affine warp matrix
warp::getWarpMatrixAffine(
ref_frame.cam_, cur_frame.cam_, ref_ftr.px, ref_ftr.f,
1.0 / std::max(0.000001, d_estimate_inv), T_cur_ref, ref_ftr.level, &A_cur_ref_);
// feature pre-selection
reject_ = false;
if (isEdgelet(ref_ftr.type) && options_.epi_search_edgelet_filtering)
{
const Eigen::Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
const double cosangle = fabs(grad_cur.dot(epi_image_.normalized()));
if (cosangle < options_.epi_search_edgelet_max_angle)
{
reject_ = true;
return MatchResult::kFailAngle;
}
}
//LOG(INFO) << "A_cur_ref_: " << A_cur_ref_;
// prepare for match
// - find best search level
// - warp the reference patch
search_level_ = warp::getBestSearchLevel(A_cur_ref_, ref_frame.img_pyr_.size() - 1);
// length and direction on SEARCH LEVEL
epi_length_pyramid_ = epi_image_.norm() / (1 << search_level_);
GradientVector epi_dir_image = epi_image_.normalized();
if (!warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
ref_ftr.level, search_level_, kHalfPatchSize + 1, patch_with_border_))
return MatchResult::kFailWarp;
patch_utils::createPatchFromPatchWithBorder(
patch_with_border_, kPatchSize, patch_);
// Case 1: direct search locally if the epipolar line is too short
if (epi_length_pyramid_ < 2.0)
{
px_cur_ = (px_A + px_B) / 2.0;
MatchResult res = findLocalMatch(cur_frame, epi_dir_image, search_level_, px_cur_);
if (res != MatchResult::kSuccess)
return res;
cur_frame.cam()->backProject3(px_cur_, &f_cur_);
f_cur_.normalize();
return matcher_utils::depthFromTriangulation(T_cur_ref, ref_ftr.f, f_cur_, &depth);
}
// Case 2: search along the epipolar line for the best match
PatchScore patch_score(patch_); // precompute for reference patch
BearingVector C = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_estimate_inv;
//LOG(INFO) << "C: " << C;
//LOG(INFO) << "px_cur_: " << std::setprecision(15) << px_cur_.transpose();
scanEpipolarLine(cur_frame, A, B, C, patch_score, search_level_, &px_cur_, &zmssd_best);
//LOG(INFO) << "zmssd_best: " << zmssd_best;
// check if the best match is good enough
if (zmssd_best < PatchScore::threshold())
{
if (options_.subpix_refinement)
{
MatchResult res = findLocalMatch(cur_frame, epi_dir_image, search_level_, px_cur_);
if (res != MatchResult::kSuccess)
return res;
}
//LOG(INFO) << "BACK PROJECT";
cur_frame.cam()->backProject3(px_cur_, &f_cur_);
f_cur_.normalize();
//LOG(INFO) << "f_cur_ NORM: " <<std::setprecision(15)<< f_cur_.x() <<" " << f_cur_.y() << " " << f_cur_.z();
//LOG(INFO) << "T_cur_ref: \n" << std::setprecision(15) << T_cur_ref;
//LOG(INFO) << "ref_ftr.f: " << std::setprecision(15) << ref_ftr.f.transpose();
return matcher_utils::depthFromTriangulation(T_cur_ref, ref_ftr.f, f_cur_, &depth);
}
else
return MatchResult::kFailScore;
}
扫一扫
4454
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
