Added Vis/PnPSamplingPolicy parameter (opengv "multi" ransac)

This commit is contained in:
matlabbe
2023-07-22 11:05:21 -04:00
parent a593b0d525
commit 3feb03cf35
9 changed files with 242 additions and 127 deletions
+1 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 21)
SET(RTABMAP_PATCH_VERSION 1)
SET(RTABMAP_PATCH_VERSION 2)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -598,6 +598,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
#endif
RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPSamplingPolicy, unsigned int, 1, uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str());
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation.");
@@ -83,6 +83,7 @@ private:
int _PnPFlags;
int _PnPRefineIterations;
float _PnPMaxVar;
unsigned int _multiSamplingPolicy;
int _correspondencesApproach;
int _flowWinSize;
int _flowIterations;
@@ -59,6 +59,7 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const std::vector<CameraModel> & cameraModels,
unsigned int samplingPolicy = 0, // 0=AUTO, 1=ANY, 2=HOMOGENEOUS
int minInliers = 10,
int iterations = 100,
double reprojError = 5.,
+3
View File
@@ -70,6 +70,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
_multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -126,6 +127,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
Parameters::parse(parameters, Parameters::kVisPnPSamplingPolicy(), _multiSamplingPolicy);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -1578,6 +1580,7 @@ Transform RegistrationVis::computeTransformationImpl(
words3A,
wordsB,
models,
_multiSamplingPolicy,
_minInliers,
_iterations,
_PnPReprojError,
+152 -76
View File
@@ -41,10 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef RTABMAP_OPENGV
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
#include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac/MultiRansac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#include <opengv/sac_problems/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp>
#endif
@@ -244,6 +245,7 @@ Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const std::vector<CameraModel> & cameraModels,
unsigned int samplingPolicy,
int minInliers,
int iterations,
double reprojError,
@@ -310,32 +312,50 @@ Transform estimateMotion3DTo2D(
cameraIndexes.resize(oi);
matches.resize(oi);
std::vector<int> cc;
cc.resize(cameraModels.size());
std::fill(cc.begin(), cc.end(),0);
for(size_t i=0; i<cameraIndexes.size(); ++i)
{
cc[cameraIndexes[i]] = cc[cameraIndexes[i]] + 1;
}
bool cameraMatchLessThan2 = false;
for (size_t i=0; i<cameraModels.size(); ++i)
{
UDEBUG("Matches in Camera %d: %d", i, cc[i]);
// opengv multi ransac needs at least 2 matches/camera
if (cc[i] < 2)
{
cameraMatchLessThan2 = true;
}
}
UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d",
UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d samplingPolicy=%ld",
(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
guess.prettyPrint().c_str(), reprojError, iterations);
guess.prettyPrint().c_str(), reprojError, iterations, samplingPolicy);
if((int)matches.size() >= minInliers && !cameraMatchLessThan2)
if((int)matches.size() >= minInliers)
{
if(samplingPolicy == 0 || samplingPolicy == 2)
{
std::vector<int> cc;
cc.resize(cameraModels.size());
std::fill(cc.begin(), cc.end(),0);
for(size_t i=0; i<cameraIndexes.size(); ++i)
{
cc[cameraIndexes[i]] = cc[cameraIndexes[i]] + 1;
}
for (size_t i=0; i<cameraModels.size(); ++i)
{
UDEBUG("Matches in Camera %d: %d", i, cc[i]);
// opengv multi ransac needs at least 2 matches/camera
if (cc[i] < 2)
{
if(samplingPolicy==2) {
UERROR("Not enough matches in camera %ld to do "
"homogenoeus random sampling, returning null "
"transform. Consider using AUTO sampling "
"policy to fallback to ANY policy.", i);
return Transform();
}
else { // samplingPolicy==0
samplingPolicy = 1;
UWARN("Not enough matches in camera %ld to do "
"homogenoeus random sampling, falling back to ANY policy.", i);
break;
}
}
}
}
if(samplingPolicy == 0)
{
samplingPolicy = 2;
}
// convert cameras
opengv::translations_t camOffsets;
opengv::rotations_t camRotations;
@@ -348,63 +368,119 @@ Transform estimateMotion3DTo2D(
camRotations.push_back(cameraModels[i].localTransform().toEigen4d().block<3,3>(0, 0));
}
// convert 3d points
std::vector<std::shared_ptr<opengv::points_t>> multiPoints;
multiPoints.resize(cameraModels.size());
// convert 2d-3d correspondences into bearing vectors
std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
multiBearingVectors.resize(cameraModels.size());
for(size_t i=0; i<cameraModels.size();++i)
Transform pnp;
if(samplingPolicy == 2) // Homogenoeus random sampling
{
multiPoints[i] = std::make_shared<opengv::points_t>();
multiBearingVectors[i] = std::make_shared<opengv::bearingVectors_t>();
// convert 3d points
std::vector<std::shared_ptr<opengv::points_t>> multiPoints;
multiPoints.resize(cameraModels.size());
// convert 2d-3d correspondences into bearing vectors
std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
multiBearingVectors.resize(cameraModels.size());
for(size_t i=0; i<cameraModels.size();++i)
{
multiPoints[i] = std::make_shared<opengv::points_t>();
multiBearingVectors[i] = std::make_shared<opengv::bearingVectors_t>();
}
for(size_t i=0; i<objectPoints.size(); ++i)
{
int cameraIndex = cameraIndexes[i];
multiPoints[cameraIndex]->push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
cv::Vec3f pt;
cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
pt = cv::normalize(pt);
multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
}
//create a non-central absolute multi adapter
opengv::absolute_pose::NoncentralAbsoluteMultiAdapter adapter(
multiBearingVectors,
multiPoints,
camOffsets,
camRotations );
adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));
//Create a MultiNoncentralAbsolutePoseSacProblem and MultiRansac
//The method is set to GP3P
opengv::sac::MultiRansac<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> ransac;
std::shared_ptr<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter));
ransac.sac_model_ = absposeproblem_ptr;
ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
ransac.max_iterations_ = iterations;
UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);
//Run the experiment
ransac.computeModel();
pnp = Transform::fromEigen3d(ransac.model_coefficients_);
UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
UDEBUG("Ransac iterations done: %d", ransac.iterations_);
for (size_t i=0; i < cameraModels.size(); ++i)
{
inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end());
}
}
else
{
// convert 3d points
opengv::points_t points;
// convert 2d-3d correspondences into bearing vectors
opengv::bearingVectors_t bearingVectors;
opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences;
for(size_t i=0; i<objectPoints.size(); ++i)
{
int cameraIndex = cameraIndexes[i];
points.push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
cv::Vec3f pt;
cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
pt = cv::normalize(pt);
bearingVectors.push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
camCorrespondences.push_back(cameraIndex);
}
//create a non-central absolute adapter
opengv::absolute_pose::NoncentralAbsoluteAdapter adapter(
bearingVectors,
camCorrespondences,
points,
camOffsets,
camRotations );
adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));
//Create a AbsolutePoseSacProblem and Ransac
//The method is set to GP3P
opengv::sac::Ransac<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;
std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P));
ransac.sac_model_ = absposeproblem_ptr;
ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
ransac.max_iterations_ = iterations;
UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);
//Run the experiment
ransac.computeModel();
pnp = Transform::fromEigen3d(ransac.model_coefficients_);
UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
UDEBUG("Ransac iterations done: %d", ransac.iterations_);
inliers = ransac.inliers_;
}
for(size_t i=0; i<objectPoints.size(); ++i)
{
int cameraIndex = cameraIndexes[i];
multiPoints[cameraIndex]->push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
cv::Vec3f pt;
cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
pt = cv::normalize(pt);
multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
}
//create a non-central absolute multi adapter
opengv::absolute_pose::NoncentralAbsoluteMultiAdapter adapter(
multiBearingVectors,
multiPoints,
camOffsets,
camRotations );
adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));
//Create a MultiNoncentralAbsolutePoseSacProblem and MultiRansac
//The method is set to GP3P
opengv::sac::MultiRansac<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> ransac;
std::shared_ptr<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter));
ransac.sac_model_ = absposeproblem_ptr;
ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
ransac.max_iterations_ = iterations;
UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);
//Run the experiment
ransac.computeModel();
Transform pnp = Transform::fromEigen3d(ransac.model_coefficients_);
UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
UDEBUG("Ransac iterations done: %d", ransac.iterations_);
for (size_t i=0; i < cameraModels.size(); ++i)
{
inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end());
}
UDEBUG("Ransac inliers: %ld", inliers.size());
if((int)inliers.size() >= minInliers)
if((int)inliers.size() >= minInliers && !pnp.isNull())
{
transform = pnp;
+1
View File
@@ -1175,6 +1175,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
_ui->loopClosure_pnpMaxVariance->setObjectName(Parameters::kVisPnPMaxVariance().c_str());
_ui->loopClosure_pnpSamplingPolicy->setObjectName(Parameters::kVisPnPSamplingPolicy().c_str());
_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());
connect(_ui->reextract_nn, SIGNAL(currentIndexChanged(int)), this, SLOT(updateFeatureMatchingVisibility()));
_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
+81 -49
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-606</y>
<y>-988</y>
<width>756</width>
<height>3657</height>
<height>3736</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -19455,16 +19455,25 @@ Lower the ratio -&gt; higher the precision.</string>
<string>Motion Estimation: 3D to 2D (PnP)</string>
</property>
<layout class="QGridLayout" name="gridLayout_59" columnstretch="0,1">
<item row="2" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2">
<property name="text">
<string>Refine iterations.</string>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_pnpMaxVariance">
<property name="suffix">
<string/>
</property>
<property name="wordWrap">
<bool>true</bool>
<property name="decimals">
<number>4</number>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>99.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
@@ -19500,6 +19509,19 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2_2">
<property name="text">
<string>Max linear variance between 3D point correspondences after PnP. 0 means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_pnpReprojError">
<property name="suffix">
@@ -19519,6 +19541,32 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_235">
<property name="text">
<string>Flags.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2">
<property name="text">
<string>Refine iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="loopClosure_pnpRefineIterations">
<property name="minimum">
@@ -19535,10 +19583,29 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_235">
<item row="4" column="0">
<widget class="QComboBox" name="loopClosure_pnpSamplingPolicy">
<item>
<property name="text">
<string>AUTO</string>
</property>
</item>
<item>
<property name="text">
<string>ANY</string>
</property>
</item>
<item>
<property name="text">
<string>HOMOGENEOUS</string>
</property>
</item>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2_3">
<property name="text">
<string>Flags.</string>
<string>Multi-camera random sampling policy. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -19548,41 +19615,6 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2_2">
<property name="text">
<string>Max linear variance between 3D point correspondences after PnP. 0 means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_pnpMaxVariance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>99.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap</name>
<version>0.21.1</version>
<version>0.21.2</version>
<description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>