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

Merge branch 'dronecrew-g2o_prior' into devel

Browse Source
This commit is contained in:
matlabbe
2017-05-22 21:14:02 -04:00
parent e361abcecd e8af8f7792
commit c8e1060ea4
14 changed files with 401 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Link.h
View File

@@ -46,7 +46,8 @@ public:
kUserClosure,
kVirtualClosure,
kNeighborMerged,
kUndef};
kPosePrior,
kUndef = 99};
Link();
Link(int from,
int to,

7
corelib/include/rtabmap/core/SensorData.h
View File

@@ -221,6 +221,10 @@ public:
void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
const Transform & groundTruth() const {return groundTruth_;}
void setGlobalPose(const Transform & pose, const cv::Mat & covariance) {globalPose_ = pose; globalPoseCovariance_ = covariance;}
const Transform & globalPose() const {return globalPose_;}
const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
long getMemoryUsed() const; // Return memory usage in Bytes
private:
@@ -258,6 +262,9 @@ private:
cv::Mat _descriptors;
Transform groundTruth_;
Transform globalPose_;
cv::Mat globalPoseCovariance_; // 6x6 double
};
}

2
corelib/src/DBDriver.cpp
View File

@@ -1127,7 +1127,7 @@ void DBDriver::generateGraph(
weightNeighbor,
colorG);
}
else
else if(iter->first != id)
{
//child
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",

101
corelib/src/Graph.cpp
View File

@@ -1456,61 +1456,64 @@ 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)
if(iter->second.from() != iter->second.to())
{
// use distance only
cost = iter->second.transform().getNorm();
}
else // use time
{
if(linearVelocity > 0.0f)
Transform nextPose = currentNode->pose()*iter->second.transform();
float cost = 0.0f;
if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
{
cost += iter->second.transform().getNorm()/linearVelocity;
// use distance only
cost = iter->second.transform().getNorm();
}
if(angularVelocity > 0.0f)
else // use time
{
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(), nextPose);
n.setCostSoFar(currentNode->costSoFar() + cost);
nodes.insert(std::make_pair(iter->second.to(), n));
if(updateNewCosts)
{
pqmap.insert(std::make_pair(n.totalCost(), n.id()));
}
else
{
pq.push(Pair(n.id(), n.totalCost()));
}
}
else if(updateNewCosts && nodeIter->second.isOpened())
{
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)
if(linearVelocity > 0.0f)
{
if(mapIter->second == nodeIter->first)
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(), nextPose);
n.setCostSoFar(currentNode->costSoFar() + cost);
nodes.insert(std::make_pair(iter->second.to(), n));
if(updateNewCosts)
{
pqmap.insert(std::make_pair(n.totalCost(), n.id()));
}
else
{
pq.push(Pair(n.id(), n.totalCost()));
}
}
else if(updateNewCosts && nodeIter->second.isOpened())
{
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)
{
pqmap.erase(mapIter);
nodeIter->second.setCostSoFar(newCostSoFar);
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
break;
if(mapIter->second == nodeIter->first)
{
pqmap.erase(mapIter);
nodeIter->second.setCostSoFar(newCostSoFar);
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
break;
}
}
}
}

89
corelib/src/Memory.cpp
View File

