i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +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

30
CMakeLists.txt
View File

@@ -345,47 +345,47 @@ IF(OpenCV_FOUND)
ENDIF(OpenCV_FOUND) ENDIF(OpenCV_FOUND)
IF(Freenect_FOUND) IF(Freenect_FOUND)
MESSAGE(STATUS " With Freenect = YES") MESSAGE(STATUS " With Freenect = YES")
ELSE() ELSE()
MESSAGE(STATUS " With Freenect = NO (libfreenect not found)") MESSAGE(STATUS " With Freenect = NO (libfreenect not found)")
ENDIF() ENDIF()
IF(OpenNI2_FOUND) IF(OpenNI2_FOUND)
MESSAGE(STATUS " With OpenNI2 = YES") MESSAGE(STATUS " With OpenNI2 = YES")
ELSE() ELSE()
MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)") MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)")
ENDIF() ENDIF()
IF(freenect2_FOUND) IF(freenect2_FOUND)
MESSAGE(STATUS " With Freenect2 = YES") MESSAGE(STATUS " With Freenect2 = YES")
ELSE() ELSE()
MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)") MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)")
ENDIF() ENDIF()
IF(DC1394_FOUND) IF(DC1394_FOUND)
MESSAGE(STATUS " With dc1394 = YES") MESSAGE(STATUS " With dc1394 = YES")
ELSE() ELSE()
MESSAGE(STATUS " With dc1394 = NO (dc1394 not found)") MESSAGE(STATUS " With dc1394 = NO (dc1394 not found)")
ENDIF() ENDIF()
IF(FlyCapture2_FOUND) IF(FlyCapture2_FOUND)
MESSAGE(STATUS " With FlyCapture2/Triclops = YES") MESSAGE(STATUS " With FlyCapture2/Triclops = YES")
ELSE() ELSE()
MESSAGE(STATUS " With FlyCapture2/Triclops = NO (Point Grey SDK not found)") MESSAGE(STATUS " With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
ENDIF() ENDIF()
IF(G2O_FOUND) IF(G2O_FOUND)
MESSAGE(STATUS " With g2o = YES") MESSAGE(STATUS " With g2o = YES")
ELSE() ELSE()
MESSAGE(STATUS " With g2o = NO (g2o not found)") MESSAGE(STATUS " With g2o = NO (g2o not found)")
ENDIF() ENDIF()
IF(QT4_FOUND) IF(QT4_FOUND)
MESSAGE(STATUS " With Qt = YES (version 4)") MESSAGE(STATUS " With Qt = YES (version 4)")
ELSEIF(Qt5_FOUND) ELSEIF(Qt5_FOUND)
MESSAGE(STATUS " With Qt = YES (version 5)") MESSAGE(STATUS " With Qt = YES (version 5)")
ELSE() ELSE()
MESSAGE(STATUS " With Qt = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)") MESSAGE(STATUS " With Qt = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)")
ENDIF() ENDIF()
MESSAGE(STATUS "--------------------------------------------") MESSAGE(STATUS "--------------------------------------------")

6
corelib/include/rtabmap/core/Odometry.h
View File

@@ -109,7 +109,7 @@ public:
virtual ~OdometryBOW(); virtual ~OdometryBOW();
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
const std::multimap<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;} const std::map<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
const Memory * getMemory() const {return _memory;} const Memory * getMemory() const {return _memory;}
private: private:
@@ -121,7 +121,7 @@ private:
std::string _fixedLocalMapPath; std::string _fixedLocalMapPath;
Memory * _memory; Memory * _memory;
std::multimap<int, pcl::PointXYZ> localMap_; std::map<int, pcl::PointXYZ> localMap_;
}; };
class RTABMAP_EXP OdometryOpticalFlow : public Odometry class RTABMAP_EXP OdometryOpticalFlow : public Odometry
@@ -195,7 +195,7 @@ private:
cv::Mat refDepthOrRight_; cv::Mat refDepthOrRight_;
std::map<int, cv::Point2f> cornersMap_; std::map<int, cv::Point2f> cornersMap_;
std::multimap<int, cv::Point3f> localMap_; std::map<int, cv::Point3f> localMap_;
std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_; std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
std::map<int, Transform> keyFramePoses_; std::map<int, Transform> keyFramePoses_;
float maxVariance_; float maxVariance_;

