v01

thirdparty/opengv/test/experiment_helpers.cpp

@@ -0,0 +1,591 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 "experiment_helpers.hpp"
+#include "random_generators.hpp"
+#include <opengv/math/cayley.hpp>
+#include <iostream>
+#include <iomanip>
+
+
+void
+opengv::generateCentralCameraSystem(
+    translations_t & camOffsets,
+    rotations_t & camRotations )
+{
+  //create a fake camera system for a central camera
+  camOffsets.push_back(Eigen::Vector3d::Zero());
+  camRotations.push_back(Eigen::Matrix3d::Identity());
+}
+
+void
+opengv::generateRandomCameraSystem(
+    int numberCameras,
+    translations_t & camOffsets,
+    rotations_t & camRotations )
+{
+  double offset = 0.5; //this is the distance from the viewpoint origin
+
+  for( int i = 0; i < numberCameras; i++ )
+  {
+    translation_t camOffset = generateRandomDirectionTranslation(offset);
+    rotation_t camRotation = generateRandomRotation();
+    camOffsets.push_back(camOffset);
+    camRotations.push_back(camRotation);
+  }
+}
+
+void
+opengv::extractRelativePose(
+    const translation_t & position1,
+    const translation_t & position2,
+    const rotation_t & rotation1,
+    const rotation_t & rotation2,
+    translation_t & relativePosition,
+    rotation_t & relativeRotation,
+    bool normalize )
+{
+  relativeRotation = rotation1.transpose() * rotation2;
+  relativePosition = rotation1.transpose() * (position2 - position1);
+  if(normalize)
+    relativePosition = relativePosition / relativePosition.norm();
+}
+
+void
+opengv::printExperimentCharacteristics(
+    const translation_t & position,
+    const rotation_t & rotation,
+    double noise,
+    double outlierFraction )
+{
+  std::cout << "the random position is:" << std::endl;
+  std::cout << position << std::endl << std::endl;
+  std::cout << "the random rotation is:" << std::endl;
+  std::cout << rotation << std::endl << std::endl;
+  std::cout << "the noise in the data is:" << std::endl;
+  std::cout << noise << std::endl;
+  std::cout << "the outlier fraction is:" << std::endl;
+  std::cout << outlierFraction << std::endl;
+}
+
+void
+opengv::printBearingVectorArraysMatlab(
+    const bearingVectors_t & bearingVectors1,
+    const bearingVectors_t & bearingVectors2 )
+{
+  size_t numberBearingVectors = bearingVectors1.size();
+
+  //temp: print the vectors in matlab format (for debugging purposes)
+  size_t precision = 10;
+  std::cout << "ov1 = [";
+  for( size_t i = 0; i < numberBearingVectors; i++ )
+    std::cout << std::setprecision(precision) << bearingVectors1[i](0,0) << " ";
+  std::cout << ";" << std::endl;
+  for( size_t i = 0; i < 8; i++ )
+    std::cout << std::setprecision(precision) << bearingVectors1[i](1,0) << " ";
+  std::cout << ";" << std::endl;
+  for( size_t i = 0; i < 8; i++ )
+    std::cout << std::setprecision(precision) << bearingVectors1[i](2,0) << " ";
+  std::cout << "];" << std::endl;
+  std::cout << "ov2 = [";
+  for( size_t i = 0; i < 8; i++ )
+    std::cout << std::setprecision(precision) << bearingVectors2[i](0,0) << " ";
+  std::cout << ";" << std::endl;
+  for( size_t i = 0; i < 8; i++ )
+    std::cout << std::setprecision(precision) << bearingVectors2[i](1,0) << " ";
+  std::cout << ";" << std::endl;
+  for( size_t i = 0; i < 8; i++ )
+    std::cout << std::setprecision(precision) << bearingVectors2[i](2,0) << " ";
+  std::cout << "];" << std::endl;
+}
+
+void
+opengv::printEssentialMatrix(
+    const translation_t & position,
+    const rotation_t & rotation)
+{
+  //E transforms vectors from vp 2 to 1: x_1^T * E * x_2 = 0
+  //and E = (t)_skew*R
+  Eigen::Matrix3d t_skew = Eigen::Matrix3d::Zero();
+  t_skew(0,1) = -position[2];
+  t_skew(0,2) = position[1];
+  t_skew(1,0) = position[2];
+  t_skew(1,2) = -position[0];
+  t_skew(2,0) = -position[1];
+  t_skew(2,1) = position[0];
+
+  essential_t E = t_skew * rotation;
+  double matrixNorm = 0.0;
+  for(size_t r = 0; r < 3; r++)
+  {
+    for(size_t c = 0; c < 3; c++)
+      matrixNorm += pow(E(r,c),2);
+  }
+  matrixNorm = sqrt(matrixNorm);
+  for(size_t r = 0; r < 3; r++)
+  {
+    for(size_t c = 0; c < 3; c++)
+      E(r,c) = E(r,c) / matrixNorm;
+  }
+
+  std::cout << "the random essential matrix is:" << std::endl;
+  std::cout << E << std::endl;
+}
+
+void
+opengv::getPerturbedPose(
+    const translation_t & position,
+    const rotation_t & rotation,
+    translation_t & perturbedPosition,
+    rotation_t & perturbedRotation,
+    double amplitude )
+{
+  perturbedPosition = position;
+  cayley_t cayley = math::rot2cayley(rotation);
+  for( size_t i = 0; i < 3; i++ )
+  {
+    perturbedPosition[i] =
+        perturbedPosition[i] +
+        (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*amplitude;
+    cayley[i] =
+        cayley[i] + (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*amplitude;
+  }
+  perturbedRotation = math::cayley2rot(cayley);
+}
+
+std::vector<int>
+opengv::getNindices( int n )
+{
+  std::vector<int> indices;
+  for(int i = 0; i < n; i++)
+    indices.push_back(i);
+  return indices;
+}
+
+
+
+
+
+void
+opengv::generateRandom2D3DCorrespondences(
+    const translation_t & position,
+    const rotation_t & rotation,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t numberPoints,
+    double noise,
+    double outlierFraction,
+    bearingVectors_t & bearingVectors,
+    points_t & points,
+    std::vector<int> & camCorrespondences,
+    Eigen::MatrixXd & gt )
+{
+  //initialize point-cloud
+  double minDepth = 4;
+  double maxDepth = 8;
+
+  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
+    gt.col(i) = generateRandomPoint( maxDepth, minDepth );
+
+  //create the 2D3D-correspondences by looping through the cameras
+  size_t numberCams = camOffsets.size();
+  size_t camCorrespondence = 0;
+
+  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
+  {
+    //get the camera transformation
+    translation_t camOffset = camOffsets[camCorrespondence];
+    rotation_t camRotation = camRotations[camCorrespondence];
+
+    //store the point
+    points.push_back(gt.col(i));
+
+    //project the point into the viewpoint frame
+    point_t bodyPoint = rotation.transpose()*(gt.col(i) - position);
+
+    //project the point into the camera frame
+    bearingVectors.push_back(camRotation.transpose()*(bodyPoint - camOffset));
+
+    //normalize the bearing-vector to 1
+    bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
+
+    //add noise
+    if( noise > 0.0 )
+      bearingVectors[i] = addNoise(noise,bearingVectors[i]);
+
+    //push back the camera correspondence
+    camCorrespondences.push_back(camCorrespondence++);
+    if(camCorrespondence > (numberCams-1) )
+      camCorrespondence = 0;
+  }
+
+  //add outliers
+  //compute the number of outliers based on fraction
+  size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
+  //make the first numberOutliers points be outliers
+  for(size_t i = 0; i < numberOutliers; i++)
+  {
+    //extract the camera transformation
+    translation_t camOffset = camOffsets[camCorrespondences[i]];
+    rotation_t camRotation = camRotations[camCorrespondences[i]];
+
+    //create random point
+    point_t p = generateRandomPoint(8,4);
+
+    //project into viewpoint frame
+    point_t bodyPoint = rotation.transpose()*(p - position);
+
+    //project into camera-frame and use as outlier measurement
+    bearingVectors[i] = camRotation.transpose()*(bodyPoint - camOffset);
+
+    //normalize the bearing vector
+    bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
+  }
+}
+
+void
+opengv::generateMulti2D3DCorrespondences(
+    const translation_t & position,
+    const rotation_t & rotation,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t pointsPerCam,
+    double noise,
+    double outlierFraction,
+    std::vector<std::shared_ptr<points_t> > & multiPoints,
+    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors,
+    std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt )
+{
+  //initialize point-cloud
+  double minDepth = 4;
+  double maxDepth = 8;
+
+  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
+  {
+    std::shared_ptr<Eigen::MatrixXd> gt_sub(new Eigen::MatrixXd(3,pointsPerCam));
+    for( size_t i = 0; i < pointsPerCam; i++ )
+      gt_sub->col(i) = generateRandomPoint( maxDepth, minDepth );
+    gt.push_back(gt_sub);
+  }
+
+  //iterate through the cameras (pairs)
+  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
+  {
+    //create the bearing-vector arrays for this camera
+    std::shared_ptr<points_t> points(new points_t());
+    std::shared_ptr<bearingVectors_t> bearingVectors(new bearingVectors_t());
+
+    //get the offset and rotation of this camera
+    translation_t camOffset = camOffsets[cam];
+    rotation_t camRotation = camRotations[cam];
+
+    //now iterate through the points of that camera
+    for( size_t i = 0; i < (size_t) pointsPerCam; i++ )
+    {
+      points->push_back(gt[cam]->col(i));
+
+      //project the point into the viewpoint frame
+      point_t bodyPoint = rotation.transpose()*(gt[cam]->col(i) - position);
+
+      //project that point into the camera
+      bearingVectors->push_back( camRotation.transpose()*(bodyPoint - camOffset) );
+
+      //normalize the vector
+      (*bearingVectors)[i] = (*bearingVectors)[i] / (*bearingVectors)[i].norm();
+
+      //add noise
+      if( noise > 0.0 )
+        (*bearingVectors)[i] = addNoise(noise,(*bearingVectors)[i]);
+    }
+
+    //push back the stuff for this camera
+    multiPoints.push_back(points);
+    multiBearingVectors.push_back(bearingVectors);
+  }
+
+  //add outliers
+  size_t outliersPerCam = (size_t) floor(outlierFraction*pointsPerCam);
+
+  //iterate through the cameras
+  for(size_t cam = 0; cam < camOffsets.size(); cam++)
+  {
+    //get the offset and rotation of this camera
+    translation_t camOffset = camOffsets[cam];
+    rotation_t camRotation = camRotations[cam];
+
+    //add outliers
+    for(size_t i = 0; i < outliersPerCam; i++)
+    {
+      //generate a random point
+      point_t p = generateRandomPoint(8,4);
+
+      //transform that point into viewpoint 2 only
+      point_t bodyPoint = rotation.transpose()*(p - position);
+
+      //use as measurement (outlier)
+      (*(multiBearingVectors[cam].get()))[i] =
+          camRotation.transpose()*(bodyPoint - camOffset);
+
+      //normalize
+      (*(multiBearingVectors[cam].get()))[i] =
+          (*(multiBearingVectors[cam].get()))[i] / (*(multiBearingVectors[cam].get()))[i].norm();
+    }
+  }
+}
+
+void
+opengv::generateRandom2D2DCorrespondences(
+    const translation_t & position1,
+    const rotation_t & rotation1,
+    const translation_t & position2,
+    const rotation_t & rotation2,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t numberPoints,
+    double noise,
+    double outlierFraction,
+    bearingVectors_t & bearingVectors1,
+    bearingVectors_t & bearingVectors2,
+    std::vector<int> & camCorrespondences1,
+    std::vector<int> & camCorrespondences2,
+    Eigen::MatrixXd & gt )
+{
+  //initialize point-cloud
+  double minDepth = 4;
+  double maxDepth = 8;
+
+  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
+    gt.col(i) = generateRandomPoint( maxDepth, minDepth );
+
+  //create the 2D3D-correspondences by looping through the cameras
+  size_t numberCams = camOffsets.size();
+  size_t camCorrespondence = 0;
+
+  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
+  {
+    //get the camera transformation
+    translation_t camOffset = camOffsets[camCorrespondence];
+    rotation_t camRotation = camRotations[camCorrespondence];
+
+    //get the point in viewpoint 1
+    point_t bodyPoint1 = rotation1.transpose()*(gt.col(i) - position1);
+
+    //get the point in viewpoint 2
+    point_t bodyPoint2 = rotation2.transpose()*(gt.col(i) - position2);
+
+    //get the point in the camera in viewpoint 1
+    bearingVectors1.push_back(camRotation.transpose()*(bodyPoint1 - camOffset));
+
+    //get the point in the camera in viewpoint 2
+    bearingVectors2.push_back(camRotation.transpose()*(bodyPoint2 - camOffset));
+
+    //normalize the bearing-vectors
+    bearingVectors1[i] = bearingVectors1[i] / bearingVectors1[i].norm();
+    bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();
+
+    //add noise
+    if( noise > 0.0 )
+    {
+      bearingVectors1[i] = addNoise(noise,bearingVectors1[i]);
+      bearingVectors2[i] = addNoise(noise,bearingVectors2[i]);
+    }
+
+    //push back the camera correspondences
+    camCorrespondences1.push_back(camCorrespondence);
+    camCorrespondences2.push_back(camCorrespondence++);
+    if(camCorrespondence > (numberCams - 1) )
+      camCorrespondence = 0;
+  }
+
+  //add outliers
+  size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
+  for(size_t i = 0; i < numberOutliers; i++)
+  {
+    //get the corresponding camera transformation
+    translation_t camOffset = camOffsets[camCorrespondence];
+    rotation_t camRotation = camRotations[camCorrespondence];
+
+    //create random point
+    point_t p = generateRandomPoint(8,4);
+
+    //project this point into viewpoint 2
+    point_t bodyPoint = rotation2.transpose()*(p - position2);
+
+    //project this point into the corresponding camera in viewpoint 2
+    //and use as outlier
+    bearingVectors2[i] = camRotation.transpose()*(bodyPoint - camOffset);
+
+    //normalize the bearing vector
+    bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();
+  }
+}
+
+void
+opengv::generateRandom3D3DCorrespondences(
+    const translation_t & position1,
+    const rotation_t & rotation1,
+    const translation_t & position2,
+    const rotation_t & rotation2,
+    size_t numberPoints,
+    double noise,
+    double outlierFraction,
+    bearingVectors_t & points1,
+    bearingVectors_t & points2,
+    Eigen::MatrixXd & gt )
+{
+  //initialize point-cloud
+  double minDepth = 4;
+  double maxDepth = 8;
+
+  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
+    gt.col(i) = generateRandomPoint( maxDepth, minDepth );
+
+  //create the 3D-3D correspondences
+  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
+  {
+    //transform the points into the frames and store
+    points1.push_back(rotation1.transpose()*(gt.col(i) - position1));
+    points2.push_back(rotation2.transpose()*(gt.col(i) - position2));
+
+    //add noise
+    if( noise > 0.0 )
+    {
+      points1[i] = points1[i] + generateRandomTranslation(noise);
+      points2[i] = points2[i] + generateRandomTranslation(noise);
+    }
+  }
+
+  //add outliers
+  size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
+  for(size_t i = 0; i < numberOutliers; i++)
+  {
+    //generate a random point
+    point_t p = generateRandomPoint(8,4);
+
+    //push-back in frame 2 only to create outlier
+    points2[i] = rotation2.transpose()*(p - position2);
+  }
+}
+
+void
+opengv::generateMulti2D2DCorrespondences(
+    const translation_t & position1,
+    const rotation_t & rotation1,
+    const translation_t & position2,
+    const rotation_t & rotation2,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t pointsPerCam,
+    double noise,
+    double outlierFraction,
+    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors1,
+    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors2,
+    std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt )
+{
+  //initialize point-cloud
+  double minDepth = 4;
+  double maxDepth = 8;
+
+  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
+  {
+    std::shared_ptr<Eigen::MatrixXd> gt_sub(new Eigen::MatrixXd(3,pointsPerCam));
+    for( size_t i = 0; i < pointsPerCam; i++ )
+      gt_sub->col(i) = generateRandomPoint( maxDepth, minDepth );
+    gt.push_back(gt_sub);
+  }
+
+  //iterate through the cameras (pairs)
+  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
+  {
+    //create the bearing-vector arrays for this camera
+    std::shared_ptr<bearingVectors_t> bearingVectors1(new bearingVectors_t());
+    std::shared_ptr<bearingVectors_t> bearingVectors2(new bearingVectors_t());
+
+    //get the offset and rotation of this camera
+    translation_t camOffset = camOffsets[cam];
+    rotation_t camRotation = camRotations[cam];
+
+    //now iterate through the points of that camera
+    for( size_t i = 0; i < (size_t) pointsPerCam; i++ )
+    {
+      //project a point into both viewpoint frames
+      point_t bodyPoint1 = rotation1.transpose()*(gt[cam]->col(i) - position1);
+      point_t bodyPoint2 = rotation2.transpose()*(gt[cam]->col(i) - position2);
+
+      //project that point into the cameras
+      bearingVectors1->push_back( camRotation.transpose()*(bodyPoint1 - camOffset) );
+      bearingVectors2->push_back( camRotation.transpose()*(bodyPoint2 - camOffset) );
+
+      //normalize the vectors
+      (*bearingVectors1)[i] = (*bearingVectors1)[i] / (*bearingVectors1)[i].norm();
+      (*bearingVectors2)[i] = (*bearingVectors2)[i] / (*bearingVectors2)[i].norm();
+
+      //add noise
+      if( noise > 0.0 )
+      {
+        (*bearingVectors1)[i] = addNoise(noise,(*bearingVectors1)[i]);
+        (*bearingVectors2)[i] = addNoise(noise,(*bearingVectors2)[i]);
+      }
+    }
+
+    //push back the stuff for this camera
+    multiBearingVectors1.push_back(bearingVectors1);
+    multiBearingVectors2.push_back(bearingVectors2);
+  }
+
+  //add outliers
+  size_t outliersPerCam = (size_t) floor(outlierFraction*pointsPerCam);
+
+  //iterate through the cameras
+  for(size_t cam = 0; cam < camOffsets.size(); cam++)
+  {
+    //get the offset and rotation of this camera
+    translation_t camOffset = camOffsets[cam];
+    rotation_t camRotation = camRotations[cam];
+
+    //add outliers
+    for(size_t i = 0; i < outliersPerCam; i++)
+    {
+      //generate a random point
+      point_t p = generateRandomPoint(8,4);
+
+      //transform that point into viewpoint 2 only
+      point_t bodyPoint = rotation2.transpose()*(p - position2);
+
+      //use as measurement (outlier)
+      (*multiBearingVectors2[cam])[i] =
+          camRotation.transpose()*(bodyPoint - camOffset);
+
+      //normalize
+      (*multiBearingVectors2[cam])[i] =
+          (*multiBearingVectors2[cam])[i] / (*multiBearingVectors2[cam])[i].norm();
+    }
+  }
+}

thirdparty/opengv/test/experiment_helpers.hpp

@@ -0,0 +1,153 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 OPENGV_EXPERIMENT_HELPERS_HPP_
+#define OPENGV_EXPERIMENT_HELPERS_HPP_
+
+#include <opengv/types.hpp>
+#include <memory>
+
+namespace opengv
+{
+
+void generateCentralCameraSystem(
+    translations_t & camOffsets,
+    rotations_t & camRotations );
+
+void generateRandomCameraSystem(
+    int numberCameras,
+    translations_t & camOffsets,
+    rotations_t & camRotations );
+
+void extractRelativePose(
+    const translation_t & position1,
+    const translation_t & position2,
+    const rotation_t & rotation1,
+    const rotation_t & rotation2,
+    translation_t & relativePosition,
+    rotation_t & relativeRotation,
+    bool normalize = true );
+
+void printExperimentCharacteristics(
+    const translation_t & position,
+    const rotation_t & rotation,
+    double noise,
+    double outlierFraction );
+
+void printBearingVectorArraysMatlab(
+    const bearingVectors_t & bearingVectors1,
+    const bearingVectors_t & bearingVectors2 );
+
+void printEssentialMatrix(
+    const translation_t & position,
+    const rotation_t & rotation);
+
+void getPerturbedPose(
+    const translation_t & position,
+    const rotation_t & rotation,
+    translation_t & perturbedPosition,
+    rotation_t & perturbedRotation,
+    double amplitude );
+
+std::vector<int> getNindices( int n );
+
+
+
+
+void generateRandom2D3DCorrespondences(
+    const translation_t & position,
+    const rotation_t & rotation,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t numberPoints,
+    double noise,
+    double outlierFraction,
+    bearingVectors_t & bearingVectors,
+    points_t & points,
+    std::vector<int> & camCorrespondences,
+    Eigen::MatrixXd & gt );
+
+void generateMulti2D3DCorrespondences(
+    const translation_t & position,
+    const rotation_t & rotation,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t pointsPerCam,
+    double noise,
+    double outlierFraction,
+    std::vector<std::shared_ptr<points_t> > & multiPoints,
+    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors,
+    std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt );
+
+void generateRandom2D2DCorrespondences(
+    const translation_t & position1,
+    const rotation_t & rotation1,
+    const translation_t & position2,
+    const rotation_t & rotation2,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t numberPoints,
+    double noise,
+    double outlierFraction,
+    bearingVectors_t & bearingVectors1,
+    bearingVectors_t & bearingVectors2,
+    std::vector<int> & camCorrespondences1,
+    std::vector<int> & camCorrespondences2,
+    Eigen::MatrixXd & gt );
+
+void generateRandom3D3DCorrespondences(
+    const translation_t & position1,
+    const rotation_t & rotation1,
+    const translation_t & position2,
+    const rotation_t & rotation2,
+    size_t numberPoints,
+    double noise,
+    double outlierFraction,
+    bearingVectors_t & points1,
+    bearingVectors_t & points2,
+    Eigen::MatrixXd & gt );
+
+void generateMulti2D2DCorrespondences(
+    const translation_t & position1,
+    const rotation_t & rotation1,
+    const translation_t & position2,
+    const rotation_t & rotation2,
+    const translations_t & camOffsets,
+    const rotations_t & camRotations,
+    size_t pointsPerCam,
+    double noise,
+    double outlierFraction,
+    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors1,
+    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors2,
+    std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt );
+
+}
+
+#endif /* OPENGV_EXPERIMENT_HELPERS_HPP_ */

thirdparty/opengv/test/random_generators.cpp

@@ -0,0 +1,251 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 "random_generators.hpp"
+#include "time_measurement.hpp"
+#include <math.h>
+#include <iostream>
+
+
+using namespace Eigen;
+
+void
+opengv::initializeRandomSeed()
+{
+  struct timeval tic;
+  gettimeofday( &tic, 0 );
+  srand ( tic.tv_usec );
+}
+
+Eigen::Vector3d
+opengv::generateRandomPoint( double maximumDepth, double minimumDepth )
+{
+  Eigen::Vector3d cleanPoint;
+  cleanPoint[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  cleanPoint[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  cleanPoint[2] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  Eigen::Vector3d direction = cleanPoint / cleanPoint.norm();
+  cleanPoint =
+      (maximumDepth-minimumDepth) * cleanPoint + minimumDepth * direction;
+  return cleanPoint;
+}
+
+Eigen::Vector3d
+opengv::generateRandomPointPlane()
+{
+  Eigen::Vector3d cleanPoint;
+  cleanPoint[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  cleanPoint[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  cleanPoint[2] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+
+  cleanPoint[0] = 6*cleanPoint[0];
+  cleanPoint[1] = 6*cleanPoint[1];
+  cleanPoint[2] = 2*cleanPoint[2]-6.0;
+
+  return cleanPoint;
+}
+
+Eigen::Vector3d
+opengv::addNoise( double noiseLevel, Eigen::Vector3d cleanPoint )
+{
+  //compute a vector in the normal plane (based on good conditioning)
+  Eigen::Vector3d normalVector1;
+  if(
+      (fabs(cleanPoint[0]) > fabs(cleanPoint[1])) &&
+      (fabs(cleanPoint[0]) > fabs(cleanPoint[2])) )
+  {
+    normalVector1[1] = 1.0;
+    normalVector1[2] = 0.0;
+    normalVector1[0] = -cleanPoint[1]/cleanPoint[0];
+  }
+  else
+  {
+    if(
+        (fabs(cleanPoint[1]) > fabs(cleanPoint[0])) &&
+        (fabs(cleanPoint[1]) > fabs(cleanPoint[2])) )
+    {
+      normalVector1[2] = 1.0;
+      normalVector1[0] = 0.0;
+      normalVector1[1] = -cleanPoint[2]/cleanPoint[1];
+    }
+    else
+    {
+      normalVector1[0] = 1.0;
+      normalVector1[1] = 0.0;
+      normalVector1[2] = -cleanPoint[0]/cleanPoint[2];
+    }
+  }
+
+  normalVector1 = normalVector1 / normalVector1.norm();
+  Eigen::Vector3d normalVector2 = cleanPoint.cross(normalVector1);
+  double noiseX =
+      noiseLevel * (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0 / 1.4142;
+  double noiseY =
+      noiseLevel * (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0 / 1.4142;
+
+  Eigen::Vector3d noisyPoint =
+      800 * cleanPoint + noiseX *normalVector1 + noiseY * normalVector2;
+  noisyPoint = noisyPoint / noisyPoint.norm();
+  return noisyPoint;
+
+}
+
+Eigen::Vector3d
+opengv::generateRandomTranslation( double maximumParallax )
+{
+  Eigen::Vector3d translation;
+  translation[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  translation[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  translation[2] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
+  return maximumParallax * translation;
+}
+
+Eigen::Vector3d
+opengv::generateRandomDirectionTranslation( double parallax )
+{
+  Eigen::Matrix3d rotation = generateRandomRotation();
+  Eigen::Vector3d translation;
+  translation << 1.0, 0.0, 0.0;
+  translation = rotation * translation;
+  translation = parallax * translation;
+  return translation;
+}
+
+Eigen::Matrix3d
+opengv::generateRandomRotation( double maxAngle )
+{
+  Eigen::Vector3d rpy;
+  rpy[0] = ((double) rand())/ ((double) RAND_MAX);
+  rpy[1] = ((double) rand())/ ((double) RAND_MAX);
+  rpy[2] = ((double) rand())/ ((double) RAND_MAX);
+
+  rpy[0] = maxAngle*2.0*(rpy[0]-0.5);
+  rpy[1] = maxAngle*2.0*(rpy[1]-0.5);
+  rpy[2] = maxAngle*2.0*(rpy[2]-0.5);
+
+  Eigen::Matrix3d R1;
+  R1(0,0) = 1.0;
+  R1(0,1) = 0.0;
+  R1(0,2) = 0.0;
+  R1(1,0) = 0.0;
+  R1(1,1) = cos(rpy[0]);
+  R1(1,2) = -sin(rpy[0]);
+  R1(2,0) = 0.0;
+  R1(2,1) = -R1(1,2);
+  R1(2,2) = R1(1,1);
+
+  Eigen::Matrix3d R2;
+  R2(0,0) = cos(rpy[1]);
+  R2(0,1) = 0.0;
+  R2(0,2) = sin(rpy[1]);
+  R2(1,0) = 0.0;
+  R2(1,1) = 1.0;
+  R2(1,2) = 0.0;
+  R2(2,0) = -R2(0,2);
+  R2(2,1) = 0.0;
+  R2(2,2) = R2(0,0);
+
+  Eigen::Matrix3d R3;
+  R3(0,0) = cos(rpy[2]);
+  R3(0,1) = -sin(rpy[2]);
+  R3(0,2) = 0.0;
+  R3(1,0) =-R3(0,1);
+  R3(1,1) = R3(0,0);
+  R3(1,2) = 0.0;
+  R3(2,0) = 0.0;
+  R3(2,1) = 0.0;
+  R3(2,2) = 1.0;
+
+  Eigen::Matrix3d rotation = R3 * R2 * R1;
+
+  rotation.col(0) = rotation.col(0) / rotation.col(0).norm();
+  rotation.col(2) = rotation.col(0).cross(rotation.col(1));
+  rotation.col(2) = rotation.col(2) / rotation.col(2).norm();
+  rotation.col(1) = rotation.col(2).cross(rotation.col(0));
+  rotation.col(1) = rotation.col(1) / rotation.col(1).norm();
+
+  return rotation;
+}
+
+Eigen::Matrix3d
+opengv::generateRandomRotation()
+{
+  Eigen::Vector3d rpy;
+  rpy[0] = ((double) rand())/ ((double) RAND_MAX);
+  rpy[1] = ((double) rand())/ ((double) RAND_MAX);
+  rpy[2] = ((double) rand())/ ((double) RAND_MAX);
+
+  rpy[0] = 2*M_PI*(rpy[0]-0.5);
+  rpy[1] = M_PI*(rpy[1]-0.5);
+  rpy[2] = 2*M_PI*(rpy[2]-0.5);
+
+  Eigen::Matrix3d R1;
+  R1(0,0) = 1.0;
+  R1(0,1) = 0.0;
+  R1(0,2) = 0.0;
+  R1(1,0) = 0.0;
+  R1(1,1) = cos(rpy[0]);
+  R1(1,2) = -sin(rpy[0]);
+  R1(2,0) = 0.0;
+  R1(2,1) = -R1(1,2);
+  R1(2,2) = R1(1,1);
+
+  Eigen::Matrix3d R2;
+  R2(0,0) = cos(rpy[1]);
+  R2(0,1) = 0.0;
+  R2(0,2) = sin(rpy[1]);
+  R2(1,0) = 0.0;
+  R2(1,1) = 1.0;
+  R2(1,2) = 0.0;
+  R2(2,0) = -R2(0,2);
+  R2(2,1) = 0.0;
+  R2(2,2) = R2(0,0);
+
+  Eigen::Matrix3d R3;
+  R3(0,0) = cos(rpy[2]);
+  R3(0,1) = -sin(rpy[2]);
+  R3(0,2) = 0.0;
+  R3(1,0) =-R3(0,1);
+  R3(1,1) = R3(0,0);
+  R3(1,2) = 0.0;
+  R3(2,0) = 0.0;
+  R3(2,1) = 0.0;
+  R3(2,2) = 1.0;
+
+  Eigen::Matrix3d rotation = R3 * R2 * R1;
+
+  rotation.col(0) = rotation.col(0) / rotation.col(0).norm();
+  rotation.col(2) = rotation.col(0).cross(rotation.col(1));
+  rotation.col(2) = rotation.col(2) / rotation.col(2).norm();
+  rotation.col(1) = rotation.col(2).cross(rotation.col(0));
+  rotation.col(1) = rotation.col(1) / rotation.col(1).norm();
+
+  return rotation;
+}

thirdparty/opengv/test/random_generators.hpp

@@ -0,0 +1,52 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 OPENGV_RANDOM_GENERATORS_HPP_
+#define OPENGV_RANDOM_GENERATORS_HPP_
+
+#include <stdlib.h>
+#include <vector>
+#include <Eigen/Eigen>
+
+namespace opengv
+{
+
+void initializeRandomSeed();
+Eigen::Vector3d generateRandomPoint( double maximumDepth, double minimumDepth );
+Eigen::Vector3d generateRandomPointPlane();
+Eigen::Vector3d addNoise( double noiseLevel, Eigen::Vector3d cleanPoint );
+Eigen::Vector3d generateRandomTranslation( double maximumParallax );
+Eigen::Vector3d generateRandomDirectionTranslation( double parallax );
+Eigen::Matrix3d generateRandomRotation( double maxAngle );
+Eigen::Matrix3d generateRandomRotation();
+
+}
+
+#endif /* OPENGV_RANDOM_GENERATORS_HPP_ */

thirdparty/opengv/test/test_Sturm.cpp

@@ -0,0 +1,145 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <opengv/math/Sturm.hpp>
+
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  std::vector<double> coeffs;
+  coeffs.push_back(1.0);
+  coeffs.push_back(4.0);
+  coeffs.push_back(1.0);
+  coeffs.push_back(-6.0);
+  
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 50;
+  
+  //for now just construct the problem
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    math::Sturm sturm(coeffs);
+  gettimeofday( &toc, 0 );
+  double time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "the initialization takes " << time << " seconds" << std::endl;
+  
+  math::Sturm sturm(coeffs);
+  
+  //test the lagrangian bounds
+  gettimeofday( &tic, 0 );
+  double bound;
+  for(size_t i = 0; i < iterations; i++)
+    bound = sturm.computeLagrangianBound();
+  gettimeofday( &toc, 0 );
+  time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "the initial bound computation takes " << time << " seconds" << std::endl;
+  std::cout << "the bound evaluates to " << bound << std::endl;
+  
+  //now evaluate the chain to verify that the number of roots is 3
+  gettimeofday( &tic, 0 );
+  size_t numberRoots;
+  for(size_t i = 0; i < iterations; i++)
+    numberRoots = sturm.evaluateChain(-bound) - sturm.evaluateChain(bound);
+  gettimeofday( &toc, 0 );
+  time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "the evaluation of two bounds takes " << time << " seconds" << std::endl;
+  std::cout << "the obtained number of roots is " << numberRoots << std::endl;
+  
+  //now test the bracketing mechanism
+  gettimeofday( &tic, 0 );
+  std::vector<double> roots;
+  for(size_t i = 0; i < iterations; i++)
+  {
+    roots.clear();
+    sturm.bracketRoots(roots,0.5);
+  }
+  gettimeofday( &toc, 0 );
+  time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "the bracketing of the roots took " << time << " seconds" << std::endl;
+  std::cout << "the obtained brackets are:" << std::endl;
+  std::vector<double>::iterator it = roots.begin();
+  while( it != roots.end() )
+  {
+    std::cout << "root:" << (*it) << std::endl; 
+    it++;
+  }
+  
+  //now test the entire root-finding mechanism
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    roots = sturm.findRoots();
+  gettimeofday( &toc, 0 );
+  time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "the entire finding of the roots took " << time << " seconds" << std::endl;
+  std::cout << "the obtained roots are:" << std::endl;
+  std::vector<double>::iterator it2 = roots.begin();
+  while( it2 != roots.end() )
+  {
+    std::cout << (*it2) << std::endl;
+    it2++;
+  }
+  
+  //now test the new root-finding with inbuild gradient descent
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+  {
+    roots.clear();
+    sturm.findRoots2(roots);
+  }
+  gettimeofday( &toc, 0 );
+  time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "the entire finding of the roots took " << time << " seconds" << std::endl;
+  std::cout << "the obtained roots are:" << std::endl;
+  it2 = roots.begin();
+  while( it2 != roots.end() )
+  {
+    std::cout << (*it2) << std::endl;
+    it2++;
+  }
+  
+  return 0;
+}

thirdparty/opengv/test/test_absolute_pose.cpp

@@ -0,0 +1,210 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/absolute_pose/methods.hpp>
+#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
+#include <opengv/math/cayley.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  //initialize random seed
+  initializeRandomSeed();
+  
+  //set experiment parameters
+  double noise = 0.0;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 100;
+
+  //create a random viewpoint pose
+  translation_t position = generateRandomTranslation(2.0);
+  rotation_t rotation = generateRandomRotation(0.5);
+  
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+  
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors;
+  points_t points;
+  std::vector<int> camCorrespondences; //unused in the central case!
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D3DCorrespondences(
+      position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors, points, camCorrespondences, gt );
+
+  //print the experiment characteristics
+  printExperimentCharacteristics(
+      position, rotation, noise, outlierFraction );
+
+  //create a central absolute adapter
+  absolute_pose::CentralAbsoluteAdapter adapter(
+      bearingVectors,
+      points,
+      rotation );
+
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 50;
+
+  //run the experiments
+  std::cout << "running Kneip's P2P (first two correspondences)" << std::endl;
+  translation_t p2p_translation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    p2p_translation = absolute_pose::p2p(adapter);
+  gettimeofday( &toc, 0 );
+  double p2p_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running Kneip's P3P (first three correspondences)" << std::endl;
+  transformations_t p3p_kneip_transformations;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    p3p_kneip_transformations = absolute_pose::p3p_kneip(adapter);
+  gettimeofday( &toc, 0 );
+  double p3p_kneip_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running Gao's P3P (first three correspondences)" << std::endl;
+  transformations_t p3p_gao_transformations;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    p3p_gao_transformations = absolute_pose::p3p_gao(adapter);
+  gettimeofday( &toc, 0 );
+  double p3p_gao_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running epnp (all correspondences)" << std::endl;
+  transformation_t epnp_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    epnp_transformation = absolute_pose::epnp(adapter);
+  gettimeofday( &toc, 0 );
+  double epnp_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running epnp with 6 correspondences" << std::endl;
+  std::vector<int> indices6 = getNindices(6);
+  transformation_t epnp_transformation_6 =
+      absolute_pose::epnp( adapter, indices6 );
+
+  std::cout << "running upnp with all correspondences" << std::endl;
+  transformations_t upnp_transformations;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    upnp_transformations = absolute_pose::upnp(adapter);
+  gettimeofday( &toc, 0 );
+  double upnp_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running upnp with 3 correspondences" << std::endl;
+  std::vector<int> indices3 = getNindices(3);
+  transformations_t upnp_transformations_3 =
+      absolute_pose::upnp( adapter, indices3 );
+
+  std::cout << "setting perturbed pose";
+  std::cout << "and performing nonlinear optimization" << std::endl;
+  //add a small perturbation to the pose
+  translation_t t_perturbed; rotation_t R_perturbed;
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1 );
+  transformation_t nonlinear_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+  {
+    adapter.sett(t_perturbed);
+    adapter.setR(R_perturbed);
+    nonlinear_transformation = absolute_pose::optimize_nonlinear(adapter);
+  }
+  gettimeofday( &toc, 0 );
+  double nonlinear_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose";
+  std::cout << "and performing nonlinear optimization with 10 correspondences";
+  std::cout << std::endl;
+  std::vector<int> indices10 = getNindices(10);
+  //add a small perturbation to the pose
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1 );
+  adapter.sett(t_perturbed);
+  adapter.setR(R_perturbed);
+  transformation_t nonlinear_transformation_10 =
+      absolute_pose::optimize_nonlinear(adapter,indices10);
+
+  //print the results
+  std::cout << "results from P2P algorithm:" << std::endl;
+  std::cout << p2p_translation << std::endl << std::endl;
+  std::cout << "results from Kneip's P3P algorithm:" << std::endl;
+  for(size_t i = 0; i < p3p_kneip_transformations.size(); i++)
+    std::cout << p3p_kneip_transformations[i] << std::endl << std::endl;
+  std::cout << "results from Gao's P3P algorithm:" << std::endl;
+  for(size_t i = 0; i < p3p_gao_transformations.size(); i++)
+    std::cout << p3p_gao_transformations[i] << std::endl << std::endl;
+  std::cout << "results from epnp algorithm:" << std::endl;
+  std::cout << epnp_transformation << std::endl << std::endl;
+  std::cout << "results from epnp algorithm with only 6 correspondences:";
+  std::cout << std::endl;
+  std::cout << epnp_transformation_6 << std::endl << std::endl;
+  std::cout << "results from upnp:" << std::endl;
+  for(size_t i = 0; i < upnp_transformations.size(); i++)
+    std::cout << upnp_transformations[i] << std::endl << std::endl;
+  std::cout << "results form upnp algorithm with only 3 correspondences:";
+  std::cout << std::endl;
+  for(size_t i = 0; i < upnp_transformations_3.size(); i++)
+    std::cout << upnp_transformations_3[i] << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm:" << std::endl;
+  std::cout << nonlinear_transformation << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm with only 10 correspondences:";
+  std::cout << std::endl;
+  std::cout << nonlinear_transformation_10 << std::endl << std::endl;
+
+  std::cout << "timings from P2P algorithm: ";
+  std::cout << p2p_time << std::endl;
+  std::cout << "timings from Kneip's P3P algorithm: ";
+  std::cout << p3p_kneip_time << std::endl;
+  std::cout << "timings from Gao's P3P algorithm: ";
+  std::cout << p3p_gao_time << std::endl;
+  std::cout << "timings from epnp algorithm: ";
+  std::cout << epnp_time << std::endl;
+  std::cout << "timings for the upnp algorithm: ";
+  std::cout << upnp_time << std::endl;
+  std::cout << "timings from nonlinear algorithm: ";
+  std::cout << nonlinear_time << std::endl;
+}

thirdparty/opengv/test/test_absolute_pose_sac.cpp

@@ -0,0 +1,147 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/absolute_pose/methods.hpp>
+#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac/Lmeds.hpp>
+#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  //initialize random seed
+  initializeRandomSeed();
+  
+  //set experiment parameters
+  double noise = 0.0;
+  double outlierFraction = 0.1;
+  size_t numberPoints = 100;
+
+  //create a random viewpoint pose
+  translation_t position = generateRandomTranslation(2.0);
+  rotation_t rotation = generateRandomRotation(0.5);
+  
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+  
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors;
+  points_t points;
+  std::vector<int> camCorrespondences; //unused in the central case!
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D3DCorrespondences(
+      position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors, points, camCorrespondences, gt );
+
+  //print the experiment characteristics
+  printExperimentCharacteristics(
+      position, rotation, noise, outlierFraction );
+
+  //create a central absolute adapter
+  absolute_pose::CentralAbsoluteAdapter adapter(
+      bearingVectors,
+      points,
+      rotation);
+
+  //Create an AbsolutePoseSac problem and Ransac
+  //The method can be set to KNEIP, GAO or EPNP
+  sac::Ransac<sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
+      new sac_problems::absolute_pose::AbsolutePoseSacProblem(
+      adapter,
+      sac_problems::absolute_pose::AbsolutePoseSacProblem::KNEIP));
+  ransac.sac_model_ = absposeproblem_ptr;
+  ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
+  ransac.max_iterations_ = 50;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel();
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << ransac.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    std::cout << ransac.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+
+  // Create LMedS
+  sac::Lmeds<sac_problems::absolute_pose::AbsolutePoseSacProblem> lmeds;
+  lmeds.sac_model_ = absposeproblem_ptr;
+  lmeds.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
+  lmeds.max_iterations_ = 50;
+
+  //Run the LMedS experiment
+  gettimeofday( &tic, 0 );
+  lmeds.computeModel();
+  gettimeofday( &toc, 0 );
+  double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the lmeds results is: " << std::endl;
+  std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Lmeds needed " << lmeds.iterations_ << " iterations and ";
+  std::cout << lmeds_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << lmeds.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < lmeds.inliers_.size(); i++)
+    std::cout << lmeds.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_eigensolver.cpp

@@ -0,0 +1,129 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.5;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 10;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomDirectionTranslation(0.0005);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1; //unused in the central case
+  std::vector<int> camCorrespondences2; //unused in the central case
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create a central relative adapter
+  relative_pose::CentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      rotation);
+
+  //run experiments
+  std::cout << "running eigensolver with perturbed rotation" << std::endl;
+  translation_t t_perturbed; rotation_t R_perturbed;
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.01);
+  adapter.setR12(R_perturbed);
+  rotation_t eigensolver_rotation;
+  eigensolverOutput_t output;
+  output.rotation = adapter.getR12(); //transferring the initial value
+  eigensolver_rotation = relative_pose::eigensolver(adapter,output);
+
+  //print results
+  std::cout << "results from eigensystem based rotation solver:" << std::endl;
+  std::cout << eigensolver_rotation << std::endl << std::endl;
+  std::cout << "the eigenvectors are: " << std::endl;
+  std::cout << output.eigenvectors << std::endl << std::endl;
+  std::cout << "the eigenvalues are: " << std::endl;
+  std::cout << output.eigenvalues << std::endl << std::endl;
+  std::cout << "the norms of the eigenvectors are: " << std::endl;
+  std::cout << output.eigenvectors.col(0).norm() << " ";
+  std::cout << output.eigenvectors.col(1).norm() << " ";
+  std::cout << output.eigenvectors.col(2).norm() << std::endl << std::endl;
+  std::cout << "the orthogongality is: " << std::endl;
+  std::cout << output.eigenvectors.col(0).transpose()*output.eigenvectors.col(1);
+  std::cout << " ";
+  std::cout << output.eigenvectors.col(0).transpose()*output.eigenvectors.col(2);
+  std::cout << " ";
+  std::cout << output.eigenvectors.col(1).transpose()*output.eigenvectors.col(2);
+  std::cout << std::endl << std::endl;
+  std::cout << "the norm of the translation is: " << std::endl;
+  std::cout << output.translation.norm() << std::endl;
+
+}

thirdparty/opengv/test/test_eigensolver_sac.cpp

@@ -0,0 +1,164 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac/Lmeds.hpp>
+#include <opengv/sac_problems/relative_pose/EigensolverSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.5;
+  double outlierFraction = 0.1;
+  size_t numberPoints = 100;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomDirectionTranslation(0.1);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1; //unused in the central case
+  std::vector<int> camCorrespondences2; //unused in the central case
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create a central relative adapter
+  relative_pose::CentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      rotation);
+
+  //Create an EigensolverSacProblem and Ransac
+  //The number of samples can be configured
+  sac::Ransac<
+      sac_problems::relative_pose::EigensolverSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::relative_pose::EigensolverSacProblem> eigenproblem_ptr(
+      new sac_problems::relative_pose::EigensolverSacProblem(adapter,10));
+  ransac.sac_model_ = eigenproblem_ptr;
+  ransac.threshold_ = 1.0;
+  ransac.max_iterations_ = 100;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel();
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //do final polishing of the model over all inliers
+  sac_problems::relative_pose::EigensolverSacProblem::model_t optimizedModel;
+  eigenproblem_ptr->optimizeModelCoefficients(
+      ransac.inliers_,
+      ransac.model_coefficients_,
+      optimizedModel);
+
+  //print the results
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_.rotation << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << ransac.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    std::cout << ransac.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+  std::cout << "the optimized result is: " << std::endl;
+  std::cout << optimizedModel.rotation << std::endl;
+
+  // Create Lmeds
+  sac::Lmeds<sac_problems::relative_pose::EigensolverSacProblem> lmeds;
+  lmeds.sac_model_ = eigenproblem_ptr;
+  lmeds.threshold_ = 1.0;
+  lmeds.max_iterations_ = 50;
+
+  //Run the experiment
+  gettimeofday( &tic, 0 );
+  lmeds.computeModel();
+  gettimeofday( &toc, 0 );
+  double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the lmeds results is: " << std::endl;
+  std::cout << lmeds.model_coefficients_.rotation << std::endl << std::endl;
+  std::cout << "lmeds needed " << lmeds.iterations_ << " iterations and ";
+  std::cout << lmeds_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << lmeds.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < lmeds.inliers_.size(); i++)
+    std::cout << lmeds.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_multi_noncentral_absolute_pose_sac.cpp

