OpenGV + covariances + MLPnP

.gitignore

@@ -1 +1,2 @@
 lib
+/matlab/CEPPnP_Toolbox

CMakeLists.txt

@@ -38,11 +38,16 @@ OPTION(BUILD_PYTHON "Build Python extension" OFF)
 OPTION(BUILD_POSITION_INDEPENDENT_CODE "Build position independent code (-fPIC)" ON)
 OPTION(INSTALL_OPENGV "Install OpenGV on the system" OFF)
 
-add_definitions (-Wall -march=native -O3) #TODO use correct c++11 def once everybody has moved to gcc 4.7 # for now I even removed std=gnu++0x
-
+IF(WIN32)
+add_definitions (-bigobj -W0 -MP -Zm10)
+ELSE()
+add_definitions (-Wall -march=native -O3 ) #TODO use correct c++11 def once everybody has moved to gcc 4.7 # for now I even removed std=gnu++0x
 IF(BUILD_POSITION_INDEPENDENT_CODE)
   add_definitions( -fPIC )
 ENDIF()
+ENDIF()
+
+
 
 
 # get eigen (under windows)
@@ -87,6 +92,8 @@ set( OPENGV_SOURCE_FILES
   src/absolute_pose/modules/gpnp5/spolynomials.cpp
   src/absolute_pose/modules/upnp2.cpp
   src/absolute_pose/modules/upnp4.cpp
+  src/absolute_pose/modules/mlpnp/mlpnp.cpp
+  src/absolute_pose/modules/mlpnp/mlpnp_lm.cpp
   src/relative_pose/modules/main.cpp
   src/relative_pose/modules/fivept_nister/modules.cpp
   src/relative_pose/modules/fivept_stewenius/modules.cpp
@@ -97,7 +104,9 @@ set( OPENGV_SOURCE_FILES
   src/relative_pose/modules/sixpt/modules2.cpp
   src/relative_pose/modules/eigensolver/modules.cpp
   src/relative_pose/modules/ge/modules.cpp
+  src/relative_pose/modules/sixpt_ventura/approx_relpose_generalized_fast_computeA.cpp
   src/math/cayley.cpp
+  src/math/rodrigues.cpp
   src/math/quaternion.cpp
   src/math/arun.cpp
   src/math/Sturm.cpp
@@ -141,6 +150,7 @@ set( OPENGV_HEADER_FILES
   include/opengv/triangulation/methods.hpp
   include/opengv/point_cloud/methods.hpp
   include/opengv/math/cayley.hpp
+  include/opengv/math/rodrigues.hpp
   include/opengv/math/quaternion.hpp
   include/opengv/math/arun.hpp
   include/opengv/math/Sturm.hpp

doc/addons/03_matlab.dox

@@ -6,6 +6,7 @@
  * <li>X = opengv ( method, data1, data2 )
  * <li>X = opengv ( method, indices, data1, data2 )
  * <li>X = opengv ( method, indices, data1, data2, prior )
+ * <li>X = opengv ( method, indices, data1, data2, prior, data3) 
  * </ul>
  *
  * where
@@ -14,7 +15,7 @@
  *
 \verbatim
 absolute pose methods:
-'p2p', 'p3p_kneip', 'p3p_gao', 'epnp', 'p3p_kneip_ransac', 'p3p_gao_ransac', 'epnp_ransac', 'abs_nonlin_central', 'gp3p', 'gp3p_ransac', 'gpnp', 'abs_nonlin_noncentral', 'upnp'.
+'p2p', 'p3p_kneip', 'p3p_gao', 'epnp', 'mlpnp' 'p3p_kneip_ransac', 'p3p_gao_ransac', 'epnp_ransac', 'abs_nonlin_central', 'gp3p', 'gp3p_ransac', 'gpnp', 'abs_nonlin_noncentral', 'upnp'.
 
 relative pose methods:
 'twopt', 'twopt_rotationOnly', 'rotationOnly', 'fivept_stewenius', 'fivept_nister', 'fivept_kneip', 'sevenpt', 'eightpt', 'eigensolver', 'rotationOnly_ransac',
@@ -30,6 +31,7 @@ point_cloud methods:
  *      supposed to be prerotated into the body frame (only rotated!).
  * <li> indices is a subset of correspondences that we plan to use for the computation.
  * <li> prior is a 3x1 (translation), 3x3 (rotation), or 3x4 ([R t]-transformation) holding a prior value for the transformation to compute.
+ * <li> data3 (dimension 9xn) are the 3x3 covariance matrices corresponding to the bearing vectors (see example below) 
  * <li> The return value X is a 3xnxm-matrix, where n is the second dimensionality of the solution space, and m is the number of solutions. n is one for methods that only compute a translation, 3 for methods that only compute a rotation, and 4 for methods that compute both rotation and translation (format: [R t]).
  * </ul>
  *
@@ -81,7 +83,35 @@ point_cloud methods:
  \endverbatim
  *
  * In order to handle outliers as well, simply switch to 'gp3p_ransac'. Now that you know how to handle 2D-3D registration in the central and non-central case, the 2D-2D registration case is also easily learned. Simply replace the world points by 2D measurements in another view.
+ * 
+ * In order to exploit 2D covariance information that might be available, we can have a look at MLPnP.
+ \verbatim
+ v = K \ [I; ones(1,size(I,2))];  % bearing vectors
+ I_norms = sqrt(sum(temp.*temp));
+ I_normalized = temp ./ repmat(I_norms,3,1);
+ % Now assume we have 2D covariance information for our 2D image points I
+ % and say we have measured our image points in x and y direction with a standard deviation of s_x=s_y=0.5 pixels
+ cov = zeros(9,size(Cu,3)); 
+ for i=1:size(I,2)
+    v = I_normalized(:,i);         % current bearing vector
+    cov2d = diag([sx^2 sy^2 0]);   % image plane covariance
+    covbearing = K\cov2d/K';       % propagate to 3D
+    J = (eye(3)-v*v')/(v'*v)) / norm(I(:,i))     % Jacobian for normalization
+    Evv = J*covbearing*J';
+    cov(:,i) = reshape(Evv,9,1);
+ end
+ \endverbatim
+ *
+ * Now we can call opengv and include the covariance by setting the data3 argument to cov
+ *
+ \verbatim
+ Tprior = [1 0 0 0; 0 1 0 0; 0 0 1 0 ; 0 0 0 0];
+ X = opengv('mlpnp', [1:size(P,2)], P, I_normalized, Tprior, cov);
+ R = X(:,1:3);
+ t = X(:,4);
+ \endverbatim
  *
+ *  
  * \section sec_benchmarks Automatic benchmarking of algorithms
  *
  * Perhaps the nicest thing about the Matlab-code is that it includes automatic benchmarks for all algorithms. The subfolder matlab/helpers contains useful functions for the benchmarks.

include/opengv/absolute_pose/AbsoluteAdapterBase.hpp

@@ -104,6 +104,24 @@ class AbsoluteAdapterBase
    * \return The corresponding bearing vector.
    */
   virtual opengv::bearingVector_t getBearingVector(size_t index ) const = 0;
+  /**
+  * \brief Retrieve all bearing vectors.
+  * \return The complete set of bearing vectors.
+  */
+  virtual opengv::bearingVectors_t getBearingVectors() const = 0;
+
+  /**
+  * \brief Retrieve all bearing vectors.
+  * \return The complete set of bearing vectors.
+  */
+  virtual opengv::cov3_mat_t getCovariance(size_t index) const = 0;
+
+  /**
+  * \brief Retrieve all bearing vectors.
+  * \return The complete set of bearing vectors.
+  */
+  virtual opengv::cov3_mats_t getCovariances() const = 0;
+
   /**
    * \brief Retrieve the weight of a correspondence. The weight is supposed to
    *        reflect the quality of a correspondence, and typically is between
@@ -134,6 +152,11 @@ class AbsoluteAdapterBase
    * \return The corresponding world point.
    */
   virtual opengv::point_t getPoint( size_t index ) const = 0;
+  /**
+  * \brief Retrieve all world points.
+  * \return The world points.
+  */
+  virtual opengv::points_t getPoints() const = 0;
   /**
    * \brief Retrieve the number of correspondences.
    * \return The number of correspondences.

include/opengv/absolute_pose/AbsoluteMultiAdapterBase.hpp

@@ -109,6 +109,14 @@ class AbsoluteMultiAdapterBase : public AbsoluteAdapterBase
    */
   virtual opengv::bearingVector_t getBearingVector(
       size_t frameIndex, size_t correspondenceIndex ) const = 0;
+  /**
+  * \brief Retrieve the covariance matrix of a correspondence in a certain frame.
+  * \param[in] frameIndex Index of the frame.
+  * \param[in] correspondenceIndex Index of the correspondence in this frame.
+  * \return The corresponding covariance matrix.
+  */
+  virtual opengv::cov3_mat_t getCovariance(
+	  size_t frameIndex, size_t correspondenceIndex) const = 0;
   /**
    * \brief Retrieve the weight of a correspondence. The weight is supposed to
    *        reflect the quality of a correspondence, and typically is between
@@ -196,6 +204,25 @@ class AbsoluteMultiAdapterBase : public AbsoluteAdapterBase
         multiFrameIndex(index), multiCorrespondenceIndex(index) );
   }
   /** See parent-class (no need to overload) */
+  // edited Steffen Urban / This is still wrong wrong!!!
+  virtual bearingVectors_t getBearingVectors() const
+  {
+	  return getBearingVectors();
+  }
+  /** See parent-class (no need to overload) */
+  // edited Steffen Urban / This is still wrong wrong!!!
+  virtual cov3_mats_t getCovariances() const
+  {
+	  return getCovariances();
+  }
+  /** See parent-class (no need to overload) */
+  // edited Steffen Urban / This is still wrong wrong!!!
+  virtual cov3_mat_t getCovariance(size_t index) const
+  {
+	  return getCovariance(
+		  multiFrameIndex(index), multiCorrespondenceIndex(index));
+  }
+  /** See parent-class (no need to overload) */
   virtual double getWeight( size_t index ) const
   {
     return getWeight(
@@ -213,6 +240,12 @@ class AbsoluteMultiAdapterBase : public AbsoluteAdapterBase
     return getPoint(
         multiFrameIndex(index), multiCorrespondenceIndex(index) );
   }
+  /** See parent-class (no need to overload) */ 
+  // edited Steffen Urban / This is still wrong wrong!!!
+  virtual points_t getPoints() const
+  {
+	  return getPoints();
+  }
   /** See parent-class (no need to overload) */
   virtual size_t getNumberCorrespondences() const
   {

include/opengv/absolute_pose/CentralAbsoluteAdapter.hpp

@@ -73,22 +73,25 @@ class CentralAbsoluteAdapter : public AbsoluteAdapterBase
    */
   CentralAbsoluteAdapter(
       const bearingVectors_t & bearingVectors,
-      const points_t & points );
+	  const points_t & points,
+	  const cov3_mats_t & covMats = cov3_mats_t(1));
   /**
    * \brief Constructor. See protected class-members to understand parameters
    */
   CentralAbsoluteAdapter(
       const bearingVectors_t & bearingVectors,
       const points_t & points,
-      const rotation_t & R );
+      const rotation_t & R,
+	  const cov3_mats_t & covMats = cov3_mats_t(1));
   /**
    * \brief Constructor. See protected class-members to understand parameters
    */
   CentralAbsoluteAdapter(
       const bearingVectors_t & bearingVectors,
       const points_t & points,
       const translation_t & t,
-      const rotation_t & R );
+	  const rotation_t & R,
+	  const cov3_mats_t & covMats = cov3_mats_t(1));
   /**
    * Destructor
    */
@@ -97,7 +100,13 @@ class CentralAbsoluteAdapter : public AbsoluteAdapterBase
   //Access of correspondences
 
   /** See parent-class */
-  virtual opengv::bearingVector_t getBearingVector( size_t index ) const;
+  virtual opengv::bearingVector_t getBearingVector(size_t index) const;
+  /** See parent-class */
+  virtual opengv::bearingVectors_t getBearingVectors() const;
+  /** See parent-class */
+  virtual opengv::cov3_mat_t getCovariance(size_t index) const;
+  /** See parent-class */
+  virtual opengv::cov3_mats_t getCovariances() const;
   /** See parent-class */
   virtual double getWeight( size_t index ) const;
   /** See parent-class. Returns zero for this adapter. */
@@ -107,11 +116,15 @@ class CentralAbsoluteAdapter : public AbsoluteAdapterBase
   /** See parent-class */
   virtual opengv::point_t getPoint( size_t index ) const;
   /** See parent-class */
+  virtual opengv::points_t getPoints() const;
+  /** See parent-class */
   virtual size_t getNumberCorrespondences() const;
 
 protected:
   /** Reference to the bearing-vectors expressed in the camera-frame */
   const bearingVectors_t & _bearingVectors;
+  /** Reference to the covariance-matrix related to the bearing vector */
+  const cov3_mats_t & _cov_mats;
   /** Reference to the points expressed in the world-frame. */
   const points_t & _points;
 };

