i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Increased version to 0.10.10. Database: added user_data field for links. DatabaseViewer: showing all scans of a local loop closure. Added parameters RGBD/PlanLinearVelocity and RGBD/PlanAngularVelocity. Updated how variance is set on links. MainWindow: added Send Waypoints action and goal can be either an ID or a label.

Browse Source
This commit is contained in:
matlabbe
2015-10-13 12:50:25 -04:00
parent aaf0eba7ba
commit 988e83cf1c
27 changed files with 1010 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/DBReader.h
+5 -2
View File
@@ -50,11 +50,13 @@ public:
DBReader(const std::string & databasePath,
float frameRate = 0.0f,
bool odometryIgnored = false,
bool ignoreGoalDelay = false);
bool ignoreGoalDelay = false,
bool goalsIgnored = false);
DBReader(const std::list<std::string> & databasePaths,
float frameRate = 0.0f,
bool odometryIgnored = false,
bool ignoreGoalDelay = false);
bool ignoreGoalDelay = false,
bool goalsIgnored = false);
virtual ~DBReader();
bool init(int startIndex=0);
@@ -70,6 +72,7 @@ private:
float _frameRate; // -1 = use Database stamps, 0 = inf
bool _odometryIgnored;
bool _ignoreGoalDelay;
bool _goalsIgnored;
DBDriver * _dbDriver;
UTimer _timer;
corelib/include/rtabmap/core/Graph.h
+7 -1
View File
@@ -307,7 +307,9 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
int toId,
const Memory * memory,
bool lookInDatabase = true,
bool updateNewCosts = false);
bool updateNewCosts = false,
float linearVelocity = 0.0f, // m/sec
float angularVelocity = 0.0f); // rad/sec
int RTABMAP_EXP findNearestNode(
const std::map<int, rtabmap::Transform> & nodes,
@@ -335,6 +337,10 @@ float RTABMAP_EXP computePathLength(
unsigned int fromIndex = 0,
unsigned int toIndex = 0);
std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
std::map<int, Transform> poses,
const std::multimap<int, Link> & links);
} /* namespace graph */
corelib/include/rtabmap/core/Link.h
+21 -81
View File
@@ -29,8 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define LINK_H_
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/core/core.hpp>
namespace rtabmap {
@@ -39,38 +37,20 @@ class Link
{
public:
enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
Link() :
from_(0),
to_(0),
type_(kUndef),
infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
{
}
Link();
Link(int from,
int to,
Type type,
const Transform & transform,
const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) :
from_(from),
to_(to),
transform_(transform),
type_(type)
{
setInfMatrix(infMatrix);
}
const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
const cv::Mat & userData = cv::Mat());
Link(int from,
int to,
Type type,
const Transform & transform,
double rotVariance,
double transVariance) :
from_(from),
to_(to),
transform_(transform),
type_(type)
{
setVariance(rotVariance, transVariance);
}
double transVariance,
const cv::Mat & userData = cv::Mat());
bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -79,69 +59,25 @@ public:
const Transform & transform() const {return transform_;}
Type type() const {return type_;}
const cv::Mat & infMatrix() const {return infMatrix_;}
double rotVariance() const
{
double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
UASSERT(min > 0.0);
return 1.0/min;
}
double transVariance() const
{
double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
UASSERT(min > 0.0);
return 1.0/min;
}
double rotVariance() const;
double transVariance() const;
void setFrom(int from) {from_ = from;}
void setTo(int to) {to_ = to;}
void setTransform(const Transform & transform) {transform_ = transform;}
void setType(Type type) {type_ = type;}
void setInfMatrix(const cv::Mat & infMatrix) {
UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
infMatrix_ = infMatrix;
}
void setVariance(double rotVariance, double transVariance) {
UASSERT(uIsFinite(rotVariance) && rotVariance>0);
UASSERT(uIsFinite(transVariance) && transVariance>0);
infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
infMatrix_.at<double>(0,0) = 1.0/transVariance;
infMatrix_.at<double>(1,1) = 1.0/transVariance;
infMatrix_.at<double>(2,2) = 1.0/transVariance;
infMatrix_.at<double>(3,3) = 1.0/rotVariance;
infMatrix_.at<double>(4,4) = 1.0/rotVariance;
infMatrix_.at<double>(5,5) = 1.0/rotVariance;
}
void setInfMatrix(const cv::Mat & infMatrix);
void setVariance(double rotVariance, double transVariance);
Link merge(const Link & link, Type outputType) const
{
UASSERT(to_ == link.from());
UASSERT(outputType != Link::kUndef);
UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
return Link(
from_,
link.to(),
outputType,
transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
}
void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
const cv::Mat & userDataRaw() const {return _userDataRaw;}
const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
void uncompressUserData();
cv::Mat uncompressUserDataConst() const;
Link inverse() const
{
return Link(
to_,
from_,
type_,
transform_.isNull()?Transform():transform_.inverse(),
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
}
Link merge(const Link & link, Type outputType) const;
Link inverse() const;
private:
int from_;
@@ -149,6 +85,10 @@ private:
Transform transform_;
Type type_;
cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
// user data
cv::Mat _userDataCompressed; // compressed data
cv::Mat _userDataRaw;
};
}
corelib/include/rtabmap/core/Parameters.h
+2 -1
View File
@@ -293,8 +293,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
RTABMAP_PARAM(RGBD, PlanLinearVelocity, float, 0.0, "Linear velocity (m/sec) used to compute path weights.");
RTABMAP_PARAM(RGBD, PlanAngularVelocity, float, 0.0, "Angular velocity (rad/sec) used to compute path weights.");
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
corelib/include/rtabmap/core/Rtabmap.h
+2 -1
View File
@@ -204,9 +204,10 @@ private:
float _reextractMaxDepth;
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _planVirtualLinks;
bool _goalsSavedInUserData;
int _pathStuckIterations;
float _pathLinearVelocity;
float _pathAngularVelocity;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
corelib/include/rtabmap/core/RtabmapEvent.h
+10
View File
@@ -234,6 +234,16 @@ private:
std::string _label;
};
class RtabmapGoalStatusEvent : public UEvent
{
public:
RtabmapGoalStatusEvent(int status):
UEvent(status){}
virtual ~RtabmapGoalStatusEvent() {}
virtual std::string getClassName() const {return std::string("RtabmapGoalStatusEvent");}
};
} // namespace rtabmap
#endif /* RTABMAPEVENT_H_ */
corelib/src/CMakeLists.txt
+1
View File
@@ -42,6 +42,7 @@ SET(SRC_FILES
SensorData.cpp
Graph.cpp
Compression.cpp
Link.cpp
Odometry.cpp
OdometryThread.cpp
corelib/src/DBDriverSqlite3.cpp
+59 -6
View File
@@ -1552,7 +1552,11 @@ void DBDriverSqlite3::loadLinksQuery(
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.8.4") >= 0)
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
}
@@ -1618,7 +1622,20 @@ void DBDriverSqlite3::loadLinksQuery(
{
rotVariance = sqlite3_column_double(ppStmt, index++);
transVariance = sqlite3_column_double(ppStmt, index++);
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
cv::Mat userDataCompressed;
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
const void * data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
}
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
@@ -1658,7 +1675,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
std::stringstream query;
int totalLinksLoaded = 0;
if(uStrNumCmp(_version, "0.8.4") >= 0)
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
<< "WHERE from_id = ? "
@@ -1702,10 +1725,22 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
toId = sqlite3_column_int(ppStmt, index++);
linkType = sqlite3_column_int(ppStmt, index++);
cv::Mat userDataCompressed;
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rotVariance = sqlite3_column_double(ppStmt, index++);
transVariance = sqlite3_column_double(ppStmt, index++);
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
const void * data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
}
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
@@ -1729,11 +1764,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance));
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance, userDataCompressed));
}
else // neighbor is 0, loop closures are 1 and 2 (child)
{
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance));
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance, userDataCompressed));
}
}
else
@@ -2516,7 +2551,11 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepLink() const
{
if(uStrNumCmp(_version, "0.8.4") >= 0)
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform, user_data) VALUES(?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform) VALUES(?,?,?,?,?,?);";
}
@@ -2571,6 +2610,20 @@ void DBDriverSqlite3::stepLink(
rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
// user_data
if(!link.userDataCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
corelib/src/DBReader.cpp
+8 -3
View File
@@ -45,11 +45,13 @@ namespace rtabmap {
DBReader::DBReader(const std::string & databasePath,
float frameRate,
bool odometryIgnored,
bool ignoreGoalDelay) :
bool ignoreGoalDelay,
bool goalsIgnored) :
_paths(uSplit(databasePath, ';')),
_frameRate(frameRate),
_odometryIgnored(odometryIgnored),
_ignoreGoalDelay(ignoreGoalDelay),
_goalsIgnored(goalsIgnored),
_dbDriver(0),
_currentId(_ids.end()),
_previousStamp(0)
@@ -59,11 +61,13 @@ DBReader::DBReader(const std::string & databasePath,
DBReader::DBReader(const std::list<std::string> & databasePaths,
float frameRate,
bool odometryIgnored,
bool ignoreGoalDelay) :
bool ignoreGoalDelay,
bool goalsIgnored) :
_paths(databasePaths),
_frameRate(frameRate),
_odometryIgnored(odometryIgnored),
_ignoreGoalDelay(ignoreGoalDelay),
_goalsIgnored(goalsIgnored),
_dbDriver(0),
_currentId(_ids.end()),
_previousStamp(0)
@@ -159,7 +163,8 @@ void DBReader::mainLoop()
{
odom.data().setStamp(UTimer::now());
}
if(odom.data().userDataRaw().type() == CV_8SC1 &&
if(!_goalsIgnored &&
odom.data().userDataRaw().type() == CV_8SC1 &&
odom.data().userDataRaw().cols >= 7 && // including null str ending
odom.data().userDataRaw().rows == 1 &&
memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
corelib/src/Graph.cpp
+128 -4
View File
@@ -1890,6 +1890,7 @@ public:
void setFromId(int fromId) {fromId_ = fromId;}
void setCostSoFar(float costSoFar) {costSoFar_ = costSoFar;}
void setDistToEnd(float distToEnd) {distToEnd_ = distToEnd;}
void setPose(const Transform & pose) {pose_ = pose;}
private:
int id_;
@@ -2021,12 +2022,21 @@ std::list<std::pair<int, Transform> > computePath(
int toId,
const Memory * memory,
bool lookInDatabase,
bool updateNewCosts)
bool updateNewCosts,
float linearVelocity, // m/sec
float angularVelocity) // rad/sec
{
UASSERT(memory!=0);
UASSERT(fromId>=0);
UASSERT(toId>=0);
std::list<std::pair<int, Transform> > path;
UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
fromId,
toId,
lookInDatabase?1:0,
updateNewCosts?1:0,
linearVelocity,
angularVelocity);
std::multimap<int, Link> allLinks;
if(lookInDatabase)
@@ -2097,11 +2107,34 @@ std::list<std::pair<int, Transform> > computePath(
}
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
Transform nextPose = currentNode->pose()*iter->second.transform();
float cost = 0.0f;
if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
{
// use distance only
cost = iter->second.transform().getNorm();
}
else // use time
{
if(linearVelocity > 0.0f)
{
cost += iter->second.transform().getNorm()/linearVelocity;
}
if(angularVelocity > 0.0f)
{
Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
float angle = pcl::getAngle3D(v1, v2);
cost += angle / angularVelocity;
}
}
std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
if(nodeIter == nodes.end())
{
Node n(iter->second.to(), currentNode->id(), currentNode->pose()*iter->second.transform());
n.setCostSoFar(currentNode->costSoFar() + iter->second.transform().getNorm());
Node n(iter->second.to(), currentNode->id(), nextPose);
n.setCostSoFar(currentNode->costSoFar() + cost);
nodes.insert(std::make_pair(iter->second.to(), n));
if(updateNewCosts)
{
@@ -2114,9 +2147,12 @@ std::list<std::pair<int, Transform> > computePath(
}
else if(updateNewCosts && nodeIter->second.isOpened())
{
float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
float newCostSoFar = currentNode->costSoFar() + cost;
if(nodeIter->second.costSoFar() > newCostSoFar)
{
// update pose with new link
nodeIter->second.setPose(nextPose);
// update the cost in the priority queue
for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
{
@@ -2132,6 +2168,61 @@ std::list<std::pair<int, Transform> > computePath(
}
}
}
// Debugging stuff
if(ULogger::level() == ULogger::kDebug)
{
std::stringstream stream;
std::vector<int> linkTypes(Link::kUndef, 0);
std::list<std::pair<int, Transform> >::const_iterator previousIter = path.end();
float length = 0.0f;
for(std::list<std::pair<int, Transform> >::const_iterator iter=path.begin(); iter!=path.end();++iter)
{
if(iter!=path.begin())
{
stream << ",";
}
if(previousIter!=path.end())
{
//UDEBUG("current %d = %s", iter->first, iter->second.prettyPrint().c_str());
if(allLinks.size())
{
std::multimap<int, Link>::iterator jter = graph::findLink(allLinks, previousIter->first, iter->first);
if(jter != allLinks.end())
{
//Transform nextPose = iter->second;
//Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-previousIter->second.x(), nextPose.y()-previousIter->second.y(), nextPose.z()-previousIter->second.z(), 1.0f);
//Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
//float angle = pcl::getAngle3D(v1, v2);
//float cost = angle ;
//UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost);
UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()<Link::kUndef);
++linkTypes[jter->second.type()];
stream << "[" << jter->second.type() << "]";
length += jter->second.transform().getNorm();
}
}
}
stream << iter->first;
previousIter=iter;
}
UDEBUG("Path (%f m) = [%s]", length, stream.str().c_str());
std::stringstream streamB;
for(unsigned int i=0; i<linkTypes.size(); ++i)
{
if(i > 0)
{
streamB << " ";
}
streamB << i << "=" << linkTypes[i];
}
UDEBUG("Link types = %s", streamB.str().c_str());
}
return path;
}
@@ -2302,6 +2393,39 @@ float computePathLength(
return length;
}
// return all paths linked only by neighbor links
std::list<std::map<int, Transform> > getPaths(
std::map<int, Transform> poses,
const std::multimap<int, Link> & links)
{
std::list<std::map<int, Transform> > paths;
if(poses.size() && links.size())
{
// Segment poses connected only by neighbor links
while(poses.size())
{
std::map<int, Transform> path;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
{
std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
if(path.size() == 0 || (jter != links.end() && jter->second.type() == Link::kNeighbor))
{
path.insert(*iter);
poses.erase(iter++);
}
else
{
break;
}
}
UASSERT(path.size());
paths.push_back(path);
}
}
return paths;
}
} /* namespace graph */
} /* namespace rtabmap */
corelib/src/Link.cpp
+198
View File
@@ -0,0 +1,198 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Link.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/core/Compression.h>
namespace rtabmap {
Link::Link() :
from_(0),
to_(0),
type_(kUndef),
infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
{
}
Link::Link(int from,
int to,
Type type,
const Transform & transform,
const cv::Mat & infMatrix,
const cv::Mat & userData) :
from_(from),
to_(to),
transform_(transform),
type_(type)
{
setInfMatrix(infMatrix);
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
Link::Link(int from,
int to,
Type type,
const Transform & transform,
double rotVariance,
double transVariance,
const cv::Mat & userData) :
from_(from),
to_(to),
transform_(transform),
type_(type)
{
setVariance(rotVariance, transVariance);
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
double Link::rotVariance() const
{
double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
UASSERT(min > 0.0);
return 1.0/min;
}
double Link::transVariance() const
{
double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
UASSERT(min > 0.0);
return 1.0/min;
}
void Link::setInfMatrix(const cv::Mat & infMatrix) {
UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
infMatrix_ = infMatrix;
}
void Link::setVariance(double rotVariance, double transVariance) {
UASSERT(uIsFinite(rotVariance) && rotVariance>0);
UASSERT(uIsFinite(transVariance) && transVariance>0);
infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
infMatrix_.at<double>(0,0) = 1.0/transVariance;
infMatrix_.at<double>(1,1) = 1.0/transVariance;
infMatrix_.at<double>(2,2) = 1.0/transVariance;
infMatrix_.at<double>(3,3) = 1.0/rotVariance;
infMatrix_.at<double>(4,4) = 1.0/rotVariance;
infMatrix_.at<double>(5,5) = 1.0/rotVariance;
}
void Link::setUserDataRaw(const cv::Mat & userDataRaw)
{
if(!_userDataRaw.empty())
{
UWARN("Writing new user data over existing user data. This may result in data loss.");
}
_userDataRaw = userDataRaw;
}
void Link::setUserData(const cv::Mat & userData)
{
if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
{
UWARN("Writing new user data over existing user data. This may result in data loss.");
}
_userDataRaw = cv::Mat();
_userDataCompressed = cv::Mat();
if(!userData.empty())
{
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
_userDataCompressed = compressData2(userData);
}
}
}
void Link::uncompressUserData()
{
cv::Mat dataRaw = uncompressUserDataConst();
if(!dataRaw.empty() && _userDataRaw.empty())
{
_userDataRaw = dataRaw;
}
}
cv::Mat Link::uncompressUserDataConst() const
{
if(!_userDataRaw.empty())
{
return _userDataRaw;
}
return uncompressData(_userDataCompressed);
}
Link Link::merge(const Link & link, Type outputType) const
{
UASSERT(to_ == link.from());
UASSERT(outputType != Link::kUndef);
UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
return Link(
from_,
link.to(),
outputType,
transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
}
Link Link::inverse() const
{
return Link(
to_,
from_,
type_,
transform_.isNull()?Transform():transform_.inverse(),
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
}
}
corelib/src/Memory.cpp
+99 -66
View File
@@ -48,7 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/Statistics.h"
#include "rtabmap/core/Compression.h"
@@ -1090,7 +1090,8 @@ std::map<int, float> Memory::getNeighborsIdRadius(
for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
{
if(!uContains(ids, iter->first) &&
uContains(optimizedPoses, iter->first))
uContains(optimizedPoses, iter->first) &&
iter->second.type()!=Link::kVirtualClosure)
{
const Transform & t = optimizedPoses.at(iter->first);
UASSERT(!t.isNull());
@@ -2224,7 +2225,7 @@ Transform Memory::computeVisualTransform(
}
if(varianceOut)
{
*varianceOut = variance;
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
@@ -2428,7 +2429,7 @@ Transform Memory::computeIcpTransform(
if(varianceOut)
{
*varianceOut = variance;
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
if(correspondencesOut)
{
@@ -2518,7 +2519,7 @@ Transform Memory::computeIcpTransform(
bool hasConverged = false;
float correspondencesRatio = 0.0f;
int correspondences = 0;
double variance = 1;
double variance = 1.0;
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
icpT = util3d::icp2D(
newCloudVoxelized,
@@ -2567,7 +2568,6 @@ Transform Memory::computeIcpTransform(
newCloud = newCloudRegistered;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
util3d::computeVarianceAndCorrespondences(
newCloud,
oldCloud,
@@ -2592,12 +2592,12 @@ Transform Memory::computeIcpTransform(
hasConverged?"true":"false",
variance,
correspondences,
(int)(newS.sensorData().laserScanMaxPts()),
newS.sensorData().laserScanMaxPts()?newS.sensorData().laserScanMaxPts():(int)(newCloud->size()>oldCloud->size()?newCloud->size():oldCloud->size()),
correspondencesRatio*100.0f);
if(varianceOut)
{
*varianceOut = variance;
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
if(correspondencesOut)
{
@@ -2627,6 +2627,21 @@ Transform Memory::computeIcpTransform(
hasConverged?"true":"false", variance);
UINFO(msg.c_str());
}
// still compute the variance for information
if(variance == 1 && varianceOut)
{
util3d::computeVarianceAndCorrespondences(
newCloudVoxelized,
oldCloudVoxelized,
_icpMaxCorrespondenceDistance,
variance,
correspondences);
if(variance > 0)
{
*varianceOut = variance;
}
}
}
else
{
@@ -2748,65 +2763,83 @@ Transform Memory::computeScanMatchingTransform(
if(!icpT.isNull() && hasConverged)
{
if(_icp2VoxelSize <= _laserScanVoxelSize)
float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
if((_icpMaxTranslation>0.0f &&
(fabs(ix) > _icpMaxTranslation ||
fabs(iy) > _icpMaxTranslation ||
fabs(iz) > _icpMaxTranslation))
||
(_icpMaxRotation>0.0f &&
(fabs(iroll) > _icpMaxRotation ||
fabs(ipitch) > _icpMaxRotation ||
fabs(iyaw) > _icpMaxRotation)))
{
newCloud = util3d::transformPointCloud(newCloud, icpT);
}
else
{
newCloud = newCloudRegistered;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
double v = 1;
util3d::computeVarianceAndCorrespondences(
newCloud,
assembledOldClouds,
_icpMaxCorrespondenceDistance,
v,
correspondences);
if(variance)
{
*variance = v;
}
// verify if there enough correspondences
float correspondencesRatio = 0.0f;
if(newS->sensorData().laserScanMaxPts())
{
correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts());
}
else
{
UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
newS->id());
correspondencesRatio = float(correspondences)/float(newCloud->size());
}
UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",
variance?*variance:-1,
correspondences,
(int)newCloud->size(),
correspondencesRatio*100.0f);
if(inliers)
{
*inliers = correspondences;
}
if(correspondencesRatio >= _icp2CorrespondenceRatio)
{
transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
}
else
{
msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)",
variance?*variance:-1,
correspondences,
(int)newCloud->size(),
correspondencesRatio);
msg = uFormat("Cannot compute transform (ICP correction too large)");
UINFO(msg.c_str());
}
else
{
if(_icp2VoxelSize <= _laserScanVoxelSize)
{
newCloud = util3d::transformPointCloud(newCloud, icpT);
}
else
{
newCloud = newCloudRegistered;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
double v = 1;
util3d::computeVarianceAndCorrespondences(
newCloud,
assembledOldClouds,
_icpMaxCorrespondenceDistance,
v,
correspondences);
if(variance)
{
*variance = v>0.0f?v:0.0001; // epsilon if exact transform
}
// verify if there enough correspondences
float correspondencesRatio = 0.0f;
if(newS->sensorData().laserScanMaxPts())
{
correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts());
}
else
{
UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
newS->id());
correspondencesRatio = float(correspondences)/float(newCloud->size());
}
UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",
v,
correspondences,
newS->sensorData().laserScanMaxPts()?newS->sensorData().laserScanMaxPts():(int)newCloud->size(),
correspondencesRatio*100.0f);
if(inliers)
{
*inliers = correspondences;
}
if(correspondencesRatio >= _icp2CorrespondenceRatio)
{
transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
}
else
{
msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)",
variance?*variance:-1,
correspondences,
newS->sensorData().laserScanMaxPts()?newS->sensorData().laserScanMaxPts():(int)newCloud->size(),
correspondencesRatio);
UINFO(msg.c_str());
}
}
}
else
{
@@ -4292,7 +4325,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
{
UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion.");
UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
}
@@ -4372,7 +4405,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
cameraModels,
id,
0,
ctUserData.getCompressedData()));
ctUserData.getCompressedData()));
}
s->setWords(words);
s->setWords3(words3D);
corelib/src/OdometryMono.cpp
+1
View File
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
corelib/src/Rtabmap.cpp
+135 -84
View File
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/Compression.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UFile.h>
@@ -109,9 +110,10 @@ Rtabmap::Rtabmap() :
_reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()),
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -415,9 +417,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1081,12 +1084,14 @@ bool Rtabmap::process(
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateLink(signature->id(), oldId, t, variance>0?variance:0.0001, variance>0?variance:0.0001);
UASSERT(variance > 0.0);
_memory->updateLink(signature->id(), oldId, t, variance, variance);
if(_optimizeFromGraphEnd)
{
@@ -1107,6 +1112,11 @@ bool Rtabmap::process(
else
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
if(variance > 0)
{
double sqrtVar = sqrt(variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
}
}
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
@@ -1191,7 +1201,8 @@ bool Rtabmap::process(
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001)))
UASSERT(variance > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1807,7 +1818,8 @@ bool Rtabmap::process(
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001));
UASSERT(variance > 0.0);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
if(!rejectedHypothesis)
{
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1850,41 +1862,45 @@ bool Rtabmap::process(
//
// 1) compare visually with nearest locations
//
float r = _localRadius;
if(_localPathFilteringRadius > 0 && _localPathFilteringRadius<_localRadius)
{
r = _localPathFilteringRadius;
}
UDEBUG("Proximity detection (local loop closure in SPACE using matching images)");
std::map<int, float> nearestIds;
if(_memory->isIncremental())
{
nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth);
nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _localDetectMaxGraphDepth);
}
else
{
nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, r);
nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
}
UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
std::map<int, Transform> nearestPoses;
for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
{
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
{
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
}
}
UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
// segment poses by paths, only one detection per path
std::list<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
UDEBUG("nearestPaths=%d", (int)nearestPaths.size());
for(std::list<std::map<int, Transform> >::const_iterator iter=nearestPaths.begin();
iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
++iter)
{
std::map<int, Transform> & path = *iter;
const std::map<int, Transform> & path = *iter;
UASSERT(path.size());
//find the nearest pose on the path
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
UASSERT(nearestId > 0);
// nearest pose must not be linked to current location, and not in STM
// nearest pose must not be linked to current location and enough
if(!signature->hasLink(nearestId) &&
_memory->getStMem().find(nearestId) == _memory->getStMem().end())
(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
{
double variance = 1.0;
Transform transform;
@@ -1953,17 +1969,27 @@ bool Rtabmap::process(
}
if(!transform.isNull())
{
UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius)
{
++localSpaceClosuresAddedVisually;
lastLocalSpaceClosureId = nearestId;
UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
{
++localSpaceClosuresAddedVisually;
lastLocalSpaceClosureId = nearestId;
}
}
else
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _localPathFilteringRadius);
}
}
}
@@ -1972,6 +1998,7 @@ bool Rtabmap::process(
//
// 2) compare locally with nearest locations by scan matching
//
UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
if( !signature->sensorData().laserScanCompressed().empty() &&
(_memory->isIncremental() || lastLocalSpaceClosureId == 0))
{
@@ -1979,18 +2006,10 @@ bool Rtabmap::process(
// closures if we are already localized by at least one
// local visual closure above.
std::map<int, Transform> forwardPoses;
forwardPoses = this->getForwardWMPoses(
signature->id(),
0,
_localRadius,
_localDetectMaxGraphDepth);
localSpacePaths = (int)nearestPaths.size();
std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
localSpacePaths = (int)forwardPaths.size();
for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin();
iter!=forwardPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
++iter)
{
std::map<int, Transform> & path = *iter;
@@ -1999,6 +2018,7 @@ bool Rtabmap::process(
//find the nearest pose on the path
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
UASSERT(nearestId > 0);
UDEBUG("Path %d distance=%fm", nearestId, _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
// nearest pose must be close and not linked to current location
if(!signature->hasLink(nearestId) &&
@@ -2021,7 +2041,7 @@ bool Rtabmap::process(
if(_localPathFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin());
@@ -2036,28 +2056,67 @@ bool Rtabmap::process(
//The nearest will be the reference for a loop closure transform
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, 0);
double variance = 1.0;
Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
if(!transform.isNull())
{
UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
// set Identify covariance for laser scan matching only
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++localSpaceClosuresAddedByICPOnly;
// no local loop closure added visually
if(localSpaceClosuresAddedVisually == 0 && _loopClosureHypothesis.first == 0)
if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius)
{
lastLocalSpaceClosureId = nearestId;
UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
cv::Mat scanMatchingIds;
bool _scanMatchingIdsSavedInUserData = true;
if(_scanMatchingIdsSavedInUserData)
{
std::stringstream stream;
stream << "SCANS:";
for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
{
if(iter->first!=signature->id())
{
if(iter != path.begin())
{
stream << ";";
}
stream << uNumber2Str(iter->first);
}
}
std::string scansStr = stream.str();
scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
scanMatchingIds = compressData2(scanMatchingIds); // compressed
}
// set Identify covariance for laser scan matching only
UASSERT(variance>0.0);
double sqrtVar = sqrt(variance);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++localSpaceClosuresAddedByICPOnly;
// no local loop closure added visually
if(localSpaceClosuresAddedVisually == 0 && _loopClosureHypothesis.first == 0)
{
lastLocalSpaceClosureId = nearestId;
}
}
else
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _localPathFilteringRadius);
}
}
}
}
}
else
{
UDEBUG("Path %d ignored", nearestId);
}
}
}
}
@@ -2158,17 +2217,21 @@ bool Rtabmap::process(
const Link * maxLinearLink = 0;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
if(linearError > maxLinearError)
// ignore links with high variance
if(iter->second.transVariance() < 1.0)
{
maxLinearError = linearError;
maxLinearLink = &iter->second;
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
if(linearError > maxLinearError)
{
maxLinearError = linearError;
maxLinearLink = &iter->second;
}
}
}
@@ -2177,12 +2240,13 @@ bool Rtabmap::process(
UWARN("Rejecting all added loop closures (%d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error of %f m (edge %d->%d). The "
"a maximum graph error of %f m (edge %d->%d, type=%d). The "
"maximum error parameter is %f m.",
(int)loopClosureLinksAdded.size(),
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
_optimizationMaxLinearError);
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
@@ -2819,7 +2883,6 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
return poses;
}
// Get paths in front of the robot, returned optimized poses
std::list<std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses) const
{
std::list<std::map<int, Transform> > paths;
@@ -3234,12 +3297,14 @@ bool Rtabmap::computePath(int targetNode, bool global)
}
if(currentNode && targetNode)
{
std::list<std::pair<int, Transform> > path = graph::computePath(
currentNode,
targetNode,
_memory,
global);
global,
false,
_pathLinearVelocity,
_pathAngularVelocity);
//transform in current referential
Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity());
@@ -3353,20 +3418,6 @@ bool Rtabmap::computePath(const Transform & targetPose)
currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
}
// Add links between neighbor nodes in the goal radius.
if(_planVirtualLinks)
{
std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
{
if(graph::findLink(links, iter->first, iter->second) == links.end())
{
links.insert(*iter);
links.insert(std::make_pair(iter->second, iter->first)); // <->
}
}
}
UINFO("Computing path from location %d to %d", currentNode, nearestId);
UTimer timer;
_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId));
@@ -3531,7 +3582,7 @@ void Rtabmap::updateGoalIndex()
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 1, 1)); // on the optimized path, set Identity variance
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}
corelib/src/RtabmapThread.cpp
+7
View File
@@ -463,12 +463,19 @@ void RtabmapThread::process()
{
if(_rtabmap->getMemory())
{
bool wasPlanning = _rtabmap->getPath().size()>0;
if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
{
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
ULOGGER_DEBUG("posting statistics_ event...");
this->post(new RtabmapEvent(stats));
if(wasPlanning && _rtabmap->getPath().size() == 0)
{
// Goal reached or failed
this->post(new RtabmapGoalStatusEvent(_rtabmap->getPathStatus()));
}
}
}
else
corelib/src/Signature.cpp
+1
View File
@@ -39,6 +39,7 @@ namespace rtabmap
Signature::Signature() :
_id(0), // invalid id
_mapId(-1),
_stamp(0.0),
_weight(0),
_saved(false),
_modified(true),
corelib/src/resources/DatabaseSchema.sql.in
+3 -2
View File
@@ -40,10 +40,11 @@ CREATE TABLE Data (
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
rot_variance FLOAT NOT NULL,
trans_variance FLOAT NOT NULL,
transform BLOB,
user_data BLOB, -- compressed data (User data)
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
@@ -82,7 +83,7 @@ CREATE TABLE Statistics (
);
CREATE TABLE Admin (
version INTEGER,
version TEXT,
time_enter DATE
);
corelib/src/util2d.cpp
+4 -1
View File
@@ -281,7 +281,10 @@ cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
}
if(countOverMax)
{
UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
UWARN("Depth conversion error, %d depth values ignored because "
"they are over the maximum depth allowed (65535 mm). Is the depth "
"image really in meters? 32 bits images should be in meters, "
"and 16 bits should be in mm.", countOverMax);
}
}
return depth16U;
corelib/src/util3d_registration.cpp
+4 -4
View File
@@ -233,8 +233,8 @@ void computeVarianceAndCorrespondences(
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
est->setInputTarget(cloudA);
est->setInputSource(cloudB);
est->setInputTarget(cloudB);
est->setInputSource(cloudA);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
@@ -266,8 +266,8 @@ void computeVarianceAndCorrespondences(
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloudA);
est->setInputSource(cloudB);
est->setInputTarget(cloudB);
est->setInputSource(cloudA);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);