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

Tango: 0.12.0 (performance optimization, time and memory)

Browse Source
This commit is contained in:
matlabbe
2017-02-22 09:48:45 -05:00
parent 595ac49552
commit a9b03bb39e
19 changed files with 1238 additions and 625 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
app/android/jni/CameraTango.cpp
View File

@@ -40,7 +40,7 @@ namespace rtabmap {
const int kVersionStringLength = 128;
const int holeSize = 5;
const float maxDepthError = 0.10;
const int scanDownsampling = 10;
const int scanDownsampling = 1;
// Callbacks
void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
@@ -97,7 +97,10 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
//////////////////////////////
// CameraTango
//////////////////////////////
CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan) :
const float CameraTango::bilateralFilteringSigmaS = 2.0f;
const float CameraTango::bilateralFilteringSigmaR = 0.075f;
CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing) :
Camera(0),
tango_config_(0),
firstFrame_(true),
@@ -105,6 +108,7 @@ CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan)
decimation_(decimation),
autoExposure_(autoExposure),
rawScanPublished_(publishRawScan),
smoothing_(smoothing),
cloudStamp_(0),
tangoColorType_(0),
tangoColorStamp_(0),
@@ -599,23 +603,41 @@ SensorData CameraTango::captureImage(CameraInfo * info)
scanData.at(oi++) = pt;
}
int pixel_x, pixel_y;
int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
// get the coordinate on image plane.
pixel_x = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
pixel_y = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
unsigned short depth_value(pt.z * 1000.0f);
if(pixel_x>=0 && pixel_x<depth.cols &&
pixel_y>0 && pixel_y<depth.rows &&
bool pixelSet = false;
if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
pixel_y_l>0 && pixel_y_l<depth.rows &&
depth_value)
{
unsigned short & depthPixel = depth.at<unsigned short>(pixel_y, pixel_x);
unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
if(depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
pixelsSet += 1;
pixelSet = true;
}
}
if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
pixel_y_h>0 && pixel_y_h<depth.rows &&
depth_value)
{
unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
if(depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
pixelSet = true;
}
}
if(pixelSet)
{
pixelsSet += 1;
}
}
if(oi)
@@ -690,6 +712,14 @@ SensorData CameraTango::captureImage(CameraInfo * info)
model.setImageSize(sizet);
}
if(smoothing_)
{
//UTimer t;
depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
data.setDepthOrRightRaw(depth);
//LOGD("Bilateral filtering, time=%fs", t.ticks());
}
if(rawScanPublished_)
{
data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);

8
app/android/jni/CameraTango.h
View File

@@ -71,7 +71,11 @@ private:
class CameraTango : public Camera, public UThread, public UEventsSender {
public:
CameraTango(int decimation, bool autoExposure, bool publishRawScan);
static const float bilateralFilteringSigmaS;
static const float bilateralFilteringSigmaR;
public:
CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing);
virtual ~CameraTango();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -81,6 +85,7 @@ public:
const CameraModel & getCameraModel() const {return model_;}
rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
void setDecimation(int value) {decimation_ = value;}
void setSmoothing(bool enabled) {smoothing_ = enabled;}
void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
@@ -107,6 +112,7 @@ private:
int decimation_;
bool autoExposure_;
bool rawScanPublished_;
bool smoothing_;
cv::Mat cloud_;
double cloudStamp_;
cv::Mat tangoColor_;

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

