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
}
ArcPtr ArcFitter::getCurve() const
{
if((int)_pts.size() < 2)
return ArcPtr();
double factor = 1. / _totWeight;
Vector3d pt = _sum * factor;
Matrix3d cov = factor * _squaredSum - pt * pt.transpose();
SelfAdjointEigenSolver<Matrix3d> eigenSolver(cov);
Vector3d eigVs = eigenSolver.eigenvalues();
Vector3d dir = eigenSolver.eigenvectors().col(0); //0 is the index of the smallest eigenvalue
dir /= (1e-16 + dir[2]);
double dot = dir.dot(pt);
//circle equation is:
//dir[0] * x + dir[1] * y + (x^2+y^2) = dot
Vector2d center = -0.5 * Vector2d(dir[0], dir[1]);
double radius = sqrt(1e-16 + dot + center.squaredNorm());
center += _pts[0];
//TODO: convert code to use AngleUtils
//Now get the arc
Vector2d c[3] = { _pts[0], _pts[_pts.size() / 2], _pts.back() };
double angle[3];
for(int i = 0; i < 3; ++i) {
c[i] = (c[i] - center).normalized() * radius;
angle[i] = atan2(c[i][1], c[i][0]);
}
if(angle[2] < angle[0])
angle[2] += PI * 2.;
if(angle[1] < angle[0])
angle[1] += PI * 2.;
if(angle[1] <= angle[2]) { //OK--CCW arc
double a = angle[2] - angle[0];
return new Arc(c[0] + center, angle[0] + PI * 0.5, a * radius, 1. / radius);
}
else { //Backwards--CW arc
for(int i = 0; i < 3; ++i)
angle[i] = atan2(c[i][1], c[i][0]);
if(angle[0] <= angle[2])
angle[0] += PI * 2.;
if(angle[1] < angle[2])
angle[1] += PI * 2.;
assert(angle[1] <= angle[0]);
double a = angle[0] - angle[2];
return new Arc(c[0] + center, angle[0] - PI * 0.5, a * radius, -1. / radius);
}
}
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;
}
}
int clipSegmentCircle(Vector2d& p1, Vector2d& p2, double r) {
double r2=r*r;
Vector2d pBase=p1;
Vector2d dp=p2-p1;
double length=dp.norm();
dp.normalize();
double p=2*dp.dot(p1);
double q=p1.squaredNorm()-r2;
double disc = p*p - 4*q;
if (disc<=0) { // no intersection or single point intersection
return -1;
}
disc = sqrt(disc);
double t1=.5*(-p-disc);
double t2=.5*(-p+disc);
if ( t1 > length || t2 <0 )
return -1; // no intersection
bool clip1=false;
bool clip2=false;
if ( t1 > 0 ) {
p1 = pBase + dp*t1;
clip1 = true;
}
if ( t2 < length ) {
p2 = pBase + dp*t1;
clip2 = true;
}
if (clip1)
if (clip2) return 3;
else return 0;
else
if (clip2) return 1;
return 2;
}
double RationalSuperShape2D :: ImplicitFunction3( const Vector2d P, vector <double> &Dffinal){
Dffinal.clear();
// nothing computable, return zero values, zero partial derivatives
// the point will have no effect on the ongoing computations
if ( P[0] == 0 && P[1] == 0)
{
// Df/Dx, Df/Dy, Df/Dr set to zero...
for (int i=0; i<3; i++) Dffinal.push_back(0);
return 0;
}
vector <double> f, Ddum;
double x(P[0]), y(P[1]), PSL(P.squaredNorm()), dthtdx (-y/PSL), dthtdy (x/PSL), R,drdth;
vector< vector<double> > Df;
//assert angular values between [0, 2q*Pi]
double tht (atan2(y,x)), thtbase(tht);
if (tht<0) thtbase += 2.*M_PI;
//compute all intersections and associated gradient values
for (int i=0; i<Get_q(); i++)
{
tht = thtbase + i*2.*M_PI;
R = radius(tht);
drdth = DrDtheta(tht);
f.push_back( log( R*R / PSL)); //store function
// store partial derivatives
drdth = DrDtheta(tht);
vector <double> rowi;
rowi.push_back( -2.*(x*R - PSL * drdth*dthtdx)/(R*PSL) ); //df/dx
rowi.push_back( -2.*(y*R - PSL * drdth*dthtdy)/(R*PSL) ); //df/dy
rowi.push_back( 2./R ); //df/dr
Df.push_back(rowi);
}
//bubble sort, not really efficient but acceptable for such small arrays
for(int i=0; i<Get_q()-1; i++)
for (int j=i+1; j<Get_q(); j++)
if (f[i]<f[j])
{
//swap values of f[i] and f[j]
swap(f[i],f[j]);
//swap rows Df[i] and Df[j]
Df[i].swap(Df[j]);
}
//Compute resulting Rfunction
vector<double> Df1; //vector for df/dxi
//iterative evaluation of:
// -the resulting R-functions
// -the associated partial derivatives
double f1,fdum;
f1 = f[0]; // first value of f
Df1 = Df[0]; // first associated row with partial derivatives
//combine functions as (...((F1 v F2) v F3 ) v F4) v ...)
for(int i=1; i<Get_q(); i++) // for all intersections
{
//compute R-function, sets all partial derivatives
//fdum and Ddum temporary results of the union from F1 to Fi
RpUnion(f1, f[i], Df1, Df[i], fdum, Ddum);
//update results in f1 and Df1, and iterate
f1 = fdum;
Df1 = Ddum;
}
//final partial derivatives df/dxi after R-functions
Dffinal = Df1;
//clear arrays
f.clear(); Df.clear(); Ddum.clear(); Df1.clear();
//return results
return f1;
}
void globalBA(Map* map)
{
// init g2o
g2o::SparseOptimizer optimizer;
setupG2o(&optimizer);
list<EdgeContainerSE3> edges;
list< pair<FramePtr,Feature*> > incorrect_edges;
// Go through all Keyframes
size_t v_id = 0;
double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
reproj_thresh_1_squared *= reproj_thresh_1_squared;
for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
it_kf != map->keyframes_.end(); ++it_kf)
{
// New Keyframe Vertex
g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false);
(*it_kf)->v_kf_ = v_kf;
optimizer.addVertex(v_kf);
for(list<PointFeat*>::iterator it_ftr=(*it_kf)->pt_fts_.begin(); it_ftr!=(*it_kf)->pt_fts_.end(); ++it_ftr)
{
// for each keyframe add edges to all observed mapoints
Point* mp = (*it_ftr)->feat3D;
if(mp == NULL)
continue;
g2oPoint* v_mp = mp->v_g2o_;
if(v_mp == NULL)
{
// mappoint-vertex doesn't exist yet. create a new one:
v_mp = createG2oPoint(mp->pos_, v_id++, false);
mp->v_g2o_ = v_mp;
optimizer.addVertex(v_mp);
}
// Due to merging of mappoints it is possible that references in kfs suddenly
// have a very large reprojection error which may result in distorted results.
Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
if(error.squaredNorm() > reproj_thresh_1_squared)
incorrect_edges.push_back(pair<FramePtr,Feature*>(*it_kf, *it_ftr));
else
{
g2oEdgeSE3* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f),
true,
reproj_thresh_2*Config::lobaRobustHuberWidth());
edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr));
optimizer.addEdge(e);
}
}
}
// Optimization
double init_error=0.0, final_error=0.0;
if(Config::lobaNumIter() > 0)
runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
// Update Keyframe and MapPoint Positions
for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
it_kf != map->keyframes_.end(); ++it_kf)
{
(*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
(*it_kf)->v_kf_->estimate().translation());
(*it_kf)->v_kf_ = NULL;
for(list<PointFeat*>::iterator it_ftr=(*it_kf)->pt_fts_.begin(); it_ftr!=(*it_kf)->pt_fts_.end(); ++it_ftr)
{
Point* mp = (*it_ftr)->feat3D;
if(mp == NULL)
continue;
if(mp->v_g2o_ == NULL)
continue; // mp was updated before
mp->pos_ = mp->v_g2o_->estimate();
mp->v_g2o_ = NULL;
}
}
// Remove Measurements with too large reprojection error
for(list< pair<FramePtr,Feature*> >::iterator it=incorrect_edges.begin(); it!=incorrect_edges.end(); ++it)
{
PointFeat* feat_aux = static_cast<PointFeat*>(it->second);
map->removePtFrameRef(it->first.get(), feat_aux);
}
double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
{
if(it->edge->chi2() > reproj_thresh_2_squared)
{
PointFeat* feat_aux = static_cast<PointFeat*>(it->feature);
map->removePtFrameRef(it->frame, feat_aux);
}
}
}
void optimizeGaussNewton(
const double reproj_thresh,
const size_t n_iter,
const bool verbose,
FramePtr& frame,
double& estimated_scale,
double& error_init,
double& error_final,
size_t& num_obs)
{
// init
double chi2(0.0);
vector<double> chi2_vec_init, chi2_vec_final;
vk::robust_cost::TukeyWeightFunction weight_function;
SE3d T_old(frame->T_f_w_);
Matrix6d A;
Vector6d b;
// compute the scale of the error for robust estimation
std::vector<float> errors; errors.reserve(frame->fts_.size());
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
e *= 1.0 / (1<<(*it)->level);
errors.push_back(e.norm());
}
if(errors.empty())
return;
vk::robust_cost::MADScaleEstimator scale_estimator;
estimated_scale = scale_estimator.compute(errors);
num_obs = errors.size();
chi2_vec_init.reserve(num_obs);
chi2_vec_final.reserve(num_obs);
double scale = estimated_scale;
for(size_t iter=0; iter<n_iter; iter++)
{
// overwrite scale
if(iter == 5)
scale = 0.85/frame->cam_->errorMultiplier2();
b.setZero();
A.setZero();
double new_chi2(0.0);
// compute residual
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Matrix26d J;
Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_);
Frame::jacobian_xyz2uv(xyz_f, J);
Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
if(iter == 0)
chi2_vec_init.push_back(e.squaredNorm()); // just for debug
J *= sqrt_inv_cov;
double weight = weight_function.value(e.norm()/scale);
A.noalias() += J.transpose()*J*weight;
b.noalias() -= J.transpose()*e*weight;
new_chi2 += e.squaredNorm()*weight;
}
// solve linear system
const Vector6d dT(A.ldlt().solve(b));
// check if error increased
if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
{
if(verbose)
std::cout << "it " << iter
<< "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl;
frame->T_f_w_ = T_old; // roll-back
break;
}
// update the model
SE3d T_new = SE3d::exp(dT)*frame->T_f_w_;
T_old = frame->T_f_w_;
frame->T_f_w_ = T_new;
chi2 = new_chi2;
if(verbose)
std::cout << "it " << iter
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(dT) = " << vk::norm_max(dT) << std::endl;
// stop when converged
if(vk::norm_max(dT) <= EPS)
break;
}
// Set covariance as inverse information matrix. Optimistic estimator!
const double pixel_variance=1.0;
frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse();
// Remove Measurements with too large reprojection error
double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2();
size_t n_deleted_refs = 0;
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
chi2_vec_final.push_back(e.squaredNorm());
if(e.norm() > reproj_thresh_scaled)
{
// we don't need to delete a reference in the point since it was not created yet
(*it)->point = NULL;
++n_deleted_refs;
}
}
error_init=0.0;
error_final=0.0;
if(!chi2_vec_init.empty())
error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2();
if(!chi2_vec_final.empty())
error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2();
estimated_scale *= frame->cam_->errorMultiplier2();
if(verbose)
std::cout << "n deleted obs = " << n_deleted_refs
<< "\t scale = " << estimated_scale
<< "\t error init = " << error_init
<< "\t error end = " << error_final << std::endl;
num_obs -= n_deleted_refs;
}