@@ -0,0 +1,131 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/absolute_pose/methods.hpp>
+#include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
+#include <opengv/sac/MultiRansac.hpp>
+#include <opengv/sac_problems/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  //initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.5;
+  double outlierFraction = 0.1;
+  size_t pointsPerCam = 25;
+  int numberCameras = 4;
+
+  //create a random viewpoint pose
+  translation_t position = generateRandomTranslation(2.0);
+  rotation_t rotation = generateRandomRotation(0.5);
+
+  //create a random camera-system
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateRandomCameraSystem( numberCameras, camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  std::vector<std::shared_ptr<points_t> > multiPoints;
+  std::vector<std::shared_ptr<bearingVectors_t> > multiBearingVectors;
+  std::vector< std::shared_ptr<Eigen::MatrixXd> > gt;
+  generateMulti2D3DCorrespondences(
+      position, rotation, camOffsets, camRotations,
+      pointsPerCam, noise, outlierFraction,
+      multiPoints, multiBearingVectors, gt );
+
+  //print the experiment characteristics
+  printExperimentCharacteristics(
+      position, rotation, noise, outlierFraction );
+
+  //create a non-central absolute adapter
+  absolute_pose::NoncentralAbsoluteMultiAdapter adapter(
+      multiBearingVectors,
+      multiPoints,
+      camOffsets,
+      camRotations );
+
+  //Create a AbsolutePoseSacProblem and Ransac
+  //The method is set to GP3P
+  sac::MultiRansac<
+      sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
+      new sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(
+      adapter ));
+
+  ransac.sac_model_ = absposeproblem_ptr;
+  ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
+  ransac.max_iterations_ = 50;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel();
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  size_t numberInliers = 0;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    numberInliers += ransac.inliers_[i].size();
+  std::cout << "the number of inliers is: " << numberInliers;
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+  {
+    for(size_t j = 0; j < ransac.inliers_[i].size(); j++)
+      std::cout << ransac.inliers_[i][j] << " ";
+  }
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_multi_noncentral_relative_pose_sac.cpp