@@ -148,6 +148,7 @@ RTABMapApp::RTABMapApp() :
trajectoryMode_(false),
autoExposure_(true),
rawScanSaved_(false),
smoothing_(true),
fullResolution_(false),
appendMode_(true),
maxCloudDepth_(0.0),
@@ -168,6 +169,8 @@ RTABMapApp::RTABMapApp() :
totalPolygons_(0),
lastDrawnCloudsCount_(0),
renderingTime_(0.0f),
processMemoryUsedBytes(0),
processGPUMemoryUsedBytes(0),
visualizingMesh_(false),
exportedMeshUpdated_(false),
exportedMesh_(new pcl::TextureMesh),
@@ -206,6 +209,8 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingTime_ = 0.0f;
processMemoryUsedBytes = 0;
processGPUMemoryUsedBytes = 0;
if(camera_)
{
@@ -227,7 +232,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
this->registerToEventsManager();
camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_, rawScanSaved_);
camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_, rawScanSaved_, smoothing_);
}
void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
@@ -563,7 +568,7 @@ int RTABMapApp::Render()
renderingTime_ = fpsTime.elapsed();
}
return notifyCameraStarted;
return notifyCameraStarted?1:0;
}
else
{
@@ -613,6 +618,8 @@ int RTABMapApp::Render()
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingTime_ = 0.0f;
processMemoryUsedBytes = 0;
processGPUMemoryUsedBytes = 0;
}
// Did we lose OpenGL context? If so, recreate the context;
@@ -622,6 +629,7 @@ int RTABMapApp::Render()
boost::mutex::scoped_lock lock(meshesMutex_);
if(added.size() != createdMeshes_.size())
{
processGPUMemoryUsedBytes = 0;
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(!main_scene_.hasCloud(iter->first))
@@ -633,10 +641,24 @@ int RTABMapApp::Render()
cv::Mat texture;
if(main_scene_.isMeshTexturing())
{
texture = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
cv::Mat textureRaw;
textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
if(!textureRaw.empty())
{
cv::Size reducedSize(textureRaw.cols/(textureRaw.cols>1000?4:2), textureRaw.rows/(textureRaw.cols>1000?4:2));
LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
cv::resize(textureRaw, texture, reducedSize, 0, 0, CV_INTER_AREA);
}
}
main_scene_.addMesh(iter->first, iter->second, texture, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.setCloudVisible(iter->first, iter->second.visible);
long estimateGPUMem = 0;
estimateGPUMem += iter->second.cloud->size()*16; // 3*float + 1 float rgb
estimateGPUMem += iter->second.indices->size()*4; // int
estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon
processGPUMemoryUsedBytes += estimateGPUMem + (texture.empty()?0:iter->second.polygons.size()*3*8+texture.total());
}
}
}
@@ -659,11 +681,16 @@ int RTABMapApp::Render()
{
for(std::map<int, rtabmap::Signature>::const_iterator jter=iter->getSignatures().begin(); jter!=iter->getSignatures().end(); ++jter)
{
bool dataDetected = false;
if(!jter->second.sensorData().imageRaw().empty() &&
!jter->second.sensorData().depthRaw().empty())
{
uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData()));
uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
if(!localizationMode_)
{
uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData()));
uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
dataDetected = true;
}
}
else if(totalPoints_ == 0 &&
!jter->second.sensorData().imageCompressed().empty() &&
@@ -676,6 +703,19 @@ int RTABMapApp::Render()
LOGI("Detecting that we are loading a database");
}
notifyDataLoaded = true;
dataDetected = true;
}
if(dataDetected)
{
processMemoryUsedBytes += jter->second.sensorData().imageCompressed().total();
processMemoryUsedBytes += jter->second.sensorData().depthOrRightCompressed().total();
processMemoryUsedBytes += jter->second.sensorData().laserScanCompressed().total();
processMemoryUsedBytes += jter->second.getWords().size()*4*8;
processMemoryUsedBytes += jter->second.getWords3().size()*4*4;
if(!jter->second.getWordsDescriptors().empty())
{
processMemoryUsedBytes += jter->second.getWordsDescriptors().size()*(4+jter->second.getWordsDescriptors().begin()->second.total());
}
}
}
}
@@ -755,15 +795,6 @@ int RTABMapApp::Render()
cv::Mat tmpA, depth;
data.uncompressData(&tmpA, &depth);
if(notifyDataLoaded && optimizeOpenedDatabase_)
{
// do post-processing bilateral filtering now
UTimer t;
depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
data.setDepthOrRightRaw(depth);
LOGI("Bilateral filtering of %d, time=%fs", id, t.ticks());
}
if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
// Voxelize and filter depending on the previous cloud?
@@ -795,7 +826,22 @@ int RTABMapApp::Render()
inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
main_scene_.addMesh(id, inserted.first->second, main_scene_.isMeshTexturing()?data.imageRaw():cv::Mat(), iter->second);
cv::Mat texture;
if(main_scene_.isMeshTexturing())
{
cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
LOGD("resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
cv::resize(data.imageRaw(), texture, reducedSize, 0, 0, CV_INTER_AREA);
}
main_scene_.addMesh(id, inserted.first->second, texture, iter->second);
long estimateCPUMem = 0;
estimateCPUMem += inserted.first->second.cloud->size()*16; // 3*float + 1 float rgb
estimateCPUMem += inserted.first->second.indices->size()*4; // int
estimateCPUMem += inserted.first->second.polygons.size()*4*3; // 3 indices per polygon
processMemoryUsedBytes += estimateCPUMem;
processGPUMemoryUsedBytes += estimateCPUMem + (texture.empty()?0:inserted.first->second.polygons.size()*3*8+texture.total());
}
else
{
@@ -1166,6 +1212,18 @@ void RTABMapApp::setFullResolution(bool enabled)
}
}
void RTABMapApp::setSmoothing(bool enabled)
{
if(smoothing_ != enabled)
{
smoothing_ = enabled;
if(camera_)
{
camera_->setSmoothing(smoothing_);
}
}
}
void RTABMapApp::setAppendMode(bool enabled)
{
if(appendMode_ != enabled)
@@ -1464,6 +1522,7 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
bool RTABMapApp::exportMesh(
const std::string & filePath,
float cloudVoxelSize,
bool regenerateCloud,
bool meshing,
int textureSize,
int normalK,
@@ -2118,18 +2177,34 @@ bool RTABMapApp::exportMesh(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::IndicesPtr indices(new std::vector<int>);
float gain = 1.0f;
if(jter != createdMeshes_.end())
{
cloud = jter->second.cloud;
indices = jter->second.indices;
gain = jter->second.gain;
}
else
if(regenerateCloud)
{
if(jter != createdMeshes_.end())
{
gain = jter->second.gain;
}
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());
// full resolution
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get());
}
}
else
{
if(jter != createdMeshes_.end())
{
cloud = jter->second.cloud;
indices = jter->second.indices;
gain = jter->second.gain;
}
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());
}
}
}
if(cloud->size() && indices->size())
@@ -2317,7 +2392,7 @@ int RTABMapApp::postProcessing(int approach)
}
// bilateral filtering
if(approach == -1 || approach == 7)
if(approach == 7)
{
bilateralFilteringOnNextRender_ = true;
}
@@ -2467,7 +2542,7 @@ void RTABMapApp::handleEvent(UEvent * event)
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFF)V" );
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFF)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
@@ -2478,6 +2553,7 @@ void RTABMapApp::handleEvent(UEvent * event)
updateTime,
loopClosureId,
highestHypId,
(int)((processMemoryUsedBytes+processGPUMemoryUsedBytes)/(1024*1024)),
databaseMemoryUsed,
inliers,
matches,

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

