UndistortImage.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "UndistortImage.h"
11 
12 // Module specification
13 // <rtc-template block="module_spec">
14 static const char* cameraimageviewercomponent_spec[] =
15 {
16  "implementation_id", "UndistortImage",
17  "type_name", "UndistortImage",
18  "description", "camera image undistortion component",
19  "version", HRPSYS_PACKAGE_VERSION,
20  "vendor", "AIST",
21  "category", "example",
22  "activity_type", "DataFlowComponent",
23  "max_instance", "10",
24  "language", "C++",
25  "lang_type", "compile",
26  // Configuration variables
27  "conf.default.calibFile", "camera.xml",
28 
29  ""
30 };
31 // </rtc-template>
32 
34  : RTC::DataFlowComponentBase(manager),
35  // <rtc-template block="initializer">
36  m_imageIn("imageIn", m_image),
37  m_imageOut("imageOut", m_image),
38  // </rtc-template>
39  m_cvImage(NULL),
40  m_intrinsic(NULL),
41  m_distortion(NULL),
42  dummy(0)
43 {
44 }
45 
47 {
48 }
49 
50 
51 
52 RTC::ReturnCode_t UndistortImage::onInitialize()
53 {
54  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
55  // <rtc-template block="bind_config">
56  // Bind variables and configuration variable
57  bindParameter("calibFile", m_calibFile, "camera.xml");
58 
59  // </rtc-template>
60 
61  // Registration: InPort/OutPort/Service
62  // <rtc-template block="registration">
63  // Set InPort buffers
64  addInPort("imageIn", m_imageIn);
65 
66  // Set OutPort buffer
67  addOutPort("imageOut", m_imageOut);
68 
69  // Set service provider to Ports
70 
71  // Set service consumers to Ports
72 
73  // Set CORBA Service Ports
74 
75  // </rtc-template>
76 
78 
79  return RTC::RTC_OK;
80 }
81 
82 
83 
84 /*
85  RTC::ReturnCode_t UndistortImage::onFinalize()
86  {
87  return RTC::RTC_OK;
88  }
89 */
90 
91 /*
92  RTC::ReturnCode_t UndistortImage::onStartup(RTC::UniqueId ec_id)
93  {
94  return RTC::RTC_OK;
95  }
96 */
97 
98 /*
99  RTC::ReturnCode_t UndistortImage::onShutdown(RTC::UniqueId ec_id)
100  {
101  return RTC::RTC_OK;
102  }
103 */
104 
106 {
107  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
108 
109  CvFileStorage *fs
110  = cvOpenFileStorage (m_calibFile.c_str(), 0, CV_STORAGE_READ);
111  if (!fs){
112  std::cerr << m_profile.instance_name << ": can't open "
113  << m_calibFile << std::endl;
114  return RTC::RTC_ERROR;
115  }
116  CvFileNode *param = cvGetFileNodeByName (fs, NULL, "intrinsic");
117  m_intrinsic = (CvMat *) cvRead (fs, param);
118  param = cvGetFileNodeByName (fs, NULL, "distortion");
119  m_distortion = (CvMat *) cvRead (fs, param);
120  cvReleaseFileStorage (&fs);
121 
122  return RTC::RTC_OK;
123 }
124 
126 {
127  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
128  if (m_cvImage) {
129  cvReleaseImage(&m_cvImage);
130  m_cvImage = NULL;
131  }
132  if (m_intrinsic) cvReleaseMat (&m_intrinsic);
133  if (m_distortion) cvReleaseMat (&m_distortion);
134 
135  return RTC::RTC_OK;
136 }
137 
138 RTC::ReturnCode_t UndistortImage::onExecute(RTC::UniqueId ec_id)
139 {
140  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
141 
142  if (!m_imageIn.isNew()) return RTC::RTC_OK;
143 
144  m_imageIn.read();
145 
146  if (m_cvImage && (m_image.data.image.width != m_cvImage->width
147  || m_image.data.image.height != m_cvImage->height)){
148  cvReleaseImage(&m_cvImage);
149  m_cvImage = NULL;
150  }
151 
152  if (!m_cvImage){
153  switch (m_image.data.image.format){
154  case Img::CF_RGB:
155  m_cvImage = cvCreateImage(cvSize(m_image.data.image.width,
156  m_image.data.image.height),
157  IPL_DEPTH_8U, 3);
158  break;
159  case Img::CF_GRAY:
160  m_cvImage = cvCreateImage(cvSize(m_image.data.image.width,
161  m_image.data.image.height),
162  IPL_DEPTH_8U, 1);
163  break;
164  default:
165  std::cerr << "unsupported color format("
166  << m_image.data.image.format << ")" << std::endl;
167  return RTC::RTC_ERROR;
168  }
169  }
170  switch(m_image.data.image.format){
171  case Img::CF_RGB:
172  {
173  // RGB -> BGR
174  char *dst = m_cvImage->imageData;
175  for (unsigned int i=0; i<m_image.data.image.raw_data.length(); i+=3){
176  dst[i ] = m_image.data.image.raw_data[i+2];
177  dst[i+1] = m_image.data.image.raw_data[i+1];
178  dst[i+2] = m_image.data.image.raw_data[i ];
179  }
180  break;
181  }
182  case Img::CF_GRAY:
183  memcpy(m_cvImage->imageData,
184  m_image.data.image.raw_data.get_buffer(),
185  m_image.data.image.raw_data.length());
186  break;
187  default:
188  break;
189  }
190 
191 
192  IplImage *dst_img = cvCloneImage (m_cvImage);
193  cvUndistort2 (m_cvImage, dst_img, m_intrinsic, m_distortion);
194 
195  switch(m_image.data.image.format){
196  case Img::CF_RGB:
197  {
198  // BGR -> RGB
199  char *src = dst_img->imageData;
200  for (unsigned int i=0; i<m_image.data.image.raw_data.length(); i+=3){
201  m_image.data.image.raw_data[i+2] = src[i ];
202  m_image.data.image.raw_data[i+1] = src[i+1];
203  m_image.data.image.raw_data[i ] = src[i+2];
204  }
205  break;
206  }
207  case Img::CF_GRAY:
208  memcpy(m_image.data.image.raw_data.get_buffer(),
209  dst_img->imageData,
210  m_image.data.image.raw_data.length());
211  break;
212  default:
213  break;
214  }
215 
216  cvReleaseImage (&dst_img);
217 
218  m_imageOut.write();
219 
220  return RTC::RTC_OK;
221 }
222 
223 /*
224  RTC::ReturnCode_t UndistortImage::onAborting(RTC::UniqueId ec_id)
225  {
226  return RTC::RTC_OK;
227  }
228 */
229 
230 /*
231  RTC::ReturnCode_t UndistortImage::onError(RTC::UniqueId ec_id)
232  {
233  return RTC::RTC_OK;
234  }
235 */
236 
237 /*
238  RTC::ReturnCode_t UndistortImage::onReset(RTC::UniqueId ec_id)
239  {
240  return RTC::RTC_OK;
241  }
242 */
243 
244 /*
245  RTC::ReturnCode_t UndistortImage::onStateUpdate(RTC::UniqueId ec_id)
246  {
247  return RTC::RTC_OK;
248  }
249 */
250 
251 /*
252  RTC::ReturnCode_t UndistortImage::onRateChanged(RTC::UniqueId ec_id)
253  {
254  return RTC::RTC_OK;
255  }
256 */
257 
258 
259 
260 extern "C"
261 {
262 
264  {
266  manager->registerFactory(profile,
267  RTC::Create<UndistortImage>,
268  RTC::Delete<UndistortImage>);
269  }
270 
271 };
272 
273 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
InPort< Img::TimedCameraImage > m_imageIn
static const char * cameraimageviewercomponent_spec[]
CvMat * m_distortion
png_uint_32 i
coil::Properties & getProperties()
Img::TimedCameraImage m_image
bool addOutPort(const char *name, OutPortBase &outport)
std::string m_calibFile
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
ExecutionContextHandle_t UniqueId
virtual ~UndistortImage()
Destructor.
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
void UndistortImageInit(RTC::Manager *manager)
prop
IplImage * m_cvImage
virtual bool isNew()
virtual bool write(DataType &value)
UndistortImage(RTC::Manager *manager)
Constructor.
null component
virtual RTC::ReturnCode_t onInitialize()
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
OutPort< Img::TimedCameraImage > m_imageOut


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51