Merge upstream branch

2026-07-30 07:43:03 +04:00 · 2020-04-21 06:19:48 +00:00
parent df10411e05 150bd3aee6
commit d92bee821d
6110 changed files with 1584756 additions and 447458 deletions
@@ -4,4 +4,25 @@ if(NOT OPENCV_MODULES_PATH)
  set(OPENCV_MODULES_PATH "${CMAKE_CURRENT_SOURCE_DIR}")
 endif()

-ocv_glob_modules(${OPENCV_MODULES_PATH} EXTRA ${OPENCV_EXTRA_MODULES_PATH})
+ocv_glob_modules(${OPENCV_MODULES_PATH} ${OPENCV_EXTRA_MODULES_PATH})
+
+# build lists of modules to be documented
+set(OPENCV_MODULES_MAIN "")
+set(OPENCV_MODULES_EXTRA "")
+
+foreach(mod ${OPENCV_MODULES_BUILD} ${OPENCV_MODULES_DISABLED_USER} ${OPENCV_MODULES_DISABLED_AUTO} ${OPENCV_MODULES_DISABLED_FORCE})
+  string(REGEX REPLACE "^opencv_" "" mod "${mod}")
+  if("${OPENCV_MODULE_opencv_${mod}_LOCATION}" STREQUAL "${OpenCV_SOURCE_DIR}/modules/${mod}")
+    list(APPEND OPENCV_MODULES_MAIN ${mod})
+  else()
+    list(APPEND OPENCV_MODULES_EXTRA ${mod})
+  endif()
+endforeach()
+ocv_list_sort(OPENCV_MODULES_MAIN)
+ocv_list_sort(OPENCV_MODULES_EXTRA)
+set(FIXED_ORDER_MODULES core imgproc imgcodecs videoio highgui video calib3d features2d objdetect dnn ml flann photo stitching)
+list(REMOVE_ITEM OPENCV_MODULES_MAIN ${FIXED_ORDER_MODULES})
+set(OPENCV_MODULES_MAIN ${FIXED_ORDER_MODULES} ${OPENCV_MODULES_MAIN})
+
+set(OPENCV_MODULES_MAIN ${OPENCV_MODULES_MAIN} CACHE INTERNAL "List of main modules" FORCE)
+set(OPENCV_MODULES_EXTRA ${OPENCV_MODULES_EXTRA} CACHE INTERNAL "List of extra modules" FORCE)
@@ -1,45 +0,0 @@
-IF(NOT ANDROID OR ANDROID_NATIVE_API_LEVEL LESS 8)
-  ocv_module_disable(androidcamera)
-ENDIF()
-
-set(the_description "Auxiliary module for Android native camera support")
-set(OPENCV_MODULE_TYPE STATIC)
-
-ocv_define_module(androidcamera INTERNAL opencv_core log dl)
-ocv_include_directories("${CMAKE_CURRENT_SOURCE_DIR}/camera_wrapper" "${OpenCV_SOURCE_DIR}/platforms/android/service/engine/jni/include")
-
-# Android source tree for native camera
-SET (ANDROID_SOURCE_TREE "ANDROID_SOURCE_TREE-NOTFOUND" CACHE PATH
-  "Path to Android source tree. Set this variable to path to your Android sources to compile libnative_camera_rx.x.x.so for your Android")
-SET(BUILD_ANDROID_CAMERA_WRAPPER OFF)
-if(ANDROID_SOURCE_TREE)
-  FILE(STRINGS "${ANDROID_SOURCE_TREE}/development/sdk/platform_source.properties" ANDROID_VERSION REGEX "Platform\\.Version=[0-9]+\\.[0-9]+(\\.[0-9]+)?" )
-  string(REGEX REPLACE "Platform\\.Version=([0-9]+\\.[0-9]+(\\.[0-9]+)?)" "\\1" ANDROID_VERSION "${ANDROID_VERSION}")
-  if(ANDROID_VERSION MATCHES "^[0-9]+\\.[0-9]+$")
-    SET(ANDROID_VERSION "${ANDROID_VERSION}.0")
-  endif()
-  if(NOT "${ANDROID_VERSION}" STREQUAL "")
-    SET(BUILD_ANDROID_CAMERA_WRAPPER ON)
-    set(ANDROID_VERSION "${ANDROID_VERSION}" CACHE INTERNAL "Version of Android source tree")
-  endif()
-endif()
-set(BUILD_ANDROID_CAMERA_WRAPPER ${BUILD_ANDROID_CAMERA_WRAPPER} CACHE INTERNAL "Build new wrapper for Android")
-MARK_AS_ADVANCED(ANDROID_SOURCE_TREE)
-
-# process wrapper libs
-if(BUILD_ANDROID_CAMERA_WRAPPER)
-  add_subdirectory(camera_wrapper)
-else()
-  file(GLOB camera_wrappers "${CMAKE_CURRENT_SOURCE_DIR}/../../3rdparty/lib/${ANDROID_NDK_ABI_NAME}/libnative_camera_r*.so")
-
-  foreach(wrapper ${camera_wrappers})
-    ADD_CUSTOM_COMMAND(
-      TARGET ${the_module} POST_BUILD
-      COMMAND ${CMAKE_COMMAND} -E copy "${wrapper}" "${LIBRARY_OUTPUT_PATH}"
-      )
-    get_filename_component(wrapper_name "${wrapper}" NAME)
-    install(FILES "${LIBRARY_OUTPUT_PATH}/${wrapper_name}"
-            DESTINATION ${OPENCV_LIB_INSTALL_PATH}
-            COMPONENT libs)
-  endforeach()
-endif()
@@ -1,66 +0,0 @@
-SET (the_target native_camera_r${ANDROID_VERSION})
-
-project(${the_target})
-
-link_directories("${ANDROID_SOURCE_TREE}/out/target/product/generic/system/lib")
-
-if (ANDROID_VERSION VERSION_LESS "4.1")
-    INCLUDE_DIRECTORIES(BEFORE
-    ${ANDROID_SOURCE_TREE}
-    ${ANDROID_SOURCE_TREE}/frameworks/base/include/ui
-    ${ANDROID_SOURCE_TREE}/frameworks/base/include/surfaceflinger
-    ${ANDROID_SOURCE_TREE}/frameworks/base/include/camera
-    ${ANDROID_SOURCE_TREE}/frameworks/base/include/media
-    ${ANDROID_SOURCE_TREE}/frameworks/base/include
-    ${ANDROID_SOURCE_TREE}/system/core/include
-    ${ANDROID_SOURCE_TREE}/hardware/libhardware/include
-    ${ANDROID_SOURCE_TREE}/frameworks/base/native/include
-    ${ANDROID_SOURCE_TREE}/frameworks/base/opengl/include
-    )
-else()
-    INCLUDE_DIRECTORIES(BEFORE
-    ${ANDROID_SOURCE_TREE}
-    ${ANDROID_SOURCE_TREE}/frameworks/native/include
-    ${ANDROID_SOURCE_TREE}/frameworks/av/include
-    ${ANDROID_SOURCE_TREE}/system/core/include
-    ${ANDROID_SOURCE_TREE}/hardware/libhardware/include
-    )
-endif()
-
-set(CMAKE_INSTALL_RPATH_USE_LINK_PATH FALSE)
-
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_RELEASE}")
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_RELEASE}")
-SET(CMAKE_C_FLAGS_RELEASE "")
-SET(CMAKE_CXX_FLAGS_RELEASE "")
-
-string(REPLACE "-O3" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
-string(REPLACE "-frtti" "-fno-rtti" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # because Android libraries are built without rtti
-
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Os -fno-strict-aliasing -finline-limit=64 -fuse-cxa-atexit" )
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Os -fno-strict-aliasing -finline-limit=64 -fuse-cxa-atexit")
-SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-z,noexecstack")
-
-ADD_LIBRARY(${the_target} SHARED camera_wrapper.h camera_wrapper.cpp)
-
-string(REGEX REPLACE "[.]" "_" LIBRARY_DEF ${ANDROID_VERSION})
-add_definitions(-DANDROID_r${LIBRARY_DEF})
-
-ocv_target_link_libraries(${the_target} c m dl utils camera_client binder log)
-
-if(NOT ANDROID_VERSION VERSION_LESS "3.0.0")
-    target_link_libraries(${the_target} gui )
-endif()
-
-SET_TARGET_PROPERTIES(${the_target} PROPERTIES
-                      OUTPUT_NAME "${the_target}"
-                      ARCHIVE_OUTPUT_DIRECTORY ${LIBRARY_OUTPUT_PATH}
-                      RUNTIME_OUTPUT_DIRECTORY ${EXECUTABLE_OUTPUT_PATH}
-                      )
-
-if (NOT (CMAKE_BUILD_TYPE MATCHES "Debug"))
-    ADD_CUSTOM_COMMAND( TARGET ${the_target} POST_BUILD COMMAND ${CMAKE_STRIP} --strip-unneeded "${LIBRARY_OUTPUT_PATH}/lib${the_target}.so" )
-endif()
-
-
-install(TARGETS ${the_target} LIBRARY DESTINATION ${OPENCV_LIB_INSTALL_PATH} COMPONENT libs)
@@ -1,70 +0,0 @@
-*** src2.3.3/frameworks/base/include/camera/Camera.h	2011-04-04 20:18:36.718480237 +0400
--- src_mock3.0.1/frameworks/base/include/camera/Camera.h	2012-01-15 20:51:36.000000000 +0400
-***************
-*** 20,25 ****
--- 20,27 ----
-  #include <utils/Timers.h>
-  #include <camera/ICameraClient.h>
-  
-+ #include <gui/ISurfaceTexture.h>
-+ 
-  namespace android {
-  
-  class ISurface;
-***************
-*** 76,81 ****
--- 78,90 ----
-      CAMERA_MSG_POSTVIEW_FRAME   = 0x040,
-      CAMERA_MSG_RAW_IMAGE        = 0x080,
-      CAMERA_MSG_COMPRESSED_IMAGE = 0x100,
-+ 
-+ #ifdef OMAP_ENHANCEMENT
-+ 
-+     CAMERA_MSG_BURST_IMAGE      = 0x200,
-+ 
-+ #endif
-+ 
-      CAMERA_MSG_ALL_MSGS         = 0x1FF
-  };
-  
-***************
-*** 144,150 ****
--- 153,164 ----
-  public:
-      virtual void notify(int32_t msgType, int32_t ext1, int32_t ext2) = 0;
-      virtual void postData(int32_t msgType, const sp<IMemory>& dataPtr) = 0;
-+ #ifdef OMAP_ENHANCEMENT
-+     virtual void postDataTimestamp(nsecs_t timestamp, int32_t msgType, const sp<IMemory>& dataPtr,
-+                     uint32_t offset=0, uint32_t stride=0) = 0;
-+ #else
-      virtual void postDataTimestamp(nsecs_t timestamp, int32_t msgType, const sp<IMemory>& dataPtr) = 0;
-+ #endif
-  };
-  
-  class Camera : public BnCameraClient, public IBinder::DeathRecipient
-***************
-*** 170,175 ****
--- 184,191 ----
-              status_t    setPreviewDisplay(const sp<Surface>& surface);
-              status_t    setPreviewDisplay(const sp<ISurface>& surface);
-  
-+             // pass the SurfaceTexture object to the Camera
-+ 			status_t    setPreviewTexture(const sp<ISurfaceTexture>& surfaceTexture);
-              // start preview mode, must call setPreviewDisplay first
-              status_t    startPreview();
-  
-***************
-*** 215,221 ****
--- 231,242 ----
-      // ICameraClient interface
-      virtual void        notifyCallback(int32_t msgType, int32_t ext, int32_t ext2);
-      virtual void        dataCallback(int32_t msgType, const sp<IMemory>& dataPtr);
-+ #ifdef OMAP_ENHANCEMENT
-+     virtual void        dataCallbackTimestamp(nsecs_t timestamp, int32_t msgType, const sp<IMemory>& dataPtr,
-+                                 uint32_t offset=0, uint32_t stride=0);
-+ #else
-      virtual void        dataCallbackTimestamp(nsecs_t timestamp, int32_t msgType, const sp<IMemory>& dataPtr);
-+ #endif
-  
-      sp<ICamera>         remote();
-  
@@ -1,14 +0,0 @@
-*** src2.3.3/frameworks/base/include/camera/ICamera.h	2011-04-04 20:18:36.718480237 +0400
--- src_mock3.0.1/frameworks/base/include/camera/ICamera.h	2012-01-15 20:50:30.000000000 +0400
-***************
-*** 48,53 ****
--- 48,56 ----
-      // pass the buffered ISurface to the camera service
-      virtual status_t        setPreviewDisplay(const sp<ISurface>& surface) = 0;
-  
-+     // pass the preview texture. This is for 3.0 and higher versions of Android
-+     setPreviewTexture(const sp<ISurfaceTexture>& surfaceTexture) = 0;
-+ 
-      // set the preview callback flag to affect how the received frames from
-      // preview are handled.
-      virtual void            setPreviewCallbackFlag(int flag) = 0;
@@ -1,9 +0,0 @@
-Building camera wrapper for Android 3.0.1:
-
-1) Get sources of Android 2.3.x (2.3.3 were used)
-2) Apply patches provided with this instruction to frameworks/base/include/camera/ICamera.h and frameworks/base/include/camera/Camera.h
-3) Get frameworks/base/include/gui/ISurfaceTexture.h and frameworks/base/include/gui/SurfaceTexture.h from Android 4.0.x (4.0.3 were used) sources and add them to your source tree.
-4) Apply provided patch to the frameworks/base/include/gui/SurfaceTexture.h.
-5) Pull /system/lib from your device running Andoid 3.x.x
-6) Edit <Android Root>/development/sdk/platform_source.properties file. Set Android version to 3.0.1.
-7) Build wrapper as normal using this modified source tree.
@@ -1,37 +0,0 @@
-*** src4.0.3/src/frameworks/base/include/gui/SurfaceTexture.h	2012-01-18 16:32:41.424750385 +0400
--- src_mock3.0.1/frameworks/base/include/gui/SurfaceTexture.h	2012-01-12 21:28:14.000000000 +0400
-***************
-*** 68,75 ****
-      // texture will be bound in updateTexImage. useFenceSync specifies whether
-      // fences should be used to synchronize access to buffers if that behavior
-      // is enabled at compile-time.
-!     SurfaceTexture(GLuint tex, bool allowSynchronousMode = true,
-!             GLenum texTarget = GL_TEXTURE_EXTERNAL_OES, bool useFenceSync = true);
-  
-      virtual ~SurfaceTexture();
-  
--- 68,74 ----
-      // texture will be bound in updateTexImage. useFenceSync specifies whether
-      // fences should be used to synchronize access to buffers if that behavior
-      // is enabled at compile-time.
-!     SurfaceTexture(GLuint tex);
-  
-      virtual ~SurfaceTexture();
-  
-***************
-*** 280,286 ****
-                mBufferState(BufferSlot::FREE),
-                mRequestBufferCalled(false),
-                mTransform(0),
-!               mScalingMode(NATIVE_WINDOW_SCALING_MODE_FREEZE),
-                mTimestamp(0),
-                mFrameNumber(0),
-                mFence(EGL_NO_SYNC_KHR) {
--- 279,285 ----
-                mBufferState(BufferSlot::FREE),
-                mRequestBufferCalled(false),
-                mTransform(0),
-!               mScalingMode(0),
-                mTimestamp(0),
-                mFrameNumber(0),
-                mFence(EGL_NO_SYNC_KHR) {
@@ -1,16 +0,0 @@
-typedef bool (*CameraCallback)(void* buffer, size_t bufferSize, void* userData);
-
-typedef void* (*InitCameraConnectC)(void* cameraCallback, int cameraId, void* userData);
-typedef void (*CloseCameraConnectC)(void**);
-typedef double (*GetCameraPropertyC)(void* camera, int propIdx);
-typedef void (*SetCameraPropertyC)(void* camera, int propIdx, double value);
-typedef void (*ApplyCameraPropertiesC)(void** camera);
-
-extern "C"
-{
-void* initCameraConnectC(void* cameraCallback, int cameraId, void* userData);
-void closeCameraConnectC(void**);
-double getCameraPropertyC(void* camera, int propIdx);
-void setCameraPropertyC(void* camera, int propIdx, double value);
-void applyCameraPropertiesC(void** camera);
-}
@@ -1,47 +0,0 @@
-#ifndef _CAMERAACTIVITY_H_
-#define _CAMERAACTIVITY_H_
-
-#include <camera_properties.h>
-
-class CameraActivity
-{
-public:
-    enum ErrorCode {
-        NO_ERROR=0,
-        ERROR_WRONG_FRAME_SIZE,
-        ERROR_WRONG_POINTER_CAMERA_WRAPPER,
-        ERROR_CAMERA_CONNECTED,
-        ERROR_CANNOT_OPEN_CAMERA_WRAPPER_LIB,
-        ERROR_CANNOT_GET_FUNCTION_FROM_CAMERA_WRAPPER_LIB,
-        ERROR_CANNOT_INITIALIZE_CONNECTION,
-        ERROR_ISNT_CONNECTED,
-        ERROR_JAVA_VM_CANNOT_GET_CLASS,
-        ERROR_JAVA_VM_CANNOT_GET_FIELD,
-        ERROR_CANNOT_SET_PREVIEW_DISPLAY,
-
-        ERROR_UNKNOWN=255
-    };
-
-    CameraActivity();
-    virtual ~CameraActivity();
-    virtual bool onFrameBuffer(void* buffer, int bufferSize);
-
-    ErrorCode connect(int cameraId = 0);
-    void disconnect();
-    bool isConnected() const;
-
-    double getProperty(int propIdx);
-    void setProperty(int propIdx, double value);
-    void applyProperties();
-
-    int getFrameWidth();
-    int getFrameHeight();
-
-    static void setPathLibFolder(const char* path);
-private:
-    void* camera;
-    int frameWidth;
-    int frameHeight;
-};
-
-#endif
@@ -1,70 +0,0 @@
-#ifndef CAMERA_PROPERTIES_H
-#define CAMERA_PROPERTIES_H
-
-enum {
-    ANDROID_CAMERA_PROPERTY_FRAMEWIDTH = 0,
-    ANDROID_CAMERA_PROPERTY_FRAMEHEIGHT = 1,
-    ANDROID_CAMERA_PROPERTY_SUPPORTED_PREVIEW_SIZES_STRING = 2,
-    ANDROID_CAMERA_PROPERTY_PREVIEW_FORMAT_STRING = 3,
-    ANDROID_CAMERA_PROPERTY_FPS = 4,
-    ANDROID_CAMERA_PROPERTY_EXPOSURE = 5,
-    ANDROID_CAMERA_PROPERTY_FLASH_MODE = 101,
-    ANDROID_CAMERA_PROPERTY_FOCUS_MODE = 102,
-    ANDROID_CAMERA_PROPERTY_WHITE_BALANCE = 103,
-    ANDROID_CAMERA_PROPERTY_ANTIBANDING = 104,
-    ANDROID_CAMERA_PROPERTY_FOCAL_LENGTH = 105,
-    ANDROID_CAMERA_PROPERTY_FOCUS_DISTANCE_NEAR = 106,
-    ANDROID_CAMERA_PROPERTY_FOCUS_DISTANCE_OPTIMAL = 107,
-    ANDROID_CAMERA_PROPERTY_FOCUS_DISTANCE_FAR = 108,
-    ANDROID_CAMERA_PROPERTY_EXPOSE_LOCK = 109,
-    ANDROID_CAMERA_PROPERTY_WHITEBALANCE_LOCK = 110
-};
-
-
-enum {
-    ANDROID_CAMERA_FLASH_MODE_AUTO = 0,
-    ANDROID_CAMERA_FLASH_MODE_OFF,
-    ANDROID_CAMERA_FLASH_MODE_ON,
-    ANDROID_CAMERA_FLASH_MODE_RED_EYE,
-    ANDROID_CAMERA_FLASH_MODE_TORCH,
-    ANDROID_CAMERA_FLASH_MODES_NUM
-};
-
-enum {
-    ANDROID_CAMERA_FOCUS_MODE_AUTO = 0,
-    ANDROID_CAMERA_FOCUS_MODE_CONTINUOUS_VIDEO,
-    ANDROID_CAMERA_FOCUS_MODE_EDOF,
-    ANDROID_CAMERA_FOCUS_MODE_FIXED,
-    ANDROID_CAMERA_FOCUS_MODE_INFINITY,
-    ANDROID_CAMERA_FOCUS_MODE_MACRO,
-    ANDROID_CAMERA_FOCUS_MODE_CONTINUOUS_PICTURE,
-    ANDROID_CAMERA_FOCUS_MODES_NUM
-};
-
-enum {
-    ANDROID_CAMERA_WHITE_BALANCE_AUTO = 0,
-    ANDROID_CAMERA_WHITE_BALANCE_CLOUDY_DAYLIGHT,
-    ANDROID_CAMERA_WHITE_BALANCE_DAYLIGHT,
-    ANDROID_CAMERA_WHITE_BALANCE_FLUORESCENT,
-    ANDROID_CAMERA_WHITE_BALANCE_INCANDESCENT,
-    ANDROID_CAMERA_WHITE_BALANCE_SHADE,
-    ANDROID_CAMERA_WHITE_BALANCE_TWILIGHT,
-    ANDROID_CAMERA_WHITE_BALANCE_WARM_FLUORESCENT,
-    ANDROID_CAMERA_WHITE_BALANCE_MODES_NUM
-};
-
-enum {
-    ANDROID_CAMERA_ANTIBANDING_50HZ = 0,
-    ANDROID_CAMERA_ANTIBANDING_60HZ,
-    ANDROID_CAMERA_ANTIBANDING_AUTO,
-    ANDROID_CAMERA_ANTIBANDING_OFF,
-    ANDROID_CAMERA_ANTIBANDING_MODES_NUM
-};
-
-enum {
-    ANDROID_CAMERA_FOCUS_DISTANCE_NEAR_INDEX = 0,
-    ANDROID_CAMERA_FOCUS_DISTANCE_OPTIMAL_INDEX,
-    ANDROID_CAMERA_FOCUS_DISTANCE_FAR_INDEX
-};
-
-#endif // CAMERA_PROPERTIES_H
@@ -1,451 +0,0 @@
-#include <dlfcn.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <dirent.h>
-#include <android/log.h>
-#include <cctype>
-#include <vector>
-#include <algorithm>
-#include <opencv2/core/version.hpp>
-#include "camera_activity.hpp"
-#include "camera_wrapper.h"
-#include "EngineCommon.h"
-
-#include "opencv2/core.hpp"
-
-#undef LOG_TAG
-#undef LOGE
-#undef LOGD
-#undef LOGI
-
-#define LOG_TAG "OpenCV::camera"
-#define LOGE(...) ((void)__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__))
-#define LOGD(...) ((void)__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__))
-#define LOGI(...) ((void)__android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__))
-
-///////
-// Debug
-#include <stdio.h>
-#include <sys/types.h>
-#include <dirent.h>
-
-struct str_greater
-{
-    bool operator() (const cv::String& a, const cv::String& b) { return a > b; }
-};
-
-class CameraWrapperConnector
-{
-public:
-    static CameraActivity::ErrorCode connect(int cameraId, CameraActivity* pCameraActivity, void** camera);
-    static CameraActivity::ErrorCode disconnect(void** camera);
-    static CameraActivity::ErrorCode setProperty(void* camera, int propIdx, double value);
-    static CameraActivity::ErrorCode getProperty(void* camera, int propIdx, double* value);
-    static CameraActivity::ErrorCode applyProperties(void** ppcamera);
-
-    static void setPathLibFolder(const cv::String& path);
-
-private:
-    static cv::String pathLibFolder;
-    static bool isConnectedToLib;
-
-    static cv::String getPathLibFolder();
-    static cv::String getDefaultPathLibFolder();
-    static CameraActivity::ErrorCode connectToLib();
-    static CameraActivity::ErrorCode getSymbolFromLib(void * libHandle, const char* symbolName, void** ppSymbol);
-    static void fillListWrapperLibs(const cv::String& folderPath, std::vector<cv::String>& listLibs);
-
-    static InitCameraConnectC pInitCameraC;
-    static CloseCameraConnectC pCloseCameraC;
-    static GetCameraPropertyC pGetPropertyC;
-    static SetCameraPropertyC pSetPropertyC;
-    static ApplyCameraPropertiesC pApplyPropertiesC;
-
-    friend bool nextFrame(void* buffer, size_t bufferSize, void* userData);
-};
-
-cv::String CameraWrapperConnector::pathLibFolder;
-
-bool CameraWrapperConnector::isConnectedToLib = false;
-InitCameraConnectC  CameraWrapperConnector::pInitCameraC = 0;
-CloseCameraConnectC  CameraWrapperConnector::pCloseCameraC = 0;
-GetCameraPropertyC CameraWrapperConnector::pGetPropertyC = 0;
-SetCameraPropertyC CameraWrapperConnector::pSetPropertyC = 0;
-ApplyCameraPropertiesC CameraWrapperConnector::pApplyPropertiesC = 0;
-
-#define INIT_CAMERA_SYMBOL_NAME "initCameraConnectC"
-#define CLOSE_CAMERA_SYMBOL_NAME "closeCameraConnectC"
-#define SET_CAMERA_PROPERTY_SYMBOL_NAME "setCameraPropertyC"
-#define GET_CAMERA_PROPERTY_SYMBOL_NAME "getCameraPropertyC"
-#define APPLY_CAMERA_PROPERTIES_SYMBOL_NAME "applyCameraPropertiesC"
-#define PREFIX_CAMERA_WRAPPER_LIB "libnative_camera"
-
-
-bool nextFrame(void* buffer, size_t bufferSize, void* userData)
-{
-    if (userData == NULL)
-        return true;
-
-    return ((CameraActivity*)userData)->onFrameBuffer(buffer, bufferSize);
-}
-
-CameraActivity::ErrorCode CameraWrapperConnector::connect(int cameraId, CameraActivity* pCameraActivity, void** camera)
-{
-    if (pCameraActivity == NULL)
-    {
-        LOGE("CameraWrapperConnector::connect error: wrong pointer to CameraActivity object");
-        return CameraActivity::ERROR_WRONG_POINTER_CAMERA_WRAPPER;
-    }
-
-    CameraActivity::ErrorCode errcode=connectToLib();
-    if (errcode) return errcode;
-
-    void* cmr = (*pInitCameraC)((void*)nextFrame, cameraId, (void*)pCameraActivity);
-    if (!cmr)
-    {
-        LOGE("CameraWrapperConnector::connectWrapper ERROR: the initializing function returned false");
-        return CameraActivity::ERROR_CANNOT_INITIALIZE_CONNECTION;
-    }
-
-    *camera = cmr;
-    return CameraActivity::NO_ERROR;
-}
-
-CameraActivity::ErrorCode CameraWrapperConnector::disconnect(void** camera)
-{
-    if (camera == NULL || *camera == NULL)
-    {
-        LOGE("CameraWrapperConnector::disconnect error: wrong pointer to camera object");
-        return CameraActivity::ERROR_WRONG_POINTER_CAMERA_WRAPPER;
-    }
-
-    CameraActivity::ErrorCode errcode=connectToLib();
-    if (errcode) return errcode;
-
-    (*pCloseCameraC)(camera);
-
-    return CameraActivity::NO_ERROR;
-}
-
-CameraActivity::ErrorCode CameraWrapperConnector::setProperty(void* camera, int propIdx, double value)
-{
-    if (camera == NULL)
-    {
-        LOGE("CameraWrapperConnector::setProperty error: wrong pointer to camera object");
-        return CameraActivity::ERROR_WRONG_POINTER_CAMERA_WRAPPER;
-    }
-
-    (*pSetPropertyC)(camera, propIdx, value);
-
-    return CameraActivity::NO_ERROR;
-}
-
-CameraActivity::ErrorCode CameraWrapperConnector::getProperty(void* camera, int propIdx, double* value)
-{
-    if (camera == NULL)
-    {
-        LOGE("CameraWrapperConnector::getProperty error: wrong pointer to camera object");
-        return CameraActivity::ERROR_WRONG_POINTER_CAMERA_WRAPPER;
-    }
-    LOGE("calling (*pGetPropertyC)(%p, %d)", camera, propIdx);
-    *value = (*pGetPropertyC)(camera, propIdx);
-    return CameraActivity::NO_ERROR;
-}
-
-CameraActivity::ErrorCode CameraWrapperConnector::applyProperties(void** ppcamera)
-{
-    if ((ppcamera == NULL) || (*ppcamera == NULL))
-    {
-        LOGE("CameraWrapperConnector::applyProperties error: wrong pointer to camera object");
-        return CameraActivity::ERROR_WRONG_POINTER_CAMERA_WRAPPER;
-    }
-
-    (*pApplyPropertiesC)(ppcamera);
-    return CameraActivity::NO_ERROR;
-}
-
-CameraActivity::ErrorCode CameraWrapperConnector::connectToLib()
-{
-    if (isConnectedToLib) {
-        return CameraActivity::NO_ERROR;
-    }
-
-    dlerror();
-    cv::String folderPath = getPathLibFolder();
-    if (folderPath.empty())
-    {
-        LOGD("Trying to find native camera in default OpenCV packages");
-        folderPath = getDefaultPathLibFolder();
-    }
-
-    LOGD("CameraWrapperConnector::connectToLib: folderPath=%s", folderPath.c_str());
-
-    std::vector<cv::String> listLibs;
-    fillListWrapperLibs(folderPath, listLibs);
-    std::sort(listLibs.begin(), listLibs.end(), str_greater());
-
-    void * libHandle=0;
-    cv::String cur_path;
-    for(size_t i = 0; i < listLibs.size(); i++) {
-        cur_path=folderPath + listLibs[i];
-        LOGD("try to load library '%s'", listLibs[i].c_str());
-        libHandle=dlopen(cur_path.c_str(), RTLD_LAZY);
-        if (libHandle) {
-            LOGD("Loaded library '%s'", cur_path.c_str());
-            break;
-        } else {
-            LOGD("CameraWrapperConnector::connectToLib ERROR: cannot dlopen camera wrapper library %s, dlerror=\"%s\"",
-                 cur_path.c_str(), dlerror());
-        }
-    }
-
-    if (!libHandle) {
-        LOGE("CameraWrapperConnector::connectToLib ERROR: cannot dlopen camera wrapper library");
-        return CameraActivity::ERROR_CANNOT_OPEN_CAMERA_WRAPPER_LIB;
-    }
-
-    InitCameraConnectC pInit_C;
-    CloseCameraConnectC pClose_C;
-    GetCameraPropertyC pGetProp_C;
-    SetCameraPropertyC pSetProp_C;
-    ApplyCameraPropertiesC pApplyProp_C;
-
-    CameraActivity::ErrorCode res;
-
-    res = getSymbolFromLib(libHandle, (const char*)INIT_CAMERA_SYMBOL_NAME, (void**)(&pInit_C));
-    if (res) return res;
-
-    res = getSymbolFromLib(libHandle, CLOSE_CAMERA_SYMBOL_NAME, (void**)(&pClose_C));
-    if (res) return res;
-
-    res = getSymbolFromLib(libHandle, GET_CAMERA_PROPERTY_SYMBOL_NAME, (void**)(&pGetProp_C));
-    if (res) return res;
-
-    res = getSymbolFromLib(libHandle, SET_CAMERA_PROPERTY_SYMBOL_NAME, (void**)(&pSetProp_C));
-    if (res) return res;
-
-    res = getSymbolFromLib(libHandle, APPLY_CAMERA_PROPERTIES_SYMBOL_NAME, (void**)(&pApplyProp_C));
-    if (res) return res;
-
-    pInitCameraC  = pInit_C;
-    pCloseCameraC = pClose_C;
-    pGetPropertyC = pGetProp_C;
-    pSetPropertyC = pSetProp_C;
-    pApplyPropertiesC = pApplyProp_C;
-    isConnectedToLib=true;
-
-    return CameraActivity::NO_ERROR;
-}
-
-CameraActivity::ErrorCode CameraWrapperConnector::getSymbolFromLib(void* libHandle, const char* symbolName, void** ppSymbol)
-{
-    dlerror();
-    *(void **) (ppSymbol)=dlsym(libHandle, symbolName);
-
-    const char* error_dlsym_init=dlerror();
-    if (error_dlsym_init) {
-        LOGE("CameraWrapperConnector::getSymbolFromLib ERROR: cannot get symbol of the function '%s' from the camera wrapper library, dlerror=\"%s\"",
-             symbolName, error_dlsym_init);
-        return CameraActivity::ERROR_CANNOT_GET_FUNCTION_FROM_CAMERA_WRAPPER_LIB;
-    }
-    return CameraActivity::NO_ERROR;
-}
-
-void CameraWrapperConnector::fillListWrapperLibs(const cv::String& folderPath, std::vector<cv::String>& listLibs)
-{
-    DIR *dp;
-    struct dirent *ep;
-
-    dp = opendir (folderPath.c_str());
-    if (dp != NULL)
-    {
-        while ((ep = readdir (dp))) {
-            const char* cur_name=ep->d_name;
-            if (strstr(cur_name, PREFIX_CAMERA_WRAPPER_LIB)) {
-                listLibs.push_back(cur_name);
-                LOGE("||%s", cur_name);
-            }
-        }
-        (void) closedir (dp);
-    }
-}
-
-cv::String CameraWrapperConnector::getDefaultPathLibFolder()
-{
-    #define BIN_PACKAGE_NAME(x) "org.opencv.lib_v" CVAUX_STR(CV_VERSION_MAJOR) CVAUX_STR(CV_VERSION_MINOR) "_" x
-    const char* const packageList[] = {BIN_PACKAGE_NAME("armv7a"), OPENCV_ENGINE_PACKAGE};
-    for (size_t i = 0; i < sizeof(packageList)/sizeof(packageList[0]); i++)
-    {
-        char path[128];
-        sprintf(path, "/data/data/%s/lib/", packageList[i]);
-        LOGD("Trying package \"%s\" (\"%s\")", packageList[i], path);
-
-        DIR* dir = opendir(path);
-        if (!dir)
-        {
-            LOGD("Package not found");
-            continue;
-        }
-        else
-        {
-            closedir(dir);
-            return path;
-        }
-    }
-
-    return cv::String();
-}
-
-cv::String CameraWrapperConnector::getPathLibFolder()
-{
-    if (!pathLibFolder.empty())
-        return pathLibFolder;
-
-    Dl_info dl_info;
-    if(0 != dladdr((void *)nextFrame, &dl_info))
-    {
-        LOGD("Library name: %s", dl_info.dli_fname);
-        LOGD("Library base address: %p", dl_info.dli_fbase);
-
-        const char* libName=dl_info.dli_fname;
-        while( ((*libName)=='/') || ((*libName)=='.') )
-        libName++;
-
-        char lineBuf[2048];
-        FILE* file = fopen("/proc/self/smaps", "rt");
-
-        if(file)
-        {
-            while (fgets(lineBuf, sizeof lineBuf, file) != NULL)
-            {
-                //verify that line ends with library name
-                int lineLength = strlen(lineBuf);
-                int libNameLength = strlen(libName);
-
-                //trim end
-                for(int i = lineLength - 1; i >= 0 && isspace(lineBuf[i]); --i)
-                {
-                    lineBuf[i] = 0;
-                    --lineLength;
-                }
-
-                if (0 != strncmp(lineBuf + lineLength - libNameLength, libName, libNameLength))
-                {
-                //the line does not contain the library name
-                    continue;
-                }
-
-                //extract path from smaps line
-                char* pathBegin = strchr(lineBuf, '/');
-                if (0 == pathBegin)
-                {
-                    LOGE("Strange error: could not find path beginning in lin \"%s\"", lineBuf);
-                    continue;
-                }
-
-                char* pathEnd = strrchr(pathBegin, '/');
-                pathEnd[1] = 0;
-
-                LOGD("Libraries folder found: %s", pathBegin);
-
-                fclose(file);
-                return pathBegin;
-            }
-            fclose(file);
-            LOGE("Could not find library path");
-        }
-        else
-        {
-            LOGE("Could not read /proc/self/smaps");
-        }
-    }
-    else
-    {
-        LOGE("Could not get library name and base address");
-    }
-
-    return cv::String();
-}
-
-void CameraWrapperConnector::setPathLibFolder(const cv::String& path)
-{
-    pathLibFolder=path;
-}
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////
-
-CameraActivity::CameraActivity() : camera(0), frameWidth(-1), frameHeight(-1)
-{
-}
-
-CameraActivity::~CameraActivity()
-{
-    if (camera != 0)
-        disconnect();
-}
-
-bool CameraActivity::onFrameBuffer(void* /*buffer*/, int /*bufferSize*/)
-{
-    LOGD("CameraActivity::onFrameBuffer - empty callback");
-    return true;
-}
-
-void CameraActivity::disconnect()
-{
-    CameraWrapperConnector::disconnect(&camera);
-}
-
-bool CameraActivity::isConnected() const
-{
-    return camera != 0;
-}
-
-CameraActivity::ErrorCode CameraActivity::connect(int cameraId)
-{
-    ErrorCode rescode = CameraWrapperConnector::connect(cameraId, this, &camera);
-    if (rescode) return rescode;
-
-    return NO_ERROR;
-}
-
-double CameraActivity::getProperty(int propIdx)
-{
-    double propVal;
-    ErrorCode rescode = CameraWrapperConnector::getProperty(camera, propIdx, &propVal);
-    if (rescode) return -1;
-    return propVal;
-}
-
-void CameraActivity::setProperty(int propIdx, double value)
-{
-    CameraWrapperConnector::setProperty(camera, propIdx, value);
-}
-
-void CameraActivity::applyProperties()
-{
-    frameWidth = -1;
-    frameHeight = -1;
-    CameraWrapperConnector::applyProperties(&camera);
-    frameWidth = getProperty(ANDROID_CAMERA_PROPERTY_FRAMEWIDTH);
-    frameHeight = getProperty(ANDROID_CAMERA_PROPERTY_FRAMEHEIGHT);
-}
-
-int CameraActivity::getFrameWidth()
-{
-    if (frameWidth <= 0)
-        frameWidth = getProperty(ANDROID_CAMERA_PROPERTY_FRAMEWIDTH);
-    return frameWidth;
-}
-
-int CameraActivity::getFrameHeight()
-{
-    if (frameHeight <= 0)
-        frameHeight = getProperty(ANDROID_CAMERA_PROPERTY_FRAMEHEIGHT);
-    return frameHeight;
-}
-
-void CameraActivity::setPathLibFolder(const char* path)
-{
-    CameraWrapperConnector::setPathLibFolder(path);
-}
@@ -1,2 +1,8 @@
 set(the_description "Camera Calibration and 3D Reconstruction")
-ocv_define_module(calib3d opencv_imgproc opencv_features2d)
+set(debug_modules "")
+if(DEBUG_opencv_calib3d)
+  list(APPEND debug_modules opencv_highgui)
+endif()
+ocv_define_module(calib3d opencv_imgproc opencv_features2d ${debug_modules}
+    WRAP java python js
+)
@@ -0,0 +1,41 @@
+@article{lepetit2009epnp,
+  title={Epnp: An accurate o (n) solution to the pnp problem},
+  author={Lepetit, Vincent and Moreno-Noguer, Francesc and Fua, Pascal},
+  journal={International journal of computer vision},
+  volume={81},
+  number={2},
+  pages={155--166},
+  year={2009},
+  publisher={Springer}
+}
+
+@article{gao2003complete,
+  title={Complete solution classification for the perspective-three-point problem},
+  author={Gao, Xiao-Shan and Hou, Xiao-Rong and Tang, Jianliang and Cheng, Hang-Fei},
+  journal={Pattern Analysis and Machine Intelligence, IEEE Transactions on},
+  volume={25},
+  number={8},
+  pages={930--943},
+  year={2003},
+  publisher={IEEE}
+}
+
+@inproceedings{hesch2011direct,
+  title={A direct least-squares (DLS) method for PnP},
+  author={Hesch, Joel and Roumeliotis, Stergios and others},
+  booktitle={Computer Vision (ICCV), 2011 IEEE International Conference on},
+  pages={383--390},
+  year={2011},
+  organization={IEEE}
+}
+
+@article{penate2013exhaustive,
+  title={Exhaustive linearization for robust camera pose and focal length estimation},
+  author={Penate-Sanchez, Adrian and Andrade-Cetto, Juan and Moreno-Noguer, Francesc},
+  journal={Pattern Analysis and Machine Intelligence, IEEE Transactions on},
+  volume={35},
+  number={10},
+  pages={2387--2400},
+  year={2013},
+  publisher={IEEE}
+}
@@ -1,8 +0,0 @@
-*************************************************
-calib3d. Camera Calibration and 3D Reconstruction
-*************************************************
-
-.. toctree::
-    :maxdepth: 2
-
-    camera_calibration_and_3d_reconstruction
@@ -41,8 +41,8 @@
 //
 //M*/

-#ifndef __OPENCV_CALIB3D_C_H__
-#define __OPENCV_CALIB3D_C_H__
+#ifndef OPENCV_CALIB3D_C_H
+#define OPENCV_CALIB3D_C_H

 #include "opencv2/core/core_c.h"

@@ -50,6 +50,10 @@
 extern "C" {
 #endif

+/** @addtogroup calib3d_c
+  @{
+  */
+
 /****************************************************************************************\
 *                      Camera Calibration, Pose Estimation and Stereo                    *
 \****************************************************************************************/
@@ -91,7 +95,8 @@ enum
 {
    CV_ITERATIVE = 0,
    CV_EPNP = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation"
-    CV_P3P = 2 // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
+    CV_P3P = 2, // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
+    CV_DLS = 3 // Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP"
 };

 CVAPI(int) cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
@@ -140,7 +145,9 @@ CVAPI(int) cvFindHomography( const CvMat* src_points,
                             CvMat* homography,
                             int method CV_DEFAULT(0),
                             double ransacReprojThreshold CV_DEFAULT(3),
-                             CvMat* mask CV_DEFAULT(0));
+                             CvMat* mask CV_DEFAULT(0),
+                             int maxIters CV_DEFAULT(2000),
+                             double confidence CV_DEFAULT(0.995));

 /* Computes RQ decomposition for 3x3 matrices */
 CVAPI(void) cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
@@ -236,7 +243,11 @@ CVAPI(void) cvDrawChessboardCorners( CvArr* image, CvSize pattern_size,
 #define CV_CALIB_RATIONAL_MODEL 16384
 #define CV_CALIB_THIN_PRISM_MODEL 32768
 #define CV_CALIB_FIX_S1_S2_S3_S4  65536
+#define CV_CALIB_TILTED_MODEL  262144
+#define CV_CALIB_FIX_TAUX_TAUY  524288
+#define CV_CALIB_FIX_TANGENT_DIST 2097152

+#define CV_CALIB_NINTRINSIC 18

 /* Finds intrinsic and extrinsic camera parameters
   from a few views of known calibration pattern */
@@ -368,6 +379,8 @@ CVAPI(void)  cvReprojectImageTo3D( const CvArr* disparityImage,
                                   CvArr* _3dImage, const CvMat* Q,
                                   int handleMissingValues CV_DEFAULT(0) );

+/** @} calib3d_c */
+
 #ifdef __cplusplus
 } // extern "C"

@@ -406,8 +419,9 @@ public:
    int state;
    int iters;
    bool completeSymmFlag;
+    int solveMethod;
 };

 #endif

-#endif /* __OPENCV_CALIB3D_C_H__ */
+#endif /* OPENCV_CALIB3D_C_H */
@@ -0,0 +1,31 @@
+{
+    "class_ignore_list": [
+        "CirclesGridFinderParameters",
+        "CirclesGridFinderParameters2"
+    ],
+    "namespaces_dict": {
+        "cv.fisheye": "fisheye"
+    },
+    "func_arg_fix" : {
+        "findFundamentalMat"  : { "points1" : {"ctype" : "vector_Point2f"},
+                                  "points2" : {"ctype" : "vector_Point2f"} },
+        "cornerSubPix" : { "corners" : {"ctype" : "vector_Point2f"} },
+        "findHomography" : { "srcPoints" : {"ctype" : "vector_Point2f"},
+                             "dstPoints" : {"ctype" : "vector_Point2f"} },
+        "solvePnP" : { "objectPoints" : {"ctype" : "vector_Point3f"},
+                       "imagePoints"  : {"ctype" : "vector_Point2f"},
+                       "distCoeffs"   : {"ctype" : "vector_double" } },
+        "solvePnPRansac" : { "objectPoints" : {"ctype" : "vector_Point3f"},
+                             "imagePoints"  : {"ctype" : "vector_Point2f"},
+                             "distCoeffs"   : {"ctype" : "vector_double" } },
+        "undistortPoints" : { "src" : {"ctype" : "vector_Point2f"},
+                              "dst" : {"ctype" : "vector_Point2f"} },
+        "projectPoints" : { "objectPoints" : {"ctype" : "vector_Point3f"},
+                            "imagePoints"  : {"ctype" : "vector_Point2f"},
+                            "distCoeffs"   : {"ctype" : "vector_double" } },
+        "initCameraMatrix2D" : { "objectPoints" : {"ctype" : "vector_vector_Point3f"},
+                                 "imagePoints"  : {"ctype" : "vector_vector_Point2f"} },
+        "findChessboardCorners" : { "corners" : {"ctype" : "vector_Point2f"} },
+        "drawChessboardCorners" : { "corners" : {"ctype" : "vector_Point2f"} }
+    }
+}
@@ -1,7 +1,8 @@
 package org.opencv.test.calib3d;

+import java.util.ArrayList;
+
 import org.opencv.calib3d.Calib3d;
-import org.opencv.core.Core;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
@@ -11,6 +12,7 @@ import org.opencv.core.Point;
 import org.opencv.core.Scalar;
 import org.opencv.core.Size;
 import org.opencv.test.OpenCVTestCase;
+import org.opencv.imgproc.Imgproc;

 public class Calib3dTest extends OpenCVTestCase {

@@ -162,7 +164,7 @@ public class Calib3dTest extends OpenCVTestCase {
    public void testFilterSpecklesMatDoubleIntDouble() {
        gray_16s_1024.copyTo(dst);
        Point center = new Point(gray_16s_1024.rows() / 2., gray_16s_1024.cols() / 2.);
-        Core.circle(dst, center, 1, Scalar.all(4096));
+        Imgproc.circle(dst, center, 1, Scalar.all(4096));

        assertMatNotEqual(gray_16s_1024, dst);
        Calib3d.filterSpeckles(dst, 1024.0, 100, 0.);
@@ -177,7 +179,7 @@ public class Calib3dTest extends OpenCVTestCase {
        Size patternSize = new Size(9, 6);
        MatOfPoint2f corners = new MatOfPoint2f();
        Calib3d.findChessboardCorners(grayChess, patternSize, corners);
-        assertTrue(!corners.empty());
+        assertFalse(corners.empty());
    }

    public void testFindChessboardCornersMatSizeMatInt() {
@@ -185,7 +187,16 @@ public class Calib3dTest extends OpenCVTestCase {
        MatOfPoint2f corners = new MatOfPoint2f();
        Calib3d.findChessboardCorners(grayChess, patternSize, corners, Calib3d.CALIB_CB_ADAPTIVE_THRESH + Calib3d.CALIB_CB_NORMALIZE_IMAGE
                + Calib3d.CALIB_CB_FAST_CHECK);
-        assertTrue(!corners.empty());
+        assertFalse(corners.empty());
+    }
+
+    public void testFind4QuadCornerSubpix() {
+        Size patternSize = new Size(9, 6);
+        MatOfPoint2f corners = new MatOfPoint2f();
+        Size region_size = new Size(5, 5);
+        Calib3d.findChessboardCorners(grayChess, patternSize, corners);
+        Calib3d.find4QuadCornerSubpix(grayChess, corners, region_size);
+        assertFalse(corners.empty());
    }

    public void testFindCirclesGridMatSizeMat() {
@@ -199,7 +210,7 @@ public class Calib3dTest extends OpenCVTestCase {
        for (int i = 0; i < 5; i++)
            for (int j = 0; j < 5; j++) {
                Point pt = new Point(size * (2 * i + 1) / 10, size * (2 * j + 1) / 10);
-                Core.circle(img, pt, 10, new Scalar(0), -1);
+                Imgproc.circle(img, pt, 10, new Scalar(0), -1);
            }

        assertTrue(Calib3d.findCirclesGrid(img, new Size(5, 5), centers));
@@ -224,7 +235,7 @@ public class Calib3dTest extends OpenCVTestCase {
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 5; j++) {
                Point pt = new Point(offsetx + (2 * i + j % 2) * step, offsety + step * j);
-                Core.circle(img, pt, 10, new Scalar(0), -1);
+                Imgproc.circle(img, pt, 10, new Scalar(0), -1);
            }

        assertTrue(Calib3d.findCirclesGrid(img, new Size(3, 5), centers, Calib3d.CALIB_CB_CLUSTERING
@@ -236,6 +247,8 @@ public class Calib3dTest extends OpenCVTestCase {
    }

    public void testFindFundamentalMatListOfPointListOfPoint() {
+        fail("Not yet implemented");
+/*
        int minFundamentalMatPoints = 8;

        MatOfPoint2f pts = new MatOfPoint2f();
@@ -252,6 +265,7 @@ public class Calib3dTest extends OpenCVTestCase {
        truth = new Mat(3, 3, CvType.CV_64F);
        truth.put(0, 0, 0, -0.577, 0.288, 0.577, 0, 0.288, -0.288, -0.288, 0);
        assertMatEqual(truth, fm, EPS);
+*/
    }

    public void testFindFundamentalMatListOfPointListOfPointInt() {
@@ -496,7 +510,7 @@ public class Calib3dTest extends OpenCVTestCase {
    }

    public void testSolvePnPListOfPoint3ListOfPointMatMatMatMat() {
-        Mat intrinsics = Mat.eye(3, 3, CvType.CV_32F);
+        Mat intrinsics = Mat.eye(3, 3, CvType.CV_64F);
        intrinsics.put(0, 0, 400);
        intrinsics.put(1, 1, 400);
        intrinsics.put(0, 2, 640 / 2);
@@ -599,4 +613,73 @@ public class Calib3dTest extends OpenCVTestCase {
        Calib3d.computeCorrespondEpilines(left, 1, fundamental, lines);
        assertMatEqual(truth, lines, EPS);
    }
+
+    public void testConstants()
+    {
+        // calib3d.hpp: some constants have conflict with constants from 'fisheye' namespace
+        assertEquals(1, Calib3d.CALIB_USE_INTRINSIC_GUESS);
+        assertEquals(2, Calib3d.CALIB_FIX_ASPECT_RATIO);
+        assertEquals(4, Calib3d.CALIB_FIX_PRINCIPAL_POINT);
+        assertEquals(8, Calib3d.CALIB_ZERO_TANGENT_DIST);
+        assertEquals(16, Calib3d.CALIB_FIX_FOCAL_LENGTH);
+        assertEquals(32, Calib3d.CALIB_FIX_K1);
+        assertEquals(64, Calib3d.CALIB_FIX_K2);
+        assertEquals(128, Calib3d.CALIB_FIX_K3);
+        assertEquals(0x0800, Calib3d.CALIB_FIX_K4);
+        assertEquals(0x1000, Calib3d.CALIB_FIX_K5);
+        assertEquals(0x2000, Calib3d.CALIB_FIX_K6);
+        assertEquals(0x4000, Calib3d.CALIB_RATIONAL_MODEL);
+        assertEquals(0x8000, Calib3d.CALIB_THIN_PRISM_MODEL);
+        assertEquals(0x10000, Calib3d.CALIB_FIX_S1_S2_S3_S4);
+        assertEquals(0x40000, Calib3d.CALIB_TILTED_MODEL);
+        assertEquals(0x80000, Calib3d.CALIB_FIX_TAUX_TAUY);
+        assertEquals(0x100000, Calib3d.CALIB_USE_QR);
+        assertEquals(0x200000, Calib3d.CALIB_FIX_TANGENT_DIST);
+        assertEquals(0x100, Calib3d.CALIB_FIX_INTRINSIC);
+        assertEquals(0x200, Calib3d.CALIB_SAME_FOCAL_LENGTH);
+        assertEquals(0x400, Calib3d.CALIB_ZERO_DISPARITY);
+        assertEquals((1 << 17), Calib3d.CALIB_USE_LU);
+        assertEquals((1 << 22), Calib3d.CALIB_USE_EXTRINSIC_GUESS);
+    }
+
+    public void testSolvePnPGeneric_regression_16040() {
+        Mat intrinsics = Mat.eye(3, 3, CvType.CV_64F);
+        intrinsics.put(0, 0, 400);
+        intrinsics.put(1, 1, 400);
+        intrinsics.put(0, 2, 640 / 2);
+        intrinsics.put(1, 2, 480 / 2);
+
+        final int minPnpPointsNum = 4;
+
+        MatOfPoint3f points3d = new MatOfPoint3f();
+        points3d.alloc(minPnpPointsNum);
+        MatOfPoint2f points2d = new MatOfPoint2f();
+        points2d.alloc(minPnpPointsNum);
+
+        for (int i = 0; i < minPnpPointsNum; i++) {
+            double x = Math.random() * 100 - 50;
+            double y = Math.random() * 100 - 50;
+            points2d.put(i, 0, x, y); //add(new Point(x, y));
+            points3d.put(i, 0, 0, y, x); // add(new Point3(0, y, x));
+        }
+
+        ArrayList<Mat> rvecs = new ArrayList<Mat>();
+        ArrayList<Mat> tvecs = new ArrayList<Mat>();
+
+        Mat rvec = new Mat();
+        Mat tvec = new Mat();
+
+        Mat reprojectionError = new Mat(2, 1, CvType.CV_64FC1);
+
+        Calib3d.solvePnPGeneric(points3d, points2d, intrinsics, new MatOfDouble(), rvecs, tvecs, false, Calib3d.SOLVEPNP_IPPE, rvec, tvec, reprojectionError);
+
+        Mat truth_rvec = new Mat(3, 1, CvType.CV_64F);
+        truth_rvec.put(0, 0, 0, Math.PI / 2, 0);
+
+        Mat truth_tvec = new Mat(3, 1, CvType.CV_64F);
+        truth_tvec.put(0, 0, -320, -240, 400);
+
+        assertMatEqual(truth_rvec, rvecs.get(0), 10 * EPS);
+        assertMatEqual(truth_tvec, tvecs.get(0), 1000 * EPS);
+    }
 }
@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+
+'''
+camera calibration for distorted images with chess board samples
+reads distorted images, calculates the calibration and write undistorted images
+'''
+
+# Python 2/3 compatibility
+from __future__ import print_function
+
+import numpy as np
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class calibration_test(NewOpenCVTests):
+
+    def test_calibration(self):
+        img_names = []
+        for i in range(1, 15):
+            if i < 10:
+                img_names.append('samples/data/left0{}.jpg'.format(str(i)))
+            elif i != 10:
+                img_names.append('samples/data/left{}.jpg'.format(str(i)))
+
+        square_size = 1.0
+        pattern_size = (9, 6)
+        pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
+        pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
+        pattern_points *= square_size
+
+        obj_points = []
+        img_points = []
+        h, w = 0, 0
+        for fn in img_names:
+            img = self.get_sample(fn, 0)
+            if img is None:
+                continue
+
+            h, w = img.shape[:2]
+            found, corners = cv.findChessboardCorners(img, pattern_size)
+            if found:
+                term = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_COUNT, 30, 0.1)
+                cv.cornerSubPix(img, corners, (5, 5), (-1, -1), term)
+
+            if not found:
+                continue
+
+            img_points.append(corners.reshape(-1, 2))
+            obj_points.append(pattern_points)
+
+        # calculate camera distortion
+        rms, camera_matrix, dist_coefs, _rvecs, _tvecs = cv.calibrateCamera(obj_points, img_points, (w, h), None, None, flags = 0)
+
+        eps = 0.01
+        normCamEps = 10.0
+        normDistEps = 0.05
+
+        cameraMatrixTest = [[ 532.80992189,    0.,          342.4952186 ],
+         [   0.,         532.93346422,  233.8879292 ],
+         [   0.,            0.,            1.        ]]
+
+        distCoeffsTest = [ -2.81325576e-01,   2.91130406e-02,
+           1.21234330e-03,  -1.40825372e-04, 1.54865844e-01]
+
+        self.assertLess(abs(rms - 0.196334638034), eps)
+        self.assertLess(cv.norm(camera_matrix - cameraMatrixTest, cv.NORM_L1), normCamEps)
+        self.assertLess(cv.norm(dist_coefs - distCoeffsTest, cv.NORM_L1), normDistEps)
+
+
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()
@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+# Python 2/3 compatibility
+from __future__ import print_function
+
+import numpy as np
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class solvepnp_test(NewOpenCVTests):
+
+    def test_regression_16040(self):
+        obj_points = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], dtype=np.float32)
+        img_points = np.array(
+            [[700, 400], [700, 600], [900, 600], [900, 400]], dtype=np.float32
+        )
+
+        cameraMatrix = np.array(
+            [[712.0634, 0, 800], [0, 712.540, 500], [0, 0, 1]], dtype=np.float32
+        )
+        distCoeffs = np.array([[0, 0, 0, 0]], dtype=np.float32)
+        r = np.array([], dtype=np.float32)
+        x, r, t, e = cv.solvePnPGeneric(
+            obj_points, img_points, cameraMatrix, distCoeffs, reprojectionError=r
+        )
+
+    def test_regression_16040_2(self):
+        obj_points = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], dtype=np.float32)
+        img_points = np.array(
+            [[[700, 400], [700, 600], [900, 600], [900, 400]]], dtype=np.float32
+        )
+
+        cameraMatrix = np.array(
+            [[712.0634, 0, 800], [0, 712.540, 500], [0, 0, 1]], dtype=np.float32
+        )
+        distCoeffs = np.array([[0, 0, 0, 0]], dtype=np.float32)
+        r = np.array([], dtype=np.float32)
+        x, r, t, e = cv.solvePnPGeneric(
+            obj_points, img_points, cameraMatrix, distCoeffs, reprojectionError=r
+        )
+
+    def test_regression_16049(self):
+        obj_points = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], dtype=np.float32)
+        img_points = np.array(
+            [[[700, 400], [700, 600], [900, 600], [900, 400]]], dtype=np.float32
+        )
+
+        cameraMatrix = np.array(
+            [[712.0634, 0, 800], [0, 712.540, 500], [0, 0, 1]], dtype=np.float32
+        )
+        distCoeffs = np.array([[0, 0, 0, 0]], dtype=np.float32)
+        x, r, t, e = cv.solvePnPGeneric(
+            obj_points, img_points, cameraMatrix, distCoeffs
+        )
+        if e is None:
+            # noArray() is supported, see https://github.com/opencv/opencv/issues/16049
+            pass
+        else:
+            eDump = cv.utils.dumpInputArray(e)
+            self.assertEqual(eDump, "InputArray: empty()=false kind=0x00010000 flags=0x01010000 total(-1)=1 dims(-1)=2 size(-1)=1x1 type(-1)=CV_32FC1")
+
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()
@@ -45,10 +45,10 @@

 #ifdef HAVE_OPENCL

-namespace cvtest {
+namespace opencv_test {
 namespace ocl {

-typedef std::tr1::tuple<int, int> StereoBMFixture_t;
+typedef tuple<int, int> StereoBMFixture_t;
 typedef TestBaseWithParam<StereoBMFixture_t> StereoBMFixture;

 OCL_PERF_TEST_P(StereoBMFixture, StereoBM, ::testing::Combine(OCL_PERF_ENUM(32, 64, 128), OCL_PERF_ENUM(11,21) ) )
@@ -63,7 +63,7 @@ OCL_PERF_TEST_P(StereoBMFixture, StereoBM, ::testing::Combine(OCL_PERF_ENUM(32,

    declare.in(left, right);

-    Ptr<StereoBM> bm = createStereoBM( n_disp, winSize );
+    Ptr<StereoBM> bm = StereoBM::create( n_disp, winSize );
    bm->setPreFilterType(bm->PREFILTER_XSOBEL);
    bm->setTextureThreshold(0);

@@ -0,0 +1,165 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//                        (3-clause BSD License)
+//
+// Copyright (C) 2015-2016, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistributions in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * Neither the names of the copyright holders nor the names of the contributors
+//     may be used to endorse or promote products derived from this software
+//     without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall copyright holders or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "perf_precomp.hpp"
+#include <algorithm>
+#include <functional>
+
+namespace opencv_test
+{
+using namespace perf;
+
+CV_ENUM(Method, RANSAC, LMEDS)
+typedef tuple<int, double, Method, size_t> AffineParams;
+typedef TestBaseWithParam<AffineParams> EstimateAffine;
+#define ESTIMATE_PARAMS Combine(Values(100000, 5000, 100), Values(0.99, 0.95, 0.9), Method::all(), Values(10, 0))
+
+static float rngIn(float from, float to) { return from + (to-from) * (float)theRNG(); }
+
+static Mat rngPartialAffMat() {
+    double theta = rngIn(0, (float)CV_PI*2.f);
+    double scale = rngIn(0, 3);
+    double tx = rngIn(-2, 2);
+    double ty = rngIn(-2, 2);
+    double aff[2*3] = { std::cos(theta) * scale, -std::sin(theta) * scale, tx,
+                        std::sin(theta) * scale,  std::cos(theta) * scale, ty };
+    return Mat(2, 3, CV_64F, aff).clone();
+}
+
+PERF_TEST_P( EstimateAffine, EstimateAffine2D, ESTIMATE_PARAMS )
+{
+    AffineParams params = GetParam();
+    const int n = get<0>(params);
+    const double confidence = get<1>(params);
+    const int method = get<2>(params);
+    const size_t refining = get<3>(params);
+
+    Mat aff(2, 3, CV_64F);
+    cv::randu(aff, -2., 2.);
+
+    // LMEDS can't handle more than 50% outliers (by design)
+    int m;
+    if (method == LMEDS)
+        m = 3*n/5;
+    else
+        m = 2*n/5;
+    const float shift_outl = 15.f;
+    const float noise_level = 20.f;
+
+    Mat fpts(1, n, CV_32FC2);
+    Mat tpts(1, n, CV_32FC2);
+
+    randu(fpts, 0., 100.);
+    transform(fpts, tpts, aff);
+
+    /* adding noise to some points */
+    Mat outliers = tpts.colRange(m, n);
+    outliers.reshape(1) += shift_outl;
+
+    Mat noise (outliers.size(), outliers.type());
+    randu(noise, 0., noise_level);
+    outliers += noise;
+
+    Mat aff_est;
+    vector<uchar> inliers (n);
+
+    warmup(inliers, WARMUP_WRITE);
+    warmup(fpts, WARMUP_READ);
+    warmup(tpts, WARMUP_READ);
+
+    TEST_CYCLE()
+    {
+        aff_est = estimateAffine2D(fpts, tpts, inliers, method, 3, 2000, confidence, refining);
+    }
+
+    // we already have accuracy tests
+    SANITY_CHECK_NOTHING();
+}
+
+PERF_TEST_P( EstimateAffine, EstimateAffinePartial2D, ESTIMATE_PARAMS )
+{
+    AffineParams params = GetParam();
+    const int n = get<0>(params);
+    const double confidence = get<1>(params);
+    const int method = get<2>(params);
+    const size_t refining = get<3>(params);
+
+    Mat aff = rngPartialAffMat();
+
+    int m;
+    // LMEDS can't handle more than 50% outliers (by design)
+    if (method == LMEDS)
+        m = 3*n/5;
+    else
+        m = 2*n/5;
+    const float shift_outl = 15.f;    const float noise_level = 20.f;
+
+    Mat fpts(1, n, CV_32FC2);
+    Mat tpts(1, n, CV_32FC2);
+
+    randu(fpts, 0., 100.);
+    transform(fpts, tpts, aff);
+
+    /* adding noise*/
+    Mat outliers = tpts.colRange(m, n);
+    outliers.reshape(1) += shift_outl;
+
+    Mat noise (outliers.size(), outliers.type());
+    randu(noise, 0., noise_level);
+    outliers += noise;
+
+    Mat aff_est;
+    vector<uchar> inliers (n);
+
+    warmup(inliers, WARMUP_WRITE);
+    warmup(fpts, WARMUP_READ);
+    warmup(tpts, WARMUP_READ);
+
+    TEST_CYCLE()
+    {
+        aff_est = estimateAffinePartial2D(fpts, tpts, inliers, method, 3, 2000, confidence, refining);
+    }
+
+    // we already have accuracy tests
+    SANITY_CHECK_NOTHING();
+}
+
+} // namespace opencv_test
@@ -1,12 +1,10 @@
 #include "perf_precomp.hpp"

-using namespace std;
-using namespace cv;
+namespace opencv_test
+{
 using namespace perf;
-using std::tr1::make_tuple;
-using std::tr1::get;

-typedef std::tr1::tuple<std::string, cv::Size> String_Size_t;
+typedef tuple<std::string, cv::Size> String_Size_t;
 typedef perf::TestBaseWithParam<String_Size_t> String_Size;

 PERF_TEST_P(String_Size, asymm_circles_grid, testing::Values(
@@ -40,3 +38,5 @@ PERF_TEST_P(String_Size, asymm_circles_grid, testing::Values(

    SANITY_CHECK(ptvec, 2);
 }
+
+} // namespace
@@ -1,26 +1,20 @@
 #include "perf_precomp.hpp"

-#ifdef HAVE_TBB
-#include "tbb/task_scheduler_init.h"
-#endif
-
-using namespace std;
-using namespace cv;
+namespace opencv_test
+{
 using namespace perf;
-using std::tr1::make_tuple;
-using std::tr1::get;

-CV_ENUM(pnpAlgo, ITERATIVE, EPNP /*, P3P*/)
+CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS, SOLVEPNP_UPNP)

-typedef std::tr1::tuple<int, pnpAlgo> PointsNum_Algo_t;
+typedef tuple<int, pnpAlgo> PointsNum_Algo_t;
 typedef perf::TestBaseWithParam<PointsNum_Algo_t> PointsNum_Algo;

 typedef perf::TestBaseWithParam<int> PointsNum;

 PERF_TEST_P(PointsNum_Algo, solvePnP,
-            testing::Combine(
-                testing::Values(/*4,*/ 3*9, 7*13), //TODO: find why results on 4 points are too unstable
-                testing::Values((int)ITERATIVE, (int)EPNP)
+            testing::Combine( //When non planar, DLT needs at least 6 points for SOLVEPNP_ITERATIVE flag
+                testing::Values(6, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
+                testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS)
                )
            )
 {
@@ -48,22 +42,31 @@ PERF_TEST_P(PointsNum_Algo, solvePnP,
    //add noise
    Mat noise(1, (int)points2d.size(), CV_32FC2);
    randu(noise, 0, 0.01);
-    add(points2d, noise, points2d);
+    cv::add(points2d, noise, points2d);

    declare.in(points3d, points2d);
+    declare.time(100);

    TEST_CYCLE_N(1000)
    {
-        solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
+        cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
    }

-    SANITY_CHECK(rvec, 1e-6);
-    SANITY_CHECK(tvec, 1e-3);
+    SANITY_CHECK(rvec, 1e-4);
+    SANITY_CHECK(tvec, 1e-4);
 }

-PERF_TEST(PointsNum_Algo, solveP3P)
+PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
+            testing::Combine(
+                testing::Values(5),
+                testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
+                )
+            )
 {
-    int pointsNum = 4;
+    int pointsNum = get<0>(GetParam());
+    pnpAlgo algo = get<1>(GetParam());
+    if( algo == SOLVEPNP_P3P )
+        pointsNum = 4;

    vector<Point2f> points2d(pointsNum);
    vector<Point3f> points3d(pointsNum);
@@ -72,8 +75,8 @@ PERF_TEST(PointsNum_Algo, solveP3P)

    Mat distortion = Mat::zeros(5, 1, CV_32FC1);
    Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
-    intrinsics.at<float> (0, 0) = 400.0;
-    intrinsics.at<float> (1, 1) = 400.0;
+    intrinsics.at<float> (0, 0) = 400.0f;
+    intrinsics.at<float> (1, 1) = 400.0f;
    intrinsics.at<float> (0, 2) = 640 / 2;
    intrinsics.at<float> (1, 2) = 480 / 2;

@@ -81,26 +84,31 @@ PERF_TEST(PointsNum_Algo, solveP3P)
    warmup(rvec, WARMUP_RNG);
    warmup(tvec, WARMUP_RNG);

-    projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
+    // normalize Rodrigues vector
+    Mat rvec_tmp = Mat::eye(3, 3, CV_32F);
+    cv::Rodrigues(rvec, rvec_tmp);
+    cv::Rodrigues(rvec_tmp, rvec);
+
+    cv::projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);

    //add noise
    Mat noise(1, (int)points2d.size(), CV_32FC2);
-    randu(noise, 0, 0.01);
-    add(points2d, noise, points2d);
+    randu(noise, -0.001, 0.001);
+    cv::add(points2d, noise, points2d);

    declare.in(points3d, points2d);
    declare.time(100);

    TEST_CYCLE_N(1000)
    {
-        solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, P3P);
+        cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
    }

-    SANITY_CHECK(rvec, 1e-6);
-    SANITY_CHECK(tvec, 1e-6);
+    SANITY_CHECK(rvec, 1e-1);
+    SANITY_CHECK(tvec, 1e-2);
 }

-PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(4, 3*9, 7*13))
+PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(5, 3*9, 7*13))
 {
    int count = GetParam();

@@ -117,8 +125,10 @@ PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(4, 3*9, 7*13))
    Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0));

    vector<cv::Point2f> image_vec;
+
    Mat rvec_gold(1, 3, CV_32FC1);
    randu(rvec_gold, 0, 1);
+
    Mat tvec_gold(1, 3, CV_32FC1);
    randu(tvec_gold, 0, 1);
    projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec);
@@ -128,16 +138,13 @@ PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(4, 3*9, 7*13))
    Mat rvec;
    Mat tvec;

-#ifdef HAVE_TBB
-    // limit concurrency to get deterministic result
-    tbb::task_scheduler_init one_thread(1);
-#endif
-
    TEST_CYCLE()
    {
-        solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
+        cv::solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
    }

    SANITY_CHECK(rvec, 1e-6);
    SANITY_CHECK(tvec, 1e-6);
 }
+
+} // namespace
@@ -1,21 +1,10 @@
-#ifdef __GNUC__
-#  pragma GCC diagnostic ignored "-Wmissing-declarations"
-#  if defined __clang__ || defined __APPLE__
-#    pragma GCC diagnostic ignored "-Wmissing-prototypes"
-#    pragma GCC diagnostic ignored "-Wextra"
-#  endif
-#endif
-
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
 #ifndef __OPENCV_PERF_PRECOMP_HPP__
 #define __OPENCV_PERF_PRECOMP_HPP__

 #include "opencv2/ts.hpp"
 #include "opencv2/calib3d.hpp"
-#include "opencv2/imgcodecs.hpp"
-#include "opencv2/imgproc.hpp"
-
-#ifdef GTEST_CREATE_SHARED_LIBRARY
-#error no modules except ts should have GTEST_CREATE_SHARED_LIBRARY defined
-#endif

 #endif
@@ -0,0 +1,182 @@
+/*
+ *  By downloading, copying, installing or using the software you agree to this license.
+ *  If you do not agree to this license, do not download, install,
+ *  copy or use the software.
+ *
+ *
+ *  License Agreement
+ *  For Open Source Computer Vision Library
+ *  (3 - clause BSD License)
+ *
+ *  Redistribution and use in source and binary forms, with or without modification,
+ *  are permitted provided that the following conditions are met :
+ *
+ *  * Redistributions of source code must retain the above copyright notice,
+ *  this list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *  this list of conditions and the following disclaimer in the documentation
+ *  and / or other materials provided with the distribution.
+ *
+ *  * Neither the names of the copyright holders nor the names of the contributors
+ *  may be used to endorse or promote products derived from this software
+ *  without specific prior written permission.
+ *
+ *  This software is provided by the copyright holders and contributors "as is" and
+ *  any express or implied warranties, including, but not limited to, the implied
+ *  warranties of merchantability and fitness for a particular purpose are disclaimed.
+ *  In no event shall copyright holders or contributors be liable for any direct,
+ *  indirect, incidental, special, exemplary, or consequential damages
+ *  (including, but not limited to, procurement of substitute goods or services;
+ *  loss of use, data, or profits; or business interruption) however caused
+ *  and on any theory of liability, whether in contract, strict liability,
+ *  or tort(including negligence or otherwise) arising in any way out of
+ *  the use of this software, even if advised of the possibility of such damage.
+ */
+
+#include "perf_precomp.hpp"
+
+namespace opencv_test
+{
+using namespace perf;
+using namespace testing;
+
+static void MakeArtificialExample(Mat& dst_left_view, Mat& dst_view);
+
+CV_ENUM(SGBMModes, StereoSGBM::MODE_SGBM, StereoSGBM::MODE_SGBM_3WAY, StereoSGBM::MODE_HH4);
+typedef tuple<Size, int, SGBMModes> SGBMParams;
+typedef TestBaseWithParam<SGBMParams> TestStereoCorrespSGBM;
+
+#ifndef _DEBUG
+PERF_TEST_P( TestStereoCorrespSGBM, SGBM, Combine(Values(Size(1280,720),Size(640,480)), Values(256,128), SGBMModes::all()) )
+#else
+PERF_TEST_P( TestStereoCorrespSGBM, DISABLED_TooLongInDebug_SGBM, Combine(Values(Size(1280,720),Size(640,480)), Values(256,128), SGBMModes::all()) )
+#endif
+{
+    SGBMParams params = GetParam();
+
+    Size sz              = get<0>(params);
+    int num_disparities  = get<1>(params);
+    int mode             = get<2>(params);
+
+    Mat src_left(sz, CV_8UC3);
+    Mat src_right(sz, CV_8UC3);
+    Mat dst(sz, CV_16S);
+
+    MakeArtificialExample(src_left,src_right);
+
+    int wsize = 3;
+    int P1 = 8*src_left.channels()*wsize*wsize;
+    TEST_CYCLE()
+    {
+        Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,num_disparities,wsize,P1,4*P1,1,63,25,0,0,mode);
+        sgbm->compute(src_left,src_right,dst);
+    }
+
+    SANITY_CHECK(dst, .01, ERROR_RELATIVE);
+}
+
+typedef tuple<Size, int> BMParams;
+typedef TestBaseWithParam<BMParams> TestStereoCorrespBM;
+
+PERF_TEST_P(TestStereoCorrespBM, BM, Combine(Values(Size(1280, 720), Size(640, 480)), Values(256, 128)))
+{
+    BMParams params = GetParam();
+    Size sz = get<0>(params);
+    int num_disparities = get<1>(params);
+
+    Mat src_left(sz, CV_8UC1);
+    Mat src_right(sz, CV_8UC1);
+    Mat dst(sz, CV_16S);
+
+    MakeArtificialExample(src_left, src_right);
+
+    int wsize = 21;
+    TEST_CYCLE()
+    {
+        Ptr<StereoBM> bm = StereoBM::create(num_disparities, wsize);
+        bm->compute(src_left, src_right, dst);
+    }
+
+    SANITY_CHECK(dst, .01, ERROR_RELATIVE);
+}
+
+void MakeArtificialExample(Mat& dst_left_view, Mat& dst_right_view)
+{
+    RNG rng(0);
+    int w = dst_left_view.cols;
+    int h = dst_left_view.rows;
+
+    //params:
+    unsigned char bg_level = (unsigned char)rng.uniform(0.0,255.0);
+    unsigned char fg_level = (unsigned char)rng.uniform(0.0,255.0);
+    int rect_width  = (int)rng.uniform(w/16,w/2);
+    int rect_height = (int)rng.uniform(h/16,h/2);
+    int rect_disparity = (int)(0.15*w);
+    double sigma = 3.0;
+
+    int rect_x_offset = (w-rect_width) /2;
+    int rect_y_offset = (h-rect_height)/2;
+
+    if(dst_left_view.channels()==3)
+    {
+        dst_left_view  = Scalar(Vec3b(bg_level,bg_level,bg_level));
+        dst_right_view = Scalar(Vec3b(bg_level,bg_level,bg_level));
+    }
+    else
+    {
+        dst_left_view  = Scalar(bg_level);
+        dst_right_view = Scalar(bg_level);
+    }
+
+    Mat dst_left_view_rect = Mat(dst_left_view, Rect(rect_x_offset,rect_y_offset,rect_width,rect_height));
+    if(dst_left_view.channels()==3)
+        dst_left_view_rect = Scalar(Vec3b(fg_level,fg_level,fg_level));
+    else
+        dst_left_view_rect = Scalar(fg_level);
+
+    rect_x_offset-=rect_disparity;
+
+    Mat dst_right_view_rect = Mat(dst_right_view, Rect(rect_x_offset,rect_y_offset,rect_width,rect_height));
+    if(dst_right_view.channels()==3)
+        dst_right_view_rect = Scalar(Vec3b(fg_level,fg_level,fg_level));
+    else
+        dst_right_view_rect = Scalar(fg_level);
+
+    //add some gaussian noise:
+    unsigned char *l, *r;
+    for(int i=0;i<h;i++)
+    {
+        l = dst_left_view.ptr(i);
+        r = dst_right_view.ptr(i);
+
+        if(dst_left_view.channels()==3)
+        {
+            for(int j=0;j<w;j++)
+            {
+                l[0] = saturate_cast<unsigned char>(l[0] + rng.gaussian(sigma));
+                l[1] = saturate_cast<unsigned char>(l[1] + rng.gaussian(sigma));
+                l[2] = saturate_cast<unsigned char>(l[2] + rng.gaussian(sigma));
+                l+=3;
+
+                r[0] = saturate_cast<unsigned char>(r[0] + rng.gaussian(sigma));
+                r[1] = saturate_cast<unsigned char>(r[1] + rng.gaussian(sigma));
+                r[2] = saturate_cast<unsigned char>(r[2] + rng.gaussian(sigma));
+                r+=3;
+            }
+        }
+        else
+        {
+            for(int j=0;j<w;j++)
+            {
+                l[0] = saturate_cast<unsigned char>(l[0] + rng.gaussian(sigma));
+                l++;
+
+                r[0] = saturate_cast<unsigned char>(r[0] + rng.gaussian(sigma));
+                r++;
+            }
+        }
+    }
+}
+
+}
@@ -0,0 +1,21 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+#include "perf_precomp.hpp"
+
+namespace opencv_test
+{
+using namespace perf;
+
+PERF_TEST(Undistort, InitUndistortMap)
+{
+    Size size_w_h(512 + 3, 512);
+    Mat k(3, 3, CV_32FC1);
+    Mat d(1, 14, CV_64FC1);
+    Mat dst(size_w_h, CV_32FC2);
+    declare.in(k, d, WARMUP_RNG).out(dst);
+    TEST_CYCLE() initUndistortRectifyMap(k, d, noArray(), k, size_w_h, CV_32FC2, dst, noArray());
+    SANITY_CHECK_NOTHING();
+}
+
+} // namespace
@@ -0,0 +1,453 @@
+#include "precomp.hpp"
+#include "ap3p.h"
+
+#include <cmath>
+#include <complex>
+#if defined (_MSC_VER) && (_MSC_VER <= 1700)
+static inline double cbrt(double x) { return (double)cv::cubeRoot((float)x); };
+#endif
+
+using namespace std;
+
+namespace {
+void solveQuartic(const double *factors, double *realRoots) {
+    const double &a4 = factors[0];
+    const double &a3 = factors[1];
+    const double &a2 = factors[2];
+    const double &a1 = factors[3];
+    const double &a0 = factors[4];
+
+    double a4_2 = a4 * a4;
+    double a3_2 = a3 * a3;
+    double a4_3 = a4_2 * a4;
+    double a2a4 = a2 * a4;
+
+    double p4 = (8 * a2a4 - 3 * a3_2) / (8 * a4_2);
+    double q4 = (a3_2 * a3 - 4 * a2a4 * a3 + 8 * a1 * a4_2) / (8 * a4_3);
+    double r4 = (256 * a0 * a4_3 - 3 * (a3_2 * a3_2) - 64 * a1 * a3 * a4_2 + 16 * a2a4 * a3_2) / (256 * (a4_3 * a4));
+
+    double p3 = ((p4 * p4) / 12 + r4) / 3; // /=-3
+    double q3 = (72 * r4 * p4 - 2 * p4 * p4 * p4 - 27 * q4 * q4) / 432; // /=2
+
+    double t; // *=2
+    complex<double> w;
+    if (q3 >= 0)
+        w = -sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
+    else
+        w = sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
+    if (w.imag() == 0.0) {
+        w.real(cbrt(w.real()));
+        t = 2.0 * (w.real() + p3 / w.real());
+    } else {
+        w = pow(w, 1.0 / 3);
+        t = 4.0 * w.real();
+    }
+
+    complex<double> sqrt_2m = sqrt(static_cast<complex<double> >(-2 * p4 / 3 + t));
+    double B_4A = -a3 / (4 * a4);
+    double complex1 = 4 * p4 / 3 + t;
+#if defined(__clang__) && defined(__arm__) && (__clang_major__ == 3 || __clang_major__ == 4) && !defined(__ANDROID__)
+    // details: https://github.com/opencv/opencv/issues/11135
+    // details: https://github.com/opencv/opencv/issues/11056
+    complex<double> complex2 = 2 * q4;
+    complex2 = complex<double>(complex2.real() / sqrt_2m.real(), 0);
+#else
+    complex<double> complex2 = 2 * q4 / sqrt_2m;
+#endif
+    double sqrt_2m_rh = sqrt_2m.real() / 2;
+    double sqrt1 = sqrt(-(complex1 + complex2)).real() / 2;
+    realRoots[0] = B_4A + sqrt_2m_rh + sqrt1;
+    realRoots[1] = B_4A + sqrt_2m_rh - sqrt1;
+    double sqrt2 = sqrt(-(complex1 - complex2)).real() / 2;
+    realRoots[2] = B_4A - sqrt_2m_rh + sqrt2;
+    realRoots[3] = B_4A - sqrt_2m_rh - sqrt2;
+}
+
+void polishQuarticRoots(const double *coeffs, double *roots) {
+    const int iterations = 2;
+    for (int i = 0; i < iterations; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            double error =
+                    (((coeffs[0] * roots[j] + coeffs[1]) * roots[j] + coeffs[2]) * roots[j] + coeffs[3]) * roots[j] +
+                    coeffs[4];
+            double
+                    derivative =
+                    ((4 * coeffs[0] * roots[j] + 3 * coeffs[1]) * roots[j] + 2 * coeffs[2]) * roots[j] + coeffs[3];
+            roots[j] -= error / derivative;
+        }
+    }
+}
+
+inline void vect_cross(const double *a, const double *b, double *result) {
+    result[0] = a[1] * b[2] - a[2] * b[1];
+    result[1] = -(a[0] * b[2] - a[2] * b[0]);
+    result[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+inline double vect_dot(const double *a, const double *b) {
+    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+}
+
+inline double vect_norm(const double *a) {
+    return sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
+}
+
+inline void vect_scale(const double s, const double *a, double *result) {
+    result[0] = a[0] * s;
+    result[1] = a[1] * s;
+    result[2] = a[2] * s;
+}
+
+inline void vect_sub(const double *a, const double *b, double *result) {
+    result[0] = a[0] - b[0];
+    result[1] = a[1] - b[1];
+    result[2] = a[2] - b[2];
+}
+
+inline void vect_divide(const double *a, const double d, double *result) {
+    result[0] = a[0] / d;
+    result[1] = a[1] / d;
+    result[2] = a[2] / d;
+}
+
+inline void mat_mult(const double a[3][3], const double b[3][3], double result[3][3]) {
+    result[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0];
+    result[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1];
+    result[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2];
+
+    result[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0];
+    result[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1];
+    result[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2];
+
+    result[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0];
+    result[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1];
+    result[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2];
+}
+}
+
+namespace cv {
+void ap3p::init_inverse_parameters() {
+    inv_fx = 1. / fx;
+    inv_fy = 1. / fy;
+    cx_fx = cx / fx;
+    cy_fy = cy / fy;
+}
+
+ap3p::ap3p(cv::Mat cameraMatrix) {
+    if (cameraMatrix.depth() == CV_32F)
+        init_camera_parameters<float>(cameraMatrix);
+    else
+        init_camera_parameters<double>(cameraMatrix);
+    init_inverse_parameters();
+}
+
+ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) {
+    fx = _fx;
+    fy = _fy;
+    cx = _cx;
+    cy = _cy;
+    init_inverse_parameters();
+}
+
+// This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
+// See https://arxiv.org/pdf/1701.08237.pdf
+// featureVectors: The 3 bearing measurements (normalized) stored as column vectors
+// worldPoints: The positions of the 3 feature points stored as column vectors
+// solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
+// solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
+int ap3p::computePoses(const double featureVectors[3][4],
+                       const double worldPoints[3][4],
+                       double solutionsR[4][3][3],
+                       double solutionsT[4][3],
+                       bool p4p) {
+
+    //world point vectors
+    double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]};
+    double w2[3] = {worldPoints[0][1], worldPoints[1][1], worldPoints[2][1]};
+    double w3[3] = {worldPoints[0][2], worldPoints[1][2], worldPoints[2][2]};
+    // k1
+    double u0[3];
+    vect_sub(w1, w2, u0);
+
+    double nu0 = vect_norm(u0);
+    double k1[3];
+    vect_divide(u0, nu0, k1);
+    // bi
+    double b1[3] = {featureVectors[0][0], featureVectors[1][0], featureVectors[2][0]};
+    double b2[3] = {featureVectors[0][1], featureVectors[1][1], featureVectors[2][1]};
+    double b3[3] = {featureVectors[0][2], featureVectors[1][2], featureVectors[2][2]};
+    // k3,tz
+    double k3[3];
+    vect_cross(b1, b2, k3);
+    double nk3 = vect_norm(k3);
+    vect_divide(k3, nk3, k3);
+
+    double tz[3];
+    vect_cross(b1, k3, tz);
+    // ui,vi
+    double v1[3];
+    vect_cross(b1, b3, v1);
+    double v2[3];
+    vect_cross(b2, b3, v2);
+
+    double u1[3];
+    vect_sub(w1, w3, u1);
+    // coefficients related terms
+    double u1k1 = vect_dot(u1, k1);
+    double k3b3 = vect_dot(k3, b3);
+    // f1i
+    double f11 = k3b3;
+    double f13 = vect_dot(k3, v1);
+    double f15 = -u1k1 * f11;
+    //delta
+    double nl[3];
+    vect_cross(u1, k1, nl);
+    double delta = vect_norm(nl);
+    vect_divide(nl, delta, nl);
+    f11 *= delta;
+    f13 *= delta;
+    // f2i
+    double u2k1 = u1k1 - nu0;
+    double f21 = vect_dot(tz, v2);
+    double f22 = nk3 * k3b3;
+    double f23 = vect_dot(k3, v2);
+    double f24 = u2k1 * f22;
+    double f25 = -u2k1 * f21;
+    f21 *= delta;
+    f22 *= delta;
+    f23 *= delta;
+    double g1 = f13 * f22;
+    double g2 = f13 * f25 - f15 * f23;
+    double g3 = f11 * f23 - f13 * f21;
+    double g4 = -f13 * f24;
+    double g5 = f11 * f22;
+    double g6 = f11 * f25 - f15 * f21;
+    double g7 = -f15 * f24;
+    double coeffs[5] = {g5 * g5 + g1 * g1 + g3 * g3,
+                        2 * (g5 * g6 + g1 * g2 + g3 * g4),
+                        g6 * g6 + 2 * g5 * g7 + g2 * g2 + g4 * g4 - g1 * g1 - g3 * g3,
+                        2 * (g6 * g7 - g1 * g2 - g3 * g4),
+                        g7 * g7 - g2 * g2 - g4 * g4};
+    double s[4];
+    solveQuartic(coeffs, s);
+    polishQuarticRoots(coeffs, s);
+
+    double temp[3];
+    vect_cross(k1, nl, temp);
+
+    double Ck1nl[3][3] =
+            {{k1[0], nl[0], temp[0]},
+             {k1[1], nl[1], temp[1]},
+             {k1[2], nl[2], temp[2]}};
+
+    double Cb1k3tzT[3][3] =
+            {{b1[0], b1[1], b1[2]},
+             {k3[0], k3[1], k3[2]},
+             {tz[0], tz[1], tz[2]}};
+
+    double b3p[3];
+    vect_scale((delta / k3b3), b3, b3p);
+
+    double X3 = worldPoints[0][3];
+    double Y3 = worldPoints[1][3];
+    double Z3 = worldPoints[2][3];
+    double mu3 = featureVectors[0][3];
+    double mv3 = featureVectors[1][3];
+    double reproj_errors[4];
+
+    int nb_solutions = 0;
+    for (int i = 0; i < 4; ++i) {
+        double ctheta1p = s[i];
+        if (abs(ctheta1p) > 1)
+            continue;
+        double stheta1p = sqrt(1 - ctheta1p * ctheta1p);
+        stheta1p = (k3b3 > 0) ? stheta1p : -stheta1p;
+        double ctheta3 = g1 * ctheta1p + g2;
+        double stheta3 = g3 * ctheta1p + g4;
+        double ntheta3 = stheta1p / ((g5 * ctheta1p + g6) * ctheta1p + g7);
+        ctheta3 *= ntheta3;
+        stheta3 *= ntheta3;
+
+        double C13[3][3] =
+                {{ctheta3,            0,         -stheta3},
+                 {stheta1p * stheta3, ctheta1p,  stheta1p * ctheta3},
+                 {ctheta1p * stheta3, -stheta1p, ctheta1p * ctheta3}};
+
+        double temp_matrix[3][3];
+        double R[3][3];
+        mat_mult(Ck1nl, C13, temp_matrix);
+        mat_mult(temp_matrix, Cb1k3tzT, R);
+
+        // R' * p3
+        double rp3[3] =
+                {w3[0] * R[0][0] + w3[1] * R[1][0] + w3[2] * R[2][0],
+                 w3[0] * R[0][1] + w3[1] * R[1][1] + w3[2] * R[2][1],
+                 w3[0] * R[0][2] + w3[1] * R[1][2] + w3[2] * R[2][2]};
+
+        double pxstheta1p[3];
+        vect_scale(stheta1p, b3p, pxstheta1p);
+
+        vect_sub(pxstheta1p, rp3, solutionsT[nb_solutions]);
+
+        solutionsR[nb_solutions][0][0] = R[0][0];
+        solutionsR[nb_solutions][1][0] = R[0][1];
+        solutionsR[nb_solutions][2][0] = R[0][2];
+        solutionsR[nb_solutions][0][1] = R[1][0];
+        solutionsR[nb_solutions][1][1] = R[1][1];
+        solutionsR[nb_solutions][2][1] = R[1][2];
+        solutionsR[nb_solutions][0][2] = R[2][0];
+        solutionsR[nb_solutions][1][2] = R[2][1];
+        solutionsR[nb_solutions][2][2] = R[2][2];
+
+        if (p4p) {
+            double X3p = solutionsR[nb_solutions][0][0] * X3 + solutionsR[nb_solutions][0][1] * Y3 + solutionsR[nb_solutions][0][2] * Z3 + solutionsT[nb_solutions][0];
+            double Y3p = solutionsR[nb_solutions][1][0] * X3 + solutionsR[nb_solutions][1][1] * Y3 + solutionsR[nb_solutions][1][2] * Z3 + solutionsT[nb_solutions][1];
+            double Z3p = solutionsR[nb_solutions][2][0] * X3 + solutionsR[nb_solutions][2][1] * Y3 + solutionsR[nb_solutions][2][2] * Z3 + solutionsT[nb_solutions][2];
+            double mu3p = X3p / Z3p;
+            double mv3p = Y3p / Z3p;
+            reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
+        }
+
+        nb_solutions++;
+    }
+
+    //sort the solutions
+    if (p4p) {
+        for (int i = 1; i < nb_solutions; i++) {
+            for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) {
+                std::swap(reproj_errors[j], reproj_errors[j-1]);
+                std::swap(solutionsR[j], solutionsR[j-1]);
+                std::swap(solutionsT[j], solutionsT[j-1]);
+            }
+        }
+    }
+
+    return nb_solutions;
+}
+
+bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints) {
+    CV_INSTRUMENT_REGION();
+
+    double rotation_matrix[3][3] = {}, translation[3] = {};
+    std::vector<double> points;
+    if (opoints.depth() == ipoints.depth()) {
+        if (opoints.depth() == CV_32F)
+            extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
+        else
+            extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
+    } else if (opoints.depth() == CV_32F)
+        extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
+    else
+        extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
+
+    bool result = solve(rotation_matrix, translation,
+                        points[0], points[1], points[2], points[3], points[4],
+                        points[5], points[6], points[7], points[8], points[9],
+                        points[10], points[11], points[12], points[13],points[14],
+                        points[15], points[16], points[17], points[18], points[19]);
+    cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
+    cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
+    return result;
+}
+
+int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints) {
+    CV_INSTRUMENT_REGION();
+
+    double rotation_matrix[4][3][3] = {}, translation[4][3] = {};
+    std::vector<double> points;
+    if (opoints.depth() == ipoints.depth()) {
+        if (opoints.depth() == CV_32F)
+            extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
+        else
+            extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
+    } else if (opoints.depth() == CV_32F)
+        extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
+    else
+        extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
+
+    const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
+    int solutions = solve(rotation_matrix, translation,
+                          points[0], points[1], points[2], points[3], points[4],
+                          points[5], points[6], points[7], points[8], points[9],
+                          points[10], points[11], points[12], points[13], points[14],
+                          points[15], points[16], points[17], points[18], points[19],
+                          p4p);
+
+    for (int i = 0; i < solutions; i++) {
+        cv::Mat R, tvec;
+        cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
+        cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
+
+        Rs.push_back(R);
+        tvecs.push_back(tvec);
+    }
+
+    return solutions;
+}
+
+bool
+ap3p::solve(double R[3][3], double t[3],
+            double mu0, double mv0, double X0, double Y0, double Z0,
+            double mu1, double mv1, double X1, double Y1, double Z1,
+            double mu2, double mv2, double X2, double Y2, double Z2,
+            double mu3, double mv3, double X3, double Y3, double Z3) {
+    double Rs[4][3][3] = {}, ts[4][3] = {};
+
+    const bool p4p = true;
+    int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);
+    if (n == 0)
+        return false;
+
+    for (int i = 0; i < 3; i++) {
+        for (int j = 0; j < 3; j++)
+            R[i][j] = Rs[0][i][j];
+        t[i] = ts[0][i];
+    }
+
+    return true;
+}
+
+int ap3p::solve(double R[4][3][3], double t[4][3],
+                double mu0, double mv0, double X0, double Y0, double Z0,
+                double mu1, double mv1, double X1, double Y1, double Z1,
+                double mu2, double mv2, double X2, double Y2, double Z2,
+                double mu3, double mv3, double X3, double Y3, double Z3,
+                bool p4p) {
+    double mk0, mk1, mk2;
+    double norm;
+
+    mu0 = inv_fx * mu0 - cx_fx;
+    mv0 = inv_fy * mv0 - cy_fy;
+    norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
+    mk0 = 1. / norm;
+    mu0 *= mk0;
+    mv0 *= mk0;
+
+    mu1 = inv_fx * mu1 - cx_fx;
+    mv1 = inv_fy * mv1 - cy_fy;
+    norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
+    mk1 = 1. / norm;
+    mu1 *= mk1;
+    mv1 *= mk1;
+
+    mu2 = inv_fx * mu2 - cx_fx;
+    mv2 = inv_fy * mv2 - cy_fy;
+    norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
+    mk2 = 1. / norm;
+    mu2 *= mk2;
+    mv2 *= mk2;
+
+    mu3 = inv_fx * mu3 - cx_fx;
+    mv3 = inv_fy * mv3 - cy_fy;
+    double mk3 = 1; //not used
+
+    double featureVectors[3][4] = {{mu0, mu1, mu2, mu3},
+                                   {mv0, mv1, mv2, mv3},
+                                   {mk0, mk1, mk2, mk3}};
+    double worldPoints[3][4] = {{X0, X1, X2, X3},
+                                {Y0, Y1, Y2, Y3},
+                                {Z0, Z1, Z2, Z3}};
+
+    return computePoses(featureVectors, worldPoints, R, t, p4p);
+}
+}
@@ -0,0 +1,75 @@
+#ifndef P3P_P3P_H
+#define P3P_P3P_H
+
+#include <opencv2/core.hpp>
+
+namespace cv {
+class ap3p {
+private:
+    template<typename T>
+    void init_camera_parameters(const cv::Mat &cameraMatrix) {
+        cx = cameraMatrix.at<T>(0, 2);
+        cy = cameraMatrix.at<T>(1, 2);
+        fx = cameraMatrix.at<T>(0, 0);
+        fy = cameraMatrix.at<T>(1, 1);
+    }
+
+    template<typename OpointType, typename IpointType>
+    void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
+        points.clear();
+        int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
+        points.resize(5*4); //resize vector to fit for p4p case
+        for (int i = 0; i < npoints; i++) {
+            points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
+            points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
+            points[i * 5 + 2] = opoints.at<OpointType>(i).x;
+            points[i * 5 + 3] = opoints.at<OpointType>(i).y;
+            points[i * 5 + 4] = opoints.at<OpointType>(i).z;
+        }
+        //Fill vectors with unused values for p3p case
+        for (int i = npoints; i < 4; i++) {
+            for (int j = 0; j < 5; j++) {
+                points[i * 5 + j] = 0;
+            }
+        }
+    }
+
+    void init_inverse_parameters();
+
+    double fx, fy, cx, cy;
+    double inv_fx, inv_fy, cx_fx, cy_fy;
+public:
+    ap3p() : fx(0), fy(0), cx(0), cy(0), inv_fx(0), inv_fy(0), cx_fx(0), cy_fy(0) {}
+
+    ap3p(double fx, double fy, double cx, double cy);
+
+    ap3p(cv::Mat cameraMatrix);
+
+    bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints);
+    int solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints);
+
+    int solve(double R[4][3][3], double t[4][3],
+              double mu0, double mv0, double X0, double Y0, double Z0,
+              double mu1, double mv1, double X1, double Y1, double Z1,
+              double mu2, double mv2, double X2, double Y2, double Z2,
+              double mu3, double mv3, double X3, double Y3, double Z3,
+              bool p4p);
+
+    bool solve(double R[3][3], double t[3],
+               double mu0, double mv0, double X0, double Y0, double Z0,
+               double mu1, double mv1, double X1, double Y1, double Z1,
+               double mu2, double mv2, double X2, double Y2, double Z2,
+               double mu3, double mv3, double X3, double Y3, double Z3);
+
+    // This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
+    // See https://arxiv.org/pdf/1701.08237.pdf
+    // featureVectors: 3 bearing measurements (normalized) stored as column vectors
+    // worldPoints: Positions of the 3 feature points stored as column vectors
+    // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
+    // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
+    int computePoses(const double featureVectors[3][4], const double worldPoints[3][4], double solutionsR[4][3][3],
+                     double solutionsT[4][3], bool p4p);
+
+};
+}
+#endif //P3P_P3P_H
@@ -0,0 +1,47 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+// This file contains wrappers for legacy OpenCV C API
+
+#include "precomp.hpp"
+#include "opencv2/calib3d/calib3d_c.h"
+
+using namespace cv;
+
+CV_IMPL void
+cvDrawChessboardCorners(CvArr* _image, CvSize pattern_size,
+                        CvPoint2D32f* corners, int count, int found)
+{
+    CV_Assert(corners != NULL); //CV_CheckNULL(corners, "NULL is not allowed for 'corners' parameter");
+    Mat image = cvarrToMat(_image);
+    CV_StaticAssert(sizeof(CvPoint2D32f) == sizeof(Point2f), "");
+    drawChessboardCorners(image, pattern_size, Mat(1, count, traits::Type<Point2f>::value, corners), found != 0);
+}
+
+CV_IMPL int
+cvFindChessboardCorners(const void* arr, CvSize pattern_size,
+                        CvPoint2D32f* out_corners_, int* out_corner_count,
+                        int flags)
+{
+    if (!out_corners_)
+        CV_Error( CV_StsNullPtr, "Null pointer to corners" );
+
+    Mat image = cvarrToMat(arr);
+    std::vector<Point2f> out_corners;
+
+    if (out_corner_count)
+        *out_corner_count = 0;
+
+    bool res = cv::findChessboardCorners(image, pattern_size, out_corners, flags);
+
+    int corner_count = (int)out_corners.size();
+    if (out_corner_count)
+        *out_corner_count = corner_count;
+    CV_CheckLE(corner_count, Size(pattern_size).area(), "Unexpected number of corners");
+    for (int i = 0; i < corner_count; ++i)
+    {
+        out_corners_[i] = cvPoint2D32f(out_corners[i]);
+    }
+    return res ? 1 : 0;
+}
@@ -1,66 +0,0 @@
-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-//  By downloading, copying, installing or using the software you agree to this license.
-//  If you do not agree to this license, do not download, install,
-//  copy or use the software.
-//
-//
-//                          License Agreement
-//                For Open Source Computer Vision Library
-//
-// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
-// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-//   * Redistribution's of source code must retain the above copyright notice,
-//     this list of conditions and the following disclaimer.
-//
-//   * Redistribution's in binary form must reproduce the above copyright notice,
-//     this list of conditions and the following disclaimer in the documentation
-//     and/or other materials provided with the distribution.
-//
-//   * The name of the copyright holders may not be used to endorse or promote products
-//     derived from this software without specific prior written permission.
-//
-// This software is provided by the copyright holders and contributors "as is" and
-// any express or implied warranties, including, but not limited to, the implied
-// warranties of merchantability and fitness for a particular purpose are disclaimed.
-// In no event shall the Intel Corporation or contributors be liable for any direct,
-// indirect, incidental, special, exemplary, or consequential damages
-// (including, but not limited to, procurement of substitute goods or services;
-// loss of use, data, or profits; or business interruption) however caused
-// and on any theory of liability, whether in contract, strict liability,
-// or tort (including negligence or otherwise) arising in any way out of
-// the use of this software, even if advised of the possibility of such damage.
-//
-//M*/
-
-#include "precomp.hpp"
-
-using namespace cv;
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-#if 0
-bool cv::initModule_calib3d(void)
-{
-    bool all = true;
-    all &= !RANSACPointSetRegistrator_info_auto.name().empty();
-    all &= !LMeDSPointSetRegistrator_info_auto.name().empty();
-    all &= !LMSolverImpl_info_auto.name().empty();
-
-    return all;
-}
-#endif
@@ -0,0 +1,770 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "precomp.hpp"
+#include "opencv2/calib3d.hpp"
+
+namespace cv {
+
+static Mat homogeneousInverse(const Mat& T)
+{
+    CV_Assert(T.rows == 4 && T.cols == 4);
+
+    Mat R = T(Rect(0, 0, 3, 3));
+    Mat t = T(Rect(3, 0, 1, 3));
+    Mat Rt = R.t();
+    Mat tinv = -Rt * t;
+    Mat Tinv = Mat::eye(4, 4, T.type());
+    Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
+    tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
+
+    return Tinv;
+}
+
+// q = rot2quatMinimal(R)
+//
+// R - 3x3 rotation matrix, or 4x4 homogeneous matrix
+// q - 3x1 unit quaternion <qx, qy, qz>
+// q = sin(theta/2) * v
+// theta - rotation angle
+// v     - unit rotation axis, |v| = 1
+static Mat rot2quatMinimal(const Mat& R)
+{
+    CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
+
+    double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
+    double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
+    double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
+    double trace = m00 + m11 + m22;
+
+    double qx, qy, qz;
+    if (trace > 0) {
+        double S = sqrt(trace + 1.0) * 2; // S=4*qw
+        qx = (m21 - m12) / S;
+        qy = (m02 - m20) / S;
+        qz = (m10 - m01) / S;
+    } else if ((m00 > m11)&(m00 > m22)) {
+        double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
+        qx = 0.25 * S;
+        qy = (m01 + m10) / S;
+        qz = (m02 + m20) / S;
+    } else if (m11 > m22) {
+        double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
+        qx = (m01 + m10) / S;
+        qy = 0.25 * S;
+        qz = (m12 + m21) / S;
+    } else {
+        double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
+        qx = (m02 + m20) / S;
+        qy = (m12 + m21) / S;
+        qz = 0.25 * S;
+    }
+
+    return (Mat_<double>(3,1) << qx, qy, qz);
+}
+
+static Mat skew(const Mat& v)
+{
+    CV_Assert(v.type() == CV_64FC1 && v.rows == 3 && v.cols == 1);
+
+    double vx = v.at<double>(0,0);
+    double vy = v.at<double>(1,0);
+    double vz = v.at<double>(2,0);
+    return (Mat_<double>(3,3) << 0, -vz, vy,
+                                vz, 0, -vx,
+                                -vy, vx, 0);
+}
+
+// R = quatMinimal2rot(q)
+//
+// q - 3x1 unit quaternion <qx, qy, qz>
+// R - 3x3 rotation matrix
+// q = sin(theta/2) * v
+// theta - rotation angle
+// v     - unit rotation axis, |v| = 1
+static Mat quatMinimal2rot(const Mat& q)
+{
+    CV_Assert(q.type() == CV_64FC1 && q.rows == 3 && q.cols == 1);
+
+    Mat p = q.t()*q;
+    double w = sqrt(1 - p.at<double>(0,0));
+
+    Mat diag_p = Mat::eye(3,3,CV_64FC1)*p.at<double>(0,0);
+    return 2*q*q.t() + 2*w*skew(q) + Mat::eye(3,3,CV_64FC1) - 2*diag_p;
+}
+
+// q = rot2quat(R)
+//
+// q - 4x1 unit quaternion <qw, qx, qy, qz>
+// R - 3x3 rotation matrix
+static Mat rot2quat(const Mat& R)
+{
+    CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
+
+    double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
+    double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
+    double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
+    double trace = m00 + m11 + m22;
+
+    double qw, qx, qy, qz;
+    if (trace > 0) {
+        double S = sqrt(trace + 1.0) * 2; // S=4*qw
+        qw = 0.25 * S;
+        qx = (m21 - m12) / S;
+        qy = (m02 - m20) / S;
+        qz = (m10 - m01) / S;
+    } else if ((m00 > m11)&(m00 > m22)) {
+        double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
+        qw = (m21 - m12) / S;
+        qx = 0.25 * S;
+        qy = (m01 + m10) / S;
+        qz = (m02 + m20) / S;
+    } else if (m11 > m22) {
+        double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
+        qw = (m02 - m20) / S;
+        qx = (m01 + m10) / S;
+        qy = 0.25 * S;
+        qz = (m12 + m21) / S;
+    } else {
+        double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
+        qw = (m10 - m01) / S;
+        qx = (m02 + m20) / S;
+        qy = (m12 + m21) / S;
+        qz = 0.25 * S;
+    }
+
+    return (Mat_<double>(4,1) << qw, qx, qy, qz);
+}
+
+// R = quat2rot(q)
+//
+// q - 4x1 unit quaternion <qw, qx, qy, qz>
+// R - 3x3 rotation matrix
+static Mat quat2rot(const Mat& q)
+{
+    CV_Assert(q.type() == CV_64FC1 && q.rows == 4 && q.cols == 1);
+
+    double qw = q.at<double>(0,0);
+    double qx = q.at<double>(1,0);
+    double qy = q.at<double>(2,0);
+    double qz = q.at<double>(3,0);
+
+    Mat R(3, 3, CV_64FC1);
+    R.at<double>(0, 0) = 1 - 2*qy*qy - 2*qz*qz;
+    R.at<double>(0, 1) = 2*qx*qy - 2*qz*qw;
+    R.at<double>(0, 2) = 2*qx*qz + 2*qy*qw;
+
+    R.at<double>(1, 0) = 2*qx*qy + 2*qz*qw;
+    R.at<double>(1, 1) = 1 - 2*qx*qx - 2*qz*qz;
+    R.at<double>(1, 2) = 2*qy*qz - 2*qx*qw;
+
+    R.at<double>(2, 0) = 2*qx*qz - 2*qy*qw;
+    R.at<double>(2, 1) = 2*qy*qz + 2*qx*qw;
+    R.at<double>(2, 2) = 1 - 2*qx*qx - 2*qy*qy;
+
+    return R;
+}
+
+// Kronecker product or tensor product
+// https://stackoverflow.com/a/36552682
+static Mat kron(const Mat& A, const Mat& B)
+{
+    CV_Assert(A.channels() == 1 && B.channels() == 1);
+
+    Mat1d Ad, Bd;
+    A.convertTo(Ad, CV_64F);
+    B.convertTo(Bd, CV_64F);
+
+    Mat1d Kd(Ad.rows * Bd.rows, Ad.cols * Bd.cols, 0.0);
+    for (int ra = 0; ra < Ad.rows; ra++)
+    {
+        for (int ca = 0; ca < Ad.cols; ca++)
+        {
+            Kd(Range(ra*Bd.rows, (ra + 1)*Bd.rows), Range(ca*Bd.cols, (ca + 1)*Bd.cols)) = Bd.mul(Ad(ra, ca));
+        }
+    }
+
+    Mat K;
+    Kd.convertTo(K, A.type());
+    return K;
+}
+
+// quaternion multiplication
+static Mat qmult(const Mat& s, const Mat& t)
+{
+    CV_Assert(s.type() == CV_64FC1 && t.type() == CV_64FC1);
+    CV_Assert(s.rows == 4 && s.cols == 1);
+    CV_Assert(t.rows == 4 && t.cols == 1);
+
+    double s0 = s.at<double>(0,0);
+    double s1 = s.at<double>(1,0);
+    double s2 = s.at<double>(2,0);
+    double s3 = s.at<double>(3,0);
+
+    double t0 = t.at<double>(0,0);
+    double t1 = t.at<double>(1,0);
+    double t2 = t.at<double>(2,0);
+    double t3 = t.at<double>(3,0);
+
+    Mat q(4, 1, CV_64FC1);
+    q.at<double>(0,0) = s0*t0 - s1*t1 - s2*t2 - s3*t3;
+    q.at<double>(1,0) = s0*t1 + s1*t0 + s2*t3 - s3*t2;
+    q.at<double>(2,0) = s0*t2 - s1*t3 + s2*t0 + s3*t1;
+    q.at<double>(3,0) = s0*t3 + s1*t2 - s2*t1 + s3*t0;
+
+    return q;
+}
+
+// dq = homogeneous2dualQuaternion(H)
+//
+// H  - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
+// dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
+static Mat homogeneous2dualQuaternion(const Mat& H)
+{
+    CV_Assert(H.type() == CV_64FC1 && H.rows == 4 && H.cols == 4);
+
+    Mat dualq(8, 1, CV_64FC1);
+    Mat R = H(Rect(0, 0, 3, 3));
+    Mat t = H(Rect(3, 0, 1, 3));
+
+    Mat q = rot2quat(R);
+    Mat qt = Mat::zeros(4, 1, CV_64FC1);
+    t.copyTo(qt(Rect(0, 1, 1, 3)));
+    Mat qprime = 0.5 * qmult(qt, q);
+
+    q.copyTo(dualq(Rect(0, 0, 1, 4)));
+    qprime.copyTo(dualq(Rect(0, 4, 1, 4)));
+
+    return dualq;
+}
+
+// H = dualQuaternion2homogeneous(dq)
+//
+// H  - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
+// dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
+static Mat dualQuaternion2homogeneous(const Mat& dualq)
+{
+    CV_Assert(dualq.type() == CV_64FC1 && dualq.rows == 8 && dualq.cols == 1);
+
+    Mat q = dualq(Rect(0, 0, 1, 4));
+    Mat qprime = dualq(Rect(0, 4, 1, 4));
+
+    Mat R = quat2rot(q);
+    q.at<double>(1,0) = -q.at<double>(1,0);
+    q.at<double>(2,0) = -q.at<double>(2,0);
+    q.at<double>(3,0) = -q.at<double>(3,0);
+
+    Mat qt = 2*qmult(qprime, q);
+    Mat t = qt(Rect(0, 1, 1, 3));
+
+    Mat H = Mat::eye(4, 4, CV_64FC1);
+    R.copyTo(H(Rect(0, 0, 3, 3)));
+    t.copyTo(H(Rect(3, 0, 1, 3)));
+
+    return H;
+}
+
+//Reference:
+//R. Y. Tsai and R. K. Lenz, "A new technique for fully autonomous and efficient 3D robotics hand/eye calibration."
+//In IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345-358, June 1989.
+//C++ code converted from Zoran Lazarevic's Matlab code:
+//http://lazax.com/www.cs.columbia.edu/~laza/html/Stewart/matlab/handEye.m
+static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
+                                 Mat& R_cam2gripper, Mat& t_cam2gripper)
+{
+    //Number of unique camera position pairs
+    int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
+    //Will store: skew(Pgij+Pcij)
+    Mat A(3*K, 3, CV_64FC1);
+    //Will store: Pcij - Pgij
+    Mat B(3*K, 1, CV_64FC1);
+
+    std::vector<Mat> vec_Hgij, vec_Hcij;
+    vec_Hgij.reserve(static_cast<size_t>(K));
+    vec_Hcij.reserve(static_cast<size_t>(K));
+
+    int idx = 0;
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++, idx++)
+        {
+            //Defines coordinate transformation from Gi to Gj
+            //Hgi is from Gi (gripper) to RW (robot base)
+            //Hgj is from Gj (gripper) to RW (robot base)
+            Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; //eq 6
+            vec_Hgij.push_back(Hgij);
+            //Rotation axis for Rgij which is the 3D rotation from gripper coordinate frame Gi to Gj
+            Mat Pgij = 2*rot2quatMinimal(Hgij);
+
+            //Defines coordinate transformation from Ci to Cj
+            //Hci is from CW (calibration target) to Ci (camera)
+            //Hcj is from CW (calibration target) to Cj (camera)
+            Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); //eq 7
+            vec_Hcij.push_back(Hcij);
+            //Rotation axis for Rcij
+            Mat Pcij = 2*rot2quatMinimal(Hcij);
+
+            //Left-hand side: skew(Pgij+Pcij)
+            skew(Pgij+Pcij).copyTo(A(Rect(0, idx*3, 3, 3)));
+            //Right-hand side: Pcij - Pgij
+            Mat diff = Pcij - Pgij;
+            diff.copyTo(B(Rect(0, idx*3, 1, 3)));
+        }
+    }
+
+    Mat Pcg_;
+    //Rotation from camera to gripper is obtained from the set of equations:
+    //    skew(Pgij+Pcij) * Pcg_ = Pcij - Pgij    (eq 12)
+    solve(A, B, Pcg_, DECOMP_SVD);
+
+    Mat Pcg_norm = Pcg_.t() * Pcg_;
+    //Obtained non-unit quaternion is scaled back to unit value that
+    //designates camera-gripper rotation
+    Mat Pcg = 2 * Pcg_ / sqrt(1 + Pcg_norm.at<double>(0,0)); //eq 14
+
+    Mat Rcg = quatMinimal2rot(Pcg/2.0);
+
+    idx = 0;
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++, idx++)
+        {
+            //Defines coordinate transformation from Gi to Gj
+            //Hgi is from Gi (gripper) to RW (robot base)
+            //Hgj is from Gj (gripper) to RW (robot base)
+            Mat Hgij = vec_Hgij[static_cast<size_t>(idx)];
+            //Defines coordinate transformation from Ci to Cj
+            //Hci is from CW (calibration target) to Ci (camera)
+            //Hcj is from CW (calibration target) to Cj (camera)
+            Mat Hcij = vec_Hcij[static_cast<size_t>(idx)];
+
+            //Left-hand side: (Rgij - I)
+            Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1);
+            diff.copyTo(A(Rect(0, idx*3, 3, 3)));
+
+            //Right-hand side: Rcg*Tcij - Tgij
+            diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3));
+            diff.copyTo(B(Rect(0, idx*3, 1, 3)));
+        }
+    }
+
+    Mat Tcg;
+    //Translation from camera to gripper is obtained from the set of equations:
+    //    (Rgij - I) * Tcg = Rcg*Tcij - Tgij    (eq 15)
+    solve(A, B, Tcg, DECOMP_SVD);
+
+    R_cam2gripper = Rcg;
+    t_cam2gripper = Tcg;
+}
+
+//Reference:
+//F. Park, B. Martin, "Robot Sensor Calibration: Solving AX = XB on the Euclidean Group."
+//In IEEE Transactions on Robotics and Automation, 10(5): 717-721, 1994.
+//Matlab code: http://math.loyola.edu/~mili/Calibration/
+static void calibrateHandEyePark(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
+                                 Mat& R_cam2gripper, Mat& t_cam2gripper)
+{
+    Mat M = Mat::zeros(3, 3, CV_64FC1);
+
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++)
+        {
+            Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
+            Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
+
+            Mat Rgij = Hgij(Rect(0, 0, 3, 3));
+            Mat Rcij = Hcij(Rect(0, 0, 3, 3));
+
+            Mat a, b;
+            Rodrigues(Rgij, a);
+            Rodrigues(Rcij, b);
+
+            M += b * a.t();
+        }
+    }
+
+    Mat eigenvalues, eigenvectors;
+    eigen(M.t()*M, eigenvalues, eigenvectors);
+
+    Mat v = Mat::zeros(3, 3, CV_64FC1);
+    for (int i = 0; i < 3; i++) {
+        v.at<double>(i,i) = 1.0 / sqrt(eigenvalues.at<double>(i,0));
+    }
+
+    Mat R = eigenvectors.t() * v * eigenvectors * M.t();
+    R_cam2gripper = R;
+
+    int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
+    Mat C(3*K, 3, CV_64FC1);
+    Mat d(3*K, 1, CV_64FC1);
+    Mat I3 = Mat::eye(3, 3, CV_64FC1);
+
+    int idx = 0;
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++, idx++)
+        {
+            Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
+            Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
+
+            Mat Rgij = Hgij(Rect(0, 0, 3, 3));
+
+            Mat tgij = Hgij(Rect(3, 0, 1, 3));
+            Mat tcij = Hcij(Rect(3, 0, 1, 3));
+
+            Mat I_tgij = I3 - Rgij;
+            I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
+
+            Mat A_RB = tgij - R*tcij;
+            A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
+        }
+    }
+
+    Mat t;
+    solve(C, d, t, DECOMP_SVD);
+    t_cam2gripper = t;
+}
+
+//Reference:
+//R. Horaud, F. Dornaika, "Hand-Eye Calibration"
+//In International Journal of Robotics Research, 14(3): 195-210, 1995.
+//Matlab code: http://math.loyola.edu/~mili/Calibration/
+static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
+                                   Mat& R_cam2gripper, Mat& t_cam2gripper)
+{
+    Mat A = Mat::zeros(4, 4, CV_64FC1);
+
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++)
+        {
+            Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
+            Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
+
+            Mat Rgij = Hgij(Rect(0, 0, 3, 3));
+            Mat Rcij = Hcij(Rect(0, 0, 3, 3));
+
+            Mat qgij = rot2quat(Rgij);
+            double r0 = qgij.at<double>(0,0);
+            double rx = qgij.at<double>(1,0);
+            double ry = qgij.at<double>(2,0);
+            double rz = qgij.at<double>(3,0);
+
+            // Q(r) Appendix A
+            Matx44d Qvi(r0, -rx, -ry, -rz,
+                        rx,  r0, -rz,  ry,
+                        ry,  rz,  r0, -rx,
+                        rz, -ry,  rx,  r0);
+
+            Mat qcij = rot2quat(Rcij);
+            r0 = qcij.at<double>(0,0);
+            rx = qcij.at<double>(1,0);
+            ry = qcij.at<double>(2,0);
+            rz = qcij.at<double>(3,0);
+
+            // W(r) Appendix A
+            Matx44d Wvi(r0, -rx, -ry, -rz,
+                        rx,  r0,  rz, -ry,
+                        ry, -rz,  r0,  rx,
+                        rz,  ry, -rx,  r0);
+
+            // Ai = (Q(vi') - W(vi))^T (Q(vi') - W(vi))
+            A += (Qvi - Wvi).t() * (Qvi - Wvi);
+        }
+    }
+
+    Mat eigenvalues, eigenvectors;
+    eigen(A, eigenvalues, eigenvectors);
+
+    Mat R = quat2rot(eigenvectors.row(3).t());
+    R_cam2gripper = R;
+
+    int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
+    Mat C(3*K, 3, CV_64FC1);
+    Mat d(3*K, 1, CV_64FC1);
+    Mat I3 = Mat::eye(3, 3, CV_64FC1);
+
+    int idx = 0;
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++, idx++)
+        {
+            Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
+            Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
+
+            Mat Rgij = Hgij(Rect(0, 0, 3, 3));
+
+            Mat tgij = Hgij(Rect(3, 0, 1, 3));
+            Mat tcij = Hcij(Rect(3, 0, 1, 3));
+
+            Mat I_tgij = I3 - Rgij;
+            I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
+
+            Mat A_RB = tgij - R*tcij;
+            A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
+        }
+    }
+
+    Mat t;
+    solve(C, d, t, DECOMP_SVD);
+    t_cam2gripper = t;
+}
+
+// sign function, return -1 if negative values, +1 otherwise
+static int sign_double(double val)
+{
+    return (0 < val) - (val < 0);
+}
+
+//Reference:
+//N. Andreff, R. Horaud, B. Espiau, "On-line Hand-Eye Calibration."
+//In Second International Conference on 3-D Digital Imaging and Modeling (3DIM'99), pages 430-436, 1999.
+//Matlab code: http://math.loyola.edu/~mili/Calibration/
+static void calibrateHandEyeAndreff(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
+                                    Mat& R_cam2gripper, Mat& t_cam2gripper)
+{
+    int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
+    Mat A(12*K, 12, CV_64FC1);
+    Mat B(12*K, 1, CV_64FC1);
+
+    Mat I9 = Mat::eye(9, 9, CV_64FC1);
+    Mat I3 = Mat::eye(3, 3, CV_64FC1);
+    Mat O9x3 = Mat::zeros(9, 3, CV_64FC1);
+    Mat O9x1 = Mat::zeros(9, 1, CV_64FC1);
+
+    int idx = 0;
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++, idx++)
+        {
+            Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
+            Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
+
+            Mat Rgij = Hgij(Rect(0, 0, 3, 3));
+            Mat Rcij = Hcij(Rect(0, 0, 3, 3));
+
+            Mat tgij = Hgij(Rect(3, 0, 1, 3));
+            Mat tcij = Hcij(Rect(3, 0, 1, 3));
+
+            //Eq 10
+            Mat a00 = I9 - kron(Rgij, Rcij);
+            Mat a01 = O9x3;
+            Mat a10 = kron(I3, tcij.t());
+            Mat a11 = I3 - Rgij;
+
+            a00.copyTo(A(Rect(0, idx*12, 9, 9)));
+            a01.copyTo(A(Rect(9, idx*12, 3, 9)));
+            a10.copyTo(A(Rect(0, idx*12 + 9, 9, 3)));
+            a11.copyTo(A(Rect(9, idx*12 + 9, 3, 3)));
+
+            O9x1.copyTo(B(Rect(0, idx*12, 1, 9)));
+            tgij.copyTo(B(Rect(0, idx*12 + 9, 1, 3)));
+        }
+    }
+
+    Mat X;
+    solve(A, B, X, DECOMP_SVD);
+
+    Mat R = X(Rect(0, 0, 1, 9));
+    int newSize[] = {3, 3};
+    R = R.reshape(1, 2, newSize);
+    //Eq 15
+    double det = determinant(R);
+    R = pow(sign_double(det) / abs(det), 1.0/3.0) * R;
+
+    Mat w, u, vt;
+    SVDecomp(R, w, u, vt);
+    R = u*vt;
+
+    if (determinant(R) < 0)
+    {
+        Mat diag = (Mat_<double>(3,3) << 1.0, 0.0, 0.0,
+                                         0.0, 1.0, 0.0,
+                                         0.0, 0.0, -1.0);
+        R = u*diag*vt;
+    }
+
+    R_cam2gripper = R;
+
+    Mat t = X(Rect(0, 9, 1, 3));
+    t_cam2gripper = t;
+}
+
+//Reference:
+//K. Daniilidis, "Hand-Eye Calibration Using Dual Quaternions."
+//In The International Journal of Robotics Research,18(3): 286-298, 1998.
+//Matlab code: http://math.loyola.edu/~mili/Calibration/
+static void calibrateHandEyeDaniilidis(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
+                                       Mat& R_cam2gripper, Mat& t_cam2gripper)
+{
+    int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
+    Mat T = Mat::zeros(6*K, 8, CV_64FC1);
+
+    int idx = 0;
+    for (size_t i = 0; i < Hg.size(); i++)
+    {
+        for (size_t j = i+1; j < Hg.size(); j++, idx++)
+        {
+            Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
+            Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
+
+            Mat dualqa = homogeneous2dualQuaternion(Hgij);
+            Mat dualqb = homogeneous2dualQuaternion(Hcij);
+
+            Mat a = dualqa(Rect(0, 1, 1, 3));
+            Mat b = dualqb(Rect(0, 1, 1, 3));
+
+            Mat aprime = dualqa(Rect(0, 5, 1, 3));
+            Mat bprime = dualqb(Rect(0, 5, 1, 3));
+
+            //Eq 31
+            Mat s00 = a - b;
+            Mat s01 = skew(a + b);
+            Mat s10 = aprime - bprime;
+            Mat s11 = skew(aprime + bprime);
+            Mat s12 = a - b;
+            Mat s13 = skew(a + b);
+
+            s00.copyTo(T(Rect(0, idx*6, 1, 3)));
+            s01.copyTo(T(Rect(1, idx*6, 3, 3)));
+            s10.copyTo(T(Rect(0, idx*6 + 3, 1, 3)));
+            s11.copyTo(T(Rect(1, idx*6 + 3, 3, 3)));
+            s12.copyTo(T(Rect(4, idx*6 + 3, 1, 3)));
+            s13.copyTo(T(Rect(5, idx*6 + 3, 3, 3)));
+        }
+    }
+
+    Mat w, u, vt;
+    SVDecomp(T, w, u, vt);
+    Mat v = vt.t();
+
+    Mat u1 = v(Rect(6, 0, 1, 4));
+    Mat v1 = v(Rect(6, 4, 1, 4));
+    Mat u2 = v(Rect(7, 0, 1, 4));
+    Mat v2 = v(Rect(7, 4, 1, 4));
+
+    //Solves Eq 34, Eq 35
+    Mat ma = u1.t()*v1;
+    Mat mb = u1.t()*v2 + u2.t()*v1;
+    Mat mc = u2.t()*v2;
+
+    double a = ma.at<double>(0,0);
+    double b = mb.at<double>(0,0);
+    double c = mc.at<double>(0,0);
+
+    double s1 = (-b + sqrt(b*b - 4*a*c)) / (2*a);
+    double s2 = (-b - sqrt(b*b - 4*a*c)) / (2*a);
+
+    Mat sol1 = s1*s1*u1.t()*u1 + 2*s1*u1.t()*u2 + u2.t()*u2;
+    Mat sol2 = s2*s2*u1.t()*u1 + 2*s2*u1.t()*u2 + u2.t()*u2;
+    double s, val;
+    if (sol1.at<double>(0,0) > sol2.at<double>(0,0))
+    {
+        s = s1;
+        val = sol1.at<double>(0,0);
+    }
+    else
+    {
+        s = s2;
+        val = sol2.at<double>(0,0);
+    }
+
+    double lambda2 = sqrt(1.0 / val);
+    double lambda1 = s * lambda2;
+
+    Mat dualq = lambda1 * v(Rect(6, 0, 1, 8)) + lambda2*v(Rect(7, 0, 1, 8));
+    Mat X = dualQuaternion2homogeneous(dualq);
+
+    Mat R = X(Rect(0, 0, 3, 3));
+    Mat t = X(Rect(3, 0, 1, 3));
+    R_cam2gripper = R;
+    t_cam2gripper = t;
+}
+
+void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base,
+                      InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam,
+                      OutputArray R_cam2gripper, OutputArray t_cam2gripper,
+                      HandEyeCalibrationMethod method)
+{
+    CV_Assert(R_gripper2base.isMatVector() && t_gripper2base.isMatVector() &&
+              R_target2cam.isMatVector() && t_target2cam.isMatVector());
+
+    std::vector<Mat> R_gripper2base_, t_gripper2base_;
+    R_gripper2base.getMatVector(R_gripper2base_);
+    t_gripper2base.getMatVector(t_gripper2base_);
+
+    std::vector<Mat> R_target2cam_, t_target2cam_;
+    R_target2cam.getMatVector(R_target2cam_);
+    t_target2cam.getMatVector(t_target2cam_);
+
+    CV_Assert(R_gripper2base_.size() == t_gripper2base_.size() &&
+              R_target2cam_.size() == t_target2cam_.size() &&
+              R_gripper2base_.size() == R_target2cam_.size());
+    CV_Assert(R_gripper2base_.size() >= 3);
+
+    //Notation used in Tsai paper
+    //Defines coordinate transformation from G (gripper) to RW (robot base)
+    std::vector<Mat> Hg;
+    Hg.reserve(R_gripper2base_.size());
+    for (size_t i = 0; i < R_gripper2base_.size(); i++)
+    {
+        Mat m = Mat::eye(4, 4, CV_64FC1);
+        Mat R = m(Rect(0, 0, 3, 3));
+        R_gripper2base_[i].convertTo(R, CV_64F);
+
+        Mat t = m(Rect(3, 0, 1, 3));
+        t_gripper2base_[i].convertTo(t, CV_64F);
+
+        Hg.push_back(m);
+    }
+
+    //Defines coordinate transformation from CW (calibration target) to C (camera)
+    std::vector<Mat> Hc;
+    Hc.reserve(R_target2cam_.size());
+    for (size_t i = 0; i < R_target2cam_.size(); i++)
+    {
+        Mat m = Mat::eye(4, 4, CV_64FC1);
+        Mat R = m(Rect(0, 0, 3, 3));
+        R_target2cam_[i].convertTo(R, CV_64F);
+
+        Mat t = m(Rect(3, 0, 1, 3));
+        t_target2cam_[i].convertTo(t, CV_64F);
+
+        Hc.push_back(m);
+    }
+
+    Mat Rcg = Mat::eye(3, 3, CV_64FC1);
+    Mat Tcg = Mat::zeros(3, 1, CV_64FC1);
+
+    switch (method)
+    {
+    case CALIB_HAND_EYE_TSAI:
+        calibrateHandEyeTsai(Hg, Hc, Rcg, Tcg);
+        break;
+
+    case CALIB_HAND_EYE_PARK:
+        calibrateHandEyePark(Hg, Hc, Rcg, Tcg);
+        break;
+
+    case CALIB_HAND_EYE_HORAUD:
+        calibrateHandEyeHoraud(Hg, Hc, Rcg, Tcg);
+        break;
+
+    case CALIB_HAND_EYE_ANDREFF:
+        calibrateHandEyeAndreff(Hg, Hc, Rcg, Tcg);
+        break;
+
+    case CALIB_HAND_EYE_DANIILIDIS:
+        calibrateHandEyeDaniilidis(Hg, Hc, Rcg, Tcg);
+        break;
+
+    default:
+        break;
+    }
+
+    Rcg.copyTo(R_cam2gripper);
+    Tcg.copyTo(t_cam2gripper);
+}
+}
@@ -46,26 +46,26 @@
 #include <vector>
 #include <algorithm>

-//#define DEBUG_WINDOWS
+using namespace cv;
+using namespace std;

-#if defined(DEBUG_WINDOWS)
-#  include "opencv2/opencv_modules.hpp"
-#  ifdef HAVE_OPENCV_HIGHGUI
-#    include "opencv2/highgui.hpp"
-#  else
-#    undef DEBUG_WINDOWS
-#  endif
-#endif
-
-static void icvGetQuadrangleHypotheses(CvSeq* contours, std::vector<std::pair<float, int> >& quads, int class_id)
+static void icvGetQuadrangleHypotheses(const std::vector<std::vector< cv::Point > > & contours, const std::vector< cv::Vec4i > & hierarchy, std::vector<std::pair<float, int> >& quads, int class_id)
 {
    const float min_aspect_ratio = 0.3f;
    const float max_aspect_ratio = 3.0f;
    const float min_box_size = 10.0f;

-    for(CvSeq* seq = contours; seq != NULL; seq = seq->h_next)
+    typedef std::vector< std::vector< cv::Point > >::const_iterator iter_t;
+    iter_t i;
+    for (i = contours.begin(); i != contours.end(); ++i)
    {
-        CvBox2D box = cvMinAreaRect2(seq);
+        const iter_t::difference_type idx = i - contours.begin();
+        if (hierarchy.at(idx)[3] != -1)
+            continue; // skip holes
+
+        const std::vector< cv::Point > & c = *i;
+        cv::RotatedRect box = cv::minAreaRect(c);
+
        float box_size = MAX(box.size.width, box.size.height);
        if(box_size < min_box_size)
        {
@@ -96,6 +96,64 @@ inline bool less_pred(const std::pair<float, int>& p1, const std::pair<float, in
    return p1.first < p2.first;
 }

+static void fillQuads(Mat & white, Mat & black, double white_thresh, double black_thresh, vector<pair<float, int> > & quads)
+{
+    Mat thresh;
+    {
+        vector< vector<Point> > contours;
+        vector< Vec4i > hierarchy;
+        threshold(white, thresh, white_thresh, 255, THRESH_BINARY);
+        findContours(thresh, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
+        icvGetQuadrangleHypotheses(contours, hierarchy, quads, 1);
+    }
+
+    {
+        vector< vector<Point> > contours;
+        vector< Vec4i > hierarchy;
+        threshold(black, thresh, black_thresh, 255, THRESH_BINARY_INV);
+        findContours(thresh, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
+        icvGetQuadrangleHypotheses(contours, hierarchy, quads, 0);
+    }
+}
+
+static bool checkQuads(vector<pair<float, int> > & quads, const cv::Size & size)
+{
+    const size_t min_quads_count = size.width*size.height/2;
+    std::sort(quads.begin(), quads.end(), less_pred);
+
+    // now check if there are many hypotheses with similar sizes
+    // do this by floodfill-style algorithm
+    const float size_rel_dev = 0.4f;
+
+    for(size_t i = 0; i < quads.size(); i++)
+    {
+        size_t j = i + 1;
+        for(; j < quads.size(); j++)
+        {
+            if(quads[j].first/quads[i].first > 1.0f + size_rel_dev)
+            {
+                break;
+            }
+        }
+
+        if(j + 1 > min_quads_count + i)
+        {
+            // check the number of black and white squares
+            std::vector<int> counts;
+            countClasses(quads, i, j, counts);
+            const int black_count = cvRound(ceil(size.width/2.0)*ceil(size.height/2.0));
+            const int white_count = cvRound(floor(size.width/2.0)*floor(size.height/2.0));
+            if(counts[0] < black_count*0.75 ||
+               counts[1] < white_count*0.75)
+            {
+                continue;
+            }
+            return true;
+        }
+    }
+    return false;
+}
+
 // does a fast check if a chessboard is in the input image. This is a workaround to
 // a problem of cvFindChessboardCorners being slow on images with no chessboard
 // - src: input image
@@ -104,104 +162,64 @@ inline bool less_pred(const std::pair<float, int>& p1, const std::pair<float, in
 // 0 if there is no chessboard, -1 in case of error
 int cvCheckChessboard(IplImage* src, CvSize size)
 {
-    if(src->nChannels > 1)
-    {
-        cvError(CV_BadNumChannels, "cvCheckChessboard", "supports single-channel images only",
-                __FILE__, __LINE__);
-    }
+    cv::Mat img = cv::cvarrToMat(src);
+    return checkChessboard(img, size);
+}

-    if(src->depth != 8)
-    {
-        cvError(CV_BadDepth, "cvCheckChessboard", "supports depth=8 images only",
-                __FILE__, __LINE__);
-    }
+int checkChessboard(const cv::Mat & img, const cv::Size & size)
+{
+    CV_Assert(img.channels() == 1 && img.depth() == CV_8U);

    const int erosion_count = 1;
    const float black_level = 20.f;
    const float white_level = 130.f;
    const float black_white_gap = 70.f;

-#if defined(DEBUG_WINDOWS)
-    cvNamedWindow("1", 1);
-    cvShowImage("1", src);
-    cvWaitKey(0);
-#endif //DEBUG_WINDOWS
-
-    CvMemStorage* storage = cvCreateMemStorage();
-
-    IplImage* white = cvCloneImage(src);
-    IplImage* black = cvCloneImage(src);
-
-    cvErode(white, white, NULL, erosion_count);
-    cvDilate(black, black, NULL, erosion_count);
-    IplImage* thresh = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
+    Mat white;
+    Mat black;
+    erode(img, white, Mat(), Point(-1, -1), erosion_count);
+    dilate(img, black, Mat(), Point(-1, -1), erosion_count);

    int result = 0;
    for(float thresh_level = black_level; thresh_level < white_level && !result; thresh_level += 20.0f)
    {
-        cvThreshold(white, thresh, thresh_level + black_white_gap, 255, CV_THRESH_BINARY);
-
-#if defined(DEBUG_WINDOWS)
-        cvShowImage("1", thresh);
-        cvWaitKey(0);
-#endif //DEBUG_WINDOWS
-
-        CvSeq* first = 0;
-        std::vector<std::pair<float, int> > quads;
-        cvFindContours(thresh, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
-        icvGetQuadrangleHypotheses(first, quads, 1);
-
-        cvThreshold(black, thresh, thresh_level, 255, CV_THRESH_BINARY_INV);
-
-#if defined(DEBUG_WINDOWS)
-        cvShowImage("1", thresh);
-        cvWaitKey(0);
-#endif //DEBUG_WINDOWS
-
-        cvFindContours(thresh, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
-        icvGetQuadrangleHypotheses(first, quads, 0);
-
-        const size_t min_quads_count = size.width*size.height/2;
-        std::sort(quads.begin(), quads.end(), less_pred);
-
-        // now check if there are many hypotheses with similar sizes
-        // do this by floodfill-style algorithm
-        const float size_rel_dev = 0.4f;
-
-        for(size_t i = 0; i < quads.size(); i++)
-        {
-            size_t j = i + 1;
-            for(; j < quads.size(); j++)
-            {
-                if(quads[j].first/quads[i].first > 1.0f + size_rel_dev)
-                {
-                    break;
-                }
-            }
-
-            if(j + 1 > min_quads_count + i)
-            {
-                // check the number of black and white squares
-                std::vector<int> counts;
-                countClasses(quads, i, j, counts);
-                const int black_count = cvRound(ceil(size.width/2.0)*ceil(size.height/2.0));
-                const int white_count = cvRound(floor(size.width/2.0)*floor(size.height/2.0));
-                if(counts[0] < black_count*0.75 ||
-                   counts[1] < white_count*0.75)
-                {
-                    continue;
-                }
-                result = 1;
-                break;
-            }
-        }
+        vector<pair<float, int> > quads;
+        fillQuads(white, black, thresh_level + black_white_gap, thresh_level, quads);
+        if (checkQuads(quads, size))
+            result = 1;
+    }
+    return result;
+}
+
+// does a fast check if a chessboard is in the input image. This is a workaround to
+// a problem of cvFindChessboardCorners being slow on images with no chessboard
+// - src: input binary image
+// - size: chessboard size
+// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called,
+// 0 if there is no chessboard, -1 in case of error
+int checkChessboardBinary(const cv::Mat & img, const cv::Size & size)
+{
+    CV_Assert(img.channels() == 1 && img.depth() == CV_8U);
+
+    Mat white = img.clone();
+    Mat black = img.clone();
+
+    int result = 0;
+    for ( int erosion_count = 0; erosion_count <= 3; erosion_count++ )
+    {
+        if ( 1 == result )
+            break;
+
+        if ( 0 != erosion_count ) // first iteration keeps original images
+        {
+            erode(white, white, Mat(), Point(-1, -1), 1);
+            dilate(black, black, Mat(), Point(-1, -1), 1);
+        }
+
+        vector<pair<float, int> > quads;
+        fillQuads(white, black, 128, 128, quads);
+        if (checkQuads(quads, size))
+            result = 1;
    }
-
-
-    cvReleaseImage(&thresh);
-    cvReleaseImage(&white);
-    cvReleaseImage(&black);
-    cvReleaseMemStorage(&storage);
-
    return result;
 }
@@ -43,9 +43,12 @@
 #include "precomp.hpp"
 #include "circlesgrid.hpp"
 #include <limits>
+
+ // Requires CMake flag: DEBUG_opencv_calib3d=ON
 //#define DEBUG_CIRCLES

 #ifdef DEBUG_CIRCLES
+#  include <iostream>
 #  include "opencv2/opencv_modules.hpp"
 #  ifdef HAVE_OPENCV_HIGHGUI
 #    include "opencv2/highgui.hpp"
@@ -66,10 +69,10 @@ void drawPoints(const std::vector<Point2f> &points, Mat &outImage, int radius =
 }
 #endif

-void CirclesGridClusterFinder::hierarchicalClustering(const std::vector<Point2f> points, const Size &patternSz, std::vector<Point2f> &patternPoints)
+void CirclesGridClusterFinder::hierarchicalClustering(const std::vector<Point2f> &points, const Size &patternSz, std::vector<Point2f> &patternPoints)
 {
 #ifdef HAVE_TEGRA_OPTIMIZATION
-    if(tegra::hierarchicalClustering(points, patternSz, patternPoints))
+    if(tegra::useTegra() && tegra::hierarchicalClustering(points, patternSz, patternPoints))
        return;
 #endif
    int j, n = (int)points.size();
@@ -116,6 +119,7 @@ void CirclesGridClusterFinder::hierarchicalClustering(const std::vector<Point2f>
        Mat tmpRow = dists.row(minIdx);
        Mat tmpCol = dists.col(minIdx);
        cv::min(dists.row(minLoc.x), dists.row(minLoc.y), tmpRow);
+        tmpRow = tmpRow.t();
        tmpRow.copyTo(tmpCol);

        clusters[minIdx].splice(clusters[minIdx].end(), clusters[maxIdx]);
@@ -129,13 +133,13 @@ void CirclesGridClusterFinder::hierarchicalClustering(const std::vector<Point2f>
    }

    patternPoints.reserve(clusters[patternClusterIdx].size());
-    for(std::list<size_t>::iterator it = clusters[patternClusterIdx].begin(); it != clusters[patternClusterIdx].end(); it++)
+    for(std::list<size_t>::iterator it = clusters[patternClusterIdx].begin(); it != clusters[patternClusterIdx].end();++it)
    {
        patternPoints.push_back(points[*it]);
    }
 }

-void CirclesGridClusterFinder::findGrid(const std::vector<cv::Point2f> points, cv::Size _patternSize, std::vector<Point2f>& centers)
+void CirclesGridClusterFinder::findGrid(const std::vector<cv::Point2f> &points, cv::Size _patternSize, std::vector<Point2f>& centers)
 {
  patternSize = _patternSize;
  centers.clear();
@@ -158,7 +162,7 @@ void CirclesGridClusterFinder::findGrid(const std::vector<cv::Point2f> points, c
 #endif

  std::vector<Point2f> hull2f;
-  convexHull(Mat(patternPoints), hull2f, false);
+  convexHull(patternPoints, hull2f, false);
  const size_t cornersCount = isAsymmetricGrid ? 6 : 4;
  if(hull2f.size() < cornersCount)
    return;
@@ -176,7 +180,7 @@ void CirclesGridClusterFinder::findGrid(const std::vector<cv::Point2f> points, c
    if(outsideCorners.size() != outsideCornersCount)
      return;
  }
-  getSortedCorners(hull2f, corners, outsideCorners, sortedCorners);
+  getSortedCorners(hull2f, patternPoints, corners, outsideCorners, sortedCorners);
  if(sortedCorners.size() != cornersCount)
    return;

@@ -222,7 +226,7 @@ void CirclesGridClusterFinder::findOutsideCorners(const std::vector<cv::Point2f>
  CV_Assert(!corners.empty());
  outsideCorners.clear();
  //find two pairs of the most nearest corners
-  int i, j, n = (int)corners.size();
+  const size_t n = corners.size();

 #ifdef DEBUG_CIRCLES
  Mat cornersImage(1024, 1248, CV_8UC1, Scalar(0));
@@ -230,22 +234,22 @@ void CirclesGridClusterFinder::findOutsideCorners(const std::vector<cv::Point2f>
  imshow("corners", cornersImage);
 #endif

-  std::vector<Point2f> tangentVectors(corners.size());
-  for(size_t k=0; k<corners.size(); k++)
+  std::vector<Point2f> tangentVectors(n);
+  for(size_t k=0; k < n; k++)
  {
-    Point2f diff = corners[(k + 1) % corners.size()] - corners[k];
+    Point2f diff = corners[(k + 1) % n] - corners[k];
    tangentVectors[k] = diff * (1.0f / norm(diff));
  }

  //compute angles between all sides
-  Mat cosAngles(n, n, CV_32FC1, 0.0f);
-  for(i = 0; i < n; i++)
+  Mat cosAngles((int)n, (int)n, CV_32FC1, 0.0f);
+  for(size_t i = 0; i < n; i++)
  {
-    for(j = i + 1; j < n; j++)
+    for(size_t j = i + 1; j < n; j++)
    {
      float val = fabs(tangentVectors[i].dot(tangentVectors[j]));
-      cosAngles.at<float>(i, j) = val;
-      cosAngles.at<float>(j, i) = val;
+      cosAngles.at<float>((int)i, (int)j) = val;
+      cosAngles.at<float>((int)j, (int)i) = val;
    }
  }

@@ -274,10 +278,10 @@ void CirclesGridClusterFinder::findOutsideCorners(const std::vector<cv::Point2f>
  const int bigDiff = 4;
  if(maxIdx - minIdx == bigDiff)
  {
-    minIdx += n;
+    minIdx += (int)n;
    std::swap(maxIdx, minIdx);
  }
-  if(maxIdx - minIdx != n - bigDiff)
+  if(maxIdx - minIdx != (int)n - bigDiff)
  {
    return;
  }
@@ -289,11 +293,22 @@ void CirclesGridClusterFinder::findOutsideCorners(const std::vector<cv::Point2f>

 #ifdef DEBUG_CIRCLES
  drawPoints(outsideCorners, cornersImage, 2, Scalar(128));
-  imshow("corners", outsideCornersImage);
+  imshow("corners", cornersImage);
 #endif
 }

-void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners)
+namespace {
+double pointLineDistance(const cv::Point2f &p, const cv::Vec4f &line)
+{
+  Vec3f pa( line[0], line[1], 1 );
+  Vec3f pb( line[2], line[3], 1 );
+  Vec3f l = pa.cross(pb);
+  return std::abs((p.x * l[0] + p.y * l[1] + l[2])) * 1.0 /
+         std::sqrt(double(l[0] * l[0] + l[1] * l[1]));
+}
+}
+
+void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners)
 {
  Point2f firstCorner;
  if(isAsymmetricGrid)
@@ -320,7 +335,7 @@ void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &

  std::vector<Point2f>::const_iterator firstCornerIterator = std::find(hull2f.begin(), hull2f.end(), firstCorner);
  sortedCorners.clear();
-  for(std::vector<Point2f>::const_iterator it = firstCornerIterator; it != hull2f.end(); it++)
+  for(std::vector<Point2f>::const_iterator it = firstCornerIterator; it != hull2f.end();++it)
  {
    std::vector<Point2f>::const_iterator itCorners = std::find(corners.begin(), corners.end(), *it);
    if(itCorners != corners.end())
@@ -328,7 +343,7 @@ void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &
      sortedCorners.push_back(*it);
    }
  }
-  for(std::vector<Point2f>::const_iterator it = hull2f.begin(); it != firstCornerIterator; it++)
+  for(std::vector<Point2f>::const_iterator it = hull2f.begin(); it != firstCornerIterator;++it)
  {
    std::vector<Point2f>::const_iterator itCorners = std::find(corners.begin(), corners.end(), *it);
    if(itCorners != corners.end())
@@ -339,10 +354,26 @@ void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &

  if(!isAsymmetricGrid)
  {
-    double dist1 = norm(sortedCorners[0] - sortedCorners[1]);
-    double dist2 = norm(sortedCorners[1] - sortedCorners[2]);
+    double dist01 = norm(sortedCorners[0] - sortedCorners[1]);
+    double dist12 = norm(sortedCorners[1] - sortedCorners[2]);
+    // Use half the average distance between circles on the shorter side as threshold for determining whether a point lies on an edge.
+    double thresh = min(dist01, dist12) / min(patternSize.width, patternSize.height) / 2;

-    if((dist1 > dist2 && patternSize.height > patternSize.width) || (dist1 < dist2 && patternSize.height < patternSize.width))
+    size_t circleCount01 = 0;
+    size_t circleCount12 = 0;
+    Vec4f line01( sortedCorners[0].x, sortedCorners[0].y, sortedCorners[1].x, sortedCorners[1].y );
+    Vec4f line12( sortedCorners[1].x, sortedCorners[1].y, sortedCorners[2].x, sortedCorners[2].y );
+    // Count the circles along both edges.
+    for (size_t i = 0; i < patternPoints.size(); i++)
+    {
+      if (pointLineDistance(patternPoints[i], line01) < thresh)
+        circleCount01++;
+      if (pointLineDistance(patternPoints[i], line12) < thresh)
+        circleCount12++;
+    }
+
+    // Ensure that the edge from sortedCorners[0] to sortedCorners[1] is the one with more circles (i.e. it is interpreted as the pattern's width).
+    if ((circleCount01 > circleCount12 && patternSize.height > patternSize.width) || (circleCount01 < circleCount12 && patternSize.height < patternSize.width))
    {
      for(size_t i=0; i<sortedCorners.size()-1; i++)
      {
@@ -382,7 +413,7 @@ void CirclesGridClusterFinder::rectifyPatternPoints(const std::vector<cv::Point2
    }
  }

-  Mat homography = findHomography(Mat(sortedCorners), Mat(idealPoints), 0);
+  Mat homography = findHomography(sortedCorners, idealPoints, 0);
  Mat rectifiedPointsMat;
  transform(patternPoints, rectifiedPointsMat, homography);
  rectifiedPatternPoints.clear();
@@ -391,6 +422,12 @@ void CirclesGridClusterFinder::rectifyPatternPoints(const std::vector<cv::Point2

 void CirclesGridClusterFinder::parsePatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &rectifiedPatternPoints, std::vector<cv::Point2f> &centers)
 {
+#ifndef HAVE_OPENCV_FLANN
+  CV_UNUSED(patternPoints);
+  CV_UNUSED(rectifiedPatternPoints);
+  CV_UNUSED(centers);
+  CV_Error(Error::StsNotImplemented, "The desired functionality requires flann module, which was disabled.");
+#else
  flann::LinearIndexParams flannIndexParams;
  flann::Index flannIndex(Mat(rectifiedPatternPoints).reshape(1), flannIndexParams);

@@ -417,13 +454,14 @@ void CirclesGridClusterFinder::parsePatternPoints(const std::vector<cv::Point2f>
      if(distsbuf[0] > maxRectifiedDistance)
      {
 #ifdef DEBUG_CIRCLES
-        cout << "Pattern not detected: too large rectified distance" << endl;
+        std::cout << "Pattern not detected: too large rectified distance" << std::endl;
 #endif
        centers.clear();
        return;
      }
    }
  }
+#endif
 }

 Graph::Graph(size_t n)
@@ -466,11 +504,10 @@ void Graph::removeEdge(size_t id1, size_t id2)

 bool Graph::areVerticesAdjacent(size_t id1, size_t id2) const
 {
-  CV_Assert( doesVertexExist( id1 ) );
-  CV_Assert( doesVertexExist( id2 ) );
-
  Vertices::const_iterator it = vertices.find(id1);
-  return it->second.neighbors.find(id2) != it->second.neighbors.end();
+  CV_Assert(it != vertices.end());
+  const Neighbors & neighbors = it->second.neighbors;
+  return neighbors.find(id2) != neighbors.end();
 }

 size_t Graph::getVerticesCount() const
@@ -480,9 +517,8 @@ size_t Graph::getVerticesCount() const

 size_t Graph::getDegree(size_t id) const
 {
-  CV_Assert( doesVertexExist(id) );
-
  Vertices::const_iterator it = vertices.find(id);
+  CV_Assert( it != vertices.end() );
  return it->second.neighbors.size();
 }

@@ -493,21 +529,21 @@ void Graph::floydWarshall(cv::Mat &distanceMatrix, int infinity) const
  const int n = (int)getVerticesCount();
  distanceMatrix.create(n, n, CV_32SC1);
  distanceMatrix.setTo(infinity);
-  for (Vertices::const_iterator it1 = vertices.begin(); it1 != vertices.end(); it1++)
+  for (Vertices::const_iterator it1 = vertices.begin(); it1 != vertices.end();++it1)
  {
    distanceMatrix.at<int> ((int)it1->first, (int)it1->first) = 0;
-    for (Neighbors::const_iterator it2 = it1->second.neighbors.begin(); it2 != it1->second.neighbors.end(); it2++)
+    for (Neighbors::const_iterator it2 = it1->second.neighbors.begin(); it2 != it1->second.neighbors.end();++it2)
    {
      CV_Assert( it1->first != *it2 );
      distanceMatrix.at<int> ((int)it1->first, (int)*it2) = edgeWeight;
    }
  }

-  for (Vertices::const_iterator it1 = vertices.begin(); it1 != vertices.end(); it1++)
+  for (Vertices::const_iterator it1 = vertices.begin(); it1 != vertices.end();++it1)
  {
-    for (Vertices::const_iterator it2 = vertices.begin(); it2 != vertices.end(); it2++)
+    for (Vertices::const_iterator it2 = vertices.begin(); it2 != vertices.end();++it2)
    {
-      for (Vertices::const_iterator it3 = vertices.begin(); it3 != vertices.end(); it3++)
+      for (Vertices::const_iterator it3 = vertices.begin(); it3 != vertices.end();++it3)
      {
      int i1 = (int)it1->first, i2 = (int)it2->first, i3 = (int)it3->first;
        int val1 = distanceMatrix.at<int> (i2, i3);
@@ -527,9 +563,8 @@ void Graph::floydWarshall(cv::Mat &distanceMatrix, int infinity) const

 const Graph::Neighbors& Graph::getNeighbors(size_t id) const
 {
-  CV_Assert( doesVertexExist(id) );
-
  Vertices::const_iterator it = vertices.find(id);
+  CV_Assert( it != vertices.end() );
  return it->second.neighbors;
 }

@@ -551,16 +586,23 @@ CirclesGridFinderParameters::CirclesGridFinderParameters()
  keypointScale = 1;

  minGraphConfidence = 9;
-  vertexGain = 2;
-  vertexPenalty = -5;
+  vertexGain = 1;
+  vertexPenalty = -0.6f;
  edgeGain = 1;
-  edgePenalty = -5;
-  existingVertexGain = 0;
+  edgePenalty = -0.6f;
+  existingVertexGain = 10000;

  minRNGEdgeSwitchDist = 5.f;
  gridType = SYMMETRIC_GRID;
 }

+CirclesGridFinderParameters2::CirclesGridFinderParameters2()
+: CirclesGridFinderParameters()
+{
+    squareSize = 1.0f;
+    maxRectifiedDistance = squareSize/2.0f;
+}
+
 CirclesGridFinder::CirclesGridFinder(Size _patternSize, const std::vector<Point2f> &testKeypoints,
                                     const CirclesGridFinderParameters &_parameters) :
  patternSize(static_cast<size_t> (_patternSize.width), static_cast<size_t> (_patternSize.height))
@@ -607,7 +649,7 @@ bool CirclesGridFinder::findHoles()
    }

    default:
-      CV_Error(Error::StsBadArg, "Unkown pattern type");
+      CV_Error(Error::StsBadArg, "Unknown pattern type");
  }
  return (isDetectionCorrect());
  //CV_Error( 0, "Detection is not correct" );
@@ -618,10 +660,10 @@ void CirclesGridFinder::rng2gridGraph(Graph &rng, std::vector<cv::Point2f> &vect
  for (size_t i = 0; i < rng.getVerticesCount(); i++)
  {
    Graph::Neighbors neighbors1 = rng.getNeighbors(i);
-    for (Graph::Neighbors::iterator it1 = neighbors1.begin(); it1 != neighbors1.end(); it1++)
+    for (Graph::Neighbors::iterator it1 = neighbors1.begin(); it1 != neighbors1.end(); ++it1)
    {
      Graph::Neighbors neighbors2 = rng.getNeighbors(*it1);
-      for (Graph::Neighbors::iterator it2 = neighbors2.begin(); it2 != neighbors2.end(); it2++)
+      for (Graph::Neighbors::iterator it2 = neighbors2.begin(); it2 != neighbors2.end(); ++it2)
      {
        if (i < *it2)
        {
@@ -746,12 +788,8 @@ bool CirclesGridFinder::isDetectionCorrect()
      }
      return (vertices.size() == largeHeight * largeWidth + smallHeight * smallWidth);
    }
-
-    default:
-      CV_Error(0, "Unknown pattern type");
  }
-
-  return false;
+  CV_Error(Error::StsBadArg, "Unknown pattern type");
 }

 void CirclesGridFinder::findMCS(const std::vector<Point2f> &basis, std::vector<Graph> &basisGraphs)
@@ -835,11 +873,15 @@ Mat CirclesGridFinder::rectifyGrid(Size detectedGridSize, const std::vector<Poin
    }
  }

-  Mat H = findHomography(Mat(centers), Mat(dstPoints), RANSAC);
-  //Mat H = findHomography( Mat( corners ), Mat( dstPoints ) );
+  Mat H = findHomography(centers, dstPoints, RANSAC);
+  //Mat H = findHomography(corners, dstPoints);

  if (H.empty())
+  {
      H = Mat::zeros(3, 3, CV_64FC1);
+      warpedKeypoints.clear();
+      return H;
+  }

  std::vector<Point2f> srcKeypoints;
  for (size_t i = 0; i < keypoints.size(); i++)
@@ -848,7 +890,7 @@ Mat CirclesGridFinder::rectifyGrid(Size detectedGridSize, const std::vector<Poin
  }

  Mat dstKeypointsMat;
-  transform(Mat(srcKeypoints), dstKeypointsMat, H);
+  transform(srcKeypoints, dstKeypointsMat, H);
  std::vector<Point2f> dstKeypoints;
  convertPointsFromHomogeneous(dstKeypointsMat, dstKeypoints);

@@ -1136,7 +1178,7 @@ void CirclesGridFinder::findBasis(const std::vector<Point2f> &samples, std::vect
  }
  for (size_t i = 0; i < basis.size(); i++)
  {
-    convexHull(Mat(clusters[i]), hulls[i]);
+    convexHull(clusters[i], hulls[i]);
  }

  basisGraphs.resize(basis.size(), Graph(keypoints.size()));
@@ -1151,7 +1193,7 @@ void CirclesGridFinder::findBasis(const std::vector<Point2f> &samples, std::vect

      for (size_t k = 0; k < hulls.size(); k++)
      {
-        if (pointPolygonTest(Mat(hulls[k]), vec, false) >= 0)
+        if (pointPolygonTest(hulls[k], vec, false) >= 0)
        {
          basisGraphs[k].addEdge(i, j);
        }
@@ -1382,7 +1424,6 @@ void CirclesGridFinder::drawHoles(const Mat &srcImage, Mat &drawImage) const
      if (i != holes.size() - 1)
        line(drawImage, keypoints[holes[i][j]], keypoints[holes[i + 1][j]], Scalar(255, 0, 0), 2);

-      //circle(drawImage, keypoints[holes[i][j]], holeRadius, holeColor, holeThickness);
      circle(drawImage, keypoints[holes[i][j]], holeRadius, holeColor, holeThickness);
    }
  }
@@ -1531,7 +1572,7 @@ void CirclesGridFinder::getCornerSegments(const std::vector<std::vector<size_t>
  if (!isClockwise)
  {
 #ifdef DEBUG_CIRCLES
-    cout << "Corners are counterclockwise" << endl;
+    std::cout << "Corners are counterclockwise" << std::endl;
 #endif
    std::reverse(segments.begin(), segments.end());
    std::reverse(cornerIndices.begin(), cornerIndices.end());
@@ -56,20 +56,20 @@ class CirclesGridClusterFinder
    CirclesGridClusterFinder& operator=(const CirclesGridClusterFinder&);
    CirclesGridClusterFinder(const CirclesGridClusterFinder&);
 public:
-  CirclesGridClusterFinder(bool _isAsymmetricGrid)
+  CirclesGridClusterFinder(const cv::CirclesGridFinderParameters2 &parameters)
  {
-    isAsymmetricGrid = _isAsymmetricGrid;
-    squareSize = 1.0f;
-    maxRectifiedDistance = (float)(squareSize / 2.0);
+    isAsymmetricGrid = parameters.gridType == cv::CirclesGridFinderParameters::ASYMMETRIC_GRID;
+    squareSize = parameters.squareSize;
+    maxRectifiedDistance = parameters.maxRectifiedDistance;
  }
-  void findGrid(const std::vector<cv::Point2f> points, cv::Size patternSize, std::vector<cv::Point2f>& centers);
+  void findGrid(const std::vector<cv::Point2f> &points, cv::Size patternSize, std::vector<cv::Point2f>& centers);

  //cluster 2d points by geometric coordinates
-  void hierarchicalClustering(const std::vector<cv::Point2f> points, const cv::Size &patternSize, std::vector<cv::Point2f> &patternPoints);
+  void hierarchicalClustering(const std::vector<cv::Point2f> &points, const cv::Size &patternSize, std::vector<cv::Point2f> &patternPoints);
 private:
  void findCorners(const std::vector<cv::Point2f> &hull2f, std::vector<cv::Point2f> &corners);
  void findOutsideCorners(const std::vector<cv::Point2f> &corners, std::vector<cv::Point2f> &outsideCorners);
-  void getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners);
+  void getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners);
  void rectifyPatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &sortedCorners, std::vector<cv::Point2f> &rectifiedPatternPoints);
  void parsePatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &rectifiedPatternPoints, std::vector<cv::Point2f> &centers);

@@ -119,35 +119,11 @@ struct Path
  }
 };

-struct CirclesGridFinderParameters
-{
-  CirclesGridFinderParameters();
-  cv::Size2f densityNeighborhoodSize;
-  float minDensity;
-  int kmeansAttempts;
-  int minDistanceToAddKeypoint;
-  int keypointScale;
-  float minGraphConfidence;
-  float vertexGain;
-  float vertexPenalty;
-  float existingVertexGain;
-  float edgeGain;
-  float edgePenalty;
-  float convexHullFactor;
-  float minRNGEdgeSwitchDist;
-
-  enum GridType
-  {
-    SYMMETRIC_GRID, ASYMMETRIC_GRID
-  };
-  GridType gridType;
-};
-
 class CirclesGridFinder
 {
 public:
  CirclesGridFinder(cv::Size patternSize, const std::vector<cv::Point2f> &testKeypoints,
-                    const CirclesGridFinderParameters &parameters = CirclesGridFinderParameters());
+                    const cv::CirclesGridFinderParameters &parameters = cv::CirclesGridFinderParameters());
  bool findHoles();
  static cv::Mat rectifyGrid(cv::Size detectedGridSize, const std::vector<cv::Point2f>& centers, const std::vector<
      cv::Point2f> &keypoint, std::vector<cv::Point2f> &warpedKeypoints);
@@ -211,7 +187,7 @@ private:
  std::vector<std::vector<size_t> > *smallHoles;

  const cv::Size_<size_t> patternSize;
-  CirclesGridFinderParameters parameters;
+  cv::CirclesGridFinderParameters parameters;

  CirclesGridFinder& operator=(const CirclesGridFinder&);
  CirclesGridFinder(const CirclesGridFinder&);
@@ -58,6 +58,7 @@ CvLevMarq::CvLevMarq()
    iters = 0;
    completeSymmFlag = false;
    errNorm = prevErrNorm = DBL_MAX;
+    solveMethod = cv::DECOMP_SVD;
 }

 CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
@@ -93,9 +94,6 @@ void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _co
    prevParam.reset(cvCreateMat( nparams, 1, CV_64F ));
    param.reset(cvCreateMat( nparams, 1, CV_64F ));
    JtJ.reset(cvCreateMat( nparams, nparams, CV_64F ));
-    JtJN.reset(cvCreateMat( nparams, nparams, CV_64F ));
-    JtJV.reset(cvCreateMat( nparams, nparams, CV_64F ));
-    JtJW.reset(cvCreateMat( nparams, 1, CV_64F ));
    JtErr.reset(cvCreateMat( nparams, 1, CV_64F ));
    if( nerrs > 0 )
    {
@@ -116,12 +114,11 @@ void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _co
    state = STARTED;
    iters = 0;
    completeSymmFlag = _completeSymmFlag;
+    solveMethod = cv::DECOMP_SVD;
 }

 bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
 {
-    double change;
-
    matJ = _err = 0;

    assert( !err.empty() );
@@ -174,7 +171,7 @@ bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )

    lambdaLg10 = MAX(lambdaLg10-1, -16);
    if( ++iters >= criteria.max_iter ||
-        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
+        cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
    {
        _param = param;
        state = DONE;
@@ -193,8 +190,6 @@ bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )

 bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
 {
-    double change;
-
    CV_Assert( !err );
    if( state == DONE )
    {
@@ -243,9 +238,11 @@ bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, d

    lambdaLg10 = MAX(lambdaLg10-1, -16);
    if( ++iters >= criteria.max_iter ||
-        (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
+        cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
    {
        _param = param;
+        _JtJ = JtJ;
+        _JtErr = JtErr;
        state = DONE;
        return false;
    }
@@ -260,35 +257,68 @@ bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, d
    return true;
 }

+namespace {
+static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
+                      const std::vector<uchar>& rows) {
+    int nonzeros_cols = cv::countNonZero(cols);
+    cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
+
+    for (int i = 0, j = 0; i < (int)cols.size(); i++)
+    {
+        if (cols[i])
+        {
+            src.col(i).copyTo(tmp.col(j++));
+        }
+    }
+
+    int nonzeros_rows  = cv::countNonZero(rows);
+    dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
+    for (int i = 0, j = 0; i < (int)rows.size(); i++)
+    {
+        if (rows[i])
+        {
+            tmp.row(i).copyTo(dst.row(j++));
+        }
+    }
+}
+
+}
+
+
 void CvLevMarq::step()
 {
+    using namespace cv;
    const double LOG10 = log(10.);
    double lambda = exp(lambdaLg10*LOG10);
-    int i, j, nparams = param->rows;
+    int nparams = param->rows;

-    for( i = 0; i < nparams; i++ )
-        if( mask->data.ptr[i] == 0 )
-        {
-            double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
-            for( j = 0; j < nparams; j++ )
-                row[j] = col[j*nparams] = 0;
-            JtErr->data.db[i] = 0;
-        }
+    Mat _JtJ = cvarrToMat(JtJ);
+    Mat _mask = cvarrToMat(mask);
+
+    int nparams_nz = countNonZero(_mask);
+    if(!JtJN || JtJN->rows != nparams_nz) {
+        // prevent re-allocation in every step
+        JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F ));
+        JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
+        JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
+    }
+
+    Mat _JtJN = cvarrToMat(JtJN);
+    Mat _JtErr = cvarrToMat(JtJV);
+    Mat_<double> nonzero_param = cvarrToMat(JtJW);
+
+    subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);
+    subMatrix(_JtJ, _JtJN, _mask, _mask);

    if( !err )
-        cvCompleteSymm( JtJ, completeSymmFlag );
-#if 1
-    cvCopy( JtJ, JtJN );
-    for( i = 0; i < nparams; i++ )
-        JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
-#else
-    cvSetIdentity(JtJN, cvRealScalar(lambda));
-    cvAdd( JtJ, JtJN, JtJN );
-#endif
-    cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
-    cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
-    for( i = 0; i < nparams; i++ )
-        param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
+        completeSymm( _JtJN, completeSymmFlag );
+
+    _JtJN.diag() *= 1. + lambda;
+    solve(_JtJN, _JtErr, nonzero_param, solveMethod);
+
+    int j = 0;
+    for( int i = 0; i < nparams; i++ )
+        param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0);
 }


@@ -299,7 +329,8 @@ CV_IMPL int cvRANSACUpdateNumIters( double p, double ep, int modelPoints, int ma


 CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, int method,
-                              double ransacReprojThreshold, CvMat* _mask )
+                              double ransacReprojThreshold, CvMat* _mask, int maxIters,
+                              double confidence)
 {
    cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);

@@ -308,9 +339,20 @@ CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H,
    if( dst.channels() == 1 && (dst.rows == 2 || dst.rows == 3) && dst.cols > 3 )
        cv::transpose(dst, dst);

+    if ( maxIters < 0 )
+        maxIters = 0;
+    if ( maxIters > 2000 )
+        maxIters = 2000;
+
+    if ( confidence < 0 )
+        confidence = 0;
+    if ( confidence > 1 )
+        confidence = 1;
+
    const cv::Mat H = cv::cvarrToMat(__H), mask = cv::cvarrToMat(_mask);
    cv::Mat H0 = cv::findHomography(src, dst, method, ransacReprojThreshold,
-                                    _mask ? cv::_OutputArray(mask) : cv::_OutputArray());
+                                    _mask ? cv::_OutputArray(mask) : cv::_OutputArray(), maxIters,
+                                    confidence);

    if( H0.empty() )
    {
@@ -92,18 +92,18 @@ void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,

    CV_Assert( state != 0 );

-    cv::Ptr<cv::StereoMatcher> sm = cv::createStereoBM(state->numberOfDisparities,
+    cv::Ptr<cv::StereoBM> sm = cv::StereoBM::create(state->numberOfDisparities,
                                                       state->SADWindowSize);
-    sm->set("preFilterType", state->preFilterType);
-    sm->set("preFilterSize", state->preFilterSize);
-    sm->set("preFilterCap", state->preFilterCap);
-    sm->set("SADWindowSize", state->SADWindowSize);
-    sm->set("numDisparities", state->numberOfDisparities > 0 ? state->numberOfDisparities : 64);
-    sm->set("textureThreshold", state->textureThreshold);
-    sm->set("uniquenessRatio", state->uniquenessRatio);
-    sm->set("speckleRange", state->speckleRange);
-    sm->set("speckleWindowSize", state->speckleWindowSize);
-    sm->set("disp12MaxDiff", state->disp12MaxDiff);
+    sm->setPreFilterType(state->preFilterType);
+    sm->setPreFilterSize(state->preFilterSize);
+    sm->setPreFilterCap(state->preFilterCap);
+    sm->setBlockSize(state->SADWindowSize);
+    sm->setNumDisparities(state->numberOfDisparities > 0 ? state->numberOfDisparities : 64);
+    sm->setTextureThreshold(state->textureThreshold);
+    sm->setUniquenessRatio(state->uniquenessRatio);
+    sm->setSpeckleRange(state->speckleRange);
+    sm->setSpeckleWindowSize(state->speckleWindowSize);
+    sm->setDisp12MaxDiff(state->disp12MaxDiff);

    sm->compute(left, right, disp);
 }
@@ -111,8 +111,8 @@ void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,
 CvRect cvGetValidDisparityROI( CvRect roi1, CvRect roi2, int minDisparity,
                              int numberOfDisparities, int SADWindowSize )
 {
-    return (CvRect)cv::getValidDisparityROI( roi1, roi2, minDisparity,
-                                            numberOfDisparities, SADWindowSize );
+    return cvRect(cv::getValidDisparityROI( roi1, roi2, minDisparity,
+                                            numberOfDisparities, SADWindowSize));
 }

 void cvValidateDisparity( CvArr* _disp, const CvArr* _cost, int minDisparity,
@@ -0,0 +1,659 @@
+#include "precomp.hpp"
+#include "dls.h"
+
+#include <iostream>
+
+#ifdef HAVE_EIGEN
+#  if defined __GNUC__ && defined __APPLE__
+#    pragma GCC diagnostic ignored "-Wshadow"
+#  endif
+#  if defined(_MSC_VER)
+#    pragma warning(push)
+#    pragma warning(disable:4701)  // potentially uninitialized local variable
+#    pragma warning(disable:4702)  // unreachable code
+#    pragma warning(disable:4714)  // const marked as __forceinline not inlined
+#  endif
+#  include <Eigen/Core>
+#  include <Eigen/Eigenvalues>
+#  if defined(_MSC_VER)
+#    pragma warning(pop)
+#  endif
+#  include "opencv2/core/eigen.hpp"
+#endif
+
+using namespace std;
+
+dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints)
+{
+
+    N =  std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
+    p = cv::Mat(3, N, CV_64F);
+    z = cv::Mat(3, N, CV_64F);
+    mn = cv::Mat::zeros(3, 1, CV_64F);
+
+    cost__ = 9999;
+
+    f1coeff.resize(21);
+    f2coeff.resize(21);
+    f3coeff.resize(21);
+
+    if (opoints.depth() == ipoints.depth())
+    {
+        if (opoints.depth() == CV_32F)
+            init_points<cv::Point3f, cv::Point2f>(opoints, ipoints);
+        else
+            init_points<cv::Point3d, cv::Point2d>(opoints, ipoints);
+    }
+    else if (opoints.depth() == CV_32F)
+        init_points<cv::Point3f, cv::Point2d>(opoints, ipoints);
+    else
+        init_points<cv::Point3d, cv::Point2f>(opoints, ipoints);
+}
+
+dls::~dls()
+{
+  // TODO Auto-generated destructor stub
+}
+
+bool dls::compute_pose(cv::Mat& R, cv::Mat& t)
+{
+
+    std::vector<cv::Mat> R_;
+    R_.push_back(rotx(CV_PI/2));
+    R_.push_back(roty(CV_PI/2));
+    R_.push_back(rotz(CV_PI/2));
+
+    // version that calls dls 3 times, to avoid Cayley singularity
+    for (int i = 0; i < 3; ++i)
+    {
+        // Make a random rotation
+        cv::Mat pp = R_[i] * ( p - cv::repeat(mn, 1, p.cols) );
+
+        // clear for new data
+        C_est_.clear();
+        t_est_.clear();
+        cost_.clear();
+
+        this->run_kernel(pp); // run dls_pnp()
+
+        // find global minimum
+        for (unsigned int j = 0; j < cost_.size(); ++j)
+        {
+            if( cost_[j] < cost__ )
+            {
+                t_est__ = t_est_[j] - C_est_[j] * R_[i] * mn;
+                C_est__ = C_est_[j] * R_[i];
+                cost__ = cost_[j];
+            }
+        }
+
+    }
+
+    if(C_est__.cols > 0 && C_est__.rows > 0)
+    {
+        C_est__.copyTo(R);
+        t_est__.copyTo(t);
+        return true;
+    }
+
+    return false;
+}
+
+void dls::run_kernel(const cv::Mat& pp)
+{
+    cv::Mat Mtilde(27, 27, CV_64F);
+    cv::Mat D = cv::Mat::zeros(9, 9, CV_64F);
+    build_coeff_matrix(pp, Mtilde, D);
+
+    cv::Mat eigenval_r, eigenval_i, eigenvec_r, eigenvec_i;
+    compute_eigenvec(Mtilde, eigenval_r, eigenval_i, eigenvec_r, eigenvec_i);
+
+    /*
+     *  Now check the solutions
+     */
+
+    // extract the optimal solutions from the eigen decomposition of the
+    // Multiplication matrix
+
+    cv::Mat sols = cv::Mat::zeros(3, 27, CV_64F);
+    std::vector<double> cost;
+    int count = 0;
+    for (int k = 0; k < 27; ++k)
+    {
+        //  V(:,k) = V(:,k)/V(1,k);
+        cv::Mat V_kA = eigenvec_r.col(k); // 27x1
+        cv::Mat V_kB = cv::Mat(1, 1, z.depth(), V_kA.at<double>(0)); // 1x1
+        cv::Mat V_k; cv::solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A'
+        cv::Mat( V_k.t()).copyTo( eigenvec_r.col(k) );
+
+        //if (imag(V(2,k)) == 0)
+#ifdef HAVE_EIGEN
+        const double epsilon = 1e-4;
+        if( eigenval_i.at<double>(k,0) >= -epsilon && eigenval_i.at<double>(k,0) <= epsilon )
+#endif
+        {
+
+            double stmp[3];
+            stmp[0] = eigenvec_r.at<double>(9, k);
+            stmp[1] = eigenvec_r.at<double>(3, k);
+            stmp[2] = eigenvec_r.at<double>(1, k);
+
+            cv::Mat H = Hessian(stmp);
+
+            cv::Mat eigenvalues, eigenvectors;
+            cv::eigen(H, eigenvalues, eigenvectors);
+
+            if(positive_eigenvalues(&eigenvalues))
+            {
+
+                // sols(:,i) = stmp;
+                cv::Mat stmp_mat(3, 1, CV_64F, &stmp);
+
+                stmp_mat.copyTo( sols.col(count) );
+
+                cv::Mat Cbar = cayley2rotbar(stmp_mat);
+                cv::Mat Cbarvec = Cbar.reshape(1,1).t();
+
+                // cost(i) = CbarVec' * D * CbarVec;
+                cv::Mat cost_mat =  Cbarvec.t() * D * Cbarvec;
+                cost.push_back( cost_mat.at<double>(0) );
+
+                count++;
+            }
+        }
+    }
+
+    // extract solutions
+    sols = sols.clone().colRange(0, count);
+
+    std::vector<cv::Mat> C_est, t_est;
+    for (int j = 0; j < sols.cols; ++j)
+    {
+        // recover the optimal orientation
+        // C_est(:,:,j) = 1/(1 + sols(:,j)' * sols(:,j)) * cayley2rotbar(sols(:,j));
+
+        cv::Mat sols_j = sols.col(j);
+        double sols_mult = 1./(1.+cv::Mat( sols_j.t() * sols_j ).at<double>(0));
+        cv::Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult);
+        C_est.push_back( C_est_j );
+
+        cv::Mat A2 = cv::Mat::zeros(3, 3, CV_64F);
+        cv::Mat b2 = cv::Mat::zeros(3, 1, CV_64F);
+        for (int i = 0; i < N; ++i)
+        {
+            cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
+            cv::Mat z_mul = z.col(i)*z.col(i).t();
+
+            A2 += eye - z_mul;
+            b2 += (z_mul - eye) * C_est_j * pp.col(i);
+        }
+
+        // recover the optimal translation
+        cv::Mat X2; cv::solve(A2, b2, X2); // A\B
+        t_est.push_back(X2);
+
+    }
+
+    // check that the points are infront of the center of perspectivity
+    for (int k = 0; k < sols.cols; ++k)
+    {
+        cv::Mat cam_points = C_est[k] * pp + cv::repeat(t_est[k], 1, pp.cols);
+        cv::Mat cam_points_k = cam_points.row(2);
+
+        if(is_empty(&cam_points_k))
+        {
+            cv::Mat C_valid = C_est[k], t_valid = t_est[k];
+            double cost_valid = cost[k];
+
+            C_est_.push_back(C_valid);
+            t_est_.push_back(t_valid);
+            cost_.push_back(cost_valid);
+        }
+    }
+
+}
+
+void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D)
+{
+    CV_Assert(!pp.empty() && N > 0);
+    cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
+
+    // build coeff matrix
+    // An intermediate matrix, the inverse of what is called "H" in the paper
+    // (see eq. 25)
+
+    cv::Mat H = cv::Mat::zeros(3, 3, CV_64F);
+    cv::Mat A = cv::Mat::zeros(3, 9, CV_64F);
+    cv::Mat pp_i(3, 1, CV_64F);
+
+    cv::Mat z_i(3, 1, CV_64F);
+    for (int i = 0; i < N; ++i)
+    {
+        z.col(i).copyTo(z_i);
+        A += ( z_i*z_i.t() - eye ) * LeftMultVec(pp.col(i));
+    }
+
+    H = eye.mul(N) - z * z.t();
+
+    // A\B
+    cv::solve(H, A, A, cv::DECOMP_NORMAL);
+    H.release();
+
+    cv::Mat ppi_A(3, 1, CV_64F);
+    for (int i = 0; i < N; ++i)
+    {
+        z.col(i).copyTo(z_i);
+        ppi_A = LeftMultVec(pp.col(i)) + A;
+        D += ppi_A.t() * ( eye - z_i*z_i.t() ) * ppi_A;
+    }
+    A.release();
+
+    // fill the coefficients
+    fill_coeff(&D);
+
+    // generate random samples
+    std::vector<double> u(5);
+    cv::randn(u, 0, 200);
+
+    cv::Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u);
+
+    cv::Mat M2_1 = M2(cv::Range(0,27), cv::Range(0,27));
+    cv::Mat M2_2 = M2(cv::Range(0,27), cv::Range(27,120));
+    cv::Mat M2_3 = M2(cv::Range(27,120), cv::Range(27,120));
+    cv::Mat M2_4 = M2(cv::Range(27,120), cv::Range(0,27));
+    M2.release();
+
+    // A/B = B'\A'
+    cv::Mat M2_5; cv::solve(M2_3.t(), M2_2.t(), M2_5);
+    M2_2.release(); M2_3.release();
+
+    // construct the multiplication matrix via schur compliment of the Macaulay
+    // matrix
+    Mtilde = M2_1 - M2_5.t()*M2_4;
+
+}
+
+void dls::compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
+                                                  cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag)
+{
+#ifdef HAVE_EIGEN
+    Eigen::MatrixXd Mtilde_eig, zeros_eig;
+    cv::cv2eigen(Mtilde, Mtilde_eig);
+    cv::cv2eigen(cv::Mat::zeros(27, 27, CV_64F), zeros_eig);
+
+    Eigen::MatrixXcd Mtilde_eig_cmplx(27, 27);
+    Mtilde_eig_cmplx.real() = Mtilde_eig;
+    Mtilde_eig_cmplx.imag() = zeros_eig;
+
+    Eigen::ComplexEigenSolver<Eigen::MatrixXcd> ces;
+    ces.compute(Mtilde_eig_cmplx);
+
+    Eigen::MatrixXd eigval_real = ces.eigenvalues().real();
+    Eigen::MatrixXd eigval_imag = ces.eigenvalues().imag();
+    Eigen::MatrixXd eigvec_real = ces.eigenvectors().real();
+    Eigen::MatrixXd eigvec_imag = ces.eigenvectors().imag();
+
+    cv::eigen2cv(eigval_real, eigenval_real);
+    cv::eigen2cv(eigval_imag, eigenval_imag);
+    cv::eigen2cv(eigvec_real, eigenvec_real);
+    cv::eigen2cv(eigvec_imag, eigenvec_imag);
+#else
+    EigenvalueDecomposition es(Mtilde);
+    eigenval_real = es.eigenvalues();
+    eigenvec_real = es.eigenvectors();
+    eigenval_imag = eigenvec_imag = cv::Mat();
+#endif
+
+}
+
+void dls::fill_coeff(const cv::Mat * D_mat)
+{
+    // TODO: shift D and coefficients one position to left
+
+    double D[10][10]; // put D_mat into array
+
+    for (int i = 0; i < D_mat->rows; ++i)
+    {
+        const double* Di = D_mat->ptr<double>(i);
+        for (int j = 0; j < D_mat->cols; ++j)
+        {
+            D[i+1][j+1] = Di[j];
+        }
+    }
+
+    // F1 COEFFICIENT
+
+    f1coeff[1] = 2*D[1][6] - 2*D[1][8] + 2*D[5][6] - 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] - 2*D[8][1] - 2*D[8][5] - 2*D[8][9] + 2*D[9][6] - 2*D[9][8]; // constant term
+    f1coeff[2] = 6*D[1][2] + 6*D[1][4] + 6*D[2][1] - 6*D[2][5] - 6*D[2][9] + 6*D[4][1] - 6*D[4][5] - 6*D[4][9] - 6*D[5][2] - 6*D[5][4] - 6*D[9][2] - 6*D[9][4]; // s1^2  * s2
+    f1coeff[3] = 4*D[1][7] - 4*D[1][3] + 8*D[2][6] - 8*D[2][8] - 4*D[3][1] + 4*D[3][5] + 4*D[3][9] + 8*D[4][6] - 8*D[4][8] + 4*D[5][3] - 4*D[5][7] + 8*D[6][2] + 8*D[6][4] + 4*D[7][1] - 4*D[7][5] - 4*D[7][9] - 8*D[8][2] - 8*D[8][4] + 4*D[9][3] - 4*D[9][7]; // s1 * s2
+    f1coeff[4] = 4*D[1][2] - 4*D[1][4] + 4*D[2][1] - 4*D[2][5] - 4*D[2][9] + 8*D[3][6] - 8*D[3][8] - 4*D[4][1] + 4*D[4][5] + 4*D[4][9] - 4*D[5][2] + 4*D[5][4] + 8*D[6][3] + 8*D[6][7] + 8*D[7][6] - 8*D[7][8] - 8*D[8][3] - 8*D[8][7] - 4*D[9][2] + 4*D[9][4]; //s1 * s3
+    f1coeff[5] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s2 * s3
+    f1coeff[6] = 4*D[2][6] - 2*D[1][7] - 2*D[1][3] + 4*D[2][8] - 2*D[3][1] + 2*D[3][5] - 2*D[3][9] + 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] + 4*D[6][4] - 2*D[7][1] + 2*D[7][5] - 2*D[7][9] + 4*D[8][2] + 4*D[8][4] - 2*D[9][3] - 2*D[9][7]; // s2^2 * s3
+    f1coeff[7] = 2*D[2][5] - 2*D[1][4] - 2*D[2][1] - 2*D[1][2] - 2*D[2][9] - 2*D[4][1] + 2*D[4][5] - 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 2*D[9][2] - 2*D[9][4]; //s2^3
+    f1coeff[8] = 4*D[1][9] - 4*D[1][1] + 8*D[3][3] + 8*D[3][7] + 4*D[5][5] + 8*D[7][3] + 8*D[7][7] + 4*D[9][1] - 4*D[9][9]; // s1 * s3^2
+    f1coeff[9] = 4*D[1][1] - 4*D[5][5] - 4*D[5][9] + 8*D[6][6] - 8*D[6][8] - 8*D[8][6] + 8*D[8][8] - 4*D[9][5] - 4*D[9][9]; // s1
+    f1coeff[10] = 2*D[1][3] + 2*D[1][7] + 4*D[2][6] - 4*D[2][8] + 2*D[3][1] + 2*D[3][5] + 2*D[3][9] - 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] - 4*D[6][4] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 4*D[8][2] + 4*D[8][4] + 2*D[9][3] + 2*D[9][7]; // s3
+    f1coeff[11] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 4*D[3][6] + 4*D[3][8] + 2*D[4][1] + 2*D[4][5] + 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] + 4*D[8][3] - 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s2
+    f1coeff[12] = 2*D[2][9] - 2*D[1][4] - 2*D[2][1] - 2*D[2][5] - 2*D[1][2] + 4*D[3][6] + 4*D[3][8] - 2*D[4][1] - 2*D[4][5] + 2*D[4][9] - 2*D[5][2] - 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] + 4*D[8][3] + 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s2 * s3^2
+    f1coeff[13] = 6*D[1][6] - 6*D[1][8] - 6*D[5][6] + 6*D[5][8] + 6*D[6][1] - 6*D[6][5] - 6*D[6][9] - 6*D[8][1] + 6*D[8][5] + 6*D[8][9] - 6*D[9][6] + 6*D[9][8]; // s1^2
+    f1coeff[14] = 2*D[1][8] - 2*D[1][6] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] - 4*D[3][4] - 4*D[4][3] - 4*D[4][7] - 2*D[5][6] + 2*D[5][8] - 2*D[6][1] - 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] - 2*D[8][9] + 2*D[9][6] - 2*D[9][8]; // s3^2
+    f1coeff[15] = 2*D[1][8] - 2*D[1][6] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] - 4*D[3][4] - 4*D[4][3] + 4*D[4][7] + 2*D[5][6] - 2*D[5][8] - 2*D[6][1] + 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] + 2*D[8][9] - 2*D[9][6] + 2*D[9][8]; // s2^2
+    f1coeff[16] = 2*D[3][9] - 2*D[1][7] - 2*D[3][1] - 2*D[3][5] - 2*D[1][3] - 2*D[5][3] - 2*D[5][7] - 2*D[7][1] - 2*D[7][5] + 2*D[7][9] + 2*D[9][3] + 2*D[9][7]; // s3^3
+    f1coeff[17] = 4*D[1][6] + 4*D[1][8] + 8*D[2][3] + 8*D[2][7] + 8*D[3][2] + 8*D[3][4] + 8*D[4][3] + 8*D[4][7] - 4*D[5][6] - 4*D[5][8] + 4*D[6][1] - 4*D[6][5] - 4*D[6][9] + 8*D[7][2] + 8*D[7][4] + 4*D[8][1] - 4*D[8][5] - 4*D[8][9] - 4*D[9][6] - 4*D[9][8]; // s1 * s2 * s3
+    f1coeff[18] = 4*D[1][5] - 4*D[1][1] + 8*D[2][2] + 8*D[2][4] + 8*D[4][2] + 8*D[4][4] + 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s1 * s2^2
+    f1coeff[19] = 6*D[1][3] + 6*D[1][7] + 6*D[3][1] - 6*D[3][5] - 6*D[3][9] - 6*D[5][3] - 6*D[5][7] + 6*D[7][1] - 6*D[7][5] - 6*D[7][9] - 6*D[9][3] - 6*D[9][7]; // s1^2 * s3
+    f1coeff[20] = 4*D[1][1] - 4*D[1][5] - 4*D[1][9] - 4*D[5][1] + 4*D[5][5] + 4*D[5][9] - 4*D[9][1] + 4*D[9][5] + 4*D[9][9]; // s1^3
+
+
+    // F2 COEFFICIENT
+
+    f2coeff[1] = - 2*D[1][3] + 2*D[1][7] - 2*D[3][1] - 2*D[3][5] - 2*D[3][9] - 2*D[5][3] + 2*D[5][7] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 2*D[9][3] + 2*D[9][7]; // constant term
+    f2coeff[2] = 4*D[1][5] - 4*D[1][1] + 8*D[2][2] + 8*D[2][4] + 8*D[4][2] + 8*D[4][4] + 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s1^2  * s2
+    f2coeff[3] = 4*D[1][8] - 4*D[1][6] - 8*D[2][3] + 8*D[2][7] - 8*D[3][2] - 8*D[3][4] - 8*D[4][3] + 8*D[4][7] + 4*D[5][6] - 4*D[5][8] - 4*D[6][1] + 4*D[6][5] - 4*D[6][9] + 8*D[7][2] + 8*D[7][4] + 4*D[8][1] - 4*D[8][5] + 4*D[8][9] - 4*D[9][6] + 4*D[9][8]; // s1 * s2
+    f2coeff[4] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s1 * s3
+    f2coeff[5] = 4*D[1][4] - 4*D[1][2] - 4*D[2][1] + 4*D[2][5] - 4*D[2][9] - 8*D[3][6] - 8*D[3][8] + 4*D[4][1] - 4*D[4][5] + 4*D[4][9] + 4*D[5][2] - 4*D[5][4] - 8*D[6][3] + 8*D[6][7] + 8*D[7][6] + 8*D[7][8] - 8*D[8][3] + 8*D[8][7] - 4*D[9][2] + 4*D[9][4]; // s2 * s3
+    f2coeff[6] = 6*D[5][6] - 6*D[1][8] - 6*D[1][6] + 6*D[5][8] - 6*D[6][1] + 6*D[6][5] - 6*D[6][9] - 6*D[8][1] + 6*D[8][5] - 6*D[8][9] - 6*D[9][6] - 6*D[9][8]; // s2^2 * s3
+    f2coeff[7] = 4*D[1][1] - 4*D[1][5] + 4*D[1][9] - 4*D[5][1] + 4*D[5][5] - 4*D[5][9] + 4*D[9][1] - 4*D[9][5] + 4*D[9][9]; // s2^3
+    f2coeff[8] = 2*D[2][9] - 2*D[1][4] - 2*D[2][1] - 2*D[2][5] - 2*D[1][2] + 4*D[3][6] + 4*D[3][8] - 2*D[4][1] - 2*D[4][5] + 2*D[4][9] - 2*D[5][2] - 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] + 4*D[8][3] + 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s1 * s3^2
+    f2coeff[9] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 4*D[3][6] + 4*D[3][8] + 2*D[4][1] + 2*D[4][5] + 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] + 4*D[8][3] - 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s1
+    f2coeff[10] = 2*D[1][6] + 2*D[1][8] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] + 4*D[3][4] + 4*D[4][3] - 4*D[4][7] + 2*D[5][6] + 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s3
+    f2coeff[11] = 8*D[3][3] - 4*D[1][9] - 4*D[1][1] - 8*D[3][7] + 4*D[5][5] - 8*D[7][3] + 8*D[7][7] - 4*D[9][1] - 4*D[9][9]; // s2
+    f2coeff[12] = 4*D[1][1] - 4*D[5][5] + 4*D[5][9] + 8*D[6][6] + 8*D[6][8] + 8*D[8][6] + 8*D[8][8] + 4*D[9][5] - 4*D[9][9]; // s2 * s3^2
+    f2coeff[13] = 2*D[1][7] - 2*D[1][3] + 4*D[2][6] - 4*D[2][8] - 2*D[3][1] + 2*D[3][5] + 2*D[3][9] + 4*D[4][6] - 4*D[4][8] + 2*D[5][3] - 2*D[5][7] + 4*D[6][2] + 4*D[6][4] + 2*D[7][1] - 2*D[7][5] - 2*D[7][9] - 4*D[8][2] - 4*D[8][4] + 2*D[9][3] - 2*D[9][7]; // s1^2
+    f2coeff[14] = 2*D[1][3] - 2*D[1][7] + 4*D[2][6] + 4*D[2][8] + 2*D[3][1] + 2*D[3][5] - 2*D[3][9] - 4*D[4][6] - 4*D[4][8] + 2*D[5][3] - 2*D[5][7] + 4*D[6][2] - 4*D[6][4] - 2*D[7][1] - 2*D[7][5] + 2*D[7][9] + 4*D[8][2] - 4*D[8][4] - 2*D[9][3] + 2*D[9][7]; // s3^2
+    f2coeff[15] = 6*D[1][3] - 6*D[1][7] + 6*D[3][1] - 6*D[3][5] + 6*D[3][9] - 6*D[5][3] + 6*D[5][7] - 6*D[7][1] + 6*D[7][5] - 6*D[7][9] + 6*D[9][3] - 6*D[9][7]; // s2^2
+    f2coeff[16] = 2*D[6][9] - 2*D[1][8] - 2*D[5][6] - 2*D[5][8] - 2*D[6][1] - 2*D[6][5] - 2*D[1][6] - 2*D[8][1] - 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s3^3
+    f2coeff[17] = 8*D[2][6] - 4*D[1][7] - 4*D[1][3] + 8*D[2][8] - 4*D[3][1] + 4*D[3][5] - 4*D[3][9] + 8*D[4][6] + 8*D[4][8] + 4*D[5][3] + 4*D[5][7] + 8*D[6][2] + 8*D[6][4] - 4*D[7][1] + 4*D[7][5] - 4*D[7][9] + 8*D[8][2] + 8*D[8][4] - 4*D[9][3] - 4*D[9][7]; // s1 * s2 * s3
+    f2coeff[18] = 6*D[2][5] - 6*D[1][4] - 6*D[2][1] - 6*D[1][2] - 6*D[2][9] - 6*D[4][1] + 6*D[4][5] - 6*D[4][9] + 6*D[5][2] + 6*D[5][4] - 6*D[9][2] - 6*D[9][4]; // s1 * s2^2
+    f2coeff[19] = 2*D[1][6] + 2*D[1][8] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] + 4*D[3][4] + 4*D[4][3] + 4*D[4][7] - 2*D[5][6] - 2*D[5][8] + 2*D[6][1] - 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s1^2 * s3
+    f2coeff[20] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] - 2*D[2][5] - 2*D[2][9] + 2*D[4][1] - 2*D[4][5] - 2*D[4][9] - 2*D[5][2] - 2*D[5][4] - 2*D[9][2] - 2*D[9][4]; // s1^3
+
+
+    // F3 COEFFICIENT
+
+    f3coeff[1] = 2*D[1][2] - 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 2*D[4][1] - 2*D[4][5] - 2*D[4][9] + 2*D[5][2] - 2*D[5][4] + 2*D[9][2] - 2*D[9][4]; // constant term
+    f3coeff[2] = 2*D[1][6] + 2*D[1][8] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] + 4*D[3][4] + 4*D[4][3] + 4*D[4][7] - 2*D[5][6] - 2*D[5][8] + 2*D[6][1] - 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s1^2  * s2
+    f3coeff[3] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s1 * s2
+    f3coeff[4] = 4*D[1][8] - 4*D[1][6] + 8*D[2][3] + 8*D[2][7] + 8*D[3][2] - 8*D[3][4] - 8*D[4][3] - 8*D[4][7] - 4*D[5][6] + 4*D[5][8] - 4*D[6][1] - 4*D[6][5] + 4*D[6][9] + 8*D[7][2] - 8*D[7][4] + 4*D[8][1] + 4*D[8][5] - 4*D[8][9] + 4*D[9][6] - 4*D[9][8]; // s1 * s3
+    f3coeff[5] = 4*D[1][3] - 4*D[1][7] + 8*D[2][6] + 8*D[2][8] + 4*D[3][1] + 4*D[3][5] - 4*D[3][9] - 8*D[4][6] - 8*D[4][8] + 4*D[5][3] - 4*D[5][7] + 8*D[6][2] - 8*D[6][4] - 4*D[7][1] - 4*D[7][5] + 4*D[7][9] + 8*D[8][2] - 8*D[8][4] - 4*D[9][3] + 4*D[9][7]; // s2 * s3
+    f3coeff[6] = 4*D[1][1] - 4*D[5][5] + 4*D[5][9] + 8*D[6][6] + 8*D[6][8] + 8*D[8][6] + 8*D[8][8] + 4*D[9][5] - 4*D[9][9]; // s2^2 * s3
+    f3coeff[7] = 2*D[5][6] - 2*D[1][8] - 2*D[1][6] + 2*D[5][8] - 2*D[6][1] + 2*D[6][5] - 2*D[6][9] - 2*D[8][1] + 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s2^3
+    f3coeff[8] = 6*D[3][9] - 6*D[1][7] - 6*D[3][1] - 6*D[3][5] - 6*D[1][3] - 6*D[5][3] - 6*D[5][7] - 6*D[7][1] - 6*D[7][5] + 6*D[7][9] + 6*D[9][3] + 6*D[9][7]; // s1 * s3^2
+    f3coeff[9] = 2*D[1][3] + 2*D[1][7] + 4*D[2][6] - 4*D[2][8] + 2*D[3][1] + 2*D[3][5] + 2*D[3][9] - 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] - 4*D[6][4] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 4*D[8][2] + 4*D[8][4] + 2*D[9][3] + 2*D[9][7]; // s1
+    f3coeff[10] = 8*D[2][2] - 4*D[1][5] - 4*D[1][1] - 8*D[2][4] - 8*D[4][2] + 8*D[4][4] - 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s3
+    f3coeff[11] = 2*D[1][6] + 2*D[1][8] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] + 4*D[3][4] + 4*D[4][3] - 4*D[4][7] + 2*D[5][6] + 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s2
+    f3coeff[12] = 6*D[6][9] - 6*D[1][8] - 6*D[5][6] - 6*D[5][8] - 6*D[6][1] - 6*D[6][5] - 6*D[1][6] - 6*D[8][1] - 6*D[8][5] + 6*D[8][9] + 6*D[9][6] + 6*D[9][8]; // s2 * s3^2
+    f3coeff[13] = 2*D[1][2] - 2*D[1][4] + 2*D[2][1] - 2*D[2][5] - 2*D[2][9] + 4*D[3][6] - 4*D[3][8] - 2*D[4][1] + 2*D[4][5] + 2*D[4][9] - 2*D[5][2] + 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] - 4*D[8][3] - 4*D[8][7] - 2*D[9][2] + 2*D[9][4]; // s1^2
+    f3coeff[14] = 6*D[1][4] - 6*D[1][2] - 6*D[2][1] - 6*D[2][5] + 6*D[2][9] + 6*D[4][1] + 6*D[4][5] - 6*D[4][9] - 6*D[5][2] + 6*D[5][4] + 6*D[9][2] - 6*D[9][4]; // s3^2
+    f3coeff[15] = 2*D[1][4] - 2*D[1][2] - 2*D[2][1] + 2*D[2][5] - 2*D[2][9] - 4*D[3][6] - 4*D[3][8] + 2*D[4][1] - 2*D[4][5] + 2*D[4][9] + 2*D[5][2] - 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] - 4*D[8][3] + 4*D[8][7] - 2*D[9][2] + 2*D[9][4]; // s2^2
+    f3coeff[16] = 4*D[1][1] + 4*D[1][5] - 4*D[1][9] + 4*D[5][1] + 4*D[5][5] - 4*D[5][9] - 4*D[9][1] - 4*D[9][5] + 4*D[9][9]; // s3^3
+    f3coeff[17] = 4*D[2][9] - 4*D[1][4] - 4*D[2][1] - 4*D[2][5] - 4*D[1][2] + 8*D[3][6] + 8*D[3][8] - 4*D[4][1] - 4*D[4][5] + 4*D[4][9] - 4*D[5][2] - 4*D[5][4] + 8*D[6][3] + 8*D[6][7] + 8*D[7][6] + 8*D[7][8] + 8*D[8][3] + 8*D[8][7] + 4*D[9][2] + 4*D[9][4]; // s1 * s2 * s3
+    f3coeff[18] = 4*D[2][6] - 2*D[1][7] - 2*D[1][3] + 4*D[2][8] - 2*D[3][1] + 2*D[3][5] - 2*D[3][9] + 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] + 4*D[6][4] - 2*D[7][1] + 2*D[7][5] - 2*D[7][9] + 4*D[8][2] + 4*D[8][4] - 2*D[9][3] - 2*D[9][7]; // s1 * s2^2
+    f3coeff[19] = 4*D[1][9] - 4*D[1][1] + 8*D[3][3] + 8*D[3][7] + 4*D[5][5] + 8*D[7][3] + 8*D[7][7] + 4*D[9][1] - 4*D[9][9]; // s1^2 * s3
+    f3coeff[20] = 2*D[1][3] + 2*D[1][7] + 2*D[3][1] - 2*D[3][5] - 2*D[3][9] - 2*D[5][3] - 2*D[5][7] + 2*D[7][1] - 2*D[7][5] - 2*D[7][9] - 2*D[9][3] - 2*D[9][7]; // s1^3
+
+}
+
+cv::Mat dls::LeftMultVec(const cv::Mat& v)
+{
+    cv::Mat mat_ = cv::Mat::zeros(3, 9, CV_64F);
+
+    for (int i = 0; i < 3; ++i)
+    {
+        mat_.at<double>(i, 3*i + 0) = v.at<double>(0);
+        mat_.at<double>(i, 3*i + 1) = v.at<double>(1);
+        mat_.at<double>(i, 3*i + 2) = v.at<double>(2);
+    }
+    return mat_;
+}
+
+cv::Mat dls::cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b, const std::vector<double>& c, const std::vector<double>& u)
+{
+    // TODO: input matrix pointer
+    // TODO: shift coefficients one position to left
+
+    cv::Mat M = cv::Mat::zeros(120, 120, CV_64F);
+
+    M.at<double>(0,0)=u[1]; M.at<double>(0,35)=a[1]; M.at<double>(0,83)=b[1]; M.at<double>(0,118)=c[1];
+    M.at<double>(1,0)=u[4]; M.at<double>(1,1)=u[1]; M.at<double>(1,34)=a[1]; M.at<double>(1,35)=a[10]; M.at<double>(1,54)=b[1]; M.at<double>(1,83)=b[10]; M.at<double>(1,99)=c[1]; M.at<double>(1,118)=c[10];
+    M.at<double>(2,1)=u[4]; M.at<double>(2,2)=u[1]; M.at<double>(2,34)=a[10]; M.at<double>(2,35)=a[14]; M.at<double>(2,51)=a[1]; M.at<double>(2,54)=b[10]; M.at<double>(2,65)=b[1]; M.at<double>(2,83)=b[14]; M.at<double>(2,89)=c[1]; M.at<double>(2,99)=c[10]; M.at<double>(2,118)=c[14];
+    M.at<double>(3,0)=u[3]; M.at<double>(3,3)=u[1]; M.at<double>(3,35)=a[11]; M.at<double>(3,49)=a[1]; M.at<double>(3,76)=b[1]; M.at<double>(3,83)=b[11]; M.at<double>(3,118)=c[11]; M.at<double>(3,119)=c[1];
+    M.at<double>(4,1)=u[3]; M.at<double>(4,3)=u[4]; M.at<double>(4,4)=u[1]; M.at<double>(4,34)=a[11]; M.at<double>(4,35)=a[5]; M.at<double>(4,43)=a[1]; M.at<double>(4,49)=a[10]; M.at<double>(4,54)=b[11]; M.at<double>(4,71)=b[1]; M.at<double>(4,76)=b[10]; M.at<double>(4,83)=b[5]; M.at<double>(4,99)=c[11]; M.at<double>(4,100)=c[1]; M.at<double>(4,118)=c[5]; M.at<double>(4,119)=c[10];
+    M.at<double>(5,2)=u[3]; M.at<double>(5,4)=u[4]; M.at<double>(5,5)=u[1]; M.at<double>(5,34)=a[5]; M.at<double>(5,35)=a[12]; M.at<double>(5,41)=a[1]; M.at<double>(5,43)=a[10]; M.at<double>(5,49)=a[14]; M.at<double>(5,51)=a[11]; M.at<double>(5,54)=b[5]; M.at<double>(5,62)=b[1]; M.at<double>(5,65)=b[11]; M.at<double>(5,71)=b[10]; M.at<double>(5,76)=b[14]; M.at<double>(5,83)=b[12]; M.at<double>(5,89)=c[11]; M.at<double>(5,99)=c[5]; M.at<double>(5,100)=c[10]; M.at<double>(5,111)=c[1]; M.at<double>(5,118)=c[12]; M.at<double>(5,119)=c[14];
+    M.at<double>(6,3)=u[3]; M.at<double>(6,6)=u[1]; M.at<double>(6,30)=a[1]; M.at<double>(6,35)=a[15]; M.at<double>(6,49)=a[11]; M.at<double>(6,75)=b[1]; M.at<double>(6,76)=b[11]; M.at<double>(6,83)=b[15]; M.at<double>(6,107)=c[1]; M.at<double>(6,118)=c[15]; M.at<double>(6,119)=c[11];
+    M.at<double>(7,4)=u[3]; M.at<double>(7,6)=u[4]; M.at<double>(7,7)=u[1]; M.at<double>(7,30)=a[10]; M.at<double>(7,34)=a[15]; M.at<double>(7,35)=a[6]; M.at<double>(7,43)=a[11]; M.at<double>(7,45)=a[1]; M.at<double>(7,49)=a[5]; M.at<double>(7,54)=b[15]; M.at<double>(7,63)=b[1]; M.at<double>(7,71)=b[11]; M.at<double>(7,75)=b[10]; M.at<double>(7,76)=b[5]; M.at<double>(7,83)=b[6]; M.at<double>(7,99)=c[15]; M.at<double>(7,100)=c[11]; M.at<double>(7,107)=c[10]; M.at<double>(7,112)=c[1]; M.at<double>(7,118)=c[6]; M.at<double>(7,119)=c[5];
+    M.at<double>(8,5)=u[3]; M.at<double>(8,7)=u[4]; M.at<double>(8,8)=u[1]; M.at<double>(8,30)=a[14]; M.at<double>(8,34)=a[6]; M.at<double>(8,41)=a[11]; M.at<double>(8,43)=a[5]; M.at<double>(8,45)=a[10]; M.at<double>(8,46)=a[1]; M.at<double>(8,49)=a[12]; M.at<double>(8,51)=a[15]; M.at<double>(8,54)=b[6]; M.at<double>(8,62)=b[11]; M.at<double>(8,63)=b[10]; M.at<double>(8,65)=b[15]; M.at<double>(8,66)=b[1]; M.at<double>(8,71)=b[5]; M.at<double>(8,75)=b[14]; M.at<double>(8,76)=b[12]; M.at<double>(8,89)=c[15]; M.at<double>(8,99)=c[6]; M.at<double>(8,100)=c[5]; M.at<double>(8,102)=c[1]; M.at<double>(8,107)=c[14]; M.at<double>(8,111)=c[11]; M.at<double>(8,112)=c[10]; M.at<double>(8,119)=c[12];
+    M.at<double>(9,0)=u[2]; M.at<double>(9,9)=u[1]; M.at<double>(9,35)=a[9]; M.at<double>(9,36)=a[1]; M.at<double>(9,83)=b[9]; M.at<double>(9,84)=b[1]; M.at<double>(9,88)=c[1]; M.at<double>(9,118)=c[9];
+    M.at<double>(10,1)=u[2]; M.at<double>(10,9)=u[4]; M.at<double>(10,10)=u[1]; M.at<double>(10,33)=a[1]; M.at<double>(10,34)=a[9]; M.at<double>(10,35)=a[4]; M.at<double>(10,36)=a[10]; M.at<double>(10,54)=b[9]; M.at<double>(10,59)=b[1]; M.at<double>(10,83)=b[4]; M.at<double>(10,84)=b[10]; M.at<double>(10,88)=c[10]; M.at<double>(10,99)=c[9]; M.at<double>(10,117)=c[1]; M.at<double>(10,118)=c[4];
+    M.at<double>(11,2)=u[2]; M.at<double>(11,10)=u[4]; M.at<double>(11,11)=u[1]; M.at<double>(11,28)=a[1]; M.at<double>(11,33)=a[10]; M.at<double>(11,34)=a[4]; M.at<double>(11,35)=a[8]; M.at<double>(11,36)=a[14]; M.at<double>(11,51)=a[9]; M.at<double>(11,54)=b[4]; M.at<double>(11,57)=b[1]; M.at<double>(11,59)=b[10]; M.at<double>(11,65)=b[9]; M.at<double>(11,83)=b[8]; M.at<double>(11,84)=b[14]; M.at<double>(11,88)=c[14]; M.at<double>(11,89)=c[9]; M.at<double>(11,99)=c[4]; M.at<double>(11,114)=c[1]; M.at<double>(11,117)=c[10]; M.at<double>(11,118)=c[8];
+    M.at<double>(12,3)=u[2]; M.at<double>(12,9)=u[3]; M.at<double>(12,12)=u[1]; M.at<double>(12,35)=a[3]; M.at<double>(12,36)=a[11]; M.at<double>(12,39)=a[1]; M.at<double>(12,49)=a[9]; M.at<double>(12,76)=b[9]; M.at<double>(12,79)=b[1]; M.at<double>(12,83)=b[3]; M.at<double>(12,84)=b[11]; M.at<double>(12,88)=c[11]; M.at<double>(12,96)=c[1]; M.at<double>(12,118)=c[3]; M.at<double>(12,119)=c[9];
+    M.at<double>(13,4)=u[2]; M.at<double>(13,10)=u[3]; M.at<double>(13,12)=u[4]; M.at<double>(13,13)=u[1]; M.at<double>(13,33)=a[11]; M.at<double>(13,34)=a[3]; M.at<double>(13,35)=a[17]; M.at<double>(13,36)=a[5]; M.at<double>(13,39)=a[10]; M.at<double>(13,43)=a[9]; M.at<double>(13,47)=a[1]; M.at<double>(13,49)=a[4]; M.at<double>(13,54)=b[3]; M.at<double>(13,59)=b[11]; M.at<double>(13,60)=b[1]; M.at<double>(13,71)=b[9]; M.at<double>(13,76)=b[4]; M.at<double>(13,79)=b[10]; M.at<double>(13,83)=b[17]; M.at<double>(13,84)=b[5]; M.at<double>(13,88)=c[5]; M.at<double>(13,90)=c[1]; M.at<double>(13,96)=c[10]; M.at<double>(13,99)=c[3]; M.at<double>(13,100)=c[9]; M.at<double>(13,117)=c[11]; M.at<double>(13,118)=c[17]; M.at<double>(13,119)=c[4];
+    M.at<double>(14,5)=u[2]; M.at<double>(14,11)=u[3]; M.at<double>(14,13)=u[4]; M.at<double>(14,14)=u[1]; M.at<double>(14,28)=a[11]; M.at<double>(14,33)=a[5]; M.at<double>(14,34)=a[17]; M.at<double>(14,36)=a[12]; M.at<double>(14,39)=a[14]; M.at<double>(14,41)=a[9]; M.at<double>(14,42)=a[1]; M.at<double>(14,43)=a[4]; M.at<double>(14,47)=a[10]; M.at<double>(14,49)=a[8]; M.at<double>(14,51)=a[3]; M.at<double>(14,54)=b[17]; M.at<double>(14,56)=b[1]; M.at<double>(14,57)=b[11]; M.at<double>(14,59)=b[5]; M.at<double>(14,60)=b[10]; M.at<double>(14,62)=b[9]; M.at<double>(14,65)=b[3]; M.at<double>(14,71)=b[4]; M.at<double>(14,76)=b[8]; M.at<double>(14,79)=b[14]; M.at<double>(14,84)=b[12]; M.at<double>(14,88)=c[12]; M.at<double>(14,89)=c[3]; M.at<double>(14,90)=c[10]; M.at<double>(14,96)=c[14]; M.at<double>(14,99)=c[17]; M.at<double>(14,100)=c[4]; M.at<double>(14,106)=c[1]; M.at<double>(14,111)=c[9]; M.at<double>(14,114)=c[11]; M.at<double>(14,117)=c[5]; M.at<double>(14,119)=c[8];
+    M.at<double>(15,6)=u[2]; M.at<double>(15,12)=u[3]; M.at<double>(15,15)=u[1]; M.at<double>(15,29)=a[1]; M.at<double>(15,30)=a[9]; M.at<double>(15,35)=a[18]; M.at<double>(15,36)=a[15]; M.at<double>(15,39)=a[11]; M.at<double>(15,49)=a[3]; M.at<double>(15,74)=b[1]; M.at<double>(15,75)=b[9]; M.at<double>(15,76)=b[3]; M.at<double>(15,79)=b[11]; M.at<double>(15,83)=b[18]; M.at<double>(15,84)=b[15]; M.at<double>(15,88)=c[15]; M.at<double>(15,94)=c[1]; M.at<double>(15,96)=c[11]; M.at<double>(15,107)=c[9]; M.at<double>(15,118)=c[18]; M.at<double>(15,119)=c[3];
+    M.at<double>(16,7)=u[2]; M.at<double>(16,13)=u[3]; M.at<double>(16,15)=u[4]; M.at<double>(16,16)=u[1]; M.at<double>(16,29)=a[10]; M.at<double>(16,30)=a[4]; M.at<double>(16,33)=a[15]; M.at<double>(16,34)=a[18]; M.at<double>(16,36)=a[6]; M.at<double>(16,39)=a[5]; M.at<double>(16,43)=a[3]; M.at<double>(16,44)=a[1]; M.at<double>(16,45)=a[9]; M.at<double>(16,47)=a[11]; M.at<double>(16,49)=a[17]; M.at<double>(16,54)=b[18]; M.at<double>(16,59)=b[15]; M.at<double>(16,60)=b[11]; M.at<double>(16,63)=b[9]; M.at<double>(16,68)=b[1]; M.at<double>(16,71)=b[3]; M.at<double>(16,74)=b[10]; M.at<double>(16,75)=b[4]; M.at<double>(16,76)=b[17]; M.at<double>(16,79)=b[5]; M.at<double>(16,84)=b[6]; M.at<double>(16,88)=c[6]; M.at<double>(16,90)=c[11]; M.at<double>(16,94)=c[10]; M.at<double>(16,96)=c[5]; M.at<double>(16,97)=c[1]; M.at<double>(16,99)=c[18]; M.at<double>(16,100)=c[3]; M.at<double>(16,107)=c[4]; M.at<double>(16,112)=c[9]; M.at<double>(16,117)=c[15]; M.at<double>(16,119)=c[17];
+    M.at<double>(17,8)=u[2]; M.at<double>(17,14)=u[3]; M.at<double>(17,16)=u[4]; M.at<double>(17,17)=u[1]; M.at<double>(17,28)=a[15]; M.at<double>(17,29)=a[14]; M.at<double>(17,30)=a[8]; M.at<double>(17,33)=a[6]; M.at<double>(17,39)=a[12]; M.at<double>(17,41)=a[3]; M.at<double>(17,42)=a[11]; M.at<double>(17,43)=a[17]; M.at<double>(17,44)=a[10]; M.at<double>(17,45)=a[4]; M.at<double>(17,46)=a[9]; M.at<double>(17,47)=a[5]; M.at<double>(17,51)=a[18]; M.at<double>(17,56)=b[11]; M.at<double>(17,57)=b[15]; M.at<double>(17,59)=b[6]; M.at<double>(17,60)=b[5]; M.at<double>(17,62)=b[3]; M.at<double>(17,63)=b[4]; M.at<double>(17,65)=b[18]; M.at<double>(17,66)=b[9]; M.at<double>(17,68)=b[10]; M.at<double>(17,71)=b[17]; M.at<double>(17,74)=b[14]; M.at<double>(17,75)=b[8]; M.at<double>(17,79)=b[12]; M.at<double>(17,89)=c[18]; M.at<double>(17,90)=c[5]; M.at<double>(17,94)=c[14]; M.at<double>(17,96)=c[12]; M.at<double>(17,97)=c[10]; M.at<double>(17,100)=c[17]; M.at<double>(17,102)=c[9]; M.at<double>(17,106)=c[11]; M.at<double>(17,107)=c[8]; M.at<double>(17,111)=c[3]; M.at<double>(17,112)=c[4]; M.at<double>(17,114)=c[15]; M.at<double>(17,117)=c[6];
+    M.at<double>(18,9)=u[2]; M.at<double>(18,18)=u[1]; M.at<double>(18,35)=a[13]; M.at<double>(18,36)=a[9]; M.at<double>(18,53)=a[1]; M.at<double>(18,82)=b[1]; M.at<double>(18,83)=b[13]; M.at<double>(18,84)=b[9]; M.at<double>(18,87)=c[1]; M.at<double>(18,88)=c[9]; M.at<double>(18,118)=c[13];
+    M.at<double>(19,10)=u[2]; M.at<double>(19,18)=u[4]; M.at<double>(19,19)=u[1]; M.at<double>(19,32)=a[1]; M.at<double>(19,33)=a[9]; M.at<double>(19,34)=a[13]; M.at<double>(19,35)=a[19]; M.at<double>(19,36)=a[4]; M.at<double>(19,53)=a[10]; M.at<double>(19,54)=b[13]; M.at<double>(19,59)=b[9]; M.at<double>(19,61)=b[1]; M.at<double>(19,82)=b[10]; M.at<double>(19,83)=b[19]; M.at<double>(19,84)=b[4]; M.at<double>(19,87)=c[10]; M.at<double>(19,88)=c[4]; M.at<double>(19,99)=c[13]; M.at<double>(19,116)=c[1]; M.at<double>(19,117)=c[9]; M.at<double>(19,118)=c[19];
+    M.at<double>(20,11)=u[2]; M.at<double>(20,19)=u[4]; M.at<double>(20,20)=u[1]; M.at<double>(20,27)=a[1]; M.at<double>(20,28)=a[9]; M.at<double>(20,32)=a[10]; M.at<double>(20,33)=a[4]; M.at<double>(20,34)=a[19]; M.at<double>(20,36)=a[8]; M.at<double>(20,51)=a[13]; M.at<double>(20,53)=a[14]; M.at<double>(20,54)=b[19]; M.at<double>(20,55)=b[1]; M.at<double>(20,57)=b[9]; M.at<double>(20,59)=b[4]; M.at<double>(20,61)=b[10]; M.at<double>(20,65)=b[13]; M.at<double>(20,82)=b[14]; M.at<double>(20,84)=b[8]; M.at<double>(20,87)=c[14]; M.at<double>(20,88)=c[8]; M.at<double>(20,89)=c[13]; M.at<double>(20,99)=c[19]; M.at<double>(20,113)=c[1]; M.at<double>(20,114)=c[9]; M.at<double>(20,116)=c[10]; M.at<double>(20,117)=c[4];
+    M.at<double>(21,12)=u[2]; M.at<double>(21,18)=u[3]; M.at<double>(21,21)=u[1]; M.at<double>(21,35)=a[2]; M.at<double>(21,36)=a[3]; M.at<double>(21,38)=a[1]; M.at<double>(21,39)=a[9]; M.at<double>(21,49)=a[13]; M.at<double>(21,53)=a[11]; M.at<double>(21,76)=b[13]; M.at<double>(21,78)=b[1]; M.at<double>(21,79)=b[9]; M.at<double>(21,82)=b[11]; M.at<double>(21,83)=b[2]; M.at<double>(21,84)=b[3]; M.at<double>(21,87)=c[11]; M.at<double>(21,88)=c[3]; M.at<double>(21,92)=c[1]; M.at<double>(21,96)=c[9]; M.at<double>(21,118)=c[2]; M.at<double>(21,119)=c[13];
+    M.at<double>(22,13)=u[2]; M.at<double>(22,19)=u[3]; M.at<double>(22,21)=u[4]; M.at<double>(22,22)=u[1]; M.at<double>(22,32)=a[11]; M.at<double>(22,33)=a[3]; M.at<double>(22,34)=a[2]; M.at<double>(22,36)=a[17]; M.at<double>(22,38)=a[10]; M.at<double>(22,39)=a[4]; M.at<double>(22,40)=a[1]; M.at<double>(22,43)=a[13]; M.at<double>(22,47)=a[9]; M.at<double>(22,49)=a[19]; M.at<double>(22,53)=a[5]; M.at<double>(22,54)=b[2]; M.at<double>(22,59)=b[3]; M.at<double>(22,60)=b[9]; M.at<double>(22,61)=b[11]; M.at<double>(22,71)=b[13]; M.at<double>(22,72)=b[1]; M.at<double>(22,76)=b[19]; M.at<double>(22,78)=b[10]; M.at<double>(22,79)=b[4]; M.at<double>(22,82)=b[5]; M.at<double>(22,84)=b[17]; M.at<double>(22,87)=c[5]; M.at<double>(22,88)=c[17]; M.at<double>(22,90)=c[9]; M.at<double>(22,92)=c[10]; M.at<double>(22,95)=c[1]; M.at<double>(22,96)=c[4]; M.at<double>(22,99)=c[2]; M.at<double>(22,100)=c[13]; M.at<double>(22,116)=c[11]; M.at<double>(22,117)=c[3]; M.at<double>(22,119)=c[19];
+    M.at<double>(23,14)=u[2]; M.at<double>(23,20)=u[3]; M.at<double>(23,22)=u[4]; M.at<double>(23,23)=u[1]; M.at<double>(23,27)=a[11]; M.at<double>(23,28)=a[3]; M.at<double>(23,32)=a[5]; M.at<double>(23,33)=a[17]; M.at<double>(23,38)=a[14]; M.at<double>(23,39)=a[8]; M.at<double>(23,40)=a[10]; M.at<double>(23,41)=a[13]; M.at<double>(23,42)=a[9]; M.at<double>(23,43)=a[19]; M.at<double>(23,47)=a[4]; M.at<double>(23,51)=a[2]; M.at<double>(23,53)=a[12]; M.at<double>(23,55)=b[11]; M.at<double>(23,56)=b[9]; M.at<double>(23,57)=b[3]; M.at<double>(23,59)=b[17]; M.at<double>(23,60)=b[4]; M.at<double>(23,61)=b[5]; M.at<double>(23,62)=b[13]; M.at<double>(23,65)=b[2]; M.at<double>(23,71)=b[19]; M.at<double>(23,72)=b[10]; M.at<double>(23,78)=b[14]; M.at<double>(23,79)=b[8]; M.at<double>(23,82)=b[12]; M.at<double>(23,87)=c[12]; M.at<double>(23,89)=c[2]; M.at<double>(23,90)=c[4]; M.at<double>(23,92)=c[14]; M.at<double>(23,95)=c[10]; M.at<double>(23,96)=c[8]; M.at<double>(23,100)=c[19]; M.at<double>(23,106)=c[9]; M.at<double>(23,111)=c[13]; M.at<double>(23,113)=c[11]; M.at<double>(23,114)=c[3]; M.at<double>(23,116)=c[5]; M.at<double>(23,117)=c[17];
+    M.at<double>(24,15)=u[2]; M.at<double>(24,21)=u[3]; M.at<double>(24,24)=u[1]; M.at<double>(24,29)=a[9]; M.at<double>(24,30)=a[13]; M.at<double>(24,36)=a[18]; M.at<double>(24,38)=a[11]; M.at<double>(24,39)=a[3]; M.at<double>(24,49)=a[2]; M.at<double>(24,52)=a[1]; M.at<double>(24,53)=a[15]; M.at<double>(24,73)=b[1]; M.at<double>(24,74)=b[9]; M.at<double>(24,75)=b[13]; M.at<double>(24,76)=b[2]; M.at<double>(24,78)=b[11]; M.at<double>(24,79)=b[3]; M.at<double>(24,82)=b[15]; M.at<double>(24,84)=b[18]; M.at<double>(24,87)=c[15]; M.at<double>(24,88)=c[18]; M.at<double>(24,92)=c[11]; M.at<double>(24,93)=c[1]; M.at<double>(24,94)=c[9]; M.at<double>(24,96)=c[3]; M.at<double>(24,107)=c[13]; M.at<double>(24,119)=c[2];
+    M.at<double>(25,16)=u[2]; M.at<double>(25,22)=u[3]; M.at<double>(25,24)=u[4]; M.at<double>(25,25)=u[1]; M.at<double>(25,29)=a[4]; M.at<double>(25,30)=a[19]; M.at<double>(25,32)=a[15]; M.at<double>(25,33)=a[18]; M.at<double>(25,38)=a[5]; M.at<double>(25,39)=a[17]; M.at<double>(25,40)=a[11]; M.at<double>(25,43)=a[2]; M.at<double>(25,44)=a[9]; M.at<double>(25,45)=a[13]; M.at<double>(25,47)=a[3]; M.at<double>(25,52)=a[10]; M.at<double>(25,53)=a[6]; M.at<double>(25,59)=b[18]; M.at<double>(25,60)=b[3]; M.at<double>(25,61)=b[15]; M.at<double>(25,63)=b[13]; M.at<double>(25,68)=b[9]; M.at<double>(25,71)=b[2]; M.at<double>(25,72)=b[11]; M.at<double>(25,73)=b[10]; M.at<double>(25,74)=b[4]; M.at<double>(25,75)=b[19]; M.at<double>(25,78)=b[5]; M.at<double>(25,79)=b[17]; M.at<double>(25,82)=b[6]; M.at<double>(25,87)=c[6]; M.at<double>(25,90)=c[3]; M.at<double>(25,92)=c[5]; M.at<double>(25,93)=c[10]; M.at<double>(25,94)=c[4]; M.at<double>(25,95)=c[11]; M.at<double>(25,96)=c[17]; M.at<double>(25,97)=c[9]; M.at<double>(25,100)=c[2]; M.at<double>(25,107)=c[19]; M.at<double>(25,112)=c[13]; M.at<double>(25,116)=c[15]; M.at<double>(25,117)=c[18];
+    M.at<double>(26,17)=u[2]; M.at<double>(26,23)=u[3]; M.at<double>(26,25)=u[4]; M.at<double>(26,26)=u[1]; M.at<double>(26,27)=a[15]; M.at<double>(26,28)=a[18]; M.at<double>(26,29)=a[8]; M.at<double>(26,32)=a[6]; M.at<double>(26,38)=a[12]; M.at<double>(26,40)=a[5]; M.at<double>(26,41)=a[2]; M.at<double>(26,42)=a[3]; M.at<double>(26,44)=a[4]; M.at<double>(26,45)=a[19]; M.at<double>(26,46)=a[13]; M.at<double>(26,47)=a[17]; M.at<double>(26,52)=a[14]; M.at<double>(26,55)=b[15]; M.at<double>(26,56)=b[3]; M.at<double>(26,57)=b[18]; M.at<double>(26,60)=b[17]; M.at<double>(26,61)=b[6]; M.at<double>(26,62)=b[2]; M.at<double>(26,63)=b[19]; M.at<double>(26,66)=b[13]; M.at<double>(26,68)=b[4]; M.at<double>(26,72)=b[5]; M.at<double>(26,73)=b[14]; M.at<double>(26,74)=b[8]; M.at<double>(26,78)=b[12]; M.at<double>(26,90)=c[17]; M.at<double>(26,92)=c[12]; M.at<double>(26,93)=c[14]; M.at<double>(26,94)=c[8]; M.at<double>(26,95)=c[5]; M.at<double>(26,97)=c[4]; M.at<double>(26,102)=c[13]; M.at<double>(26,106)=c[3]; M.at<double>(26,111)=c[2]; M.at<double>(26,112)=c[19]; M.at<double>(26,113)=c[15]; M.at<double>(26,114)=c[18]; M.at<double>(26,116)=c[6];
+    M.at<double>(27,15)=u[3]; M.at<double>(27,29)=a[11]; M.at<double>(27,30)=a[3]; M.at<double>(27,36)=a[7]; M.at<double>(27,39)=a[15]; M.at<double>(27,49)=a[18]; M.at<double>(27,69)=b[9]; M.at<double>(27,70)=b[1]; M.at<double>(27,74)=b[11]; M.at<double>(27,75)=b[3]; M.at<double>(27,76)=b[18]; M.at<double>(27,79)=b[15]; M.at<double>(27,84)=b[7]; M.at<double>(27,88)=c[7]; M.at<double>(27,91)=c[1]; M.at<double>(27,94)=c[11]; M.at<double>(27,96)=c[15]; M.at<double>(27,107)=c[3]; M.at<double>(27,110)=c[9]; M.at<double>(27,119)=c[18];
+    M.at<double>(28,6)=u[3]; M.at<double>(28,30)=a[11]; M.at<double>(28,35)=a[7]; M.at<double>(28,49)=a[15]; M.at<double>(28,69)=b[1]; M.at<double>(28,75)=b[11]; M.at<double>(28,76)=b[15]; M.at<double>(28,83)=b[7]; M.at<double>(28,107)=c[11]; M.at<double>(28,110)=c[1]; M.at<double>(28,118)=c[7]; M.at<double>(28,119)=c[15];
+    M.at<double>(29,24)=u[3]; M.at<double>(29,29)=a[3]; M.at<double>(29,30)=a[2]; M.at<double>(29,38)=a[15]; M.at<double>(29,39)=a[18]; M.at<double>(29,52)=a[11]; M.at<double>(29,53)=a[7]; M.at<double>(29,69)=b[13]; M.at<double>(29,70)=b[9]; M.at<double>(29,73)=b[11]; M.at<double>(29,74)=b[3]; M.at<double>(29,75)=b[2]; M.at<double>(29,78)=b[15]; M.at<double>(29,79)=b[18]; M.at<double>(29,82)=b[7]; M.at<double>(29,87)=c[7]; M.at<double>(29,91)=c[9]; M.at<double>(29,92)=c[15]; M.at<double>(29,93)=c[11]; M.at<double>(29,94)=c[3]; M.at<double>(29,96)=c[18]; M.at<double>(29,107)=c[2]; M.at<double>(29,110)=c[13];
+    M.at<double>(30,37)=a[18]; M.at<double>(30,48)=a[7]; M.at<double>(30,52)=a[2]; M.at<double>(30,70)=b[20]; M.at<double>(30,73)=b[2]; M.at<double>(30,77)=b[18]; M.at<double>(30,81)=b[7]; M.at<double>(30,85)=c[7]; M.at<double>(30,91)=c[20]; M.at<double>(30,93)=c[2]; M.at<double>(30,98)=c[18];
+    M.at<double>(31,29)=a[2]; M.at<double>(31,37)=a[15]; M.at<double>(31,38)=a[18]; M.at<double>(31,50)=a[7]; M.at<double>(31,52)=a[3]; M.at<double>(31,69)=b[20]; M.at<double>(31,70)=b[13]; M.at<double>(31,73)=b[3]; M.at<double>(31,74)=b[2]; M.at<double>(31,77)=b[15]; M.at<double>(31,78)=b[18]; M.at<double>(31,80)=b[7]; M.at<double>(31,86)=c[7]; M.at<double>(31,91)=c[13]; M.at<double>(31,92)=c[18]; M.at<double>(31,93)=c[3]; M.at<double>(31,94)=c[2]; M.at<double>(31,98)=c[15]; M.at<double>(31,110)=c[20];
+    M.at<double>(32,48)=a[9]; M.at<double>(32,50)=a[13]; M.at<double>(32,53)=a[20]; M.at<double>(32,80)=b[13]; M.at<double>(32,81)=b[9]; M.at<double>(32,82)=b[20]; M.at<double>(32,85)=c[9]; M.at<double>(32,86)=c[13]; M.at<double>(32,87)=c[20];
+    M.at<double>(33,29)=a[15]; M.at<double>(33,30)=a[18]; M.at<double>(33,39)=a[7]; M.at<double>(33,64)=b[9]; M.at<double>(33,69)=b[3]; M.at<double>(33,70)=b[11]; M.at<double>(33,74)=b[15]; M.at<double>(33,75)=b[18]; M.at<double>(33,79)=b[7]; M.at<double>(33,91)=c[11]; M.at<double>(33,94)=c[15]; M.at<double>(33,96)=c[7]; M.at<double>(33,103)=c[9]; M.at<double>(33,107)=c[18]; M.at<double>(33,110)=c[3];
+    M.at<double>(34,29)=a[18]; M.at<double>(34,38)=a[7]; M.at<double>(34,52)=a[15]; M.at<double>(34,64)=b[13]; M.at<double>(34,69)=b[2]; M.at<double>(34,70)=b[3]; M.at<double>(34,73)=b[15]; M.at<double>(34,74)=b[18]; M.at<double>(34,78)=b[7]; M.at<double>(34,91)=c[3]; M.at<double>(34,92)=c[7]; M.at<double>(34,93)=c[15]; M.at<double>(34,94)=c[18]; M.at<double>(34,103)=c[13]; M.at<double>(34,110)=c[2];
+    M.at<double>(35,37)=a[7]; M.at<double>(35,52)=a[18]; M.at<double>(35,64)=b[20]; M.at<double>(35,70)=b[2]; M.at<double>(35,73)=b[18]; M.at<double>(35,77)=b[7]; M.at<double>(35,91)=c[2]; M.at<double>(35,93)=c[18]; M.at<double>(35,98)=c[7]; M.at<double>(35,103)=c[20];
+    M.at<double>(36,5)=u[4]; M.at<double>(36,34)=a[12]; M.at<double>(36,41)=a[10]; M.at<double>(36,43)=a[14]; M.at<double>(36,49)=a[16]; M.at<double>(36,51)=a[5]; M.at<double>(36,54)=b[12]; M.at<double>(36,62)=b[10]; M.at<double>(36,65)=b[5]; M.at<double>(36,71)=b[14]; M.at<double>(36,76)=b[16]; M.at<double>(36,89)=c[5]; M.at<double>(36,99)=c[12]; M.at<double>(36,100)=c[14]; M.at<double>(36,101)=c[1]; M.at<double>(36,109)=c[11]; M.at<double>(36,111)=c[10]; M.at<double>(36,119)=c[16];
+    M.at<double>(37,2)=u[4]; M.at<double>(37,34)=a[14]; M.at<double>(37,35)=a[16]; M.at<double>(37,51)=a[10]; M.at<double>(37,54)=b[14]; M.at<double>(37,65)=b[10]; M.at<double>(37,83)=b[16]; M.at<double>(37,89)=c[10]; M.at<double>(37,99)=c[14]; M.at<double>(37,109)=c[1]; M.at<double>(37,118)=c[16];
+    M.at<double>(38,30)=a[15]; M.at<double>(38,49)=a[7]; M.at<double>(38,64)=b[1]; M.at<double>(38,69)=b[11]; M.at<double>(38,75)=b[15]; M.at<double>(38,76)=b[7]; M.at<double>(38,103)=c[1]; M.at<double>(38,107)=c[15]; M.at<double>(38,110)=c[11]; M.at<double>(38,119)=c[7];
+    M.at<double>(39,28)=a[14]; M.at<double>(39,33)=a[16]; M.at<double>(39,51)=a[8]; M.at<double>(39,57)=b[14]; M.at<double>(39,59)=b[16]; M.at<double>(39,65)=b[8]; M.at<double>(39,89)=c[8]; M.at<double>(39,105)=c[9]; M.at<double>(39,108)=c[10]; M.at<double>(39,109)=c[4]; M.at<double>(39,114)=c[14]; M.at<double>(39,117)=c[16];
+    M.at<double>(40,27)=a[14]; M.at<double>(40,28)=a[8]; M.at<double>(40,32)=a[16]; M.at<double>(40,55)=b[14]; M.at<double>(40,57)=b[8]; M.at<double>(40,61)=b[16]; M.at<double>(40,105)=c[13]; M.at<double>(40,108)=c[4]; M.at<double>(40,109)=c[19]; M.at<double>(40,113)=c[14]; M.at<double>(40,114)=c[8]; M.at<double>(40,116)=c[16];
+    M.at<double>(41,30)=a[7]; M.at<double>(41,64)=b[11]; M.at<double>(41,69)=b[15]; M.at<double>(41,75)=b[7]; M.at<double>(41,103)=c[11]; M.at<double>(41,107)=c[7]; M.at<double>(41,110)=c[15];
+    M.at<double>(42,27)=a[8]; M.at<double>(42,31)=a[16]; M.at<double>(42,55)=b[8]; M.at<double>(42,58)=b[16]; M.at<double>(42,105)=c[20]; M.at<double>(42,108)=c[19]; M.at<double>(42,113)=c[8]; M.at<double>(42,115)=c[16];
+    M.at<double>(43,29)=a[7]; M.at<double>(43,64)=b[3]; M.at<double>(43,69)=b[18]; M.at<double>(43,70)=b[15]; M.at<double>(43,74)=b[7]; M.at<double>(43,91)=c[15]; M.at<double>(43,94)=c[7]; M.at<double>(43,103)=c[3]; M.at<double>(43,110)=c[18];
+    M.at<double>(44,28)=a[16]; M.at<double>(44,57)=b[16]; M.at<double>(44,105)=c[4]; M.at<double>(44,108)=c[14]; M.at<double>(44,109)=c[8]; M.at<double>(44,114)=c[16];
+    M.at<double>(45,27)=a[16]; M.at<double>(45,55)=b[16]; M.at<double>(45,105)=c[19]; M.at<double>(45,108)=c[8]; M.at<double>(45,113)=c[16];
+    M.at<double>(46,52)=a[7]; M.at<double>(46,64)=b[2]; M.at<double>(46,70)=b[18]; M.at<double>(46,73)=b[7]; M.at<double>(46,91)=c[18]; M.at<double>(46,93)=c[7]; M.at<double>(46,103)=c[2];
+    M.at<double>(47,40)=a[7]; M.at<double>(47,44)=a[18]; M.at<double>(47,52)=a[6]; M.at<double>(47,64)=b[19]; M.at<double>(47,67)=b[2]; M.at<double>(47,68)=b[18]; M.at<double>(47,70)=b[17]; M.at<double>(47,72)=b[7]; M.at<double>(47,73)=b[6]; M.at<double>(47,91)=c[17]; M.at<double>(47,93)=c[6]; M.at<double>(47,95)=c[7]; M.at<double>(47,97)=c[18]; M.at<double>(47,103)=c[19]; M.at<double>(47,104)=c[2];
+    M.at<double>(48,30)=a[6]; M.at<double>(48,43)=a[7]; M.at<double>(48,45)=a[15]; M.at<double>(48,63)=b[15]; M.at<double>(48,64)=b[10]; M.at<double>(48,67)=b[11]; M.at<double>(48,69)=b[5]; M.at<double>(48,71)=b[7]; M.at<double>(48,75)=b[6]; M.at<double>(48,100)=c[7]; M.at<double>(48,103)=c[10]; M.at<double>(48,104)=c[11]; M.at<double>(48,107)=c[6]; M.at<double>(48,110)=c[5]; M.at<double>(48,112)=c[15];
+    M.at<double>(49,41)=a[12]; M.at<double>(49,45)=a[16]; M.at<double>(49,46)=a[14]; M.at<double>(49,62)=b[12]; M.at<double>(49,63)=b[16]; M.at<double>(49,66)=b[14]; M.at<double>(49,101)=c[5]; M.at<double>(49,102)=c[14]; M.at<double>(49,105)=c[15]; M.at<double>(49,109)=c[6]; M.at<double>(49,111)=c[12]; M.at<double>(49,112)=c[16];
+    M.at<double>(50,41)=a[16]; M.at<double>(50,62)=b[16]; M.at<double>(50,101)=c[14]; M.at<double>(50,105)=c[5]; M.at<double>(50,109)=c[12]; M.at<double>(50,111)=c[16];
+    M.at<double>(51,64)=b[18]; M.at<double>(51,70)=b[7]; M.at<double>(51,91)=c[7]; M.at<double>(51,103)=c[18];
+    M.at<double>(52,41)=a[6]; M.at<double>(52,45)=a[12]; M.at<double>(52,46)=a[5]; M.at<double>(52,62)=b[6]; M.at<double>(52,63)=b[12]; M.at<double>(52,66)=b[5]; M.at<double>(52,67)=b[14]; M.at<double>(52,69)=b[16]; M.at<double>(52,101)=c[15]; M.at<double>(52,102)=c[5]; M.at<double>(52,104)=c[14]; M.at<double>(52,109)=c[7]; M.at<double>(52,110)=c[16]; M.at<double>(52,111)=c[6]; M.at<double>(52,112)=c[12];
+    M.at<double>(53,64)=b[15]; M.at<double>(53,69)=b[7]; M.at<double>(53,103)=c[15]; M.at<double>(53,110)=c[7];
+    M.at<double>(54,105)=c[14]; M.at<double>(54,109)=c[16];
+    M.at<double>(55,44)=a[7]; M.at<double>(55,64)=b[17]; M.at<double>(55,67)=b[18]; M.at<double>(55,68)=b[7]; M.at<double>(55,70)=b[6]; M.at<double>(55,91)=c[6]; M.at<double>(55,97)=c[7]; M.at<double>(55,103)=c[17]; M.at<double>(55,104)=c[18];
+    M.at<double>(56,105)=c[8]; M.at<double>(56,108)=c[16];
+    M.at<double>(57,64)=b[6]; M.at<double>(57,67)=b[7]; M.at<double>(57,103)=c[6]; M.at<double>(57,104)=c[7];
+    M.at<double>(58,46)=a[7]; M.at<double>(58,64)=b[12]; M.at<double>(58,66)=b[7]; M.at<double>(58,67)=b[6]; M.at<double>(58,102)=c[7]; M.at<double>(58,103)=c[12]; M.at<double>(58,104)=c[6];
+    M.at<double>(59,8)=u[4]; M.at<double>(59,30)=a[16]; M.at<double>(59,41)=a[5]; M.at<double>(59,43)=a[12]; M.at<double>(59,45)=a[14]; M.at<double>(59,46)=a[10]; M.at<double>(59,51)=a[6]; M.at<double>(59,62)=b[5]; M.at<double>(59,63)=b[14]; M.at<double>(59,65)=b[6]; M.at<double>(59,66)=b[10]; M.at<double>(59,71)=b[12]; M.at<double>(59,75)=b[16]; M.at<double>(59,89)=c[6]; M.at<double>(59,100)=c[12]; M.at<double>(59,101)=c[11]; M.at<double>(59,102)=c[10]; M.at<double>(59,107)=c[16]; M.at<double>(59,109)=c[15]; M.at<double>(59,111)=c[5]; M.at<double>(59,112)=c[14];
+    M.at<double>(60,8)=u[3]; M.at<double>(60,30)=a[12]; M.at<double>(60,41)=a[15]; M.at<double>(60,43)=a[6]; M.at<double>(60,45)=a[5]; M.at<double>(60,46)=a[11]; M.at<double>(60,51)=a[7]; M.at<double>(60,62)=b[15]; M.at<double>(60,63)=b[5]; M.at<double>(60,65)=b[7]; M.at<double>(60,66)=b[11]; M.at<double>(60,67)=b[10]; M.at<double>(60,69)=b[14]; M.at<double>(60,71)=b[6]; M.at<double>(60,75)=b[12]; M.at<double>(60,89)=c[7]; M.at<double>(60,100)=c[6]; M.at<double>(60,102)=c[11]; M.at<double>(60,104)=c[10]; M.at<double>(60,107)=c[12]; M.at<double>(60,110)=c[14]; M.at<double>(60,111)=c[15]; M.at<double>(60,112)=c[5];
+    M.at<double>(61,42)=a[16]; M.at<double>(61,56)=b[16]; M.at<double>(61,101)=c[8]; M.at<double>(61,105)=c[17]; M.at<double>(61,106)=c[16]; M.at<double>(61,108)=c[12];
+    M.at<double>(62,64)=b[7]; M.at<double>(62,103)=c[7];
+    M.at<double>(63,105)=c[16];
+    M.at<double>(64,46)=a[12]; M.at<double>(64,66)=b[12]; M.at<double>(64,67)=b[16]; M.at<double>(64,101)=c[6]; M.at<double>(64,102)=c[12]; M.at<double>(64,104)=c[16]; M.at<double>(64,105)=c[7];
+    M.at<double>(65,46)=a[6]; M.at<double>(65,64)=b[16]; M.at<double>(65,66)=b[6]; M.at<double>(65,67)=b[12]; M.at<double>(65,101)=c[7]; M.at<double>(65,102)=c[6]; M.at<double>(65,103)=c[16]; M.at<double>(65,104)=c[12];
+    M.at<double>(66,46)=a[16]; M.at<double>(66,66)=b[16]; M.at<double>(66,101)=c[12]; M.at<double>(66,102)=c[16]; M.at<double>(66,105)=c[6];
+    M.at<double>(67,101)=c[16]; M.at<double>(67,105)=c[12];
+    M.at<double>(68,41)=a[14]; M.at<double>(68,43)=a[16]; M.at<double>(68,51)=a[12]; M.at<double>(68,62)=b[14]; M.at<double>(68,65)=b[12]; M.at<double>(68,71)=b[16]; M.at<double>(68,89)=c[12]; M.at<double>(68,100)=c[16]; M.at<double>(68,101)=c[10]; M.at<double>(68,105)=c[11]; M.at<double>(68,109)=c[5]; M.at<double>(68,111)=c[14];
+    M.at<double>(69,37)=a[2]; M.at<double>(69,48)=a[18]; M.at<double>(69,52)=a[20]; M.at<double>(69,73)=b[20]; M.at<double>(69,77)=b[2]; M.at<double>(69,81)=b[18]; M.at<double>(69,85)=c[18]; M.at<double>(69,93)=c[20]; M.at<double>(69,98)=c[2];
+    M.at<double>(70,20)=u[2]; M.at<double>(70,27)=a[9]; M.at<double>(70,28)=a[13]; M.at<double>(70,31)=a[10]; M.at<double>(70,32)=a[4]; M.at<double>(70,33)=a[19]; M.at<double>(70,50)=a[14]; M.at<double>(70,51)=a[20]; M.at<double>(70,53)=a[8]; M.at<double>(70,55)=b[9]; M.at<double>(70,57)=b[13]; M.at<double>(70,58)=b[10]; M.at<double>(70,59)=b[19]; M.at<double>(70,61)=b[4]; M.at<double>(70,65)=b[20]; M.at<double>(70,80)=b[14]; M.at<double>(70,82)=b[8]; M.at<double>(70,86)=c[14]; M.at<double>(70,87)=c[8]; M.at<double>(70,89)=c[20]; M.at<double>(70,113)=c[9]; M.at<double>(70,114)=c[13]; M.at<double>(70,115)=c[10]; M.at<double>(70,116)=c[4]; M.at<double>(70,117)=c[19];
+    M.at<double>(71,45)=a[7]; M.at<double>(71,63)=b[7]; M.at<double>(71,64)=b[5]; M.at<double>(71,67)=b[15]; M.at<double>(71,69)=b[6]; M.at<double>(71,103)=c[5]; M.at<double>(71,104)=c[15]; M.at<double>(71,110)=c[6]; M.at<double>(71,112)=c[7];
+    M.at<double>(72,41)=a[7]; M.at<double>(72,45)=a[6]; M.at<double>(72,46)=a[15]; M.at<double>(72,62)=b[7]; M.at<double>(72,63)=b[6]; M.at<double>(72,64)=b[14]; M.at<double>(72,66)=b[15]; M.at<double>(72,67)=b[5]; M.at<double>(72,69)=b[12]; M.at<double>(72,102)=c[15]; M.at<double>(72,103)=c[14]; M.at<double>(72,104)=c[5]; M.at<double>(72,110)=c[12]; M.at<double>(72,111)=c[7]; M.at<double>(72,112)=c[6];
+    M.at<double>(73,48)=a[13]; M.at<double>(73,50)=a[20]; M.at<double>(73,80)=b[20]; M.at<double>(73,81)=b[13]; M.at<double>(73,85)=c[13]; M.at<double>(73,86)=c[20];
+    M.at<double>(74,25)=u[3]; M.at<double>(74,29)=a[17]; M.at<double>(74,32)=a[7]; M.at<double>(74,38)=a[6]; M.at<double>(74,40)=a[15]; M.at<double>(74,44)=a[3]; M.at<double>(74,45)=a[2]; M.at<double>(74,47)=a[18]; M.at<double>(74,52)=a[5]; M.at<double>(74,60)=b[18]; M.at<double>(74,61)=b[7]; M.at<double>(74,63)=b[2]; M.at<double>(74,67)=b[13]; M.at<double>(74,68)=b[3]; M.at<double>(74,69)=b[19]; M.at<double>(74,70)=b[4]; M.at<double>(74,72)=b[15]; M.at<double>(74,73)=b[5]; M.at<double>(74,74)=b[17]; M.at<double>(74,78)=b[6]; M.at<double>(74,90)=c[18]; M.at<double>(74,91)=c[4]; M.at<double>(74,92)=c[6]; M.at<double>(74,93)=c[5]; M.at<double>(74,94)=c[17]; M.at<double>(74,95)=c[15]; M.at<double>(74,97)=c[3]; M.at<double>(74,104)=c[13]; M.at<double>(74,110)=c[19]; M.at<double>(74,112)=c[2]; M.at<double>(74,116)=c[7];
+    M.at<double>(75,21)=u[2]; M.at<double>(75,36)=a[2]; M.at<double>(75,37)=a[1]; M.at<double>(75,38)=a[9]; M.at<double>(75,39)=a[13]; M.at<double>(75,49)=a[20]; M.at<double>(75,50)=a[11]; M.at<double>(75,53)=a[3]; M.at<double>(75,76)=b[20]; M.at<double>(75,77)=b[1]; M.at<double>(75,78)=b[9]; M.at<double>(75,79)=b[13]; M.at<double>(75,80)=b[11]; M.at<double>(75,82)=b[3]; M.at<double>(75,84)=b[2]; M.at<double>(75,86)=c[11]; M.at<double>(75,87)=c[3]; M.at<double>(75,88)=c[2]; M.at<double>(75,92)=c[9]; M.at<double>(75,96)=c[13]; M.at<double>(75,98)=c[1]; M.at<double>(75,119)=c[20];
+    M.at<double>(76,48)=a[20]; M.at<double>(76,81)=b[20]; M.at<double>(76,85)=c[20];
+    M.at<double>(77,34)=a[16]; M.at<double>(77,51)=a[14]; M.at<double>(77,54)=b[16]; M.at<double>(77,65)=b[14]; M.at<double>(77,89)=c[14]; M.at<double>(77,99)=c[16]; M.at<double>(77,105)=c[1]; M.at<double>(77,109)=c[10];
+    M.at<double>(78,27)=a[17]; M.at<double>(78,31)=a[12]; M.at<double>(78,37)=a[16]; M.at<double>(78,40)=a[8]; M.at<double>(78,42)=a[19]; M.at<double>(78,55)=b[17]; M.at<double>(78,56)=b[19]; M.at<double>(78,58)=b[12]; M.at<double>(78,72)=b[8]; M.at<double>(78,77)=b[16]; M.at<double>(78,95)=c[8]; M.at<double>(78,98)=c[16]; M.at<double>(78,101)=c[20]; M.at<double>(78,106)=c[19]; M.at<double>(78,108)=c[2]; M.at<double>(78,113)=c[17]; M.at<double>(78,115)=c[12];
+    M.at<double>(79,42)=a[12]; M.at<double>(79,44)=a[16]; M.at<double>(79,46)=a[8]; M.at<double>(79,56)=b[12]; M.at<double>(79,66)=b[8]; M.at<double>(79,68)=b[16]; M.at<double>(79,97)=c[16]; M.at<double>(79,101)=c[17]; M.at<double>(79,102)=c[8]; M.at<double>(79,105)=c[18]; M.at<double>(79,106)=c[12]; M.at<double>(79,108)=c[6];
+    M.at<double>(80,14)=u[4]; M.at<double>(80,28)=a[5]; M.at<double>(80,33)=a[12]; M.at<double>(80,39)=a[16]; M.at<double>(80,41)=a[4]; M.at<double>(80,42)=a[10]; M.at<double>(80,43)=a[8]; M.at<double>(80,47)=a[14]; M.at<double>(80,51)=a[17]; M.at<double>(80,56)=b[10]; M.at<double>(80,57)=b[5]; M.at<double>(80,59)=b[12]; M.at<double>(80,60)=b[14]; M.at<double>(80,62)=b[4]; M.at<double>(80,65)=b[17]; M.at<double>(80,71)=b[8]; M.at<double>(80,79)=b[16]; M.at<double>(80,89)=c[17]; M.at<double>(80,90)=c[14]; M.at<double>(80,96)=c[16]; M.at<double>(80,100)=c[8]; M.at<double>(80,101)=c[9]; M.at<double>(80,106)=c[10]; M.at<double>(80,108)=c[11]; M.at<double>(80,109)=c[3]; M.at<double>(80,111)=c[4]; M.at<double>(80,114)=c[5]; M.at<double>(80,117)=c[12];
+    M.at<double>(81,31)=a[3]; M.at<double>(81,32)=a[2]; M.at<double>(81,37)=a[4]; M.at<double>(81,38)=a[19]; M.at<double>(81,40)=a[13]; M.at<double>(81,47)=a[20]; M.at<double>(81,48)=a[5]; M.at<double>(81,50)=a[17]; M.at<double>(81,58)=b[3]; M.at<double>(81,60)=b[20]; M.at<double>(81,61)=b[2]; M.at<double>(81,72)=b[13]; M.at<double>(81,77)=b[4]; M.at<double>(81,78)=b[19]; M.at<double>(81,80)=b[17]; M.at<double>(81,81)=b[5]; M.at<double>(81,85)=c[5]; M.at<double>(81,86)=c[17]; M.at<double>(81,90)=c[20]; M.at<double>(81,92)=c[19]; M.at<double>(81,95)=c[13]; M.at<double>(81,98)=c[4]; M.at<double>(81,115)=c[3]; M.at<double>(81,116)=c[2];
+    M.at<double>(82,29)=a[6]; M.at<double>(82,44)=a[15]; M.at<double>(82,45)=a[18]; M.at<double>(82,47)=a[7]; M.at<double>(82,60)=b[7]; M.at<double>(82,63)=b[18]; M.at<double>(82,64)=b[4]; M.at<double>(82,67)=b[3]; M.at<double>(82,68)=b[15]; M.at<double>(82,69)=b[17]; M.at<double>(82,70)=b[5]; M.at<double>(82,74)=b[6]; M.at<double>(82,90)=c[7]; M.at<double>(82,91)=c[5]; M.at<double>(82,94)=c[6]; M.at<double>(82,97)=c[15]; M.at<double>(82,103)=c[4]; M.at<double>(82,104)=c[3]; M.at<double>(82,110)=c[17]; M.at<double>(82,112)=c[18];
+    M.at<double>(83,26)=u[2]; M.at<double>(83,27)=a[18]; M.at<double>(83,31)=a[6]; M.at<double>(83,37)=a[12]; M.at<double>(83,40)=a[17]; M.at<double>(83,42)=a[2]; M.at<double>(83,44)=a[19]; M.at<double>(83,46)=a[20]; M.at<double>(83,52)=a[8]; M.at<double>(83,55)=b[18]; M.at<double>(83,56)=b[2]; M.at<double>(83,58)=b[6]; M.at<double>(83,66)=b[20]; M.at<double>(83,68)=b[19]; M.at<double>(83,72)=b[17]; M.at<double>(83,73)=b[8]; M.at<double>(83,77)=b[12]; M.at<double>(83,93)=c[8]; M.at<double>(83,95)=c[17]; M.at<double>(83,97)=c[19]; M.at<double>(83,98)=c[12]; M.at<double>(83,102)=c[20]; M.at<double>(83,106)=c[2]; M.at<double>(83,113)=c[18]; M.at<double>(83,115)=c[6];
+    M.at<double>(84,16)=u[3]; M.at<double>(84,29)=a[5]; M.at<double>(84,30)=a[17]; M.at<double>(84,33)=a[7]; M.at<double>(84,39)=a[6]; M.at<double>(84,43)=a[18]; M.at<double>(84,44)=a[11]; M.at<double>(84,45)=a[3]; M.at<double>(84,47)=a[15]; M.at<double>(84,59)=b[7]; M.at<double>(84,60)=b[15]; M.at<double>(84,63)=b[3]; M.at<double>(84,67)=b[9]; M.at<double>(84,68)=b[11]; M.at<double>(84,69)=b[4]; M.at<double>(84,70)=b[10]; M.at<double>(84,71)=b[18]; M.at<double>(84,74)=b[5]; M.at<double>(84,75)=b[17]; M.at<double>(84,79)=b[6]; M.at<double>(84,90)=c[15]; M.at<double>(84,91)=c[10]; M.at<double>(84,94)=c[5]; M.at<double>(84,96)=c[6]; M.at<double>(84,97)=c[11]; M.at<double>(84,100)=c[18]; M.at<double>(84,104)=c[9]; M.at<double>(84,107)=c[17]; M.at<double>(84,110)=c[4]; M.at<double>(84,112)=c[3]; M.at<double>(84,117)=c[7];
+    M.at<double>(85,25)=u[2]; M.at<double>(85,29)=a[19]; M.at<double>(85,31)=a[15]; M.at<double>(85,32)=a[18]; M.at<double>(85,37)=a[5]; M.at<double>(85,38)=a[17]; M.at<double>(85,40)=a[3]; M.at<double>(85,44)=a[13]; M.at<double>(85,45)=a[20]; M.at<double>(85,47)=a[2]; M.at<double>(85,50)=a[6]; M.at<double>(85,52)=a[4]; M.at<double>(85,58)=b[15]; M.at<double>(85,60)=b[2]; M.at<double>(85,61)=b[18]; M.at<double>(85,63)=b[20]; M.at<double>(85,68)=b[13]; M.at<double>(85,72)=b[3]; M.at<double>(85,73)=b[4]; M.at<double>(85,74)=b[19]; M.at<double>(85,77)=b[5]; M.at<double>(85,78)=b[17]; M.at<double>(85,80)=b[6]; M.at<double>(85,86)=c[6]; M.at<double>(85,90)=c[2]; M.at<double>(85,92)=c[17]; M.at<double>(85,93)=c[4]; M.at<double>(85,94)=c[19]; M.at<double>(85,95)=c[3]; M.at<double>(85,97)=c[13]; M.at<double>(85,98)=c[5]; M.at<double>(85,112)=c[20]; M.at<double>(85,115)=c[15]; M.at<double>(85,116)=c[18];
+    M.at<double>(86,31)=a[18]; M.at<double>(86,37)=a[17]; M.at<double>(86,40)=a[2]; M.at<double>(86,44)=a[20]; M.at<double>(86,48)=a[6]; M.at<double>(86,52)=a[19]; M.at<double>(86,58)=b[18]; M.at<double>(86,68)=b[20]; M.at<double>(86,72)=b[2]; M.at<double>(86,73)=b[19]; M.at<double>(86,77)=b[17]; M.at<double>(86,81)=b[6]; M.at<double>(86,85)=c[6]; M.at<double>(86,93)=c[19]; M.at<double>(86,95)=c[2]; M.at<double>(86,97)=c[20]; M.at<double>(86,98)=c[17]; M.at<double>(86,115)=c[18];
+    M.at<double>(87,22)=u[2]; M.at<double>(87,31)=a[11]; M.at<double>(87,32)=a[3]; M.at<double>(87,33)=a[2]; M.at<double>(87,37)=a[10]; M.at<double>(87,38)=a[4]; M.at<double>(87,39)=a[19]; M.at<double>(87,40)=a[9]; M.at<double>(87,43)=a[20]; M.at<double>(87,47)=a[13]; M.at<double>(87,50)=a[5]; M.at<double>(87,53)=a[17]; M.at<double>(87,58)=b[11]; M.at<double>(87,59)=b[2]; M.at<double>(87,60)=b[13]; M.at<double>(87,61)=b[3]; M.at<double>(87,71)=b[20]; M.at<double>(87,72)=b[9]; M.at<double>(87,77)=b[10]; M.at<double>(87,78)=b[4]; M.at<double>(87,79)=b[19]; M.at<double>(87,80)=b[5]; M.at<double>(87,82)=b[17]; M.at<double>(87,86)=c[5]; M.at<double>(87,87)=c[17]; M.at<double>(87,90)=c[13]; M.at<double>(87,92)=c[4]; M.at<double>(87,95)=c[9]; M.at<double>(87,96)=c[19]; M.at<double>(87,98)=c[10]; M.at<double>(87,100)=c[20]; M.at<double>(87,115)=c[11]; M.at<double>(87,116)=c[3]; M.at<double>(87,117)=c[2];
+    M.at<double>(88,27)=a[2]; M.at<double>(88,31)=a[17]; M.at<double>(88,37)=a[8]; M.at<double>(88,40)=a[19]; M.at<double>(88,42)=a[20]; M.at<double>(88,48)=a[12]; M.at<double>(88,55)=b[2]; M.at<double>(88,56)=b[20]; M.at<double>(88,58)=b[17]; M.at<double>(88,72)=b[19]; M.at<double>(88,77)=b[8]; M.at<double>(88,81)=b[12]; M.at<double>(88,85)=c[12]; M.at<double>(88,95)=c[19]; M.at<double>(88,98)=c[8]; M.at<double>(88,106)=c[20]; M.at<double>(88,113)=c[2]; M.at<double>(88,115)=c[17];
+    M.at<double>(89,31)=a[7]; M.at<double>(89,37)=a[6]; M.at<double>(89,40)=a[18]; M.at<double>(89,44)=a[2]; M.at<double>(89,52)=a[17]; M.at<double>(89,58)=b[7]; M.at<double>(89,67)=b[20]; M.at<double>(89,68)=b[2]; M.at<double>(89,70)=b[19]; M.at<double>(89,72)=b[18]; M.at<double>(89,73)=b[17]; M.at<double>(89,77)=b[6]; M.at<double>(89,91)=c[19]; M.at<double>(89,93)=c[17]; M.at<double>(89,95)=c[18]; M.at<double>(89,97)=c[2]; M.at<double>(89,98)=c[6]; M.at<double>(89,104)=c[20]; M.at<double>(89,115)=c[7];
+    M.at<double>(90,27)=a[12]; M.at<double>(90,40)=a[16]; M.at<double>(90,42)=a[8]; M.at<double>(90,55)=b[12]; M.at<double>(90,56)=b[8]; M.at<double>(90,72)=b[16]; M.at<double>(90,95)=c[16]; M.at<double>(90,101)=c[19]; M.at<double>(90,105)=c[2]; M.at<double>(90,106)=c[8]; M.at<double>(90,108)=c[17]; M.at<double>(90,113)=c[12];
+    M.at<double>(91,23)=u[2]; M.at<double>(91,27)=a[3]; M.at<double>(91,28)=a[2]; M.at<double>(91,31)=a[5]; M.at<double>(91,32)=a[17]; M.at<double>(91,37)=a[14]; M.at<double>(91,38)=a[8]; M.at<double>(91,40)=a[4]; M.at<double>(91,41)=a[20]; M.at<double>(91,42)=a[13]; M.at<double>(91,47)=a[19]; M.at<double>(91,50)=a[12]; M.at<double>(91,55)=b[3]; M.at<double>(91,56)=b[13]; M.at<double>(91,57)=b[2]; M.at<double>(91,58)=b[5]; M.at<double>(91,60)=b[19]; M.at<double>(91,61)=b[17]; M.at<double>(91,62)=b[20]; M.at<double>(91,72)=b[4]; M.at<double>(91,77)=b[14]; M.at<double>(91,78)=b[8]; M.at<double>(91,80)=b[12]; M.at<double>(91,86)=c[12]; M.at<double>(91,90)=c[19]; M.at<double>(91,92)=c[8]; M.at<double>(91,95)=c[4]; M.at<double>(91,98)=c[14]; M.at<double>(91,106)=c[13]; M.at<double>(91,111)=c[20]; M.at<double>(91,113)=c[3]; M.at<double>(91,114)=c[2]; M.at<double>(91,115)=c[5]; M.at<double>(91,116)=c[17];
+    M.at<double>(92,17)=u[4]; M.at<double>(92,28)=a[6]; M.at<double>(92,29)=a[16]; M.at<double>(92,41)=a[17]; M.at<double>(92,42)=a[5]; M.at<double>(92,44)=a[14]; M.at<double>(92,45)=a[8]; M.at<double>(92,46)=a[4]; M.at<double>(92,47)=a[12]; M.at<double>(92,56)=b[5]; M.at<double>(92,57)=b[6]; M.at<double>(92,60)=b[12]; M.at<double>(92,62)=b[17]; M.at<double>(92,63)=b[8]; M.at<double>(92,66)=b[4]; M.at<double>(92,68)=b[14]; M.at<double>(92,74)=b[16]; M.at<double>(92,90)=c[12]; M.at<double>(92,94)=c[16]; M.at<double>(92,97)=c[14]; M.at<double>(92,101)=c[3]; M.at<double>(92,102)=c[4]; M.at<double>(92,106)=c[5]; M.at<double>(92,108)=c[15]; M.at<double>(92,109)=c[18]; M.at<double>(92,111)=c[17]; M.at<double>(92,112)=c[8]; M.at<double>(92,114)=c[6];
+    M.at<double>(93,17)=u[3]; M.at<double>(93,28)=a[7]; M.at<double>(93,29)=a[12]; M.at<double>(93,41)=a[18]; M.at<double>(93,42)=a[15]; M.at<double>(93,44)=a[5]; M.at<double>(93,45)=a[17]; M.at<double>(93,46)=a[3]; M.at<double>(93,47)=a[6]; M.at<double>(93,56)=b[15]; M.at<double>(93,57)=b[7]; M.at<double>(93,60)=b[6]; M.at<double>(93,62)=b[18]; M.at<double>(93,63)=b[17]; M.at<double>(93,66)=b[3]; M.at<double>(93,67)=b[4]; M.at<double>(93,68)=b[5]; M.at<double>(93,69)=b[8]; M.at<double>(93,70)=b[14]; M.at<double>(93,74)=b[12]; M.at<double>(93,90)=c[6]; M.at<double>(93,91)=c[14]; M.at<double>(93,94)=c[12]; M.at<double>(93,97)=c[5]; M.at<double>(93,102)=c[3]; M.at<double>(93,104)=c[4]; M.at<double>(93,106)=c[15]; M.at<double>(93,110)=c[8]; M.at<double>(93,111)=c[18]; M.at<double>(93,112)=c[17]; M.at<double>(93,114)=c[7];
+    M.at<double>(94,31)=a[2]; M.at<double>(94,37)=a[19]; M.at<double>(94,40)=a[20]; M.at<double>(94,48)=a[17]; M.at<double>(94,58)=b[2]; M.at<double>(94,72)=b[20]; M.at<double>(94,77)=b[19]; M.at<double>(94,81)=b[17]; M.at<double>(94,85)=c[17]; M.at<double>(94,95)=c[20]; M.at<double>(94,98)=c[19]; M.at<double>(94,115)=c[2];
+    M.at<double>(95,26)=u[4]; M.at<double>(95,27)=a[6]; M.at<double>(95,40)=a[12]; M.at<double>(95,42)=a[17]; M.at<double>(95,44)=a[8]; M.at<double>(95,46)=a[19]; M.at<double>(95,52)=a[16]; M.at<double>(95,55)=b[6]; M.at<double>(95,56)=b[17]; M.at<double>(95,66)=b[19]; M.at<double>(95,68)=b[8]; M.at<double>(95,72)=b[12]; M.at<double>(95,73)=b[16]; M.at<double>(95,93)=c[16]; M.at<double>(95,95)=c[12]; M.at<double>(95,97)=c[8]; M.at<double>(95,101)=c[2]; M.at<double>(95,102)=c[19]; M.at<double>(95,106)=c[17]; M.at<double>(95,108)=c[18]; M.at<double>(95,113)=c[6];
+    M.at<double>(96,23)=u[4]; M.at<double>(96,27)=a[5]; M.at<double>(96,28)=a[17]; M.at<double>(96,32)=a[12]; M.at<double>(96,38)=a[16]; M.at<double>(96,40)=a[14]; M.at<double>(96,41)=a[19]; M.at<double>(96,42)=a[4]; M.at<double>(96,47)=a[8]; M.at<double>(96,55)=b[5]; M.at<double>(96,56)=b[4]; M.at<double>(96,57)=b[17]; M.at<double>(96,60)=b[8]; M.at<double>(96,61)=b[12]; M.at<double>(96,62)=b[19]; M.at<double>(96,72)=b[14]; M.at<double>(96,78)=b[16]; M.at<double>(96,90)=c[8]; M.at<double>(96,92)=c[16]; M.at<double>(96,95)=c[14]; M.at<double>(96,101)=c[13]; M.at<double>(96,106)=c[4]; M.at<double>(96,108)=c[3]; M.at<double>(96,109)=c[2]; M.at<double>(96,111)=c[19]; M.at<double>(96,113)=c[5]; M.at<double>(96,114)=c[17]; M.at<double>(96,116)=c[12];
+    M.at<double>(97,42)=a[6]; M.at<double>(97,44)=a[12]; M.at<double>(97,46)=a[17]; M.at<double>(97,56)=b[6]; M.at<double>(97,66)=b[17]; M.at<double>(97,67)=b[8]; M.at<double>(97,68)=b[12]; M.at<double>(97,70)=b[16]; M.at<double>(97,91)=c[16]; M.at<double>(97,97)=c[12]; M.at<double>(97,101)=c[18]; M.at<double>(97,102)=c[17]; M.at<double>(97,104)=c[8]; M.at<double>(97,106)=c[6]; M.at<double>(97,108)=c[7];
+    M.at<double>(98,28)=a[12]; M.at<double>(98,41)=a[8]; M.at<double>(98,42)=a[14]; M.at<double>(98,47)=a[16]; M.at<double>(98,56)=b[14]; M.at<double>(98,57)=b[12]; M.at<double>(98,60)=b[16]; M.at<double>(98,62)=b[8]; M.at<double>(98,90)=c[16]; M.at<double>(98,101)=c[4]; M.at<double>(98,105)=c[3]; M.at<double>(98,106)=c[14]; M.at<double>(98,108)=c[5]; M.at<double>(98,109)=c[17]; M.at<double>(98,111)=c[8]; M.at<double>(98,114)=c[12];
+    M.at<double>(99,42)=a[7]; M.at<double>(99,44)=a[6]; M.at<double>(99,46)=a[18]; M.at<double>(99,56)=b[7]; M.at<double>(99,64)=b[8]; M.at<double>(99,66)=b[18]; M.at<double>(99,67)=b[17]; M.at<double>(99,68)=b[6]; M.at<double>(99,70)=b[12]; M.at<double>(99,91)=c[12]; M.at<double>(99,97)=c[6]; M.at<double>(99,102)=c[18]; M.at<double>(99,103)=c[8]; M.at<double>(99,104)=c[17]; M.at<double>(99,106)=c[7];
+    M.at<double>(100,51)=a[16]; M.at<double>(100,65)=b[16]; M.at<double>(100,89)=c[16]; M.at<double>(100,105)=c[10]; M.at<double>(100,109)=c[14];
+    M.at<double>(101,37)=a[9]; M.at<double>(101,38)=a[13]; M.at<double>(101,39)=a[20]; M.at<double>(101,48)=a[11]; M.at<double>(101,50)=a[3]; M.at<double>(101,53)=a[2]; M.at<double>(101,77)=b[9]; M.at<double>(101,78)=b[13]; M.at<double>(101,79)=b[20]; M.at<double>(101,80)=b[3]; M.at<double>(101,81)=b[11]; M.at<double>(101,82)=b[2]; M.at<double>(101,85)=c[11]; M.at<double>(101,86)=c[3]; M.at<double>(101,87)=c[2]; M.at<double>(101,92)=c[13]; M.at<double>(101,96)=c[20]; M.at<double>(101,98)=c[9];
+    M.at<double>(102,37)=a[13]; M.at<double>(102,38)=a[20]; M.at<double>(102,48)=a[3]; M.at<double>(102,50)=a[2]; M.at<double>(102,77)=b[13]; M.at<double>(102,78)=b[20]; M.at<double>(102,80)=b[2]; M.at<double>(102,81)=b[3]; M.at<double>(102,85)=c[3]; M.at<double>(102,86)=c[2]; M.at<double>(102,92)=c[20]; M.at<double>(102,98)=c[13];
+    M.at<double>(103,37)=a[20]; M.at<double>(103,48)=a[2]; M.at<double>(103,77)=b[20]; M.at<double>(103,81)=b[2]; M.at<double>(103,85)=c[2]; M.at<double>(103,98)=c[20];
+    M.at<double>(104,11)=u[4]; M.at<double>(104,28)=a[10]; M.at<double>(104,33)=a[14]; M.at<double>(104,34)=a[8]; M.at<double>(104,36)=a[16]; M.at<double>(104,51)=a[4]; M.at<double>(104,54)=b[8]; M.at<double>(104,57)=b[10]; M.at<double>(104,59)=b[14]; M.at<double>(104,65)=b[4]; M.at<double>(104,84)=b[16]; M.at<double>(104,88)=c[16]; M.at<double>(104,89)=c[4]; M.at<double>(104,99)=c[8]; M.at<double>(104,108)=c[1]; M.at<double>(104,109)=c[9]; M.at<double>(104,114)=c[10]; M.at<double>(104,117)=c[14];
+    M.at<double>(105,20)=u[4]; M.at<double>(105,27)=a[10]; M.at<double>(105,28)=a[4]; M.at<double>(105,32)=a[14]; M.at<double>(105,33)=a[8]; M.at<double>(105,51)=a[19]; M.at<double>(105,53)=a[16]; M.at<double>(105,55)=b[10]; M.at<double>(105,57)=b[4]; M.at<double>(105,59)=b[8]; M.at<double>(105,61)=b[14]; M.at<double>(105,65)=b[19]; M.at<double>(105,82)=b[16]; M.at<double>(105,87)=c[16]; M.at<double>(105,89)=c[19]; M.at<double>(105,108)=c[9]; M.at<double>(105,109)=c[13]; M.at<double>(105,113)=c[10]; M.at<double>(105,114)=c[4]; M.at<double>(105,116)=c[14]; M.at<double>(105,117)=c[8];
+    M.at<double>(106,27)=a[4]; M.at<double>(106,28)=a[19]; M.at<double>(106,31)=a[14]; M.at<double>(106,32)=a[8]; M.at<double>(106,50)=a[16]; M.at<double>(106,55)=b[4]; M.at<double>(106,57)=b[19]; M.at<double>(106,58)=b[14]; M.at<double>(106,61)=b[8]; M.at<double>(106,80)=b[16]; M.at<double>(106,86)=c[16]; M.at<double>(106,108)=c[13]; M.at<double>(106,109)=c[20]; M.at<double>(106,113)=c[4]; M.at<double>(106,114)=c[19]; M.at<double>(106,115)=c[14]; M.at<double>(106,116)=c[8];
+    M.at<double>(107,27)=a[19]; M.at<double>(107,31)=a[8]; M.at<double>(107,48)=a[16]; M.at<double>(107,55)=b[19]; M.at<double>(107,58)=b[8]; M.at<double>(107,81)=b[16]; M.at<double>(107,85)=c[16]; M.at<double>(107,108)=c[20]; M.at<double>(107,113)=c[19]; M.at<double>(107,115)=c[8];
+    M.at<double>(108,36)=a[20]; M.at<double>(108,48)=a[1]; M.at<double>(108,50)=a[9]; M.at<double>(108,53)=a[13]; M.at<double>(108,80)=b[9]; M.at<double>(108,81)=b[1]; M.at<double>(108,82)=b[13]; M.at<double>(108,84)=b[20]; M.at<double>(108,85)=c[1]; M.at<double>(108,86)=c[9]; M.at<double>(108,87)=c[13]; M.at<double>(108,88)=c[20];
+    M.at<double>(109,26)=u[3]; M.at<double>(109,27)=a[7]; M.at<double>(109,40)=a[6]; M.at<double>(109,42)=a[18]; M.at<double>(109,44)=a[17]; M.at<double>(109,46)=a[2]; M.at<double>(109,52)=a[12]; M.at<double>(109,55)=b[7]; M.at<double>(109,56)=b[18]; M.at<double>(109,66)=b[2]; M.at<double>(109,67)=b[19]; M.at<double>(109,68)=b[17]; M.at<double>(109,70)=b[8]; M.at<double>(109,72)=b[6]; M.at<double>(109,73)=b[12]; M.at<double>(109,91)=c[8]; M.at<double>(109,93)=c[12]; M.at<double>(109,95)=c[6]; M.at<double>(109,97)=c[17]; M.at<double>(109,102)=c[2]; M.at<double>(109,104)=c[19]; M.at<double>(109,106)=c[18]; M.at<double>(109,113)=c[7];
+    M.at<double>(110,7)=u[3]; M.at<double>(110,30)=a[5]; M.at<double>(110,34)=a[7]; M.at<double>(110,43)=a[15]; M.at<double>(110,45)=a[11]; M.at<double>(110,49)=a[6]; M.at<double>(110,54)=b[7]; M.at<double>(110,63)=b[11]; M.at<double>(110,67)=b[1]; M.at<double>(110,69)=b[10]; M.at<double>(110,71)=b[15]; M.at<double>(110,75)=b[5]; M.at<double>(110,76)=b[6]; M.at<double>(110,99)=c[7]; M.at<double>(110,100)=c[15]; M.at<double>(110,104)=c[1]; M.at<double>(110,107)=c[5]; M.at<double>(110,110)=c[10]; M.at<double>(110,112)=c[11]; M.at<double>(110,119)=c[6];
+    M.at<double>(111,18)=u[2]; M.at<double>(111,35)=a[20]; M.at<double>(111,36)=a[13]; M.at<double>(111,50)=a[1]; M.at<double>(111,53)=a[9]; M.at<double>(111,80)=b[1]; M.at<double>(111,82)=b[9]; M.at<double>(111,83)=b[20]; M.at<double>(111,84)=b[13]; M.at<double>(111,86)=c[1]; M.at<double>(111,87)=c[9]; M.at<double>(111,88)=c[13]; M.at<double>(111,118)=c[20];
+    M.at<double>(112,19)=u[2]; M.at<double>(112,31)=a[1]; M.at<double>(112,32)=a[9]; M.at<double>(112,33)=a[13]; M.at<double>(112,34)=a[20]; M.at<double>(112,36)=a[19]; M.at<double>(112,50)=a[10]; M.at<double>(112,53)=a[4]; M.at<double>(112,54)=b[20]; M.at<double>(112,58)=b[1]; M.at<double>(112,59)=b[13]; M.at<double>(112,61)=b[9]; M.at<double>(112,80)=b[10]; M.at<double>(112,82)=b[4]; M.at<double>(112,84)=b[19]; M.at<double>(112,86)=c[10]; M.at<double>(112,87)=c[4]; M.at<double>(112,88)=c[19]; M.at<double>(112,99)=c[20]; M.at<double>(112,115)=c[1]; M.at<double>(112,116)=c[9]; M.at<double>(112,117)=c[13];
+    M.at<double>(113,31)=a[9]; M.at<double>(113,32)=a[13]; M.at<double>(113,33)=a[20]; M.at<double>(113,48)=a[10]; M.at<double>(113,50)=a[4]; M.at<double>(113,53)=a[19]; M.at<double>(113,58)=b[9]; M.at<double>(113,59)=b[20]; M.at<double>(113,61)=b[13]; M.at<double>(113,80)=b[4]; M.at<double>(113,81)=b[10]; M.at<double>(113,82)=b[19]; M.at<double>(113,85)=c[10]; M.at<double>(113,86)=c[4]; M.at<double>(113,87)=c[19]; M.at<double>(113,115)=c[9]; M.at<double>(113,116)=c[13]; M.at<double>(113,117)=c[20];
+    M.at<double>(114,31)=a[13]; M.at<double>(114,32)=a[20]; M.at<double>(114,48)=a[4]; M.at<double>(114,50)=a[19]; M.at<double>(114,58)=b[13]; M.at<double>(114,61)=b[20]; M.at<double>(114,80)=b[19]; M.at<double>(114,81)=b[4]; M.at<double>(114,85)=c[4]; M.at<double>(114,86)=c[19]; M.at<double>(114,115)=c[13]; M.at<double>(114,116)=c[20];
+    M.at<double>(115,31)=a[20]; M.at<double>(115,48)=a[19]; M.at<double>(115,58)=b[20]; M.at<double>(115,81)=b[19]; M.at<double>(115,85)=c[19]; M.at<double>(115,115)=c[20];
+    M.at<double>(116,24)=u[2]; M.at<double>(116,29)=a[13]; M.at<double>(116,30)=a[20]; M.at<double>(116,37)=a[11]; M.at<double>(116,38)=a[3]; M.at<double>(116,39)=a[2]; M.at<double>(116,50)=a[15]; M.at<double>(116,52)=a[9]; M.at<double>(116,53)=a[18]; M.at<double>(116,73)=b[9]; M.at<double>(116,74)=b[13]; M.at<double>(116,75)=b[20]; M.at<double>(116,77)=b[11]; M.at<double>(116,78)=b[3]; M.at<double>(116,79)=b[2]; M.at<double>(116,80)=b[15]; M.at<double>(116,82)=b[18]; M.at<double>(116,86)=c[15]; M.at<double>(116,87)=c[18]; M.at<double>(116,92)=c[3]; M.at<double>(116,93)=c[9]; M.at<double>(116,94)=c[13]; M.at<double>(116,96)=c[2]; M.at<double>(116,98)=c[11]; M.at<double>(116,107)=c[20];
+    M.at<double>(117,29)=a[20]; M.at<double>(117,37)=a[3]; M.at<double>(117,38)=a[2]; M.at<double>(117,48)=a[15]; M.at<double>(117,50)=a[18]; M.at<double>(117,52)=a[13]; M.at<double>(117,73)=b[13]; M.at<double>(117,74)=b[20]; M.at<double>(117,77)=b[3]; M.at<double>(117,78)=b[2]; M.at<double>(117,80)=b[18]; M.at<double>(117,81)=b[15]; M.at<double>(117,85)=c[15]; M.at<double>(117,86)=c[18]; M.at<double>(117,92)=c[2]; M.at<double>(117,93)=c[13]; M.at<double>(117,94)=c[20]; M.at<double>(117,98)=c[3];
+    M.at<double>(118,27)=a[13]; M.at<double>(118,28)=a[20]; M.at<double>(118,31)=a[4]; M.at<double>(118,32)=a[19]; M.at<double>(118,48)=a[14]; M.at<double>(118,50)=a[8]; M.at<double>(118,55)=b[13]; M.at<double>(118,57)=b[20]; M.at<double>(118,58)=b[4]; M.at<double>(118,61)=b[19]; M.at<double>(118,80)=b[8]; M.at<double>(118,81)=b[14]; M.at<double>(118,85)=c[14]; M.at<double>(118,86)=c[8]; M.at<double>(118,113)=c[13]; M.at<double>(118,114)=c[20]; M.at<double>(118,115)=c[4]; M.at<double>(118,116)=c[19];
+    M.at<double>(119,27)=a[20]; M.at<double>(119,31)=a[19]; M.at<double>(119,48)=a[8]; M.at<double>(119,55)=b[20]; M.at<double>(119,58)=b[19]; M.at<double>(119,81)=b[8]; M.at<double>(119,85)=c[8]; M.at<double>(119,113)=c[20]; M.at<double>(119,115)=c[19];
+
+    return M.t();
+}
+
+cv::Mat dls::Hessian(const double s[])
+{
+    // the vector of monomials is
+    // m = [ const ; s1^2 * s2 ; s1 * s2 ; s1 * s3 ; s2 * s3 ; s2^2 * s3 ; s2^3 ; ...
+    //       s1 * s3^2 ; s1 ; s3 ; s2 ; s2 * s3^2 ; s1^2 ; s3^2 ; s2^2 ; s3^3 ;   ...
+    //       s1 * s2 * s3 ; s1 * s2^2 ; s1^2 * s3 ; s1^3]
+    //
+
+    // deriv of m w.r.t. s1
+    //Hs3 = [0 ; 2 * s(1) * s(2) ; s(2) ; s(3) ; 0 ; 0 ; 0 ; ...
+    //         s(3)^2 ; 1 ; 0 ; 0 ; 0 ; 2 * s(1) ; 0 ; 0 ; 0 ; ...
+    //         s(2) * s(3) ; s(2)^2 ; 2*s(1)*s(3); 3 * s(1)^2];
+
+    double Hs1[20];
+    Hs1[0]=0; Hs1[1]=2*s[0]*s[1]; Hs1[2]=s[1]; Hs1[3]=s[2]; Hs1[4]=0; Hs1[5]=0; Hs1[6]=0;
+    Hs1[7]=s[2]*s[2]; Hs1[8]=1; Hs1[9]=0; Hs1[10]=0; Hs1[11]=0; Hs1[12]=2*s[0]; Hs1[13]=0;
+    Hs1[14]=0; Hs1[15]=0; Hs1[16]=s[1]*s[2]; Hs1[17]=s[1]*s[1]; Hs1[18]=2*s[0]*s[2]; Hs1[19]=3*s[0]*s[0];
+
+    // deriv of m w.r.t. s2
+    //Hs2 = [0 ; s(1)^2 ; s(1) ; 0 ; s(3) ; 2 * s(2) * s(3) ; 3 * s(2)^2 ; ...
+    //         0 ; 0 ; 0 ; 1 ; s(3)^2 ; 0 ; 0 ; 2 * s(2) ; 0 ; ...
+    //         s(1) * s(3) ; s(1) * 2 * s(2) ; 0 ; 0];
+
+    double Hs2[20];
+    Hs2[0]=0; Hs2[1]=s[0]*s[0]; Hs2[2]=s[0]; Hs2[3]=0; Hs2[4]=s[2]; Hs2[5]=2*s[1]*s[2]; Hs2[6]=3*s[1]*s[1];
+    Hs2[7]=0; Hs2[8]=0; Hs2[9]=0; Hs2[10]=1; Hs2[11]=s[2]*s[2]; Hs2[12]=0; Hs2[13]=0;
+    Hs2[14]=2*s[1]; Hs2[15]=0; Hs2[16]=s[0]*s[2]; Hs2[17]=2*s[0]*s[1]; Hs2[18]=0; Hs2[19]=0;
+
+    // deriv of m w.r.t. s3
+    //Hs3 = [0 ; 0 ; 0 ; s(1) ; s(2) ; s(2)^2 ; 0 ; ...
+    //         s(1) * 2 * s(3) ; 0 ; 1 ; 0 ; s(2) * 2 * s(3) ; 0 ; 2 * s(3) ; 0 ; 3 * s(3)^2 ; ...
+    //         s(1) * s(2) ; 0 ; s(1)^2 ; 0];
+
+    double Hs3[20];
+    Hs3[0]=0; Hs3[1]=0; Hs3[2]=0; Hs3[3]=s[0]; Hs3[4]=s[1]; Hs3[5]=s[1]*s[1]; Hs3[6]=0;
+    Hs3[7]=2*s[0]*s[2]; Hs3[8]=0; Hs3[9]=1; Hs3[10]=0; Hs3[11]=2*s[1]*s[2]; Hs3[12]=0; Hs3[13]=2*s[2];
+    Hs3[14]=0; Hs3[15]=3*s[2]*s[2]; Hs3[16]=s[0]*s[1]; Hs3[17]=0; Hs3[18]=s[0]*s[0]; Hs3[19]=0;
+
+    // fill Hessian matrix
+    cv::Mat H(3, 3, CV_64F);
+    H.at<double>(0,0) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at<double>(0,0);
+    H.at<double>(0,1) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at<double>(0,0);
+    H.at<double>(0,2) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at<double>(0,0);
+
+    H.at<double>(1,0) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at<double>(0,0);
+    H.at<double>(1,1) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at<double>(0,0);
+    H.at<double>(1,2) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at<double>(0,0);
+
+    H.at<double>(2,0) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at<double>(0,0);
+    H.at<double>(2,1) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at<double>(0,0);
+    H.at<double>(2,2) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at<double>(0,0);
+
+    return H;
+}
+
+cv::Mat dls::cayley2rotbar(const cv::Mat& s)
+{
+    double s_mul1 = cv::Mat(s.t()*s).at<double>(0,0);
+    cv::Mat s_mul2 = s*s.t();
+    cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
+
+    return cv::Mat( eye.mul(1.-s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.) ).t();
+}
+
+cv::Mat dls::skewsymm(const cv::Mat * X1)
+{
+    cv::MatConstIterator_<double> it = X1->begin<double>();
+    return (cv::Mat_<double>(3,3) <<        0, -*(it+2),  *(it+1),
+                                      *(it+2),        0, -*(it+0),
+                                     -*(it+1),  *(it+0),       0);
+}
+
+cv::Mat dls::rotx(const double t)
+{
+    // rotx: rotation about y-axis
+    double ct = cos(t);
+    double st = sin(t);
+    return (cv::Mat_<double>(3,3) << 1, 0, 0, 0, ct, -st, 0, st, ct);
+}
+
+cv::Mat dls::roty(const double t)
+{
+    // roty: rotation about y-axis
+    double ct = cos(t);
+    double st = sin(t);
+    return (cv::Mat_<double>(3,3) << ct, 0, st, 0, 1, 0, -st, 0, ct);
+}
+
+cv::Mat dls::rotz(const double t)
+{
+    // rotz: rotation about y-axis
+    double ct = cos(t);
+    double st = sin(t);
+    return (cv::Mat_<double>(3,3) << ct, -st, 0, st, ct, 0, 0, 0, 1);
+}
+
+cv::Mat dls::mean(const cv::Mat& M)
+{
+    cv::Mat m = cv::Mat::zeros(3, 1, CV_64F);
+    for (int i = 0; i < M.cols; ++i) m += M.col(i);
+    return m.mul(1./(double)M.cols);
+}
+
+bool dls::is_empty(const cv::Mat * M)
+{
+    cv::MatConstIterator_<double> it = M->begin<double>(), it_end = M->end<double>();
+    for(; it != it_end; ++it)
+    {
+        if(*it < 0) return false;
+    }
+    return true;
+}
+
+bool dls::positive_eigenvalues(const cv::Mat * eigenvalues)
+{
+    CV_Assert(eigenvalues && !eigenvalues->empty());
+    cv::MatConstIterator_<double> it = eigenvalues->begin<double>();
+    return *(it) > 0 && *(it+1) > 0 && *(it+2) > 0;
+}
@@ -0,0 +1,773 @@
+#ifndef DLS_H_
+#define DLS_H_
+
+#include "precomp.hpp"
+
+#include <iostream>
+
+using namespace std;
+using namespace cv;
+
+class dls
+{
+public:
+    dls(const cv::Mat& opoints, const cv::Mat& ipoints);
+    ~dls();
+
+    bool compute_pose(cv::Mat& R, cv::Mat& t);
+
+private:
+
+    // initialisation
+    template <typename OpointType, typename IpointType>
+    void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
+    {
+        for(int i = 0; i < N; i++)
+        {
+            p.at<double>(0,i) = opoints.at<OpointType>(i).x;
+            p.at<double>(1,i) = opoints.at<OpointType>(i).y;
+            p.at<double>(2,i) = opoints.at<OpointType>(i).z;
+
+            // compute mean of object points
+            mn.at<double>(0) += p.at<double>(0,i);
+            mn.at<double>(1) += p.at<double>(1,i);
+            mn.at<double>(2) += p.at<double>(2,i);
+
+            // make z into unit vectors from normalized pixel coords
+            double sr = std::pow(ipoints.at<IpointType>(i).x, 2) +
+                        std::pow(ipoints.at<IpointType>(i).y, 2) + (double)1;
+                   sr = std::sqrt(sr);
+
+            z.at<double>(0,i) = ipoints.at<IpointType>(i).x / sr;
+            z.at<double>(1,i) = ipoints.at<IpointType>(i).y / sr;
+            z.at<double>(2,i) = (double)1 / sr;
+        }
+
+        mn.at<double>(0) /= (double)N;
+        mn.at<double>(1) /= (double)N;
+        mn.at<double>(2) /= (double)N;
+    }
+
+    // main algorithm
+    cv::Mat LeftMultVec(const cv::Mat& v);
+    void run_kernel(const cv::Mat& pp);
+    void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D);
+    void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
+                                                 cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
+    void fill_coeff(const cv::Mat * D);
+
+    // useful functions
+    cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
+                        const std::vector<double>& c, const std::vector<double>& u);
+    cv::Mat Hessian(const double s[]);
+    cv::Mat cayley2rotbar(const cv::Mat& s);
+    cv::Mat skewsymm(const cv::Mat * X1);
+
+    // extra functions
+    cv::Mat rotx(const double t);
+    cv::Mat roty(const double t);
+    cv::Mat rotz(const double t);
+    cv::Mat mean(const cv::Mat& M);
+    bool is_empty(const cv::Mat * v);
+    bool positive_eigenvalues(const cv::Mat * eigenvalues);
+
+    cv::Mat p, z, mn;        // object-image points
+    int N;                // number of input points
+    std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix
+    std::vector<cv::Mat> C_est_, t_est_;    // optimal candidates
+    cv::Mat C_est__, t_est__;                // optimal found solution
+    double cost__;                            // optimal found solution
+};
+
+class EigenvalueDecomposition {
+private:
+
+    // Holds the data dimension.
+    int n;
+
+    // Stores real/imag part of a complex division.
+    double cdivr, cdivi;
+
+    // Pointer to internal memory.
+    double *d, *e, *ort;
+    double **V, **H;
+
+    // Holds the computed eigenvalues.
+    Mat _eigenvalues;
+
+    // Holds the computed eigenvectors.
+    Mat _eigenvectors;
+
+    // Allocates memory.
+    template<typename _Tp>
+    _Tp *alloc_1d(int m) {
+        return new _Tp[m];
+    }
+
+    // Allocates memory.
+    template<typename _Tp>
+    _Tp *alloc_1d(int m, _Tp val) {
+        _Tp *arr = alloc_1d<_Tp> (m);
+        for (int i = 0; i < m; i++)
+            arr[i] = val;
+        return arr;
+    }
+
+    // Allocates memory.
+    template<typename _Tp>
+    _Tp **alloc_2d(int m, int _n) {
+        _Tp **arr = new _Tp*[m];
+        for (int i = 0; i < m; i++)
+            arr[i] = new _Tp[_n];
+        return arr;
+    }
+
+    // Allocates memory.
+    template<typename _Tp>
+    _Tp **alloc_2d(int m, int _n, _Tp val) {
+        _Tp **arr = alloc_2d<_Tp> (m, _n);
+        for (int i = 0; i < m; i++) {
+            for (int j = 0; j < _n; j++) {
+                arr[i][j] = val;
+            }
+        }
+        return arr;
+    }
+
+    void cdiv(double xr, double xi, double yr, double yi) {
+        double r, dv;
+        if (std::abs(yr) > std::abs(yi)) {
+            r = yi / yr;
+            dv = yr + r * yi;
+            cdivr = (xr + r * xi) / dv;
+            cdivi = (xi - r * xr) / dv;
+        } else {
+            r = yr / yi;
+            dv = yi + r * yr;
+            cdivr = (r * xr + xi) / dv;
+            cdivi = (r * xi - xr) / dv;
+        }
+    }
+
+    // Nonsymmetric reduction from Hessenberg to real Schur form.
+
+    void hqr2() {
+
+        //  This is derived from the Algol procedure hqr2,
+        //  by Martin and Wilkinson, Handbook for Auto. Comp.,
+        //  Vol.ii-Linear Algebra, and the corresponding
+        //  Fortran subroutine in EISPACK.
+
+        // Initialize
+        int nn = this->n;
+        int n1 = nn - 1;
+        int low = 0;
+        int high = nn - 1;
+        double eps = std::pow(2.0, -52.0);
+        double exshift = 0.0;
+        double p = 0, q = 0, r = 0, s = 0, z = 0, t, w, x, y;
+
+        // Store roots isolated by balanc and compute matrix norm
+
+        double norm = 0.0;
+        for (int i = 0; i < nn; i++) {
+            if (i < low || i > high) {
+                d[i] = H[i][i];
+                e[i] = 0.0;
+            }
+            for (int j = std::max(i - 1, 0); j < nn; j++) {
+                norm = norm + std::abs(H[i][j]);
+            }
+        }
+
+        // Outer loop over eigenvalue index
+        int iter = 0;
+        while (n1 >= low) {
+
+            // Look for single small sub-diagonal element
+            int l = n1;
+            while (l > low) {
+                s = std::abs(H[l - 1][l - 1]) + std::abs(H[l][l]);
+                if (s == 0.0) {
+                    s = norm;
+                }
+                if (std::abs(H[l][l - 1]) < eps * s) {
+                    break;
+                }
+                l--;
+            }
+
+            // Check for convergence
+            // One root found
+
+            if (l == n1) {
+                H[n1][n1] = H[n1][n1] + exshift;
+                d[n1] = H[n1][n1];
+                e[n1] = 0.0;
+                n1--;
+                iter = 0;
+
+                // Two roots found
+
+            } else if (l == n1 - 1) {
+                w = H[n1][n1 - 1] * H[n1 - 1][n1];
+                p = (H[n1 - 1][n1 - 1] - H[n1][n1]) / 2.0;
+                q = p * p + w;
+                z = std::sqrt(std::abs(q));
+                H[n1][n1] = H[n1][n1] + exshift;
+                H[n1 - 1][n1 - 1] = H[n1 - 1][n1 - 1] + exshift;
+                x = H[n1][n1];
+
+                // Real pair
+
+                if (q >= 0) {
+                    if (p >= 0) {
+                        z = p + z;
+                    } else {
+                        z = p - z;
+                    }
+                    d[n1 - 1] = x + z;
+                    d[n1] = d[n1 - 1];
+                    if (z != 0.0) {
+                        d[n1] = x - w / z;
+                    }
+                    e[n1 - 1] = 0.0;
+                    e[n1] = 0.0;
+                    x = H[n1][n1 - 1];
+                    s = std::abs(x) + std::abs(z);
+                    p = x / s;
+                    q = z / s;
+                    r = std::sqrt(p * p + q * q);
+                    p = p / r;
+                    q = q / r;
+
+                    // Row modification
+
+                    for (int j = n1 - 1; j < nn; j++) {
+                        z = H[n1 - 1][j];
+                        H[n1 - 1][j] = q * z + p * H[n1][j];
+                        H[n1][j] = q * H[n1][j] - p * z;
+                    }
+
+                    // Column modification
+
+                    for (int i = 0; i <= n1; i++) {
+                        z = H[i][n1 - 1];
+                        H[i][n1 - 1] = q * z + p * H[i][n1];
+                        H[i][n1] = q * H[i][n1] - p * z;
+                    }
+
+                    // Accumulate transformations
+
+                    for (int i = low; i <= high; i++) {
+                        z = V[i][n1 - 1];
+                        V[i][n1 - 1] = q * z + p * V[i][n1];
+                        V[i][n1] = q * V[i][n1] - p * z;
+                    }
+
+                    // Complex pair
+
+                } else {
+                    d[n1 - 1] = x + p;
+                    d[n1] = x + p;
+                    e[n1 - 1] = z;
+                    e[n1] = -z;
+                }
+                n1 = n1 - 2;
+                iter = 0;
+
+                // No convergence yet
+
+            } else {
+
+                // Form shift
+
+                x = H[n1][n1];
+                y = 0.0;
+                w = 0.0;
+                if (l < n1) {
+                    y = H[n1 - 1][n1 - 1];
+                    w = H[n1][n1 - 1] * H[n1 - 1][n1];
+                }
+
+                // Wilkinson's original ad hoc shift
+
+                if (iter == 10) {
+                    exshift += x;
+                    for (int i = low; i <= n1; i++) {
+                        H[i][i] -= x;
+                    }
+                    s = std::abs(H[n1][n1 - 1]) + std::abs(H[n1 - 1][n1 - 2]);
+                    x = y = 0.75 * s;
+                    w = -0.4375 * s * s;
+                }
+
+                // MATLAB's new ad hoc shift
+
+                if (iter == 30) {
+                    s = (y - x) / 2.0;
+                    s = s * s + w;
+                    if (s > 0) {
+                        s = std::sqrt(s);
+                        if (y < x) {
+                            s = -s;
+                        }
+                        s = x - w / ((y - x) / 2.0 + s);
+                        for (int i = low; i <= n1; i++) {
+                            H[i][i] -= s;
+                        }
+                        exshift += s;
+                        x = y = w = 0.964;
+                    }
+                }
+
+                iter = iter + 1; // (Could check iteration count here.)
+
+                // Look for two consecutive small sub-diagonal elements
+                int m = n1 - 2;
+                while (m >= l) {
+                    z = H[m][m];
+                    r = x - z;
+                    s = y - z;
+                    p = (r * s - w) / H[m + 1][m] + H[m][m + 1];
+                    q = H[m + 1][m + 1] - z - r - s;
+                    r = H[m + 2][m + 1];
+                    s = std::abs(p) + std::abs(q) + std::abs(r);
+                    p = p / s;
+                    q = q / s;
+                    r = r / s;
+                    if (m == l) {
+                        break;
+                    }
+                    if (std::abs(H[m][m - 1]) * (std::abs(q) + std::abs(r)) < eps * (std::abs(p)
+                                                                                     * (std::abs(H[m - 1][m - 1]) + std::abs(z) + std::abs(
+                                                                                                                                           H[m + 1][m + 1])))) {
+                        break;
+                    }
+                    m--;
+                }
+
+                for (int i = m + 2; i <= n1; i++) {
+                    H[i][i - 2] = 0.0;
+                    if (i > m + 2) {
+                        H[i][i - 3] = 0.0;
+                    }
+                }
+
+                // Double QR step involving rows l:n and columns m:n
+
+                for (int k = m; k <= n1 - 1; k++) {
+                    bool notlast = (k != n1 - 1);
+                    if (k != m) {
+                        p = H[k][k - 1];
+                        q = H[k + 1][k - 1];
+                        r = (notlast ? H[k + 2][k - 1] : 0.0);
+                        x = std::abs(p) + std::abs(q) + std::abs(r);
+                        if (x != 0.0) {
+                            p = p / x;
+                            q = q / x;
+                            r = r / x;
+                        }
+                    }
+                    if (x == 0.0) {
+                        break;
+                    }
+                    s = std::sqrt(p * p + q * q + r * r);
+                    if (p < 0) {
+                        s = -s;
+                    }
+                    if (s != 0) {
+                        if (k != m) {
+                            H[k][k - 1] = -s * x;
+                        } else if (l != m) {
+                            H[k][k - 1] = -H[k][k - 1];
+                        }
+                        p = p + s;
+                        x = p / s;
+                        y = q / s;
+                        z = r / s;
+                        q = q / p;
+                        r = r / p;
+
+                        // Row modification
+
+                        for (int j = k; j < nn; j++) {
+                            p = H[k][j] + q * H[k + 1][j];
+                            if (notlast) {
+                                p = p + r * H[k + 2][j];
+                                H[k + 2][j] = H[k + 2][j] - p * z;
+                            }
+                            H[k][j] = H[k][j] - p * x;
+                            H[k + 1][j] = H[k + 1][j] - p * y;
+                        }
+
+                        // Column modification
+
+                        for (int i = 0; i <= std::min(n1, k + 3); i++) {
+                            p = x * H[i][k] + y * H[i][k + 1];
+                            if (notlast) {
+                                p = p + z * H[i][k + 2];
+                                H[i][k + 2] = H[i][k + 2] - p * r;
+                            }
+                            H[i][k] = H[i][k] - p;
+                            H[i][k + 1] = H[i][k + 1] - p * q;
+                        }
+
+                        // Accumulate transformations
+
+                        for (int i = low; i <= high; i++) {
+                            p = x * V[i][k] + y * V[i][k + 1];
+                            if (notlast) {
+                                p = p + z * V[i][k + 2];
+                                V[i][k + 2] = V[i][k + 2] - p * r;
+                            }
+                            V[i][k] = V[i][k] - p;
+                            V[i][k + 1] = V[i][k + 1] - p * q;
+                        }
+                    } // (s != 0)
+                } // k loop
+            } // check convergence
+        } // while (n1 >= low)
+
+        // Backsubstitute to find vectors of upper triangular form
+
+        if (norm == 0.0) {
+            return;
+        }
+
+        for (n1 = nn - 1; n1 >= 0; n1--) {
+            p = d[n1];
+            q = e[n1];
+
+            // Real vector
+
+            if (q == 0) {
+                int l = n1;
+                H[n1][n1] = 1.0;
+                for (int i = n1 - 1; i >= 0; i--) {
+                    w = H[i][i] - p;
+                    r = 0.0;
+                    for (int j = l; j <= n1; j++) {
+                        r = r + H[i][j] * H[j][n1];
+                    }
+                    if (e[i] < 0.0) {
+                        z = w;
+                        s = r;
+                    } else {
+                        l = i;
+                        if (e[i] == 0.0) {
+                            if (w != 0.0) {
+                                H[i][n1] = -r / w;
+                            } else {
+                                H[i][n1] = -r / (eps * norm);
+                            }
+
+                            // Solve real equations
+
+                        } else {
+                            x = H[i][i + 1];
+                            y = H[i + 1][i];
+                            q = (d[i] - p) * (d[i] - p) + e[i] * e[i];
+                            t = (x * s - z * r) / q;
+                            H[i][n1] = t;
+                            if (std::abs(x) > std::abs(z)) {
+                                H[i + 1][n1] = (-r - w * t) / x;
+                            } else {
+                                H[i + 1][n1] = (-s - y * t) / z;
+                            }
+                        }
+
+                        // Overflow control
+
+                        t = std::abs(H[i][n1]);
+                        if ((eps * t) * t > 1) {
+                            for (int j = i; j <= n1; j++) {
+                                H[j][n1] = H[j][n1] / t;
+                            }
+                        }
+                    }
+                }
+                // Complex vector
+            } else if (q < 0) {
+                int l = n1 - 1;
+
+                // Last vector component imaginary so matrix is triangular
+
+                if (std::abs(H[n1][n1 - 1]) > std::abs(H[n1 - 1][n1])) {
+                    H[n1 - 1][n1 - 1] = q / H[n1][n1 - 1];
+                    H[n1 - 1][n1] = -(H[n1][n1] - p) / H[n1][n1 - 1];
+                } else {
+                    cdiv(0.0, -H[n1 - 1][n1], H[n1 - 1][n1 - 1] - p, q);
+                    H[n1 - 1][n1 - 1] = cdivr;
+                    H[n1 - 1][n1] = cdivi;
+                }
+                H[n1][n1 - 1] = 0.0;
+                H[n1][n1] = 1.0;
+                for (int i = n1 - 2; i >= 0; i--) {
+                    double ra, sa;
+                    ra = 0.0;
+                    sa = 0.0;
+                    for (int j = l; j <= n1; j++) {
+                        ra = ra + H[i][j] * H[j][n1 - 1];
+                        sa = sa + H[i][j] * H[j][n1];
+                    }
+                    w = H[i][i] - p;
+
+                    if (e[i] < 0.0) {
+                        z = w;
+                        r = ra;
+                        s = sa;
+                    } else {
+                        l = i;
+                        if (e[i] == 0) {
+                            cdiv(-ra, -sa, w, q);
+                            H[i][n1 - 1] = cdivr;
+                            H[i][n1] = cdivi;
+                        } else {
+
+                            // Solve complex equations
+
+                            x = H[i][i + 1];
+                            y = H[i + 1][i];
+                            double vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q;
+                            double vi = (d[i] - p) * 2.0 * q;
+                            if (vr == 0.0 && vi == 0.0) {
+                                vr = eps * norm * (std::abs(w) + std::abs(q) + std::abs(x)
+                                                   + std::abs(y) + std::abs(z));
+                            }
+                            cdiv(x * r - z * ra + q * sa,
+                                 x * s - z * sa - q * ra, vr, vi);
+                            H[i][n1 - 1] = cdivr;
+                            H[i][n1] = cdivi;
+                            if (std::abs(x) > (std::abs(z) + std::abs(q))) {
+                                H[i + 1][n1 - 1] = (-ra - w * H[i][n1 - 1] + q
+                                                   * H[i][n1]) / x;
+                                H[i + 1][n1] = (-sa - w * H[i][n1] - q * H[i][n1
+                                                                            - 1]) / x;
+                            } else {
+                                cdiv(-r - y * H[i][n1 - 1], -s - y * H[i][n1], z,
+                                     q);
+                                H[i + 1][n1 - 1] = cdivr;
+                                H[i + 1][n1] = cdivi;
+                            }
+                        }
+
+                        // Overflow control
+
+                        t = std::max(std::abs(H[i][n1 - 1]), std::abs(H[i][n1]));
+                        if ((eps * t) * t > 1) {
+                            for (int j = i; j <= n1; j++) {
+                                H[j][n1 - 1] = H[j][n1 - 1] / t;
+                                H[j][n1] = H[j][n1] / t;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        // Vectors of isolated roots
+
+        for (int i = 0; i < nn; i++) {
+            if (i < low || i > high) {
+                for (int j = i; j < nn; j++) {
+                    V[i][j] = H[i][j];
+                }
+            }
+        }
+
+        // Back transformation to get eigenvectors of original matrix
+
+        for (int j = nn - 1; j >= low; j--) {
+            for (int i = low; i <= high; i++) {
+                z = 0.0;
+                for (int k = low; k <= std::min(j, high); k++) {
+                    z = z + V[i][k] * H[k][j];
+                }
+                V[i][j] = z;
+            }
+        }
+    }
+
+    // Nonsymmetric reduction to Hessenberg form.
+    void orthes() {
+        //  This is derived from the Algol procedures orthes and ortran,
+        //  by Martin and Wilkinson, Handbook for Auto. Comp.,
+        //  Vol.ii-Linear Algebra, and the corresponding
+        //  Fortran subroutines in EISPACK.
+        int low = 0;
+        int high = n - 1;
+
+        for (int m = low + 1; m <= high - 1; m++) {
+
+            // Scale column.
+
+            double scale = 0.0;
+            for (int i = m; i <= high; i++) {
+                scale = scale + std::abs(H[i][m - 1]);
+            }
+            if (scale != 0.0) {
+
+                // Compute Householder transformation.
+
+                double h = 0.0;
+                for (int i = high; i >= m; i--) {
+                    ort[i] = H[i][m - 1] / scale;
+                    h += ort[i] * ort[i];
+                }
+                double g = std::sqrt(h);
+                if (ort[m] > 0) {
+                    g = -g;
+                }
+                h = h - ort[m] * g;
+                ort[m] = ort[m] - g;
+
+                // Apply Householder similarity transformation
+                // H = (I-u*u'/h)*H*(I-u*u')/h)
+
+                for (int j = m; j < n; j++) {
+                    double f = 0.0;
+                    for (int i = high; i >= m; i--) {
+                        f += ort[i] * H[i][j];
+                    }
+                    f = f / h;
+                    for (int i = m; i <= high; i++) {
+                        H[i][j] -= f * ort[i];
+                    }
+                }
+
+                for (int i = 0; i <= high; i++) {
+                    double f = 0.0;
+                    for (int j = high; j >= m; j--) {
+                        f += ort[j] * H[i][j];
+                    }
+                    f = f / h;
+                    for (int j = m; j <= high; j++) {
+                        H[i][j] -= f * ort[j];
+                    }
+                }
+                ort[m] = scale * ort[m];
+                H[m][m - 1] = scale * g;
+            }
+        }
+
+        // Accumulate transformations (Algol's ortran).
+
+        for (int i = 0; i < n; i++) {
+            for (int j = 0; j < n; j++) {
+                V[i][j] = (i == j ? 1.0 : 0.0);
+            }
+        }
+
+        for (int m = high - 1; m >= low + 1; m--) {
+            if (H[m][m - 1] != 0.0) {
+                for (int i = m + 1; i <= high; i++) {
+                    ort[i] = H[i][m - 1];
+                }
+                for (int j = m; j <= high; j++) {
+                    double g = 0.0;
+                    for (int i = m; i <= high; i++) {
+                        g += ort[i] * V[i][j];
+                    }
+                    // Double division avoids possible underflow
+                    g = (g / ort[m]) / H[m][m - 1];
+                    for (int i = m; i <= high; i++) {
+                        V[i][j] += g * ort[i];
+                    }
+                }
+            }
+        }
+    }
+
+    // Releases all internal working memory.
+    void release() {
+        // releases the working data
+        delete[] d;
+        delete[] e;
+        delete[] ort;
+        for (int i = 0; i < n; i++) {
+            delete[] H[i];
+            delete[] V[i];
+        }
+        delete[] H;
+        delete[] V;
+    }
+
+    // Computes the Eigenvalue Decomposition for a matrix given in H.
+    void compute() {
+        // Allocate memory for the working data.
+        V = alloc_2d<double> (n, n, 0.0);
+        d = alloc_1d<double> (n);
+        e = alloc_1d<double> (n);
+        ort = alloc_1d<double> (n);
+        // Reduce to Hessenberg form.
+        orthes();
+        // Reduce Hessenberg to real Schur form.
+        hqr2();
+        // Copy eigenvalues to OpenCV Matrix.
+        _eigenvalues.create(1, n, CV_64FC1);
+        for (int i = 0; i < n; i++) {
+            _eigenvalues.at<double> (0, i) = d[i];
+        }
+        // Copy eigenvectors to OpenCV Matrix.
+        _eigenvectors.create(n, n, CV_64FC1);
+        for (int i = 0; i < n; i++)
+            for (int j = 0; j < n; j++)
+                _eigenvectors.at<double> (i, j) = V[i][j];
+        // Deallocate the memory by releasing all internal working data.
+        release();
+    }
+
+public:
+    EigenvalueDecomposition()
+    : n(0), cdivr(0), cdivi(0), d(0), e(0), ort(0), V(0), H(0) {}
+
+    // Initializes & computes the Eigenvalue Decomposition for a general matrix
+    // given in src. This function is a port of the EigenvalueSolver in JAMA,
+    // which has been released to public domain by The MathWorks and the
+    // National Institute of Standards and Technology (NIST).
+    EigenvalueDecomposition(InputArray src) {
+        compute(src);
+    }
+
+    // This function computes the Eigenvalue Decomposition for a general matrix
+    // given in src. This function is a port of the EigenvalueSolver in JAMA,
+    // which has been released to public domain by The MathWorks and the
+    // National Institute of Standards and Technology (NIST).
+    void compute(InputArray src)
+    {
+        /*if(isSymmetric(src)) {
+            // Fall back to OpenCV for a symmetric matrix!
+            cv::eigen(src, _eigenvalues, _eigenvectors);
+        } else {*/
+            Mat tmp;
+            // Convert the given input matrix to double. Is there any way to
+            // prevent allocating the temporary memory? Only used for copying
+            // into working memory and deallocated after.
+            src.getMat().convertTo(tmp, CV_64FC1);
+            // Get dimension of the matrix.
+            this->n = tmp.cols;
+            // Allocate the matrix data to work on.
+            this->H = alloc_2d<double> (n, n);
+            // Now safely copy the data.
+            for (int i = 0; i < tmp.rows; i++) {
+                for (int j = 0; j < tmp.cols; j++) {
+                    this->H[i][j] = tmp.at<double>(i, j);
+                }
+            }
+            // Deallocates the temporary matrix before computing.
+            tmp.release();
+            // Performs the eigenvalue decomposition of H.
+            compute();
+       // }
+    }
+
+    ~EigenvalueDecomposition() {}
+
+    // Returns the eigenvalues of the Eigenvalue Decomposition.
+    Mat eigenvalues() {  return _eigenvalues; }
+    // Returns the eigenvectors of the Eigenvalue Decomposition.
+    Mat eigenvectors() { return _eigenvectors; }
+};
+
+#endif // DLS_H
@@ -2,7 +2,10 @@
 #include "precomp.hpp"
 #include "epnp.h"

-epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints)
+namespace cv
+{
+
+epnp::epnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints)
 {
  if (cameraMatrix.depth() == CV_32F)
      init_camera_parameters<float>(cameraMatrix);
@@ -17,14 +20,14 @@ epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& i
  if (opoints.depth() == ipoints.depth())
  {
    if (opoints.depth() == CV_32F)
-      init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
+      init_points<Point3f,Point2f>(opoints, ipoints);
    else
-      init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
+      init_points<Point3d,Point2d>(opoints, ipoints);
  }
  else if (opoints.depth() == CV_32F)
-    init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
+    init_points<Point3f,Point2d>(opoints, ipoints);
  else
-    init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
+    init_points<Point3d,Point2f>(opoints, ipoints);

  alphas.resize(4 * number_of_correspondences);
  pcs.resize(3 * number_of_correspondences);
@@ -57,7 +60,7 @@ void epnp::choose_control_points(void)
  // Take C1, C2, and C3 from PCA on the reference points:
  CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);

-  double pw0tpw0[3 * 3], dc[3], uct[3 * 3];
+  double pw0tpw0[3 * 3] = {}, dc[3] = {}, uct[3 * 3] = {};
  CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
  CvMat DC      = cvMat(3, 1, CV_64F, dc);
  CvMat UCt     = cvMat(3, 3, CV_64F, uct);
@@ -80,7 +83,7 @@ void epnp::choose_control_points(void)

 void epnp::compute_barycentric_coordinates(void)
 {
-  double cc[3 * 3], cc_inv[3 * 3];
+  double cc[3 * 3] = {}, cc_inv[3 * 3] = {};
  CvMat CC     = cvMat(3, 3, CV_64F, cc);
  CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);

@@ -95,10 +98,12 @@ void epnp::compute_barycentric_coordinates(void)
    double * a = &alphas[0] + 4 * i;

    for(int j = 0; j < 3; j++)
+    {
      a[1 + j] =
-  ci[3 * j    ] * (pi[0] - cws[0][0]) +
-  ci[3 * j + 1] * (pi[1] - cws[0][1]) +
-  ci[3 * j + 2] * (pi[2] - cws[0][2]);
+          ci[3 * j    ] * (pi[0] - cws[0][0]) +
+          ci[3 * j + 1] * (pi[1] - cws[0][1]) +
+          ci[3 * j + 2] * (pi[2] - cws[0][2]);
+    }
    a[0] = 1.0f - a[1] - a[2] - a[3];
  }
 }
@@ -129,7 +134,7 @@ void epnp::compute_ccs(const double * betas, const double * ut)
    const double * v = ut + 12 * (11 - i);
    for(int j = 0; j < 4; j++)
      for(int k = 0; k < 3; k++)
-  ccs[j][k] += betas[i] * v[3 * j + k];
+        ccs[j][k] += betas[i] * v[3 * j + k];
  }
 }

@@ -144,7 +149,7 @@ void epnp::compute_pcs(void)
  }
 }

-void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
+void epnp::compute_pose(Mat& R, Mat& t)
 {
  choose_control_points();
  compute_barycentric_coordinates();
@@ -154,7 +159,7 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
  for(int i = 0; i < number_of_correspondences; i++)
    fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]);

-  double mtm[12 * 12], d[12], ut[12 * 12];
+  double mtm[12 * 12] = {}, d[12] = {}, ut[12 * 12] = {};
  CvMat MtM = cvMat(12, 12, CV_64F, mtm);
  CvMat D   = cvMat(12,  1, CV_64F, d);
  CvMat Ut  = cvMat(12, 12, CV_64F, ut);
@@ -163,15 +168,15 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
  cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
  cvReleaseMat(&M);

-  double l_6x10[6 * 10], rho[6];
+  double l_6x10[6 * 10] = {}, rho[6] = {};
  CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
  CvMat Rho    = cvMat(6,  1, CV_64F, rho);

  compute_L_6x10(ut, l_6x10);
  compute_rho(rho);

-  double Betas[4][4], rep_errors[4];
-  double Rs[4][3][3], ts[4][3];
+  double Betas[4][4] = {}, rep_errors[4] = {};
+  double Rs[4][3][3] = {}, ts[4][3] = {};

  find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
  gauss_newton(&L_6x10, &Rho, Betas[1]);
@@ -189,8 +194,8 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
  if (rep_errors[2] < rep_errors[1]) N = 2;
  if (rep_errors[3] < rep_errors[N]) N = 3;

-  cv::Mat(3, 1, CV_64F, ts[N]).copyTo(t);
-  cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
+  Mat(3, 1, CV_64F, ts[N]).copyTo(t);
+  Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
 }

 void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
@@ -218,7 +223,7 @@ double epnp::dot(const double * v1, const double * v2)

 void epnp::estimate_R_and_t(double R[3][3], double t[3])
 {
-  double pc0[3], pw0[3];
+  double pc0[3] = {}, pw0[3] = {};

  pc0[0] = pc0[1] = pc0[2] = 0.0;
  pw0[0] = pw0[1] = pw0[2] = 0.0;
@@ -237,7 +242,7 @@ void epnp::estimate_R_and_t(double R[3][3], double t[3])
    pw0[j] /= number_of_correspondences;
  }

-  double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
+  double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_v[3 * 3] = {};
  CvMat ABt   = cvMat(3, 3, CV_64F, abt);
  CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);
  CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);
@@ -281,7 +286,7 @@ void epnp::solve_for_sign(void)
  if (pcs[2] < 0.0) {
    for(int i = 0; i < 4; i++)
      for(int j = 0; j < 3; j++)
-  ccs[i][j] = -ccs[i][j];
+        ccs[i][j] = -ccs[i][j];

    for(int i = 0; i < number_of_correspondences; i++) {
      pcs[3 * i    ] = -pcs[3 * i];
@@ -329,7 +334,7 @@ double epnp::reprojection_error(const double R[3][3], const double t[3])
 void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
             double * betas)
 {
-  double l_6x4[6 * 4], b4[4];
+  double l_6x4[6 * 4] = {}, b4[4] = {};
  CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
  CvMat B4    = cvMat(4, 1, CV_64F, b4);

@@ -361,7 +366,7 @@ void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
 void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
             double * betas)
 {
-  double l_6x3[6 * 3], b3[3];
+  double l_6x3[6 * 3] = {}, b3[3] = {};
  CvMat L_6x3  = cvMat(6, 3, CV_64F, l_6x3);
  CvMat B3     = cvMat(3, 1, CV_64F, b3);

@@ -393,7 +398,7 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
 void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
             double * betas)
 {
-  double l_6x5[6 * 5], b5[5];
+  double l_6x5[6 * 5] = {}, b5[5] = {};
  CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
  CvMat B5    = cvMat(5, 1, CV_64F, b5);

@@ -428,7 +433,7 @@ void epnp::compute_L_6x10(const double * ut, double * l_6x10)
  v[2] = ut + 12 *  9;
  v[3] = ut + 12 *  8;

-  double dv[4][6][3];
+  double dv[4][6][3] = {};

  for(int i = 0; i < 4; i++) {
    int a = 0, b = 1;
@@ -439,8 +444,8 @@ void epnp::compute_L_6x10(const double * ut, double * l_6x10)

      b++;
      if (b > 3) {
-  a++;
-  b = a + 1;
+        a++;
+        b = a + 1;
      }
    }
  }
@@ -503,7 +508,7 @@ void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double betas[4]
 {
  const int iterations_number = 5;

-  double a[6*4], b[6], x[4];
+  double a[6*4] = {}, b[6] = {}, x[4] = {};
  CvMat A = cvMat(6, 4, CV_64F, a);
  CvMat B = cvMat(6, 1, CV_64F, b);
  CvMat X = cvMat(4, 1, CV_64F, x);
@@ -522,6 +527,8 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
 {
  const int nr = A->rows;
  const int nc = A->cols;
+  if (nc <= 0 || nr <= 0)
+      return;

  if (max_nr != 0 && max_nr < nr)
  {
@@ -621,3 +628,5 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
    pX[i] = (pb[i] - sum) / A2[i];
  }
 }
+
+}
@@ -4,6 +4,9 @@
 #include "precomp.hpp"
 #include "opencv2/core/core_c.h"

+namespace cv
+{
+
 class epnp {
 public:
  epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
@@ -14,6 +17,8 @@ class epnp {

  void compute_pose(cv::Mat& R, cv::Mat& t);
 private:
+  epnp(const epnp &); // copy disabled
+  epnp& operator=(const epnp &); // assign disabled
  template <typename T>
  void init_camera_parameters(const cv::Mat& cameraMatrix)
  {
@@ -27,12 +32,12 @@ class epnp {
  {
      for(int i = 0; i < number_of_correspondences; i++)
      {
-          pws[3 * i    ] = opoints.at<OpointType>(0,i).x;
-          pws[3 * i + 1] = opoints.at<OpointType>(0,i).y;
-          pws[3 * i + 2] = opoints.at<OpointType>(0,i).z;
+          pws[3 * i    ] = opoints.at<OpointType>(i).x;
+          pws[3 * i + 1] = opoints.at<OpointType>(i).y;
+          pws[3 * i + 2] = opoints.at<OpointType>(i).z;

-          us[2 * i    ] = ipoints.at<IpointType>(0,i).x*fu + uc;
-          us[2 * i + 1] = ipoints.at<IpointType>(0,i).y*fv + vc;
+          us[2 * i    ] = ipoints.at<IpointType>(i).x*fu + uc;
+          us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc;
      }
  }
  double reprojection_error(const double R[3][3], const double t[3]);
@@ -78,4 +83,6 @@ class epnp {
  double * A1, * A2;
 };

+}
+
 #endif
@@ -42,6 +42,7 @@

 #include "precomp.hpp"
 #include "fisheye.hpp"
+#include <limits>

 namespace cv { namespace
 {
@@ -53,7 +54,7 @@ namespace cv { namespace
        double dalpha;
    };

-    void subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows);
+    void subMatrix(const Mat& src, Mat& dst, const std::vector<uchar>& cols, const std::vector<uchar>& rows);
 }}

 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -62,12 +63,16 @@ namespace cv { namespace
 void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine,
    InputArray K, InputArray D, double alpha, OutputArray jacobian)
 {
+    CV_INSTRUMENT_REGION();
+
    projectPoints(objectPoints, imagePoints, affine.rvec(), affine.translation(), K, D, alpha, jacobian);
 }

 void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray _rvec,
        InputArray _tvec, InputArray _K, InputArray _D, double alpha, OutputArray jacobian)
 {
+    CV_INSTRUMENT_REGION();
+
    // will support only 3-channel data now for points
    CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
    imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2));
@@ -99,8 +104,9 @@ void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints

    Vec4d k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr<Vec4f>(): *_D.getMat().ptr<Vec4d>();

+    const bool isJacobianNeeded = jacobian.needed();
    JacobianRow *Jn = 0;
-    if (jacobian.needed())
+    if (isJacobianNeeded)
    {
        int nvars = 2 + 2 + 1 + 4 + 3 + 3; // f, c, alpha, k, om, T,
        jacobian.create(2*(int)n, nvars, CV_64F);
@@ -121,7 +127,8 @@ void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints
    {
        Vec3d Xi = objectPoints.depth() == CV_32F ? (Vec3d)Xf[i] : Xd[i];
        Vec3d Y = aff*Xi;
-
+        if (fabs(Y[2]) < DBL_MIN)
+            Y[2] = 1;
        Vec2d x(Y[0]/Y[2], Y[1]/Y[2]);

        double r2 = x.dot(x);
@@ -147,7 +154,7 @@ void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints
        else
            xpd[i] = final_point;

-        if (jacobian.needed())
+        if (isJacobianNeeded)
        {
            //Vec3d Xi = pdepth == CV_32F ? (Vec3d)Xf[i] : Xd[i];
            //Vec3d Y = aff*Xi;
@@ -249,6 +256,8 @@ void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints

 void cv::fisheye::distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha)
 {
+    CV_INSTRUMENT_REGION();
+
    // will support only 2-channel data now for points
    CV_Assert(undistorted.type() == CV_32FC2 || undistorted.type() == CV_64FC2);
    distorted.create(undistorted.size(), undistorted.type());
@@ -311,6 +320,8 @@ void cv::fisheye::distortPoints(InputArray undistorted, OutputArray distorted, I

 void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray R, InputArray P)
 {
+    CV_INSTRUMENT_REGION();
+
    // will support only 2-channel data now for points
    CV_Assert(distorted.type() == CV_32FC2 || distorted.type() == CV_64FC2);
    undistorted.create(distorted.size(), distorted.type());
@@ -369,14 +380,28 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted
        double scale = 1.0;

        double theta_d = sqrt(pw[0]*pw[0] + pw[1]*pw[1]);
+
+        // the current camera model is only valid up to 180 FOV
+        // for larger FOV the loop below does not converge
+        // clip values so we still get plausible results for super fisheye images > 180 grad
+        theta_d = min(max(-CV_PI/2., theta_d), CV_PI/2.);
+
        if (theta_d > 1e-8)
        {
            // compensate distortion iteratively
            double theta = theta_d;
-            for(int j = 0; j < 10; j++ )
+
+            const double EPS = 1e-8; // or std::numeric_limits<double>::epsilon();
+            for (int j = 0; j < 10; j++)
            {
                double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2;
-                theta = theta_d / (1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 + k[3] * theta8);
+                double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, k3_theta8 = k[3] * theta8;
+                /* new_theta = theta - theta_fix, theta_fix = f0(theta) / f0'(theta) */
+                double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) /
+                                   (1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8);
+                theta = theta - theta_fix;
+                if (fabs(theta_fix) < EPS)
+                    break;
            }

            scale = std::tan(theta) / theta_d;
@@ -401,12 +426,14 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted
 void cv::fisheye::initUndistortRectifyMap( InputArray K, InputArray D, InputArray R, InputArray P,
    const cv::Size& size, int m1type, OutputArray map1, OutputArray map2 )
 {
+    CV_INSTRUMENT_REGION();
+
    CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 );
    map1.create( size, m1type <= 0 ? CV_16SC2 : m1type );
    map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F );

    CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F));
-    CV_Assert((P.depth() == CV_32F || P.depth() == CV_64F) && (R.depth() == CV_32F || R.depth() == CV_64F));
+    CV_Assert((P.empty() || P.depth() == CV_32F || P.depth() == CV_64F) && (R.empty() || R.depth() == CV_32F || R.depth() == CV_64F));
    CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4));
    CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3);
    CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3));
@@ -458,17 +485,26 @@ void cv::fisheye::initUndistortRectifyMap( InputArray K, InputArray D, InputArra

        for( int j = 0; j < size.width; ++j)
        {
-            double x = _x/_w, y = _y/_w;
+            double u, v;
+            if( _w <= 0)
+            {
+                u = (_x > 0) ? -std::numeric_limits<double>::infinity() : std::numeric_limits<double>::infinity();
+                v = (_y > 0) ? -std::numeric_limits<double>::infinity() : std::numeric_limits<double>::infinity();
+            }
+            else
+            {
+                double x = _x/_w, y = _y/_w;

-            double r = sqrt(x*x + y*y);
-            double theta = atan(r);
+                double r = sqrt(x*x + y*y);
+                double theta = atan(r);

-            double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta4*theta4;
-            double theta_d = theta * (1 + k[0]*theta2 + k[1]*theta4 + k[2]*theta6 + k[3]*theta8);
+                double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta4*theta4;
+                double theta_d = theta * (1 + k[0]*theta2 + k[1]*theta4 + k[2]*theta6 + k[3]*theta8);

-            double scale = (r == 0) ? 1.0 : theta_d / r;
-            double u = f[0]*x*scale + c[0];
-            double v = f[1]*y*scale + c[1];
+                double scale = (r == 0) ? 1.0 : theta_d / r;
+                u = f[0]*x*scale + c[0];
+                v = f[1]*y*scale + c[1];
+            }

            if( m1type == CV_16SC2 )
            {
@@ -497,7 +533,9 @@ void cv::fisheye::initUndistortRectifyMap( InputArray K, InputArray D, InputArra
 void cv::fisheye::undistortImage(InputArray distorted, OutputArray undistorted,
        InputArray K, InputArray D, InputArray Knew, const Size& new_size)
 {
-    Size size = new_size.area() != 0 ? new_size : distorted.size();
+    CV_INSTRUMENT_REGION();
+
+    Size size = !new_size.empty() ? new_size : distorted.size();

    cv::Mat map1, map2;
    fisheye::initUndistortRectifyMap(K, D, cv::Matx33d::eye(), Knew, size, CV_16SC2, map1, map2 );
@@ -511,8 +549,10 @@ void cv::fisheye::undistortImage(InputArray distorted, OutputArray undistorted,
 void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R,
    OutputArray P, double balance, const Size& new_size, double fov_scale)
 {
+    CV_INSTRUMENT_REGION();
+
    CV_Assert( K.size() == Size(3, 3)       && (K.depth() == CV_32F || K.depth() == CV_64F));
-    CV_Assert((D.empty() || D.total() == 4) && (D.depth() == CV_32F || D.depth() == CV_64F || D.empty()));
+    CV_Assert(D.empty() || ((D.total() == 4) && (D.depth() == CV_32F || D.depth() == CV_64F)));

    int w = image_size.width, h = image_size.height;
    balance = std::min(std::max(balance, 0.0), 1.0);
@@ -524,20 +564,6 @@ void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, Input
    pptr[2] = Vec2d(w/2, h);
    pptr[3] = Vec2d(0, h/2);

-#if 0
-    const int N = 10;
-    cv::Mat points(1, N * 4, CV_64FC2);
-    Vec2d* pptr = points.ptr<Vec2d>();
-    for(int i = 0, k = 0; i < 10; ++i)
-    {
-        pptr[k++] = Vec2d(w/2,   0) - Vec2d(w/8,   0) + Vec2d(w/4/N*i,   0);
-        pptr[k++] = Vec2d(w/2, h-1) - Vec2d(w/8, h-1) + Vec2d(w/4/N*i, h-1);
-
-        pptr[k++] = Vec2d(0,   h/2) - Vec2d(0,   h/8) + Vec2d(0,   h/4/N*i);
-        pptr[k++] = Vec2d(w-1, h/2) - Vec2d(w-1, h/8) + Vec2d(w-1, h/4/N*i);
-    }
-#endif
-
    fisheye::undistortPoints(points, points, K, D, R);
    cv::Scalar center_mass = mean(points);
    cv::Vec2d cn(center_mass.val);
@@ -559,17 +585,6 @@ void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, Input
        maxx = std::max(maxx, pptr[i][0]);
    }

-#if 0
-    double minx = -DBL_MAX, miny = -DBL_MAX, maxx = DBL_MAX, maxy = DBL_MAX;
-    for(size_t i = 0; i < points.total(); ++i)
-    {
-        if (i % 4 == 0) miny = std::max(miny, pptr[i][1]);
-        if (i % 4 == 1) maxy = std::min(maxy, pptr[i][1]);
-        if (i % 4 == 2) minx = std::max(minx, pptr[i][0]);
-        if (i % 4 == 3) maxx = std::min(maxx, pptr[i][0]);
-    }
-#endif
-
    double f1 = w * 0.5/(cn[0] - minx);
    double f2 = w * 0.5/(maxx - cn[0]);
    double f3 = h * 0.5 * aspect_ratio/(cn[1] - miny);
@@ -587,7 +602,7 @@ void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, Input
    new_f[1] /= aspect_ratio;
    new_c[1] /= aspect_ratio;

-    if (new_size.area() > 0)
+    if (!new_size.empty())
    {
        double rx = new_size.width /(double)image_size.width;
        double ry = new_size.height/(double)image_size.height;
@@ -609,6 +624,8 @@ void cv::fisheye::stereoRectify( InputArray K1, InputArray D1, InputArray K2, In
        InputArray _R, InputArray _tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2,
        OutputArray Q, int flags, const Size& newImageSize, double balance, double fov_scale)
 {
+    CV_INSTRUMENT_REGION();
+
    CV_Assert((_R.size() == Size(3, 3) || _R.total() * _R.channels() == 3) && (_R.depth() == CV_32F || _R.depth() == CV_64F));
    CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F));

@@ -691,15 +708,17 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray
                                    InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
                                    int flags , cv::TermCriteria criteria)
 {
+    CV_INSTRUMENT_REGION();
+
    CV_Assert(!objectPoints.empty() && !imagePoints.empty() && objectPoints.total() == imagePoints.total());
    CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
    CV_Assert(imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2);
-    CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty());
-    CV_Assert((!D.empty() && D.total() == 4) || D.empty());
-    CV_Assert((!rvecs.empty() && rvecs.channels() == 3) || rvecs.empty());
-    CV_Assert((!tvecs.empty() && tvecs.channels() == 3) || tvecs.empty());
+    CV_Assert(K.empty() || (K.size() == Size(3,3)));
+    CV_Assert(D.empty() || (D.total() == 4));
+    CV_Assert(rvecs.empty() || (rvecs.channels() == 3));
+    CV_Assert(tvecs.empty() || (tvecs.channels() == 3));

-    CV_Assert(((flags & CALIB_USE_INTRINSIC_GUESS) && !K.empty() && !D.empty()) || !(flags & CALIB_USE_INTRINSIC_GUESS));
+    CV_Assert((!K.empty() && !D.empty()) || !(flags & CALIB_USE_INTRINSIC_GUESS));

    using namespace cv::internal;
    //-------------------------------Initialization
@@ -709,8 +728,8 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray

    finalParam.isEstimate[0] = 1;
    finalParam.isEstimate[1] = 1;
-    finalParam.isEstimate[2] = 1;
-    finalParam.isEstimate[3] = 1;
+    finalParam.isEstimate[2] = flags & CALIB_FIX_PRINCIPAL_POINT ? 0 : 1;
+    finalParam.isEstimate[3] = flags & CALIB_FIX_PRINCIPAL_POINT ? 0 : 1;
    finalParam.isEstimate[4] = flags & CALIB_FIX_SKEW ? 0 : 1;
    finalParam.isEstimate[5] = flags & CALIB_FIX_K1 ? 0 : 1;
    finalParam.isEstimate[6] = flags & CALIB_FIX_K2 ? 0 : 1;
@@ -753,7 +772,7 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray


    //-------------------------------Optimization
-    for(int iter = 0; ; ++iter)
+    for(int iter = 0; iter < std::numeric_limits<int>::max(); ++iter)
    {
        if ((criteria.type == 1 && iter >= criteria.maxCount)  ||
            (criteria.type == 2 && change <= criteria.epsilon) ||
@@ -762,12 +781,12 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray

        double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0);

-        Mat JJ2_inv, ex3;
-        ComputeJacobians(objectPoints, imagePoints, finalParam, omc, Tc, check_cond,thresh_cond, JJ2_inv, ex3);
+        Mat JJ2, ex3;
+        ComputeJacobians(objectPoints, imagePoints, finalParam, omc, Tc, check_cond,thresh_cond, JJ2, ex3);

-        Mat G =  alpha_smooth2 * JJ2_inv * ex3;
-
-        currentParam = finalParam + G;
+        Mat G;
+        solve(JJ2, ex3, G);
+        currentParam = finalParam + alpha_smooth2*G;

        change = norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]) -
                Vec4d(finalParam.f[0], finalParam.f[1], finalParam.c[0], finalParam.c[1]))
@@ -794,8 +813,29 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray

    if (K.needed()) cv::Mat(_K).convertTo(K, K.empty() ? CV_64FC1 : K.type());
    if (D.needed()) cv::Mat(finalParam.k).convertTo(D, D.empty() ? CV_64FC1 : D.type());
-    if (rvecs.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type());
-    if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type());
+    if (rvecs.isMatVector())
+    {
+        int N = (int)objectPoints.total();
+
+        if(rvecs.empty())
+            rvecs.create(N, 1, CV_64FC3);
+
+        if(tvecs.empty())
+            tvecs.create(N, 1, CV_64FC3);
+
+        for(int i = 0; i < N; i++ )
+        {
+            rvecs.create(3, 1, CV_64F, i, true);
+            tvecs.create(3, 1, CV_64F, i, true);
+            memcpy(rvecs.getMat(i).ptr(), omc[i].val, sizeof(Vec3d));
+            memcpy(tvecs.getMat(i).ptr(), Tc[i].val, sizeof(Vec3d));
+        }
+    }
+    else
+    {
+        if (rvecs.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type());
+        if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type());
+    }

    return rms;
 }
@@ -807,18 +847,20 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
                                    InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
                                    OutputArray R, OutputArray T, int flags, TermCriteria criteria)
 {
+    CV_INSTRUMENT_REGION();
+
    CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty());
    CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total());
    CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
    CV_Assert(imagePoints1.type() == CV_32FC2 || imagePoints1.type() == CV_64FC2);
    CV_Assert(imagePoints2.type() == CV_32FC2 || imagePoints2.type() == CV_64FC2);

-    CV_Assert((!K1.empty() && K1.size() == Size(3,3)) || K1.empty());
-    CV_Assert((!D1.empty() && D1.total() == 4) || D1.empty());
-    CV_Assert((!K2.empty() && K1.size() == Size(3,3)) || K2.empty());
-    CV_Assert((!D2.empty() && D1.total() == 4) || D2.empty());
+    CV_Assert(K1.empty() || (K1.size() == Size(3,3)));
+    CV_Assert(D1.empty() || (D1.total() == 4));
+    CV_Assert(K2.empty() || (K2.size() == Size(3,3)));
+    CV_Assert(D2.empty() || (D2.total() == 4));

-    CV_Assert(((flags & CALIB_FIX_INTRINSIC) && !K1.empty() && !K2.empty() && !D1.empty() && !D2.empty()) || !(flags & CALIB_FIX_INTRINSIC));
+    CV_Assert((!K1.empty() && !K2.empty() && !D1.empty() && !D2.empty()) || !(flags & CALIB_FIX_INTRINSIC));

    //-------------------------------Initialization

@@ -860,8 +902,8 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO

    if ((flags & CALIB_FIX_INTRINSIC))
    {
-        internal::CalibrateExtrinsics(objectPoints,  imagePoints1, intrinsicLeft, check_cond, thresh_cond, rvecs1, tvecs1);
-        internal::CalibrateExtrinsics(objectPoints,  imagePoints2, intrinsicRight, check_cond, thresh_cond, rvecs2, tvecs2);
+        cv::internal::CalibrateExtrinsics(objectPoints,  imagePoints1, intrinsicLeft, check_cond, thresh_cond, rvecs1, tvecs1);
+        cv::internal::CalibrateExtrinsics(objectPoints,  imagePoints2, intrinsicRight, check_cond, thresh_cond, rvecs2, tvecs2);
    }

    intrinsicLeft.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
@@ -887,8 +929,8 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
    intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate;
    intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate;

-    std::vector<int> selectedParams;
-    std::vector<int> tmp(6 * (n_images + 1), 1);
+    std::vector<uchar> selectedParams;
+    std::vector<uchar> tmp(6 * (n_images + 1), 1);
    selectedParams.insert(selectedParams.end(), intrinsicLeft.isEstimate.begin(), intrinsicLeft.isEstimate.end());
    selectedParams.insert(selectedParams.end(), intrinsicRight.isEstimate.begin(), intrinsicRight.isEstimate.end());
    selectedParams.insert(selectedParams.end(), tmp.begin(), tmp.end());
@@ -906,12 +948,11 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
        om_ref.reshape(3, 1).copyTo(om_list.col(image_idx));
        T_ref.reshape(3, 1).copyTo(T_list.col(image_idx));
    }
-    cv::Vec3d omcur = internal::median3d(om_list);
-    cv::Vec3d Tcur  = internal::median3d(T_list);
+    cv::Vec3d omcur = cv::internal::median3d(om_list);
+    cv::Vec3d Tcur  = cv::internal::median3d(T_list);

    cv::Mat J = cv::Mat::zeros(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1),
            e = cv::Mat::zeros(4 * n_points * n_images, 1, CV_64FC1), Jkk, ekk;
-    cv::Mat J2_inv;

    for(int iter = 0; ; ++iter)
    {
@@ -949,7 +990,7 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
            jacobians.col(14).copyTo(Jkk.col(4).rowRange(0, 2 * n_points));

            //right camera jacobian
-            internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT);
+            cv::internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT);
            rvec = cv::Mat(rvecs2[image_idx]);
            tvec  = cv::Mat(tvecs2[image_idx]);

@@ -988,14 +1029,15 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
        cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);

        //update all parameters
-        cv::subMatrix(J, J, selectedParams, std::vector<int>(J.rows, 1));
-        cv::Mat J2 = J.t() * J;
-        J2_inv = J2.inv();
+        cv::subMatrix(J, J, selectedParams, std::vector<uchar>(J.rows, 1));
        int a = cv::countNonZero(intrinsicLeft.isEstimate);
        int b = cv::countNonZero(intrinsicRight.isEstimate);
-        cv::Mat deltas = J2_inv * J.t() * e;
-        intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a);
-        intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b);
+        cv::Mat deltas;
+        solve(J.t() * J, J.t()*e, deltas);
+        if (a > 0)
+            intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a);
+        if (b > 0)
+            intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b);
        omcur = omcur + cv::Vec3d(deltas.rowRange(a + b, a + b + 3));
        Tcur = Tcur + cv::Vec3d(deltas.rowRange(a + b + 3, a + b + 6));
        for (int image_idx = 0; image_idx < n_images; ++image_idx)
@@ -1040,12 +1082,12 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
 }

 namespace cv{ namespace {
-void subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows)
+void subMatrix(const Mat& src, Mat& dst, const std::vector<uchar>& cols, const std::vector<uchar>& rows)
 {
-    CV_Assert(src.type() == CV_64FC1);
+    CV_Assert(src.channels() == 1);

    int nonzeros_cols = cv::countNonZero(cols);
-    Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
+    Mat tmp(src.rows, nonzeros_cols, CV_64F);

    for (int i = 0, j = 0; i < (int)cols.size(); i++)
    {
@@ -1056,16 +1098,14 @@ void subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std
    }

    int nonzeros_rows  = cv::countNonZero(rows);
-    Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1);
+    dst.create(nonzeros_rows, nonzeros_cols, CV_64F);
    for (int i = 0, j = 0; i < (int)rows.size(); i++)
    {
        if (rows[i])
        {
-            tmp.row(i).copyTo(tmp1.row(j++));
+            tmp.row(i).copyTo(dst.row(j++));
        }
    }
-
-    dst = tmp1.clone();
 }

 }}
@@ -1090,8 +1130,8 @@ cv::internal::IntrinsicParams cv::internal::IntrinsicParams::operator+(const Mat
    tmp.f[0]    = this->f[0]    + (isEstimate[0] ? ptr[j++] : 0);
    tmp.f[1]    = this->f[1]    + (isEstimate[1] ? ptr[j++] : 0);
    tmp.c[0]    = this->c[0]    + (isEstimate[2] ? ptr[j++] : 0);
-    tmp.alpha   = this->alpha   + (isEstimate[4] ? ptr[j++] : 0);
    tmp.c[1]    = this->c[1]    + (isEstimate[3] ? ptr[j++] : 0);
+    tmp.alpha   = this->alpha   + (isEstimate[4] ? ptr[j++] : 0);
    tmp.k[0]    = this->k[0]    + (isEstimate[5] ? ptr[j++] : 0);
    tmp.k[1]    = this->k[1]    + (isEstimate[6] ? ptr[j++] : 0);
    tmp.k[2]    = this->k[2]    + (isEstimate[7] ? ptr[j++] : 0);
@@ -1133,7 +1173,9 @@ void cv::internal::projectPoints(cv::InputArray objectPoints, cv::OutputArray im
                   cv::InputArray _rvec,cv::InputArray _tvec,
                   const IntrinsicParams& param, cv::OutputArray jacobian)
 {
-    CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
+    CV_INSTRUMENT_REGION();
+
+    CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
    Matx33d K(param.f[0], param.f[0] * param.alpha, param.c[0],
                       0,               param.f[1], param.c[1],
                       0,                        0,         1);
@@ -1146,6 +1188,7 @@ void cv::internal::ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& obj
 {
    CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
    CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
+    CV_Assert(rvec.total() > 2 && tvec.total() > 2);
    Vec6d extrinsics(rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2),
                    tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
    double change = 1;
@@ -1185,6 +1228,8 @@ void cv::internal::ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& obj

 cv::Mat cv::internal::ComputeHomography(Mat m, Mat M)
 {
+    CV_INSTRUMENT_REGION();
+
    int Np = m.cols;

    if (m.rows < 3)
@@ -1284,15 +1329,17 @@ cv::Mat cv::internal::ComputeHomography(Mat m, Mat M)

 cv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param)
 {
+    CV_INSTRUMENT_REGION();
+
    CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);

    Mat distorted((int)imagePoints.total(), 1, CV_64FC2), undistorted;
-    const Vec2d* ptr   = imagePoints.ptr<Vec2d>(0);
-    Vec2d* ptr_d = distorted.ptr<Vec2d>(0);
+    const Vec2d* ptr   = imagePoints.ptr<Vec2d>();
+    Vec2d* ptr_d = distorted.ptr<Vec2d>();
    for (size_t i = 0; i < imagePoints.total(); ++i)
    {
        ptr_d[i] = (ptr[i] - param.c).mul(Vec2d(1.0 / param.f[0], 1.0 / param.f[1]));
-        ptr_d[i][0] = ptr_d[i][0] - param.alpha * ptr_d[i][1];
+        ptr_d[i][0] -= param.alpha * ptr_d[i][1];
    }
    cv::fisheye::undistortPoints(distorted, undistorted, Matx33d::eye(), param.k);
    return undistorted;
@@ -1300,12 +1347,11 @@ cv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicPar

 void cv::internal::InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk)
 {
-
    CV_Assert(!_objectPoints.empty() && _objectPoints.type() == CV_64FC3);
    CV_Assert(!_imagePoints.empty() && _imagePoints.type() == CV_64FC2);

-    Mat imagePointsNormalized = NormalizePixels(_imagePoints.t(), param).reshape(1).t();
-    Mat objectPoints = Mat(_objectPoints.t()).reshape(1).t();
+    Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t();
+    Mat objectPoints = _objectPoints.reshape(1).t();
    Mat objectPointsMean, covObjectPoints;
    Mat Rckk;
    int Np = imagePointsNormalized.cols;
@@ -1322,9 +1368,13 @@ void cv::internal::InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoi
    double sc = .5 * (norm(H.col(0)) + norm(H.col(1)));
    H = H / sc;
    Mat u1 = H.col(0).clone();
-    u1  = u1 / norm(u1);
+    double norm_u1 = norm(u1);
+    CV_Assert(fabs(norm_u1) > 0);
+    u1  = u1 / norm_u1;
    Mat u2 = H.col(1).clone() - u1.dot(H.col(1).clone()) * u1;
-    u2 = u2 / norm(u2);
+    double norm_u2 = norm(u2);
+    CV_Assert(fabs(norm_u2) > 0);
+    u2 = u2 / norm_u2;
    Mat u3 = u1.cross(u2);
    Mat RRR;
    hconcat(u1, u2, RRR);
@@ -1358,23 +1408,26 @@ void cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArr
        objectPoints.getMat(image_idx).convertTo(object,  CV_64FC3);
        imagePoints.getMat (image_idx).convertTo(image, CV_64FC2);

-        InitExtrinsics(image, object, param, omckk, Tckk);
+        bool imT = image.rows < image.cols;
+        bool obT = object.rows < object.cols;

-        ComputeExtrinsicRefine(image, object, omckk, Tckk, JJ_kk, maxIter, param, thresh_cond);
+        InitExtrinsics(imT ? image.t() : image, obT ? object.t() : object, param, omckk, Tckk);
+
+        ComputeExtrinsicRefine(!imT ? image.t() : image, !obT ? object.t() : object, omckk, Tckk, JJ_kk, maxIter, param, thresh_cond);
        if (check_cond)
        {
            SVD svd(JJ_kk, SVD::NO_UV);
-            CV_Assert(svd.w.at<double>(0) / svd.w.at<double>((int)svd.w.total() - 1) < thresh_cond);
+            if(svd.w.at<double>(0) / svd.w.at<double>((int)svd.w.total() - 1) > thresh_cond )
+                CV_Error( cv::Error::StsInternal, format("CALIB_CHECK_COND - Ill-conditioned matrix for input array %d",image_idx));
        }
        omckk.reshape(3,1).copyTo(omc.getMat().col(image_idx));
        Tckk.reshape(3,1).copyTo(Tc.getMat().col(image_idx));
    }
 }

-
 void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
                      const IntrinsicParams& param,  InputArray omc, InputArray Tc,
-                      const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3)
+                      const int& check_cond, const double& thresh_cond, Mat& JJ2, Mat& ex3)
 {
    CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
    CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
@@ -1384,7 +1437,7 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO

    int n = (int)objectPoints.total();

-    Mat JJ3 = Mat::zeros(9 + 6 * n, 9 + 6 * n, CV_64FC1);
+    JJ2 = Mat::zeros(9 + 6 * n, 9 + 6 * n, CV_64FC1);
    ex3 = Mat::zeros(9 + 6 * n, 1, CV_64FC1 );

    for (int image_idx = 0; image_idx < n; ++image_idx)
@@ -1393,12 +1446,13 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO
        objectPoints.getMat(image_idx).convertTo(object, CV_64FC3);
        imagePoints.getMat (image_idx).convertTo(image, CV_64FC2);

+        bool imT = image.rows < image.cols;
        Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx));

        std::vector<Point2d> x;
        Mat jacobians;
        projectPoints(object, x, om, T, param, jacobians);
-        Mat exkk = image.t() - Mat(x);
+        Mat exkk = (imT ? image.t() : image) - Mat(x);

        Mat A(jacobians.rows, 9, CV_64FC1);
        jacobians.colRange(0, 4).copyTo(A.colRange(0, 4));
@@ -1410,16 +1464,14 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO
        Mat B = jacobians.colRange(8, 14).clone();
        B = B.t();

-        JJ3(Rect(0, 0, 9, 9)) = JJ3(Rect(0, 0, 9, 9)) + A * A.t();
-        JJ3(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t();
+        JJ2(Rect(0, 0, 9, 9)) += A * A.t();
+        JJ2(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t();

-        Mat AB = A * B.t();
-        AB.copyTo(JJ3(Rect(9 + 6 * image_idx, 0, 6, 9)));
+        JJ2(Rect(9 + 6 * image_idx, 0, 6, 9)) = A * B.t();
+        JJ2(Rect(0, 9 + 6 * image_idx, 9, 6)) = JJ2(Rect(9 + 6 * image_idx, 0, 6, 9)).t();

-        JJ3(Rect(0, 9 + 6 * image_idx, 9, 6)) = AB.t();
-        ex3(Rect(0,0,1,9)) = ex3(Rect(0,0,1,9)) + A * exkk.reshape(1, 2 * exkk.rows);
-
-        ex3(Rect(0, 9 + 6 * image_idx, 1, 6)) = B * exkk.reshape(1, 2 * exkk.rows);
+        ex3.rowRange(0, 9) += A * exkk.reshape(1, 2 * exkk.rows);
+        ex3.rowRange(9 + 6 * image_idx, 9 + 6 * (image_idx + 1)) = B * exkk.reshape(1, 2 * exkk.rows);

        if (check_cond)
        {
@@ -1429,12 +1481,11 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO
        }
    }

-    std::vector<int> idxs(param.isEstimate);
+    std::vector<uchar> idxs(param.isEstimate);
    idxs.insert(idxs.end(), 6 * n, 1);

-    subMatrix(JJ3, JJ3, idxs, idxs);
-    subMatrix(ex3, ex3, std::vector<int>(1, 1), idxs);
-    JJ2_inv = JJ3.inv();
+    subMatrix(JJ2, JJ2, idxs, idxs);
+    subMatrix(ex3, ex3, std::vector<uchar>(1, 1), idxs);
 }

 void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
@@ -1447,49 +1498,44 @@ void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputA
    CV_Assert(!omc.empty() && omc.type() == CV_64FC3);
    CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3);

-    Mat ex((int)(objectPoints.getMat(0).total() * objectPoints.total()), 1, CV_64FC2);
-
+    int total_ex = 0;
+    for (int image_idx = 0; image_idx < (int)objectPoints.total(); ++image_idx)
+    {
+        total_ex += (int)objectPoints.getMat(image_idx).total();
+    }
+    Mat ex(total_ex, 1, CV_64FC2);
+    int insert_idx = 0;
    for (int image_idx = 0; image_idx < (int)objectPoints.total(); ++image_idx)
    {
        Mat image, object;
        objectPoints.getMat(image_idx).convertTo(object, CV_64FC3);
        imagePoints.getMat (image_idx).convertTo(image, CV_64FC2);

+        bool imT = image.rows < image.cols;
+
        Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx));

        std::vector<Point2d> x;
        projectPoints(object, x, om, T, params, noArray());
-        Mat ex_ = image.t() - Mat(x);
-        ex_.copyTo(ex.rowRange(ex_.rows * image_idx,  ex_.rows * (image_idx + 1)));
+        Mat ex_ = (imT ? image.t() : image) - Mat(x);
+        ex_.copyTo(ex.rowRange(insert_idx, insert_idx + ex_.rows));
+        insert_idx += ex_.rows;
    }

    meanStdDev(ex, noArray(), std_err);
    std_err *= sqrt((double)ex.total()/((double)ex.total() - 1.0));

-    Mat sigma_x;
+    Vec<double, 1> sigma_x;
    meanStdDev(ex.reshape(1, 1), noArray(), sigma_x);
    sigma_x  *= sqrt(2.0 * (double)ex.total()/(2.0 * (double)ex.total() - 1.0));

-    Mat _JJ2_inv, ex3;
-    ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, _JJ2_inv, ex3);
+    Mat JJ2, ex3;
+    ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, JJ2, ex3);

-    Mat_<double>& JJ2_inv = (Mat_<double>&)_JJ2_inv;
+    sqrt(JJ2.inv(), JJ2);

-    sqrt(JJ2_inv, JJ2_inv);
-
-    double s  = sigma_x.at<double>(0);
-    Mat r = 3 * s * JJ2_inv.diag();
-    errors = r;
-
-    rms = 0;
-    const Vec2d* ptr_ex = ex.ptr<Vec2d>();
-    for (size_t i = 0; i < ex.total(); i++)
-    {
-        rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1];
-    }
-
-    rms /= (double)ex.total();
-    rms = sqrt(rms);
+    errors = 3 * sigma_x(0) * JJ2.diag();
+    rms = sqrt(norm(ex, NORM_L2SQR)/ex.total());
 }

 void cv::internal::dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB)
@@ -10,7 +10,7 @@ struct CV_EXPORTS IntrinsicParams
    Vec2d c;
    Vec4d k;
    double alpha;
-    std::vector<int> isEstimate;
+    std::vector<uchar> isEstimate;

    IntrinsicParams();
    IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0);
@@ -34,10 +34,10 @@
 namespace cv
 {

-class EMEstimatorCallback : public PointSetRegistrator::Callback
+class EMEstimatorCallback CV_FINAL : public PointSetRegistrator::Callback
 {
 public:
-    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
    {
        Mat q1 = _m1.getMat(), q2 = _m2.getMat();
        Mat Q1 = q1.reshape(1, (int)q1.total());
@@ -61,7 +61,7 @@ public:
        Mat EE = Mat(Vt.t()).colRange(5, 9) * 1.0;
        Mat A(10, 20, CV_64F);
        EE = EE.t();
-        getCoeffMat((double*)EE.data, (double*)A.data);
+        getCoeffMat(EE.ptr<double>(), A.ptr<double>());
        EE = EE.t();

        A = A.colRange(0, 10).inv() * A.colRange(10, 20);
@@ -137,7 +137,7 @@ public:
            cv::Mat Evec = EE.col(0) * xs.back() + EE.col(1) * ys.back() + EE.col(2) * zs.back() + EE.col(3);
            Evec /= norm(Evec);

-            memcpy(e + count * 9, Evec.data, 9 * sizeof(double));
+            memcpy(e + count * 9, Evec.ptr(), 9 * sizeof(double));
            count++;
        }

@@ -370,7 +370,7 @@ protected:
    }


-    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
    {
        Mat X1 = _m1.getMat(), X2 = _m2.getMat(), model = _model.getMat();
        const Point2d* x1ptr = X1.ptr<Point2d>();
@@ -402,37 +402,43 @@ protected:
 }

 // Input should be a vector of n 2D points or a Nx2 matrix
-cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, double focal, Point2d pp,
+cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
                              int method, double prob, double threshold, OutputArray _mask)
 {
-    Mat points1, points2;
+    CV_INSTRUMENT_REGION();
+
+    Mat points1, points2, cameraMatrix;
    _points1.getMat().convertTo(points1, CV_64F);
    _points2.getMat().convertTo(points2, CV_64F);
+    _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);

    int npoints = points1.checkVector(2);
-    CV_Assert( npoints >= 5 && points2.checkVector(2) == npoints &&
+    CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
                              points1.type() == points2.type());

-    if( points1.channels() > 1 )
+    CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
+
+    if (points1.channels() > 1)
    {
        points1 = points1.reshape(1, npoints);
        points2 = points2.reshape(1, npoints);
    }

-    double ifocal = focal != 0 ? 1./focal : 1.;
-    for( int i = 0; i < npoints; i++ )
-    {
-        points1.at<double>(i, 0) = (points1.at<double>(i, 0) - pp.x)*ifocal;
-        points1.at<double>(i, 1) = (points1.at<double>(i, 1) - pp.y)*ifocal;
-        points2.at<double>(i, 0) = (points2.at<double>(i, 0) - pp.x)*ifocal;
-        points2.at<double>(i, 1) = (points2.at<double>(i, 1) - pp.y)*ifocal;
-    }
+    double fx = cameraMatrix.at<double>(0,0);
+    double fy = cameraMatrix.at<double>(1,1);
+    double cx = cameraMatrix.at<double>(0,2);
+    double cy = cameraMatrix.at<double>(1,2);
+
+    points1.col(0) = (points1.col(0) - cx) / fx;
+    points2.col(0) = (points2.col(0) - cx) / fx;
+    points1.col(1) = (points1.col(1) - cy) / fy;
+    points2.col(1) = (points2.col(1) - cy) / fy;

    // Reshape data to fit opencv ransac function
    points1 = points1.reshape(2, npoints);
    points2 = points2.reshape(2, npoints);

-    threshold /= focal;
+    threshold /= (fx+fy)/2;

    Mat E;
    if( method == RANSAC )
@@ -443,29 +449,47 @@ cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, double f
    return E;
 }

-int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, OutputArray _R,
-                     OutputArray _t, double focal, Point2d pp, InputOutputArray _mask)
+cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, double focal, Point2d pp,
+                              int method, double prob, double threshold, OutputArray _mask)
 {
-    Mat points1, points2;
-    _points1.getMat().copyTo(points1);
-    _points2.getMat().copyTo(points2);
+    CV_INSTRUMENT_REGION();
+
+    Mat cameraMatrix = (Mat_<double>(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1);
+    return cv::findEssentialMat(_points1, _points2, cameraMatrix, method, prob, threshold, _mask);
+}
+
+int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2,
+                            InputArray _cameraMatrix, OutputArray _R, OutputArray _t, double distanceThresh,
+                     InputOutputArray _mask, OutputArray triangulatedPoints)
+{
+    CV_INSTRUMENT_REGION();
+
+    Mat points1, points2, cameraMatrix;
+    _points1.getMat().convertTo(points1, CV_64F);
+    _points2.getMat().convertTo(points2, CV_64F);
+    _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);

    int npoints = points1.checkVector(2);
    CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
                              points1.type() == points2.type());

+    CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
+
    if (points1.channels() > 1)
    {
        points1 = points1.reshape(1, npoints);
        points2 = points2.reshape(1, npoints);
    }
-    points1.convertTo(points1, CV_64F);
-    points2.convertTo(points2, CV_64F);

-    points1.col(0) = (points1.col(0) - pp.x) / focal;
-    points2.col(0) = (points2.col(0) - pp.x) / focal;
-    points1.col(1) = (points1.col(1) - pp.y) / focal;
-    points2.col(1) = (points2.col(1) - pp.y) / focal;
+    double fx = cameraMatrix.at<double>(0,0);
+    double fy = cameraMatrix.at<double>(1,1);
+    double cx = cameraMatrix.at<double>(0,2);
+    double cy = cameraMatrix.at<double>(1,2);
+
+    points1.col(0) = (points1.col(0) - cx) / fx;
+    points2.col(0) = (points2.col(0) - cx) / fx;
+    points1.col(1) = (points1.col(1) - cy) / fy;
+    points2.col(1) = (points2.col(1) - cy) / fy;

    points1 = points1.t();
    points2 = points2.t();
@@ -482,52 +506,61 @@ int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, Out
    // Do the cheirality check.
    // Notice here a threshold dist is used to filter
    // out far away points (i.e. infinite points) since
-    // there depth may vary between postive and negtive.
-    double dist = 50.0;
+    // their depth may vary between positive and negative.
+    std::vector<Mat> allTriangulations(4);
    Mat Q;
+
    triangulatePoints(P0, P1, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[0]);
    Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;
    Q.row(0) /= Q.row(3);
    Q.row(1) /= Q.row(3);
    Q.row(2) /= Q.row(3);
    Q.row(3) /= Q.row(3);
-    mask1 = (Q.row(2) < dist) & mask1;
+    mask1 = (Q.row(2) < distanceThresh) & mask1;
    Q = P1 * Q;
    mask1 = (Q.row(2) > 0) & mask1;
-    mask1 = (Q.row(2) < dist) & mask1;
+    mask1 = (Q.row(2) < distanceThresh) & mask1;

    triangulatePoints(P0, P2, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[1]);
    Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;
    Q.row(0) /= Q.row(3);
    Q.row(1) /= Q.row(3);
    Q.row(2) /= Q.row(3);
    Q.row(3) /= Q.row(3);
-    mask2 = (Q.row(2) < dist) & mask2;
+    mask2 = (Q.row(2) < distanceThresh) & mask2;
    Q = P2 * Q;
    mask2 = (Q.row(2) > 0) & mask2;
-    mask2 = (Q.row(2) < dist) & mask2;
+    mask2 = (Q.row(2) < distanceThresh) & mask2;

    triangulatePoints(P0, P3, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[2]);
    Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;
    Q.row(0) /= Q.row(3);
    Q.row(1) /= Q.row(3);
    Q.row(2) /= Q.row(3);
    Q.row(3) /= Q.row(3);
-    mask3 = (Q.row(2) < dist) & mask3;
+    mask3 = (Q.row(2) < distanceThresh) & mask3;
    Q = P3 * Q;
    mask3 = (Q.row(2) > 0) & mask3;
-    mask3 = (Q.row(2) < dist) & mask3;
+    mask3 = (Q.row(2) < distanceThresh) & mask3;

    triangulatePoints(P0, P4, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[3]);
    Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;
    Q.row(0) /= Q.row(3);
    Q.row(1) /= Q.row(3);
    Q.row(2) /= Q.row(3);
    Q.row(3) /= Q.row(3);
-    mask4 = (Q.row(2) < dist) & mask4;
+    mask4 = (Q.row(2) < distanceThresh) & mask4;
    Q = P4 * Q;
    mask4 = (Q.row(2) > 0) & mask4;
-    mask4 = (Q.row(2) < dist) & mask4;
+    mask4 = (Q.row(2) < distanceThresh) & mask4;

    mask1 = mask1.t();
    mask2 = mask2.t();
@@ -538,7 +571,8 @@ int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, Out
    if (!_mask.empty())
    {
        Mat mask = _mask.getMat();
-        CV_Assert(mask.size() == mask1.size());
+        CV_Assert(npoints == mask.checkVector(1));
+        mask = mask.reshape(1, npoints);
        bitwise_and(mask, mask1, mask1);
        bitwise_and(mask, mask2, mask2);
        bitwise_and(mask, mask3, mask3);
@@ -560,6 +594,7 @@ int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, Out

    if (good1 >= good2 && good1 >= good3 && good1 >= good4)
    {
+        if(triangulatedPoints.needed()) allTriangulations[0].copyTo(triangulatedPoints);
        R1.copyTo(_R);
        t.copyTo(_t);
        if (_mask.needed()) mask1.copyTo(_mask);
@@ -567,6 +602,7 @@ int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, Out
    }
    else if (good2 >= good1 && good2 >= good3 && good2 >= good4)
    {
+        if(triangulatedPoints.needed()) allTriangulations[1].copyTo(triangulatedPoints);
        R2.copyTo(_R);
        t.copyTo(_t);
        if (_mask.needed()) mask2.copyTo(_mask);
@@ -574,6 +610,7 @@ int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, Out
    }
    else if (good3 >= good1 && good3 >= good2 && good3 >= good4)
    {
+        if(triangulatedPoints.needed()) allTriangulations[2].copyTo(triangulatedPoints);
        t = -t;
        R1.copyTo(_R);
        t.copyTo(_t);
@@ -582,6 +619,7 @@ int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, Out
    }
    else
    {
+        if(triangulatedPoints.needed()) allTriangulations[3].copyTo(triangulatedPoints);
        t = -t;
        R2.copyTo(_R);
        t.copyTo(_t);
@@ -590,9 +628,23 @@ int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, Out
    }
 }

+int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
+                     OutputArray _R, OutputArray _t, InputOutputArray _mask)
+{
+    return cv::recoverPose(E, _points1, _points2, _cameraMatrix, _R, _t, 50, _mask);
+}
+
+int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, OutputArray _R,
+                     OutputArray _t, double focal, Point2d pp, InputOutputArray _mask)
+{
+    Mat cameraMatrix = (Mat_<double>(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1);
+    return cv::recoverPose(E, _points1, _points2, cameraMatrix, _R, _t, _mask);
+}

 void cv::decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t )
 {
+    CV_INSTRUMENT_REGION();
+
    Mat E = _E.getMat().reshape(1, 3);
    CV_Assert(E.cols == 3 && E.rows == 3);

@@ -41,52 +41,28 @@
 //M*/

 #include "precomp.hpp"
+#include "rho.h"
 #include <iostream>

 namespace cv
 {

-static bool haveCollinearPoints( const Mat& m, int count )
-{
-    int j, k, i = count-1;
-    const Point2f* ptr = m.ptr<Point2f>();
-
-    // check that the i-th selected point does not belong
-    // to a line connecting some previously selected points
-    for( j = 0; j < i; j++ )
-    {
-        double dx1 = ptr[j].x - ptr[i].x;
-        double dy1 = ptr[j].y - ptr[i].y;
-        for( k = 0; k < j; k++ )
-        {
-            double dx2 = ptr[k].x - ptr[i].x;
-            double dy2 = ptr[k].y - ptr[i].y;
-            if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
-                return true;
-        }
-    }
-    return false;
-}
-
-
-template<typename T> int compressPoints( T* ptr, const uchar* mask, int mstep, int count )
-{
-    int i, j;
-    for( i = j = 0; i < count; i++ )
-        if( mask[i*mstep] )
-        {
-            if( i > j )
-                ptr[j] = ptr[i];
-            j++;
-        }
-    return j;
-}
-
-
-class HomographyEstimatorCallback : public PointSetRegistrator::Callback
+/**
+ * This class estimates a homography \f$H\in \mathbb{R}^{3\times 3}\f$
+ * between \f$\mathbf{x} \in \mathbb{R}^3\f$ and
+ * \f$\mathbf{X} \in \mathbb{R}^3\f$ using DLT (direct linear transform)
+ * with algebraic distance.
+ *
+ * \f[
+ *   \lambda \mathbf{x} = H \mathbf{X}
+ * \f]
+ * where \f$\lambda \in \mathbb{R} \f$.
+ *
+ */
+class HomographyEstimatorCallback CV_FINAL : public PointSetRegistrator::Callback
 {
 public:
-    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const CV_OVERRIDE
    {
        Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
        if( haveCollinearPoints(ms1, count) || haveCollinearPoints(ms2, count) )
@@ -96,7 +72,7 @@ public:
        // are geometrically consistent. We check if every 3 correspondences sets
        // fulfills the constraint.
        //
-        // The usefullness of this constraint is explained in the paper:
+        // The usefulness of this constraint is explained in the paper:
        //
        // "Speeding-up homography estimation in mobile devices"
        // Journal of Real-Time Image Processing. 2013. DOI: 10.1007/s11554-012-0314-1
@@ -123,7 +99,21 @@ public:
        return true;
    }

-    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
+    /**
+     * Normalization method:
+     *  - $x$ and $y$ coordinates are normalized independently
+     *  - first the coordinates are shifted so that the average coordinate is \f$(0,0)\f$
+     *  - then the coordinates are scaled so that the average L1 norm is 1, i.e,
+     *  the average L1 norm of the \f$x\f$ coordinates is 1 and the average
+     *  L1 norm of the \f$y\f$ coordinates is also 1.
+     *
+     * @param _m1 source points containing (X,Y), depth is CV_32F with 1 column 2 channels or
+     *            2 columns 1 channel
+     * @param _m2 destination points containing (x,y), depth is CV_32F with 1 column 2 channels or
+     *            2 columns 1 channel
+     * @param _model, CV_64FC1, 3x3, normalized, i.e., the last element is 1
+     */
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
    {
        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
        int i, count = m1.checkVector(2);
@@ -190,7 +180,15 @@ public:
        return 1;
    }

-    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
+    /**
+     * Compute the reprojection error.
+     * m2 = H*m1
+     * @param _m1 depth CV_32F, 1-channel with 2 columns or 2-channel with 1 column
+     * @param _m2 depth CV_32F, 1-channel with 2 columns or 2-channel with 1 column
+     * @param _model CV_64FC1, 3x3
+     * @param _err, output, CV_32FC1, square of the L2 norm
+     */
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
    {
        Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
        int i, count = m1.checkVector(2);
@@ -207,13 +205,13 @@ public:
            float ww = 1.f/(Hf[6]*M[i].x + Hf[7]*M[i].y + 1.f);
            float dx = (Hf[0]*M[i].x + Hf[1]*M[i].y + Hf[2])*ww - m[i].x;
            float dy = (Hf[3]*M[i].x + Hf[4]*M[i].y + Hf[5])*ww - m[i].y;
-            err[i] = (float)(dx*dx + dy*dy);
+            err[i] = dx*dx + dy*dy;
        }
    }
 };


-class HomographyRefineCallback : public LMSolver::Callback
+class HomographyRefineCallback CV_FINAL : public LMSolver::Callback
 {
 public:
    HomographyRefineCallback(InputArray _src, InputArray _dst)
@@ -222,7 +220,7 @@ public:
        dst = _dst.getMat();
    }

-    bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const
+    bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
    {
        int i, count = src.checkVector(2);
        Mat param = _param.getMat();
@@ -269,15 +267,92 @@ public:

    Mat src, dst;
 };
+} // end namesapce cv

+namespace cv{
+static bool createAndRunRHORegistrator(double confidence,
+                                       int    maxIters,
+                                       double ransacReprojThreshold,
+                                       int    npoints,
+                                       InputArray  _src,
+                                       InputArray  _dst,
+                                       OutputArray _H,
+                                       OutputArray _tempMask){
+    Mat    src = _src.getMat();
+    Mat    dst = _dst.getMat();
+    Mat    tempMask;
+    bool   result;
+    double beta = 0.35;/* 0.35 is a value that often works. */
+
+    /* Create temporary output matrix (RHO outputs a single-precision H only). */
+    Mat tmpH = Mat(3, 3, CV_32FC1);
+
+    /* Create output mask. */
+    tempMask = Mat(npoints, 1, CV_8U);
+
+    /**
+     * Make use of the RHO estimator API.
+     *
+     * This is where the math happens. A homography estimation context is
+     * initialized, used, then finalized.
+     */
+
+    Ptr<RHO_HEST> p = rhoInit();
+
+    /**
+     * Optional. Ideally, the context would survive across calls to
+     * findHomography(), but no clean way appears to exit to do so. The price
+     * to pay is marginally more computational work than strictly needed.
+     */
+
+    rhoEnsureCapacity(p, npoints, beta);
+
+    /**
+     * The critical call. All parameters are heavily documented in rho.h.
+     *
+     * Currently, NR (Non-Randomness criterion) and Final Refinement (with
+     * internal, optimized Levenberg-Marquardt method) are enabled. However,
+     * while refinement seems to correctly smooth jitter most of the time, when
+     * refinement fails it tends to make the estimate visually very much worse.
+     * It may be necessary to remove the refinement flags in a future commit if
+     * this behaviour is too problematic.
+     */
+
+    result = !!rhoHest(p,
+                      (const float*)src.data,
+                      (const float*)dst.data,
+                      (char*)       tempMask.data,
+                      (unsigned)    npoints,
+                      (float)       ransacReprojThreshold,
+                      (unsigned)    maxIters,
+                      (unsigned)    maxIters,
+                      confidence,
+                      4U,
+                      beta,
+                      RHO_FLAG_ENABLE_NR | RHO_FLAG_ENABLE_FINAL_REFINEMENT,
+                      NULL,
+                      (float*)tmpH.data);
+
+    /* Convert float homography to double precision. */
+    tmpH.convertTo(_H, CV_64FC1);
+
+    /* Maps non-zero mask elements to 1, for the sake of the test case. */
+    for(int k=0;k<npoints;k++){
+        tempMask.data[k] = !!tempMask.data[k];
+    }
+    tempMask.copyTo(_tempMask);
+
+    return result;
+}
 }


 cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
-                            int method, double ransacReprojThreshold, OutputArray _mask )
+                            int method, double ransacReprojThreshold, OutputArray _mask,
+                            const int maxIters, const double confidence)
 {
-    const double confidence = 0.995;
-    const int maxIters = 2000;
+    CV_INSTRUMENT_REGION();
+
    const double defaultRANSACReprojThreshold = 3;
    bool result = false;

@@ -318,13 +393,15 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
        result = createRANSACPointSetRegistrator(cb, 4, ransacReprojThreshold, confidence, maxIters)->run(src, dst, H, tempMask);
    else if( method == LMEDS )
        result = createLMeDSPointSetRegistrator(cb, 4, confidence, maxIters)->run(src, dst, H, tempMask);
+    else if( method == RHO )
+        result = createAndRunRHORegistrator(confidence, maxIters, ransacReprojThreshold, npoints, src, dst, H, tempMask);
    else
        CV_Error(Error::StsBadArg, "Unknown estimation method");

-    if( result && npoints > 4 )
+    if( result && npoints > 4 && method != RHO)
    {
-        compressPoints( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
-        npoints = compressPoints( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
+        compressElems( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
+        npoints = compressElems( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
        if( npoints > 0 )
        {
            Mat src1 = src.rowRange(0, npoints);
@@ -344,7 +421,13 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
            tempMask.copyTo(_mask);
    }
    else
+    {
        H.release();
+        if(_mask.needed() ) {
+            tempMask = Mat::zeros(npoints >= 0 ? npoints : 0, 1, CV_8U);
+            tempMask.copyTo(_mask);
+        }
+    }

    return H;
 }
@@ -369,9 +452,31 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
 namespace cv
 {

+/**
+ * Compute the fundamental matrix using the 7-point algorithm.
+ *
+ * \f[
+ *  (\mathrm{m2}_i,1)^T \mathrm{fmatrix} (\mathrm{m1}_i,1) = 0
+ * \f]
+ *
+ * @param _m1 Contain points in the reference view. Depth CV_32F with 2-channel
+ *            1 column or 1-channel 2 columns. It has 7 rows.
+ * @param _m2 Contain points in the other view. Depth CV_32F with 2-channel
+ *            1 column or 1-channel 2 columns. It has 7 rows.
+ * @param _fmatrix Output fundamental matrix (or matrices) of type CV_64FC1.
+ *                 The user is responsible for allocating the memory before calling
+ *                 this function.
+ * @return Number of fundamental matrices. Valid values are 1, 2 or 3.
+ *  - 1, row 0 to row 2 in _fmatrix is a valid fundamental matrix
+ *  - 2, row 3 to row 5 in _fmatrix is a valid fundamental matrix
+ *  - 3, row 6 to row 8 in _fmatrix is a valid fundamental matrix
+ *
+ * Note that the computed fundamental matrix is normalized, i.e.,
+ * the last element \f$F_{33}\f$ is 1.
+ */
 static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
 {
-    double a[7*9], w[7], u[9*9], v[9*9], c[4], r[3];
+    double a[7*9], w[7], u[9*9], v[9*9], c[4], r[3] = {0};
    double* f1, *f2;
    double t0, t1, t2;
    Mat A( 7, 9, CV_64F, a );
@@ -385,12 +490,47 @@ static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
    double* fmatrix = _fmatrix.ptr<double>();
    int i, k, n;

+    Point2d m1c(0, 0), m2c(0, 0);
+    double t, scale1 = 0, scale2 = 0;
+    const int count = 7;
+
+    // compute centers and average distances for each of the two point sets
+    for( i = 0; i < count; i++ )
+    {
+        m1c += Point2d(m1[i]);
+        m2c += Point2d(m2[i]);
+    }
+
+    // calculate the normalizing transformations for each of the point sets:
+    // after the transformation each set will have the mass center at the coordinate origin
+    // and the average distance from the origin will be ~sqrt(2).
+    t = 1./count;
+    m1c *= t;
+    m2c *= t;
+
+    for( i = 0; i < count; i++ )
+    {
+        scale1 += norm(Point2d(m1[i].x - m1c.x, m1[i].y - m1c.y));
+        scale2 += norm(Point2d(m2[i].x - m2c.x, m2[i].y - m2c.y));
+    }
+
+    scale1 *= t;
+    scale2 *= t;
+
+    if( scale1 < FLT_EPSILON || scale2 < FLT_EPSILON )
+        return 0;
+
+    scale1 = std::sqrt(2.)/scale1;
+    scale2 = std::sqrt(2.)/scale2;
+
    // form a linear system: i-th row of A(=a) represents
    // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
    for( i = 0; i < 7; i++ )
    {
-        double x0 = m1[i].x, y0 = m1[i].y;
-        double x1 = m2[i].x, y1 = m2[i].y;
+        double x0 = (m1[i].x - m1c.x)*scale1;
+        double y0 = (m1[i].y - m1c.y)*scale1;
+        double x1 = (m2[i].x - m2c.x)*scale2;
+        double y1 = (m2[i].y - m2c.y)*scale2;

        a[i*9+0] = x1*x0;
        a[i*9+1] = x1*y0;
@@ -411,7 +551,7 @@ static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
    f1 = v + 7*9;
    f2 = v + 8*9;

-    // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
+    // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary fundamental matrix,
    // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
    // so f ~ lambda*f1 + (1 - lambda)*f2.
    // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
@@ -454,6 +594,10 @@ static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
    if( n < 1 || n > 3 )
        return n;

+    // transformation matrices
+    Matx33d T1( scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 );
+    Matx33d T2( scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 );
+
    for( k = 0; k < n; k++, fmatrix += 9 )
    {
        // for each root form the fundamental matrix
@@ -472,53 +616,66 @@ static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )

        for( i = 0; i < 8; i++ )
            fmatrix[i] = f1[i]*lambda + f2[i]*mu;
+
+        // de-normalize
+        Mat F(3, 3, CV_64F, fmatrix);
+        F = T2.t() * F * T1;
+
+        // make F(3,3) = 1
+        if(fabs(F.at<double>(8)) > FLT_EPSILON )
+            F *= 1. / F.at<double>(8);
    }

    return n;
 }

-
+/**
+ * Compute the fundamental matrix using the 8-point algorithm.
+ *
+ * \f[
+ *  (\mathrm{m2}_i,1)^T \mathrm{fmatrix} (\mathrm{m1}_i,1) = 0
+ * \f]
+ *
+ * @param _m1 Contain points in the reference view. Depth CV_32F with 2-channel
+ *            1 column or 1-channel 2 columns. It has 8 rows.
+ * @param _m2 Contain points in the other view. Depth CV_32F with 2-channel
+ *            1 column or 1-channel 2 columns. It has 8 rows.
+ * @param _fmatrix Output fundamental matrix (or matrices) of type CV_64FC1.
+ *                 The user is responsible for allocating the memory before calling
+ *                 this function.
+ * @return 1 on success, 0 on failure.
+ *
+ * Note that the computed fundamental matrix is normalized, i.e.,
+ * the last element \f$F_{33}\f$ is 1.
+ */
 static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
 {
-    double a[9*9], w[9], v[9*9];
-    Mat W( 9, 1, CV_64F, w );
-    Mat V( 9, 9, CV_64F, v );
-    Mat A( 9, 9, CV_64F, a );
-    Mat U, F0, TF;
-
    Point2d m1c(0,0), m2c(0,0);
    double t, scale1 = 0, scale2 = 0;

    const Point2f* m1 = _m1.ptr<Point2f>();
    const Point2f* m2 = _m2.ptr<Point2f>();
-    double* fmatrix = _fmatrix.ptr<double>();
    CV_Assert( (_m1.cols == 1 || _m1.rows == 1) && _m1.size() == _m2.size());
-    int i, j, k, count = _m1.checkVector(2);
+    int i, count = _m1.checkVector(2);

    // compute centers and average distances for each of the two point sets
    for( i = 0; i < count; i++ )
    {
-        double x = m1[i].x, y = m1[i].y;
-        m1c.x += x; m1c.y += y;
-
-        x = m2[i].x, y = m2[i].y;
-        m2c.x += x; m2c.y += y;
+        m1c += Point2d(m1[i]);
+        m2c += Point2d(m2[i]);
    }

    // calculate the normalizing transformations for each of the point sets:
    // after the transformation each set will have the mass center at the coordinate origin
    // and the average distance from the origin will be ~sqrt(2).
    t = 1./count;
-    m1c.x *= t; m1c.y *= t;
-    m2c.x *= t; m2c.y *= t;
+    m1c *= t;
+    m2c *= t;

    for( i = 0; i < count; i++ )
    {
-        double x = m1[i].x - m1c.x, y = m1[i].y - m1c.y;
-        scale1 += std::sqrt(x*x + y*y);
-
-        x = m2[i].x - m2c.x, y = m2[i].y - m2c.y;
-        scale2 += std::sqrt(x*x + y*y);
+        scale1 += norm(Point2d(m1[i].x - m1c.x, m1[i].y - m1c.y));
+        scale2 += norm(Point2d(m2[i].x - m2c.x, m2[i].y - m2c.y));
    }

    scale1 *= t;
@@ -530,7 +687,7 @@ static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
    scale1 = std::sqrt(2.)/scale1;
    scale2 = std::sqrt(2.)/scale2;

-    A.setTo(Scalar::all(0));
+    Matx<double, 9, 9> A;

    // form a linear system Ax=0: for each selected pair of points m1 & m2,
    // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
@@ -541,71 +698,65 @@ static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
        double y1 = (m1[i].y - m1c.y)*scale1;
        double x2 = (m2[i].x - m2c.x)*scale2;
        double y2 = (m2[i].y - m2c.y)*scale2;
-        double r[9] = { x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 };
-        for( j = 0; j < 9; j++ )
-            for( k = 0; k < 9; k++ )
-                a[j*9+k] += r[j]*r[k];
+        Vec<double, 9> r( x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 );
+        A += r*r.t();
    }

+    Vec<double, 9> W;
+    Matx<double, 9, 9> V;
+
    eigen(A, W, V);

    for( i = 0; i < 9; i++ )
    {
-        if( fabs(w[i]) < DBL_EPSILON )
+        if( fabs(W[i]) < DBL_EPSILON )
            break;
    }

    if( i < 8 )
        return 0;

-    F0 = Mat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
+    Matx33d F0( V.val + 9*8 ); // take the last column of v as a solution of Af = 0

    // make F0 singular (of rank 2) by decomposing it with SVD,
    // zeroing the last diagonal element of W and then composing the matrices back.

-    // use v as a temporary storage for different 3x3 matrices
-    W = U = V = TF = F0;
-    W = Mat(3, 1, CV_64F, v);
-    U = Mat(3, 3, CV_64F, v + 9);
-    V = Mat(3, 3, CV_64F, v + 18);
-    TF = Mat(3, 3, CV_64F, v + 27);
+    Vec3d w;
+    Matx33d U;
+    Matx33d Vt;

-    SVDecomp( F0, W, U, V, SVD::MODIFY_A );
-    W.at<double>(2) = 0.;
+    SVD::compute( F0, w, U, Vt);
+    w[2] = 0.;

-    // F0 <- U*diag([W(1), W(2), 0])*V'
-    gemm( U, Mat::diag(W), 1., 0, 0., TF, GEMM_1_T );
-    gemm( TF, V, 1., 0, 0., F0, 0/*CV_GEMM_B_T*/ );
+    F0 = U*Matx33d::diag(w)*Vt;

    // apply the transformation that is inverse
    // to what we used to normalize the point coordinates
-    double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
-    double tt2[] = { scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 };
-    Mat T1(3, 3, CV_64F, tt1), T2(3, 3, CV_64F, tt2);
+    Matx33d T1( scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 );
+    Matx33d T2( scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 );

-    // F0 <- T2'*F0*T1
-    gemm( T2, F0, 1., 0, 0., TF, GEMM_1_T );
-    F0 = Mat(3, 3, CV_64F, fmatrix);
-    gemm( TF, T1, 1., 0, 0., F0, 0 );
+    F0 = T2.t()*F0*T1;

    // make F(3,3) = 1
-    if( fabs(F0.at<double>(2,2)) > FLT_EPSILON )
-        F0 *= 1./F0.at<double>(2,2);
+    if( fabs(F0(2,2)) > FLT_EPSILON )
+        F0 *= 1./F0(2,2);
+
+    Mat(F0).copyTo(_fmatrix);

    return 1;
 }


-class FMEstimatorCallback : public PointSetRegistrator::Callback
+class FMEstimatorCallback CV_FINAL : public PointSetRegistrator::Callback
 {
 public:
-    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const CV_OVERRIDE
    {
        Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
        return !haveCollinearPoints(ms1, count) && !haveCollinearPoints(ms2, count);
    }

-    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
    {
        double f[9*3];
        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
@@ -621,7 +772,7 @@ public:
        return n;
    }

-    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
    {
        Mat __m1 = _m1.getMat(), __m2 = _m2.getMat(), __model = _model.getMat();
        int i, count = __m1.checkVector(2);
@@ -657,9 +808,11 @@ public:
 }

 cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
-                                int method, double param1, double param2,
-                                OutputArray _mask )
+                                int method, double ransacReprojThreshold, double confidence,
+                                int maxIters, OutputArray _mask )
 {
+    CV_INSTRUMENT_REGION();
+
    Mat points1 = _points1.getMat(), points2 = _points2.getMat();
    Mat m1, m2, F;
    int npoints = -1;
@@ -702,15 +855,15 @@ cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
    }
    else
    {
-        if( param1 <= 0 )
-            param1 = 3;
-        if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
-            param2 = 0.99;
+        if( ransacReprojThreshold <= 0 )
+            ransacReprojThreshold = 3;
+        if( confidence < DBL_EPSILON || confidence > 1 - DBL_EPSILON )
+            confidence = 0.99;

        if( (method & ~3) == FM_RANSAC && npoints >= 15 )
-            result = createRANSACPointSetRegistrator(cb, 7, param1, param2)->run(m1, m2, F, _mask);
+            result = createRANSACPointSetRegistrator(cb, 7, ransacReprojThreshold, confidence, maxIters)->run(m1, m2, F, _mask);
        else
-            result = createLMeDSPointSetRegistrator(cb, 7, param2)->run(m1, m2, F, _mask);
+            result = createLMeDSPointSetRegistrator(cb, 7, confidence)->run(m1, m2, F, _mask);
    }

    if( result <= 0 )
@@ -719,17 +872,26 @@ cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
    return F;
 }

-cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
-                               OutputArray _mask, int method, double param1, double param2 )
+cv::Mat cv::findFundamentalMat( cv::InputArray points1, cv::InputArray points2,
+                                     int method, double ransacReprojThreshold, double confidence,
+                                     cv::OutputArray mask )
 {
-    return cv::findFundamentalMat(_points1, _points2, method, param1, param2, _mask);
+    return cv::findFundamentalMat(points1, points2, method, ransacReprojThreshold, confidence, 1000, mask);
+}
+
+cv::Mat cv::findFundamentalMat( cv::InputArray points1, cv::InputArray points2, cv::OutputArray mask,
+                                int method, double ransacReprojThreshold, double confidence )
+{
+    return cv::findFundamentalMat(points1, points2, method, ransacReprojThreshold, confidence, 1000, mask);
 }


 void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
                                    InputArray _Fmat, OutputArray _lines )
 {
-    double f[9];
+    CV_INSTRUMENT_REGION();
+
+    double f[9] = {0};
    Mat tempF(3, 3, CV_64F, f);
    Mat points = _points.getMat(), F = _Fmat.getMat();

@@ -767,8 +929,8 @@ void cv::computeCorrespondEpilines( InputArray _points, int whichImage,

    if( depth == CV_32S || depth == CV_32F )
    {
-        const Point* ptsi = (const Point*)points.data;
-        const Point2f* ptsf = (const Point2f*)points.data;
+        const Point* ptsi = points.ptr<Point>();
+        const Point2f* ptsf = points.ptr<Point2f>();
        Point3f* dstf = lines.ptr<Point3f>();
        for( int i = 0; i < npoints; i++ )
        {
@@ -784,7 +946,7 @@ void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
    }
    else
    {
-        const Point2d* ptsd = (const Point2d*)points.data;
+        const Point2d* ptsd = points.ptr<Point2d>();
        Point3d* dstd = lines.ptr<Point3d>();
        for( int i = 0; i < npoints; i++ )
        {
@@ -800,8 +962,18 @@ void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
    }
 }

+static inline double scaleFor(double x){
+    return (std::fabs(x) > std::numeric_limits<float>::epsilon()) ? 1./x : 1.;
+}
+static inline float scaleFor(float x){
+    return (std::fabs(x) > std::numeric_limits<float>::epsilon()) ? 1.f/x : 1.f;
+}
+
+
 void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
 {
+    CV_INSTRUMENT_REGION();
+
    Mat src = _src.getMat();
    if( !src.isContinuous() )
        src = src.clone();
@@ -829,8 +1001,8 @@ void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
    {
        if( cn == 3 )
        {
-            const Point3i* sptr = (const Point3i*)src.data;
-            Point2f* dptr = (Point2f*)dst.data;
+            const Point3i* sptr = src.ptr<Point3i>();
+            Point2f* dptr = dst.ptr<Point2f>();
            for( i = 0; i < npoints; i++ )
            {
                float scale = sptr[i].z != 0 ? 1.f/sptr[i].z : 1.f;
@@ -839,8 +1011,8 @@ void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
        }
        else
        {
-            const Vec4i* sptr = (const Vec4i*)src.data;
-            Point3f* dptr = (Point3f*)dst.data;
+            const Vec4i* sptr = src.ptr<Vec4i>();
+            Point3f* dptr = dst.ptr<Point3f>();
            for( i = 0; i < npoints; i++ )
            {
                float scale = sptr[i][3] != 0 ? 1.f/sptr[i][3] : 1.f;
@@ -852,21 +1024,21 @@ void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
    {
        if( cn == 3 )
        {
-            const Point3f* sptr = (const Point3f*)src.data;
-            Point2f* dptr = (Point2f*)dst.data;
+            const Point3f* sptr = src.ptr<Point3f>();
+            Point2f* dptr = dst.ptr<Point2f>();
            for( i = 0; i < npoints; i++ )
            {
-                float scale = sptr[i].z != 0.f ? 1.f/sptr[i].z : 1.f;
+                float scale = scaleFor(sptr[i].z);
                dptr[i] = Point2f(sptr[i].x*scale, sptr[i].y*scale);
            }
        }
        else
        {
-            const Vec4f* sptr = (const Vec4f*)src.data;
-            Point3f* dptr = (Point3f*)dst.data;
+            const Vec4f* sptr = src.ptr<Vec4f>();
+            Point3f* dptr = dst.ptr<Point3f>();
            for( i = 0; i < npoints; i++ )
            {
-                float scale = sptr[i][3] != 0.f ? 1.f/sptr[i][3] : 1.f;
+                float scale = scaleFor(sptr[i][3]);
                dptr[i] = Point3f(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale);
            }
        }
@@ -875,21 +1047,21 @@ void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
    {
        if( cn == 3 )
        {
-            const Point3d* sptr = (const Point3d*)src.data;
-            Point2d* dptr = (Point2d*)dst.data;
+            const Point3d* sptr = src.ptr<Point3d>();
+            Point2d* dptr = dst.ptr<Point2d>();
            for( i = 0; i < npoints; i++ )
            {
-                double scale = sptr[i].z != 0. ? 1./sptr[i].z : 1.;
+                double scale = scaleFor(sptr[i].z);
                dptr[i] = Point2d(sptr[i].x*scale, sptr[i].y*scale);
            }
        }
        else
        {
-            const Vec4d* sptr = (const Vec4d*)src.data;
-            Point3d* dptr = (Point3d*)dst.data;
+            const Vec4d* sptr = src.ptr<Vec4d>();
+            Point3d* dptr = dst.ptr<Point3d>();
            for( i = 0; i < npoints; i++ )
            {
-                double scale = sptr[i][3] != 0.f ? 1./sptr[i][3] : 1.;
+                double scale = scaleFor(sptr[i][3]);
                dptr[i] = Point3d(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale);
            }
        }
@@ -901,6 +1073,8 @@ void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )

 void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
 {
+    CV_INSTRUMENT_REGION();
+
    Mat src = _src.getMat();
    if( !src.isContinuous() )
        src = src.clone();
@@ -913,7 +1087,7 @@ void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
    }
    CV_Assert( npoints >= 0 && (depth == CV_32S || depth == CV_32F || depth == CV_64F));

-    int dtype = CV_MAKETYPE(depth <= CV_32F ? CV_32F : CV_64F, cn+1);
+    int dtype = CV_MAKETYPE(depth, cn+1);
    _dst.create(npoints, 1, dtype);
    Mat dst = _dst.getMat();
    if( !dst.isContinuous() )
@@ -928,15 +1102,15 @@ void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
    {
        if( cn == 2 )
        {
-            const Point2i* sptr = (const Point2i*)src.data;
-            Point3i* dptr = (Point3i*)dst.data;
+            const Point2i* sptr = src.ptr<Point2i>();
+            Point3i* dptr = dst.ptr<Point3i>();
            for( i = 0; i < npoints; i++ )
                dptr[i] = Point3i(sptr[i].x, sptr[i].y, 1);
        }
        else
        {
-            const Point3i* sptr = (const Point3i*)src.data;
-            Vec4i* dptr = (Vec4i*)dst.data;
+            const Point3i* sptr = src.ptr<Point3i>();
+            Vec4i* dptr = dst.ptr<Vec4i>();
            for( i = 0; i < npoints; i++ )
                dptr[i] = Vec4i(sptr[i].x, sptr[i].y, sptr[i].z, 1);
        }
@@ -945,15 +1119,15 @@ void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
    {
        if( cn == 2 )
        {
-            const Point2f* sptr = (const Point2f*)src.data;
-            Point3f* dptr = (Point3f*)dst.data;
+            const Point2f* sptr = src.ptr<Point2f>();
+            Point3f* dptr = dst.ptr<Point3f>();
            for( i = 0; i < npoints; i++ )
                dptr[i] = Point3f(sptr[i].x, sptr[i].y, 1.f);
        }
        else
        {
-            const Point3f* sptr = (const Point3f*)src.data;
-            Vec4f* dptr = (Vec4f*)dst.data;
+            const Point3f* sptr = src.ptr<Point3f>();
+            Vec4f* dptr = dst.ptr<Vec4f>();
            for( i = 0; i < npoints; i++ )
                dptr[i] = Vec4f(sptr[i].x, sptr[i].y, sptr[i].z, 1.f);
        }
@@ -962,15 +1136,15 @@ void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
    {
        if( cn == 2 )
        {
-            const Point2d* sptr = (const Point2d*)src.data;
-            Point3d* dptr = (Point3d*)dst.data;
+            const Point2d* sptr = src.ptr<Point2d>();
+            Point3d* dptr = dst.ptr<Point3d>();
            for( i = 0; i < npoints; i++ )
                dptr[i] = Point3d(sptr[i].x, sptr[i].y, 1.);
        }
        else
        {
-            const Point3d* sptr = (const Point3d*)src.data;
-            Vec4d* dptr = (Vec4d*)dst.data;
+            const Point3d* sptr = src.ptr<Point3d>();
+            Vec4d* dptr = dst.ptr<Vec4d>();
            for( i = 0; i < npoints; i++ )
                dptr[i] = Vec4d(sptr[i].x, sptr[i].y, sptr[i].z, 1.);
        }
@@ -982,6 +1156,8 @@ void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )

 void cv::convertPointsHomogeneous( InputArray _src, OutputArray _dst )
 {
+    CV_INSTRUMENT_REGION();
+
    int stype = _src.type(), dtype = _dst.type();
    CV_Assert( _dst.fixedType() );

@@ -991,4 +1167,27 @@ void cv::convertPointsHomogeneous( InputArray _src, OutputArray _dst )
        convertPointsToHomogeneous(_src, _dst);
 }

+double cv::sampsonDistance(InputArray _pt1, InputArray _pt2, InputArray _F)
+{
+    CV_INSTRUMENT_REGION();
+
+    CV_Assert(_pt1.type() == CV_64F && _pt2.type() == CV_64F && _F.type() == CV_64F);
+    CV_DbgAssert(_pt1.rows() == 3 && _F.size() == Size(3, 3) && _pt1.rows() == _pt2.rows());
+
+    Mat pt1(_pt1.getMat());
+    Mat pt2(_pt2.getMat());
+    Mat F(_F.getMat());
+
+    Vec3d F_pt1 = *F.ptr<Matx33d>() * *pt1.ptr<Vec3d>();
+    Vec3d Ft_pt2 = F.ptr<Matx33d>()->t() * *pt2.ptr<Vec3d>();
+
+    double v = pt2.ptr<Vec3d>()->dot(F_pt1);
+
+    // square
+    Ft_pt2 = Ft_pt2.mul(Ft_pt2);
+    F_pt1 = F_pt1.mul(F_pt1);
+
+    return v*v / (F_pt1[0] + F_pt1[1] + Ft_pt2[0] + Ft_pt2[1]);
+}
+
 /* End of file. */
@@ -1,50 +1,51 @@
 /*M///////////////////////////////////////////////////////////////////////////////////////
- //
- // This is a homography decomposition implementation contributed to OpenCV
- // by Samson Yilma. It implements the homography decomposition algorithm
- // descriped in the research report:
- // Malis, E and Vargas, M, "Deeper understanding of the homography decomposition
- // for vision-based control", Research Report 6303, INRIA (2007)
- //
- //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
- //
- //  By downloading, copying, installing or using the software you agree to this license.
- //  If you do not agree to this license, do not download, install,
- //  copy or use the software.
- //
- //
- //                           License Agreement
- //                For Open Source Computer Vision Library
- //
- // Copyright (C) 2014, Samson Yilma¸ (samson_yilma@yahoo.com), all rights reserved.
- //
- // Third party copyrights are property of their respective owners.
- //
- // Redistribution and use in source and binary forms, with or without modification,
- // are permitted provided that the following conditions are met:
- //
- //   * Redistribution's of source code must retain the above copyright notice,
- //     this list of conditions and the following disclaimer.
- //
- //   * Redistribution's in binary form must reproduce the above copyright notice,
- //     this list of conditions and the following disclaimer in the documentation
- //     and/or other materials provided with the distribution.
- //
- //   * The name of the copyright holders may not be used to endorse or promote products
- //     derived from this software without specific prior written permission.
- //
- // This software is provided by the copyright holders and contributors "as is" and
- // any express or implied warranties, including, but not limited to, the implied
- // warranties of merchantability and fitness for a particular purpose are disclaimed.
- // In no event shall the Intel Corporation or contributors be liable for any direct,
- // indirect, incidental, special, exemplary, or consequential damages
- // (including, but not limited to, procurement of substitute goods or services;
- // loss of use, data, or profits; or business interruption) however caused
- // and on any theory of liability, whether in contract, strict liability,
- // or tort (including negligence or otherwise) arising in any way out of
- // the use of this software, even if advised of the possibility of such damage.
- //
- //M*/
+//
+// This is a homography decomposition implementation contributed to OpenCV
+// by Samson Yilma. It implements the homography decomposition algorithm
+// described in the research report:
+// Malis, E and Vargas, M, "Deeper understanding of the homography decomposition
+// for vision-based control", Research Report 6303, INRIA (2007)
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, Samson Yilma (samson_yilma@yahoo.com), all rights reserved.
+// Copyright (C) 2018, Intel Corporation, all rights reserved.
+//
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/

 #include "precomp.hpp"
 #include <memory>
@@ -84,30 +85,40 @@ protected:
    }

 private:
+    /**
+     * Normalize the homograhpy \f$H\f$.
+     *
+     * @param H Homography matrix.
+     * @param K Intrinsic parameter matrix.
+     * @return It returns
+     * \f[
+     *   K^{-1} * H * K
+     * \f]
+     */
    cv::Matx33d normalize(const cv::Matx33d& H, const cv::Matx33d& K);
    void removeScale();
    cv::Matx33d _Hnorm;
 };

-class HomographyDecompZhang : public HomographyDecomp {
+class HomographyDecompZhang CV_FINAL : public HomographyDecomp {

 public:
    HomographyDecompZhang():HomographyDecomp() {}
    virtual ~HomographyDecompZhang() {}

 private:
-    virtual void decompose(std::vector<CameraMotion>& camMotions);
+    virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE;
    bool findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion);
 };

-class HomographyDecompInria : public HomographyDecomp {
+class HomographyDecompInria CV_FINAL : public HomographyDecomp {

 public:
    HomographyDecompInria():HomographyDecomp() {}
    virtual ~HomographyDecompInria() {}

 private:
-    virtual void decompose(std::vector<CameraMotion>& camMotions);
+    virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE;
    double oppositeOfMinor(const cv::Matx33d& M, const int row, const int col);
    void findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R);
 };
@@ -174,6 +185,10 @@ bool HomographyDecompZhang::findMotionFrom_tstar_n(const cv::Vec3d& tstar, const
    temp(1, 1) += 1.0;
    temp(2, 2) += 1.0;
    motion.R = getHnorm() * temp.inv();
+    if (cv::determinant(motion.R) < 0)
+    {
+        motion.R *= -1;
+    }
    motion.t = motion.R * tstar;
    motion.n = n;
    return passesSameSideOfPlaneConstraint(motion);
@@ -183,6 +198,7 @@ void HomographyDecompZhang::decompose(std::vector<CameraMotion>& camMotions)
 {
    Mat W, U, Vt;
    SVD::compute(getHnorm(), W, U, Vt);
+    CV_Assert(W.total() > 2 && Vt.total() > 7);
    double lambda1=W.at<double>(0);
    double lambda3=W.at<double>(2);
    double lambda1m3 =  (lambda1-lambda3);
@@ -300,6 +316,10 @@ void HomographyDecompInria::findRmatFrom_tstar_n(const cv::Vec3d& tstar, const c
              0.0, 0.0, 1.0);

    R = getHnorm() * (I - (2/v) * tstar_m * n_m.t() );
+    if (cv::determinant(R) < 0)
+    {
+        R *= -1;
+    }
 }

 void HomographyDecompInria::decompose(std::vector<CameraMotion>& camMotions)
@@ -447,7 +467,7 @@ int decomposeHomographyMat(InputArray _H,
    Mat K = _K.getMat().reshape(1, 3);
    CV_Assert(K.cols == 3 && K.rows == 3);

-    auto_ptr<HomographyDecomp> hdecomp(new HomographyDecompInria);
+    cv::Ptr<HomographyDecomp> hdecomp(new HomographyDecompInria);

    vector<CameraMotion> motions;
    hdecomp->decomposeHomography(H, K, motions);
@@ -479,4 +499,67 @@ int decomposeHomographyMat(InputArray _H,
    return nsols;
 }

+void filterHomographyDecompByVisibleRefpoints(InputArrayOfArrays _rotations,
+                                              InputArrayOfArrays _normals,
+                                              InputArray _beforeRectifiedPoints,
+                                              InputArray _afterRectifiedPoints,
+                                              OutputArray _possibleSolutions,
+                                              InputArray _pointsMask)
+{
+    CV_Assert(_beforeRectifiedPoints.type() == CV_32FC2 && _afterRectifiedPoints.type() == CV_32FC2);
+    CV_Assert(_pointsMask.empty() || _pointsMask.type() == CV_8U);
+
+    Mat beforeRectifiedPoints = _beforeRectifiedPoints.getMat();
+    Mat afterRectifiedPoints = _afterRectifiedPoints.getMat();
+    Mat pointsMask = _pointsMask.getMat();
+    int nsolutions = (int)_rotations.total();
+    int npoints = (int)beforeRectifiedPoints.total();
+    CV_Assert(pointsMask.empty() || pointsMask.checkVector(1, CV_8U) == npoints);
+    const uchar* pointsMaskPtr = pointsMask.data;
+
+    std::vector<uchar> solutionMask(nsolutions, (uchar)1);
+    std::vector<Mat> normals(nsolutions);
+    std::vector<Mat> rotnorm(nsolutions);
+    Mat R;
+
+    for( int i = 0; i < nsolutions; i++ )
+    {
+        _normals.getMat(i).convertTo(normals[i], CV_64F);
+        CV_Assert(normals[i].total() == 3);
+        _rotations.getMat(i).convertTo(R, CV_64F);
+        rotnorm[i] = R*normals[i];
+        CV_Assert(rotnorm[i].total() == 3);
+    }
+
+    for( int j = 0; j < npoints; j++ )
+    {
+        if( !pointsMaskPtr || pointsMaskPtr[j] )
+        {
+            Point2f prevPoint = beforeRectifiedPoints.at<Point2f>(j);
+            Point2f currPoint = afterRectifiedPoints.at<Point2f>(j);
+
+            for( int i = 0; i < nsolutions; i++ )
+            {
+                if( !solutionMask[i] )
+                    continue;
+
+                const double* normal_i = normals[i].ptr<double>();
+                const double* rotnorm_i = rotnorm[i].ptr<double>();
+                double prevNormDot = normal_i[0]*prevPoint.x + normal_i[1]*prevPoint.y + normal_i[2];
+                double currNormDot = rotnorm_i[0]*currPoint.x + rotnorm_i[1]*currPoint.y + rotnorm_i[2];
+
+                if (prevNormDot <= 0 || currNormDot <= 0)
+                    solutionMask[i] = (uchar)0;
+            }
+        }
+    }
+
+    std::vector<int> possibleSolutions;
+    for( int i = 0; i < nsolutions; i++ )
+        if( solutionMask[i] )
+            possibleSolutions.push_back(i);
+
+    Mat(possibleSolutions).copyTo(_possibleSolutions);
+}
+
 } //namespace cv
@@ -0,0 +1,259 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+// This file is based on file issued with the following license:
+
+/*============================================================================
+
+Copyright 2017 Toby Collins
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this
+   list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef OPENCV_CALIB3D_IPPE_HPP
+#define OPENCV_CALIB3D_IPPE_HPP
+
+#include <opencv2/core.hpp>
+
+namespace cv {
+namespace IPPE {
+
+class PoseSolver {
+public:
+    /**
+     * @brief PoseSolver constructor
+     */
+    PoseSolver();
+
+    /**
+     * @brief                Finds the two possible poses of a planar object given a set of correspondences and their respective reprojection errors.
+     *                       The poses are sorted with the first having the lowest reprojection error.
+     * @param objectPoints   Array of 4 or more coplanar object points defined in object coordinates.
+     *                       1xN/Nx1 3-channel (float or double) where N is the number of points
+     * @param imagePoints    Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates.
+     * @param rvec1          First rotation solution (3x1 rotation vector)
+     * @param tvec1          First translation solution (3x1 vector)
+     * @param reprojErr1     Reprojection error of first solution
+     * @param rvec2          Second rotation solution (3x1 rotation vector)
+     * @param tvec2          Second translation solution (3x1 vector)
+     * @param reprojErr2     Reprojection error of second solution
+     */
+    void solveGeneric(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1,
+                      float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2);
+
+    /**
+     * @brief                   Finds the two possible poses of a square planar object and their respective reprojection errors using IPPE.
+     *                          The poses are sorted so that the first one is the one with the lowest reprojection error.
+     *
+     * @param objectPoints      Array of 4 coplanar object points defined in the following object coordinates:
+     *                            - point 0: [-squareLength / 2.0, squareLength / 2.0, 0]
+     *                            - point 1: [squareLength / 2.0, squareLength / 2.0, 0]
+     *                            - point 2: [squareLength / 2.0, -squareLength / 2.0, 0]
+     *                            - point 3: [-squareLength / 2.0, -squareLength / 2.0, 0]
+     *                          1xN/Nx1 3-channel (float or double) where N is the number of points
+     * @param imagePoints       Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates.
+     * @param rvec1             First rotation solution (3x1 rotation vector)
+     * @param tvec1             First translation solution (3x1 vector)
+     * @param reprojErr1        Reprojection error of first solution
+     * @param rvec2             Second rotation solution (3x1 rotation vector)
+     * @param tvec2             Second translation solution (3x1 vector)
+     * @param reprojErr2        Reprojection error of second solution
+     */
+    void solveSquare(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1,
+                     float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2);
+
+private:
+    /**
+     * @brief                         Finds the two possible poses of a planar object given a set of correspondences in normalized pixel coordinates.
+     *                                These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms.
+     * @param objectPoints            Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double).
+     * @param normalizedImagePoints   Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (float or double).
+     * @param Ma                      First pose solution (unsorted)
+     * @param Mb                      Second pose solution (unsorted)
+     */
+    void solveGeneric(InputArray objectPoints, InputArray normalizedImagePoints, OutputArray Ma, OutputArray Mb);
+
+    /**
+     * @brief                         Finds the two possible poses of a planar object in its canonical position, given a set of correspondences in normalized pixel coordinates.
+     *                                These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms.
+     * @param canonicalObjPoints      Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (double) where N is the number of points
+     * @param normalizedInputPoints   Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (double) where N is the number of points
+     * @param H                       Homography mapping canonicalObjPoints to normalizedInputPoints.
+     * @param Ma
+     * @param Mb
+     */
+    void solveCanonicalForm(InputArray canonicalObjPoints, InputArray normalizedInputPoints, const Matx33d& H,
+                            OutputArray Ma, OutputArray Mb);
+
+    /**
+     * @brief                           Computes the translation solution for a given rotation solution
+     * @param objectPoints              Array of corresponding object points, 1xN/Nx1 3-channel where N is the number of points
+     * @param normalizedImagePoints     Array of corresponding image points (undistorted), 1xN/Nx1 2-channel where N is the number of points
+     * @param R                         Rotation solution (3x1 rotation vector)
+     * @param t                         Translation solution (3x1 rotation vector)
+     */
+    void computeTranslation(InputArray objectPoints, InputArray normalizedImgPoints, InputArray R, OutputArray t);
+
+    /**
+     * @brief                           Computes the two rotation solutions from the Jacobian of a homography matrix H at a point (ux,uy) on the object plane.
+     *                                  For highest accuracy the Jacobian should be computed at the centroid of the point correspondences (see the IPPE paper for the explanation of this).
+     *                                  For a point (ux,uy) on the object plane, suppose the homography H maps (ux,uy) to a point (p,q) in the image (in normalized pixel coordinates).
+     *                                  The Jacobian matrix [J00, J01; J10,J11] is the Jacobian of the mapping evaluated at (ux,uy).
+     * @param j00                       Homography jacobian coefficient at (ux,uy)
+     * @param j01                       Homography jacobian coefficient at (ux,uy)
+     * @param j10                       Homography jacobian coefficient at (ux,uy)
+     * @param j11                       Homography jacobian coefficient at (ux,uy)
+     * @param p                         The x coordinate of point (ux,uy) mapped into the image (undistorted and normalized position)
+     * @param q                         The y coordinate of point (ux,uy) mapped into the image (undistorted and normalized position)
+    */
+    void computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2);
+
+    /**
+     * @brief                         Closed-form solution for the homography mapping with four corner correspondences of a square (it maps source points to target points).
+     *                                The source points are the four corners of a zero-centred squared defined by:
+     *                                  - point 0: [-squareLength / 2.0, squareLength / 2.0]
+     *                                  - point 1: [squareLength / 2.0, squareLength / 2.0]
+     *                                  - point 2: [squareLength / 2.0, -squareLength / 2.0]
+     *                                  - point 3: [-squareLength / 2.0, -squareLength / 2.0]
+     *
+     * @param targetPoints            Array of four corresponding target points, 1x4/4x1 2-channel. Note that the points should be ordered to correspond with points 0, 1, 2 and 3.
+     * @param halfLength              The square's half length (i.e. squareLength/2.0)
+     * @param H                       Homograhy mapping the source points to the target points, 3x3 single channel
+    */
+    void homographyFromSquarePoints(InputArray targetPoints, double halfLength, OutputArray H);
+
+    /**
+     * @brief                  Fast conversion from a rotation matrix to a rotation vector using Rodrigues' formula
+     * @param R                Input rotation matrix, 3x3 1-channel (double)
+     * @param r                Output rotation vector, 3x1/1x3 1-channel (double)
+     */
+    void rot2vec(InputArray R, OutputArray r);
+
+    /**
+     * @brief                         Takes a set of planar object points and transforms them to 'canonical' object coordinates This is when they have zero mean and are on the plane z=0
+     * @param objectPoints            Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
+     * @param canonicalObjectPoints   Object points in canonical coordinates 1xN/Nx1 2-channel (double)
+     * @param MobjectPoints2Canonical Transform matrix mapping _objectPoints to _canonicalObjectPoints: 4x4 1-channel (double)
+     */
+    void makeCanonicalObjectPoints(InputArray objectPoints, OutputArray canonicalObjectPoints, OutputArray MobjectPoints2Canonical);
+
+    /**
+     * @brief                         Evaluates the Root Mean Squared (RMS) reprojection error of a pose solution.
+     * @param objectPoints            Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
+     * @param imagePoints             Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates.
+     * @param M                       Pose matrix from 3D object to camera coordinates: 4x4 1-channel (double)
+     * @param err                     RMS reprojection error
+     */
+    void evalReprojError(InputArray objectPoints, InputArray imagePoints, InputArray M, float& err);
+
+    /**
+     * @brief                         Sorts two pose solutions according to their RMS reprojection error (lowest first).
+     * @param objectPoints            Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
+     * @param imagePoints             Array of corresponding image points, 1xN/Nx1 2-channel.  This can either be in pixel coordinates or normalized pixel coordinates.
+     * @param Ma                      Pose matrix 1: 4x4 1-channel
+     * @param Mb                      Pose matrix 2: 4x4 1-channel
+     * @param M1                      Member of (Ma,Mb} with lowest RMS reprojection error. Performs deep copy.
+     * @param M2                      Member of (Ma,Mb} with highest RMS reprojection error. Performs deep copy.
+     * @param err1                    RMS reprojection error of _M1
+     * @param err2                    RMS reprojection error of _M2
+     */
+    void sortPosesByReprojError(InputArray objectPoints, InputArray imagePoints, InputArray Ma, InputArray Mb, OutputArray M1, OutputArray M2, float& err1, float& err2);
+
+    /**
+     * @brief                         Finds the rotation _Ra that rotates a vector _a to the z axis (0,0,1)
+     * @param a                       vector: 3x1 mat (double)
+     * @param Ra                      Rotation: 3x3 mat (double)
+     */
+    void rotateVec2ZAxis(const Matx31d& a, Matx33d& Ra);
+
+    /**
+     * @brief                         Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points.
+     * @param objectPoints            Array of N>=3 coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
+     * @param R                       Rotation Mat: 3x3 (double)
+     * @return                        Success (true) or failure (false)
+     */
+    bool computeObjextSpaceR3Pts(InputArray objectPoints, Matx33d& R);
+
+    /**
+     * @brief computeObjextSpaceRSvD   Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points.
+     * @param objectPointsZeroMean     Zero-meaned coplanar object points: 3xN matrix (double) where N>=3
+     * @param R                        Rotation Mat: 3x3 (double)
+     */
+    void computeObjextSpaceRSvD(InputArray objectPointsZeroMean, OutputArray R);
+
+    /**
+     * @brief                   Generates the 4 object points of a square planar object
+     * @param squareLength      The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.)
+     * @param objectPoints      Set of 4 object points (1x4 3-channel double)
+     */
+    void generateSquareObjectCorners3D(double squareLength, OutputArray objectPoints);
+
+    /**
+     * @brief                   Generates the 4 object points of a square planar object, without including the z-component (which is z=0 for all points).
+     * @param squareLength      The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.)
+     * @param objectPoints      Set of 4 object points (1x4 2-channel double)
+     */
+    void generateSquareObjectCorners2D(double squareLength, OutputArray objectPoints);
+
+    /**
+     * @brief                   Computes the average depth of an object given its pose in camera coordinates
+     * @param objectPoints:     Object points defined in 3D object space
+     * @param rvec:             Rotation component of pose
+     * @param tvec:             Translation component of pose
+     * @return:                 average depth of the object
+     */
+    double meanSceneDepth(InputArray objectPoints, InputArray rvec, InputArray tvec);
+
+    //! a small constant used to test 'small' values close to zero.
+    double IPPE_SMALL;
+};
+} //namespace IPPE
+
+namespace HomographyHO {
+
+/**
+* @brief                   Computes the best-fitting homography matrix from source to target points using Harker and O'Leary's method:
+*                          Harker, M., O'Leary, P., Computation of Homographies, Proceedings of the British Machine Vision Conference 2005, Oxford, England.
+*                          This is not the author's implementation.
+* @param srcPoints         Array of source points: 1xN/Nx1 2-channel (float or double) where N is the number of points
+* @param targPoints        Array of target points: 1xN/Nx1 2-channel (float or double)
+* @param H                 Homography from source to target: 3x3 1-channel (double)
+*/
+void homographyHO(InputArray srcPoints, InputArray targPoints, Matx33d& H);
+
+/**
+* @brief                      Performs data normalization before homography estimation. For details see Hartley, R., Zisserman, A., Multiple View Geometry in Computer Vision,
+*                             Cambridge University Press, Cambridge, 2001
+* @param Data                 Array of source data points: 1xN/Nx1 2-channel (float or double) where N is the number of points
+* @param DataN                Normalized data points: 1xN/Nx1 2-channel (float or double) where N is the number of points
+* @param T                    Homogeneous transform from source to normalized: 3x3 1-channel (double)
+* @param Ti                   Homogeneous transform from normalized to source: 3x3 1-channel (double)
+*/
+void normalizeDataIsotropic(InputArray Data, OutputArray DataN, OutputArray T, OutputArray Ti);
+
+}
+} //namespace cv
+#endif
@@ -44,7 +44,7 @@
 #include <stdio.h>

 /*
-   This is translation to C++ of the Matlab's LMSolve package by Miroslav Balda.
+   This is a translation to C++ from the Matlab's LMSolve package by Miroslav Balda.
   Here is the original copyright:
   ============================================================================

@@ -77,19 +77,16 @@
 namespace cv
 {

-class LMSolverImpl : public LMSolver
+class LMSolverImpl CV_FINAL : public LMSolver
 {
 public:
-    LMSolverImpl() : maxIters(100) { init(); }
-    LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), maxIters(_maxIters) { init(); }
-
-    void init()
+    LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters, double _eps = FLT_EPSILON)
+        : cb(_cb), epsx(_eps), epsf(_eps), maxIters(_maxIters)
    {
-        epsx = epsf = FLT_EPSILON;
        printInterval = 0;
    }

-    int run(InputOutputArray _param0) const
+    int run(InputOutputArray _param0) const CV_OVERRIDE
    {
        Mat param0 = _param0.getMat(), x, xd, r, rd, J, A, Ap, v, temp_d, d;
        int ptype = param0.type();
@@ -198,9 +195,7 @@ public:
        return iter;
    }

-    void setCallback(const Ptr<LMSolver::Callback>& _cb) { cb = _cb; }
-
-    AlgorithmInfo* info() const;
+    void setCallback(const Ptr<LMSolver::Callback>& _cb) CV_OVERRIDE { cb = _cb; }

    Ptr<LMSolver::Callback> cb;

@@ -211,16 +206,14 @@ public:
 };


-CV_INIT_ALGORITHM(LMSolverImpl, "LMSolver",
-                  obj.info()->addParam(obj, "epsx", obj.epsx);
-                  obj.info()->addParam(obj, "epsf", obj.epsf);
-                  obj.info()->addParam(obj, "maxIters", obj.maxIters);
-                  obj.info()->addParam(obj, "printInterval", obj.printInterval))
-
 Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters)
 {
-    CV_Assert( !LMSolverImpl_info_auto.name().empty() );
    return makePtr<LMSolverImpl>(cb, maxIters);
 }

+Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)
+{
+    return makePtr<LMSolverImpl>(cb, maxIters, eps);
+}
+
 }
@@ -7,11 +7,12 @@
 //  copy or use the software.
 //
 //
-//                          License Agreement
+//                           License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Copyright (C) 2015, Itseez Inc., all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
@@ -40,15 +41,12 @@
 //
 //M*/

+//
+// Library initialization file
+//
+
 #include "precomp.hpp"
-#include "opencv2/video.hpp"

-namespace cv
-{
+IPP_INITIALIZER_AUTO

-bool initModule_video(void)
-{
-    return true;
-}
-
-}
+/* End of file. */
@@ -44,217 +44,251 @@
 ////////////////////////////////////////// stereoBM //////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////

-#ifdef csize
-
 #define MAX_VAL 32767

-void calcDisp(__local short * cost, __global short * disp, int uniquenessRatio, int mindisp, int ndisp, int w,
-              __local int * bestDisp, __local int * bestCost, int d, int x, int y, int cols, int rows, int wsz2)
+#ifndef WSZ
+#define WSZ     2
+#endif
+
+#define WSZ2    (WSZ / 2)
+
+#ifdef DEFINE_KERNEL_STEREOBM
+
+#define DISPARITY_SHIFT     4
+#define FILTERED            ((MIN_DISP - 1) << DISPARITY_SHIFT)
+
+void calcDisp(__local short * cost, __global short * disp, int uniquenessRatio,
+              __local int * bestDisp, __local int * bestCost, int d, int x, int y, int cols, int rows)
 {
-    short FILTERED = (mindisp - 1)<<4;
-    int best_disp = *bestDisp, best_cost = *bestCost, best_disp_back = ndisp - best_disp - 1;
+    int best_disp = *bestDisp, best_cost = *bestCost;
+    barrier(CLK_LOCAL_MEM_FENCE);

    short c = cost[0];
+    int thresh = best_cost + (best_cost * uniquenessRatio / 100);
+    bool notUniq = ( (c <= thresh) && (d < (best_disp - 1) || d > (best_disp + 1) ) );

-    int thresh = best_cost + (best_cost * uniquenessRatio/100);
-    bool notUniq = ( (c <= thresh) && (d < (best_disp_back - 1) || d > (best_disp_back + 1) ) );
-
-    if(notUniq)
+    if (notUniq)
        *bestCost = FILTERED;
    barrier(CLK_LOCAL_MEM_FENCE);

-    if( *bestCost != FILTERED && x < cols-wsz2-mindisp && y < rows-wsz2 && d == best_disp_back)
+    if( *bestCost != FILTERED && x < cols - WSZ2 - MIN_DISP && y < rows - WSZ2 && d == best_disp)
    {
-        int y3 = (best_disp_back > 0) ? cost[-w] : cost[w],
-            y2 = c,
-            y1 = (best_disp_back < ndisp-1) ? cost[w] : cost[-w];
-        int d_aprox = y3+y1-2*y2 + abs(y3-y1);
-        disp[0] = (short)(((best_disp_back + mindisp)*256 + (d_aprox != 0 ? (y3-y1)*256/d_aprox : 0) + 15) >> 4);
+        int d_aprox = 0;
+        int yp =0, yn = 0;
+        if ((0 < best_disp) && (best_disp < NUM_DISP - 1))
+        {
+            yp = cost[-2 * BLOCK_SIZE_Y];
+            yn = cost[2 * BLOCK_SIZE_Y];
+            d_aprox = yp + yn - 2 * c + abs(yp - yn);
+        }
+        disp[0] = (short)(((best_disp + MIN_DISP)*256 + (d_aprox != 0 ? (yp - yn) * 256 / d_aprox : 0) + 15) >> 4);
    }
 }

-int calcLocalIdx(int x, int y, int d, int w)
-{
-    return d*2*w + (w - 1 - y + x);
-}
-
-void calcNewCoordinates(int * x, int * y, int nthread)
-{
-    int oldX = *x - (1-nthread), oldY = *y;
-    *x = (oldX == oldY) ? (0*nthread + (oldX + 2)*(1-nthread) ) : (oldX+1)*(1-nthread) + (oldX+1)*nthread;
-    *y = (oldX == oldY) ? (0*(1-nthread) + (oldY + 1)*nthread) : oldY + 1*(1-nthread);
-}
-
 short calcCostBorder(__global const uchar * leftptr, __global const uchar * rightptr, int x, int y, int nthread,
-                     int wsz2, short * costbuf, int * h, int cols, int d, short cost, int winsize)
+                     short * costbuf, int *h, int cols, int d, short cost)
 {
-    int head = (*h)%wsz;
+    int head = (*h) % WSZ;
    __global const uchar * left, * right;
-    int idx = mad24(y+wsz2*(2*nthread-1), cols, x+wsz2*(1-2*nthread));
+    int idx = mad24(y + WSZ2 * (2 * nthread - 1), cols, x + WSZ2 * (1 - 2 * nthread));
    left = leftptr + idx;
    right = rightptr + (idx - d);
-    int shift = 1*nthread + cols*(1-nthread);

    short costdiff = 0;
-    for(int i = 0; i < winsize; i++)
+    if (0 == nthread)
    {
-        costdiff += abs( left[0] - right[0] );
-        left += shift;
-        right += shift;
+        #pragma unroll
+        for (int i = 0; i < WSZ; i++)
+        {
+            costdiff += abs( left[0] - right[0] );
+            left += cols;
+            right += cols;
+        }
+    }
+    else // (1 == nthread)
+    {
+        #pragma unroll
+        for (int i = 0; i < WSZ; i++)
+        {
+            costdiff += abs(left[i] - right[i]);
+        }
    }
    cost += costdiff - costbuf[head];
    costbuf[head] = costdiff;
-    (*h) = (*h)%wsz + 1;
+    *h = head + 1;
    return cost;
 }

 short calcCostInside(__global const uchar * leftptr, __global const uchar * rightptr, int x, int y,
-                     int wsz2, int cols, int d, short cost_up_left, short cost_up, short cost_left,
-                     int winsize)
+                     int cols, int d, short cost_up_left, short cost_up, short cost_left)
 {
    __global const uchar * left, * right;
-    int idx = mad24(y-wsz2-1, cols, x-wsz2-1);
+    int idx = mad24(y - WSZ2 - 1, cols, x - WSZ2 - 1);
    left = leftptr + idx;
    right = rightptr + (idx - d);
-    int idx2 = winsize*cols;
+    int idx2 = WSZ*cols;

    uchar corrner1 = abs(left[0] - right[0]),
-          corrner2 = abs(left[winsize] - right[winsize]),
+          corrner2 = abs(left[WSZ] - right[WSZ]),
          corrner3 = abs(left[idx2] - right[idx2]),
-          corrner4 = abs(left[idx2 + winsize] - right[idx2 + winsize]);
+          corrner4 = abs(left[idx2 + WSZ] - right[idx2 + WSZ]);

    return cost_up + cost_left - cost_up_left + corrner1 -
        corrner2 - corrner3 + corrner4;
 }

-__kernel void stereoBM(__global const uchar * leftptr, __global const uchar * rightptr, __global uchar * dispptr,
-                       int disp_step, int disp_offset, int rows, int cols, int mindisp, int ndisp,
-                       int preFilterCap, int textureTreshold, int uniquenessRatio, int sizeX, int sizeY, int winsize)
+__kernel void stereoBM(__global const uchar * leftptr,
+                       __global const uchar * rightptr,
+                       __global uchar * dispptr, int disp_step, int disp_offset,
+                       int rows, int cols,                                              // rows, cols of left and right images, not disp
+                       int textureThreshold, int uniquenessRatio)
 {
-    int gx = get_global_id(0)*sizeX;
-    int gy = get_global_id(1)*sizeY;
-    int lz = get_local_id(2);
+    int lz = get_local_id(0);
+    int gx = get_global_id(1) * BLOCK_SIZE_X;
+    int gy = get_global_id(2) * BLOCK_SIZE_Y;

-    int nthread = lz/ndisp;
-    int d = lz%ndisp;
-    int wsz2 = wsz/2;
+    int nthread = lz / NUM_DISP;
+    int disp_idx = lz % NUM_DISP;

    __global short * disp;
    __global const uchar * left, * right;

-    __local short costFunc[csize];
+    __local short costFunc[2 * BLOCK_SIZE_Y * NUM_DISP];
+
    __local short * cost;
    __local int best_disp[2];
    __local int best_cost[2];
    best_cost[nthread] = MAX_VAL;
-    best_disp[nthread] = MAX_VAL;
+    best_disp[nthread] = -1;
    barrier(CLK_LOCAL_MEM_FENCE);

-    short costbuf[wsz];
+    short costbuf[WSZ];
    int head = 0;

-    int shiftX = wsz2 + ndisp + mindisp - 1;
-    int shiftY = wsz2;
+    int shiftX = WSZ2 + NUM_DISP + MIN_DISP - 1;
+    int shiftY = WSZ2;

    int x = gx + shiftX, y = gy + shiftY, lx = 0, ly = 0;

-    int costIdx = calcLocalIdx(lx, ly, d, sizeY);
+    int costIdx = disp_idx * 2 * BLOCK_SIZE_Y + (BLOCK_SIZE_Y - 1);
    cost = costFunc + costIdx;

    int tempcost = 0;
-    if(x < cols-wsz2-mindisp && y < rows-wsz2)
+    if (x < cols - WSZ2 - MIN_DISP && y < rows - WSZ2)
    {
-        int shift = 1*nthread + cols*(1-nthread);
-        for(int i = 0; i < winsize; i++)
+        if (0 == nthread)
        {
-            int idx = mad24(y-wsz2+i*nthread, cols, x-wsz2+i*(1-nthread));
-            left = leftptr + idx;
-            right = rightptr + (idx - d);
-            short costdiff = 0;
-            for(int j = 0; j < winsize; j++)
+            #pragma unroll
+            for (int i = 0; i < WSZ; i++)
            {
-                costdiff += abs( left[0] - right[0] );
-                left += shift;
-                right += shift;
+                int idx = mad24(y - WSZ2, cols, x - WSZ2 + i);
+                left = leftptr + idx;
+                right = rightptr + (idx - disp_idx);
+                short costdiff = 0;
+                for(int j = 0; j < WSZ; j++)
+                {
+                    costdiff += abs( left[0] - right[0] );
+                    left += cols;
+                    right += cols;
+                }
+                costbuf[i] = costdiff;
            }
-            if(nthread==1)
+        }
+        else // (1 == nthread)
+        {
+            #pragma unroll
+            for (int i = 0; i < WSZ; i++)
            {
+                int idx = mad24(y - WSZ2 + i, cols, x - WSZ2);
+                left = leftptr + idx;
+                right = rightptr + (idx - disp_idx);
+                short costdiff = 0;
+                for (int j = 0; j < WSZ; j++)
+                {
+                    costdiff += abs( left[j] - right[j]);
+                }
                tempcost += costdiff;
+                costbuf[i] = costdiff;
            }
-            costbuf[head] = costdiff;
-            head++;
        }
    }
-    if(nthread==1)
+    if (nthread == 1)
    {
        cost[0] = tempcost;
-        atomic_min(best_cost+nthread, tempcost);
+        atomic_min(best_cost + 1, tempcost);
    }
    barrier(CLK_LOCAL_MEM_FENCE);

-    if(best_cost[1] == tempcost)
-        atomic_min(best_disp + 1, ndisp - d - 1);
+    if (best_cost[1] == tempcost)
+         atomic_max(best_disp + 1, disp_idx);
    barrier(CLK_LOCAL_MEM_FENCE);

-    int dispIdx = mad24(gy, disp_step, disp_offset + gx*(int)sizeof(short));
+    int dispIdx = mad24(gy, disp_step, mad24((int)sizeof(short), gx, disp_offset));
    disp = (__global short *)(dispptr + dispIdx);
-    calcDisp(cost, disp, uniquenessRatio, mindisp, ndisp, 2*sizeY,
-        best_disp + 1, best_cost+1, d, x, y, cols, rows, wsz2);
+    calcDisp(cost, disp, uniquenessRatio, best_disp + 1, best_cost + 1, disp_idx, x, y, cols, rows);
    barrier(CLK_LOCAL_MEM_FENCE);

    lx = 1 - nthread;
    ly = nthread;

-    for(int i = 0; i < sizeY*sizeX/2; i++)
+    for (int i = 0; i < BLOCK_SIZE_Y * BLOCK_SIZE_X / 2; i++)
    {
-        x = (lx < sizeX) ? gx + shiftX + lx : cols;
-        y = (ly < sizeY) ? gy + shiftY + ly : rows;
+        x = (lx < BLOCK_SIZE_X) ? gx + shiftX + lx : cols;
+        y = (ly < BLOCK_SIZE_Y) ? gy + shiftY + ly : rows;

        best_cost[nthread] = MAX_VAL;
-        best_disp[nthread] = MAX_VAL;
+        best_disp[nthread] = -1;
        barrier(CLK_LOCAL_MEM_FENCE);

-        costIdx = calcLocalIdx(lx, ly, d, sizeY);
+        costIdx = mad24(2 * BLOCK_SIZE_Y, disp_idx, (BLOCK_SIZE_Y - 1 - ly + lx));
+        if (0 > costIdx)
+            costIdx = BLOCK_SIZE_Y - 1;
        cost = costFunc + costIdx;
-
-        if(x < cols-wsz2-mindisp && y < rows-wsz2 )
+        if (x < cols - WSZ2 - MIN_DISP && y < rows - WSZ2)
        {
-            tempcost = ( ly*(1-nthread) + lx*nthread == 0 ) ?
-                calcCostBorder(leftptr, rightptr, x, y, nthread, wsz2, costbuf, &head, cols, d,
-                    cost[2*nthread-1], winsize) :
-                calcCostInside(leftptr, rightptr, x, y, wsz2, cols, d,
-                    cost[0], cost[1], cost[-1], winsize);
+            tempcost = (ly * (1 - nthread) + lx * nthread == 0) ?
+                calcCostBorder(leftptr, rightptr, x, y, nthread, costbuf, &head, cols, disp_idx, cost[2*nthread-1]) :
+                calcCostInside(leftptr, rightptr, x, y, cols, disp_idx, cost[0], cost[1], cost[-1]);
        }
        cost[0] = tempcost;
        atomic_min(best_cost + nthread, tempcost);
        barrier(CLK_LOCAL_MEM_FENCE);

-        if(best_cost[nthread] == tempcost)
-            atomic_min(best_disp + nthread, ndisp - d - 1);
+        if (best_cost[nthread] == tempcost)
+            atomic_max(best_disp + nthread, disp_idx);
        barrier(CLK_LOCAL_MEM_FENCE);

-        int dispIdx = mad24(gy+ly, disp_step, disp_offset + (gx+lx)*(int)sizeof(short));
+        dispIdx = mad24(gy + ly, disp_step, mad24((int)sizeof(short), (gx + lx), disp_offset));
        disp = (__global short *)(dispptr + dispIdx);
-        calcDisp(cost, disp, uniquenessRatio, mindisp, ndisp, 2*sizeY,
-            best_disp + nthread, best_cost + nthread, d, x, y, cols, rows, wsz2);
+        calcDisp(cost, disp, uniquenessRatio, best_disp + nthread, best_cost + nthread, disp_idx, x, y, cols, rows);
+
        barrier(CLK_LOCAL_MEM_FENCE);

-        calcNewCoordinates(&lx, &ly, nthread);
+        if (lx + nthread - 1 == ly)
+        {
+            lx = (lx + nthread + 1) * (1 - nthread);
+            ly = (ly + 1) * nthread;
+        }
+        else
+        {
+            lx += nthread;
+            ly = ly - nthread + 1;
+        }
    }
 }
-
-#endif
+#endif //DEFINE_KERNEL_STEREOBM

 //////////////////////////////////////////////////////////////////////////////////////////////////
 /////////////////////////////////////// Norm Prefiler ////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////

 __kernel void prefilter_norm(__global unsigned char *input, __global unsigned char *output,
-                               int rows, int cols, int prefilterCap, int winsize, int scale_g, int scale_s)
+                               int rows, int cols, int prefilterCap, int scale_g, int scale_s)
 {
+    // prefilterCap in range 1..63, checked in StereoBMImpl::compute
+
    int x = get_global_id(0);
    int y = get_global_id(1);
-    int wsz2 = winsize/2;

    if(x < cols && y < rows)
    {
@@ -262,13 +296,13 @@ __kernel void prefilter_norm(__global unsigned char *input, __global unsigned ch
                  input[y * cols + max(x-1,0)] * 1 + input[      y          * cols + x] * 4 + input[y * cols + min(x+1, cols-1)] * 1 +
                                                     input[min(y+1, rows-1) * cols + x] * 1;
        int cov2 = 0;
-        for(int i = -wsz2; i < wsz2+1; i++)
-            for(int j = -wsz2; j < wsz2+1; j++)
+        for(int i = -WSZ2; i < WSZ2+1; i++)
+            for(int j = -WSZ2; j < WSZ2+1; j++)
                cov2 += input[clamp(y+i, 0, rows-1) * cols + clamp(x+j, 0, cols-1)];

        int res = (cov1*scale_g - cov2*scale_s)>>10;
-        res = min(clamp(res, -prefilterCap, prefilterCap) + prefilterCap, 255);
-        output[y * cols + x] = res & 0xFF;
+        res = clamp(res, -prefilterCap, prefilterCap) + prefilterCap;
+        output[y * cols + x] = res;
    }
 }

@@ -280,20 +314,21 @@ __kernel void prefilter_norm(__global unsigned char *input, __global unsigned ch
 __kernel void prefilter_xsobel(__global unsigned char *input, __global unsigned char *output,
                               int rows, int cols, int prefilterCap)
 {
+    // prefilterCap in range 1..63, checked in StereoBMImpl::compute
    int x = get_global_id(0);
    int y = get_global_id(1);
    if(x < cols && y < rows)
    {
-            output[y * cols + x] = min(prefilterCap, 255) & 0xFF;
-    }
+        if (0 < x && !((y == rows-1) & (rows%2==1) ) )
+        {
+            int cov = input[ ((y > 0) ? y-1 : y+1)  * cols + (x-1)] * (-1) + input[ ((y > 0) ? y-1 : y+1)  * cols + ((x<cols-1) ? x+1 : x-1)] * (1) +
+                      input[              (y)       * cols + (x-1)] * (-2) + input[        (y)             * cols + ((x<cols-1) ? x+1 : x-1)] * (2) +
+                      input[((y<rows-1)?(y+1):(y-1))* cols + (x-1)] * (-1) + input[((y<rows-1)?(y+1):(y-1))* cols + ((x<cols-1) ? x+1 : x-1)] * (1);

-    if(x < cols && y < rows && x > 0 && !((y == rows-1)&(rows%2==1) ) )
-    {
-        int cov = input[ ((y > 0) ? y-1 : y+1)  * cols + (x-1)] * (-1) + input[ ((y > 0) ? y-1 : y+1)  * cols + ((x<cols-1) ? x+1 : x-1)] * (1) +
-                  input[              (y)       * cols + (x-1)] * (-2) + input[        (y)             * cols + ((x<cols-1) ? x+1 : x-1)] * (2) +
-                  input[((y<rows-1)?(y+1):(y-1))* cols + (x-1)] * (-1) + input[((y<rows-1)?(y+1):(y-1))* cols + ((x<cols-1) ? x+1 : x-1)] * (1);
-
-        cov = min(clamp(cov, -prefilterCap, prefilterCap) + prefilterCap, 255);
-        output[y * cols + x] = cov & 0xFF;
+            cov = clamp(cov, -prefilterCap, prefilterCap) + prefilterCap;
+            output[y * cols + x] = cov;
+        }
+        else
+            output[y * cols + x] = prefilterCap;
    }
-}
+}
@@ -33,7 +33,9 @@ p3p::p3p(double _fx, double _fy, double _cx, double _cy)

 bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints)
 {
-    double rotation_matrix[3][3], translation[3];
+    CV_INSTRUMENT_REGION();
+
+    double rotation_matrix[3][3] = {}, translation[3] = {};
    std::vector<double> points;
    if (opoints.depth() == ipoints.depth())
    {
@@ -47,55 +49,83 @@ bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat
    else
        extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);

-    bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
-          points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14],
-          points[15], points[16], points[17], points[18], points[19]);
+    bool result = solve(rotation_matrix, translation,
+                        points[0], points[1], points[2], points[3], points[4],
+                        points[5], points[6], points[7], points[8], points[9],
+                        points[10], points[11], points[12], points[13], points[14],
+                        points[15], points[16], points[17], points[18], points[19]);
    cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
    cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
    return result;
 }

+int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints)
+{
+    CV_INSTRUMENT_REGION();
+
+    double rotation_matrix[4][3][3] = {}, translation[4][3] = {};
+    std::vector<double> points;
+    if (opoints.depth() == ipoints.depth())
+    {
+        if (opoints.depth() == CV_32F)
+            extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
+        else
+            extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
+    }
+    else if (opoints.depth() == CV_32F)
+        extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
+    else
+        extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
+
+    const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
+    int solutions = solve(rotation_matrix, translation,
+                          points[0], points[1], points[2], points[3], points[4],
+                          points[5], points[6], points[7], points[8], points[9],
+                          points[10], points[11], points[12], points[13], points[14],
+                          points[15], points[16], points[17], points[18], points[19],
+                          p4p);
+
+    for (int i = 0; i < solutions; i++) {
+        cv::Mat R, tvec;
+        cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
+        cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
+
+        Rs.push_back(R);
+        tvecs.push_back(tvec);
+    }
+
+    return solutions;
+}
+
 bool p3p::solve(double R[3][3], double t[3],
    double mu0, double mv0,   double X0, double Y0, double Z0,
    double mu1, double mv1,   double X1, double Y1, double Z1,
    double mu2, double mv2,   double X2, double Y2, double Z2,
    double mu3, double mv3,   double X3, double Y3, double Z3)
 {
-    double Rs[4][3][3], ts[4][3];
+    double Rs[4][3][3] = {}, ts[4][3] = {};

-    int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0,  mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
+    const bool p4p = true;
+    int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0,  mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);

    if (n == 0)
        return false;

-    int ns = 0;
-    double min_reproj = 0;
-    for(int i = 0; i < n; i++) {
-        double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
-        double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
-        double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
-        double mu3p = cx + fx * X3p / Z3p;
-        double mv3p = cy + fy * Y3p / Z3p;
-        double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
-        if (i == 0 || min_reproj > reproj) {
-            ns = i;
-            min_reproj = reproj;
-        }
-    }
-
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++)
-            R[i][j] = Rs[ns][i][j];
-        t[i] = ts[ns][i];
+            R[i][j] = Rs[0][i][j];
+        t[i] = ts[0][i];
    }

    return true;
 }

 int p3p::solve(double R[4][3][3], double t[4][3],
-    double mu0, double mv0,   double X0, double Y0, double Z0,
-    double mu1, double mv1,   double X1, double Y1, double Z1,
-    double mu2, double mv2,   double X2, double Y2, double Z2)
+               double mu0, double mv0,   double X0, double Y0, double Z0,
+               double mu1, double mv1,   double X1, double Y1, double Z1,
+               double mu2, double mv2,   double X2, double Y2, double Z2,
+               double mu3, double mv3,   double X3, double Y3, double Z3,
+               bool p4p)
 {
    double mk0, mk1, mk2;
    double norm;
@@ -115,6 +145,9 @@ int p3p::solve(double R[4][3][3], double t[4][3],
    norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
    mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;

+    mu3 = inv_fx * mu3 - cx_fx;
+    mv3 = inv_fy * mv3 - cy_fy;
+
    double distances[3];
    distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
    distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
@@ -126,10 +159,11 @@ int p3p::solve(double R[4][3][3], double t[4][3],
    cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
    cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;

-    double lengths[4][3];
+    double lengths[4][3] = {};
    int n = solve_for_lengths(lengths, distances, cosines);

    int nb_solutions = 0;
+    double reproj_errors[4];
    for(int i = 0; i < n; i++) {
        double M_orig[3][3];

@@ -148,14 +182,34 @@ int p3p::solve(double R[4][3][3], double t[4][3],
        if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
            continue;

+        if (p4p) {
+            double X3p = R[nb_solutions][0][0] * X3 + R[nb_solutions][0][1] * Y3 + R[nb_solutions][0][2] * Z3 + t[nb_solutions][0];
+            double Y3p = R[nb_solutions][1][0] * X3 + R[nb_solutions][1][1] * Y3 + R[nb_solutions][1][2] * Z3 + t[nb_solutions][1];
+            double Z3p = R[nb_solutions][2][0] * X3 + R[nb_solutions][2][1] * Y3 + R[nb_solutions][2][2] * Z3 + t[nb_solutions][2];
+            double mu3p = X3p / Z3p;
+            double mv3p = Y3p / Z3p;
+            reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
+        }
+
        nb_solutions++;
    }

+    if (p4p) {
+        //sort the solutions
+        for (int i = 1; i < nb_solutions; i++) {
+            for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) {
+                std::swap(reproj_errors[j], reproj_errors[j-1]);
+                std::swap(R[j], R[j-1]);
+                std::swap(t[j], t[j-1]);
+            }
+        }
+    }
+
    return nb_solutions;
 }

 /// Given 3D distances between three points and cosines of 3 angles at the apex, calculates
-/// the lentghs of the line segments connecting projection center (P) and the three 3D points (A, B, C).
+/// the lengths of the line segments connecting projection center (P) and the three 3D points (A, B, C).
 /// Returned distances are for |PA|, |PB|, |PC| respectively.
 /// Only the solution to the main branch.
 /// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
@@ -265,21 +319,21 @@ bool p3p::align(double M_end[3][3],
    double R[3][3], double T[3])
 {
    // Centroids:
-    double C_start[3], C_end[3];
+    double C_start[3] = {}, C_end[3] = {};
    for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;
    C_start[0] = (X0 + X1 + X2) / 3;
    C_start[1] = (Y0 + Y1 + Y2) / 3;
    C_start[2] = (Z0 + Z1 + Z2) / 3;

    // Covariance matrix s:
-    double s[3 * 3];
+    double s[3 * 3] = {};
    for(int j = 0; j < 3; j++) {
        s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0];
        s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1];
        s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2];
    }

-    double Qs[16], evs[4], U[16];
+    double Qs[16] = {}, evs[4] = {}, U[16] = {};

    Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];
    Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];
@@ -332,7 +386,7 @@ bool p3p::align(double M_end[3][3],

 bool p3p::jacobi_4x4(double * A, double * D, double * U)
 {
-    double B[4], Z[4];
+    double B[4] = {}, Z[4] = {};
    double Id[16] = {1., 0., 0., 0.,
        0., 1., 0., 0.,
        0., 0., 1., 0.,
@@ -342,7 +396,6 @@ bool p3p::jacobi_4x4(double * A, double * D, double * U)

    B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15];
    memcpy(D, B, 4 * sizeof(double));
-    memset(Z, 0, 4 * sizeof(double));

    for(int iter = 0; iter < 50; iter++) {
        double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]);
@@ -11,10 +11,13 @@ class p3p
  p3p(cv::Mat cameraMatrix);

  bool solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints);
+  int solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints);
  int solve(double R[4][3][3], double t[4][3],
            double mu0, double mv0,   double X0, double Y0, double Z0,
            double mu1, double mv1,   double X1, double Y1, double Z1,
-            double mu2, double mv2,   double X2, double Y2, double Z2);
+            double mu2, double mv2,   double X2, double Y2, double Z2,
+            double mu3, double mv3,   double X3, double Y3, double Z3,
+            bool p4p);
  bool solve(double R[3][3], double t[3],
             double mu0, double mv0,   double X0, double Y0, double Z0,
             double mu1, double mv1,   double X1, double Y1, double Z1,
@@ -34,14 +37,21 @@ class p3p
  void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
  {
      points.clear();
-      points.resize(20);
-      for(int i = 0; i < 4; i++)
+      int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
+      points.resize(5*4); //resize vector to fit for p4p case
+      for(int i = 0; i < npoints; i++)
      {
-          points[i*5] = ipoints.at<IpointType>(0,i).x*fx + cx;
-          points[i*5+1] = ipoints.at<IpointType>(0,i).y*fy + cy;
-          points[i*5+2] = opoints.at<OpointType>(0,i).x;
-          points[i*5+3] = opoints.at<OpointType>(0,i).y;
-          points[i*5+4] = opoints.at<OpointType>(0,i).z;
+          points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
+          points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;
+          points[i*5+2] = opoints.at<OpointType>(i).x;
+          points[i*5+3] = opoints.at<OpointType>(i).y;
+          points[i*5+4] = opoints.at<OpointType>(i).z;
+      }
+      //Fill vectors with unused values for p3p case
+      for (int i = npoints; i < 4; i++) {
+          for (int j = 0; j < 5; j++) {
+              points[i * 5 + j] = 0;
+          }
      }
  }
  void init_inverse_parameters();
@@ -32,7 +32,7 @@ int solve_deg3(double a, double b, double c, double d,
               double & x0, double & x1, double & x2)
 {
  if (a == 0) {
-    // Solve second order sytem
+    // Solve second order system
    if (b == 0)	{
      // Solve first order system
      if (c == 0)
@@ -115,10 +115,10 @@ static  CvStatus  icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
                            float* rotation, float* translation )
 {
    int i, j, k;
-    int count = 0, converged = 0;
-    float inorm, jnorm, invInorm, invJnorm, invScale, scale = 0, inv_Z = 0;
+    int count = 0;
+    bool converged = false;
+    float scale = 0, inv_Z = 0;
    float diff = (float)criteria.epsilon;
-    float inv_focalLength = 1 / focalLength;

    /* Check bad arguments */
    if( imagePoints == NULL )
@@ -139,6 +139,7 @@ static  CvStatus  icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
        return CV_BADFACTOR_ERR;

    /* init variables */
+    float inv_focalLength = 1 / focalLength;
    int N = pObject->N;
    float *objectVectors = pObject->obj_vecs;
    float *invMatrix = pObject->inv_matr;
@@ -195,16 +196,18 @@ static  CvStatus  icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
            }
        }

-        inorm = rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ +
+        float inorm =
+                rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ +
                rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ +
                rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/;

-        jnorm = rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ +
+        float jnorm =
+                rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ +
                rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ +
                rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/;

-        invInorm = cvInvSqrt( inorm );
-        invJnorm = cvInvSqrt( jnorm );
+        const float invInorm = cvInvSqrt( inorm );
+        const float invJnorm = cvInvSqrt( jnorm );

        inorm *= invInorm;
        jnorm *= invJnorm;
@@ -231,10 +234,10 @@ static  CvStatus  icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
        inv_Z = scale * inv_focalLength;

        count++;
-        converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon));
-        converged |= ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter));
+        converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon))
+                    || ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter));
    }
-    invScale = 1 / scale;
+    const float invScale = 1 / scale;
    translation[0] = imagePoints[0].x * invScale;
    translation[1] = imagePoints[0].y * invScale;
    translation[2] = 1 / inv_Z;
@@ -266,8 +269,6 @@ static  CvStatus  icvReleasePOSITObject( CvPOSITObject ** ppObject )
 void
 icvPseudoInverse3D( float *a, float *b, int n, int method )
 {
-    int k;
-
    if( method == 0 )
    {
        float ata00 = 0;
@@ -276,8 +277,8 @@ icvPseudoInverse3D( float *a, float *b, int n, int method )
        float ata01 = 0;
        float ata02 = 0;
        float ata12 = 0;
-        float det = 0;

+        int k;
        /* compute matrix ata = transpose(a) * a  */
        for( k = 0; k < n; k++ )
        {
@@ -295,7 +296,6 @@ icvPseudoInverse3D( float *a, float *b, int n, int method )
        }
        /* inverse matrix ata */
        {
-            float inv_det;
            float p00 = ata11 * ata22 - ata12 * ata12;
            float p01 = -(ata01 * ata22 - ata12 * ata02);
            float p02 = ata12 * ata01 - ata11 * ata02;
@@ -304,11 +304,12 @@ icvPseudoInverse3D( float *a, float *b, int n, int method )
            float p12 = -(ata00 * ata12 - ata01 * ata02);
            float p22 = ata00 * ata11 - ata01 * ata01;

+            float det = 0;
            det += ata00 * p00;
            det += ata01 * p01;
            det += ata02 * p02;

-            inv_det = 1 / det;
+            const float inv_det = 1 / det;

            /* compute resultant matrix */
            for( k = 0; k < n; k++ )
@@ -42,13 +42,15 @@
 #ifndef __OPENCV_PRECOMP_H__
 #define __OPENCV_PRECOMP_H__

-#include "opencv2/calib3d.hpp"
-#include "opencv2/imgproc.hpp"
-#include "opencv2/features2d.hpp"
 #include "opencv2/core/utility.hpp"

 #include "opencv2/core/private.hpp"

+#include "opencv2/calib3d.hpp"
+#include "opencv2/imgproc.hpp"
+#include "opencv2/features2d.hpp"
+
+
 #include "opencv2/core/ocl.hpp"

 #ifdef HAVE_TEGRA_OPTIMIZATION
@@ -61,6 +63,21 @@
 namespace cv
 {

+/**
+ * Compute the number of iterations given the confidence, outlier ratio, number
+ * of model points and the maximum iteration number.
+ *
+ * @param p confidence value
+ * @param ep outlier ratio
+ * @param modelPoints number of model points required for estimation
+ * @param maxIters maximum number of iterations
+ * @return
+ * \f[
+ * \frac{\ln(1-p)}{\ln\left(1-(1-ep)^\mathrm{modelPoints}\right)}
+ * \f]
+ *
+ * If the computed number of iterations is larger than maxIters, then maxIters is returned.
+ */
 int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters );

 class CV_EXPORTS LMSolver : public Algorithm
@@ -78,6 +95,7 @@ public:
 };

 CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters);
+CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);

 class CV_EXPORTS PointSetRegistrator : public Algorithm
 {
@@ -102,6 +120,45 @@ CV_EXPORTS Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<Po
 CV_EXPORTS Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& cb,
                                                                   int modelPoints, double confidence=0.99, int maxIters=1000 );

+template<typename T> inline int compressElems( T* ptr, const uchar* mask, int mstep, int count )
+{
+    int i, j;
+    for( i = j = 0; i < count; i++ )
+        if( mask[i*mstep] )
+        {
+            if( i > j )
+                ptr[j] = ptr[i];
+            j++;
+        }
+    return j;
 }

+static inline bool haveCollinearPoints( const Mat& m, int count )
+{
+    int j, k, i = count-1;
+    const Point2f* ptr = m.ptr<Point2f>();
+
+    // check that the i-th selected point does not belong
+    // to a line connecting some previously selected points
+    // also checks that points are not too close to each other
+    for( j = 0; j < i; j++ )
+    {
+        double dx1 = ptr[j].x - ptr[i].x;
+        double dy1 = ptr[j].y - ptr[i].y;
+        for( k = 0; k < j; k++ )
+        {
+            double dx2 = ptr[k].x - ptr[i].x;
+            double dy2 = ptr[k].y - ptr[i].y;
+            if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
+                return true;
+        }
+    }
+    return false;
+}
+
+} // namespace cv
+
+int checkChessboard(const cv::Mat & img, const cv::Size & size);
+int checkChessboardBinary(const cv::Mat & img, const cv::Size & size);
+
 #endif
@@ -78,10 +78,7 @@ class RANSACPointSetRegistrator : public PointSetRegistrator
 public:
    RANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
                              int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
-    : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters)
-    {
-        checkPartialSubsets = true;
-    }
+      : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters) {}

    int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
    {
@@ -102,63 +99,63 @@ public:
        return nz;
    }

-    bool getSubset( const Mat& m1, const Mat& m2,
-                    Mat& ms1, Mat& ms2, RNG& rng,
-                    int maxAttempts=1000 ) const
+    bool getSubset( const Mat& m1, const Mat& m2, Mat& ms1, Mat& ms2, RNG& rng, int maxAttempts=1000 ) const
    {
        cv::AutoBuffer<int> _idx(modelPoints);
-        int* idx = _idx;
-        int i = 0, j, k, iters = 0;
-        int esz1 = (int)m1.elemSize(), esz2 = (int)m2.elemSize();
-        int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
-        int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
-        int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
-        const int *m1ptr = (const int*)m1.data, *m2ptr = (const int*)m2.data;
+        int* idx = _idx.data();
+
+        const int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
+        const int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
+
+        int esz1 = (int)m1.elemSize1() * d1;
+        int esz2 = (int)m2.elemSize1() * d2;
+        CV_Assert((esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0);
+        esz1 /= sizeof(int);
+        esz2 /= sizeof(int);
+
+        const int count = m1.checkVector(d1);
+        const int count2 = m2.checkVector(d2);
+        CV_Assert(count >= modelPoints && count == count2);
+
+        const int *m1ptr = m1.ptr<int>();
+        const int *m2ptr = m2.ptr<int>();

        ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1));
        ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2));

-        int *ms1ptr = (int*)ms1.data, *ms2ptr = (int*)ms2.data;
+        int *ms1ptr = ms1.ptr<int>();
+        int *ms2ptr = ms2.ptr<int>();

-        CV_Assert( count >= modelPoints && count == count2 );
-        CV_Assert( (esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0 );
-        esz1 /= sizeof(int);
-        esz2 /= sizeof(int);
-
-        for(; iters < maxAttempts; iters++)
+        for( int iters = 0; iters < maxAttempts; ++iters )
        {
-            for( i = 0; i < modelPoints && iters < maxAttempts; )
+            int i;
+
+            for( i = 0; i < modelPoints; ++i )
            {
-                int idx_i = 0;
-                for(;;)
-                {
-                    idx_i = idx[i] = rng.uniform(0, count);
-                    for( j = 0; j < i; j++ )
-                        if( idx_i == idx[j] )
-                            break;
-                    if( j == i )
-                        break;
-                }
-                for( k = 0; k < esz1; k++ )
+                int idx_i;
+
+                for ( idx_i = rng.uniform(0, count);
+                    std::find(idx, idx + i, idx_i) != idx + i;
+                    idx_i = rng.uniform(0, count) )
+                {}
+
+                idx[i] = idx_i;
+
+                for( int k = 0; k < esz1; ++k )
                    ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
-                for( k = 0; k < esz2; k++ )
+
+                for( int k = 0; k < esz2; ++k )
                    ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
-                if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 ))
-                {
-                    iters++;
-                    continue;
-                }
-                i++;
            }
-            if( !checkPartialSubsets && i == modelPoints && !cb->checkSubset(ms1, ms2, i))
-                continue;
-            break;
+
+            if( cb->checkSubset(ms1, ms2, i) )
+                return true;
        }

-        return i == modelPoints && iters < maxAttempts;
+        return false;
    }

-    bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
+    bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const CV_OVERRIDE
    {
        bool result = false;
        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
@@ -203,10 +200,10 @@ public:

        for( iter = 0; iter < niters; iter++ )
        {
-            int i, goodCount, nmodels;
+            int i, nmodels;
            if( count > modelPoints )
            {
-                bool found = getSubset( m1, m2, ms1, ms2, rng );
+                bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
                if( !found )
                {
                    if( iter == 0 )
@@ -224,7 +221,7 @@ public:
            for( i = 0; i < nmodels; i++ )
            {
                Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
-                goodCount = findInliers( m1, m2, model_i, err, mask, threshold );
+                int goodCount = findInliers( m1, m2, model_i, err, mask, threshold );

                if( goodCount > MAX(maxGoodCount, modelPoints-1) )
                {
@@ -254,13 +251,10 @@ public:
        return result;
    }

-    void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) { cb = _cb; }
-
-    AlgorithmInfo* info() const;
+    void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) CV_OVERRIDE { cb = _cb; }

    Ptr<PointSetRegistrator::Callback> cb;
    int modelPoints;
-    bool checkPartialSubsets;
    double threshold;
    double confidence;
    int maxIters;
@@ -273,7 +267,7 @@ public:
                              int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
    : RANSACPointSetRegistrator(_cb, _modelPoints, 0, _confidence, _maxIters) {}

-    bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
+    bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const CV_OVERRIDE
    {
        const double outlierRatio = 0.45;
        bool result = false;
@@ -283,7 +277,7 @@ public:
        int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
        int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
        int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
-        double minMedian = DBL_MAX, sigma;
+        double minMedian = DBL_MAX;

        RNG rng((uint64)-1);

@@ -343,10 +337,8 @@ public:
                else
                    errf = err;
                CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (int)errf.total() == count );
-                std::sort((int*)errf.data, (int*)errf.data + count);
-
-                double median = count % 2 != 0 ?
-                errf.at<float>(count/2) : (errf.at<float>(count/2-1) + errf.at<float>(count/2))*0.5;
+                std::nth_element(errf.ptr<int>(), errf.ptr<int>() + count/2, errf.ptr<int>() + count);
+                double median = errf.at<float>(count/2);

                if( median < minMedian )
                {
@@ -358,7 +350,7 @@ public:

        if( minMedian < DBL_MAX )
        {
-            sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
+            double sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
            sigma = MAX( sigma, 0.001 );

            count = findInliers( m1, m2, bestModel, err, mask, sigma );
@@ -378,24 +370,12 @@ public:
        return result;
    }

-    AlgorithmInfo* info() const;
 };

-
-CV_INIT_ALGORITHM(RANSACPointSetRegistrator, "PointSetRegistrator.RANSAC",
-                  obj.info()->addParam(obj, "threshold", obj.threshold);
-                  obj.info()->addParam(obj, "confidence", obj.confidence);
-                  obj.info()->addParam(obj, "maxIters", obj.maxIters))
-
-CV_INIT_ALGORITHM(LMeDSPointSetRegistrator, "PointSetRegistrator.LMeDS",
-                  obj.info()->addParam(obj, "confidence", obj.confidence);
-                  obj.info()->addParam(obj, "maxIters", obj.maxIters))
-
 Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
                                                         int _modelPoints, double _threshold,
                                                         double _confidence, int _maxIters)
 {
-    CV_Assert( !RANSACPointSetRegistrator_info_auto.name().empty() );
    return Ptr<PointSetRegistrator>(
        new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters));
 }
@@ -404,15 +384,28 @@ Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegis
 Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
                             int _modelPoints, double _confidence, int _maxIters)
 {
-    CV_Assert( !LMeDSPointSetRegistrator_info_auto.name().empty() );
    return Ptr<PointSetRegistrator>(
        new LMeDSPointSetRegistrator(_cb, _modelPoints, _confidence, _maxIters));
 }

+
+/*
+ * Compute
+ *  x    a b c   X     t1
+ *  y  = d e f * Y  +  t2
+ *  z    g h i   Z     t3
+ *
+ *  - every element in _m1 contains (X,Y,Z), which are called source points
+ *  - every element in _m2 contains (x,y,z), which are called destination points
+ *  - _model is of size 3x4, which contains
+ *     a b c t1
+ *     d e f t2
+ *     g h i t3
+ */
 class Affine3DEstimatorCallback : public PointSetRegistrator::Callback
 {
 public:
-    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
    {
        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
        const Point3f* from = m1.ptr<Point3f>();
@@ -450,7 +443,7 @@ public:
        return 1;
    }

-    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
    {
        Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
        const Point3f* from = m1.ptr<Point3f>();
@@ -473,11 +466,11 @@ public:
            double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
            double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;

-            errptr[i] = (float)std::sqrt(a*a + b*b + c*c);
+            errptr[i] = (float)(a*a + b*b + c*c);
        }
    }

-    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const CV_OVERRIDE
    {
        const float threshold = 0.996f;
        Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
@@ -495,13 +488,13 @@ public:
            for(j = 0; j < i; ++j)
            {
                Point3f d1 = ptr[j] - ptr[i];
-                float n1 = d1.x*d1.x + d1.y*d1.y;
+                float n1 = d1.x*d1.x + d1.y*d1.y + d1.z*d1.z;

                for(k = 0; k < j; ++k)
                {
                    Point3f d2 = ptr[k] - ptr[i];
-                    float denom = (d2.x*d2.x + d2.y*d2.y)*n1;
-                    float num = d1.x*d2.x + d1.y*d2.y;
+                    float denom = (d2.x*d2.x + d2.y*d2.y + d2.z*d2.z)*n1;
+                    float num = d1.x*d2.x + d1.y*d2.y + d1.z*d2.z;

                    if( num*num > threshold*threshold*denom )
                        return false;
@@ -512,12 +505,301 @@ public:
    }
 };

-}
-
-int cv::estimateAffine3D(InputArray _from, InputArray _to,
-                         OutputArray _out, OutputArray _inliers,
-                         double param1, double param2)
+/*
+ * Compute
+ *  x     a  b   X    c
+ *     =       *    +
+ *  y     d  e   Y    f
+ *
+ *  - every element in _m1 contains (X,Y), which are called source points
+ *  - every element in _m2 contains (x,y), which are called destination points
+ *  - _model is of size 2x3, which contains
+ *    a b c
+ *    d e f
+ */
+class Affine2DEstimatorCallback : public PointSetRegistrator::Callback
 {
+public:
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
+    {
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        const Point2f* from = m1.ptr<Point2f>();
+        const Point2f* to   = m2.ptr<Point2f>();
+        _model.create(2, 3, CV_64F);
+        Mat M_mat = _model.getMat();
+        double *M = M_mat.ptr<double>();
+
+        // we need 3 points to estimate affine transform
+        double x1 = from[0].x;
+        double y1 = from[0].y;
+        double x2 = from[1].x;
+        double y2 = from[1].y;
+        double x3 = from[2].x;
+        double y3 = from[2].y;
+
+        double X1 = to[0].x;
+        double Y1 = to[0].y;
+        double X2 = to[1].x;
+        double Y2 = to[1].y;
+        double X3 = to[2].x;
+        double Y3 = to[2].y;
+
+        /*
+        We want to solve AX = B
+
+            | x1 y1  1  0  0  0 |
+            |  0  0  0 x1 y1  1 |
+            | x2 y2  1  0  0  0 |
+        A = |  0  0  0 x2 y2  1 |
+            | x3 y3  1  0  0  0 |
+            |  0  0  0 x3 y3  1 |
+        B = (X1, Y1, X2, Y2, X3, Y3).t()
+        X = (a, b, c, d, e, f).t()
+
+        As the estimate of (a, b, c) only depends on the Xi, and (d, e, f) only
+        depends on the Yi, we do the *trick* to solve each one analytically.
+
+        | X1 |   | x1 y1 1 |   | a |
+        | X2 | = | x2 y2 1 | * | b |
+        | X3 |   | x3 y3 1 |   | c |
+
+        | Y1 |   | x1 y1 1 |   | d |
+        | Y2 | = | x2 y2 1 | * | e |
+        | Y3 |   | x3 y3 1 |   | f |
+        */
+
+        double d = 1. / ( x1*(y2-y3) + x2*(y3-y1) + x3*(y1-y2) );
+
+        M[0] = d * ( X1*(y2-y3) + X2*(y3-y1) + X3*(y1-y2) );
+        M[1] = d * ( X1*(x3-x2) + X2*(x1-x3) + X3*(x2-x1) );
+        M[2] = d * ( X1*(x2*y3 - x3*y2) + X2*(x3*y1 - x1*y3) + X3*(x1*y2 - x2*y1) );
+
+        M[3] = d * ( Y1*(y2-y3) + Y2*(y3-y1) + Y3*(y1-y2) );
+        M[4] = d * ( Y1*(x3-x2) + Y2*(x1-x3) + Y3*(x2-x1) );
+        M[5] = d * ( Y1*(x2*y3 - x3*y2) + Y2*(x3*y1 - x1*y3) + Y3*(x1*y2 - x2*y1) );
+        return 1;
+    }
+
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
+    {
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
+        const Point2f* from = m1.ptr<Point2f>();
+        const Point2f* to   = m2.ptr<Point2f>();
+        const double* F = model.ptr<double>();
+
+        int count = m1.checkVector(2);
+        CV_Assert( count > 0 );
+
+        _err.create(count, 1, CV_32F);
+        Mat err = _err.getMat();
+        float* errptr = err.ptr<float>();
+        // transform matrix to floats
+        float F0 = (float)F[0], F1 = (float)F[1], F2 = (float)F[2];
+        float F3 = (float)F[3], F4 = (float)F[4], F5 = (float)F[5];
+
+        for(int i = 0; i < count; i++ )
+        {
+            const Point2f& f = from[i];
+            const Point2f& t = to[i];
+
+            float a = F0*f.x + F1*f.y + F2 - t.x;
+            float b = F3*f.x + F4*f.y + F5 - t.y;
+
+            errptr[i] = a*a + b*b;
+        }
+    }
+
+    bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const CV_OVERRIDE
+    {
+        Mat ms1 = _ms1.getMat();
+        Mat ms2 = _ms2.getMat();
+        // check collinearity and also check that points are too close
+        return !haveCollinearPoints(ms1, count) && !haveCollinearPoints(ms2, count);
+    }
+};
+
+/*
+ * Compute
+ *  x    c -s    X    t1
+ *    =       *     +
+ *  y    s  c    Y    t2
+ *
+ *  - every element in _m1 contains (X,Y), which are called source points
+ *  - every element in _m2 contains (x,y), which are called destination points
+ *  - _model is of size 2x3, which contains
+ *    c  -s  t1
+ *    s   c  t2
+ */
+class AffinePartial2DEstimatorCallback : public Affine2DEstimatorCallback
+{
+public:
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
+    {
+        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
+        const Point2f* from = m1.ptr<Point2f>();
+        const Point2f* to   = m2.ptr<Point2f>();
+        _model.create(2, 3, CV_64F);
+        Mat M_mat = _model.getMat();
+        double *M = M_mat.ptr<double>();
+
+        // we need only 2 points to estimate transform
+        double x1 = from[0].x;
+        double y1 = from[0].y;
+        double x2 = from[1].x;
+        double y2 = from[1].y;
+
+        double X1 = to[0].x;
+        double Y1 = to[0].y;
+        double X2 = to[1].x;
+        double Y2 = to[1].y;
+
+        /*
+        we are solving AS = B
+            | x1 -y1 1 0 |
+            | y1  x1 0 1 |
+        A = | x2 -y2 1 0 |
+            | y2  x2 0 1 |
+        B = (X1, Y1, X2, Y2).t()
+        we solve that analytically
+        */
+        double d = 1./((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
+
+        // solution vector
+        double S0 = d * ( (X1-X2)*(x1-x2) + (Y1-Y2)*(y1-y2) );
+        double S1 = d * ( (Y1-Y2)*(x1-x2) - (X1-X2)*(y1-y2) );
+        double S2 = d * ( (Y1-Y2)*(x1*y2 - x2*y1) - (X1*y2 - X2*y1)*(y1-y2) - (X1*x2 - X2*x1)*(x1-x2) );
+        double S3 = d * (-(X1-X2)*(x1*y2 - x2*y1) - (Y1*x2 - Y2*x1)*(x1-x2) - (Y1*y2 - Y2*y1)*(y1-y2) );
+
+        // set model, rotation part is antisymmetric
+        M[0] = M[4] = S0;
+        M[1] = -S1;
+        M[2] = S2;
+        M[3] = S1;
+        M[5] = S3;
+        return 1;
+    }
+};
+
+class Affine2DRefineCallback : public LMSolver::Callback
+{
+public:
+    Affine2DRefineCallback(InputArray _src, InputArray _dst)
+    {
+        src = _src.getMat();
+        dst = _dst.getMat();
+    }
+
+    bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
+    {
+        int i, count = src.checkVector(2);
+        Mat param = _param.getMat();
+        _err.create(count*2, 1, CV_64F);
+        Mat err = _err.getMat(), J;
+        if( _Jac.needed())
+        {
+            _Jac.create(count*2, param.rows, CV_64F);
+            J = _Jac.getMat();
+            CV_Assert( J.isContinuous() && J.cols == 6 );
+        }
+
+        const Point2f* M = src.ptr<Point2f>();
+        const Point2f* m = dst.ptr<Point2f>();
+        const double* h = param.ptr<double>();
+        double* errptr = err.ptr<double>();
+        double* Jptr = J.data ? J.ptr<double>() : 0;
+
+        for( i = 0; i < count; i++ )
+        {
+            double Mx = M[i].x, My = M[i].y;
+            double xi = h[0]*Mx + h[1]*My + h[2];
+            double yi = h[3]*Mx + h[4]*My + h[5];
+            errptr[i*2] = xi - m[i].x;
+            errptr[i*2+1] = yi - m[i].y;
+
+            /*
+            Jacobian should be:
+                {x, y, 1, 0, 0, 0}
+                {0, 0, 0, x, y, 1}
+            */
+            if( Jptr )
+            {
+                Jptr[0] = Mx; Jptr[1] = My; Jptr[2] = 1.;
+                Jptr[3] = Jptr[4] = Jptr[5] = 0.;
+                Jptr[6] = Jptr[7] = Jptr[8] = 0.;
+                Jptr[9] = Mx; Jptr[10] = My; Jptr[11] = 1.;
+
+                Jptr += 6*2;
+            }
+        }
+
+        return true;
+    }
+
+    Mat src, dst;
+};
+
+class AffinePartial2DRefineCallback : public LMSolver::Callback
+{
+public:
+    AffinePartial2DRefineCallback(InputArray _src, InputArray _dst)
+    {
+        src = _src.getMat();
+        dst = _dst.getMat();
+    }
+
+    bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
+    {
+        int i, count = src.checkVector(2);
+        Mat param = _param.getMat();
+        _err.create(count*2, 1, CV_64F);
+        Mat err = _err.getMat(), J;
+        if( _Jac.needed())
+        {
+            _Jac.create(count*2, param.rows, CV_64F);
+            J = _Jac.getMat();
+            CV_Assert( J.isContinuous() && J.cols == 4 );
+        }
+
+        const Point2f* M = src.ptr<Point2f>();
+        const Point2f* m = dst.ptr<Point2f>();
+        const double* h = param.ptr<double>();
+        double* errptr = err.ptr<double>();
+        double* Jptr = J.data ? J.ptr<double>() : 0;
+
+        for( i = 0; i < count; i++ )
+        {
+            double Mx = M[i].x, My = M[i].y;
+            double xi = h[0]*Mx - h[1]*My + h[2];
+            double yi = h[1]*Mx + h[0]*My + h[3];
+            errptr[i*2] = xi - m[i].x;
+            errptr[i*2+1] = yi - m[i].y;
+
+            /*
+            Jacobian should be:
+                {x, -y, 1, 0}
+                {y,  x, 0, 1}
+            */
+            if( Jptr )
+            {
+                Jptr[0] = Mx; Jptr[1] = -My; Jptr[2] = 1.; Jptr[3] = 0.;
+                Jptr[4] = My; Jptr[5] =  Mx; Jptr[6] = 0.; Jptr[7] = 1.;
+
+                Jptr += 4*2;
+            }
+        }
+
+        return true;
+    }
+
+    Mat src, dst;
+};
+
+int estimateAffine3D(InputArray _from, InputArray _to,
+                     OutputArray _out, OutputArray _inliers,
+                     double ransacThreshold, double confidence)
+{
+    CV_INSTRUMENT_REGION();
+
    Mat from = _from.getMat(), to = _to.getMat();
    int count = from.checkVector(3);

@@ -530,8 +812,171 @@ int cv::estimateAffine3D(InputArray _from, InputArray _to,
    dTo = dTo.reshape(3, count);

    const double epsilon = DBL_EPSILON;
-    param1 = param1 <= 0 ? 3 : param1;
-    param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2;
+    ransacThreshold = ransacThreshold <= 0 ? 3 : ransacThreshold;
+    confidence = (confidence < epsilon) ? 0.99 : (confidence > 1 - epsilon) ? 0.99 : confidence;

-    return createRANSACPointSetRegistrator(makePtr<Affine3DEstimatorCallback>(), 4, param1, param2)->run(dFrom, dTo, _out, _inliers);
+    return createRANSACPointSetRegistrator(makePtr<Affine3DEstimatorCallback>(), 4, ransacThreshold, confidence)->run(dFrom, dTo, _out, _inliers);
 }
+
+Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray _inliers,
+                     const int method, const double ransacReprojThreshold,
+                     const size_t maxIters, const double confidence,
+                     const size_t refineIters)
+{
+    Mat from = _from.getMat(), to = _to.getMat();
+    int count = from.checkVector(2);
+    bool result = false;
+    Mat H;
+
+    CV_Assert( count >= 0 && to.checkVector(2) == count );
+
+    if (from.type() != CV_32FC2 || to.type() != CV_32FC2)
+    {
+        Mat tmp1, tmp2;
+        from.convertTo(tmp1, CV_32FC2);
+        from = tmp1;
+        to.convertTo(tmp2, CV_32FC2);
+        to = tmp2;
+    }
+    else
+    {
+        // avoid changing of inputs in compressElems() call
+        from = from.clone();
+        to = to.clone();
+    }
+
+    // convert to N x 1 vectors
+    from = from.reshape(2, count);
+    to = to.reshape(2, count);
+
+    Mat inliers;
+    if(_inliers.needed())
+    {
+        _inliers.create(count, 1, CV_8U, -1, true);
+        inliers = _inliers.getMat();
+    }
+
+    // run robust method
+    Ptr<PointSetRegistrator::Callback> cb = makePtr<Affine2DEstimatorCallback>();
+    if( method == RANSAC )
+        result = createRANSACPointSetRegistrator(cb, 3, ransacReprojThreshold, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
+    else if( method == LMEDS )
+        result = createLMeDSPointSetRegistrator(cb, 3, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
+    else
+        CV_Error(Error::StsBadArg, "Unknown or unsupported robust estimation method");
+
+    if(result && count > 3 && refineIters)
+    {
+        // reorder to start with inliers
+        compressElems(from.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
+        int inliers_count = compressElems(to.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
+        if(inliers_count > 0)
+        {
+            Mat src = from.rowRange(0, inliers_count);
+            Mat dst = to.rowRange(0, inliers_count);
+            Mat Hvec = H.reshape(1, 6);
+            createLMSolver(makePtr<Affine2DRefineCallback>(src, dst), static_cast<int>(refineIters))->run(Hvec);
+        }
+    }
+
+    if (!result)
+    {
+        H.release();
+        if(_inliers.needed())
+        {
+            inliers = Mat::zeros(count, 1, CV_8U);
+            inliers.copyTo(_inliers);
+        }
+    }
+
+    return H;
+}
+
+Mat estimateAffinePartial2D(InputArray _from, InputArray _to, OutputArray _inliers,
+                            const int method, const double ransacReprojThreshold,
+                            const size_t maxIters, const double confidence,
+                            const size_t refineIters)
+{
+    Mat from = _from.getMat(), to = _to.getMat();
+    const int count = from.checkVector(2);
+    bool result = false;
+    Mat H;
+
+    CV_Assert( count >= 0 && to.checkVector(2) == count );
+
+    if (from.type() != CV_32FC2 || to.type() != CV_32FC2)
+    {
+        Mat tmp1, tmp2;
+        from.convertTo(tmp1, CV_32FC2);
+        from = tmp1;
+        to.convertTo(tmp2, CV_32FC2);
+        to = tmp2;
+    }
+    else
+    {
+        // avoid changing of inputs in compressElems() call
+        from = from.clone();
+        to = to.clone();
+    }
+
+    // convert to N x 1 vectors
+    from = from.reshape(2, count);
+    to = to.reshape(2, count);
+
+    Mat inliers;
+    if(_inliers.needed())
+    {
+        _inliers.create(count, 1, CV_8U, -1, true);
+        inliers = _inliers.getMat();
+    }
+
+    // run robust estimation
+    Ptr<PointSetRegistrator::Callback> cb = makePtr<AffinePartial2DEstimatorCallback>();
+    if( method == RANSAC )
+        result = createRANSACPointSetRegistrator(cb, 2, ransacReprojThreshold, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
+    else if( method == LMEDS )
+        result = createLMeDSPointSetRegistrator(cb, 2, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
+    else
+        CV_Error(Error::StsBadArg, "Unknown or unsupported robust estimation method");
+
+    if(result && count > 2 && refineIters)
+    {
+        // reorder to start with inliers
+        compressElems(from.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
+        int inliers_count = compressElems(to.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
+        if(inliers_count > 0)
+        {
+            Mat src = from.rowRange(0, inliers_count);
+            Mat dst = to.rowRange(0, inliers_count);
+            // H is
+            //     a -b tx
+            //     b  a ty
+            // Hvec model for LevMarq is
+            //     (a, b, tx, ty)
+            double *Hptr = H.ptr<double>();
+            double Hvec_buf[4] = {Hptr[0], Hptr[3], Hptr[2], Hptr[5]};
+            Mat Hvec (4, 1, CV_64F, Hvec_buf);
+            createLMSolver(makePtr<AffinePartial2DRefineCallback>(src, dst), static_cast<int>(refineIters))->run(Hvec);
+            // update H with refined parameters
+            Hptr[0] = Hptr[4] = Hvec_buf[0];
+            Hptr[1] = -Hvec_buf[1];
+            Hptr[2] = Hvec_buf[2];
+            Hptr[3] = Hvec_buf[1];
+            Hptr[5] = Hvec_buf[3];
+        }
+    }
+
+    if (!result)
+    {
+        H.release();
+        if(_inliers.needed())
+        {
+            inliers = Mat::zeros(count, 1, CV_8U);
+            inliers.copyTo(_inliers);
+        }
+    }
+
+    return H;
+}
+
+} // namespace cv
@@ -61,13 +61,13 @@ static void orderContours(const std::vector<std::vector<Point> >& contours, Poin
    for(i = 0; i < n; i++)
    {
        size_t ni = contours[i].size();
-        double min_dist = std::numeric_limits<double>::max();
+        float min_dist = std::numeric_limits<float>::max();
        for(j = 0; j < ni; j++)
        {
            double dist = norm(Point2f((float)contours[i][j].x, (float)contours[i][j].y) - point);
-            min_dist = MIN(min_dist, dist);
+            min_dist = (float)MIN((double)min_dist, dist);
        }
-        order.push_back(std::pair<int, float>((int)i, (float)min_dist));
+        order.push_back(std::pair<int, float>((int)i, min_dist));
    }

    std::sort(order.begin(), order.end(), is_smaller);
@@ -163,6 +163,8 @@ static int segment_hist_max(const Mat& hist, int& low_thresh, int& high_thresh)

 bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size region_size)
 {
+    CV_INSTRUMENT_REGION();
+
    Mat img = _img.getMat(), cornersM = _corners.getMat();
    int ncorners = cornersM.checkVector(2, CV_32F);
    CV_Assert( ncorners >= 0 );
@@ -192,9 +194,8 @@ bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size
        erode(white_comp, white_comp, Mat(), Point(-1, -1), erode_count);

        std::vector<std::vector<Point> > white_contours, black_contours;
-        std::vector<Vec4i> white_hierarchy, black_hierarchy;
-        findContours(black_comp, black_contours, black_hierarchy, RETR_LIST, CHAIN_APPROX_SIMPLE);
-        findContours(white_comp, white_contours, white_hierarchy, RETR_LIST, CHAIN_APPROX_SIMPLE);
+        findContours(black_comp, black_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
+        findContours(white_comp, white_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

        if(black_contours.size() < 5 || white_contours.size() < 5) continue;

@@ -0,0 +1,267 @@
+/*
+  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+
+  By downloading, copying, installing or using the software you agree to this license.
+  If you do not agree to this license, do not download, install,
+  copy or use the software.
+
+
+                          BSD 3-Clause License
+
+ Copyright (C) 2014, Olexa Bilaniuk, Hamid Bazargani & Robert Laganiere, all rights reserved.
+
+ Redistribution and use in source and binary forms, with or without modification,
+ are permitted provided that the following conditions are met:
+
+   * Redistribution's of source code must retain the above copyright notice,
+     this list of conditions and the following disclaimer.
+
+   * Redistribution's in binary form must reproduce the above copyright notice,
+     this list of conditions and the following disclaimer in the documentation
+     and/or other materials provided with the distribution.
+
+   * The name of the copyright holders may not be used to endorse or promote products
+     derived from this software without specific prior written permission.
+
+ This software is provided by the copyright holders and contributors "as is" and
+ any express or implied warranties, including, but not limited to, the implied
+ warranties of merchantability and fitness for a particular purpose are disclaimed.
+ In no event shall the Intel Corporation or contributors be liable for any direct,
+ indirect, incidental, special, exemplary, or consequential damages
+ (including, but not limited to, procurement of substitute goods or services;
+ loss of use, data, or profits; or business interruption) however caused
+ and on any theory of liability, whether in contract, strict liability,
+ or tort (including negligence or otherwise) arising in any way out of
+ the use of this software, even if advised of the possibility of such damage.
+*/
+
+/**
+ * Bilaniuk, Olexa, Hamid Bazargani, and Robert Laganiere. "Fast Target
+ * Recognition on Mobile Devices: Revisiting Gaussian Elimination for the
+ * Estimation of Planar Homographies." In Computer Vision and Pattern
+ * Recognition Workshops (CVPRW), 2014 IEEE Conference on, pp. 119-125.
+ * IEEE, 2014.
+ */
+
+/* Include Guards */
+#ifndef __OPENCV_RHO_H__
+#define __OPENCV_RHO_H__
+
+
+
+/* Includes */
+#include <opencv2/core.hpp>
+
+
+
+
+
+/* Defines */
+
+
+/* Flags */
+#ifndef RHO_FLAG_NONE
+#define RHO_FLAG_NONE                        (0U<<0)
+#endif
+#ifndef RHO_FLAG_ENABLE_NR
+#define RHO_FLAG_ENABLE_NR                   (1U<<0)
+#endif
+#ifndef RHO_FLAG_ENABLE_REFINEMENT
+#define RHO_FLAG_ENABLE_REFINEMENT           (1U<<1)
+#endif
+#ifndef RHO_FLAG_ENABLE_FINAL_REFINEMENT
+#define RHO_FLAG_ENABLE_FINAL_REFINEMENT     (1U<<2)
+#endif
+
+
+
+/* Namespace cv */
+namespace cv{
+
+/* Data structures */
+
+/**
+ * Homography Estimation context.
+ */
+
+struct RHO_HEST;
+typedef struct RHO_HEST RHO_HEST;
+
+
+/* Functions */
+
+/**
+ * Initialize the estimator context, by allocating the aligned buffers
+ * internally needed.
+ *
+ * @return A pointer to the context if successful; NULL if an error occurred.
+ */
+
+Ptr<RHO_HEST> rhoInit(void);
+
+
+/**
+ * Ensure that the estimator context's internal table for non-randomness
+ * criterion is at least of the given size, and uses the given beta. The table
+ * should be larger than the maximum number of matches fed into the estimator.
+ *
+ * A value of N of 0 requests deallocation of the table.
+ *
+ * @param [in] p     The initialized estimator context
+ * @param [in] N     If 0, deallocate internal table. If > 0, ensure that the
+ *                   internal table is of at least this size, reallocating if
+ *                   necessary.
+ * @param [in] beta  The beta-factor to use within the table.
+ * @return 0 if unsuccessful; non-zero otherwise.
+ */
+
+int  rhoEnsureCapacity(Ptr<RHO_HEST> p, unsigned N, double beta);
+
+
+
+/**
+ * Seeds the internal PRNG with the given seed.
+ *
+ * Although it is not required to call this function, since context
+ * initialization seeds itself with entropy from rand(), this function allows
+ * reproducible results by using a specified seed.
+ *
+ * @param [in] p    The estimator context whose PRNG is to be seeded.
+ * @param [in] seed The 64-bit integer seed.
+ */
+
+void rhoSeed(Ptr<RHO_HEST> p, uint64_t seed);
+
+
+/**
+ * Estimates the homography using the given context, matches and parameters to
+ * PROSAC.
+ *
+ * The given context must have been initialized.
+ *
+ * The matches are provided as two arrays of N single-precision, floating-point
+ * (x,y) points. Points with corresponding offsets in the two arrays constitute
+ * a match. The homography estimation attempts to find the 3x3 matrix H which
+ * best maps the homogeneous-coordinate points in the source array to their
+ * corresponding homogeneous-coordinate points in the destination array.
+ *
+ *     Note: At least 4 matches must be provided (N >= 4).
+ *     Note: A point in either array takes up 2 floats. The first of two stores
+ *           the x-coordinate and the second of the two stores the y-coordinate.
+ *           Thus, the arrays resemble this in memory:
+ *
+ *           src =    [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...]
+ *           Matches:     |       |       |       |       |
+ *           dst =    [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...]
+ *     Note: The matches are expected to be provided sorted by quality, or at
+ *           least not be worse-than-random in ordering.
+ *
+ * A pointer to the base of an array of N bytes can be provided. It serves as
+ * an output mask to indicate whether the corresponding match is an inlier to
+ * the returned homography, if any. A zero indicates an outlier; A non-zero
+ * value indicates an inlier.
+ *
+ * The PROSAC estimator requires a few parameters of its own. These are:
+ *
+ *     - The maximum distance that a source point projected onto the destination
+ *           plane can be from its putative match and still be considered an
+ *           inlier. Must be non-negative.
+ *           A sane default is 3.0.
+ *     - The maximum number of PROSAC iterations. This corresponds to the
+ *           largest number of samples that will be drawn and tested.
+ *           A sane default is 2000.
+ *     - The RANSAC convergence parameter. This corresponds to the number of
+ *           iterations after which PROSAC will start sampling like RANSAC.
+ *           A sane default is 2000.
+ *     - The confidence threshold. This corresponds to the probability of
+ *           finding a correct solution. Must be bounded by [0, 1].
+ *           A sane default is 0.995.
+ *     - The minimum number of inliers acceptable. Only a solution with at
+ *           least this many inliers will be returned. The minimum is 4.
+ *           A sane default is 10% of N.
+ *     - The beta-parameter for the non-randomness termination criterion.
+ *           Ignored if non-randomness criterion disabled, otherwise must be
+ *           bounded by (0, 1).
+ *           A sane default is 0.35.
+ *     - Optional flags to control the estimation. Available flags are:
+ *           HEST_FLAG_NONE:
+ *               No special processing.
+ *           HEST_FLAG_ENABLE_NR:
+ *               Enable non-randomness criterion. If set, the beta parameter
+ *               must also be set.
+ *           HEST_FLAG_ENABLE_REFINEMENT:
+ *               Enable refinement of each new best model, as they are found.
+ *           HEST_FLAG_ENABLE_FINAL_REFINEMENT:
+ *               Enable one final refinement of the best model found before
+ *               returning it.
+ *
+ * The PROSAC estimator optionally accepts an extrinsic initial guess of H.
+ *
+ * The PROSAC estimator outputs a final estimate of H provided it was able to
+ * find one with a minimum of supporting inliers. If it was not, it outputs
+ * the all-zero matrix.
+ *
+ * The extrinsic guess at and final estimate of H are both in the same form:
+ * A 3x3 single-precision floating-point matrix with step 3. Thus, it is a
+ * 9-element array of floats, with the elements as follows:
+ *
+ *     [ H00, H01, H02,
+ *       H10, H11, H12,
+ *       H20, H21, 1.0 ]
+ *
+ * Notice that the homography is normalized to H22 = 1.0.
+ *
+ * The function returns the number of inliers if it was able to find a
+ * homography with at least the minimum required support, and 0 if it was not.
+ *
+ *
+ * @param [in/out] p       The context to use for homography estimation. Must
+ *                             be already initialized. Cannot be NULL.
+ * @param [in]     src     The pointer to the source points of the matches.
+ *                             Must be aligned to 4 bytes. Cannot be NULL.
+ * @param [in]     dst     The pointer to the destination points of the matches.
+ *                             Must be aligned to 4 bytes. Cannot be NULL.
+ * @param [out]    inl     The pointer to the output mask of inlier matches.
+ *                             Must be aligned to 4 bytes. May be NULL.
+ * @param [in]     N       The number of matches. Minimum 4.
+ * @param [in]     maxD    The maximum distance. Minimum 0.
+ * @param [in]     maxI    The maximum number of PROSAC iterations.
+ * @param [in]     rConvg  The RANSAC convergence parameter.
+ * @param [in]     cfd     The required confidence in the solution.
+ * @param [in]     minInl  The minimum required number of inliers. Minimum 4.
+ * @param [in]     beta    The beta-parameter for the non-randomness criterion.
+ * @param [in]     flags   A union of flags to fine-tune the estimation.
+ * @param [in]     guessH  An extrinsic guess at the solution H, or NULL if
+ *                         none provided.
+ * @param [out]    finalH  The final estimation of H, or the zero matrix if
+ *                         the minimum number of inliers was not met.
+ *                         Cannot be NULL.
+ * @return                 The number of inliers if the minimum number of
+ *                         inliers for acceptance was reached; 0 otherwise.
+ */
+
+unsigned rhoHest(Ptr<RHO_HEST> p,       /* Homography estimation context. */
+                 const float*  src,     /* Source points */
+                 const float*  dst,     /* Destination points */
+                 char*         inl,     /* Inlier mask */
+                 unsigned      N,       /*  = src.length = dst.length = inl.length */
+                 float         maxD,    /*   3.0 */
+                 unsigned      maxI,    /*  2000 */
+                 unsigned      rConvg,  /*  2000 */
+                 double        cfd,     /* 0.995 */
+                 unsigned      minInl,  /*     4 */
+                 double        beta,    /*  0.35 */
+                 unsigned      flags,   /*     0 */
+                 const float*  guessH,  /* Extrinsic guess, NULL if none provided */
+                 float*        finalH); /* Final result. */
+
+
+
+
+/* End Namespace cv */
+}
+
+
+
+
+#endif
@@ -63,8 +63,7 @@ cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMa
      !CV_IS_MAT(points4D) )
      CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );

-    int numPoints;
-    numPoints = projPoints1->cols;
+    int numPoints = projPoints1->cols;

    if( numPoints < 1 )
        CV_Error( CV_StsOutOfRange, "Number of points must be more than zero" );
@@ -82,104 +81,39 @@ cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMa
       projMatr2->cols != 4 || projMatr2->rows != 3)
        CV_Error( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );

-    CvMat matrA;
-    double matrA_dat[24];
-    matrA = cvMat(6,4,CV_64F,matrA_dat);
+    // preallocate SVD matrices on stack
+    cv::Matx<double, 4, 4> matrA;
+    cv::Matx<double, 4, 4> matrU;
+    cv::Matx<double, 4, 1> matrW;
+    cv::Matx<double, 4, 4> matrV;

-    //CvMat matrU;
-    CvMat matrW;
-    CvMat matrV;
-    //double matrU_dat[9*9];
-    double matrW_dat[6*4];
-    double matrV_dat[4*4];
-
-    //matrU = cvMat(6,6,CV_64F,matrU_dat);
-    matrW = cvMat(6,4,CV_64F,matrW_dat);
-    matrV = cvMat(4,4,CV_64F,matrV_dat);
-
-    CvMat* projPoints[2];
-    CvMat* projMatrs[2];
-
-    projPoints[0] = projPoints1;
-    projPoints[1] = projPoints2;
-
-    projMatrs[0] = projMatr1;
-    projMatrs[1] = projMatr2;
+    CvMat* projPoints[2] = {projPoints1, projPoints2};
+    CvMat* projMatrs[2] = {projMatr1, projMatr2};

    /* Solve system for each point */
-    int i,j;
-    for( i = 0; i < numPoints; i++ )/* For each point */
+    for( int i = 0; i < numPoints; i++ )/* For each point */
    {
        /* Fill matrix for current point */
-        for( j = 0; j < 2; j++ )/* For each view */
+        for( int j = 0; j < 2; j++ )/* For each view */
        {
            double x,y;
            x = cvmGet(projPoints[j],0,i);
            y = cvmGet(projPoints[j],1,i);
            for( int k = 0; k < 4; k++ )
            {
-                cvmSet(&matrA, j*3+0, k, x * cvmGet(projMatrs[j],2,k) -     cvmGet(projMatrs[j],0,k) );
-                cvmSet(&matrA, j*3+1, k, y * cvmGet(projMatrs[j],2,k) -     cvmGet(projMatrs[j],1,k) );
-                cvmSet(&matrA, j*3+2, k, x * cvmGet(projMatrs[j],1,k) - y * cvmGet(projMatrs[j],0,k) );
+                matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k);
+                matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k);
            }
        }
        /* Solve system for current point */
-        {
-            cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
+        cv::SVD::compute(matrA, matrW, matrU, matrV);

-            /* Copy computed point */
-            cvmSet(points4D,0,i,cvmGet(&matrV,3,0));/* X */
-            cvmSet(points4D,1,i,cvmGet(&matrV,3,1));/* Y */
-            cvmSet(points4D,2,i,cvmGet(&matrV,3,2));/* Z */
-            cvmSet(points4D,3,i,cvmGet(&matrV,3,3));/* W */
-        }
+        /* Copy computed point */
+        cvmSet(points4D,0,i,matrV(3,0));/* X */
+        cvmSet(points4D,1,i,matrV(3,1));/* Y */
+        cvmSet(points4D,2,i,matrV(3,2));/* Z */
+        cvmSet(points4D,3,i,matrV(3,3));/* W */
    }
-
-#if 0
-    double err = 0;
-    /* Points was reconstructed. Try to reproject points */
-    /* We can compute reprojection error if need */
-    {
-        int i;
-        CvMat point3D;
-        double point3D_dat[4];
-        point3D = cvMat(4,1,CV_64F,point3D_dat);
-
-        CvMat point2D;
-        double point2D_dat[3];
-        point2D = cvMat(3,1,CV_64F,point2D_dat);
-
-        for( i = 0; i < numPoints; i++ )
-        {
-            double W = cvmGet(points4D,3,i);
-
-            point3D_dat[0] = cvmGet(points4D,0,i)/W;
-            point3D_dat[1] = cvmGet(points4D,1,i)/W;
-            point3D_dat[2] = cvmGet(points4D,2,i)/W;
-            point3D_dat[3] = 1;
-
-            /* !!! Project this point for each camera */
-            for( int currCamera = 0; currCamera < 2; currCamera++ )
-            {
-                cvMatMul(projMatrs[currCamera], &point3D, &point2D);
-
-                float x,y;
-                float xr,yr,wr;
-                x = (float)cvmGet(projPoints[currCamera],0,i);
-                y = (float)cvmGet(projPoints[currCamera],1,i);
-
-                wr = (float)point2D_dat[2];
-                xr = (float)(point2D_dat[0]/wr);
-                yr = (float)(point2D_dat[1]/wr);
-
-                float deltaX,deltaY;
-                deltaX = (float)fabs(x-xr);
-                deltaY = (float)fabs(y-yr);
-                err += deltaX*deltaX + deltaY*deltaY;
-            }
-        }
-    }
-#endif
 }


@@ -347,7 +281,7 @@ cvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1
        c = cvGetReal2D(RTFTR,2,1);
        d = cvGetReal2D(RTFTR,2,2);

-        // Form the polynomial g(t) = k6*t⁶ + k5*t⁵ + k4*t⁴ + k3*t³ + k2*t² + k1*t + k0
+        // Form the polynomial g(t) = k6*t^6 + k5*t^5 + k4*t^4 + k3*t^3 + k2*t^2 + k1*t + k0
        // from f1, f2, a, b, c and d
        cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c ));
        cvSetReal2D(polynomial,0,5,( +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a ));
@@ -413,6 +347,8 @@ void cv::triangulatePoints( InputArray _projMatr1, InputArray _projMatr2,
                            InputArray _projPoints1, InputArray _projPoints2,
                            OutputArray _points4D )
 {
+    CV_INSTRUMENT_REGION();
+
    Mat matr1 = _projMatr1.getMat(), matr2 = _projMatr2.getMat();
    Mat points1 = _projPoints1.getMat(), points2 = _projPoints2.getMat();

@@ -422,11 +358,12 @@ void cv::triangulatePoints( InputArray _projMatr1, InputArray _projMatr2,
    if((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2)
        points2 = points2.reshape(1, static_cast<int>(points2.total())).t();

-    CvMat cvMatr1 = matr1, cvMatr2 = matr2;
-    CvMat cvPoints1 = points1, cvPoints2 = points2;
+    CvMat cvMatr1 = cvMat(matr1), cvMatr2 = cvMat(matr2);
+    CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2);

    _points4D.create(4, points1.cols, points1.type());
-    CvMat cvPoints4D = _points4D.getMat();
+    Mat cvPoints4D_ = _points4D.getMat();
+    CvMat cvPoints4D = cvMat(cvPoints4D_);

    cvTriangulatePoints(&cvMatr1, &cvMatr2, &cvPoints1, &cvPoints2, &cvPoints4D);
 }
@@ -434,15 +371,18 @@ void cv::triangulatePoints( InputArray _projMatr1, InputArray _projMatr2,
 void cv::correctMatches( InputArray _F, InputArray _points1, InputArray _points2,
                         OutputArray _newPoints1, OutputArray _newPoints2 )
 {
+    CV_INSTRUMENT_REGION();
+
    Mat F = _F.getMat();
    Mat points1 = _points1.getMat(), points2 = _points2.getMat();

-    CvMat cvPoints1 = points1, cvPoints2 = points2;
-    CvMat cvF = F;
+    CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2);
+    CvMat cvF = cvMat(F);

    _newPoints1.create(points1.size(), points1.type());
    _newPoints2.create(points2.size(), points2.type());
-    CvMat cvNewPoints1 = _newPoints1.getMat(), cvNewPoints2 = _newPoints2.getMat();
+    Mat cvNewPoints1_ = _newPoints1.getMat(), cvNewPoints2_ = _newPoints2.getMat();
+    CvMat cvNewPoints1 = cvMat(cvNewPoints1_), cvNewPoints2 = cvMat(cvNewPoints2_);

    cvCorrectMatches(&cvF, &cvPoints1, &cvPoints2, &cvNewPoints1, &cvNewPoints2);
 }
@@ -0,0 +1,829 @@
+//M*//////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of the copyright holders may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+/****************************************************************************************\
+* Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
+* Contributed by Edgar Riba
+\****************************************************************************************/
+
+#include "precomp.hpp"
+#include "upnp.h"
+#include <limits>
+
+#if 0  // fix buffer overflow first (FIXIT mark in .cpp file)
+
+using namespace std;
+using namespace cv;
+
+upnp::upnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints)
+{
+  if (cameraMatrix.depth() == CV_32F)
+    init_camera_parameters<float>(cameraMatrix);
+  else
+    init_camera_parameters<double>(cameraMatrix);
+
+  number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
+
+  pws.resize(3 * number_of_correspondences);
+  us.resize(2 * number_of_correspondences);
+
+  if (opoints.depth() == ipoints.depth())
+  {
+    if (opoints.depth() == CV_32F)
+      init_points<Point3f,Point2f>(opoints, ipoints);
+    else
+      init_points<Point3d,Point2d>(opoints, ipoints);
+  }
+  else if (opoints.depth() == CV_32F)
+    init_points<Point3f,Point2d>(opoints, ipoints);
+  else
+    init_points<Point3d,Point2f>(opoints, ipoints);
+
+  alphas.resize(4 * number_of_correspondences);
+  pcs.resize(3 * number_of_correspondences);
+
+  max_nr = 0;
+  A1 = NULL;
+  A2 = NULL;
+}
+
+upnp::~upnp()
+{
+  if (A1)
+    delete[] A1;
+  if (A2)
+    delete[] A2;
+}
+
+double upnp::compute_pose(Mat& R, Mat& t)
+{
+  choose_control_points();
+  compute_alphas();
+
+  Mat * M = new Mat(2 * number_of_correspondences, 12, CV_64F);
+
+  for(int i = 0; i < number_of_correspondences; i++)
+  {
+    fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]);
+  }
+
+  double mtm[12 * 12], d[12], ut[12 * 12], vt[12 * 12];
+  Mat MtM = Mat(12, 12, CV_64F, mtm);
+  Mat D   = Mat(12,  1, CV_64F, d);
+  Mat Ut  = Mat(12, 12, CV_64F, ut);
+  Mat Vt  = Mat(12, 12, CV_64F, vt);
+
+  MtM = M->t() * (*M);
+  SVD::compute(MtM, D, Ut, Vt, SVD::MODIFY_A | SVD::FULL_UV);
+  Mat(Ut.t()).copyTo(Ut);
+  M->release();
+  delete M;
+
+  double l_6x12[6 * 12], rho[6];
+  Mat L_6x12 = Mat(6, 12, CV_64F, l_6x12);
+  Mat Rho    = Mat(6,  1, CV_64F, rho);
+
+  compute_L_6x12(ut, l_6x12);
+  compute_rho(rho);
+
+  double Betas[3][4], Efs[3][1], rep_errors[3];
+  double Rs[3][3][3], ts[3][3];
+
+  find_betas_and_focal_approx_1(&Ut, &Rho, Betas[1], Efs[1]);
+  gauss_newton(&L_6x12, &Rho, Betas[1], Efs[1]);
+  rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
+
+  find_betas_and_focal_approx_2(&Ut, &Rho, Betas[2], Efs[2]);
+  gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]);
+  rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
+
+  int N = 1;
+  if (rep_errors[2] < rep_errors[1]) N = 2;
+
+  Mat(3, 1, CV_64F, ts[N]).copyTo(t);
+  Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
+  fu = fv = Efs[N][0];
+
+  return fu;
+}
+
+void upnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
+     double R_dst[3][3], double t_dst[3])
+{
+  for(int i = 0; i < 3; i++) {
+    for(int j = 0; j < 3; j++)
+      R_dst[i][j] = R_src[i][j];
+    t_dst[i] = t_src[i];
+  }
+}
+
+void upnp::estimate_R_and_t(double R[3][3], double t[3])
+{
+  double pc0[3], pw0[3];
+
+  pc0[0] = pc0[1] = pc0[2] = 0.0;
+  pw0[0] = pw0[1] = pw0[2] = 0.0;
+
+  for(int i = 0; i < number_of_correspondences; i++) {
+    const double * pc = &pcs[3 * i];
+    const double * pw = &pws[3 * i];
+
+    for(int j = 0; j < 3; j++) {
+      pc0[j] += pc[j];
+      pw0[j] += pw[j];
+    }
+  }
+  for(int j = 0; j < 3; j++) {
+    pc0[j] /= number_of_correspondences;
+    pw0[j] /= number_of_correspondences;
+  }
+
+  double abt[3 * 3] = {0}, abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
+  Mat ABt   = Mat(3, 3, CV_64F, abt);
+  Mat ABt_D = Mat(3, 1, CV_64F, abt_d);
+  Mat ABt_U = Mat(3, 3, CV_64F, abt_u);
+  Mat ABt_V = Mat(3, 3, CV_64F, abt_v);
+
+  ABt.setTo(0.0);
+  for(int i = 0; i < number_of_correspondences; i++) {
+    double * pc = &pcs[3 * i];
+    double * pw = &pws[3 * i];
+
+    for(int j = 0; j < 3; j++) {
+      abt[3 * j    ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
+      abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
+      abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
+    }
+  }
+
+  SVD::compute(ABt, ABt_D, ABt_U, ABt_V, SVD::MODIFY_A);
+  Mat(ABt_V.t()).copyTo(ABt_V);
+
+  for(int i = 0; i < 3; i++)
+    for(int j = 0; j < 3; j++)
+      R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
+
+  const double det =
+    R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
+    R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
+
+  if (det < 0) {
+    R[2][0] = -R[2][0];
+    R[2][1] = -R[2][1];
+    R[2][2] = -R[2][2];
+  }
+
+  t[0] = pc0[0] - dot(R[0], pw0);
+  t[1] = pc0[1] - dot(R[1], pw0);
+  t[2] = pc0[2] - dot(R[2], pw0);
+}
+
+void upnp::solve_for_sign(void)
+{
+  if (pcs[2] < 0.0) {
+    for(int i = 0; i < 4; i++)
+      for(int j = 0; j < 3; j++)
+        ccs[i][j] = -ccs[i][j];
+
+    for(int i = 0; i < number_of_correspondences; i++) {
+      pcs[3 * i    ] = -pcs[3 * i];
+      pcs[3 * i + 1] = -pcs[3 * i + 1];
+      pcs[3 * i + 2] = -pcs[3 * i + 2];
+    }
+  }
+}
+
+double upnp::compute_R_and_t(const double * ut, const double * betas,
+         double R[3][3], double t[3])
+{
+  compute_ccs(betas, ut);
+  compute_pcs();
+
+  solve_for_sign();
+
+  estimate_R_and_t(R, t);
+
+  return reprojection_error(R, t);
+}
+
+double upnp::reprojection_error(const double R[3][3], const double t[3])
+{
+  double sum2 = 0.0;
+
+  for(int i = 0; i < number_of_correspondences; i++) {
+    double * pw = &pws[3 * i];
+    double Xc = dot(R[0], pw) + t[0];
+    double Yc = dot(R[1], pw) + t[1];
+    double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
+    double ue = uc + fu * Xc * inv_Zc;
+    double ve = vc + fv * Yc * inv_Zc;
+    double u = us[2 * i], v = us[2 * i + 1];
+
+    sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) );
+  }
+
+  return sum2 / number_of_correspondences;
+}
+
+void upnp::choose_control_points()
+{
+    for (int i = 0; i < 4; ++i)
+      cws[i][0] = cws[i][1] = cws[i][2] = 0.0;
+    cws[0][0] = cws[1][1] = cws[2][2] = 1.0;
+}
+
+void upnp::compute_alphas()
+{
+    Mat CC = Mat(4, 3, CV_64F, &cws);
+    Mat PC = Mat(number_of_correspondences, 3, CV_64F, &pws[0]);
+    Mat ALPHAS = Mat(number_of_correspondences, 4, CV_64F, &alphas[0]);
+
+    Mat CC_ = CC.clone().t();
+    Mat PC_ = PC.clone().t();
+
+    Mat row14 = Mat::ones(1, 4, CV_64F);
+    Mat row1n = Mat::ones(1, number_of_correspondences, CV_64F);
+
+    CC_.push_back(row14);
+    PC_.push_back(row1n);
+
+    ALPHAS = Mat( CC_.inv() * PC_ ).t();
+}
+
+void upnp::fill_M(Mat * M, const int row, const double * as, const double u, const double v)
+{
+  double * M1 = M->ptr<double>(row);
+  double * M2 = M1 + 12;
+
+  for(int i = 0; i < 4; i++) {
+    M1[3 * i    ] = as[i] * fu;
+    M1[3 * i + 1] = 0.0;
+    M1[3 * i + 2] = as[i] * (uc - u);
+
+    M2[3 * i    ] = 0.0;
+    M2[3 * i + 1] = as[i] * fv;
+    M2[3 * i + 2] = as[i] * (vc - v);
+  }
+}
+
+void upnp::compute_ccs(const double * betas, const double * ut)
+{
+    for(int i = 0; i < 4; ++i)
+      ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0;
+
+    int N = 4;
+    for(int i = 0; i < N; ++i) {
+      const double * v = ut + 12 * (9 + i);
+      for(int j = 0; j < 4; ++j)
+        for(int k = 0; k < 3; ++k)
+          ccs[j][k] += betas[i] * v[3 * j + k];  // FIXIT: array subscript 144 is outside array bounds of 'double [144]' [-Warray-bounds]
+                                                 //        line 109: double ut[12 * 12]
+                                                 //        line 359: double u[12*12]
+    }
+
+    for (int i = 0; i < 4; ++i) ccs[i][2] *= fu;
+}
+
+void upnp::compute_pcs(void)
+{
+  for(int i = 0; i < number_of_correspondences; i++) {
+    double * a = &alphas[0] + 4 * i;
+    double * pc = &pcs[0] + 3 * i;
+
+    for(int j = 0; j < 3; j++)
+      pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j];
+  }
+}
+
+void upnp::find_betas_and_focal_approx_1(Mat * Ut, Mat * Rho, double * betas, double * efs)
+{
+  Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(11));
+  Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0));
+
+  Mat D = compute_constraint_distance_2param_6eq_2unk_f_unk( Kmf1 );
+  Mat Dt = D.t();
+
+  Mat A = Dt * D;
+  Mat b = Dt * dsq;
+
+  Mat x = Mat(2, 1, CV_64F);
+  solve(A, b, x);
+
+  betas[0] = sqrt( abs( x.at<double>(0) ) );
+  betas[1] = betas[2] = betas[3] = 0.0;
+
+  efs[0] = sqrt( abs( x.at<double>(1) ) ) / betas[0];
+}
+
+void upnp::find_betas_and_focal_approx_2(Mat * Ut, Mat * Rho, double * betas, double * efs)
+{
+  double u[12*12];
+  Mat U = Mat(12, 12, CV_64F, u);
+  Ut->copyTo(U);
+
+  Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(10));
+  Mat Kmf2 = Mat(12, 1, CV_64F, Ut->ptr<double>(11));
+  Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0));
+
+  Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk( Kmf1, Kmf2 );
+
+  Mat A = D;
+  Mat b = dsq;
+
+  double x[6];
+  Mat X = Mat(6, 1, CV_64F, x);
+
+  solve(A, b, X, DECOMP_QR);
+
+  double solutions[18][3];
+  generate_all_possible_solutions_for_f_unk(x, solutions);
+
+  // find solution with minimum reprojection error
+  double min_error = std::numeric_limits<double>::max();
+  int min_sol = 0;
+  for (int i = 0; i < 18; ++i) {
+
+    betas[3] = solutions[i][0];
+    betas[2] = solutions[i][1];
+    betas[1] = betas[0] = 0.0;
+    fu = fv = solutions[i][2];
+
+    double Rs[3][3], ts[3];
+    double error_i = compute_R_and_t( u, betas, Rs, ts);
+
+    if( error_i < min_error)
+    {
+      min_error = error_i;
+      min_sol = i;
+    }
+}
+
+  betas[0] = solutions[min_sol][0];
+  betas[1] = solutions[min_sol][1];
+  betas[2] = betas[3] = 0.0;
+
+  efs[0] = solutions[min_sol][2];
+}
+
+Mat upnp::compute_constraint_distance_2param_6eq_2unk_f_unk(const Mat& M1)
+{
+  Mat P = Mat(6, 2, CV_64F);
+
+  double m[13];
+  for (int i = 1; i < 13; ++i) m[i] = *M1.ptr<double>(i-1);
+
+  double t1 = pow( m[4], 2 );
+  double t4 = pow( m[1], 2 );
+  double t5 = pow( m[5], 2 );
+  double t8 = pow( m[2], 2 );
+  double t10 = pow( m[6], 2 );
+  double t13 = pow( m[3], 2 );
+  double t15 = pow( m[7], 2 );
+  double t18 = pow( m[8], 2 );
+  double t22 = pow( m[9], 2 );
+  double t26 = pow( m[10], 2 );
+  double t29 = pow( m[11], 2 );
+  double t33 = pow( m[12], 2 );
+
+  *P.ptr<double>(0,0) = t1 - 2 * m[4] * m[1] + t4 + t5 - 2 * m[5] * m[2] + t8;
+  *P.ptr<double>(0,1) = t10 - 2 * m[6] * m[3] + t13;
+  *P.ptr<double>(1,0) = t15 - 2 * m[7] * m[1] + t4 + t18 - 2 * m[8] * m[2] + t8;
+  *P.ptr<double>(1,1) = t22 - 2 * m[9] * m[3] + t13;
+  *P.ptr<double>(2,0) = t26 - 2 * m[10] * m[1] + t4 + t29 - 2 * m[11] * m[2] + t8;
+  *P.ptr<double>(2,1) = t33 - 2 * m[12] * m[3] + t13;
+  *P.ptr<double>(3,0) = t15 - 2 * m[7] * m[4] + t1 + t18 - 2 * m[8] * m[5] + t5;
+  *P.ptr<double>(3,1) = t22 - 2 * m[9] * m[6] + t10;
+  *P.ptr<double>(4,0) = t26 - 2 * m[10] * m[4] + t1 + t29 - 2 * m[11] * m[5] + t5;
+  *P.ptr<double>(4,1) = t33 - 2 * m[12] * m[6] + t10;
+  *P.ptr<double>(5,0) = t26 - 2 * m[10] * m[7] + t15 + t29 - 2 * m[11] * m[8] + t18;
+  *P.ptr<double>(5,1) = t33 - 2 * m[12] * m[9] + t22;
+
+  return P;
+}
+
+Mat upnp::compute_constraint_distance_3param_6eq_6unk_f_unk(const Mat& M1, const Mat& M2)
+{
+  Mat P = Mat(6, 6, CV_64F);
+
+  double m[3][13];
+  for (int i = 1; i < 13; ++i)
+  {
+    m[1][i] = *M1.ptr<double>(i-1);
+    m[2][i] = *M2.ptr<double>(i-1);
+  }
+
+  double t1 = pow( m[1][4], 2 );
+  double t2 = pow( m[1][1], 2 );
+  double t7 = pow( m[1][5], 2 );
+  double t8 = pow( m[1][2], 2 );
+  double t11 = m[1][1] * m[2][1];
+  double t12 = m[1][5] * m[2][5];
+  double t15 = m[1][2] * m[2][2];
+  double t16 = m[1][4] * m[2][4];
+  double t19 = pow( m[2][4], 2 );
+  double t22 = pow( m[2][2], 2 );
+  double t23 = pow( m[2][1], 2 );
+  double t24 = pow( m[2][5], 2 );
+  double t28 = pow( m[1][6], 2 );
+  double t29 = pow( m[1][3], 2 );
+  double t34 = pow( m[1][3], 2 );
+  double t36 = m[1][6] * m[2][6];
+  double t40 = pow( m[2][6], 2 );
+  double t41 = pow( m[2][3], 2 );
+  double t47 = pow( m[1][7], 2 );
+  double t48 = pow( m[1][8], 2 );
+  double t52 = m[1][7] * m[2][7];
+  double t55 = m[1][8] * m[2][8];
+  double t59 = pow( m[2][8], 2 );
+  double t62 = pow( m[2][7], 2 );
+  double t64 = pow( m[1][9], 2 );
+  double t68 = m[1][9] * m[2][9];
+  double t74 = pow( m[2][9], 2 );
+  double t78 = pow( m[1][10], 2 );
+  double t79 = pow( m[1][11], 2 );
+  double t84 = m[1][10] * m[2][10];
+  double t87 = m[1][11] * m[2][11];
+  double t90 = pow( m[2][10], 2 );
+  double t95 = pow( m[2][11], 2 );
+  double t99 = pow( m[1][12], 2 );
+  double t101 = m[1][12] * m[2][12];
+  double t105 = pow( m[2][12], 2 );
+
+  *P.ptr<double>(0,0) = t1 + t2 - 2 * m[1][4] * m[1][1] - 2 * m[1][5] * m[1][2] + t7 + t8;
+  *P.ptr<double>(0,1) = -2 * m[2][4] * m[1][1] + 2 * t11 + 2 * t12 - 2 * m[1][4] * m[2][1] - 2 * m[2][5] * m[1][2] + 2 * t15 + 2 * t16 - 2 * m[1][5] * m[2][2];
+  *P.ptr<double>(0,2) = t19 - 2 * m[2][4] * m[2][1] + t22 + t23 + t24 - 2 * m[2][5] * m[2][2];
+  *P.ptr<double>(0,3) = t28 + t29 - 2 * m[1][6] * m[1][3];
+  *P.ptr<double>(0,4) = -2 * m[2][6] * m[1][3] + 2 * t34 - 2 * m[1][6] * m[2][3] + 2 * t36;
+  *P.ptr<double>(0,5) = -2 * m[2][6] * m[2][3] + t40 + t41;
+
+  *P.ptr<double>(1,0) = t8 - 2 * m[1][8] * m[1][2] - 2 * m[1][7] * m[1][1] + t47 + t48 + t2;
+  *P.ptr<double>(1,1) = 2 * t15 - 2 * m[1][8] * m[2][2] - 2 * m[2][8] * m[1][2] + 2 * t52 - 2 * m[1][7] * m[2][1] - 2 * m[2][7] * m[1][1] + 2 * t55 + 2 * t11;
+  *P.ptr<double>(1,2) = -2 * m[2][8] * m[2][2] + t22 + t23 + t59 - 2 * m[2][7] * m[2][1] + t62;
+  *P.ptr<double>(1,3) = t29 + t64 - 2 * m[1][9] * m[1][3];
+  *P.ptr<double>(1,4) = 2 * t34 + 2 * t68 - 2 * m[2][9] * m[1][3] - 2 * m[1][9] * m[2][3];
+  *P.ptr<double>(1,5) = -2 * m[2][9] * m[2][3] + t74 + t41;
+
+  *P.ptr<double>(2,0) = -2 * m[1][11] * m[1][2] + t2 + t8 + t78 + t79 - 2 * m[1][10] * m[1][1];
+  *P.ptr<double>(2,1) = 2 * t15 - 2 * m[1][11] * m[2][2] + 2 * t84 - 2 * m[1][10] * m[2][1] - 2 * m[2][10] * m[1][1] + 2 * t87 - 2 * m[2][11] * m[1][2]+ 2 * t11;
+  *P.ptr<double>(2,2) = t90 + t22 - 2 * m[2][10] * m[2][1] + t23 - 2 * m[2][11] * m[2][2] + t95;
+  *P.ptr<double>(2,3) = -2 * m[1][12] * m[1][3] + t99 + t29;
+  *P.ptr<double>(2,4) = 2 * t34 + 2 * t101 - 2 * m[2][12] * m[1][3] - 2 * m[1][12] * m[2][3];
+  *P.ptr<double>(2,5) = t41 + t105 - 2 * m[2][12] * m[2][3];
+
+  *P.ptr<double>(3,0) = t48 + t1 - 2 * m[1][8] * m[1][5] + t7 - 2 * m[1][7] * m[1][4] + t47;
+  *P.ptr<double>(3,1) = 2 * t16 - 2 * m[1][7] * m[2][4] + 2 * t55 + 2 * t52 - 2 * m[1][8] * m[2][5] - 2 * m[2][8] * m[1][5] - 2 * m[2][7] * m[1][4] + 2 * t12;
+  *P.ptr<double>(3,2) = t24 - 2 * m[2][8] * m[2][5] + t19 - 2 * m[2][7] * m[2][4] + t62 + t59;
+  *P.ptr<double>(3,3) = -2 * m[1][9] * m[1][6] + t64 + t28;
+  *P.ptr<double>(3,4) = 2 * t68 + 2 * t36 - 2 * m[2][9] * m[1][6] - 2 * m[1][9] * m[2][6];
+  *P.ptr<double>(3,5) = t40 + t74 - 2 * m[2][9] * m[2][6];
+
+  *P.ptr<double>(4,0) = t1 - 2 * m[1][10] * m[1][4] + t7 + t78 + t79 - 2 * m[1][11] * m[1][5];
+  *P.ptr<double>(4,1) = 2 * t84 - 2 * m[1][11] * m[2][5] - 2 * m[1][10] * m[2][4] + 2 * t16 - 2 * m[2][11] * m[1][5] + 2 * t87 - 2 * m[2][10] * m[1][4] + 2 * t12;
+  *P.ptr<double>(4,2) = t19 + t24 - 2 * m[2][10] * m[2][4] - 2 * m[2][11] * m[2][5] + t95 + t90;
+  *P.ptr<double>(4,3) = t28 - 2 * m[1][12] * m[1][6] + t99;
+  *P.ptr<double>(4,4) = 2 * t101 + 2 * t36 - 2 * m[2][12] * m[1][6] - 2 * m[1][12] * m[2][6];
+  *P.ptr<double>(4,5) = t105 - 2 * m[2][12] * m[2][6] + t40;
+
+  *P.ptr<double>(5,0) = -2 * m[1][10] * m[1][7] + t47 + t48 + t78 + t79 - 2 * m[1][11] * m[1][8];
+  *P.ptr<double>(5,1) = 2 * t84 + 2 * t87 - 2 * m[2][11] * m[1][8] - 2 * m[1][10] * m[2][7] - 2 * m[2][10] * m[1][7] + 2 * t55 + 2 * t52 - 2 * m[1][11] * m[2][8];
+  *P.ptr<double>(5,2) = -2 * m[2][10] * m[2][7] - 2 * m[2][11] * m[2][8] + t62 + t59 + t90 + t95;
+  *P.ptr<double>(5,3) = t64 - 2 * m[1][12] * m[1][9] + t99;
+  *P.ptr<double>(5,4) = 2 * t68 - 2 * m[2][12] * m[1][9] - 2 * m[1][12] * m[2][9] + 2 * t101;
+  *P.ptr<double>(5,5) = t105 - 2 * m[2][12] * m[2][9] + t74;
+
+  return P;
+}
+
+void upnp::generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])
+{
+  int matrix_to_resolve[18][9] = {
+    { 2, 0, 0, 1, 1, 0, 2, 0, 2 }, { 2, 0, 0, 1, 1, 0, 1, 1, 2 },
+    { 2, 0, 0, 1, 1, 0, 0, 2, 2 }, { 2, 0, 0, 0, 2, 0, 2, 0, 2 },
+    { 2, 0, 0, 0, 2, 0, 1, 1, 2 }, { 2, 0, 0, 0, 2, 0, 0, 2, 2 },
+    { 2, 0, 0, 2, 0, 2, 1, 1, 2 }, { 2, 0, 0, 2, 0, 2, 0, 2, 2 },
+    { 2, 0, 0, 1, 1, 2, 0, 2, 2 }, { 1, 1, 0, 0, 2, 0, 2, 0, 2 },
+    { 1, 1, 0, 0, 2, 0, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
+    { 1, 1, 0, 2, 0, 2, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
+    { 1, 1, 0, 1, 1, 2, 0, 2, 2 }, { 0, 2, 0, 2, 0, 2, 1, 1, 2 },
+    { 0, 2, 0, 2, 0, 2, 0, 2, 2 }, { 0, 2, 0, 1, 1, 2, 0, 2, 2 }
+  };
+
+  int combination[18][3] = {
+    { 1, 2, 4 }, { 1, 2, 5 }, { 1, 2, 6 }, { 1, 3, 4 },
+    { 1, 3, 5 }, { 1, 3, 6 }, { 1, 4, 5 }, { 1, 4, 6 },
+    { 1, 5, 6 }, { 2, 3, 4 }, { 2, 3, 5 }, { 2, 3, 6 },
+    { 2, 4, 5 }, { 2, 4, 6 }, { 2, 5, 6 }, { 3, 4, 5 },
+    { 3, 4, 6 }, { 3, 5, 6 }
+  };
+
+  for (int i = 0; i < 18; ++i) {
+    double matrix[9], independent_term[3];
+    Mat M = Mat(3, 3, CV_64F, matrix);
+    Mat I = Mat(3, 1, CV_64F, independent_term);
+    Mat S = Mat(1, 3, CV_64F);
+
+    for (int j = 0; j < 9; ++j) matrix[j] = (double)matrix_to_resolve[i][j];
+
+    independent_term[0] = log( abs( betas[ combination[i][0]-1 ] ) );
+    independent_term[1] = log( abs( betas[ combination[i][1]-1 ] ) );
+    independent_term[2] = log( abs( betas[ combination[i][2]-1 ] ) );
+
+    exp( Mat(M.inv() * I), S);
+
+    solutions[i][0] = S.at<double>(0);
+    solutions[i][1] = S.at<double>(1) * sign( betas[1] );
+    solutions[i][2] = abs( S.at<double>(2) );
+  }
+}
+
+void upnp::gauss_newton(const Mat * L_6x12, const Mat * Rho, double betas[4], double * f)
+{
+  const int iterations_number = 50;
+
+  double a[6*4], b[6], x[4] = {0};
+  Mat * A = new Mat(6, 4, CV_64F, a);
+  Mat * B = new Mat(6, 1, CV_64F, b);
+  Mat * X = new Mat(4, 1, CV_64F, x);
+
+  for(int k = 0; k < iterations_number; k++)
+  {
+    compute_A_and_b_gauss_newton(L_6x12->ptr<double>(0), Rho->ptr<double>(0), betas, A, B, f[0]);
+    qr_solve(A, B, X);
+    for(int i = 0; i < 3; i++)
+      betas[i] += x[i];
+    f[0] += x[3];
+  }
+
+  if (f[0] < 0) f[0] = -f[0];
+  fu = fv = f[0];
+
+  A->release();
+  delete A;
+
+  B->release();
+  delete B;
+
+  X->release();
+  delete X;
+
+}
+
+void upnp::compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
+        const double betas[4], Mat * A, Mat * b, double const f)
+{
+
+  for(int i = 0; i < 6; i++) {
+    const double * rowL = l_6x12 + i * 12;
+    double * rowA = A->ptr<double>(i);
+
+    rowA[0] = 2 * rowL[0] * betas[0] +    rowL[1] * betas[1] +    rowL[2] * betas[2] + f*f * ( 2 * rowL[6]*betas[0] +    rowL[7]*betas[1]  +    rowL[8]*betas[2] );
+    rowA[1] =    rowL[1] * betas[0] + 2 * rowL[3] * betas[1] +    rowL[4] * betas[2] + f*f * (    rowL[7]*betas[0] + 2 * rowL[9]*betas[1]  +    rowL[10]*betas[2] );
+    rowA[2] =    rowL[2] * betas[0] +    rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + f*f * (    rowL[8]*betas[0] +    rowL[10]*betas[1] + 2 * rowL[11]*betas[2] );
+    rowA[3] = 2*f * ( rowL[6]*betas[0]*betas[0] + rowL[7]*betas[0]*betas[1] + rowL[8]*betas[0]*betas[2] + rowL[9]*betas[1]*betas[1] + rowL[10]*betas[1]*betas[2] + rowL[11]*betas[2]*betas[2] ) ;
+
+    *b->ptr<double>(i) = rho[i] -
+    (
+      rowL[0] * betas[0] * betas[0] +
+      rowL[1] * betas[0] * betas[1] +
+      rowL[2] * betas[0] * betas[2] +
+      rowL[3] * betas[1] * betas[1] +
+      rowL[4] * betas[1] * betas[2] +
+      rowL[5] * betas[2] * betas[2] +
+      f*f * rowL[6] * betas[0] * betas[0] +
+      f*f * rowL[7] * betas[0] * betas[1] +
+      f*f * rowL[8] * betas[0] * betas[2] +
+      f*f * rowL[9] * betas[1] * betas[1] +
+      f*f * rowL[10] * betas[1] * betas[2] +
+      f*f * rowL[11] * betas[2] * betas[2]
+     );
+  }
+}
+
+void upnp::compute_L_6x12(const double * ut, double * l_6x12)
+{
+  const double * v[3];
+
+  v[0] = ut + 12 * 9;
+  v[1] = ut + 12 * 10;
+  v[2] = ut + 12 * 11;
+
+  double dv[3][6][3];
+
+  for(int i = 0; i < 3; i++) {
+    int a = 0, b = 1;
+    for(int j = 0; j < 6; j++) {
+      dv[i][j][0] = v[i][3 * a    ] - v[i][3 * b];
+      dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];
+      dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];
+
+      b++;
+      if (b > 3) {
+        a++;
+        b = a + 1;
+      }
+    }
+  }
+
+  for(int i = 0; i < 6; i++) {
+    double * row = l_6x12 + 12 * i;
+
+    row[0] =         dotXY(dv[0][i], dv[0][i]);
+    row[1] =  2.0f * dotXY(dv[0][i], dv[1][i]);
+    row[2] =         dotXY(dv[1][i], dv[1][i]);
+    row[3] =  2.0f * dotXY(dv[0][i], dv[2][i]);
+    row[4] =  2.0f * dotXY(dv[1][i], dv[2][i]);
+    row[5] =         dotXY(dv[2][i], dv[2][i]);
+
+    row[6] =         dotZ(dv[0][i], dv[0][i]);
+    row[7] =  2.0f * dotZ(dv[0][i], dv[1][i]);
+    row[8] =  2.0f * dotZ(dv[0][i], dv[2][i]);
+    row[9] =         dotZ(dv[1][i], dv[1][i]);
+    row[10] = 2.0f * dotZ(dv[1][i], dv[2][i]);
+    row[11] =        dotZ(dv[2][i], dv[2][i]);
+  }
+}
+
+void upnp::compute_rho(double * rho)
+{
+  rho[0] = dist2(cws[0], cws[1]);
+  rho[1] = dist2(cws[0], cws[2]);
+  rho[2] = dist2(cws[0], cws[3]);
+  rho[3] = dist2(cws[1], cws[2]);
+  rho[4] = dist2(cws[1], cws[3]);
+  rho[5] = dist2(cws[2], cws[3]);
+}
+
+double upnp::dist2(const double * p1, const double * p2)
+{
+  return
+    (p1[0] - p2[0]) * (p1[0] - p2[0]) +
+    (p1[1] - p2[1]) * (p1[1] - p2[1]) +
+    (p1[2] - p2[2]) * (p1[2] - p2[2]);
+}
+
+double upnp::dot(const double * v1, const double * v2)
+{
+  return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+}
+
+double upnp::dotXY(const double * v1, const double * v2)
+{
+  return v1[0] * v2[0] + v1[1] * v2[1];
+}
+
+double upnp::dotZ(const double * v1, const double * v2)
+{
+  return v1[2] * v2[2];
+}
+
+double upnp::sign(const double v)
+{
+  return ( v < 0.0 ) ? -1.0 : ( v > 0.0 ) ? 1.0 : 0.0;
+}
+
+void upnp::qr_solve(Mat * A, Mat * b, Mat * X)
+{
+  const int nr = A->rows;
+  const int nc = A->cols;
+  if (nr <= 0 || nc <= 0)
+      return;
+
+  if (max_nr != 0 && max_nr < nr)
+  {
+    delete [] A1;
+    delete [] A2;
+  }
+  if (max_nr < nr)
+  {
+    max_nr = nr;
+    A1 = new double[nr];
+    A2 = new double[nr];
+  }
+
+  double * pA = A->ptr<double>(0), * ppAkk = pA;
+  for(int k = 0; k < nc; k++)
+  {
+    double * ppAik1 = ppAkk, eta = fabs(*ppAik1);
+    for(int i = k + 1; i < nr; i++)
+    {
+      double elt = fabs(*ppAik1);
+      if (eta < elt) eta = elt;
+      ppAik1 += nc;
+    }
+    if (eta == 0)
+    {
+      A1[k] = A2[k] = 0.0;
+      //cerr << "God damnit, A is singular, this shouldn't happen." << endl;
+      return;
+    }
+    else
+    {
+     double * ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta;
+     for(int i = k; i < nr; i++)
+     {
+       *ppAik2 *= inv_eta;
+       sum2 += *ppAik2 * *ppAik2;
+       ppAik2 += nc;
+     }
+     double sigma = sqrt(sum2);
+     if (*ppAkk < 0)
+     sigma = -sigma;
+     *ppAkk += sigma;
+     A1[k] = sigma * *ppAkk;
+     A2[k] = -eta * sigma;
+     for(int j = k + 1; j < nc; j++)
+     {
+       double * ppAik = ppAkk, sum = 0;
+       for(int i = k; i < nr; i++)
+       {
+        sum += *ppAik * ppAik[j - k];
+        ppAik += nc;
+       }
+       double tau = sum / A1[k];
+       ppAik = ppAkk;
+       for(int i = k; i < nr; i++)
+       {
+        ppAik[j - k] -= tau * *ppAik;
+        ppAik += nc;
+       }
+     }
+    }
+    ppAkk += nc + 1;
+  }
+
+  // b <- Qt b
+  double * ppAjj = pA, * pb = b->ptr<double>(0);
+  for(int j = 0; j < nc; j++)
+  {
+    double * ppAij = ppAjj, tau = 0;
+    for(int i = j; i < nr; i++)
+    {
+     tau += *ppAij * pb[i];
+     ppAij += nc;
+    }
+    tau /= A1[j];
+    ppAij = ppAjj;
+    for(int i = j; i < nr; i++)
+    {
+     pb[i] -= tau * *ppAij;
+     ppAij += nc;
+    }
+    ppAjj += nc + 1;
+  }
+
+  // X = R-1 b
+  double * pX = X->ptr<double>(0);
+  pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
+  for(int i = nc - 2; i >= 0; i--)
+  {
+    double * ppAij = pA + i * nc + (i + 1), sum = 0;
+
+    for(int j = i + 1; j < nc; j++)
+    {
+     sum += *ppAij * pX[j];
+     ppAij++;
+    }
+    pX[i] = (pb[i] - sum) / A2[i];
+  }
+}
+
+#endif
@@ -0,0 +1,140 @@
+//M*//////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of the copyright holders may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+/****************************************************************************************\
+* Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
+* Contributed by Edgar Riba
+\****************************************************************************************/
+
+#ifndef OPENCV_CALIB3D_UPNP_H_
+#define OPENCV_CALIB3D_UPNP_H_
+
+#include "precomp.hpp"
+#include "opencv2/core/core_c.h"
+#include <iostream>
+
+#if 0  // fix buffer overflow first (FIXIT mark in .cpp file)
+
+class upnp
+{
+public:
+    upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
+    ~upnp();
+
+    double compute_pose(cv::Mat& R, cv::Mat& t);
+private:
+    upnp(const upnp &); // copy disabled
+    upnp& operator=(const upnp &); // assign disabled
+    template <typename T>
+      void init_camera_parameters(const cv::Mat& cameraMatrix)
+      {
+        uc = cameraMatrix.at<T> (0, 2);
+        vc = cameraMatrix.at<T> (1, 2);
+        fu = 1;
+        fv = 1;
+      }
+      template <typename OpointType, typename IpointType>
+      void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
+      {
+          for(int i = 0; i < number_of_correspondences; i++)
+          {
+            pws[3 * i    ] = opoints.at<OpointType>(i).x;
+            pws[3 * i + 1] = opoints.at<OpointType>(i).y;
+            pws[3 * i + 2] = opoints.at<OpointType>(i).z;
+
+            us[2 * i    ] = ipoints.at<IpointType>(i).x;
+            us[2 * i + 1] = ipoints.at<IpointType>(i).y;
+          }
+      }
+
+      double reprojection_error(const double R[3][3], const double t[3]);
+      void choose_control_points();
+      void compute_alphas();
+      void fill_M(cv::Mat * M, const int row, const double * alphas, const double u, const double v);
+      void compute_ccs(const double * betas, const double * ut);
+      void compute_pcs(void);
+
+      void solve_for_sign(void);
+
+      void find_betas_and_focal_approx_1(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
+      void find_betas_and_focal_approx_2(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
+      void qr_solve(cv::Mat * A, cv::Mat * b, cv::Mat * X);
+
+      cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1);
+      cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2);
+      void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3]);
+
+      double sign(const double v);
+      double dot(const double * v1, const double * v2);
+      double dotXY(const double * v1, const double * v2);
+      double dotZ(const double * v1, const double * v2);
+      double dist2(const double * p1, const double * p2);
+
+      void compute_rho(double * rho);
+      void compute_L_6x12(const double * ut, double * l_6x12);
+
+      void gauss_newton(const cv::Mat * L_6x12, const cv::Mat * Rho, double current_betas[4], double * efs);
+      void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
+                          const double cb[4], cv::Mat * A, cv::Mat * b, double const f);
+
+      double compute_R_and_t(const double * ut, const double * betas,
+                   double R[3][3], double t[3]);
+
+      void estimate_R_and_t(double R[3][3], double t[3]);
+
+      void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
+                  double R_src[3][3], double t_src[3]);
+
+
+      double uc, vc, fu, fv;
+
+      std::vector<double> pws, us, alphas, pcs;
+      int number_of_correspondences;
+
+      double cws[4][3], ccs[4][3];
+      int max_nr;
+      double * A1, * A2;
+};
+
+#endif
+
+#endif // OPENCV_CALIB3D_UPNP_H_
@@ -46,7 +46,7 @@

 #ifdef HAVE_OPENCL

-namespace cvtest {
+namespace opencv_test {
 namespace ocl {

 PARAM_TEST_CASE(StereoBMFixture, int, int)
@@ -79,7 +79,7 @@ PARAM_TEST_CASE(StereoBMFixture, int, int)

 OCL_TEST_P(StereoBMFixture, StereoBM)
 {
-    Ptr<StereoBM> bm = createStereoBM( n_disp, winSize);
+    Ptr<StereoBM> bm = StereoBM::create( n_disp, winSize);
    bm->setPreFilterType(bm->PREFILTER_XSOBEL);
    bm->setTextureThreshold(0);

@@ -0,0 +1,197 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//                        (3-clause BSD License)
+//
+// Copyright (C) 2015-2016, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistributions in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * Neither the names of the copyright holders nor the names of the contributors
+//     may be used to endorse or promote products derived from this software
+//     without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall copyright holders or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+CV_ENUM(Method, RANSAC, LMEDS)
+typedef TestWithParam<Method> EstimateAffine2D;
+
+static float rngIn(float from, float to) { return from + (to-from) * (float)theRNG(); }
+
+TEST_P(EstimateAffine2D, test3Points)
+{
+    // try more transformations
+    for (size_t i = 0; i < 500; ++i)
+    {
+        Mat aff(2, 3, CV_64F);
+        cv::randu(aff, 1., 3.);
+
+        Mat fpts(1, 3, CV_32FC2);
+        Mat tpts(1, 3, CV_32FC2);
+
+        // setting points that are not in the same line
+        fpts.at<Point2f>(0) = Point2f( rngIn(1,2), rngIn(5,6) );
+        fpts.at<Point2f>(1) = Point2f( rngIn(3,4), rngIn(3,4) );
+        fpts.at<Point2f>(2) = Point2f( rngIn(1,2), rngIn(3,4) );
+
+        transform(fpts, tpts, aff);
+
+        vector<uchar> inliers;
+        Mat aff_est = estimateAffine2D(fpts, tpts, inliers, GetParam() /* method */);
+
+        EXPECT_NEAR(0., cvtest::norm(aff_est, aff, NORM_INF), 1e-3);
+
+        // all must be inliers
+        EXPECT_EQ(countNonZero(inliers), 3);
+    }
+}
+
+TEST_P(EstimateAffine2D, testNPoints)
+{
+    // try more transformations
+    for (size_t i = 0; i < 500; ++i)
+    {
+        Mat aff(2, 3, CV_64F);
+        cv::randu(aff, -2., 2.);
+        const int method = GetParam();
+        const int n = 100;
+        int m;
+        // LMEDS can't handle more than 50% outliers (by design)
+        if (method == LMEDS)
+            m = 3*n/5;
+        else
+            m = 2*n/5;
+        const float shift_outl = 15.f;
+        const float noise_level = 20.f;
+
+        Mat fpts(1, n, CV_32FC2);
+        Mat tpts(1, n, CV_32FC2);
+
+        randu(fpts, 0., 100.);
+        transform(fpts, tpts, aff);
+
+        /* adding noise to some points */
+        Mat outliers = tpts.colRange(m, n);
+        outliers.reshape(1) += shift_outl;
+
+        Mat noise (outliers.size(), outliers.type());
+        randu(noise, 0., noise_level);
+        outliers += noise;
+
+        vector<uchar> inliers;
+        Mat aff_est = estimateAffine2D(fpts, tpts, inliers, method);
+
+        EXPECT_FALSE(aff_est.empty()) << "estimation failed, unable to estimate transform";
+
+        EXPECT_NEAR(0., cvtest::norm(aff_est, aff, NORM_INF), 1e-4);
+
+        bool inliers_good = count(inliers.begin(), inliers.end(), 1) == m &&
+            m == accumulate(inliers.begin(), inliers.begin() + m, 0);
+
+        EXPECT_TRUE(inliers_good);
+    }
+}
+
+// test conversion from other datatypes than float
+TEST_P(EstimateAffine2D, testConversion)
+{
+    Mat aff(2, 3, CV_32S);
+    cv::randu(aff, 1., 3.);
+
+    std::vector<Point> fpts(3);
+    std::vector<Point> tpts(3);
+
+    // setting points that are not in the same line
+    fpts[0] = Point2f( rngIn(1,2), rngIn(5,6) );
+    fpts[1] = Point2f( rngIn(3,4), rngIn(3,4) );
+    fpts[2] = Point2f( rngIn(1,2), rngIn(3,4) );
+
+    transform(fpts, tpts, aff);
+
+    vector<uchar> inliers;
+    Mat aff_est = estimateAffine2D(fpts, tpts, inliers, GetParam() /* method */);
+
+    ASSERT_FALSE(aff_est.empty());
+
+    aff.convertTo(aff, CV_64F); // need to convert before compare
+    EXPECT_NEAR(0., cvtest::norm(aff_est, aff, NORM_INF), 1e-3);
+
+    // all must be inliers
+    EXPECT_EQ(countNonZero(inliers), 3);
+}
+
+INSTANTIATE_TEST_CASE_P(Calib3d, EstimateAffine2D, Method::all());
+
+
+// https://github.com/opencv/opencv/issues/14259
+TEST(EstimateAffine2D, issue_14259_dont_change_inputs)
+{
+    /*const static*/ float pts0_[10] = {
+            0.0f, 0.0f,
+            0.0f, 8.0f,
+            4.0f, 0.0f, // outlier
+            8.0f, 8.0f,
+            8.0f, 0.0f
+    };
+    /*const static*/ float pts1_[10] = {
+            0.1f, 0.1f,
+            0.1f, 8.1f,
+            0.0f, 4.0f, // outlier
+            8.1f, 8.1f,
+            8.1f, 0.1f
+    };
+
+    Mat pts0(Size(1, 5), CV_32FC2, (void*)pts0_);
+    Mat pts1(Size(1, 5), CV_32FC2, (void*)pts1_);
+
+    Mat pts0_copy = pts0.clone();
+    Mat pts1_copy = pts1.clone();
+
+    Mat inliers;
+
+    cv::Mat A = cv::estimateAffine2D(pts0, pts1, inliers);
+
+    for(int i = 0; i < pts0.rows; ++i)
+    {
+        EXPECT_EQ(pts0_copy.at<Vec2f>(i), pts0.at<Vec2f>(i)) << "pts0: i=" << i;
+    }
+
+    for(int i = 0; i < pts1.rows; ++i)
+    {
+        EXPECT_EQ(pts1_copy.at<Vec2f>(i), pts1.at<Vec2f>(i)) << "pts1: i=" << i;
+    }
+
+    EXPECT_EQ(0, (int)inliers.at<uchar>(2));
+}
+
+}} // namespace
@@ -42,21 +42,20 @@

 #include "test_precomp.hpp"
 #include "opencv2/core/affine.hpp"
-#include "opencv2/calib3d.hpp"
-#include <iostream>
+
+namespace opencv_test { namespace {

 TEST(Calib3d_Affine3f, accuracy)
 {
+    const double eps = 1e-5;
    cv::Vec3d rvec(0.2, 0.5, 0.3);
    cv::Affine3d affine(rvec);

    cv::Mat expected;
    cv::Rodrigues(rvec, expected);

-
-    ASSERT_EQ(0, cvtest::norm(cv::Mat(affine.matrix, false).colRange(0, 3).rowRange(0, 3) != expected, cv::NORM_L2));
-    ASSERT_EQ(0, cvtest::norm(cv::Mat(affine.linear()) != expected, cv::NORM_L2));
-
+    ASSERT_LE(cvtest::norm(cv::Mat(affine.matrix, false).colRange(0, 3).rowRange(0, 3), expected, cv::NORM_L2), eps);
+    ASSERT_LE(cvtest::norm(cv::Mat(affine.linear()), expected, cv::NORM_L2), eps);

    cv::Matx33d R = cv::Matx33d::eye();

@@ -106,3 +105,5 @@ TEST(Calib3d_Affine3f, accuracy_rvec)
        ASSERT_LT(cvtest::norm(va, vo, cv::NORM_L2), 1e-9);
    }
 }
+
+}} // namespace
@@ -42,16 +42,7 @@

 #include "test_precomp.hpp"

-using namespace cv;
-using namespace std;
-
-#include <string>
-#include <iostream>
-#include <fstream>
-#include <functional>
-#include <iterator>
-#include <limits>
-#include <numeric>
+namespace opencv_test { namespace {

 class CV_Affine3D_EstTest : public cvtest::BaseTest
 {
@@ -80,9 +71,9 @@ struct WrapAff
    WrapAff(const Mat& aff) : F(aff.ptr<double>()) {}
    Point3f operator()(const Point3f& p)
    {
-        return Point3d( p.x * F[0] + p.y * F[1] + p.z *  F[2] +  F[3],
-                        p.x * F[4] + p.y * F[5] + p.z *  F[6] +  F[7],
-                        p.x * F[8] + p.y * F[9] + p.z * F[10] + F[11]  );
+        return Point3f( (float)(p.x * F[0] + p.y * F[1] + p.z *  F[2] +  F[3]),
+                        (float)(p.x * F[4] + p.y * F[5] + p.z *  F[6] +  F[7]),
+                        (float)(p.x * F[8] + p.y * F[9] + p.z * F[10] + F[11])  );
    }
 };

@@ -101,7 +92,7 @@ bool CV_Affine3D_EstTest::test4Points()
    fpts.ptr<Point3f>()[2] = Point3f( rngIn(1,2), rngIn(3,4), rngIn(5, 6) );
    fpts.ptr<Point3f>()[3] = Point3f( rngIn(3,4), rngIn(1,2), rngIn(5, 6) );

-    transform(fpts.ptr<Point3f>(), fpts.ptr<Point3f>() + 4, tpts.ptr<Point3f>(), WrapAff(aff));
+    std::transform(fpts.ptr<Point3f>(), fpts.ptr<Point3f>() + 4, tpts.ptr<Point3f>(), WrapAff(aff));

    Mat aff_est;
    vector<uchar> outliers;
@@ -144,11 +135,16 @@ bool CV_Affine3D_EstTest::testNPoints()
    Mat tpts(1, n, CV_32FC3);

    randu(fpts, Scalar::all(0), Scalar::all(100));
-    transform(fpts.ptr<Point3f>(), fpts.ptr<Point3f>() + n, tpts.ptr<Point3f>(), WrapAff(aff));
+    std::transform(fpts.ptr<Point3f>(), fpts.ptr<Point3f>() + n, tpts.ptr<Point3f>(), WrapAff(aff));

    /* adding noise*/
-    transform(tpts.ptr<Point3f>() + m, tpts.ptr<Point3f>() + n, tpts.ptr<Point3f>() + m, bind2nd(plus<Point3f>(), shift_outl));
-    transform(tpts.ptr<Point3f>() + m, tpts.ptr<Point3f>() + n, tpts.ptr<Point3f>() + m, Noise(noise_level));
+#ifdef CV_CXX11
+    std::transform(tpts.ptr<Point3f>() + m, tpts.ptr<Point3f>() + n, tpts.ptr<Point3f>() + m,
+        [=] (const Point3f& pt) -> Point3f { return Noise(noise_level)(pt + shift_outl); });
+#else
+    std::transform(tpts.ptr<Point3f>() + m, tpts.ptr<Point3f>() + n, tpts.ptr<Point3f>() + m, std::bind2nd(std::plus<Point3f>(), shift_outl));
+    std::transform(tpts.ptr<Point3f>() + m, tpts.ptr<Point3f>() + n, tpts.ptr<Point3f>() + m, Noise(noise_level));
+#endif

    Mat aff_est;
    vector<uchar> outl;
@@ -194,4 +190,20 @@ void CV_Affine3D_EstTest::run( int /* start_from */)
    ts->set_failed_test_info(cvtest::TS::OK);
 }

-TEST(Calib3d_EstimateAffineTransform, accuracy) { CV_Affine3D_EstTest test; test.safe_run(); }
+TEST(Calib3d_EstimateAffine3D, accuracy) { CV_Affine3D_EstTest test; test.safe_run(); }
+
+TEST(Calib3d_EstimateAffine3D, regression_16007)
+{
+    std::vector<cv::Point3f> m1, m2;
+    m1.push_back(Point3f(1.0f, 0.0f, 0.0f)); m2.push_back(Point3f(1.0f, 1.0f, 0.0f));
+    m1.push_back(Point3f(1.0f, 0.0f, 1.0f)); m2.push_back(Point3f(1.0f, 1.0f, 1.0f));
+    m1.push_back(Point3f(0.5f, 0.0f, 0.5f)); m2.push_back(Point3f(0.5f, 1.0f, 0.5f));
+    m1.push_back(Point3f(2.5f, 0.0f, 2.5f)); m2.push_back(Point3f(2.5f, 1.0f, 2.5f));
+    m1.push_back(Point3f(2.0f, 0.0f, 1.0f)); m2.push_back(Point3f(2.0f, 1.0f, 1.0f));
+
+    cv::Mat m3D, inl;
+    int res = cv::estimateAffine3D(m1, m2, m3D, inl);
+    EXPECT_EQ(1, res);
+}
+
+}} // namespace
@@ -0,0 +1,206 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//                        (3-clause BSD License)
+//
+// Copyright (C) 2015-2016, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistributions in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * Neither the names of the copyright holders nor the names of the contributors
+//     may be used to endorse or promote products derived from this software
+//     without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall copyright holders or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+CV_ENUM(Method, RANSAC, LMEDS)
+typedef TestWithParam<Method> EstimateAffinePartial2D;
+
+static float rngIn(float from, float to) { return from + (to-from) * (float)theRNG(); }
+
+// get random matrix of affine transformation limited to combinations of translation,
+// rotation, and uniform scaling
+static Mat rngPartialAffMat() {
+    double theta = rngIn(0, (float)CV_PI*2.f);
+    double scale = rngIn(0, 3);
+    double tx = rngIn(-2, 2);
+    double ty = rngIn(-2, 2);
+    double aff[2*3] = { std::cos(theta) * scale, -std::sin(theta) * scale, tx,
+                        std::sin(theta) * scale,  std::cos(theta) * scale, ty };
+    return Mat(2, 3, CV_64F, aff).clone();
+}
+
+TEST_P(EstimateAffinePartial2D, test2Points)
+{
+    // try more transformations
+    for (size_t i = 0; i < 500; ++i)
+    {
+        Mat aff = rngPartialAffMat();
+
+        // setting points that are no in the same line
+        Mat fpts(1, 2, CV_32FC2);
+        Mat tpts(1, 2, CV_32FC2);
+
+        fpts.at<Point2f>(0) = Point2f( rngIn(1,2), rngIn(5,6) );
+        fpts.at<Point2f>(1) = Point2f( rngIn(3,4), rngIn(3,4) );
+
+        transform(fpts, tpts, aff);
+
+        vector<uchar> inliers;
+        Mat aff_est = estimateAffinePartial2D(fpts, tpts, inliers, GetParam() /* method */);
+
+        EXPECT_NEAR(0., cvtest::norm(aff_est, aff, NORM_INF), 1e-3);
+
+        // all must be inliers
+        EXPECT_EQ(countNonZero(inliers), 2);
+    }
+}
+
+TEST_P(EstimateAffinePartial2D, testNPoints)
+{
+    // try more transformations
+    for (size_t i = 0; i < 500; ++i)
+    {
+        Mat aff = rngPartialAffMat();
+
+        const int method = GetParam();
+        const int n = 100;
+        int m;
+        // LMEDS can't handle more than 50% outliers (by design)
+        if (method == LMEDS)
+            m = 3*n/5;
+        else
+            m = 2*n/5;
+        const float shift_outl = 15.f;
+        const float noise_level = 20.f;
+
+        Mat fpts(1, n, CV_32FC2);
+        Mat tpts(1, n, CV_32FC2);
+
+        randu(fpts, 0., 100.);
+        transform(fpts, tpts, aff);
+
+        /* adding noise to some points */
+        Mat outliers = tpts.colRange(m, n);
+        outliers.reshape(1) += shift_outl;
+
+        Mat noise (outliers.size(), outliers.type());
+        randu(noise, 0., noise_level);
+        outliers += noise;
+
+        vector<uchar> inliers;
+        Mat aff_est = estimateAffinePartial2D(fpts, tpts, inliers, method);
+
+        EXPECT_FALSE(aff_est.empty());
+
+        EXPECT_NEAR(0., cvtest::norm(aff_est, aff, NORM_INF), 1e-4);
+
+        bool inliers_good = count(inliers.begin(), inliers.end(), 1) == m &&
+            m == accumulate(inliers.begin(), inliers.begin() + m, 0);
+
+        EXPECT_TRUE(inliers_good);
+    }
+}
+
+// test conversion from other datatypes than float
+TEST_P(EstimateAffinePartial2D, testConversion)
+{
+    Mat aff = rngPartialAffMat();
+    aff.convertTo(aff, CV_32S); // convert to int to transform ints properly
+
+    std::vector<Point> fpts(3);
+    std::vector<Point> tpts(3);
+
+    fpts[0] = Point2f( rngIn(1,2), rngIn(5,6) );
+    fpts[1] = Point2f( rngIn(3,4), rngIn(3,4) );
+    fpts[2] = Point2f( rngIn(1,2), rngIn(3,4) );
+
+    transform(fpts, tpts, aff);
+
+    vector<uchar> inliers;
+    Mat aff_est = estimateAffinePartial2D(fpts, tpts, inliers, GetParam() /* method */);
+
+    ASSERT_FALSE(aff_est.empty());
+
+    aff.convertTo(aff, CV_64F); // need to convert back before compare
+    EXPECT_NEAR(0., cvtest::norm(aff_est, aff, NORM_INF), 1e-3);
+
+    // all must be inliers
+    EXPECT_EQ(countNonZero(inliers), 3);
+}
+
+INSTANTIATE_TEST_CASE_P(Calib3d, EstimateAffinePartial2D, Method::all());
+
+
+// https://github.com/opencv/opencv/issues/14259
+TEST(EstimateAffinePartial2D, issue_14259_dont_change_inputs)
+{
+    /*const static*/ float pts0_[10] = {
+            0.0f, 0.0f,
+            0.0f, 8.0f,
+            4.0f, 0.0f, // outlier
+            8.0f, 8.0f,
+            8.0f, 0.0f
+    };
+    /*const static*/ float pts1_[10] = {
+            0.1f, 0.1f,
+            0.1f, 8.1f,
+            0.0f, 4.0f, // outlier
+            8.1f, 8.1f,
+            8.1f, 0.1f
+    };
+
+    Mat pts0(Size(1, 5), CV_32FC2, (void*)pts0_);
+    Mat pts1(Size(1, 5), CV_32FC2, (void*)pts1_);
+
+    Mat pts0_copy = pts0.clone();
+    Mat pts1_copy = pts1.clone();
+
+    Mat inliers;
+
+    cv::Mat A = cv::estimateAffinePartial2D(pts0, pts1, inliers);
+
+    for(int i = 0; i < pts0.rows; ++i)
+    {
+        EXPECT_EQ(pts0_copy.at<Vec2f>(i), pts0.at<Vec2f>(i)) << "pts0: i=" << i;
+    }
+
+    for(int i = 0; i < pts1.rows; ++i)
+    {
+        EXPECT_EQ(pts1_copy.at<Vec2f>(i), pts1.at<Vec2f>(i)) << "pts1: i=" << i;
+    }
+
+    EXPECT_EQ(0, (int)inliers.at<uchar>(2));
+}
+
+}} // namespace
@@ -0,0 +1,381 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "test_precomp.hpp"
+#include "opencv2/calib3d.hpp"
+
+namespace opencv_test { namespace {
+
+class CV_CalibrateHandEyeTest : public cvtest::BaseTest
+{
+public:
+    CV_CalibrateHandEyeTest() {
+        eps_rvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
+        eps_rvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
+        eps_rvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
+        eps_rvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
+        eps_rvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
+
+        eps_tvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
+        eps_tvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
+        eps_tvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
+        eps_tvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
+        eps_tvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
+
+        eps_rvec_noise[CALIB_HAND_EYE_TSAI] = 2.0e-2;
+        eps_rvec_noise[CALIB_HAND_EYE_PARK] = 2.0e-2;
+        eps_rvec_noise[CALIB_HAND_EYE_HORAUD] = 2.0e-2;
+        eps_rvec_noise[CALIB_HAND_EYE_ANDREFF] = 1.0e-2;
+        eps_rvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-2;
+
+        eps_tvec_noise[CALIB_HAND_EYE_TSAI] = 5.0e-2;
+        eps_tvec_noise[CALIB_HAND_EYE_PARK] = 5.0e-2;
+        eps_tvec_noise[CALIB_HAND_EYE_HORAUD] = 5.0e-2;
+        eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 5.0e-2;
+        eps_tvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 5.0e-2;
+    }
+protected:
+    virtual void run(int);
+    void generatePose(RNG& rng, double min_theta, double max_theta,
+                      double min_tx, double max_tx,
+                      double min_ty, double max_ty,
+                      double min_tz, double max_tz,
+                      Mat& R, Mat& tvec,
+                      bool randSign=false);
+    void simulateData(RNG& rng, int nPoses,
+                      std::vector<Mat> &R_gripper2base, std::vector<Mat> &t_gripper2base,
+                      std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
+                      bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper);
+    Mat homogeneousInverse(const Mat& T);
+    std::string getMethodName(HandEyeCalibrationMethod method);
+    double sign_double(double val);
+
+    double eps_rvec[5];
+    double eps_tvec[5];
+    double eps_rvec_noise[5];
+    double eps_tvec_noise[5];
+};
+
+void CV_CalibrateHandEyeTest::run(int)
+{
+    ts->set_failed_test_info(cvtest::TS::OK);
+
+    RNG& rng = ts->get_rng();
+
+    std::vector<std::vector<double> > vec_rvec_diff(5);
+    std::vector<std::vector<double> > vec_tvec_diff(5);
+    std::vector<std::vector<double> > vec_rvec_diff_noise(5);
+    std::vector<std::vector<double> > vec_tvec_diff_noise(5);
+
+    std::vector<HandEyeCalibrationMethod> methods;
+    methods.push_back(CALIB_HAND_EYE_TSAI);
+    methods.push_back(CALIB_HAND_EYE_PARK);
+    methods.push_back(CALIB_HAND_EYE_HORAUD);
+    methods.push_back(CALIB_HAND_EYE_ANDREFF);
+    methods.push_back(CALIB_HAND_EYE_DANIILIDIS);
+
+    const int nTests = 100;
+    for (int i = 0; i < nTests; i++)
+    {
+        const int nPoses = 10;
+        {
+            //No noise
+            std::vector<Mat> R_gripper2base, t_gripper2base;
+            std::vector<Mat> R_target2cam, t_target2cam;
+            Mat R_cam2gripper_true, t_cam2gripper_true;
+
+            const bool noise = false;
+            simulateData(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, R_cam2gripper_true, t_cam2gripper_true);
+
+            for (size_t idx = 0; idx < methods.size(); idx++)
+            {
+                Mat rvec_cam2gripper_true;
+                cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
+
+                Mat R_cam2gripper_est, t_cam2gripper_est;
+                calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
+
+                Mat rvec_cam2gripper_est;
+                cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
+
+                double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
+                double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
+
+                vec_rvec_diff[idx].push_back(rvecDiff);
+                vec_tvec_diff[idx].push_back(tvecDiff);
+
+                const double epsilon_rvec = eps_rvec[idx];
+                const double epsilon_tvec = eps_tvec[idx];
+
+                //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
+                if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
+                {
+                    ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
+                               getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
+                    ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+                }
+            }
+        }
+
+        {
+            //Gaussian noise on transformations between calibration target frame and camera frame and between gripper and robot base frames
+            std::vector<Mat> R_gripper2base, t_gripper2base;
+            std::vector<Mat> R_target2cam, t_target2cam;
+            Mat R_cam2gripper_true, t_cam2gripper_true;
+
+            const bool noise = true;
+            simulateData(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, R_cam2gripper_true, t_cam2gripper_true);
+
+            for (size_t idx = 0; idx < methods.size(); idx++)
+            {
+                Mat rvec_cam2gripper_true;
+                cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
+
+                Mat R_cam2gripper_est, t_cam2gripper_est;
+                calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
+
+                Mat rvec_cam2gripper_est;
+                cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
+
+                double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
+                double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
+
+                vec_rvec_diff_noise[idx].push_back(rvecDiff);
+                vec_tvec_diff_noise[idx].push_back(tvecDiff);
+
+                const double epsilon_rvec = eps_rvec_noise[idx];
+                const double epsilon_tvec = eps_tvec_noise[idx];
+
+                //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
+                if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
+                {
+                    ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
+                               getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
+                    ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+                }
+            }
+        }
+    }
+
+    for (size_t idx = 0; idx < methods.size(); idx++)
+    {
+        {
+            double max_rvec_diff = *std::max_element(vec_rvec_diff[idx].begin(), vec_rvec_diff[idx].end());
+            double mean_rvec_diff = std::accumulate(vec_rvec_diff[idx].begin(),
+                                                    vec_rvec_diff[idx].end(), 0.0) / vec_rvec_diff[idx].size();
+            double sq_sum_rvec_diff = std::inner_product(vec_rvec_diff[idx].begin(), vec_rvec_diff[idx].end(),
+                                                         vec_rvec_diff[idx].begin(), 0.0);
+            double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / vec_rvec_diff[idx].size() - mean_rvec_diff * mean_rvec_diff);
+
+            double max_tvec_diff = *std::max_element(vec_tvec_diff[idx].begin(), vec_tvec_diff[idx].end());
+            double mean_tvec_diff = std::accumulate(vec_tvec_diff[idx].begin(),
+                                                    vec_tvec_diff[idx].end(), 0.0) / vec_tvec_diff[idx].size();
+            double sq_sum_tvec_diff = std::inner_product(vec_tvec_diff[idx].begin(), vec_tvec_diff[idx].end(),
+                                                         vec_tvec_diff[idx].begin(), 0.0);
+            double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / vec_tvec_diff[idx].size() - mean_tvec_diff * mean_tvec_diff);
+
+            std::cout << "\nMethod " << getMethodName(methods[idx]) << ":\n"
+                      << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff
+                      << ", Std rvec error: " << std_rvec_diff << "\n"
+                      << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff
+                      << ", Std tvec error: " << std_tvec_diff << std::endl;
+        }
+        {
+            double max_rvec_diff = *std::max_element(vec_rvec_diff_noise[idx].begin(), vec_rvec_diff_noise[idx].end());
+            double mean_rvec_diff = std::accumulate(vec_rvec_diff_noise[idx].begin(),
+                                                    vec_rvec_diff_noise[idx].end(), 0.0) / vec_rvec_diff_noise[idx].size();
+            double sq_sum_rvec_diff = std::inner_product(vec_rvec_diff_noise[idx].begin(), vec_rvec_diff_noise[idx].end(),
+                                                         vec_rvec_diff_noise[idx].begin(), 0.0);
+            double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / vec_rvec_diff_noise[idx].size() - mean_rvec_diff * mean_rvec_diff);
+
+            double max_tvec_diff = *std::max_element(vec_tvec_diff_noise[idx].begin(), vec_tvec_diff_noise[idx].end());
+            double mean_tvec_diff = std::accumulate(vec_tvec_diff_noise[idx].begin(),
+                                                    vec_tvec_diff_noise[idx].end(), 0.0) / vec_tvec_diff_noise[idx].size();
+            double sq_sum_tvec_diff = std::inner_product(vec_tvec_diff_noise[idx].begin(), vec_tvec_diff_noise[idx].end(),
+                                                         vec_tvec_diff_noise[idx].begin(), 0.0);
+            double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / vec_tvec_diff_noise[idx].size() - mean_tvec_diff * mean_tvec_diff);
+
+            std::cout << "Method (noise) " << getMethodName(methods[idx]) << ":\n"
+                      << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff
+                      << ", Std rvec error: " << std_rvec_diff << "\n"
+                      << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff
+                      << ", Std tvec error: " << std_tvec_diff << std::endl;
+        }
+    }
+}
+
+void CV_CalibrateHandEyeTest::generatePose(RNG& rng, double min_theta, double max_theta,
+                                               double min_tx, double max_tx,
+                                               double min_ty, double max_ty,
+                                               double min_tz, double max_tz,
+                                               Mat& R, Mat& tvec,
+                                               bool random_sign)
+{
+    Mat axis(3, 1, CV_64FC1);
+    for (int i = 0; i < 3; i++)
+    {
+        axis.at<double>(i,0) = rng.uniform(-1.0, 1.0);
+    }
+    double theta = rng.uniform(min_theta, max_theta);
+    if (random_sign)
+    {
+        theta *= sign_double(rng.uniform(-1.0, 1.0));
+    }
+
+    Mat rvec(3, 1, CV_64FC1);
+    rvec.at<double>(0,0) = theta*axis.at<double>(0,0);
+    rvec.at<double>(1,0) = theta*axis.at<double>(1,0);
+    rvec.at<double>(2,0) = theta*axis.at<double>(2,0);
+
+    tvec.create(3, 1, CV_64FC1);
+    tvec.at<double>(0,0) = rng.uniform(min_tx, max_tx);
+    tvec.at<double>(1,0) = rng.uniform(min_ty, max_ty);
+    tvec.at<double>(2,0) = rng.uniform(min_tz, max_tz);
+
+    if (random_sign)
+    {
+        tvec.at<double>(0,0) *= sign_double(rng.uniform(-1.0, 1.0));
+        tvec.at<double>(1,0) *= sign_double(rng.uniform(-1.0, 1.0));
+        tvec.at<double>(2,0) *= sign_double(rng.uniform(-1.0, 1.0));
+    }
+
+    cv::Rodrigues(rvec, R);
+}
+
+void CV_CalibrateHandEyeTest::simulateData(RNG& rng, int nPoses,
+                                               std::vector<Mat> &R_gripper2base, std::vector<Mat> &t_gripper2base,
+                                               std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
+                                               bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper)
+{
+    //to avoid generating values close to zero,
+    //we use positive range values and randomize the sign
+    const bool random_sign = true;
+    generatePose(rng, 10.0*CV_PI/180.0, 50.0*CV_PI/180.0,
+                 0.05, 0.5, 0.05, 0.5, 0.05, 0.5,
+                 R_cam2gripper, t_cam2gripper, random_sign);
+
+    Mat R_target2base, t_target2base;
+    generatePose(rng, 5.0*CV_PI/180.0, 85.0*CV_PI/180.0,
+                 0.5, 3.5, 0.5, 3.5, 0.5, 3.5,
+                 R_target2base, t_target2base, random_sign);
+
+    for (int i = 0; i < nPoses; i++)
+    {
+        Mat R_gripper2base_, t_gripper2base_;
+        generatePose(rng, 5.0*CV_PI/180.0, 45.0*CV_PI/180.0,
+                     0.5, 1.5, 0.5, 1.5, 0.5, 1.5,
+                     R_gripper2base_, t_gripper2base_, random_sign);
+
+        R_gripper2base.push_back(R_gripper2base_);
+        t_gripper2base.push_back(t_gripper2base_);
+
+        Mat T_cam2gripper = Mat::eye(4, 4, CV_64FC1);
+        R_cam2gripper.copyTo(T_cam2gripper(Rect(0, 0, 3, 3)));
+        t_cam2gripper.copyTo(T_cam2gripper(Rect(3, 0, 1, 3)));
+
+        Mat T_gripper2base = Mat::eye(4, 4, CV_64FC1);
+        R_gripper2base_.copyTo(T_gripper2base(Rect(0, 0, 3, 3)));
+        t_gripper2base_.copyTo(T_gripper2base(Rect(3, 0, 1, 3)));
+
+        Mat T_base2cam = homogeneousInverse(T_cam2gripper) * homogeneousInverse(T_gripper2base);
+        Mat T_target2base = Mat::eye(4, 4, CV_64FC1);
+        R_target2base.copyTo(T_target2base(Rect(0, 0, 3, 3)));
+        t_target2base.copyTo(T_target2base(Rect(3, 0, 1, 3)));
+        Mat T_target2cam = T_base2cam * T_target2base;
+
+        if (noise)
+        {
+            //Add some noise for the transformation between the target and the camera
+            Mat R_target2cam_noise = T_target2cam(Rect(0, 0, 3, 3));
+            Mat rvec_target2cam_noise;
+            cv::Rodrigues(R_target2cam_noise, rvec_target2cam_noise);
+            rvec_target2cam_noise.at<double>(0,0) += rng.gaussian(0.002);
+            rvec_target2cam_noise.at<double>(1,0) += rng.gaussian(0.002);
+            rvec_target2cam_noise.at<double>(2,0) += rng.gaussian(0.002);
+
+            cv::Rodrigues(rvec_target2cam_noise, R_target2cam_noise);
+
+            Mat t_target2cam_noise = T_target2cam(Rect(3, 0, 1, 3));
+            t_target2cam_noise.at<double>(0,0) += rng.gaussian(0.005);
+            t_target2cam_noise.at<double>(1,0) += rng.gaussian(0.005);
+            t_target2cam_noise.at<double>(2,0) += rng.gaussian(0.005);
+
+            //Add some noise for the transformation between the gripper and the robot base
+            Mat R_gripper2base_noise = T_gripper2base(Rect(0, 0, 3, 3));
+            Mat rvec_gripper2base_noise;
+            cv::Rodrigues(R_gripper2base_noise, rvec_gripper2base_noise);
+            rvec_gripper2base_noise.at<double>(0,0) += rng.gaussian(0.001);
+            rvec_gripper2base_noise.at<double>(1,0) += rng.gaussian(0.001);
+            rvec_gripper2base_noise.at<double>(2,0) += rng.gaussian(0.001);
+
+            cv::Rodrigues(rvec_gripper2base_noise, R_gripper2base_noise);
+
+            Mat t_gripper2base_noise = T_gripper2base(Rect(3, 0, 1, 3));
+            t_gripper2base_noise.at<double>(0,0) += rng.gaussian(0.001);
+            t_gripper2base_noise.at<double>(1,0) += rng.gaussian(0.001);
+            t_gripper2base_noise.at<double>(2,0) += rng.gaussian(0.001);
+        }
+
+        R_target2cam.push_back(T_target2cam(Rect(0, 0, 3, 3)));
+        t_target2cam.push_back(T_target2cam(Rect(3, 0, 1, 3)));
+    }
+}
+
+Mat CV_CalibrateHandEyeTest::homogeneousInverse(const Mat& T)
+{
+    CV_Assert( T.rows == 4 && T.cols == 4 );
+
+    Mat R = T(Rect(0, 0, 3, 3));
+    Mat t = T(Rect(3, 0, 1, 3));
+    Mat Rt = R.t();
+    Mat tinv = -Rt * t;
+    Mat Tinv = Mat::eye(4, 4, T.type());
+    Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
+    tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
+
+    return Tinv;
+}
+
+std::string CV_CalibrateHandEyeTest::getMethodName(HandEyeCalibrationMethod method)
+{
+    std::string method_name = "";
+    switch (method)
+    {
+    case CALIB_HAND_EYE_TSAI:
+        method_name = "Tsai";
+        break;
+
+    case CALIB_HAND_EYE_PARK:
+        method_name = "Park";
+        break;
+
+    case CALIB_HAND_EYE_HORAUD:
+        method_name = "Horaud";
+        break;
+
+    case CALIB_HAND_EYE_ANDREFF:
+        method_name = "Andreff";
+        break;
+
+    case CALIB_HAND_EYE_DANIILIDIS:
+        method_name = "Daniilidis";
+        break;
+
+    default:
+        break;
+    }
+
+    return method_name;
+}
+
+double CV_CalibrateHandEyeTest::sign_double(double val)
+{
+    return (0 < val) - (val < 0);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST(Calib3d_CalibrateHandEye, regression) { CV_CalibrateHandEyeTest test; test.safe_run(); }
+
+}} // namespace
@@ -41,18 +41,9 @@
 //M*/

 #include "test_precomp.hpp"
-
-#include <string>
-#include <limits>
-#include <vector>
-#include <iostream>
-#include <sstream>
-#include <iomanip>
-
 #include "test_chessboardgenerator.hpp"

-using namespace cv;
-using namespace std;
+namespace opencv_test { namespace {

 //template<class T> ostream& operator<<(ostream& out, const Mat_<T>& mat)
 //{
@@ -75,10 +66,10 @@ Mat calcRvec(const vector<Point3f>& points, const Size& cornerSize)
    Mat rot(3, 3, CV_64F);
    *rot.ptr<Vec3d>(0) = ex;
    *rot.ptr<Vec3d>(1) = ey;
-    *rot.ptr<Vec3d>(2) = ez * (1.0/norm(ez));
+    *rot.ptr<Vec3d>(2) = ez * (1.0/cv::norm(ez)); // TODO cvtest

    Mat res;
-    Rodrigues(rot.t(), res);
+    cvtest::Rodrigues(rot.t(), res);
    return res.reshape(1, 1);
 }

@@ -154,7 +145,7 @@ protected:

        if (fail)
        {
-            // commented according to vp123's recomendation. TODO - improve accuaracy
+            // commented according to vp123's recommendation. TODO - improve accuracy
            //ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); ss
        }
        ts->printf( cvtest::TS::LOG, "%d) DistCoeff exp=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1, k2, p1, p2, k3);
@@ -174,7 +165,9 @@ protected:
            const Point3d& tvec = *tvecs[i].ptr<Point3d>();
            const Point3d& tvec_est = *tvecs_est[i].ptr<Point3d>();

-            if (norm(tvec_est - tvec) > eps* (norm(tvec) + dlt))
+            double n1 = cv::norm(tvec_est - tvec); // TODO cvtest
+            double n2 = cv::norm(tvec); // TODO cvtest
+            if (n1 > eps* (n2 + dlt))
            {
                if (err_count++ < errMsgNum)
                {
@@ -183,7 +176,7 @@ protected:
                    else
                    {
                        ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned tvecs. Index = %d\n", r, i);
-                        ts->printf( cvtest::TS::LOG, "%d) norm(tvec_est - tvec) = %f, norm(tvec_exp) = %f \n", r, norm(tvec_est - tvec), norm(tvec));
+                        ts->printf( cvtest::TS::LOG, "%d) norm(tvec_est - tvec) = %f, norm(tvec_exp) = %f \n", r, n1, n2);
                    }
                }
                ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
@@ -201,8 +194,8 @@ protected:
        const int errMsgNum = 4;
        for(size_t i = 0; i < rvecs.size(); ++i)
        {
-            Rodrigues(rvecs[i], rmat);
-            Rodrigues(rvecs_est[i], rmat_est);
+            cvtest::Rodrigues(rvecs[i], rmat);
+            cvtest::Rodrigues(rvecs_est[i], rmat_est);

            if (cvtest::norm(rmat_est, rmat, NORM_L2) > eps* (cvtest::norm(rmat, NORM_L2) + dlt))
            {
@@ -237,7 +230,7 @@ protected:
            projectPoints(_chessboard3D, _rvecs_exp[i], _tvecs_exp[i], eye33, zero15, uv_exp);
            projectPoints(_chessboard3D, rvecs_est[i], tvecs_est[i], eye33, zero15, uv_est);
            for(size_t j = 0; j < cb3d.size(); ++j)
-                res += norm(uv_exp[i] - uv_est[i]);
+                res += cv::norm(uv_exp[i] - uv_est[i]); // TODO cvtest
        }
        return res;
    }
@@ -360,7 +353,7 @@ protected:
        rvecs_spnp.resize(brdsNum);
        tvecs_spnp.resize(brdsNum);
        for(size_t i = 0; i < brdsNum; ++i)
-            solvePnP(Mat(objectPoints[i]), Mat(imagePoints[i]), camMat, distCoeffs, rvecs_spnp[i], tvecs_spnp[i]);
+            solvePnP(objectPoints[i], imagePoints[i], camMat, distCoeffs, rvecs_spnp[i], tvecs_spnp[i]);

        compareShiftVecs(tvecs_exp, tvecs_spnp);
        compareRotationVecs(rvecs_exp, rvecs_spnp);
@@ -429,3 +422,5 @@ protected:
 };

 TEST(Calib3d_CalibrateCamera_CPP, DISABLED_accuracy_on_artificial_data) { CV_CalibrateCameraArtificialTest test; test.safe_run(); }
+
+}} // namespace
@@ -43,10 +43,7 @@
 #include "test_chessboardgenerator.hpp"
 #include "opencv2/calib3d/calib3d_c.h"

-#include <iostream>
-
-using namespace cv;
-using namespace std;
+namespace opencv_test { namespace {

 class CV_CameraCalibrationBadArgTest : public cvtest::BadArgTest
 {
@@ -78,7 +75,7 @@ protected:

        void operator()() const
        {
-            cvCalibrateCamera2(objPts, imgPts, npoints, imageSize,
+            cvCalibrateCamera2(objPts, imgPts, npoints, cvSize(imageSize),
                cameraMatrix, distCoeffs, rvecs, tvecs, flags );
        }
    };
@@ -140,13 +137,13 @@ void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )
    //CV_CALIB_FIX_PRINCIPAL_POINT    //CV_CALIB_ZERO_TANGENT_DIST
    //CV_CALIB_FIX_FOCAL_LENGTH    //CV_CALIB_FIX_K1    //CV_CALIB_FIX_K2    //CV_CALIB_FIX_K3

-    objPts = objPts_cpp;
-    imgPts = imgPts_cpp;
-    npoints = npoints_cpp;
-    cameraMatrix = cameraMatrix_cpp;
-    distCoeffs = distCoeffs_cpp;
-    rvecs = rvecs_cpp;
-    tvecs = tvecs_cpp;
+    objPts = cvMat(objPts_cpp);
+    imgPts = cvMat(imgPts_cpp);
+    npoints = cvMat(npoints_cpp);
+    cameraMatrix = cvMat(cameraMatrix_cpp);
+    distCoeffs = cvMat(distCoeffs_cpp);
+    rvecs = cvMat(rvecs_cpp);
+    tvecs = cvMat(tvecs_cpp);

    /* /*//*/ */
    int errors = 0;
@@ -181,8 +178,8 @@ void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )

    Mat bad_nts_cpp1 = Mat_<float>(M, 1, 1.f);
    Mat bad_nts_cpp2 = Mat_<int>(3, 3, corSize.width * corSize.height);
-    CvMat bad_npts_c1 = bad_nts_cpp1;
-    CvMat bad_npts_c2 = bad_nts_cpp2;
+    CvMat bad_npts_c1 = cvMat(bad_nts_cpp1);
+    CvMat bad_npts_c2 = cvMat(bad_nts_cpp2);

    bad_caller = caller;
    bad_caller.npoints = &bad_npts_c1;
@@ -200,13 +197,13 @@ void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )
    bad_caller.tvecs = (CvMat*)zeros.ptr();
    errors += run_test_case( CV_StsBadArg, "Bad tvecs header", bad_caller );

-    Mat bad_rvecs_cpp1(M+1, 1, CV_32FC3); CvMat bad_rvecs_c1 = bad_rvecs_cpp1;
-    Mat bad_tvecs_cpp1(M+1, 1, CV_32FC3); CvMat bad_tvecs_c1 = bad_tvecs_cpp1;
+    Mat bad_rvecs_cpp1(M+1, 1, CV_32FC3); CvMat bad_rvecs_c1 = cvMat(bad_rvecs_cpp1);
+    Mat bad_tvecs_cpp1(M+1, 1, CV_32FC3); CvMat bad_tvecs_c1 = cvMat(bad_tvecs_cpp1);



-    Mat bad_rvecs_cpp2(M, 2, CV_32FC3); CvMat bad_rvecs_c2 = bad_rvecs_cpp2;
-    Mat bad_tvecs_cpp2(M, 2, CV_32FC3); CvMat bad_tvecs_c2 = bad_tvecs_cpp2;
+    Mat bad_rvecs_cpp2(M, 2, CV_32FC3); CvMat bad_rvecs_c2 = cvMat(bad_rvecs_cpp2);
+    Mat bad_tvecs_cpp2(M, 2, CV_32FC3); CvMat bad_tvecs_c2 = cvMat(bad_tvecs_cpp2);

    bad_caller = caller;
    bad_caller.rvecs = &bad_rvecs_c1;
@@ -224,9 +221,9 @@ void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )
    bad_caller.tvecs = &bad_tvecs_c2;
    errors += run_test_case( CV_StsBadArg, "Bad tvecs header", bad_caller );

-    Mat bad_cameraMatrix_cpp1(3, 3, CV_32S); CvMat bad_cameraMatrix_c1 = bad_cameraMatrix_cpp1;
-    Mat bad_cameraMatrix_cpp2(2, 3, CV_32F); CvMat bad_cameraMatrix_c2 = bad_cameraMatrix_cpp2;
-    Mat bad_cameraMatrix_cpp3(3, 2, CV_64F); CvMat bad_cameraMatrix_c3 = bad_cameraMatrix_cpp3;
+    Mat bad_cameraMatrix_cpp1(3, 3, CV_32S); CvMat bad_cameraMatrix_c1 = cvMat(bad_cameraMatrix_cpp1);
+    Mat bad_cameraMatrix_cpp2(2, 3, CV_32F); CvMat bad_cameraMatrix_c2 = cvMat(bad_cameraMatrix_cpp2);
+    Mat bad_cameraMatrix_cpp3(3, 2, CV_64F); CvMat bad_cameraMatrix_c3 = cvMat(bad_cameraMatrix_cpp3);



@@ -242,9 +239,9 @@ void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )
    bad_caller.cameraMatrix = &bad_cameraMatrix_c3;
    errors += run_test_case( CV_StsBadArg, "Bad camearaMatrix header", bad_caller );

-    Mat bad_distCoeffs_cpp1(1, 5, CV_32S); CvMat bad_distCoeffs_c1 = bad_distCoeffs_cpp1;
-    Mat bad_distCoeffs_cpp2(2, 2, CV_64F); CvMat bad_distCoeffs_c2 = bad_distCoeffs_cpp2;
-    Mat bad_distCoeffs_cpp3(1, 6, CV_64F); CvMat bad_distCoeffs_c3 = bad_distCoeffs_cpp3;
+    Mat bad_distCoeffs_cpp1(1, 5, CV_32S); CvMat bad_distCoeffs_c1 = cvMat(bad_distCoeffs_cpp1);
+    Mat bad_distCoeffs_cpp2(2, 2, CV_64F); CvMat bad_distCoeffs_c2 = cvMat(bad_distCoeffs_cpp2);
+    Mat bad_distCoeffs_cpp3(1, 6, CV_64F); CvMat bad_distCoeffs_c3 = cvMat(bad_distCoeffs_cpp3);



@@ -262,7 +259,7 @@ void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )
    errors += run_test_case( CV_StsBadArg, "Bad distCoeffs header", bad_caller );

    double CM[] = {0, 0, 0, /**/0, 0, 0, /**/0, 0, 0};
-    Mat bad_cameraMatrix_cpp4(3, 3, CV_64F, CM); CvMat bad_cameraMatrix_c4 = bad_cameraMatrix_cpp4;
+    Mat bad_cameraMatrix_cpp4(3, 3, CV_64F, CM); CvMat bad_cameraMatrix_c4 = cvMat(bad_cameraMatrix_cpp4);

    bad_caller = caller;
    bad_caller.flags |= CV_CALIB_USE_INTRINSIC_GUESS;
@@ -305,7 +302,7 @@ void CV_CameraCalibrationBadArgTest::run( int /* start_from */ )

    /////////////////////////////////////////////////////////////////////////////////////
    bad_caller = caller;
-    Mat bad_objPts_cpp5 = objPts_cpp.clone(); CvMat bad_objPts_c5 = bad_objPts_cpp5;
+    Mat bad_objPts_cpp5 = objPts_cpp.clone(); CvMat bad_objPts_c5 = cvMat(bad_objPts_cpp5);
    bad_caller.objPts = &bad_objPts_c5;

    cv::RNG& rng = theRNG();
@@ -350,9 +347,9 @@ protected:
        Mat zeros(1, sizeof(CvMat), CV_8U, Scalar(0));
        CvMat src_c, dst_c, jacobian_c;

-        Mat src_cpp(3, 1, CV_32F); src_c = src_cpp;
-        Mat dst_cpp(3, 3, CV_32F); dst_c = dst_cpp;
-        Mat jacobian_cpp(3, 9, CV_32F); jacobian_c = jacobian_cpp;
+        Mat src_cpp(3, 1, CV_32F); src_c = cvMat(src_cpp);
+        Mat dst_cpp(3, 3, CV_32F); dst_c = cvMat(dst_cpp);
+        Mat jacobian_cpp(3, 9, CV_32F); jacobian_c = cvMat(jacobian_cpp);

        C_Caller caller, bad_caller;
        caller.src = &src_c;
@@ -376,11 +373,11 @@ protected:
        bad_caller.dst = 0;
        errors += run_test_case( CV_StsNullPtr, "Dst is zero pointer", bad_caller );

-        Mat bad_src_cpp1(3, 1, CV_8U); CvMat bad_src_c1 = bad_src_cpp1;
-        Mat bad_dst_cpp1(3, 1, CV_8U); CvMat bad_dst_c1 = bad_dst_cpp1;
-        Mat bad_jac_cpp1(3, 1, CV_8U); CvMat bad_jac_c1 = bad_jac_cpp1;
-        Mat bad_jac_cpp2(3, 1, CV_32FC2); CvMat bad_jac_c2 = bad_jac_cpp2;
-        Mat bad_jac_cpp3(3, 1, CV_32F); CvMat bad_jac_c3 = bad_jac_cpp3;
+        Mat bad_src_cpp1(3, 1, CV_8U); CvMat bad_src_c1 = cvMat(bad_src_cpp1);
+        Mat bad_dst_cpp1(3, 1, CV_8U); CvMat bad_dst_c1 = cvMat(bad_dst_cpp1);
+        Mat bad_jac_cpp1(3, 1, CV_8U); CvMat bad_jac_c1 = cvMat(bad_jac_cpp1);
+        Mat bad_jac_cpp2(3, 1, CV_32FC2); CvMat bad_jac_c2 = cvMat(bad_jac_cpp2);
+        Mat bad_jac_cpp3(3, 1, CV_32F); CvMat bad_jac_c3 = cvMat(bad_jac_cpp3);

        bad_caller = caller;
        bad_caller.src = &bad_src_c1;
@@ -406,15 +403,15 @@ protected:
        bad_caller.jacobian = &bad_jac_c3;
        errors += run_test_case( CV_StsBadSize, "Bad jacobian format", bad_caller );

-        Mat bad_src_cpp2(1, 1, CV_32F); CvMat bad_src_c2 = bad_src_cpp2;
+        Mat bad_src_cpp2(1, 1, CV_32F); CvMat bad_src_c2 = cvMat(bad_src_cpp2);

        bad_caller = caller;
        bad_caller.src = &bad_src_c2;
        errors += run_test_case( CV_StsBadSize, "Bad src format", bad_caller );

-        Mat bad_dst_cpp2(2, 1, CV_32F); CvMat bad_dst_c2 = bad_dst_cpp2;
-        Mat bad_dst_cpp3(3, 2, CV_32F); CvMat bad_dst_c3 = bad_dst_cpp3;
-        Mat bad_dst_cpp4(3, 3, CV_32FC2); CvMat bad_dst_c4 = bad_dst_cpp4;
+        Mat bad_dst_cpp2(2, 1, CV_32F); CvMat bad_dst_c2 = cvMat(bad_dst_cpp2);
+        Mat bad_dst_cpp3(3, 2, CV_32F); CvMat bad_dst_c3 = cvMat(bad_dst_cpp3);
+        Mat bad_dst_cpp4(3, 3, CV_32FC2); CvMat bad_dst_c4 = cvMat(bad_dst_cpp4);

        bad_caller = caller;
        bad_caller.dst = &bad_dst_c2;
@@ -430,11 +427,11 @@ protected:


        /********/
-        src_cpp.create(3, 3, CV_32F); src_c = src_cpp;
-        dst_cpp.create(3, 1, CV_32F); dst_c = dst_cpp;
+        src_cpp.create(3, 3, CV_32F); src_c = cvMat(src_cpp);
+        dst_cpp.create(3, 1, CV_32F); dst_c = cvMat(dst_cpp);


-        Mat bad_dst_cpp5(5, 5, CV_32F); CvMat bad_dst_c5 = bad_dst_cpp5;
+        Mat bad_dst_cpp5(5, 5, CV_32F); CvMat bad_dst_c5 = cvMat(bad_dst_cpp5);

        bad_caller = caller;
        bad_caller.dst = &bad_dst_c5;
@@ -491,8 +488,7 @@ protected:

    void run(int /* start_from */ )
    {
-        CvMat zeros;
-        memset(&zeros, 0, sizeof(zeros));
+        CvMat zeros = CvMat();

        C_Caller caller, bad_caller;
        CvMat objectPoints_c, r_vec_c, t_vec_c, A_c, distCoeffs_c, imagePoints_c,
@@ -500,24 +496,24 @@ protected:

        const int n = 10;

-        Mat imagePoints_cpp(1, n, CV_32FC2); imagePoints_c = imagePoints_cpp;
+        Mat imagePoints_cpp(1, n, CV_32FC2); imagePoints_c = cvMat(imagePoints_cpp);

        Mat objectPoints_cpp(1, n, CV_32FC3);
        randu(objectPoints_cpp, Scalar::all(1), Scalar::all(10));
-        objectPoints_c = objectPoints_cpp;
+        objectPoints_c = cvMat(objectPoints_cpp);

-        Mat t_vec_cpp(Mat::zeros(1, 3, CV_32F)); t_vec_c = t_vec_cpp;
-        Mat r_vec_cpp;
-        Rodrigues(Mat::eye(3, 3, CV_32F), r_vec_cpp); r_vec_c = r_vec_cpp;
+        Mat t_vec_cpp(Mat::zeros(1, 3, CV_32F)); t_vec_c = cvMat(t_vec_cpp);
+        Mat r_vec_cpp(3, 1, CV_32F);
+        cvtest::Rodrigues(Mat::eye(3, 3, CV_32F), r_vec_cpp); r_vec_c = cvMat(r_vec_cpp);

-        Mat A_cpp = camMat.clone(); A_c = A_cpp;
-        Mat distCoeffs_cpp = distCoeffs.clone(); distCoeffs_c = distCoeffs_cpp;
+        Mat A_cpp = camMat.clone(); A_c = cvMat(A_cpp);
+        Mat distCoeffs_cpp = distCoeffs.clone(); distCoeffs_c = cvMat(distCoeffs_cpp);

-        Mat dpdr_cpp(2*n, 3, CV_32F); dpdr_c = dpdr_cpp;
-        Mat dpdt_cpp(2*n, 3, CV_32F); dpdt_c = dpdt_cpp;
-        Mat dpdf_cpp(2*n, 2, CV_32F); dpdf_c = dpdf_cpp;
-        Mat dpdc_cpp(2*n, 2, CV_32F); dpdc_c = dpdc_cpp;
-        Mat dpdk_cpp(2*n, 4, CV_32F); dpdk_c = dpdk_cpp;
+        Mat dpdr_cpp(2*n, 3, CV_32F); dpdr_c = cvMat(dpdr_cpp);
+        Mat dpdt_cpp(2*n, 3, CV_32F); dpdt_c = cvMat(dpdt_cpp);
+        Mat dpdf_cpp(2*n, 2, CV_32F); dpdf_c = cvMat(dpdf_cpp);
+        Mat dpdc_cpp(2*n, 2, CV_32F); dpdc_c = cvMat(dpdc_cpp);
+        Mat dpdk_cpp(2*n, 4, CV_32F); dpdk_c = cvMat(dpdk_cpp);

        caller.aspectRatio = 1.0;
        caller.objectPoints = &objectPoints_c;
@@ -557,9 +553,9 @@ protected:
        errors += run_test_case( CV_StsBadArg, "Zero imagePoints", bad_caller );

        /****************************/
-        Mat bad_r_vec_cpp1(r_vec_cpp.size(), CV_32S); CvMat bad_r_vec_c1 = bad_r_vec_cpp1;
-        Mat bad_r_vec_cpp2(2, 2, CV_32F); CvMat bad_r_vec_c2 = bad_r_vec_cpp2;
-        Mat bad_r_vec_cpp3(r_vec_cpp.size(), CV_32FC2); CvMat bad_r_vec_c3 = bad_r_vec_cpp3;
+        Mat bad_r_vec_cpp1(r_vec_cpp.size(), CV_32S); CvMat bad_r_vec_c1 = cvMat(bad_r_vec_cpp1);
+        Mat bad_r_vec_cpp2(2, 2, CV_32F); CvMat bad_r_vec_c2 = cvMat(bad_r_vec_cpp2);
+        Mat bad_r_vec_cpp3(r_vec_cpp.size(), CV_32FC2); CvMat bad_r_vec_c3 = cvMat(bad_r_vec_cpp3);

        bad_caller = caller;
        bad_caller.r_vec = &bad_r_vec_c1;
@@ -574,9 +570,9 @@ protected:
        errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller );

        /****************************/
-        Mat bad_t_vec_cpp1(t_vec_cpp.size(), CV_32S); CvMat bad_t_vec_c1 = bad_t_vec_cpp1;
-        Mat bad_t_vec_cpp2(2, 2, CV_32F); CvMat bad_t_vec_c2 = bad_t_vec_cpp2;
-        Mat bad_t_vec_cpp3(1, 1, CV_32FC2); CvMat bad_t_vec_c3 = bad_t_vec_cpp3;
+        Mat bad_t_vec_cpp1(t_vec_cpp.size(), CV_32S); CvMat bad_t_vec_c1 = cvMat(bad_t_vec_cpp1);
+        Mat bad_t_vec_cpp2(2, 2, CV_32F); CvMat bad_t_vec_c2 = cvMat(bad_t_vec_cpp2);
+        Mat bad_t_vec_cpp3(1, 1, CV_32FC2); CvMat bad_t_vec_c3 = cvMat(bad_t_vec_cpp3);

        bad_caller = caller;
        bad_caller.t_vec = &bad_t_vec_c1;
@@ -591,8 +587,8 @@ protected:
        errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller );

        /****************************/
-        Mat bad_A_cpp1(A_cpp.size(), CV_32S); CvMat bad_A_c1 = bad_A_cpp1;
-        Mat bad_A_cpp2(2, 2, CV_32F); CvMat bad_A_c2 = bad_A_cpp2;
+        Mat bad_A_cpp1(A_cpp.size(), CV_32S); CvMat bad_A_c1 = cvMat(bad_A_cpp1);
+        Mat bad_A_cpp2(2, 2, CV_32F); CvMat bad_A_c2 = cvMat(bad_A_cpp2);

        bad_caller = caller;
        bad_caller.A = &bad_A_c1;
@@ -603,9 +599,9 @@ protected:
        errors += run_test_case( CV_StsBadArg, "Bad A format", bad_caller );

        /****************************/
-        Mat bad_distCoeffs_cpp1(distCoeffs_cpp.size(), CV_32S); CvMat bad_distCoeffs_c1 = bad_distCoeffs_cpp1;
-        Mat bad_distCoeffs_cpp2(2, 2, CV_32F); CvMat bad_distCoeffs_c2 = bad_distCoeffs_cpp2;
-        Mat bad_distCoeffs_cpp3(1, 7, CV_32F); CvMat bad_distCoeffs_c3 = bad_distCoeffs_cpp3;
+        Mat bad_distCoeffs_cpp1(distCoeffs_cpp.size(), CV_32S); CvMat bad_distCoeffs_c1 = cvMat(bad_distCoeffs_cpp1);
+        Mat bad_distCoeffs_cpp2(2, 2, CV_32F); CvMat bad_distCoeffs_c2 = cvMat(bad_distCoeffs_cpp2);
+        Mat bad_distCoeffs_cpp3(1, 7, CV_32F); CvMat bad_distCoeffs_c3 = cvMat(bad_distCoeffs_cpp3);

        bad_caller = caller;
        bad_caller.distCoeffs = &zeros;
@@ -625,9 +621,9 @@ protected:


        /****************************/
-        Mat bad_dpdr_cpp1(dpdr_cpp.size(), CV_32S); CvMat bad_dpdr_c1 = bad_dpdr_cpp1;
-        Mat bad_dpdr_cpp2(dpdr_cpp.cols+1, 3, CV_32F); CvMat bad_dpdr_c2 = bad_dpdr_cpp2;
-        Mat bad_dpdr_cpp3(dpdr_cpp.cols, 7, CV_32F); CvMat bad_dpdr_c3 = bad_dpdr_cpp3;
+        Mat bad_dpdr_cpp1(dpdr_cpp.size(), CV_32S); CvMat bad_dpdr_c1 = cvMat(bad_dpdr_cpp1);
+        Mat bad_dpdr_cpp2(dpdr_cpp.cols+1, 3, CV_32F); CvMat bad_dpdr_c2 = cvMat(bad_dpdr_cpp2);
+        Mat bad_dpdr_cpp3(dpdr_cpp.cols, 7, CV_32F); CvMat bad_dpdr_c3 = cvMat(bad_dpdr_cpp3);

        bad_caller = caller;
        bad_caller.dpdr = &zeros;
@@ -665,7 +661,7 @@ protected:

        /****************************/

-        Mat bad_dpdf_cpp2(dpdr_cpp.cols+1, 2, CV_32F); CvMat bad_dpdf_c2 = bad_dpdf_cpp2;
+        Mat bad_dpdf_cpp2(dpdr_cpp.cols+1, 2, CV_32F); CvMat bad_dpdf_c2 = cvMat(bad_dpdf_cpp2);

        bad_caller = caller;
        bad_caller.dpdf = &zeros;
@@ -735,3 +731,5 @@ protected:
 TEST(Calib3d_CalibrateCamera_C, badarg) { CV_CameraCalibrationBadArgTest test; test.safe_run(); }
 TEST(Calib3d_Rodrigues_C, badarg) { CV_Rodrigues2BadArgTest test; test.safe_run(); }
 TEST(Calib3d_ProjectPoints_C, badarg) { CV_ProjectPoints2BadArgTest test; test.safe_run(); }
+
+}} // namespace
@@ -0,0 +1,693 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+#include "opencv2/ts/cuda_test.hpp" // EXPECT_MAT_NEAR
+
+namespace opencv_test { namespace {
+
+#define NUM_DIST_COEFF_TILT 14
+
+/**
+Some conventions:
+- the first camera determines the world coordinate system
+- y points down, hence top means minimal y value (negative) and
+bottom means maximal y value (positive)
+- the field of view plane is tilted around x such that it
+intersects the xy-plane in a line with a large (positive)
+y-value
+- image sensor and object are both modelled in the halfspace
+z > 0
+
+
+**/
+class cameraCalibrationTiltTest : public ::testing::Test {
+
+protected:
+    cameraCalibrationTiltTest()
+        : m_toRadian(acos(-1.0)/180.0)
+        , m_toDegree(180.0/acos(-1.0))
+    {}
+    virtual void SetUp();
+
+protected:
+    static const cv::Size m_imageSize;
+    static const double m_pixelSize;
+    static const double m_circleConfusionPixel;
+    static const double m_lensFocalLength;
+    static const double m_lensFNumber;
+    static const double m_objectDistance;
+    static const double m_planeTiltDegree;
+    static const double m_pointTargetDist;
+    static const int m_pointTargetNum;
+
+    /** image distance corresponding to working distance */
+    double m_imageDistance;
+    /** image tilt angle corresponding to the tilt of the object plane */
+    double m_imageTiltDegree;
+    /** center of the field of view, near and far plane */
+    std::vector<cv::Vec3d> m_fovCenter;
+    /** normal of the field of view, near and far plane */
+    std::vector<cv::Vec3d> m_fovNormal;
+    /** points on a plane calibration target */
+    std::vector<cv::Point3d> m_pointTarget;
+    /** rotations for the calibration target */
+    std::vector<cv::Vec3d> m_pointTargetRvec;
+    /** translations for the calibration target */
+    std::vector<cv::Vec3d> m_pointTargetTvec;
+    /** camera matrix */
+    cv::Matx33d m_cameraMatrix;
+    /** distortion coefficients */
+    cv::Vec<double, NUM_DIST_COEFF_TILT> m_distortionCoeff;
+
+    /** random generator */
+    cv::RNG m_rng;
+    /** degree to radian conversion factor */
+    const double m_toRadian;
+    /** radian to degree conversion factor */
+    const double m_toDegree;
+
+    /**
+    computes for a given distance of an image or object point
+    the distance of the corresponding object or image point
+    */
+    double opticalMap(double dist) {
+        return m_lensFocalLength*dist/(dist - m_lensFocalLength);
+    }
+
+    /** magnification of the optical map */
+    double magnification(double dist) {
+        return m_lensFocalLength/(dist - m_lensFocalLength);
+    }
+
+    /**
+    Changes given distortion coefficients randomly by adding
+    a uniformly distributed random variable in [-max max]
+    \param coeff input
+    \param max limits for the random variables
+    */
+    void randomDistortionCoeff(
+        cv::Vec<double, NUM_DIST_COEFF_TILT>& coeff,
+        const cv::Vec<double, NUM_DIST_COEFF_TILT>& max)
+    {
+        for (int i = 0; i < coeff.rows; ++i)
+            coeff(i) += m_rng.uniform(-max(i), max(i));
+    }
+
+    /** numerical jacobian */
+    void numericalDerivative(
+        cv::Mat& jac,
+        double eps,
+        const std::vector<cv::Point3d>& obj,
+        const cv::Vec3d& rvec,
+        const cv::Vec3d& tvec,
+        const cv::Matx33d& camera,
+        const cv::Vec<double, NUM_DIST_COEFF_TILT>& distor);
+
+    /** remove points with projection outside the sensor array */
+    void removeInvalidPoints(
+        std::vector<cv::Point2d>& imagePoints,
+        std::vector<cv::Point3d>& objectPoints);
+
+    /** add uniform distribute noise in [-halfWidthNoise, halfWidthNoise]
+    to the image points and remove out of range points */
+    void addNoiseRemoveInvalidPoints(
+        std::vector<cv::Point2f>& imagePoints,
+        std::vector<cv::Point3f>& objectPoints,
+        std::vector<cv::Point2f>& noisyImagePoints,
+        double halfWidthNoise);
+};
+
+/** Number of Pixel of the sensor */
+const cv::Size cameraCalibrationTiltTest::m_imageSize(1600, 1200);
+/** Size of a pixel in mm */
+const double cameraCalibrationTiltTest::m_pixelSize(.005);
+/** Diameter of the circle of confusion */
+const double cameraCalibrationTiltTest::m_circleConfusionPixel(3);
+/** Focal length of the lens */
+const double cameraCalibrationTiltTest::m_lensFocalLength(16.4);
+/** F-Number */
+const double cameraCalibrationTiltTest::m_lensFNumber(8);
+/** Working distance */
+const double cameraCalibrationTiltTest::m_objectDistance(200);
+/** Angle between optical axis and object plane normal */
+const double cameraCalibrationTiltTest::m_planeTiltDegree(55);
+/** the calibration target are points on a square grid with this side length */
+const double cameraCalibrationTiltTest::m_pointTargetDist(5);
+/** the calibration target has (2*n + 1) x (2*n + 1) points */
+const int cameraCalibrationTiltTest::m_pointTargetNum(15);
+
+
+void cameraCalibrationTiltTest::SetUp()
+{
+    m_imageDistance = opticalMap(m_objectDistance);
+    m_imageTiltDegree = m_toDegree * atan2(
+        m_imageDistance * tan(m_toRadian * m_planeTiltDegree),
+        m_objectDistance);
+    // half sensor height
+    double tmp = .5 * (m_imageSize.height - 1) * m_pixelSize
+        * cos(m_toRadian * m_imageTiltDegree);
+    // y-Value of tilted sensor
+    double yImage[2] = {tmp, -tmp};
+    // change in z because of the tilt
+    tmp *= sin(m_toRadian * m_imageTiltDegree);
+    // z-values of the sensor lower and upper corner
+    double zImage[2] = {
+        m_imageDistance + tmp,
+        m_imageDistance - tmp};
+    // circle of confusion
+    double circleConfusion = m_circleConfusionPixel*m_pixelSize;
+    // aperture of the lense
+    double aperture = m_lensFocalLength/m_lensFNumber;
+    // near and far factor on the image side
+    double nearFarFactorImage[2] = {
+        aperture/(aperture - circleConfusion),
+        aperture/(aperture + circleConfusion)};
+    // on the object side - points that determine the field of
+    // view
+    std::vector<cv::Vec3d> fovBottomTop(6);
+    std::vector<cv::Vec3d>::iterator itFov = fovBottomTop.begin();
+    for (size_t iBottomTop = 0; iBottomTop < 2; ++iBottomTop)
+    {
+        // mapping sensor to field of view
+        *itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]);
+        *itFov *= magnification((*itFov)(2));
+        ++itFov;
+        for (size_t iNearFar = 0; iNearFar < 2; ++iNearFar, ++itFov)
+        {
+            // scaling to the near and far distance on the
+            // image side
+            *itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]) *
+                nearFarFactorImage[iNearFar];
+            // scaling to the object side
+            *itFov *= magnification((*itFov)(2));
+        }
+    }
+    m_fovCenter.resize(3);
+    m_fovNormal.resize(3);
+    for (size_t i = 0; i < 3; ++i)
+    {
+        m_fovCenter[i] = .5*(fovBottomTop[i] + fovBottomTop[i+3]);
+        m_fovNormal[i] = fovBottomTop[i+3] - fovBottomTop[i];
+        m_fovNormal[i] = cv::normalize(m_fovNormal[i]);
+        m_fovNormal[i] = cv::Vec3d(
+            m_fovNormal[i](0),
+            -m_fovNormal[i](2),
+            m_fovNormal[i](1));
+        // one target position in each plane
+        m_pointTargetTvec.push_back(m_fovCenter[i]);
+        cv::Vec3d rvec = cv::Vec3d(0,0,1).cross(m_fovNormal[i]);
+        rvec = cv::normalize(rvec);
+        rvec *= acos(m_fovNormal[i](2));
+        m_pointTargetRvec.push_back(rvec);
+    }
+    // calibration target
+    size_t num = 2*m_pointTargetNum + 1;
+    m_pointTarget.resize(num*num);
+    std::vector<cv::Point3d>::iterator itTarget = m_pointTarget.begin();
+    for (int iY = -m_pointTargetNum; iY <= m_pointTargetNum; ++iY)
+    {
+        for (int iX = -m_pointTargetNum; iX <= m_pointTargetNum; ++iX, ++itTarget)
+        {
+            *itTarget = cv::Point3d(iX, iY, 0) * m_pointTargetDist;
+        }
+    }
+    // oblique target positions
+    // approximate distance to the near and far plane
+    double dist = std::max(
+        std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[1])),
+        std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[2])));
+    // maximal angle such that target border "reaches" near and far plane
+    double maxAngle = atan2(dist, m_pointTargetNum*m_pointTargetDist);
+    std::vector<double> angle;
+    angle.push_back(-maxAngle);
+    angle.push_back(maxAngle);
+    cv::Matx33d baseMatrix;
+    cv::Rodrigues(m_pointTargetRvec.front(), baseMatrix);
+    for (std::vector<double>::const_iterator itAngle = angle.begin(); itAngle != angle.end(); ++itAngle)
+    {
+        cv::Matx33d rmat;
+        for (int i = 0; i < 2; ++i)
+        {
+            cv::Vec3d rvec(0,0,0);
+            rvec(i) = *itAngle;
+            cv::Rodrigues(rvec, rmat);
+            rmat = baseMatrix*rmat;
+            cv::Rodrigues(rmat, rvec);
+            m_pointTargetTvec.push_back(m_fovCenter.front());
+            m_pointTargetRvec.push_back(rvec);
+        }
+    }
+    // camera matrix
+    double cx = .5 * (m_imageSize.width - 1);
+    double cy = .5 * (m_imageSize.height - 1);
+    double f = m_imageDistance/m_pixelSize;
+    m_cameraMatrix = cv::Matx33d(
+        f,0,cx,
+        0,f,cy,
+        0,0,1);
+    // distortion coefficients
+    m_distortionCoeff = cv::Vec<double, NUM_DIST_COEFF_TILT>::all(0);
+    // tauX
+    m_distortionCoeff(12) = -m_toRadian*m_imageTiltDegree;
+
+}
+
+void cameraCalibrationTiltTest::numericalDerivative(
+    cv::Mat& jac,
+    double eps,
+    const std::vector<cv::Point3d>& obj,
+    const cv::Vec3d& rvec,
+    const cv::Vec3d& tvec,
+    const cv::Matx33d& camera,
+    const cv::Vec<double, NUM_DIST_COEFF_TILT>& distor)
+{
+    cv::Vec3d r(rvec);
+    cv::Vec3d t(tvec);
+    cv::Matx33d cm(camera);
+    cv::Vec<double, NUM_DIST_COEFF_TILT> dc(distor);
+    double* param[10+NUM_DIST_COEFF_TILT] = {
+        &r(0), &r(1), &r(2),
+        &t(0), &t(1), &t(2),
+        &cm(0,0), &cm(1,1), &cm(0,2), &cm(1,2),
+        &dc(0), &dc(1), &dc(2), &dc(3), &dc(4), &dc(5), &dc(6),
+        &dc(7), &dc(8), &dc(9), &dc(10), &dc(11), &dc(12), &dc(13)};
+    std::vector<cv::Point2d> pix0, pix1;
+    double invEps = .5/eps;
+
+    for (int col = 0; col < 10+NUM_DIST_COEFF_TILT; ++col)
+    {
+        double save = *(param[col]);
+        *(param[col]) = save + eps;
+        cv::projectPoints(obj, r, t, cm, dc, pix0);
+        *(param[col]) = save - eps;
+        cv::projectPoints(obj, r, t, cm, dc, pix1);
+        *(param[col]) = save;
+
+        std::vector<cv::Point2d>::const_iterator it0 = pix0.begin();
+        std::vector<cv::Point2d>::const_iterator it1 = pix1.begin();
+        int row = 0;
+        for (;it0 != pix0.end(); ++it0, ++it1)
+        {
+            cv::Point2d d = invEps*(*it0 - *it1);
+            jac.at<double>(row, col) = d.x;
+            ++row;
+            jac.at<double>(row, col) = d.y;
+            ++row;
+        }
+    }
+}
+
+void cameraCalibrationTiltTest::removeInvalidPoints(
+    std::vector<cv::Point2d>& imagePoints,
+    std::vector<cv::Point3d>& objectPoints)
+{
+    // remove object and imgage points out of range
+    std::vector<cv::Point2d>::iterator itImg = imagePoints.begin();
+    std::vector<cv::Point3d>::iterator itObj = objectPoints.begin();
+    while (itImg != imagePoints.end())
+    {
+        bool ok =
+            itImg->x >= 0 &&
+            itImg->x <= m_imageSize.width - 1.0 &&
+            itImg->y >= 0 &&
+            itImg->y <= m_imageSize.height - 1.0;
+        if (ok)
+        {
+            ++itImg;
+            ++itObj;
+        }
+        else
+        {
+            itImg = imagePoints.erase(itImg);
+            itObj = objectPoints.erase(itObj);
+        }
+    }
+}
+
+void cameraCalibrationTiltTest::addNoiseRemoveInvalidPoints(
+    std::vector<cv::Point2f>& imagePoints,
+    std::vector<cv::Point3f>& objectPoints,
+    std::vector<cv::Point2f>& noisyImagePoints,
+    double halfWidthNoise)
+{
+    std::vector<cv::Point2f>::iterator itImg = imagePoints.begin();
+    std::vector<cv::Point3f>::iterator itObj = objectPoints.begin();
+    noisyImagePoints.clear();
+    noisyImagePoints.reserve(imagePoints.size());
+    while (itImg != imagePoints.end())
+    {
+        cv::Point2f pix = *itImg + cv::Point2f(
+            (float)m_rng.uniform(-halfWidthNoise, halfWidthNoise),
+            (float)m_rng.uniform(-halfWidthNoise, halfWidthNoise));
+        bool ok =
+            pix.x >= 0 &&
+            pix.x <= m_imageSize.width - 1.0 &&
+            pix.y >= 0 &&
+            pix.y <= m_imageSize.height - 1.0;
+        if (ok)
+        {
+            noisyImagePoints.push_back(pix);
+            ++itImg;
+            ++itObj;
+        }
+        else
+        {
+            itImg = imagePoints.erase(itImg);
+            itObj = objectPoints.erase(itObj);
+        }
+    }
+}
+
+
+TEST_F(cameraCalibrationTiltTest, projectPoints)
+{
+    std::vector<cv::Point2d> imagePoints;
+    std::vector<cv::Point3d> objectPoints = m_pointTarget;
+    cv::Vec3d rvec = m_pointTargetRvec.front();
+    cv::Vec3d tvec = m_pointTargetTvec.front();
+
+    cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
+        .1, .1, // k1 k2
+        .01, .01, // p1 p2
+        .001, .001, .001, .001, // k3 k4 k5 k6
+        .001, .001, .001, .001, // s1 s2 s3 s4
+        .01, .01); // tauX tauY
+    for (size_t numTest = 0; numTest < 10; ++numTest)
+    {
+        // create random distortion coefficients
+        cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
+        randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
+
+        // projection
+        cv::projectPoints(
+            objectPoints,
+            rvec,
+            tvec,
+            m_cameraMatrix,
+            distortionCoeff,
+            imagePoints);
+
+        // remove object and imgage points out of range
+        removeInvalidPoints(imagePoints, objectPoints);
+
+        int numPoints = (int)imagePoints.size();
+        int numParams = 10 + distortionCoeff.rows;
+        cv::Mat jacobian(2*numPoints, numParams, CV_64FC1);
+
+        // projection and jacobian
+        cv::projectPoints(
+            objectPoints,
+            rvec,
+            tvec,
+            m_cameraMatrix,
+            distortionCoeff,
+            imagePoints,
+            jacobian);
+
+        // numerical derivatives
+        cv::Mat numericJacobian(2*numPoints, numParams, CV_64FC1);
+        double eps = 1e-7;
+        numericalDerivative(
+            numericJacobian,
+            eps,
+            objectPoints,
+            rvec,
+            tvec,
+            m_cameraMatrix,
+            distortionCoeff);
+
+#if 0
+        for (size_t row = 0; row < 2; ++row)
+        {
+            std::cout << "------ Row = " << row << " ------\n";
+            for (size_t i = 0; i < 10+NUM_DIST_COEFF_TILT; ++i)
+            {
+                std::cout << i
+                    << "  jac = " << jacobian.at<double>(row,i)
+                    << "  num = " << numericJacobian.at<double>(row,i)
+                    << "  rel. diff = " << abs(numericJacobian.at<double>(row,i) - jacobian.at<double>(row,i))/abs(numericJacobian.at<double>(row,i))
+                    << "\n";
+            }
+        }
+#endif
+        // relative difference for large values (rvec and tvec)
+        cv::Mat check = abs(jacobian(cv::Range::all(), cv::Range(0,6)) - numericJacobian(cv::Range::all(), cv::Range(0,6)))/
+            (1 + abs(jacobian(cv::Range::all(), cv::Range(0,6))));
+        double minVal, maxVal;
+        cv::minMaxIdx(check, &minVal, &maxVal);
+        EXPECT_LE(maxVal, .01);
+        // absolute difference for distortion and camera matrix
+        EXPECT_MAT_NEAR(jacobian(cv::Range::all(), cv::Range(6,numParams)), numericJacobian(cv::Range::all(), cv::Range(6,numParams)), 1e-5);
+    }
+}
+
+TEST_F(cameraCalibrationTiltTest, undistortPoints)
+{
+    cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
+        .2, .1, // k1 k2
+        .01, .01, // p1 p2
+        .01, .01, .01, .01, // k3 k4 k5 k6
+        .001, .001, .001, .001, // s1 s2 s3 s4
+        .001, .001); // tauX tauY
+    double step = 99;
+    double toleranceBackProjection = 1e-5;
+
+    for (size_t numTest = 0; numTest < 10; ++numTest)
+    {
+        cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
+        randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
+
+        // distorted points
+        std::vector<cv::Point2d> distorted;
+        for (double x = 0; x <= m_imageSize.width-1; x += step)
+            for (double y = 0; y <= m_imageSize.height-1; y += step)
+                distorted.push_back(cv::Point2d(x,y));
+        std::vector<cv::Point2d> normalizedUndistorted;
+
+        // undistort
+        cv::undistortPoints(distorted,
+            normalizedUndistorted,
+            m_cameraMatrix,
+            distortionCoeff);
+
+        // copy normalized points to 3D
+        std::vector<cv::Point3d> objectPoints;
+        for (std::vector<cv::Point2d>::const_iterator itPnt = normalizedUndistorted.begin();
+            itPnt != normalizedUndistorted.end(); ++itPnt)
+            objectPoints.push_back(cv::Point3d(itPnt->x, itPnt->y, 1));
+
+        // project
+        std::vector<cv::Point2d> imagePoints(objectPoints.size());
+        cv::projectPoints(objectPoints,
+            cv::Vec3d(0,0,0),
+            cv::Vec3d(0,0,0),
+            m_cameraMatrix,
+            distortionCoeff,
+            imagePoints);
+
+        EXPECT_MAT_NEAR(distorted, imagePoints, toleranceBackProjection);
+    }
+}
+
+template <typename INPUT, typename ESTIMATE>
+void show(const std::string& name, const INPUT in, const ESTIMATE est)
+{
+    std::cout << name << " = " << est << " (init = " << in
+        << ", diff = " << est-in << ")\n";
+}
+
+template <typename INPUT>
+void showVec(const std::string& name, const INPUT& in, const cv::Mat& est)
+{
+
+    for (size_t i = 0; i < in.channels; ++i)
+    {
+        std::stringstream ss;
+        ss << name << "[" << i << "]";
+        show(ss.str(), in(i), est.at<double>(i));
+    }
+}
+
+/**
+For given camera matrix and distortion coefficients
+- project point target in different positions onto the sensor
+- add pixel noise
+- estimate camera model with noisy measurements
+- compare result with initial model parameter
+
+Parameter are differently affected by the noise
+*/
+TEST_F(cameraCalibrationTiltTest, calibrateCamera)
+{
+    cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
+        .2, .1, // k1 k2
+        .01, .01, // p1 p2
+        0, 0, 0, 0, // k3 k4 k5 k6
+        .001, .001, .001, .001, // s1 s2 s3 s4
+        .001, .001); // tauX tauY
+    double pixelNoiseHalfWidth = .5;
+    std::vector<cv::Point3f> pointTarget;
+    pointTarget.reserve(m_pointTarget.size());
+    for (std::vector<cv::Point3d>::const_iterator it = m_pointTarget.begin(); it != m_pointTarget.end(); ++it)
+        pointTarget.push_back(cv::Point3f(
+        (float)(it->x),
+        (float)(it->y),
+        (float)(it->z)));
+
+    for (size_t numTest = 0; numTest < 5; ++numTest)
+    {
+        // create random distortion coefficients
+        cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
+        randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
+
+        // container for calibration data
+        std::vector<std::vector<cv::Point3f> > viewsObjectPoints;
+        std::vector<std::vector<cv::Point2f> > viewsImagePoints;
+        std::vector<std::vector<cv::Point2f> > viewsNoisyImagePoints;
+
+        // simulate calibration data with projectPoints
+        std::vector<cv::Vec3d>::const_iterator itRvec = m_pointTargetRvec.begin();
+        std::vector<cv::Vec3d>::const_iterator itTvec = m_pointTargetTvec.begin();
+        // loop over different views
+        for (;itRvec != m_pointTargetRvec.end(); ++ itRvec, ++itTvec)
+        {
+            std::vector<cv::Point3f> objectPoints(pointTarget);
+            std::vector<cv::Point2f> imagePoints;
+            std::vector<cv::Point2f> noisyImagePoints;
+            // project calibration target to sensor
+            cv::projectPoints(
+                objectPoints,
+                *itRvec,
+                *itTvec,
+                m_cameraMatrix,
+                distortionCoeff,
+                imagePoints);
+            // remove invisible points
+            addNoiseRemoveInvalidPoints(
+                imagePoints,
+                objectPoints,
+                noisyImagePoints,
+                pixelNoiseHalfWidth);
+            // add data for view
+            viewsNoisyImagePoints.push_back(noisyImagePoints);
+            viewsImagePoints.push_back(imagePoints);
+            viewsObjectPoints.push_back(objectPoints);
+        }
+
+        // Output
+        std::vector<cv::Mat> outRvecs, outTvecs;
+        cv::Mat outCameraMatrix(3, 3, CV_64F, cv::Scalar::all(1)), outDistCoeff;
+
+        // Stopping criteria
+        cv::TermCriteria stop(
+            cv::TermCriteria::COUNT+cv::TermCriteria::EPS,
+            50000,
+            1e-14);
+        // model choice
+        int flag =
+            cv::CALIB_FIX_ASPECT_RATIO |
+            // cv::CALIB_RATIONAL_MODEL |
+            cv::CALIB_FIX_K3 |
+            // cv::CALIB_FIX_K6 |
+            cv::CALIB_THIN_PRISM_MODEL |
+            cv::CALIB_TILTED_MODEL;
+        // estimate
+        double backProjErr = cv::calibrateCamera(
+            viewsObjectPoints,
+            viewsNoisyImagePoints,
+            m_imageSize,
+            outCameraMatrix,
+            outDistCoeff,
+            outRvecs,
+            outTvecs,
+            flag,
+            stop);
+
+        EXPECT_LE(backProjErr, pixelNoiseHalfWidth);
+
+#if 0
+        std::cout << "------ estimate ------\n";
+        std::cout << "back projection error = " << backProjErr << "\n";
+        std::cout << "points per view = {" << viewsObjectPoints.front().size();
+        for (size_t i = 1; i < viewsObjectPoints.size(); ++i)
+            std::cout << ", " << viewsObjectPoints[i].size();
+        std::cout << "}\n";
+        show("fx", m_cameraMatrix(0,0), outCameraMatrix.at<double>(0,0));
+        show("fy", m_cameraMatrix(1,1), outCameraMatrix.at<double>(1,1));
+        show("cx", m_cameraMatrix(0,2), outCameraMatrix.at<double>(0,2));
+        show("cy", m_cameraMatrix(1,2), outCameraMatrix.at<double>(1,2));
+        showVec("distor", distortionCoeff, outDistCoeff);
+#endif
+        if (pixelNoiseHalfWidth > 0)
+        {
+            double tolRvec = pixelNoiseHalfWidth;
+            double tolTvec = m_objectDistance * tolRvec;
+            // back projection error
+            for (size_t i = 0; i < viewsNoisyImagePoints.size(); ++i)
+            {
+                double dRvec = cv::norm(m_pointTargetRvec[i],
+                        cv::Vec3d(outRvecs[i].at<double>(0), outRvecs[i].at<double>(1), outRvecs[i].at<double>(2))
+                );
+                EXPECT_LE(dRvec, tolRvec);
+                double dTvec = cv::norm(m_pointTargetTvec[i],
+                        cv::Vec3d(outTvecs[i].at<double>(0), outTvecs[i].at<double>(1), outTvecs[i].at<double>(2))
+                );
+                EXPECT_LE(dTvec, tolTvec);
+
+                std::vector<cv::Point2f> backProjection;
+                cv::projectPoints(
+                    viewsObjectPoints[i],
+                    outRvecs[i],
+                    outTvecs[i],
+                    outCameraMatrix,
+                    outDistCoeff,
+                    backProjection);
+                EXPECT_MAT_NEAR(backProjection, viewsNoisyImagePoints[i], 1.5*pixelNoiseHalfWidth);
+                EXPECT_MAT_NEAR(backProjection, viewsImagePoints[i], 1.5*pixelNoiseHalfWidth);
+            }
+        }
+        pixelNoiseHalfWidth *= .25;
+    }
+}
+
+}} // namespace
@@ -43,28 +43,24 @@
 #include "test_precomp.hpp"
 #include "test_chessboardgenerator.hpp"

-#include <vector>
-#include <iterator>
-#include <algorithm>
-
-using namespace cv;
-using namespace std;
+namespace cv {

 ChessBoardGenerator::ChessBoardGenerator(const Size& _patternSize) : sensorWidth(32), sensorHeight(24),
-    squareEdgePointsNum(200), min_cos(std::sqrt(2.f)*0.5f), cov(0.5),
+    squareEdgePointsNum(200), min_cos(std::sqrt(3.f)*0.5f), cov(0.5),
    patternSize(_patternSize), rendererResolutionMultiplier(4), tvec(Mat::zeros(1, 3, CV_32F))
 {
-    Rodrigues(Mat::eye(3, 3, CV_32F), rvec);
+    rvec.create(3, 1, CV_32F);
+    cvtest::Rodrigues(Mat::eye(3, 3, CV_32F), rvec);
 }

-void cv::ChessBoardGenerator::generateEdge(const Point3f& p1, const Point3f& p2, vector<Point3f>& out) const
+void ChessBoardGenerator::generateEdge(const Point3f& p1, const Point3f& p2, vector<Point3f>& out) const
 {
    Point3f step = (p2 - p1) * (1.f/squareEdgePointsNum);
    for(size_t n = 0; n < squareEdgePointsNum; ++n)
        out.push_back( p1 + step * (float)n);
 }

-Size cv::ChessBoardGenerator::cornersSize() const
+Size ChessBoardGenerator::cornersSize() const
 {
    return Size(patternSize.width-1, patternSize.height-1);
 }
@@ -76,7 +72,7 @@ struct Mult
    Point2f operator()(const Point2f& p)const { return p * m; }
 };

-void cv::ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const
+void ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const
 {
    RNG& rng = theRNG();

@@ -85,8 +81,10 @@ void cv::ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const
    {
        n[0] = rng.uniform(-1.f, 1.f);
        n[1] = rng.uniform(-1.f, 1.f);
-        n[2] = rng.uniform(-1.f, 1.f);
+        n[2] = rng.uniform(0.0f, 1.f);
        float len = (float)norm(n);
+        if (len < 1e-3)
+            continue;
        n[0]/=len;
        n[1]/=len;
        n[2]/=len;
@@ -106,7 +104,7 @@ void cv::ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const
 }


-Mat cv::ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
+Mat ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
                                                const Point3f& zero, const Point3f& pb1, const Point3f& pb2,
                                                float sqWidth, float sqHeight, const vector<Point3f>& whole,
                                                vector<Point2f>& corners) const
@@ -128,10 +126,10 @@ Mat cv::ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat
                generateEdge(p3, p4, pts_square3d);
                generateEdge(p4, p1, pts_square3d);

-                projectPoints(Mat(pts_square3d), rvec, tvec, camMat, distCoeffs, pts_square2d);
+                projectPoints(pts_square3d, rvec, tvec, camMat, distCoeffs, pts_square2d);
                squares_black.resize(squares_black.size() + 1);
                vector<Point2f> temp;
-                approxPolyDP(Mat(pts_square2d), temp, 1.0, true);
+                approxPolyDP(pts_square2d, temp, 1.0, true);
                transform(temp.begin(), temp.end(), back_inserter(squares_black.back()), Mult(rendererResolutionMultiplier));
            }

@@ -141,7 +139,7 @@ Mat cv::ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat
        for(int i = 0; i < patternSize.width - 1; ++i)
            corners3d.push_back(zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2);
    corners.clear();
-    projectPoints(Mat(corners3d), rvec, tvec, camMat, distCoeffs, corners);
+    projectPoints(corners3d, rvec, tvec, camMat, distCoeffs, corners);

    vector<Point3f> whole3d;
    vector<Point2f> whole2d;
@@ -149,9 +147,9 @@ Mat cv::ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat
    generateEdge(whole[1], whole[2], whole3d);
    generateEdge(whole[2], whole[3], whole3d);
    generateEdge(whole[3], whole[0], whole3d);
-    projectPoints(Mat(whole3d), rvec, tvec, camMat, distCoeffs, whole2d);
+    projectPoints(whole3d, rvec, tvec, camMat, distCoeffs, whole2d);
    vector<Point2f> temp_whole2d;
-    approxPolyDP(Mat(whole2d), temp_whole2d, 1.0, true);
+    approxPolyDP(whole2d, temp_whole2d, 1.0, true);

    vector< vector<Point > > whole_contour(1);
    transform(temp_whole2d.begin(), temp_whole2d.end(),
@@ -167,7 +165,7 @@ Mat cv::ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat
    else
    {
        Mat tmp;
-        resize(bg, tmp, bg.size() * rendererResolutionMultiplier);
+        resize(bg, tmp, bg.size() * rendererResolutionMultiplier, 0, 0, INTER_LINEAR_EXACT);
        drawContours(tmp, whole_contour, -1, Scalar::all(255), FILLED, LINE_AA);
        drawContours(tmp, squares_black, -1, Scalar::all(0), FILLED, LINE_AA);
        resize(tmp, result, bg.size(), 0, 0, INTER_AREA);
@@ -176,7 +174,7 @@ Mat cv::ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat
    return result;
 }

-Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector<Point2f>& corners) const
+Mat ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector<Point2f>& corners) const
 {
    cov = std::min(cov, 0.8);
    double fovx, fovy, focalLen;
@@ -215,7 +213,7 @@ Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const
        pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;

        /* can remake with better perf */
-        projectPoints(Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d);
+        projectPoints(pts3d, rvec, tvec, camMat, distCoeffs, pts2d);

        bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0;
        bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0;
@@ -240,7 +238,7 @@ Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const
 }


-Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
+Mat ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
                                         const Size2f& squareSize, vector<Point2f>& corners) const
 {
    cov = std::min(cov, 0.8);
@@ -280,7 +278,7 @@ Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const
        pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;

        /* can remake with better perf */
-        projectPoints(Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d);
+        projectPoints(pts3d, rvec, tvec, camMat, distCoeffs, pts2d);

        bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0;
        bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0;
@@ -299,7 +297,7 @@ Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const
        squareSize.width, squareSize.height, pts3d, corners);
 }

-Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
+Mat ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
                                         const Size2f& squareSize, const Point3f& pos, vector<Point2f>& corners) const
 {
    cov = std::min(cov, 0.8);
@@ -322,10 +320,12 @@ Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const
    pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2;

    /* can remake with better perf */
-    projectPoints(Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d);
+    projectPoints(pts3d, rvec, tvec, camMat, distCoeffs, pts2d);

    Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2;

    return generateChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2,
        squareSize.width, squareSize.height,  pts3d, corners);
 }
+
+} // namespace
@@ -1,11 +1,14 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
 #ifndef CV_CHESSBOARDGENERATOR_H143KJTVYM389YTNHKFDHJ89NYVMO3VLMEJNTBGUEIYVCM203P
 #define CV_CHESSBOARDGENERATOR_H143KJTVYM389YTNHKFDHJ89NYVMO3VLMEJNTBGUEIYVCM203P

-#include "opencv2/calib3d.hpp"
-
 namespace cv
 {

+using std::vector;
+
 class ChessBoardGenerator
 {
 public:
@@ -43,37 +43,35 @@
 #include "test_chessboardgenerator.hpp"

 #include <functional>
-#include <limits>
-#include <numeric>

-using namespace std;
-using namespace cv;
+namespace opencv_test { namespace {

 #define _L2_ERR

-void show_points( const Mat& gray, const Mat& u, const vector<Point2f>& v, Size pattern_size, bool was_found )
+//#define DEBUG_CHESSBOARD
+
+#ifdef DEBUG_CHESSBOARD
+void show_points( const Mat& gray, const Mat& expected, const vector<Point2f>& actual, bool was_found )
 {
    Mat rgb( gray.size(), CV_8U);
    merge(vector<Mat>(3, gray), rgb);

-    for(size_t i = 0; i < v.size(); i++ )
-        circle( rgb, v[i], 3, Scalar(255, 0, 0), FILLED);
+    for(size_t i = 0; i < actual.size(); i++ )
+        circle( rgb, actual[i], 5, Scalar(0, 0, 200), 1, LINE_AA);

-    if( !u.empty() )
+    if( !expected.empty() )
    {
-        const Point2f* u_data = u.ptr<Point2f>();
-        size_t count = u.cols * u.rows;
+        const Point2f* u_data = expected.ptr<Point2f>();
+        size_t count = expected.cols * expected.rows;
        for(size_t i = 0; i < count; i++ )
-            circle( rgb, u_data[i], 3, Scalar(0, 255, 0), FILLED);
+            circle(rgb, u_data[i], 4, Scalar(0, 240, 0), 1, LINE_AA);
    }
-    if (!v.empty())
-    {
-        Mat corners((int)v.size(), 1, CV_32FC2, (void*)&v[0]);
-        drawChessboardCorners( rgb, pattern_size, corners, was_found );
-    }
-    //namedWindow( "test", 0 ); imshow( "test", rgb ); waitKey(0);
+    putText(rgb, was_found ? "FOUND !!!" : "NOT FOUND", Point(5, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 240, 0));
+    imshow( "test", rgb ); while ((uchar)waitKey(0) != 'q') {};
 }
-
+#else
+#define show_points(...)
+#endif

 enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };

@@ -101,7 +99,7 @@ double calcError(const vector<Point2f>& v, const Mat& u)
    int count_exp = u.cols * u.rows;
    const Point2f* u_data = u.ptr<Point2f>();

-    double err = numeric_limits<double>::max();
+    double err = std::numeric_limits<double>::max();
    for( int k = 0; k < 2; ++k )
    {
        double err1 = 0;
@@ -200,7 +198,7 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename )

    if( !fs.isOpened() || board_list.empty() || !board_list.isSeq() || board_list.size() % 2 != 0 )
    {
-        ts->printf( cvtest::TS::LOG, "%s can not be readed or is not valid\n", (folder + filename).c_str() );
+        ts->printf( cvtest::TS::LOG, "%s can not be read or is not valid\n", (folder + filename).c_str() );
        ts->printf( cvtest::TS::LOG, "fs.isOpened=%d, board_list.empty=%d, board_list.isSeq=%d,board_list.size()%2=%d\n",
            fs.isOpened(), (int)board_list.empty(), board_list.isSeq(), board_list.size()%2);
        ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
@@ -253,7 +251,6 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename )
                result = findCirclesGrid(gray, pattern_size, v, CALIB_CB_ASYMMETRIC_GRID | algorithmFlags);
                break;
        }
-        show_points( gray, Mat(), v, pattern_size, result );

        if( result ^ doesContatinChessboard || v.size() != count_exp )
        {
@@ -267,37 +264,31 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename )

 #ifndef WRITE_POINTS
            double err = calcError(v, expected);
-#if 0
-            if( err > rough_success_error_level )
-            {
-                ts.printf( cvtest::TS::LOG, "bad accuracy of corner guesses\n" );
-                ts.set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
-                continue;
-            }
-#endif
            max_rough_error = MAX( max_rough_error, err );
 #endif
            if( pattern == CHESSBOARD )
                cornerSubPix( gray, v, Size(5, 5), Size(-1,-1), TermCriteria(TermCriteria::EPS|TermCriteria::MAX_ITER, 30, 0.1));
            //find4QuadCornerSubpix(gray, v, Size(5, 5));
-            show_points( gray, expected, v, pattern_size, result  );
+            show_points( gray, expected, v, result  );
 #ifndef WRITE_POINTS
    //        printf("called find4QuadCornerSubpix\n");
            err = calcError(v, expected);
            sum_error += err;
            count++;
-#if 1
            if( err > precise_success_error_level )
            {
                ts->printf( cvtest::TS::LOG, "Image %s: bad accuracy of adjusted corners %f\n", img_file.c_str(), err );
                ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
                return;
            }
-#endif
            ts->printf(cvtest::TS::LOG, "Error on %s is %f\n", img_file.c_str(), err);
            max_precise_error = MAX( max_precise_error, err );
 #endif
        }
+        else
+        {
+            show_points( gray, Mat(), v, result );
+        }

 #ifdef WRITE_POINTS
        Mat mat_v(pattern_size, CV_32FC2, (void*)&v[0]);
@@ -341,21 +332,21 @@ bool validateData(const ChessBoardGenerator& cbg, const Size& imgSz,
        {
            const Point2f& cur = mat(i, j);

-            tmp = norm( cur - mat(i + 1, j + 1) );
+            tmp = cv::norm(cur - mat(i + 1, j + 1)); // TODO cvtest
            if (tmp < minNeibDist)
-                tmp = minNeibDist;
+                minNeibDist = tmp;

-            tmp = norm( cur - mat(i - 1, j + 1 ) );
+            tmp = cv::norm(cur - mat(i - 1, j + 1)); // TODO cvtest
            if (tmp < minNeibDist)
-                tmp = minNeibDist;
+                minNeibDist = tmp;

-            tmp = norm( cur - mat(i + 1, j - 1) );
+            tmp = cv::norm(cur - mat(i + 1, j - 1)); // TODO cvtest
            if (tmp < minNeibDist)
-                tmp = minNeibDist;
+                minNeibDist = tmp;

-            tmp = norm( cur - mat(i - 1, j - 1) );
+            tmp = cv::norm(cur - mat(i - 1, j - 1)); // TODO cvtest
            if (tmp < minNeibDist)
-                tmp = minNeibDist;
+                minNeibDist = tmp;
        }

    const double threshold = 0.25;
@@ -368,8 +359,6 @@ bool CV_ChessboardDetectorTest::checkByGenerator()
 {
    bool res = true;

-// for some reason, this test sometimes fails on Ubuntu
-#if (defined __APPLE__ && defined __x86_64__) || defined _MSC_VER
    //theRNG() = 0x58e6e895b9913160;
    //cv::DefaultRngAuto dra;
    //theRNG() = *ts->get_rng();
@@ -445,7 +434,7 @@ bool CV_ChessboardDetectorTest::checkByGenerator()
        if (found)
            res = false;

-        Point2f c = std::accumulate(cg.begin(), cg.end(), Point2f(), plus<Point2f>()) * (1.f/cg.size());
+        Point2f c = std::accumulate(cg.begin(), cg.end(), Point2f(), std::plus<Point2f>()) * (1.f/cg.size());

        Mat_<double> aff(2, 3);
        aff << 1.0, 0.0, -(double)c.x, 0.0, 1.0, 0.0;
@@ -468,7 +457,6 @@ bool CV_ChessboardDetectorTest::checkByGenerator()

        cv::drawChessboardCorners(cb, cbg.cornersSize(), Mat(corners_found), found);
    }
-#endif

    return res;
 }
@@ -476,6 +464,28 @@ bool CV_ChessboardDetectorTest::checkByGenerator()
 TEST(Calib3d_ChessboardDetector, accuracy) {  CV_ChessboardDetectorTest test( CHESSBOARD ); test.safe_run(); }
 TEST(Calib3d_CirclesPatternDetector, accuracy) { CV_ChessboardDetectorTest test( CIRCLES_GRID ); test.safe_run(); }
 TEST(Calib3d_AsymmetricCirclesPatternDetector, accuracy) { CV_ChessboardDetectorTest test( ASYMMETRIC_CIRCLES_GRID ); test.safe_run(); }
+#ifdef HAVE_OPENCV_FLANN
 TEST(Calib3d_AsymmetricCirclesPatternDetectorWithClustering, accuracy) { CV_ChessboardDetectorTest test( ASYMMETRIC_CIRCLES_GRID, CALIB_CB_CLUSTERING ); test.safe_run(); }
+#endif

+TEST(Calib3d_CirclesPatternDetectorWithClustering, accuracy)
+{
+    cv::String dataDir = string(TS::ptr()->get_data_path()) + "cv/cameracalibration/circles/";
+
+    cv::Mat expected;
+    FileStorage fs(dataDir + "circles_corners15.dat", FileStorage::READ);
+    fs["corners"] >> expected;
+    fs.release();
+
+    cv::Mat image = cv::imread(dataDir + "circles15.png");
+
+    std::vector<Point2f> centers;
+    cv::findCirclesGrid(image, Size(10, 8), centers, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
+    ASSERT_EQ(expected.total(), centers.size());
+
+    double error = calcError(centers, expected);
+    ASSERT_LE(error, precise_success_error_level);
+}
+
+}} // namespace
 /* End of file. */
@@ -43,10 +43,7 @@
 #include "test_chessboardgenerator.hpp"
 #include "opencv2/calib3d/calib3d_c.h"

-#include <limits>
-
-using namespace std;
-using namespace cv;
+namespace opencv_test { namespace {

 class CV_ChessboardDetectorBadArgTest : public cvtest::BadArgTest
 {
@@ -81,9 +78,9 @@ protected:
            findChessboardCorners(img, pattern_size, corners, flags);
        else
            if (!drawCorners)
-                cvFindChessboardCorners( &arr, pattern_size, out_corners, out_corner_count, flags );
+                cvFindChessboardCorners( &arr, cvSize(pattern_size), out_corners, out_corner_count, flags );
            else
-                cvDrawChessboardCorners( &drawCorImg, pattern_size,
+                cvDrawChessboardCorners( &drawCorImg, cvSize(pattern_size),
                    (CvPoint2D32f*)(corners.empty() ? 0 : &corners[0]),
                    (int)corners.size(), was_found);
    }
@@ -118,7 +115,7 @@ void CV_ChessboardDetectorBadArgTest::run( int /*start_from */)

    img = cb.clone();
    pattern_size = Size(2,2);
-    errors += run_test_case( CV_StsOutOfRange, "Invlid pattern size" );
+    errors += run_test_case( CV_StsOutOfRange, "Invalid pattern size" );

    pattern_size = cbg.cornersSize();
    cb.convertTo(img, CV_32F);
@@ -131,14 +128,14 @@ void CV_ChessboardDetectorBadArgTest::run( int /*start_from */)
    drawCorners = false;

    img = cb.clone();
-    arr = img;
+    arr = cvMat(img);
    out_corner_count = 0;
    out_corners = 0;
    errors += run_test_case( CV_StsNullPtr, "Null pointer to corners" );

    drawCorners = true;
    Mat cvdrawCornImg(img.size(), CV_8UC2);
-    drawCorImg = cvdrawCornImg;
+    drawCorImg = cvMat(cvdrawCornImg);
    was_found = true;
    errors += run_test_case( CV_StsUnsupportedFormat, "2 channel image" );

@@ -151,4 +148,5 @@ void CV_ChessboardDetectorBadArgTest::run( int /*start_from */)

 TEST(Calib3d_ChessboardDetector, badarg) { CV_ChessboardDetectorBadArgTest test; test.safe_run(); }

+}} // namespace
 /* End of file. */
@@ -43,6 +43,8 @@
 #include "opencv2/imgproc/imgproc_c.h"
 #include "opencv2/calib3d/calib3d_c.h"

+namespace opencv_test { namespace {
+
 class CV_ChessboardDetectorTimingTest : public cvtest::BaseTest
 {
 public:
@@ -83,7 +85,7 @@ void CV_ChessboardDetectorTimingTest::run( int start_from )
    if( !fs || !board_list || !CV_NODE_IS_SEQ(board_list->tag) ||
        board_list->data.seq->total % 4 != 0 )
    {
-        ts->printf( cvtest::TS::LOG, "chessboard_timing_list.dat can not be readed or is not valid" );
+        ts->printf( cvtest::TS::LOG, "chessboard_timing_list.dat can not be read or is not valid" );
        code = cvtest::TS::FAIL_MISSING_TEST_DATA;
        goto _exit_;
    }
@@ -94,7 +96,7 @@ void CV_ChessboardDetectorTimingTest::run( int start_from )
    {
        int count0 = -1;
        int count = 0;
-        CvSize pattern_size;
+        Size pattern_size;
        int result, result1 = 0;

        const char* imgname = cvReadString((CvFileNode*)cvGetSeqElem(board_list->data.seq,idx*4), "dummy.txt");
@@ -108,16 +110,12 @@ void CV_ChessboardDetectorTimingTest::run( int start_from )
        filename = cv::format("%s%s", filepath.c_str(), imgname );

        cv::Mat img2 = cv::imread( filename );
-        img = img2;
+        img = cvIplImage(img2);

        if( img2.empty() )
        {
            ts->printf( cvtest::TS::LOG, "one of chessboard images can't be read: %s\n", filename.c_str() );
-            if( max_idx == 1 )
-            {
-                code = cvtest::TS::FAIL_MISSING_TEST_DATA;
-                goto _exit_;
-            }
+            code = cvtest::TS::FAIL_MISSING_TEST_DATA;
            continue;
        }

@@ -137,11 +135,11 @@ void CV_ChessboardDetectorTimingTest::run( int start_from )
        v = (CvPoint2D32f*)_v->data.fl;

        int64 _time0 = cvGetTickCount();
-        result = cvCheckChessboard(gray, pattern_size);
+        result = cvCheckChessboard(gray, cvSize(pattern_size));
        int64 _time01 = cvGetTickCount();

        OPENCV_CALL( result1 = cvFindChessboardCorners(
-                 gray, pattern_size, v, &count, 15 ));
+                 gray, cvSize(pattern_size), v, &count, 15 ));
        int64 _time1 = cvGetTickCount();

        if( result != is_chessboard )
@@ -186,4 +184,5 @@ _exit_:

 TEST(Calib3d_ChessboardDetector, timing) { CV_ChessboardDetectorTimingTest test; test.safe_run(); }

+}} // namespace
 /* End of file. */
@@ -42,8 +42,7 @@

 #include "test_precomp.hpp"

-using namespace cv;
-using namespace std;
+namespace opencv_test { namespace {

 class Differential
 {
@@ -155,14 +154,14 @@ protected:
        Mat rvec3_exp, tvec3_exp;

        Mat rmat1, rmat2;
-        Rodrigues(rvec1, rmat1);
-        Rodrigues(rvec2, rmat2);
-        Rodrigues(rmat2 * rmat1, rvec3_exp);
+        cv::Rodrigues(rvec1, rmat1); // TODO cvtest
+        cv::Rodrigues(rvec2, rmat2); // TODO cvtest
+        cv::Rodrigues(rmat2 * rmat1, rvec3_exp); // TODO cvtest

        tvec3_exp = rmat2 * tvec1 + tvec2;

        const double thres = 1e-5;
-        if (norm(rvec3_exp, rvec3) > thres ||  norm(tvec3_exp, tvec3) > thres)
+        if (cv::norm(rvec3_exp, rvec3) > thres ||  cv::norm(tvec3_exp, tvec3) > thres)
            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);

        const double eps = 1e-3;
@@ -176,7 +175,7 @@ protected:
        Mat_<double> dr3_dr1, dt3_dr1;
           diff.dRv1(dr3_dr1, dt3_dr1);

-        if (norm(dr3_dr1, dr3dr1) > thres || norm(dt3_dr1, dt3dr1) > thres)
+        if (cv::norm(dr3_dr1, dr3dr1) > thres || cv::norm(dt3_dr1, dt3dr1) > thres)
        {
            ts->printf( cvtest::TS::LOG, "Invalid derivates by r1\n" );
            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
@@ -185,7 +184,7 @@ protected:
        Mat_<double> dr3_dr2, dt3_dr2;
           diff.dRv2(dr3_dr2, dt3_dr2);

-        if (norm(dr3_dr2, dr3dr2) > thres || norm(dt3_dr2, dt3dr2) > thres)
+        if (cv::norm(dr3_dr2, dr3dr2) > thres || cv::norm(dt3_dr2, dt3dr2) > thres)
        {
            ts->printf( cvtest::TS::LOG, "Invalid derivates by r2\n" );
            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
@@ -194,7 +193,7 @@ protected:
        Mat_<double> dr3_dt1, dt3_dt1;
           diff.dTv1(dr3_dt1, dt3_dt1);

-        if (norm(dr3_dt1, dr3dt1) > thres || norm(dt3_dt1, dt3dt1) > thres)
+        if (cv::norm(dr3_dt1, dr3dt1) > thres || cv::norm(dt3_dt1, dt3dt1) > thres)
        {
            ts->printf( cvtest::TS::LOG, "Invalid derivates by t1\n" );
            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
@@ -203,7 +202,7 @@ protected:
        Mat_<double> dr3_dt2, dt3_dt2;
           diff.dTv2(dr3_dt2, dt3_dt2);

-        if (norm(dr3_dt2, dr3dt2) > thres || norm(dt3_dt2, dt3dt2) > thres)
+        if (cv::norm(dr3_dt2, dr3dt2) > thres || cv::norm(dt3_dt2, dt3dt2) > thres)
        {
            ts->printf( cvtest::TS::LOG, "Invalid derivates by t2\n" );
            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
@@ -212,3 +211,5 @@ protected:
 };

 TEST(Calib3d_ComposeRT, accuracy) { CV_composeRT_Test test; test.safe_run(); }
+
+}} // namespace
@@ -41,11 +41,9 @@

 #include "test_precomp.hpp"
 #include "opencv2/imgproc/imgproc_c.h"
-#include <limits>
 #include "test_chessboardgenerator.hpp"

-using namespace std;
-using namespace cv;
+namespace opencv_test { namespace {

 class CV_ChessboardSubpixelTest : public cvtest::BaseTest
 {
@@ -78,7 +76,7 @@ int calcDistance(const vector<Point2f>& set1, const vector<Point2f>& set2, doubl

        for(int j = 0; j < (int)set2.size(); j++)
        {
-            double dist = norm(set1[i] - set2[j]);
+            double dist = cv::norm(set1[i] - set2[j]); // TODO cvtest
            if(dist < min_dist)
            {
                min_idx = j;
@@ -182,7 +180,7 @@ void CV_ChessboardSubpixelTest::run( int )
            break;
        }

-        IplImage chessboard_image_header = chessboard_image;
+        IplImage chessboard_image_header = cvIplImage(chessboard_image);
        cvFindCornerSubPix(&chessboard_image_header, (CvPoint2D32f*)&test_corners[0],
            (int)test_corners.size(), cvSize(3, 3), cvSize(1, 1), cvTermCriteria(CV_TERMCRIT_EPS|CV_TERMCRIT_ITER,300,0.1));
        find4QuadCornerSubpix(chessboard_image, test_corners, Size(5, 5));
@@ -242,4 +240,19 @@ void CV_ChessboardSubpixelTest::generateIntrinsicParams()

 TEST(Calib3d_ChessboardSubPixDetector, accuracy) { CV_ChessboardSubpixelTest test; test.safe_run(); }

+TEST(Calib3d_CornerSubPix, regression_7204)
+{
+    cv::Mat image(cv::Size(70, 38), CV_8UC1, cv::Scalar::all(0));
+    image(cv::Rect(65, 26, 5, 5)).setTo(cv::Scalar::all(255));
+    image(cv::Rect(55, 31, 8, 1)).setTo(cv::Scalar::all(255));
+    image(cv::Rect(56, 35, 14, 2)).setTo(cv::Scalar::all(255));
+    image(cv::Rect(66, 24, 4, 2)).setTo(cv::Scalar::all(255));
+    image.at<uchar>(24, 69) = 0;
+    std::vector<cv::Point2f> corners;
+    corners.push_back(cv::Point2f(65, 30));
+    cv::cornerSubPix(image, corners, cv::Size(3, 3), cv::Size(-1, -1),
+        cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
+}
+
+}} // namespace
 /* End of file. */
@@ -0,0 +1,144 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+class CV_DecomposeProjectionMatrixTest : public cvtest::BaseTest
+{
+public:
+    CV_DecomposeProjectionMatrixTest();
+protected:
+    void run(int);
+};
+
+CV_DecomposeProjectionMatrixTest::CV_DecomposeProjectionMatrixTest()
+{
+    test_case_count = 30;
+}
+
+
+void CV_DecomposeProjectionMatrixTest::run(int start_from)
+{
+
+    ts->set_failed_test_info(cvtest::TS::OK);
+
+    cv::RNG& rng = ts->get_rng();
+    int progress = 0;
+
+
+    for (int iter = start_from; iter < test_case_count; ++iter)
+    {
+        ts->update_context(this, iter, true);
+        progress = update_progress(progress, iter, test_case_count, 0);
+
+        // Create the original (and random) camera matrix, rotation, and translation
+        cv::Vec2d f, c;
+        rng.fill(f, cv::RNG::UNIFORM, 300, 1000);
+        rng.fill(c, cv::RNG::UNIFORM, 150, 600);
+
+        double alpha = 0.01*rng.gaussian(1);
+
+        cv::Matx33d origK(f(0), alpha*f(0), c(0),
+                          0,          f(1), c(1),
+                          0,             0,   1);
+
+
+        cv::Vec3d rVec;
+        rng.fill(rVec, cv::RNG::UNIFORM, -CV_PI, CV_PI);
+
+        cv::Matx33d origR;
+        cv::Rodrigues(rVec, origR); // TODO cvtest
+
+        cv::Vec3d origT;
+        rng.fill(origT, cv::RNG::NORMAL, 0, 1);
+
+
+        // Compose the projection matrix
+        cv::Matx34d P(3,4);
+        hconcat(origK*origR, origK*origT, P);
+
+
+        // Decompose
+        cv::Matx33d K, R;
+        cv::Vec4d homogCameraCenter;
+        decomposeProjectionMatrix(P, K, R, homogCameraCenter);
+
+
+        // Recover translation from the camera center
+        cv::Vec3d cameraCenter(homogCameraCenter(0), homogCameraCenter(1), homogCameraCenter(2));
+        cameraCenter /= homogCameraCenter(3);
+
+        cv::Vec3d t = -R*cameraCenter;
+
+
+        const double thresh = 1e-6;
+        if (cv::norm(origK, K, cv::NORM_INF) > thresh)
+        {
+            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+            break;
+        }
+
+        if (cv::norm(origR, R, cv::NORM_INF) > thresh)
+        {
+            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+            break;
+        }
+
+        if (cv::norm(origT, t, cv::NORM_INF) > thresh)
+        {
+            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+            break;
+        }
+
+    }
+
+}
+
+TEST(Calib3d_DecomposeProjectionMatrix, accuracy)
+{
+    CV_DecomposeProjectionMatrixTest test;
+    test.safe_run();
+}
+
+}} // namespace
@@ -0,0 +1,575 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2016, OpenCV Foundation, all rights reserved.
+//
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+#include "opencv2/calib3d.hpp"
+
+namespace opencv_test { namespace {
+
+class CV_FilterHomographyDecompTest : public cvtest::BaseTest {
+
+public:
+    CV_FilterHomographyDecompTest()
+    {
+        buildTestDataSet();
+    }
+
+protected:
+    void run(int)
+    {
+        vector<int> finalSolutions;
+        filterHomographyDecompByVisibleRefpoints(_rotations, _normals, _prevRectifiedPoints, _currRectifiedPoints, finalSolutions, _mask);
+
+        //there should be at least 2 solution
+        ASSERT_EQ(finalSolutions, _validSolutions);
+    }
+
+private:
+
+    void buildTestDataSet()
+    {
+        double rotationsArray[4][9] = {
+            {
+                0.98811084196540500,
+                -0.15276633082836735,
+                0.017303530150126534,
+                0.14161851662094097,
+                0.94821044891315664,
+                0.28432576443578628,
+                -0.059842791884259422,
+                -0.27849487021693553,
+                0.95857156619751127
+            },
+            {
+                0.98811084196540500,
+                -0.15276633082836735,
+                0.017303530150126534,
+                0.14161851662094097,
+                0.94821044891315664,
+                0.28432576443578628,
+                -0.059842791884259422,
+                -0.27849487021693553,
+                0.95857156619751127
+            },
+            {
+                0.95471096402077438,
+                -0.21080808634428211,
+                -0.20996886890771557,
+                0.20702063153797226,
+                0.97751379914116743,
+                -0.040115216641822840,
+                0.21370407880090386,
+                -0.0051694506925720751,
+                0.97688476468997820
+            },
+            {
+                0.95471096402077438,
+                -0.21080808634428211,
+                -0.20996886890771557,
+                0.20702063153797226,
+                0.97751379914116743,
+                -0.040115216641822840,
+                0.21370407880090386,
+                -0.0051694506925720751,
+                0.97688476468997820
+            }
+        };
+
+        double normalsArray[4][3] = {
+            {
+                -0.023560516110791116,
+                0.085818414407956692,
+                0.99603217911325403
+            },
+            {
+                0.023560516110791116,
+                -0.085818414407956692,
+                -0.99603217911325403
+            },
+            {
+                -0.62483547397726014,
+                -0.56011861446691769,
+                0.54391889853844289
+            },
+            {
+                0.62483547397726014,
+                0.56011861446691769,
+                -0.54391889853844289
+            }
+        };
+
+        uchar maskArray[514] =
+        {
+            0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 0, 0, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 0,
+            0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0,
+            1, 1, 1, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 1,
+            0, 0, 1, 1, 1, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 0,
+            0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 0, 1,
+            1, 1, 1, 0, 1, 1, 0, 0, 1, 1, 1, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 1, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 0,
+            0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 0, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 0,
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 1, 1, 1,
+            1, 1, 0, 1, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 0, 0,
+            0, 1, 0, 1, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0,
+            0, 1, 0, 1, 0, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0,
+            1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1,
+            1, 0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0,
+            0, 1, 0, 0, 1, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 0, 1, 0, 1, 0, 0, 0, 1, 1, 0, 0,
+            0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+        };
+
+        static const float currRectifiedPointArr[] =
+        {
+            -0.565732896f, -0.321162999f, -0.416198403f, -0.299646467f, -0.408354312f, -0.290387660f,
+            -0.386555284f, -0.287677139f, -0.348475337f, -0.276208878f, -0.415957332f, -0.266133875f,
+            -0.354961902f, -0.257545590f, -0.420189440f, -0.255190015f, -0.379785866f, -0.252570540f,
+            -0.144345313f, -0.249134675f, -0.162417486f, -0.223227784f, -0.129876539f, -0.219722182f,
+            -0.470801264f, -0.211166814f, 0.0992607549f, -0.209064797f, 0.123508267f, -0.196303099f,
+            -0.521849990f, -0.190706849f, -0.513497114f, -0.189186409f, -0.534959674f, -0.185138911f,
+            0.121614374f, -0.182721153f, 0.154205695f, -0.183763996f, -0.516449869f, -0.181606859f,
+            -0.523427486f, -0.180088669f, 0.149494573f, -0.179563865f, -0.552187204f, -0.172630817f,
+            -0.322249800f, -0.172333881f, 0.127574071f, -0.165683150f, 0.159817487f, -0.162389070f,
+            -0.578930736f, -0.160272732f, -0.600617707f, -0.155920163f, -0.249115735f, -0.154711768f,
+            -0.543279886f, -0.144873798f, -0.529992998f, -0.142433196f, 0.0554505363f, -0.142878756f,
+            -0.613355398f, -0.132748783f, 0.190059289f, -0.128930226f, -0.255682647f, -0.127393380f,
+            0.0299431719f, -0.125339776f, -0.282943249f, -0.118550651f, -0.0348402821f, -0.115398556f,
+            -0.0362741761f, -0.110100254f, -0.319089264f, -0.104354575f, -0.0401916653f, -0.0852083191f,
+            -0.372183621f, -0.0812346712f, -0.00707253255f, -0.0810251758f, 0.267345309f, -0.0787685066f,
+            0.258760840f, -0.0768160895f, -0.377273679f, -0.0763452053f, -0.0314898677f, -0.0743160769f,
+            0.223423928f, -0.0724818707f, 0.00284322398f, -0.0720727518f, 0.232531011f, -0.0682833865f,
+            0.282355100f, -0.0655683428f, -0.233353317f, -0.0613981225f, 0.290982842f, -0.0607336313f,
+            -0.0994169787f, -0.0376472026f, 0.257561266f, -0.0331368558f, 0.265076399f, -0.0320781991f,
+            0.0454338901f, -0.0238198638f, 0.0409987904f, -0.0186991505f, -0.502306283f, -0.0172236171f,
+            -0.464807063f, -0.0149533665f, -0.185798749f, -0.00540314987f, 0.182073534f, -0.000651287497f,
+            -0.435764432f, 0.00162558386f, -0.181552932f, 0.00792864431f, -0.700565279f, 0.0110246018f,
+            -0.144087434f, 0.0120453080f, -0.524990261f, 0.0138590708f, -0.182723984f, 0.0165519360f,
+            -0.217308879f, 0.0208590515f, 0.462978750f, 0.0247372910f, 0.0956632495f, 0.0323494300f,
+            0.0843820646f, 0.0424364135f, 0.122466311f, 0.0441578403f, -0.162433729f, 0.0528083183f,
+            0.0964344442f, 0.0624147579f, -0.271349967f, 0.0727724135f, -0.266336441f, 0.0719895661f,
+            0.0675768778f, 0.0848240927f, -0.689944625f, 0.0889045894f, -0.680990934f, 0.0903657600f,
+            -0.119472280f, 0.0930491239f, -0.124393739f, 0.0933082998f, -0.323403478f, 0.0937438533f,
+            -0.323273063f, 0.0969979763f, -0.352427900f, 0.101048596f, -0.327554941f, 0.104539163f,
+            -0.330044419f, 0.114519835f, 0.0235135648f, 0.118004657f, -0.671623945f, 0.130437061f,
+            -0.385111898f, 0.142786101f, -0.376281500f, 0.145800456f, -0.0169987213f, 0.148056105f,
+            -0.326495141f, 0.152596891f, -0.337120056f, 0.154522225f, -0.336885720f, 0.154304653f,
+            0.322089493f, 0.155130088f, -0.0713477954f, 0.163638428f, -0.0208650175f, 0.171433330f,
+            -0.380652726f, 0.172022790f, -0.0599780641f, 0.182294667f, 0.244408697f, 0.194245726f,
+            -0.101454332f, 0.198159069f, 0.257901788f, 0.200226694f, -0.0775909275f, 0.205242962f,
+            0.231870517f, 0.222396746f, -0.546760798f, 0.242291704f, -0.538914979f, 0.243761152f,
+            0.206653103f, 0.244874880f, -0.595693469f, 0.264329463f, -0.581023335f, 0.265664101f,
+            0.00444878871f, 0.267031074f, -0.573156178f, 0.271591753f, -0.543381274f, 0.271759123f,
+            0.00450209389f, 0.271335930f, -0.223618075f, 0.278416723f, 0.161934286f, 0.289435983f,
+            -0.199636295f, 0.296817899f, -0.250217140f, 0.299677849f, -0.258231103f, 0.314012855f,
+            -0.628315628f, 0.316889286f, 0.320948511f, 0.316358119f, -0.246845752f, 0.320511192f,
+            0.0687271580f, 0.321383297f, 0.0784438103f, 0.322898388f, 0.0946765989f, 0.325111747f,
+            -0.249674007f, 0.328731328f, -0.244633347f, 0.329467386f, -0.245841011f, 0.334985316f,
+            0.118609101f, 0.343532443f, 0.0497615598f, 0.348162144f, -0.221477821f, 0.349263757f,
+            0.0759577379f, 0.351840734f, 0.0504637137f, 0.373238713f, 0.0730970055f, 0.376537383f,
+            -0.204333842f, 0.381100655f, -0.557245076f, -0.339432925f, -0.402010202f, -0.288829565f,
+            -0.350465477f, -0.281259984f, -0.352995187f, -0.264569730f, -0.466762394f, -0.217114508f,
+            0.152002022f, -0.217566550f, 0.146226048f, -0.183914393f, 0.0949312001f, -0.177005857f,
+            -0.211882949f, -0.175594494f, -0.531562269f, -0.173924312f, -0.0727246776f, -0.167270422f,
+            0.0546481088f, -0.140193000f, -0.296819001f, -0.137850702f, -0.261863053f, -0.139540121f,
+            0.187967837f, -0.131033540f, 0.322852045f, -0.112108752f, -0.0432251953f, -0.102951847f,
+            -0.0453428440f, -0.0914504975f, -0.0182842426f, -0.0918859020f, 0.0140433423f, -0.0904538929f,
+            -0.377287626f, -0.0817026496f, 0.266108125f, -0.0797783583f, 0.257961422f, -0.0767710134f,
+            -0.495943695f, -0.0683977529f, 0.231466040f, -0.0675206482f, -0.240675926f, -0.0551427566f,
+            -0.482824773f, -0.0510699376f, -0.491354793f, -0.0414650664f, -0.0960614979f, -0.0377000235f,
+            -0.102409534f, -0.0369749814f, -0.471273214f, -0.0325376652f, -0.483320534f, -0.0174943600f,
+            -0.457503378f, -0.0152483145f, -0.178161725f, -0.0153892851f, -0.483233035f, -0.0106405178f,
+            -0.472914547f, -0.0105228210f, -0.166542307f, -0.00667150877f, 0.181261331f, -0.00449455017f,
+            -0.474292487f, -0.00428914558f, -0.185297221f, -0.00575157674f, -0.494381040f, -0.00278507406f,
+            -0.141748473f, -0.00289725070f, -0.487515569f, 0.000758233888f, 0.322646528f, 0.0197495818f,
+            0.142943904f, 0.0276249554f, -0.563232243f, 0.0306834858f, -0.555995941f, 0.0367121249f,
+            0.114935011f, 0.0496927276f, -0.152954608f, 0.0538645200f, -0.594885707f, 0.0562511310f,
+            0.0678326488f, 0.0756176412f, -0.667605639f, 0.0828208700f, -0.354470938f, 0.101424232f,
+            0.0228204262f, 0.120382607f, -0.639557123f, 0.124422595f, -0.690505445f, 0.126883239f,
+            -0.395509213f, 0.130242139f, -0.00618012529f, 0.139929801f, 0.175945997f, 0.140235618f,
+            0.198833048f, 0.167587668f, -0.334679037f, 0.177859858f, 0.236127406f, 0.192743436f,
+            0.283146858f, 0.204260647f, -0.0354267135f, 0.206209183f, 0.247388184f, 0.207016930f,
+            -0.0422560424f, 0.212493256f, 0.261681855f, 0.215763748f, 0.207528576f, 0.219807997f,
+            -0.300219178f, 0.221922547f, 0.206393883f, 0.245171010f, 0.239619836f, 0.244768366f,
+            -0.523026288f, 0.250639766f, -0.591975033f, 0.254252791f, 0.246785000f, 0.252878994f,
+            0.272995651f, 0.255815417f, 0.00825022161f, 0.265591830f, 0.192723796f, 0.266924977f,
+            -0.222951472f, 0.290150762f, -0.545146644f, 0.304910392f, 0.131736591f, 0.319247276f,
+            0.319435924f, 0.317917794f, 0.0687546134f, 0.321296155f, -0.255853772f, 0.327258259f,
+            0.0948092714f, 0.325284332f, 0.104488030f, 0.327628911f, -0.245483562f, 0.327617317f,
+            0.0647632629f, 0.363111496f, -0.382861346f, -0.287226975f, -0.354297429f, -0.278708905f,
+            -0.356116027f, -0.262691110f, -0.369049937f, -0.237850189f, -0.146217853f, -0.233530551f,
+            0.102752604f, -0.223108903f, 0.137545392f, -0.218163848f, 0.125815898f, -0.216970086f,
+            -0.557826996f, -0.194665924f, -0.533946335f, -0.184958249f, 0.0976954028f, -0.173691019f,
+            -0.240166873f, -0.160652772f, 0.166464865f, -0.154563308f, -0.0330923162f, -0.125799045f,
+            -0.290044904f, -0.118914597f, 0.00350888353f, -0.108661920f, -0.0109116854f, -0.106212743f,
+            -0.0298740193f, -0.102953635f, -0.287203342f, -0.0997403413f, -0.269498408f, -0.0981520712f,
+            -0.000815737061f, -0.0938294530f, 0.274663270f, -0.0844340026f, -0.371082008f, -0.0805466920f,
+            -0.368196100f, -0.0743779093f, 0.00675902702f, -0.0735078678f, 0.226267770f, -0.0744194537f,
+            -0.241736412f, -0.0630025938f, -0.408663541f, -0.0564615242f, 0.251640886f, -0.0519632548f,
+            0.249993712f, -0.0519672707f, -0.426033378f, -0.0365641154f, -0.467352122f, -0.0305716563f,
+            0.251341015f, -0.0268137120f, -0.443456501f, -0.0243669953f, -0.502199471f, -0.0151771074f,
+            -0.178487480f, -0.0155749097f, 0.178145915f, -0.00528379623f, -0.492981344f, -0.00174682145f,
+            -0.150337398f, 0.000692513015f, -0.457302928f, 0.00352234906f, 0.190587431f, 0.00151424226f,
+            -0.482671946f, 0.00682042213f, -0.158589542f, 0.0150188655f, -0.182223722f, 0.0145649035f,
+            0.107089065f, 0.0223725326f, 0.135399371f, 0.0275243558f, -0.552838683f, 0.0275048595f,
+            -0.432176501f, 0.0248741303f, -0.192510992f, 0.0281074084f, -0.553043425f, 0.0298770685f,
+            -0.684887648f, 0.0436144769f, 0.0850105733f, 0.0448755622f, -0.165784389f, 0.0439001285f,
+            0.102653719f, 0.0457992665f, 0.114853017f, 0.0504316092f, -0.647432685f, 0.0608204119f,
+            0.0828530043f, 0.0608987175f, 0.0894377902f, 0.0742467493f, 0.0702404827f, 0.0767309442f,
+            -0.613642335f, 0.0779517740f, -0.670592189f, 0.0849624202f, -0.395209312f, 0.0854151621f,
+            0.125186160f, 0.0919951499f, -0.359707922f, 0.102121405f, -0.354259193f, 0.101300709f,
+            0.0304000825f, 0.110619470f, -0.677573025f, 0.114422500f, 0.0305799693f, 0.121603437f,
+            -0.358950615f, 0.121660560f, -0.718753040f, 0.134569481f, 0.256451160f, 0.141883001f,
+            -0.0904129520f, 0.146879435f, -0.0184279438f, 0.148968369f, -0.356992692f, 0.160104826f,
+            -0.337676436f, 0.161766291f, 0.201174691f, 0.169025913f, -0.378423393f, 0.170933828f,
+            -0.601599216f, 0.174998865f, -0.0902864039f, 0.184311926f, -0.0584819093f, 0.184186250f,
+            0.294467270f, 0.182560727f, 0.250262231f, 0.186239958f, -0.326370239f, 0.191697389f,
+            -0.0980727375f, 0.196913749f, 0.253085673f, 0.201914877f, -0.0344332159f, 0.205900863f,
+            0.255287141f, 0.203029931f, -0.452713937f, 0.205191836f, 0.264822274f, 0.217408702f,
+            -0.0290334225f, 0.221684650f, -0.583990574f, 0.237398431f, -0.145020664f, 0.240374506f,
+            0.249667659f, 0.254706532f, 0.274279058f, 0.256447285f, -0.282936275f, 0.259140193f,
+            0.241211995f, 0.260401577f, -0.590560019f, 0.272659779f, -0.574947417f, 0.272671998f,
+            -0.224780366f, 0.279990941f, -0.525540829f, 0.287235677f, -0.247069210f, 0.298608154f,
+            -0.201292604f, 0.298156679f, 0.319822490f, 0.317605704f, -0.248013541f, 0.320789784f,
+            0.0957527757f, 0.326543272f, 0.105006196f, 0.328469753f, -0.264089525f, 0.332354158f,
+            -0.670460403f, 0.339870930f, 0.118318990f, 0.345167071f, 0.0737744719f, 0.353734553f,
+            0.0655663237f, 0.361025929f, -0.306805104f, 0.363820761f, 0.0524423867f, 0.371921480f,
+            0.0713953897f, 0.375074357f, -0.411387652f, -0.268335998f, -0.357590824f, -0.263346583f,
+            -0.407676578f, -0.253785878f, 0.0660323426f, -0.253718942f, -0.157670841f, -0.225629836f,
+            0.170453921f, -0.220800355f, -0.475751191f, -0.209005311f, -0.331408232f, -0.203059763f,
+            -0.173841938f, -0.199112654f, -0.503261328f, -0.193795130f, -0.532277644f, -0.190292686f,
+            -0.0972326621f, -0.191563144f, -0.0692789108f, -0.172031537f, -0.318824291f, -0.169072524f,
+            -0.576232314f, -0.162124678f, -0.0839322209f, -0.156304389f, -0.583625376f, -0.142171323f,
+            -0.0546422042f, -0.135338858f, 0.0501612425f, -0.132490858f, -0.645011544f, -0.111341864f,
+            -0.0925374180f, -0.0483307689f, -0.444242209f, -0.0263337940f, 0.0335495919f, -0.0281750113f,
+            0.274629444f, -0.0259516705f, 0.213774025f, -0.0240113474f, -0.194874078f, -0.0151330847f,
+            0.175111562f, -0.00868577976f, -0.185011521f, -0.000680683181f, 0.152071685f, 0.0204544198f,
+            0.321354061f, 0.0199794695f, -0.192160159f, 0.0275637116f, -0.189656645f, 0.0275667012f,
+            0.137452200f, 0.0298070628f, -0.194602579f, 0.0449027494f, -0.647751570f, 0.0625102371f,
+            0.124078721f, 0.0639316663f, 0.125849217f, 0.0762147456f, -0.614036798f, 0.0778791085f,
+            -0.684063017f, 0.0867959261f, -0.670344174f, 0.0846142769f, -0.127689242f, 0.0883567855f,
+            0.123796627f, 0.0907361880f, -0.356352538f, 0.101948388f, -0.388843179f, 0.110183217f,
+            0.0316384435f, 0.123791300f, -0.627986908f, 0.146491125f, -0.0747071728f, 0.158135459f,
+            -0.0235102437f, 0.168867558f, -0.0903210714f, 0.184088305f, 0.292073458f, 0.183571488f,
+            -0.0585953295f, 0.184784085f, -0.0317775607f, 0.218368888f, 0.209752038f, 0.223883361f,
+            -0.295424402f, 0.229150623f, -0.144439027f, 0.237902716f, -0.284140587f, 0.262761474f,
+            0.289083928f, 0.276900887f, 0.159017235f, 0.300793648f, -0.204925507f, 0.298536539f,
+            -0.544958472f, 0.305164427f, -0.261615157f, 0.306550682f, 0.0977220088f, 0.327949613f,
+            0.109876208f, 0.337665111f, -0.283918083f, 0.347385526f, 0.0436712503f, 0.350702018f,
+            0.114512287f, 0.367949426f, 0.106543839f, 0.375095814f, 0.505324781f, -0.272183985f,
+            0.0645913780f, -0.251512915f, -0.457196057f, -0.225893468f, -0.480293810f, -0.222602293f,
+            -0.138176888f, -0.209798917f, -0.110901751f, -0.198036820f, -0.196451947f, -0.191723794f,
+            -0.537742376f, -0.174413025f, -0.0650562346f, -0.174762890f, -0.567489207f, -0.165461496f,
+            0.0879585966f, -0.163023785f, -0.303777844f, -0.142031133f, 0.199195996f, -0.141861767f,
+            0.0491657220f, -0.132264882f, -0.497363061f, -0.107934952f, -0.000536393432f, -0.102828167f,
+            0.0155952247f, -0.0998895392f, -0.363601953f, -0.0897399634f, -0.224325985f, -0.0719678402f,
+            -0.0638299435f, -0.0646244809f, -0.108656809f, -0.0468749776f, -0.0865045264f, -0.0512534790f,
+            -0.469339728f, -0.0279338267f, 0.0578282699f, -0.0133374622f, -0.195265710f, -0.0115369316f,
+            0.296735317f, -0.0132813146f, 0.0664219409f, 0.0134935537f, 0.126060545f, 0.0333039127f,
+            0.139887005f, 0.0334976614f, -0.547339618f, 0.0433730707f, 0.0866046399f, 0.0527233221f,
+            0.131943896f, 0.0657638907f, -0.280056775f, 0.0685855150f, 0.0746403933f, 0.0795079395f,
+            0.125382811f, 0.0822770745f, -0.648187757f, 0.103887804f, -0.107411072f, 0.107508548f,
+            0.0155869983f, 0.108978622f, 0.0189307462f, 0.129617691f, 0.162685350f, 0.127225950f,
+            -0.0875291452f, 0.142281070f, 0.319728941f, 0.148827255f, -0.0259547811f, 0.169724479f,
+            0.259297132f, 0.190075457f, -0.467013776f, 0.212794706f, -0.315732479f, 0.219243437f,
+            -0.111042649f, 0.217940107f, 0.239550352f, 0.222786069f, 0.263966352f, 0.260309041f,
+            0.320023954f, -0.222228840f, -0.322707742f, -0.213004455f, -0.224977970f, -0.169595599f,
+            -0.605799317f, -0.142425537f, 0.0454332717f, -0.129945949f, 0.205748767f, -0.113405459f,
+            0.317985803f, -0.118630089f, 0.497755647f, -0.0962266177f, -0.393495560f, -0.0904672816f,
+            0.240035087f, -0.0737613589f, -0.212947786f, -0.0280145984f, 0.0674179196f, 0.0124880793f,
+            -0.545862198f, 0.0207057912f, -0.284409463f, 0.0626631007f, -0.107082598f, 0.0854173824f,
+            0.0578137375f, 0.0917839557f, 0.145844117f, 0.102937251f, 0.183878779f, 0.119614877f,
+            -0.626380265f, 0.140862882f, -0.0325521491f, 0.161834121f, -0.590211987f, 0.167720392f,
+            0.289599866f, 0.186565816f, -0.328821093f, 0.187714070f, -0.289086968f, 0.205165654f,
+            -0.445392698f, 0.215343162f, 0.173715711f, 0.273563296f, 0.284015119f, 0.270610362f,
+            0.0174398609f, 0.283809274f, -0.496335506f, -0.202981815f, 0.0389454551f, -0.166210428f,
+            -0.317301393f, -0.156280205f, -0.396320462f, -0.0949599668f, -0.213638976f, -0.0776446015f,
+            0.497601509f, -0.0928353444f, -0.260220319f, -0.0718628615f, -0.116495222f, -0.0543703064f,
+            -0.118132629f, -0.0156126227f, 0.0242815297f, 0.00629332382f, -0.537928998f, 0.00815516617f,
+            0.317720622f, 0.0271231923f, -0.582170665f, 0.0478387438f, -0.536856830f, 0.0466793887f,
+            -0.220819592f, 0.0433096550f, -0.246473342f, 0.0572598167f, 0.481240988f, 0.0503845438f,
+            -0.102453016f, 0.0649363101f, -0.149955124f, 0.0744054317f, -0.248215869f, 0.0916868672f,
+            -0.101221249f, 0.110788561f, -0.437672526f, 0.179065496f, -0.0383506976f, 0.183546484f,
+            -0.279600590f, 0.208760634f, 0.182261929f, 0.275244594f, 0.0253023170f, -0.170456246f,
+            -0.476852804f, -0.123630777f, -0.0803126246f, -0.0782076195f, -0.133338496f, -0.0659459904f,
+            -0.0822777376f, -0.00390591589f, 0.149250969f, 0.104314201f, 0.0418044887f, 0.149009049f,
+            -0.438308835f, 0.164682120f
+        };
+
+        const Point2f* currRectifiedPointArr_2f = (const Point2f*)currRectifiedPointArr;
+        vector<Point2f> currRectifiedPoints(currRectifiedPointArr_2f,
+            currRectifiedPointArr_2f + sizeof(currRectifiedPointArr) / sizeof(currRectifiedPointArr[0]) / 2);
+
+        _currRectifiedPoints.swap(currRectifiedPoints);
+
+        static const float prevRectifiedPointArr[] = {
+            -0.599324584f, -0.381164283f, -0.387985110f, -0.385367423f, -0.371437579f, -0.371891201f,
+            -0.340867460f, -0.370632380f, -0.289822906f, -0.364118159f, -0.372411519f, -0.335272551f,
+            -0.289586753f, -0.335766882f, -0.372335523f, -0.316857219f, -0.321099430f, -0.323233813f,
+            0.208661616f, -0.153931335f, -0.559897065f, 0.193362445f, 0.0181128159f, -0.325224668f,
+            -0.427504510f, 0.105302416f, 0.487470537f, -0.187071189f, 0.343267351f, -0.339755565f,
+            -0.477639943f, -0.204375938f, -0.466626763f, -0.204072326f, 0.340813518f, -0.347292691f,
+            0.342682719f, -0.320172101f, 0.383663863f, -0.327343374f, -0.467062414f, -0.193995550f,
+            -0.475603998f, -0.189820126f, 0.552475691f, 0.198386014f, -0.508027375f, -0.174297482f,
+            -0.211989403f, -0.217261642f, 0.180832058f, -0.127527758f, -0.112721168f, -0.125876635f,
+            -0.112387165f, -0.167135969f, -0.562491000f, -0.140186235f, 0.395156831f, -0.298828602f,
+            -0.485202312f, -0.135626689f, 0.148358017f, -0.195937276f, -0.248159677f, -0.254669130f,
+            -0.568366945f, -0.105187029f, -0.0714842379f, -0.0832463056f, -0.497599572f, -0.205334768f,
+            -0.0948727652f, 0.245045587f, 0.160857186f, 0.138075173f, 0.164952606f, -0.195109487f,
+            0.165254518f, -0.186554477f, -0.183777973f, -0.124357253f, 0.166813776f, -0.153241888f,
+            -0.241765827f, -0.0820638761f, 0.208661616f, -0.153931335f, 0.540147483f, -0.203156039f,
+            0.529201686f, -0.199348077f, -0.248159677f, -0.254669130f, 0.180369601f, -0.139303327f,
+            0.570952237f, -0.185722873f, 0.221771300f, -0.143187970f, 0.498627752f, -0.183768719f,
+            0.561214447f, -0.188666284f, -0.241409421f, -0.253560483f, 0.569648385f, -0.184499770f,
+            0.276665628f, -0.0881819800f, 0.533934176f, -0.142226711f, -0.299728751f, -0.330407321f,
+            0.270322412f, -0.256552309f, -0.255016476f, -0.0823200271f, -0.378096581f, 0.0264666155f,
+            -0.331565350f, 0.0210608803f, 0.0100810500f, -0.0213523544f, -0.248159677f, -0.254669130f,
+            0.249623299f, 0.164078355f, 0.0190342199f, -0.00415771967f, 0.604407132f, -0.259350061f,
+            0.0660026148f, -0.00787150953f, 0.605921566f, 0.114344336f, 0.0208173525f, 0.00527517078f,
+            -0.0200567022f, 0.0183092188f, -0.184784368f, -0.193566754f, -0.0125719802f, -0.344967902f,
+            0.343063682f, -0.0121044181f, 0.389022052f, -0.0171062462f, 0.163190305f, 0.200014487f,
+            0.362440646f, 0.0120019922f, -0.427743971f, 0.100272447f, -0.0714842379f, -0.0832463056f,
+            0.0664352402f, 0.0467514023f, -0.559897065f, 0.193362445f, -0.549086213f, 0.193808615f,
+            -0.241472989f, -0.253163874f, -0.241765827f, -0.0820638761f, -0.122216024f, 0.132651567f,
+            -0.122216024f, 0.132651567f, 0.515065968f, 0.205271944f, 0.180832058f, -0.127527758f,
+            -0.123633556f, 0.154476687f, -0.248159677f, -0.254669130f, 0.0208173525f, 0.00527517078f,
+            -0.483276874f, 0.191274792f, -0.167928949f, 0.200682297f, 0.232745290f, -0.211950779f,
+            -0.288701504f, -0.334238827f, -0.119621970f, 0.204155236f, -0.119621970f, 0.204155236f,
+            0.632996142f, 0.0804972649f, 0.189231426f, 0.164325386f, 0.249623299f, 0.164078355f,
+            0.0676716864f, 0.0479496233f, 0.207636267f, 0.184271768f, -0.300510556f, 0.358790994f,
+            -0.107678331f, 0.188473806f, 0.565983415f, 0.144723341f, 0.191329703f, 0.213909492f,
+            -0.0283227600f, -0.373237878f, -0.184958130f, 0.200373843f, 0.0346363746f, -0.0259889495f,
+            -0.112387165f, -0.167135969f, 0.251426309f, 0.210430339f, -0.477397382f, -0.131372169f,
+            -0.0667442903f, 0.0997460634f, 0.251426309f, 0.210430339f, -0.317926824f, 0.375238001f,
+            -0.0621999837f, 0.280056626f, 0.0443522707f, 0.321513236f, 0.471269101f, 0.260774940f,
+            -0.107678331f, 0.188473806f, 0.0208210852f, 0.350526422f, 0.0157474391f, 0.367335707f,
+            0.632996142f, 0.0804972649f, 0.646697879f, 0.265504390f, 0.0295150280f, 0.371205181f,
+            0.376071006f, 0.313471258f, -0.379525930f, 0.364357829f, -0.00628023129f, -0.0373278372f,
+            0.0291138459f, 0.381194293f, 0.0358079821f, 0.381886899f, 0.0344478637f, 0.386993408f,
+            0.433862329f, 0.328515977f, 0.359724253f, 0.345606029f, 0.0651357397f, 0.397334814f,
+            0.388413996f, 0.344747871f, -0.140228778f, 0.216103494f, 0.389989913f, 0.372472703f,
+            0.444995403f, 0.300240308f, -0.606455386f, 0.100793049f, -0.362332910f, -0.371920794f,
+            -0.478956074f, 0.234040022f, -0.289441198f, -0.344822973f, -0.0714842379f, -0.0832463056f,
+            0.375879139f, -0.374975592f, 0.376526117f, -0.326493502f, 0.313251913f, -0.306372881f,
+            -0.0577337518f, 0.0893306211f, -0.483683407f, -0.179540694f, -0.0763650239f, -0.258294433f,
+            0.276665628f, -0.0881819800f, -0.167122558f, -0.175508693f, -0.164081737f, 0.176902041f,
+            0.276665628f, -0.0881819800f, 0.602967978f, -0.260941893f, 0.158573851f, -0.178748295f,
+            0.159815103f, -0.160761341f, 0.194283918f, -0.165657878f, 0.231515527f, -0.172808051f,
+            -0.247000366f, 0.277822912f, 0.538969517f, -0.204621449f, 0.531404376f, -0.198565826f,
+            -0.388338953f, -0.0433262810f, 0.499413073f, -0.181929186f, -0.237337112f, 0.0934364349f,
+            -0.368045300f, -0.0204487685f, -0.374767631f, -0.00678646797f, -0.0667242110f, -0.248651102f,
+            -0.248159677f, -0.254669130f, -0.345217139f, -0.00101677026f, -0.353382975f, 0.0210586078f,
+            -0.322639942f, 0.0211628731f, 0.0184581745f, -0.0366852731f, 0.0259528626f, -0.0136881955f,
+            -0.339446336f, 0.0286702402f, 0.0335014127f, -0.0271516014f, 0.465966076f, 0.0830826238f,
+            -0.337860256f, 0.0362124667f, 0.188271523f, -0.146541893f, -0.298272073f, -0.323130161f,
+            0.0643569306f, -0.0264105909f, -0.353804410f, 0.0433940105f, 0.618646920f, -0.0855877250f,
+            0.411329508f, -0.0414552018f, -0.427743971f, 0.100272447f, -0.247000366f, 0.277822912f,
+            0.381912649f, -0.00914942939f, 0.0664352402f, 0.0467514023f, 0.138687640f, -0.114854909f,
+            -0.0170480162f, -0.372787565f, -0.535477102f, 0.183755845f, -0.155668780f, 0.144164801f,
+            -0.427504510f, 0.105302416f, -0.484430760f, 0.227277100f, -0.361284673f, -0.373513311f,
+            -0.316764563f, 0.331503242f, -0.0230990555f, 0.314180285f, 0.101539977f, -0.256640851f,
+            -0.210743994f, -0.111771651f, -0.560086846f, 0.151153624f, 0.542884171f, 0.141691014f,
+            0.596041858f, 0.144990161f, 0.239398748f, 0.207432285f, 0.557545543f, 0.155783832f,
+            0.233033463f, 0.214694947f, 0.572789013f, 0.162068501f, 0.512761712f, 0.176260322f,
+            0.287076950f, 0.0868823677f, 0.515065968f, 0.205271944f, 0.552475691f, 0.198386014f,
+            -0.301232725f, 0.347804308f, -0.379525930f, 0.364357829f, 0.561403453f, 0.206571117f,
+            0.590792358f, 0.206283644f, -0.428855836f, 0.100270294f, 0.300039053f, -0.283949375f,
+            0.0481642894f, 0.334260821f, -0.173260480f, -0.167126089f, 0.444995403f, 0.300240308f,
+            0.646697879f, 0.265504390f, 0.375487208f, 0.314186513f, 0.0217850581f, 0.381838262f,
+            0.404422343f, 0.313856274f, 0.417644382f, 0.314869910f, 0.0358079821f, 0.381886899f,
+            0.378262609f, 0.358303785f, -0.336999178f, -0.367679387f, -0.295442462f, -0.365161836f,
+            -0.293496192f, -0.342732310f, -0.298767596f, -0.303165644f, -0.0111337993f, -0.342149645f,
+            0.310648471f, -0.374146342f, 0.359467417f, -0.373746723f, 0.340779394f, -0.369219989f,
+            -0.527450860f, -0.203896046f, -0.490746915f, -0.194764644f, 0.314866364f, -0.300261766f,
+            -0.0298556220f, 0.0591949411f, 0.319549739f, 0.0552458987f, 0.163977623f, -0.209844783f,
+            -0.149107113f, -0.149005055f, 0.212483421f, -0.191198543f, 0.197611198f, -0.187811792f,
+            0.174361721f, -0.179897651f, 0.0387913659f, -0.0366905928f, -0.122265801f, -0.126270071f,
+            0.211038783f, -0.172842503f, 0.246728286f, 0.134398326f, -0.0577337518f, 0.0893306211f,
+            -0.415295422f, 0.105914228f, -0.292730510f, 0.0379575789f, 0.489636958f, -0.194117576f,
+            -0.254337519f, 0.0937413648f, 0.336177140f, 0.305443168f, 0.526942134f, -0.164069965f,
+            0.524966419f, -0.165161178f, -0.379173398f, 0.332068861f, -0.340792000f, 0.00105464540f,
+            0.525632977f, -0.134992197f, -0.308774501f, 0.00290521770f, -0.375407755f, 0.0294080544f,
+            0.0178439785f, -0.0365749858f, -0.255016476f, -0.0823200271f, -0.359951973f, 0.0446678996f,
+            0.0564084686f, -0.0197724514f, -0.315141559f, 0.0424463004f, 0.292196661f, 0.279810339f,
+            -0.345294952f, 0.0533128195f, 0.0458479226f, -0.00109126628f, 0.0179449394f, 0.00371767790f,
+            0.365872562f, -0.0412087664f, 0.403013051f, -0.0416624695f, -0.0714842379f, -0.0832463056f,
+            -0.209011748f, 0.133690849f, 0.0122421598f, 0.0230175443f, -0.0577337518f, 0.0893306211f,
+            -0.572846889f, 0.141102776f, 0.345340014f, -0.0111671211f, 0.0479373708f, 0.0379454680f,
+            0.363291621f, -0.00829032529f, 0.381912649f, -0.00914942939f, -0.521542430f, 0.151489466f,
+            0.345966965f, 0.0110620018f, 0.354562849f, 0.0254590791f, 0.334322065f, 0.0310698878f,
+            -0.00463629747f, -0.0357710384f, -0.538667142f, 0.185365483f, -0.209011748f, 0.133690849f,
+            0.398122877f, 0.0403857268f, -0.160881191f, 0.145009249f, -0.155668780f, 0.144164801f,
+            -0.0714842379f, -0.0832463056f, -0.536377013f, 0.221241340f, -0.0632879063f, -0.247039422f,
+            -0.155869946f, 0.169341147f, 0.578685045f, -0.223878756f, 0.557447612f, 0.0768704116f,
+            -0.188812047f, 0.228197843f, 0.246747240f, 0.136472240f, -0.142677084f, 0.213736445f,
+            -0.118143238f, 0.208306640f, -0.388338953f, -0.0433262810f, -0.163515776f, 0.231573820f,
+            -0.0738375857f, -0.256104171f, 0.173092276f, 0.191535592f, 0.208548918f, 0.185476139f,
+            -0.392410189f, 0.0686017647f, 0.555366814f, 0.130478472f, -0.101943128f, -0.113997340f,
+            0.0716935173f, 0.340265751f, 0.561738014f, 0.148283109f, 0.242452115f, 0.205116034f,
+            0.561738014f, 0.148283109f, -0.427743971f, 0.100272447f, 0.578137994f, 0.163653031f,
+            0.251277626f, 0.223055005f, -0.376505047f, 0.343530416f, -0.0714842379f, -0.0832463056f,
+            0.567448437f, 0.207419440f, 0.590792358f, 0.206283644f, 0.578685045f, -0.223878756f,
+            0.0635343120f, -0.00499309227f, -0.370767444f, 0.384881169f, -0.485191971f, -0.120962359f,
+            0.512761712f, 0.176260322f, -0.375972956f, 0.0288736783f, -0.147176415f, -0.185790271f,
+            0.0752977654f, 0.339190871f, 0.646697879f, 0.265504390f, 0.0282997675f, 0.373214334f,
+            0.410353780f, 0.316089481f, 0.417644382f, 0.314869910f, 0.0147482762f, 0.389459789f,
+            -0.182916895f, -0.140514761f, 0.433515042f, 0.330774426f, 0.388069838f, 0.347381502f,
+            0.378925055f, 0.357438952f, 0.247128293f, -0.116897359f, -0.0230906308f, 0.314556211f,
+            0.388534039f, 0.370789021f, -0.368050814f, -0.339653373f, -0.292694926f, -0.341653705f,
+            -0.353774697f, -0.320387989f, 0.599263310f, -0.264537901f, -0.0213720929f, -0.326088905f,
+            -0.571947694f, 0.141147330f, -0.0577337518f, 0.0893306211f, 0.108424753f, -0.267108470f,
+            -0.0317604132f, -0.0458168685f, -0.0967136100f, 0.242639020f, -0.486509413f, -0.204596937f,
+            0.239178345f, -0.219647482f, 0.108424753f, -0.267108470f, -0.280393064f, -0.283867925f,
+            -0.533659995f, -0.151733354f, 0.0880429000f, -0.240412414f, -0.534965396f, -0.124174178f,
+            0.142445788f, -0.118948005f, 0.0947291106f, 0.0767719224f, -0.597055852f, -0.0692315027f,
+            -0.254337519f, 0.0937413648f, -0.308869720f, 0.00354974205f, -0.409894019f, -0.0694356859f,
+            0.556049764f, -0.137727231f, -0.0317604132f, -0.0458168685f, -0.524152219f, 0.239541322f,
+            0.108424753f, -0.267108470f, 0.0143662402f, -0.0164190196f, 0.150936082f, 0.128616557f,
+            0.618646920f, -0.0855877250f, 0.0122421598f, 0.0230175443f, 0.0122421598f, 0.0230175443f,
+            -0.188812047f, 0.228197843f, 0.00441747159f, -0.297387213f, -0.520719767f, 0.152393058f,
+            0.392849416f, 0.00738697406f, 0.400074363f, 0.0185570847f, -0.161484867f, -0.192373112f,
+            -0.554901838f, 0.190730989f, -0.538667142f, 0.185365483f, -0.0667442903f, 0.0997460634f,
+            0.399885803f, 0.0410231315f, -0.159816831f, 0.145826310f, -0.193316415f, 0.161277503f,
+            -0.0678345188f, 0.287081748f, -0.383089483f, -0.283330113f, -0.538667142f, 0.185365483f,
+            0.245664895f, 0.162005231f, 0.173092276f, 0.191535592f, 0.601281762f, 0.120500855f,
+            0.208548918f, 0.185476139f, 0.246893004f, 0.220670119f, 0.516039073f, 0.178782418f,
+            -0.254337519f, 0.0937413648f, -0.254337519f, 0.0937413648f, -0.0230990555f, 0.314180285f,
+            0.610029638f, 0.227215171f, -0.254337519f, 0.0937413648f, 0.0697976872f, 0.343245506f,
+            0.538969517f, -0.204621449f, 0.00916308723f, 0.359826297f, 0.410353780f, 0.316089481f,
+            0.423950195f, 0.324112266f, 0.166566655f, 0.145402640f, 0.354594171f, 0.350193948f,
+            0.433712035f, 0.356235564f, 0.425307065f, 0.364637494f, 0.166924104f, -0.152513608f,
+            0.594130874f, -0.268246830f, -0.0843627378f, -0.0962528363f, 0.108424753f, -0.267108470f,
+            0.00760878995f, -0.304247797f, -0.471018314f, -0.178305879f, -0.0817007348f, -0.0933016762f,
+            0.232274890f, 0.154553935f, 0.108424753f, -0.267108470f, -0.525787771f, -0.161353886f,
+            -0.206048280f, 0.241006181f, -0.178062543f, -0.184703678f, 0.105906568f, 0.268231422f,
+            -0.0817007348f, -0.0933016762f, 0.490914792f, 0.276718110f, -0.176861435f, -0.153617889f,
+            0.0387795344f, 0.0457828715f, 0.456206828f, -0.250739783f, 0.0982551053f, 0.104225174f,
+            0.142445788f, -0.118948005f, 0.108424753f, -0.267108470f, -0.0817007348f, -0.0933016762f,
+            -0.340707630f, 0.00498990202f, 0.0947291106f, 0.0767719224f, 0.169802040f, 0.203134149f,
+            0.577375948f, -0.125099033f, 0.318376005f, -0.0486588739f, 0.388697982f, -0.0351444185f,
+            0.406605273f, -0.0364143848f, 0.274859309f, 0.0776181892f, 0.349759877f, -7.70174083e-05f,
+            0.402967423f, 0.00697830878f, 0.105906568f, 0.268231422f, 0.338973522f, 0.0359939188f,
+            0.394951165f, 0.0322254188f, -0.503028810f, 0.203627899f, -0.0840740278f, -0.234684706f,
+            0.108424753f, -0.267108470f, 0.286642373f, 0.103878126f, -0.0817007348f, -0.0933016762f,
+            0.332983583f, -0.0356097035f, 0.628004134f, 0.0766527727f, -0.112659439f, -0.196833044f,
+            0.568797410f, 0.136423931f, 0.456206828f, -0.250739783f, -0.254337519f, 0.0937413648f,
+            -0.206692874f, -0.210832119f, 0.550912619f, 0.171586066f, 0.581267595f, 0.213235661f,
+            0.334484309f, 0.303876013f, -0.469516128f, 0.0883551016f, 0.133899942f, 0.106862970f,
+            -0.560961962f, -0.114681393f, -0.0840740278f, -0.234684706f, 0.459386230f, -0.236088052f,
+            0.594130874f, -0.268246830f, -0.124856450f, 0.193096936f, -0.469516128f, 0.0883551016f,
+            0.514290810f, -0.193822652f, 0.158255994f, 0.233290926f, 0.317973822f, -0.0477817170f,
+            -0.0817007348f, -0.0933016762f, -0.0702776462f, -0.0671426803f, 0.440836668f, -0.100193374f,
+            0.326240778f, 0.0523138903f, -0.279556662f, -0.283929169f, -0.485202312f, -0.135626689f,
+            -0.467358112f, 0.246376559f, 0.232274890f, 0.154553935f, 0.258349210f, -0.269529581f,
+            0.600620329f, 0.126268178f, -0.0985416993f, 0.245674044f, -0.279264033f, -0.0990248993f,
+            0.108424753f, -0.267108470f, 0.259638488f, -0.100053802f, 0.605106652f, 0.223564968f,
+            0.129683495f, -0.100376993f, -0.0953388065f, 0.112722203f, -0.440420747f, -0.0396305211f,
+            -0.0181254297f, 0.0439292751f, -0.0878356919f, 0.0847257674f, -0.271582603f, 0.126064256f,
+            -0.183777973f, -0.124357253f, 0.431088895f, 0.0680654719f, -0.469516128f, 0.0883551016f,
+            -0.445174575f, 0.133306518f, -0.0878356919f, 0.0847257674f, -0.279039949f, 0.0810008645f,
+            0.612402737f, -0.0826834291f, -0.454494953f, 0.122878648f, 0.244000912f, -0.264438629f,
+            0.142445788f, -0.118948005f, 0.129683495f, -0.100376993f, -0.210078895f, 0.131698489f,
+            -0.277847171f, 0.0665081516f, 0.431088895f, 0.0680654719f, 0.252345473f, 0.0688349009f,
+            0.133899942f, 0.106862970f, 0.133899942f, 0.106862970f, -0.486509413f, -0.204596937f,
+            -0.0940247625f, 0.0698821172f, 0.133899942f, 0.106862970f, -0.440420747f, -0.0396305211f,
+            -0.0878356919f, 0.0847257674f, -0.0954068601f, -0.0968973264f, -0.277847171f, 0.0665081516f,
+            -0.277847171f, 0.0665081516f, 0.266677618f, 0.111257851f, 0.292424291f, -0.230888903f,
+            -0.0954068601f, -0.0968973264f
+        };
+
+        const Point2f* prevRectifiedPointArr_2f = (const Point2f*)prevRectifiedPointArr;
+        vector<Point2f> prevRectifiedPoints(prevRectifiedPointArr_2f, prevRectifiedPointArr_2f +
+            sizeof(prevRectifiedPointArr) / sizeof(prevRectifiedPointArr[0]) / 2);
+
+        _prevRectifiedPoints.swap(prevRectifiedPoints);
+
+        int validSolutionArr[2] = { 0, 2 };
+
+        vector<int> validSolutions(validSolutionArr, validSolutionArr +
+            sizeof(validSolutionArr) / sizeof(validSolutionArr[0]));
+
+        _validSolutions.swap(validSolutions);
+
+        vector<Mat> rotations;
+        vector<Mat> normals;
+
+        for (size_t i = 0; i < (sizeof(rotationsArray) / sizeof(*rotationsArray)); i++) {
+            Mat tempRotMat = Mat(Matx33d(
+                rotationsArray[i][0],
+                rotationsArray[i][1],
+                rotationsArray[i][2],
+                rotationsArray[i][3],
+                rotationsArray[i][4],
+                rotationsArray[i][5],
+                rotationsArray[i][6],
+                rotationsArray[i][7],
+                rotationsArray[i][8]
+            ));
+
+            Mat tempNormMat = Mat(Matx31d(
+                normalsArray[i][0],
+                normalsArray[i][1],
+                normalsArray[i][2]
+            ));
+
+            rotations.push_back(tempRotMat);
+            normals.push_back(tempNormMat);
+        }
+
+        _rotations.swap(rotations);
+        _normals.swap(normals);
+
+        _mask = Mat(514, 1, CV_8U, maskArray).clone();
+    }
+
+    bool isValidResult(const vector<int>& solutions)
+    {
+        return (solutions == _validSolutions);
+    }
+
+    vector<int> _validSolutions;
+    vector<Point2f> _prevRectifiedPoints, _currRectifiedPoints;
+    Mat _mask;
+    vector<Mat> _rotations, _normals;
+};
+
+TEST(Calib3d_FilterDecomposeHomography, regression) { CV_FilterHomographyDecompTest test; test.safe_run(); }
+
+}}
@@ -41,8 +41,11 @@
 //M*/

 #include "test_precomp.hpp"
-#include <opencv2/ts/cuda_test.hpp>
+#include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
 #include "../src/fisheye.hpp"
+#include "opencv2/videoio.hpp"
+
+namespace opencv_test { namespace {

 class fisheyeTest : public ::testing::Test {

@@ -60,7 +63,7 @@ protected:

 protected:
    std::string combine(const std::string& _item1, const std::string& _item2);
-    cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r);
+    static void merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged);
 };

 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -100,15 +103,15 @@ TEST_F(fisheyeTest, projectPoints)

 TEST_F(fisheyeTest, DISABLED_undistortImage)
 {
-    cv::Matx33d K = this->K;
-    cv::Mat D = cv::Mat(this->D);
+    cv::Matx33d theK = this->K;
+    cv::Mat theD = cv::Mat(this->D);
    std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg");
-    cv::Matx33d newK = K;
+    cv::Matx33d newK = theK;
    cv::Mat distorted = cv::imread(file), undistorted;
    {
        newK(0, 0) = 100;
        newK(1, 1) = 100;
-        cv::fisheye::undistortImage(distorted, undistorted, K, D, newK);
+        cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
        cv::Mat correct = cv::imread(combine(datasets_repository_path, "new_f_100.png"));
        if (correct.empty())
            CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_f_100.png"), undistorted));
@@ -117,8 +120,8 @@ TEST_F(fisheyeTest, DISABLED_undistortImage)
    }
    {
        double balance = 1.0;
-        cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance);
-        cv::fisheye::undistortImage(distorted, undistorted, K, D, newK);
+        cv::fisheye::estimateNewCameraMatrixForUndistortRectify(theK, theD, distorted.size(), cv::noArray(), newK, balance);
+        cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
        cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_1.0.png"));
        if (correct.empty())
            CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_1.0.png"), undistorted));
@@ -128,8 +131,8 @@ TEST_F(fisheyeTest, DISABLED_undistortImage)

    {
        double balance = 0.0;
-        cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance);
-        cv::fisheye::undistortImage(distorted, undistorted, K, D, newK);
+        cv::fisheye::estimateNewCameraMatrixForUndistortRectify(theK, theD, distorted.size(), cv::noArray(), newK, balance);
+        cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
        cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_0.0.png"));
        if (correct.empty())
            CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_0.0.png"), undistorted));
@@ -142,7 +145,7 @@ TEST_F(fisheyeTest, jacobians)
 {
    int n = 10;
    cv::Mat X(1, n, CV_64FC3);
-    cv::Mat om(3, 1, CV_64F), T(3, 1, CV_64F);
+    cv::Mat om(3, 1, CV_64F), theT(3, 1, CV_64F);
    cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F);
    cv::Mat k(4, 1, CV_64F);
    double alpha;
@@ -155,8 +158,8 @@ TEST_F(fisheyeTest, jacobians)
    r.fill(om, cv::RNG::NORMAL, 0, 1);
    om = cv::abs(om);

-    r.fill(T, cv::RNG::NORMAL, 0, 1);
-    T = cv::abs(T); T.at<double>(2) = 4; T *= 10;
+    r.fill(theT, cv::RNG::NORMAL, 0, 1);
+    theT = cv::abs(theT); theT.at<double>(2) = 4; theT *= 10;

    r.fill(f, cv::RNG::NORMAL, 0, 1);
    f = cv::abs(f) * 1000;
@@ -170,19 +173,19 @@ TEST_F(fisheyeTest, jacobians)
    alpha = 0.01*r.gaussian(1);

    cv::Mat x1, x2, xpred;
-    cv::Matx33d K(f.at<double>(0), alpha * f.at<double>(0), c.at<double>(0),
+    cv::Matx33d theK(f.at<double>(0), alpha * f.at<double>(0), c.at<double>(0),
                     0,            f.at<double>(1), c.at<double>(1),
                     0,            0,    1);

    cv::Mat jacobians;
-    cv::fisheye::projectPoints(X, x1, om, T, K, k, alpha, jacobians);
+    cv::fisheye::projectPoints(X, x1, om, theT, theK, k, alpha, jacobians);

    //test on T:
    cv::Mat dT(3, 1, CV_64FC1);
    r.fill(dT, cv::RNG::NORMAL, 0, 1);
-    dT *= 1e-9*cv::norm(T);
-    cv::Mat T2 = T + dT;
-    cv::fisheye::projectPoints(X, x2, om, T2, K, k, alpha, cv::noArray());
+    dT *= 1e-9*cv::norm(theT);
+    cv::Mat T2 = theT + dT;
+    cv::fisheye::projectPoints(X, x2, om, T2, theK, k, alpha, cv::noArray());
    xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1);
    CV_Assert (cv::norm(x2 - xpred) < 1e-10);

@@ -191,7 +194,7 @@ TEST_F(fisheyeTest, jacobians)
    r.fill(dom, cv::RNG::NORMAL, 0, 1);
    dom *= 1e-9*cv::norm(om);
    cv::Mat om2 = om + dom;
-    cv::fisheye::projectPoints(X, x2, om2, T, K, k, alpha, cv::noArray());
+    cv::fisheye::projectPoints(X, x2, om2, theT, theK, k, alpha, cv::noArray());
    xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1);
    CV_Assert (cv::norm(x2 - xpred) < 1e-10);

@@ -199,8 +202,8 @@ TEST_F(fisheyeTest, jacobians)
    cv::Mat df(2, 1, CV_64FC1);
    r.fill(df, cv::RNG::NORMAL, 0, 1);
    df *= 1e-9*cv::norm(f);
-    cv::Matx33d K2 = K + cv::Matx33d(df.at<double>(0), df.at<double>(0) * alpha, 0, 0, df.at<double>(1), 0, 0, 0, 0);
-    cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
+    cv::Matx33d K2 = theK + cv::Matx33d(df.at<double>(0), df.at<double>(0) * alpha, 0, 0, df.at<double>(1), 0, 0, 0, 0);
+    cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
    xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1);
    CV_Assert (cv::norm(x2 - xpred) < 1e-10);

@@ -208,8 +211,8 @@ TEST_F(fisheyeTest, jacobians)
    cv::Mat dc(2, 1, CV_64FC1);
    r.fill(dc, cv::RNG::NORMAL, 0, 1);
    dc *= 1e-9*cv::norm(c);
-    K2 = K + cv::Matx33d(0, 0, dc.at<double>(0), 0, 0, dc.at<double>(1), 0, 0, 0);
-    cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
+    K2 = theK + cv::Matx33d(0, 0, dc.at<double>(0), 0, 0, dc.at<double>(1), 0, 0, 0);
+    cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
    xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1);
    CV_Assert (cv::norm(x2 - xpred) < 1e-10);

@@ -218,7 +221,7 @@ TEST_F(fisheyeTest, jacobians)
    r.fill(dk, cv::RNG::NORMAL, 0, 1);
    dk *= 1e-9*cv::norm(k);
    cv::Mat k2 = k + dk;
-    cv::fisheye::projectPoints(X, x2, om, T, K, k2, alpha, cv::noArray());
+    cv::fisheye::projectPoints(X, x2, om, theT, theK, k2, alpha, cv::noArray());
    xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1);
    CV_Assert (cv::norm(x2 - xpred) < 1e-10);

@@ -227,8 +230,8 @@ TEST_F(fisheyeTest, jacobians)
    r.fill(dalpha, cv::RNG::NORMAL, 0, 1);
    dalpha *= 1e-9*cv::norm(f);
    double alpha2 = alpha + dalpha.at<double>(0);
-    K2 = K + cv::Matx33d(0, f.at<double>(0) * dalpha.at<double>(0), 0, 0, 0, 0, 0, 0, 0);
-    cv::fisheye::projectPoints(X, x2, om, T, K, k, alpha2, cv::noArray());
+    K2 = theK + cv::Matx33d(0, f.at<double>(0) * dalpha.at<double>(0), 0, 0, 0, 0, 0, 0, 0);
+    cv::fisheye::projectPoints(X, x2, om, theT, theK, k, alpha2, cv::noArray());
    xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1);
    CV_Assert (cv::norm(x2 - xpred) < 1e-10);
 }
@@ -244,13 +247,13 @@ TEST_F(fisheyeTest, Calibration)
    cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
    CV_Assert(fs_left.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_left[cv::format("image_%d", i )] >> imagePoints[i];
+        fs_left[cv::format("image_%d", i )] >> imagePoints[i];
    fs_left.release();

    cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
    CV_Assert(fs_object.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_object[cv::format("image_%d", i )] >> objectPoints[i];
+        fs_object[cv::format("image_%d", i )] >> objectPoints[i];
    fs_object.release();

    int flag = 0;
@@ -258,14 +261,14 @@ TEST_F(fisheyeTest, Calibration)
    flag |= cv::fisheye::CALIB_CHECK_COND;
    flag |= cv::fisheye::CALIB_FIX_SKEW;

-    cv::Matx33d K;
-    cv::Vec4d D;
+    cv::Matx33d theK;
+    cv::Vec4d theD;

-    cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
+    cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
                           cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));

-    EXPECT_MAT_NEAR(K, this->K, 1e-10);
-    EXPECT_MAT_NEAR(D, this->D, 1e-10);
+    EXPECT_MAT_NEAR(theK, this->K, 1e-10);
+    EXPECT_MAT_NEAR(theD, this->D, 1e-10);
 }

 TEST_F(fisheyeTest, Homography)
@@ -279,13 +282,13 @@ TEST_F(fisheyeTest, Homography)
    cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
    CV_Assert(fs_left.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_left[cv::format("image_%d", i )] >> imagePoints[i];
+        fs_left[cv::format("image_%d", i )] >> imagePoints[i];
    fs_left.release();

    cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
    CV_Assert(fs_object.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_object[cv::format("image_%d", i )] >> objectPoints[i];
+        fs_object[cv::format("image_%d", i )] >> objectPoints[i];
    fs_object.release();

    cv::internal::IntrinsicParams param;
@@ -302,15 +305,15 @@ TEST_F(fisheyeTest, Homography)
    int Np = imagePointsNormalized.cols;
    cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, cv::COVAR_NORMAL | cv::COVAR_COLS);
    cv::SVD svd(covObjectPoints);
-    cv::Mat R(svd.vt);
+    cv::Mat theR(svd.vt);

-    if (cv::norm(R(cv::Rect(2, 0, 1, 2))) < 1e-6)
-        R = cv::Mat::eye(3,3, CV_64FC1);
-    if (cv::determinant(R) < 0)
-        R = -R;
+    if (cv::norm(theR(cv::Rect(2, 0, 1, 2))) < 1e-6)
+        theR = cv::Mat::eye(3,3, CV_64FC1);
+    if (cv::determinant(theR) < 0)
+        theR = -theR;

-    cv::Mat T = -R * objectPointsMean;
-    cv::Mat X_new = R * _objectPoints + T * cv::Mat::ones(1, Np, CV_64FC1);
+    cv::Mat theT = -theR * objectPointsMean;
+    cv::Mat X_new = theR * _objectPoints + theT * cv::Mat::ones(1, Np, CV_64FC1);
    cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2));

    cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1);
@@ -329,7 +332,7 @@ TEST_F(fisheyeTest, Homography)
    EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12);
 }

-TEST_F(fisheyeTest, EtimateUncertainties)
+TEST_F(fisheyeTest, EstimateUncertainties)
 {
    const int n_images = 34;

@@ -340,13 +343,13 @@ TEST_F(fisheyeTest, EtimateUncertainties)
    cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
    CV_Assert(fs_left.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_left[cv::format("image_%d", i )] >> imagePoints[i];
+        fs_left[cv::format("image_%d", i )] >> imagePoints[i];
    fs_left.release();

    cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
    CV_Assert(fs_object.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_object[cv::format("image_%d", i )] >> objectPoints[i];
+        fs_object[cv::format("image_%d", i )] >> objectPoints[i];
    fs_object.release();

    int flag = 0;
@@ -354,20 +357,20 @@ TEST_F(fisheyeTest, EtimateUncertainties)
    flag |= cv::fisheye::CALIB_CHECK_COND;
    flag |= cv::fisheye::CALIB_FIX_SKEW;

-    cv::Matx33d K;
-    cv::Vec4d D;
+    cv::Matx33d theK;
+    cv::Vec4d theD;
    std::vector<cv::Vec3d> rvec;
    std::vector<cv::Vec3d> tvec;

-    cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
+    cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
                           rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6));

    cv::internal::IntrinsicParams param, errors;
    cv::Vec2d err_std;
    double thresh_cond = 1e6;
    int check_cond = 1;
-    param.Init(cv::Vec2d(K(0,0), K(1,1)), cv::Vec2d(K(0,2), K(1, 2)), D);
-    param.isEstimate = std::vector<int>(9, 1);
+    param.Init(cv::Vec2d(theK(0,0), theK(1,1)), cv::Vec2d(theK(0,2), theK(1, 2)), theD);
+    param.isEstimate = std::vector<uchar>(9, 1);
    param.isEstimate[4] = 0;

    errors.isEstimate = param.isEstimate;
@@ -381,11 +384,11 @@ TEST_F(fisheyeTest, EtimateUncertainties)
    EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10);
    EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10);
    EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10);
-    CV_Assert(abs(rms - 0.263782587133546) < 1e-10);
+    CV_Assert(fabs(rms - 0.263782587133546) < 1e-10);
    CV_Assert(errors.alpha == 0);
 }

-TEST_F(fisheyeTest, rectify)
+TEST_F(fisheyeTest, stereoRectify)
 {
    const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");

@@ -393,28 +396,73 @@ TEST_F(fisheyeTest, rectify)
    cv::Matx33d K1 = this->K, K2 = K1;
    cv::Mat D1 = cv::Mat(this->D), D2 = D1;

-    cv::Vec3d T = this->T;
-    cv::Matx33d R = this->R;
+    cv::Vec3d theT = this->T;
+    cv::Matx33d theR = this->R;

    double balance = 0.0, fov_scale = 1.1;
    cv::Mat R1, R2, P1, P2, Q;
-    cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, R, T, R1, R2, P1, P2, Q,
+    cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, theR, theT, R1, R2, P1, P2, Q,
                      cv::CALIB_ZERO_DISPARITY, requested_size, balance, fov_scale);

+    // Collected with these CMake flags: -DWITH_IPP=OFF -DCV_ENABLE_INTRINSICS=OFF -DCV_DISABLE_OPTIMIZATION=ON -DCMAKE_BUILD_TYPE=Debug
+    cv::Matx33d R1_ref(
+        0.9992853269091279, 0.03779164101000276, -0.0007920188690205426,
+        -0.03778569762983931, 0.9992646472015868, 0.006511981857667881,
+        0.001037534936357442, -0.006477400933964018, 0.9999784831677112
+    );
+    cv::Matx33d R2_ref(
+        0.9994868963898833, -0.03197579751378937, -0.001868774538573449,
+        0.03196298186616116, 0.9994677442608699, -0.0065265589947392,
+        0.002076471801477729, 0.006463478587068991, 0.9999769555891836
+    );
+    cv::Matx34d P1_ref(
+        420.8551870450913, 0, 586.501617798451, 0,
+        0, 420.8551870450913, 374.7667511986098, 0,
+        0, 0, 1, 0
+    );
+    cv::Matx34d P2_ref(
+        420.8551870450913, 0, 586.501617798451, -41.77758076597302,
+        0, 420.8551870450913, 374.7667511986098, 0,
+        0, 0, 1, 0
+    );
+    cv::Matx44d Q_ref(
+        1, 0, 0, -586.501617798451,
+        0, 1, 0, -374.7667511986098,
+        0, 0, 0, 420.8551870450913,
+        0, 0, 10.07370889670733, -0
+    );
+
+    const double eps = 1e-10;
+    EXPECT_MAT_NEAR(R1_ref, R1, eps);
+    EXPECT_MAT_NEAR(R2_ref, R2, eps);
+    EXPECT_MAT_NEAR(P1_ref, P1, eps);
+    EXPECT_MAT_NEAR(P2_ref, P2, eps);
+    EXPECT_MAT_NEAR(Q_ref, Q, eps);
+
+    if (::testing::Test::HasFailure())
+    {
+        std::cout << "Actual values are:" << std::endl
+            << "R1 =" << std::endl << R1 << std::endl
+            << "R2 =" << std::endl << R2 << std::endl
+            << "P1 =" << std::endl << P1 << std::endl
+            << "P2 =" << std::endl << P2 << std::endl
+            << "Q =" << std::endl << Q << std::endl;
+    }
+
+#if 1 // Debug code
    cv::Mat lmapx, lmapy, rmapx, rmapy;
    //rewrite for fisheye
    cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy);
    cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy);

    cv::Mat l, r, lundist, rundist;
-    cv::VideoCapture lcap(combine(folder, "left/stereo_pair_%03d.jpg")),
-                     rcap(combine(folder, "right/stereo_pair_%03d.jpg"));
-
-    for(int i = 0;; ++i)
+    for (int i = 0; i < 34; ++i)
    {
-        lcap >> l; rcap >> r;
-        if (l.empty() || r.empty())
-            break;
+        SCOPED_TRACE(cv::format("image %d", i));
+        l = imread(combine(folder, cv::format("left/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
+        r = imread(combine(folder, cv::format("right/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
+        ASSERT_FALSE(l.empty());
+        ASSERT_FALSE(r.empty());

        int ndisp = 128;
        cv::rectangle(l, cv::Rect(255,       0, 829,       l.rows-1), cv::Scalar(0, 0, 255));
@@ -423,15 +471,18 @@ TEST_F(fisheyeTest, rectify)
        cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR);
        cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR);

-        cv::Mat rectification = mergeRectification(lundist, rundist);
+        for (int ii = 0; ii < lundist.rows; ii += 20)
+        {
+            cv::line(lundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
+            cv::line(rundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
+        }

-        cv::Mat correct = cv::imread(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i)));
+        cv::Mat rectification;
+        merge4(l, r, lundist, rundist, rectification);

-        if (correct.empty())
-            cv::imwrite(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i)), rectification);
-         else
-             EXPECT_MAT_NEAR(correct, rectification, 1e-10);
-     }
+        cv::imwrite(cv::format("fisheye_rectification_AB_%03d.png", i), rectification);
+    }
+#endif
 }

 TEST_F(fisheyeTest, stereoCalibrate)
@@ -447,33 +498,32 @@ TEST_F(fisheyeTest, stereoCalibrate)
    cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
    CV_Assert(fs_left.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_left[cv::format("image_%d", i )] >> leftPoints[i];
+        fs_left[cv::format("image_%d", i )] >> leftPoints[i];
    fs_left.release();

    cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
    CV_Assert(fs_right.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_right[cv::format("image_%d", i )] >> rightPoints[i];
+        fs_right[cv::format("image_%d", i )] >> rightPoints[i];
    fs_right.release();

    cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
    CV_Assert(fs_object.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_object[cv::format("image_%d", i )] >> objectPoints[i];
+        fs_object[cv::format("image_%d", i )] >> objectPoints[i];
    fs_object.release();

-    cv::Matx33d K1, K2, R;
-    cv::Vec3d T;
+    cv::Matx33d K1, K2, theR;
+    cv::Vec3d theT;
    cv::Vec4d D1, D2;

    int flag = 0;
    flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
    flag |= cv::fisheye::CALIB_CHECK_COND;
    flag |= cv::fisheye::CALIB_FIX_SKEW;
-   // flag |= cv::fisheye::CALIB_FIX_INTRINSIC;

    cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
-                    K1, D1, K2, D2, imageSize, R, T, flag,
+                    K1, D1, K2, D2, imageSize, theR, theT, flag,
                    cv::TermCriteria(3, 12, 0));

    cv::Matx33d R_correct(   0.9975587205950972,   0.06953016383322372, 0.006492709911733523,
@@ -491,8 +541,8 @@ TEST_F(fisheyeTest, stereoCalibrate)
    cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
    cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);

-    EXPECT_MAT_NEAR(R, R_correct, 1e-10);
-    EXPECT_MAT_NEAR(T, T_correct, 1e-10);
+    EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
+    EXPECT_MAT_NEAR(theT, T_correct, 1e-10);

    EXPECT_MAT_NEAR(K1, K1_correct, 1e-10);
    EXPECT_MAT_NEAR(K2, K2_correct, 1e-10);
@@ -515,23 +565,23 @@ TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
    cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
    CV_Assert(fs_left.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_left[cv::format("image_%d", i )] >> leftPoints[i];
+        fs_left[cv::format("image_%d", i )] >> leftPoints[i];
    fs_left.release();

    cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
    CV_Assert(fs_right.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_right[cv::format("image_%d", i )] >> rightPoints[i];
+        fs_right[cv::format("image_%d", i )] >> rightPoints[i];
    fs_right.release();

    cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
    CV_Assert(fs_object.isOpened());
    for(int i = 0; i < n_images; ++i)
-    fs_object[cv::format("image_%d", i )] >> objectPoints[i];
+        fs_object[cv::format("image_%d", i )] >> objectPoints[i];
    fs_object.release();

-    cv::Matx33d R;
-    cv::Vec3d T;
+    cv::Matx33d theR;
+    cv::Vec3d theT;

    int flag = 0;
    flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
@@ -551,7 +601,7 @@ TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
    cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);

    cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
-                    K1, D1, K2, D2, imageSize, R, T, flag,
+                    K1, D1, K2, D2, imageSize, theR, theT, flag,
                    cv::TermCriteria(3, 12, 0));

    cv::Matx33d R_correct(   0.9975587205950972,   0.06953016383322372, 0.006492709911733523,
@@ -560,8 +610,50 @@ TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
    cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);


-    EXPECT_MAT_NEAR(R, R_correct, 1e-10);
-    EXPECT_MAT_NEAR(T, T_correct, 1e-10);
+    EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
+    EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
+}
+
+TEST_F(fisheyeTest, CalibrationWithDifferentPointsNumber)
+{
+    const int n_images = 2;
+
+    std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
+    std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
+
+    std::vector<cv::Point2d> imgPoints1(10);
+    std::vector<cv::Point2d> imgPoints2(15);
+
+    std::vector<cv::Point3d> objectPoints1(imgPoints1.size());
+    std::vector<cv::Point3d> objectPoints2(imgPoints2.size());
+
+    for (size_t i = 0; i < imgPoints1.size(); i++)
+    {
+        imgPoints1[i] = cv::Point2d((double)i, (double)i);
+        objectPoints1[i] = cv::Point3d((double)i, (double)i, 10.0);
+    }
+
+    for (size_t i = 0; i < imgPoints2.size(); i++)
+    {
+        imgPoints2[i] = cv::Point2d(i + 0.5, i + 0.5);
+        objectPoints2[i] = cv::Point3d(i + 0.5, i + 0.5, 10.0);
+    }
+
+    imagePoints[0] = imgPoints1;
+    imagePoints[1] = imgPoints2;
+    objectPoints[0] = objectPoints1;
+    objectPoints[1] = objectPoints2;
+
+    cv::Matx33d theK = cv::Matx33d::eye();
+    cv::Vec4d theD;
+
+    int flag = 0;
+    flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
+    flag |= cv::fisheye::CALIB_USE_INTRINSIC_GUESS;
+    flag |= cv::fisheye::CALIB_FIX_SKEW;
+
+    cv::fisheye::calibrate(objectPoints, imagePoints, cv::Size(100, 100), theK, theD,
+        cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
 }

 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -597,17 +689,19 @@ std::string fisheyeTest::combine(const std::string& _item1, const std::string& _
    return item1 + (last != '/' ? "/" : "") + item2;
 }

-cv::Mat fisheyeTest::mergeRectification(const cv::Mat& l, const cv::Mat& r)
+void fisheyeTest::merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged)
 {
-    CV_Assert(l.type() == r.type() && l.size() == r.size());
-    cv::Mat merged(l.rows, l.cols * 2, l.type());
-    cv::Mat lpart = merged.colRange(0, l.cols);
-    cv::Mat rpart = merged.colRange(l.cols, merged.cols);
-    l.copyTo(lpart);
-    r.copyTo(rpart);
+    int type = tl.type();
+    cv::Size sz = tl.size();
+    ASSERT_EQ(type, tr.type()); ASSERT_EQ(type, bl.type()); ASSERT_EQ(type, br.type());
+    ASSERT_EQ(sz.width, tr.cols); ASSERT_EQ(sz.width, bl.cols); ASSERT_EQ(sz.width, br.cols);
+    ASSERT_EQ(sz.height, tr.rows); ASSERT_EQ(sz.height, bl.rows); ASSERT_EQ(sz.height, br.rows);

-    for(int i = 0; i < l.rows; i+=20)
-        cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), cv::Scalar(0, 255, 0));
-
-    return merged;
+    merged.create(cv::Size(sz.width * 2, sz.height * 2), type);
+    tl.copyTo(merged(cv::Rect(0, 0, sz.width, sz.height)));
+    tr.copyTo(merged(cv::Rect(sz.width, 0, sz.width, sz.height)));
+    bl.copyTo(merged(cv::Rect(0, sz.height, sz.width, sz.height)));
+    br.copyTo(merged(cv::Rect(sz.width, sz.height, sz.width, sz.height)));
 }
+
+}} // namespace
@@ -42,10 +42,9 @@
 #include "test_precomp.hpp"
 #include "opencv2/calib3d/calib3d_c.h"

-using namespace cv;
-using namespace std;
+namespace cvtest {

-int cvTsRodrigues( const CvMat* src, CvMat* dst, CvMat* jacobian )
+static int cvTsRodrigues( const CvMat* src, CvMat* dst, CvMat* jacobian )
 {
    int depth;
    int i;
@@ -348,14 +347,18 @@ int cvTsRodrigues( const CvMat* src, CvMat* dst, CvMat* jacobian )
 }


-void cvtest::Rodrigues(const Mat& src, Mat& dst, Mat* jac)
+/*extern*/ void Rodrigues(const Mat& src, Mat& dst, Mat* jac)
 {
-    CvMat _src = src, _dst = dst, _jac;
+    CV_Assert(src.data != dst.data && "Inplace is not supported");
+    CV_Assert(!dst.empty() && "'dst' must be allocated");
+    CvMat _src = cvMat(src), _dst = cvMat(dst), _jac;
    if( jac )
-        _jac = *jac;
+        _jac = cvMat(*jac);
    cvTsRodrigues(&_src, &_dst, jac ? &_jac : 0);
 }

+} // namespace
+namespace opencv_test {

 static void test_convertHomogeneous( const Mat& _src, Mat& _dst )
 {
@@ -463,6 +466,7 @@ static void test_convertHomogeneous( const Mat& _src, Mat& _dst )
        dst.convertTo(_dst, _dst.depth());
 }

+namespace {

 void
 test_projectPoints( const Mat& _3d, const Mat& Rt, const Mat& A, Mat& _2d, RNG* rng, double sigma )
@@ -663,13 +667,13 @@ void CV_RodriguesTest::run_func()

    if( calc_jacobians )
    {
-        v2m_jac = test_mat[OUTPUT][1];
-        m2v_jac = test_mat[OUTPUT][3];
+        v2m_jac = cvMat(test_mat[OUTPUT][1]);
+        m2v_jac = cvMat(test_mat[OUTPUT][3]);
    }

    if( !test_cpp )
    {
-        CvMat _input = test_mat[INPUT][0], _output = test_mat[OUTPUT][0], _output2 = test_mat[OUTPUT][2];
+        CvMat _input = cvMat(test_mat[INPUT][0]), _output = cvMat(test_mat[OUTPUT][0]), _output2 = cvMat(test_mat[OUTPUT][2]);
        cvRodrigues2( &_input, &_output, calc_jacobians ? &v2m_jac : 0 );
        cvRodrigues2( &_output, &_output2, calc_jacobians ? &m2v_jac : 0 );
    }
@@ -728,7 +732,7 @@ void CV_RodriguesTest::prepare_to_validation( int /*test_case_idx*/ )
    cvtest::Rodrigues( m, vec2, m2v_jac );
    cvtest::copy( vec, vec2 );

-    theta0 = norm( vec2, CV_L2 );
+    theta0 = cvtest::norm( vec2, CV_L2 );
    theta1 = fmod( theta0, CV_PI*2 );

    if( theta1 > CV_PI )
@@ -973,26 +977,12 @@ int CV_FundamentalMatTest::prepare_test_case( int test_case_idx )
    return code;
 }

-
 void CV_FundamentalMatTest::run_func()
 {
-    //if(!test_cpp)
-    {
-        CvMat _input0 = test_mat[INPUT][0], _input1 = test_mat[INPUT][1];
-        CvMat F = test_mat[TEMP][0], mask = test_mat[TEMP][1];
-        f_result = cvFindFundamentalMat( &_input0, &_input1, &F, method, MAX(sigma*3, 0.01), 0, &mask );
-    }
-    /*else
-    {
-        cv::findFundamentalMat(const Mat& points1, const Mat& points2,
-        vector<uchar>& mask, int method=FM_RANSAC,
-        double param1=3., double param2=0.99 );
-
-        CV_EXPORTS Mat findFundamentalMat( const Mat& points1, const Mat& points2,
-                                          int method=FM_RANSAC,
-                                          double param1=3., double param2=0.99 );
-    }*/
-
+    // cvFindFundamentalMat calls cv::findFundamentalMat
+    CvMat _input0 = cvMat(test_mat[INPUT][0]), _input1 = cvMat(test_mat[INPUT][1]);
+    CvMat F = cvMat(test_mat[TEMP][0]), mask = cvMat(test_mat[TEMP][1]);
+    f_result = cvFindFundamentalMat( &_input0, &_input1, &F, method, MAX(sigma*3, 0.01), 0, &mask );
 }


@@ -1021,12 +1011,12 @@ void CV_FundamentalMatTest::prepare_to_validation( int test_case_idx )
    cv::gemm( T, invA2, 1, Mat(), 0, F0 );
    F0 *= 1./f0[8];

-    uchar* status = test_mat[TEMP][1].data;
-    double err_level = method <= CV_FM_8POINT ? 1 : get_success_error_level( test_case_idx, OUTPUT, 1 );
-    uchar* mtfm1 = test_mat[REF_OUTPUT][1].data;
-    uchar* mtfm2 = test_mat[OUTPUT][1].data;
-    double* f_prop1 = (double*)test_mat[REF_OUTPUT][0].data;
-    double* f_prop2 = (double*)test_mat[OUTPUT][0].data;
+    uchar* status = test_mat[TEMP][1].ptr();
+    double err_level = get_success_error_level( test_case_idx, OUTPUT, 1 );
+    uchar* mtfm1 = test_mat[REF_OUTPUT][1].ptr();
+    uchar* mtfm2 = test_mat[OUTPUT][1].ptr();
+    double* f_prop1 = test_mat[REF_OUTPUT][0].ptr<double>();
+    double* f_prop2 = test_mat[OUTPUT][0].ptr<double>();

    int i, pt_count = test_mat[INPUT][2].cols;
    Mat p1( 1, pt_count, CV_64FC2 );
@@ -1081,7 +1071,9 @@ protected:
    void run_func();
    void prepare_to_validation( int );

+#if 0
    double sampson_error(const double* f, double x1, double y1, double x2, double y2);
+#endif

    int method;
    int img_size;
@@ -1313,6 +1305,7 @@ void CV_EssentialMatTest::run_func()
    mask2.copyTo(test_mat[TEMP][4]);
 }

+#if 0
 double CV_EssentialMatTest::sampson_error(const double * f, double x1, double y1, double x2, double y2)
 {
    double Fx1[3] = {
@@ -1330,8 +1323,8 @@ double CV_EssentialMatTest::sampson_error(const double * f, double x1, double y1
    double error = x2tFx1 * x2tFx1 / (Fx1[0] * Fx1[0] + Fx1[1] * Fx1[1] + Ftx2[0] * Ftx2[0] + Ftx2[1] * Ftx2[1]);
    error = sqrt(error);
    return error;
-
 }
+#endif

 void CV_EssentialMatTest::prepare_to_validation( int test_case_idx )
 {
@@ -1357,12 +1350,12 @@ void CV_EssentialMatTest::prepare_to_validation( int test_case_idx )
    cv::gemm( T1, T2, 1, Mat(), 0, F0 );
    F0 *= 1./f0[8];

-    uchar* status = test_mat[TEMP][1].data;
+    uchar* status = test_mat[TEMP][1].ptr();
    double err_level = get_success_error_level( test_case_idx, OUTPUT, 1 );
-    uchar* mtfm1 = test_mat[REF_OUTPUT][1].data;
-    uchar* mtfm2 = test_mat[OUTPUT][1].data;
-    double* e_prop1 = (double*)test_mat[REF_OUTPUT][0].data;
-    double* e_prop2 = (double*)test_mat[OUTPUT][0].data;
+    uchar* mtfm1 = test_mat[REF_OUTPUT][1].ptr();
+    uchar* mtfm2 = test_mat[OUTPUT][1].ptr();
+    double* e_prop1 = test_mat[REF_OUTPUT][0].ptr<double>();
+    double* e_prop2 = test_mat[OUTPUT][0].ptr<double>();
    Mat E_prop2 = Mat(3, 1, CV_64F, e_prop2);

    int i, pt_count = test_mat[INPUT][2].cols;
@@ -1407,12 +1400,12 @@ void CV_EssentialMatTest::prepare_to_validation( int test_case_idx )



-    double* pose_prop1 = (double*)test_mat[REF_OUTPUT][2].data;
-    double* pose_prop2 = (double*)test_mat[OUTPUT][2].data;
-    double terr1 = cvtest::norm(Rt0.col(3) / norm(Rt0.col(3)) + test_mat[TEMP][3], NORM_L2);
-    double terr2 = cvtest::norm(Rt0.col(3) / norm(Rt0.col(3)) - test_mat[TEMP][3], NORM_L2);
-    Mat rvec;
-    Rodrigues(Rt0.colRange(0, 3), rvec);
+    double* pose_prop1 = test_mat[REF_OUTPUT][2].ptr<double>();
+    double* pose_prop2 = test_mat[OUTPUT][2].ptr<double>();
+    double terr1 = cvtest::norm(Rt0.col(3) / cvtest::norm(Rt0.col(3), NORM_L2) + test_mat[TEMP][3], NORM_L2);
+    double terr2 = cvtest::norm(Rt0.col(3) / cvtest::norm(Rt0.col(3), NORM_L2) - test_mat[TEMP][3], NORM_L2);
+    Mat rvec(3, 1, CV_32F);
+    cvtest::Rodrigues(Rt0.colRange(0, 3), rvec);
    pose_prop1[0] = 0;
    // No check for CV_LMeDS on translation. Since it
    // involves with some degraded problem, when data is exact inliers.
@@ -1550,7 +1543,7 @@ void CV_ConvertHomogeneousTest::fill_array( int /*test_case_idx*/, int /*i*/, in

 void CV_ConvertHomogeneousTest::run_func()
 {
-    CvMat _input = test_mat[INPUT][0], _output = test_mat[OUTPUT][0];
+    CvMat _input = cvMat(test_mat[INPUT][0]), _output = cvMat(test_mat[OUTPUT][0]);
    cvConvertPointsHomogeneous( &_input, &_output );
 }

@@ -1685,7 +1678,7 @@ void CV_ComputeEpilinesTest::fill_array( int test_case_idx, int i, int j, Mat& a

 void CV_ComputeEpilinesTest::run_func()
 {
-    CvMat _points = test_mat[INPUT][0], _F = test_mat[INPUT][1], _lines = test_mat[OUTPUT][0];
+    CvMat _points = cvMat(test_mat[INPUT][0]), _F = cvMat(test_mat[INPUT][1]), _lines = cvMat(test_mat[OUTPUT][0]);
    cvComputeCorrespondEpilines( &_points, which_image, &_F, &_lines );
 }

@@ -1723,4 +1716,22 @@ TEST(Calib3d_ConvertHomogeneoous, accuracy) { CV_ConvertHomogeneousTest test; te
 TEST(Calib3d_ComputeEpilines, accuracy) { CV_ComputeEpilinesTest test; test.safe_run(); }
 TEST(Calib3d_FindEssentialMat, accuracy) { CV_EssentialMatTest test; test.safe_run(); }

+TEST(Calib3d_FindFundamentalMat, correctMatches)
+{
+    double fdata[] = {0, 0, 0, 0, 0, -1, 0, 1, 0};
+    double p1data[] = {200, 0, 1};
+    double p2data[] = {170, 0, 1};
+
+    Mat F(3, 3, CV_64F, fdata);
+    Mat p1(1, 1, CV_64FC2, p1data);
+    Mat p2(1, 1, CV_64FC2, p2data);
+    Mat np1, np2;
+
+    correctMatches(F, p1, p2, np1, np2);
+
+    cout << np1 << endl;
+    cout << np2 << endl;
+}
+
+}} // namespace
 /* End of file. */
@@ -12,6 +12,7 @@
 //
 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Copyright (C) 2015, Itseez Inc., all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
@@ -41,7 +42,8 @@
 //M*/

 #include "test_precomp.hpp"
-#include <time.h>
+
+namespace opencv_test { namespace {

 #define CALIB3D_HOMOGRAPHY_ERROR_MATRIX_SIZE 1
 #define CALIB3D_HOMOGRAPHY_ERROR_MATRIX_DIFF 2
@@ -62,10 +64,10 @@

 #define MAX_COUNT_OF_POINTS 303
 #define COUNT_NORM_TYPES 3
-#define METHODS_COUNT 3
+#define METHODS_COUNT 4

 int NORM_TYPE[COUNT_NORM_TYPES] = {cv::NORM_L1, cv::NORM_L2, cv::NORM_INF};
-int METHOD[METHODS_COUNT] = {0, cv::RANSAC, cv::LMEDS};
+int METHOD[METHODS_COUNT] = {0, cv::RANSAC, cv::LMEDS, cv::RHO};

 using namespace cv;
 using namespace std;
@@ -94,12 +96,12 @@ private:

    void print_information_1(int j, int N, int method, const Mat& H);
    void print_information_2(int j, int N, int method, const Mat& H, const Mat& H_res, int k, double diff);
-    void print_information_3(int j, int N, const Mat& mask);
+    void print_information_3(int method, int j, int N, const Mat& mask);
    void print_information_4(int method, int j, int N, int k, int l, double diff);
    void print_information_5(int method, int j, int N, int l, double diff);
-    void print_information_6(int j, int N, int k, double diff, bool value);
-    void print_information_7(int j, int N, int k, double diff, bool original_value, bool found_value);
-    void print_information_8(int j, int N, int k, int l, double diff);
+    void print_information_6(int method, int j, int N, int k, double diff, bool value);
+    void print_information_7(int method, int j, int N, int k, double diff, bool original_value, bool found_value);
+    void print_information_8(int method, int j, int N, int k, int l, double diff);
 };

 CV_HomographyTest::CV_HomographyTest() : max_diff(1e-2f), max_2diff(2e-2f)
@@ -144,7 +146,7 @@ void CV_HomographyTest::print_information_1(int j, int N, int _method, const Mat
    cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else  cout << "vector <Point2f>";
    cout << "   Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
    cout << "Count of points: " << N << endl; cout << endl;
-    cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl;
+    cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else if (_method == cv::RHO) cout << "RHO"; else cout << "LMEDS"; cout << endl;
    cout << "Homography matrix:" << endl; cout << endl;
    cout << H << endl; cout << endl;
    cout << "Number of rows: " << H.rows << "   Number of cols: " << H.cols << endl; cout << endl;
@@ -156,7 +158,7 @@ void CV_HomographyTest::print_information_2(int j, int N, int _method, const Mat
    cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else  cout << "vector <Point2f>";
    cout << "   Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
    cout << "Count of points: " << N << endl; cout << endl;
-    cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl;
+    cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else if (_method == cv::RHO) cout << "RHO"; else cout << "LMEDS"; cout << endl;
    cout << "Original matrix:" << endl; cout << endl;
    cout << H << endl; cout << endl;
    cout << "Found matrix:" << endl; cout << endl;
@@ -166,13 +168,13 @@ void CV_HomographyTest::print_information_2(int j, int N, int _method, const Mat
    cout << "Maximum allowed difference: " << max_diff << endl; cout << endl;
 }

-void CV_HomographyTest::print_information_3(int j, int N, const Mat& mask)
+void CV_HomographyTest::print_information_3(int _method, int j, int N, const Mat& mask)
 {
    cout << endl; cout << "Checking for inliers/outliers mask..." << endl; cout << endl;
    cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else  cout << "vector <Point2f>";
    cout << "   Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
    cout << "Count of points: " << N << endl; cout << endl;
-    cout << "Method: RANSAC" << endl;
+    cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl;
    cout << "Found mask:" << endl; cout << endl;
    cout << mask << endl; cout << endl;
    cout << "Number of rows: " << mask.rows << "   Number of cols: " << mask.cols << endl; cout << endl;
@@ -189,7 +191,7 @@ void CV_HomographyTest::print_information_4(int _method, int j, int N, int k, in
    cout << "Number of point: " << k << endl;
    cout << "Norm type using in criteria: "; if (NORM_TYPE[l] == 1) cout << "INF"; else if (NORM_TYPE[l] == 2) cout << "L1"; else cout << "L2"; cout << endl;
    cout << "Difference with noise of point: " << diff << endl;
-    cout << "Maxumum allowed difference: " << max_2diff << endl; cout << endl;
+    cout << "Maximum allowed difference: " << max_2diff << endl; cout << endl;
 }

 void CV_HomographyTest::print_information_5(int _method, int j, int N, int l, double diff)
@@ -202,13 +204,13 @@ void CV_HomographyTest::print_information_5(int _method, int j, int N, int l, do
    cout << "Count of points: " << N << endl;
    cout << "Norm type using in criteria: "; if (NORM_TYPE[l] == 1) cout << "INF"; else if (NORM_TYPE[l] == 2) cout << "L1"; else cout << "L2"; cout << endl;
    cout << "Difference with noise of points: " << diff << endl;
-    cout << "Maxumum allowed difference: " << max_diff << endl; cout << endl;
+    cout << "Maximum allowed difference: " << max_diff << endl; cout << endl;
 }

-void CV_HomographyTest::print_information_6(int j, int N, int k, double diff, bool value)
+void CV_HomographyTest::print_information_6(int _method, int j, int N, int k, double diff, bool value)
 {
    cout << endl; cout << "Checking for inliers/outliers mask..." << endl; cout << endl;
-    cout << "Method: RANSAC" << endl;
+    cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl;
    cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else  cout << "vector <Point2f>";
    cout << "   Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
    cout << "Count of points: " << N << "   " << endl;
@@ -218,10 +220,10 @@ void CV_HomographyTest::print_information_6(int j, int N, int k, double diff, bo
    cout << "Value of found mask: "<< value << endl; cout << endl;
 }

-void CV_HomographyTest::print_information_7(int j, int N, int k, double diff, bool original_value, bool found_value)
+void CV_HomographyTest::print_information_7(int _method, int j, int N, int k, double diff, bool original_value, bool found_value)
 {
    cout << endl; cout << "Checking for inliers/outliers mask..." << endl; cout << endl;
-    cout << "Method: RANSAC" << endl;
+    cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl;
    cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else  cout << "vector <Point2f>";
    cout << "   Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
    cout << "Count of points: " << N << "   " << endl;
@@ -231,10 +233,10 @@ void CV_HomographyTest::print_information_7(int j, int N, int k, double diff, bo
    cout << "Value of original mask: "<< original_value << "   Value of found mask: " << found_value << endl; cout << endl;
 }

-void CV_HomographyTest::print_information_8(int j, int N, int k, int l, double diff)
+void CV_HomographyTest::print_information_8(int _method, int j, int N, int k, int l, double diff)
 {
    cout << endl; cout << "Checking for reprojection error of inlier..." << endl; cout << endl;
-    cout << "Method: RANSAC" << endl;
+    cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl;
    cout << "Sigma of normal noise: " << sigma << endl;
    cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else  cout << "vector <Point2f>";
    cout << "   Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
@@ -242,7 +244,7 @@ void CV_HomographyTest::print_information_8(int j, int N, int k, int l, double d
    cout << "Number of point: " << k << "   " << endl;
    cout << "Norm type using in criteria: "; if (NORM_TYPE[l] == 1) cout << "INF"; else if (NORM_TYPE[l] == 2) cout << "L1"; else cout << "L2"; cout << endl;
    cout << "Difference with noise of point: " << diff << endl;
-    cout << "Maxumum allowed difference: " << max_2diff << endl; cout << endl;
+    cout << "Maximum allowed difference: " << max_2diff << endl; cout << endl;
 }

 void CV_HomographyTest::run(int)
@@ -339,14 +341,15 @@ void CV_HomographyTest::run(int)

                    continue;
                }
+            case cv::RHO:
            case RANSAC:
                {
                    cv::Mat mask [4]; double diff;

-                    Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, RANSAC, reproj_threshold, mask[0]),
-                                         cv::findHomography(src_mat_2f, dst_vec, RANSAC, reproj_threshold, mask[1]),
-                                         cv::findHomography(src_vec, dst_mat_2f, RANSAC, reproj_threshold, mask[2]),
-                                         cv::findHomography(src_vec, dst_vec, RANSAC, reproj_threshold, mask[3]) };
+                    Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, method, reproj_threshold, mask[0]),
+                                         cv::findHomography(src_mat_2f, dst_vec, method, reproj_threshold, mask[1]),
+                                         cv::findHomography(src_vec, dst_mat_2f, method, reproj_threshold, mask[2]),
+                                         cv::findHomography(src_vec, dst_vec, method, reproj_threshold, mask[3]) };

                    for (int j = 0; j < 4; ++j)
                    {
@@ -370,7 +373,7 @@ void CV_HomographyTest::run(int)

                        if (code)
                        {
-                            print_information_3(j, N, mask[j]);
+                            print_information_3(method, j, N, mask[j]);

                            switch (code)
                            {
@@ -466,14 +469,15 @@ void CV_HomographyTest::run(int)

                    continue;
                }
+            case cv::RHO:
            case RANSAC:
                {
                    cv::Mat mask_res [4];

-                    Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, RANSAC, reproj_threshold, mask_res[0]),
-                                         cv::findHomography(src_mat_2f, dst_vec, RANSAC, reproj_threshold, mask_res[1]),
-                                         cv::findHomography(src_vec, dst_mat_2f, RANSAC, reproj_threshold, mask_res[2]),
-                                         cv::findHomography(src_vec, dst_vec, RANSAC, reproj_threshold, mask_res[3]) };
+                    Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, method, reproj_threshold, mask_res[0]),
+                                         cv::findHomography(src_mat_2f, dst_vec, method, reproj_threshold, mask_res[1]),
+                                         cv::findHomography(src_vec, dst_mat_2f, method, reproj_threshold, mask_res[2]),
+                                         cv::findHomography(src_vec, dst_vec, method, reproj_threshold, mask_res[3]) };

                    for (int j = 0; j < 4; ++j)
                    {
@@ -488,7 +492,7 @@ void CV_HomographyTest::run(int)

                        if (code)
                        {
-                            print_information_3(j, N, mask_res[j]);
+                            print_information_3(method, j, N, mask_res[j]);

                            switch (code)
                            {
@@ -520,14 +524,14 @@ void CV_HomographyTest::run(int)

                            if (mask_res[j].at<bool>(k, 0) != (diff <= reproj_threshold))
                            {
-                                print_information_6(j, N, k, diff, mask_res[j].at<bool>(k, 0));
+                                print_information_6(method, j, N, k, diff, mask_res[j].at<bool>(k, 0));
                                CV_Error(CALIB3D_HOMOGRAPHY_ERROR_RANSAC_MASK, MESSAGE_RANSAC_MASK_4);
                                return;
                            }

                            if (mask.at<bool>(k, 0) && !mask_res[j].at<bool>(k, 0))
                            {
-                                print_information_7(j, N, k, diff, mask.at<bool>(k, 0), mask_res[j].at<bool>(k, 0));
+                                print_information_7(method, j, N, k, diff, mask.at<bool>(k, 0), mask_res[j].at<bool>(k, 0));
                                CV_Error(CALIB3D_HOMOGRAPHY_ERROR_RANSAC_MASK, MESSAGE_RANSAC_MASK_5);
                                return;
                            }
@@ -547,7 +551,7 @@ void CV_HomographyTest::run(int)

                                    if (diff - cv::norm(noise_2d, NORM_TYPE[l]) > max_2diff)
                                    {
-                                        print_information_8(j, N, k, l, diff - cv::norm(noise_2d, NORM_TYPE[l]));
+                                        print_information_8(method, j, N, k, l, diff - cv::norm(noise_2d, NORM_TYPE[l]));
                                        CV_Error(CALIB3D_HOMOGRAPHY_ERROR_RANSAC_DIFF, MESSAGE_RANSAC_DIFF);
                                        return;
                                    }
@@ -562,7 +566,149 @@ void CV_HomographyTest::run(int)
            default: continue;
            }
        }
+
+        delete[]src_data;
+        src_data = NULL;
    }
 }

 TEST(Calib3d_Homography, accuracy) { CV_HomographyTest test; test.safe_run(); }
+
+TEST(Calib3d_Homography, EKcase)
+{
+    float pt1data[] =
+    {
+        2.80073029e+002f, 2.39591217e+002f, 2.21912201e+002f, 2.59783997e+002f,
+        2.16053192e+002f, 2.78826569e+002f, 2.22782532e+002f, 2.82330383e+002f,
+        2.09924820e+002f, 2.89122559e+002f, 2.11077698e+002f, 2.89384674e+002f,
+        2.25287689e+002f, 2.88795532e+002f, 2.11180801e+002f, 2.89653503e+002f,
+        2.24126404e+002f, 2.90466064e+002f, 2.10914429e+002f, 2.90886963e+002f,
+        2.23439362e+002f, 2.91657715e+002f, 2.24809387e+002f, 2.91891602e+002f,
+        2.09809082e+002f, 2.92891113e+002f, 2.08771164e+002f, 2.93093231e+002f,
+        2.23160095e+002f, 2.93259460e+002f, 2.07874023e+002f, 2.93989990e+002f,
+        2.08963638e+002f, 2.94209839e+002f, 2.23963165e+002f, 2.94479645e+002f,
+        2.23241791e+002f, 2.94887817e+002f, 2.09438782e+002f, 2.95233337e+002f,
+        2.08901886e+002f, 2.95762878e+002f, 2.21867981e+002f, 2.95747711e+002f,
+        2.24195511e+002f, 2.98270905e+002f, 2.09331345e+002f, 3.05958191e+002f,
+        2.24727875e+002f, 3.07186035e+002f, 2.26718842e+002f, 3.08095795e+002f,
+        2.25363953e+002f, 3.08200226e+002f, 2.19897797e+002f, 3.13845093e+002f,
+        2.25013474e+002f, 3.15558777e+002f
+    };
+
+    float pt2data[] =
+    {
+        1.84072723e+002f, 1.43591202e+002f, 1.25912483e+002f, 1.63783859e+002f,
+        2.06439407e+002f, 2.20573929e+002f, 1.43801437e+002f, 1.80703903e+002f,
+        9.77904129e+000f, 2.49660202e+002f, 1.38458405e+001f, 2.14502701e+002f,
+        1.50636337e+002f, 2.15597183e+002f, 6.43103180e+001f, 2.51667648e+002f,
+        1.54952499e+002f, 2.20780014e+002f, 1.26638412e+002f, 2.43040924e+002f,
+        3.67568909e+002f, 1.83624954e+001f, 1.60657944e+002f, 2.21794052e+002f,
+        -1.29507828e+000f, 3.32472443e+002f, 8.51442242e+000f, 4.15561554e+002f,
+        1.27161377e+002f, 1.97260361e+002f, 5.40714645e+000f, 4.90978302e+002f,
+        2.25571690e+001f, 3.96912415e+002f, 2.95664978e+002f, 7.36064959e+000f,
+        1.27241104e+002f, 1.98887573e+002f, -1.25569367e+000f, 3.87713226e+002f,
+        1.04194012e+001f, 4.31495758e+002f, 1.25868874e+002f, 1.99751617e+002f,
+        1.28195480e+002f, 2.02270355e+002f, 2.23436356e+002f, 1.80489182e+002f,
+        1.28727692e+002f, 2.11185410e+002f, 2.03336639e+002f, 2.52182083e+002f,
+        1.29366486e+002f, 2.12201904e+002f, 1.23897598e+002f, 2.17847351e+002f,
+        1.29015259e+002f, 2.19560623e+002f
+    };
+
+    int npoints = (int)(sizeof(pt1data)/sizeof(pt1data[0])/2);
+
+    Mat p1(1, npoints, CV_32FC2, pt1data);
+    Mat p2(1, npoints, CV_32FC2, pt2data);
+    Mat mask;
+
+    Mat h = findHomography(p1, p2, RANSAC, 0.01, mask);
+    ASSERT_TRUE(!h.empty());
+
+    cv::transpose(mask, mask);
+    Mat p3, mask2;
+    int ninliers = countNonZero(mask);
+    Mat nmask[] = { mask, mask };
+    merge(nmask, 2, mask2);
+    perspectiveTransform(p1, p3, h);
+    mask2 = mask2.reshape(1);
+    p2 = p2.reshape(1);
+    p3 = p3.reshape(1);
+    double err = cvtest::norm(p2, p3, NORM_INF, mask2);
+
+    printf("ninliers: %d, inliers err: %.2g\n", ninliers, err);
+    ASSERT_GE(ninliers, 10);
+    ASSERT_LE(err, 0.01);
+}
+
+TEST(Calib3d_Homography, fromImages)
+{
+    Mat img_1 = imread(cvtest::TS::ptr()->get_data_path() + "cv/optflow/image1.png", 0);
+    Mat img_2 = imread(cvtest::TS::ptr()->get_data_path() + "cv/optflow/image2.png", 0);
+    Ptr<ORB> orb = ORB::create();
+    vector<KeyPoint> keypoints_1, keypoints_2;
+    Mat descriptors_1, descriptors_2;
+    orb->detectAndCompute( img_1, Mat(), keypoints_1, descriptors_1, false );
+    orb->detectAndCompute( img_2, Mat(), keypoints_2, descriptors_2, false );
+
+    //-- Step 3: Matching descriptor vectors using Brute Force matcher
+    BFMatcher  matcher(NORM_HAMMING,false);
+    std::vector< DMatch > matches;
+    matcher.match( descriptors_1, descriptors_2, matches );
+
+    double max_dist = 0; double min_dist = 100;
+    //-- Quick calculation of max and min distances between keypoints
+    for( int i = 0; i < descriptors_1.rows; i++ )
+    {
+        double dist = matches[i].distance;
+        if( dist < min_dist ) min_dist = dist;
+        if( dist > max_dist ) max_dist = dist;
+    }
+
+    //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
+    std::vector< DMatch > good_matches;
+    for( int i = 0; i < descriptors_1.rows; i++ )
+    {
+        if( matches[i].distance <= 100 )
+            good_matches.push_back( matches[i]);
+    }
+
+    //-- Localize the model
+    std::vector<Point2f> pointframe1;
+    std::vector<Point2f> pointframe2;
+    for( int i = 0; i < (int)good_matches.size(); i++ )
+    {
+        //-- Get the keypoints from the good matches
+        pointframe1.push_back( keypoints_1[ good_matches[i].queryIdx ].pt );
+        pointframe2.push_back( keypoints_2[ good_matches[i].trainIdx ].pt );
+    }
+
+    Mat H0, H1, inliers0, inliers1;
+    double min_t0 = DBL_MAX, min_t1 = DBL_MAX;
+    for( int i = 0; i < 10; i++ )
+    {
+        double t = (double)getTickCount();
+        H0 = findHomography( pointframe1, pointframe2, RANSAC, 3.0, inliers0 );
+        t = (double)getTickCount() - t;
+        min_t0 = std::min(min_t0, t);
+    }
+    int ninliers0 = countNonZero(inliers0);
+    for( int i = 0; i < 10; i++ )
+    {
+        double t = (double)getTickCount();
+        H1 = findHomography( pointframe1, pointframe2, RHO, 3.0, inliers1 );
+        t = (double)getTickCount() - t;
+        min_t1 = std::min(min_t1, t);
+    }
+    int ninliers1 = countNonZero(inliers1);
+    double freq = getTickFrequency();
+    printf("nfeatures1 = %d, nfeatures2=%d, matches=%d, ninliers(RANSAC)=%d, "
+           "time(RANSAC)=%.2fmsec, ninliers(RHO)=%d, time(RHO)=%.2fmsec\n",
+           (int)keypoints_1.size(), (int)keypoints_2.size(),
+           (int)good_matches.size(), ninliers0, min_t0*1000./freq, ninliers1, min_t1*1000./freq);
+
+    ASSERT_TRUE(!H0.empty());
+    ASSERT_GE(ninliers0, 80);
+    ASSERT_TRUE(!H1.empty());
+    ASSERT_GE(ninliers1, 80);
+}
+
+}} // namespace
@@ -13,7 +13,7 @@
 //                           License Agreement
 //                For Open Source Computer Vision Library
 //
- // Copyright (C) 2014, Samson Yilma¸ (samson_yilma@yahoo.com), all rights reserved.
+ // Copyright (C) 2014, Samson Yilma (samson_yilma@yahoo.com), all rights reserved.
 //
 // Third party copyrights are property of their respective owners.
 //
@@ -44,11 +44,8 @@
 //M*/

 #include "test_precomp.hpp"
-#include "opencv2/calib3d.hpp"
-#include <vector>

-using namespace cv;
-using namespace std;
+namespace opencv_test { namespace {

 class CV_HomographyDecompTest: public cvtest::BaseTest {

@@ -118,9 +115,9 @@ private:
             riter != rotations.end() && titer != translations.end() && niter != normals.end();
             ++riter, ++titer, ++niter) {

-            double rdist = norm(*riter, _R, NORM_INF);
-            double tdist = norm(*titer, _t, NORM_INF);
-            double ndist = norm(*niter, _n, NORM_INF);
+            double rdist = cvtest::norm(*riter, _R, NORM_INF);
+            double tdist = cvtest::norm(*titer, _t, NORM_INF);
+            double ndist = cvtest::norm(*niter, _n, NORM_INF);

            if (   rdist < max_error
                && tdist < max_error
@@ -136,3 +133,37 @@ private:
 };

 TEST(Calib3d_DecomposeHomography, regression) { CV_HomographyDecompTest test; test.safe_run(); }
+
+
+TEST(Calib3d_DecomposeHomography, issue_4978)
+{
+    Matx33d K(
+        1.0,   0.0,    0.0,
+        0.0,   1.0,    0.0,
+        0.0,   0.0,    1.0
+    );
+
+    Matx33d H(
+        -0.102896, 0.270191,   -0.0031153,
+        0.0406387, 1.19569,    -0.0120456,
+        0.445351,  0.0410889,  1
+    );
+
+    vector<Mat> rotations;
+    vector<Mat> translations;
+    vector<Mat> normals;
+
+    decomposeHomographyMat(H, K, rotations, translations, normals);
+
+    ASSERT_GT(rotations.size(), (size_t)0u);
+    for (size_t i = 0; i < rotations.size(); i++)
+    {
+        // check: det(R) = 1
+        EXPECT_TRUE(std::fabs(cv::determinant(rotations[i]) - 1.0) < 0.01)
+            << "R: det=" << cv::determinant(rotations[0]) << std::endl << rotations[i] << std::endl
+            << "T:" << std::endl << translations[i] << std::endl;
+    }
+}
+
+
+}} // namespace
@@ -1,3 +1,6 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
 #include "test_precomp.hpp"

 CV_TEST_MAIN("")
--- a/Show More
+++ b/Show More