Tango: added fisheye option

app/android/CMakeLists.txt

@@ -1,4 +1,15 @@
 
+option(DISABLE_LOG        "Disable Android logging (should be true in release)"    ON)
+IF(DISABLE_LOG)
+  ADD_DEFINITIONS(-DDISABLE_LOG)
+ENDIF(DISABLE_LOG)
+
+IF(DISABLE_LOG)
+   SET(ANDROID_DEBUGGABLE false)
+ELSE()
+   SET(ANDROID_DEBUGGABLE true)
+ENDIF()
+
 add_subdirectory(jni)
 
 
@@ -6,12 +17,6 @@ add_subdirectory(jni)
 # Packaging
 ######
 
-IF(DISABLE_LOG)
-   SET(ANDROID_DEBUGGABLE false)
-ELSE()
-   SET(ANDROID_DEBUGGABLE true)
-ENDIF()
-
 # find android
 find_host_program(ANDROID_EXECUTABLE
   NAMES android

app/android/jni/CameraTango.cpp

@@ -68,6 +68,10 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
 	{
 		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
 	}
+	else if(color->format == 35)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
 	else
 	{
 		LOGE("Not supported color format : %d.", color->format);
@@ -100,11 +104,12 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 const float CameraTango::bilateralFilteringSigmaS = 2.0f;
 const float CameraTango::bilateralFilteringSigmaR = 0.075f;
 
-CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing) :
+CameraTango::CameraTango(bool colorCamera, int decimation, bool autoExposure, bool publishRawScan, bool smoothing) :
 		Camera(0),
 		tango_config_(0),
 		firstFrame_(true),
 		stampEpochOffset_(0.0),
+		colorCamera_(colorCamera),
 		decimation_(decimation),
 		autoExposure_(autoExposure),
 		rawScanPublished_(publishRawScan),
@@ -122,6 +127,65 @@ CameraTango::~CameraTango() {
 	close();
 }
 
+// Compute fisheye distorted coordinates from undistorted coordinates.
+// The distortion model used by the Tango fisheye camera is called FOV and is
+// described in 'Straight lines have to be straight' by Frederic Devernay and
+// Olivier Faugeras. See https://hal.inria.fr/inria-00267247/document.
+// Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
+void applyFovModel(
+    double xu, double yu, double w, double w_inverse, double two_tan_w_div_two,
+    double* xd, double* yd) {
+  double ru = sqrt(xu * xu + yu * yu);
+  constexpr double epsilon = 1e-7;
+  if (w < epsilon || ru < epsilon) {
+    *xd = xu;
+    *yd = yu ;
+  } else {
+    double rd_div_ru = std::atan(ru * two_tan_w_div_two) * w_inverse / ru;
+    *xd = xu * rd_div_ru;
+    *yd = yu * rd_div_ru;
+  }
+}
+// Compute the warp maps to undistort the Tango fisheye image using the FOV
+// model. See OpenCV documentation for more information on warp maps:
+// http://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html
+// Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
+// @param fisheyeModel the fisheye camera intrinsics.
+// @param mapX the output map for the x direction.
+// @param mapY the output map for the y direction.
+void initFisheyeRectificationMap(
+    const CameraModel& fisheyeModel,
+    cv::Mat & mapX, cv::Mat & mapY) {
+  const double & fx = fisheyeModel.K().at<double>(0,0);
+  const double & fy = fisheyeModel.K().at<double>(1,1);
+  const double & cx = fisheyeModel.K().at<double>(0,2);
+  const double & cy = fisheyeModel.K().at<double>(1,2);
+  const double & w = fisheyeModel.D().at<double>(0,0);
+  mapX.create(fisheyeModel.imageSize(), CV_32FC1);
+  mapY.create(fisheyeModel.imageSize(), CV_32FC1);
+  LOGD("initFisheyeRectificationMap: fx=%f fy=%f, cx=%f, cy=%f, w=%f", fx, fy, cx, cy, w);
+  // Pre-computed variables for more efficiency.
+  const double fy_inverse = 1.0 / fy;
+  const double fx_inverse = 1.0 / fx;
+  const double w_inverse = 1 / w;
+  const double two_tan_w_div_two = 2.0 * std::tan(w * 0.5);
+  // Compute warp maps in x and y directions.
+  // OpenCV expects maps from dest to src, i.e. from undistorted to distorted
+  // pixel coordinates.
+  for(int iu = 0; iu < fisheyeModel.imageHeight(); ++iu) {
+    for (int ju = 0; ju < fisheyeModel.imageWidth(); ++ju) {
+      double xu = (ju - cx) * fx_inverse;
+      double yu = (iu - cy) * fy_inverse;
+      double xd, yd;
+      applyFovModel(xu, yu, w, w_inverse, two_tan_w_div_two, &xd, &yd);
+      double jd = cx + xd * fx;
+      double id = cy + yd * fy;
+      mapX.at<float>(iu, ju) = jd;
+      mapY.at<float>(iu, ju) = id;
+    }
+  }
+}
+
 bool CameraTango::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	close();
@@ -146,38 +210,40 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
-	// Enable color.
-	ret = TangoConfig_setBool(tango_config_, "config_enable_color_camera", true);
-	if (ret != TANGO_SUCCESS)
+	if(colorCamera_)
 	{
-		LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
-		return false;
-	}
-
-	// disable auto exposure (disabled, seems broken on latest Tango releases)
-	ret = TangoConfig_setBool(tango_config_, "config_color_mode_auto", autoExposure_);
-	if (ret != TANGO_SUCCESS)
-	{
-		LOGE("NativeRTABMap: config_color_mode_auto() failed with error code: %d", ret);
-		//return false;
-	}
-	else
-	{
-		if(!autoExposure_)
+		// Enable color.
+		ret = TangoConfig_setBool(tango_config_, "config_enable_color_camera", true);
+		if (ret != TANGO_SUCCESS)
 		{
-			ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
-			if (ret != TANGO_SUCCESS)
-			{
-				LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret);
-				return false;
-			}
+			LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
+			return false;
+		}
+		// disable auto exposure (disabled, seems broken on latest Tango releases)
+		ret = TangoConfig_setBool(tango_config_, "config_color_mode_auto", autoExposure_);
+		if (ret != TANGO_SUCCESS)
+		{
+			LOGE("NativeRTABMap: config_color_mode_auto() failed with error code: %d", ret);
+			//return false;
+		}
+		else
+		{
+			if(!autoExposure_)
+			{
+				ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
+				if (ret != TANGO_SUCCESS)
+				{
+					LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret);
+					return false;
+				}
+			}
+			bool verifyAutoExposureState;
+			int32_t verifyIso, verifyExp;
+			TangoConfig_getBool( tango_config_, "config_color_mode_auto", &verifyAutoExposureState );
+			TangoConfig_getInt32( tango_config_, "config_color_iso", &verifyIso );
+			TangoConfig_getInt32( tango_config_, "config_color_exp", &verifyExp );
+			LOGI( "NativeRTABMap: config_color autoExposure=%s %d %d", verifyAutoExposureState?"On" : "Off", verifyIso, verifyExp );
 		}
