void CLog::SetExtraLogLevels(int level)
{
CSingleLock waitLock(g_logState.critSec);
g_logState.m_extraLogLevels = level;
}
void CLog::Close()
{
CSingleLock waitLock(g_logState.critSec);
g_logState.m_platform.CloseLogFile();
g_logState.m_repeatLine.clear();
}
void CLog::Log(int loglevel, const char *format, ... )
{
static const char* prefixFormat = "%02.2d:%02.2d:%02.2d T:%"PRIu64" M:%9"PRIu64" %7s: ";
CSingleLock waitLock(critSec);
#if !(defined(_DEBUG) || defined(PROFILE))
if (m_logLevel > LOG_LEVEL_NORMAL ||
(m_logLevel > LOG_LEVEL_NONE && loglevel >= LOGNOTICE))
#endif
{
if (!m_file)
return;
SYSTEMTIME time;
GetLocalTime(&time);
MEMORYSTATUS stat;
GlobalMemoryStatus(&stat);
CStdString strPrefix, strData;
strData.reserve(16384);
va_list va;
va_start(va, format);
strData.FormatV(format,va);
va_end(va);
if (m_repeatLogLevel == loglevel && m_repeatLine == strData)
{
m_repeatCount++;
return;
}
else if (m_repeatCount)
{
CStdString strData2;
strPrefix.Format(prefixFormat, time.wHour, time.wMinute, time.wSecond, (uint64_t)CThread::GetCurrentThreadId(), (uint64_t)stat.dwAvailPhys, levelNames[m_repeatLogLevel]);
strData2.Format("Previous line repeats %d times." LINE_ENDING, m_repeatCount);
fputs(strPrefix.c_str(), m_file);
fputs(strData2.c_str(), m_file);
OutputDebugString(strData2);
m_repeatCount = 0;
}
m_repeatLine = strData;
m_repeatLogLevel = loglevel;
unsigned int length = 0;
while ( length != strData.length() )
{
length = strData.length();
strData.TrimRight(" ");
strData.TrimRight('\n');
strData.TrimRight("\r");
}
if (!length)
return;
OutputDebugString(strData);
/* fixup newline alignment, number of spaces should equal prefix length */
strData.Replace("\n", LINE_ENDING" ");
strData += LINE_ENDING;
strPrefix.Format(prefixFormat, time.wHour, time.wMinute, time.wSecond, (uint64_t)CThread::GetCurrentThreadId(), (uint64_t)stat.dwAvailPhys, levelNames[loglevel]);
fputs(strPrefix.c_str(), m_file);
fputs(strData.c_str(), m_file);
fflush(m_file);
}
}
void CLog::Log(int loglevel, const char *format, ... )
{
if (g_advancedSettings.m_logLevel > LOG_LEVEL_NORMAL ||
(g_advancedSettings.m_logLevel > LOG_LEVEL_NONE && loglevel >= LOGNOTICE))
{
CSingleLock waitLock(critSec);
if (!fd)
{
// g_stSettings.m_logFolder is initialized in the CSettings constructor to Q:
// and if we are running from DVD, it's changed to T: in CApplication::Create()
CStdString strLogFile, strLogFileOld;
strLogFile.Format("%sxbmc.log", _P(g_stSettings.m_logFolder).c_str());
strLogFileOld.Format("%sxbmc.old.log", _P(g_stSettings.m_logFolder).c_str());
#ifndef _LINUX
::DeleteFile(strLogFileOld.c_str());
::MoveFile(strLogFile.c_str(), strLogFileOld.c_str());
#else
::unlink(strLogFileOld.c_str());
::rename(strLogFile.c_str(), strLogFileOld.c_str());
#endif
#ifndef _LINUX
fd = _fsopen(strLogFile, "a+", _SH_DENYWR);
#else
fd = fopen(strLogFile, "a+");
#endif
}
if (!fd)
return ;
SYSTEMTIME time;
GetLocalTime(&time);
MEMORYSTATUS stat;
GlobalMemoryStatus(&stat);
CStdString strPrefix, strData;
#ifdef __APPLE__
strPrefix.Format("%02.2d:%02.2d:%02.2d T:%lu M:%9ju %7s: ", time.wHour, time.wMinute, time.wSecond, GetCurrentThreadId(), stat.dwAvailPhys, levelNames[loglevel]);
#else
strPrefix.Format("%02.2d:%02.2d:%02.2d T:%lu M:%9u %7s: ", time.wHour, time.wMinute, time.wSecond, GetCurrentThreadId(), stat.dwAvailPhys, levelNames[loglevel]);
#endif
strData.reserve(16384);
va_list va;
va_start(va, format);
strData.FormatV(format,va);
va_end(va);
int length = 0;
while ( length != (int)strData.length() )
{
length = strData.length();
strData.TrimRight(" ");
strData.TrimRight('\n');
strData.TrimRight("\r");
}
#if !defined(_LINUX) && (defined(_DEBUG) || defined(PROFILE))
OutputDebugString(strData.c_str());
OutputDebugString("\n");
#endif
/* fixup newline alignment, number of spaces should equal prefix length */
strData.Replace("\n", "\n ");
strData += "\n";
fwrite(strPrefix.c_str(), strPrefix.size(), 1, fd);
fwrite(strData.c_str(), strData.size(), 1, fd);
fflush(fd);
}
#ifndef _LINUX
#if defined(_DEBUG) || defined(PROFILE)
else
{
// In debug mode dump everything to devstudio regardless of level
CSingleLock waitLock(critSec);
CStdString strData;
strData.reserve(16384);
va_list va;
va_start(va, format);
strData.FormatV(format, va);
va_end(va);
OutputDebugString(strData.c_str());
if( strData.Right(1) != "\n" )
OutputDebugString("\n");
}
#endif
#endif
}
bool ICLVideoSurface::present(const QVideoFrame &frame)
{
if (frame.isValid()) {
QVideoFrame cloneFrame(frame);
cloneFrame.map(QAbstractVideoBuffer::ReadWrite);
lock.lock();
std::swap(imgWork,imgNextDisplay);
if(cloneFrame.pixelFormat() == QVideoFrame::Format_RGB24) {
imgWork->setChannels(3);
imgWork->setSize(utils::Size(cloneFrame.width(),cloneFrame.height()));
core::interleavedToPlanar<uchar,icl8u>(cloneFrame.bits(),imgWork,cloneFrame.bytesPerLine());
}
else if(cloneFrame.pixelFormat() == QVideoFrame::Format_YUV420P) {
imgWork->setChannels(3);
imgWork->setSize(utils::Size(cloneFrame.bytesPerLine(0),cloneFrame.height()));
core::convertYUV420ToRGB8(cloneFrame.bits(),utils::Size(cloneFrame.bytesPerLine(),cloneFrame.height()),imgWork);
if(cloneFrame.bytesPerLine()!=cloneFrame.width()) {
imgWork->setROI(utils::Point(0,0),utils::Size(cloneFrame.width(),cloneFrame.height()));
}
else
imgWork->setFullROI();
} else if(cloneFrame.pixelFormat() == QVideoFrame::Format_ARGB32) {
imgWork->setChannels(3);
imgWork->setSize(utils::Size(cloneFrame.width(),cloneFrame.height()));
const int dim = imgWork->getDim();
icl8u *res_r = imgWork->begin(0);
icl8u *res_g = imgWork->begin(1);
icl8u *res_b = imgWork->begin(2);
uchar *src = cloneFrame.bits();
// channel order in QVideoFrame seems switched
// its rather 0xBBGGRRAA than 0xAARRBBGG
// that is why we match Channel 0 -> Blue and 2 -> Reds
for(int i=0;i<dim;++i){
res_r[i] = src[i*4+2]; // red channel
res_g[i] = src[i*4+1]; // green channel
res_b[i] = src[i*4+0]; // blue channel
}
} else {
// fallback if no native conversion is available
imgWork->setChannels(3);
imgWork->setSize(utils::Size(cloneFrame.width(),cloneFrame.height()));
const QImage tmpImg(cloneFrame.bits(),
cloneFrame.width(),
cloneFrame.height(),
QVideoFrame::imageFormatFromPixelFormat(cloneFrame.pixelFormat()));
QVideoFrame tmp = QVideoFrame(tmpImg.convertToFormat(QImage::Format_RGB888));
tmp.map(QAbstractVideoBuffer::ReadWrite);
cloneFrame = tmp;
imgWork->setChannels(3);
imgWork->setSize(utils::Size(cloneFrame.width(),cloneFrame.height()));
core::interleavedToPlanar<uchar,icl8u>(cloneFrame.bits(),imgWork,cloneFrame.bytesPerLine());
}
if(useLocking.load()) {
QMutexLocker waitLock(&waitMutex);// boost::mutex::scoped_lock waitLock(waitMutex);
if(!nextFrameReady)
{
nextFrameReady=true;
waitCondition.wakeOne();// notify_one();
}
}
nextFrameReady=true;
lock.unlock();
return true;
}
return false;
}
void CLog::SetLogLevel(int level)
{
CSingleLock waitLock(critSec);
m_logLevel = level;
CLog::Log(LOGNOTICE, "Log level changed to %d", m_logLevel);
}
void CLog::Log(int loglevel, const char *format, ... )
{
static const char* prefixFormat = "%02.2d:%02.2d:%02.2d T:%"PRIu64" %7s: ";
CSingleLock waitLock(critSec);
int extras = (loglevel >> LOGMASKBIT) << LOGMASKBIT;
loglevel = loglevel & LOGMASK;
#if 0
if (m_logLevel > LOG_LEVEL_NORMAL ||
(m_logLevel > LOG_LEVEL_NONE && loglevel >= LOGNOTICE))
#endif
{
if (!m_file)
return;
if (extras != 0 && (m_extraLogLevels & extras) == 0)
return;
SYSTEMTIME time;
GetLocalTime(&time);
CStdString strPrefix, strData;
strData.reserve(16384);
va_list va;
va_start(va, format);
strData = StringUtils::FormatV(format,va);
va_end(va);
if (m_repeatLogLevel == loglevel && m_repeatLine == strData)
{
m_repeatCount++;
return;
}
else if (m_repeatCount)
{
strPrefix = StringUtils::Format(prefixFormat,
time.wHour,
time.wMinute,
time.wSecond,
(uint64_t)CThread::GetCurrentThreadId(),
levelNames[m_repeatLogLevel]);
CStdString strData2 = StringUtils::Format("Previous line repeats %d times."
LINE_ENDING,
m_repeatCount);
fputs(strPrefix.c_str(), m_file);
fputs(strData2.c_str(), m_file);
OutputDebugString(strData2);
m_repeatCount = 0;
}
m_repeatLine = strData;
m_repeatLogLevel = loglevel;
StringUtils::TrimRight(strData);
if (strData.empty())
return;
OutputDebugString(strData);
/* fixup newline alignment, number of spaces should equal prefix length */
StringUtils::Replace(strData, "\n", LINE_ENDING" ");
strData += LINE_ENDING;
strPrefix = StringUtils::Format(prefixFormat,
time.wHour,
time.wMinute,
time.wSecond,
(uint64_t)CThread::GetCurrentThreadId(),
levelNames[loglevel]);
//print to adb
#if defined(TARGET_ANDROID) && defined(_DEBUG)
CXBMCApp::android_printf("%s%s",strPrefix.c_str(), strData.c_str());
#endif
fputs(strPrefix.c_str(), m_file);
fputs(strData.c_str(), m_file);
fflush(m_file);
}
}
// 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 LSDWrapper::HandleFrame()
{
//printf("tracking Frame at ms=%d (from %d)\n",getMS(ros::Time::now()),mimFrameTime-filter->delayVideo);
// prep data
msg = "";
ros::Time startedFunc = ros::Time::now();
// reset?
if(resetLSDRequested)
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> filterPosePreLSD = filter->getPoseAtAsVec(mimFrameTime_workingCopy-filter->delayVideo,true);
pthread_mutex_unlock( &filter->filter_CS );
// ------------------------ do PTAM -------------------------
myGLWindow->SetupViewport();
myGLWindow->SetupVideoOrtho();
myGLWindow->SetupVideoRasterPosAndZoom();
// 1. transform with filter
TooN::Vector<6> LSDPoseGuess = filter->backTransformLSDObservation(filterPosePreLSD.slice<0,6>());
// 2. convert to se3
predConvert->setPosRPY(LSDPoseGuess[0], LSDPoseGuess[1], LSDPoseGuess[2], LSDPoseGuess[3], LSDPoseGuess[4], LSDPoseGuess[5]);
// 3. multiply with rotation matrix
TooN::SE3<> LSDPoseGuessSE3 = predConvert->droneToFrontNT * predConvert->globaltoDrone;
boost::unique_lock<boost::recursive_mutex> waitLock(imageStream->getBuffer()->getMutex());
while (!fullResetRequested && !(imageStream->getBuffer()->size() > 0)) {
notifyCondition.wait(waitLock);
}
waitLock.unlock();
// Track image and get current pose estimate
/***Note: image is of type imagedata from lsd-slam change it***/
TooN::SE3<> LSDResultSE3;
newImageCallback(mimFrameBW_workingCopy.data, mimFrameBW_workingCopy.timestamp);
LSDResultSE3 = monoOdometry->getCurrentPoseEstimate();
ros::Duration timeLSD= ros::Time::now() - startedLSD;
TooN::Vector<6> LSDResultSE3TwistOrg = LSDResultSE3.ln();
node->publishTf(LSDResultSE3, mimFrameTimeRos_workingCopy, mimFrameSEQ_workingCopy,"cam_front");
// 1. multiply from left by frontToDroneNT.
// 2. convert to xyz,rpy
predConvert->setPosSE3_globalToDrone(predConvert->frontToDroneNT * LSDResultSE3);
TooN::Vector<6> LSDResult = TooN::makeVector(predConvert->x, predConvert->y, predConvert->z, predConvert->roll, predConvert->pitch, predConvert->yaw);
// 3. transform with filter
TooN::Vector<6> LSDResultTransformed = filter->transformLSDObservation(LSDResult);
//Init failed code removed
// --------------------------- assess result ------------------------------
bool isGood = true;
bool diverged = false;
// calculate absolute differences.
TooN::Vector<6> diffs = LSDResultTransformed - filterPosePreLSD.slice<0,6>();
for(int i=0;1<1;i++) diffs[i] = abs(diffs[i]);
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
{
void CLog::SetExtraLogLevels(int level)
{
CSingleLock waitLock(critSec);
m_extraLogLevels = level;
}
void CLog::Log(int loglevel, const char *format, ... )
{
if (g_advancedSettings.m_logLevel > LOG_LEVEL_NORMAL ||
(g_advancedSettings.m_logLevel > LOG_LEVEL_NONE && loglevel >= LOGNOTICE))
{
CSingleLock waitLock(critSec);
if (!m_file)
return;
SYSTEMTIME time;
GetLocalTime(&time);
MEMORYSTATUS stat;
GlobalMemoryStatus(&stat);
CStdString strPrefix, strData;
strPrefix.Format("%02.2d:%02.2d:%02.2d T:%"PRIu64" M:%9"PRIu64" %7s: ", time.wHour, time.wMinute, time.wSecond, (uint64_t)CThread::GetCurrentThreadId(), (uint64_t)stat.dwAvailPhys, levelNames[loglevel]);
strData.reserve(16384);
va_list va;
va_start(va, format);
strData.FormatV(format,va);
va_end(va);
if (m_repeatLogLevel == loglevel && *m_repeatLine == strData)
{
m_repeatCount++;
return;
}
else if (m_repeatCount)
{
CStdString strPrefix2, strData2;
strPrefix2.Format("%02.2d:%02.2d:%02.2d T:%"PRIu64" M:%9"PRIu64" %7s: ", time.wHour, time.wMinute, time.wSecond, (uint64_t)CThread::GetCurrentThreadId(), (uint64_t)stat.dwAvailPhys, levelNames[m_repeatLogLevel]);
strData2.Format("Previous line repeats %d times." LINE_ENDING, m_repeatCount);
fwrite(strPrefix2.c_str(),strPrefix2.size(),1,m_file);
fwrite(strData2.c_str(),strData2.size(),1,m_file);
#if !defined(_LINUX) && (defined(_DEBUG) || defined(PROFILE))
OutputDebugString(strData2.c_str());
#endif
m_repeatCount = 0;
}
*m_repeatLine = strData;
m_repeatLogLevel = loglevel;
unsigned int length = 0;
while ( length != strData.length() )
{
length = strData.length();
strData.TrimRight(" ");
strData.TrimRight('\n');
strData.TrimRight("\r");
}
if (!length)
return;
#if !defined(_LINUX) && (defined(_DEBUG) || defined(PROFILE))
OutputDebugString(strData.c_str());
OutputDebugString("\n");
#endif
/* fixup newline alignment, number of spaces should equal prefix length */
strData.Replace("\n", LINE_ENDING" ");
strData += LINE_ENDING;
fwrite(strPrefix.c_str(),strPrefix.size(),1,m_file);
fwrite(strData.c_str(),strData.size(),1,m_file);
fflush(m_file);
}
#ifndef _LINUX
#if defined(_DEBUG) || defined(PROFILE)
else
{
// In debug mode dump everything to devstudio regardless of level
CSingleLock waitLock(critSec);
CStdString strData;
strData.reserve(16384);
va_list va;
va_start(va, format);
strData.FormatV(format, va);
va_end(va);
OutputDebugString(strData.c_str());
if( strData.Right(1) != "\n" )
OutputDebugString("\n");
}
#endif
#endif
}
void CLog::Close()
{
CSingleLock waitLock(s_globals.critSec);
s_globals.m_platform.CloseLogFile();
s_globals.m_repeatLine.clear();
}
void CLog::SetExtraLogLevels(int level)
{
CSingleLock waitLock(s_globals.critSec);
s_globals.m_extraLogLevels = level;
}
// PD_TRACE_DECLARE_FUNCTION ( SDB_DPSLOCKBUCKET_ACQUIRE, "dpsLockBucket::acquire" )
INT32 dpsLockBucket::acquire( _pmdEDUCB *eduCB,
const dpsTransLockId &lockId,
DPS_TRANSLOCK_TYPE lockType )
{
INT32 rc = SDB_OK;
dpsTransLockUnit *pLockUnit = NULL;
{
ossScopedLock _lock( &_lstMutex );
dpsTransLockUnitList::iterator iterLst
= _lockLst.find( lockId );
if ( _lockLst.end() == iterLst )
{
pLockUnit = SDB_OSS_NEW dpsTransLockUnit();
if ( NULL == pLockUnit )
{
rc = SDB_OOM;
goto error;
}
_lockLst[ lockId ] = pLockUnit;
}
else
{
pLockUnit = iterLst->second ;
}
rc = appendToRun( eduCB, lockType, pLockUnit );
if ( rc )
{
appendToWait( eduCB, lockId, pLockUnit );
rc = SDB_OK ;
}
else
{
goto done ;
}
}
PD_RC_CHECK( rc, PDERROR, "Failed to get the lock, append failed(rc=%d)",
rc );
waitretry:
rc = waitLock( eduCB );
{
ossScopedLock _lock_2( &_lstMutex );
if ( rc )
{
removeFromWait( eduCB, pLockUnit, lockId );
goto error ;
}
rc = appendToRun( eduCB, lockType, pLockUnit );
if ( rc )
{
goto waitretry;
}
removeFromWait( eduCB, pLockUnit, lockId );
}
done:
PD_TRACE_EXIT ( SDB_DPSLOCKBUCKET_ACQUIRE );
return rc;
error:
PD_LOG ( PDERROR, "Failed to get the lock(rc=%d)", rc ) ;
goto done;
}
