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

fixed g2o crash when failing computing marginals

Browse Source
This commit is contained in:
matlabbe
2018-06-15 16:31:45 -04:00
parent a3bdb027e7
commit c91431410e
3 changed files with 39 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
corelib/src/OptimizerG2O.cpp
View File

@@ -172,6 +172,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
double * finalError,
int * iterationsDone)
{
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_G2O
UDEBUG("Optimizing graph...");
@@ -671,18 +672,28 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
optimizer.computeMarginals(spinv, v);
UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 3 && block->cols() == 3);
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
{
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 3 && block->cols() == 3);
outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
}
else if(v->hessianIndex() < 0)
{
UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
}
else
{
UWARN("Computing marginals: vertex %d has hessian not valid (%d > block size=%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex(), (int)spinv.blockCols().size());
}
}
else
{
@@ -713,10 +724,20 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
optimizer.computeMarginals(spinv, v);
UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 6 && block->cols() == 6);
outputCovariance = cv::Mat(6,6,CV_64FC1);
memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
{
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 6 && block->cols() == 6);
memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
}
else if(v->hessianIndex() < 0)
{
UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
}
else
{
UWARN("Computing marginals: vertex %d has hessian not valid (%d > block size=%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex(), (int)spinv.blockCols().size());
}
}
else
{

3
corelib/src/OptimizerGTSAM.cpp
View File

@@ -84,6 +84,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
double * finalError,
int * iterationsDone)
{
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_GTSAM
@@ -409,7 +410,6 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if(isSlam2d())
{
UASSERT(info.cols() == 3 && info.cols() == 3);
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
outputCovariance.at<double>(0,0) = info(0,0); // x-x
outputCovariance.at<double>(0,1) = info(0,1); // x-y
outputCovariance.at<double>(0,5) = info(0,2); // x-theta
@@ -428,7 +428,6 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
outputCovariance = cv::Mat(6,6,CV_64FC1);
memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
}
} catch(std::exception& e) {

2
corelib/src/OptimizerTORO.cpp
View File

@@ -60,6 +60,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
double * finalError,
int * iterationsDone)
{
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_TORO
UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
@@ -315,7 +316,6 @@ std::map<int, Transform> OptimizerTORO::optimize(
}
// TORO doesn't compute marginals...
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
}
else if(poses.size() == 1 || iterations() <= 0)
{