From ef7bf87187b0bfa92758867646504b7c12a7f4b2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 13 Mar 2017 20:51:59 -0400 Subject: [PATCH] Tango: added fisheye option --- CMakeLists.txt | 7 +- app/android/CMakeLists.txt | 17 +- app/android/jni/CameraTango.cpp | 219 ++++++++++++++---- app/android/jni/CameraTango.h | 6 +- app/android/jni/RTABMapApp.cpp | 31 ++- app/android/jni/RTABMapApp.h | 2 + app/android/jni/jni_interface.cpp | 6 + app/android/res/layout/activity_settings.xml | 5 + app/android/res/values/strings.xml | 6 +- .../com/introlab/rtabmap/RTABMapActivity.java | 5 +- .../src/com/introlab/rtabmap/RTABMapLib.java | 1 + 11 files changed, 234 insertions(+), 71 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 035879d5..33fddbe3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,12 +43,7 @@ ENDIF(${CMAKE_GENERATOR} MATCHES ".*Makefiles") IF(NOT ANDROID) SET(CMAKE_DEBUG_POSTFIX "d") -ELSE() - option(DISABLE_LOG "Disable Android logging (should be true in release)" ON) - IF(DISABLE_LOG) - ADD_DEFINITIONS(-DDISABLE_LOG) - ENDIF(DISABLE_LOG) -ENDIF() +ENDIF(NOT ANDROID) IF(WIN32 AND NOT MINGW) ADD_DEFINITIONS("-DNOMINMAX") diff --git a/app/android/CMakeLists.txt b/app/android/CMakeLists.txt index 04754236..b04a4bbe 100644 --- a/app/android/CMakeLists.txt +++ b/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 diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index b4b6ed35..75da9626 100644 --- a/app/android/jni/CameraTango.cpp +++ b/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(0,0); + const double & fy = fisheyeModel.K().at(1,1); + const double & cx = fisheyeModel.K().at(0,2); + const double & cy = fisheyeModel.K().at(1,2); + const double & w = fisheyeModel.D().at(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(iu, ju) = jd; + mapY.at(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_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_) { - LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret); - return false; + 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(0,0) = color_camera_intrinsics.fx; + K.at(1,1) = color_camera_intrinsics.fy; + K.at(0,2) = color_camera_intrinsics.cx; + K.at(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(0,0) = color_camera_intrinsics.distortion[0]; + D.at(0,1) = color_camera_intrinsics.distortion[1]; + D.at(0,2) = color_camera_intrinsics.distortion[2]; + D.at(0,3) = color_camera_intrinsics.distortion[3]; + D.at(0,4) = color_camera_intrinsics.distortion[4]; + } + else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_3_PARAMETERS) + { + D.at(0,0) = color_camera_intrinsics.distortion[0]; + D.at(0,1) = color_camera_intrinsics.distortion[1]; + D.at(0,2) = 0.; + D.at(0,3) = 0.; + D.at(0,4) = color_camera_intrinsics.distortion[2]; + } + else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_2_PARAMETERS) + { + D.at(0,0) = color_camera_intrinsics.distortion[0]; + D.at(0,1) = color_camera_intrinsics.distortion[1]; + D.at(0,2) = 0.; + D.at(0,3) = 0.; + D.at(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(0,0), D.at(0,1), D.at(0,2), D.at(0,3), D.at(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 scanData(rawScanPublished_?cloud.total():0); int oi=0; for(unsigned int i=0; iisRunning()) 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 */ diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 3725bae4..fa96594e 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/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 RTABMapApp::filterOrganizedPolygons( } } - int biggestCluster = 0; unsigned int biggestClusterSize = 0; for(std::map >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) { @@ -533,12 +533,11 @@ std::vector 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 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::Ptr mergedClouds(new pcl::PointCloud); for(std::map::iterator iter=poses.begin(); iter!= poses.end(); diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 5ff0ec91..f01f80a1 100644 --- a/app/android/jni/RTABMapApp.h +++ b/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_; diff --git a/app/android/jni/jni_interface.cpp b/app/android/jni/jni_interface.cpp index 7a7d54de..ad9461de 100644 --- a/app/android/jni/jni_interface.cpp +++ b/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) { diff --git a/app/android/res/layout/activity_settings.xml b/app/android/res/layout/activity_settings.xml index d8763b62..a11636bb 100644 --- a/app/android/res/layout/activity_settings.xml +++ b/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"/> + diff --git a/app/android/res/values/strings.xml b/app/android/res/values/strings.xml index 5833724b..48220259 100644 --- a/app/android/res/values/strings.xml +++ b/app/android/res/values/strings.xml @@ -64,6 +64,8 @@ false pref_key_smoothing true + pref_key_fisheye + false pref_key_update_rate 1 @@ -240,9 +242,11 @@ 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. HD Mode - Save HD images if you want very detailed textures. More memory will be required. + Save HD images of the color camera if you want very detailed textures. More memory will be required. Smoothing Smooth the point clouds. + Fish Eye Camera + Use fish eye camera instead of the color camera. Cannot be used on Yellowstone tablet. Update Rate Rate at which a new node is added to map. Time Limit diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 091359bf..c1117dd2 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/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); } diff --git a/app/android/src/com/introlab/rtabmap/RTABMapLib.java b/app/android/src/com/introlab/rtabmap/RTABMapLib.java index 82db2be9..c8c3c392 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapLib.java +++ b/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);