@@ -805,6 +805,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
if(!merge)
{
merge = iter->second.to() < s->id() && // should be a parent->child link
iter->second.to() != iter->second.from() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.userDataCompressed().empty() &&
@@ -958,6 +959,7 @@ std::map<int, Link> Memory::getLoopClosureLinks(
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->second.type() != Link::kUndef)
{
loopClosures.insert(*iter);
@@ -1843,27 +1845,30 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Signature * sTo = this->_getSignature(iter->first);
// neighbor to s
UASSERT_MSG(sTo!=0,
uFormat("A neighbor (%d) of the deleted location %d is "
"not found in WM/STM! Are you deleting a location "
"outside the STM?", iter->first, s->id()).c_str());
if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
if(iter->second.from() != iter->second.to())
{
UWARN("Link %d of %d is newer, removing neighbor link "
"may split the map!",
iter->first, s->id());
}
Signature * sTo = this->_getSignature(iter->first);
// neighbor to s
UASSERT_MSG(sTo!=0,
uFormat("A neighbor (%d) of the deleted location %d is "
"not found in WM/STM! Are you deleting a location "
"outside the STM?", iter->first, s->id()).c_str());
// child
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
{
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
}
if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
{
UWARN("Link %d of %d is newer, removing neighbor link "
"may split the map!",
iter->first, s->id());
}
sTo->removeLink(s->id());
// child
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
{
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
}
sTo->removeLink(s->id());
}
}
s->removeLinks(); // remove all links
@@ -2089,6 +2094,7 @@ void Memory::removeLink(int oldId, int newId)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->first < newS->id())
{
noChildrenAnymore = false;
@@ -2694,7 +2700,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
{
childIds.insert(*iter);
}
else
else if(iter->second.from() != iter->second.to())
{
loopIds.insert(*iter);
}
@@ -2724,8 +2730,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
void Memory::rehearsal(Signature * signature, Statistics * stats)
{
UTimer timer;
if(signature->getLinks().size() != 1 ||
signature->isBadSignature())
if(signature->isBadSignature())
{
return;
}
@@ -2790,7 +2795,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
if(iter != oldS->getLinks().end() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
iter->second.type() != Link::kNeighborMerged &&
iter->second.from() != iter->second.to())
{
// do nothing, already merged
UWARN("already merged, old=%d, new=%d", oldId, newId);
@@ -2804,7 +2810,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
bool fullMerge;
bool intermediateMerge = false;
if(!newS->getLinks().begin()->second.transform().isNull())
if(!newS->getLinks().empty() && !newS->getLinks().begin()->second.transform().isNull())
{
// we are in metric SLAM mode:
// 1) Normal merge if not moving AND has direct link
@@ -2844,22 +2850,25 @@ bool Memory::rehearsalMerge(int oldId, int newId)
const std::map<int, Link> & links = oldS->getLinks();
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
Link link = iter->second;
Link mergedLink = newToOldLink.merge(link, link.type());
UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
Signature * s = this->_getSignature(link.to());
if(s)
if(iter->second.from() != iter->second.to())
{
// modify neighbor "from"
s->removeLink(oldS->id());
s->addLink(mergedLink.inverse());
Link link = iter->second;
Link mergedLink = newToOldLink.merge(link, link.type());
UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
newS->addLink(mergedLink);
}
else
{
UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
Signature * s = this->_getSignature(link.to());
if(s)
{
// modify neighbor "from"
s->removeLink(oldS->id());
s->addLink(mergedLink.inverse());
newS->addLink(mergedLink);
}
else
{
UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
}
}
}
newS->setLabel(oldS->getLabel());
@@ -3847,6 +3856,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
// prior
if(!isIntermediateNode && !data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
{
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
}
return s;
}

270
corelib/src/OptimizerG2O.cpp
View File

@@ -47,6 +47,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/linear_solver.h"
#include "g2o/types/sba/types_sba.h"
#include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/core/robust_kernel_impl.h"
#ifdef G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
@@ -56,10 +58,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#endif
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam2d/vertex_se2.h"
#include "g2o/types/slam2d/edge_se2.h"
enum {
PARAM_OFFSET=0,
};
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
@@ -165,6 +167,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
g2o::ParameterSE3Offset* odomOffset = new g2o::ParameterSE3Offset();
odomOffset->setId(PARAM_OFFSET);
optimizer.addParameter(odomOffset);
SlamBlockSolver * blockSolver = 0;
@@ -211,6 +216,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
UDEBUG("fill poses to g2o...");
std::map<int, std::pair<Transform, cv::Mat> > geoPoses; // pose / information matrix
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
@@ -255,127 +261,175 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::HyperGraph::Edge * edge = 0;
#ifdef RTABMAP_VERTIGO
VertexSwitchLinear * v = 0;
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
if(id1 == id2)
{
// For loop closure links, add switchable edges
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
v = new VertexSwitchLinear();
v->setEstimate(1.0);
v->setId(vertigoVertexId++);
UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
EdgeSwitchPrior * prior = new EdgeSwitchPrior();
prior->setMeasurement(1.0);
prior->setVertex(0, v);
UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
}
#endif
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
if(isSlam2d())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE2Switchable * e = new EdgeSE2Switchable();
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else
#endif
{
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d pose;
pose = a.rotation();
pose.translation() = a.translation();
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
priorEdge->setInformation(information);
edge = priorEdge;
}
}
else
{
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d constraint;
constraint = a.rotation();
constraint.translation() = a.translation();
#ifdef RTABMAP_VERTIGO
VertexSwitchLinear * v = 0;
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE3Switchable * e = new EdgeSE3Switchable();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
// For loop closure links, add switchable edges
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
v = new VertexSwitchLinear();
v->setEstimate(1.0);
v->setId(vertigoVertexId++);
UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
EdgeSwitchPrior * prior = new EdgeSwitchPrior();
prior->setMeasurement(1.0);
prior->setVertex(0, v);
UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
}
#endif
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE2Switchable * e = new EdgeSE2Switchable();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
}
else
#endif
{
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
}
}
else
#endif
{
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d constraint;
constraint = a.rotation();
constraint.translation() = a.translation();
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE3Switchable * e = new EdgeSE3Switchable();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
else
#endif
{
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
}
}
@@ -384,6 +438,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
delete edge;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
}
}
UDEBUG("Initial optimization...");

