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

ios first release

Browse Source
This commit is contained in:
matlabbe
2021-06-08 00:06:49 -04:00
parent 630cb6a59f
commit 289d4afd76
88 changed files with 9260 additions and 473 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
corelib/include/rtabmap/core/Landmark.h
View File

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
#include <rtabmap/core/RtabmapExp.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
@@ -39,38 +40,60 @@ class Landmark
{
public:
Landmark() :
id_(0)
id_(0),
size_(0.0f)
{}
Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
id_(id),
pose_(pose),
covariance_(covariance)
{
UASSERT(id_>0);
UASSERT(!pose_.isNull());
UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
}
Landmark(const int & id, const float & size, const Transform & pose, const cv::Mat & covariance) :
id_(id),
size_(size),
pose_(pose),
covariance_(covariance)
{
UASSERT(id_>0);
UASSERT(!pose_.isNull());
UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
}
RTABMAP_DEPRECATED(Landmark(const int & id, const Transform & pose, const cv::Mat & covariance), "Use constructor with size instead.");
virtual ~Landmark() {}
const int & id() const {return id_;}
const float & size() const {return size_;}
const Transform & pose() const {return pose_;}
const cv::Mat & covariance() const {return covariance_;}
private:
int id_;
float size_;
Transform pose_;
cv::Mat covariance_;
};
typedef std::map<int, Landmark> Landmarks;
inline Landmark::Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
id_(id),
size_(0.0f),
pose_(pose),
covariance_(covariance)
{
UASSERT(id_>0);
UASSERT(!pose_.isNull());
UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
}
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */

35
corelib/include/rtabmap/core/MarkerDetector.h
View File

