From 5b08a69607f7693392a4eeb9a7abc294d1b060fd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 15 Mar 2017 17:05:19 -0400 Subject: [PATCH] Tango: added comptible fisheye cloud density level --- app/android/jni/CameraTango.cpp | 4 +- app/android/jni/RTABMapApp.cpp | 123 ++++++++++-------- app/android/jni/RTABMapApp.h | 5 +- app/android/jni/jni_interface.cpp | 4 +- .../com/introlab/rtabmap/RTABMapActivity.java | 2 +- .../src/com/introlab/rtabmap/RTABMapLib.java | 2 +- 6 files changed, 79 insertions(+), 61 deletions(-) diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index 75da9626..e1fdc4c1 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -736,7 +736,7 @@ SensorData CameraTango::captureImage(CameraInfo * info) bool pixelSet = false; if(pixel_x_l>=0 && pixel_x_l0 && pixel_y_l0 && pixel_y_l(pixel_y_l, pixel_x_l); @@ -747,7 +747,7 @@ SensorData CameraTango::captureImage(CameraInfo * info) } } if(pixel_x_h>=0 && pixel_x_h0 && pixel_y_h0 && pixel_y_h(pixel_y_h, pixel_x_h); diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index fa96594e..e0ec4811 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/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); - 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_); + cloudDensityLevel_ = value; } void RTABMapApp::setMeshAngleTolerance(float value) diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index f01f80a1..9cca2076 100644 --- a/app/android/jni/RTABMapApp.h +++ b/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_; diff --git a/app/android/jni/jni_interface.cpp b/app/android/jni/jni_interface.cpp index ad9461de..4abc61c5 100644 --- a/app/android/jni/jni_interface.cpp +++ b/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( diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 0593ccda..48d7f088 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/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)))); diff --git a/app/android/src/com/introlab/rtabmap/RTABMapLib.java b/app/android/src/com/introlab/rtabmap/RTABMapLib.java index c8c3c392..abd66cc3 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapLib.java +++ b/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);