Tango: updated export workflow, added sketchfab activity, added graph optimization parameters, fixed raw images kept in memory (disabled reexctract words on loop closure while updating memory to be able to create more features for transform estimation than needed in vocabulary), portrait/landscape orientation, updated to Eisa TangoSDK. DBReader: fixed high variance when node has no links (if not the first in the current map id). MainWindow: remove frustums not in the current graph.

app/android/AndroidManifest.xml.in

@@ -29,7 +29,7 @@
         <activity android:name="RTABMapActivity"
                 android:label="@string/app_name"
                 android:launchMode="singleTask"
-		android:screenOrientation="landscape"
+		android:screenOrientation="fullSensor"
 		android:configChanges="orientation|screenSize|keyboardHidden">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
@@ -40,7 +40,8 @@
             </intent-filter>
         </activity>
 
-        <activity android:name="SettingsActivity" android:label="@string/settings"/>
+        <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>
+        <activity android:name="SketchfabActivity" android:label="@string/sketchfab" android:screenOrientation="fullSensor"/>
     </application>
 
 </manifest>

app/android/jni/CameraTango.cpp

@@ -46,7 +46,10 @@ const int scanDownsampling = 10;
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
+	if(point_cloud->num_points>0)
+	{
+		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
+	}
 }
 
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
@@ -104,7 +107,8 @@ CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan)
 		rawScanPublished_(publishRawScan),
 		cloudStamp_(0),
 		tangoColorType_(0),
-		tangoColorStamp_(0)
+		tangoColorStamp_(0),
+		colorCameraToDisplayRotation_(ROTATION_0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -641,6 +645,51 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
 			//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
 
+			// Rotate image depending on the camera orientation
+			if(colorCameraToDisplayRotation_ == ROTATION_90)
+			{
+				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
+				cv::flip(rgb,rgb,1);
+				cv::transpose(rgb,rgbt);
+				rgb = rgbt;
+				cv::Mat deptht(depth.cols, depth.rows, depth.type());
+				cv::flip(depth,depth,1);
+				cv::transpose(depth,deptht);
+				depth = deptht;
+				cv::Size sizet(model.imageHeight(), model.imageWidth());
+				model = CameraModel(model.fy(), model.fx(), model.cy(), model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144));
+				model.setImageSize(sizet);
+			}
+			else if(colorCameraToDisplayRotation_ == ROTATION_180)
+			{
+				cv::flip(rgb,rgb,1);
+				cv::flip(rgb,rgb,0);
+				cv::flip(depth,depth,1);
+				cv::flip(depth,depth,0);
+				cv::Size sizet(model.imageWidth(), model.imageHeight());
+				model = CameraModel(
+						model.fx(),
+						model.fy(),
+						model.cx()>0?model.imageWidth()-model.cx():0,
+						model.cy()>0?model.imageHeight()-model.cy():0,
+						model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144*2.0));
+				model.setImageSize(sizet);
+			}
+			else if(colorCameraToDisplayRotation_ == ROTATION_270)
+			{
+				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
+				cv::transpose(rgb,rgbt);
+				cv::flip(rgbt,rgbt,1);
+				rgb = rgbt;
+				cv::Mat deptht(depth.cols, depth.rows, depth.type());
+				cv::transpose(depth,deptht);
+				cv::flip(deptht,deptht,1);
+				depth = deptht;
+				cv::Size sizet(model.imageHeight(), model.imageWidth());
+				model = CameraModel(model.fy(), model.fx(), model.cy()>0?model.imageHeight()-model.cy():0, model.cx(), model.localTransform()*rtabmap::Transform(0,0,0,0,0,-1.57079632679489661923132169163975144));
+				model.setImageSize(sizet);
+			}
+
 			if(rawScanPublished_)
 			{
 				data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);

app/android/jni/CameraTango.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <tango_support_api.h>
 
 class TangoPoseData;
 
@@ -82,6 +83,7 @@ public:
 	void setDecimation(int value) {decimation_ = value;}
 	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
+	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
@@ -114,6 +116,7 @@ private:
 	USemaphore dataReady_;
 	CameraModel model_;
 	Transform deviceTColorCamera_;
+	TangoSupportRotation colorCameraToDisplayRotation_;
 };
 
 } /* namespace rtabmap */

app/android/jni/RTABMapApp.cpp

@@ -94,18 +94,22 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
 
-	if(parameters.find(rtabmap::Parameters::kKpMaxFeatures())!=parameters.end() &&
+	if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end())
-	   parameters.find(rtabmap::Parameters::kVisMaxFeatures())!=parameters.end())
 	{
-		int featuresVoc = uStr2Int(parameters.at(rtabmap::Parameters::kKpMaxFeatures()));
+		if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("2") == 0) // GTSAM
-		int featuresLoop = uStr2Int(parameters.at(rtabmap::Parameters::kVisMaxFeatures()));
-		if(featuresVoc==0 || featuresLoop < featuresVoc)
 		{
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
 		}
-		else
+		else if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("1") == 0) // g2o
 		{
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("true")));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.0"));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
+		}
+		else // TORO
+		{
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"100":"0"));
 		}
 	}
 
@@ -166,7 +170,8 @@ RTABMapApp::RTABMapApp() :
 		renderingTime_(0.0f),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
-		exportedMesh_(new pcl::TextureMesh)
+		exportedMesh_(new pcl::TextureMesh),
+		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
 	mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
@@ -227,11 +232,14 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 
 void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 {
-	LOGI("Set orientation: display=%d camera=%d", displayRotation, cameraRotation);
+	TangoSupportRotation rotation = tango_gl::util::GetAndroidRotationFromColorCameraToDisplay(
-	main_scene_.setScreenRotation(displayRotation, cameraRotation);
+			displayRotation, cameraRotation);
+	LOGI("Set orientation: display=%d camera=%d -> %d", displayRotation, cameraRotation, (int)rotation);
+	main_scene_.setScreenRotation(rotation);
+	camera_->setScreenRotation(rotation);
 }
 
-void RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize)
+int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize)
 {
 	this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
@@ -245,6 +253,7 @@ void RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInM
 	}
 
 	//Rtabmap
+	mapToOdom_.setIdentity();
 	rtabmap_ = new rtabmap::Rtabmap();
 	rtabmap::ParametersMap parameters = getRtabmapParameters();
 
@@ -267,6 +276,13 @@ void RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInM
 			true,
 			true);
 
+	int status = 0;
+	if(signatures.size() && poses.empty())
+	{
+		LOGE("Failed to optimize the graph!");
+		status = -1;
+	}
+
 	optimizeOpenedDatabase_ = optimize;
 	clearSceneOnNextRender_ = true;
 	rtabmap::Statistics stats;
@@ -289,6 +305,8 @@ void RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInM
 	status_.first = rtabmap::RtabmapEventInit::kInitialized;
 	status_.second = "";
 	rtabmapMutex_.unlock();
+
+	return status;
 }
 
 bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
@@ -437,6 +455,11 @@ int RTABMapApp::Render()
 		UTimer fpsTime;
 		boost::mutex::scoped_lock  lock(renderingMutex_);
 
+		if(clearSceneOnNextRender_)
+		{
+			visualizingMesh_ = false;
+		}
+
 		bool notifyCameraStarted = false;
 
 		// process only pose events in vsualization mode
@@ -449,10 +472,19 @@ int RTABMapApp::Render()
 				poseEvents_.clear();
 			}
 		}
+		rtabmap::Transform mapOdom = rtabmap::Transform::getIdentity();
 		if(!pose.isNull())
 		{
 			// update camera pose?
-			main_scene_.SetCameraPose(opengl_world_T_tango_world*pose);
+			if(graphOptimization_ && !visualizingMesh_ && !mapToOdom_.isIdentity())
+			{
+				mapOdom = mapToOdom_;
+				main_scene_.SetCameraPose(opengl_world_T_rtabmap_world*mapOdom*rtabmap_world_T_tango_world*pose);
+			}
+			else
+			{
+				main_scene_.SetCameraPose(opengl_world_T_tango_world*pose);
+			}
 			if(!camera_->isRunning() && cameraJustInitialized_)
 			{
 				notifyCameraStarted = true;
@@ -476,11 +508,6 @@ int RTABMapApp::Render()
 			}
 		}
 
-		if(clearSceneOnNextRender_)
-		{
-			visualizingMesh_ = false;
-		}
-
 		if(visualizingMesh_)
 		{
 			if(exportedMeshUpdated_)
@@ -675,6 +702,10 @@ int RTABMapApp::Render()
 				}
 
 				std::map<int, rtabmap::Transform> poses = rtabmapEvents.back().poses();
+				if(!rtabmapEvents.back().mapCorrection().isNull())
+				{
+					mapToOdom_ = rtabmapEvents.back().mapCorrection();
+				}
 
 				// Transform pose in OpenGL world
 				for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -833,7 +864,7 @@ int RTABMapApp::Render()
 										odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
 										odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
 									   (int)cloud->width, (int)cloud->height);
-								main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*odomEvent.pose());
+								main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*mapOdom*odomEvent.pose());
 								main_scene_.setCloudVisible(-1, true);
 							}
 							else
@@ -1042,6 +1073,10 @@ void RTABMapApp::setLighting(bool enabled)
 {
 	main_scene_.setLighting(enabled);
 }
