C++ SE3::rotationMatrix方法代码示例

bool depthFromTriangulation(
    const SE3& T_search_ref,
    const Vector3d& f_ref,
    const Vector3d& f_cur,
    double& depth)
{
  Matrix<double,3,2> A; A << T_search_ref.rotationMatrix() * f_ref, f_cur;
  const Matrix2d AtA = A.transpose()*A;
  if(AtA.determinant() < 0.000001)
    return false;
  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
  depth = fabs(depth2[0]);
  return true;
}

void SlamSystem::setReprojectionListRelateToLastestKeyFrame(int begin, int end, Frame* current,
                                                            const Eigen::Matrix3d& R_i_2_c, const Eigen::Vector3d& T_i_2_c )
{
    int num = end - begin;
    if ( num < 0 ){
        num += slidingWindowSize ;
    }
    int trackFrameCnt = 0 ;
    for (int i = 0; i < num; i++)
    {
        int ref_id = begin + i;
        if (ref_id >=  slidingWindowSize) {
            ref_id -= slidingWindowSize;
        }
        if ( slidingWindow[ref_id]->keyFrameFlag == false
             || trackFrameCnt > 10
             ){
            continue;
        }

        double closenessTH = 1.0 ;
        double distanceTH = closenessTH * 15 / (KFDistWeight*KFDistWeight);
        //double cosAngleTH = 1.0 - 0.25 * closenessTH ;

        //euclideanOverlapCheck
        double distFac = slidingWindow[ref_id]->meanIdepth ;
        Eigen::Vector3d dd = ( slidingWindow[ref_id]->T_bk_2_b0 - current->T_bk_2_b0) * distFac;
        if( dd.dot(dd) > distanceTH) continue;

//        Eigen::Quaterniond qq( slidingWindow[ref_id]->R_bk_2_b0.transpose() * current->R_bk_2_b0) ;
//        Eigen::Vector3d aa = qq.vec()*2.0 ;
//        double dirDotProd = aa.dot( aa ) ;
//        if(dirDotProd < cosAngleTH) continue;


        Matrix3d R_i_2_j ;
        Vector3d T_i_2_j ;
        SE3 c2f_init ;
        //check from current to ref
        R_i_2_j = slidingWindow[ref_id]->R_bk_2_b0.transpose() * current->R_bk_2_b0 ;
        T_i_2_j = -slidingWindow[ref_id]->R_bk_2_b0.transpose() * ( slidingWindow[ref_id]->T_bk_2_b0 - current->T_bk_2_b0 ) ;
        c2f_init.setRotationMatrix(R_i_2_j);
        c2f_init.translation() = T_i_2_j ;
        trackerConstraint->trackFrameOnPermaref(current, slidingWindow[ref_id].get(), c2f_init ) ;
        if ( trackerConstraint->trackingWasGood == false ){
            //ROS_WARN("first check fail") ;
            continue ;
        }
        //ROS_WARN("pass first check") ;

        //check from ref to current
        R_i_2_j = current->R_bk_2_b0.transpose() * slidingWindow[ref_id]->R_bk_2_b0 ;
        T_i_2_j = -current->R_bk_2_b0.transpose() * ( current->T_bk_2_b0 - slidingWindow[ref_id]->T_bk_2_b0 ) ;
        c2f_init.setRotationMatrix(R_i_2_j);
        c2f_init.translation() = T_i_2_j ;
        trackerConstraint->trackFrameOnPermaref(slidingWindow[ref_id].get(), current, c2f_init ) ;
        if ( trackerConstraint->trackingWasGood == false ){
            //ROS_WARN("second check fail") ;
            continue ;
        }
        //ROS_WARN("pass second check") ;

        //Pass the cross check
        if (  trackingReferenceConstraint->keyframe != slidingWindow[ref_id].get() ){
             trackingReferenceConstraint->importFrame( slidingWindow[ref_id].get() );
        }

        SE3 RefToFrame = trackerConstraint->trackFrame( trackingReferenceConstraint, current,
                                   c2f_init );
        trackFrameCnt++ ;
        //float tracking_lastResidual = trackerConstraint->lastResidual;
        //float tracking_lastUsage = trackerConstraint->pointUsage;
        //float tracking_lastGoodPerBad = trackerConstraint->lastGoodCount / (trackerConstraint->lastGoodCount + trackerConstraint->lastBadCount);
        float tracking_lastGoodPerTotal = trackerConstraint->lastGoodCount / (current->width(SE3TRACKING_MIN_LEVEL)*current->height(SE3TRACKING_MIN_LEVEL));
        Sophus::Vector3d dist = RefToFrame.translation() * slidingWindow[ref_id]->meanIdepth;
        float minVal = 1.0f;
        float lastTrackingClosenessScore = getRefFrameScore(dist.dot(dist), trackerConstraint->pointUsage, KFDistWeight, KFUsageWeight);
        if ( trackerConstraint->trackingWasGood == false
             ||  tracking_lastGoodPerTotal < MIN_GOODPERALL_PIXEL
             || lastTrackingClosenessScore > minVal
             )
        {
            continue ;
        }



#ifdef PROJECT_TO_IMU_CENTER
        Eigen::Matrix3d r_i_2_j = RefToFrame.rotationMatrix().cast<double>();
        Eigen::Vector3d t_i_2_j = RefToFrame.translation().cast<double>();
        Eigen::Matrix3d final_R = R_i_2_c.transpose()*r_i_2_j*R_i_2_c;
        Eigen::Vector3d final_T = R_i_2_c.transpose()*(r_i_2_j*T_i_2_c + t_i_2_j ) - R_i_2_c.transpose()*T_i_2_c ;
#else
        Eigen::Matrix3d final_R = RefToFrame.rotationMatrix().cast<double>();
        Eigen::Vector3d final_T = RefToFrame.translation().cast<double>();
#endif

        //ROS_WARN("[add link, from %d to %d]", slidingWindow[ref_id]->id(), current->id() ) ;
        insertCameraLink( slidingWindow[ref_id].get(), current,
                          final_R,
//.........这里部分代码省略.........