YARP
Yet Another Robot Platform
DepthImage.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #include "DepthImage.h"
10 
11 #include <algorithm>
12 #include <cmath>
13 
14 #include <yarp/os/LogComponent.h>
15 #include <yarp/sig/Image.h>
16 
17 using namespace yarp::os;
18 using namespace yarp::sig;
19 
20 namespace {
21 YARP_LOG_COMPONENT(DEPTHIMAGE,
22  "yarp.carrier.portmonitor.depthimage",
26  nullptr)
27 }
28 
30 {
31  min = 0.2;
32  max = 10.0;
33  inMatrix = nullptr;
34  outMatrix = nullptr;
35  outImg.setPixelCode(VOCAB_PIXEL_MONO);
36  return true;
37 }
38 
40 {
41 }
42 
44 {
45  return false;
46 }
47 
49 {
50  return false;
51 }
52 
54 {
55  auto* img = thing.cast_as<Image>();
56  if(img == nullptr) {
57  yCError(DEPTHIMAGE, "DepthImageConverter: expected type FlexImage but got wrong data type!");
58  return false;
59  }
60 
61  if( img->getPixelCode() == VOCAB_PIXEL_MONO_FLOAT)
62  {
63  return true;
64  }
65 
66  yCError(DEPTHIMAGE,
67  "DepthImageConverter: expected %s, got %s, not doing any conversion!",
69  yarp::os::Vocab::decode(img->getPixelCode()).c_str() );
70  return false;
71 }
72 
74 {
75  auto* img = thing.cast_as<Image>();
76  inMatrix = reinterpret_cast<float **> (img->getRawImage());
77 
79  outImg.setPixelSize(1);
80  outImg.resize(img->width(), img->height());
81 
82  outImg.zero();
83  auto* inPixels = reinterpret_cast<float *> (img->getRawImage());
84  unsigned char *pixels = outImg.getRawImage();
85  for(size_t h=0; h<img->height(); h++)
86  {
87  for(size_t w=0; w<img->width(); w++)
88  {
89  float inVal = inPixels[w + (h * img->width())];
90  if (inVal != inVal /* NaN */ || inVal < min || inVal > max) {
91  pixels[w + (h * (img->width() ))] = 0;
92  } else {
93  int val = (int) (255.0 - (inVal * 255.0 / (max - min)));
94  if(val >= 255)
95  val = 255;
96  if(val <= 0)
97  val = 0;
98  pixels[w + (h * (img->width() ))] = (char) val;
99  }
100  }
101  }
102  th.setPortWriter(&outImg);
103  return th;
104 }
bool create(const yarp::os::Property &options) override
This will be called when the dll is properly loaded by the portmonitor carrier.
Definition: DepthImage2.cpp:53
bool setparam(const yarp::os::Property &params) override
This will be called when the portmonitor carrier parameters are set via Yarp admin port.
Definition: DepthImage2.cpp:65
bool getparam(yarp::os::Property &params) override
This will be called when the portmonitor carrier parameters are requested via Yarp admin port.
Definition: DepthImage2.cpp:70
yarp::os::Things & update(yarp::os::Things &thing) override
After data get accpeted in the accept() callback, an instance of that is given to the update function...
Definition: DepthImage2.cpp:96
void destroy() override
This will be called when the portmonitor object destroyes.
Definition: DepthImage2.cpp:61
bool accept(yarp::os::Things &thing) override
This will be called when the data reach the portmonitor object.
Definition: DepthImage2.cpp:75
static LogCallback printCallback()
Get current print callback.
Definition: Log.cpp:852
static LogType minimumPrintLevel()
Get current minimum print level.
Definition: Log.cpp:805
@ LogTypeReserved
Definition: Log.h:83
A class for storing options and configuration information.
Definition: Property.h:37
Base class for generic things.
Definition: Things.h:22
T * cast_as()
Definition: Things.h:57
Base class for storing images.
Definition: Image.h:85
void setPixelCode(int imgPixelCode)
Definition: Image.cpp:504
#define yCError(component,...)
Definition: LogComponent.h:157
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:80
@ VOCAB_PIXEL_MONO_FLOAT
Definition: Image.h:59
@ VOCAB_PIXEL_MONO
Definition: Image.h:48
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition: Vocab.cpp:36
An interface to the operating system, including Port based communication.
Signal processing.
Definition: Image.h:25