void Point::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;
// compute residuals
for(auto it=obs_.begin(); it!=obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotationMatrix(), J);
const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
new_chi2 += e.squaredNorm();
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
}
// solve linear system
const Vector3d dp(A.ldlt().solve(b));
// check if error increased
if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
{
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
#endif
pos_ = old_point; // roll-back
break;
}
// update the model
Vector3d new_point = pos_ + dp;
old_point = pos_;
pos_ = new_point;
chi2 = new_chi2;
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(b) = " << vk::norm_max(b)
<< endl;
#endif
// stop when converged
if(vk::norm_max(dp) <= EPS)
break;
}
#ifdef POINT_OPTIMIZER_DEBUG
cout << endl;
#endif
}
Vector6d Line3D::toCartesian() const{
Vector6d cartesian;
cartesian.tail<3>() = d()/d().norm();
Matrix3d W=-_skew(d());
double damping = 1e-9;
Matrix3d A = W.transpose()*W+(Matrix3d::Identity()*damping);
cartesian.head<3>() = A.ldlt().solve(W.transpose()*w());
return cartesian;
}
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;
}
}