@@ -0,0 +1,139 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/NoncentralRelativeMultiAdapter.hpp>
+#include <opengv/sac/MultiRansac.hpp>
+#include <opengv/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.3;
+  double outlierFraction = 0.1;
+  size_t pointsPerCam = 25;
+  int numberCameras = 4;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateRandomCameraSystem( numberCameras, camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  std::vector<std::shared_ptr<bearingVectors_t> > multiBearingVectors1;
+  std::vector<std::shared_ptr<bearingVectors_t> > multiBearingVectors2;
+  std::vector<std::shared_ptr<Eigen::MatrixXd> > gt;
+  generateMulti2D2DCorrespondences(
+      position1, rotation1, position2, rotation2, camOffsets, camRotations,
+      pointsPerCam, noise, outlierFraction,
+      multiBearingVectors1, multiBearingVectors2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation, false );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+  
+  //create a non-central relative multi-adapter
+  relative_pose::NoncentralRelativeMultiAdapter adapter(
+      multiBearingVectors1,
+      multiBearingVectors2,
+      camOffsets,
+      camRotations);
+
+  //Create a MultiNoncentralRelativePoseSacProblem and Ransac
+  opengv::sac::MultiRansac<
+      sac_problems::relative_pose::MultiNoncentralRelativePoseSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::relative_pose::MultiNoncentralRelativePoseSacProblem> relposeproblem_ptr(
+      new sac_problems::relative_pose::MultiNoncentralRelativePoseSacProblem(
+      adapter,
+      sac_problems::relative_pose::MultiNoncentralRelativePoseSacProblem::SIXPT));
+  ransac.sac_model_ = relposeproblem_ptr;
+  ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
+  ransac.max_iterations_ = 100;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel();
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the ransac threshold is: " << ransac.threshold_ << std::endl;
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  size_t numberInliers = 0;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    numberInliers += ransac.inliers_[i].size();
+  std::cout << "the number of inliers is: " << numberInliers;
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+  {
+    for(size_t j = 0; j < ransac.inliers_[i].size(); j++)
+      std::cout << ransac.inliers_[i][j] << " ";
+  }
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_noncentral_absolute_pose.cpp

@@ -0,0 +1,189 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/absolute_pose/methods.hpp>
+#include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  //initialize random seed
+  initializeRandomSeed();
+  
+  //set experiment parameters
+  double noise = 0.0;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 100;
+  int numberCameras = 4;
+
+  //create a random viewpoint pose
+  translation_t position = generateRandomTranslation(2.0);
+  rotation_t rotation = generateRandomRotation(0.5);
+  
+  //create a random camera-system
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateRandomCameraSystem( numberCameras, camOffsets, camRotations );
+  
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors;
+  points_t points;
+  std::vector<int> camCorrespondences;
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D3DCorrespondences(
+      position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors, points, camCorrespondences, gt );
+
+  //print the experiment characteristics
+  printExperimentCharacteristics(
+      position, rotation, noise, outlierFraction );
+
+  //create a non-central absolute adapter
+  absolute_pose::NoncentralAbsoluteAdapter adapter(
+      bearingVectors,
+      camCorrespondences,
+      points,
+      camOffsets,
+      camRotations,
+      rotation);
+
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 50;
+
+  //run the experiments
+  std::cout << "running Kneip's GP3P (using first three correspondences/";
+  std::cout << std::endl;
+  transformations_t gp3p_transformations;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    gp3p_transformations = absolute_pose::gp3p(adapter);
+  gettimeofday( &toc, 0 );
+  double gp3p_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running gpnp over all correspondences" << std::endl;
+  transformation_t gpnp_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    gpnp_transformation = absolute_pose::gpnp(adapter);
+  gettimeofday( &toc, 0 );
+  double gpnp_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running gpnp over 6 correspondences" << std::endl;
+  std::vector<int> indices6 = getNindices(6);
+  transformation_t gpnp_transformation_6 =
+      absolute_pose::gpnp( adapter, indices6 );
+
+  std::cout << "running upnp over all correspondences" << std::endl;
+  transformations_t upnp_transformations;
+  gettimeofday( &tic, 0 );
+  for( size_t i = 0; i < iterations; i++ )
+    upnp_transformations = absolute_pose::upnp(adapter);
+  gettimeofday( &toc, 0);
+  double upnp_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "running upnp over 3 correspondences" << std::endl;
+  std::vector<int> indices3 = getNindices(3);
+  transformations_t upnp_transformations_3 =
+      absolute_pose::upnp( adapter, indices3 );
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization" << std::endl;
+  //add a small perturbation to the rotation
+  translation_t t_perturbed; rotation_t R_perturbed;
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1 );
+  transformation_t nonlinear_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+  {
+    adapter.sett(t_perturbed);
+    adapter.setR(R_perturbed);
+    nonlinear_transformation = absolute_pose::optimize_nonlinear(adapter);
+  }
+  gettimeofday( &toc, 0 );
+  double nonlinear_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization with 10 correspondences";
+  std::cout << std::endl;
+  std::vector<int> indices10 = getNindices(10);
+  //add a small perturbation to the rotation
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1 );
+  adapter.sett(t_perturbed);
+  adapter.setR(R_perturbed);
+  transformation_t nonlinear_transformation_10 =
+      absolute_pose::optimize_nonlinear(adapter,indices10);
+
+  //print the results
+  std::cout << "results from gp3p algorithm:" << std::endl;
+  for(size_t i = 0; i < gp3p_transformations.size(); i++)
+    std::cout << gp3p_transformations[i] << std::endl << std::endl;
+  std::cout << "results from gpnp algorithm:" << std::endl;
+  std::cout << gpnp_transformation << std::endl << std::endl;
+  std::cout << "results from gpnp algorithm with only 6 correspondences:";
+  std::cout << std::endl;
+  std::cout << gpnp_transformation_6 << std::endl << std::endl;
+  std::cout << "results from upnp algorithm:" << std::endl;
+  for(size_t i = 0; i < upnp_transformations.size(); i++)
+    std::cout << upnp_transformations[i] << std::endl << std::endl;
+  std::cout << "results from upnp algorithm with only 3 correspondences:";
+  std::cout << std::endl;
+  for(size_t i = 0; i < upnp_transformations_3.size(); i++)
+    std::cout << upnp_transformations_3[i] << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm:" << std::endl;
+  std::cout << nonlinear_transformation << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm with only 10 correspondences:";
+  std::cout << std::endl;
+  std::cout << nonlinear_transformation_10 << std::endl << std::endl;
+
+  std::cout << "timings from gp3p algorithm: ";
+  std::cout << gp3p_time << std::endl;
+  std::cout << "timings from gpnp algorithm: ";
+  std::cout << gpnp_time << std::endl;
+  std::cout << "timings from upnp algorithm: ";
+  std::cout << upnp_time << std::endl;
+  std::cout << "timings from nonlinear algorithm: ";
+  std::cout << nonlinear_time << std::endl;
+}

