i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2017-04-01 14:14:36 -04:00
parent d68eeada0e
commit aa37ac6038
19 changed files with 271 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
####################### #######################
SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 12) SET(RTABMAP_MINOR_VERSION 12)
SET(RTABMAP_PATCH_VERSION 3) SET(RTABMAP_PATCH_VERSION 4)
SET(RTABMAP_VERSION SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})

2
app/android/AndroidManifest.xml.in
View File

@@ -2,7 +2,7 @@
<!-- BEGIN_INCLUDE(manifest) --> <!-- BEGIN_INCLUDE(manifest) -->
<manifest xmlns:android="http://schemas.android.com/apk/res/android" <manifest xmlns:android="http://schemas.android.com/apk/res/android"
package="com.introlab.rtabmap" package="com.introlab.rtabmap"
android:versionCode="47" android:versionCode="48"
android:versionName="@RTABMAP_VERSION@"> android:versionName="@RTABMAP_VERSION@">
<uses-permission android:name="android.permission.CAMERA" /> <uses-permission android:name="android.permission.CAMERA" />

2
app/android/CMakeLists.txt
View File

@@ -4,6 +4,8 @@ IF(DISABLE_LOG)
ADD_DEFINITIONS(-DDISABLE_LOG) ADD_DEFINITIONS(-DDISABLE_LOG)
ENDIF(DISABLE_LOG) ENDIF(DISABLE_LOG)
MESSAGE(STATUS "DISABLE_LOG = ${DISABLE_LOG}")
IF(DISABLE_LOG) IF(DISABLE_LOG)
SET(ANDROID_DEBUGGABLE false) SET(ANDROID_DEBUGGABLE false)
ELSE() ELSE()

145
app/android/jni/RTABMapApp.cpp
View File

