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

Merge branch 'guoqingz/multi_noncentral' of https://github.com/guoqingzh/rtabmap into guoqingzh-guoqingz/multi_noncentral

Browse Source
This commit is contained in:
matlabbe
2023-07-21 11:35:26 -04:00
parent ca4276a951 ddeca4585d
commit a593b0d525
1 changed files with 49 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
corelib/src/util3d_motion_estimation.cpp
View File

@@ -41,9 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef RTABMAP_OPENGV #ifdef RTABMAP_OPENGV
#include <opengv/absolute_pose/methods.hpp> #include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp> #include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
#include <opengv/sac/Ransac.hpp> #include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp> #include <opengv/sac/MultiRansac.hpp>
#include <opengv/sac_problems/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp>
#endif #endif
namespace rtabmap namespace rtabmap
@@ -308,11 +310,31 @@ Transform estimateMotion3DTo2D(
cameraIndexes.resize(oi); cameraIndexes.resize(oi);
matches.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",
(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(), (int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
guess.prettyPrint().c_str(), reprojError, iterations); guess.prettyPrint().c_str(), reprojError, iterations);
if((int)matches.size() >= minInliers) if((int)matches.size() >= minInliers && !cameraMatchLessThan2)
{ {
// convert cameras // convert cameras
opengv::translations_t camOffsets; opengv::translations_t camOffsets;
@@ -327,37 +349,42 @@ Transform estimateMotion3DTo2D(
} }
// convert 3d points // convert 3d points
opengv::points_t points; std::vector<std::shared_ptr<opengv::points_t>> multiPoints;
multiPoints.resize(cameraModels.size());
// convert 2d-3d correspondences into bearing vectors // convert 2d-3d correspondences into bearing vectors
opengv::bearingVectors_t bearingVectors; std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences; 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) for(size_t i=0; i<objectPoints.size(); ++i)
{ {
int cameraIndex = cameraIndexes[i]; int cameraIndex = cameraIndexes[i];
points.push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z)); multiPoints[cameraIndex]->push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
cv::Vec3f pt; cv::Vec3f pt;
cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]); cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
pt = cv::normalize(pt); pt = cv::normalize(pt);
bearingVectors.push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2])); multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
camCorrespondences.push_back(cameraIndex);
} }
//create a non-central absolute adapter //create a non-central absolute multi adapter
opengv::absolute_pose::NoncentralAbsoluteAdapter adapter( opengv::absolute_pose::NoncentralAbsoluteMultiAdapter adapter(
bearingVectors, multiBearingVectors,
camCorrespondences, multiPoints,
points,
camOffsets, camOffsets,
camRotations ); camRotations );
adapter.setR(guess.toEigen4d().block<3,3>(0, 0)); adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z())); adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));
//Create a AbsolutePoseSacProblem and Ransac //Create a MultiNoncentralAbsolutePoseSacProblem and MultiRansac
//The method is set to GP3P //The method is set to GP3P
opengv::sac::Ransac<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac; opengv::sac::MultiRansac<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> ransac;
std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr( std::shared_ptr<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P)); new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter));
ransac.sac_model_ = absposeproblem_ptr; ransac.sac_model_ = absposeproblem_ptr;
ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx())); ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
@@ -371,7 +398,10 @@ Transform estimateMotion3DTo2D(
UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str()); UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
UDEBUG("Ransac iterations done: %d", ransac.iterations_); UDEBUG("Ransac iterations done: %d", ransac.iterations_);
inliers = ransac.inliers_; 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()); UDEBUG("Ransac inliers: %ld", inliers.size());
if((int)inliers.size() >= minInliers) if((int)inliers.size() >= minInliers)