? src/core/SurfelMapping.cpp
? src/core/Preprocessing.cpp
? src/core/Frame2Model.cpp
? src/core/SurfelMap.cpp
? src/core/lie_algebra.cpp
? src/core/LieGaussNewton.cpp
? src/core/Posegraph.cpp
? src/core/ImagePyramidGenerator.cpp

//先看看SurfelMapping

//主要操作是icp和閉包檢測。

//先看輸入處理

void processScan(const rv::LaserScan& scan)

{

??? Stopwatch::tic();//時間tictak

??? //檢查優化是否就緒，復制pose，重新初始化循環閉包計數。

??? if(makeLoopClosures_ && performMapping_)

??????? integrateLoopClosures();

??? //初始化雷達數據,將雷達數據表示成xyz(為啥不用點云庫？),復制給current_pts_

??? Stopwatch::tic();

??? initialize(scan);

??? statistics_["initialize-time"]=Stopwatch::toc();//輸出時間

??? //預處理,將點運數據映射到當前坐標系

??? Stopwatch::tic();

??? preprocess();

??? statistics_["preprocessing-time"] = Stopwatch::toc();

??? //歷史點云累計到當前幀

??? if (timestamp_>0)

??? {

??????? Stopwatch::tic();

??????? updatePose();

??????? statistics_["icp-time"]=Stopwatch::toc();

??????? Stopwatch::tic();

??????? if(makeLoopClosures_ && performMapping_)

??????????? checkLoopClosure();

??????? statistics_["loop-time"]=Stopwatch::toc();

??? }

???

}

void initialize(const Laserscan& scan)

{

??? lastFrame_.swap(currentFrame_);

??? lastModelFrame_.swap(currentModelFrame_);

??? current_pts_.assign(scan.points());

}

?

void preprocess()

{

??? Stopwatch::tic();

??? preprocessor_.process(current_pts_,*currentFrame_);

??? if(performMapping_)

??? {

??????? float ct = getConfidenceThreshold();

??????? Stopwatch::tic();

??????? map_.render（currentPose_old_.cast<float>(),currentPose_new_.cast<float>(),*lastModelFrame_,ct）;

??????? statistics_["map rendering"]=1000. * Stopwatch::toc();

??????? lastModelFrame_->pose = currentPose_new_.cast<float>();

??? }

??? statistics_["preprocessing-time"] = Stopwatch::toc();

?

}

void updatePose()

{

??? if(!initialize_identity_)

??? {

??????? T0 = lastIncrement_;

??? }

??? else

??? {

??????? T0 = Eigen::Matrix4d::Identity();

??? }

??? Stopwatch::tic();

??? Stopwatch::tic();

??? if(performMapping_)

??????? objective_->setData(currentFrame_,map_->newMapFrame());

??? else

??????? objective_->setData(currentFrame_,lastFrame_);

??? Stopwatch::tic();

??? int32_t success = gn_->minimize(*objective_,T0);

??? odom_poses_ = gn_->history();

??? statistics_["opt-time"]=Stopwatch::toc();

??? statistics_["num_iterations"]=gn_->iterationCount();

??? Eigen::Matrix4d increment = gn_->pose();

??? Eigen::Matrix4d delta = lastIncrement_.inverse() * increment;

??? Eigen::MatrixXd JtJ_new(6,6);

??? Eigen::MatrixXd Jtr(6,1);

??? Stopwatch::tic();

??? if(performMapping_)

??? {

??????? map_->render_active((currentPose_new_ * increment).cast<float>(),getConfidenceThreshold());

??????? lastModelFrame_->copy(*map_->newMapFrame());

??????? objective_->setData(currentFrame_,map_->newMapFrame());

???????

??? }

??? objective_->initialize(Eigen::Matrix4d::Identity());

??? double residual = objective_->jacobianProducts(JtJ_new,Jtr);

??? statistics_["time_residual_new"] = Stopwatch::toc();

??? result_new_.error = residual;

??? result_new_.inlier = objective_->inlier();

??? result_new_.outlier = objective_->outlier();

??? result_new_.residual = result_new_.error/(result_new_.inlier + result_new_.outlier);

??? result_new_.inlier_residual = objective_->inlier_residual()/result_new_.inlier;

??? result_new_.invalid = objective_->invalid();

??? result_new_.valid = objective_->valid();

??? statistics_["icp-time"] = Stopwatch::toc();

? if (success < -1) {
??? std::cerr << "minimization not successful. reason => " << gn_->reason(success) << std::endl;
? }

? // replace this with simple brute force try (try always both possible initializations and take the one with smaller
? // inlier point2point error.
? // check pose increment error:
? float t_err = std::sqrt(delta(0, 3) * delta(0, 3) + delta(1, 3) * delta(1, 3) + delta(2, 3) * delta(2, 3));//平移誤差
? float angle = 0.5 * (delta(0, 0) + delta(1, 1) + delta(2, 2) - 1.0);//角度誤差
? float r_err = std::acos(std::max(std::min(angle, 1.0f), -1.0f));//限制在-pi到pi

? if (timestamp_ > 1 && (t_err > 0.4 || r_err > 0.1) && fallbackMode_ && recovery_ != nullptr) {
??? std::cout << "t_err: " << t_err << ", r_err: " << r_err << std::endl;
??? std::cout << "Lost track: Pose increment change too large. Falling back to frame-to-frame tracking." << std::endl;

??? trackLoss_ += 1;
??? recovery_->setData(currentFrame_, lastFrame_);

??? success = gn_->minimize(*recovery_, T0);
??? increment = gn_->pose();

??? if (success < -1) std::cout << "minimization not successful. reason => " << gn_->reason(success) << std::endl;
? }

? lastError = result_new_.error;

? lastPose_ = currentPose_;
? currentPose_ = currentPose_ * increment;
? lastPose_old_ = currentPose_old_;
? currentPose_old_ = currentPose_;
? currentPose_new_ = currentPose_;

? currentFrame_->pose = currentPose_.cast<float>();

? float distance = 0;

? if (timestamp_ > 0) {
??? posegraph_->setInitial(timestamp_, posegraph_->pose(timestamp_ - 1) * increment);

??? posegraph_->addEdge(timestamp_ - 1, timestamp_, increment, info_);
??? //??? posegraph_->addEdge(timestamp_ - 1, timestamp_, increment, JtJ_new);
??? distance = (posegraph_->pose(timestamp_ - 1).col(3) - currentPose_.col(3)).norm();
??? distance += trajectory_distances_[timestamp_ - 1];
? }

? trajectory_distances_.push_back(distance);

? lastIncrement_ = increment;? // take last pose increment for initialization.

? statistics_["icp-overall"] = Stopwatch::toc();

}