include/opengv/absolute_pose/MACentralAbsolute.hpp

@@ -76,7 +76,18 @@ class MACentralAbsolute : public AbsoluteAdapterBase
       const double * bearingVectors,
       int numberPoints,
       int numberBearingVectors );
-
+  /**
+  * \brief Constructor. See protected class-members to understand parameters
+  */
+  // edited by Steffen Urban / [email protected]
+  MACentralAbsolute(
+	  const double * points,
+	  const double * bearingVectors,
+	  const double * covMats,
+	  int numberPoints,
+	  int numberBearingVectors,
+	  int numberCovMats);
+
   /**
    * Destructor
    */
@@ -87,6 +98,12 @@ class MACentralAbsolute : public AbsoluteAdapterBase
   /** See parent-class */
   virtual opengv::bearingVector_t getBearingVector( size_t index ) const;
   /** See parent-class */
+  virtual opengv::bearingVectors_t getBearingVectors() const;
+  /** See parent-class */
+  virtual opengv::cov3_mat_t getCovariance(size_t index) const;
+  /** See parent-class */
+  virtual opengv::cov3_mats_t getCovariances() const;
+  /** See parent-class */
   virtual double getWeight( size_t index ) const;
   /** See parent-class. Returns zero for this adapter. */
   virtual opengv::translation_t getCamOffset( size_t index ) const;