-		bool verifyAutoExposureState;
-		int32_t verifyIso, verifyExp;
-		TangoConfig_getBool( tango_config_, "config_color_mode_auto", &verifyAutoExposureState );
-		TangoConfig_getInt32( tango_config_, "config_color_iso", &verifyIso );
-		TangoConfig_getInt32( tango_config_, "config_color_exp", &verifyExp );
-		LOGI( "NativeRTABMap: config_color autoExposure=%s %d %d", verifyAutoExposureState?"On" : "Off", verifyIso, verifyExp );
 	}
 
 	// Enable depth.
@@ -242,7 +308,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
-	ret = TangoService_connectOnFrameAvailable(TANGO_CAMERA_COLOR, this, onFrameAvailableRouter);
+	ret = TangoService_connectOnFrameAvailable(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, this, onFrameAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: Failed to connect to color callback with error code: %d", ret);
@@ -291,7 +357,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	//
 	// Get color camera with respect to device transformation matrix.
 	frame_pair.base = TANGO_COORDINATE_FRAME_DEVICE;
-	frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_COLOR;
+	frame_pair.target = colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE;
 	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
 	if (ret != TANGO_SUCCESS)
 	{
@@ -309,22 +375,59 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 
 	// camera intrinsic
 	TangoCameraIntrinsics color_camera_intrinsics;
-	ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics);
+	ret = TangoService_getCameraIntrinsics(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, &color_camera_intrinsics);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: Failed to get the intrinsics for the color camera with error code: %d.", ret);
 		return false;
 	}
-	model_ = CameraModel(
-			color_camera_intrinsics.fx,
-			color_camera_intrinsics.fy,
-			color_camera_intrinsics.cx,
-			color_camera_intrinsics.cy,
-			this->getLocalTransform());
-	model_.setImageSize(cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height));
 
