Tango: added comptible fisheye cloud density level

app/android/jni/CameraTango.cpp

@@ -736,7 +736,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 				bool pixelSet = false;
 				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
-				   pixel_y_l>0 && pixel_y_l<depth.rows &&
+				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
 				   depth_value)
 				{
 					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
@@ -747,7 +747,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 					}
 				}
 				if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
-				   pixel_y_h>0 && pixel_y_h<depth.rows &&
+				   pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
 				   depth_value)
 				{
 					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);

app/android/jni/RTABMapApp.cpp

@@ -157,7 +157,7 @@ RTABMapApp::RTABMapApp() :
 		fullResolution_(false),
 		appendMode_(true),
 		maxCloudDepth_(0.0),
-		meshDecimation_(1),
+		cloudDensityLevel_(1),
 		meshTrianglePix_(1),
 		meshAngleToleranceDeg_(15.0),
 		clusterRatio_(0.1),
@@ -173,6 +173,7 @@ RTABMapApp::RTABMapApp() :
 		gainCompensationOnNextRender_(0),
 		bilateralFilteringOnNextRender_(false),
 		cameraJustInitialized_(false),
+		meshDecimation_(1),
 		totalPoints_(0),
 		totalPolygons_(0),
 		lastDrawnCloudsCount_(0),
