Ceres optimization for 3d + all libs in 1 CMake variable

8e13c82a · gineshidalgo99 · 27903fb3 · 8e13c82a · 8e13c82a · 8e13c82a
18 changed file
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -37,7 +37,7 @@ if (WIN32)
  # sdl flags causes error -- error : unknown attribute \"guard\"

  set(CMAKE_CONFIGURATION_TYPES Debug Release CACHE TYPE INTERNAL FORCE)
-  
+
  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP")
  string(REPLACE "/W3" "/W4" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
  string(REPLACE "/GR" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
@@ -47,7 +47,7 @@ if (WIN32)
  set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Ot /Oi /Gy /Z7")

  set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /LTCG:incremental /OPT:REF /OPT:ICF")
-  
+
  string(REPLACE "/MDd" "/MD" CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG}")
  string(REPLACE "/Zi" "/Z7" CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG}")
  string(REPLACE "/RTC1" "" CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG}")
@@ -180,12 +180,15 @@ endif (${GPU_MODE} MATCHES "CUDA")

 # Suboptions for OpenPose 3D Reconstruction demo
 option(WITH_3D_RENDERER "Add OpenPose 3D renderer module (it requires FreeGLUT library)." OFF)
+if (UNIX AND NOT APPLE)
+  option(WITH_CERES "Add Ceres support. Used e.g., for advanced 3-D reconstruction." OFF)
+endif (UNIX AND NOT APPLE)
 option(WITH_FLIR_CAMERA "Add FLIR (formerly Point Grey) camera code (requires Spinnaker SDK already installed)." OFF)

 # Download the models
 option(DOWNLOAD_COCO_MODEL "Download COCO model." ON)
 option(DOWNLOAD_MPI_MODEL "Download MPI model." OFF)
-option(DOWNLOAD_HAND_MODEL "Download hand model." ON)  
+option(DOWNLOAD_HAND_MODEL "Download hand model." ON)
 option(DOWNLOAD_FACE_MODEL "Download face model." ON)

 # More options
@@ -202,8 +205,6 @@ option(PROFILER_ENABLED "If enabled, OpenPose will be able to print out speed in
 if (${GPU_MODE} MATCHES "OPENCL")
  unset(CMAKE_THREAD_LIBS_INIT CACHE)
  find_package(Threads)
-else ()
-  set(CMAKE_THREAD_LIBS_INIT "")
 endif ()

 ### FIND REQUIRED PACKAGES
@@ -218,6 +219,9 @@ if (WITH_3D_RENDERER)
  # OpenPose flags
  add_definitions(-DWITH_3D_RENDERER)
 endif (WITH_3D_RENDERER)
+if (WITH_CERES)
+  add_definitions(-DWITH_CERES)
+endif (WITH_CERES)
 if (WITH_FLIR_CAMERA)
  # OpenPose flags
  add_definitions(-DWITH_FLIR_CAMERA)
@@ -244,6 +248,12 @@ if (UNIX AND NOT APPLE)
    # GLUT
    find_package(GLUT REQUIRED)
  endif (WITH_3D_RENDERER)
+  if (WITH_CERES)
+    # Eigen + Ceres
+    find_package(PkgConfig)
+    pkg_check_modules(EIGEN3 REQUIRED eigen3)
+    find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
+  endif (WITH_CERES)
  if (WITH_FLIR_CAMERA)
    # Spinnaker
    find_package(Spinnaker)
@@ -251,9 +261,6 @@ if (UNIX AND NOT APPLE)
      message(FATAL_ERROR "Spinnaker not found. Either turn off the `WITH_FLIR_CAMERA` option or specify the path to
        the Spinnaker includes and libs.")
    endif (NOT SPINNAKER_FOUND)
-  # Otherwise error if 3D module enabled, and then disabled (with Spinnaker not installed)
-  else (WITH_FLIR_CAMERA)
-    set(SPINNAKER_LIB "")
  endif (WITH_FLIR_CAMERA)

  # OpenMP
@@ -359,9 +366,6 @@ if (WIN32)
    unset(BOOST_SYSTEM_LIB_DEBUG CACHE)
    find_library(BOOST_SYSTEM_LIB_RELEASE boost_system-vc140-mt-1_61 HINTS ${FIND_LIB_PREFIX}/caffe3rdparty/lib)
    find_library(BOOST_SYSTEM_LIB_DEBUG boost_system-vc140-mt-gd-1_61 HINTS ${FIND_LIB_PREFIX}/caffe3rdparty/lib)
-  else ()
-    set(BOOST_SYSTEM_LIB_RELEASE "")
-    set(BOOST_SYSTEM_LIB_DEBUG "")
  endif ()

  if (WITH_3D_RENDERER)
@@ -507,7 +511,7 @@ if (UNIX AND NOT APPLE)
      if (NOT Caffe_FOUND)
        add_custom_command(TARGET openpose_caffe
            POST_BUILD
-            COMMAND ${CMAKE_COMMAND} ${CMAKE_SOURCE_DIR} 
+            COMMAND ${CMAKE_COMMAND} ${CMAKE_SOURCE_DIR}
            COMMAND $(MAKE)
            WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
            COMMENT "Rerunning cmake after building Caffe submodule")
@@ -554,6 +558,10 @@ endif ()
 if (WITH_3D_RENDERER)
  include_directories(${GLUT_INCLUDE_DIRS})
 endif (WITH_3D_RENDERER)
+if (WITH_CERES)
+  include_directories(${CERES_INCLUDE_DIRS})
+  include_directories(${EIGEN3_INCLUDE_DIRS})
+endif (WITH_CERES)
 if (WITH_FLIR_CAMERA)
  include_directories(${SPINNAKER_INCLUDE_DIRS})
 endif (WITH_FLIR_CAMERA)
@@ -565,6 +573,44 @@ if (WIN32)
 endif (WIN32)


+### COLLECT ALL 3RD-PARTY LIBRARIES TO BE LINKED AGAINST
+set(OpenPose_3rdparty_libraries ${OpenCV_LIBS} ${GLOG_LIBRARY})
+if (UNIX AND NOT APPLE)
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${GLOG_LIBRARY})
+elseif (WIN32)
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries}
+      debug ${GFLAGS_LIBRARY_DEBUG} optimized ${GFLAGS_LIBRARY_RELEASE}
+      debug ${GLOG_LIBRARY_DEBUG} optimized ${GLOG_LIBRARY_RELEASE})
+endif (UNIX AND NOT APPLE)
+# Deep net Framework
+if (${DL_FRAMEWORK} MATCHES "CAFFE")
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${Caffe_LIBS} ${GFLAGS_LIBRARY})
+endif (${DL_FRAMEWORK} MATCHES "CAFFE")
+# CPU vs. GPU
+if (USE_MKL)
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${MKL_LIBS})
+endif (USE_MKL)
+if (${GPU_MODE} MATCHES "OPENCL")
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${CMAKE_THREAD_LIBS_INIT} ${OpenCL_LIBRARIES})
+  if (WIN32)
+    set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries}
+        debug ${BOOST_SYSTEM_LIB_DEBUG} optimized ${BOOST_SYSTEM_LIB_RELEASE})
+  endif (WIN32)
+endif (${GPU_MODE} MATCHES "OPENCL")
+# 3-D
+if (WITH_3D_RENDERER)
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${GLUT_LIBRARY})
+endif (WITH_3D_RENDERER)
+if (WITH_CERES)
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${CERES_LIBRARIES})
+endif (WITH_CERES)
+if (WITH_FLIR_CAMERA)
+  set(OpenPose_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${SPINNAKER_LIB})
+endif (WITH_FLIR_CAMERA)
+
+set(examples_3rdparty_libraries ${OpenPose_3rdparty_libraries} ${GFLAGS_LIBRARY})
+
+
 ### ADD SUBDIRECTORIES
 if (Caffe_FOUND)
  add_subdirectory(src)