-	// device to camera optical rotation in rtabmap frame
-	model_.setLocalTransform(tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_);
+	cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+	K.at<double>(0,0) = color_camera_intrinsics.fx;
+	K.at<double>(1,1) = color_camera_intrinsics.fy;
+	K.at<double>(0,2) = color_camera_intrinsics.cx;
+	K.at<double>(1,2) = color_camera_intrinsics.cy;
+	cv::Mat D = cv::Mat::zeros(1, 5, CV_64FC1);
+	LOGD("Calibration type = %d", color_camera_intrinsics.calibration_type);
+	if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_5_PARAMETERS ||
+			color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_EQUIDISTANT)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = color_camera_intrinsics.distortion[2];
+		D.at<double>(0,3) = color_camera_intrinsics.distortion[3];
+		D.at<double>(0,4) = color_camera_intrinsics.distortion[4];
+	}
+	else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_3_PARAMETERS)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = 0.;
+		D.at<double>(0,3) = 0.;
+		D.at<double>(0,4) = color_camera_intrinsics.distortion[2];
+	}
+	else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_2_PARAMETERS)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = 0.;
+		D.at<double>(0,3) = 0.;
+		D.at<double>(0,4) = 0.;
+	}
+
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+	cv::Mat P;
+
+	LOGD("Distortion params: %f, %f, %f, %f, %f", D.at<double>(0,0), D.at<double>(0,1), D.at<double>(0,2), D.at<double>(0,3), D.at<double>(0,4));
+	model_ = CameraModel(colorCamera_?"color":"fisheye",
+			cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
+			K, D, R, P,
+			tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_); // device to camera optical rotation in rtabmap frame
+
+	if(!colorCamera_)
+	{
+		initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
+	}
 
 	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
 	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
@@ -343,6 +446,8 @@ void CameraTango::close()
 		TangoService_disconnect();
 	}
 	firstFrame_ = true;
+	fisheyeRectifyMapX_ = cv::Mat();
+	fisheyeRectifyMapY_ = cv::Mat();
 }
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -397,7 +502,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	}
 }
 
-static rtabmap::Transform opticalRotationTango(
+static rtabmap::Transform opticalRotation(
 								1.0f,  0.0f,  0.0f, 0.0f,
 							    0.0f, -1.0f,  0.0f, 0.0f,
 								0.0f,  0.0f, -1.0f, 0.0f);
@@ -406,7 +511,7 @@ void CameraTango::poseReceived(const Transform & pose)
 	if(!pose.isNull() && pose.getNormSquared() < 100000)
 	{
 		// send pose of the camera (without optical rotation), not the device
-		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotationTango));
+		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotation));
 	}
 }
 
@@ -521,6 +626,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			tangoColorType_ = 0;
 		}
 
+		LOGD("tangoColorType=%d", tangoColorType);
 		if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 		{
 			cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
@@ -533,6 +639,10 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
 		}
