UndistortImage.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "UndistortImage.h"
00011 
00012 // Module specification
00013 // <rtc-template block="module_spec">
00014 static const char* cameraimageviewercomponent_spec[] =
00015 {
00016     "implementation_id", "UndistortImage",
00017     "type_name",         "UndistortImage",
00018     "description",       "camera image undistortion component",
00019     "version",           HRPSYS_PACKAGE_VERSION,
00020     "vendor",            "AIST",
00021     "category",          "example",
00022     "activity_type",     "DataFlowComponent",
00023     "max_instance",      "10",
00024     "language",          "C++",
00025     "lang_type",         "compile",
00026     // Configuration variables
00027     "conf.default.calibFile", "camera.xml",
00028 
00029     ""
00030 };
00031 // </rtc-template>
00032 
00033 UndistortImage::UndistortImage(RTC::Manager* manager)
00034     : RTC::DataFlowComponentBase(manager),
00035       // <rtc-template block="initializer">
00036       m_imageIn("imageIn", m_image),
00037       m_imageOut("imageOut", m_image),
00038       // </rtc-template>
00039       m_cvImage(NULL),
00040       m_intrinsic(NULL),
00041       m_distortion(NULL),
00042       dummy(0)
00043 {
00044 }
00045 
00046 UndistortImage::~UndistortImage()
00047 {
00048 }
00049 
00050 
00051 
00052 RTC::ReturnCode_t UndistortImage::onInitialize()
00053 {
00054     std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00055     // <rtc-template block="bind_config">
00056     // Bind variables and configuration variable
00057     bindParameter("calibFile", m_calibFile, "camera.xml");
00058   
00059     // </rtc-template>
00060 
00061     // Registration: InPort/OutPort/Service
00062     // <rtc-template block="registration">
00063     // Set InPort buffers
00064     addInPort("imageIn", m_imageIn);
00065 
00066     // Set OutPort buffer
00067     addOutPort("imageOut", m_imageOut);
00068   
00069     // Set service provider to Ports
00070   
00071     // Set service consumers to Ports
00072   
00073     // Set CORBA Service Ports
00074   
00075     // </rtc-template>
00076 
00077     RTC::Properties& prop = getProperties();
00078 
00079     return RTC::RTC_OK;
00080 }
00081 
00082 
00083 
00084 /*
00085   RTC::ReturnCode_t UndistortImage::onFinalize()
00086   {
00087   return RTC::RTC_OK;
00088   }
00089 */
00090 
00091 /*
00092   RTC::ReturnCode_t UndistortImage::onStartup(RTC::UniqueId ec_id)
00093   {
00094   return RTC::RTC_OK;
00095   }
00096 */
00097 
00098 /*
00099   RTC::ReturnCode_t UndistortImage::onShutdown(RTC::UniqueId ec_id)
00100   {
00101   return RTC::RTC_OK;
00102   }
00103 */
00104 
00105 RTC::ReturnCode_t UndistortImage::onActivated(RTC::UniqueId ec_id)
00106 {
00107     std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00108 
00109     CvFileStorage *fs 
00110         = cvOpenFileStorage (m_calibFile.c_str(), 0, CV_STORAGE_READ);
00111     if (!fs){
00112         std::cerr << m_profile.instance_name << ": can't open "
00113                   << m_calibFile << std::endl;
00114         return RTC::RTC_ERROR;
00115     }
00116     CvFileNode *param = cvGetFileNodeByName (fs, NULL, "intrinsic");
00117     m_intrinsic = (CvMat *) cvRead (fs, param);
00118     param = cvGetFileNodeByName (fs, NULL, "distortion");
00119     m_distortion = (CvMat *) cvRead (fs, param);
00120     cvReleaseFileStorage (&fs);
00121 
00122     return RTC::RTC_OK;
00123 }
00124 
00125 RTC::ReturnCode_t UndistortImage::onDeactivated(RTC::UniqueId ec_id)
00126 {
00127     std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00128     if (m_cvImage) {
00129         cvReleaseImage(&m_cvImage);
00130         m_cvImage = NULL;
00131     }
00132     if (m_intrinsic) cvReleaseMat (&m_intrinsic);
00133     if (m_distortion) cvReleaseMat (&m_distortion);
00134     
00135     return RTC::RTC_OK;
00136 }
00137 
00138 RTC::ReturnCode_t UndistortImage::onExecute(RTC::UniqueId ec_id)
00139 {
00140     //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")"  << std::endl;
00141 
00142     if (!m_imageIn.isNew()) return RTC::RTC_OK;
00143 
00144     m_imageIn.read();
00145 
00146     if (m_cvImage && (m_image.data.image.width != m_cvImage->width 
00147                       || m_image.data.image.height != m_cvImage->height)){
00148         cvReleaseImage(&m_cvImage);
00149         m_cvImage = NULL;
00150     }
00151 
00152     if (!m_cvImage){
00153         switch (m_image.data.image.format){
00154         case Img::CF_RGB:
00155             m_cvImage = cvCreateImage(cvSize(m_image.data.image.width,
00156                                              m_image.data.image.height),
00157                                       IPL_DEPTH_8U, 3);
00158             break;
00159         case Img::CF_GRAY:
00160             m_cvImage = cvCreateImage(cvSize(m_image.data.image.width,
00161                                              m_image.data.image.height),
00162                                       IPL_DEPTH_8U, 1);
00163             break;
00164         default:
00165             std::cerr << "unsupported color format(" 
00166                       << m_image.data.image.format << ")" << std::endl;
00167             return RTC::RTC_ERROR;
00168         }
00169     }
00170     switch(m_image.data.image.format){
00171     case Img::CF_RGB:
00172         {
00173             // RGB -> BGR
00174             char *dst = m_cvImage->imageData;
00175             for (unsigned int i=0; i<m_image.data.image.raw_data.length(); i+=3){
00176                 dst[i  ] = m_image.data.image.raw_data[i+2]; 
00177                 dst[i+1] = m_image.data.image.raw_data[i+1]; 
00178                 dst[i+2] = m_image.data.image.raw_data[i  ]; 
00179             }
00180             break;
00181         }
00182     case Img::CF_GRAY:
00183         memcpy(m_cvImage->imageData, 
00184                m_image.data.image.raw_data.get_buffer(),
00185                m_image.data.image.raw_data.length());
00186         break;
00187     default:
00188         break;
00189     }
00190     
00191     
00192     IplImage *dst_img = cvCloneImage (m_cvImage);
00193     cvUndistort2 (m_cvImage, dst_img, m_intrinsic, m_distortion);
00194 
00195     switch(m_image.data.image.format){
00196     case Img::CF_RGB:
00197         {
00198             // BGR -> RGB
00199             char *src = dst_img->imageData;
00200             for (unsigned int i=0; i<m_image.data.image.raw_data.length(); i+=3){
00201                 m_image.data.image.raw_data[i+2] = src[i  ]; 
00202                 m_image.data.image.raw_data[i+1] = src[i+1]; 
00203                 m_image.data.image.raw_data[i  ] = src[i+2]; 
00204             }
00205             break;
00206         }
00207     case Img::CF_GRAY:
00208         memcpy(m_image.data.image.raw_data.get_buffer(),
00209                dst_img->imageData,
00210                m_image.data.image.raw_data.length());
00211         break;
00212     default:
00213         break;
00214     }
00215 
00216     cvReleaseImage (&dst_img);
00217 
00218     m_imageOut.write();
00219 
00220     return RTC::RTC_OK;
00221 }
00222 
00223 /*
00224   RTC::ReturnCode_t UndistortImage::onAborting(RTC::UniqueId ec_id)
00225   {
00226   return RTC::RTC_OK;
00227   }
00228 */
00229 
00230 /*
00231   RTC::ReturnCode_t UndistortImage::onError(RTC::UniqueId ec_id)
00232   {
00233   return RTC::RTC_OK;
00234   }
00235 */
00236 
00237 /*
00238   RTC::ReturnCode_t UndistortImage::onReset(RTC::UniqueId ec_id)
00239   {
00240   return RTC::RTC_OK;
00241   }
00242 */
00243 
00244 /*
00245   RTC::ReturnCode_t UndistortImage::onStateUpdate(RTC::UniqueId ec_id)
00246   {
00247   return RTC::RTC_OK;
00248   }
00249 */
00250 
00251 /*
00252   RTC::ReturnCode_t UndistortImage::onRateChanged(RTC::UniqueId ec_id)
00253   {
00254   return RTC::RTC_OK;
00255   }
00256 */
00257 
00258 
00259 
00260 extern "C"
00261 {
00262 
00263     void UndistortImageInit(RTC::Manager* manager)
00264     {
00265         RTC::Properties profile(cameraimageviewercomponent_spec);
00266         manager->registerFactory(profile,
00267                                  RTC::Create<UndistortImage>,
00268                                  RTC::Delete<UndistortImage>);
00269     }
00270 
00271 };
00272 
00273 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56