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

53
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,18 +672,28 @@ 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);
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second; if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
UASSERT(block && block->cols() == 3 && block->cols() == 3); {
outputCovariance = cv::Mat::eye(6,6,CV_64FC1); g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x UASSERT(block && block->cols() == 3 && block->cols() == 3);
outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta 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 else
{ {
@@ -713,10 +724,20 @@ 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);
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second; if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
UASSERT(block && block->cols() == 6 && block->cols() == 6); {
outputCovariance = cv::Mat(6,6,CV_64FC1); g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double)); 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 else
{ {

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)
{ {