@@ -95,18 +112,24 @@ class MACentralAbsolute : public AbsoluteAdapterBase
   /** See parent-class */
   virtual opengv::point_t getPoint( size_t index ) const;
   /** See parent-class */
+  virtual opengv::points_t getPoints() const;
+  /** See parent-class */
   virtual size_t getNumberCorrespondences() const;
 
 protected:
 
-  /** A pointer to the point data */  
+  /** A pointer to the point data */
   const double * _points;
   /** A pointer to the bearing-vectors */
   const double * _bearingVectors;
+  /** A pointer to the covariance-vectors */
+  const double * _covMats;
   /** The number of points */
   int _numberPoints;
   /** The number of bearing vectors */
   int _numberBearingVectors;
+  /** The number of covariance matrices */
+  int _numberCovMats;
 };
 
 }

include/opengv/absolute_pose/MANoncentralAbsolute.hpp

@@ -76,7 +76,17 @@ class MANoncentralAbsolute : public AbsoluteAdapterBase
       const double * bearingVectors,
       int numberPoints,
       int numberBearingVectors );
-
+  /**
+  * \brief Constructor. See protected class-members to understand parameters
+  */
+  // edited by Steffen Urban / [email protected]
+  MANoncentralAbsolute(
+	  const double * points,
+	  const double * bearingVectors,
+	  const double * covMats,
+	  int numberPoints,
+	  int numberBearingVectors,
+	  int numberCovMats);
   /**
    * Destructor
    */