+		else if(tangoColorType == 35)
+		{
+			cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
+		}
 		else
 		{
 			LOGE("Not supported color format : %d.", tangoColorType);
@@ -545,10 +655,22 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		//}
 
 		CameraModel model = model_;
-		if(decimation_ > 1)
+
+		if(colorCamera_)
 		{
-			rgb = util2d::decimate(rgb, decimation_);
-			model = model.scaled(1.0/double(decimation_));
+			if(decimation_ > 1)
+			{
+				rgb = util2d::decimate(rgb, decimation_);
+				model = model.scaled(1.0/double(decimation_));
+			}
+		}
+		else
+		{
+			//UTimer t;
+			cv::Mat rgbRect;
+			cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
+			rgb = rgbRect;
+			//LOGD("Rectification time=%fs", t.ticks());
 		}
 
 		// Querying the depth image's frame transformation based on the depth image's
@@ -561,7 +683,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		Transform colorToDepth;
 		TangoPoseData pose_color_image_t1_T_depth_image_t0;
 		if (TangoSupport_calculateRelativePose(
-				rgbStamp, TANGO_COORDINATE_FRAME_CAMERA_COLOR, cloudStamp,
+				rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
 			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
 			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
 		{
@@ -589,8 +711,9 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
 
 			int pixelsSet = 0;
-			depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
-			CameraModel depthModel = model_.scaled(1.0f/8.0f);
+			int depthSizeDec = colorCamera_?8:1;
+			depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
+			CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
 			std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
 			int oi=0;
 			for(unsigned int i=0; i<cloud.total(); ++i)

app/android/jni/CameraTango.h

@@ -75,7 +75,7 @@ public:
 	static const float bilateralFilteringSigmaR;
 
 public:
-	CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing);
+	CameraTango(bool colorCamera, int decimation, bool autoExposure, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -84,6 +84,7 @@ public:
 	virtual std::string getSerial() const;
 	const CameraModel & getCameraModel() const {return model_;}
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
+	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
@@ -109,6 +110,7 @@ private:
 	bool firstFrame_;
 	UTimer cameraStartedTime_;
 	double stampEpochOffset_;
+	bool colorCamera_;
 	int decimation_;
 	bool autoExposure_;
 	bool rawScanPublished_;
@@ -123,6 +125,8 @@ private:
 	CameraModel model_;
 	Transform deviceTColorCamera_;
 	TangoSupportRotation colorCameraToDisplayRotation_;
+	cv::Mat fisheyeRectifyMapX_;
+	cv::Mat fisheyeRectifyMapY_;
 };
 
 } /* namespace rtabmap */

app/android/jni/RTABMapApp.cpp

@@ -76,7 +76,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(cameraColor_&&fullResolution_?"2":"1")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
@@ -153,6 +153,7 @@ RTABMapApp::RTABMapApp() :
 		autoExposure_(true),
 		rawScanSaved_(false),
 		smoothing_(true),
+		cameraColor_(true),
 		fullResolution_(false),
 		appendMode_(true),
 		maxCloudDepth_(0.0),
@@ -257,13 +258,12 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 
 	this->registerToEventsManager();
 
-	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_, rawScanSaved_, smoothing_);
+	camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, autoExposure_, rawScanSaved_, smoothing_);
 }
 
 void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 {
-	TangoSupportRotation rotation = tango_gl::util::GetAndroidRotationFromColorCameraToDisplay(
-			displayRotation, cameraRotation);
+	TangoSupportRotation rotation = tango_gl::util::GetAndroidRotationFromColorCameraToDisplay(displayRotation, cameraRotation);
 	LOGI("Set orientation: display=%d camera=%d -> %d", displayRotation, cameraRotation, (int)rotation);
 	main_scene_.setScreenRotation(rotation);
 	camera_->setScreenRotation(rotation);
@@ -458,6 +458,7 @@ bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
 		    return false;
 		}
 
+		camera_->setColorCamera(cameraColor_);
 		if(camera_->init())
 		{
 			LOGI("Start camera thread");
@@ -524,7 +525,6 @@ std::vector<pcl::Vertices> RTABMapApp::filterOrganizedPolygons(
 		}
 	}
 
-	int biggestCluster = 0;
 	unsigned int biggestClusterSize = 0;
 	for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
 	{
@@ -533,12 +533,11 @@ std::vector<pcl::Vertices> RTABMapApp::filterOrganizedPolygons(
 		if(iter->second.size() > biggestClusterSize)
 		{
 			biggestClusterSize = iter->second.size();
-			biggestCluster = iter->first;
 		}
 	}
 	unsigned int minClusterSize = (unsigned int)(float(biggestClusterSize)*clusterRatio_);
-	LOGI("Biggest cluster is %d = %d -> minClusterSize(ratio=%f)=%d",
-			biggestCluster, biggestClusterSize, clusterRatio_, (int)minClusterSize);
+	LOGI("Biggest cluster %d -> minClusterSize(ratio=%f)=%d",
+			biggestClusterSize, clusterRatio_, (int)minClusterSize);
 
 	std::vector<pcl::Vertices> filteredPolygons(polygons.size());
 	int oi = 0;
@@ -1558,6 +1557,14 @@ void RTABMapApp::setRawScanSaved(bool enabled)
 	}
 }
 