@@ -157,6 +157,7 @@ RTABMapApp::RTABMapApp() :
fullResolution_(false), fullResolution_(false),
appendMode_(true), appendMode_(true),
maxCloudDepth_(0.0), maxCloudDepth_(0.0),
minCloudDepth_(0.0),
cloudDensityLevel_(1), cloudDensityLevel_(1),
meshTrianglePix_(1), meshTrianglePix_(1),
meshAngleToleranceDeg_(15.0), meshAngleToleranceDeg_(15.0),
@@ -252,9 +253,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
if(logHandler_ == 0) if(logHandler_ == 0)
{ {
#ifndef DISABLE_LOG
logHandler_ = new LogHandler(); logHandler_ = new LogHandler();
#endif
} }
this->registerToEventsManager(); this->registerToEventsManager();
@@ -346,7 +345,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
// Voxelize and filter depending on the previous cloud? // Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
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()) if(cloud->size() && indices->size())
{ {
std::vector<pcl::Vertices> polygons; std::vector<pcl::Vertices> polygons;
@@ -367,11 +366,20 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
inserted.first->second.polygonsLowRes = polygonsLowRes; inserted.first->second.polygonsLowRes = polygonsLowRes;
inserted.first->second.visible = true; inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0]; 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()) 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_)); if(renderingTextureDecimation_>1)
cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); {
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()); LOGI("Created cloud %d (%fs)", id, timer.ticks());
} }
@@ -426,6 +434,8 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
stats.setConstraints(links); stats.setConstraints(links);
rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats)); rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
rtabmap_->setOptimizedPoses(poses);
// Start threads // Start threads
LOGI("Start rtabmap thread"); LOGI("Start rtabmap thread");
rtabmapThread_->registerToEventsManager(); rtabmapThread_->registerToEventsManager();
@@ -799,7 +809,7 @@ void RTABMapApp::gainCompensation(bool full)
} }
UASSERT(maxGainRadius_>0.0f); UASSERT(maxGainRadius_>0.0f);
rtabmap::GainCompensator compensator(maxGainRadius_); rtabmap::GainCompensator compensator(maxGainRadius_, 0.0f, 0.01f, 1.0f);
if(clouds.size() > 1 && links.size()) if(clouds.size() > 1 && links.size())
{ {
compensator.feed(clouds, indices, links); compensator.feed(clouds, indices, links);
@@ -812,8 +822,8 @@ void RTABMapApp::gainCompensation(bool full)
{ {
if(clouds.size() > 1 && links.size()) if(clouds.size() > 1 && links.size())
{ {
iter->second.gain = compensator.getGain(iter->first); compensator.getGain(iter->first, &iter->second.gains[0], &iter->second.gains[1], &iter->second.gains[2]);
LOGI("%d mesh has gain %f", iter->first, iter->second.gain); 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()) if(exportedMesh_->tex_polygons.size() && exportedMesh_->tex_polygons[0].size())
{ {
Mesh mesh; Mesh mesh;
mesh.gain = 1.0f; mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0;
mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>); mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
mesh.normals.reset(new pcl::PointCloud<pcl::Normal>); mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
pcl::fromPCLPointCloud2(exportedMesh_->cloud, *mesh.cloud); pcl::fromPCLPointCloud2(exportedMesh_->cloud, *mesh.cloud);
@@ -1039,9 +1049,16 @@ int RTABMapApp::Render()
textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first)); textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
if(!textureRaw.empty()) if(!textureRaw.empty())
{ {
cv::Size reducedSize(textureRaw.cols/(textureRaw.cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_), textureRaw.rows/(textureRaw.cols>1000?renderingTextureDecimation_*2:renderingTextureDecimation_)); if(renderingTextureDecimation_ > 1)
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); 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); 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? // Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
#ifdef DEBUG_RENDERING_PERFORMANCE #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()); 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 #endif
@@ -1241,14 +1258,23 @@ int RTABMapApp::Render()
inserted.first->second.polygonsLowRes = polygonsLowRes; inserted.first->second.polygonsLowRes = polygonsLowRes;
inserted.first->second.visible = true; inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0]; 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()) 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_)); if(renderingTextureDecimation_ > 1)
cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); {
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 #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 #endif
}
else
{
inserted.first->second.texture = data.imageRaw();
}
} }
} }
} }
@@ -1330,7 +1356,7 @@ int RTABMapApp::Render()
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
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()) if(cloud->size() && indices->size())
{ {
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)", LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
@@ -1358,7 +1384,7 @@ int RTABMapApp::Render()
gainCompensation(gainCompensationOnNextRender_==2); gainCompensation(gainCompensationOnNextRender_==2);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) for(std::map<int, Mesh>::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; gainCompensationOnNextRender_ = 0;
notifyDataLoaded = true; notifyDataLoaded = true;
@@ -1683,6 +1709,11 @@ void RTABMapApp::setMaxCloudDepth(float value)
maxCloudDepth_ = value; maxCloudDepth_ = value;
} }
void RTABMapApp::setMinCloudDepth(float value)
{
minCloudDepth_ = value;
}
void RTABMapApp::setCloudDensityLevel(int value) void RTABMapApp::setCloudDensityLevel(int value)
{ {
cloudDensityLevel_ = value; cloudDensityLevel_ = value;
@@ -1857,9 +1888,12 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
int cols = float(textureSize)/(scale*imageSize.width); int cols = float(textureSize)/(scale*imageSize.width);
globalTexture = cv::Mat(textureSize, textureSize, imageType, cv::Scalar::all(255)); 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 // make a blank texture
cv::Mat emptyImage(int(imageSize.height*scale), int(imageSize.width*scale), imageType, cv::Scalar::all(255)); 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; int oi=0;
for(int i=0; i<(int)textures.size(); ++i) 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()); UASSERT(!image.empty());
cv::Mat resizedImage; cv::Mat resizedImage;
cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA); 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<cv::Mat> 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) if(resizedImage.type() == CV_8UC1)
{ {
@@ -1890,6 +1932,7 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
} }
UASSERT(resizedImage.type() == globalTexture.type()); UASSERT(resizedImage.type() == globalTexture.type());
resizedImage.copyTo(globalTexture(cv::Rect(u, v, resizedImage.cols, resizedImage.rows))); resizedImage.copyTo(globalTexture(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
emptyImageMask.copyTo(globalTextureMask(cv::Rect(u, v, emptyImageMask.cols, emptyImageMask.rows)));
} }
else else
{ {
@@ -1905,6 +1948,10 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
progressionStatus_.increment(); progressionStatus_.increment();
} }
if(gainApplied)
{
rtabmap::util2d::brightnessAndContrastAuto(globalTexture, globalTextureMask, 0.0f, 10.0f);
}
} }
else else
{ {
@@ -2019,20 +2066,22 @@ bool RTABMapApp::exportMesh(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
rtabmap::CameraModel model; rtabmap::CameraModel model;
float gain = 1.0f; float gains[3] = {1.0f};
if(jter != createdMeshes_.end()) if(jter != createdMeshes_.end())
{ {
cloud = jter->second.cloud; cloud = jter->second.cloud;
indices = jter->second.indices; indices = jter->second.indices;
model = jter->second.cameraModel; 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 else
{ {
rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) 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]; model = data.cameraModels()[0];
} }
} }
@@ -2058,14 +2107,14 @@ bool RTABMapApp::exportMesh(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals); 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; i<cloudWithNormals->size(); ++i) for(unsigned int i=0; i<cloudWithNormals->size(); ++i)
{ {
pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i); pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i);
pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * 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) * gain))); 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) * gain))); 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<int, Mesh>::iterator jter = createdMeshes_.find(iter->first); std::map<int, Mesh>::iterator jter = createdMeshes_.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> polygons; std::vector<pcl::Vertices> polygons;
float gain = 1.0f; float gains[3] = {1.0f};
if(jter != createdMeshes_.end()) if(jter != createdMeshes_.end())
{ {
cloud = jter->second.cloud; cloud = jter->second.cloud;
@@ -2531,14 +2580,16 @@ bool RTABMapApp::exportMesh(
{ {
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); 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 else
{ {
rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) 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_); polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
} }
} }
@@ -2564,14 +2615,14 @@ bool RTABMapApp::exportMesh(
// colored mesh // colored mesh
cloudWithNormals = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second); 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; i<cloudWithNormals->size(); ++i) for(unsigned int i=0; i<cloudWithNormals->size(); ++i)
{ {
pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i); pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i);
pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * 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) * gain))); 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) * gain))); 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<int, Mesh>::iterator jter=createdMeshes_.find(iter->first); std::map<int, Mesh>::iterator jter=createdMeshes_.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
float gain = 1.0f; float gains[3] = {1.0f};
if(regenerateCloud) if(regenerateCloud)
{ {
if(jter != createdMeshes_.end()) 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); rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
if(!data.imageRaw().empty() && !data.depthRaw().empty()) if(!data.imageRaw().empty() && !data.depthRaw().empty())
{ {
// full resolution // full resolution
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get()); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
} }
} }
else else
@@ -2787,14 +2840,16 @@ bool RTABMapApp::exportMesh(
{ {
cloud = jter->second.cloud; cloud = jter->second.cloud;
indices = jter->second.indices; 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 else
{ {
rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
if(!data.imageRaw().empty() && !data.depthRaw().empty()) 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); 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); //LOGD("cloud %d, gain=%f", iter->first, gain);
for(unsigned int i=0; i<transformedCloud->size(); ++i) for(unsigned int i=0; i<transformedCloud->size(); ++i)
{ {
pcl::PointXYZRGB & pt = transformedCloud->at(i); pcl::PointXYZRGB & pt = transformedCloud->at(i);
//LOGI("color %d = %d %d %d", i, (int)pt.r, (int)pt.g, (int)pt.b); //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.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) * gain))); 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) * gain))); pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2])));
} }
} }

2
app/android/jni/RTABMapApp.h
View File

@@ -134,6 +134,7 @@ class RTABMapApp : public UEventsHandler {
void setAppendMode(bool enabled); void setAppendMode(bool enabled);
void setDataRecorderMode(bool enabled); void setDataRecorderMode(bool enabled);
void setMaxCloudDepth(float value); void setMaxCloudDepth(float value);
void setMinCloudDepth(float value);
void setCloudDensityLevel(int value); void setCloudDensityLevel(int value);
void setMeshAngleTolerance(float value); void setMeshAngleTolerance(float value);
void setMeshTriangleSize(int value); void setMeshTriangleSize(int value);
@@ -194,6 +195,7 @@ class RTABMapApp : public UEventsHandler {
bool fullResolution_; bool fullResolution_;
bool appendMode_; bool appendMode_;
float maxCloudDepth_; float maxCloudDepth_;
float minCloudDepth_;
int cloudDensityLevel_; int cloudDensityLevel_;
int meshTrianglePix_; int meshTrianglePix_;
float meshAngleToleranceDeg_; float meshAngleToleranceDeg_;

6
app/android/jni/jni_interface.cpp
View File

@@ -247,6 +247,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
return app.setMaxCloudDepth(value); return app.setMaxCloudDepth(value);
} }
JNIEXPORT void JNICALL 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( Java_com_introlab_rtabmap_RTABMapLib_setCloudDensityLevel(
JNIEnv*, jobject, int value) JNIEnv*, jobject, int value)
{ {

81
app/android/jni/point_cloud_drawable.cpp
View File

@@ -44,35 +44,41 @@ PointCloudDrawable::PointCloudDrawable(
GLuint textureShaderProgram, GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float gain) : float gainR,
vertex_buffers_(0), float gainG,
textures_(0), float gainB) :
nPoints_(0), vertex_buffers_(0),
pose_(rtabmap::Transform::getIdentity()), textures_(0),
poseGl_(1.0f), nPoints_(0),
visible_(true), pose_(rtabmap::Transform::getIdentity()),
hasNormals_(false), poseGl_(1.0f),
cloud_shader_program_(cloudShaderProgram), visible_(true),
texture_shader_program_(textureShaderProgram), hasNormals_(false),
gain_(1.0f) cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram),
gainR_(gainR),
gainG_(gainG),
gainB_(gainB)
{ {
updateCloud(cloud, indices, gain); updateCloud(cloud, indices);
} }
PointCloudDrawable::PointCloudDrawable( PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram, GLuint cloudShaderProgram,
GLuint textureShaderProgram, GLuint textureShaderProgram,
const Mesh & mesh) : const Mesh & mesh) :
vertex_buffers_(0), vertex_buffers_(0),
textures_(0), textures_(0),
nPoints_(0), nPoints_(0),
pose_(rtabmap::Transform::getIdentity()), pose_(rtabmap::Transform::getIdentity()),
poseGl_(1.0f), poseGl_(1.0f),
visible_(true), visible_(true),
hasNormals_(false), hasNormals_(false),
cloud_shader_program_(cloudShaderProgram), cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram), texture_shader_program_(textureShaderProgram),
gain_(1.0f) gainR_(1.0f),
gainG_(1.0f),
gainB_(1.0f)
{ {
updateMesh(mesh); updateMesh(mesh);
} }
@@ -132,13 +138,12 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
} }
} }
void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain) void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices)
{ {
UASSERT(cloud.get() && !cloud->empty()); UASSERT(cloud.get() && !cloud->empty());
nPoints_ = 0; nPoints_ = 0;
polygons_.clear(); polygons_.clear();
polygonsLowRes_.clear(); polygonsLowRes_.clear();
gain_ = gain;
verticesLowRes_.clear(); verticesLowRes_.clear();
verticesLowLowRes_.clear(); verticesLowLowRes_.clear();
aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000); aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
@@ -264,7 +269,9 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
vertex_buffers_ = 0; vertex_buffers_ = 0;
} }
gain_ = mesh.gain; gainR_ = mesh.gains[0];
gainG_ = mesh.gains[1];
gainB_ = mesh.gains[2];
bool textureUpdate = false; bool textureUpdate = false;
if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3) if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
@@ -513,14 +520,16 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
glBindTexture(GL_TEXTURE_2D, textures_); glBindTexture(GL_TEXTURE_2D, textures_);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); 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_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::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_ROW_LENGTH, 0);
//glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0); //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
//glPixelStorei(GL_UNPACK_SKIP_ROWS, 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(); GLint error = glGetError();
if(error != GL_NO_ERROR) if(error != GL_NO_ERROR)
@@ -636,8 +645,12 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "uTexture"); GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "uTexture");
glUniform1i(texture_handle, 0); glUniform1i(texture_handle, 0);
GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "uGain"); GLuint gainR_handle = glGetUniformLocation(texture_shader_program_, "uGainR");
glUniform1f(gain_handle, gain_); 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_vertex = glGetAttribLocation(texture_shader_program_, "aVertex");
GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "aTexCoord"); 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"); GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "uPointSize");
glUniform1f(point_size_handle_, pointSize); glUniform1f(point_size_handle_, pointSize);
GLuint gain_handle = glGetUniformLocation(cloud_shader_program_, "uGain"); GLuint gainR_handle = glGetUniformLocation(cloud_shader_program_, "uGainR");
glUniform1f(gain_handle, gain_); 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_vertex = glGetAttribLocation(cloud_shader_program_, "aVertex");
GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "aColor"); GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "aColor");