@@ -85,7 +95,13 @@ class MANoncentralAbsolute : public AbsoluteAdapterBase
   //Access of correspondences
 
   /** See parent-class */
-  virtual opengv::bearingVector_t getBearingVector( size_t index ) const;
+  virtual opengv::bearingVector_t getBearingVector(size_t index) const;
+  /** See parent-class */
+  virtual opengv::bearingVectors_t getBearingVectors() const;
+  /** See parent-class */
+  virtual opengv::cov3_mat_t getCovariance(size_t index) const;
+  /** See parent-class */
+  virtual opengv::cov3_mats_t getCovariances() const;
   /** See parent-class */
   virtual double getWeight( size_t index ) const;
   /** See parent-class */
@@ -95,18 +111,24 @@ class MANoncentralAbsolute : public AbsoluteAdapterBase
   /** See parent-class */
   virtual opengv::point_t getPoint( size_t index ) const;
   /** See parent-class */
+  virtual opengv::points_t getPoints() const;
+  /** See parent-class */
   virtual size_t getNumberCorrespondences() const;
 
 protected:
 
-  /** A pointer to the point data */  
+  /** A pointer to the point data */
   const double * _points;
   /** A pointer to the bearing-vectors */
   const double * _bearingVectors;
+  /** A pointer to the covariance-vectors */
+  const double * _covMats;
   /** The number of points */
   int _numberPoints;
   /** The number of bearing vectors */
   int _numberBearingVectors;
+  /** The number of covariance matrices */
+  int _numberCovMats;
 };
 
 }