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

9
corelib/src/RegistrationVis.cpp
View File

@@ -1671,10 +1671,18 @@ Transform RegistrationVis::computeTransformationImpl(
models.insert(std::make_pair(2, cameraModelTo)); models.insert(std::make_pair(2, cameraModelTo));
std::map<int, std::map<int, FeatureBA> > wordReferences; std::map<int, std::map<int, FeatureBA> > wordReferences;
std::set<int> sbaOutliers;
for(unsigned int i=0; i<allInliers.size(); ++i) for(unsigned int i=0; i<allInliers.size(); ++i)
{ {
int wordId = allInliers[i]; int wordId = allInliers[i];
const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second; const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
if(!util3d::isFinite(pt3D))
{
UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
sbaOutliers.insert(wordId);
continue;
}
points3DMap.insert(std::make_pair(wordId, pt3D)); points3DMap.insert(std::make_pair(wordId, pt3D));
std::map<int, FeatureBA> ptMap; std::map<int, FeatureBA> ptMap;
@@ -1704,7 +1712,6 @@ Transform RegistrationVis::computeTransformationImpl(
//} //}
} }
std::set<int> sbaOutliers;
optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers); optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
delete sba; delete sba;

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

@@ -1496,6 +1496,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(points3DMap.find(id) != points3DMap.end()) if(points3DMap.find(id) != points3DMap.end())
{ {
cv::Point3f pt3d = points3DMap.at(id); 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(); g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z)); 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; const FeatureBA & pt = jter->second;
double depth = pt.depth; 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; g2o::OptimizableGraph::Edge * e;
double baseline = 0.0; double baseline = 0.0;
@@ -1568,9 +1573,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(baseline > 0.0) if(baseline > 0.0)
{ {
UDEBUG("Stereo camera model detected but current " 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.", "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 // mono edge
#ifdef RTABMAP_ORB_SLAM2 #ifdef RTABMAP_ORB_SLAM2