suma++笔记二

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

                }

            }

        }

    }

}