28 #include <opencv2/opencv.hpp>
33 #include <Image/ImageProcessor.h>
52 std::string ImageProcessor::ListenerSuffix =
".ImageListener";
55 ImageProcessor::onInitComponent()
57 if (hasProperty(
"CompressionType") && hasProperty(
"CompressionQuality"))
59 setCompressionType(getProperty<CompressionType>(
"CompressionType").getValue(),
60 getProperty<int>(
"CompressionQuality").getValue());
63 onInitImageProcessor();
64 float cycleTime = desiredFps == 0 ? 0 : 1000.f / desiredFps;
66 this, &ImageProcessor::runProcessor, cycleTime,
false,
"ImageProcessingThread",
false);
69 processorTask->setDelayWarningTolerance(1000);
73 processorTask->setDelayWarningTolerance(cycleTime * 5);
78 ImageProcessor::onConnectComponent()
80 onConnectImageProcessor();
83 if (startProcessorTask)
85 processorTask->start();
90 ImageProcessor::onDisconnectComponent()
92 processorTask->stop();
93 onDisconnectImageProcessor();
97 ImageProcessor::onExitComponent()
100 onExitImageProcessor();
106 ImageProcessor::createPropertyDefinitions()
113 ImageProcessor::runProcessor()
122 ImageProcessor::getImageListenerTopicName(std::string providerName)
const
124 return providerName + ListenerSuffix;
128 ImageProcessor::usingImageProvider(std::string providerName)
132 usingTopic(getImageListenerTopicName(providerName));
137 if (hasProperty(
"ForceIceTransfer") && getProperty<bool>(
"ForceIceTransfer").getValue())
139 consumer->setTransferMode(armarx::eIce);
142 std::pair<std::string, armarx::IceSharedMemoryConsumer<unsigned char>::pointer_type>
144 consumer_info.first = providerName;
145 consumer_info.second = consumer;
147 usedImageProviders.insert(consumer_info);
151 ImageProcessor::releaseImageProvider(std::string providerName)
154 auto it = usedImageProviders.find(providerName);
155 if (it != usedImageProviders.end())
157 removeProxyDependency(providerName);
159 removeProxyDependency(consumer->getMemoryName());
160 unsubscribeFromTopic(getImageListenerTopicName(providerName));
161 imageProviderInfoMap.erase(providerName);
162 usedImageProviders.erase(it);
167 ImageProcessor::getImageProvider(std::string providerName,
168 ImageType destinationImageType,
174 std::unique_lock lock(imageProviderInfoMutex);
180 provider_info.
proxy = getProxy<ImageProviderInterfacePrx>(providerName, waitForProxy);
185 provider_info.
buffer.resize(
196 std::pair<std::string, ImageProviderInfo> entry(providerName, provider_info);
197 imageProviderInfoMap.insert(entry);
200 ImageProviderMap::iterator iter = usedImageProviders.find(providerName);
202 if (iter == usedImageProviders.end())
204 usingImageProvider(providerName);
207 if (!provider_info.
proxy->hasSharedMemorySupport())
209 ARMARX_INFO <<
"shared memory not available for provider " << providerName;
210 usedImageProviders[providerName]->setTransferMode(eIce);
211 imageProviderInfoMap[providerName].imageTransferMode = eIceTransfer;
212 imageProviderInfoMap[providerName].info =
214 removeProxyDependency(usedImageProviders[providerName]->getMemoryName());
221 usedImageProviders[providerName]->start();
223 auto transferMode = usedImageProviders[providerName]->getTransferMode();
224 switch (transferMode)
227 imageProviderInfoMap[providerName].imageTransferMode = eIceTransfer;
230 imageProviderInfoMap[providerName].imageTransferMode = eSharedMemoryTransfer;
235 std::unique_lock lock2(statisticsMutex);
236 statistics[providerName].pollingFPS.reset();
237 statistics[providerName].imageProviderFPS.reset();
239 return provider_info;
243 ImageProcessor::getImageProvider(std::string name,
245 ImageType destinationImageType)
247 return getImageProvider(name, destinationImageType, waitForProxy);
251 ImageProcessor::enableResultImages(
int numberImages,
252 ImageDimension imageDimension,
254 const std::string& name)
256 if (!resultImageProvider)
258 ARMARX_VERBOSE <<
"Enabling ResultImageProvider with " << numberImages
259 <<
" result images.";
260 resultImageProvider = Component::create<ResultImageProvider>();
261 resultImageProvider->setName(name.empty() ? getName() +
"Result" : name);
263 getArmarXManager()->addObject(resultImageProvider);
265 resultImageProvider->setNumberResultImages(numberImages);
266 resultImageProvider->setResultImageFormat(imageDimension, imageType);
269 resultImageProvider->getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted);
274 ImageProcessor::provideResultImages(CByteImage** images, armarx::MetaInfoSizeBasePtr info)
276 if (resultImageProvider)
280 resultImageProvider->provideResultImages(images, info->timeProvided);
284 resultImageProvider->provideResultImages(
291 ImageProcessor::provideResultImages(
const std::vector<CByteImageUPtr>& images,
292 MetaInfoSizeBasePtr info)
294 if (resultImageProvider)
298 resultImageProvider->provideResultImages(images, info->timeProvided);
302 resultImageProvider->provideResultImages(
309 ImageProcessor::waitForImages(
int milliseconds)
311 if (imageProviderInfoMap.size() == 0)
313 ARMARX_ERROR <<
"Calling getImages without ImageProvider name, and no ImageProvider is "
314 "available - did you forget to call useImageProvider?";
317 else if (imageProviderInfoMap.size() > 1)
319 std::string outputText =
320 "Several image providers are available - using the first one in the list:";
321 std::map<std::string, ImageProviderInfo>::iterator iter;
323 for (iter = imageProviderInfoMap.begin(); iter != imageProviderInfoMap.end(); iter++)
325 outputText <<
" " << iter->first;
331 return waitForImages(imageProviderInfoMap.begin()->first, milliseconds);
335 ImageProcessor::waitForImages(std::string providerName,
int milliseconds)
337 std::shared_ptr<std::condition_variable> cond;
340 std::unique_lock lock(imageProviderInfoMutex);
342 std::map<std::string, ImageProviderInfo>::iterator iter =
343 imageProviderInfoMap.find(providerName);
345 if (iter == imageProviderInfoMap.end())
347 ARMARX_ERROR <<
"Trying to wait for images from unknown image provider. Call "
348 "useImageProvider before";
351 cond = iter->second.imageAvailableEvent;
355 ARMARX_DEBUG <<
"Waiting for images from provider proxy " << providerName;
359 std::unique_lock lock(mut);
360 auto td = std::chrono::milliseconds(milliseconds);
362 return cond->wait_for(lock, td) != std::cv_status::timeout;
366 ImageProcessor::waitForImages(std::string providerName,
IceUtil::Time waitTime)
368 return waitForImages(providerName,
static_cast<int>(waitTime.toMilliSeconds()));
372 ImageProcessor::isNewImageAvailable()
374 return isNewImageAvailable(imageProviderInfoMap.begin()->first);
378 ImageProcessor::isNewImageAvailable(
const std::string& providerName)
380 std::unique_lock lock(imageProviderInfoMutex);
381 std::map<std::string, ImageProviderInfo>::iterator iter =
382 imageProviderInfoMap.find(providerName);
384 if (iter == imageProviderInfoMap.end())
386 ARMARX_ERROR <<
"Trying access unknown image provider. Call useImageProvider before";
390 bool result = iter->second.imageAvailable;
395 ImageProcessor::getImages(CByteImage** ppImages)
397 int numberOfImages = getNumberOfImages();
404 for (
int i = 0; i < numberOfImages; ++i)
412 void** ppBuffer =
new void*[numberOfImages];
414 for (
int i = 0; i < numberOfImages; ++i)
416 ppBuffer[i] = (
void*)ppImages[i]->pixels;
419 numberOfImages = getImages(ppBuffer);
422 return numberOfImages;
426 ImageProcessor::getImages(CFloatImage** ppImages)
428 int numberOfImages = getNumberOfImages();
430 void** ppBuffer =
new void*[numberOfImages];
432 for (
int i = 0; i < numberOfImages; ++i)
434 ppBuffer[i] = (
void*)ppImages[i]->pixels;
437 numberOfImages = getImages(ppBuffer);
440 return numberOfImages;
444 ImageProcessor::getImages(
void** ppBuffer)
446 if (imageProviderInfoMap.size() == 0)
448 ARMARX_ERROR <<
"Calling getImages without ImageProvider name, and no ImageProvider is "
449 "available - did you forget to call useImageProvider?";
452 else if (imageProviderInfoMap.size() > 1)
454 std::string outputText =
455 "Several image providers are available - using the first one in the list:";
456 std::map<std::string, ImageProviderInfo>::iterator iter;
458 for (iter = imageProviderInfoMap.begin(); iter != imageProviderInfoMap.end(); iter++)
460 outputText <<
" " << iter->first;
465 std::string providerName;
467 std::unique_lock lock(imageProviderInfoMutex);
468 providerName = imageProviderInfoMap.begin()->first;
472 armarx::MetaInfoSizeBasePtr info;
473 return getImages(providerName, ppBuffer, info);
477 ImageProcessor::getImages(std::string providerName,
478 CByteImage** ppImages,
479 armarx::MetaInfoSizeBasePtr& info)
481 int numberOfImages = getNumberOfImages(providerName);
483 void** ppBuffer =
new void*[numberOfImages];
485 for (
int i = 0; i < numberOfImages; ++i)
487 ppBuffer[i] = (
void*)ppImages[i]->pixels;
490 numberOfImages = getImages(providerName, ppBuffer, info);
493 return numberOfImages;
497 ImageProcessor::getImages(std::string providerName,
498 const std::vector<CByteImageUPtr>& ppImages,
499 MetaInfoSizeBasePtr& info)
501 int numberOfImages = getNumberOfImages(providerName);
503 void** ppBuffer =
new void*[numberOfImages];
505 for (
int i = 0; i < numberOfImages; ++i)
507 ppBuffer[i] = (
void*)ppImages.at(i)->pixels;
510 numberOfImages = getImages(providerName, ppBuffer, info);
513 return numberOfImages;
517 ImageProcessor::getImages(std::string providerName,
518 CFloatImage** ppImages,
519 armarx::MetaInfoSizeBasePtr& info)
521 int numberOfImages = getNumberOfImages(providerName);
523 void** ppBuffer =
new void*[numberOfImages];
525 for (
int i = 0; i < numberOfImages; ++i)
527 ppBuffer[i] = (
void*)ppImages[i]->pixels;
530 numberOfImages = getImages(providerName, ppBuffer, info);
533 return numberOfImages;
537 ImageProcessor::getImages(std::string providerName,
539 armarx::MetaInfoSizeBasePtr& info)
541 int numberImages = -1;
545 std::unique_lock lock(imageProviderInfoMutex);
548 std::map<std::string, ImageProviderInfo>::iterator iter =
549 imageProviderInfoMap.find(providerName);
551 if (iter == imageProviderInfoMap.end())
553 ARMARX_ERROR <<
"Trying to retrieve images from unknown image provider. Call "
554 "useImageProvider before";
558 numberImages = iter->second.numberImages;
561 if (!iter->second.imageAvailable)
571 CompressedImageProviderInterfacePrx compressedImageProvider;
572 if (compressionType != eNoCompression)
574 compressedImageProvider =
575 CompressedImageProviderInterfacePrx::checkedCast(iter->second.proxy);
576 if (!compressedImageProvider)
580 <<
"Trying to use image provider as "
581 "CompressedImageProviderInterfacePrx, but it is not of this type: "
582 << iter->second.proxy->ice_id();
585 if (compressionType != eNoCompression && compressedImageProvider)
588 auto blob = compressedImageProvider->getCompressedImagesAndMetaInfo(
589 compressionType, compressionQuality, providerInfo.
info);
592 cv::Mat mat = cv::imdecode(blob, cv::IMREAD_COLOR);
595 << mat.size().width <<
"x" << mat.size().height
596 <<
", bpp: " << mat.channels();
597 providerInfo.
buffer = Blob(mat.datastart, mat.dataend);
599 << providerInfo.
buffer.size();
606 blob = iter->second.proxy->getImagesAndMetaInfo(providerInfo.
info);
610 IceUtil::Time::microSeconds(providerInfo.
info->timeProvided))
611 .toMilliSecondsDouble()
612 <<
" ms old, timestamp: " << providerInfo.
info->timeProvided;
617 <<
"using fall back remote procedure call without timestamp!";
620 blob = iter->second.proxy->getImages();
623 blob.swap(providerInfo.
buffer);
628 usedImageProviders[providerName]->getData(providerInfo.
buffer, providerInfo.
info);
631 int imageSize = providerInfo.
imageFormat.dimension.width *
636 for (
int i = 0; i < numberImages; i++)
639 iter->second, &iter->second.buffer[0] + i * imageSize, ppBuffer[i]);
642 info = iter->second.info;
645 iter->second.imageAvailable =
false;
650 std::unique_lock lock(statisticsMutex);
651 statistics[providerName].pollingFPS.update();
657 ImageProcessor::getNumberOfImages(
const std::string& providerName)
659 if (imageProviderInfoMap.size() == 0 && providerName.empty())
661 ARMARX_ERROR <<
"Calling getNumberOfImages() without ImageProvider name, and no "
662 "ImageProvider is available - did you forget to call useImageProvider?";
665 else if (imageProviderInfoMap.size() > 1 && providerName.empty())
667 std::string outputText =
668 "Several image providers are available and no name was specified when calling "
669 "getNumberOfImages() - using the first one in the list:";
670 std::map<std::string, ImageProviderInfo>::iterator iter;
672 for (iter = imageProviderInfoMap.begin(); iter != imageProviderInfoMap.end(); iter++)
674 outputText <<
" " << iter->first;
680 int numberOfImages = 0;
682 if (providerName.empty())
684 std::unique_lock lock(imageProviderInfoMutex);
686 numberOfImages = imageProviderInfoMap.begin()->second.numberImages;
690 std::unique_lock lock(imageProviderInfoMutex);
693 std::map<std::string, ImageProviderInfo>::iterator iter =
694 imageProviderInfoMap.find(providerName);
696 if (iter == imageProviderInfoMap.end())
698 ARMARX_ERROR <<
"Trying to retrieve images from unknown image provider. Call "
699 "useImageProvider before";
703 numberOfImages = iter->second.numberImages;
706 return numberOfImages;
710 ImageProcessor::getImageTransferStats(std::string providerName,
bool resetStats)
712 std::unique_lock lock(statisticsMutex);
714 std::map<std::string, ImageTransferStats>::iterator iter = statistics.find(providerName);
716 if (iter == statistics.end())
718 ARMARX_ERROR <<
"Requesting statistics for unknown image provider (" << providerName
728 iter->second.pollingFPS.reset();
732 iter->second.imageProviderFPS.recalculate();
733 iter->second.pollingFPS.recalculate();
740 ImageProcessor::getImageMetaInfo(
const std::string& imageProviderName)
const
745 if (usedImageProviders.empty())
750 if (imageProviderName.empty())
752 return usedImageProviders.begin()->second->getMetaInfo();
756 auto it = usedImageProviders.find(imageProviderName);
757 if (it != usedImageProviders.end())
759 return it->second->getMetaInfo();
763 ARMARX_ERROR <<
"Requesting meta info for unknown image provider ("
764 << imageProviderName <<
")";
771 ImageProcessor::setFramerate(
float fps)
773 this->desiredFps = fps;
776 processorTask->changeInterval(1000.f / fps);
781 ImageProcessor::getFramerate()
const
787 ImageProcessor::setCompressionType(CompressionType compressionType,
int compressionQuality)
789 ARMARX_VERBOSE <<
"Setting compression to " << (int)compressionType <<
" and quality "
790 << compressionQuality;
791 this->compressionType = compressionType;
792 this->compressionQuality = compressionQuality;
796 ImageProcessor::reportImageAvailable(
const std::string& providerName,
const Ice::Current&
c)
800 std::unique_lock lock(imageProviderInfoMutex);
802 std::map<std::string, ImageProviderInfo>::iterator iter =
803 imageProviderInfoMap.find(providerName);
805 if (iter == imageProviderInfoMap.end())
808 <<
"Received notification from unknown imageprovider (" << providerName
814 iter->second.imageAvailable =
true;
815 iter->second.imageAvailableEvent->notify_all();
816 ARMARX_DEBUG <<
"Got notification of image provider " << providerName;
820 std::unique_lock lock(statisticsMutex);
821 statistics[providerName].imageProviderFPS.update();
825 ImageProcessor::cleanup()
827 std::map<std::string, ImageProviderInfo>::iterator iter = imageProviderInfoMap.begin();
829 while (iter != imageProviderInfoMap.end())
834 if (resultImageProvider)
836 getArmarXManager()->removeObjectBlocking(resultImageProvider);
841 ImageProcessor::componentPropertiesUpdated(
const std::set<std::string>& changedProperties)
844 if (changedProperties.count(
"CompressionType"))
846 setCompressionType(getProperty<CompressionType>(
"CompressionType").getValue());
848 if (changedProperties.count(
"CompressionQuality"))
850 setCompressionType(getProperty<CompressionType>(
"CompressionType").getValue(),
851 getProperty<int>(
"CompressionQuality").getValue());
856 ImageProcessor::getCompressionQuality()
const
858 return compressionQuality;
862 ImageProcessor::getCompressionType()
const
864 return compressionType;