12
app/android/jni/point_cloud_drawable.h
View File

@@ -46,7 +46,9 @@ class PointCloudDrawable {
GLuint textureShaderProgram, GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float gain); float gainR = 1.0f,
float gainG = 1.0f,
float gainB = 1.0f);
PointCloudDrawable( PointCloudDrawable(
GLuint cloudShaderProgram, GLuint cloudShaderProgram,
GLuint textureShaderProgram, GLuint textureShaderProgram,
@@ -54,11 +56,11 @@ class PointCloudDrawable {
virtual ~PointCloudDrawable(); virtual ~PointCloudDrawable();
void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>()); void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>());
void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain); void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
void updateMesh(const Mesh & mesh); void updateMesh(const Mesh & mesh);
void setPose(const rtabmap::Transform & pose); void setPose(const rtabmap::Transform & pose);
void setVisible(bool visible) {visible_=visible;} 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_;} rtabmap::Transform getPose() const {return pose_;}
const glm::mat4 & getPoseGl() const {return poseGl_;} const glm::mat4 & getPoseGl() const {return poseGl_;}
bool isVisible() const {return visible_;} bool isVisible() const {return visible_;}
@@ -114,7 +116,9 @@ class PointCloudDrawable {
GLuint cloud_shader_program_; GLuint cloud_shader_program_;
GLuint texture_shader_program_; GLuint texture_shader_program_;
float gain_; float gainR_;
float gainG_;
float gainB_;
pcl::PointXYZ aabbMinModel_; pcl::PointXYZ aabbMinModel_;
pcl::PointXYZ aabbMaxModel_; pcl::PointXYZ aabbMaxModel_;

19
app/android/jni/scene.cpp
View File

@@ -74,12 +74,14 @@ const std::string kPointCloudVertexShader =
const std::string kPointCloudFragmentShader = const std::string kPointCloudFragmentShader =
"precision mediump float;\n" "precision mediump float;\n"
"precision mediump int;\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 vec3 vColor;\n"
"varying float vLightWeighting;\n" "varying float vLightWeighting;\n"
"void main() {\n" "void main() {\n"
" vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\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"; "}\n";
const std::string kTextureMeshVertexShader = const std::string kTextureMeshVertexShader =
@@ -120,12 +122,14 @@ const std::string kTextureMeshFragmentShader =
"precision mediump float;\n" "precision mediump float;\n"
"precision mediump int;\n" "precision mediump int;\n"
"uniform sampler2D uTexture;\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 vec2 vTexCoord;\n"
"varying float vLightWeighting;\n" "varying float vLightWeighting;\n"
"void main() {\n" "void main() {\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\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"; "}\n";
const std::string kGraphVertexShader = const std::string kGraphVertexShader =
@@ -624,8 +628,7 @@ void Scene::addCloud(
cloud_shader_program_, cloud_shader_program_,
texture_mesh_shader_program_, texture_mesh_shader_program_,
cloud, cloud,
indices, indices);
1.0f);
drawable->setPose(pose); drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable)); 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<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id); std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end()) if(iter != pointClouds_.end())
{ {
iter->second->setGain(gain); iter->second->setGains(gainR, gainG, gainB);
} }
} }