thirdparty/opengv/test/test_noncentral_absolute_pose_sac.cpp

@@ -0,0 +1,152 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/absolute_pose/methods.hpp>
+#include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac/Lmeds.hpp>
+#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  //initialize random seed
+  initializeRandomSeed();
+  
+  //set experiment parameters
+  double noise = 0.5;
+  double outlierFraction = 0.1;
+  size_t numberPoints = 100;
+  int numberCameras = 4;
+
+  //create a random viewpoint pose
+  translation_t position = generateRandomTranslation(2.0);
+  rotation_t rotation = generateRandomRotation(0.5);
+  
+  //create a random camera-system
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateRandomCameraSystem( numberCameras, camOffsets, camRotations );
+  
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors;
+  points_t points;
+  std::vector<int> camCorrespondences;
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D3DCorrespondences(
+      position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors, points, camCorrespondences, gt );
+
+  //print the experiment characteristics
+  printExperimentCharacteristics(
+      position, rotation, noise, outlierFraction );
+
+  //create a non-central absolute adapter
+  absolute_pose::NoncentralAbsoluteAdapter adapter(
+      bearingVectors,
+      camCorrespondences,
+      points,
+      camOffsets,
+      camRotations,
+      rotation);
+
+  //Create a AbsolutePoseSacProblem and Ransac
+  //The method is set to GP3P
+  sac::Ransac<
+      sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
+      new sac_problems::absolute_pose::AbsolutePoseSacProblem(
+      adapter,
+      sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P));
+  ransac.sac_model_ = absposeproblem_ptr;
+  ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
+  ransac.max_iterations_ = 50;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel();
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << ransac.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    std::cout << ransac.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+
+  // Create LMedS
+  sac::Lmeds<sac_problems::absolute_pose::AbsolutePoseSacProblem> lmeds;
+  lmeds.sac_model_ = absposeproblem_ptr;
+  lmeds.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
+  lmeds.max_iterations_ = 50;
+
+  //Run the experiment
+  gettimeofday( &tic, 0 );
+  lmeds.computeModel();
+  gettimeofday( &toc, 0 );
+  double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the lmeds results is: " << std::endl;
+  std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Lmeds needed " << lmeds.iterations_ << " iterations and ";
+  std::cout << lmeds_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << lmeds.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < lmeds.inliers_.size(); i++)
+    std::cout << lmeds.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_noncentral_relative_pose.cpp

