void dbPrintSets(FILE *f)
{
dictIterator *iter = NULL;
dictEntry *entry = NULL;
if (NULL == f)
return;
lockRead(sets);
if (0 == dictSize(sets))
{
unlockRead(sets);
fprintf(f, "No sets in db.\r\n");
return;
}
if (NULL == (iter = dictGetIterator(sets)))
{
unlockRead(sets);
return;
}
while (NULL != (entry = dictNext(iter)))
{
fprintf(f, "%s\r\n", dictGetEntryKey(entry));
lockRead(dictGetEntryVal(entry));
setPrint((set *) dictGetEntryVal(entry), f, 1);
unlockRead(dictGetEntryVal(entry));
fprintf(f, "\r\n");
}
dictReleaseIterator(iter);
unlockRead(sets);
}
int dbFindSet(const set *s, valType *index, int lock)
{
valType i;
if (NULL == s)
return 0;
if (lock)
lockRead(objectIndex);
for (i = 0; i < objectIndexLength; i++)
{
if (NULL != objectIndex[i] &&
objectSet == objectIndex[i]->objectType &&
1 == setCmpE(s, objectIndex[i]->objectPtr.setPtr))
{
if (index)
*index = i;
if (lock)
unlockRead(objectIndex);
return 1;
}
}
if (lock)
unlockRead(objectIndex);
return 0;
}
int dbFindObject(const dbObject *object, valType *index, int lock)
{
valType i;
if (NULL == object)
return 0;
if (lock)
lockRead(objectIndex);
for (i = 0; i < objectIndexLength; i++)
{
if (NULL != objectIndex[i] &&
1 == dbObjectCompare(object, objectIndex[i]))
{
if (index)
*index = i;
if (lock)
unlockRead(objectIndex);
return 1;
}
}
if (lock)
unlockRead(objectIndex);
return 0;
}
void ARWindow::updateState()
{
shared_lock<shared_mutex> lockRead(mApp->getSystem().getMap().getMutex());
//Clear all
mFeatureVertices.clear();
mFeatureColors.clear();
mImagePoints.clear();
mImagePointColors.clear();
//Add features
mTrackerCamera = &mApp->getSystem().getCamera();
mTrackerPoseR = mTracker->getPose3D().R;
mTrackerPoseT = mTracker->getPose3D().t;
const float pointAlpha = 0.6f;
if (mDisplayType == EDisplayType::ShowMatches)
{
//Get matches from tracker
const TrackingFrame *frame = mTracker->getFrame();
if (frame)
{
for (auto &match : frame->getMatches())
{
Eigen::Vector4f color = StaticColors::Blue(pointAlpha);
//Position is in 3D world coordinates
mFeatureVertices.push_back(match.getFeature().mPosition3D.homogeneous());
mFeatureColors.push_back(color);
}
}
}
else
{
//Determine features in view
//Add 3D features
for(auto &featurePtr : mApp->getSystem().getMap().getFeatures())
{
const Feature &feature = *featurePtr;
//Color
Eigen::Vector4f color;
color = StaticColors::Blue(pointAlpha);
//Add
mFeatureVertices.push_back(feature.mPosition3D.homogeneous());
mFeatureColors.push_back(color);
}
}
//Cube
mCubeTriangleIndices.clear();
mCubeVertices.clear();
mCubeColors.clear();
mCubeNormals.clear();
GenerateARCubeVertices(*mMap, mCubeTriangleIndices, mCubeVertices, mCubeColors, mCubeNormals);
}
void TwoFrameWindow::updateState()
{
shared_lock<shared_mutex> lockRead(mSlam->getMap().getMutex());
mValidFrameA = false;
mValidFrameB = false;
mDisplayText.str("");
const SlamKeyFrame *frameA = NULL;
const SlamKeyFrame *frameB=NULL;
int ridx=0;
int fidx=0;
for(auto ®ion : mSlam->getMap().getRegions())
{
for(auto &frame : region->getKeyFrames())
{
if(fidx==mFrameAIdx)
{
mValidFrameA = true;
frameA = frame.get();
mDisplayText << "Left: region " << ridx << ", frame " << fidx << ", time " << frameA->getTimestamp() << "\n";
}
if(fidx==mFrameBIdx)
{
mValidFrameB = true;
frameB = frame.get();
mDisplayText << "Right: region " << ridx << ", frame " << fidx << ", time " << frameB->getTimestamp() << "\n";
}
++fidx;
}
++ridx;
}
//Handle special case when viewing the tracker frame
if(mFrameBIdx == -1)
{
const SlamKeyFrame *frame = mSlam->getTracker().getFrame();
if(frame)
{
mValidFrameB = true;
frameB = frame;
mDisplayText << "Right: tracker frame, time " << frameB->getTimestamp() << "\n";;
}
}
//Update textures
if(mValidFrameA)
mFrameATexture.update(frameA->getColorImage());
if(mValidFrameB)
mFrameBTexture.update(frameB->getColorImage());
if(mValidFrameA && mValidFrameB)
updateState(*frameA, *frameB);
}
const set *dbGet(const sds setName)
{
const set *result = NULL;
lockRead(sets);
result = (const set *) dictFetchValue(sets, setName);
unlockRead(sets);
return result;
}
void BufferFrame::lockFrame(bool isExclusive) {
if(isExclusive) {
lockWrite();
}
else {
lockRead();
}
}
const sds dbGetRandSet(void)
{
dictEntry *entry = NULL;
sds result = NULL;
lockRead(sets);
if (NULL != (entry = dictGetRandomKey(sets)))
{
result = (sds) entry->key;
}
unlockRead(sets);
return result;
}
flatbuffers::Offset<FlatResult> GraphHolder::execute(Nd4jLong graphId, flatbuffers::FlatBufferBuilder &builder, const FlatInferenceRequest* request) {
if (!hasGraph(graphId))
throw unknown_graph_exception(graphId);
lockRead(graphId);
auto graph = cloneGraph(graphId);
auto res = GraphExecutioner::execute(graph, builder, request);
delete graph;
unlockRead(graphId);
return res;
}
int read(int pos)
{
readLock lockRead(m_rwMutex);
if(size()==0 || pos>=size())
return -1;
std::list<int>::iterator itr = m_list.begin();
std::advance(itr , pos);
int value = *itr;
// std::cout<<"read! now size is "<<size()<<" Thread id is "<<boost::this_thread::get_id()<<std::endl;
return value;
}
const dbObject *dbGetObject(valType id, int lock)
{
const dbObject *result = NULL;
if (lock)
lockRead(objectIndex);
if (id >= objectIndexLength)
{
unlockRead(objectIndex);
return NULL;
}
result = objectIndex[id];
if (lock)
unlockRead(objectIndex);
return result;
}
valType dbSetTrunc(void)
{
valType freed = 0, i;
dictIterator *iter = NULL;
dictEntry *entry = NULL;
lockWrite(objectIndex);
lockRead(sets);
if (NULL == (iter = dictGetIterator(sets)))
{
unlockRead(sets);
unlockWrite(objectIndex);
return 0;
}
while (NULL != (entry = dictNext(iter)))
{
lockWrite(entry);
freed += setTrunc((set *) dictGetEntryVal(entry));
unlockWrite(entry);
}
for (i = 0; i < objectIndexLength; i++)
{
if (NULL != objectIndex[i] &&
objectSet == objectIndex[i]->objectType)
{
freed += setTrunc(objectIndex[i]->objectPtr.setPtr);
}
}
dictReleaseIterator(iter);
unlockRead(sets);
unlockWrite(objectIndex);
return freed;
}
valType dbGC(void)
{
valType collected = 0, i;
dictIterator *iter = NULL;
dictEntry *entry = NULL;
set *acc = NULL;
lockWrite(objectIndex);
lockRead(sets);
// Step 1. Union all sets in db.
if (NULL == (iter = dictGetIterator(sets)))
{
unlockRead(sets);
unlockWrite(objectIndex);
return 0;
}
if (NULL == (acc = setCreate()))
{
unlockRead(sets);
unlockWrite(objectIndex);
return 0;
}
while (NULL != (entry = dictNext(iter)))
{
valType currentSetId = 0;
set *tmp = NULL, *currentSet = (set *) dictGetEntryVal(entry);
set *flattened = setFlatten(currentSet, 0);
if (NULL == flattened)
{
continue;
}
if (1 == dbFindSet(currentSet, ¤tSetId, 0))
{
if (-1 == setAdd(acc, currentSetId))
{
setDestroy(acc);
dictReleaseIterator(iter);
unlockRead(sets);
unlockWrite(objectIndex);
return 0;
}
}
if (NULL == (tmp = setUnion(acc, flattened)))
{
setDestroy(flattened);
setDestroy(acc);
dictReleaseIterator(iter);
unlockRead(sets);
unlockWrite(objectIndex);
return 0;
}
setDestroy(flattened);
setDestroy(acc);
acc = tmp;
}
dictReleaseIterator(iter);
// Step 2. Find objects not present in grand total union.
for (i = 0; i < objectIndexLength; i++)
{
if (NULL != objectIndex[i] && !setIsMember(acc, i))
{
dbObjectRelease((dbObject *) objectIndex[i]);
free((dbObject *) objectIndex[i]);
objectIndex[i] = NULL;
if (i < objectIndexFreeId)
{
objectIndexFreeId = i;
}
collected++;
}
}
setDestroy(acc);
unlockRead(sets);
unlockWrite(objectIndex);
return collected;
}
/////////////////////////////////////////////////////////////////////////
/// Run
/// @description
/// This is the main communication engine.
///
/// @I/O
/// At every timestep, a message is sent to FPGA via TCP socket connection,
/// then a message is retrieved from FPGA via the same connection.
/// On the FPGA side, it's the reverse order -- receive and then send.
/// Both DGI and FPGA sides' receive function will block until a message
/// arrives, creating a synchronous, lock-step communication between DGI
/// and FPGA. In this sense, how frequently send and receive get executed
/// by CRtdsAdapter is dependent on how fast FPGA runs.
///
/// @Error_Handling
/// Throws an exception if reading from or writing to the socket fails
///
/// @pre
/// Connection with FPGA is established.
///
/// @post
/// All values in the cmdTable is written to a buffer and sent to FPGA.
/// All values in the stateTable is rewritten with value received
/// from FPGA.
///
/// @limitations
/// Synchronous commnunication
//////////////////////////////////////////////////////////////////////////
void CRtdsAdapter::Run()
{
Logger.Trace << __PRETTY_FUNCTION__ << std::endl;
//TIMESTEP is used by deadline_timer.async_wait at the end of Run().
//We keep TIMESTEP very small as we actually do not care if we wait at all.
//We simply need to use deadline_timer.async_wait to pass control back
//to io_service, so it can schedule Run() with other callback functions
//under its watch.
const int TIMESTEP = 1; //in microseconds. NEEDS MORE TESTING TO SET CORRECTLY
//**********************************
//* Always send data to FPGA first *
//**********************************
{
boost::shared_lock<boost::shared_mutex> lockRead(m_cmdTable.m_mutex);
Logger.Debug << "Obtained mutex as reader" << std::endl;
//read from cmdTable
memcpy(m_txBuffer, m_cmdTable.m_data, m_txBufSize);
Logger.Debug << "Released reader mutex" << std::endl;
}// the scope is needed for mutex to auto release
// FPGA will send values in big-endian byte order
// If host machine is in little-endian byte order, convert to big-endian
#if __BYTE_ORDER == __LITTLE_ENDIAN
for (int i = 0; i < m_txCount; i++)
{
//should be 4 bytes in float.
endian_swap((char *) &m_txBuffer[4 * i], sizeof (float));
}
#endif
// send to FPGA
try
{
boost::asio::write(m_socket,
boost::asio::buffer(m_txBuffer, m_txBufSize));
}
catch (std::exception & e)
{
std::stringstream ss;
ss << "Send to FPGA failed for the following reason: " << e.what();
throw std::runtime_error(ss.str());
}
//*******************************
//* Receive data from FPGA next *
//*******************************
try
{
boost::asio::read(m_socket,
boost::asio::buffer(m_rxBuffer, m_rxBufSize));
}
catch (std::exception & e)
{
std::stringstream ss;
ss << "Receive from FPGA failed for the following reason" << e.what();
throw std::runtime_error(ss.str());
}
// FPGA will send values in big-endian byte order
// If host machine is in little-endian byte order, convert to little-endian
#if __BYTE_ORDER == __LITTLE_ENDIAN
for (int j = 0; j < m_rxCount; j++)
{
endian_swap((char *) &m_rxBuffer[4 * j], sizeof (float));
}
#endif
{
boost::unique_lock<boost::shared_mutex> lockWrite(m_stateTable.m_mutex);
Logger.Debug << "Client_RTDS - obtained mutex as writer" << std::endl;
//write to stateTable
memcpy(m_stateTable.m_data, m_rxBuffer, m_rxBufSize);
Logger.Debug << "Client_RTDS - released writer mutex" << std::endl;
} //scope is needed for mutex to auto release
//Start the timer; on timeout, this function is called again
m_GlobalTimer.expires_from_now(boost::posix_time::microseconds(TIMESTEP));
m_GlobalTimer.async_wait(boost::bind(&CRtdsAdapter::Run, this));
}
