Tango: Localization mode can be used in visualization

app/android/jni/RTABMapApp.cpp

@@ -187,6 +187,8 @@ RTABMapApp::RTABMapApp() :
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
 		optMesh_(new pcl::TextureMesh),
+		optRefId_(0),
+		optRefPose_(0),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
@@ -194,28 +196,45 @@ RTABMapApp::RTABMapApp() :
 }
 
 RTABMapApp::~RTABMapApp() {
-  if(camera_)
+	if(camera_)
-  {
+	{
-	  delete camera_;
+		delete camera_;
-  }
+	}
-  if(rtabmapThread_)
+	if(rtabmapThread_)
-  {
+	{
-	  rtabmapThread_->close(false);
+		rtabmapThread_->close(false);
-	  delete rtabmapThread_;
+		delete rtabmapThread_;
-  }
+	}
-  if(logHandler_)
+	if(logHandler_)
-  {
+	{
-	  delete logHandler_;
+		delete logHandler_;
-  }
+	}
-  boost::mutex::scoped_lock  lock(rtabmapMutex_);
+	if(optRefPose_)
-  if(rtabmapEvents_.size())
+	{
-  {
+		delete optRefPose_;
-	  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
+	}
-	  {
+	{
-		  delete *iter;
+		boost::mutex::scoped_lock  lock(rtabmapMutex_);
-	  }
+		if(rtabmapEvents_.size())
-  }
+		{
-  rtabmapEvents_.clear();
+			for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
+			{
+				delete *iter;
+			}
+		}
+		rtabmapEvents_.clear();
+	}
+	{
+		boost::mutex::scoped_lock  lock(visLocalizationMutex_);
+		if(visLocalizationEvents_.size())
+		{
+			for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents_.begin(); iter!=visLocalizationEvents_.end(); ++iter)
+			{
+				delete *iter;
+			}
+		}
+		visLocalizationEvents_.clear();
+	}
 }
 
 void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
@@ -276,6 +295,13 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
 	rtabmapMutex_.lock();
+	if(rtabmapEvents_.size())
+	{
+		for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
+		{
+			delete *iter;
+		}
+	}
 	rtabmapEvents_.clear();
 	openingDatabase_ = true;
 	if(rtabmapThread_)
@@ -293,6 +319,12 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	// Open visualization while we load (if there is an optimized mesh saved in database)
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
+	optRefId_ = 0;
+	if(optRefPose_)
+	{
+		delete optRefPose_;
+		optRefPose_ = 0;
+	}
 	cv::Mat cloudMat;
 	std::vector<std::vector<std::vector<unsigned int> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -314,6 +346,12 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 				LOGI("Open: Found optimized mesh! Visualizing it.");
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
+				if(optPoses.size())
+				{
+					// just take the last as reference
+					optRefId_ = optPoses.rbegin()->first;
+					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
+				}
 				if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
@@ -987,14 +1025,12 @@ int RTABMapApp::Render()
 				poseEvents_.clear();
 			}
 		}
-		rtabmap::Transform mapOdom = rtabmap::Transform::getIdentity();
 		if(!pose.isNull())
 		{
 			// update camera pose?
-			if(graphOptimization_ && !visualizingMesh_ && !mapToOdom_.isIdentity())
+			if(graphOptimization_ && !mapToOdom_.isIdentity())
 			{
-				mapOdom = mapToOdom_;
+				main_scene_.SetCameraPose(opengl_world_T_rtabmap_world*mapToOdom_*rtabmap_world_T_tango_world*pose);
-				main_scene_.SetCameraPose(opengl_world_T_rtabmap_world*mapOdom*rtabmap_world_T_tango_world*pose);
 			}
 			else
 			{
@@ -1063,6 +1099,58 @@ int RTABMapApp::Render()
 					pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud);
 					main_scene_.addCloud(g_optMeshId, cloud, indices, opengl_world_T_rtabmap_world);
 				}
