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; }