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

using map instead of multimap for motion estimation methods (only unique words were used)

Browse Source
This commit is contained in:
matlabbe
2015-07-28 23:48:50 -04:00
parent a4039241c5
commit b686103765
14 changed files with 121 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Memory.cpp
View File

@@ -2102,15 +2102,15 @@ Transform Memory::computeVisualTransform(
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo2D(
oldS.getWords3(),
newS.getWords(),
uMultimapToMap(oldS.getWords3()),
uMultimapToMap(newS.getWords()),
cameraModel,
_bowMinInliers,
_bowIterations,
_bowPnPReprojError,
_bowPnPFlags,
Transform::getIdentity(),
newS.getWords3(),
uMultimapToMap(newS.getWords3()),
&variance,
0,
&inliersV);
@@ -2143,8 +2143,8 @@ Transform Memory::computeVisualTransform(
{
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo3D(
oldS.getWords3(),
newS.getWords3(),
uMultimapToMap(oldS.getWords3()),
uMultimapToMap(newS.getWords3()),
_bowMinInliers,
_bowInlierDistance,
_bowIterations,

6
corelib/src/OdometryBOW.cpp
View File

@@ -247,14 +247,14 @@ Transform OdometryBOW::computeTransform(
UDEBUG("");
t = util3d::estimateMotion3DTo2D(
localMap_,
newSignature->getWords(),
uMultimapToMap(newSignature->getWords()),
cameraModel,
this->getMinInliers(),
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPose(),
newSignature->getWords3(),
uMultimapToMap(newSignature->getWords3()),
&variance,
&matches,
&inliers);
@@ -271,7 +271,7 @@ Transform OdometryBOW::computeTransform(
{
t = util3d::estimateMotion3DTo3D(
localMap_,
newSignature->getWords3(),
uMultimapToMap(newSignature->getWords3()),
this->getMinInliers(),
this->getInlierDistance(),
this->getIterations(),

13
corelib/src/OdometryMono.cpp
View File

@@ -833,11 +833,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
scale = scales.begin()->second;
UWARN("scale used = %f (variance=%f scales=%d)", scale, scales.begin()->first, (int)scales.size());
maxVariance_ = 0.01;
UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first);
if(scales.begin()->first > 0.01)
UDEBUG("Max noise variance = %f current variance=%f", maxVariance_, scales.begin()->first);
if(scales.begin()->first > maxVariance_)
{
UWARN("Too high variance %f (should be < 0.01)", scales.begin()->first);
UWARN("Too high variance %f (should be < %f)", scales.begin()->first, maxVariance_);
reject = true; // 20 cm for good initialization
}
}
@@ -849,7 +848,6 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*inliersRef, centroid);
scale = 1.0f / centroid[2];
maxVariance_ = 0.01;
}
else
{
@@ -960,7 +958,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
if(words.count(iter->first) == 1)
{
cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
}
}
refDepthOrRight_ = data.depthOrRightRaw().clone();
keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));

81
corelib/src/util3d_correspondences.cpp
View File

@@ -314,41 +314,6 @@ void findCorrespondences(
}
}
std::list<std::pair<cv::Point2f, cv::Point2f> > findCorrespondences(
const std::multimap<int, cv::KeyPoint> & words1,
const std::multimap<int, cv::KeyPoint> & words2)
{
std::list<std::pair<cv::Point2f, cv::Point2f> > correspondences;
// Find pairs
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
rtabmap::EpipolarGeometry::findPairsUnique(words1, words2, pairs);
if(pairs.size() > 7) // 8 min?
{
// Find fundamental matrix
std::vector<uchar> status;
cv::Mat fundamentalMatrix = rtabmap::EpipolarGeometry::findFFromWords(pairs, status);
//ROS_INFO("inliers = %d/%d", uSum(status), pairs.size());
if(!fundamentalMatrix.empty())
{
int i = 0;
//int goodCount = 0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
{
if(status[i])
{
correspondences.push_back(std::pair<cv::Point2f, cv::Point2f>(iter->second.first.pt, iter->second.second.pt));
//ROS_INFO("inliers kpts %f %f vs %f %f", iter->second.first.pt.x, iter->second.first.pt.y, iter->second.second.pt.x, iter->second.second.pt.y);
}
++i;
}
}
}
return correspondences;
}
void findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
@@ -395,6 +360,52 @@ void findCorrespondences(
}
}
void findCorrespondences(
const std::map<int, pcl::PointXYZ> & words1,
const std::map<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
std::vector<int> * correspondences)
{
std::vector<int> ids = uKeys(words1);
// Find pairs
inliers1.resize(ids.size());
inliers2.resize(ids.size());
if(correspondences)
{
correspondences->resize(ids.size());
}
int oi=0;
for(std::vector<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(words2.find(*iter) != words2.end())
{
inliers1[oi] = words1.find(*iter)->second;
inliers2[oi] = words2.find(*iter)->second;
if(pcl::isFinite(inliers1[oi]) &&
pcl::isFinite(inliers2[oi]) &&
(inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
(inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
(maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
{
if(correspondences)
{
correspondences->at(oi) = *iter;
}
++oi;
}
}
}
inliers1.resize(oi);
inliers2.resize(oi);
if(correspondences)
{
correspondences->resize(oi);
}
}
}
}

14
corelib/src/util3d_motion_estimation.cpp
View File

@@ -40,15 +40,15 @@ namespace util3d
{
Transform estimateMotion3DTo2D(
const std::multimap<int, pcl::PointXYZ> & words3A,
const std::multimap<int, cv::KeyPoint> & words2B,
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel,
int minInliers,
int iterations,
double reprojError,
int flagsPnP,
const Transform & guess,
const std::multimap<int, pcl::PointXYZ> & words3B,
const std::map<int, pcl::PointXYZ> & words3B,
double * varianceOut,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
@@ -63,14 +63,14 @@ Transform estimateMotion3DTo2D(
}
// find correspondences
std::vector<int> ids = uListToVector(uUniqueKeys(words2B));
std::vector<int> ids = uKeys(words2B);
std::vector<cv::Point3f> objectPoints(ids.size());
std::vector<cv::Point2f> imagePoints(ids.size());
int oi=0;
matches.resize(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
if(words3A.count(ids[i]) == 1)
if(words3A.find(ids[i]) != words3A.end())
{
pcl::PointXYZ pt = words3A.find(ids[i])->second;
objectPoints[oi].x = pt.x;
@@ -170,8 +170,8 @@ Transform estimateMotion3DTo2D(
}
Transform estimateMotion3DTo3D(
const std::multimap<int, pcl::PointXYZ> & words3A,
const std::multimap<int, pcl::PointXYZ> & words3B,
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, pcl::PointXYZ> & words3B,
int minInliers,
double inliersDistance,
int iterations,