+void RTABMapApp::setBackfaceCulling(bool enabled)
+{
+	main_scene_.setBackfaceCulling(enabled);
+}
 
 void RTABMapApp::setLocalizationMode(bool enabled)
 {
@@ -1286,6 +1321,7 @@ void RTABMapApp::resetMapping()
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
 	status_.second = "";
 
+	mapToOdom_.setIdentity();
 	clearSceneOnNextRender_ = true;
 
 	UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdResetMemory));
@@ -1557,13 +1593,6 @@ bool RTABMapApp::exportMesh(
 
 					if(mergedClouds->size())
 					{
-						int before = mergedClouds->size();
-						if(optimizedVoxelSize > 0.0f)
-						{
-							mergedClouds = rtabmap::util3d::voxelize(mergedClouds, optimizedVoxelSize);
-							LOGI("Voxelized from %d points to %d points", before, (int)mergedClouds->size());
-						}
-
 						// Mesh reconstruction
 						LOGI("Mesh reconstruction...");
 						pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
@@ -2223,15 +2252,7 @@ int RTABMapApp::postProcessing(int approach)
 		// detect more loop closures
 		if(approach == -1 || approach == 2)
 		{
-			// detect more loop closures, don't re-extract features for this
-			rtabmap::ParametersMap parameters;
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
-			rtabmap_->parseParameters(parameters);
-
 			returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1);
-
-			// put back re-extraction if it was set
-			rtabmap_->parseParameters(this->getRtabmapParameters());
 		}
 
 		// graph optimization
@@ -2427,7 +2448,9 @@ void RTABMapApp::handleEvent(UEvent * event)
 		int highestHypId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f);
 		int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
 		int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
+		int matches = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_matches(), 0.0f);
 		int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
+		float optimizationMaxError = uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f);
 		float rehearsalValue = uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f);
 		int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
 		float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
@@ -2444,7 +2467,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFIF)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFF)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -2457,12 +2480,14 @@ void RTABMapApp::handleEvent(UEvent * event)
 								highestHypId,
 								databaseMemoryUsed,
 								inliers,
+								matches,
 								featuresExtracted,
 								hypothesis,
 								lastDrawnCloudsCount_,
 								renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
 								rejected,
-								rehearsalValue);
+								rehearsalValue,
+								optimizationMaxError);
 						success = true;
 					}
 				}

app/android/jni/RTABMapApp.h

