From aa37ac60386516d7cc1aff93156b3f1423b2c992 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Apr 2017 14:14:36 -0400 Subject: [PATCH] Increased version to 0.12.4, Tango: gain compensation on each channel, added min depth parameter, fixed exporting failing when doing export just after opening a database --- CMakeLists.txt | 2 +- app/android/AndroidManifest.xml.in | 2 +- app/android/CMakeLists.txt | 2 + app/android/jni/RTABMapApp.cpp | 145 ++++++++++++------ app/android/jni/RTABMapApp.h | 2 + app/android/jni/jni_interface.cpp | 6 + app/android/jni/point_cloud_drawable.cpp | 81 ++++++---- app/android/jni/point_cloud_drawable.h | 12 +- app/android/jni/scene.cpp | 19 ++- app/android/jni/scene.h | 2 +- .../jni/tango-gl/include/tango-gl/util.h | 3 +- app/android/jni/util.h | 9 +- app/android/res/layout/activity_settings.xml | 7 + app/android/res/values/strings.xml | 44 ++++-- .../com/introlab/rtabmap/RTABMapActivity.java | 5 +- .../src/com/introlab/rtabmap/RTABMapLib.java | 1 + .../introlab/rtabmap/SettingsActivity.java | 22 ++- .../include/rtabmap/core/GainCompensator.h | 4 +- corelib/src/GainCompensator.cpp | 16 +- 19 files changed, 271 insertions(+), 113 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 33fddbe3..aea17c5b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 12) -SET(RTABMAP_PATCH_VERSION 3) +SET(RTABMAP_PATCH_VERSION 4) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in index 3a684a0e..b6b6cb3f 100644 --- a/app/android/AndroidManifest.xml.in +++ b/app/android/AndroidManifest.xml.in @@ -2,7 +2,7 @@ diff --git a/app/android/CMakeLists.txt b/app/android/CMakeLists.txt index b04a4bbe..10630b9b 100644 --- a/app/android/CMakeLists.txt +++ b/app/android/CMakeLists.txt @@ -4,6 +4,8 @@ IF(DISABLE_LOG) ADD_DEFINITIONS(-DDISABLE_LOG) ENDIF(DISABLE_LOG) +MESSAGE(STATUS "DISABLE_LOG = ${DISABLE_LOG}") + IF(DISABLE_LOG) SET(ANDROID_DEBUGGABLE false) ELSE() diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 91a08d56..0e1c8aec 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -157,6 +157,7 @@ RTABMapApp::RTABMapApp() : fullResolution_(false), appendMode_(true), maxCloudDepth_(0.0), + minCloudDepth_(0.0), cloudDensityLevel_(1), meshTrianglePix_(1), meshAngleToleranceDeg_(15.0), @@ -252,9 +253,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity) if(logHandler_ == 0) { -#ifndef DISABLE_LOG logHandler_ = new LogHandler(); -#endif } this->registerToEventsManager(); @@ -346,7 +345,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe // Voxelize and filter depending on the previous cloud? pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get()); if(cloud->size() && indices->size()) { std::vector polygons; @@ -367,11 +366,20 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe inserted.first->second.polygonsLowRes = polygonsLowRes; inserted.first->second.visible = true; inserted.first->second.cameraModel = data.cameraModels()[0]; - inserted.first->second.gain = 1.0f; + inserted.first->second.gains[0] = 1.0; + inserted.first->second.gains[1] = 1.0; + inserted.first->second.gains[2] = 1.0; if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering()) { - cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_), data.imageRaw().rows/(data.imageRaw().cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_)); - cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); + if(renderingTextureDecimation_>1) + { + cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_); + cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); + } + else + { + inserted.first->second.texture = data.imageRaw(); + } } LOGI("Created cloud %d (%fs)", id, timer.ticks()); } @@ -426,6 +434,8 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe stats.setConstraints(links); rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats)); + rtabmap_->setOptimizedPoses(poses); + // Start threads LOGI("Start rtabmap thread"); rtabmapThread_->registerToEventsManager(); @@ -799,7 +809,7 @@ void RTABMapApp::gainCompensation(bool full) } UASSERT(maxGainRadius_>0.0f); - rtabmap::GainCompensator compensator(maxGainRadius_); + rtabmap::GainCompensator compensator(maxGainRadius_, 0.0f, 0.01f, 1.0f); if(clouds.size() > 1 && links.size()) { compensator.feed(clouds, indices, links); @@ -812,8 +822,8 @@ void RTABMapApp::gainCompensation(bool full) { if(clouds.size() > 1 && links.size()) { - iter->second.gain = compensator.getGain(iter->first); - LOGI("%d mesh has gain %f", iter->first, iter->second.gain); + compensator.getGain(iter->first, &iter->second.gains[0], &iter->second.gains[1], &iter->second.gains[2]); + LOGI("%d mesh has gain %f,%f,%f", iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]); } } } @@ -900,7 +910,7 @@ int RTABMapApp::Render() if(exportedMesh_->tex_polygons.size() && exportedMesh_->tex_polygons[0].size()) { Mesh mesh; - mesh.gain = 1.0f; + mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0; mesh.cloud.reset(new pcl::PointCloud); mesh.normals.reset(new pcl::PointCloud); pcl::fromPCLPointCloud2(exportedMesh_->cloud, *mesh.cloud); @@ -1039,9 +1049,16 @@ int RTABMapApp::Render() textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first)); if(!textureRaw.empty()) { - cv::Size reducedSize(textureRaw.cols/(textureRaw.cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_), textureRaw.rows/(textureRaw.cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_)); - LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height); - cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); + if(renderingTextureDecimation_ > 1) + { + cv::Size reducedSize(textureRaw.cols/renderingTextureDecimation_, textureRaw.rows/renderingTextureDecimation_); + LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height); + cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); + } + else + { + iter->second.texture = textureRaw; + } } } main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose); @@ -1211,7 +1228,7 @@ int RTABMapApp::Render() // Voxelize and filter depending on the previous cloud? pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get()); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Creating node cloud %d (depth=%dx%d rgb=%dx%d, %fs)", id, data.depthRaw().cols, data.depthRaw().rows, data.imageRaw().cols, data.imageRaw().rows, time.ticks()); #endif @@ -1241,14 +1258,23 @@ int RTABMapApp::Render() inserted.first->second.polygonsLowRes = polygonsLowRes; inserted.first->second.visible = true; inserted.first->second.cameraModel = data.cameraModels()[0]; - inserted.first->second.gain = 1.0f; + inserted.first->second.gains[0] = 1.0; + inserted.first->second.gains[1] = 1.0; + inserted.first->second.gains[2] = 1.0; if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering()) { - cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_), data.imageRaw().rows/(data.imageRaw().cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_)); - cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); + if(renderingTextureDecimation_ > 1) + { + cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_); + cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); #ifdef DEBUG_RENDERING_PERFORMANCE - LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks()); + LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks()); #endif + } + else + { + inserted.first->second.texture = data.imageRaw(); + } } } } @@ -1330,7 +1356,7 @@ int RTABMapApp::Render() { pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get()); + cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get()); if(cloud->size() && indices->size()) { LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)", @@ -1358,7 +1384,7 @@ int RTABMapApp::Render() gainCompensation(gainCompensationOnNextRender_==2); for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) { - main_scene_.updateGain(iter->first, iter->second.gain); + main_scene_.updateGains(iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]); } gainCompensationOnNextRender_ = 0; notifyDataLoaded = true; @@ -1683,6 +1709,11 @@ void RTABMapApp::setMaxCloudDepth(float value) maxCloudDepth_ = value; } +void RTABMapApp::setMinCloudDepth(float value) +{ + minCloudDepth_ = value; +} + void RTABMapApp::setCloudDensityLevel(int value) { cloudDensityLevel_ = value; @@ -1857,9 +1888,12 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons int cols = float(textureSize)/(scale*imageSize.width); globalTexture = cv::Mat(textureSize, textureSize, imageType, cv::Scalar::all(255)); + cv::Mat globalTextureMask = cv::Mat(textureSize, textureSize, CV_8UC1, cv::Scalar::all(0)); // make a blank texture cv::Mat emptyImage(int(imageSize.height*scale), int(imageSize.width*scale), imageType, cv::Scalar::all(255)); + cv::Mat emptyImageMask(int(imageSize.height*scale), int(imageSize.width*scale), CV_8UC1, cv::Scalar::all(255)); + bool gainApplied = false; int oi=0; for(int i=0; i<(int)textures.size(); ++i) { @@ -1878,9 +1912,17 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons UASSERT(!image.empty()); cv::Mat resizedImage; cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA); - if(createdMeshes_.find(textures[i]) != createdMeshes_.end() && createdMeshes_.at(textures[i]).gain != 1.0f) + if(createdMeshes_.find(textures[i]) != createdMeshes_.end() && + (createdMeshes_.at(textures[i]).gains[0] != 1.0 || createdMeshes_.at(textures[i]).gains[1] != 1.0 || createdMeshes_.at(textures[i]).gains[2] != 1.0)) { - cv::multiply(resizedImage, createdMeshes_.at(textures[i]).gain, resizedImage); + std::vector channels; + cv::split(resizedImage, channels); + // assuming BGR + cv::multiply(channels[0], createdMeshes_.at(textures[i]).gains[2], channels[0]); + cv::multiply(channels[1], createdMeshes_.at(textures[i]).gains[1], channels[1]); + cv::multiply(channels[2], createdMeshes_.at(textures[i]).gains[0], channels[2]); + cv::merge(channels, resizedImage); + gainApplied = true; } if(resizedImage.type() == CV_8UC1) { @@ -1890,6 +1932,7 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons } UASSERT(resizedImage.type() == globalTexture.type()); resizedImage.copyTo(globalTexture(cv::Rect(u, v, resizedImage.cols, resizedImage.rows))); + emptyImageMask.copyTo(globalTextureMask(cv::Rect(u, v, emptyImageMask.cols, emptyImageMask.rows))); } else { @@ -1905,6 +1948,10 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons progressionStatus_.increment(); } + if(gainApplied) + { + rtabmap::util2d::brightnessAndContrastAuto(globalTexture, globalTextureMask, 0.0f, 10.0f); + } } else { @@ -2019,20 +2066,22 @@ bool RTABMapApp::exportMesh( pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); rtabmap::CameraModel model; - float gain = 1.0f; + float gains[3] = {1.0f}; if(jter != createdMeshes_.end()) { cloud = jter->second.cloud; indices = jter->second.indices; model = jter->second.cameraModel; - gain = jter->second.gain; + gains[0] = jter->second.gains[0]; + gains[1] = jter->second.gains[1]; + gains[2] = jter->second.gains[2]; } else { rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) { - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get()); model = data.cameraModels()[0]; } } @@ -2058,14 +2107,14 @@ bool RTABMapApp::exportMesh( pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals); - if(textureSize == 0 && gain != 1.0f) + if(textureSize == 0 && (gains[0] != 1.0 || gains[1] != 1.0 || gains[2] != 1.0)) { for(unsigned int i=0; isize(); ++i) { pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i); - pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain))); - pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain))); - pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain))); + pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0]))); + pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1]))); + pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2]))); } } @@ -2522,7 +2571,7 @@ bool RTABMapApp::exportMesh( std::map::iterator jter = createdMeshes_.find(iter->first); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); std::vector polygons; - float gain = 1.0f; + float gains[3] = {1.0f}; if(jter != createdMeshes_.end()) { cloud = jter->second.cloud; @@ -2531,14 +2580,16 @@ bool RTABMapApp::exportMesh( { polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); } - gain = jter->second.gain; + gains[0] = jter->second.gains[0]; + gains[1] = jter->second.gains[1]; + gains[2] = jter->second.gains[2]; } else { rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) { - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0); + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_); polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); } } @@ -2564,14 +2615,14 @@ bool RTABMapApp::exportMesh( // colored mesh cloudWithNormals = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second); - if(gain != 1.0f) + if(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 1.0f) { for(unsigned int i=0; isize(); ++i) { pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i); - pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain))); - pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain))); - pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain))); + pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0]))); + pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1]))); + pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2]))); } } @@ -2767,18 +2818,20 @@ bool RTABMapApp::exportMesh( std::map::iterator jter=createdMeshes_.find(iter->first); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::IndicesPtr indices(new std::vector); - float gain = 1.0f; + float gains[3] = {1.0f}; if(regenerateCloud) { if(jter != createdMeshes_.end()) { - gain = jter->second.gain; + gains[0] = jter->second.gains[0]; + gains[1] = jter->second.gains[1]; + gains[2] = jter->second.gains[2]; } rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); if(!data.imageRaw().empty() && !data.depthRaw().empty()) { // full resolution - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get()); + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get()); } } else @@ -2787,14 +2840,16 @@ bool RTABMapApp::exportMesh( { cloud = jter->second.cloud; indices = jter->second.indices; - gain = jter->second.gain; + gains[0] = jter->second.gains[0]; + gains[1] = jter->second.gains[1]; + gains[2] = jter->second.gains[2]; } else { rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); if(!data.imageRaw().empty() && !data.depthRaw().empty()) { - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get()); } } } @@ -2815,16 +2870,16 @@ bool RTABMapApp::exportMesh( transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); } - if(gain != 1.0f) + if(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 1.0f) { //LOGD("cloud %d, gain=%f", iter->first, gain); for(unsigned int i=0; isize(); ++i) { pcl::PointXYZRGB & pt = transformedCloud->at(i); //LOGI("color %d = %d %d %d", i, (int)pt.r, (int)pt.g, (int)pt.b); - pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain))); - pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain))); - pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain))); + pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0]))); + pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1]))); + pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2]))); } } diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 9cca2076..2d1e9438 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -134,6 +134,7 @@ class RTABMapApp : public UEventsHandler { void setAppendMode(bool enabled); void setDataRecorderMode(bool enabled); void setMaxCloudDepth(float value); + void setMinCloudDepth(float value); void setCloudDensityLevel(int value); void setMeshAngleTolerance(float value); void setMeshTriangleSize(int value); @@ -194,6 +195,7 @@ class RTABMapApp : public UEventsHandler { bool fullResolution_; bool appendMode_; float maxCloudDepth_; + float minCloudDepth_; int cloudDensityLevel_; int meshTrianglePix_; float meshAngleToleranceDeg_; diff --git a/app/android/jni/jni_interface.cpp b/app/android/jni/jni_interface.cpp index 4abc61c5..346c1fc3 100644 --- a/app/android/jni/jni_interface.cpp +++ b/app/android/jni/jni_interface.cpp @@ -247,6 +247,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth( return app.setMaxCloudDepth(value); } JNIEXPORT void JNICALL +Java_com_introlab_rtabmap_RTABMapLib_setMinCloudDepth( + JNIEnv*, jobject, float value) +{ + return app.setMinCloudDepth(value); +} +JNIEXPORT void JNICALL Java_com_introlab_rtabmap_RTABMapLib_setCloudDensityLevel( JNIEnv*, jobject, int value) { diff --git a/app/android/jni/point_cloud_drawable.cpp b/app/android/jni/point_cloud_drawable.cpp index 8b821162..4bdbb609 100644 --- a/app/android/jni/point_cloud_drawable.cpp +++ b/app/android/jni/point_cloud_drawable.cpp @@ -44,35 +44,41 @@ PointCloudDrawable::PointCloudDrawable( GLuint textureShaderProgram, const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, - float gain) : - vertex_buffers_(0), - textures_(0), - nPoints_(0), - pose_(rtabmap::Transform::getIdentity()), - poseGl_(1.0f), - visible_(true), - hasNormals_(false), - cloud_shader_program_(cloudShaderProgram), - texture_shader_program_(textureShaderProgram), - gain_(1.0f) + float gainR, + float gainG, + float gainB) : + vertex_buffers_(0), + textures_(0), + nPoints_(0), + pose_(rtabmap::Transform::getIdentity()), + poseGl_(1.0f), + visible_(true), + hasNormals_(false), + cloud_shader_program_(cloudShaderProgram), + texture_shader_program_(textureShaderProgram), + gainR_(gainR), + gainG_(gainG), + gainB_(gainB) { - updateCloud(cloud, indices, gain); + updateCloud(cloud, indices); } PointCloudDrawable::PointCloudDrawable( GLuint cloudShaderProgram, GLuint textureShaderProgram, const Mesh & mesh) : - vertex_buffers_(0), - textures_(0), - nPoints_(0), - pose_(rtabmap::Transform::getIdentity()), - poseGl_(1.0f), - visible_(true), - hasNormals_(false), - cloud_shader_program_(cloudShaderProgram), - texture_shader_program_(textureShaderProgram), - gain_(1.0f) + vertex_buffers_(0), + textures_(0), + nPoints_(0), + pose_(rtabmap::Transform::getIdentity()), + poseGl_(1.0f), + visible_(true), + hasNormals_(false), + cloud_shader_program_(cloudShaderProgram), + texture_shader_program_(textureShaderProgram), + gainR_(1.0f), + gainG_(1.0f), + gainB_(1.0f) { updateMesh(mesh); } @@ -132,13 +138,12 @@ void PointCloudDrawable::updatePolygons(const std::vector & polyg } } -void PointCloudDrawable::updateCloud(const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float gain) +void PointCloudDrawable::updateCloud(const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices) { UASSERT(cloud.get() && !cloud->empty()); nPoints_ = 0; polygons_.clear(); polygonsLowRes_.clear(); - gain_ = gain; verticesLowRes_.clear(); verticesLowLowRes_.clear(); aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000); @@ -264,7 +269,9 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh) vertex_buffers_ = 0; } - gain_ = mesh.gain; + gainR_ = mesh.gains[0]; + gainG_ = mesh.gains[1]; + gainB_ = mesh.gains[2]; bool textureUpdate = false; if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3) @@ -513,14 +520,16 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh) glBindTexture(GL_TEXTURE_2D, textures_); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); cv::Mat rgbImage; - cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGB); + cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGBA); - glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glPixelStorei(GL_UNPACK_ALIGNMENT, 4); //glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0); //glPixelStorei(GL_UNPACK_SKIP_ROWS, 0); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data); GLint error = glGetError(); if(error != GL_NO_ERROR) @@ -636,8 +645,12 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "uTexture"); glUniform1i(texture_handle, 0); - GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "uGain"); - glUniform1f(gain_handle, gain_); + GLuint gainR_handle = glGetUniformLocation(texture_shader_program_, "uGainR"); + GLuint gainG_handle = glGetUniformLocation(texture_shader_program_, "uGainG"); + GLuint gainB_handle = glGetUniformLocation(texture_shader_program_, "uGainB"); + glUniform1f(gainR_handle, gainR_); + glUniform1f(gainG_handle, gainG_); + glUniform1f(gainB_handle, gainB_); GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "aVertex"); GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "aTexCoord"); @@ -705,8 +718,12 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "uPointSize"); glUniform1f(point_size_handle_, pointSize); - GLuint gain_handle = glGetUniformLocation(cloud_shader_program_, "uGain"); - glUniform1f(gain_handle, gain_); + GLuint gainR_handle = glGetUniformLocation(cloud_shader_program_, "uGainR"); + GLuint gainG_handle = glGetUniformLocation(cloud_shader_program_, "uGainG"); + GLuint gainB_handle = glGetUniformLocation(cloud_shader_program_, "uGainB"); + glUniform1f(gainR_handle, gainR_); + glUniform1f(gainG_handle, gainG_); + glUniform1f(gainB_handle, gainB_); GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "aVertex"); GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "aColor"); diff --git a/app/android/jni/point_cloud_drawable.h b/app/android/jni/point_cloud_drawable.h index cc836c09..b54283d0 100644 --- a/app/android/jni/point_cloud_drawable.h +++ b/app/android/jni/point_cloud_drawable.h @@ -46,7 +46,9 @@ class PointCloudDrawable { GLuint textureShaderProgram, const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, - float gain); + float gainR = 1.0f, + float gainG = 1.0f, + float gainB = 1.0f); PointCloudDrawable( GLuint cloudShaderProgram, GLuint textureShaderProgram, @@ -54,11 +56,11 @@ class PointCloudDrawable { virtual ~PointCloudDrawable(); void updatePolygons(const std::vector & polygons, const std::vector & polygonsLowRes = std::vector()); - void updateCloud(const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float gain); + void updateCloud(const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices); void updateMesh(const Mesh & mesh); void setPose(const rtabmap::Transform & pose); void setVisible(bool visible) {visible_=visible;} - void setGain(float gain) {gain_ = gain;} + void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;} rtabmap::Transform getPose() const {return pose_;} const glm::mat4 & getPoseGl() const {return poseGl_;} bool isVisible() const {return visible_;} @@ -114,7 +116,9 @@ class PointCloudDrawable { GLuint cloud_shader_program_; GLuint texture_shader_program_; - float gain_; + float gainR_; + float gainG_; + float gainB_; pcl::PointXYZ aabbMinModel_; pcl::PointXYZ aabbMaxModel_; diff --git a/app/android/jni/scene.cpp b/app/android/jni/scene.cpp index c1deb377..26c845f7 100644 --- a/app/android/jni/scene.cpp +++ b/app/android/jni/scene.cpp @@ -74,12 +74,14 @@ const std::string kPointCloudVertexShader = const std::string kPointCloudFragmentShader = "precision mediump float;\n" "precision mediump int;\n" - "uniform float uGain;\n" + "uniform float uGainR;\n" + "uniform float uGainG;\n" + "uniform float uGainB;\n" "varying vec3 vColor;\n" "varying float vLightWeighting;\n" "void main() {\n" " vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n" - " gl_FragColor = vec4(textureColor.rgb * uGain * vLightWeighting, textureColor.a);\n" + " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n" "}\n"; const std::string kTextureMeshVertexShader = @@ -120,12 +122,14 @@ const std::string kTextureMeshFragmentShader = "precision mediump float;\n" "precision mediump int;\n" "uniform sampler2D uTexture;\n" - "uniform float uGain;\n" + "uniform float uGainR;\n" + "uniform float uGainG;\n" + "uniform float uGainB;\n" "varying vec2 vTexCoord;\n" "varying float vLightWeighting;\n" "void main() {\n" " vec4 textureColor = texture2D(uTexture, vTexCoord);\n" - " gl_FragColor = vec4(textureColor.rgb * uGain * vLightWeighting, textureColor.a);\n" + " gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n" "}\n"; const std::string kGraphVertexShader = @@ -624,8 +628,7 @@ void Scene::addCloud( cloud_shader_program_, texture_mesh_shader_program_, cloud, - indices, - 1.0f); + indices); drawable->setPose(pose); pointClouds_.insert(std::make_pair(id, drawable)); } @@ -711,11 +714,11 @@ void Scene::updateMesh(int id, const Mesh & mesh) } } -void Scene::updateGain(int id, float gain) +void Scene::updateGains(int id, float gainR, float gainG, float gainB) { std::map::iterator iter=pointClouds_.find(id); if(iter != pointClouds_.end()) { - iter->second->setGain(gain); + iter->second->setGains(gainR, gainG, gainB); } } diff --git a/app/android/jni/scene.h b/app/android/jni/scene.h index d285f179..c0ce7f3b 100644 --- a/app/android/jni/scene.h +++ b/app/android/jni/scene.h @@ -118,7 +118,7 @@ class Scene { std::set getAddedClouds() const; void updateCloudPolygons(int id, const std::vector & polygons); void updateMesh(int id, const Mesh & mesh); - void updateGain(int id, float gain); + void updateGains(int id, float gainR, float gainG, float gainB); void setMapRendering(bool enabled) {mapRendering_ = enabled;} void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;} diff --git a/app/android/jni/tango-gl/include/tango-gl/util.h b/app/android/jni/tango-gl/include/tango-gl/util.h index 967c2cb3..4694d67a 100644 --- a/app/android/jni/tango-gl/include/tango-gl/util.h +++ b/app/android/jni/tango-gl/include/tango-gl/util.h @@ -38,13 +38,12 @@ #define LOGD(...) ; #define LOGI(...) ; #define LOGW(...) ; -#define LOGE(...) ; #else #define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__) #define LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__) #define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__) -#define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__) #endif +#define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__) #ifndef M_PI #define M_PI 3.1415926f diff --git a/app/android/jni/util.h b/app/android/jni/util.h index 7d99ecbb..d053e965 100644 --- a/app/android/jni/util.h +++ b/app/android/jni/util.h @@ -151,9 +151,10 @@ public: cloud(new pcl::PointCloud), normals(new pcl::PointCloud), indices(new std::vector), - visible(true), - gain(1.0f) - {} + visible(true) + { + gains[0] = gains[1] = gains[2] = 1.0f; + } pcl::PointCloud::Ptr cloud; // organized cloud pcl::PointCloud::Ptr normals; @@ -163,7 +164,7 @@ public: rtabmap::Transform pose; // in rtabmap coordinates bool visible; rtabmap::CameraModel cameraModel; - float gain; + double gains[3]; // RGB gains #if PCL_VERSION_COMPARE(>=, 1, 8, 0) std::vector > texCoords; #else diff --git a/app/android/res/layout/activity_settings.xml b/app/android/res/layout/activity_settings.xml index a11636bb..5772fc52 100644 --- a/app/android/res/layout/activity_settings.xml +++ b/app/android/res/layout/activity_settings.xml @@ -16,6 +16,13 @@ android:entries="@array/pref_depth_keys" android:entryValues="@array/pref_depth_values" android:defaultValue="@string/pref_default_depth"/> + pref_key_reset_button pref_key_density 1 + pref_key_min_depth + 0 pref_key_depth 2.5 pref_key_point_size @@ -131,6 +133,8 @@ Size in pixels of the polygons created from the depth image. Texture Resolution Resolution of the texture for online rendering. This doesn\'t affect Export results. + Min Depth + Points under the minimum depth are not rendered. Max Depth Points over the maximum depth are not rendered. Point Size @@ -150,17 +154,39 @@ "2" "3" + + "0 m" + "0.3 m" + "0.5 m" + "0.75 m" + "1 m" + "1.5 m" + "2 m" + "2.5 m" + "3 m" + + + "0" + "0.3" + "0.5" + "0.75" + "1" + "1.5" + "2" + "2.5" + "3" + "No Limit" - "5" - "4.5" - "4" - "3.5" - "3" - "2.5" - "2" - "1.5" - "1" + "5 m" + "4.5 m" + "4 m" + "3.5 m" + "3 m" + "2.5 m" + "2 m" + "1.5 m" + "1 m" "0" diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 92374773..309d5818 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -480,6 +480,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { if(!DISABLE_LOG) Log.d(TAG, "set exporting parameters..."); 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.setMinCloudDepth(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_min_depth), getString(R.string.pref_default_min_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)))); RTABMapLib.setMeshTriangleSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_triangle), getString(R.string.pref_default_triangle)))); @@ -1309,14 +1310,14 @@ public class RTABMapActivity extends Activity implements OnClickListener { else if (itemId == R.id.gain_compensation_fast) { mProgressDialog.setTitle("Post-Processing"); - mProgressDialog.setMessage(String.format("Fast gain compensation...")); + mProgressDialog.setMessage(String.format("Adjusting Colors (Fast)...")); 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.setMessage(String.format("Adjusting Colors (Full)...")); mProgressDialog.show(); RTABMapLib.postProcessing(6); } diff --git a/app/android/src/com/introlab/rtabmap/RTABMapLib.java b/app/android/src/com/introlab/rtabmap/RTABMapLib.java index abd66cc3..175d1379 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapLib.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapLib.java @@ -75,6 +75,7 @@ public class RTABMapLib public static native void setAppendMode(boolean enabled); public static native void setDataRecorderMode(boolean enabled); public static native void setMaxCloudDepth(float value); + public static native void setMinCloudDepth(float value); public static native void setPointSize(float value); public static native void setLighting(boolean enabled); public static native void setBackfaceCulling(boolean enabled); diff --git a/app/android/src/com/introlab/rtabmap/SettingsActivity.java b/app/android/src/com/introlab/rtabmap/SettingsActivity.java index f8ddc2e7..405f7b5b 100644 --- a/app/android/src/com/introlab/rtabmap/SettingsActivity.java +++ b/app/android/src/com/introlab/rtabmap/SettingsActivity.java @@ -30,6 +30,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref ((Preference)findPreference(getString(R.string.pref_key_density))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_density))).getEntry() + ") "+getString(R.string.pref_summary_density)); ((Preference)findPreference(getString(R.string.pref_key_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth))).getEntry() + ") "+getString(R.string.pref_summary_depth)); + ((Preference)findPreference(getString(R.string.pref_key_min_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getEntry() + ") "+getString(R.string.pref_summary_min_depth)); ((Preference)findPreference(getString(R.string.pref_key_point_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_point_size))).getEntry() + ") "+getString(R.string.pref_summary_point_size)); ((Preference)findPreference(getString(R.string.pref_key_angle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_angle))).getEntry() + ") "+getString(R.string.pref_summary_angle)); ((Preference)findPreference(getString(R.string.pref_key_triangle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_triangle))).getEntry() + ") "+getString(R.string.pref_summary_triangle)); @@ -65,7 +66,26 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref if (pref instanceof ListPreference) { if(key.compareTo(getString(R.string.pref_key_density))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_density)); - if(key.compareTo(getString(R.string.pref_key_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth)); + if(key.compareTo(getString(R.string.pref_key_depth))==0) + { + pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth)); + float maxDepth = Float.parseFloat(((ListPreference)pref).getValue()); + float minDepth = Float.parseFloat(((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getValue()); + if(maxDepth > 0.0f && maxDepth <= minDepth) + { + ((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).setValueIndex(0); + } + } + if(key.compareTo(getString(R.string.pref_key_min_depth))==0) + { + pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_depth)); + float maxDepth = Float.parseFloat(((ListPreference)findPreference(getString(R.string.pref_key_depth))).getValue()); + float minDepth = Float.parseFloat(((ListPreference)pref).getValue()); + if(minDepth >= maxDepth) + { + ((ListPreference)findPreference(getString(R.string.pref_key_depth))).setValueIndex(0); + } + } if(key.compareTo(getString(R.string.pref_key_point_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_point_size)); if(key.compareTo(getString(R.string.pref_key_angle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_angle)); if(key.compareTo(getString(R.string.pref_key_triangle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_triangle)); diff --git a/corelib/include/rtabmap/core/GainCompensator.h b/corelib/include/rtabmap/core/GainCompensator.h index 621845cd..239a8fbf 100644 --- a/corelib/include/rtabmap/core/GainCompensator.h +++ b/corelib/include/rtabmap/core/GainCompensator.h @@ -43,7 +43,7 @@ namespace rtabmap { */ class RTABMAP_EXP GainCompensator { public: - GainCompensator(double maxCorrespondenceDistance = 0.02, double minOverlap = 0.05, double alpha = 0.01, double beta = 10); + GainCompensator(double maxCorrespondenceDistance = 0.02, double minOverlap = 0.0, double alpha = 0.01, double beta = 10); virtual ~GainCompensator(); void feed( @@ -86,7 +86,7 @@ public: int id, cv::Mat & image) const; - double getGain(int id) const; + double getGain(int id, double * r=0, double * g=0, double * b=0) const; int getIndex(int id) const; private: diff --git a/corelib/src/GainCompensator.cpp b/corelib/src/GainCompensator.cpp index 5516bb42..07eb6501 100644 --- a/corelib/src/GainCompensator.cpp +++ b/corelib/src/GainCompensator.cpp @@ -468,9 +468,23 @@ void GainCompensator::apply( } } -double GainCompensator::getGain(int id) const +double GainCompensator::getGain(int id, double * r, double * g, double * b) const { UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str()); + + if(r) + { + *r = gains_(idToIndex_.at(id), 1); + } + if(g) + { + *g = gains_(idToIndex_.at(id), 2); + } + if(b) + { + *b = gains_(idToIndex_.at(id), 3); + } + return gains_(idToIndex_.at(id), 0); }