@@ -0,0 +1,199 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/NoncentralRelativeAdapter.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.0;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 100;
+  int numberCameras = 4;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateRandomCameraSystem( numberCameras, camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1;
+  std::vector<int> camCorrespondences2;
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation, false );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create non-central relative adapter
+  relative_pose::NoncentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      camCorrespondences1,
+      camCorrespondences2,
+      camOffsets,
+      camRotations,
+      position,
+      rotation);
+
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 100;
+
+  //running experiment
+  std::cout << "running sixpt with 6 correspondences" << std::endl;
+  std::vector<int> indices6 = getNindices(6);
+  rotations_t sixpt_rotations;
+  gettimeofday( &tic, 0 );
+  for( size_t i = 0; i < iterations; i++ )
+    sixpt_rotations = relative_pose::sixpt(adapter,indices6);
+  gettimeofday( &toc, 0 );
+  double sixpt_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "running ge with 8 correspondences" << std::endl;
+  std::vector<int> indices8 = getNindices(8);
+  rotation_t ge_rotation;
+  gettimeofday( &tic, 0 );
+  for( size_t i = 0; i < iterations; i++ )
+    ge_rotation = relative_pose::ge(adapter,indices8);
+  gettimeofday( &toc, 0 );
+  double ge_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  
+  std::cout << "running seventeenpt algorithm with 17 correspondences";
+  std::cout << std::endl;
+  std::vector<int> indices17 = getNindices(17);
+  transformation_t seventeenpt_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    seventeenpt_transformation = relative_pose::seventeenpt(adapter,indices17);
+  gettimeofday( &toc, 0 );
+  double seventeenpt_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running seventeenpt algorithm with all correspondences";
+  std::cout << std::endl;
+  transformation_t seventeenpt_transformation_all;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    seventeenpt_transformation_all = relative_pose::seventeenpt(adapter);
+  gettimeofday( &toc, 0 );
+  double seventeenpt_time_all =
+      TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization" << std::endl;
+  translation_t t_perturbed; rotation_t R_perturbed;
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1);
+  transformation_t nonlinear_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+  {
+    adapter.sett12(t_perturbed);
+    adapter.setR12(R_perturbed);
+    nonlinear_transformation = relative_pose::optimize_nonlinear(adapter);
+  }
+  gettimeofday( &toc, 0 );
+  double nonlinear_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization with 10 correspondences";
+  std::cout << std::endl;
+  std::vector<int> indices10 = getNindices(10);
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1);
+  adapter.sett12(t_perturbed);
+  adapter.setR12(R_perturbed);
+  transformation_t nonlinear_transformation_10 =
+      relative_pose::optimize_nonlinear(adapter,indices10);
+
+  //print results
+  std::cout << "results from 6pt algorithm:" << std::endl;
+  for( size_t i = 0; i < sixpt_rotations.size(); i++ )
+    std::cout << sixpt_rotations[i] << std::endl << std::endl;
+  std::cout << "result from ge using 8 points:" << std::endl;
+  std::cout << ge_rotation << std::endl << std::endl;
+  std::cout << "results from 17pt algorithm:" << std::endl;
+  std::cout << seventeenpt_transformation << std::endl << std::endl;
+  std::cout << "results from 17pt algorithm with all points:" << std::endl;
+  std::cout << seventeenpt_transformation_all << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm:" << std::endl;
+  std::cout << nonlinear_transformation << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm with only few correspondences:";
+  std::cout << std::endl;
+  std::cout << nonlinear_transformation_10 << std::endl << std::endl;
+  
+  std::cout << "timings from 6pt algorithm: ";
+  std::cout << sixpt_time << std::endl;
+  std::cout << "timings from ge: ";
+  std::cout << ge_time << std::endl;
+  std::cout << "timings from 17pt algorithm: ";
+  std::cout << seventeenpt_time << std::endl;
+  std::cout << "timings from 17pt algorithm with all the points: ";
+  std::cout << seventeenpt_time_all << std::endl;
+  std::cout << "timings from nonlinear algorithm: ";
+  std::cout << nonlinear_time << std::endl;
+}