@@ -609,7 +655,7 @@ if (UNIX AND NOT APPLE)
        COMMENT "Generating API documentation with Doxygen"
        VERBATIM)
    else (DOXYGEN_FOUND)
-      message(FATAL_ERROR "Doxygen need to be installed to generate the doxygen documentation.")
+      message(FATAL_ERROR "Doxygen needs to be installed to generate the doxygen documentation.")
    endif (DOXYGEN_FOUND)
  endif (BUILD_DOCS)


--- a/doc/3d_reconstruction_demo.md
+++ b/doc/3d_reconstruction_demo.md
@@ -3,15 +3,16 @@

 ## Contents
 1. [Introduction](#introduction)
-2. [Features](#features)
-3. [Required Hardware](#required-hardware)
-4. [Camera Calibration](#camera-calibration)
-5. [Camera Ordering](#camera-ordering)
-6. [Installing the OpenPose 3-D Reconstruction Module](#installing-the-openpose-3-d-reconstruction-module)
-7. [Quick Start](#quick-start)
-8. [Expected Visual Results](#expected-visual-results)
-9. [Using a Different Camera Brand](#using-a-different-camera-brand)
-10. [Known Bug](#known-bug)
+2. [Installing the OpenPose 3-D Reconstruction Module](#installing-the-openpose-3-d-reconstruction-module)
+3. [Non Linear Optimization](#non-linear-optimization)
+4. [Features](#features)
+5. [Required Hardware](#required-hardware)
+6. [Camera Calibration](#camera-calibration)
+7. [Camera Ordering](#camera-ordering)
+8. [Quick Start](#quick-start)
+9. [Expected Visual Results](#expected-visual-results)
+10. [Using a Different Camera Brand](#using-a-different-camera-brand)
+11. [Known Bug](#known-bug)



@@ -20,6 +21,18 @@ This experimental module performs 3-D keypoint (body, face, and hand) reconstruc



+
+## Installing the OpenPose 3-D Reconstruction Module
+Check [doc/installation.md#3d-reconstruction-module](./installation.md#3d-reconstruction-module) for installation steps.
+
+
+
+
+## Non Linear Optimization
+In order to increase the 3-D reconstruction accuracy, OpenPose optionally performs non-linear optimization if Ceres solver support is enabled (only available in Ubuntu for now). To enable it, check [doc/installation.md#3d-reconstruction-module](./installation.md#3d-reconstruction-module) for more details.
+
+
+
 ## Features
 - Auto detection of all FLIR cameras connected to your machine, and image streaming from all of them.
 - Hardware trigger and buffer `NewestFirstOverwrite` modes enabled. Hence, the algorithm will always get the last synchronized frame from each camera, deleting the rest.
@@ -72,12 +85,6 @@ In order to verify that the camera parameters introduced by the user are sorted



-
-## Installing the OpenPose 3-D Reconstruction Module
-Check the [doc/installation.md#3d-reconstruction-module](./installation.md#3d-reconstruction-module) for installation steps.
-
-
-
 ## Quick Start
 Check the [doc/quick_start.md#3-d-reconstruction](./quick_start.md#3-d-reconstruction) for basic examples.


--- a/doc/installation.md
+++ b/doc/installation.md
@@ -270,6 +270,7 @@ You can include the 3D reconstruction module by:
                - Copy `{freeglutParentDirectory}\freeglut\include\` as `{OpenPoseDirectory}\3rdparty\windows\freeglut\include\`.
                - Copy `{freeglutParentDirectory}\freeglut\lib\x64\` as `{OpenPoseDirectory}\3rdparty\windows\freeglut\lib\`.
 3. Follow the CMake installation steps. In addition, set the `WITH_FLIR_CAMERA` (only if Spinnaker was installed) and `WITH_3D_RENDERER` options.
+4. Increased accuracy with Ceres solver (Ubuntu only): For extra 3-D reconstruction accuracy, run `sudo apt-get install libeigen3-dev`, install [Ceres solver](http://ceres-solver.org/installation.html), and enable `WITH_CERES` in CMake when installing OpenPose. Ceres is harder to install in Windows, so we have not tested it so far in there. Feel free to make a pull request if you do.

 After installation, check the [doc/3d_reconstruction_demo.md](./3d_reconstruction_demo.md) instructions.


--- a/doc/release_notes.md
+++ b/doc/release_notes.md
@@ -214,6 +214,8 @@ OpenPose Library - Release Notes
 ## Current version - future OpenPose 1.3.1
 1. Main improvements:
    1. Flir cameras: Added software trigger and a dedicated thread to keep reading images to remove latency (analogously to webcamReader).
+    2. 3-D reconstruction: Added non-linear minimization to further improve 3-D triangulation accuracy by ~5% (Ubuntu only).
+    2. CMake: All libraries as single variable (simpler to add/remove libraries).
 2. Functions or parameters renamed:
    1. Removed scale parameter from hand and face rectangle extractor (causing wrong results if custom `--output_resolution`).
 3. Main bugs fixed:

--- a/examples/openpose/CMakeLists.txt
+++ b/examples/openpose/CMakeLists.txt
@@ -4,7 +4,7 @@ set(EXAMPLE_FILES
 foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  get_filename_component(SOURCE_NAME ${EXAMPLE_FILE} NAME_WE)
-  
+
  if (UNIX AND NOT APPLE)
    set(EXE_NAME "${SOURCE_NAME}.bin")
  elseif (WIN32)
@@ -13,8 +13,8 @@ foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  message(STATUS "Adding Example ${EXE_NAME}")
  add_executable(${EXE_NAME} ${EXAMPLE_FILE})
-  target_link_libraries( ${EXE_NAME} openpose ${GLOG_LIBRARY} ${GFLAGS_LIBRARY} ${Caffe_LIBS} ${MKL_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB} ${OpenCL_LIBRARIES})
-  
+  target_link_libraries(${EXE_NAME} openpose ${examples_3rdparty_libraries})
+
  if (WIN32)
    set_property(TARGET ${EXE_NAME} PROPERTY FOLDER "Examples")
    configure_file(${CMAKE_SOURCE_DIR}/cmake/OpenPose${CL}.vcxproj.user

--- a/examples/tests/CMakeLists.txt
+++ b/examples/tests/CMakeLists.txt
@@ -5,7 +5,7 @@ set(EXAMPLE_FILES
 foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  get_filename_component(SOURCE_NAME ${EXAMPLE_FILE} NAME_WE)
-  
+
  if (UNIX AND NOT APPLE)
    set(EXE_NAME "${SOURCE_NAME}.bin")
  elseif (WIN32)
@@ -14,8 +14,8 @@ foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  message(STATUS "Adding Example ${EXE_NAME}")
  add_executable(${EXE_NAME} ${EXAMPLE_FILE})
-  target_link_libraries( ${EXE_NAME} openpose ${GLOG_LIBRARY} ${GFLAGS_LIBRARY} ${Caffe_LIBS} ${MKL_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB} ${OpenCL_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
-  
+  target_link_libraries(${EXE_NAME} openpose ${examples_3rdparty_libraries})
+
  if (WIN32)
    set_property(TARGET ${EXE_NAME} PROPERTY FOLDER "Examples/Tutorial/Tests")
    configure_file(${CMAKE_SOURCE_DIR}/cmake/OpenPose${CL}.vcxproj.user
@@ -26,4 +26,3 @@ foreach(EXAMPLE_FILE ${EXAMPLE_FILES})
  endif (WIN32)

 endforeach()
-
--- a/examples/tutorial_add_module/CMakeLists.txt
+++ b/examples/tutorial_add_module/CMakeLists.txt
@@ -15,8 +15,7 @@ foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  message(STATUS "Adding Example ${EXE_NAME}")
  add_executable(${EXE_NAME} ${EXAMPLE_FILE})
-  target_link_libraries( ${EXE_NAME} openpose ${GLOG_LIBRARY} ${GFLAGS_LIBRARY}
-      ${Caffe_LIBS} ${MKL_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB} ${CMAKE_THREAD_LIBS_INIT})
+  target_link_libraries(${EXE_NAME} openpose ${examples_3rdparty_libraries})

  if (WIN32)
    set_property(TARGET ${EXE_NAME} PROPERTY FOLDER "Examples/Tutorial/AddModule")

--- a/examples/tutorial_pose/CMakeLists.txt
+++ b/examples/tutorial_pose/CMakeLists.txt
@@ -5,7 +5,7 @@ set(EXAMPLE_FILES
 foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  get_filename_component(SOURCE_NAME ${EXAMPLE_FILE} NAME_WE)
-  
+
  if (UNIX AND NOT APPLE)
    set(EXE_NAME "${SOURCE_NAME}.bin")
  elseif (WIN32)
@@ -14,8 +14,8 @@ foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  message(STATUS "Adding Example ${EXE_NAME}")
  add_executable(${EXE_NAME} ${EXAMPLE_FILE})
-  target_link_libraries(${EXE_NAME} openpose ${GLOG_LIBRARY} ${GFLAGS_LIBRARY} ${Caffe_LIBS} ${MKL_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB} ${OpenCL_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
-  
+  target_link_libraries(${EXE_NAME} openpose ${examples_3rdparty_libraries})
+
  if (WIN32)
    set_property(TARGET ${EXE_NAME} PROPERTY FOLDER "Examples/Tutorial/Pose")
    configure_file(${CMAKE_SOURCE_DIR}/cmake/OpenPose${CL}.vcxproj.user

--- a/examples/tutorial_thread/CMakeLists.txt
+++ b/examples/tutorial_thread/CMakeLists.txt
@@ -7,7 +7,7 @@ set(EXAMPLE_FILES
 foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  get_filename_component(SOURCE_NAME ${EXAMPLE_FILE} NAME_WE)
-  
+
  if (UNIX AND NOT APPLE)
    set(EXE_NAME "${SOURCE_NAME}.bin")
  elseif (WIN32)
@@ -16,7 +16,7 @@ foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  message(STATUS "Adding Example ${EXE_NAME}")
  add_executable(${EXE_NAME} ${EXAMPLE_FILE})
-  target_link_libraries( ${EXE_NAME} openpose ${GLOG_LIBRARY} ${GFLAGS_LIBRARY} ${Caffe_LIBS} ${MKL_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB} ${CMAKE_THREAD_LIBS_INIT})
+  target_link_libraries(${EXE_NAME} openpose ${examples_3rdparty_libraries})

  if (WIN32)
    set_property(TARGET ${EXE_NAME} PROPERTY FOLDER "Examples/Tutorial/Thread")

--- a/examples/tutorial_wrapper/CMakeLists.txt
+++ b/examples/tutorial_wrapper/CMakeLists.txt
@@ -8,7 +8,7 @@ include(${CMAKE_SOURCE_DIR}/cmake/Utils.cmake)
 foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  get_filename_component(SOURCE_NAME ${EXAMPLE_FILE} NAME_WE)
-  
+
  if (UNIX AND NOT APPLE)
    set(EXE_NAME "${SOURCE_NAME}.bin")
  elseif (WIN32)
@@ -17,8 +17,8 @@ foreach(EXAMPLE_FILE ${EXAMPLE_FILES})

  message(STATUS "Adding Example ${EXE_NAME}")
  add_executable(${EXE_NAME} ${EXAMPLE_FILE})
-  target_link_libraries( ${EXE_NAME} openpose ${GLOG_LIBRARY} ${GFLAGS_LIBRARY} ${Caffe_LIBS} ${MKL_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB} ${CMAKE_THREAD_LIBS_INIT})
-  
+  target_link_libraries(${EXE_NAME} openpose ${examples_3rdparty_libraries})
+
  if (WIN32)
    set_property(TARGET ${EXE_NAME} PROPERTY FOLDER "Examples/Tutorial/Wrapper")
    configure_file(${CMAKE_SOURCE_DIR}/cmake/OpenPose${CL}.vcxproj.user

--- a/src/openpose/3d/poseTriangulation.cpp
+++ b/src/openpose/3d/poseTriangulation.cpp
+#ifdef WITH_CERES
+    #include <ceres/ceres.h>
+    #include <ceres/rotation.h>
+#endif
 #include <opencv2/calib3d/calib3d.hpp>
 #include <openpose/3d/poseTriangulation.hpp>

 namespace op
 {
-    double calcReprojectionError(const cv::Mat& reconstructedPoint, const std::vector<cv::Mat>& M,
-                                 const std::vector<cv::Point2d>& points2d)
+    double calcReprojectionError(const cv::Mat& reconstructedPoint, const std::vector<cv::Mat>& cameraMatrices,
+                                 const std::vector<cv::Point2d>& pointsOnEachCamera)
    {
        try
        {
            auto averageError = 0.;
-            for (auto i = 0u ; i < M.size() ; i++)
+            for (auto i = 0u ; i < cameraMatrices.size() ; i++)
            {
-                cv::Mat imageX = M[i] * reconstructedPoint;
+                cv::Mat imageX = cameraMatrices[i] * reconstructedPoint;
                imageX /= imageX.at<double>(2,0);
-                const auto error = std::sqrt(std::pow(imageX.at<double>(0,0) -  points2d[i].x,2)
-                                             + std::pow(imageX.at<double>(1,0) - points2d[i].y,2));
+                const auto error = std::sqrt(std::pow(imageX.at<double>(0,0) -  pointsOnEachCamera[i].x,2)
+                                             + std::pow(imageX.at<double>(1,0) - pointsOnEachCamera[i].y,2));
                // log("Error: " + std::to_string(error));
                averageError += error;
            }
-            return averageError / M.size();
+            return averageError / cameraMatrices.size();
        }
        catch (const std::exception& e)
        {
@@ -27,8 +31,96 @@ namespace op
        }
    }

-    void triangulate(cv::Mat& reconstructedPoint, const std::vector<cv::Mat>& cameraMatrices,
-                     const std::vector<cv::Point2d>& pointsOnEachCamera)
+    #ifdef WITH_CERES
+        // Nonlinear Optimization for 3D Triangulation
+        struct ReprojectionErrorForTriangulation
+        {
+            ReprojectionErrorForTriangulation(double x,double y,double* param)
+            {
+                observed_x = x;
+                observed_y = y;
+                memcpy(camParam,param,sizeof(double)*12);
+            }
+
+            template <typename T>
+            bool operator()(const T* const pt,
+                            T* residuals) const ;
+
+            inline virtual bool Evaluate(double const* const* pt,
+                                         double* residuals,
+                                         double** jacobians) const;
+
+            double observed_x;
+            double observed_y;
+            double camParam[12];
+        };
+
+        template <typename T>
+        bool ReprojectionErrorForTriangulation::operator()(const T* const pt,
+                                                           T* residuals) const
+        {
+            try
+            {
+                T predicted[3];
+                predicted[0] = T(camParam[0])*pt[0]+ T(camParam[1])*pt[1] + T(camParam[2])*pt[2] + T(camParam[3]);
+                predicted[1] = T(camParam[4])*pt[0]+ T(camParam[5])*pt[1] + T(camParam[6])*pt[2] + T(camParam[7]);
+                predicted[2] = T(camParam[8])*pt[0]+ T(camParam[9])*pt[1] + T(camParam[10])*pt[2] + T(camParam[11]);
+
+                predicted[0] = predicted[0]/predicted[2];
+                predicted[1] = predicted[1]/predicted[2];
+
+                residuals[0] = T(observed_x) - predicted[0];
+                residuals[1] = T(observed_y) - predicted[1];
+
+                // residuals[0] = T(pow(predicted[0] - observed_x,2) + pow(predicted[1] - observed_y,2));
+                // residuals[0] = -pow(predicted[0] - T(observed_x),2);
+                // residuals[1] = -pow(predicted[1] - T(observed_y),2);
+
+                return  true;
+            }
+            catch (const std::exception& e)
+            {
+                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+                return false;
+            }
+        }
+
+        bool ReprojectionErrorForTriangulation::Evaluate(double const* const* pt,
+            double* residuals,
+            double** jacobians) const
+        {
+            try
+            {
+                UNUSED(jacobians);
+
+                double predicted[3];
+                predicted[0] = camParam[0]*pt[0][0]+ camParam[1]*pt[0][1] + camParam[2]*pt[0][2] + camParam[3];
+                predicted[1] = camParam[4]*pt[0][0]+ camParam[5]*pt[0][1] + camParam[6]*pt[0][2] + camParam[7];
+                predicted[2] = camParam[8]*pt[0][0]+ camParam[9]*pt[0][1] + camParam[10]*pt[0][2] + camParam[11];
+
+                predicted[0] /= predicted[2];
+                predicted[1] /= predicted[2];
+
+                //residuals[0] = predicted[0] - observed_x;
+                //residuals[1] = predicted[1] - observed_y;
+
+                residuals[0] = std::sqrt(std::pow(predicted[0] - observed_x,2) + std::pow(predicted[1] - observed_y,2));
+                // log("Residuals:");
+                // residuals[0]= pow(predicted[0] - (observed_x),2);
+                // residuals[1]= pow(predicted[1] - (observed_y),2);
+
+                return true;
+            }
+            catch (const std::exception& e)
+            {
+                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+                return false;
+            }
+        }
+    #endif
+
+    double triangulate(cv::Mat& reconstructedPoint, const std::vector<cv::Mat>& cameraMatrices,
+                       const std::vector<cv::Point2d>& pointsOnEachCamera)
    {
        try
        {
@@ -55,30 +147,71 @@ namespace op
            cv::SVD svd{A};
            svd.solveZ(A,reconstructedPoint);
            reconstructedPoint /= reconstructedPoint.at<double>(3);
+
+            return calcReprojectionError(reconstructedPoint, cameraMatrices, pointsOnEachCamera);
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return -1.;
        }
    }

-    // TODO: ask for the missing function: TriangulationOptimization
    double triangulateWithOptimization(cv::Mat& reconstructedPoint, const std::vector<cv::Mat>& cameraMatrices,
                                       const std::vector<cv::Point2d>& pointsOnEachCamera)
    {
        try
        {
+            // Information for 3 cameras:
+            //     - Speed: triangulate ~0.01 ms vs. optimization ~0.2 ms
+            //     - Accuracy: initial reprojection error ~14-21, reduced ~5% with non-linear optimization
+
+            // Basic triangulation
            triangulate(reconstructedPoint, cameraMatrices, pointsOnEachCamera);

-            return 0.;
-            // return calcReprojectionError(X, cameraMatrices, pointsOnEachCamera);
+            #ifdef WITH_CERES
+                double paramX[3];
+                paramX[0] = reconstructedPoint.at<double>(0,0);
+                paramX[1] = reconstructedPoint.at<double>(1,0);
+                paramX[2] = reconstructedPoint.at<double>(2,0);
+                ceres::Problem problem;
+                for (auto i = 0u; i < cameraMatrices.size(); ++i)
+                {
+                    double camParam[12];
+                    memcpy(camParam, cameraMatrices[i].data, sizeof(double)*12);
+                    // Each Residual block takes a point and a camera as input and outputs a 2
+                    // dimensional residual. Internally, the cost function stores the observed
+                    // image location and compares the reprojection against the observation.
+                    ceres::CostFunction* cost_function =
+                        new ceres::AutoDiffCostFunction<ReprojectionErrorForTriangulation, 2, 3>(
+                        new ReprojectionErrorForTriangulation(pointsOnEachCamera[i].x, pointsOnEachCamera[i].y, camParam));
+                    // Add to problem
+                    problem.AddResidualBlock(cost_function,
+                        //NULL, //squared loss
+                        new ceres::HuberLoss(2.0),
+                        paramX);
+                }
+
+                ceres::Solver::Options options;
+                options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
+                // options.minimizer_progress_to_stdout = true;
+                // options.parameter_tolerance = 1e-20;
+                // options.function_tolerance = 1e-20;
+                ceres::Solver::Summary summary;
+                ceres::Solve(options, &problem, &summary);
+                // if (summary.initial_cost > summary.final_cost)
+                //     std::cout << summary.FullReport() << "\n";
+
+                reconstructedPoint.at<double>(0,0) = paramX[0];
+                reconstructedPoint.at<double>(1,0) = paramX[1];
+                reconstructedPoint.at<double>(2,0) = paramX[2];
+                reconstructedPoint.at<double>(3,0) = 1;
+
+                // const auto reprojectionErrorDecrease = std::sqrt((summary.initial_cost - summary.final_cost)/double(cameraMatrices.size()));
+                // return reprojectionErrorDecrease;
+            #endif

-            // //if (cameraMatrices.size() >= 3)
-            // //double beforeError = calcReprojectionError(&cameraMatrices, pointsOnEachCamera, X);
-            // double change = TriangulationOptimization(&cameraMatrices, pointsOnEachCamera, X);
-            // //double afterError = calcReprojectionError(&cameraMatrices,pointsOnEachCamera,X);
-            // //printfLog("!!Mine %.8f , inFunc %.8f \n",beforeError-afterError,change);
-            // return change;
+            return calcReprojectionError(reconstructedPoint, cameraMatrices, pointsOnEachCamera);
        }
        catch (const std::exception& e)
        {

--- a/src/openpose/CMakeLists.txt
+++ b/src/openpose/CMakeLists.txt
@@ -5,8 +5,8 @@ foreach (SUB_DIR ${SUB_DIRS})
 endforeach (SUB_DIR ${SUB_DIRS})

 # make the openpose.so/dll
-file(GLOB_RECURSE OP_HEADERS_UNFILTERED "${CMAKE_SOURCE_DIR}/include/openpose/*.h" 
-    "${CMAKE_SOURCE_DIR}/include/openpose/*.hpp" 
+file(GLOB_RECURSE OP_HEADERS_UNFILTERED "${CMAKE_SOURCE_DIR}/include/openpose/*.h"
+    "${CMAKE_SOURCE_DIR}/include/openpose/*.hpp"
    "${CMAKE_SOURCE_DIR}/include/openpose/*.hu")

 set(OP_HEADERS "")
@@ -23,9 +23,8 @@ else()
 endif ()

 # Ubuntu
-if(UNIX AND NOT APPLE)
-  target_link_libraries(openpose ${OpenCV_LIBS} ${Caffe_LIBS}
-      ${GFLAGS_LIBRARY} ${GLOG_LIBRARY} ${MKL_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB})
+if (UNIX AND NOT APPLE)
+  target_link_libraries(openpose ${OpenPose_3rdparty_libraries})
  if (CMAKE_COMPILER_IS_GNUCXX)
    foreach (SUB_DIR ${SUB_DIRS})
        set_target_properties(openpose_${SUB_DIR} PROPERTIES COMPILE_FLAGS ${OP_CXX_FLAGS})
@@ -35,22 +34,17 @@ if(UNIX AND NOT APPLE)
 # Windows
 elseif (WIN32)
  set_property(TARGET openpose PROPERTY DEBUG_POSTFIX d)
-  target_link_libraries(openpose ${OpenCV_LIBS} ${Caffe_LIBS} ${GLUT_LIBRARY} ${SPINNAKER_LIB} ${OpenCL_LIBRARIES}
-      debug ${GFLAGS_LIBRARY_DEBUG} optimized ${GFLAGS_LIBRARY_RELEASE}
-      debug ${GLOG_LIBRARY_DEBUG} optimized ${GLOG_LIBRARY_RELEASE})
-  if (${GPU_MODE} MATCHES "OPENCL")
-    target_link_libraries(openpose debug ${BOOST_SYSTEM_LIB_DEBUG} optimized ${BOOST_SYSTEM_LIB_RELEASE})
-  endif (${GPU_MODE} MATCHES "OPENCL")
+  target_link_libraries(openpose ${OpenPose_3rdparty_libraries})
  if (${DL_FRAMEWORK} MATCHES "CAFFE")
    target_compile_definitions(openpose PRIVATE BOOST_ALL_NO_LIB)
  endif(${DL_FRAMEWORK} MATCHES "CAFFE")
  set_property(TARGET openpose PROPERTY FOLDER "OpenPose library")
-  
+
  foreach (SUB_DIR ${SUB_DIRS})
    string(TOUPPER ${SUB_DIR} SUB_DIR_UPPERCASE)
    source_group("Source Files\\${SUB_DIR}" FILES ${SOURCES_OP_${SUB_DIR_UPPERCASE}_WITH_CP} "*")
-    file(GLOB_RECURSE SUB_MOD_HEADERS "${CMAKE_SOURCE_DIR}/include/openpose/${SUB_DIR}/*.h" 
-        "${CMAKE_SOURCE_DIR}/include/openpose/${SUB_DIR}/*.hpp" 
+    file(GLOB_RECURSE SUB_MOD_HEADERS "${CMAKE_SOURCE_DIR}/include/openpose/${SUB_DIR}/*.h"
+        "${CMAKE_SOURCE_DIR}/include/openpose/${SUB_DIR}/*.hpp"
        "${CMAKE_SOURCE_DIR}/include/openpose/${SUB_DIR}/*.hu")
    source_group("Header Files\\${SUB_DIR}" FILES ${SUB_MOD_HEADERS})
  endforeach (SUB_DIR ${SUB_DIRS})

--- a/ubuntu/Makefile.config.Ubuntu14_cuda7.example
+++ b/ubuntu/Makefile.config.Ubuntu14_cuda7.example
@@ -54,7 +54,10 @@ CAFFE_DIR := 3rdparty/caffe/distribute

 # OpenPose 3-D Reconstruction
 # WITH_3D_RENDERER := 1
+# WITH_CERES := 1
 # WITH_FLIR_CAMERA := 1
+# Eigen directory (Ceres)
+EIGEN_DIR := /usr/include/eigen3/
 # Spinnaker directory
 SPINNAKER_DIR := /usr/include/spinnaker


--- a/ubuntu/Makefile.config.Ubuntu14_cuda8.example
+++ b/ubuntu/Makefile.config.Ubuntu14_cuda8.example
@@ -60,7 +60,10 @@ CAFFE_DIR := 3rdparty/caffe/distribute

 # OpenPose 3-D Reconstruction
 # WITH_3D_RENDERER := 1
+# WITH_CERES := 1
 # WITH_FLIR_CAMERA := 1
+# Eigen directory (Ceres)
+EIGEN_DIR := /usr/include/eigen3/
 # Spinnaker directory
 SPINNAKER_DIR := /usr/include/spinnaker


--- a/ubuntu/Makefile.config.Ubuntu16_cuda7.example
+++ b/ubuntu/Makefile.config.Ubuntu16_cuda7.example
@@ -54,7 +54,10 @@ CAFFE_DIR := 3rdparty/caffe/distribute

 # OpenPose 3-D Reconstruction
 # WITH_3D_RENDERER := 1
+# WITH_CERES := 1
 # WITH_FLIR_CAMERA := 1
+# Eigen directory (Ceres)
+EIGEN_DIR := /usr/include/eigen3/
 # Spinnaker directory
 SPINNAKER_DIR := /usr/include/spinnaker


--- a/ubuntu/Makefile.config.Ubuntu16_cuda8.example
+++ b/ubuntu/Makefile.config.Ubuntu16_cuda8.example
@@ -60,7 +60,10 @@ CAFFE_DIR := 3rdparty/caffe/distribute

 # OpenPose 3-D Reconstruction
 # WITH_3D_RENDERER := 1
+# WITH_CERES := 1
 # WITH_FLIR_CAMERA := 1
+# Eigen directory (Ceres)
+EIGEN_DIR := /usr/include/eigen3/
 # Spinnaker directory
 SPINNAKER_DIR := /usr/include/spinnaker


--- a/ubuntu/Makefile.config.Ubuntu16_cuda8_JetsonTX2
+++ b/ubuntu/Makefile.config.Ubuntu16_cuda8_JetsonTX2
@@ -60,7 +60,10 @@ CAFFE_DIR := 3rdparty/caffe/distribute

 # OpenPose 3-D Reconstruction
 # WITH_3D_RENDERER := 1
+# WITH_CERES := 1
 # WITH_FLIR_CAMERA := 1
+# Eigen directory (Ceres)
+EIGEN_DIR := /usr/include/eigen3/
 # Spinnaker directory
 SPINNAKER_DIR := /usr/include/spinnaker


--- a/ubuntu/Makefile.example
+++ b/ubuntu/Makefile.example
@@ -80,6 +80,14 @@ ifneq ($(WITH_3D_RENDERER), 0)
 	LIBRARIES += glut
 	# LIBRARIES += GLU GL glut
 endif
+# Ceres support
+WITH_CERES ?= 0
+ifneq ($(WITH_CERES), 0)
+	COMMON_FLAGS += -DWITH_CERES
+	# Ceres
+	LIBRARIES += ceres
+	INCLUDE_DIRS += $(EIGEN_DIR)
+endif
 # Spinnaker SDK
 WITH_FLIR_CAMERA ?= 0
 ifneq ($(WITH_FLIR_CAMERA), 0)