@@ -38,12 +38,43 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class MarkerDetector {
typedef std::map<int, Transform> MapIdPose;
class MarkerInfo {
public:
MarkerInfo(int id, float length, Transform pose) :
id_(id),
length_(length),
pose_(pose)
{}
int id() const {return id_;}
float length() const {return length_;}
const Transform & pose() const {return pose_;}
private:
int id_;
float length_;
Transform pose_;
};
class RTABMAP_EXP MarkerDetector {
public:
MarkerDetector(const ParametersMap & parameters = ParametersMap());
virtual ~MarkerDetector();
void parseParameters(const ParametersMap & parameters);
std::map<int, Transform> detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth = cv::Mat(), float * estimatedMarkerLength = 0, cv::Mat * imageWithDetections = 0);
RTABMAP_DEPRECATED(
MapIdPose detect(const cv::Mat & image,
const CameraModel & model,
const cv::Mat & depth = cv::Mat(),
float * estimatedMarkerLength = 0,
cv::Mat * imageWithDetections = 0), "Use the other constructor, in which the returned map contains the length of each marker detected.");
std::map<int, MarkerInfo> detect(const cv::Mat & image,
const CameraModel & model,
const cv::Mat & depth = cv::Mat(),
const std::map<int, float> & markerLengths = std::map<int, float>(),
cv::Mat * imageWithDetections = 0);
private:
#ifdef HAVE_OPENCV_ARUCO

5
corelib/include/rtabmap/core/Memory.h
View File

@@ -165,7 +165,6 @@ public:
bool labelSignature(int id, const std::string & label);
const std::map<int, std::string> & getAllLabels() const {return _labels;}
const std::map<int, std::set<int> > & getLandmarksIndex() const {return _landmarksIndex;}
const std::map<int, std::set<int> > & getLandmarksInvertedIndex() const {return _landmarksInvertedIndex;}
bool allNodesInWM() const {return _allNodesInWM;}
/**
@@ -346,8 +345,8 @@ private:
std::map<int, double> _workingMem; // id,age
std::map<int, Transform> _groundTruths;
std::map<int, std::string> _labels;
std::map<int, std::set<int> > _landmarksIndex; // <nodeId, landmarkIds>
std::map<int, std::set<int> > _landmarksInvertedIndex; // <landmarkId, nodeIds>
std::map<int, std::set<int> > _landmarksIndex; // < -landmarkId, nodeIds >
std::map<int, float> _landmarksSize; // +landmarkId
//Keypoint stuff
VWDictionary * _vwd;

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

@@ -177,7 +177,7 @@ public:
void setWorkingDirectory(std::string path);
void rejectLastLoopClosure();
void deleteLastLocation();
void setOptimizedPoses(const std::map<int, Transform> & poses);
void setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints);
Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
RTABMAP_DEPRECATED(
void get3DMap(std::map<int, Signature> & signatures,

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

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef RTABMAPEXP_H
#define RTABMAPEXP_H
#if defined(_WIN32)
#if defined(rtabmap_core_EXPORTS)
#define RTABMAP_EXP __declspec( dllexport )

7
corelib/src/CMakeLists.txt
View File

@@ -349,6 +349,13 @@ IF(depthai_FOUND)
)
ENDIF(depthai_FOUND)
IF(TARGET OpenMP::OpenMP_CXX)
SET(LIBRARIES
${LIBRARIES}
OpenMP::OpenMP_CXX
)
ENDIF(TARGET OpenMP::OpenMP_CXX)
IF(WITH_TORO)
SET(SRC_FILES
${SRC_FILES}

1
corelib/src/Link.cpp
View File

@@ -85,6 +85,7 @@ Link::Link(int from,
else
{
_userDataRaw = userData;
_userDataCompressed = compressData2(_userDataRaw);
}
}

119
corelib/src/MarkerDetector.cpp
View File

@@ -114,7 +114,29 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
{
std::map<int, Transform> detections;
std::map<int, Transform> detections;
std::map<int, MarkerInfo> infos = detect(image, model, depth, std::map<int, float>(), imageWithDetections);
for(std::map<int, MarkerInfo>::iterator iter=infos.begin(); iter!=infos.end(); ++iter)
{
detections.insert(std::make_pair(iter->first, iter->second.pose()));
if(markerLengthOut)
{
*markerLengthOut = iter->second.length();
}
}
return detections;
}
std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
const CameraModel & model,
const cv::Mat & depth,
const std::map<int, float> & markerLengths,
cv::Mat * imageWithDetections)
{
std::map<int, MarkerInfo> detections;
#ifdef HAVE_OPENCV_ARUCO
@@ -133,23 +155,29 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
{
float rgbToDepthFactorX = 1.0f;
float rgbToDepthFactorY = 1.0f;
if(markerLength_ == 0)
if(!depth.empty())
{
if(depth.empty())
{
UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
return detections;
}
rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?model.imageWidth()/depth.cols:1);
rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?model.imageHeight()/depth.rows:1);
}
else if(markerLength_ == 0)
{
if(depth.empty())
{
UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
return detections;
}
}
cv::aruco::estimatePoseSingleMarkers(corners, markerLength_==0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs);
cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs);
std::vector<float> scales;
for(size_t i=0; i<ids.size(); ++i)
{
if(markerLength_ == 0)
float length = 0.0f;
std::map<int, float>::const_iterator findIter = markerLengths.find(ids[i]);
if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end())))
{
float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true);
float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true);
float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true);
@@ -157,43 +185,65 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
// Accept measurement only if all 4 depth values are valid and
// they are at the same depth (camera should be perpendicular to marker for
// best depth estimation)
if(d1>0 && d2>0 && d3>0 && d4>0)
if(d>0 && d1>0 && d2>0 && d3>0 && d4>0)
{
if( fabs(d1-d2) < maxDepthError_ &&
fabs(d1-d3) < maxDepthError_ &&
fabs(d1-d4) < maxDepthError_)
float scale = d/tvecs[i].val[2];
if( fabs(d-d1) < maxDepthError_ &&
fabs(d-d2) < maxDepthError_ &&
fabs(d-d3) < maxDepthError_ &&
fabs(d-d4) < maxDepthError_)
{
float depth = (d1+d2+d3+d4)/4.0f;
scales.push_back(depth/tvecs[i].val[2]);
length = scale;
scales.push_back(length);
tvecs[i] *= scales.back();
UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], depth, scales.back());
UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, scales.back());
}
else
{
UWARN("The four marker's corners should be "
"perpendicular to camera to estimate correctly "
"the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f. "
"the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). "
"Parameter %s can be set to non-null to skip automatic "
"marker length estimation. Detections are ignored.",
fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(),
d1, d2, d3, d4,
d1, d2, d3, d4, d,
Parameters::kMarkerLength().c_str());
detections.clear();
return detections;
continue;
}
}
else
{
UWARN("Some depth values (%f,%f,%f,%f) cannot be detected on the "
UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the "
"marker's corners, cannot initialize marker length. "
"Parameter %s can be set to non-null to skip automatic "
"marker length estimation. Detections are ignored.",
d1,d2,d3,d4,
d1,d2,d3,d4,d,
Parameters::kMarkerLength().c_str());
detections.clear();
return detections;
continue;
}
}
else if(markerLength_ < 0)
{
if(findIter!=markerLengths.end())
{
length = findIter->second;
tvecs[i] *= length;
}
else
{
UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)markerLengths.size());
continue;
}
}
else if(markerLength_ > 0)
{
length = markerLength_;
}
else
{
continue;
}
// Limit the detection range to be between the min / max range.
// If the ranges are -1, allow any detection within that direction.
@@ -206,11 +256,11 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
Transform pose = model.localTransform() * t;
detections.insert(std::make_pair(ids[i], pose));
detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
}
}
if(markerLength_ == 0)
if(markerLength_ == 0 && !scales.empty())
{
float sum = 0.0f;
float maxError = 0.0f;
@@ -222,10 +272,10 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
if(error > 0.001f)
{
UWARN("The marker's length detected between 2 of the "
"markers doesn't match (%d->%fm vs %d->%fm)."
"markers doesn't match (%fm vs %fm)."
"Parameter %s can be set to non-null to skip automatic "
"marker length estimation. Detections are ignored.",
ids[i], scales[i], ids[0], scales[0],
scales[i], scales[0],
Parameters::kMarkerLength().c_str());
detections.clear();
return detections;
@@ -242,21 +292,20 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
}
}
if(markerLengthOut)
{
*markerLengthOut = markerLength_;
}
if(imageWithDetections)
{
image.copyTo(*imageWithDetections);
if(ids.size() > 0)
if(!ids.empty())
{
cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);
for(unsigned int i = 0; i < ids.size(); i++)
{
cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], markerLength_ * 0.5f);
std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
if(iter!=detections.end())
{
cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
}
}
}
}

176
corelib/src/Memory.cpp
View File

@@ -91,7 +91,7 @@ Memory::Memory(const ParametersMap & parameters) :
_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
_depthAsMask(Parameters::defaultMemDepthAsMask()),
_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
@@ -259,20 +259,24 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
{
int landmarkId = jter->first;
UASSERT(landmarkId < 0);
cv::Mat landmarkSize = jter->second.uncompressUserDataConst();
if(!landmarkSize.empty() && landmarkSize.type() == CV_32FC1 && landmarkSize.total()==1)
{
std::pair<std::map<int, float>::iterator, bool> inserted=_landmarksSize.insert(std::make_pair(-landmarkId, landmarkSize.at<float>(0,0)));
if(!inserted.second)
{
if(inserted.first->second != landmarkSize.at<float>(0,0))
{
UWARN("Trying to update landmark size buffer for landmark %d with size=%f but "
"it has already a different size set. Keeping old size (%f).",
-landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
}
}
}
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find((*iter)->id());
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
if(nter!=_landmarksIndex.end())
{
nter->second.insert(landmarkId);
}
else
{
std::set<int> tmp;
tmp.insert(landmarkId);
_landmarksIndex.insert(std::make_pair((*iter)->id(), tmp));
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.insert((*iter)->id());
}
@@ -280,7 +284,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
{
std::set<int> tmp;
tmp.insert((*iter)->id());
_landmarksInvertedIndex.insert(std::make_pair(landmarkId, tmp));
_landmarksIndex.insert(std::make_pair(landmarkId, tmp));
}
}
}
@@ -1269,8 +1273,8 @@ std::multimap<int, Link> Memory::getLinks(
else if(signatureId < 0) //landmark
{
int landmarkId = signatureId;
std::map<int, std::set<int> >::const_iterator iter = _landmarksInvertedIndex.find(landmarkId);
if(iter != _landmarksInvertedIndex.end())
std::map<int, std::set<int> >::const_iterator iter = _landmarksIndex.find(landmarkId);
if(iter != _landmarksIndex.end())
{
for(std::set<int>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
@@ -1488,8 +1492,8 @@ std::map<int, int> Memory::getNeighborsId(
// landmarks
for(std::map<int, Link>::const_iterator iter=landmarks->begin(); iter!=landmarks->end(); ++iter)
{
const std::map<int, std::set<int> >::const_iterator kter = _landmarksInvertedIndex.find(iter->first);
if(kter != _landmarksInvertedIndex.end())
const std::map<int, std::set<int> >::const_iterator kter = _landmarksIndex.find(iter->first);
if(kter != _landmarksIndex.end())
{
for(std::set<int>::const_iterator nter=kter->second.begin(); nter!=kter->second.end(); ++nter)
{
@@ -1767,7 +1771,7 @@ void Memory::clear()
_groundTruths.clear();
_labels.clear();
_landmarksIndex.clear();
_landmarksInvertedIndex.clear();
_landmarksSize.clear();
_allNodesInWM = true;
if(_dbDriver)
@@ -2326,22 +2330,13 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
for(std::map<int, Link>::const_iterator iter=s->getLandmarks().begin(); iter!=s->getLandmarks().end(); ++iter)
{
int landmarkId = iter->first;
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(s->id());
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
if(nter!=_landmarksIndex.end())
{
nter->second.erase(landmarkId);
if(nter->second.empty())
{
_landmarksIndex.erase(nter);
}
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.erase(s->id());
if(nter->second.empty())
{
_landmarksInvertedIndex.erase(nter);
_landmarksIndex.erase(nter);
}
}
}
@@ -2485,8 +2480,8 @@ std::map<int, Link> Memory::getNodesObservingLandmark(int landmarkId, bool lookI
std::map<int, Link> nodes;
if(landmarkId < 0)
{
std::map<int, std::set<int> >::const_iterator iter = _landmarksInvertedIndex.find(landmarkId);
if(iter != _landmarksInvertedIndex.end())
std::map<int, std::set<int> >::const_iterator iter = _landmarksIndex.find(landmarkId);
if(iter != _landmarksIndex.end())
{
for(std::set<int>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
@@ -3556,15 +3551,10 @@ unsigned long Memory::getMemoryUsed() const
memoryUsage+=iter->second.size();
}
memoryUsage += _landmarksIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::map<int, std::set<int> >::iterator)) + sizeof(std::map<int, std::set<int> >);
memoryUsage += _landmarksInvertedIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::map<int, std::set<int> >::iterator)) + sizeof(std::map<int, std::set<int> >);
for(std::map<int, std::set<int> >::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
{
memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
}
for(std::map<int, std::set<int> >::const_iterator iter=_landmarksInvertedIndex.begin(); iter!=_landmarksInvertedIndex.end(); ++iter)
{
memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
}
memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
memoryUsage += sizeof(Registration);
@@ -4373,16 +4363,31 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
UDEBUG("Using provided keypoints (%d)", (int)data.keypoints().size());
keypoints = data.keypoints();
// In case we provided corresponding 3D features
if(keypoints.size() == data.keypoints3D().size())
{
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].class_id = i;
}
useProvided3dPoints = true;
}
useProvided3dPoints = keypoints.size() == data.keypoints3D().size();
// A: Adjust keypoint position so that descriptors are correctly extracted
// B: In case we provided corresponding 3D features
if(preDecimation > 1 || useProvided3dPoints)
{
float decimationRatio = 1.0f / float(preDecimation);
double log2value = log(double(preDecimation))/log(2.0);
for(unsigned int i=0; i < keypoints.size(); ++i)
{
cv::KeyPoint & kpt = keypoints[i];
if(preDecimation > 1)
{
kpt.pt.x *= decimationRatio;
kpt.pt.y *= decimationRatio;
kpt.size *= decimationRatio;
kpt.octave += log2value;
}
if(useProvided3dPoints)
{
keypoints[i].class_id = i;
}
}
}
}
else
{
@@ -4797,7 +4802,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("Detecting markers...");
if(landmarks.empty())
{
std::map<int, Transform> markers;
std::map<int, MarkerInfo> markers;
if(!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection())
{
if(data.cameraModels().size() > 1)
@@ -4811,14 +4816,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
else
{
markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0], data.depthRaw());
markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0], data.depthRaw(), _landmarksSize);
}
}
else if(data.stereoCameraModel().isValidForProjection())
{
markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModel().left());
markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModel().left(), cv::Mat(), _landmarksSize);
}
for(std::map<int, Transform>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
{
if(iter->first <= 0)
{
@@ -4828,7 +4833,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second, covariance)));
landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
}
UDEBUG("Markers detected = %d", (int)markers.size());
}
@@ -5373,23 +5378,30 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(iter->second.id() > 0)
{
int landmarkId = -iter->first;
Link landmark(s->id(), landmarkId, Link::kLandmark, iter->second.pose(), iter->second.covariance().inv());
cv::Mat landmarkSize;
if(iter->second.size() > 0.0f)
{
landmarkSize = cv::Mat(1,1,CV_32FC1);
landmarkSize.at<float>(0,0) = iter->second.size();
std::pair<std::map<int, float>::iterator, bool> inserted=_landmarksSize.insert(std::make_pair(iter->first, iter->second.size()));
if(!inserted.second)
{
if(inserted.first->second != landmarkSize.at<float>(0,0))
{
UWARN("Trying to update landmark size buffer for landmark %d with size=%f but "
"it has already a different size set. Keeping old size (%f).",
-landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
}
}
}
Link landmark(s->id(), landmarkId, Link::kLandmark, iter->second.pose(), iter->second.covariance().inv(), landmarkSize);
s->addLandmark(landmark);
// Update landmark indexes
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(s->id());
// Update landmark index
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
if(nter!=_landmarksIndex.end())
{
nter->second.insert(landmarkId);
}
else
{
std::set<int> tmp;
tmp.insert(landmarkId);
_landmarksIndex.insert(std::make_pair(s->id(), tmp));
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.insert(s->id());
}
@@ -5397,7 +5409,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
std::set<int> tmp;
tmp.insert(s->id());
_landmarksInvertedIndex.insert(std::make_pair(landmarkId, tmp));
_landmarksIndex.insert(std::make_pair(landmarkId, tmp));
}
}
else
@@ -5606,20 +5618,24 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
{
int landmarkId = iter->first;
UASSERT(landmarkId < 0);
cv::Mat landmarkSize = iter->second.uncompressUserDataConst();
if(!landmarkSize.empty() && landmarkSize.type() == CV_32FC1 && landmarkSize.total()==1)
{
std::pair<std::map<int, float>::iterator, bool> inserted=_landmarksSize.insert(std::make_pair(-landmarkId, landmarkSize.at<float>(0,0)));
if(!inserted.second)
{
if(inserted.first->second != landmarkSize.at<float>(0,0))
{
UWARN("Trying to update landmark size buffer for landmark %d with size=%f but "
"it has already a different size set. Keeping old size (%f).",
-landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
}
}
}
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find((*i)->id());
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
if(nter!=_landmarksIndex.end())
{
nter->second.insert(landmarkId);
}
else
{
std::set<int> tmp;
tmp.insert(landmarkId);
_landmarksIndex.insert(std::make_pair((*i)->id(), tmp));
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.insert((*i)->id());
}
@@ -5627,7 +5643,7 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
{
std::set<int> tmp;
tmp.insert((*i)->id());
_landmarksInvertedIndex.insert(std::make_pair(landmarkId, tmp));
_landmarksIndex.insert(std::make_pair(landmarkId, tmp));
}
}
}

13
corelib/src/Rtabmap.cpp
View File

@@ -2239,11 +2239,11 @@ bool Rtabmap::process(
{
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
{
if(uContains(_memory->getLandmarksInvertedIndex(), iter->first) &&
_memory->getLandmarksInvertedIndex().find(iter->first)->second.size()>1)
if(uContains(_memory->getLandmarksIndex(), iter->first) &&
_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
{
landmarkDetected = iter->first;
landmarkDetectedNodesRef = _memory->getLandmarksInvertedIndex().find(iter->first)->second;
landmarkDetectedNodesRef = _memory->getLandmarksIndex().find(iter->first)->second;
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *landmarkDetectedNodesRef.begin());
break;
}
@@ -3146,8 +3146,8 @@ bool Rtabmap::process(
_lastLocalizationNodeId = newLocId!=0?newLocId:_lastLocalizationNodeId;
if(newLocId==0 && landmarkDetected!=0)
{
std::map<int, std::set<int> >::const_iterator iter = _memory->getLandmarksInvertedIndex().find(landmarkDetected);
if(iter!=_memory->getLandmarksInvertedIndex().end())
std::map<int, std::set<int> >::const_iterator iter = _memory->getLandmarksIndex().find(landmarkDetected);
if(iter!=_memory->getLandmarksIndex().end())
{
if(iter->second.size() && *iter->second.begin()!=signature->id())
{
@@ -4002,9 +4002,10 @@ void Rtabmap::deleteLastLocation()
}
}
void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses)
void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints)
{
_optimizedPoses = poses;
_constraints = constraints;
}
void Rtabmap::dumpData() const

1
corelib/src/pcl18/surface/impl/texture_mapping.hpp
View File

@@ -780,7 +780,6 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh
// CREATE UV MAP FOR CURRENT FACES
pcl::PointCloud<pcl::PointXY>::Ptr projections (new pcl::PointCloud<pcl::PointXY>);
std::vector<pcl::Vertices>::iterator current_face;
std::vector<bool> visibility;
visibility.resize (mesh.tex_polygons[current_cam].size ());
std::vector<UvIndex> indexes_uv_to_points;

8
corelib/src/rtflann/algorithms/kdtree_index.h
View File

@@ -362,7 +362,7 @@ public:
size_t knn,
const SearchParams& params) const
{
assert(queries.cols == veclen());
assert(queries.cols == veclen_);
assert(indices.rows >= queries.rows);
assert(dists.rows >= queries.rows);
assert(indices.cols >= knn);
@@ -430,7 +430,7 @@ public:
size_t knn,
const SearchParams& params) const
{
assert(queries.cols == veclen());
assert(queries.cols == veclen_);
bool use_heap;
if (params.use_heap==FLANN_Undefined) {
use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
@@ -503,7 +503,7 @@ public:
float radius,
const SearchParams& params) const
{
assert(queries.cols == veclen());
assert(queries.cols == veclen);
int count = 0;
size_t num_neighbors = std::min(indices.cols, dists.cols);
int max_neighbors = params.max_neighbors;
@@ -588,7 +588,7 @@ public:
float radius,
const SearchParams& params) const
{
assert(queries.cols == veclen());
assert(queries.cols == veclen_);
int count = 0;
Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);

2
corelib/src/util3d_surface.cpp
View File

@@ -1513,7 +1513,7 @@ cv::Mat mergeTextures(
UASSERT(textureSize%256 == 0);
UDEBUG("textureSize = %d", textureSize);
cv::Mat globalTextures;
if(mesh.tex_materials.size() > 1)
if(!mesh.tex_materials.empty())
{
std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,0));
cv::Size imageSize;