thirdparty/opengv/test/test_noncentral_relative_pose_sac.cpp

@@ -0,0 +1,168 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/NoncentralRelativeAdapter.hpp>
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac/Lmeds.hpp>
+#include <opengv/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.3;
+  double outlierFraction = 0.3;
+  size_t numberPoints = 100;
+  int numberCameras = 4;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateRandomCameraSystem( numberCameras, camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1;
+  std::vector<int> camCorrespondences2;
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation, false );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create non-central relative adapter
+  relative_pose::NoncentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      camCorrespondences1,
+      camCorrespondences2,
+      camOffsets,
+      camRotations,
+      position,
+      rotation);
+
+  //Create a NoncentralRelativePoseSacProblem and Ransac
+  sac::Ransac<
+      sac_problems::relative_pose::NoncentralRelativePoseSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::relative_pose::NoncentralRelativePoseSacProblem>
+      relposeproblem_ptr(
+      new sac_problems::relative_pose::NoncentralRelativePoseSacProblem(
+      adapter,
+      sac_problems::relative_pose::NoncentralRelativePoseSacProblem::SIXPT));
+  ransac.sac_model_ = relposeproblem_ptr;
+  ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
+  ransac.max_iterations_ = 10000;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel();
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the ransac threshold is: " << ransac.threshold_ << std::endl;
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << ransac.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    std::cout << ransac.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+
+  // Create LMedS
+  sac::Lmeds<
+      sac_problems::relative_pose::NoncentralRelativePoseSacProblem> lmeds;
+  lmeds.sac_model_ = relposeproblem_ptr;
+  lmeds.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
+  lmeds.max_iterations_ = 10000;
+
+  //Run the experiment
+  gettimeofday( &tic, 0 );
+  lmeds.computeModel();
+  gettimeofday( &toc, 0 );
+  double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the lmeds threshold is: " << lmeds.threshold_ << std::endl;
+  std::cout << "the lmeds results is: " << std::endl;
+  std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
+  std::cout << "lmeds needed " << lmeds.iterations_ << " iterations and ";
+  std::cout << lmeds_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << lmeds.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < lmeds.inliers_.size(); i++)
+    std::cout << lmeds.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_point_cloud.cpp

@@ -0,0 +1,150 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/point_cloud/methods.hpp>
+#include <opengv/point_cloud/PointCloudAdapter.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  //initialize random seed
+  initializeRandomSeed();
+  
+  //set experiment parameters
+  double noise = 0.05;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 10;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //derive the correspondences based on random point-cloud
+  points_t points1;
+  points_t points2;
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom3D3DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      numberPoints, noise, outlierFraction, points1, points2, gt );
+    
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation, false );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create the point-cloud adapter
+  point_cloud::PointCloudAdapter adapter(
+      points1, points2, position, rotation);
+
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 100;
+
+  //run experiments
+  std::cout << "running threept with all the points" << std::endl;
+  transformation_t threept_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    threept_transformation = point_cloud::threept_arun(adapter);
+  gettimeofday( &toc, 0 );
+  double threept_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running threept with three points only" << std::endl;
+  std::vector<int> indices3 = getNindices(3);
+  transformation_t threept_transformation_3;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    threept_transformation_3 = point_cloud::threept_arun(adapter,indices3);
+  gettimeofday( &toc, 0 );
+  double threept_time_3 = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization" << std::endl;
+  //add a small perturbation to the rotation
+  translation_t t_perturbed; rotation_t R_perturbed;
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1);
+  adapter.sett12(t_perturbed);
+  adapter.setR12(R_perturbed);
+  transformation_t nonlinear_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    nonlinear_transformation = point_cloud::optimize_nonlinear(adapter);
+  gettimeofday( &toc, 0 );
+  double nonlinear_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization with three points only";
+  std::cout << std::endl;
+  //add a small perturbation to the rotation
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.01);
+  adapter.sett12(t_perturbed);
+  adapter.setR12(R_perturbed);
+  transformation_t nonlinear_transformation_3 =
+      point_cloud::optimize_nonlinear(adapter,indices3);
+
+  //print results
+  std::cout << "results from threept_arun algorithm:" << std::endl;
+  std::cout << threept_transformation << std::endl << std::endl;
+  std::cout << "results from threept_arun with 3 points only:" << std::endl;
+  std::cout << threept_transformation_3 << std::endl << std::endl;
+  std::cout << "results of nonlinear optimization:" << std::endl;
+  std::cout << nonlinear_transformation << std::endl << std::endl;
+  std::cout << "results of nonlinear optimization with 3 points only:" << std::endl;
+  std::cout << nonlinear_transformation_3 << std::endl << std::endl;
+
+  std::cout << "timings from threept_arun algorithm: ";
+  std::cout << threept_time << std::endl;
+  std::cout << "timings from threept_arun algorithm with 3 points only: ";
+  std::cout << threept_time_3 << std::endl;
+  std::cout << "timing of nonlinear optimization: ";
+  std::cout << nonlinear_time << std::endl;
+}

