void SlamSystem::trackFrame(uchar* image, unsigned int frameID, bool blockUntilMapped, double timestamp)
{
// Create new frame
std::shared_ptr<Frame> trackingNewFrame(new Frame(frameID, width, height, K, timestamp, image));
if(!trackingIsGood)
{
relocalizer.updateCurrentFrame(trackingNewFrame);
unmappedTrackedFramesMutex.lock();
unmappedTrackedFramesSignal.notify_one();
unmappedTrackedFramesMutex.unlock();
return;
}
currentKeyFrameMutex.lock();
bool my_createNewKeyframe = createNewKeyFrame; // pre-save here, to make decision afterwards.
if(trackingReference->keyframe != currentKeyFrame.get() || currentKeyFrame->depthHasBeenUpdatedFlag)
{
trackingReference->importFrame(currentKeyFrame.get());
currentKeyFrame->depthHasBeenUpdatedFlag = false;
trackingReferenceFrameSharedPT = currentKeyFrame;
}
FramePoseStruct* trackingReferencePose = trackingReference->keyframe->pose;
currentKeyFrameMutex.unlock();
// DO TRACKING & Show tracking result.
if(enablePrintDebugInfo && printThreadingInfo)
printf("TRACKING %d on %d\n", trackingNewFrame->id(), trackingReferencePose->frameID);
poseConsistencyMutex.lock_shared();
SE3 frameToReference_initialEstimate = se3FromSim3(
trackingReferencePose->getCamToWorld().inverse() * keyFrameGraph->allFramePoses.back()->getCamToWorld());
poseConsistencyMutex.unlock_shared();
struct timeval tv_start, tv_end;
gettimeofday(&tv_start, NULL);
SE3 newRefToFrame_poseUpdate = tracker->trackFrame(
trackingReference,
trackingNewFrame.get(),
frameToReference_initialEstimate);
gettimeofday(&tv_end, NULL);
msTrackFrame = 0.9*msTrackFrame + 0.1*((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);
nTrackFrame++;
tracking_lastResidual = tracker->lastResidual;
tracking_lastUsage = tracker->pointUsage;
tracking_lastGoodPerBad = tracker->lastGoodCount / (tracker->lastGoodCount + tracker->lastBadCount);
tracking_lastGoodPerTotal = tracker->lastGoodCount / (trackingNewFrame->width(SE3TRACKING_MIN_LEVEL)*trackingNewFrame->height(SE3TRACKING_MIN_LEVEL));
if(manualTrackingLossIndicated || tracker->diverged || (keyFrameGraph->keyframesAll.size() > INITIALIZATION_PHASE_COUNT && !tracker->trackingWasGood))
{
printf("TRACKING LOST for frame %d (%1.2f%% good Points, which is %1.2f%% of available points, %s)!\n",
trackingNewFrame->id(),
100*tracking_lastGoodPerTotal,
100*tracking_lastGoodPerBad,
tracker->diverged ? "DIVERGED" : "NOT DIVERGED");
trackingReference->invalidate();
trackingIsGood = false;
nextRelocIdx = -1;
unmappedTrackedFramesMutex.lock();
unmappedTrackedFramesSignal.notify_one();
unmappedTrackedFramesMutex.unlock();
manualTrackingLossIndicated = false;
return;
}
if(plotTracking)
{
Eigen::Matrix<float, 20, 1> data;
data.setZero();
data[0] = tracker->lastResidual;
data[3] = tracker->lastGoodCount / (tracker->lastGoodCount + tracker->lastBadCount);
data[4] = 4*tracker->lastGoodCount / (width*height);
data[5] = tracker->pointUsage;
data[6] = tracker->affineEstimation_a;
data[7] = tracker->affineEstimation_b;
//outputWrapper->publishDebugInfo(data);
}
keyFrameGraph->addFrame(trackingNewFrame.get());
//Sim3 lastTrackedCamToWorld = mostCurrentTrackedFrame->getScaledCamToWorld();// mostCurrentTrackedFrame->TrackingParent->getScaledCamToWorld() * sim3FromSE3(mostCurrentTrackedFrame->thisToParent_SE3TrackingResult, 1.0);
// if (outputWrapper != 0)
// {
// outputWrapper->publishTrackedFrame(trackingNewFrame.get());
// }
// Keyframe selection
latestTrackedFrame = trackingNewFrame;
if (!my_createNewKeyframe && currentKeyFrame->numMappedOnThisTotal > MIN_NUM_MAPPED)
{
Sophus::Vector3d dist = newRefToFrame_poseUpdate.translation() * currentKeyFrame->meanIdepth;
float minVal = fmin(0.2f + keyFrameGraph->keyframesAll.size() * 0.8f / INITIALIZATION_PHASE_COUNT,1.0f);
if(keyFrameGraph->keyframesAll.size() < INITIALIZATION_PHASE_COUNT) minVal *= 0.7;
lastTrackingClosenessScore = trackableKeyFrameSearch->getRefFrameScore(dist.dot(dist), tracker->pointUsage);
if (lastTrackingClosenessScore > minVal)
{
createNewKeyFrame = true;
if(enablePrintDebugInfo && printKeyframeSelectionInfo)
printf("SELECT %d on %d! dist %.3f + usage %.3f = %.3f > 1\n",trackingNewFrame->id(),trackingNewFrame->getTrackingParent()->id(), dist.dot(dist), tracker->pointUsage, trackableKeyFrameSearch->getRefFrameScore(dist.dot(dist), tracker->pointUsage));
}
else
{
if(enablePrintDebugInfo && printKeyframeSelectionInfo)
printf("SKIPPD %d on %d! dist %.3f + usage %.3f = %.3f > 1\n",trackingNewFrame->id(),trackingNewFrame->getTrackingParent()->id(), dist.dot(dist), tracker->pointUsage, trackableKeyFrameSearch->getRefFrameScore(dist.dot(dist), tracker->pointUsage));
}
}
unmappedTrackedFramesMutex.lock();
if(unmappedTrackedFrames.size() < 50 || (unmappedTrackedFrames.size() < 100 && trackingNewFrame->getTrackingParent()->numMappedOnThisTotal < 10))
unmappedTrackedFrames.push_back(trackingNewFrame);
unmappedTrackedFramesSignal.notify_one();
unmappedTrackedFramesMutex.unlock();
// implement blocking
if(blockUntilMapped && trackingIsGood)
{
boost::unique_lock<boost::mutex> lock(newFrameMappedMutex);
while(unmappedTrackedFrames.size() > 0)
{
//printf("TRACKING IS BLOCKING, waiting for %d frames to finish mapping.\n", (int)unmappedTrackedFrames.size());
newFrameMappedSignal.wait(lock);
}
lock.unlock();
}
}
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 ;
}
}
void SlamSystem::trackFrame(cv::Mat img0, unsigned int frameID,
ros::Time imageTimeStamp, Eigen::Matrix3d deltaR,
const Eigen::Matrix3d R_i_2_c,const Eigen::Vector3d T_i_2_c
)
{
// Create new frame
std::shared_ptr<Frame> trackingNewFrame(
new Frame( frameID, width, height, K, imageTimeStamp.toSec(), img0.data,
Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero() )
);
//updateTrackingReference() ;
//initial guess
SE3 RefToFrame_initialEstimate ;
RefToFrame_initialEstimate.setRotationMatrix( deltaR.transpose()*RefToFrame.rotationMatrix() );
RefToFrame_initialEstimate.translation() =
deltaR.transpose()*RefToFrame.translation() ;
//track
struct timeval tv_start, tv_end;
gettimeofday(&tv_start, NULL);
RefToFrame = tracker->trackFrame( trackingReference, trackingNewFrame.get(),
RefToFrame_initialEstimate );
gettimeofday(&tv_end, NULL);
// Eigen::Matrix3d R_k_2_c = RefToFrame.rotationMatrix();
// Eigen::Vector3d T_k_2_c = RefToFrame.translation();
// Matrix3d R_bk1_2_b0 = trackingReference->keyframe->R_bk_2_b0 * R_k_2_c.transpose();
// Vector3d T_bk1_2_b0 = trackingReference->keyframe->T_bk_2_b0 + R_bk1_2_b0*T_k_2_c ;
// pubOdometry(-T_bk1_2_b0, R_bk1_2_b0, pub_odometry, pub_pose );
// pubPath(-T_bk1_2_b0, path_line, pub_path );
//debug information
//msTrackFrame = 0.9*msTrackFrame + 0.1*((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);
msTrackFrame = (tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f ;
printf("msTrackFrame = %0.f\n", msTrackFrame ) ;
nTrackFrame++;
tracking_lastResidual = tracker->lastResidual;
tracking_lastUsage = tracker->pointUsage;
tracking_lastGoodPerBad = tracker->lastGoodCount / (tracker->lastGoodCount + tracker->lastBadCount);
tracking_lastGoodPerTotal = tracker->lastGoodCount / (trackingNewFrame->width(SE3TRACKING_MIN_LEVEL)*trackingNewFrame->height(SE3TRACKING_MIN_LEVEL));
// geometry_msgs::Vector3 v_pub ;
// Vector3d translation = RefToFrame.translation() ;
// v_pub.x = translation(0) ;
// v_pub.y = translation(1) ;
// v_pub.z = translation(2) ;
// pub_denseTracking.publish( v_pub ) ;
// Keyframe selection
createNewKeyFrame = false ;
//printf("tracking_lastGoodPerTotal = %f\n", tracking_lastGoodPerTotal ) ;
if ( trackingReference->keyframe->numFramesTrackedOnThis > MIN_NUM_MAPPED )
{
Sophus::Vector3d dist = RefToFrame.translation() * currentKeyFrame->meanIdepth;
float minVal = 1.0f;
lastTrackingClosenessScore = getRefFrameScore(dist.dot(dist), tracker->pointUsage, KFDistWeight, KFUsageWeight);
if (lastTrackingClosenessScore > minVal || tracker->trackingWasGood == false
|| tracking_lastGoodPerTotal < MIN_GOODPERALL_PIXEL
)
{
createNewKeyFrame = true;
// if(enablePrintDebugInfo && printKeyframeSelectionInfo)
// printf("[insert KF] dist %.3f + usage %.3f = %.3f > 1\n", dist.dot(dist), tracker->pointUsage, lastTrackingClosenessScore );
}
else
{
// if(enablePrintDebugInfo && printKeyframeSelectionInfo)
// printf("SKIPPD %d on %d! dist %.3f + usage %.3f = %.3f < 1\n",trackingNewFrame->id(),trackingNewFrame->getTrackingParent()->id(), dist.dot(dist), tracker->pointUsage, lastTrackingClosenessScore );
}
}
if ( tracker->diverged ){
createNewKeyFrame = true ;
}
frameInfoList_mtx.lock();
int tmpTail = frameInfoListTail+1 ;
if ( tmpTail >= frameInfoListSize ){
tmpTail -= frameInfoListSize;
}
FRAMEINFO& tmpFrameInfo = frameInfoList[tmpTail] ;
tmpFrameInfo.t = imageTimeStamp ;
#ifdef PROJECT_TO_IMU_CENTER
Eigen::Matrix3d r_k_2_c = RefToFrame.rotationMatrix().cast<double>();
Eigen::Vector3d t_k_2_c = RefToFrame.translation().cast<double>();
tmpFrameInfo.R_k_2_c = R_i_2_c.transpose()*r_k_2_c*R_i_2_c;
tmpFrameInfo.T_k_2_c = R_i_2_c.transpose()*(r_k_2_c*T_i_2_c + t_k_2_c ) - R_i_2_c.transpose()*T_i_2_c ;
#else
tmpFrameInfo.R_k_2_c = RefToFrame.rotationMatrix().cast<double>();
tmpFrameInfo.T_k_2_c = RefToFrame.translation().cast<double>();
#endif
// ROS_WARN("trackFrame = ") ;
// std::cout << tmpFrameInfo.T_k_2_c.transpose() << std::endl;
tmpFrameInfo.trust = tracker->trackingWasGood ;
tmpFrameInfo.keyFrameFlag = createNewKeyFrame ;
tmpFrameInfo.lastestATA = MatrixXd::Identity(6, 6)*DENSE_TRACKING_WEIGHT ;
frameInfoListTail = tmpTail ;
frameInfoList_mtx.unlock();
if ( createNewKeyFrame == true ){
tracking_mtx.lock();
lock_densetracking = true ;
tracking_mtx.unlock();
}
}
Frame* TrackableKeyFrameSearch::findRePositionCandidate(Frame* frame, float maxScore)
{
std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > potentialReferenceFrames =
findEuclideanOverlapFrames(frame, maxScore / (KFDistWeight*KFDistWeight), 0.75);
float bestScore = maxScore;
float bestDist, bestUsage;
float bestPoseDiscrepancy = 0;
Frame* bestFrame = 0;
SE3 bestRefToFrame = SE3();
SE3 bestRefToFrame_tracked = SE3();
int checkedSecondary = 0;
for(unsigned int i=0;i<potentialReferenceFrames.size();i++)
{
if(frame->getTrackingParent() == potentialReferenceFrames[i].ref)
continue;
if(potentialReferenceFrames[i].ref->idxInKeyframes < INITIALIZATION_PHASE_COUNT)
continue;
struct timeval tv_start, tv_end;
gettimeofday(&tv_start, NULL);
tracker->checkPermaRefOverlap(potentialReferenceFrames[i].ref, potentialReferenceFrames[i].refToFrame);
gettimeofday(&tv_end, NULL);
msTrackPermaRef = 0.9*msTrackPermaRef + 0.1*((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);
nTrackPermaRef++;
float score = getRefFrameScore(potentialReferenceFrames[i].dist, tracker->pointUsage);
if(score < maxScore)
{
SE3 RefToFrame_tracked = tracker->trackFrameOnPermaref(potentialReferenceFrames[i].ref, frame, potentialReferenceFrames[i].refToFrame);
Sophus::Vector3d dist = RefToFrame_tracked.translation() * potentialReferenceFrames[i].ref->meanIdepth;
float newScore = getRefFrameScore(dist.dot(dist), tracker->pointUsage);
float poseDiscrepancy = (potentialReferenceFrames[i].refToFrame * RefToFrame_tracked.inverse()).log().norm();
float goodVal = tracker->pointUsage * tracker->lastGoodCount / (tracker->lastGoodCount+tracker->lastBadCount);
checkedSecondary++;
if(tracker->trackingWasGood && goodVal > relocalizationTH && newScore < bestScore && poseDiscrepancy < 0.2)
{
bestPoseDiscrepancy = poseDiscrepancy;
bestScore = score;
bestFrame = potentialReferenceFrames[i].ref;
bestRefToFrame = potentialReferenceFrames[i].refToFrame;
bestRefToFrame_tracked = RefToFrame_tracked;
bestDist = dist.dot(dist);
bestUsage = tracker->pointUsage;
}
}
}
if(bestFrame != 0)
{
if(enablePrintDebugInfo && printRelocalizationInfo)
printf("FindReferences for %d: Checked %d (%d). dist %.3f + usage %.3f = %.3f. pose discrepancy %.2f. TAKE %d!\n",
(int)frame->id(), (int)potentialReferenceFrames.size(), checkedSecondary,
bestDist, bestUsage, bestScore,
bestPoseDiscrepancy, bestFrame->id());
return bestFrame;
}
else
{
if(enablePrintDebugInfo && printRelocalizationInfo)
printf("FindReferences for %d: Checked %d (%d), bestScore %.2f. MAKE NEW\n",
(int)frame->id(), (int)potentialReferenceFrames.size(), checkedSecondary, bestScore);
return 0;
}
}