void checkLoopClosure()

{

??? //1.檢查附近的poses以搜索循環閉合。

??? //2.添加已驗證的循環閉包作為圖的約束邊。

??? //

??? Stopwatch::tic();

??? Eigen::MatrixXd JtJ(6,6);

??? Eigen::MatrixXd Jtr(6,1);

??? foundLoopClosureCandidate_ = false;

??? loopClosurePoses_.clear();

??? result_old_ = OptResult();

??? useLoopClosureCandidate_ = false;

??? Eigen::Matrix4d increment_old;

??? bool candidateAdded = false;

??? int32_t minCandidate = -1;

??? float outlier_ratio_new = result_new_.outlier/float(result_new_.outlier + result_new_.inlier);

??? float valid_ratio_new = float(result_new_.valid)/float(result_new_.invalid + result_new_.valid);

??? timeWithoutLoopClosure_ += 1;

??? //如果存在未經驗證的循環關閉，請驗證循環關閉：

??? if(unverifiedLoopClosures_.size() >0 || alreadyVerifiedLoopClosure_)

??? {

??????? Eigen::Matrix4f pose_old = lastPose_old_.cast<float>();

??????? //驗證和添加約束。

??????? map_->render_inactive(pose_old,getConfidenceThreshold());

??????? //調整增量。

??????? objective_->setData(currentFrame_,map->oldMapFrame());

??????? gn_->minimize(*objective_,lastIncrement_);

??????? float valid_ratio = float(objective_->valid())/float(objective_->valid() + objective_->invalid());

??????? float outlier_ratio = float(objective_->outlier())/float(objective_->outlier() + objective_->lnlier());

??????? increment_old = gn_->pose();

??????? float increment_difference = (SE3::log(lastIncrement_)? - SE3::log(increment_old)).norm();//增加距離

??????? if(valid_ratio > 0.2 && outlier_ratio<0.85 && increment_difference<0.1)

??????? {

??????????? pose_old = (lastPose_old_ * increment_old).cast<float>();

??????????? //融合虛擬地圖的繪制及與當前里程估計的比較

??????????? map_->render_composed(pose_old,currentPose_new_.cast<float>(),getConfidenceThreshold());

??????????? objective_->setData(currentFrame_,map->composedFrame());

??????????? objective_->initialize(Eigen::Matrix4d::Identity());

??????????? float error = objective_->jacobianProducts(JtJ,Jtr);

??????????? float residual = error / float(objective_->inlier()+objective_->outlier());

??????????? result_old_.error = error;

??????????? result_old_.inlier = objective_->inlier();

??????????? result_old_.outlier =objective_->outlier();

??????????? result_old_.residual = result_old_.error/float(result_old_.inlier+result_old_.outlier);

??????????? result_old_.inlier_residual = objective_->inlier_residual()/result_old_.inlier;

??????????? result_old_.valid = objective_->valid();

??????????? result_old_.invalid = objective_->invalid();

??????????? float rel_error_all = residual / result_new_.residual;

??????????? foundLoopClosureCandidate_ = true;

??????????? currentPose_old_ = lastPose_old_ * increment_old;

??????????? loopClosurePoses_.push_back(currentPose_old_.cast<float>());

??????????? bool loop_closure = (rel_error_all < loopResidualThres_) || (residual - result_new_.residual)<0.1;

??????????? if(loop_closure)

??????????? {

??????????????? timeWithoutLoopClosure_ = 0;

??????????????? LoopClosureCandidate candidate;

??????????????? candidate.from = timestamp_;

??????????????? int32_t index = getClosestIndex(currentPose_old_);

??????????????? if(index>-1)

??????????????? {

??????????????????? candidate.to =index;

??????????????????? Eigen::Matrix4d nearest_pose= posegraph_->pose(index);

??????????????? }

??????????? }

??????? }

??? }

}

總結

以上是生活随笔為你收集整理的suma++笔记二的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。