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

g2o: only check for variance >= 9999 as zero should never happen. For landmark, use SE3 or PointXYZ factor depending if angular covariance is set or not (>=9999). Same for gtsam (pose vs bearingRange factor respectively).

Browse Source
This commit is contained in:
matlabbe
2019-01-06 17:00:39 -05:00
parent bf5d2b7f04
commit 2a8e5be361
3 changed files with 387 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/Link.cpp
View File

@@ -78,12 +78,12 @@ double Link::transVariance() const
void Link::setInfMatrix(const cv::Mat & infMatrix) { void Link::setInfMatrix(const cv::Mat & infMatrix) {
UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1); 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, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(0,0)).c_str()); UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, uFormat("Linear information X should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(0,0)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(1,1)).c_str()); UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, uFormat("Linear information Y should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(1,1)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(2,2)).c_str()); UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, uFormat("Linear information Z should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(2,2)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(3,3)).c_str()); UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, uFormat("Angular information roll should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(3,3)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(4,4)).c_str()); UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, uFormat("Angular information pitch should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(4,4)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(5,5)).c_str()); UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, uFormat("Angular information yaw should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(5,5)).c_str());
infMatrix_ = infMatrix; infMatrix_ = infMatrix;
} }

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

@@ -315,6 +315,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
int landmarkVertexOffset = poses.rbegin()->first+1; int landmarkVertexOffset = poses.rbegin()->first+1;
std::map<int, bool> isLandmarkWithRotation;
UDEBUG("fill poses to g2o..."); UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -336,10 +337,31 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
g2o::VertexPointXY * v2 = new g2o::VertexPointXY(); // check if it is SE2 or only PointXY
v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y())); std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
vertex = v2; if(jter != edgeConstraints.end())
id = landmarkVertexOffset - id; {
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;
}
}
else
{
continue;
}
} }
else else
{ {
@@ -365,10 +387,37 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ(); // check if it is SE3 or only PointXYZ
v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z())); std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
vertex = v3; if(jter != edgeConstraints.end())
id = landmarkVertexOffset - id; {
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;
}
}
else
{
continue;
}
} }
else else
{ {
@@ -398,8 +447,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
{ {
if(isSlam2d()) if(isSlam2d())
{ {
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0 || if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{ {
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior(); g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
@@ -442,19 +490,16 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
else else
{ {
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 && if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 && 1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0) || 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0))
{ {
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior(); EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1); priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(Eigen::Vector3d(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z())); priorEdge->setMeasurement(Eigen::Vector3d(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
priorEdge->setParameterId(0, PARAM_OFFSET); priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity(); Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
@@ -498,63 +543,108 @@ std::map<int, Transform> OptimizerG2O::optimize(
{ {
//landmarks //landmarks
UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0)); UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
if(isSlam2d())
Transform t;
if(id2 < 0)
{ {
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity(); t = iter->second.transform();
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 else
{ {
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity(); t = iter->second.transform().inverse();
if(!isCovarianceIgnored()) std::swap(id1, id2); // should be node -> landmark
{ }
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone(); int idTag= id2;
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double)); id2 = landmarkVertexOffset - id2;
}
Transform t; if(isSlam2d())
if(id2 < 0) {
if(isLandmarkWithRotation.at(idTag))
{ {
t = iter->second.transform(); 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
}
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(t.x(), t.y(), t.theta()));
e->setInformation(information);
edge = e;
} }
else else
{ {
t = iter->second.transform().inverse(); Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
std::swap(id1, id2); // should be node -> landmark 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));
}
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;
} }
id2 = landmarkVertexOffset - id2; }
else
{
if(isLandmarkWithRotation.at(idTag))
{
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));
}
g2o::EdgeSE3PointXYZ* e = new g2o::EdgeSE3PointXYZ; Eigen::Affine3d a = t.toEigen3d();
e->vertices()[0] = optimizer.vertex(id1); Eigen::Isometry3d constraint;
e->vertices()[1] = optimizer.vertex(id2); constraint = a.linear();
e->setMeasurement(Eigen::Vector3d(t.x(), t.y(), t.z())); constraint.translation() = a.translation();
e->setInformation(information);
e->setParameterId(0, PARAM_OFFSET); g2o::EdgeSE3 * e = new g2o::EdgeSE3();
edge = e; 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;
}
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));
}
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;
}
} }
} }
} }
@@ -742,14 +832,27 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
const g2o::VertexPointXY* v = (const g2o::VertexPointXY*)optimizer.vertex(landmarkVertexOffset - id); const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
if(v) if(v)
{ {
float roll, pitch, yaw; if(isLandmarkWithRotation.at(id))
iter->second.getEulerAngles(roll, pitch, yaw); {
Transform t(v->estimate()[0], v->estimate()[1], iter->second.z(), roll, pitch, yaw); const g2o::VertexSE2* vSE2 = (const g2o::VertexSE2*)v;
tmpPoses.insert(std::pair<int, Transform>(id, t)); float roll, pitch, yaw;
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str()); iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(vSE2->estimate().translation()[0], vSE2->estimate().translation()[1], iter->second.z(), roll, pitch, vSE2->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
{
const g2o::VertexPointXY* vP = (const g2o::VertexPointXY*)v;
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(vP->estimate()[0], vP->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 else
{ {
@@ -779,14 +882,25 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
const g2o::VertexPointXYZ* v = (const g2o::VertexPointXYZ*)optimizer.vertex(landmarkVertexOffset - id); const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
if(v) if(v)
{ {
float roll, pitch, yaw; if(isLandmarkWithRotation.at(id))
iter->second.getEulerAngles(roll, pitch, yaw); {
Transform t(v->estimate()[0], v->estimate()[1], v->estimate()[2], roll, pitch, yaw); const g2o::VertexSE3* vSE3 = (const g2o::VertexSE3*)v;
tmpPoses.insert(std::pair<int, Transform>(id, t)); Transform t = Transform::fromEigen3d(vSE3->estimate());
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str()); tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
const g2o::VertexPointXYZ* vP = (const g2o::VertexPointXYZ*)v;
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(vP->estimate()[0], vP->estimate()[1], vP->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 else
{ {
@@ -878,14 +992,28 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
const g2o::VertexPointXY* v = (const g2o::VertexPointXY*)optimizer.vertex(landmarkVertexOffset-id); const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
if(v) if(v)
{ {
float roll, pitch, yaw; if(isLandmarkWithRotation.at(id))
iter->second.getEulerAngles(roll, pitch, yaw); {
Transform t(v->estimate()[0], v->estimate()[1], iter->second.z(), roll, pitch, yaw); const g2o::VertexSE2* vSE2 = (const g2o::VertexSE2*)v;
optimizedPoses.insert(std::pair<int, Transform>(id, t)); float roll, pitch, yaw;
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str()); iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(vSE2->estimate().translation()[0], vSE2->estimate().translation()[1], iter->second.z(), roll, pitch, vSE2->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
{
const g2o::VertexPointXY* vP = (const g2o::VertexPointXY*)v;
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(vP->estimate()[0], vP->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 else
{ {
@@ -950,14 +1078,26 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
const g2o::VertexPointXYZ* v = (const g2o::VertexPointXYZ*)optimizer.vertex(landmarkVertexOffset-id); const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
if(v) if(v)
{ {
float roll, pitch, yaw; if(isLandmarkWithRotation.at(id))
iter->second.getEulerAngles(roll, pitch, yaw); {
Transform t(v->estimate()[0], v->estimate()[1], v->estimate()[2], roll, pitch, yaw); const g2o::VertexSE3* vSE3 = (const g2o::VertexSE3*)v;
optimizedPoses.insert(std::pair<int, Transform>(id, t)); Transform t = Transform::fromEigen3d(vSE3->estimate());
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str()); optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
const g2o::VertexPointXYZ* vP = (const g2o::VertexPointXYZ*)v;
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(vP->estimate()[0], vP->estimate()[1], vP->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 else
{ {
@@ -1753,8 +1893,7 @@ bool OptimizerG2O::saveGraph(
} }
if (isSlam2d()) if (isSlam2d())
{ {
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 || if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{ {
prefix = "EDGE_PRIOR_SE2_XY"; prefix = "EDGE_PRIOR_SE2_XY";
isSE2 = false; isSE2 = false;
@@ -1770,12 +1909,9 @@ bool OptimizerG2O::saveGraph(
} }
else else
{ {
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) > 9999.0 && if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) > 9999.0 && 1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0) || 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0))
{ {
to = ""; to = "";
prefix = "EDGE_POINTXYZ_PRIOR"; prefix = "EDGE_POINTXYZ_PRIOR";

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

@@ -134,6 +134,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UDEBUG("fill poses to gtsam..."); UDEBUG("fill poses to gtsam...");
gtsam::Values initialEstimate; gtsam::Values initialEstimate;
std::map<int, bool> isLandmarkWithRotation;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
UASSERT(!iter->second.isNull()); UASSERT(!iter->second.isNull());
@@ -145,7 +146,21 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y())); // check if it is SE2 or only PointXY
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
if(jter != edgeConstraints.end())
{
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));
}
}
} }
} }
@@ -157,7 +172,23 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored())
{ {
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z())); // check if it is SE3 or only PointXYZ
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
if(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)
{
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));
}
}
} }
} }
} }
@@ -217,53 +248,83 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{ {
//landmarks //landmarks
UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0)); UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
if(isSlam2d()) Transform t;
if(id2 < 0)
{ {
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity(); t = iter->second.transform();
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 else
{ {
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity(); t = iter->second.transform().inverse();
if(!isCovarianceIgnored()) std::swap(id1, id2); // should be node -> landmark
}
if(isSlam2d())
{
if(isLandmarkWithRotation.at(id2))
{ {
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();; Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double)); if(!isCovarianceIgnored())
} {
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information); information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
Transform t; information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
if(id2 < 0) information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
{ information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
t = iter->second.transform(); 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
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
} }
else else
{ {
t = iter->second.transform().inverse(); Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
std::swap(id1, id2); // should be node -> landmark 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);
gtsam::Point3 landmark(t.x(), t.y(), t.z()); gtsam::Point2 landmark(t.x(), t.y());
gtsam::Pose3 p; gtsam::Pose2 p;
graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model)); graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
}
}
else
{
if(isLandmarkWithRotation.at(id2))
{
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::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), 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);
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));
}
} }
} }
} }
@@ -404,11 +465,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>(); gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta()))); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
{ {
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if(isLandmarkWithRotation.at(key))
gtsam::Point2 p = iter->value.cast<gtsam::Point2>(); {
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw))); gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
}
else
{
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 else
@@ -418,11 +487,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>(); gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
{ {
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if(isLandmarkWithRotation.at(key))
gtsam::Point3 p = iter->value.cast<gtsam::Point3>(); {
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw))); gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else
{
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)));
}
} }
} }
} }
@@ -490,11 +567,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>(); gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta()))); optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
{ {
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if(isLandmarkWithRotation.at(key))
gtsam::Point2 p = iter->value.cast<gtsam::Point2>(); {
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z,roll,pitch,yaw))); gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
}
else
{
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 else
@@ -504,11 +589,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>(); gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
} }
else if(!landmarksIgnored()) else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
{ {
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if(isLandmarkWithRotation.at(key))
gtsam::Point3 p = iter->value.cast<gtsam::Point3>(); {
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw))); gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else
{
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)));
}
} }
} }
} }