Tango: Added full gain compensation option

app/android/jni/RTABMapApp.cpp

@@ -125,7 +125,7 @@ RTABMapApp::RTABMapApp() :
 		paused_(false),
 		clearSceneOnNextRender_(false),
 		filterPolygonsOnNextRender_(false),
-		gainCompensationOnNextRender_(false),
+		gainCompensationOnNextRender_(0),
 		cameraJustInitialized_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
@@ -600,10 +600,9 @@ int RTABMapApp::Render()
 		}
 	}
 
-	if(notifyDataLoaded || gainCompensationOnNextRender_)
+	if(notifyDataLoaded || gainCompensationOnNextRender_>0)
 	{
 		LOGI("Gain compensation...");
-		gainCompensationOnNextRender_ = false;
 		boost::mutex::scoped_lock  lock(meshesMutex_);
 		if(createdMeshes_.size() > 1)
 		{
@@ -616,6 +615,23 @@ int RTABMapApp::Render()
 			std::map<int, rtabmap::Transform> poses;
 			std::multimap<int, rtabmap::Link> links;
 			rtabmap_->getGraph(poses, links, false, true);
+			if(gainCompensationOnNextRender_ == 2)
+			{
+				// full compensation
+				links.clear();
+				for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
+				{
+					int from = iter->first;
+					std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator jter = iter;
+					++jter;
+					for(;jter!=clouds.end(); ++jter)
+					{
+						int to = jter->first;
+						links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to))));
+					}
+				}
+			}
+
 			compensator.feed(clouds, links);
 			for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 			{
@@ -633,6 +649,7 @@ int RTABMapApp::Render()
 				iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
 			}
 		}
+		gainCompensationOnNextRender_ = 0;
 		notifyDataLoaded = true;
 	}
 
@@ -1082,7 +1099,7 @@ int RTABMapApp::postProcessing(int approach)
 					LOGE("g2o not available!");
 				}
 			}
-			else if(approach!=4 || approach!=5)
+			else if(approach!=4 && approach!=5)
 			{
 				// simple graph optmimization
 				rtabmap_->getGraph(poses, links, true, true);
@@ -1099,7 +1116,7 @@ int RTABMapApp::postProcessing(int approach)
 
 			rtabmap_->setOptimizedPoses(poses);
 		}
-		else if(approach!=4 || approach!=5)
+		else if(approach!=4 && approach!=5)
 		{
 			returnedValue = -1;
 		}
@@ -1114,10 +1131,10 @@ int RTABMapApp::postProcessing(int approach)
 			}
 
 			// gain compensation
-			if(approach == -1 || approach == 5)
+			if(approach == -1 || approach == 5 || approach == 6)
 			{
 				boost::mutex::scoped_lock  lock(renderingMutex_);
-				gainCompensationOnNextRender_ = true;
+				gainCompensationOnNextRender_ = approach == 6 ? 2 : 1; // 2 = full, 1 = fast
 			}
 		}
 	}

app/android/jni/RTABMapApp.h

@@ -161,7 +161,7 @@ class RTABMapApp : public UEventsHandler {
   bool paused_;
   bool clearSceneOnNextRender_;
   bool filterPolygonsOnNextRender_;
-  bool gainCompensationOnNextRender_;
+  int gainCompensationOnNextRender_;
   bool cameraJustInitialized_;
   int totalPoints_;
   int totalPolygons_;

app/android/res/menu/optionmenu.xml

@@ -10,11 +10,12 @@
           <menu>
              <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
              <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
-	            <menu >
+	            <menu>
 	              <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
 	              <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
 	              <item android:id="@+id/icp_refining" android:title="ICP Refining" />
-	              <item android:id="@+id/gain_compensation" android:title="Adjust Colors" />
+	              <item android:id="@+id/gain_compensation_fast" android:title="Adjust Colors (Fast)" />
+	              <item android:id="@+id/gain_compensation_full" android:title="Adjust Colors (Full)" />
 	              <item android:id="@+id/sba" android:title="Bundle Adjustement" />
 	              <item android:id="@+id/polygons_filtering" android:title="Noise Filtering" />
 	            </menu>  

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

@@ -130,7 +130,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   protected void onCreate(Bundle savedInstanceState) {
     super.onCreate(savedInstanceState);
     setTitle(R.string.menu_name);
-
+    
     // Query screen size, the screen size is used for computing the normalized
     // touch point.
     Display display = getWindowManager().getDefaultDisplay();
@@ -791,13 +791,20 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  mProgressDialog.show();
     	  RTABMapLib.postProcessing(4);
       }
-      else if (itemId == R.id.gain_compensation)
+      else if (itemId == R.id.gain_compensation_fast)
       {		
     	  mProgressDialog.setTitle("Post-Processing");
-		  mProgressDialog.setMessage(String.format("Gain compensation..."));
+		  mProgressDialog.setMessage(String.format("Fast gain compensation..."));
 		  mProgressDialog.show();
     	  RTABMapLib.postProcessing(5);
       }
+      else if (itemId == R.id.gain_compensation_full)
+      {		
+    	  mProgressDialog.setTitle("Post-Processing");
+		  mProgressDialog.setMessage(String.format("Full gain compensation..."));
+		  mProgressDialog.show();
+    	  RTABMapLib.postProcessing(6);
+      }
       else if (itemId == R.id.sba)
       {
 			mProgressDialog.setTitle("Post-Processing");