thirdparty/opengv/test/test_point_cloud_sac.cpp

@@ -0,0 +1,148 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/point_cloud/methods.hpp>
+#include <opengv/point_cloud/PointCloudAdapter.hpp>
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac/Lmeds.hpp>
+#include <opengv/sac_problems/point_cloud/PointCloudSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  //initialize random seed
+  initializeRandomSeed();
+  
+  //set experiment parameters
+  double noise = 0.05;
+  double outlierFraction = 0.1;
+  size_t numberPoints = 100;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //derive the correspondences based on random point-cloud
+  points_t points1;
+  points_t points2;
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom3D3DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      numberPoints, noise, outlierFraction, points1, points2, gt );
+    
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation, false );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create the point-cloud adapter
+  point_cloud::PointCloudAdapter adapter(
+      points1, points2, position, rotation);
+
+  //Create a PointCloudSacProblem and Ransac
+  sac::Ransac<
+      sac_problems::point_cloud::PointCloudSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::point_cloud::PointCloudSacProblem> relposeproblem_ptr(
+      new sac_problems::point_cloud::PointCloudSacProblem(adapter));
+  ransac.sac_model_ = relposeproblem_ptr;
+  ransac.threshold_ = 0.1;
+  ransac.max_iterations_ = 50;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel(0);
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the ransac threshold is: " << ransac.threshold_ << std::endl;
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << ransac.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    std::cout << ransac.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+
+  // Create Lmeds
+  sac::Lmeds<
+      sac_problems::point_cloud::PointCloudSacProblem> lmeds;
+  lmeds.sac_model_ = relposeproblem_ptr;
+  lmeds.threshold_ = 0.1;
+  lmeds.max_iterations_ = 50;
+
+  //Run the experiment
+  gettimeofday( &tic, 0 );
+  lmeds.computeModel(0);
+  gettimeofday( &toc, 0 );
+  double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the lmeds threshold is: " << lmeds.threshold_ << std::endl;
+  std::cout << "the lmeds results is: " << std::endl;
+  std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Lmeds needed " << lmeds.iterations_ << " iterations and ";
+  std::cout << lmeds_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << lmeds.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < lmeds.inliers_.size(); i++)
+    std::cout << lmeds.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_relative_pose.cpp

@@ -0,0 +1,234 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.0;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 10;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1; //unused in the central case
+  std::vector<int> camCorrespondences2; //unused in the central case
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+  
+  //compute and print the essential-matrix
+  printEssentialMatrix( position, rotation );
+
+  //create a central relative adapter
+  relative_pose::CentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      rotation);
+
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 50;
+
+  //running experiments
+  std::cout << "running twopt" << std::endl;
+  translation_t twopt_translation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    twopt_translation = relative_pose::twopt(adapter,true);
+  gettimeofday( &toc, 0 );
+  double twopt_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running fivept_stewenius" << std::endl;
+  complexEssentials_t fivept_stewenius_essentials;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    fivept_stewenius_essentials = relative_pose::fivept_stewenius(adapter);
+  gettimeofday( &toc, 0 );
+  double fivept_stewenius_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running fivept_nister" << std::endl;
+  essentials_t fivept_nister_essentials;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    fivept_nister_essentials = relative_pose::fivept_nister(adapter);
+  gettimeofday( &toc, 0 );
+  double fivept_nister_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running fivept_kneip" << std::endl;
+  rotations_t fivept_kneip_rotations;
+  gettimeofday( &tic, 0 );
+  std::vector<int> indices5 = getNindices(5);
+  for(size_t i = 0; i < iterations; i++)
+    fivept_kneip_rotations = relative_pose::fivept_kneip(adapter,indices5);
+  gettimeofday( &toc, 0 );
+  double fivept_kneip_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running sevenpt" << std::endl;
+  essentials_t sevenpt_essentials;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    sevenpt_essentials = relative_pose::sevenpt(adapter);
+  gettimeofday( &toc, 0 );
+  double sevenpt_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running eightpt" << std::endl;
+  essential_t eightpt_essential;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    eightpt_essential = relative_pose::eightpt(adapter);
+  gettimeofday( &toc, 0 );
+  double eightpt_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed rotation and ";
+  std::cout << "running eigensolver" << std::endl;
+  translation_t t_perturbed; rotation_t R_perturbed;
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.01);
+  rotation_t eigensolver_rotation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+  {
+    adapter.setR12(R_perturbed);
+    eigensolver_rotation = relative_pose::eigensolver(adapter);
+  }
+  gettimeofday( &toc, 0 );
+  double eigensolver_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization" << std::endl;
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1);
+  transformation_t nonlinear_transformation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+  {
+    adapter.sett12(t_perturbed);
+    adapter.setR12(R_perturbed);
+    nonlinear_transformation = relative_pose::optimize_nonlinear(adapter);
+  }
+  gettimeofday( &toc, 0 );
+  double nonlinear_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "setting perturbed pose and ";
+  std::cout << "performing nonlinear optimization with 10 indices" << std::endl;
+  std::vector<int> indices10 = getNindices(10);
+  getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1);
+  adapter.sett12(t_perturbed);
+  adapter.setR12(R_perturbed);
+  transformation_t nonlinear_transformation_10 =
+      relative_pose::optimize_nonlinear(adapter,indices10);
+
+  //print results
+  std::cout << "results from two-points algorithm:" << std::endl;
+  std::cout << twopt_translation << std::endl << std::endl;
+  std::cout << "results from stewenius' five-point algorithm:" << std::endl;
+  for( size_t i = 0; i < fivept_stewenius_essentials.size(); i++ )
+    std::cout << fivept_stewenius_essentials.at(i) << std::endl << std::endl;
+  std::cout << "results from nisters' five-point algorithm:" << std::endl;
+  for( size_t i = 0; i < fivept_nister_essentials.size(); i++ )
+    std::cout << fivept_nister_essentials.at(i) << std::endl << std::endl;
+  std::cout << "results from kneip's five-point algorithm:" << std::endl;
+  for( size_t i = 0; i < fivept_kneip_rotations.size(); i++ )
+    std::cout << fivept_kneip_rotations.at(i) << std::endl << std::endl;
+  std::cout << "results from seven-point algorithm:" << std::endl;
+  for( size_t i = 0; i < sevenpt_essentials.size(); i++ )
+    std::cout << sevenpt_essentials.at(i) << std::endl << std::endl;
+  std::cout << "results from eight-point algorithm:" << std::endl;
+  std::cout << eightpt_essential << std::endl << std::endl;
+  std::cout << "results from eigensystem based rotation solver:" << std::endl;
+  std::cout << eigensolver_rotation << std::endl << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm:" << std::endl;
+  std::cout << nonlinear_transformation << std::endl << std::endl;
+  std::cout << "results from nonlinear algorithm with only few correspondences:";
+  std::cout << std::endl;
+  std::cout << nonlinear_transformation_10 << std::endl << std::endl;
+
+  std::cout << "timings from two-points algorithm: ";
+  std::cout << twopt_time << std::endl;
+  std::cout << "timings from stewenius' five-point algorithm: ";
+  std::cout << fivept_stewenius_time << std::endl;
+  std::cout << "timings from nisters' five-point algorithm: ";
+  std::cout << fivept_nister_time << std::endl;
+  std::cout << "timings from kneip's five-point algorithm: ";
+  std::cout << fivept_kneip_time << std::endl;
+  std::cout << "timings from seven-point algorithm: ";
+  std::cout << sevenpt_time << std::endl;
+  std::cout << "timings from eight-point algorithm: ";
+  std::cout << eightpt_time << std::endl;
+  std::cout << "timings from eigensystem based rotation solver: ";
+  std::cout << eigensolver_time << std::endl;
+  std::cout << "timings from nonlinear algorithm: ";
+  std::cout << nonlinear_time << std::endl;
+}

thirdparty/opengv/test/test_relative_pose_rotationOnly.cpp

@@ -0,0 +1,146 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.0;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 10;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = Eigen::Vector3d::Zero();
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1; //unused in the central case
+  std::vector<int> camCorrespondences2; //unused in the central case
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation, false );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create a central relative adapter
+  relative_pose::CentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      rotation);
+
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 100;
+
+  //run experiment
+  std::cout << "running twopt_rotationOnly (using first 2 correpondences";
+  std::cout << std::endl;
+  rotation_t twopt_rotationOnly_rotation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    twopt_rotationOnly_rotation = relative_pose::twopt_rotationOnly(adapter);
+  gettimeofday( &toc, 0 );
+  double twopt_rotationOnly_time =
+      TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running rotationOnly (using all correspondences)" << std::endl;
+  rotation_t rotationOnly_rotation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    rotationOnly_rotation = relative_pose::rotationOnly(adapter);
+  gettimeofday( &toc, 0 );
+  double rotationOnly_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  std::cout << "running rotationOnly with 10 correspondences" << std::endl;
+  std::vector<int> indices10 = getNindices(10);
+  rotation_t rotationOnly10_rotation;
+  gettimeofday( &tic, 0 );
+  for(size_t i = 0; i < iterations; i++)
+    rotationOnly10_rotation = relative_pose::rotationOnly(adapter,indices10);
+  gettimeofday( &toc, 0 );
+  double rotationOnly10_time =
+      TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+
+  //print results
+  std::cout << "results from two-points rotation algorithm:" << std::endl;
+  std::cout << twopt_rotationOnly_rotation << std::endl << std::endl;
+  std::cout << "results from rotation algorithm:" << std::endl;
+  std::cout << rotationOnly_rotation << std::endl << std::endl;
+  std::cout << "results from rotation algorithm with 10 points:" << std::endl;
+  std::cout << rotationOnly10_rotation << std::endl << std::endl;
+
+  std::cout << "timings from two-points rotation algorithm: ";
+  std::cout << twopt_rotationOnly_time << std::endl;
+  std::cout << "timings from rotation algorithm: ";
+  std::cout << rotationOnly_time << std::endl;
+  std::cout << "timings from rotation algorithm with 10 points: ";
+  std::cout << rotationOnly10_time << std::endl;
+}

thirdparty/opengv/test/test_relative_pose_rotationOnly_sac.cpp

