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 "[Setjac] infinite jac" error when Vis/ForwardEstOnly is false. OptimizerG2O: Added a check to ignore invalid 3d points.

Browse Source
This commit is contained in:
matlabbe
2020-09-15 15:18:49 -04:00
parent 29368ebdb3
commit 4e6e404951
2 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1496,6 +1496,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(points3DMap.find(id) != points3DMap.end())
{
cv::Point3f pt3d = points3DMap.at(id);
if(!util3d::isFinite(pt3d))
{
UWARN("Ignoring 3D point %d because it has nan value(s)!", id);
continue;
}
g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
@@ -1522,7 +1527,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
const FeatureBA & pt = jter->second;
double depth = pt.depth;
//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
//UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
g2o::OptimizableGraph::Edge * e;
double baseline = 0.0;
@@ -1568,9 +1573,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(baseline > 0.0)
{
UDEBUG("Stereo camera model detected but current "
"observation (pt=%d to cam=%d) has null depth (%f m), adding "
"observation (pt=%d to cam=%d, kpt=[%d,%d]) has null depth (%f m), adding "
"mono observation instead.",
vpt3d->id()-stepVertexId, camId, depth);
vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
}
// mono edge
#ifdef RTABMAP_ORB_SLAM2