bool ReadFile( int file, void *buf, uint32_t length, uint32_t *readAmt, void *reserved )
{
uint64_t start = getMS();
*readAmt = 0;
while( true )
{
int amount = read( file, buf, length );
if( amount < 0 )
{
if( errno == EAGAIN || errno == EWOULDBLOCK )
{
if( (getMS() - start) > WAIT_TIME )
{
return false;
}
/* Try again */
continue;
}
return false;
}
*readAmt = amount;
return true;
}
}
bool WriteFile( int file, const void * const buf, uint32_t length, uint32_t *writeAmt, void *reserved )
{
uint64_t start = getMS();
while( true )
{
int amount = write( file, buf, length );
if( amount < 0 )
{
if( errno == EAGAIN || errno == EWOULDBLOCK )
{
if( (getMS() - start) > 1000 )
{
return false;
}
/* Try again */
continue;
}
return false;
}
else
{
*writeAmt = amount;
return true;
}
}
}
void PTAMWrapper::newImage(sensor_msgs::ImageConstPtr img)
{
// copy to internal image, convert to bw, set flag.
if(ros::Time::now() - img->header.stamp > ros::Duration(30.0))
mimFrameTime = getMS(ros::Time::now()-ros::Duration(0.001));
else
mimFrameTime = getMS(img->header.stamp);
// convert to CVImage
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
//mimFrameTime = getMS(img->header.stamp);
mimFrameSEQ = img->header.seq;
// copy to mimFrame.
// TODO: make this threadsafe (save pointer only and copy in HandleFrame)
if(mimFrameBW.size().x != img->width || mimFrameBW.size().y != img->height)
mimFrameBW.resize(CVD::ImageRef(img->width, img->height));
memcpy(mimFrameBW.data(),cv_ptr->image.data,img->width * img->height);
newImageAvailable = true;
}
TooN::Vector<3> PTAMWrapper::evalNavQue(unsigned int from, unsigned int to, bool* zCorrupted, bool* allCorrupted)
{
predIMUOnlyForScale->resetPos();
int firstAdded = 0, lastAdded = 0;
pthread_mutex_lock(&navInfoQueueCS);
int totSize = navInfoQueue.size();
int skipped=0;
int used = 0;
int firstZ = 0;
while(navInfoQueue.size() > 0)
{
if(getMS(navInfoQueue.front().header.stamp) < from) // packages before: delete
{
navInfoQueue.pop_front();
skipped++;
}
else if(getMS(navInfoQueue.front().header.stamp) >= from && getMS(navInfoQueue.front().header.stamp) <= to)
{
if(firstAdded == 0)
{
firstAdded = getMS(navInfoQueue.front().header.stamp);
firstZ = navInfoQueue.front().altd;
predIMUOnlyForScale->z = firstZ*0.001; // avoid height check initially!
}
lastAdded = getMS(navInfoQueue.front().header.stamp);
// add
predIMUOnlyForScale->predictOneStep(&(navInfoQueue.front()));
// pop
navInfoQueue.pop_front();
used++;
}
else
{
break;
}
}
//printf("QueEval: before: %i; skipped: %i, used: %i, left: %i\n", totSize, skipped, used, navInfoQueue.size());
predIMUOnlyForScale->z -= firstZ*0.001; // make height to height-diff
*zCorrupted = predIMUOnlyForScale->zCorrupted;
*allCorrupted = abs(firstAdded - (int)from) + abs(lastAdded - (int)to) > 80;
pthread_mutex_unlock(&navInfoQueueCS);
if(*allCorrupted)
printf("scalePackage corrupted (imu data gap for %ims)\n",abs(firstAdded - (int)from) + abs(lastAdded - (int)to));
else if(*zCorrupted)
printf("scalePackage z corrupted (jump in meters: %.3f)!\n",predIMUOnlyForScale->zCorruptedJump);
return TooN::makeVector(predIMUOnlyForScale->x,predIMUOnlyForScale->y,predIMUOnlyForScale->z);
}
/// Sleeps for the indicated amount of milliseconds or longer.
/// Will not sleep if ms is negative.
/// Will not sleep for longer than 10 minutes (600000ms).
/// If interrupted by signal, resumes sleep until at least ms milliseconds have passed.
/// Can be slightly off (in positive direction only) depending on OS accuracy.
void Util::wait(int64_t ms){
if (ms < 0) {
return;
}
if (ms > 600000) {
ms = 600000;
}
uint64_t start = getMS();
uint64_t now = start;
while (now < start+ms){
sleep(start+ms-now);
now = getMS();
}
}
//--------------------------------- StrmSetup ----------------------------------
ULONG StrmSetup(DDCMDSETUP* packet)
{
ULONG rc=-1;
struct stream_instance* psi=Strm_Find_SI(0,packet->hStream);
if(psi)
{
struct open_instance* poi=Strm_Find_OI(psi->ulSysFileNum);
if(poi)
{
SETUP_PARM* psu;
DevVirtToLin(SELECTOROF(packet->pSetupParm)
,OFFSETOF(packet->pSetupParm)
,(LINEAR*)&psu);
cur_streams[psi->devNo-1]=psi; // Make this the current stream instance
psi->ulSHTime=psu->ulStreamTime;
psi->ulTime=psu->ulStreamTime;
{
ULONG ms=getMS();
psi->ulSTime_Base=ms;
psi->ulTime_Delta=ms;
}
psu->ulFlags=1;
rc=0;
}
}
return rc;
}
Timer::Timer(unsigned int ms, ActionBase *action)
{
last = getMS();
next = last + ms; // overflow allowed
this->action = action;
singleUse = false;
}
void PTAMWrapper::newNavdata(ardrone_autonomy::Navdata* nav)
{
lastNavinfoReceived = *nav;
if(getMS(lastNavinfoReceived.header.stamp) > 2000000)
{
printf("PTAMSystem: ignoring navdata package with timestamp %i\n", lastNavinfoReceived.tm);
return;
}
if(lastNavinfoReceived.header.seq > 2000000 || lastNavinfoReceived.header.seq < 0)
{
printf("PTAMSystem: ignoring navdata package with ID %i\n", lastNavinfoReceived.header.seq);
return;
}
// correct yaw with filter-yaw (!):
lastNavinfoReceived.rotZ = filter->getCurrentPose()[5];
pthread_mutex_lock( &navInfoQueueCS );
navInfoQueue.push_back(lastNavinfoReceived);
if(navInfoQueue.size() > 1000) // respective 5s
{
navInfoQueue.pop_front();
if(!navQueueOverflown)
printf("NavQue Overflow detected!\n");
navQueueOverflown = true;
}
pthread_mutex_unlock( &navInfoQueueCS );
//filter->setPing(nav->pingNav, nav->pingVid);
imuOnlyPred->yaw = filter->getCurrentPose()[5];
imuOnlyPred->predictOneStep(&lastNavinfoReceived);
}
int Dynacoe::Clock::GetTimeSince() {
if (!paused) {
long currentTime = getMS();
return currentTime - startTime - (timePaused);
} else {
return pauseStart - startTime - timePaused;
}
}
void Dynacoe::Clock::Set(int msToExpire) {
startTime = getMS();
endTime = startTime + msToExpire;
expired = false;
paused = false;
timePaused= 0;
lastDuration = msToExpire;
}
/**
* @brief [brief description]
* @details [long description]
*/
void blink(){
unsigned long long ms_curr = getMS();
if (ms_curr - ms_since > 100) {
digitalWrite(3,!digitalRead(3));
ms_since = ms_curr;
}
yield();
}
/**
* @brief [brief description]
* @details [long description]
*/
void blink_faster() {
unsigned long long ms_curr = getMS();
if (ms_curr - ms_since2 > 1000) {
digitalWrite(2,!digitalRead(2));
ms_since2 = ms_curr;
}
yield();
}
uint32_t getMSSince(uint32_t startTime) {
uint32_t currentTime = getMS();
if(currentTime < startTime) {
return (uint64_t)currentTime + (uint64_t)PERIOD_IN_MS - (uint64_t)startTime;
} else {
return currentTime - startTime;
}
}
void Dynacoe::Clock::checkTime() {
if (paused) return;
if (endTime < startTime) {
expired = false;
return;
}
long currentTime = getMS();
if (currentTime - timePaused >= endTime && !expired) {
expired = true;
EmitEvent("clock-expire");
}
}
int main(int argc, char* argv[])
{
init_genrand(getMS());
COUTHANDLE hOUT = ConsoleInit();
g.pTaskMan = new TaskMan();
g.keys = 0;
g.pTaskMan->AddTask( new taskStage() );
ConsoleClear(hOUT);
unsigned int dwNextTime = 0;
while( (g.keys & KEY_ESCAPE) == false ) {
g.onUpdate();
unsigned int dwTime;
while( (dwTime=getMS()) < dwNextTime )
mSleep(1);
dwNextTime = dwTime + 16;
g.pTaskMan->onUpdate(hOUT);
}
delete g.pTaskMan;
return 0;
}
//------------------------------- StrmCtrlStart --------------------------------
ULONG StrmCtrlStart(DDCMDCONTROL* packet,stream_instance* psi)
{
ULONG rc=-1;
if(!psi->ucIsStreaming)
{
ULONG t_EAX=-1;
ULONG t_EBX=-1;
psi->ulFrames=0;
psi->ulSkipped=0;
psi->ulMissing=0;
psi->ulOverrun=0;
psi->ulUnderrun=0;
psi->tic_next=getMS();
psi->ulSTime_Base=psi->tic_next;
if(0==psi->ucT_Paused)
psi->ulTime_Delta=psi->ulSHTime;
psi->ulSTime=psi->tic_next;
if(0==psi->ulFPSFlags)
{
if(psi->ulFPS)
{
t_EBX=psi->ulFPS;
psi->ulDivisor=t_EBX;
t_EAX=1000;
}
}
else
{
t_EBX=1000;
psi->ulDivisor=t_EBX;
t_EAX=psi->ulFPS;
}
if(t_EBX>0 && -1!=t_EBX)
{
ULONG q=t_EAX/t_EBX;
ULONG r=t_EAX%t_EBX;
psi->ulTic_Delta=q;
psi->ulCur_Remain=0;
psi->ulOrg_Remain=r;
psi->tic_next+=psi->ulTic_Delta;
StrmStartStreaming(psi);
rc=0;
}
}
else
rc=StrmCtrlStop(packet,psi);
return rc;
}
void conInit() {
memset(con, 0, sizeof(con));
memset(conCommand, 0, sizeof(conCommand));
conCommandPos = 0;
conBlinkTime = getMS();
conBlinkOn = 1;
memset(conTypedHistory, 0, sizeof(conTypedHistory));
conTypedHistoryPos = 0;
conTypedHistoryPointer = 0;
conCompPos = 0;
conCompWord[0] = 0;
conCompWordsFoundCount = 0;
memset(conCompWordsFoundPtrs, 0, sizeof(conCompWordsFoundPtrs));
conCompWordsFoundIndex = 0;
}
void runVideo() {
#ifndef NO_GUI
Uint32 ts;
// runVideo might be called before SDL starts, say from a startup script
if (!video.sdlStarted)
return;
ts = getMS();
// record mode: if we are still faster than 30 FPS, skip video update (only if frameskip is set)
if ((view.frameSkip != 0) && (state.mode & SM_RECORD) && (view.dirty < 1) && (view.minVideoRefreshTime == 0)) {
if ((view.lastRenderTime + ts - view.lastVideoFrame) < RECORD_MIN_REDRAW_TIME) {
view.lastVideoFrameSkip++;
return;
}
}
// smooth zoom
smoothChange(view.zoomTarget, 8.0f, 0.01f, &(view.zoom), &(view.zoomSpeed));
// smooth rotate
smoothChange(view.rotTarget[0], 10.0f, 0.1f, &(view.rot[0]), &(view.rotSpeed[0]));
smoothChange(view.rotTarget[1], 10.0f, 0.1f, &(view.rot[1]), &(view.rotSpeed[1]));
smoothChange(view.rotTarget[2], 10.0f, 0.1f, &(view.rot[2]), &(view.rotSpeed[2]));
drawAll();
view.lastVideoFrameSkip=0;
view.deltaVideoFrame = ts - view.lastVideoFrame;
view.lastVideoFrame = ts;
fpsUpdate((float)view.deltaVideoFrame);
// fix freak-out values (to avoid "wild" zoom & rotate)
if (view.deltaVideoFrame < 1) view.deltaVideoFrame = 1; // div by zero and negative values
if (view.deltaVideoFrame > 70) view.deltaVideoFrame = 70; // 15FPS
#endif
}
//---------------------------- StrmCtrlEnableEvent -----------------------------
ULONG StrmCtrlEnableEvent(DDCMDCONTROL* packet,stream_instance* psi)
{
ULONG f=DevPushfCli();
ULONG rc=-1;
struct stream_instance_event* pev=Strm_Find_EV(psi,packet->hEvent);
if(!pev)
{
pev=Strm_Add_EV(psi,packet->hEvent);
if(pev)
{
CONTROL_PARM* pctl;
DevVirtToLin(SELECTOROF(packet->pParm),OFFSETOF(packet->pParm),(LINEAR _far*)&pctl);
pev->Time=pctl->ulTime;
pev->NTime=psi->ulTime+getMS();
rc=0;
}
}
else
rc=-2; // Event already in queue
DevPopf(f);
return rc;
}
void Dynacoe::Clock::Resume() {
if (paused) {
timePaused += (getMS() - pauseStart);
paused = false;
}
}
// called every time a new frame is available.
// needs to be able to
// - (finally) roll forward filter
// - query it's state
// - add a PTAM observation to filter.
void PTAMWrapper::HandleFrame()
{
// prep data
msg = "";
ros::Time startedFunc = ros::Time::now();
// reset?
if(resetPTAMRequested)
ResetInternal();
// make filter thread-safe.
// --------------------------- ROLL FORWARD TIL FRAME. This is ONLY done here. ---------------------------
pthread_mutex_lock( &filter->filter_CS );
//filter->predictUpTo(mimFrameTime,true, true);
TooN::Vector<10> filterPosePrePTAM = filter->getPoseAtAsVec(mimFrameTime-filter->delayVideo,true);
pthread_mutex_unlock( &filter->filter_CS );
// ------------------------ do PTAM -------------------------
myGLWindow->SetupViewport();
myGLWindow->SetupVideoOrtho();
myGLWindow->SetupVideoRasterPosAndZoom();
// 1. transform with filter
TooN::Vector<6> PTAMPoseGuess = filter->backTransformPTAMObservation(filterPosePrePTAM.slice<0,6>());
// 2. convert to se3
predConvert->setPosRPY(PTAMPoseGuess[0], PTAMPoseGuess[1], PTAMPoseGuess[2], PTAMPoseGuess[3], PTAMPoseGuess[4], PTAMPoseGuess[5]);
// 3. multiply with rotation matrix
TooN::SE3<> PTAMPoseGuessSE3 = predConvert->droneToFrontNT * predConvert->globaltoDrone;
// set
mpTracker->setPredictedCamFromW(PTAMPoseGuessSE3);
//mpTracker->setLastFrameLost((isGoodCount < -10), (videoFrameID%2 != 0));
mpTracker->setLastFrameLost((isGoodCount < -20), (mimFrameSEQ%3 == 0));
// track
ros::Time startedPTAM = ros::Time::now();
mpTracker->TrackFrame(mimFrameBW, true);
TooN::SE3<> PTAMResultSE3 = mpTracker->GetCurrentPose();
lastPTAMMessage = msg = mpTracker->GetMessageForUser();
ros::Duration timePTAM= ros::Time::now() - startedPTAM;
// 1. multiply from left by frontToDroneNT.
// 2. convert to xyz,rpy
predConvert->setPosSE3_globalToDrone(predConvert->frontToDroneNT * PTAMResultSE3);
TooN::Vector<6> PTAMResult = TooN::makeVector(predConvert->x, predConvert->y, predConvert->z, predConvert->roll, predConvert->pitch, predConvert->yaw);
// 3. transform with filter
TooN::Vector<6> PTAMResultTransformed = filter->transformPTAMObservation(PTAMResult);
// init failed?
if(mpTracker->lastStepResult == mpTracker->I_FAILED)
{
ROS_INFO("initializing PTAM failed, resetting!");
resetPTAMRequested = true;
}
if(mpTracker->lastStepResult == mpTracker->I_SECOND)
{
PTAMInitializedClock = getMS();
filter->setCurrentScales(TooN::makeVector(mpMapMaker->initialScaleFactor*1.5,mpMapMaker->initialScaleFactor*1.5,mpMapMaker->initialScaleFactor*1.5));
mpMapMaker->currentScaleFactor = filter->getCurrentScales()[0];
ROS_INFO("PTAM initialized!");
ROS_INFO("initial scale: %f\n",mpMapMaker->initialScaleFactor*1.5);
node->publishCommand("u l PTAM initialized (took second KF)");
framesIncludedForScaleXYZ = -1;
lockNextFrame = true;
imuOnlyPred->resetPos();
}
if(mpTracker->lastStepResult == mpTracker->I_FIRST)
{
node->publishCommand("u l PTAM initialization started (took first KF)");
}
// --------------------------- assess result ------------------------------
bool isGood = true;
bool isVeryGood = true;
// calculate absolute differences.
TooN::Vector<6> diffs = PTAMResultTransformed - filterPosePrePTAM.slice<0,6>();
for(int i=0;1<1;i++) diffs[i] = abs(diffs[i]);
if(filter->getNumGoodPTAMObservations() < 10 && mpMap->IsGood())
{
isGood = true;
isVeryGood = false;
}
else if(mpTracker->lastStepResult == mpTracker->I_FIRST ||
mpTracker->lastStepResult == mpTracker->I_SECOND ||
mpTracker->lastStepResult == mpTracker->I_FAILED ||
mpTracker->lastStepResult == mpTracker->T_LOST ||
mpTracker->lastStepResult == mpTracker->NOT_TRACKING ||
mpTracker->lastStepResult == mpTracker->INITIALIZING)
isGood = isVeryGood = false;
else
{
// some chewy heuristic when to add and when not to.
bool dodgy = mpTracker->lastStepResult == mpTracker->T_DODGY ||
mpTracker->lastStepResult == mpTracker->T_RECOVERED_DODGY;
// if yaw difference too big: something certainly is wrong.
// maximum difference is 5 + 2*(number of seconds since PTAM observation).
double maxYawDiff = 10.0 + (getMS()-lastGoodYawClock)*0.002;
if(maxYawDiff > 20) maxYawDiff = 1000;
if(false && diffs[5] > maxYawDiff)
isGood = false;
if(diffs[5] < 10)
lastGoodYawClock = getMS();
if(diffs[5] > 4.0)
isVeryGood = false;
// if rp difference too big: something certainly is wrong.
if(diffs[3] > 20 || diffs[4] > 20)
isGood = false;
if(diffs[3] > 3 || diffs[4] > 3 || dodgy)
isVeryGood = false;
}
if(isGood)
{
if(isGoodCount < 0) isGoodCount = 0;
isGoodCount++;
}
else
{
if(isGoodCount > 0) isGoodCount = 0;
isGoodCount--;
if(mpTracker->lastStepResult == mpTracker->T_RECOVERED_DODGY)
isGoodCount = std::max(isGoodCount,-2);
if(mpTracker->lastStepResult == mpTracker->T_RECOVERED_GOOD)
isGoodCount = std::max(isGoodCount,-5);
}
TooN::Vector<10> filterPosePostPTAM;
// --------------------------- scale estimation & update filter (REDONE) -----------------------------
// interval length is always between 1s and 2s, to enshure approx. same variances.
// if interval contained height inconsistency, z-distances are simply both set to zero, which does not generate a bias.
// otherwise distances are scaled such that height is weighted more.
// if isGood>=3 && framesIncludedForScale < 0 ===> START INTERVAL
// if 18 <= framesIncludedForScale <= 36 AND isGood>=3 ===> ADD INTERVAL, START INTERVAL
// if framesIncludedForScale > 36 ===> set framesIncludedForScale=-1
// include!
// TODO: make shure filter is handled properly with permanent roll-forwards.
pthread_mutex_lock( &filter->filter_CS );
if(filter->usePTAM && isGoodCount >= 3)
{
filter->addPTAMObservation(PTAMResult,mimFrameTime-filter->delayVideo);
}
else
filter->addFakePTAMObservation(mimFrameTime-filter->delayVideo);
filterPosePostPTAM = filter->getCurrentPoseSpeedAsVec();
pthread_mutex_unlock( &filter->filter_CS );
TooN::Vector<6> filterPosePostPTAMBackTransformed = filter->backTransformPTAMObservation(filterPosePostPTAM.slice<0,6>());
// if interval is started: add one step.
int includedTime = mimFrameTime - ptamPositionForScaleTakenTimestamp;
if(framesIncludedForScaleXYZ >= 0) framesIncludedForScaleXYZ++;
// if interval is overdue: reset & dont add
if(includedTime > 3000)
{
framesIncludedForScaleXYZ = -1;
}
if(isGoodCount >= 3)
{
// filter stuff
lastScaleEKFtimestamp = mimFrameTime;
if(includedTime >= 2000 && framesIncludedForScaleXYZ > 1) // ADD! (if too many, was resetted before...)
{
TooN::Vector<3> diffPTAM = filterPosePostPTAMBackTransformed.slice<0,3>() - PTAMPositionForScale;
bool zCorrupted, allCorrupted;
TooN::Vector<3> diffIMU = evalNavQue(ptamPositionForScaleTakenTimestamp - filter->delayVideo + filter->delayXYZ,mimFrameTime - filter->delayVideo + filter->delayXYZ,&zCorrupted, &allCorrupted);
if(!allCorrupted)
{
// filtering: z more weight, but only if not corrupted.
double xyFactor = 0.25;
double zFactor = zCorrupted ? 0 : 3;
diffPTAM.slice<0,2>() *= xyFactor; diffPTAM[2] *= zFactor;
diffIMU.slice<0,2>() *= xyFactor; diffIMU[2] *= zFactor;
filter->updateScaleXYZ(diffPTAM, diffIMU, PTAMResult.slice<0,3>());
mpMapMaker->currentScaleFactor = filter->getCurrentScales()[0];
}
framesIncludedForScaleXYZ = -1; // causing reset afterwards
}
if(framesIncludedForScaleXYZ == -1) // RESET!
{
framesIncludedForScaleXYZ = 0;
PTAMPositionForScale = filterPosePostPTAMBackTransformed.slice<0,3>();
//predIMUOnlyForScale->resetPos(); // also resetting z corrupted flag etc. (NOT REquired as reset is done in eval)
ptamPositionForScaleTakenTimestamp = mimFrameTime;
}
}
if(lockNextFrame && isGood)
{
filter->scalingFixpoint = PTAMResult.slice<0,3>();
lockNextFrame = false;
//filter->useScalingFixpoint = true;
snprintf(charBuf,500,"locking scale fixpoint to %.3f %.3f %.3f",PTAMResultTransformed[0], PTAMResultTransformed[1], PTAMResultTransformed[2]);
ROS_INFO(charBuf);
node->publishCommand(std::string("u l ")+charBuf);
}
// ----------------------------- Take KF? -----------------------------------
if(!mapLocked && isVeryGood && (forceKF || mpMap->vpKeyFrames.size() < maxKF || maxKF <= 1))
{
mpTracker->TakeKF(forceKF);
forceKF = false;
}
// ---------------- save PTAM status for KI --------------------------------
if(mpTracker->lastStepResult == mpTracker->NOT_TRACKING)
PTAMStatus = PTAM_IDLE;
else if(mpTracker->lastStepResult == mpTracker->I_FIRST ||
mpTracker->lastStepResult == mpTracker->I_SECOND ||
mpTracker->lastStepResult == mpTracker->T_TOOK_KF)
PTAMStatus = PTAM_TOOKKF;
else if(mpTracker->lastStepResult == mpTracker->INITIALIZING)
PTAMStatus = PTAM_INITIALIZING;
else if(isVeryGood)
PTAMStatus = PTAM_BEST;
else if(isGood)
PTAMStatus = PTAM_GOOD;
else if(mpTracker->lastStepResult == mpTracker->T_DODGY ||
mpTracker->lastStepResult == mpTracker->T_GOOD)
PTAMStatus = PTAM_FALSEPOSITIVE;
else
PTAMStatus = PTAM_LOST;
/*
// ------------------------- LED anim --------------------------------
int sinceLastAnim = clock() - lastAnimSentClock;
if(sinceLastAnim > 19000) lastAnimSent = ANIM_NONE;
if(mpTracker->lastStepResult == mpTracker->T_TOOK_KF && (lastAnimSent != ANIM_TOOKKF || sinceLastAnim > 200)) // if kf taken: send respective anim, but only if no kf was sent within 200ms.
{
lastAnimSent = ANIM_TOOKKF;
lastAnimSentClock = clock();
l->writePipeUI("FORWARD LED blink_orange 10 1\n");
}
else if(sinceLastAnim > 500)
{
bool sent = true;
if((PTAMStatus == PTAM_BEST || PTAMStatus == PTAM_GOOD) && lastAnimSent != ANIM_GOOD)
{
l->writePipeUI("FORWARD LED green 5 20\n");
lastAnimSentClock = clock();
lastAnimSent = ANIM_GOOD;
}
if(PTAMStatus == PTAM_INITIALIZING && lastAnimSent != ANIM_INIT)
{
l->writePipeUI("FORWARD LED blink_green 5 20\n");
lastAnimSentClock = clock();
lastAnimSent = ANIM_INIT;
}
if(PTAMStatus == PTAM_LOST && lastAnimSent != ANIM_LOST)
{
l->writePipeUI("FORWARD LED red 5 20\n");
lastAnimSentClock = clock();
lastAnimSent = ANIM_LOST;
}
if(PTAMStatus == PTAM_FALSEPOSITIVE && lastAnimSent != ANIM_FALSEPOS)
{
l->writePipeUI("FORWARD LED blink_red 5 20\n");
lastAnimSentClock = clock();
lastAnimSent = ANIM_FALSEPOS;
}
}
*/
// ----------------------------- update shallow map --------------------------
if(!mapLocked)
{
pthread_mutex_lock(&shallowMapCS);
mapPointsTransformed.clear();
keyFramesTransformed.clear();
for(unsigned int i=0;i<mpMap->vpKeyFrames.size();i++)
{
predConvert->setPosSE3_globalToDrone(predConvert->frontToDroneNT * mpMap->vpKeyFrames[i]->se3CfromW);
TooN::Vector<6> CamPos = TooN::makeVector(predConvert->x, predConvert->y, predConvert->z, predConvert->roll, predConvert->pitch, predConvert->yaw);
CamPos = filter->transformPTAMObservation(CamPos);
predConvert->setPosRPY(CamPos[0], CamPos[1], CamPos[2], CamPos[3], CamPos[4], CamPos[5]);
keyFramesTransformed.push_back(predConvert->droneToGlobal);
}
TooN::Vector<3> PTAMScales = filter->getCurrentScales();
TooN::Vector<3> PTAMOffsets = filter->getCurrentOffsets().slice<0,3>();
for(unsigned int i=0;i<mpMap->vpPoints.size();i++)
{
TooN::Vector<3> pos = (mpMap->vpPoints)[i]->v3WorldPos;
pos[0] *= PTAMScales[0];
pos[1] *= PTAMScales[1];
pos[2] *= PTAMScales[2];
pos += PTAMOffsets;
mapPointsTransformed.push_back(pos);
}
pthread_mutex_unlock(&shallowMapCS);
}
// ---------------------- output and render! ---------------------------
ros::Duration timeALL = ros::Time::now() - startedFunc;
if(isVeryGood) snprintf(charBuf,1000,"\nQuality: best ");
else if(isGood) snprintf(charBuf,1000,"\nQuality: good ");
else snprintf(charBuf,1000,"\nQuality: lost ");
snprintf(charBuf+20,800, "scale: %.3f (acc: %.3f) ",filter->getCurrentScales()[0],(double)filter->getScaleAccuracy());
snprintf(charBuf+50,800, "PTAM time: %i ms ",(int)(1000*timeALL.toSec()));
snprintf(charBuf+68,800, "(%i ms total) ",(int)(1000*timeALL.toSec()));
if(mapLocked) snprintf(charBuf+83,800, "m.l. ");
else snprintf(charBuf+83,800, " ");
if(filter->allSyncLocked) snprintf(charBuf+88,800, "s.l. ");
else snprintf(charBuf+88,800, " ");
msg += charBuf;
if(mpMap->IsGood())
{
if(drawUI == UI_DEBUG)
{
snprintf(charBuf,1000,"\nPTAM Diffs: ");
snprintf(charBuf+13,800, "x: %.3f ",diffs[0]);
snprintf(charBuf+23,800, "y: %.3f ",diffs[1]);
snprintf(charBuf+33,800, "z: %.3f ",diffs[2]);
snprintf(charBuf+43,800, "r: %.2f ",diffs[3]);
snprintf(charBuf+53,800, "p: %.2f ",diffs[4]);
snprintf(charBuf+63,800, "y: %.2f",diffs[5]);
msg += charBuf;
snprintf(charBuf,1000,"\nPTAM Pose: ");
snprintf(charBuf+13,800, "x: %.3f ",PTAMResultTransformed[0]);
snprintf(charBuf+23,800, "y: %.3f ",PTAMResultTransformed[1]);
snprintf(charBuf+33,800, "z: %.3f ",PTAMResultTransformed[2]);
snprintf(charBuf+43,800, "r: %.2f ",PTAMResultTransformed[3]);
snprintf(charBuf+53,800, "p: %.2f ",PTAMResultTransformed[4]);
snprintf(charBuf+63,800, "y: %.2f",PTAMResultTransformed[5]);
msg += charBuf;
snprintf(charBuf,1000,"\nPTAM WiggleDist: ");
snprintf(charBuf+18,800, "%.3f ",mpMapMaker->lastWiggleDist);
snprintf(charBuf+24,800, "MetricDist: %.3f",mpMapMaker->lastMetricDist);
msg += charBuf;
}
}
if(drawUI != UI_NONE)
{
// render grid
predConvert->setPosRPY(filterPosePostPTAM[0], filterPosePostPTAM[1], filterPosePostPTAM[2], filterPosePostPTAM[3], filterPosePostPTAM[4], filterPosePostPTAM[5]);
//renderGrid(predConvert->droneToFrontNT * predConvert->globaltoDrone);
//renderGrid(PTAMResultSE3);
// draw HUD
//if(mod->getControlSystem()->isControlling())
{
myGLWindow->SetupViewport();
myGLWindow->SetupVideoOrtho();
myGLWindow->SetupVideoRasterPosAndZoom();
//glDisable(GL_LINE_SMOOTH);
glLineWidth(2);
glBegin(GL_LINES);
glColor3f(0,0,1);
glVertex2f(0,frameHeight/2);
glVertex2f(frameWidth,frameHeight/2);
glVertex2f(frameWidth/2,0);
glVertex2f(frameWidth/2,frameHeight);
// 1m lines
glVertex2f(0.25*frameWidth,0.48*frameHeight);
glVertex2f(0.25*frameWidth,0.52*frameHeight);
glVertex2f(0.75*frameWidth,0.48*frameHeight);
glVertex2f(0.75*frameWidth,0.52*frameHeight);
glVertex2f(0.48*frameWidth,0.25*frameHeight);
glVertex2f(0.52*frameWidth,0.25*frameHeight);
glVertex2f(0.48*frameWidth,0.75*frameHeight);
glVertex2f(0.52*frameWidth,0.75*frameHeight);
glEnd();
}
myGLWindow->DrawCaption(msg);
}
lastPTAMResultRaw = PTAMResultSE3;
// ------------------------ LOG --------------------------------------
// log!
if(node->logfilePTAM != NULL)
{
TooN::Vector<3> scales = filter->getCurrentScalesForLog();
TooN::Vector<3> sums = TooN::makeVector(0,0,0);
TooN::Vector<6> offsets = filter->getCurrentOffsets();
pthread_mutex_lock(&(node->logPTAM_CS));
// log:
// - filterPosePrePTAM estimated for videoFrameTimestamp-delayVideo.
// - PTAMResulttransformed estimated for videoFrameTimestamp-delayVideo. (using imu only for last step)
// - predictedPoseSpeed estimated for lastNfoTimestamp+filter->delayControl (actually predicting)
// - predictedPoseSpeedATLASTNFO estimated for lastNfoTimestamp (using imu only)
if(node->logfilePTAM != NULL)
(*(node->logfilePTAM)) << (isGood ? (isVeryGood ? 2 : 1) : 0) << " " <<
(mimFrameTime-filter->delayVideo) << " " << filterPosePrePTAM[0] << " " << filterPosePrePTAM[1] << " " << filterPosePrePTAM[2] << " " << filterPosePrePTAM[3] << " " << filterPosePrePTAM[4] << " " << filterPosePrePTAM[5] << " " << filterPosePrePTAM[6] << " " << filterPosePrePTAM[7] << " " << filterPosePrePTAM[8] << " " << filterPosePrePTAM[9] << " " <<
filterPosePostPTAM[0] << " " << filterPosePostPTAM[1] << " " << filterPosePostPTAM[2] << " " << filterPosePostPTAM[3] << " " << filterPosePostPTAM[4] << " " << filterPosePostPTAM[5] << " " << filterPosePostPTAM[6] << " " << filterPosePostPTAM[7] << " " << filterPosePostPTAM[8] << " " << filterPosePostPTAM[9] << " " <<
PTAMResultTransformed[0] << " " << PTAMResultTransformed[1] << " " << PTAMResultTransformed[2] << " " << PTAMResultTransformed[3] << " " << PTAMResultTransformed[4] << " " << PTAMResultTransformed[5] << " " <<
scales[0] << " " << scales[1] << " " << scales[2] << " " <<
offsets[0] << " " << offsets[1] << " " << offsets[2] << " " << offsets[3] << " " << offsets[4] << " " << offsets[5] << " " <<
sums[0] << " " << sums[1] << " " << sums[2] << " " <<
videoFramePing << std::endl;
pthread_mutex_unlock(&(node->logPTAM_CS));
}
myGLWindow->swap_buffers();
myGLWindow->HandlePendingEvents();
}
void MapView::Render()
{
// get new pose.
pthread_mutex_lock(&filter->filter_CS);
lastFramePoseSpeed = filter->getCurrentPoseSpeedAsVec(); // Note: this is maybe an old pose, but max. one frame old = 50ms = not noticable.
pthread_mutex_unlock(&filter->filter_CS);
if(clearTrail)
{
trailPoints.clear();
clearTrail = false;
}
// render
bool addTrail;
if(trailPoints.size() == 0)
addTrail = true;
else
{
TooN::Vector<3> distToLast = lastFramePoseSpeed.slice<0,3>() - trailPoints[trailPoints.size()-1].pointFilter;
double d = distToLast[0]*distToLast[0] + distToLast[1]*distToLast[1]+distToLast[2]*distToLast[2];
addTrail = d > 0.1*0.1;
}
// the following complicated code is to save trail-points in ptam-scale, such that scale-reestimation will re-scale the drawn path.
if(addTrail)
{
if(ptamWrapper->PTAMStatus == ptamWrapper->PTAM_BEST ||
ptamWrapper->PTAMStatus == ptamWrapper->PTAM_TOOKKF ||
ptamWrapper->PTAMStatus == ptamWrapper->PTAM_GOOD)
{
if(ptamWrapper->PTAMInitializedClock != 0 && getMS() - ptamWrapper->PTAMInitializedClock > 200)
{
TooN::Vector<3> PTAMScales = filter->getCurrentScales();
TooN::Vector<3> PTAMOffsets = filter->getCurrentOffsets().slice<0,3>();
TooN::Vector<3> ptamPointPos = lastFramePoseSpeed.slice<0,3>();
ptamPointPos -= PTAMOffsets;
ptamPointPos /= PTAMScales[0];
trailPoints.push_back(TrailPoint(
lastFramePoseSpeed.slice<0,3>(),
ptamPointPos
));
}
}
else if(ptamWrapper->PTAMStatus == ptamWrapper->PTAM_LOST ||
ptamWrapper->PTAMStatus == ptamWrapper->PTAM_FALSEPOSITIVE)
{
if(ptamWrapper->PTAMInitializedClock != 0 && getMS() - ptamWrapper->PTAMInitializedClock > 200)
{
TooN::Vector<3> PTAMScales = filter->getCurrentScales();
TooN::Vector<3> PTAMOffsets = filter->getCurrentOffsets().slice<0,3>();
TooN::Vector<3> ptamPointPos = lastFramePoseSpeed.slice<0,3>();
ptamPointPos -= PTAMOffsets;
ptamPointPos /= PTAMScales[0];
trailPoints.push_back(TrailPoint(
lastFramePoseSpeed.slice<0,3>(),
ptamPointPos
));
}
}
else
{
trailPoints.push_back(TrailPoint(
lastFramePoseSpeed.slice<0,3>()
));
}
}
if(resetMapViewFlag)
{
resetMapView();
resetMapViewFlag = false;
}
// get lineWidthFactor
lineWidthFactor = sqrt((float)(myGLWindow->size()[0] * myGLWindow->size()[1] / (640*480)));
plotGrid();
pthread_mutex_lock(&ptamWrapper->shallowMapCS);
std::vector<tse3>* kfl = &(ptamWrapper->keyFramesTransformed);
// draw keyframes
for(unsigned int i=0;i<kfl->size();i++)
{
plotCam((*kfl)[i],false,2,0.04f,1);
}
// draw trail
drawTrail();
// draw keypoints
plotMapPoints();
pthread_mutex_unlock(&ptamWrapper->shallowMapCS);
// draw predicted cam
// real in opaque
predConvert->setPosRPY(lastFramePoseSpeed[0], lastFramePoseSpeed[1], lastFramePoseSpeed[2], lastFramePoseSpeed[3], lastFramePoseSpeed[4], lastFramePoseSpeed[5]);
plotCam(predConvert->droneToGlobal,true,5.0f,0.2f,1);
// --------------------- make msg ------------------------------
msg = "";
TooN::Vector<6> of = filter->getCurrentOffsets();
TooN::Vector<3> sc = filter->getCurrentScales();
if(drawUI == UI_DEBUG)
{
snprintf(charBuf,1000,"Pose: ");
snprintf(charBuf+10,800, "x: %.2f ",lastFramePoseSpeed[0]);
snprintf(charBuf+20,800, "y: %.2f ",lastFramePoseSpeed[1]);
snprintf(charBuf+30,800, "z: %.2f ",lastFramePoseSpeed[2]);
snprintf(charBuf+40,800, "r: %.2f ",lastFramePoseSpeed[3]);
snprintf(charBuf+50,800, "p: %.2f ",lastFramePoseSpeed[4]);
snprintf(charBuf+60,800, "y: %.2f ",lastFramePoseSpeed[5]);
snprintf(charBuf+70,800, "vx: %.2f ",lastFramePoseSpeed[6]);
snprintf(charBuf+80,800, "vy: %.2f ",lastFramePoseSpeed[7]);
snprintf(charBuf+90,800, "vz: %.2f ",lastFramePoseSpeed[8]);
snprintf(charBuf+100,800, "vy: %.2f",lastFramePoseSpeed[9]);
msg += charBuf;
snprintf(charBuf,1000,"\nSync: ");
snprintf(charBuf+10,800, "ox: %.2f ",of[0]);
snprintf(charBuf+20,800, "oy: %.2f ",of[1]);
snprintf(charBuf+30,800, "oz: %.2f ",of[2]);
snprintf(charBuf+40,800, "or: %.2f ",of[3]);
snprintf(charBuf+50,800, "op: %.2f ",of[4]);
snprintf(charBuf+60,800, "oy: %.2f ",of[5]);
snprintf(charBuf+70,800, "Sx: %.2f ",sc[0]);
snprintf(charBuf+80,800, "Sy: %.2f ",sc[1]);
snprintf(charBuf+90,800, "Sz: %.2f",sc[2]);
msg += charBuf;
snprintf(charBuf,1000,"\nStDvs: ");
snprintf(charBuf+10,800, "x: %.2f ",std::sqrt((double)lastFramePoseSpeed[0]));
snprintf(charBuf+20,800, "y: %.2f ",std::sqrt((double)lastFramePoseSpeed[1]));
snprintf(charBuf+30,800, "z: %.2f ",std::sqrt((double)lastFramePoseSpeed[2]));
snprintf(charBuf+40,800, "r: %.2f ",std::sqrt((double)lastFramePoseSpeed[3]));
snprintf(charBuf+50,800, "p: %.2f ",std::sqrt((double)lastFramePoseSpeed[4]));
snprintf(charBuf+60,800, "y: %.2f ",std::sqrt((double)lastFramePoseSpeed[5]));
snprintf(charBuf+70,800, "vx: %.2f ",std::sqrt((double)lastFramePoseSpeed[6]));
snprintf(charBuf+80,800, "vy: %.2f ",std::sqrt((double)lastFramePoseSpeed[7]));
snprintf(charBuf+90,800, "vz: %.2f ",std::sqrt((double)lastFramePoseSpeed[8]));
snprintf(charBuf+100,800, "vy: %.2f",std::sqrt((double)lastFramePoseSpeed[9]));
msg += charBuf;
}
else
{
snprintf(charBuf,1000,"Drone Pose: ");
snprintf(charBuf+13,800, "xyz=(%.2f, ",lastFramePoseSpeed[0]);
snprintf(charBuf+25,800, "%.2f, ",lastFramePoseSpeed[1]);
snprintf(charBuf+32,800, "%.2f), ",lastFramePoseSpeed[2]);
snprintf(charBuf+42,800, "rpy=(%.2f, ",lastFramePoseSpeed[3]);
snprintf(charBuf+54,800, "%.2f, ",lastFramePoseSpeed[4]);
snprintf(charBuf+61,800, "%.2f) ",lastFramePoseSpeed[5]);
msg += charBuf;
}
myGLWindow->GetMousePoseUpdate();
CVD::glSetFont("sans");
if(drawUI != UI_NONE)
myGLWindow->DrawCaption(msg);
if(drawUI == UI_DEBUG)
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glTranslatef((float)0, (float)100, 0.0);
glScalef(45,-45,1);
snprintf(charBuf,1000,"xyz: %.2f %.2f %.2f",lastFramePoseSpeed[0],lastFramePoseSpeed[1],lastFramePoseSpeed[2]);
CVD::glDrawText(charBuf, CVD::NICE, 1.6, 0.1);
glPopMatrix();
}
myGLWindow->swap_buffers();
myGLWindow->HandlePendingEvents();
}
void Dynacoe::Clock::Pause() {
if (!paused) {
paused = true;
pauseStart = getMS();
}
}
bool Timer::isTime() const
{
unsigned int now = getMS();
return (last < now && next <= now) || // normal
(last > now && (next <= now || next > last)); // overflow
}
static void updatePos()
{
if (!moving)
return;
int ms = getMS();
int time = ms-lastMS;
if (time < 1)
return;
lastMS = ms;
// printf("updating pos time %d: %f vs %d\n",time,actualX,currentState.xPos);
char stillMoving = 0;
if (currentState.onOff == ON)
stillMoving = 1;
if (actualX != currentState.xPos)
{
if (actualX < currentState.xPos)
{
actualX += DIST_PER_MS * time;
if (actualX >= currentState.xPos)
actualX = currentState.xPos;
else
stillMoving = 1;
}
else
{
actualX -= DIST_PER_MS * time;
if (actualX <= currentState.xPos)
actualX = currentState.xPos;
else
stillMoving = 1;
}
}
if (actualY != currentState.yPos)
{
if (actualY < currentState.yPos)
{
actualY += DIST_PER_MS * time;
if (actualY >= currentState.yPos)
actualY = currentState.yPos;
else
stillMoving = 1;
}
else
{
actualY -= DIST_PER_MS * time;
if (actualY <= currentState.yPos)
actualY = currentState.yPos;
else
stillMoving = 1;
}
}
if (actualZ != currentState.zPos)
{
if (actualZ < currentState.zPos)
{
actualZ += DIST_PER_MS * time;
if (actualZ >= currentState.zPos)
actualZ = currentState.zPos;
else
stillMoving = 1;
}
else
{
actualZ -= DIST_PER_MS * time;
if (actualZ <= currentState.zPos)
actualZ = currentState.zPos;
else
stillMoving = 1;
}
}
// printf("Moved to %f,%f,%f\n",actualX,actualY,actualZ);
moving = stillMoving;
glutPostRedisplay();
}
TooN::Vector<3> PTAMWrapper::evalNavQue(unsigned int from, unsigned int to, bool* zCorrupted, bool* allCorrupted, float* out_start_pressure, float* out_end_pressure)
{
predIMUOnlyForScale->resetPos();
int firstAdded = 0, lastAdded = 0;
pthread_mutex_lock(&navInfoQueueCS);
int skipped=0;
int used = 0;
int firstZ = 0;
float sum_first=0, num_first=0, sum_last=0, num_last=0;
int pressureAverageRange = 100;
for(std::deque<ardrone_autonomy::Navdata>::iterator cur = navInfoQueue.begin();
cur != navInfoQueue.end();
)
{
int curStampMs = getMS(cur->header.stamp);
if(curStampMs < (int)from-pressureAverageRange)
cur = navInfoQueue.erase(cur);
else
{
if(curStampMs >= (int)from-pressureAverageRange && curStampMs <= (int)from+pressureAverageRange)
{
sum_first += cur->pressure;
num_first++;
}
if(curStampMs >= (int)to-pressureAverageRange && curStampMs <= (int)to+pressureAverageRange)
{
sum_last += cur->pressure;
num_last++;
}
cur++;
}
}
for(std::deque<ardrone_autonomy::Navdata>::iterator cur = navInfoQueue.begin();
cur != navInfoQueue.end();
cur++
)
{
int frontStamp = getMS(cur->header.stamp);
if(frontStamp < from) // packages before: delete
{
//navInfoQueue.pop_front();
skipped++;
}
else if(frontStamp >= from && frontStamp <= to)
{
if(firstAdded == 0)
{
firstAdded = frontStamp;
firstZ = cur->altd;
predIMUOnlyForScale->z = firstZ*0.001; // avoid height check initially!
}
lastAdded = frontStamp;
// add
predIMUOnlyForScale->predictOneStep(&(*cur));
// pop
//navInfoQueue.pop_front();
used++;
}
else
break;
}
//printf("QueEval: before: %i; skipped: %i, used: %i, left: %i\n", totSize, skipped, used, navInfoQueue.size());
predIMUOnlyForScale->z -= firstZ*0.001; // make height to height-diff
*zCorrupted = predIMUOnlyForScale->zCorrupted;
*allCorrupted = abs(firstAdded - (int)from) + abs(lastAdded - (int)to) > 80;
pthread_mutex_unlock(&navInfoQueueCS);
if(*allCorrupted)
printf("scalePackage corrupted (imu data gap for %ims)\n",abs(firstAdded - (int)from) + abs(lastAdded - (int)to));
else if(*zCorrupted)
printf("scalePackage z corrupted (jump in meters: %.3f)!\n",predIMUOnlyForScale->zCorruptedJump);
printf("first: %f (%f); last: %f (%f)=> diff: %f (z alt diff: %f)\n",
sum_first/num_first,
num_first,
sum_last/num_last,
num_last,
sum_last/num_last - sum_first/num_first,
predIMUOnlyForScale->z
);
*out_end_pressure = sum_last/num_last;
*out_start_pressure = sum_first/num_first;
return TooN::makeVector(predIMUOnlyForScale->x,predIMUOnlyForScale->y,predIMUOnlyForScale->z);
}
static jfloat
getTime(JNIEnv* env, jobject thiz, jlong memoryPointer)
{
Variables* inParam = (Variables*) memoryPointer;
return getMS(inParam->timer);
}
static void checkForChanges(void)
{
if (requestedState->shutdown)
exit(0);
if (requestedState->xPos != currentState.xPos)
{
if (requestedState->xPos < currentState.xPos)
currentState.xPos--;
else
currentState.xPos++;
//printf("Moving x to %d\n",currentState.xPos);
if (!moving)
{
moving = 1;
lastMS = getMS();
}
}
if (requestedState->yPos != currentState.yPos)
{
currentState.yPos = requestedState->yPos;
//printf("Moving y to %d\n",currentState.yPos);
if (!moving)
{
moving = 1;
lastMS = getMS();
}
}
if (requestedState->zPos != currentState.zPos)
{
if (requestedState->zPos < currentState.zPos)
currentState.zPos--;
else
currentState.zPos++;
//printf("Moving z to %d\n",currentState.zPos);
if (!moving)
{
moving = 1;
lastMS = getMS();
}
}
if (requestedState->temp != currentState.temp)
{
if (requestedState->temp < currentState.temp)
currentState.temp--;
else
currentState.temp++;
moving = 1;
printf("Moving temp to %d\n",currentState.temp);
}
if (requestedState->onOff != currentState.onOff)
{
currentState.onOff = requestedState->onOff;
if (currentState.onOff == ON)
printf("Turning water ON\n");
else
printf("Turning water OFF\n");
moving = 1;
}
if (requestedState->mode != currentState.mode)
{
currentState.mode = requestedState->mode;
moving = 1;
switch (currentState.mode)
{
case SHOWER:
printf("setting mode to SHOWER\n");
break;
case JET:
printf("setting mode to JET\n");
break;
case RAIN:
printf("setting mode to RAIN\n");
break;
case MASSAGE:
printf("setting mode to MASSAGE\n");
break;
}
}
}
if(filter->getNumGoodLSDObservations() < 10 && monoOdometry->IsGood())
{
isGood = true;
}
//If the last tracking step result is lost
else if(lsdTracker->lastStepResult == LOST)
isGood = false;
diverged = true;
else
{
// if yaw difference too big: something certainly is wrong.
// maximum difference is 5 + 2*(number of seconds since PTAM observation).
double maxYawDiff = 10.0 + (getMS()-lastGoodYawClock)*0.002;
if(maxYawDiff > 20) maxYawDiff = 1000;
if(false && diffs[5] > maxYawDiff)
isGood = false;
if(diffs[5] < 10)
lastGoodYawClock = getMS();
if(diffs[5] > 4.0)
isGood = false;
// if rp difference too big: something certainly is wrong.
if(diffs[3] > 20 || diffs[4] > 20)
isGood = false;
if(diffs[3] > 3 || diffs[4] > 3 || dodgy)
void run() {
view.firstTimeStamp = view.lastRecordFrame = view.lastVideoFrame = getMS();
view.dirty = 0;
view.lastVideoFrameSkip=0;
view.zoomTarget = view.zoom;
view.zoomSpeed = 0;
VectorCopy(view.rot, view.rotTarget);
VectorZero(view.rotSpeed);
while (!view.quit) {
Uint32 ts_before, ts_after;
Uint32 ts;
ts_before = getMS();
if (state.mode & SM_RECORD) {
view.frameSkipCounter = 0;
//if (view.verboseMode)
// conAdd(LLOW, "R frame:%5i dt:%5i fs:%2i", state.totalFrames, view.deltaVideoFrame, state.historyNFrame);
setTitle(va("%s frame: %i/%i (skip:%i)", STRING_RECORD, state.totalFrames, state.historyFrames, state.historyNFrame));
processFrame();
ts = getMS();
view.deltaRecordFrame = ts - view.lastRecordFrame;
view.lastRecordFrame = ts;
if (state.autoSave && (state.totalFrames - state.lastSave) >= state.autoSave) {
cmdSaveFrameDump(0);
state.lastSave = state.totalFrames;
}
}
else if (state.mode & SM_PLAY) {
if (view.frameSkip < 0) {
view.frameSkipCounter++;
if (view.frameSkipCounter > -view.frameSkip) {
view.frameSkipCounter = 0;
state.currentFrame++;
}
} else {
state.currentFrame++;
state.currentFrame+=view.frameSkip;
}
if (state.currentFrame >= state.frame) {
state.currentFrame = 0;
view.frameSkipCounter = 0;
}
//if (view.verboseMode)
// conAdd(LLOW, "P frame:%5i dt:%5i fs:%2i", state.currentFrame, view.deltaVideoFrame, state.historyNFrame);
}
timerUpdate();
runInput();
if (view.quit) return;
if (state.autoRecordNext) {
state.autoRecordNext = 0;
cmdRecord(0);
}
setColours();
if (view.zoomFitAuto == 2) {
cmdZoomFit(NULL);
view.zoomTarget = view.zoom;
view.zoomSpeed = 0;
}
runVideo();
/* if we are not recording or replaying, wait a bit -- helps to cool down you laptop :-)) */
if (((state.mode & (SM_RECORD|SM_PLAY) ) == 0) && (view.dirty < 1)) {
ts_after = getMS();
if (ts_after < (ts_before + SMALL_NAP)) SDL_Delay( SMALL_NAP - (ts_after - ts_before));
}
/* pull the break on very fast video cards - 60fps playback is enough */
if (((state.mode & SM_PLAY ) == SM_PLAY) || (state.mode == 0)) {
ts_after = getMS();
if (ts_after < (ts_before + PLAY_MIN_TIME)) SDL_Delay(PLAY_MIN_TIME - (ts_after - ts_before));
}
/* if minVideoRefreshTime is set, hold the current frame a bit longer*/
if (view.minVideoRefreshTime >= SDL_TIMESLICE) {
ts_after = getMS();
if (ts_after < (ts_before + view.minVideoRefreshTime)) SDL_Delay(view.minVideoRefreshTime - (ts_after - ts_before));
}
// if last video frame was displayed, reset dirty flag
if (view.lastVideoFrameSkip==0) {
view.dirty = 0;
if (view.drawAxis==3) view.drawAxis=1;
}
}
}
