i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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 17:45:30 +00: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

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

@@ -453,7 +453,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
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())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x