+
+				// clean up old messages if there are ones
+				boost::mutex::scoped_lock  lock(visLocalizationMutex_);
+				if(visLocalizationEvents_.size())
+				{
+					for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents_.begin(); iter!=visLocalizationEvents_.end(); ++iter)
+					{
+						delete *iter;
+					}
+				}
+				visLocalizationEvents_.clear();
+			}
+
+			std::list<rtabmap::RtabmapEvent*> visLocalizationEvents;
+			visLocalizationMutex_.lock();
+			visLocalizationEvents = visLocalizationEvents_;
+			visLocalizationEvents_.clear();
+			visLocalizationMutex_.unlock();
+
+			if(visLocalizationEvents.size())
+			{
+				const rtabmap::Statistics & stats = visLocalizationEvents.back()->getStats();
+				if(!stats.mapCorrection().isNull())
+				{
+					mapToOdom_ = stats.mapCorrection();
+				}
+
+				std::map<int, rtabmap::Transform>::const_iterator iter = stats.poses().find(optRefId_);
+				if(iter != stats.poses().end() && !iter->second.isNull() && optRefPose_)
+				{
+					// adjust opt mesh pose
+					main_scene_.setCloudPose(g_optMeshId, opengl_world_T_rtabmap_world * iter->second * (*optRefPose_).inverse());
+				}
+				int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
+				int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
+				int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
+				if(!paused_ && loopClosure>0)
+				{
+					main_scene_.setBackgroundColor(0, 0.5f, 0); // green
+				}
+				else if(!paused_ && rejected>0)
+				{
+					main_scene_.setBackgroundColor(0, 0.2f, 0); // dark green
+				}
+				else if(!paused_ && fastMovement)
+				{
+					main_scene_.setBackgroundColor(0.2f, 0, 0.2f); // dark magenta
+				}
+				else
+				{
+					main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
+				}
 			}
 
 			//backup state
@@ -1080,6 +1168,27 @@ int RTABMapApp::Render()
 
 			// revert state
 			main_scene_.setMeshRendering(isMeshRendering, isTextureRendering);
+
+			fpsTime.restart();
+			lastDrawnCloudsCount_ = main_scene_.Render();
+			if(renderingTime_ < fpsTime.elapsed())
+			{
+				renderingTime_ = fpsTime.elapsed();
+			}
+
+			if(visLocalizationEvents.size())
+			{
+				// send statistics to GUI
+				UEventsManager::post(new PostRenderEvent(visLocalizationEvents.back()));
+				visLocalizationEvents.pop_back();
+
+				for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents.begin(); iter!=visLocalizationEvents.end(); ++iter)
+				{
+					delete *iter;
+				}
+				visLocalizationEvents.clear();
+				lastPostRenderEventTime_ = UTimer::now();
+			}
 		}
 		else
 		{
@@ -1466,7 +1575,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*mapOdom*odomEvent.pose());
+								main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
 								main_scene_.setCloudVisible(-1, true);
 							}
 							else
@@ -1663,7 +1772,10 @@ void RTABMapApp::setPausedMapping(bool paused)
 {
 	{
 		boost::mutex::scoped_lock  lock(renderingMutex_);
-		visualizingMesh_ = false;
+		if(!localizationMode_)
+		{
+			visualizingMesh_ = false;
+		}
 		main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
 	}
 	paused_ = paused;
@@ -2778,11 +2890,17 @@ bool RTABMapApp::postExportation(bool visualize)
 	LOGI("postExportation(visualize=%d)", visualize?1:0);
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
+	optRefId_ = 0;
+	if(optRefPose_)
+	{
+		delete optRefPose_;
+		optRefPose_ = 0;
+	}
 	exportedMeshUpdated_ = false;
-	visualizingMesh_ = false;
 
 	if(visualize)
 	{
+		visualizingMesh_ = false;
 		cv::Mat cloudMat;
 		std::vector<std::vector<std::vector<unsigned int> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -2801,6 +2919,13 @@ bool RTABMapApp::postExportation(bool visualize)
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
 
+				if(optPoses.size())
+				{
+					// just take the last as reference
+					optRefId_ = optPoses.rbegin()->first;
+					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
+				}
+
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
 				exportedMeshUpdated_ = true;
@@ -2811,6 +2936,19 @@ bool RTABMapApp::postExportation(bool visualize)
 			}
 		}
 	}
+	else if(visualizingMesh_)
+	{
+		rtabmapMutex_.lock();
+		if(!rtabmap_->getLocalOptimizedPoses().empty())
+		{
+			rtabmap::Statistics stats;
+			stats.setPoses(rtabmap_->getLocalOptimizedPoses());
+			rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
+		}
+		rtabmapMutex_.unlock();
+
+		visualizingMesh_ = false;
+	}
 
 	return visualizingMesh_;
 }
@@ -3048,8 +3186,16 @@ bool RTABMapApp::handleEvent(UEvent * event)
 			LOGI("Received RtabmapEvent event!");
 			if(camera_->isRunning())
 			{
-				boost::mutex::scoped_lock lock(rtabmapMutex_);
+				if(visualizingMesh_)
-				rtabmapEvents_.push_back((rtabmap::RtabmapEvent*)event);
+				{
+					boost::mutex::scoped_lock lock(visLocalizationMutex_);
+					visLocalizationEvents_.push_back((rtabmap::RtabmapEvent*)event);
+				}
+				else
+				{
+					boost::mutex::scoped_lock lock(rtabmapMutex_);
+					rtabmapEvents_.push_back((rtabmap::RtabmapEvent*)event);
+				}
 				return true;
 			}
 		}

app/android/jni/RTABMapApp.h

