diff --git a/CMakeLists.txt b/CMakeLists.txt
index cb21d372433..52d103bbaf8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -152,7 +152,10 @@ pkg_check_modules(OpenCV opencv)
 if (NOT OpenCV_FOUND)
   pkg_check_modules(OpenCV opencv-2.3.1)
   if (NOT OpenCV_FOUND)
-    message(WARNING "opencv not found")
+    pkg_check_modules(OpenCV opencv4)
+    if (NOT OpenCV_FOUND)
+      message(WARNING "opencv not found")
+    endif()
   endif()
 endif()
 pkg_check_modules(GLEW glew)
diff --git a/lib/util/GLsceneBase.h b/lib/util/GLsceneBase.h
index 34e5e086368..76913a388b7 100644
--- a/lib/util/GLsceneBase.h
+++ b/lib/util/GLsceneBase.h
@@ -6,8 +6,13 @@
 #include <map>
 #include <sys/time.h>
 //Open CV header
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
+#ifndef CV_VERSION_EPOCH
+  #if CV_VERSION_MAJOR > 3
+    #include <opencv2/videoio/videoio_c.h>
+  #endif
+#endif
 #include <SDL/SDL_thread.h>
 #include <hrpCorba/ModelLoader.hh>
 #include <hrpModel/ConstraintForceSolver.h>
diff --git a/rtc/CameraImageLoader/CameraImageLoader.cpp b/rtc/CameraImageLoader/CameraImageLoader.cpp
index ee3aed4b058..4b82726dca4 100644
--- a/rtc/CameraImageLoader/CameraImageLoader.cpp
+++ b/rtc/CameraImageLoader/CameraImageLoader.cpp
@@ -7,8 +7,14 @@
  * $Id$
  */
 
-#include <cv.h>
-#include <highgui.h>
+#ifndef CV_VERSION_EPOCH
+  #define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/imgcodecs.hpp>
+#else
+#include <opencv2/highgui/highgui.hpp>
+#endif
 #include "CameraImageLoader.h"
 
 // Module specification
@@ -116,26 +122,26 @@ RTC::ReturnCode_t CameraImageLoader::onExecute(RTC::UniqueId ec_id)
   std::string filename;
   std::cin >> filename;
 