@@ -55,7 +55,7 @@ class RTABMapApp : public UEventsHandler {
 
   void setScreenRotation(int displayRotation, int cameraRotation);
 
-  void openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
 
   bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
@@ -118,6 +118,7 @@ class RTABMapApp : public UEventsHandler {
   void setMeshRendering(bool enabled, bool withTexture);
   void setPointSize(float value);
   void setLighting(bool enabled);
+  void setBackfaceCulling(bool enabled);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
@@ -215,6 +216,8 @@ class RTABMapApp : public UEventsHandler {
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 
+	rtabmap::Transform mapToOdom_;
+
 	boost::mutex rtabmapMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;

app/android/jni/jni_interface.cpp

@@ -62,7 +62,7 @@ Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(
 	return app.setScreenRotation(displayRotation, cameraRotation);
 }
 
-JNIEXPORT void JNICALL
+JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
     JNIEnv* env, jobject, jstring databasePath, bool databaseInMemory, bool optimize)
 {
@@ -157,6 +157,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setLighting(
 	return app.setLighting(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setBackfaceCulling(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setBackfaceCulling(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
 		JNIEnv*, jobject, bool enabled)
 {

app/android/jni/scene.cpp

@@ -169,6 +169,7 @@ Scene::Scene() :
 		pointSize_(5.0f),
 		frustumCulling_(true),
 		lighting_(true),
+		backfaceCulling_(true),
 		r_(0.0f),
 		g_(0.0f),
 		b_(0.0f)
@@ -255,14 +256,6 @@ void Scene::DeleteResources() {
 	clear();
 }
 
-void Scene::setScreenRotation(int displayOrientation, int cameraOrientation)
-{
-	color_camera_to_display_rotation_ =
-	      tango_gl::util::GetAndroidRotationFromColorCameraToDisplay(
-	    		  displayOrientation, cameraOrientation);
-	LOGI("color_camera_to_display_rotation_=%d", color_camera_to_display_rotation_);
-}
-
 //Should only be called in OpenGL thread!
 void Scene::clear()
 {
@@ -299,32 +292,37 @@ int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
 
 	glEnable(GL_DEPTH_TEST);
-	glEnable(GL_CULL_FACE);
+	if(backfaceCulling_)
+	{
+		glEnable(GL_CULL_FACE);
+	}
+	else
+	{
+		glDisable(GL_CULL_FACE);
+	}
 
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
-	glm::mat4 rotateM;
-	if(gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
-	{
-		rotateM = glm::rotate<float>(float(color_camera_to_display_rotation_)*1.57079632679489661923132169163975144, glm::vec3(0.0f, 0.0f, 1.0f));
-	}
 	if(!currentPose_->isNull())
 	{
 		glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
 		Eigen::Quaternionf quat = currentPose_->getQuaternionf();
 		glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
 
+		glm::mat4 rotateM;
+		rotateM = glm::rotate<float>(float(color_camera_to_display_rotation_)*-1.57079632679489661923132169163975144, glm::vec3(0.0f, 0.0f, 1.0f));
+
 		if (gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
 		{
 			// In first person mode, we directly control camera's motion.
 			gesture_camera_->SetPosition(position);
-			gesture_camera_->SetRotation(rotation);
+			gesture_camera_->SetRotation(rotation*glm::quat(rotateM));
 		}
 		else
 		{
 			// In third person or top down mode, we follow the camera movement.
-			gesture_camera_->SetAnchorPosition(position, rotation);
+			gesture_camera_->SetAnchorPosition(position, rotation*glm::quat(rotateM));
 
 			frustum_->SetPosition(position);
 			frustum_->SetRotation(rotation);
@@ -332,25 +330,25 @@ int Scene::Render() {
 			// camera's aspect ratio, this is just for visualization purposes.
 			frustum_->SetScale(kFrustumScale);
 			frustum_->Render(gesture_camera_->GetProjectionMatrix(),
-					rotateM*gesture_camera_->GetViewMatrix());
+					gesture_camera_->GetViewMatrix());
 
 			axis_->SetPosition(position);
 			axis_->SetRotation(rotation);
 			axis_->Render(gesture_camera_->GetProjectionMatrix(),
-					rotateM*gesture_camera_->GetViewMatrix());
+					gesture_camera_->GetViewMatrix());
 		}
 
 		trace_->UpdateVertexArray(position);
 		if(traceVisible_)
 		{
 			trace_->Render(gesture_camera_->GetProjectionMatrix(),
-					rotateM*gesture_camera_->GetViewMatrix());
+					gesture_camera_->GetViewMatrix());
 		}
 
 		if(gridVisible_)
 		{
 			grid_->Render(gesture_camera_->GetProjectionMatrix(),
-					rotateM*gesture_camera_->GetViewMatrix());
+					gesture_camera_->GetViewMatrix());
 		}
 	}
 
@@ -401,7 +399,7 @@ int Scene::Render() {
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				++cloudDrawn;
-				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), rotateM*gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
+				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
 			}
 		}
 	}
@@ -412,14 +410,14 @@ int Scene::Render() {
 			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
 			{
 				++cloudDrawn;
-				iter->second->Render(gesture_camera_->GetProjectionMatrix(), rotateM*gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
 			}
 		}
 	}
 
 	if(graphVisible_ && graph_)
 	{
-		graph_->Render(gesture_camera_->GetProjectionMatrix(), rotateM*gesture_camera_->GetViewMatrix());
+		graph_->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix());
 	}
 
 	return cloudDrawn;

app/android/jni/scene.h

@@ -57,7 +57,7 @@ class Scene {
   // Setup GL view port.
   void SetupViewPort(int w, int h);
 
-  void setScreenRotation(int displayRotation, int cameraRotation);
+  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 
   void clear(); // removed all point clouds
 
@@ -125,6 +125,7 @@ class Scene {
   void setPointSize(float size) {pointSize_ = size;}
   void setFrustumCulling(bool enabled) {frustumCulling_ = enabled;}
   void setLighting(bool enabled) {lighting_ = enabled;}
+  void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
   void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
 
   bool isMeshRendering() const {return meshRendering_;}
@@ -132,6 +133,7 @@ class Scene {
   float getPointSize() const {return pointSize_;}
   bool isFrustumCulling() const {return frustumCulling_;}
   bool isLighting() const {return lighting_;}
+  bool isBackfaceCulling() const {return backfaceCulling_;}
 
  private:
   // Camera object that allows user to use touch input to interact with.
@@ -153,7 +155,7 @@ class Scene {
   bool gridVisible_;
   bool traceVisible_;
 
-  TangoSupportDisplayRotation color_camera_to_display_rotation_;
+  TangoSupportRotation color_camera_to_display_rotation_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
 
@@ -170,6 +172,7 @@ class Scene {
   float pointSize_;
   bool frustumCulling_;
   bool lighting_;
+  bool backfaceCulling_;
   float r_;
   float g_;
   float b_;

app/android/jni/tango-gl/include/tango-gl/util.h

@@ -89,7 +89,7 @@ namespace util {
   // available are 0, 90, 180, 270. Followed by Android camera orientation
   // standard:
   // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportDisplayRotation GetAndroidRotationFromColorCameraToDisplay(
+  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
       int display_rotation, int color_camera_rotation);
 
   // Get the Android rotation integer value from color camera to display.
@@ -101,8 +101,8 @@ namespace util {
   // available are 0, 90, 180, 270. Followed by Android camera orientation
   // standard:
   // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportDisplayRotation GetAndroidRotationFromColorCameraToDisplay(
+  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      TangoSupportDisplayRotation display_rotation, int color_camera_rotation);
+      TangoSupportRotation display_rotation, int color_camera_rotation);
 
 }  // namespace util
 }  // namespace tango_gl

app/android/jni/tango-gl/util.cpp

@@ -238,23 +238,23 @@ glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
   return glm::vec3(mat * glm::vec4(vec, 1.0f));
 }
 
-TangoSupportDisplayRotation util::GetAndroidRotationFromColorCameraToDisplay(
+TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
     int display_rotation, int color_camera_rotation) {
-  TangoSupportDisplayRotation r =
+  TangoSupportRotation r =
-      static_cast<TangoSupportDisplayRotation>(display_rotation);
+      static_cast<TangoSupportRotation>(display_rotation);
   return util::GetAndroidRotationFromColorCameraToDisplay(
       r, color_camera_rotation);
 }
 
-TangoSupportDisplayRotation util::GetAndroidRotationFromColorCameraToDisplay(
+TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    TangoSupportDisplayRotation display_rotation, int color_camera_rotation) {
+    TangoSupportRotation display_rotation, int color_camera_rotation) {
   int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
 
   int ret = static_cast<int>(display_rotation) - color_camera_n;
   if (ret < 0) {
     ret += 4;
   }
-  return static_cast<TangoSupportDisplayRotation>(ret % 4);
+  return static_cast<TangoSupportRotation>(ret % 4);
 }
 
 }  // namespace tango_gl

app/android/res/layout/activity_rtabmap.xml

@@ -280,6 +280,19 @@
            
     </LinearLayout>
 
+    <ToggleButton
+        android:id="@+id/backface_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/light_button"
+        android:layout_alignLeft="@+id/light_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="5dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/backface_off"
+        android:textOn="@string/backface_on" />
+   
     <ToggleButton
         android:id="@+id/light_button"
         android:layout_width="100dp"
@@ -336,18 +349,35 @@
         android:layout_alignLeft="@+id/first_person_button"
         android:layout_alignParentTop="true"
         android:layout_marginTop="61dp"
+        android:layout_alignParentRight="true"
         android:paddingRight="5dp"
         android:textOff="@string/pause"
         android:textOn="@string/resume" />
-    
+
+    <Button
+        android:id="@+id/button_shareToSketchfab"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:layout_alignParentTop="true"
+        android:layout_alignRight="@+id/pause_button"
+        android:text="@string/share_to_sketchfab" />
+
+    <Button
+        android:id="@+id/button_saveOnDevice"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:layout_alignParentTop="true"
+        android:layout_toLeftOf="@+id/button_shareToSketchfab"
+        android:text="@string/save_to_file" />
+
     <Button
         android:id="@+id/close_visualization_button"
         android:layout_width="200dp"
         android:layout_height="wrap_content"
-        android:layout_alignParentLeft="true"
+        android:layout_alignBaseline="@+id/top_down_button"
-        android:layout_alignParentBottom="true"
+        android:layout_alignBottom="@+id/top_down_button"
-        android:layout_marginLeft="5dp"
+        android:layout_centerHorizontal="true"
         android:paddingLeft="5dp"
-        android:text="@string/close_visualization"/>
+        android:text="@string/close_visualization" />
         
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -106,6 +106,13 @@
 		   			android:entries="@array/pref_sim_thr_keys"
 		   			android:entryValues="@array/pref_sim_thr_values"
 		   			android:defaultValue="@string/pref_default_sim_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_min_inliers"
+		    		android:title="@string/pref_title_min_inliers"
+		    		android:summary="@string/pref_summary_min_inliers"
+		   			android:entries="@array/pref_min_inliers_keys"
+		   			android:entryValues="@array/pref_min_inliers_values"
+		   			android:defaultValue="@string/pref_default_min_inliers"/>
 		        <ListPreference 
 		            android:key="@string/pref_key_opt_error"
 		    		android:title="@string/pref_title_opt_error"
@@ -134,6 +141,18 @@
 		   			android:entries="@array/pref_features_type_keys"
 		   			android:entryValues="@array/pref_features_type_values"
 		   			android:defaultValue="@string/pref_default_features_type"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_optimizer"
+		    		android:title="@string/pref_title_optimizer"
+		    		android:summary="@string/pref_summary_optimizer"
+		   			android:entries="@array/pref_optimizer_keys"
+		   			android:entryValues="@array/pref_optimizer_values"
+		   			android:defaultValue="@string/pref_default_optimizer"/>
+		        <SwitchPreference 
+		            android:key="@string/pref_key_optimize_end"
+		    		android:title="@string/pref_title_optimize_end"
+		    		android:summary="@string/pref_summary_optimize_end"
+		   			android:defaultValue="@string/pref_default_optimize_end"/>
 		    </PreferenceCategory>
 	        <PreferenceCategory
 	        	android:title="@string/pref_title_mapping_database">

app/android/res/layout/activity_sketchfab.xml

@@ -0,0 +1,81 @@
+<?xml version="1.0" encoding="utf-8"?>
+<ScrollView xmlns:android="http://schemas.android.com/apk/res/android" 
+    android:layout_width="match_parent"
+    android:layout_height="match_parent">
+<LinearLayout android:layout_width="match_parent"
+              android:layout_height="match_parent"
+              android:orientation="vertical"
+              android:focusableInTouchMode="true" >
+              
+    <TextView android:id="@+id/text"
+              android:layout_width="wrap_content"
+              android:layout_height="wrap_content"
+              android:text="Model Name*:" />
+
+    <EditText
+        android:id="@+id/editText_filename"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:imeOptions="flagNoExtractUi"
+        android:ems="10" />
+
+    <TextView
+        android:id="@+id/textView1"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:text="Description:" />
+
+    <EditText
+        android:id="@+id/editText_description"
+        android:ems="10"
+        android:lines="4" 
+	    android:minLines="2" 
+	    android:gravity="top|left" 
+	    android:maxLines="8" 
+	    android:layout_weight="1" 
+	    android:layout_height="wrap_content" 
+	    android:layout_width="fill_parent" 
+	    android:scrollbars="vertical"
+	    android:imeOptions="flagNoExtractUi"
+        android:inputType="textMultiLine" >
+
+        <requestFocus />
+    </EditText>
+
+    <TextView
+        android:id="@+id/textView2"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:text="Tags:" />
+
+    <EditText
+        android:id="@+id/editText_tags"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:imeOptions="flagNoExtractUi"
+        android:ems="10" />
+
+    <CheckBox
+        android:id="@+id/checkBox_draft"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:checked="true"
+        android:text="Draft Mode" />
+
+    <LinearLayout
+        android:layout_width="match_parent"
+        android:layout_height="0dip"
+        android:layout_weight="1"
+        android:gravity="right|bottom"
+        android:orientation="horizontal" >
+
+        <Button
+            android:id="@+id/button_ok"
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:layout_weight="0.06"
+            android:text="Upload" />
+    </LinearLayout>
+
+</LinearLayout>
+</ScrollView>

app/android/res/menu/optionmenu.xml

@@ -8,8 +8,26 @@
 		  <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
 	   </group>
 	   <item android:id="@+id/save" android:title="Save" android:showAsAction="ifRoom"/>
-              
+                     
-       <item android:id="@+id/post_processing" android:title="Optimize" android:showAsAction="ifRoom">
+       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
+          <menu>
+              <item android:id="@+id/export_point_cloud" android:title="Point Cloud" />
+              <item android:id="@+id/export_mesh_menu" android:title="Raw Mesh..." >
+	              <menu>
+		              <item android:id="@+id/export_mesh" android:title="Colored Mesh" />
+			          <item android:id="@+id/export_mesh_texture" android:title="Textured Mesh" />
+		          </menu>
+	          </item>
+	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
+	              <menu>
+		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
+			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
+		          </menu>
+	          </item>
+          </menu>
+       </item>
+       
+        <item android:id="@+id/post_processing" android:title="Optimize" android:showAsAction="ifRoom">
           <menu>
              <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
              <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
@@ -26,24 +44,6 @@
           </menu>
        </item>
        
-       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
-          <menu>
-              <item android:id="@+id/export_point_cloud" android:title="Point Cloud (*.ply)" />
-              <item android:id="@+id/export_mesh_menu" android:title="Raw Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_mesh" android:title="Colored Mesh (*.ply)" />
-			          <item android:id="@+id/export_mesh_texture" android:title="Textured Mesh (*.obj)" />
-		          </menu>
-	          </item>
-	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh (*.ply)" />
-			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh (*.obj)" />
-		          </menu>
-	          </item>
-          </menu>
-       </item>
-       
        <item android:id="@+id/open" android:title="Open" android:showAsAction="ifRoom"/>
        
        <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -27,7 +27,7 @@ public class RTABMapLib
     
     public static native void setScreenRotation(int displayRotation, int cameraRotation);
     
-    public static native void openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
     
     /*
      * Called when the Tango service is connected.
@@ -75,6 +75,7 @@ public class RTABMapLib
     public static native void setMaxCloudDepth(float value);
     public static native void setPointSize(float value);
     public static native void setLighting(boolean enabled);
+    public static native void setBackfaceCulling(boolean enabled);
     public static native void setMeshDecimation(int value);
     public static native void setMeshAngleTolerance(float value);
     public static native void setMeshTriangleSize(int value);

app/android/src/com/introlab/rtabmap/SettingsActivity.java

@@ -39,10 +39,12 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
         ((Preference)findPreference(getString(R.string.pref_key_sim_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_sim_thr))).getEntry() + ") "+getString(R.string.pref_summary_sim_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_min_inliers))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_inliers))).getEntry() + ") "+getString(R.string.pref_summary_min_inliers));
         ((Preference)findPreference(getString(R.string.pref_key_opt_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_error))).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
         ((Preference)findPreference(getString(R.string.pref_key_features_voc))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_voc))).getEntry() + ") "+getString(R.string.pref_summary_features_voc));
         ((Preference)findPreference(getString(R.string.pref_key_features))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features))).getEntry() + ") "+getString(R.string.pref_summary_features));
         ((Preference)findPreference(getString(R.string.pref_key_features_type))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_type))).getEntry() + ") "+getString(R.string.pref_summary_features_type));
+        ((Preference)findPreference(getString(R.string.pref_key_optimizer))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_optimizer))).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
         
         ((Preference)findPreference(getString(R.string.pref_key_cloud_voxel))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cloud_voxel))).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         ((Preference)findPreference(getString(R.string.pref_key_texture_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_size))).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
@@ -72,10 +74,12 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
         	if(key.compareTo(getString(R.string.pref_key_sim_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_sim_thr));
+        	if(key.compareTo(getString(R.string.pref_key_min_inliers))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_inliers));
         	if(key.compareTo(getString(R.string.pref_key_opt_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
         	if(key.compareTo(getString(R.string.pref_key_features_voc))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_voc));
         	if(key.compareTo(getString(R.string.pref_key_features))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features));
         	if(key.compareTo(getString(R.string.pref_key_features_type))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_type));
+        	if(key.compareTo(getString(R.string.pref_key_optimizer))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
         	
         	if(key.compareTo(getString(R.string.pref_key_cloud_voxel))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         	if(key.compareTo(getString(R.string.pref_key_texture_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_size));

app/android/src/com/introlab/rtabmap/SketchfabActivity.java

@@ -0,0 +1,505 @@
+package com.introlab.rtabmap;
+
+import java.io.BufferedInputStream;
+import java.io.BufferedOutputStream;
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.InputStreamReader;
+import java.net.SocketTimeoutException;
+import java.text.SimpleDateFormat;
+import java.util.Date;
+import java.util.List;
+import java.util.zip.ZipEntry;
+import java.util.zip.ZipOutputStream;
+
+import org.apache.http.HttpEntity;
+import org.apache.http.HttpResponse;
+import org.apache.http.client.HttpClient;
+import org.apache.http.client.methods.HttpPatch;
+import org.apache.http.entity.StringEntity;
+import org.apache.http.impl.client.DefaultHttpClient;
+
+import android.app.Activity;
+import android.app.AlertDialog;
+import android.app.Dialog;
+import android.app.ProgressDialog;
+import android.content.Context;
+import android.content.DialogInterface;
+import android.content.Intent;
+import android.content.SharedPreferences;
+import android.net.ConnectivityManager;
+import android.net.NetworkInfo;
+import android.os.AsyncTask;
+import android.os.Bundle;
+import android.preference.PreferenceManager;
+import android.text.Editable;
+import android.text.InputType;
+import android.text.SpannableString;
+import android.text.TextWatcher;
+import android.text.method.LinkMovementMethod;
+import android.text.util.Linkify;
+import android.util.Log;
+import android.view.View;
+import android.view.View.OnClickListener;
+import android.webkit.WebView;
+import android.webkit.WebViewClient;
+import android.widget.Button;
+import android.widget.CheckBox;
+import android.widget.EditText;
+import android.widget.TextView;
+import android.widget.Toast;
+import android.widget.ToggleButton;
+
+public class SketchfabActivity extends Activity implements OnClickListener {
+	
+	private static final String AUTHORIZE_PATH	= "https://sketchfab.com/oauth2/authorize";
+	private static final String CLIENT_ID		= "RXrIJYAwlTELpySsyM8TrK9r3kOGQ5Qjj9VVDIfV";
+	private static final String REDIRECT_URI	= "https://introlab.github.io/rtabmap/oauth2_redirect";
+	
+	public static final int ZIP_BUFFER_SIZE = 1<<20; // 1MB
+	
+	ProgressDialog mProgressDialog;
+	
+	private Dialog mAuthDialog;
+	
+	private String mAuthToken;
+	private boolean mExportedOBJ;
+	private String mWorkingDirectory;
+	
+	EditText mFilename;
+	EditText mDescription;
+	EditText mTags;
+	CheckBox mDraft;
+	Button mButtonOk;
+	
+	private SketchfabActivity getActivity() {return this;}
+
+	@Override
+	protected void onCreate(Bundle savedInstanceState) {
+		super.onCreate(savedInstanceState);
+		setContentView(R.layout.activity_sketchfab);
+		
+		mFilename = (EditText)findViewById(R.id.editText_filename);
+		mDescription = (EditText)findViewById(R.id.editText_description);
+		mTags = (EditText)findViewById(R.id.editText_tags);
+		mDraft = (CheckBox)findViewById(R.id.checkBox_draft);
+		mButtonOk = (Button)findViewById(R.id.button_ok);
+		
+		mProgressDialog = new ProgressDialog(this);
+		mProgressDialog.setCanceledOnTouchOutside(false);
+		
+		mAuthToken = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY);
+		mExportedOBJ = getIntent().getExtras().getBoolean(RTABMapActivity.RTABMAP_EXPORTED_OBJ_KEY);
+		mFilename.setText(getIntent().getExtras().getString(RTABMapActivity.RTABMAP_FILENAME_KEY));
+		mWorkingDirectory = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_WORKING_DIR_KEY);
+		
+		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		String tags = sharedPref.getString(getString(R.string.pref_key_tags), getString(R.string.pref_default_tags));
+		if(tags.isEmpty())
+		{
+			tags = getString(R.string.pref_default_tags);
+		}
+		mTags.setText(tags);
+		
+		mButtonOk.setEnabled(mFilename.getText().toString().length()>0);
+		mButtonOk.setOnClickListener(this);
+		
+		mFilename.addTextChangedListener(new TextWatcher() {
+
+			   @Override
+			   public void afterTextChanged(Editable s) {}
+
+			   @Override    
+			   public void beforeTextChanged(CharSequence s, int start,
+			     int count, int after) {
+			   }
+
+			   @Override    
+			   public void onTextChanged(CharSequence s, int start,
+			     int before, int count) {
+			      mButtonOk.setEnabled(s.length() != 0);
+			   }
+			  });
+		
+		mFilename.setSelectAllOnFocus(true);
+		mFilename.requestFocus();
+	}
+	
+	@Override
+	public void onClick(View v) {
+		// Handle button clicks.
+		switch (v.getId()) {
+		case R.id.button_ok:
+			shareToSketchfab();
+			break;
+		default:
+			return;
+		}
+	}
+	
+	private void shareToSketchfab()
+	{
+		if(!mTags.getText().toString().isEmpty())
+		{
+			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+			SharedPreferences.Editor editor = sharedPref.edit();
+			editor.putString(getString(R.string.pref_key_tags), mTags.getText().toString());
+			// Commit the edits!
+			editor.commit();
+		}
+		
+		final String extension  = mExportedOBJ?".obj":".ply";
+		
+		String[] files = new String[0];
+		// verify if we have all files
+		if(extension.compareTo(".obj") == 0)
+		{
+			File objFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".obj");
+			File mltFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".mtl");
+			File jpgFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".jpg");
+			if(objFile.exists() && mltFile.exists() && jpgFile.exists())
+			{
+				files = new String[3];
+				files[0] = objFile.getAbsolutePath();
+				files[1] = mltFile.getAbsolutePath();
+				files[2] = jpgFile.getAbsolutePath();
+			}
+			else
+			{
+				Toast.makeText(getActivity(), String.format("Missing OBJ files!"), Toast.LENGTH_LONG).show();
+			}
+		}
+		else if(extension.compareTo(".ply") == 0)
+		{
+			File plyFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + extension);
+			if(plyFile.exists())
+			{
+				files = new String[1];
+				files[0] = plyFile.getAbsolutePath();
+			}
+			else
+			{
+				Toast.makeText(getActivity(), String.format("Missing PLY file!"), Toast.LENGTH_LONG).show();
+			}
+		}
+		else
+		{
+			Toast.makeText(getActivity(), String.format("Unknown file extension \"%s\"!", extension), Toast.LENGTH_LONG).show();
+		}
+
+		if(files.length > 0)
+		{
+			final String[] filesToZip = files;
+			authorizeAndPublish(filesToZip, mFilename.getText().toString());
+		}
+	
+	}
+	
+	private void authorizeAndPublish(final String[] filesToZip, final String fileName)
+	{
+		if(!isNetworkAvailable())
+		{
+			// Visualize the result?
+			new AlertDialog.Builder(this)
+			.setTitle("Sharing to Sketchfab...")
+			.setMessage("Network is not available. Make sure you have internet before continuing.")
+			.setPositiveButton("Try Again", new DialogInterface.OnClickListener() {
+				public void onClick(DialogInterface dialog, int which) {
+					authorizeAndPublish(filesToZip, fileName);
+				}
+			})
+			.setNeutralButton("Abort", new DialogInterface.OnClickListener() {
+				public void onClick(DialogInterface dialog, int which) {
+				}
+			})
+			.show();
+			return;
+		}	
+
+		// get token the first time
+		if(mAuthToken == null)
+		{
+			Log.i(RTABMapActivity.TAG,"We don't have the token, get it!");
+
+			WebView web;
+			mAuthDialog = new Dialog(this);
+			mAuthDialog.setContentView(R.layout.auth_dialog);
+			web = (WebView)mAuthDialog.findViewById(R.id.webv);
+			web.getSettings().setJavaScriptEnabled(true);
+			String auth_url = AUTHORIZE_PATH+"?redirect_uri="+REDIRECT_URI+"&response_type=token&client_id="+CLIENT_ID;
+			Log.i(RTABMapActivity.TAG, "Auhorize url="+auth_url);
+			web.setWebViewClient(new WebViewClient() {
+
+				boolean authComplete = false;
+
+				@Override
+				public void onPageFinished(WebView view, String url) {
+					super.onPageFinished(view, url);
+
+					//Log.i(TAG,"onPageFinished url="+url);
+					if(url.contains("error=access_denied")){
+						Log.e(RTABMapActivity.TAG, "ACCESS_DENIED_HERE");
+						authComplete = true;
+						Toast.makeText(getApplicationContext(), "Error Occured", Toast.LENGTH_SHORT).show();
+						mAuthDialog.dismiss();
+					}
+					else if (url.startsWith(REDIRECT_URI) && url.contains("access_token") && authComplete != true) {
+						//Log.i(TAG,"onPageFinished received token="+url);
+						String[] sArray = url.split("access_token=");
+						mAuthToken = (sArray[1].split("&token_type=Bearer"))[0];
+						authComplete = true;
+						
+						mAuthDialog.dismiss();
+
+						zipAndPublish(filesToZip, fileName);
+					}
+				}
+			});
+			mAuthDialog.show();
+			mAuthDialog.setTitle("Authorize RTAB-Map");
+			mAuthDialog.setCancelable(true);
+			web.loadUrl(auth_url);
+		}
+		else
+		{
+			zipAndPublish(filesToZip, fileName);
+		}
+	}
+
+	private void zipAndPublish(final String[] filesToZip, final String fileName)
+	{		
+		final String zipOutput = mWorkingDirectory+fileName+".zip";
+
+		mProgressDialog.setTitle("Upload to Sketchfab");
+		mProgressDialog.setMessage(String.format("Compressing the files..."));
+		mProgressDialog.show();
+
+		Thread workingThread = new Thread(new Runnable() {
+			public void run() {
+				try{
+					zip(filesToZip, zipOutput);
+					runOnUiThread(new Runnable() {
+						public void run() {
+							mProgressDialog.dismiss();
+
+							File f = new File(zipOutput);
+
+							// Continue?
+							AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+							builder.setTitle("File(s) compressed and ready to upload!");
+							
+							final int fileSizeMB = (int)f.length()/(1024 * 1024);
+							final int fileSizeKB = (int)f.length()/(1024);
+							if(fileSizeMB == 0)
+							{
+								Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d KB", fileSizeKB));
+								builder.setMessage(String.format("Total size to upload = %d KB. Do you want to continue?\n\n", fileSizeKB));
+							}
+							else
+							{
+								Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d MB", fileSizeMB));
+								builder.setMessage(String.format("Total size to upload = %d MB. %sDo you want to continue?\n\n"
+										+ "Tip: To reduce the model size, try \"Optimized Mesh\" export. "
+										+ "You can also look at the Settings->Exporting options to reduce the output size.", fileSizeMB,
+										fileSizeMB>=50?"Note that for size over 50 MB, a Sketchfab PRO account is required, otherwise the upload may fail. ":""));
+							}
+
+							
+							builder.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+								public void onClick(DialogInterface dialog, int which) {
+									mProgressDialog.setTitle("Upload to Sketchfab");
+									if(fileSizeMB == 0)
+									{
+										mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d KB) to Sketchfab...", fileName, fileSizeKB));
+									}
+									else
+									{
+										mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d MB) to Sketchfab...", fileName, fileSizeMB));
+									}
+									mProgressDialog.show();
+									new uploadToSketchfabTask().execute(zipOutput, fileName);
+								}
+							});
+							builder.setNegativeButton("No", new DialogInterface.OnClickListener() {
+								public void onClick(DialogInterface dialog, int which) {
+									// do nothing...
+								}
+							});
+							builder.show();
+						}
+					});
+				}
+				catch(IOException ex) {
+					Log.e(RTABMapActivity.TAG, "Failed to zip", ex);
+				}
+			}
+		});
+
+		workingThread.start();
+	}
+	
+	private boolean isNetworkAvailable() {
+	    ConnectivityManager connectivityManager 
+	          = (ConnectivityManager) getSystemService(Context.CONNECTIVITY_SERVICE);
+	    NetworkInfo activeNetworkInfo = connectivityManager.getActiveNetworkInfo();
+	    return activeNetworkInfo != null && activeNetworkInfo.isConnected();
+	}
+
+	public static void zip(String file, String zipFile) throws IOException {
+		Log.i(RTABMapActivity.TAG, "Zipping " + file +" to " + zipFile);
+		String[] files = new String[1];
+		files[0] = file;
+		zip(files, zipFile);
+	}
+
+	public static void zip(String[] files, String zipFile) throws IOException {
+		Log.i(RTABMapActivity.TAG, "Zipping " + String.valueOf(files.length) +" files to " + zipFile);
+		BufferedInputStream origin = null;
+		ZipOutputStream out = new ZipOutputStream(new BufferedOutputStream(new FileOutputStream(zipFile)));
+		try { 
+			byte data[] = new byte[ZIP_BUFFER_SIZE];
+
+			for (int i = 0; i < files.length; i++) {
+				FileInputStream fi = new FileInputStream(files[i]);    
+				origin = new BufferedInputStream(fi, ZIP_BUFFER_SIZE);
+				try {
+					ZipEntry entry = new ZipEntry(files[i].substring(files[i].lastIndexOf("/") + 1));
+					out.putNextEntry(entry);
+					int count;
+					while ((count = origin.read(data, 0, ZIP_BUFFER_SIZE)) != -1) {
+						out.write(data, 0, count);
+					}
+				}
+				finally {
+					origin.close();
+				}
+			}
+		}
+		finally {
+			out.close();
+		}
+	}
+
+	private class uploadToSketchfabTask extends AsyncTask<String, Void, Void>
+	{
+		String mModelUri;
+		String mModelFilePath;
+		String error = "";
+		String mFileName;
+
+		protected void onPreExecute() {
+			//display progress dialog.
+		}
+
+		@Override
+		protected void onPostExecute(Void result) {
+			
+			mProgressDialog.dismiss();
+			//Task you want to do on UIThread after completing Network operation
+			//onPostExecute is called after doInBackground finishes its task.
+			if(mModelFilePath!= null)
+			{
+				File f = new File(mModelFilePath);
+				f.delete(); // cleanup
+
+				// See on sketchfab?
+				  final SpannableString s = new SpannableString(
+				        "Model \"" + mFileName + "\" is now processing on Sketchfab! You can click "
+				        + "on the link below to see it on Sketchfab.\n\nhttps://sketchfab.com/models/"+mModelUri);
+				  Linkify.addLinks(s, Linkify.WEB_URLS);
+				final AlertDialog d = new AlertDialog.Builder(getActivity())
+				.setTitle("Upload finished!")
+				.setCancelable(false)
+				.setMessage(s)
+				.setPositiveButton("Close", new DialogInterface.OnClickListener() {
+					public void onClick(DialogInterface dialog, int which) {
+						Intent resultIntent = new Intent();
+						resultIntent.putExtra(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY, mAuthToken); 
+						setResult(Activity.RESULT_OK, resultIntent);
+						finish();
+					}
+				}).create();
+				d.show();
+				((TextView)d.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
+			}
+			else
+			{
+				Toast.makeText(getApplicationContext(), String.format("Upload failed! Error=\"%s\"", error), Toast.LENGTH_SHORT).show();
+			}
+		}
+
+		@Override
+		protected Void doInBackground(String... files) {
+			String charset = "UTF-8";
+			File uploadFile = new File(files[0]);
+			mFileName = files[1];
+			String requestURL = "https://api.sketchfab.com/v3/models";
+
+			Log.i(RTABMapActivity.TAG, "Uploading " + files[0]);
+
+			try {
+				MultipartUtility multipart = new MultipartUtility(requestURL, mAuthToken, charset);
+
+				multipart.addFormField("name", mFileName);
+				multipart.addFormField("description", mDescription.getText().toString());
+				multipart.addFormField("tags", mTags.getText().toString());
+				multipart.addFormField("source", "RTAB-Map");
+				multipart.addFormField("isPublished", mDraft.isChecked()?"true":"false");  				
+				multipart.addFilePart("modelFile", uploadFile);
+
+				Log.i(RTABMapActivity.TAG, "Starting multipart request");
+				List<String> response = multipart.finish();
+
+				Log.i(RTABMapActivity.TAG, "SERVER REPLIED:");
+
+				for (String line : response) {
+					Log.i(RTABMapActivity.TAG, line);
+					//{"uri":"https:\/\/api.sketchfab.com\/v3\/models\/XXXXXXXXX","uid":"XXXXXXXXXX"}
+					if(line.contains("\"uid\":\""))
+					{
+						String[] sArray = line.split("\"uid\":\"");
+						mModelUri = (sArray[1].split("\""))[0];
+						mModelFilePath = files[0];
+
+						//patch model for orientation
+						/*HttpClient httpClient = new DefaultHttpClient(); 
+						try {
+							String patchURL = "https://api.sketchfab.com/v3/models/"+ mModelUri +"/options";
+							HttpPatch request = new HttpPatch(patchURL);
+							String json = 
+											"{\n"+
+												"uid: "+ mModelUri + "\n" +
+												"shading: shadeless\n"+
+												//"orientation:\n"+
+												//"{\n"+
+												//	"axis : [1, 0, 0]\n"+
+												//	"angle : 0\n"+
+												//"}\n"+
+											"}";
+
+							request.setHeader("Authorization", "Bearer " + mAuthToken);
+							StringEntity params =new StringEntity(json, "UTF-8");
+							params.setContentType("application/json");
+							request.setEntity(params);
+							HttpResponse responsePatch = httpClient.execute(request);
+							int responseStatus = responsePatch.getStatusLine().getStatusCode();
+							Log.i(RTABMapActivity.TAG, "get data responseStatus: " + responseStatus);
+
+						}catch (Exception e) {
+							Log.e(RTABMapActivity.TAG, "Error while patching model", e);
+							error = e.getMessage();
+						}*/
+					}
+				}    		  
+			} catch (IOException ex) {
+				Log.e(RTABMapActivity.TAG, "Error while uploading", ex);
+				error = ex.getMessage();
+			}
+			return null;
+		}
+	}
+}

