New parameter: OdomF2M/ValidDepthRatio

corelib/include/rtabmap/core/Parameters.h

@@ -429,6 +429,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
+    RTABMAP_PARAM(OdomF2M, ValidDepthRatio,     float, 0.75,  "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
     RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
 #else

corelib/include/rtabmap/core/odometry/OdometryF2M.h

@@ -68,6 +68,7 @@ private:
 	float scanSubtractAngle_;
 	int bundleAdjustment_;
 	int bundleMaxFrames_;
+	float validDepthRatio_;
 
 	Registration * regPipeline_;
 	Signature * map_;

corelib/src/RegistrationVis.cpp

@@ -790,6 +790,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					// For each projected feature guess of "from" in "to", find its matching feature in
 					// the radius around the projected guess.
 					// TODO: do cross-check?
+					UDEBUG("guessMatchToProjection=%d, cornersProjected=%d", _guessMatchToProjection?1:0, (int)cornersProjected.size());
 					if(cornersProjected.size())
 					{
 						if(_guessMatchToProjection)
@@ -822,8 +823,6 @@ Transform RegistrationVis::computeTransformationImpl(
 							int newWords = 0;
 							cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
 							for(unsigned int i = 0; i < pointsToMat.rows; ++i)
-							{
-								if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
 							{
 								// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
 								int octave = kptsTo[i].octave & 255;
@@ -918,7 +917,6 @@ Transform RegistrationVis::computeTransformationImpl(
 									++newWords;
 								}
 							}
-							}
 							UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
 								(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
 								(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
@@ -1048,8 +1046,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
 									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
 
-									if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
+									if(	matchedIndexTo >= 0 &&
-										matchedIndexTo >= 0 &&
 										addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
 									{
 										addedWordsTo.insert(matchedIndexTo);
@@ -1353,6 +1350,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								&inliersV);
 						inliers[dir] = inliersV;
 						matches[dir] = matchesV;
+						UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
 						if(transforms[dir].isNull())
 						{
 							msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
@@ -1394,6 +1392,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							&inliersV);
 					inliers[dir] = inliersV;
 					matches[dir] = matchesV;
+					UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
 					if(transforms[dir].isNull())
 					{
 						msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
@@ -1460,6 +1459,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			fromSignature.sensorData().cameraModels().size() <= 1 &&
 			toSignature.sensorData().cameraModels().size() <= 1)
 		{
+			UDEBUG("Refine with bundle adjustment");
 			Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
 
 			std::map<int, Transform> poses;

corelib/src/odometry/OdometryF2M.cpp

@@ -68,6 +68,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
 	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
+	validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
 	map_(new Signature(-1)),
 	lastFrame_(new Signature(1)),
 	lastFrameOldestNewId_(0),
@@ -88,6 +89,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	}
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
+	Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
+
 	UASSERT(bundleMaxFrames_ >= 0);
 	ParametersMap bundleParameters = parameters;
 	if(bundleAdjustment_ > 0)
@@ -201,6 +204,8 @@ Transform OdometryF2M::computeTransform(
 	int totalBundleWordReferencesUsed = 0;
 	int totalBundleOutliers = 0;
 	float bundleTime = 0.0f;
+	bool visDepthAsMask = Parameters::defaultVisDepthAsMask();
+	Parameters::parse(parameters_, Parameters::kVisDepthAsMask(), visDepthAsMask);
 
 	// Generate keypoints from the new data
 	if(lastFrame_->sensorData().isValid())
@@ -567,11 +572,35 @@ Transform OdometryF2M::computeTransform(
 							UFATAL("no valid camera model!");
 						}
 					}
-					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
+
+					// add points without depth only if the local map has reached its maximum size
+					bool addPointsWithoutDepth = false;
+					if(!visDepthAsMask && validDepthRatio_ < 1.0f)
+					{
+						int ptsWithDepth = 0;
+						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+							iter != lastFrame_->getWords3().end();
+							++iter)
 						{
 							if(util3d::isFinite(iter->second))
+							{
+								++ptsWithDepth;
+							}
+						}
+						float r = float(ptsWithDepth) / float(lastFrame_->getWords3().size());
+						addPointsWithoutDepth = r > validDepthRatio_;
+						if(!addPointsWithoutDepth)
+						{
+							UWARN("Not enough points with valid depth in current frame (%d/%d=%f < %s=%f), points without depth are not added to map.",
+									ptsWithDepth, (int)lastFrame_->getWords3().size(), r, Parameters::kOdomF2MValidDepthRatio().c_str(), validDepthRatio_);
+						}
+					}
+
+					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
 					{
 						if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
+						{
+							if(util3d::isFinite(iter->second) || addPointsWithoutDepth)
 							{
 								newIds.insert(
 										std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
@@ -579,6 +608,7 @@ Transform OdometryF2M::computeTransform(
 														std::make_pair(iter2D->second,
 																std::make_pair(iter->second, iterDesc->second)))));
 							}
+						}
 						else if(bundleAdjustment_>0)
 						{
 							if(lastFrame_->getWords().count(iter->first) == 1)
@@ -593,17 +623,20 @@ Transform OdometryF2M::computeTransform(
 								iterBundlePosesRef->second += 1;
 
 								//move back point in camera frame (to get depth along z)
-									cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
+								float depth = 0.0f;
+								if(util3d::isFinite(iter->second))
+								{
+									depth = util3d::transformPoint(iter->second, invLocalTransform).z;
+								}
 								if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
 								{
 									std::map<int, FeatureBA> framePt;
-										framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
+									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
 									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 								}
 								else
 								{
-										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
+									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
-									}
 								}
 							}
 						}
@@ -626,22 +659,61 @@ Transform OdometryF2M::computeTransform(
 									iterBundlePosesRef->second += 1;
 
 									//move back point in camera frame (to get depth along z)
-									cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
+									float depth = 0.0f;
+									if(util3d::isFinite(iter->second.second.second.first))
+									{
+										depth = util3d::transformPoint(iter->second.second.second.first, invLocalTransform).z;
+									}
 									if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
 									{
 										std::map<int, FeatureBA> framePt;
-										framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, pt3d.z)));
+										framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
 										bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
 									}
 									else
 									{
-										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, pt3d.z)));
+										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
 									}
 								}
 							}
 
 							mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