-  IplImage *image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR);
-  if (!image) {
+  cv::Mat image = cv::imread(filename.c_str(), cv::IMREAD_COLOR);
+  if (image.empty()) {
       std::cerr << m_profile.instance_name << ": failed to load("
                 << filename << ")" << std::endl;
       return RTC::RTC_OK;
   }
 
-  m_image.data.image.width = image->width;
-  m_image.data.image.height = image->height;
-  m_image.data.image.raw_data.length(image->imageSize);
-  switch(image->nChannels){
+  m_image.data.image.width = image.size().width;
+  m_image.data.image.height = image.size().height;
+  m_image.data.image.raw_data.length(image.size().area());
+  switch(image.channels()){
   case 3:
       m_image.data.image.format = Img::CF_RGB;
       {
           // BGR -> RGB
-          char *src;
+          unsigned char *src;
           unsigned char *dst = m_image.data.image.raw_data.get_buffer();
-          for (int i=0; i<image->height; i++){
-              for (int j=0; j<image->width; j++){
-                  src = image->imageData + image->widthStep*i + j*3;
+          for (int i=0; i<image.size().height; i++){
+              for (int j=0; j<image.size().width; j++){
+                  src = image.data + image.step * i + j * 3;
                   dst[2] = src[0];
                   dst[1] = src[1];
                   dst[0] = src[2];
@@ -147,15 +153,13 @@ RTC::ReturnCode_t CameraImageLoader::onExecute(RTC::UniqueId ec_id)
   case 1:
       m_image.data.image.format = Img::CF_GRAY;
       memcpy(m_image.data.image.raw_data.get_buffer(),
-             image->imageData,
+             image.data,
              m_image.data.image.raw_data.length());
       break;
   default:
       break;
   }
   
-  cvReleaseImage (&image);
-  
   m_imageOut.write();
   
   return RTC::RTC_OK;
diff --git a/rtc/CameraImageSaver/CameraImageSaver.cpp b/rtc/CameraImageSaver/CameraImageSaver.cpp
index c7a8ca0d3ad..e246a5348d3 100644
--- a/rtc/CameraImageSaver/CameraImageSaver.cpp
+++ b/rtc/CameraImageSaver/CameraImageSaver.cpp
@@ -7,8 +7,16 @@
  * $Id$
  */
 
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
+#ifndef CV_VERSION_EPOCH
+  #define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/imgcodecs.hpp>
+#else
+#include <opencv2/highgui/highgui.hpp>
+#endif
 #include "CameraImageSaver.h"
 
 // Module specification
@@ -158,7 +166,8 @@ RTC::ReturnCode_t CameraImageSaver::onExecute(RTC::UniqueId ec_id)
 
     char fname[256];
     sprintf(fname, "%s%04d.png", m_basename.c_str(), m_count++);
-    cvSaveImage(fname, cvImage);
+    cv::Mat image = cv::cvarrToMat(cvImage);
+    cv::imwrite(fname, image);
     
     cvReleaseImage(&cvImage);
   }
diff --git a/rtc/CameraImageViewer/CameraImageViewer.h b/rtc/CameraImageViewer/CameraImageViewer.h
index 229d816410f..ff05ae16a5c 100644
--- a/rtc/CameraImageViewer/CameraImageViewer.h
+++ b/rtc/CameraImageViewer/CameraImageViewer.h
@@ -21,8 +21,8 @@
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include <rtm/idl/InterfaceDataTypesSkel.h>
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
diff --git a/rtc/ColorExtractor/ColorExtractor.cpp b/rtc/ColorExtractor/ColorExtractor.cpp
index 3b66d19adba..90fa858141d 100644
--- a/rtc/ColorExtractor/ColorExtractor.cpp
+++ b/rtc/ColorExtractor/ColorExtractor.cpp
@@ -7,7 +7,11 @@
  * $Id$
  */
 
-#include <highgui.h>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
+#ifndef CV_RGB
+#define CV_RGB( r, g, b )  cvScalar( (b), (g), (r), 0 )
+#endif
 #include "ColorExtractor.h"
 #include "hrpsys/util/VectorConvert.h"
 
diff --git a/rtc/ColorExtractor/ColorExtractor.h b/rtc/ColorExtractor/ColorExtractor.h
index a0dfb4e1c91..a858e5d89b1 100644
--- a/rtc/ColorExtractor/ColorExtractor.h
+++ b/rtc/ColorExtractor/ColorExtractor.h
@@ -19,7 +19,7 @@
 #include <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
-#include <cv.h>
+#include <opencv2/core/core_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
diff --git a/rtc/JpegDecoder/JpegDecoder.cpp b/rtc/JpegDecoder/JpegDecoder.cpp
index c67b0a4ee18..5ce4ac9e30a 100644
--- a/rtc/JpegDecoder/JpegDecoder.cpp
+++ b/rtc/JpegDecoder/JpegDecoder.cpp
@@ -9,6 +9,12 @@
 
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/core/core.hpp>
+#ifndef CV_VERSION_EPOCH
+#define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/legacy/constants_c.h>
+#endif
 #include "JpegDecoder.h"
 
 // Module specification
diff --git a/rtc/JpegEncoder/JpegEncoder.cpp b/rtc/JpegEncoder/JpegEncoder.cpp
index cf9fd0a69ee..42b6b194213 100644
--- a/rtc/JpegEncoder/JpegEncoder.cpp
+++ b/rtc/JpegEncoder/JpegEncoder.cpp
@@ -10,6 +10,12 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/core/core.hpp>
+#ifndef CV_VERSION_EPOCH
+#define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/legacy/constants_c.h>
+#endif
 #include "JpegEncoder.h"
 
 // Module specification
diff --git a/rtc/RGB2Gray/RGB2Gray.cpp b/rtc/RGB2Gray/RGB2Gray.cpp
index d01b1df4ac7..f7f49cb6e11 100644
--- a/rtc/RGB2Gray/RGB2Gray.cpp
+++ b/rtc/RGB2Gray/RGB2Gray.cpp
@@ -10,6 +10,7 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/types_c.h>
 #include "RGB2Gray.h"
 
 // Module specification
diff --git a/rtc/RangeDataViewer/RangeDataViewer.h b/rtc/RangeDataViewer/RangeDataViewer.h
index fbc25998442..6e830404c87 100644
--- a/rtc/RangeDataViewer/RangeDataViewer.h
+++ b/rtc/RangeDataViewer/RangeDataViewer.h
@@ -19,8 +19,8 @@
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include <rtm/idl/InterfaceDataTypesSkel.h>
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
diff --git a/rtc/ResizeImage/ResizeImage.cpp b/rtc/ResizeImage/ResizeImage.cpp
index 6eb73ea013f..a6685d5d9ea 100644
--- a/rtc/ResizeImage/ResizeImage.cpp
+++ b/rtc/ResizeImage/ResizeImage.cpp
@@ -7,7 +7,8 @@
  * $Id$
  */
 
-#include <highgui.h>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
 #include "ResizeImage.h"
 
 // Module specification
diff --git a/rtc/ResizeImage/ResizeImage.h b/rtc/ResizeImage/ResizeImage.h
index 1251382aece..b895a162a88 100644
--- a/rtc/ResizeImage/ResizeImage.h
+++ b/rtc/ResizeImage/ResizeImage.h
@@ -18,7 +18,7 @@
 #include <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
-#include <cv.h>
+#include <opencv2/core/core_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
diff --git a/rtc/RotateImage/RotateImage.cpp b/rtc/RotateImage/RotateImage.cpp
index e3328a6c96d..23ebdccd05a 100644
--- a/rtc/RotateImage/RotateImage.cpp
+++ b/rtc/RotateImage/RotateImage.cpp
@@ -7,7 +7,8 @@
  * $Id$
  */
 
-#include <highgui.h>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
 #include "RotateImage.h"
 
 // Module specification
diff --git a/rtc/RotateImage/RotateImage.h b/rtc/RotateImage/RotateImage.h
index 288fae83179..ba4ec3654f0 100644
--- a/rtc/RotateImage/RotateImage.h
+++ b/rtc/RotateImage/RotateImage.h
@@ -18,7 +18,7 @@
 #include <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
-#include <cv.h>
+#include <opencv2/core/core_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
diff --git a/rtc/UndistortImage/UndistortImage.cpp b/rtc/UndistortImage/UndistortImage.cpp
index abedbe6adcb..e2bf0e50405 100644
--- a/rtc/UndistortImage/UndistortImage.cpp
+++ b/rtc/UndistortImage/UndistortImage.cpp
@@ -9,6 +9,10 @@
 
 #include "UndistortImage.h"
 
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
 // Module specification
 // <rtc-template block="module_spec">
 static const char* cameraimageviewercomponent_spec[] =
@@ -37,8 +41,6 @@ UndistortImage::UndistortImage(RTC::Manager* manager)
       m_imageOut("imageOut", m_image),
       // </rtc-template>
       m_cvImage(NULL),
-      m_intrinsic(NULL),
-      m_distortion(NULL),
       dummy(0)
 {
 }
@@ -106,18 +108,16 @@ RTC::ReturnCode_t UndistortImage::onActivated(RTC::UniqueId ec_id)
 {
     std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
 
-    CvFileStorage *fs 
-        = cvOpenFileStorage (m_calibFile.c_str(), 0, CV_STORAGE_READ);
-    if (!fs){
+    cv::FileStorage fs(m_calibFile.c_str(), cv::FileStorage::READ);
+    if (!fs.isOpened()){
         std::cerr << m_profile.instance_name << ": can't open "
                   << m_calibFile << std::endl;
         return RTC::RTC_ERROR;
     }
-    CvFileNode *param = cvGetFileNodeByName (fs, NULL, "intrinsic");
-    m_intrinsic = (CvMat *) cvRead (fs, param);
-    param = cvGetFileNodeByName (fs, NULL, "distortion");
-    m_distortion = (CvMat *) cvRead (fs, param);
-    cvReleaseFileStorage (&fs);
+    cv::FileNode param = fs["intrinsic"];
+    param >> m_intrinsic;
+    param = fs["distortion"];
+    param >> m_distortion;
 
     return RTC::RTC_OK;
 }
@@ -129,9 +129,7 @@ RTC::ReturnCode_t UndistortImage::onDeactivated(RTC::UniqueId ec_id)
         cvReleaseImage(&m_cvImage);
         m_cvImage = NULL;
     }
-    if (m_intrinsic) cvReleaseMat (&m_intrinsic);
-    if (m_distortion) cvReleaseMat (&m_distortion);
-    
+
     return RTC::RTC_OK;
 }
 
@@ -189,14 +187,16 @@ RTC::ReturnCode_t UndistortImage::onExecute(RTC::UniqueId ec_id)
     }
     
     
-    IplImage *dst_img = cvCloneImage (m_cvImage);
-    cvUndistort2 (m_cvImage, dst_img, m_intrinsic, m_distortion);
+    cv::Mat src_img = cv::cvarrToMat(m_cvImage);
+    cv::Mat dst_img;
+    src_img.copyTo(dst_img);
+    cv::undistort (src_img, dst_img, m_intrinsic, m_distortion);
 
     switch(m_image.data.image.format){
     case Img::CF_RGB:
         {
             // BGR -> RGB
-            char *src = dst_img->imageData;
+            unsigned char *src = dst_img.data;
             for (unsigned int i=0; i<m_image.data.image.raw_data.length(); i+=3){
                 m_image.data.image.raw_data[i+2] = src[i  ]; 
                 m_image.data.image.raw_data[i+1] = src[i+1]; 
@@ -206,15 +206,13 @@ RTC::ReturnCode_t UndistortImage::onExecute(RTC::UniqueId ec_id)
         }
     case Img::CF_GRAY:
         memcpy(m_image.data.image.raw_data.get_buffer(),
-               dst_img->imageData,
+               dst_img.data,
                m_image.data.image.raw_data.length());
         break;
     default:
         break;
     }
 
-    cvReleaseImage (&dst_img);
-
     m_imageOut.write();
 
     return RTC::RTC_OK;
diff --git a/rtc/UndistortImage/UndistortImage.h b/rtc/UndistortImage/UndistortImage.h
index 03c6e88b24e..edac8e53045 100644
--- a/rtc/UndistortImage/UndistortImage.h
+++ b/rtc/UndistortImage/UndistortImage.h
@@ -18,8 +18,9 @@
 #include <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
@@ -137,7 +138,7 @@ class UndistortImage
  private:
     IplImage* m_cvImage;
     std::string m_calibFile;
-    CvMat *m_intrinsic, *m_distortion;
+    cv::Mat m_intrinsic, m_distortion;
     int dummy;
 };
 
diff --git a/rtc/VideoCapture/camera.h b/rtc/VideoCapture/camera.h
index 4ea2b8d7682..68aab3ea786 100644
--- a/rtc/VideoCapture/camera.h
+++ b/rtc/VideoCapture/camera.h
@@ -1,7 +1,6 @@
 
 /* Most of capture.cpp and capture.h are copied from http://jsk-enshu.svn.sourceforge.net/viewvc/jsk-enshu/trunk/keisanki/2009/ */
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <opencv2/core/core.hpp>
 #include <linux/videodev2.h>
 #include <fcntl.h>
 #include <errno.h>
diff --git a/rtc/VirtualCamera/VirtualCamera.h b/rtc/VirtualCamera/VirtualCamera.h
index a78eeb17956..01e895f1b07 100644
--- a/rtc/VirtualCamera/VirtualCamera.h
+++ b/rtc/VirtualCamera/VirtualCamera.h
@@ -22,8 +22,8 @@
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 //Open CV headder
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
 //
 #include "hrpsys/util/LogManager.h"
 #include "hrpsys/util/SDLUtil.h"
diff --git a/util/simulator/CMakeLists.txt b/util/simulator/CMakeLists.txt
index 1f6c0c607b7..beb53389d08 100644
--- a/util/simulator/CMakeLists.txt
+++ b/util/simulator/CMakeLists.txt
@@ -32,19 +32,12 @@ add_library(hrpsysext SHARED
   PyShape.cpp
   )
 
-if (NOT APPLE)
-   target_link_libraries(hrpsysext
-     boost_python
-     hrpsysUtil
-     ${PYTHON_LIBRARIES}
-     )
-else()
-   target_link_libraries(hrpsysext 
-     boost_python27-mt
-     hrpsysUtil
-     ${PYTHON_LIBRARIES}
-     )
-endif()
+find_package(Boost REQUIRED COMPONENTS python)
+target_link_libraries(hrpsysext
+  Boost::python
+  hrpsysUtil
+  ${PYTHON_LIBRARIES}
+  )
 
 set_target_properties(hrpsysext PROPERTIES PREFIX "")
 set_target_properties(hrpsysext PROPERTIES SUFFIX ".so")