Fixed ICP-only registration with already provided guess (no need to do visual guess, as the guess can be already good)

Fixed "HAVE_OPENCV_CUDAFEATURES2D" build error of https://github.com/introlab/rtabmap/issues/85

app/src/CMakeLists.txt

@@ -150,7 +150,7 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
 		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
 		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
 		list(GET loc_list 1 plugin_type)
-	    #MESSAGE(STATUS "Qt5 plugin "${plugin_loc}" installed in "${plugin_dest_dir}/plugins${plugin_type}"")
+	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -116,14 +116,17 @@ void segmentObstaclesFromGround(
 			}
 
 			//Cluster remaining stuff (obstacles)
-			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
-					cloud,
-					otherStuffIndices,
-					clusterRadius,
-					minClusterSize);
+			if(otherStuffIndices->size())
+			{
+				std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+						cloud,
+						otherStuffIndices,
+						clusterRadius,
+						minClusterSize);
 
-			// merge indices
-			obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+				// merge indices
+				obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+			}
 		}
 	}
 }

corelib/src/CMakeLists.txt

@@ -188,10 +188,17 @@ IF(G2O_FOUND)
 ENDIF(G2O_FOUND)
 
 IF(GTSAM_FOUND)
-	SET(INCLUDE_DIRS 
-		${INCLUDE_DIRS} 
-		${GTSAM_INCLUDE_DIRS}
-	)
+	IF(GTSAM_INCLUDE_DIR)
+		SET(INCLUDE_DIRS 
+			${GTSAM_INCLUDE_DIR} # place it in front to use Eigen installed by GTSAM
+			${INCLUDE_DIRS} 
+		)
+	ELSE()
+		SET(INCLUDE_DIRS 
+			${GTSAM_INCLUDE_DIRS} # cmake standard
+			${INCLUDE_DIRS} 
+		)
+	ENDIF()
 	SET(LIBRARIES
 		${LIBRARIES}
 		gtsam

corelib/src/Memory.cpp

@@ -1902,6 +1902,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 		if(s)
 		{
 			s->setLabel(label);
+			_linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
 			UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 			return true;
 		}
@@ -2123,32 +2124,13 @@ Transform Memory::computeTransform(
 
 	// compute transform fromId -> toId
 	std::vector<int> inliersV;
-	if(_reextractLoopClosureFeatures || (fromS.getWords().size() && toS.getWords().size()))
+	if(_reextractLoopClosureFeatures ||
+		(fromS.getWords().size() && toS.getWords().size()) ||
+		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
 	{
 		Signature tmpFrom = fromS;
 		Signature tmpTo = toS;
-
-		// make a guess fast with known correspondences (if there are)
-		RegistrationVis regVis(parameters_);
-		if(tmpFrom.getWords().size() &&
-			tmpTo.getWords().size() &&
-			tmpFrom.getWords3().size() &&
-			tmpTo.getWords3().size())
-		{
-			UDEBUG("");
-			// Remove descriptors, this will avoid recomputation of the correspondences in regVis
-			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			Transform t = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-			// set back descriptors
-			tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
-			tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
-			if(!t.isNull())
-			{
-				guess = t;
-			}
-		}
-
+
 		if(_reextractLoopClosureFeatures)
 		{
 			UDEBUG("");
@@ -2166,6 +2148,7 @@ Transform Memory::computeTransform(
 		{
 			UDEBUG("");
 			// no visual in the pipeline, make visual registration for guess
+			RegistrationVis regVis(parameters_);
 			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
 		}
 

corelib/src/Parameters.cpp

@@ -155,6 +155,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.11.8
+		removedParameters_.insert(std::make_pair("Reg/Force2D",                   std::make_pair(true,  Parameters::kRegForce3DoF())));
+
 		// 0.11.6
 		removedParameters_.insert(std::make_pair("RGBD/ProximityPathScansMerged", std::make_pair(false,  "")));
 

corelib/src/Rtabmap.cpp

@@ -1834,7 +1834,8 @@ bool Rtabmap::process(
 							 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
 						{
 							RegistrationInfo info;
-							Transform transform = _memory->computeTransform(signature->id(), nearestId, Transform(), &info);
+							Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
+							Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info);
 							if(!transform.isNull())
 							{
 								if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)

corelib/src/VWDictionary.cpp

@@ -542,7 +542,7 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 		}
 #endif
 #else
-#if HAVE_OPENCV_CUDAFEATURES2D
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
 		if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
 		{
 			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");

guilib/src/ExportCloudsDialog.cpp

@@ -255,7 +255,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_filteringMinNeighbors->setValue(2);
 
 	_ui->checkBox_assemble->setChecked(true);
-	_ui->doubleSpinBox_voxelSize_assembled->setValue(0.01);
+	_ui->doubleSpinBox_voxelSize_assembled->setValue(0.0);
 
 	_ui->groupBox_subtraction->setChecked(false);
 	_ui->doubleSpinBox_subtractPointFilteringRadius->setValue(0.02);