-							mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
+							cv::Point3f pt = iter->second.second.second.first;
+							if(!util3d::isFinite(pt))
+							{
+								// get the ray instead
+								float x = iter->second.second.first.pt.x;
+								float y = iter->second.second.first.pt.y;
+								float subImageWidth = lastFrame_->sensorData().imageRaw().cols;
+								CameraModel model;
+								if(lastFrame_->sensorData().cameraModels().size() > 1)
+								{
+									subImageWidth = lastFrame_->sensorData().imageRaw().cols/lastFrame_->sensorData().cameraModels().size();
+									int cameraIndex = int(x / subImageWidth);
+									model = lastFrame_->sensorData().cameraModels()[cameraIndex];
+									x = x-subImageWidth*cameraIndex;
+								}
+								else if(lastFrame_->sensorData().cameraModels().size() == 1)
+								{
+									model = lastFrame_->sensorData().cameraModels()[0];
+								}
+								else
+								{
+									model = lastFrame_->sensorData().stereoCameraModel().left();
+								}
+
+								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
+										model.imageSize(),
+										x,
+										y,
+										model.cx(),
+										model.cy(),
+										model.fx(),
+										model.fy());
+								float scaleInf = (0.05 * model.fx()) / 0.01;
+								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
+							}
+							mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(pt, newFramePose)));
 							mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
 							if(lastFrameOldestNewId_ > iter->second.first)
 							{
@@ -941,7 +1013,18 @@ Transform OdometryF2M::computeTransform(
 			Transform newFramePose = this->getPose(); // initial pose may be not identity...
 			if(regPipeline_->isImageRequired())
 			{
-				if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
+				int ptsWithDepth = 0;
+				for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+					iter != lastFrame_->getWords3().end();
+					++iter)
+				{
+					if(util3d::isFinite(iter->second))
+					{
+						++ptsWithDepth;
+					}
+				}
+
+				if (ptsWithDepth >= regPipeline_->getMinVisualCorrespondences())
 				{
 					frameValid = true;
 					// update local map
@@ -994,11 +1077,11 @@ Transform OdometryF2M::computeTransform(
 
 								//get depth
 								float d = 0.0f;
-								if(lastFrame_->getWords3().count(iter->first) == 1)
+								if(lastFrame_->getWords3().count(iter->first) == 1 &&
+									util3d::isFinite(lastFrame_->getWords3().find(iter->first)->second))
 								{
 									//move back point in camera frame (to get depth along z)
-									cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform);
+									d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform).z;
-									d = pt3d.z;
 								}
 
 

corelib/src/util3d_motion_estimation.cpp

@@ -176,6 +176,11 @@ Transform estimateMotion3DTo2D(
 				{
 					UWARN("Not enough close points to compute covariance!");
 				}
+
+				if(float(oi) / float(inliers.size()) < 0.2f)
+				{
+					UWARN("A very low number of inliers have valid depth (%d/%d), the transform returned may be wrong!", oi, (int)inliers.size());
+				}
 			}
 			else if(covariance)
 			{

guilib/src/MainWindow.cpp

@@ -1196,7 +1196,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 					for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
 					{
 						// filter very far features from current location
-						if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 50*50)
+						if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 100*100)
 						{
 							(*cloud)[i].x = iter->second.x;
 							(*cloud)[i].y = iter->second.y;

guilib/src/PreferencesDialog.cpp

@@ -1050,6 +1050,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanAngle->setObjectName(Parameters::kOdomF2MScanSubtractAngle().c_str());
+	_ui->odom_f2m_validDepthRatio->setObjectName(Parameters::kOdomF2MValidDepthRatio().c_str());
 	_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());
 	_ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str());
 

guilib/src/ui/preferencesDialog.ui

@@ -94,16 +94,25 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-516</y>
-            <width>673</width>
+            <width>680</width>
-            <height>3038</height>
+            <height>3082</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="margin">
+           <property name="leftMargin">
+            <number>0</number>
+           </property>
+           <property name="topMargin">
+            <number>0</number>
+           </property>
+           <property name="rightMargin">
+            <number>0</number>
+           </property>
+           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -117,7 +126,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>18</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -5164,7 +5173,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="margin">
+                       <property name="leftMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="topMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="rightMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -11888,7 +11906,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_odometryType">
                       <property name="currentIndex">
-                       <number>8</number>
+                       <number>0</number>
                       </property>
                       <widget class="QWidget" name="page_52">
                        <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -12017,7 +12035,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="5" column="1">
+                             <item row="6" column="1">
                               <widget class="QLabel" name="label_357">
                                <property name="text">
                                 <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
@@ -12030,14 +12048,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="6" column="0">
+                             <item row="7" column="0">
                               <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
                                <property name="maximum">
                                 <number>999999</number>
                                </property>
                               </widget>
                              </item>
-                             <item row="6" column="1">
+                             <item row="7" column="1">
                               <widget class="QLabel" name="label_358">
                                <property name="text">
                                 <string>[Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).</string>
@@ -12050,7 +12068,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="5" column="0">
+                             <item row="6" column="0">
                               <widget class="QComboBox" name="odom_f2m_bundleStrategy">
                                <property name="sizeAdjustPolicy">
                                 <enum>QComboBox::AdjustToContents</enum>
@@ -12101,6 +12119,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
+                             <item row="5" column="1">
+                              <widget class="QLabel" name="label_524">
+                               <property name="text">
+                                <string>[Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="5" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_f2m_validDepthRatio">
+                               <property name="suffix">
+                                <string/>
+                               </property>
+                               <property name="decimals">
+                                <number>2</number>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.050000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.750000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
                             </layout>
                            </item>
                            <item>
@@ -15833,7 +15883,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -15913,7 +15972,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -16025,7 +16093,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>