i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -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.");

1
corelib/include/rtabmap/core/RegistrationVis.h
View File

@@ -83,6 +83,7 @@ private:
int _PnPFlags;
int _PnPRefineIterations;
float _PnPMaxVar;
unsigned int _multiSamplingPolicy;
int _correspondencesApproach;
int _flowWinSize;
int _flowIterations;

1
corelib/include/rtabmap/core/util3d_motion_estimation.h
View File

@@ -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
corelib/src/RegistrationVis.cpp
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,

228
corelib/src/util3d_motion_estimation.cpp
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;