i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 04:19:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

EdgeSE3XYZPrior: add ParamOffset (#349)

Browse Source
This commit is contained in:
Nuno Marques
2019-01-04 12:45:30 -05:00
committed by matlabbe
parent 18c35954f0
commit bf5d2b7f04
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/OptimizerG2O.cpp
+2 -1
View File
@@ -453,7 +453,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
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()));
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity(); priorEdge->setParameterId(0, PARAM_OFFSET);
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