void DebugOutput3DWrapper::publishTrackedFrame(Frame* kf)
{
KeyFrameMessage fMsg;
fMsg.id = kf->id();
fMsg.time = kf->timestamp();
fMsg.isKeyframe = false;
memcpy(fMsg.camToWorld.data(),kf->getScaledCamToWorld().cast<float>().data(),sizeof(float)*7);
fMsg.fx = kf->fx(publishLvl);
fMsg.fy = kf->fy(publishLvl);
fMsg.cx = kf->cx(publishLvl);
fMsg.cy = kf->cy(publishLvl);
fMsg.width = kf->width(publishLvl);
fMsg.height = kf->height(publishLvl);
/*fMsg.pointcloud.clear();
liveframe_publisher.publish(fMsg);*/
SE3 camToWorld = se3FromSim3(kf->getScaledCamToWorld());
/*geometry_msgs::PoseStamped pMsg;
pMsg.pose.position.x = camToWorld.translation()[0];
pMsg.pose.position.y = camToWorld.translation()[1];
pMsg.pose.position.z = camToWorld.translation()[2];
pMsg.pose.orientation.x = camToWorld.so3().unit_quaternion().x();
pMsg.pose.orientation.y = camToWorld.so3().unit_quaternion().y();
pMsg.pose.orientation.z = camToWorld.so3().unit_quaternion().z();
pMsg.pose.orientation.w = camToWorld.so3().unit_quaternion().w();
if (pMsg.pose.orientation.w < 0)
{
pMsg.pose.orientation.x *= -1;
pMsg.pose.orientation.y *= -1;
pMsg.pose.orientation.z *= -1;
pMsg.pose.orientation.w *= -1;
}
pMsg.header.stamp = ros::Time(kf->timestamp());
pMsg.header.frame_id = "world";
pose_publisher.publish(pMsg);*/
cv::circle(tracker_display, cv::Point(320+camToWorld.translation()[0]*640, -240 + camToWorld.translation()[1]*480), 2, cv::Scalar(255, 0, 0),4);
cv::imshow("Tracking_output", tracker_display);
std::cout << "PublishTrackedKeyframe: " << camToWorld.translation()[0] << " " << camToWorld.translation()[1] << " " << camToWorld.translation()[2] << std::endl;
}
void ROSOutput3DWrapper::publishTrackedFrame(Frame* kf)
{
lsd_slam_viewer::keyframeMsg fMsg;
fMsg.id = kf->id();
fMsg.time = kf->timeStampNs()/1000000.0;
fMsg.isKeyframe = false;
memcpy(fMsg.camToWorld.data(),kf->getScaledCamToWorld().cast<float>().data(),sizeof(float)*7);
fMsg.fx = kf->fx(publishLvl);
fMsg.fy = kf->fy(publishLvl);
fMsg.cx = kf->cx(publishLvl);
fMsg.cy = kf->cy(publishLvl);
fMsg.width = kf->width(publishLvl);
fMsg.height = kf->height(publishLvl);
fMsg.pointcloud.clear();
liveframe_publisher.publish(fMsg);
SE3 camToWorld = se3FromSim3(kf->getScaledCamToWorld());
geometry_msgs::PoseStamped pMsg;
pMsg.pose.position.x = camToWorld.translation()[0];
pMsg.pose.position.y = camToWorld.translation()[1];
pMsg.pose.position.z = camToWorld.translation()[2];
pMsg.pose.orientation.x = camToWorld.so3().unit_quaternion().x();
pMsg.pose.orientation.y = camToWorld.so3().unit_quaternion().y();
pMsg.pose.orientation.z = camToWorld.so3().unit_quaternion().z();
pMsg.pose.orientation.w = camToWorld.so3().unit_quaternion().w();
if (pMsg.pose.orientation.w < 0)
{
pMsg.pose.orientation.x *= -1;
pMsg.pose.orientation.y *= -1;
pMsg.pose.orientation.z *= -1;
pMsg.pose.orientation.w *= -1;
}
pMsg.header.stamp = ros::Time(kf->timeStampNs()/1000000.0);
pMsg.header.frame_id = "world";
pose_publisher.publish(pMsg);
}
std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > TrackableKeyFrameSearch::findEuclideanOverlapFrames(Frame* frame, float distanceTH, float angleTH, bool checkBothScales)
{
// basically the maximal angle-difference in viewing direction is angleTH*(average FoV).
// e.g. if the FoV is 130°, then it is angleTH*130°.
float cosAngleTH = cosf(angleTH*0.5f*(fowX + fowY));
Eigen::Vector3d pos = frame->getScaledCamToWorld().translation();
Eigen::Vector3d viewingDir = frame->getScaledCamToWorld().rotationMatrix().rightCols<1>();
std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > potentialReferenceFrames;
float distFacReciprocal = 1;
if(checkBothScales)
distFacReciprocal = frame->meanIdepth / frame->getScaledCamToWorld().scale();
// for each frame, calculate the rough score, consisting of pose, scale and angle overlap.
graph->keyframesAllMutex.lock_shared();
for(unsigned int i=0;i<graph->keyframesAll.size();i++)
{
Eigen::Vector3d otherPos = graph->keyframesAll[i]->getScaledCamToWorld().translation();
// get distance between the frames, scaled to fit the potential reference frame.
float distFac = graph->keyframesAll[i]->meanIdepth / graph->keyframesAll[i]->getScaledCamToWorld().scale();
if(checkBothScales && distFacReciprocal < distFac) distFac = distFacReciprocal;
Eigen::Vector3d dist = (pos - otherPos) * distFac;
float dNorm2 = dist.dot(dist);
if(dNorm2 > distanceTH) continue;
Eigen::Vector3d otherViewingDir = graph->keyframesAll[i]->getScaledCamToWorld().rotationMatrix().rightCols<1>();
float dirDotProd = otherViewingDir.dot(viewingDir);
if(dirDotProd < cosAngleTH) continue;
potentialReferenceFrames.push_back(TrackableKFStruct());
potentialReferenceFrames.back().ref = graph->keyframesAll[i];
potentialReferenceFrames.back().refToFrame = se3FromSim3(graph->keyframesAll[i]->getScaledCamToWorld().inverse() * frame->getScaledCamToWorld()).inverse();
potentialReferenceFrames.back().dist = dNorm2;
potentialReferenceFrames.back().angle = dirDotProd;
}
graph->keyframesAllMutex.unlock_shared();
return potentialReferenceFrames;
}
void Relocalizer::threadLoop(int idx)
{
if(!multiThreading && idx != 0) return;
SE3Tracker* tracker = new SE3Tracker(w,h,K);
boost::unique_lock<boost::mutex> lock(exMutex);
while(continueRunning)
{
// if got something: do it (unlock in the meantime)
if(nextRelocIDX < maxRelocIDX && CurrentRelocFrame)
{
Frame* todo = KFForReloc[nextRelocIDX%KFForReloc.size()];
nextRelocIDX++;
if(todo->neighbors.size() <= 2) continue;
std::shared_ptr<Frame> myRelocFrame = CurrentRelocFrame;
lock.unlock();
// initial Alignment
SE3 todoToFrame = tracker->trackFrameOnPermaref(todo, myRelocFrame.get(), SE3());
// try neighbours
float todoGoodVal = tracker->pointUsage * tracker->lastGoodCount / (tracker->lastGoodCount+tracker->lastBadCount);
if(todoGoodVal > relocalizationTH)
{
int numGoodNeighbours = 0;
int numBadNeighbours = 0;
float bestNeightbourGoodVal = todoGoodVal;
float bestNeighbourUsage = tracker->pointUsage;
Frame* bestKF = todo;
SE3 bestKFToFrame = todoToFrame;
for(Frame* nkf : todo->neighbors)
{
SE3 nkfToFrame_init = se3FromSim3((nkf->getScaledCamToWorld().inverse() * todo->getScaledCamToWorld() * sim3FromSE3(todoToFrame.inverse(), 1))).inverse();
SE3 nkfToFrame = tracker->trackFrameOnPermaref(nkf, myRelocFrame.get(), nkfToFrame_init);
float goodVal = tracker->pointUsage * tracker->lastGoodCount / (tracker->lastGoodCount+tracker->lastBadCount);
if(goodVal > relocalizationTH*0.8 && (nkfToFrame * nkfToFrame_init.inverse()).log().norm() < 0.1)
numGoodNeighbours++;
else
numBadNeighbours++;
if(goodVal > bestNeightbourGoodVal)
{
bestNeightbourGoodVal = goodVal;
bestKF = nkf;
bestKFToFrame = nkfToFrame;
bestNeighbourUsage = tracker->pointUsage;
}
}
if(numGoodNeighbours > numBadNeighbours || numGoodNeighbours >= 5)
{
if(enablePrintDebugInfo && printRelocalizationInfo)
printf("RELOCALIZED! frame %d on %d (bestNeighbour %d): good %2.1f%%, usage %2.1f%%, GoodNeighbours %d / %d\n",
myRelocFrame->id(), todo->id(), bestKF->id(),
100*bestNeightbourGoodVal, 100*bestNeighbourUsage,
numGoodNeighbours, numGoodNeighbours+numBadNeighbours);
// set everything to stop!
continueRunning = false;
lock.lock();
resultRelocFrame = myRelocFrame;
resultFrameID = myRelocFrame->id();
resultKF = bestKF;
resultFrameToKeyframe = bestKFToFrame.inverse();
resultReadySignal.notify_all();
hasResult = true;
lock.unlock();
}
else
{
if(enablePrintDebugInfo && printRelocalizationInfo)
printf("FAILED RELOCALIZE! frame %d on %d (bestNeighbour %d): good %2.1f%%, usage %2.1f%%, GoodNeighbours %d / %d\n",
myRelocFrame->id(), todo->id(), bestKF->id(),
100*bestNeightbourGoodVal, 100*bestNeighbourUsage,
numGoodNeighbours, numGoodNeighbours+numBadNeighbours);
}
}
lock.lock();
}
else
{
newCurrentFrameSignal.wait(lock);
}
}
delete tracker;
}