2
app/android/jni/scene.h
View File

@@ -118,7 +118,7 @@ class Scene {
std::set<int> getAddedClouds() const; std::set<int> getAddedClouds() const;
void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons); void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
void updateMesh(int id, const Mesh & mesh); 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 setMapRendering(bool enabled) {mapRendering_ = enabled;}
void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;} void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}

3
app/android/jni/tango-gl/include/tango-gl/util.h
View File

@@ -38,13 +38,12 @@
#define LOGD(...) ; #define LOGD(...) ;
#define LOGI(...) ; #define LOGI(...) ;
#define LOGW(...) ; #define LOGW(...) ;
#define LOGE(...) ;
#else #else
#define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__) #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 LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define LOGW(...) __android_log_print(ANDROID_LOG_WARN,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 #endif
#define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
#ifndef M_PI #ifndef M_PI
#define M_PI 3.1415926f #define M_PI 3.1415926f

9
app/android/jni/util.h
View File

@@ -151,9 +151,10 @@ public:
cloud(new pcl::PointCloud<pcl::PointXYZRGB>), cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
normals(new pcl::PointCloud<pcl::Normal>), normals(new pcl::PointCloud<pcl::Normal>),
indices(new std::vector<int>), indices(new std::vector<int>),
visible(true), visible(true)
gain(1.0f) {
{} gains[0] = gains[1] = gains[2] = 1.0f;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
pcl::PointCloud<pcl::Normal>::Ptr normals; pcl::PointCloud<pcl::Normal>::Ptr normals;
@@ -163,7 +164,7 @@ public:
rtabmap::Transform pose; // in rtabmap coordinates rtabmap::Transform pose; // in rtabmap coordinates
bool visible; bool visible;
rtabmap::CameraModel cameraModel; rtabmap::CameraModel cameraModel;
float gain; double gains[3]; // RGB gains
#if PCL_VERSION_COMPARE(>=, 1, 8, 0) #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords; std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
#else #else

7
app/android/res/layout/activity_settings.xml
View File

@@ -16,6 +16,13 @@
android:entries="@array/pref_depth_keys" android:entries="@array/pref_depth_keys"
android:entryValues="@array/pref_depth_values" android:entryValues="@array/pref_depth_values"
android:defaultValue="@string/pref_default_depth"/> android:defaultValue="@string/pref_default_depth"/>
<ListPreference
android:key="@string/pref_key_min_depth"
android:title="@string/pref_title_min_depth"
android:summary="@string/pref_summary_min_depth"
android:entries="@array/pref_min_depth_keys"
android:entryValues="@array/pref_min_depth_values"
android:defaultValue="@string/pref_default_min_depth"/>
<ListPreference <ListPreference
android:key="@string/pref_key_point_size" android:key="@string/pref_key_point_size"
android:title="@string/pref_title_point_size" android:title="@string/pref_title_point_size"

44
app/android/res/values/strings.xml
View File

@@ -44,6 +44,8 @@
<string name="pref_key_reset_button">pref_key_reset_button</string> <string name="pref_key_reset_button">pref_key_reset_button</string>
<string name="pref_key_density">pref_key_density</string> <string name="pref_key_density">pref_key_density</string>
<string name="pref_default_density">1</string> <string name="pref_default_density">1</string>
<string name="pref_key_min_depth">pref_key_min_depth</string>
<string name="pref_default_min_depth">0</string>
<string name="pref_key_depth">pref_key_depth</string> <string name="pref_key_depth">pref_key_depth</string>
<string name="pref_default_depth">2.5</string> <string name="pref_default_depth">2.5</string>
<string name="pref_key_point_size">pref_key_point_size</string> <string name="pref_key_point_size">pref_key_point_size</string>
@@ -131,6 +133,8 @@
<string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string> <string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string>
<string name="pref_title_rendering_texture_decimation">Texture Resolution</string> <string name="pref_title_rendering_texture_decimation">Texture Resolution</string>
<string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string> <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string>
<string name="pref_title_min_depth">Min Depth</string>
<string name="pref_summary_min_depth">Points under the minimum depth are not rendered.</string>
<string name="pref_title_depth">Max Depth</string> <string name="pref_title_depth">Max Depth</string>
<string name="pref_summary_depth">Points over the maximum depth are not rendered.</string> <string name="pref_summary_depth">Points over the maximum depth are not rendered.</string>
<string name="pref_title_point_size">Point Size</string> <string name="pref_title_point_size">Point Size</string>
@@ -150,17 +154,39 @@
<item>"2"</item> <item>"2"</item>
<item>"3"</item> <item>"3"</item>
</string-array> </string-array>
<string-array name="pref_min_depth_keys">
<item>"0 m"</item>
<item>"0.3 m"</item>
<item>"0.5 m"</item>
<item>"0.75 m"</item>
<item>"1 m"</item>
<item>"1.5 m"</item>
<item>"2 m"</item>
<item>"2.5 m"</item>
<item>"3 m"</item>
</string-array>
<string-array name="pref_min_depth_values">
<item>"0"</item>
<item>"0.3"</item>
<item>"0.5"</item>
<item>"0.75"</item>
<item>"1"</item>
<item>"1.5"</item>
<item>"2"</item>
<item>"2.5"</item>
<item>"3"</item>
</string-array>
<string-array name="pref_depth_keys"> <string-array name="pref_depth_keys">
<item>"No Limit"</item> <item>"No Limit"</item>
<item>"5"</item> <item>"5 m"</item>
<item>"4.5"</item> <item>"4.5 m"</item>
<item>"4"</item> <item>"4 m"</item>
<item>"3.5"</item> <item>"3.5 m"</item>
<item>"3"</item> <item>"3 m"</item>
<item>"2.5"</item> <item>"2.5 m"</item>
<item>"2"</item> <item>"2 m"</item>
<item>"1.5"</item> <item>"1.5 m"</item>
<item>"1"</item> <item>"1 m"</item>
</string-array> </string-array>
<string-array name="pref_depth_values"> <string-array name="pref_depth_values">
<item>"0"</item> <item>"0"</item>

5
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
View File

@@ -480,6 +480,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
if(!DISABLE_LOG) Log.d(TAG, "set exporting parameters..."); 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.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.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.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.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)))); 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) else if (itemId == R.id.gain_compensation_fast)
{ {
mProgressDialog.setTitle("Post-Processing"); mProgressDialog.setTitle("Post-Processing");
mProgressDialog.setMessage(String.format("Fast gain compensation...")); mProgressDialog.setMessage(String.format("Adjusting Colors (Fast)..."));
mProgressDialog.show(); mProgressDialog.show();
RTABMapLib.postProcessing(5); RTABMapLib.postProcessing(5);
} }
else if (itemId == R.id.gain_compensation_full) else if (itemId == R.id.gain_compensation_full)
{ {
mProgressDialog.setTitle("Post-Processing"); mProgressDialog.setTitle("Post-Processing");
mProgressDialog.setMessage(String.format("Full gain compensation...")); mProgressDialog.setMessage(String.format("Adjusting Colors (Full)..."));
mProgressDialog.show(); mProgressDialog.show();
RTABMapLib.postProcessing(6); RTABMapLib.postProcessing(6);
} }

