bool SemiglobalLabMatcher::update()
{
WriteGuard<ReadWritePipe<FloatImage, FloatImage> > wguard(m_wpipe);
FloatImage leftImg, rightImg;
if ( m_lrpipe->read(&leftImg) && m_rrpipe->read(&rightImg) )
{
Dim dsiDim(leftImg.dim().width(), leftImg.dim().height(),
m_maxDisparity);
float *leftImg_d = leftImg.devMem();
float *rightImg_d = rightImg.devMem();
FloatImage dispImage = FloatImage::CreateDev(
Dim(dsiDim.width(), dsiDim.height()));
cudaPitchedPtr aggregDSI = m_aggregDSI.mem(dsiDim);
SGPath *paths = m_sgPaths.getDescDev(dispImage.dim());
SemiGlobalLabDevRun(dsiDim, paths, m_sgPaths.pathCount(),
leftImg_d, rightImg_d,
aggregDSI, dispImage.devMem(), m_zeroAggregDSI);
m_zeroAggregDSI = false;
dispImage.cpuMem();
wguard.write(dispImage);
}
return wguard.wasWrite();
}
bool WLivelinessPeriodicAssertion::ManualByRTPSParticipantLivelinessAssertion()
{
std::lock_guard<std::recursive_mutex> guard(*this->mp_WLP->getBuiltinProtocols()->mp_PDP->getMutex());
bool livelinessAsserted = false;
for(std::vector<RTPSWriter*>::iterator wit=this->mp_WLP->m_livManRTPSParticipantWriters.begin();
wit!=this->mp_WLP->m_livManRTPSParticipantWriters.end();++wit)
{
if((*wit)->getLivelinessAsserted())
{
livelinessAsserted = true;
}
(*wit)->setLivelinessAsserted(false);
}
if(livelinessAsserted)
{
auto writer = this->mp_WLP->getBuiltinWriter();
auto history = this->mp_WLP->getBuiltinWriterHistory();
std::lock_guard<std::recursive_timed_mutex> wguard(writer->getMutex());
CacheChange_t* change=writer->new_change([]() -> uint32_t {return BUILTIN_PARTICIPANT_DATA_MAX_SIZE;}, ALIVE);
if(change!=nullptr)
{
change->instanceHandle = m_iHandle;
#if __BIG_ENDIAN__
change->serializedPayload.encapsulation = (uint16_t)PL_CDR_BE;
#else
change->serializedPayload.encapsulation = (uint16_t)PL_CDR_LE;
#endif
memcpy(change->serializedPayload.data,m_guidP.value,12);
for(uint8_t i =12;i<24;++i)
change->serializedPayload.data[i] = 0;
change->serializedPayload.data[15] = m_livelinessKind+1;
change->serializedPayload.length = 12+4+4+4;
for(auto ch = history->changesBegin();
ch!=history->changesEnd();++ch)
{
if((*ch)->instanceHandle == change->instanceHandle)
{
history->remove_change(*ch);
}
}
history->add_change(change);
}
}
return false;
}
bool FastWTAMatcher::update()
{
WriteGuard<ReadWritePipe<FloatImage, FloatImage> > wguard(m_wpipe);
FloatImage leftImg, rightImg;
if ( m_lrpipe->read(&leftImg) && m_rrpipe->read(&rightImg) )
{
float *leftImg_d = leftImg.devMem();
float *rightImg_d = rightImg.devMem();
Dim dsiDim(leftImg.dim().width(), leftImg.dim().height(),
m_maxDisparity);
FloatImage image = FloatImage::CreateDev(
Dim(dsiDim.width(), dsiDim.height()));
FastWTADevRun(dsiDim, leftImg_d, rightImg_d, image.devMem());
image.cpuMem();
wguard.write(image);
}
return wguard.wasWrite();
}
bool WLivelinessPeriodicAssertion::AutomaticLivelinessAssertion()
{
std::lock_guard<std::recursive_mutex> guard(*this->mp_WLP->getBuiltinProtocols()->mp_PDP->getMutex());
if(this->mp_WLP->m_livAutomaticWriters.size()>0)
{
auto writer = this->mp_WLP->getBuiltinWriter();
auto history = this->mp_WLP->getBuiltinWriterHistory();
std::lock_guard<std::recursive_timed_mutex> wguard(writer->getMutex());
CacheChange_t* change=writer->new_change([]() -> uint32_t {return BUILTIN_PARTICIPANT_DATA_MAX_SIZE;}, ALIVE,m_iHandle);
if(change!=nullptr)
{
//change->instanceHandle = m_iHandle;
#if __BIG_ENDIAN__
change->serializedPayload.encapsulation = (uint16_t)PL_CDR_BE;
#else
change->serializedPayload.encapsulation = (uint16_t)PL_CDR_LE;
#endif
memcpy(change->serializedPayload.data,m_guidP.value,12);
for(uint8_t i =12;i<24;++i)
change->serializedPayload.data[i] = 0;
change->serializedPayload.data[15] = m_livelinessKind+1;
change->serializedPayload.length = 12+4+4+4;
if(history->getHistorySize() > 0)
{
for(std::vector<CacheChange_t*>::iterator chit = history->changesBegin();
chit!=history->changesEnd();++chit)
{
if((*chit)->instanceHandle == change->instanceHandle)
{
history->remove_change(*chit);
break;
}
}
}
history->add_change(change);
}
}
return true;
}
bool StereoCorrespondenceCV::update()
{
WriteGuard<ReadWritePipe<FloatImage> > wguard(m_wpipe);
FloatImage limg, rimg;
if ( m_lrpipe->read(&limg) && m_rrpipe->read(&rimg) )
{
CvMat *limg_c = limg.cpuMem();
CvMat *rimg_c = rimg.cpuMem();
CvMat *limg8u_c = m_limg8U.getImage(cvGetSize(limg_c));
CvMat *rimg8u_c = m_rimg8U.getImage(cvGetSize(rimg_c));
cvConvertScale(limg_c, limg8u_c, 255.0);
cvConvertScale(rimg_c, rimg8u_c, 255.0);
FloatImage output = FloatImage::CreateCPU(
Dim::minDim(limg.dim(), rimg.dim()));
CvMat *out_c = output.cpuMem();
CudaBenchmarker bMarker;
bMarker.begin();
if ( m_mode == LocalMatching )
cvFindStereoCorrespondenceBM(limg8u_c, rimg8u_c, out_c, m_bmState);
else
cvFindStereoCorrespondenceGC(limg8u_c, rimg8u_c, out_c, NULL,
m_gcState);
bMarker.end();
m_mark.addProbe(bMarker.elapsedTime());
wguard.write(output);
FloatImageSinkPol::sink(limg);
FloatImageSinkPol::sink(rimg);
}
return wguard.wasWrite();
}
