i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.18.3: added landmarks (graph optimization, localization, navigation)

Browse Source
This commit is contained in:
matlabbe
2018-12-07 18:29:41 -05:00
parent b771aa00e0
commit 200ec8e5db
35 changed files with 1509 additions and 709 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/optimizer/OptimizerCVSBA.cpp
View File

@@ -55,7 +55,7 @@ bool OptimizerCVSBA::available()
std::map<int, Transform> OptimizerCVSBA::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
@@ -66,6 +66,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
// run sba optimization
cvsba::Sba sba;
std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
// change params if desired
cvsba::Sba::Params params ;
params.type = cvsba::Sba::MOTIONSTRUCTURE;

516
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -189,7 +189,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
#endif
optimizedPoses.clear();
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0 && poses.rbegin()->first > 0)
{
// Apply g2o optimization
@@ -313,43 +313,74 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
int landmarkVertexOffset = poses.rbegin()->first+1;
UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
g2o::HyperGraph::Vertex * vertex = 0;
int id = iter->first;
if(isSlam2d())
{
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(iter->first == rootId)
if(id > 0)
{
v2->setFixed(true);
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(id == rootId)
{
v2->setFixed(true);
}
vertex = v2;
}
else if(!landmarksIgnored())
{
g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
vertex = v2;
id = landmarkVertexOffset - id;
}
else
{
continue;
}
vertex = v2;
}
else
{
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(iter->first == rootId)
if(id > 0)
{
v3->setFixed(true);
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(id == rootId)
{
v3->setFixed(true);
}
vertex = v3;
}
else if(!landmarksIgnored())
{
g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
vertex = v3;
id = landmarkVertexOffset - id;
}
else
{
continue;
}
vertex = v3;
}
vertex->setId(iter->first);
vertex->setId(id);
UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
UDEBUG("fill edges to g2o...");
#if defined(RTABMAP_VERTIGO)
int vertigoVertexId = poses.rbegin()->first+1;
int vertigoVertexId = landmarkVertexOffset - (poses.begin()->first<0?poses.begin()->first:0);
#endif
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
@@ -408,6 +439,72 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
}
else if(id1<0 || id2 < 0)
{
if(!landmarksIgnored())
{
//landmarks
UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
if(isSlam2d())
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
id2 = landmarkVertexOffset - id2;
g2o::EdgeSE2PointXY* e = new g2o::EdgeSE2PointXY;
e->vertices()[0] = optimizer.vertex(id1);
e->vertices()[1] = optimizer.vertex(id2);
e->setMeasurement(Eigen::Vector2d(t.x(), t.y()));
e->setInformation(information);
e->setParameterId(0, PARAM_OFFSET);
edge = e;
}
else
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
id2 = landmarkVertexOffset - id2;
g2o::EdgeSE3PointXYZ* e = new g2o::EdgeSE3PointXYZ;
e->vertices()[0] = optimizer.vertex(id1);
e->vertices()[1] = optimizer.vertex(id2);
e->setMeasurement(Eigen::Vector3d(t.x(), t.y(), t.z()));
e->setInformation(information);
e->setParameterId(0, PARAM_OFFSET);
edge = e;
}
}
}
else
{
#if defined(RTABMAP_VERTIGO)
@@ -573,18 +670,38 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXY* v = (const g2o::VertexPointXY*)optimizer.vertex(landmarkVertexOffset - id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], iter->second.z(), roll, pitch, yaw);
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
}
@@ -592,16 +709,36 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
Transform t = Transform::fromEigen3d(v->estimate());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXYZ* v = (const g2o::VertexPointXYZ*)optimizer.vertex(landmarkVertexOffset - id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], v->estimate()[2], roll, pitch, yaw);
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
}
@@ -669,18 +806,38 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXY* v = (const g2o::VertexPointXY*)optimizer.vertex(landmarkVertexOffset-id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], iter->second.z(), roll, pitch, yaw);
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
@@ -723,16 +880,36 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
Transform t = Transform::fromEigen3d(v->estimate());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXYZ* v = (const g2o::VertexPointXYZ*)optimizer.vertex(landmarkVertexOffset-id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], v->estimate()[2], roll, pitch, yaw);
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
@@ -874,7 +1051,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("Optimizing graph...");
optimizedPoses.clear();
if(poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
if(poses.size()>=2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0))
{
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
@@ -952,61 +1129,64 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
if(iter->first > 0)
{
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
Transform camPose = iter->second * iterModel->second.localTransform();
Transform camPose = iter->second * iterModel->second.localTransform();
// Add node's pose
UASSERT(!camPose.isNull());
// Add node's pose
UASSERT(!camPose.isNull());
#ifdef RTABMAP_ORB_SLAM2
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
#else
g2o::VertexCam * vCam = new g2o::VertexCam();
g2o::VertexCam * vCam = new g2o::VertexCam();
#endif
Eigen::Affine3d a = camPose.toEigen3d();
Eigen::Affine3d a = camPose.toEigen3d();
#ifdef RTABMAP_ORB_SLAM2
a = a.inverse();
vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
a = a.inverse();
vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
#else
g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
cam.setKcam(
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
vCam->setEstimate(cam);
g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
cam.setKcam(
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
vCam->setEstimate(cam);
#endif
vCam->setId(iter->first);
vCam->setId(iter->first);
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
vCam->fixed()?1:0,
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
camPose.prettyPrint().c_str());
UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
vCam->fixed()?1:0,
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
camPose.prettyPrint().c_str());
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
++iter;
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
}
UDEBUG("fill edges to g2o...");
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(uContains(poses, iter->second.from()) &&
if(iter->second.from() > 0 &&
iter->second.to() > 0 &&
uContains(poses, iter->second.from()) &&
uContains(poses, iter->second.to()))
{
// add edge
@@ -1283,46 +1463,49 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// update poses
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
#ifdef RTABMAP_ORB_SLAM2
const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
#else
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
#endif
if(v)
if(iter->first > 0)
{
Transform t = Transform::fromEigen3d(v->estimate());
#ifdef RTABMAP_ORB_SLAM2
const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
#else
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
#endif
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
#ifdef RTABMAP_ORB_SLAM2
t=t.inverse();
t=t.inverse();
#endif
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull())
{
UERROR("Optimized pose %d is null!?!?", iter->first);
optimizedPoses.clear();
return optimizedPoses;
}
UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull())
{
UERROR("Optimized pose %d is null!?!?", iter->first);
optimizedPoses.clear();
return optimizedPoses;
}
// FIXME: is there a way that we can add the 2D constraint directly in SBA?
if(this->isSlam2d())
{
// get transform between old and new pose
t = iter->second.inverse() * t;
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
// FIXME: is there a way that we can add the 2D constraint directly in SBA?
if(this->isSlam2d())
{
// get transform between old and new pose
t = iter->second.inverse() * t;
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
}
else
{
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
}
}
else
{
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UERROR("Vertex (pose) %d not found!?", iter->first);
}
}
else
{
UERROR("Vertex (pose) %d not found!?", iter->first);
}
}
//update points3D
@@ -1352,7 +1535,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
}
}
else if(poses.size() > 1 && poses.size() != models.size())
else if(poses.size() > 1 && (poses.size() != models.size() && poses.begin()->first > 0))
{
UERROR("This method should be called with size of poses = size camera models!");
}
@@ -1410,37 +1593,98 @@ bool OptimizerG2O::saveGraph(
q.w());
}
int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first+1:0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if (isSlam2d())
{
// VERTEX_SE2 id x y theta
fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.theta());
if(iter->first > 0)
{
// VERTEX_SE2 id x y theta
fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.theta());
}
else if(!landmarksIgnored())
{
// VERTEX_XY id x y
fprintf(file, "VERTEX_XY %d %f %f\n",
iter->first,
iter->second.x(),
iter->second.y());
}
}
else
{
// VERTEX_SE3 id x y z qw qx qy qz
Eigen::Quaternionf q = iter->second.getQuaternionf();
fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z(),
q.x(),
q.y(),
q.z(),
q.w());
if(iter->first > 0)
{
// VERTEX_SE3 id x y z qw qx qy qz
Eigen::Quaternionf q = iter->second.getQuaternionf();
fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z(),
q.x(),
q.y(),
q.z(),
q.w());
}
else if(!landmarksIgnored())
{
// VERTEX_XYZ id x y z
fprintf(file, "VERTEX_XYZ %d %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z());
}
}
}
int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0);
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if (iter->second.type() == Link::kLandmark)
{
if (this->landmarksIgnored())
{
continue;
}
if(isSlam2d())
{
// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1));
}
else
{
// EDGE_SE3_XYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE2_XY %d %d %d %f %f %f %f %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
PARAM_OFFSET,
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.transform().z(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2));
}
continue;
}
std::string prefix = isSlam2d()? "EDGE_SE2" :"EDGE_SE3:QUAT";
std::string suffix = "";
std::string to = uFormat(" %d", iter->second.to());