5
corelib/src/OptimizerGTSAM.cpp
View File

@@ -134,6 +134,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
int id1 = iter->second.from();
int id2 = iter->second.to();
if(id1 == id2)
{
// not supporting pose prior
continue;
}
UASSERT(!iter->second.transform().isNull());

36
corelib/src/OptimizerTORO.cpp
View File

@@ -126,15 +126,19 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph2::Transformation t(p);
if (!pg2.addEdge(v1, v2, t, inf))
if(id1 != id2)
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph2::Transformation t(p);
if (!pg2.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
//else // not supporting pose prior
}
}
else
@@ -154,15 +158,19 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg3.addEdge(v1, v2, t, inf))
if(id1 != id2)
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg3.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
//else // not supporting pose prior
}
}
UDEBUG("buildMST... root=%d", rootId);

41
corelib/src/Rtabmap.cpp
View File

@@ -1033,7 +1033,7 @@ bool Rtabmap::process(
// Minimum displacement required to add to Memory
//============================================================
const std::map<int, Link> & links = signature->getLinks();
if(links.size() == 1)
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
// don't do this if there are intermediate nodes
const Signature * s = _memory->getSignature(links.begin()->second.to());
@@ -1061,7 +1061,8 @@ bool Rtabmap::process(
// Update optimizedPoses with the newly added node
Transform newPose;
if(_neighborLinkRefining &&
signature->getLinks().size() == 1 &&
signature->getLinks().size() &&
signature->getLinks().begin()->second.type() == Link::kNeighbor &&
_memory->isIncremental() && // ignore pose matching in localization mode
rehearsedId == 0) // don't do it if rehearsal happened
{
@@ -1166,7 +1167,7 @@ bool Rtabmap::process(
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(signature->getLinks().size() == 1 &&
if(signature->getLinks().size() &&
signature->getLinks().begin()->second.type() == Link::kNeighbor)
{
// link should be old to new
@@ -1297,7 +1298,7 @@ bool Rtabmap::process(
//============================================================
// Bayes filter update
//============================================================
int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0;
int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
// Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure
if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
{
@@ -1383,7 +1384,7 @@ bool Rtabmap::process(
{
float loopThr = _loopThr;
if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
signature->getLinks().size() == 0 && // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
_rgbdSlamMode)
@@ -2154,14 +2155,16 @@ bool Rtabmap::process(
(_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() ||
signature->hasLink(signature->id()) || // prior edge
proximityDetectionsInTimeFound>0 ||
((_memory->isIncremental() || signature->getLinks().size()) && // In localization mode, the new node should be linked
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked
signaturesRetrieved.size()))) // can be different map of the current one
{
UASSERT(uContains(_optimizedPoses, signature->id()));
//used in localization mode: filter virtual links
std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior);
// Note that in localization mode, we don't re-optimize the graph
// if:
@@ -2239,7 +2242,7 @@ bool Rtabmap::process(
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0)
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
@@ -2437,7 +2440,7 @@ bool Rtabmap::process(
Signature lastSignatureData(signature->id());
Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && signature->getLinks().size())
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && graph::filterLinks(signature->getLinks(), Link::kPosePrior).size())
{
// only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
@@ -2466,7 +2469,7 @@ bool Rtabmap::process(
{
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
signature->getLinks().size() == 0 && // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
{
UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -3121,18 +3124,26 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
}
}
}
int ignoredLinks = 0;
if(edgeConstraints.size() != linksOut.size())
{
for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end())
{
UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
if(iter->second.type() == Link::kPosePrior)
{
++ignoredLinks;
}
else
{
UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
}
}
}
}
UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size() == linksOut.size(),
uFormat("nodes %d->%d, links %d->%d", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size()).c_str());
UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size()-ignoredLinks == linksOut.size(),
uFormat("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks).c_str());
}
if(constraints)
@@ -3143,7 +3154,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
UASSERT(_graphOptimizer!=0);
if(_graphOptimizer->iterations() == 0)
{
// Optimization desactivated! Return not optimized poses.
// Optimization disabled! Return not optimized poses.
optimizedPoses = poses;
}
else
@@ -3574,7 +3585,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0)
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
UASSERT(optimizedPoses.find(iter->second.from())!=optimizedPoses.end());
UASSERT(optimizedPoses.find(iter->second.to())!=optimizedPoses.end());
@@ -3906,7 +3917,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter)
{
// only add links for which poses are in "nodes"
if(uContains(nodes, jter->second.to()))
if(jter->second.from() != jter->second.to() && uContains(nodes, jter->second.to()))
{
links.insert(std::make_pair(jter->second.from(), jter->second.to()));
//links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes)

2
corelib/src/Signature.cpp
View File

@@ -120,7 +120,7 @@ void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() != this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_linksModified = true;