Added Bundle Adjustment options to OdometryF2M (OdomF2M/BundleAdjustment) and RegistrationVis (Vis/BundleAdjustment). Added logger thread filter. RegistrationVis: Copy back corresponding input word IDs to output words when available. Memory::computeTransform() added option to use already computed corespondences if possible (proximity detection by time uses this). Updated Optimizer::optimizeBA() interface.

utilite/include/rtabmap/utilite/ULogger.h

@@ -31,6 +31,8 @@
 #include <time.h>
 #include <string>
 #include <vector>
+#include <map>
+#include <set>
 
 #include <stdarg.h>
 
@@ -346,6 +348,21 @@ public:
 	static void setEventLevel(ULogger::Level eventSentLevel) {eventLevel_ = eventSentLevel;}
 	static ULogger::Level eventLevel() {return eventLevel_;}
 
+	/**
+	 * If not empty, only show log messages from threads included in this list.
+	 */
+	static void setTreadIdFilter(const std::set<unsigned long> & ids) {threadIdFilter_ = ids;}
+	static void setTreadIdFilter(const std::vector<std::string> & ids);
+	static const std::set<unsigned long> & getTreadIdFilter() {return threadIdFilter_;}
+
+	/**
+	 * Threads can register to this list. If name is empty, it will
+	 * clear the thread in the list. Should be called from the thread itself.
+	 */
+	static void registerCurrentThread(const std::string & name);
+	static void unregisterCurrentThread();
+	static std::map<std::string, unsigned long> getRegisteredThreads();
+
     /**
      * Reset to default parameters.
      */
@@ -550,6 +567,9 @@ private:
 	static bool buffered_;
 
 	static std::string bufferedMsgs_;
+
+	static std::set<unsigned long> threadIdFilter_;
+	static std::map<std::string, unsigned long> registeredThreads_;
 };
 
 #endif // ULOGGER_H

utilite/src/ULogger.cpp

@@ -63,6 +63,8 @@ UMutex ULogger::loggerMutex_;
 const std::string ULogger::kDefaultLogFileName = "./ULog.txt";
 std::string ULogger::logFileName_;
 std::string ULogger::bufferedMsgs_;
+std::set<unsigned long> ULogger::threadIdFilter_;
+std::map<std::string, unsigned long> ULogger::registeredThreads_;
 
 /**
  * This class is used to write logs in the console. This class cannot
@@ -199,6 +201,58 @@ void ULogger::setType(Type type, const std::string &fileName, bool append)
     loggerMutex_.unlock();
 }
 
+void ULogger::setTreadIdFilter(const std::vector<std::string> & ids)
+{
+	loggerMutex_.lock();
+	threadIdFilter_.clear();
+	for(unsigned int i=0;i<ids.size();++i)
+	{
+		if(registeredThreads_.find(ids[i]) != registeredThreads_.end())
+		{
+			threadIdFilter_.insert(registeredThreads_.at(ids[i]));
+		}
+	}
+	loggerMutex_.unlock();
+}
+
+void ULogger::registerCurrentThread(const std::string & name)
+{
+	loggerMutex_.lock();
+	UASSERT(!name.empty());
+	uInsert(registeredThreads_, std::make_pair(name, UThread::currentThreadId()));
+	loggerMutex_.unlock();
+}
+
+void ULogger::unregisterCurrentThread()
+{
+	loggerMutex_.lock();
+
+	unsigned long id = UThread::currentThreadId();
+	for(std::map<std::string, unsigned long>::iterator iter=registeredThreads_.begin(); iter!=registeredThreads_.end();)
+	{
+		if(iter->second == id)
+		{
+			iter = registeredThreads_.erase(iter);
+			threadIdFilter_.erase(id);
+			break;
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
+	loggerMutex_.unlock();
+}
+
+std::map<std::string, unsigned long> ULogger::getRegisteredThreads()
+{
+	loggerMutex_.lock();
+	std::map<std::string, unsigned long> out = registeredThreads_;
+	loggerMutex_.unlock();
+	return out;
+}
+
 void ULogger::reset()
 {
 	ULogger::setType(ULogger::kTypeNoLog);
@@ -323,6 +377,13 @@ void ULogger::write(ULogger::Level level,
 		// No need to show an empty message if we don't print where.
 		return;
 	}
+	if(level < kFatal &&
+		threadIdFilter_.size() &&
+		threadIdFilter_.find(UThread::currentThreadId()) == threadIdFilter_.end())
+	{
+		loggerMutex_.unlock();
+		return;
+	}
 
     if(level >= level_ || level >= eventLevel_)
     {