@@ -34,9 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <tango_client_api.h> // NOLINT
#include <tango-gl/util.h>
#include <scene.h>
#include <CameraTango.h>
#include <util.h>
#include "scene.h"
#include "CameraTango.h"
#include "util.h"
#include <rtabmap/core/RtabmapThread.h>
#include <rtabmap/utilite/UEventsHandler.h>
@@ -128,6 +128,7 @@ class RTABMapApp : public UEventsHandler {
void setAutoExposure(bool enabled);
void setRawScanSaved(bool enabled);
void setFullResolution(bool enabled);
void setSmoothing(bool enabled);
void setAppendMode(bool enabled);
void setDataRecorderMode(bool enabled);
void setMaxCloudDepth(float value);
@@ -144,6 +145,7 @@ class RTABMapApp : public UEventsHandler {
bool exportMesh(
const std::string & filePath,
float cloudVoxelSize,
bool regenerateCloud,
bool meshing,
int textureSize,
int normalK,
@@ -179,6 +181,7 @@ class RTABMapApp : public UEventsHandler {
bool trajectoryMode_;
bool autoExposure_;
bool rawScanSaved_;
bool smoothing_;
bool fullResolution_;
bool appendMode_;
float maxCloudDepth_;
@@ -202,6 +205,8 @@ class RTABMapApp : public UEventsHandler {
int totalPolygons_;
int lastDrawnCloudsCount_;
float renderingTime_;
long processMemoryUsedBytes;
long processGPUMemoryUsedBytes;
bool visualizingMesh_;
bool exportedMeshUpdated_;

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

@@ -217,6 +217,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
return app.setFullResolution(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
JNIEnv*, jobject, bool enabled)
{
return app.setSmoothing(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
JNIEnv*, jobject, bool enabled)
{
@@ -295,6 +301,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
JNIEnv* env, jobject,
jstring filePath,
float cloudVoxelSize,
bool regenerateCloud,
bool meshing,
int textureSize,
int normalK,
@@ -313,6 +320,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
return app.exportMesh(
filePathC,
cloudVoxelSize,
regenerateCloud,
meshing,
textureSize,
normalK,

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

@@ -226,7 +226,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
}
}
LOGD("Creating cloud buffer %d", vertex_buffers_);
//LOGD("Creating cloud buffer %d", vertex_buffers_);
std::vector<float> vertices;
int totalPoints = 0;
std::vector<pcl::Vertices> polygons = mesh.polygons;
@@ -238,7 +238,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
totalPoints = mesh.indices->size();
if(textures_ && polygons.size())
{
LOGD("Organized mesh with texture");
//LOGD("Organized mesh with texture");
int items = hasNormals_?9:6;
vertices = std::vector<float>(mesh.indices->size()*9);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
@@ -268,7 +268,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
}
else
{
LOGD("Organized mesh");
//LOGD("Organized mesh");
int items = hasNormals_?7:4;
vertices = std::vector<float>(mesh.indices->size()*items);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
@@ -295,8 +295,8 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
totalPoints = mesh.cloud->size();
if(textures_ && polygons.size() && mesh.normals->size())
{
LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
// (int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
// Texturing issue:
// tex_coordinates should be linked to points, not
@@ -355,7 +355,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
}
else
{
LOGD("Dense mesh");
//LOGD("Dense mesh");
int items = hasNormals_?7:4;
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->size(), -1);
vertices = std::vector<float>(mesh.cloud->size()*items);
@@ -392,9 +392,12 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
if(textures_ && textureUpdate)
{
GLint maxTextureSize = 0;
glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
LOGI("maxTextureSize=%d", maxTextureSize);
//GLint maxTextureSize = 0;
//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
//LOGI("maxTextureSize=%d", maxTextureSize);
//GLint maxTextureUnits = 0;
//glGetIntegerv(GL_MAX_TEXTURE_IMAGE_UNITS, &maxTextureUnits);
//LOGW("maxTextureUnits=%d", maxTextureUnits);
// gen texture from image
glBindTexture(GL_TEXTURE_2D, textures_);
@@ -402,6 +405,11 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
cv::Mat rgbImage;
cv::cvtColor(texture, rgbImage, CV_BGR2RGB);
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
//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);
GLint error = glGetError();