@@ -461,6 +462,72 @@ bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
 		camera_->setColorCamera(cameraColor_);
 		if(camera_->init())
 		{
+			//update mesh decimation based on camera calibration
+			LOGI("Cloud density level %d", cloudDensityLevel_);
+			meshDecimation_ = 1;
+			if(camera_)
+			{
+				// Google Tango Tablet 160x90
+				// Phab2Pro 240x135
+				// FishEye 640x480
+				int width = camera_->getCameraModel().imageWidth()/(cameraColor_?8:1);
+				int height = camera_->getCameraModel().imageHeight()/(cameraColor_?8:1);
+				if(cloudDensityLevel_ == 3) // high
+				{
+					if(height >= 480 && width % 20 == 0 && height % 20 == 0)
+					{
+						meshDecimation_ = 20;
+					}
+					else if(width % 10 == 0 && height % 10 == 0)
+					{
+						meshDecimation_ = 10;
+					}
+					else if(width % 15 == 0 && height % 15 == 0)
+					{
+						meshDecimation_ = 15;
+					}
+					else
+					{
+						UERROR("Could not set decimation to high (size=%dx%d)", width, height);
+					}
+				}
+				else if(cloudDensityLevel_ == 2) // medium
+				{
+					if(height >= 480 && width % 10 == 0 && height % 10 == 0)
+					{
+						meshDecimation_ = 10;
+					}
+					else if(width % 5 == 0 && height % 5 == 0)
+					{
+						meshDecimation_ = 5;
+					}
+					else
+					{
+						UERROR("Could not set decimation to medium (size=%dx%d)", width, height);
+					}
+				}
+				else if(cloudDensityLevel_ == 1) // low
+				{
+					if(height >= 480 && width % 5 == 0 && height % 5 == 0)
+					{
+						meshDecimation_ = 5;
+					}
+					else if(width % 3 == 0 && width % 3 == 0)
+					{
+						meshDecimation_ = 3;
+					}
+					else if(width % 2 == 0 && width % 2 == 0)
+					{
+						meshDecimation_ = 2;
+					}
+					else
+					{
+						UERROR("Could not set decimation to low (size=%dx%d)", width, height);
+					}
+				}
+			}
+			LOGI("Set decimation to %d", meshDecimation_);
+
 			LOGI("Start camera thread");
 			if(!paused_)
 			{
@@ -1616,59 +1683,9 @@ void RTABMapApp::setMaxCloudDepth(float value)
 	maxCloudDepth_ = value;
 }
 
-void RTABMapApp::setMeshDecimation(int value)
+void RTABMapApp::setCloudDensityLevel(int value)
 {
-	LOGW("Set mesh decimation to level %d", value);
+	cloudDensityLevel_ = value;
-	meshDecimation_ = 1;
-	if(camera_)
-	{
-		// Google Tango Tablet 160x90
-		// Phab2Pro 240x135
-		int width = camera_->getCameraModel().imageWidth()/8;
-		int height = camera_->getCameraModel().imageHeight()/8;
-		if(value == 3) // high
-		{
-			if(width % 10 == 0 && height % 10 == 0)
-			{
-				meshDecimation_ = 10;
-			}
-			else if(width % 15 == 0 && height % 15 == 0)
-			{
-				meshDecimation_ = 15;
-			}
-			else
-			{
-				UERROR("Could not set decimation to high (size=%dx%d)", width, height);
-			}
-		}
-		else if(value == 2) // medium
-		{
-			if(width % 5 == 0 && height % 5 == 0)
-			{
-				meshDecimation_ = 5;
-			}
-			else
-			{
-				UERROR("Could not set decimation to medium (size=%dx%d)", width, height);
-			}
-		}
-		else if(value == 1) // low
-		{
-			if(width % 3 == 0 && width % 3 == 0)
-			{
-				meshDecimation_ = 3;
-			}
-			else if(width % 2 == 0 && width % 2 == 0)
-			{
-				meshDecimation_ = 2;
-			}
-			else
-			{
-				UERROR("Could not set decimation to low (size=%dx%d)", width, height);
-			}
-		}
-	}
-	UINFO("Set decimation to %d", meshDecimation_);
 }
 
 void RTABMapApp::setMeshAngleTolerance(float value)

app/android/jni/RTABMapApp.h

@@ -134,7 +134,7 @@ class RTABMapApp : public UEventsHandler {
   void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
-  void setMeshDecimation(int value);
+  void setCloudDensityLevel(int value);
   void setMeshAngleTolerance(float value);
   void setMeshTriangleSize(int value);
   void setClusterRatio(float value);
@@ -194,7 +194,7 @@ class RTABMapApp : public UEventsHandler {
   bool fullResolution_;
   bool appendMode_;
   float maxCloudDepth_;
-  int meshDecimation_;
+  int cloudDensityLevel_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
   float clusterRatio_;
@@ -213,6 +213,7 @@ class RTABMapApp : public UEventsHandler {
   int gainCompensationOnNextRender_;
   bool bilateralFilteringOnNextRender_;
   bool cameraJustInitialized_;
+  int meshDecimation_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;

app/android/jni/jni_interface.cpp

@@ -247,10 +247,10 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
 	return app.setMaxCloudDepth(value);
 }
 JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_setMeshDecimation(
+Java_com_introlab_rtabmap_RTABMapLib_setCloudDensityLevel(
 		JNIEnv*, jobject, int value)
 {
-	return app.setMeshDecimation(value);
+	return app.setCloudDensityLevel(value);
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(

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

@@ -478,7 +478,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			RTABMapLib.setMappingParameter("Optimizer/Strategy", optimizer);
 	
 			if(!DISABLE_LOG) Log.d(TAG, "set exporting parameters...");
-			RTABMapLib.setMeshDecimation(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_density), getString(R.string.pref_default_density))));
+			RTABMapLib.setCloudDensityLevel(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_density), getString(R.string.pref_default_density))));
 			RTABMapLib.setMaxCloudDepth(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth), getString(R.string.pref_default_depth))));
 			RTABMapLib.setPointSize(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_point_size), getString(R.string.pref_default_point_size))));
 			RTABMapLib.setMeshAngleTolerance(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_angle), getString(R.string.pref_default_angle))));

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

@@ -78,7 +78,7 @@ public class RTABMapLib
     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 setCloudDensityLevel(int value);
     public static native void setMeshAngleTolerance(float value);
     public static native void setMeshTriangleSize(int value);
     public static native void setClusterRatio(float value);