@@ -234,18 +234,22 @@ class RTABMapApp : public UEventsHandler {
   bool exportedMeshUpdated_;
   pcl::TextureMesh::Ptr optMesh_;
   cv::Mat optTexture_;
+  int optRefId_;
+  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
   Scene main_scene_;
 
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> visLocalizationEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 
 	rtabmap::Transform mapToOdom_;
 
 	boost::mutex rtabmapMutex_;
+	boost::mutex visLocalizationMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;

app/android/res/layout/activity_rtabmap.xml

@@ -96,9 +96,9 @@
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
         android:layout_marginRight="5dp"
-        android:layout_alignParentTop="true"
         android:layout_marginTop="10dp"
         android:layout_alignParentRight="true"
+        android:layout_below="@+id/pause_button"
         android:text="@string/share_to_sketchfab" />
 
     <Button

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

@@ -1539,10 +1539,10 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		case STATE_VISUALIZING:
 			mButtonLighting.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
 			mButtonWireframe.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
-			mButtonCloseVisualization.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
+			mButtonCloseVisualization.setVisibility(mHudVisible && mButtonPause.isChecked()?View.VISIBLE:View.INVISIBLE);
 			mButtonCloseVisualization.setEnabled(true);
-			mButtonSaveOnDevice.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
+			mButtonSaveOnDevice.setVisibility(mHudVisible && mButtonPause.isChecked()?View.VISIBLE:View.INVISIBLE);
-			mButtonShareOnSketchfab.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
+			mButtonShareOnSketchfab.setVisibility(mHudVisible && mButtonPause.isChecked()?View.VISIBLE:View.INVISIBLE);
 			mItemSave.setEnabled(mButtonPause.isChecked());
 			mItemExport.setEnabled(mButtonPause.isChecked() && !mItemDataRecorderMode.isChecked());
 			mItemOpen.setEnabled(false);
@@ -1550,7 +1550,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			mItemSettings.setEnabled(true);
 			mItemReset.setEnabled(true);
 			mItemModes.setEnabled(true);
-			mButtonPause.setVisibility(View.INVISIBLE);
+			mButtonPause.setVisibility(mHudVisible && mItemLocalizationMode.isChecked()?View.VISIBLE:View.GONE);
+			mItemLocalizationMode.setEnabled(mButtonPause.isChecked());
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
 			break;
 		case STATE_VISUALIZING_WHILE_LOADING:
@@ -1594,7 +1595,14 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 
 	private void pauseMapping() {
 
-		updateState(State.STATE_IDLE);
+		if(mState != State.STATE_VISUALIZING)
+		{
+			updateState(State.STATE_IDLE);
+		}
+		else
+		{
+			updateState(State.STATE_VISUALIZING);
+		}
 
 		if(mButtonPause.isChecked())
 		{
@@ -1645,7 +1653,14 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			}
 			else if(!mMapIsEmpty)
 			{
-				mToast.makeText(getActivity(), String.format("On resume, a new map is created. Tip: Try relocalizing in the previous area."), mToast.LENGTH_LONG).show();
+				if(mItemLocalizationMode!=null && mItemLocalizationMode.isChecked())
+				{
+					mToast.makeText(getActivity(), String.format("Localization mode"), mToast.LENGTH_LONG).show();
+				}
+				else
+				{
+					mToast.makeText(getActivity(), String.format("On resume, a new map is created. Tip: Try relocalizing in the previous area."), mToast.LENGTH_LONG).show();
+				}
 			}
 			else if(mMapIsEmpty && mItemLocalizationMode!=null && mItemLocalizationMode.isChecked())
 			{
@@ -1817,6 +1832,10 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		{
 			item.setChecked(!item.isChecked());
 			RTABMapLib.setLocalizationMode(item.isChecked());
+			if(mState == State.STATE_VISUALIZING)
+			{
+				mButtonPause.setVisibility(mItemLocalizationMode.isChecked()?View.VISIBLE:View.GONE);
+			}
 		}
 		else if(itemId == R.id.trajectory_mode)
 		{

corelib/src/DBDriverSqlite3.cpp

@@ -3965,7 +3965,8 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
 					serializedIds[i] = iter->first;
-					memcpy(serializedPoses.data()+(12*sizeof(float)*i), iter->second.data(), 12*sizeof(float));
+					memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
+					++i;
 				}
 
 				compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
@@ -4175,6 +4176,16 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 				if(poses)
 				{
 					UASSERT(serializedIds.cols == serializedPoses.cols/12);
+					UASSERT(serializedPoses.type() == CV_32FC1);
+					UASSERT(serializedIds.type() == CV_32SC1);
+					for(int i=0; i<serializedIds.cols; ++i)
+					{
+						Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
+								serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
+								serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
+						poses->insert(std::make_pair(serializedIds.at<int>(i), t));
+						UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
+					}
 				}
 			}