Wrap p3p_kneip in python

CMakeLists.txt

@@ -12,7 +12,7 @@ set (OPENGV_VERSION_MINOR 0)
 #  RelWithDebInfo (CMAKE_C_FLAGS_RELWITHDEBINFO or CMAKE_CXX_FLAGS_RELWITHDEBINFO
 #  MinSizeRel (CMAKE_C_FLAGS_MINSIZEREL or CMAKE_CXX_FLAGS_MINSIZEREL) 
 
-set(CMAKE_BUILD_TYPE Release)
+#set(CMAKE_BUILD_TYPE Release)
 
 #set the default path for built executables to the "bin" directory
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
@@ -205,3 +205,5 @@ target_link_libraries( test_point_cloud opengv random_generators )
 
 add_Executable( test_point_cloud_sac test/test_point_cloud_sac.cpp )
 target_link_libraries( test_point_cloud_sac opengv random_generators )
+
+add_subdirectory( python )

modules/FindNumPy.cmake

@@ -0,0 +1,102 @@
+# - Find the NumPy libraries
+# This module finds if NumPy is installed, and sets the following variables
+# indicating where it is.
+#
+# TODO: Update to provide the libraries and paths for linking npymath lib.
+#
+#  NUMPY_FOUND               - was NumPy found
+#  NUMPY_VERSION             - the version of NumPy found as a string
+#  NUMPY_VERSION_MAJOR       - the major version number of NumPy
+#  NUMPY_VERSION_MINOR       - the minor version number of NumPy
+#  NUMPY_VERSION_PATCH       - the patch version number of NumPy
+#  NUMPY_VERSION_DECIMAL     - e.g. version 1.6.1 is 10601
+#  NUMPY_INCLUDE_DIRS        - path to the NumPy include files
+
+#============================================================================
+# Copyright 2012 Continuum Analytics, Inc.
+#
+# MIT License
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files
+# (the "Software"), to deal in the Software without restriction, including
+# without limitation the rights to use, copy, modify, merge, publish,
+# distribute, sublicense, and/or sell copies of the Software, and to permit
+# persons to whom the Software is furnished to do so, subject to
+# the following conditions:
+#
+# The above copyright notice and this permission notice shall be included
+# in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+#============================================================================
+
+# Finding NumPy involves calling the Python interpreter
+if(NumPy_FIND_REQUIRED)
+    find_package(PythonInterp REQUIRED)
+else()
+    find_package(PythonInterp)
+endif()
+
+if(NOT PYTHONINTERP_FOUND)
+    set(NUMPY_FOUND FALSE)
+    return()
+endif()
+
+execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
+    "import numpy as n; print(n.__version__); print(n.get_include());"
+    RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS
+    OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT
+    ERROR_VARIABLE _NUMPY_ERROR_VALUE
+    OUTPUT_STRIP_TRAILING_WHITESPACE)
+
+if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0)
+    if(NumPy_FIND_REQUIRED)
+        message(FATAL_ERROR
+            "NumPy import failure:\n${_NUMPY_ERROR_VALUE}")
+    endif()
+    set(NUMPY_FOUND FALSE)
+    return()
+endif()
+
+# Convert the process output into a list
+string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT})
+string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES})
+# Just in case there is unexpected output from the Python command.
+list(GET _NUMPY_VALUES -2 NUMPY_VERSION)
+list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS)
+
+string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}")
+if("${_VER_CHECK}" STREQUAL "")
+    # The output from Python was unexpected. Raise an error always
+    # here, because we found NumPy, but it appears to be corrupted somehow.
+    message(FATAL_ERROR
+        "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n")
+    return()
+endif()
+
+# Make sure all directory separators are '/'
+string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS})
+
+# Get the major and minor version numbers
+string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION})
+list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR)
+list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR)
+list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH)
+string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH})
+math(EXPR NUMPY_VERSION_DECIMAL
+    "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}")
+
+find_package_message(NUMPY
+    "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}"
+    "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}")
+
+set(NUMPY_FOUND TRUE)
+

python/CMakeLists.txt

@@ -0,0 +1,31 @@
+
+find_package(Boost COMPONENTS python REQUIRED)
+include_directories(${Boost_INCLUDE_DIRS})
+
+set(Python_ADDITIONAL_VERSIONS "2.7")
+find_package(PythonLibs REQUIRED)
+include_directories(${PYTHON_INCLUDE_DIRS})
+
+find_package(NumPy REQUIRED)
+include_directories(${NUMPY_INCLUDE_DIRS})
+
+
+add_library( pyopengv SHARED pyopengv.cpp )
+target_link_libraries(pyopengv opengv)
+
+
+set_target_properties(pyopengv PROPERTIES
+    PREFIX ""
+    SUFFIX ".so"
+)
+if(APPLE)
+    set_target_properties(pyopengv PROPERTIES
+        LINK_FLAGS "-undefined dynamic_lookup"
+    )
+    target_link_libraries(pyopengv "${Boost_LIBRARY_DIR}/libboost_python.a")
+else()
+    target_link_libraries(pyopengv
+        ${Boost_LIBRARIES}
+        ${PYTHON_LIBRARIES}
+    )
+endif()

python/pyopengv.cpp

