i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +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

25
corelib/src/OptimizerG2O.cpp
View File

@@ -172,6 +172,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
double * finalError, double * finalError,
int * iterationsDone) int * iterationsDone)
{ {
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
std::map<int, Transform> optimizedPoses; std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_G2O #ifdef RTABMAP_G2O
UDEBUG("Optimizing graph..."); UDEBUG("Optimizing graph...");
@@ -671,9 +672,10 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::SparseBlockMatrix<g2o::MatrixXD> spinv; g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
optimizer.computeMarginals(spinv, v); 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); UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
{
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second; g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 3 && block->cols() == 3); 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,0) = (*block)(0,0); // x-x
outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
@@ -684,6 +686,15 @@ std::map<int, Transform> OptimizerG2O::optimize(
outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta 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 else
{ {
UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first); UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
@@ -713,11 +724,21 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::SparseBlockMatrix<g2o::MatrixXD> spinv; g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
optimizer.computeMarginals(spinv, v); 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); UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
{
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second; g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 6 && block->cols() == 6); UASSERT(block && block->cols() == 6 && block->cols() == 6);
outputCovariance = cv::Mat(6,6,CV_64FC1);
memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double)); 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 else
{ {
UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first); UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);

3
corelib/src/OptimizerGTSAM.cpp
View File

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

2
corelib/src/OptimizerTORO.cpp
View File

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