Removed a bug in the MultiRansac which caused it to stop too early and with not enough inliers.

Author: laurentkneip

include/opengv/sac/implementation/MultiRansac.hpp

@@ -107,7 +107,7 @@ opengv::sac::MultiRansac<PROBLEM_T>::computeModel(
 
       size_t multiSelectionSize = 0;
       for( size_t multiIter = 0; multiIter < selection.size(); multiIter++ )
-        multiSelectionSize += selection.size();
+        multiSelectionSize += selection[multiIter].size();
 
       // Compute the k parameter (k=log(z)/log(1-w^n))
       double w = static_cast<double> (n_best_inliers_count) /
@@ -143,7 +143,7 @@ opengv::sac::MultiRansac<PROBLEM_T>::computeModel(
       multiModelSize += model_[modelIter].size();
     fprintf(stdout,
         "[sm::RandomSampleConsensus::computeModel] Model: %zu size, %d inliers.\n",
-        model_.size(), n_best_inliers_count );
+        multiModelSize, n_best_inliers_count );
   }
 
   if(model_.empty())

src/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.cpp

@@ -251,7 +251,7 @@ opengv::sac_problems::relative_pose::MultiNoncentralRelativePoseSacProblem::
     opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem
         centralProblem(_adapter, algorithm);
     returnValue = centralProblem.computeModelCoefficients(
-        _adapter.convertMultiIndices(indices),outModel);
+        _adapter.convertMultiIndices(indices),outModel);    
 
     //The transformation has been computed from cam to cam now, so transform
     //that into the body frame
@@ -281,30 +281,30 @@ opengv::sac_problems::relative_pose::
 
   for( size_t camIndex = 0; camIndex < indices.size(); camIndex++ )
   {
+    translation_t cam1Offset = _adapter.getCamOffset(camIndex);
+    rotation_t cam1Rotation = _adapter.getCamRotation(camIndex);
+    translation_t cam2Offset = _adapter.getCamOffset(camIndex);
+    rotation_t cam2Rotation = _adapter.getCamRotation(camIndex);
+
+    translation_t directTranslation =
+        cam1Rotation.transpose() *
+        ((translation - cam1Offset) + rotation * cam2Offset);
+    rotation_t directRotation =
+        cam1Rotation.transpose() * rotation * cam2Rotation;
+
+    _adapter.sett12(directTranslation);
+    _adapter.setR12(directRotation);
+
+    transformation_t inverseSolution;
+    inverseSolution.block<3,3>(0,0) = directRotation.transpose();
+    inverseSolution.col(3) =
+        -inverseSolution.block<3,3>(0,0)*directTranslation;
+
     for(
         size_t correspondenceIndex = 0;
         correspondenceIndex < indices[camIndex].size();
         correspondenceIndex++ )
     {
-      translation_t cam1Offset = _adapter.getCamOffset(camIndex);
-      rotation_t cam1Rotation = _adapter.getCamRotation(camIndex);
-      translation_t cam2Offset = _adapter.getCamOffset(camIndex);
-      rotation_t cam2Rotation = _adapter.getCamRotation(camIndex);
-
-      translation_t directTranslation =
-          cam1Rotation.transpose() *
-          ((translation - cam1Offset) + rotation * cam2Offset);
-      rotation_t directRotation =
-          cam1Rotation.transpose() * rotation * cam2Rotation;
-
-      _adapter.sett12(directTranslation);
-      _adapter.setR12(directRotation);
-
-      transformation_t inverseSolution;
-      inverseSolution.block<3,3>(0,0) = directRotation.transpose();
-      inverseSolution.col(3) =
-          -inverseSolution.block<3,3>(0,0)*directTranslation;
-
       p_hom.block<3,1>(0,0) =
           opengv::triangulation::triangulate2(
           _adapter,

test/test_relative_pose_sac.cpp

@@ -38,6 +38,9 @@
 #include <opengv/relative_pose/CentralRelativeAdapter.hpp>
 #include <opengv/sac/Ransac.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>
 
@@ -110,7 +113,7 @@ int main( int argc, char** argv )
       sac_problems::relative_pose::CentralRelativePoseSacProblem> relposeproblem_ptr(
       new sac_problems::relative_pose::CentralRelativePoseSacProblem(
       adapter,
-      sac_problems::relative_pose::CentralRelativePoseSacProblem::NISTER));
+      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;
@@ -123,7 +126,7 @@ int main( int argc, char** argv )
   gettimeofday( &toc, 0 );
   double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
 
-  //print results
+  //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;