i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

OptimizedG2O: assotiate VertexSE3 instead of VertexPointXYZ to EdgeXYZPrior

Browse Source
This commit is contained in:
TSC21
2018-12-28 14:08:53 +00:00
parent 22d631d7aa
commit 698293e2d8
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -449,7 +449,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)) static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0))
{ {
g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior(); g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior();
g2o::VertexPointXYZ* v1 = (g2o::VertexPointXYZ*)optimizer.vertex(id1); g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1); priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z())); priorEdge->setMeasurement(g2o::Vector3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity(); Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();