1
app/android/src/com/introlab/rtabmap/RTABMapLib.java
View File

@@ -75,6 +75,7 @@ public class RTABMapLib
public static native void setAppendMode(boolean enabled); public static native void setAppendMode(boolean enabled);
public static native void setDataRecorderMode(boolean enabled); public static native void setDataRecorderMode(boolean enabled);
public static native void setMaxCloudDepth(float value); 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 setPointSize(float value);
public static native void setLighting(boolean enabled); public static native void setLighting(boolean enabled);
public static native void setBackfaceCulling(boolean enabled); public static native void setBackfaceCulling(boolean enabled);

22
app/android/src/com/introlab/rtabmap/SettingsActivity.java
View File

@@ -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_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_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_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_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)); ((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 (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_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_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_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)); if(key.compareTo(getString(R.string.pref_key_triangle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_triangle));

4
corelib/include/rtabmap/core/GainCompensator.h
View File

@@ -43,7 +43,7 @@ namespace rtabmap {
*/ */
class RTABMAP_EXP GainCompensator { class RTABMAP_EXP GainCompensator {
public: 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(); virtual ~GainCompensator();
void feed( void feed(
@@ -86,7 +86,7 @@ public:
int id, int id,
cv::Mat & image) const; 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; int getIndex(int id) const;
private: private:

16
corelib/src/GainCompensator.cpp
View File

@@ -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()); 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); return gains_(idToIndex_.at(id), 0);
} }