i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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
app/android/AndroidManifest.xml.in
View File

@@ -2,7 +2,7 @@
<!-- BEGIN_INCLUDE(manifest) -->
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
package="com.introlab.rtabmap"
android:versionCode="47"
android:versionCode="48"
android:versionName="@RTABMAP_VERSION@">
<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)
ENDIF(DISABLE_LOG)
MESSAGE(STATUS "DISABLE_LOG = ${DISABLE_LOG}")
IF(DISABLE_LOG)
SET(ANDROID_DEBUGGABLE false)
ELSE()

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

@@ -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<pcl::PointXYZRGB>::Ptr cloud;
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())
{
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.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<pcl::PointXYZRGB>);
mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
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<pcl::PointXYZRGB>::Ptr cloud;
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
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<pcl::PointXYZRGB>::Ptr cloud;
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())
{
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<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;
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<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)
{
@@ -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<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
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<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
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)
{
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<int, Mesh>::iterator jter = createdMeshes_.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> 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; i<cloudWithNormals->size(); ++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<int, Mesh>::iterator jter=createdMeshes_.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::IndicesPtr indices(new std::vector<int>);
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; i<transformedCloud->size(); ++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])));
}
}

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

@@ -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_;

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

@@ -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)
{

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

@@ -44,35 +44,41 @@ PointCloudDrawable::PointCloudDrawable(
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::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<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());
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");

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

@@ -46,7 +46,9 @@ class PointCloudDrawable {
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::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<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 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_;

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

@@ -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<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
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;
void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & 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;}

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

@@ -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

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

@@ -151,9 +151,10 @@ public:
cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
normals(new pcl::PointCloud<pcl::Normal>),
indices(new std::vector<int>),
visible(true),
gain(1.0f)
{}
visible(true)
{
gains[0] = gains[1] = gains[2] = 1.0f;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
pcl::PointCloud<pcl::Normal>::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<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
#else

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

@@ -16,6 +16,13 @@
android:entries="@array/pref_depth_keys"
android:entryValues="@array/pref_depth_values"
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
android:key="@string/pref_key_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_density">pref_key_density</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_default_depth">2.5</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_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_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_summary_depth">Points over the maximum depth are not rendered.</string>
<string name="pref_title_point_size">Point Size</string>
@@ -150,17 +154,39 @@
<item>"2"</item>
<item>"3"</item>
</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">
<item>"No Limit"</item>
<item>"5"</item>
<item>"4.5"</item>
<item>"4"</item>
<item>"3.5"</item>
<item>"3"</item>
<item>"2.5"</item>
<item>"2"</item>
<item>"1.5"</item>
<item>"1"</item>
<item>"5 m"</item>
<item>"4.5 m"</item>
<item>"4 m"</item>
<item>"3.5 m"</item>
<item>"3 m"</item>
<item>"2.5 m"</item>
<item>"2 m"</item>
<item>"1.5 m"</item>
<item>"1 m"</item>
</string-array>
<string-array name="pref_depth_values">
<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...");
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);
}

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 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);

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_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));