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

Optimizer: fixed bad g2o optimizations on 2d slam when there are 3D landmarks (#384). DBViewer: landmarks can be now visualized.

Browse Source
This commit is contained in:
matlabbe
2019-04-26 14:03:25 -04:00
parent f9b7b54454
commit a5350c4891
11 changed files with 289 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -341,28 +341,23 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
// check if it is SE2 or only PointXY
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
if(jter != edgeConstraints.end())
UASSERT(jter != edgeConstraints.end());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
else
{
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
else
{
continue;
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
}
else
@@ -391,34 +386,29 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
// check if it is SE3 or only PointXYZ
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
if(jter != edgeConstraints.end())
UASSERT(jter != edgeConstraints.end());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
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);
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
else
{
continue;
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);
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
}
else

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

@@ -168,18 +168,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
// check if it is SE2 or only PointXY
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
if(jter != edgeConstraints.end())
UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
}
@@ -194,20 +193,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
// check if it is SE3 or only PointXYZ
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
if(jter != edgeConstraints.end())
UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
}
}