void Initialization::computeHomography(
const std::vector<Vector3d>& f_ref,
const std::vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
std::vector<int>& inliers,
std::vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref)
{
std::vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
std::vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.size());
for (size_t i = 0, i_max = f_ref.size(); i < i_max; ++i)
{
uv_ref[i] = project2d(f_ref[i]);
uv_cur[i] = project2d(f_cur[i]);
}
Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
std::vector<int> outliers;
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);
T_cur_from_ref = Homography.T_c2_from_c1_;
}
Vector2d CameraParameters::cam_map(const Vector3d & trans_xyz) const {
Vector2d proj = project2d(trans_xyz);
Vector2d res;
res[0] = proj[0]*focal_length + principle_point[0];
res[1] = proj[1]*focal_length + principle_point[1];
return res;
}
Vector2d EdgeSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz) const {
Vector2d proj = project2d(trans_xyz);
Vector2d res;
res[0] = proj[0]*fx + cx;
res[1] = proj[1]*fy + cy;
return res;
}
void Point3D::optimize(const size_t n_iter)
{
Vector3d old_point = pos_;
double chi2 = 0.0;
Matrix3d A;
Vector3d b;
for (size_t i = 0; i < n_iter; i++)
{
A.setZero();
b.setZero();
double new_chi2 = 0.0;
// 计算残差
for (auto it = obs_.begin(); it != obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
Point3D::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
const Vector2d e(project2d((*it)->f) - project2d(p_in_f));
new_chi2 += e.squaredNorm();
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
}
// 求解线性系统
const Vector3d dp(A.ldlt().solve(b));
// 检测误差有没有增长
if ((i > 0 && new_chi2 > chi2) || (bool)std::isnan((double)dp[0]))
{
pos_ = old_point; // 回滚
break;
}
// 更新模型
Vector3d new_point = pos_ + dp;
old_point = pos_;
pos_ = new_point;
chi2 = new_chi2;
// 收敛则停止
if (norm_max(dp) <= EPS)
break;
}
}
size_t Homography::
computeMatchesInliers()
{
inliers.clear(); inliers.resize(fts_c1.size());
size_t n_inliers = 0;
for(size_t i=0; i<fts_c1.size(); i++)
{
Vector2d projected = project2d(H_c2_from_c1 * unproject2d(fts_c1[i]));
Vector2d e = fts_c2[i] - projected;
double e_px = error_multiplier2 * e.norm();
inliers[i] = (e_px < thresh);
n_inliers += inliers[i];
}
return n_inliers;
}