2
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -68,7 +68,7 @@ public:
std::multimap<int, cv::KeyPoint> words; std::multimap<int, cv::KeyPoint> words;
std::vector<int> wordMatches; std::vector<int> wordMatches;
std::vector<int> wordInliers; std::vector<int> wordInliers;
std::multimap<int, cv::Point3f> localMap; std::map<int, cv::Point3f> localMap;
// Optical Flow odometry // Optical Flow odometry
std::vector<cv::Point2f> refCorners; std::vector<cv::Point2f> refCorners;

8
corelib/include/rtabmap/core/util3d_correspondences.h
View File

@@ -57,6 +57,14 @@ void RTABMAP_EXP findCorrespondences(
float maxDepth, float maxDepth,
std::vector<int> * uniqueCorrespondences = 0); std::vector<int> * uniqueCorrespondences = 0);
void RTABMAP_EXP 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 = 0);
// remove depth by z axis // remove depth by z axis
void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1, void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2, const std::multimap<int, pcl::PointXYZ> & words2,

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

@@ -41,23 +41,23 @@ namespace rtabmap
namespace util3d namespace util3d
{ {
Transform estimateMotion3DTo2D( Transform RTABMAP_EXP estimateMotion3DTo2D(
const std::multimap<int, pcl::PointXYZ> & words3A, const std::map<int, pcl::PointXYZ> & words3A,
const std::multimap<int, cv::KeyPoint> & words2B, const std::map<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel, const CameraModel & cameraModel,
int minInliers = 10, int minInliers = 10,
int iterations = 100, int iterations = 100,
double reprojError = 5., double reprojError = 5.,
int flagsPnP = 0, int flagsPnP = 0,
const Transform & guess = Transform::getIdentity(), const Transform & guess = Transform::getIdentity(),
const std::multimap<int, pcl::PointXYZ> & words3B = std::multimap<int, pcl::PointXYZ>(), const std::map<int, pcl::PointXYZ> & words3B = std::map<int, pcl::PointXYZ>(),
double * varianceOut = 0, double * varianceOut = 0,
std::vector<int> * matchesOut = 0, std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0); std::vector<int> * inliersOut = 0);
Transform estimateMotion3DTo3D( Transform RTABMAP_EXP estimateMotion3DTo3D(
const std::multimap<int, pcl::PointXYZ> & words3A, const std::map<int, pcl::PointXYZ> & words3A,
const std::multimap<int, pcl::PointXYZ> & words3B, const std::map<int, pcl::PointXYZ> & words3B,
int minInliers = 10, int minInliers = 10,
double inliersDistance = 0.1, double inliersDistance = 0.1,
int iterations = 100, int iterations = 100,

10
corelib/src/Memory.cpp
View File

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

6
corelib/src/OdometryBOW.cpp
View File

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

13
corelib/src/OdometryMono.cpp
View File

@@ -833,11 +833,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
scale = scales.begin()->second; scale = scales.begin()->second;
UWARN("scale used = %f (variance=%f scales=%d)", scale, scales.begin()->first, (int)scales.size()); 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", maxVariance_, scales.begin()->first);
UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first); if(scales.begin()->first > maxVariance_)
if(scales.begin()->first > 0.01)
{ {
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 reject = true; // 20 cm for good initialization
} }
} }
@@ -849,7 +848,6 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
Eigen::Vector4f centroid; Eigen::Vector4f centroid;
pcl::compute3DCentroid(*inliersRef, centroid); pcl::compute3DCentroid(*inliersRef, centroid);
scale = 1.0f / centroid[2]; scale = 1.0f / centroid[2];
maxVariance_ = 0.01;
} }
else 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) 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(); refDepthOrRight_ = data.depthOrRightRaw().clone();
keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity())); 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( void findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words1, const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2, 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( Transform estimateMotion3DTo2D(
const std::multimap<int, pcl::PointXYZ> & words3A, const std::map<int, pcl::PointXYZ> & words3A,
const std::multimap<int, cv::KeyPoint> & words2B, const std::map<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel, const CameraModel & cameraModel,
int minInliers, int minInliers,
int iterations, int iterations,
double reprojError, double reprojError,
int flagsPnP, int flagsPnP,
const Transform & guess, const Transform & guess,
const std::multimap<int, pcl::PointXYZ> & words3B, const std::map<int, pcl::PointXYZ> & words3B,
double * varianceOut, double * varianceOut,
std::vector<int> * matchesOut, std::vector<int> * matchesOut,
std::vector<int> * inliersOut) std::vector<int> * inliersOut)
@@ -63,14 +63,14 @@ Transform estimateMotion3DTo2D(
} }
// find correspondences // 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::Point3f> objectPoints(ids.size());
std::vector<cv::Point2f> imagePoints(ids.size()); std::vector<cv::Point2f> imagePoints(ids.size());
int oi=0; int oi=0;
matches.resize(ids.size()); matches.resize(ids.size());
for(unsigned int i=0; i<ids.size(); ++i) 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; pcl::PointXYZ pt = words3A.find(ids[i])->second;
objectPoints[oi].x = pt.x; objectPoints[oi].x = pt.x;
@@ -170,8 +170,8 @@ Transform estimateMotion3DTo2D(
} }
Transform estimateMotion3DTo3D( Transform estimateMotion3DTo3D(
const std::multimap<int, pcl::PointXYZ> & words3A, const std::map<int, pcl::PointXYZ> & words3A,
const std::multimap<int, pcl::PointXYZ> & words3B, const std::map<int, pcl::PointXYZ> & words3B,
int minInliers, int minInliers,
double inliersDistance, double inliersDistance,
int iterations, int iterations,

9
guilib/src/MainWindow.cpp
View File

@@ -848,8 +848,15 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
} }
} }
//detect if it is OdometryMono intitialization
bool monoInitialization = false;
if(_preferencesDialog->getOdomStrategy() == 2 && odom.info().type == 1)
{
monoInitialization = true;
}
_ui->imageView_odometry->clearLines(); _ui->imageView_odometry->clearLines();
if(lost) if(lost && !monoInitialization)
{ {
if(lostStateChanged) if(lostStateChanged)
{ {

2
guilib/src/OdometryViewer.cpp
View File

@@ -265,7 +265,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(odom.info().localMap.size()); cloud->resize(odom.info().localMap.size());
int i=0; int i=0;
for(std::multimap<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter) for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
{ {
(*cloud)[i].x = iter->second.x; (*cloud)[i].x = iter->second.x;
(*cloud)[i].y = iter->second.y; (*cloud)[i].y = iter->second.y;

1
tools/OdometryViewer/main.cpp
View File

@@ -807,6 +807,7 @@ int main (int argc, char * argv[])
if(camera->isCalibrated()) if(camera->isCalibrated())
{ {
rtabmap::CameraThread cameraThread(camera); rtabmap::CameraThread cameraThread(camera);
cameraThread.setColorOnly(true);
odomThread.start(); odomThread.start();
cameraThread.start(); cameraThread.start();

9
utilite/include/rtabmap/utilite/UStl.h
View File

@@ -470,6 +470,15 @@ inline std::list<V> uVectorToList(const std::vector<V> & v)
return std::list<V>(v.begin(), v.end()); return std::list<V>(v.begin(), v.end());
} }
/**
* Convert a std::multimap to a std::map
*/
template<class K, class V>
inline std::map<K, V> uMultimapToMap(const std::multimap<K, V> & m)
{
return std::map<K, V>(m.begin(), m.end());
}
/** /**
* Append a list to another list. * Append a list to another list.
* @param list the list on which the other list will be appended * @param list the list on which the other list will be appended