@@ -0,0 +1,198 @@
+#include <boost/python.hpp>
+#include <iostream>
+#include <vector>
+#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
+#include <opengv/absolute_pose/methods.hpp>
+#include "types.hpp"
+
+#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
+#include <numpy/ndarrayobject.h>
+
+#if (PY_VERSION_HEX < 0x03000000)
+static void numpy_import_array_wrapper()
+#else
+static int* numpy_import_array_wrapper()
+#endif
+{
+  /* Initialise numpy API and use 2/3 compatible return */
+  import_array();
+}
+
+
+
+
+namespace pyopengv {
+
+namespace bp = boost::python;
+namespace bpn = boost::python::numeric;
+
+typedef PyArrayContiguousView<double> pyarray_t;
+
+
+opengv::bearingVector_t bearingVectorFromArray(
+    const pyarray_t &array,
+    size_t index )
+{
+  opengv::bearingVector_t v;
+  v[0] = array.get(index, 0);
+  v[1] = array.get(index, 1);
+  v[2] = array.get(index, 2);
+    for (int i = 0; i < 3; ++i) {
+      std::cout << v[i] << " ";
+    }
+    std::cout << "  bearing\n";
+  return v;
+}
+
+
+opengv::point_t pointFromArray(
+    const pyarray_t &array,
+    size_t index )
+{
+  opengv::point_t p;
+  p[0] = array.get(index, 0);
+  p[1] = array.get(index, 1);
+  p[2] = array.get(index, 2);
+    for (int i = 0; i < 3; ++i) {
+      std::cout << p[i] << " ";
+    }
+    std::cout << "  point\n";
+  return p;
+}
+
+
+bp::list listFromTransformations( const opengv::transformations_t &t )
+{
+  bp::list retn;
+  for (size_t i = 0; i < t.size(); ++i) {
+    std::cout << t[i] << "\n";
+    Eigen::Matrix<double, 3, 4, Eigen::RowMajor> t_row_major = t[i];
+    npy_intp shape[2] = {3, 4};
+    retn.append(bpn_array_from_data(2, shape, t_row_major.data()));
+  }
+  return retn;
+}
+
+
+class CentralAbsoluteAdapter : public opengv::absolute_pose::AbsoluteAdapterBase
+{
+protected:
+  using AbsoluteAdapterBase::_t;
+  using AbsoluteAdapterBase::_R;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * \brief Constructor. See protected class-members to understand parameters
+   */
+  CentralAbsoluteAdapter(
+      bpn::array & bearingVectors,
+      bpn::array & points )
+    : _bearingVectors(bearingVectors)
+    , _points(points)
+  {}
+
+  /**
+   * \brief Constructor. See protected class-members to understand parameters
+   */
+  CentralAbsoluteAdapter(
+      bpn::array & bearingVectors,
+      bpn::array & points,
+      bpn::array & R )
+    : _bearingVectors(bearingVectors)
+    , _points(points)
+  {
+    pyarray_t R_view(R);
+    for (int i = 0; i < 3; ++i) {
+      for (int j = 0; j < 3; ++j) {
+        _R(i, j) = R_view.get(i, j);
+      }
+    }
+  }
+
+  /**
+   * \brief Constructor. See protected class-members to understand parameters
+   */
+  CentralAbsoluteAdapter(
+      bpn::array & bearingVectors,
+      bpn::array & points,
+      bpn::array & t,
+      bpn::array & R )
+    : _bearingVectors(bearingVectors)
+    , _points(points)
+  {
+    pyarray_t t_view(t);
+    for (int i = 0; i < 3; ++i) {
+      _t(i) = t_view.get(i);
+    }
+    pyarray_t R_view(R);
+    for (int i = 0; i < 3; ++i) {
+      for (int j = 0; j < 3; ++j) {
+        _R(i, j) = R_view.get(i, j);
+      }
+    }
+  }
+  /**
+   * Destructor
+   */
+  virtual ~CentralAbsoluteAdapter() {}
+
+  //Access of correspondences
+
+  /** See parent-class */
+  virtual opengv::bearingVector_t getBearingVector( size_t index ) const {
+    opengv::bearingVector_t b = bearingVectorFromArray(_bearingVectors, index);
+    return b;
+  }
+  /** See parent-class */
+  virtual double getWeight( size_t index ) const {
+    return 1.0;
+  }
+  /** See parent-class. Returns zero for this adapter. */
+  virtual opengv::translation_t getCamOffset( size_t index ) const {
+    return Eigen::Vector3d::Zero();
+  }
+  /** See parent-class Returns identity for this adapter. */
+  virtual opengv::rotation_t getCamRotation( size_t index ) const {
+    return opengv::rotation_t::Identity();
+  }
+  /** See parent-class */
+  virtual opengv::point_t getPoint( size_t index ) const {
+    return pointFromArray(_points, index);
+  }
+  /** See parent-class */
+  virtual size_t getNumberCorrespondences() const {
+    return _bearingVectors.shape(0);
+  }
+
+protected:
+  /** Reference to the bearing-vectors expressed in the camera-frame */
+  pyarray_t _bearingVectors;
+  /** Reference to the points expressed in the world-frame. */
+  pyarray_t _points;
+};
+
+
+
+bp::object absolute_pose_p3p_kneip( bpn::array &v, bpn::array &p )
+{
+  CentralAbsoluteAdapter adapter( v, p );
+
+  opengv::transformations_t transformations =
+      opengv::absolute_pose::p3p_kneip( adapter, 0, 1, 2 );
+
+  return listFromTransformations(transformations);
+}
+
+}
+
+BOOST_PYTHON_MODULE(pyopengv) {
+  using namespace boost::python;
+
+  boost::python::numeric::array::set_module_and_type("numpy", "ndarray");
+  numpy_import_array_wrapper();
+
+  def("absolute_pose_p3p_kneip", pyopengv::absolute_pose_p3p_kneip);
+
+}