+void RTABMapApp::setCameraColor(bool enabled)
+{
+	if(cameraColor_ != enabled)
+	{
+		cameraColor_ = enabled;
+	}
+}
+
 void RTABMapApp::setFullResolution(bool enabled)
 {
 	if(fullResolution_ != enabled)
@@ -1858,6 +1865,12 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
 							{
 								cv::multiply(resizedImage, createdMeshes_.at(textures[i]).gain, resizedImage);
 							}
+							if(resizedImage.type() == CV_8UC1)
+							{
+								cv::Mat resizedImageColor;
+								cv::cvtColor(resizedImage,resizedImageColor,CV_GRAY2RGB);
+								resizedImage = resizedImageColor;
+							}
 							UASSERT(resizedImage.type() == globalTexture.type());
 							resizedImage.copyTo(globalTexture(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
 						}
@@ -1979,7 +1992,9 @@ bool RTABMapApp::exportMesh(
 
 					UTimer timer;
 					LOGI("Assemble clouds (%d)...", (int)poses.size());
+#ifndef DISABLE_LOG
 					int cloudCount=0;
+#endif
 					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 					for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin();
 						iter!= poses.end();

app/android/jni/RTABMapApp.h

@@ -128,6 +128,7 @@ class RTABMapApp : public UEventsHandler {
   void setGridVisible(bool visible);
   void setAutoExposure(bool enabled);
   void setRawScanSaved(bool enabled);
+  void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
   void setSmoothing(bool enabled);
   void setAppendMode(bool enabled);
@@ -189,6 +190,7 @@ class RTABMapApp : public UEventsHandler {
   bool autoExposure_;
   bool rawScanSaved_;
   bool smoothing_;
+  bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;
   float maxCloudDepth_;

app/android/jni/jni_interface.cpp

@@ -223,6 +223,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
 	return app.setSmoothing(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setCameraColor(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setCameraColor(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
 		JNIEnv*, jobject, bool enabled)
 {

app/android/res/layout/activity_settings.xml

@@ -80,6 +80,11 @@
 		        android:title="@string/pref_title_smoothing"
 		        android:summary="@string/pref_summary_smoothing"
 		        android:defaultValue="@string/pref_default_smoothing"/>
+		    <SwitchPreference
+		        android:key="@string/pref_key_fisheye"
+		        android:title="@string/pref_title_fisheye"
+		        android:summary="@string/pref_summary_fisheye"
+		        android:defaultValue="@string/pref_default_fisheye"/>
 	  
 		     <PreferenceCategory
 	        	android:title="@string/pref_title_mapping_core">

app/android/res/values/strings.xml

@@ -64,6 +64,8 @@
     <string name="pref_default_resolution">false</string>
     <string name="pref_key_smoothing">pref_key_smoothing</string>
     <string name="pref_default_smoothing">true</string>
+    <string name="pref_key_fisheye">pref_key_fisheye</string>
+    <string name="pref_default_fisheye">false</string>
     
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
@@ -240,9 +242,11 @@
     <string name="pref_title_auto_exposure">Auto Exposure</string>
     <string name="pref_summary_auto_exposure">Adjust camera exposure depending on the lighting to get always maximum contrast. This may change texture color between scanned images. Color correction option in Post-Processing can help to uniformize colors. May not work on some devices.</string>
     <string name="pref_title_resolution">HD Mode</string>
-    <string name="pref_summary_resolution">Save HD images if you want very detailed textures. More memory will be required.</string>
+    <string name="pref_summary_resolution">Save HD images of the color camera if you want very detailed textures. More memory will be required.</string>
     <string name="pref_title_smoothing">Smoothing</string>
     <string name="pref_summary_smoothing">Smooth the point clouds.</string>
+    <string name="pref_title_fisheye">Fish Eye Camera</string>
+    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. Cannot be used on Yellowstone tablet.</string>
     <string name="pref_title_update_rate">Update Rate</string>
     <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
     <string name="pref_title_time_thr">Time Limit</string>

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

@@ -460,6 +460,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			RTABMapLib.setRawScanSaved(sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
 			RTABMapLib.setFullResolution(sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution))));
 			RTABMapLib.setSmoothing(sharedPref.getBoolean(getString(R.string.pref_key_smoothing), Boolean.parseBoolean(getString(R.string.pref_default_smoothing))));
+			RTABMapLib.setCameraColor(!sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
 			RTABMapLib.setAppendMode(sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));
 			RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", mUpdateRate);
 			RTABMapLib.setMappingParameter("Rtabmap/TimeThr", mTimeThr);
@@ -585,7 +586,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private void setAndroidOrientation() {
         Display display = getWindowManager().getDefaultDisplay();
         Camera.CameraInfo colorCameraInfo = new Camera.CameraInfo();
-        Camera.getCameraInfo(0, colorCameraInfo);
+        SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+        boolean fisheye = sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye)));
+        Camera.getCameraInfo(fisheye?1:0, colorCameraInfo);
         RTABMapLib.setScreenRotation(display.getRotation(), colorCameraInfo.orientation);
     }
 

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

@@ -71,6 +71,7 @@ public class RTABMapLib
     public static native void setRawScanSaved(boolean enabled);
     public static native void setFullResolution(boolean enabled);
     public static native void setSmoothing(boolean enabled);
+    public static native void setCameraColor(boolean enabled);
     public static native void setAppendMode(boolean enabled);
     public static native void setDataRecorderMode(boolean enabled);
     public static native void setMaxCloudDepth(float value);