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