corelib/include/rtabmap/core/DBReader.h

@@ -87,6 +87,8 @@ private:
 	UTimer _timer;
 	std::set<int> _ids;
 	std::set<int>::iterator _currentId;
+	int _previousMapId;
+	cv::Mat _previousInfMatrix;
 	double _previousStamp;
 	int _previousMapID;
 	bool _calibrated;

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -94,7 +94,8 @@ public:
 	static int findPairs(
 			const std::map<int, cv::KeyPoint> & wordsA,
 			const std::map<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
@@ -103,7 +104,8 @@ public:
 	static int findPairs(
 			const std::multimap<int, cv::KeyPoint> & wordsA,
 			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
@@ -112,7 +114,8 @@ public:
 	static int findPairsUnique(
 			const std::multimap<int, cv::KeyPoint> & wordsA,
 			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
@@ -121,7 +124,8 @@ public:
 	static int findPairsAll(
 			const std::multimap<int, cv::KeyPoint> & wordsA,
 			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	static cv::Mat linearLSTriangulation(
 			cv::Point3d u,    //homogenous image point (u,v,1)

corelib/include/rtabmap/core/Features2d.h

@@ -135,6 +135,7 @@ public:
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);

corelib/include/rtabmap/core/Memory.h

@@ -263,6 +263,7 @@ private:
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
 	bool _createOccupancyGrid;
+	int _visMaxFeatures;
 
 	int _idCount;
 	int _idMapCount;

corelib/include/rtabmap/core/Signature.h

@@ -102,10 +102,11 @@ public:
 	void removeAllWords();
 	void removeWord(int wordId);
 	void changeWordsRef(int oldWordId, int activeWordId);
-	void setWords(const std::multimap<int, cv::KeyPoint> & words) {_enabled = false;_words = words;}
+	void setWords(const std::multimap<int, cv::KeyPoint> & words);
 	bool isEnabled() const {return _enabled;}
 	void setEnabled(bool enabled) {_enabled = enabled;}
 	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
+	int getInvalidWordsCount() const {return _invalidWordsCount;}
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
 	const std::multimap<int, cv::Mat> & getWordsDescriptors() const {return _wordsDescriptors;}
 	void setWordsDescriptors(const std::multimap<int, cv::Mat> & descriptors) {_wordsDescriptors = descriptors;}
@@ -142,6 +143,7 @@ private:
 	std::multimap<int, cv::Mat> _wordsDescriptors;
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
+	int _invalidWordsCount;
 
 	Transform _pose;
 	Transform _groundTruthPose;

corelib/include/rtabmap/core/Statistics.h

@@ -61,6 +61,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
 	RTABMAP_STATS(Loop, Visual_inliers,);
+	RTABMAP_STATS(Loop, Visual_matches,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
 	RTABMAP_STATS(Loop, Optimization_error, );

corelib/src/DBDriver.cpp

@@ -513,6 +513,7 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 		{
 			for(std::set<int>::iterator iter = ids.begin(); iter != ids.end();)
 			{
+				UASSERT(*iter>0);
 				wIter = _trashVisualWords.find(*iter);
 				if(wIter != _trashVisualWords.end())
 				{

corelib/src/DBDriverSqlite3.cpp

@@ -2322,37 +2322,32 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 		{
 			int index=0;
 			id = sqlite3_column_int(ppStmt, index++); 			// VisualWord Id
-			if(id>0)
+
+			descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
+			descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
+			dRealSize = sqlite3_column_bytes(ppStmt, index++);
+
+			cv::Mat d;
+			if(dRealSize == descriptorSize)
 			{
-				descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
+				// CV_8U binary descriptors
-				descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
+				d = cv::Mat(1, descriptorSize, CV_8U);
-				dRealSize = sqlite3_column_bytes(ppStmt, index++);
+			}
-
+			else if(dRealSize/int(sizeof(float)) == descriptorSize)
-				cv::Mat d;
+			{
-				if(dRealSize == descriptorSize)
+				// CV_32F
-				{
+				d = cv::Mat(1, descriptorSize, CV_32F);
-					// CV_8U binary descriptors
-					d = cv::Mat(1, descriptorSize, CV_8U);
-				}
-				else if(dRealSize/int(sizeof(float)) == descriptorSize)
-				{
-					// CV_32F
-					d = cv::Mat(1, descriptorSize, CV_32F);
-				}
-				else
-				{
-					UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
-				}
-
-				memcpy(d.data, descriptor, dRealSize);
-				VisualWord * vw = new VisualWord(id, d);
-				vw->setSaved(true);
-				dictionary->addWord(vw);
 			}
 			else
 			{
-				ULOGGER_ERROR("Wrong word id ?!? (%d)", id);
+				UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
 			}
+
+			memcpy(d.data, descriptor, dRealSize);
+			VisualWord * vw = new VisualWord(id, d);
+			vw->setSaved(true);
+			dictionary->addWord(vw);
+
 			if(++count % 5000 == 0)
 			{
 				ULOGGER_DEBUG("Loaded %d words...", count);
@@ -2888,18 +2883,18 @@ void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool up
 		{
 			w = *i;
 			int index = 1;
-			if(w)
+			UASSERT(w);
-			{
-				rc = sqlite3_bind_int(ppStmt, index++, w->id());
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-				//step
+			rc = sqlite3_bind_int(ppStmt, index++, w->id());
-				rc=sqlite3_step(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			//step
+			rc=sqlite3_step(ppStmt);
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			rc = sqlite3_reset(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-				rc = sqlite3_reset(ppStmt);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-			}
 		}
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
@@ -3083,7 +3078,8 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 			for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
 				const VisualWord * w = *iter;
-				if(w && !w->isSaved())
+				UASSERT(w);
+				if(!w->isSaved())
 				{
 					rc = sqlite3_bind_int(ppStmt, 1, w->id());
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBReader.cpp

@@ -59,6 +59,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
+	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
 	_calibrated(false)
@@ -81,6 +82,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
+	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
 	_calibrated(false)
@@ -108,6 +110,8 @@ bool DBReader::init(
 	}
 	_ids.clear();
 	_currentId=_ids.end();
+	_previousMapId = -1;
+	_previousInfMatrix = cv::Mat();
 	_previousStamp = 0;
 	_previousMapID = 0;
 	_calibrated = false;
@@ -327,12 +331,24 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				{
 					// assume the first is the backward neighbor, take its variance
 					infMatrix = links.begin()->second.infMatrix();
+					_previousInfMatrix = infMatrix;
 				}
-				else
+				else if(_previousMapId != mapId)
 				{
 					// first node, set high variance to make rtabmap trigger a new map
 					infMatrix /= 9999.0;
+					UDEBUG("First node of map %d, variance set to 9999", mapId);
 				}
+				else
+				{
+					if(_previousInfMatrix.empty())
+					{
+						_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+					}
+					// we have a node not linked to map, use last variance
+					infMatrix = _previousInfMatrix;
+				}
+				_previousMapId = mapId;
 			}
 			else
 			{
@@ -435,6 +451,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				{
 					info->odomPose = pose;
 					info->odomCovariance = infMatrix.inv();
+					UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
 				}
 			}
 		}

corelib/src/EpipolarGeometry.cpp

@@ -413,17 +413,21 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
 int EpipolarGeometry::findPairs(
 		const std::map<int, cv::KeyPoint> & wordsA,
 		const std::map<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	int realPairsCount = 0;
 	pairs.clear();
 	for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
 	{
-		std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+		if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
-		if(ptB != wordsB.end())
 		{
-			pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
+			std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
-			++realPairsCount;
+			if(ptB != wordsB.end())
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
+				++realPairsCount;
+			}
 		}
 	}
 	return realPairsCount;
@@ -435,7 +439,8 @@ int EpipolarGeometry::findPairs(
  */
 int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	const std::list<int> & ids = uUniqueKeys(wordsA);
 	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
@@ -444,14 +449,17 @@ int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
 	int realPairsCount = 0;
 	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
 	{
-		iterA = wordsA.find(*i);
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
-		iterB = wordsB.find(*i);
-		while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
 		{
-			pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
+			iterA = wordsA.find(*i);
-			++iterA;
+			iterB = wordsB.find(*i);
-			++iterB;
+			while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
-			++realPairsCount;
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
+				++iterA;
+				++iterB;
+				++realPairsCount;
+			}
 		}
 	}
 	return realPairsCount;
@@ -464,24 +472,28 @@ int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
 int EpipolarGeometry::findPairsUnique(
 		const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	const std::list<int> & ids = uUniqueKeys(wordsA);
 	int realPairsCount = 0;
 	pairs.clear();
 	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
 	{
-		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
-		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
-		if(ptsA.size() == 1 && ptsB.size() == 1)
 		{
-			pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
+			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
-			++realPairsCount;
+			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
-		}
+			if(ptsA.size() == 1 && ptsB.size() == 1)
-		else if(ptsA.size()>1 && ptsB.size()>1)
+			{
-		{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
-			// just update the count
+				++realPairsCount;
-			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+			}
+			else if(ptsA.size()>1 && ptsB.size()>1)
+			{
+				// just update the count
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+			}
 		}
 	}
 	return realPairsCount;
@@ -493,7 +505,8 @@ int EpipolarGeometry::findPairsUnique(
  */
 int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	UTimer timer;
 	timer.start();
@@ -502,16 +515,19 @@ int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & word
 	int realPairsCount = 0;;
 	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 	{
-		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
-		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
-
-		realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-
-		for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
 		{
-			for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
+			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
+
+			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+			for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
 			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
+				for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+				{
+					pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
+				}
 			}
 		}
 	}

corelib/src/Features2d.cpp

@@ -272,6 +272,41 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 	}
 }
 
+void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints)
+{
+	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
+	{
+		UTimer timer;
+		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+		// Remove words under the new hessian threshold
+
+		// Sort words by hessian
+		std::multimap<float, int> hessianMap; // <hessian,id>
+		for(unsigned int i = 0; i <keypoints.size(); ++i)
+		{
+			//Keep track of the data, to be easier to manage the data in the next step
+			hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
+		}
+
+		// Keep keypoints with highest response
+		int removed = (int)hessianMap.size()-maxKeypoints;
+		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
+		inliers.resize(keypoints.size(), false);
+		float minimumHessian = 0.0f;
+		for(int k=0; k < maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
+		{
+			inliers[iter->second] = true;
+			minimumHessian = iter->first;
+		}
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
+		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
+	}
+	else
+	{
+		inliers.resize(keypoints.size(), true);
+	}
+}
+
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
 {
 	return util2d::computeRoi(image, roiRatios);

corelib/src/Memory.cpp

@@ -93,6 +93,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
 	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
+	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -246,7 +247,13 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			{
 				const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
 				std::list<int> keys = uUniqueKeys(words);
-				wordIds.insert(keys.begin(), keys.end());
+				for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end();)
+				{
+					if(*iter > 0)
+					{
+						wordIds.insert(*iter);
+					}
+				}
 			}
 			if(wordIds.size())
 			{
@@ -285,7 +292,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 				UDEBUG("node=%d, word references=%d", s->id(), words.size());
 				for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
 				{
-					_vwd->addWordRef(iter->first, i->first);
+					if(iter->first > 0)
+					{
+						_vwd->addWordRef(iter->first, i->first);
+					}
 				}
 				s->setEnabled(true);
 			}
@@ -427,6 +437,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
 	Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
 	Parameters::parse(parameters, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
+	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _visMaxFeatures);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -1464,10 +1475,12 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
 			// Pour chaque mot dans la signature SURF
 			for(std::list<int>::const_iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
 			{
-				// "Inverted index" - Pour chaque endroit contenu dans chaque mot
+				if(*i>0)
-				vw = _vwd->getWord(*i);
-				if(vw)
 				{
+					// "Inverted index" - Pour chaque endroit contenu dans chaque mot
+					vw = _vwd->getWord(*i);
+					UASSERT(vw!=0);
+
 					const std::map<int, int> & refs = vw->getReferences();
 					nw = refs.size();
 					if(nw)
@@ -2229,13 +2242,12 @@ Transform Memory::computeTransform(
 			// verify if it is a 180 degree transform, well verify > 90
 			float x,y,z, roll,pitch,yaw;
 			transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-			if(fabs(roll) > CV_PI/2 ||
+			if(fabs(pitch) > CV_PI/2 ||
-			   fabs(pitch) > CV_PI/2 ||
 			   fabs(yaw) > CV_PI/2)
 			{
 				transform.setNull();
-				std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
+				std::string msg = uFormat("Too large rotation detected! (pitch=%f, yaw=%f) max is %f",
-						roll, pitch, yaw);
+						roll, pitch, yaw, CV_PI/2);
 				UINFO(msg.c_str());
 				if(info)
 				{
@@ -3302,9 +3314,26 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				}
 			}
 
+			int oldMaxFeatures = _feature2D->getMaxFeatures();
+			UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
+			ParametersMap tmpMaxFeatureParameter;
+			if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
+			{
+				// The total extracted features should match the number of features used for transformation estimation
+				UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
+				tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+				_feature2D->parseParameters(tmpMaxFeatureParameter);
+			}
+
 			keypoints = _feature2D->generateKeypoints(
 					imageMono,
 					depthMask);
+
+			if(tmpMaxFeatureParameter.size())
+			{
+				tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+				_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
+			}
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
@@ -3351,9 +3380,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
 		UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
 
-		if((int)keypoints.size() > _feature2D->getMaxFeatures())
+		int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
+		if((int)keypoints.size() > maxFeatures)
 		{
-			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, _feature2D->getMaxFeatures());
+			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
 		}
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
@@ -3443,10 +3473,60 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
 		}
 
-		wordIds = _vwd->addNewWords(descriptors, id);
+		// In case the number of features we want to do quantization is lower
+		// than extracted ones (that would be used for transform estimation)
+		std::vector<bool> inliers;
+		cv::Mat descriptorsForQuantization = descriptors;
+		std::vector<int> quantizedToRawIndices;
+		if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
+		{
+			UASSERT((int)keypoints.size() == descriptors.rows);
+			Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
+
+			descriptorsForQuantization = cv::Mat(_feature2D->getMaxFeatures(), descriptors.cols, descriptors.type());
+			quantizedToRawIndices.resize(_feature2D->getMaxFeatures());
+			unsigned int oi=0;
+			UASSERT((int)inliers.size() == descriptors.rows);
+			for(int k=0; k < descriptors.rows; ++k)
+			{
+				if(inliers[k])
+				{
+					UASSERT(oi < quantizedToRawIndices.size());
+					if(descriptors.type() == CV_32FC1)
+					{
+						memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
+					}
+					else
+					{
+						memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
+					}
+					quantizedToRawIndices[oi] = k;
+					++oi;
+				}
+			}
+			UASSERT((int)oi == _feature2D->getMaxFeatures());
+		}
+
+		// Quantization to vocabulary
+		wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
+
+		// Set ID -1 to features not used for quantization
+		if(wordIds.size() < keypoints.size())
+		{
+			std::vector<int> allWordIds;
+			allWordIds.resize(keypoints.size(),-1);
+			int i=0;
+			for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
+			{
+				allWordIds[quantizedToRawIndices[i]] = *iter;
+				++i;
+			}
+			wordIds = uVectorToList(allWordIds);
+		}
+
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
-		UDEBUG("time addNewWords %fs", t);
+		UDEBUG("time addNewWords %fs indexed=%d not=%d", t, _vwd->getIndexedWordsCount(), _vwd->getNotIndexedWordsCount());
 	}
 	else if(id>0)
 	{
@@ -3805,7 +3885,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 			//Find words in the signature which they are not in the current dictionary
 			for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
 			{
-				if(_vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
+				if(*k>0 && _vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
 				{
 					oldWordIds.insert(oldWordIds.end(), *k);
 				}
@@ -3875,13 +3955,13 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 		const std::vector<int> & keys = uKeys((*j)->getWords());
 		if(keys.size())
 		{
-			const VisualWord * wordFirst = _vwd->getWord(keys.front()); //get descriptor size
-			UASSERT(wordFirst!=0);
-
 			// Add all references
 			for(unsigned int i=0; i<keys.size(); ++i)
 			{
-				_vwd->addWordRef(keys.at(i), (*j)->id());
+				if(keys.at(i)>0)
+				{
+					_vwd->addWordRef(keys.at(i), (*j)->id());
+				}
 			}
 			(*j)->setEnabled(true);
 		}

corelib/src/RegistrationVis.cpp

@@ -301,10 +301,21 @@ Transform RegistrationVis::computeTransformationImpl(
 			kptsFrom.resize(fromSignature.getWords().size());
 			orignalWordsFromIds.resize(fromSignature.getWords().size());
 			int i=0;
+			bool allUniques = true;
 			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
 			{
 				kptsFrom[i] = iter->second;
-				orignalWordsFromIds[i++] = iter->first;
+				orignalWordsFromIds[i] = iter->first;
+				if(i>0 && iter->first==orignalWordsFromIds[i-1])
+				{
+					allUniques = false;
+				}
+				++i;
+			}
+			if(!allUniques)
+			{
+				UDEBUG("IDs are not unique, IDs will be regenerated!");
+				orignalWordsFromIds.clear();
 			}
 		}
 

corelib/src/Rtabmap.cpp

@@ -1782,6 +1782,7 @@ bool Rtabmap::process(
 	//=============================================================
 	std::list<std::pair<int, int> > loopClosureLinksAdded;
 	int loopClosureVisualInliers = 0; // for statistics
+	int loopClosureVisualMatches = 0;
 	if(_loopClosureHypothesis.first>0)
 	{
 		//Compute transform if metric data are present
@@ -1792,6 +1793,7 @@ bool Rtabmap::process(
 		{
 			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
 			loopClosureVisualInliers = info.inliers;
+			loopClosureVisualMatches = info.matches;
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
@@ -1920,6 +1922,10 @@ bool Rtabmap::process(
 									{
 										loopClosureVisualInliers = info.inliers;
 									}
+									if(loopClosureVisualMatches == 0)
+									{
+										loopClosureVisualMatches = info.matches;
+									}
 
 									if(_loopClosureHypothesis.first == 0)
 									{
@@ -2314,6 +2320,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
+			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
 			statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);

corelib/src/RtabmapThread.cpp

@@ -573,7 +573,16 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 						odomEvent.rotVariance()>=9999 ||
 						odomEvent.transVariance()>=9999))
 		{
-			UWARN("Odometry is reset (identity pose or high variance >=9999 detected). Increment map id!");
+			if(odomEvent.pose().isIdentity())
+			{
+				UWARN("Odometry is reset (identity pose detected). Increment map id!");
+			}
+			else
+			{
+				UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!",
+						odomEvent.info().varianceLin>odomEvent.transVariance()?odomEvent.info().varianceLin:odomEvent.transVariance(),
+						odomEvent.info().varianceAng>odomEvent.rotVariance()?odomEvent.info().varianceAng:odomEvent.rotVariance());
+			}
 			pushNewState(kStateTriggeringMap);
 			_rotVariance = 0;
 			_transVariance = 0;

corelib/src/Signature.cpp

@@ -44,7 +44,8 @@ Signature::Signature() :
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_enabled(false)
+	_enabled(false),
+	_invalidWordsCount(0)
 {
 }
 
@@ -66,6 +67,7 @@ Signature::Signature(
 	_modified(true),
 	_linksModified(true),
 	_enabled(false),
+	_invalidWordsCount(0),
 	_pose(pose),
 	_groundTruthPose(groundTruthPose),
 	_sensorData(sensorData)
@@ -87,6 +89,7 @@ Signature::Signature(const SensorData & data) :
 	_modified(true),
 	_linksModified(true),
 	_enabled(false),
+	_invalidWordsCount(0),
 	_pose(Transform::getIdentity()),
 	_groundTruthPose(data.groundTruth()),
 	_sensorData(data)
@@ -178,10 +181,12 @@ float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
 	const std::multimap<int, cv::KeyPoint> & words = s.getWords();
-	if(words.size() != 0 && _words.size() != 0)
+
+	if(!s.isBadSignature() && !this->isBadSignature())
 	{
 		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
-		unsigned int totalWords = _words.size()>words.size()?_words.size():words.size();
+		int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
+		UASSERT(totalWords > 0);
 		EpipolarGeometry::findPairs(words, _words, pairs);
 
 		similarity = float(pairs.size()) / float(totalWords);
@@ -196,7 +201,15 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 	{
 		std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
 		std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
-		_words.erase(oldWordId);
+		if(oldWordId<=0)
+		{
+			_invalidWordsCount-=(int)_words.erase(oldWordId);
+			UASSERT(_invalidWordsCount>=0);
+		}
+		else
+		{
+			_words.erase(oldWordId);
+		}
 		_words3.erase(oldWordId);
 		_wordsDescriptors.erase(oldWordId);
 		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
@@ -215,9 +228,24 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 	}
 }
 
+void Signature::setWords(const std::multimap<int, cv::KeyPoint> & words)
+{
+	_enabled = false;
+	_words = words;
+	_invalidWordsCount = 0;
+	for(std::multimap<int, cv::KeyPoint>::iterator iter=_words.begin(); iter!=_words.end(); ++iter)
+	{
+		if(iter->first>0)
+		{
+			break;
+		}
+		++_invalidWordsCount;
+	}
+}
+
 bool Signature::isBadSignature() const
 {
-	return !_words.size();
+	return _words.size()-_invalidWordsCount <= 0;
 }
 
 void Signature::removeAllWords()
@@ -225,11 +253,20 @@ void Signature::removeAllWords()
 	_words.clear();
 	_words3.clear();
 	_wordsDescriptors.clear();
+	_invalidWordsCount = 0;
 }
 
 void Signature::removeWord(int wordId)
 {
-	_words.erase(wordId);
+	if(wordId<=0)
+	{
+		_invalidWordsCount-=(int)_words.erase(wordId);
+		UASSERT(_invalidWordsCount>=0);
+	}
+	else
+	{
+		_words.erase(wordId);
+	}
 	_words3.erase(wordId);
 	_wordsDescriptors.clear();
 }

corelib/src/VWDictionary.cpp

@@ -551,7 +551,7 @@ int VWDictionary::getNextId()
 
 void VWDictionary::addWordRef(int wordId, int signatureId)
 {
-	if(signatureId > 0 && wordId > 0)
+	if(signatureId > 0)
 	{
 		VisualWord * vw = 0;
 		vw = uValue(_visualWords, wordId, vw);

corelib/src/resources/DatabaseSchema.sql.in

@@ -81,8 +81,7 @@ CREATE TABLE Map_Node_Word (
 	depth_z FLOAT,
 	descriptor_size INTEGER,
 	descriptor BLOB,
-	FOREIGN KEY (node_id) REFERENCES Node(id),
+	FOREIGN KEY (node_id) REFERENCES Node(id)
-	FOREIGN KEY (word_id) REFERENCES Word(id)
 );
 
 CREATE TABLE Info (

guilib/src/MainWindow.cpp

@@ -2174,7 +2174,7 @@ void MainWindow::updateMapCloud(
 			// get local transforms for frustums on the graph
 			if(_preferencesDialog->isFrustumsShown())
 			{
-				std::string frustumId = uFormat("f_%d_%d", mapId, iter->first);
+				std::string frustumId = uFormat("f_%d", iter->first);
 				if(_cloudViewer->getAddedFrustums().contains(frustumId))
 				{
 					_cloudViewer->updateFrustumPose(frustumId, iter->second);
@@ -2190,6 +2190,13 @@ void MainWindow::updateMapCloud(
 						{
 							QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
 							_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
+
+							if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
+							{
+								std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
+								color = Qt::gray;
+								_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color);
+							}
 						}
 					}
 				}
@@ -2212,34 +2219,6 @@ void MainWindow::updateMapCloud(
 				pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
 				kter->second->push_back(pt);
 			}
-
-			// get local transforms for frustums on the graph
-			if(_preferencesDialog->isFrustumsShown() && _cachedSignatures.contains(iter->first))
-			{
-				std::string frustumId = uFormat("f_gt_%d", iter->first);
-				QMap<std::string, Transform>::const_iterator jter=_cloudViewer->getAddedFrustums().find(frustumId);
-				if(jter != _cloudViewer->getAddedFrustums().end())
-				{
-					if(jter.value() != iter->second)
-					{
-						_cloudViewer->updateFrustumPose(frustumId, iter->second);
-					}
-				}
-				else if(_cachedSignatures.contains(iter->first))
-				{
-					const Signature & s = _cachedSignatures.value(iter->first);
-					// Supporting only one frustum per node
-					if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
-					{
-						Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
-						if(!t.isNull())
-						{
-							QColor color = Qt::gray;
-							_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
-						}
-					}
-				}
-			}
 		}
 
 		// add graphs
@@ -2253,6 +2232,25 @@ void MainWindow::updateMapCloud(
 			_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
 
+		if(_preferencesDialog->isFrustumsShown())
+		{
+			QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
+			UDEBUG("remove not used frustums");
+			for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
+			{
+				std::list<std::string> splitted = uSplitNumChar(iter.key());
+				if(splitted.size() == 2)
+				{
+					int id = std::atoi(splitted.back().c_str());
+					if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0) &&
+						_currentPosesMap.find(id) == _currentPosesMap.end())
+					{
+						_cloudViewer->removeFrustum(iter.key());
+					}
+				}
+			}
+		}
+
 		UDEBUG("timerGraph=%fs", timerGraph.ticks());
 	}