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, final_T, MatrixXd::Identity(6, 6)*DENSE_TRACKING_WEIGHT ) ; break ; } }