From fe7c4d4b50cca43499a404102ead2bc280b3bda9 Mon Sep 17 00:00:00 2001 From: Mu Xiang Date: Mon, 2 Sep 2019 22:08:15 +0800 Subject: [PATCH] add dogleg by yangxinglong --- src/backend/problem1.cc | 1041 +++++++++++++++++++++++++++++++++++++++ src/backend/problem1.h | 225 +++++++++ 2 files changed, 1266 insertions(+) create mode 100644 src/backend/problem1.cc create mode 100644 src/backend/problem1.h diff --git a/src/backend/problem1.cc b/src/backend/problem1.cc new file mode 100644 index 0000000..e49cb3c --- /dev/null +++ b/src/backend/problem1.cc @@ -0,0 +1,1041 @@ +#include +#include +#include +#include +#include "backend/problem.h" +#include "utility/tic_toc.h" + +#ifdef USE_OPENMP + +#include + +#endif + +using namespace std; + +// define the format you want, you only need one instance of this... +const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n"); + +void writeToCSVfile(std::string name, Eigen::MatrixXd matrix) { + std::ofstream f(name.c_str()); + f << matrix.format(CSVFormat); +} + +namespace myslam { +namespace backend { +void Problem::LogoutVectorSize() { + // LOG(INFO) << + // "1 problem::LogoutVectorSize verticies_:" << verticies_.size() << + // " edges:" << edges_.size(); +} + +Problem::Problem(ProblemType problemType) : + problemType_(problemType) { + LogoutVectorSize(); + verticies_marg_.clear(); +} + +Problem::~Problem() { +// std::cout << "Problem IS Deleted"< vertex) { + //如果找到id就不需要进行添加 + if (verticies_.find(vertex->Id()) != verticies_.end()) { + // LOG(WARNING) << "Vertex " << vertex->Id() << " has been added before"; + return false; + } else { + verticies_.insert(pair>(vertex->Id(), vertex)); + } + + if (problemType_ == ProblemType::SLAM_PROBLEM) { + if (IsPoseVertex(vertex)) { + ResizePoseHessiansWhenAddingPose(vertex); + } + } + return true; +} + +void Problem::AddOrderingSLAM(std::shared_ptr v) { + if (IsPoseVertex(v)) { + v->SetOrderingId(ordering_poses_); + idx_pose_vertices_.insert(pair>(v->Id(), v)); + ordering_poses_ += v->LocalDimension(); + } else if (IsLandmarkVertex(v)) { + v->SetOrderingId(ordering_landmarks_); + ordering_landmarks_ += v->LocalDimension(); + idx_landmark_vertices_.insert(pair>(v->Id(), v)); + } +} + +void Problem::ResizePoseHessiansWhenAddingPose(shared_ptr v) { + + int size = H_prior_.rows() + v->LocalDimension(); + H_prior_.conservativeResize(size, size); + b_prior_.conservativeResize(size); + + b_prior_.tail(v->LocalDimension()).setZero(); + H_prior_.rightCols(v->LocalDimension()).setZero(); + H_prior_.bottomRows(v->LocalDimension()).setZero(); + +} +void Problem::ExtendHessiansPriorSize(int dim) +{ + int size = H_prior_.rows() + dim; + H_prior_.conservativeResize(size, size); + b_prior_.conservativeResize(size); + + b_prior_.tail(dim).setZero(); + H_prior_.rightCols(dim).setZero(); + H_prior_.bottomRows(dim).setZero(); +} + +bool Problem::IsPoseVertex(std::shared_ptr v) { + string type = v->TypeInfo(); + return type == string("VertexPose") || + type == string("VertexSpeedBias"); +} + +bool Problem::IsLandmarkVertex(std::shared_ptr v) { + string type = v->TypeInfo(); + return type == string("VertexPointXYZ") || + type == string("VertexInverseDepth"); +} + +bool Problem::AddEdge(shared_ptr edge) { + if (edges_.find(edge->Id()) == edges_.end()) { + edges_.insert(pair>(edge->Id(), edge)); + } else { + // LOG(WARNING) << "Edge " << edge->Id() << " has been added before!"; + return false; + } + + for (auto &vertex: edge->Verticies()) { + vertexToEdge_.insert(pair>(vertex->Id(), edge)); + } + return true; +} + +vector> Problem::GetConnectedEdges(std::shared_ptr vertex) { + vector> edges; + auto range = vertexToEdge_.equal_range(vertex->Id()); + for (auto iter = range.first; iter != range.second; ++iter) { + + // 并且这个edge还需要存在，而不是已经被remove了 + if (edges_.find(iter->second->Id()) == edges_.end()) + continue; + + edges.emplace_back(iter->second); + } + return edges; +} + +bool Problem::RemoveVertex(std::shared_ptr vertex) { + //check if the vertex is in map_verticies_ + if (verticies_.find(vertex->Id()) == verticies_.end()) { + // LOG(WARNING) << "The vertex " << vertex->Id() << " is not in the problem!" << endl; + return false; + } + + // 这里要 remove 该顶点对应的 edge. + vector> remove_edges = GetConnectedEdges(vertex); + for (size_t i = 0; i < remove_edges.size(); i++) { + RemoveEdge(remove_edges[i]); + } + + if (IsPoseVertex(vertex)) + idx_pose_vertices_.erase(vertex->Id()); + else + idx_landmark_vertices_.erase(vertex->Id()); + + vertex->SetOrderingId(-1); // used to debug + verticies_.erase(vertex->Id()); + vertexToEdge_.erase(vertex->Id()); + + return true; +} + +bool Problem::RemoveEdge(std::shared_ptr edge) { + //check if the edge is in map_edges_ + if (edges_.find(edge->Id()) == edges_.end()) { + // LOG(WARNING) << "The edge " << edge->Id() << " is not in the problem!" << endl; + return false; + } + + edges_.erase(edge->Id()); + return true; +} +bool Problem::SolveWithDogleg(int iterations){ + if (edges_.size() == 0 || verticies_.size() == 0) { + std::cerr << "\nCannot solve problem without edges or verticies" << std::endl; + return false; + } + SetOrdering(); + + double eps1=1e-12,eps2=1e-12,eps3=1e-12; + //计算初始时刻残差 + for (auto edge: edges_) { + currentChi_ += edge.second->RobustChi2(); + } + if (err_prior_.rows() > 0) + currentChi_ += err_prior_.squaredNorm();//norm + currentChi_ *= 0.5; + + _delta=1.0; + + // bool stop=false; + int iter = 0; + while(iter saes2( Hessian_); + Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0)); + Eigen::VectorXd S_sqrt = S.cwiseSqrt(); + MatXX J = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); + + double alpha=b_.squaredNorm()/(J*-b_).squaredNorm(); + + //double alpha=b_.squaredNorm()/(b.transpose()*Hessian_.b).norm(); + VecX _hsd=alpha*b_; + VecX hgn=Hessian_.ldlt().solve(b_); + cout<<"enter Dog-leg2"<=_delta) + { + delta_x_=(_delta/_hsd.norm())*_hsd; + + } + + else{ + _auxVector=hgn - _hsd; + double c = _hsd.dot(_auxVector); + double bmaSquaredNorm = _auxVector.squaredNorm(); + double beta; + if (c <= 0.) + beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm; + else { + double hsdSqrNorm = _hsd.squaredNorm(); + beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm))); + } + assert(beta > 0. && beta < 1 && "Error while computing beta"); + delta_x_= _hsd + beta * (hgn - _hsd); + assert(delta_x_.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region"); + +// assert(_hdl.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region"); +// VecX a = alpha * stepest_descent; +// VecX b = gauss_newton; +// double c = a.transpose() * (b - a); +// beta = (sqrt(c*c + (b-a).squaredNorm()*(_delta*_delta-a.squaredNorm()))-c) +// /(b-a).squaredNorm(); +// +// delta_x_= alpha * stepest_descent + beta * (gauss_newton - alpha * stepest_descent); + } + cout<<"enter Dog-leg3"< 0) +// { +// +// } else +// { +// +// } + cout<<"enter Dog-leg5"< 0.75) + { + _delta = max(_delta, 3.0 * delta_x_.norm()); + } + else if(rho < 0.25) + { + _delta = _delta*0.5; + if( _delta <= eps2*(getXnorm()+eps2)) + { + cout << "trust region radius is too small." << endl; + break; + } + } + + cout<<"enter Dog-leg6"<LocalDimension(); // 所有的优化变量总维数 + + if (problemType_ == ProblemType::SLAM_PROBLEM) // 如果是 slam 问题，还要分别统计 pose 和 landmark 的维数，后面会对他们进行排序 + { + AddOrderingSLAM(vertex.second); + } + + } + + if (problemType_ == ProblemType::SLAM_PROBLEM) { + // 这里要把 landmark 的 ordering 加上 pose 的数量，就保持了 landmark 在后,而 pose 在前 + ulong all_pose_dimension = ordering_poses_; + for (auto landmarkVertex : idx_landmark_vertices_) { + landmarkVertex.second->SetOrderingId( + landmarkVertex.second->OrderingId() + all_pose_dimension + ); + } + } + +// CHECK_EQ(CheckOrdering(), true); +} + +bool Problem::CheckOrdering() { + if (problemType_ == ProblemType::SLAM_PROBLEM) { + int current_ordering = 0; + for (auto v: idx_pose_vertices_) { + assert(v.second->OrderingId() == current_ordering); + current_ordering += v.second->LocalDimension(); + } + + for (auto v: idx_landmark_vertices_) { + assert(v.second->OrderingId() == current_ordering); + current_ordering += v.second->LocalDimension(); + } + } + return true; +} + +void Problem::MakeHessian() { + TicToc t_h; + // 直接构造大的 H 矩阵 + ulong size = ordering_generic_; + MatXX H(MatXX::Zero(size, size)); + VecX b(VecX::Zero(size)); + + // TODO:: accelate, accelate, accelate +//#ifdef USE_OPENMP +//#pragma omp parallel for +//#endif + for (auto &edge: edges_) { + + edge.second->ComputeResidual(); + edge.second->ComputeJacobians(); + + // TODO:: robust cost + auto jacobians = edge.second->Jacobians(); + auto verticies = edge.second->Verticies(); + assert(jacobians.size() == verticies.size()); + for (size_t i = 0; i < verticies.size(); ++i) { + auto v_i = verticies[i]; + if (v_i->IsFixed()) continue; // Hessian 里不需要添加它的信息，也就是它的雅克比为 0 + + auto jacobian_i = jacobians[i]; + ulong index_i = v_i->OrderingId(); + ulong dim_i = v_i->LocalDimension(); + + // 鲁棒核函数会修改残差和信息矩阵，如果没有设置 robust cost function，就会返回原来的 + double drho; + MatXX robustInfo(edge.second->Information().rows(),edge.second->Information().cols()); + edge.second->RobustInfo(drho,robustInfo); + + MatXX JtW = jacobian_i.transpose() * robustInfo; + for (size_t j = i; j < verticies.size(); ++j) { + auto v_j = verticies[j]; + + if (v_j->IsFixed()) continue; + + auto jacobian_j = jacobians[j]; + ulong index_j = v_j->OrderingId(); + ulong dim_j = v_j->LocalDimension(); + + assert(v_j->OrderingId() != -1); + MatXX hessian = JtW * jacobian_j; + + // 所有的信息矩阵叠加起来 + H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian; + if (j != i) { + // 对称的下三角 + H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose(); + + } + } + b.segment(index_i, dim_i).noalias() -= drho * jacobian_i.transpose()* edge.second->Information() * edge.second->Residual(); + } + + } + Hessian_ = H; + b_ = b; + t_hessian_cost_ += t_h.toc(); + + if(H_prior_.rows() > 0) + { + MatXX H_prior_tmp = H_prior_; + VecX b_prior_tmp = b_prior_; + + /// 遍历所有 POSE 顶点，然后设置相应的先验维度为 0 . fix 外参数, SET PRIOR TO ZERO + /// landmark 没有先验 + for (auto vertex: verticies_) { + if (IsPoseVertex(vertex.second) && vertex.second->IsFixed() ) { + int idx = vertex.second->OrderingId(); + int dim = vertex.second->LocalDimension(); + H_prior_tmp.block(idx,0, dim, H_prior_tmp.cols()).setZero(); + H_prior_tmp.block(0,idx, H_prior_tmp.rows(), dim).setZero(); + b_prior_tmp.segment(idx,dim).setZero(); +// std::cout << " fixed prior, set the Hprior and bprior part to zero, idx: "< Hpp, bpp + int reserve_size = ordering_poses_; + int marg_size = ordering_landmarks_; + MatXX Hmm = Hessian_.block(reserve_size, reserve_size, marg_size, marg_size); + MatXX Hpm = Hessian_.block(0, reserve_size, reserve_size, marg_size); + MatXX Hmp = Hessian_.block(reserve_size, 0, marg_size, reserve_size); + VecX bpp = b_.segment(0, reserve_size); + VecX bmm = b_.segment(reserve_size, marg_size); + + // Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速 + MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size)); + // TODO:: use openMP + for (auto landmarkVertex : idx_landmark_vertices_) { + int idx = landmarkVertex.second->OrderingId() - reserve_size; + int size = landmarkVertex.second->LocalDimension(); + Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse(); + } + + MatXX tempH = Hpm * Hmm_inv; + H_pp_schur_ = Hessian_.block(0, 0, ordering_poses_, ordering_poses_) - tempH * Hmp; + b_pp_schur_ = bpp - tempH * bmm; + + // step2: solve Hpp * delta_x = bpp + VecX delta_x_pp(VecX::Zero(reserve_size)); + + for (ulong i = 0; i < ordering_poses_; ++i) { + H_pp_schur_(i, i) += currentLambda_; // LM Method + } + + // TicToc t_linearsolver; + delta_x_pp = H_pp_schur_.ldlt().solve(b_pp_schur_);// SVec.asDiagonal() * svd.matrixV() * Ub; + delta_x_.head(reserve_size) = delta_x_pp; + // std::cout << " Linear Solver Time Cost: " << t_linearsolver.toc() << std::endl; + + // step3: solve Hmm * delta_x = bmm - Hmp * delta_x_pp; + VecX delta_x_ll(marg_size); + delta_x_ll = Hmm_inv * (bmm - Hmp * delta_x_pp); + delta_x_.tail(marg_size) = delta_x_ll; + +// std::cout << "schur time cost: "<< t_Hmminv.toc()<OrderingId(); +// ulong dim = vertex.second->LocalDimension(); + delta+=vertex.second->Parameters().norm(); // 保存上次的估计值 + + } + return delta; +} +void Problem::UpdateStates() { + + // update vertex + for (auto vertex: verticies_) { + vertex.second->BackUpParameters(); // 保存上次的估计值 + + ulong idx = vertex.second->OrderingId(); + ulong dim = vertex.second->LocalDimension(); + VecX delta = delta_x_.segment(idx, dim); + vertex.second->Plus(delta); + } + + // update prior + if (err_prior_.rows() > 0) { + // BACK UP b_prior_ + b_prior_backup_ = b_prior_; + err_prior_backup_ = err_prior_; + + /// update with first order Taylor, b' = b + \frac{\delta b}{\delta x} * \delta x + /// \delta x = Computes the linearized deviation from the references (linearization points) + b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_); // update the error_prior + err_prior_ = -Jt_prior_inv_ * b_prior_.head(ordering_poses_ - 15); + +// std::cout << " : "<< b_prior_.norm()<<" " <RollBackParameters(); + } + + // Roll back prior_ + if (err_prior_.rows() > 0) { + b_prior_ = b_prior_backup_; + err_prior_ = err_prior_backup_; + } +} + +/// LM +void Problem::ComputeLambdaInitLM() { + ni_ = 2.; + currentLambda_ = -1.; + currentChi_ = 0.0; + + for (auto edge: edges_) { + currentChi_ += edge.second->RobustChi2(); + } + if (err_prior_.rows() > 0) + currentChi_ += err_prior_.squaredNorm();//norm + currentChi_ *= 0.5; + + stopThresholdLM_ = 1e-10 * currentChi_; // 迭代条件为 误差下降 1e-6 倍 + + double maxDiagonal = 0; + ulong size = Hessian_.cols(); + assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square"); + //寻找对角线最大的值 + for (ulong i = 0; i < size; ++i) { + maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal); + } + + maxDiagonal = std::min(5e10, maxDiagonal); + double tau = 1e-5; // 1e-5 + currentLambda_ = tau * maxDiagonal; +// std::cout << "currentLamba_: "<ComputeResidual(); + tempChi += edge.second->RobustChi2(); + } + if (err_prior_.size() > 0) + tempChi += err_prior_.norm();//不是平方？？？ + tempChi *= 0.5; // 1/2 * err^2 + + rho = (currentChi_ - tempChi) / scale; + currentChi_ = tempChi; + + +} +bool Problem::IsGoodStepInLM() { + double scale = 0; +// scale = 0.5 * delta_x_.transpose() * (currentLambda_ * delta_x_ + b_); +// scale += 1e-3; // make sure it's non-zero :) + scale = 0.5* delta_x_.transpose() * (currentLambda_ * delta_x_ + b_); + scale += 1e-6; // make sure it's non-zero :) + + // recompute residuals after update state + double tempChi = 0.0; + for (auto edge: edges_) { + edge.second->ComputeResidual(); + tempChi += edge.second->RobustChi2(); + } + if (err_prior_.size() > 0) + tempChi += err_prior_.norm();//不是平方？？？ + tempChi *= 0.5; // 1/2 * err^2 + + double rho = (currentChi_ - tempChi) / scale; + if (rho > 0 && isfinite(tempChi)) // last step was good, 误差在下降 + { + double alpha = 1. - pow((2 * rho - 1), 3); + alpha = std::min(alpha, 2. / 3.); + double scaleFactor = (std::max)(1. / 3., alpha); + currentLambda_ *= scaleFactor; + ni_ = 2; + currentChi_ = tempChi; + return true; + } else { + currentLambda_ *= ni_; + ni_ *= 2; + return false; + } +} + + +/** @brief conjugate gradient with perconditioning + * + * the jacobi PCG method + * + */ +VecX Problem::PCGSolver(const MatXX &A, const VecX &b, int maxIter = -1) { + assert(A.rows() == A.cols() && "PCG solver ERROR: A is not a square matrix"); + int rows = b.rows(); + int n = maxIter < 0 ? rows : maxIter; + VecX x(VecX::Zero(rows)); + MatXX M_inv = A.diagonal().asDiagonal().inverse(); + VecX r0(b); // initial r = b - A*0 = b + VecX z0 = M_inv * r0; + VecX p(z0); + VecX w = A * p; + double r0z0 = r0.dot(z0); + double alpha = r0z0 / p.dot(w); + VecX r1 = r0 - alpha * w; + int i = 0; + double threshold = 1e-6 * r0.norm(); + while (r1.norm() > threshold && i < n) { + i++; + VecX z1 = M_inv * r1; + double r1z1 = r1.dot(z1); + double belta = r1z1 / r0z0; + z0 = z1; + r0z0 = r1z1; + r0 = r1; + p = belta * p + z1; + w = A * p; + alpha = r1z1 / p.dot(w); + x += alpha * p; + r1 -= alpha * w; + } + return x; +} + +/* + * marg 所有和 frame 相连的 edge: imu factor, projection factor + * 如果某个landmark和该frame相连，但是又不想加入marg, 那就把改edge先去掉 + * + */ +bool Problem::Marginalize(const std::vector > margVertexs, int pose_dim) { + + SetOrdering(); + /// 找到需要 marg 的 edge, margVertexs[0] is frame, its edge contained pre-intergration + std::vector> marg_edges = GetConnectedEdges(margVertexs[0]); + + std::unordered_map> margLandmark; + // 构建 Hessian 的时候 pose 的顺序不变，landmark的顺序要重新设定 + int marg_landmark_size = 0; +// std::cout << "\n marg edge 1st id: "<< marg_edges.front()->Id() << " end id: "<Id()<Id() <Verticies(); + for (auto iter : verticies) { + if (IsLandmarkVertex(iter) && margLandmark.find(iter->Id()) == margLandmark.end()) { + iter->SetOrderingId(pose_dim + marg_landmark_size); + margLandmark.insert(make_pair(iter->Id(), iter)); + marg_landmark_size += iter->LocalDimension(); + } + } + } +// std::cout << "pose dim: " << pose_dim <ComputeResidual(); + edge->ComputeJacobians(); + auto jacobians = edge->Jacobians(); + auto verticies = edge->Verticies(); + ii++; + + assert(jacobians.size() == verticies.size()); + for (size_t i = 0; i < verticies.size(); ++i) { + auto v_i = verticies[i]; + auto jacobian_i = jacobians[i]; + ulong index_i = v_i->OrderingId(); + ulong dim_i = v_i->LocalDimension(); + + double drho; + MatXX robustInfo(edge->Information().rows(),edge->Information().cols()); + edge->RobustInfo(drho,robustInfo); + + for (size_t j = i; j < verticies.size(); ++j) { + auto v_j = verticies[j]; + auto jacobian_j = jacobians[j]; + ulong index_j = v_j->OrderingId(); + ulong dim_j = v_j->LocalDimension(); + + MatXX hessian = jacobian_i.transpose() * robustInfo * jacobian_j; + + assert(hessian.rows() == v_i->LocalDimension() && hessian.cols() == v_j->LocalDimension()); + // 所有的信息矩阵叠加起来 + H_marg.block(index_i, index_j, dim_i, dim_j) += hessian; + if (j != i) { + // 对称的下三角 + H_marg.block(index_j, index_i, dim_j, dim_i) += hessian.transpose(); + } + } + b_marg.segment(index_i, dim_i) -= drho * jacobian_i.transpose() * edge->Information() * edge->Residual(); + } + + } + std::cout << "edge factor cnt: " << ii < 0) { + int marg_size = marg_landmark_size; + MatXX Hmm = H_marg.block(reserve_size, reserve_size, marg_size, marg_size); + MatXX Hpm = H_marg.block(0, reserve_size, reserve_size, marg_size); + MatXX Hmp = H_marg.block(reserve_size, 0, marg_size, reserve_size); + VecX bpp = b_marg.segment(0, reserve_size); + VecX bmm = b_marg.segment(reserve_size, marg_size); + + // Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速 + MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size)); + // TODO:: use openMP + for (auto iter: margLandmark) { + int idx = iter.second->OrderingId() - reserve_size; + int size = iter.second->LocalDimension(); + Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse(); + } + + MatXX tempH = Hpm * Hmm_inv; + MatXX Hpp = H_marg.block(0, 0, reserve_size, reserve_size) - tempH * Hmp; + bpp = bpp - tempH * bmm; + H_marg = Hpp; + b_marg = bpp; + } + + VecX b_prior_before = b_prior_; + if(H_prior_.rows() > 0) + { + H_marg += H_prior_; + b_marg += b_prior_; + } + + /// marg frame and speedbias + int marg_dim = 0; + + // index 大的先移动 + for (int k = margVertexs.size() -1 ; k >= 0; --k) + { + + int idx = margVertexs[k]->OrderingId(); + int dim = margVertexs[k]->LocalDimension(); +// std::cout << k << " "< saes(Amm); + Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd( + (saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * + saes.eigenvectors().transpose(); + + Eigen::VectorXd bmm2 = b_marg.segment(n2, m2); + Eigen::MatrixXd Arm = H_marg.block(0, n2, n2, m2); + Eigen::MatrixXd Amr = H_marg.block(n2, 0, m2, n2); + Eigen::MatrixXd Arr = H_marg.block(0, 0, n2, n2); + Eigen::VectorXd brr = b_marg.segment(0, n2); + Eigen::MatrixXd tempB = Arm * Amm_inv; + H_prior_ = Arr - tempB * Amr; + b_prior_ = brr - tempB * bmm2; + + Eigen::SelfAdjointEigenSolver saes2(H_prior_); + Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0)); + Eigen::VectorXd S_inv = Eigen::VectorXd( + (saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0)); + + Eigen::VectorXd S_sqrt = S.cwiseSqrt(); + Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); + Jt_prior_inv_ = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); + err_prior_ = -Jt_prior_inv_ * b_prior_; + + MatXX J = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); + H_prior_ = J.transpose() * J; + MatXX tmp_h = MatXX( (H_prior_.array().abs() > 1e-9).select(H_prior_.array(),0) ); + H_prior_ = tmp_h; + + // std::cout << "my marg b prior: " < +#include +#include + +#include "eigen_types.h" +#include "edge.h" +#include "vertex.h" + +typedef unsigned long ulong; + +namespace myslam { +namespace backend { + +typedef unsigned long ulong; +// typedef std::unordered_map> HashVertex; +typedef std::map> HashVertex; +typedef std::unordered_map> HashEdge; +typedef std::unordered_multimap> HashVertexIdToEdge; + +class Problem { +public: + + /** + * 问题的类型 + * SLAM问题还是通用的问题 + * + * 如果是SLAM问题那么pose和landmark是区分开的，Hessian以稀疏方式存储 + * SLAM问题只接受一些特定的Vertex和Edge + * 如果是通用问题那么hessian是稠密的，除非用户设定某些vertex为marginalized + */ + enum class ProblemType { + SLAM_PROBLEM, + GENERIC_PROBLEM + }; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + Problem(ProblemType problemType); + + ~Problem(); + + bool AddVertex(std::shared_ptr vertex); + + /** + * remove a vertex + * @param vertex_to_remove + */ + bool RemoveVertex(std::shared_ptr vertex); + + bool AddEdge(std::shared_ptr edge); + + bool RemoveEdge(std::shared_ptr edge); + + /** + * 取得在优化中被判断为outlier部分的边，方便前端去除outlier + * @param outlier_edges + */ + void GetOutlierEdges(std::vector> &outlier_edges); + + /** + * 求解此问题 + * @param iterations + * @return + */ + bool Solve(int iterations = 10); + bool SolveWithDogleg(int iterations); + + /// 边缘化一个frame和以它为host的landmark + bool Marginalize(std::shared_ptr frameVertex, + const std::vector> &landmarkVerticies); + + bool Marginalize(const std::shared_ptr frameVertex); + bool Marginalize(const std::vector > frameVertex,int pose_dim); + + MatXX GetHessianPrior(){ return H_prior_;} + VecX GetbPrior(){ return b_prior_;} + VecX GetErrPrior(){ return err_prior_;} + MatXX GetJtPrior(){ return Jt_prior_inv_;} + + void SetHessianPrior(const MatXX& H){H_prior_ = H;} + void SetbPrior(const VecX& b){b_prior_ = b;} + void SetErrPrior(const VecX& b){err_prior_ = b;} + void SetJtPrior(const MatXX& J){Jt_prior_inv_ = J;} + + void ExtendHessiansPriorSize(int dim); + + //test compute prior + void TestComputePrior(); + +private: + + /// Solve的实现，解通用问题 + bool SolveGenericProblem(int iterations); + + /// Solve的实现，解SLAM问题 + bool SolveSLAMProblem(int iterations); + + /// 设置各顶点的ordering_index + void SetOrdering(); + + /// set ordering for new vertex in slam problem + void AddOrderingSLAM(std::shared_ptr v); + + /// 构造大H矩阵 + void MakeHessian(); + + /// schur求解SBA + void SchurSBA(); + + /// 解线性方程 + void SolveLinearSystem(); + + /// 更新状态变量 + void UpdateStates(); + + void RollbackStates(); // 有时候 update 后残差会变大，需要退回去，重来 + + /// 计算并更新Prior部分 + void ComputePrior(); + + /// 判断一个顶点是否为Pose顶点 + bool IsPoseVertex(std::shared_ptr v); + + /// 判断一个顶点是否为landmark顶点 + bool IsLandmarkVertex(std::shared_ptr v); + + /// 在新增顶点后，需要调整几个hessian的大小 + void ResizePoseHessiansWhenAddingPose(std::shared_ptr v); + + /// 检查ordering是否正确 + bool CheckOrdering(); + + void LogoutVectorSize(); + + /// 获取某个顶点连接到的边 + std::vector> GetConnectedEdges(std::shared_ptr vertex); + + /// Levenberg + /// 计算LM算法的初始Lambda + void ComputeLambdaInitLM(); + + /// Hessian 对角线加上或者减去 Lambda + void AddLambdatoHessianLM(); + + void RemoveLambdaHessianLM(); + + /// LM 算法中用于判断 Lambda 在上次迭代中是否可以，以及Lambda怎么缩放 + bool IsGoodStepInLM(); + + /// PCG 迭代线性求解器 + VecX PCGSolver(const MatXX &A, const VecX &b, int maxIter); + + double getXnorm(); + + void updateRang(); + void IsGoodStepbeta(); + + double currentLambda_; + double currentChi_; + double _delta; + double rho; + double alpha_; + double stopThresholdLM_; // LM 迭代退出阈值条件 + double ni_; //控制 Lambda 缩放大小 + + ProblemType problemType_; + + /// 整个信息矩阵 + MatXX Hessian_; + VecX b_; + VecX delta_x_; + VecX _auxVector; + VecX _hsd; ///< steepest decent step + VecX _hdl; ///< final dogleg step + + + /// 先验部分信息 + MatXX H_prior_; + VecX b_prior_; + VecX b_prior_backup_; + VecX err_prior_backup_; + + MatXX Jt_prior_inv_; + VecX err_prior_; + + /// SBA的Pose部分 + MatXX H_pp_schur_; + VecX b_pp_schur_; + // Heesian 的 Landmark 和 pose 部分 + MatXX H_pp_; + VecX b_pp_; + MatXX H_ll_; + VecX b_ll_; + + /// all vertices + HashVertex verticies_; + + /// all edges + HashEdge edges_; + + /// 由vertex id查询edge + HashVertexIdToEdge vertexToEdge_; + + /// Ordering related + ulong ordering_poses_ = 0; + ulong ordering_landmarks_ = 0; + ulong ordering_generic_ = 0; + std::map> idx_pose_vertices_; // 以ordering排序的pose顶点 + std::map> idx_landmark_vertices_; // 以ordering排序的landmark顶点 + + // verticies need to marg. + HashVertex verticies_marg_; + + bool bDebug = false; + double t_hessian_cost_ = 0.0; + double t_PCGsovle_cost_ = 0.0; +}; + +} +} + +#endif