138
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
@@ -138,11 +139,26 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UASSERT(!iter->second.isNull());
if(isSlam2d())
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(iter->first > 0)
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
}
else if(!landmarksIgnored())
{
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
}
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
if(iter->first > 0)
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
}
else if(!landmarksIgnored())
{
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
}
}
}
@@ -195,6 +211,62 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else if(id1<0 || id2 < 0)
{
if(!landmarksIgnored())
{
//landmarks
UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
if(isSlam2d())
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();;
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
gtsam::Point2 landmark(t.x(), t.y());
gtsam::Pose2 p;
graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
}
else
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();;
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
gtsam::Point3 landmark(t.x(), t.y(), t.z());
gtsam::Pose3 p;
graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
}
}
}
else
{
#ifdef RTABMAP_VERTIGO
@@ -318,20 +390,40 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(intermediateGraphes && i > 0)
{
float x,y,z,roll,pitch,yaw;
std::map<int, Transform> tmpPoses;
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
int key = (int)iter->key;
if(isSlam2d())
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
if(key > 0)
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw)));
}
}
else
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
if(key > 0)
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
}
}
}
}
@@ -385,30 +477,50 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
gtsam::Marginals marginals(graph, optimizer->values());
float x,y,z,roll,pitch,yaw;
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
int key = (int)iter->key;
if(isSlam2d())
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
if(key > 0)
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z,roll,pitch,yaw)));
}
}
else
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
if(key > 0)
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
}
}
}
}
// compute marginals
try {
UDEBUG("Computing marginals...");
UTimer t;
gtsam::Marginals marginals(graph, optimizer->values());
gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first);
UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first);
if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
{
outputCovariance.at<double>(0,0) = info(0,0); // x-x

92
corelib/src/optimizer/OptimizerTORO.cpp
View File

@@ -77,23 +77,29 @@ std::map<int, Transform> OptimizerTORO::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
float x,y,z, roll,pitch,yaw;
iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
float x,y,z, roll,pitch,yaw;
iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
}
}
}
@@ -128,7 +134,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
if(id1 != id2)
if(id1 != id2 && id1 > 0 && id2 > 0)
{
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
@@ -140,7 +146,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
//else // not supporting pose prior
//else // not supporting pose prior and landmarks
}
}
else
@@ -160,7 +166,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
if(id1 != id2)
if(id1 != id2 && id1 > 0 && id2 > 0)
{
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
@@ -172,7 +178,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
//else // not supporting pose prior
//else // not supporting pose prior and landmarks
}
}
UDEBUG("buildMST... root=%d", rootId);
@@ -214,25 +220,31 @@ std::map<int, Transform> OptimizerTORO::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
intermediateGraphes->push_back(tmpPoses);
@@ -293,25 +305,31 @@ std::map<int, Transform> OptimizerTORO::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}