@@ -0,0 +1,159 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac/Lmeds.hpp>
+#include <opengv/sac_problems/relative_pose/RotationOnlySacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.5;
+  double outlierFraction = 0.1;
+  size_t numberPoints = 100;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = Eigen::Vector3d::Zero();
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1; //unused in the central case
+  std::vector<int> camCorrespondences2; //unused in the central case
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create a central relative adapter
+  relative_pose::CentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      rotation);
+
+  //Create a RotationOnlySacProblem and Ransac
+  sac::Ransac<
+      sac_problems::relative_pose::RotationOnlySacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::relative_pose::RotationOnlySacProblem> relposeproblem_ptr(
+      new sac_problems::relative_pose::RotationOnlySacProblem(adapter));
+  ransac.sac_model_ = relposeproblem_ptr;
+  ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
+  ransac.max_iterations_ = 50;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel(0);
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the ransac threshold is: " << ransac.threshold_ << std::endl;
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << ransac.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    std::cout << ransac.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+
+  // Create Lmeds
+  sac::Lmeds<
+      sac_problems::relative_pose::RotationOnlySacProblem> lmeds;
+  lmeds.sac_model_ = relposeproblem_ptr;
+  lmeds.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
+  lmeds.max_iterations_ = 50;
+
+  //Run the experiment
+  gettimeofday( &tic, 0 );
+  lmeds.computeModel(0);
+  gettimeofday( &toc, 0 );
+  double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print the results
+  std::cout << "the lmeds threshold is: " << lmeds.threshold_ << std::endl;
+  std::cout << "the lmeds results is: " << std::endl;
+  std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
+  std::cout << "Lmeds needed " << lmeds.iterations_ << " iterations and ";
+  std::cout << lmeds_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << lmeds.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < lmeds.inliers_.size(); i++)
+    std::cout << lmeds.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_relative_pose_sac.cpp

@@ -0,0 +1,174 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+#include <Eigen/Eigen>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac/Lmeds.hpp>
+#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
+#include <opengv/relative_pose/NoncentralRelativeMultiAdapter.hpp>
+#include <opengv/sac/MultiRansac.hpp>
+#include <opengv/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.5;
+  double outlierFraction = 0.1;
+  size_t numberPoints = 100;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1; //unused in the central case
+  std::vector<int> camCorrespondences2; //unused in the central case
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+  
+  //compute and print the essential-matrix
+  printEssentialMatrix( position, rotation );
+
+  //create a central relative adapter
+  relative_pose::CentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      rotation);
+
+  //Create a RelativePoseSac problem and Ransac
+  //Set algorithm to NISTER, STEWENIUS, SEVENPT, or EIGHTPT
+  sac::Ransac<
+      sac_problems::relative_pose::CentralRelativePoseSacProblem> ransac;
+  std::shared_ptr<
+      sac_problems::relative_pose::CentralRelativePoseSacProblem> relposeproblem_ptr(
+      new sac_problems::relative_pose::CentralRelativePoseSacProblem(
+      adapter,
+      sac_problems::relative_pose::CentralRelativePoseSacProblem::STEWENIUS));
+  ransac.sac_model_ = relposeproblem_ptr;
+  ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
+  ransac.max_iterations_ = 50;
+
+  //Run the experiment
+  struct timeval tic;
+  struct timeval toc;
+  gettimeofday( &tic, 0 );
+  ransac.computeModel();
+  gettimeofday( &toc, 0 );
+  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print results for ransac 1
+  std::cout << "the ransac threshold is: " << ransac.threshold_ << std::endl;
+  std::cout << "the ransac results is: " << std::endl;
+  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
+  std::cout << "the normalized translation is: " << std::endl;
+  std::cout << ransac.model_coefficients_.col(3)/
+      ransac.model_coefficients_.col(3).norm() << std::endl << std::endl;
+  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
+  std::cout << ransac_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << ransac.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < ransac.inliers_.size(); i++)
+    std::cout << ransac.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+
+  // Create Lmeds
+  sac::Lmeds<
+      sac_problems::relative_pose::CentralRelativePoseSacProblem> lmeds;
+  lmeds.sac_model_ = relposeproblem_ptr;
+  lmeds.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
+  lmeds.max_iterations_ = 50;
+
+  //Run the experiment
+  gettimeofday( &tic, 0 );
+  lmeds.computeModel();
+  gettimeofday( &toc, 0 );
+  double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
+
+  //print results
+  std::cout << "the lmeds threshold is: " << lmeds.threshold_ << std::endl;
+  std::cout << "the lmeds results is: " << std::endl;
+  std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
+  std::cout << "the normalized translation is: " << std::endl;
+  std::cout << lmeds.model_coefficients_.col(3)/
+      lmeds.model_coefficients_.col(3).norm() << std::endl << std::endl;
+  std::cout << "Lmeds needed " << lmeds.iterations_ << " iterations and ";
+  std::cout << lmeds_time << " seconds" << std::endl << std::endl;
+  std::cout << "the number of inliers is: " << lmeds.inliers_.size();
+  std::cout << std::endl << std::endl;
+  std::cout << "the found inliers are: " << std::endl;
+  for(size_t i = 0; i < lmeds.inliers_.size(); i++)
+    std::cout << lmeds.inliers_[i] << " ";
+  std::cout << std::endl << std::endl;
+}

thirdparty/opengv/test/test_triangulation.cpp

@@ -0,0 +1,154 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <opengv/triangulation/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <sstream>
+#include <fstream>
+
+#include "random_generators.hpp"
+#include "experiment_helpers.hpp"
+#include "time_measurement.hpp"
+
+
+using namespace std;
+using namespace Eigen;
+using namespace opengv;
+
+int main( int argc, char** argv )
+{
+  // initialize random seed
+  initializeRandomSeed();
+
+  //set experiment parameters
+  double noise = 0.5;
+  double outlierFraction = 0.0;
+  size_t numberPoints = 10;
+
+  //generate a random pose for viewpoint 1
+  translation_t position1 = Eigen::Vector3d::Zero();
+  rotation_t rotation1 = Eigen::Matrix3d::Identity();
+
+  //generate a random pose for viewpoint 2
+  translation_t position2 = generateRandomTranslation(2.0);
+  rotation_t rotation2 = generateRandomRotation(0.5);
+
+  //create a fake central camera
+  translations_t camOffsets;
+  rotations_t camRotations;
+  generateCentralCameraSystem( camOffsets, camRotations );
+
+  //derive correspondences based on random point-cloud
+  bearingVectors_t bearingVectors1;
+  bearingVectors_t bearingVectors2;
+  std::vector<int> camCorrespondences1; //unused in the central case
+  std::vector<int> camCorrespondences2; //unused in the central case
+  Eigen::MatrixXd gt(3,numberPoints);
+  generateRandom2D2DCorrespondences(
+      position1, rotation1, position2, rotation2,
+      camOffsets, camRotations, numberPoints, noise, outlierFraction,
+      bearingVectors1, bearingVectors2,
+      camCorrespondences1, camCorrespondences2, gt );
+    
+    
+  std::cout << "the original points are: " << std::endl << gt;
+  std::cout << std::endl << std::endl;
+
+  //Extract the relative pose
+  translation_t position; rotation_t rotation;
+  extractRelativePose(
+      position1, position2, rotation1, rotation2, position, rotation, false );
+
+  //print experiment characteristics
+  printExperimentCharacteristics( position, rotation, noise, outlierFraction );
+
+  //create a central relative adapter and pass the relative pose
+  relative_pose::CentralRelativeAdapter adapter(
+      bearingVectors1,
+      bearingVectors2,
+      position,
+      rotation);
+
+  //timer
+  struct timeval tic;
+  struct timeval toc;
+  size_t iterations = 100;
+
+  //run experiments
+  std::cout << "running triangulation algorithm 1" << std::endl << std::endl;
+  double triangulate_time;
+  MatrixXd triangulate_results(3,numberPoints);
+  for(size_t j = 0; j < numberPoints; j++)
+  {
+    gettimeofday( &tic, 0 );
+    for(size_t i = 0; i < iterations; i++)
+      triangulate_results.block<3,1>(0,j) =
+          triangulation::triangulate(adapter,j);
+    gettimeofday( &toc, 0 );
+    triangulate_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  }
+  std::cout << "timing: " << triangulate_time << std::endl;
+  std::cout << "triangulation result: " << std::endl;
+  std::cout << triangulate_results << std::endl;
+  MatrixXd error(1,numberPoints);
+  for(size_t i = 0; i < numberPoints; i++)
+  {
+    Vector3d singleError = triangulate_results.col(i) - gt.col(i);
+    error(0,i) = singleError.norm();
+  }
+  std::cout << "triangulation error is: " << std::endl;
+  std::cout << error << std::endl << std::endl;
+
+  std::cout << "running triangulation algorithm 2" << std::endl << std::endl;
+  double triangulate2_time;
+  MatrixXd triangulate2_results(3,numberPoints);
+  for(size_t j = 0; j < numberPoints; j++)
+  {
+    gettimeofday( &tic, 0 );
+    for(size_t i = 0; i < iterations; i++)
+      triangulate2_results.block<3,1>(0,j) =
+          triangulation::triangulate2(adapter,j);
+    gettimeofday( &toc, 0 );
+    triangulate2_time = TIMETODOUBLE(timeval_minus(toc,tic)) / iterations;
+  }
+  std::cout << "timing: " << triangulate2_time << std::endl;
+  std::cout << "triangulation result: " << std::endl;
+  std::cout << triangulate2_results << std::endl;
+  for(size_t i = 0; i < numberPoints; i++)
+  {
+    Vector3d singleError = triangulate2_results.col(i) - gt.col(i);
+    error(0,i) = singleError.norm();
+  }
+  std::cout << "triangulation error is: " << std::endl << error << std::endl;
+}

thirdparty/opengv/test/time_measurement.cpp

@@ -0,0 +1,63 @@
+/******************************************************************************
+ * Author: Laurent Kneip                                                      *
+ * Contact: kneip.laurent@gmail.com                                           *
+ * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.       *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 "time_measurement.hpp"
+
+#ifdef WIN32
+
+#include <sys\timeb.h>
+
+void
+gettimeofday( timeval * timeofday, int dummy)
+{
+  struct timeb time;
+  ftime(&time);
+  timeofday->tv_sec = (int) time.time;
+  timeofday->tv_usec = 1000 * (int) time.millitm;
+}
+
+#endif
+
+timeval
+timeval_minus( const struct timeval &t1, const struct timeval &t2 )
+{
+  timeval ret;
+  ret.tv_sec = t1.tv_sec - t2.tv_sec;
+  if( t1.tv_usec < t2.tv_usec )
+  {
+    ret.tv_sec--;
+    ret.tv_usec = t1.tv_usec - t2.tv_usec + 1000000;
+  }
+  else
+    ret.tv_usec = t1.tv_usec - t2.tv_usec;
+
+  return ret;
+}
+

thirdparty/opengv/test/time_measurement.hpp

@@ -0,0 +1,53 @@
+/******************************************************************************
+ * Author:   Laurent Kneip                                                    *
+ * Contact:  kneip.laurent@gmail.com                                          *
+ * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
+ *                                                                            *
+ * 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 name of ANU 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 ANU OR THE 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 OPENGV_TIME_MEASUREMENT_HPP_
+#define OPENGV_TIME_MEASUREMENT_HPP_
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef WIN32
+  struct timeval {
+    int tv_sec;
+    int tv_usec;
+  };
+
+  void gettimeofday( timeval * timeofday, int dummy);
+#else
+  #include <sys/time.h>
+#endif
+
+#define TIMETODOUBLE(x) ( x.tv_sec + x.tv_usec * 1e-6 )
+
+timeval
+timeval_minus( const struct timeval &t1, const struct timeval &t2 );
+
+#endif /* OPENGV_TIME_MEASUREMENT_HPP_ */