Added error log on Kp/NNStrategy not compatible with huge vocabulary. ReduceGraph/DetectMoreLoopClosures: Make sure original parameters are saved back on closing. g2o: fixing optimizer to Levenberg for SBA to avoid [SetJac] infinite jac fatal error.

Author: matlabbe

corelib/src/FlannIndex.cpp

@@ -96,7 +96,7 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons
 #else
 		UTimer timer;
 		const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
-		std::vector<unsigned char> indexData(1024*1024*100 + headerSizeBytes); // Max 100 MB
+		std::vector<unsigned char> indexData(1024*1024*1024 + headerSizeBytes); // Max 1 GB
         FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb");
 		long bytes_written = 0;
         if (indexDataPtr) {

corelib/src/VWDictionary.cpp

@@ -573,6 +573,15 @@ void VWDictionary::update()
 				_removedIndexedWords.size() == 0 &&
 				_visualWords.size())
 		{
+			const int IMGIDX_SHIFT = 18;
+    		const int IMGIDX_ONE = (1 << IMGIDX_SHIFT); // a limit defined in https://github.com/opencv/opencv/blob/4.x/modules/features2d/src/matchers.cpp
+			if(_dataTree.rows >= IMGIDX_ONE)
+			{
+				UWARN("%s=%d is not a FLANN strategy and the number of words in the vocabulary (%d) is over %d (IMGIDX_ONE), so opencv may "
+					"assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (%s<%d).",
+					Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNBruteForce);
+			}
+
 			//just add not indexed words
 			int i = _dataTree.rows;
 			if(!_dataTree.empty()) {
@@ -1006,7 +1015,7 @@ std::list<int> VWDictionary::addNewWords(
 	if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 	{
 		//Find nearest neighbors
-		UDEBUG("newPts.total()=%d ", descriptors.rows);
+		UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy);
 
 		if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
 		{

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1559,7 +1559,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 #endif // RTABMAP_ORB_SLAM
 
 #ifndef RTABMAP_ORB_SLAM
-		if(optimizer_ == 1)
+		// ISSUE: It seems the fatal error
+		//        "[SetJac] infinite jac" happens relatively
+		//        easily with GaussNewton on SBA problem,
+		//        ignore optimizer_ and always use Levenberg for SBA.
+		// TODO: Note that g2o/RobustKernelDelta parameter could be
+		//       potentially tuned to avoid that error with GaussNewton.
+		if(0)//optimizer_ == 1)
 		{
 #ifdef RTABMAP_G2O_CPP11
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
@@ -1944,7 +1950,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 			if(uIsNan(chi2))
 			{
-				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
+				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current %s=%d) or solver (current %s=%d).",
+						Parameters::kg2oOptimizer().c_str(), optimizer_, Parameters::kg2oSolver().c_str(), solver_);
 				return optimizedPoses;
 			}
 			UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
@@ -1978,15 +1985,18 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #endif
 
-						cv::Point3f pt3d;
+						int id=-1;
 						if((*iter)->vertex(0)->id() > negVertexOffset)
 						{
-							pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
+							id = negVertexOffset - (*iter)->vertex(0)->id();
 						}
 						else
 						{
-							pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+							id = (*iter)->vertex(0)->id() - stepVertexId;
 						}
+						UASSERT_MSG(points3DMap.find(id) != points3DMap.end(), uFormat("word id=%d points3DMap=%ld vertex id=%d (negVertexOffset=%d stepVertexId=%d)",
+							id, points3DMap.size(), (*iter)->vertex(0)->id(), negVertexOffset, stepVertexId).c_str());
+						cv::Point3f pt3d = points3DMap.at(id);
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
 
 						if(outliers)

tools/DetectMoreLoopClosures/main.cpp

@@ -198,6 +198,7 @@ int main(int argc, char * argv[])
 	// Add some optimizations (soft set, can be overriden by arguments)
 	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
 	inputParams.insert(ParametersPair(Parameters::kKpNNStrategy(), "3")); // don't need flann index
+	inputParams.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // should be incremental to update links
 
 	std::string dbPath = argv[argc-1];
 	if(!UFile::exists(dbPath))
@@ -247,6 +248,7 @@ int main(int argc, char * argv[])
 	Rtabmap rtabmap;
 	printf("Initialization...\n");
 	UTimer timer;
+	ParametersMap originalParameters = parameters;
 	uInsert(parameters, inputParams);
 	rtabmap.init(parameters, dbPath);
 	printf("Initialization... done! (%f sec)\n", timer.ticks());
@@ -307,6 +309,9 @@ int main(int argc, char * argv[])
 		}
 	}
 
+	// Restore original parameters before saving back the database
+	rtabmap.parseParameters(originalParameters);
+
 	rtabmap.close();
 
 	return 0;

tools/ReduceGraph/main.cpp

@@ -106,6 +106,8 @@ int main(int argc, char * argv[])
 	}
 	printf("Parameters:\n");
 	printf("  radius = %f m\n", radius);
+	printf("  keep_latest = %s\n", keepLatest?"true":"false");
+	printf("  keep_linked = %s\n", keepLinked?"true":"false");
 
 	// Just parse logging options
 	Parameters::parseArguments(argc, argv);
@@ -142,6 +144,7 @@ int main(int argc, char * argv[])
 	Memory memory;
 	printf("Initialization...\n");
 	UTimer timer;
+	ParametersMap originalParameters = parameters;
 	uInsert(parameters, inputParams);
 	if(!memory.init(dbPath, false, parameters))
 	{
@@ -232,6 +235,9 @@ int main(int argc, char * argv[])
 			printf("Saved the new global occupancy grid!\n");
 		}
 	}
+
+	// Restore original parameters before saving back the database
+	memory.parseParameters(originalParameters);
 
 	memory.close(true);