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 #57: Added 720p option, Export PLY or OBJ, Added Rendering and Mapping menus, RtabmapThread: Fixed large covariance (9999) detection

Browse Source
This commit is contained in:
matlabbe
2016-04-02 15:17:40 -04:00
parent d489cd48e9
commit 30e52b785a
15 changed files with 339 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -99,8 +99,12 @@ static rtabmap::Transform opticalRotation(
CameraTango::CameraTango(int decimation, bool autoExposure) :
Camera(0, opticalRotation),
tango_config_(0),
firstFrame_(true),
decimation_(decimation),
autoExposure_(autoExposure)
autoExposure_(autoExposure),
cloudStamp_(0),
tangoColorType_(0),
tangoColorStamp_(0)
{
UASSERT(decimation >= 1);
}
@@ -318,6 +322,7 @@ void CameraTango::close()
tango_config_ = nullptr;
TangoService_disconnect();
}
firstFrame_ = true;
}
void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -624,7 +629,9 @@ void CameraTango::mainLoop()
{
rtabmap::Transform pose = data.groundTruth();
data.setGroundTruth(Transform());
this->post(new OdometryEvent(data, pose, 0.0001, 0.0001));
LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
firstFrame_ = false;
}
else if(!this->isKilled())
{

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

@@ -77,6 +77,7 @@ public:
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const;
void setDecimation(int value) {decimation_ = value;}
void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
void cloudReceived(const cv::Mat & cloud, double timestamp);
@@ -95,6 +96,7 @@ private:
private:
void * tango_config_;
bool firstFrame_;
int decimation_;
bool autoExposure_;
cv::Mat cloud_;

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

@@ -49,18 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/io/obj_io.h>
const int kVersionStringLength = 128;
const int cameraTangoDecimation = 2;
const int renderingCloudDecimation = 8;
const float renderingCloudMaxDepth = 4.0f;
const int maxFeatures = 400;
const float meshAngleTolerance = 0.1745; // 10 degrees
const int meshTrianglePixels = 1;
const bool substractFiltering = false;
const float subtractRadius = 0.02;
const float subtractMaxAngle = M_PI/4.0f;
const bool textureMeshing = true;
const int minNeighborsInRadius = 5;
const float closeVerticesDistance = 0.02f;
static JavaVM *jvm;
static jobject RTABMapActivity = 0;
@@ -69,34 +59,22 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
{
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), std::string("6"))); // GFTT/BRIEF
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTMinDistance(), std::string("10")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTMinDistance(), std::string(fullResolution_?"15":"5")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), std::string("1")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImageKept(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemRawDescriptorsKept(), "true")); // for visual registration
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), std::string("1"))); // Kd-tree
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNndrRatio(), std::string("0.8"))); // set the one for kd-tree
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), !loopClosureDetection_?std::string("-1"):uNumber2Str(maxFeatures))); // Kd-tree
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), uNumber2Str(graphOptimization_?rtabmap::Parameters::defaultOptimizerIterations():0)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
//parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImageDecimation(), std::string("2")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("1")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerRobust(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerVarianceIgnored(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("700")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisRefineIterations(), std::string("5")));
return parameters;
}
@@ -107,16 +85,18 @@ RTABMapApp::RTABMapApp() :
logHandler_(0),
mapCloudShown_(true),
odomCloudShown_(true),
loopClosureDetection_(true),
graphOptimization_(true),
localizationMode_(false),
trajectoryMode_(false),
autoExposure_(false),
fullResolution_(false),
maxCloudDepth_(0.0),
clearSceneOnNextRender_(false),
totalPoints_(0),
totalPolygons_(0),
lastDrawnCloudsCount_(0)
{
}
RTABMapApp::~RTABMapApp() {
@@ -143,9 +123,6 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
LOGI("RTABMapApp::TangoInitialize()");
createdMeshes_.clear();
previousCloud_.first = 0;
previousCloud_.second.first.reset();
previousCloud_.second.second.reset();
rawPoses_.clear();
clearSceneOnNextRender_ = true;
totalPoints_ = 0;
@@ -172,7 +149,7 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
this->registerToEventsManager();
camera_ = new rtabmap::CameraTango(cameraTangoDecimation, autoExposure_);
camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_);
// The first thing we need to do for any Tango enabled application is to
@@ -300,9 +277,6 @@ int RTABMapApp::Render()
main_scene_.clear();
clearSceneOnNextRender_ = false;
createdMeshes_.clear();
previousCloud_.first = 0;
previousCloud_.second.first.reset();
previousCloud_.second.second.reset();
rawPoses_.clear();
totalPoints_ = 0;
totalPolygons_ = 0;
@@ -410,85 +384,37 @@ int RTABMapApp::Render()
if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows);
cloudWithoutNormals = rtabmap::util3d::cloudRGBFromSensorData(data, renderingCloudDecimation, renderingCloudMaxDepth, 0, 0, indices.get());
//compute normals
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::computeNormals(cloudWithoutNormals, 6);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, 0, indices.get());
if(cloud->size() && indices->size())
{
UTimer time;
// substract? set points to NaN which are over previous cloud
pcl::IndicesPtr indicesKept = indices;
if(substractFiltering &&
subtractRadius > 0.0 &&
indices->size() &&
previousCloud_.first > 0 &&
previousCloud_.second.first.get() != 0 &&
previousCloud_.second.second.get() != 0 &&
previousCloud_.second.second->size() &&
poses.find(previousCloud_.first) != poses.end())
{
rtabmap::Transform t = iter->second.inverse() * poses.at(previousCloud_.first);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(previousCloud_.second.first, t);
indicesKept = rtabmap::util3d::subtractFiltering(
cloud,
indices,
previousCloud,
previousCloud_.second.second,
subtractRadius,
subtractMaxAngle,
minNeighborsInRadius);
UINFO("Subtraction %fs", time.ticks());
}
previousCloud_.first = id;
previousCloud_.second.first = cloud;
previousCloud_.second.second = indices;
// pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::ExtractIndices<pcl::PointXYZRGBNormal> filter;
filter.setIndices(indicesKept);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ExtractIndices<pcl::PointXYZRGB> filter;
filter.setIndices(indices);
filter.setKeepOrganized(true);
filter.setInputCloud(cloud);
filter.filter(*output);
LOGE("Filtering %d from %d -> %d (%fs)", (int)indices->size(), (int)indices->size(), (int)indicesKept->size(), time.ticks());
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleTolerance, false, meshTrianglePixels);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
if(!textureMeshing)
{
rtabmap::util3d::filterNotUsedVerticesFromMesh(
*output,
polygons,
*outputCloud,
outputPolygons);
}
else
{
outputCloud = output;
outputPolygons = polygons;
}
outputCloud = output;
outputPolygons = polygons;
LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
if(outputCloud->size() && (!textureMeshing || outputPolygons.size()))
if(outputCloud->size() && outputPolygons.size())
{
totalPolygons_ += outputPolygons.size();
if(textureMeshing)
{
main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
}
else
{
main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second);
}
main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
// protect createdMeshes_ used also by exportMesh() method
@@ -498,10 +424,7 @@ int RTABMapApp::Render()
inserted.first->second.cloud = outputCloud;
inserted.first->second.polygons = outputPolygons;
inserted.first->second.pose = iter->second;
if(textureMeshing)
{
inserted.first->second.texture = data.imageCompressed();
}
inserted.first->second.texture = data.imageCompressed();
}
else
{
@@ -554,11 +477,11 @@ int RTABMapApp::Render()
event.data().imageRaw().cols, event.data().imageRaw().rows,
event.data().depthRaw().cols, event.data().depthRaw().rows);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), renderingCloudDecimation, renderingCloudMaxDepth);
cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
if(cloud->size())
{
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleTolerance, false, meshTrianglePixels);
main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), textureMeshing?event.data().imageRaw():cv::Mat());
main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
main_scene_.setCloudVisible(-1, true);
}
else
@@ -655,7 +578,39 @@ void RTABMapApp::setAutoExposure(bool enabled)
camera_->setAutoExposure(autoExposure_);
onResume();
}
resetMapping();
}
}
void RTABMapApp::setFullResolution(bool enabled)
{
if(fullResolution_ != enabled)
{
fullResolution_ = enabled;
if(camera_)
{
camera_->setDecimation(fullResolution_?1:2);
}
}
}
void RTABMapApp::setMaxCloudDepth(float value)
{
maxCloudDepth_ = value;
}
int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
{
if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end())
{
LOGI(uFormat("Setting param \"%s\" to \"\"", key.c_str(), value.c_str()).c_str());
uInsert(mappingParameters_, rtabmap::ParametersPair(key, value));
UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
return 0;
}
else
{
LOGE(uFormat("Key \"%s\" doesn't exist!", key.c_str()).c_str());
return -1;
}
}
@@ -680,7 +635,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
bool success = false;
//Assemble the meshes
if(textureMeshing)
if(UFile::getExtension(filePath).compare("obj") == 0)
{
pcl::TextureMesh textureMesh;
std::vector<cv::Mat> textures;
@@ -705,12 +660,16 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter->second.cloud->size() &&
iter->second.polygons.size())
{
// OBJ format requires normals
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
cloudWithNormals = rtabmap::util3d::computeNormals(iter->second.cloud, 20);
// create dense cloud
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector<pcl::Vertices> densePolygons;
std::map<int, int> newToOldIndices;
newToOldIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(
*iter->second.cloud,
*cloudWithNormals,
iter->second.polygons,
*denseCloud,
densePolygons);
@@ -789,7 +748,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
}
else
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> mergedPolygons;
{
@@ -799,23 +758,16 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter!= createdMeshes_.end();
++iter)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> densePolygons;
if(iter->second.cloud->is_dense)
{
denseCloud = iter->second.cloud;
densePolygons = iter->second.polygons;
}
else
{
rtabmap::util3d::filterNotUsedVerticesFromMesh(
*iter->second.cloud,
iter->second.polygons,
*denseCloud,
densePolygons);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
rtabmap::util3d::filterNotUsedVerticesFromMesh(
*iter->second.cloud,
iter->second.polygons,
*denseCloud,
densePolygons);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
if(mergedClouds->size() == 0)
{
*mergedClouds = *transformedCloud;
@@ -827,30 +779,6 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
}
}
}
if(closeVerticesDistance)
{
UINFO("Filtering assembled mesh (points=%d, polygons=%d, close vertices=%fm)...",
(int)mergedClouds->size(), (int)mergedPolygons.size(), closeVerticesDistance);
mergedPolygons = rtabmap::util3d::filterCloseVerticesFromMesh(
mergedClouds,
mergedPolygons,
closeVerticesDistance,
M_PI/4,
true);
// filter invalid polygons
unsigned int count = mergedPolygons.size();
mergedPolygons = rtabmap::util3d::filterInvalidPolygons(mergedPolygons);
UINFO("Filtered %d invalid polygons.", (int)count-mergedPolygons.size());
// filter not used vertices
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector<pcl::Vertices> filteredPolygons;
rtabmap::util3d::filterNotUsedVerticesFromMesh(*mergedClouds, mergedPolygons, *filteredCloud, filteredPolygons);
mergedClouds = filteredCloud;
mergedPolygons = filteredPolygons;
}
if(mergedClouds->size() && mergedPolygons.size())
{

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

@@ -121,6 +121,9 @@ class RTABMapApp : public UEventsHandler {
void setGraphOptimization(bool enabled);
void setGraphVisible(bool visible);
void setAutoExposure(bool enabled);
void setFullResolution(bool enabled);
void setMaxCloudDepth(float value);
int setMappingParameter(const std::string & key, const std::string & value);
void resetMapping();
void save();
@@ -139,11 +142,15 @@ class RTABMapApp : public UEventsHandler {
bool mapCloudShown_;
bool odomCloudShown_;
bool loopClosureDetection_;
bool graphOptimization_;
bool localizationMode_;
bool trajectoryMode_;
bool autoExposure_;
bool fullResolution_;
float maxCloudDepth_;
rtabmap::ParametersMap mappingParameters_;
bool clearSceneOnNextRender_;
int totalPoints_;
@@ -165,7 +172,7 @@ class RTABMapApp : public UEventsHandler {
struct Mesh
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
std::vector<pcl::Vertices> polygons;
rtabmap::Transform pose;
cv::Mat texture;
@@ -173,7 +180,6 @@ class RTABMapApp : public UEventsHandler {
std::map<int, Mesh> createdMeshes_;
std::map<int, rtabmap::Transform> rawPoses_;
std::pair<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > previousCloud_;
std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
};

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

@@ -167,6 +167,27 @@ Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
{
return app.setAutoExposure(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
JNIEnv*, jobject, bool enabled)
{
return app.setFullResolution(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
JNIEnv*, jobject, float value)
{
return app.setMaxCloudDepth(value);
}
JNIEXPORT jint JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
JNIEnv* env, jobject, jstring key, jstring value)
{
std::string keyC, valueC;
GetJStringContent(env,key,keyC);
GetJStringContent(env,value,valueC);
return app.setMappingParameter(keyC, valueC);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_resetMapping(

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

@@ -35,124 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <GLES2/gl2.h>
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons,
const cv::Mat & image) :
vertex_buffers_(0),
textures_(0),
nPoints_(0),
pose_(1.0f),
visible_(true),
shader_program_(cloudShaderProgram!=0?cloudShaderProgram:textureShaderProgram)
{
UASSERT(!cloud->empty());
glGenBuffers(1, &vertex_buffers_);
if(!vertex_buffers_)
{
LOGE("OpenGL: could not generate vertex buffers\n");
return;
}
if(textureShaderProgram)
{
LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
UASSERT_MSG(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3,
uFormat("polygons=%d dense=%d image=%d", (int)polygons.size(), cloud->is_dense?1:0, image.type()).c_str());
glGenTextures(1, &textures_);
if(!textures_)
{
vertex_buffers_ = 0;
LOGE("OpenGL: could not generate vertex buffers\n");
return;
}
}
LOGI("Creating cloud buffer %d", vertex_buffers_);
std::vector<float> vertices;
if(textures_)
{
vertices = std::vector<float>(cloud->size()*5);
for(unsigned int i=0; i<cloud->size(); ++i)
{
vertices[i*5] = cloud->at(i).x;
vertices[i*5+1] = cloud->at(i).y;
vertices[i*5+2] = cloud->at(i).z;
// texture uv
vertices[i*5+3] = float(i % cloud->width)/float(cloud->width); //u
vertices[i*5+4] = float(i/cloud->width)/float(cloud->height); //v
}
}
else
{
vertices = std::vector<float>(cloud->size()*4);
for(unsigned int i=0; i<cloud->size(); ++i)
{
vertices[i*4] = cloud->at(i).x;
vertices[i*4+1] = cloud->at(i).y;
vertices[i*4+2] = cloud->at(i).z;
vertices[i*4+3] = cloud->at(i).rgb;
}
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
glBindBuffer(GL_ARRAY_BUFFER, 0);
GLint error = glGetError();
if(error != GL_NO_ERROR)
{
LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
vertex_buffers_ = 0;
return;
}
if(textures_)
{
// gen texture from image
glBindTexture(GL_TEXTURE_2D, textures_);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
cv::Mat rgbImage;
cv::cvtColor(image, rgbImage, CV_BGR2RGB);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, (const void *)rgbImage.data);
glBindTexture(GL_TEXTURE_2D, 0);
GLint error = glGetError();
if(error != GL_NO_ERROR)
{
LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
textures_ = 0;
glDeleteBuffers(1, &vertex_buffers_);
vertex_buffers_ = 0;
return;
}
}
nPoints_ = cloud->size();
if(polygons.size())
{
int polygonSize = polygons[0].vertices.size();
UASSERT(polygonSize == 3);
polygons_.resize(polygons.size() * polygonSize);
int oi = 0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
UASSERT((int)polygons[i].vertices.size() == polygonSize);
for(int j=0; j<polygonSize; ++j)
{
polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
}
}
}
}
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,

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

@@ -41,12 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// PointCloudDrawable is responsible for the point cloud rendering.
class PointCloudDrawable {
public:
PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(),
const cv::Mat & image = cv::Mat());
PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,

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

@@ -402,32 +402,6 @@ void Scene::setTraceVisible(bool visible)
}
//Should only be called in OpenGL thread!
void Scene::addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons,
const rtabmap::Transform & pose,
const cv::Mat & image)
{
LOGI("addOrUpdateCloud cloud %d", id);
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
delete iter->second;
pointClouds_.erase(iter);
}
//create
UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
PointCloudDrawable * drawable = new PointCloudDrawable(
image.empty()?cloud_shader_program_:0,
image.empty()?0:texture_mesh_shader_program_,
cloud,
polygons,
image);
drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable));
}
void Scene::addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -446,8 +420,8 @@ void Scene::addCloud(
//create
UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
PointCloudDrawable * drawable = new PointCloudDrawable(
image.empty()?cloud_shader_program_:0,
image.empty()?0:texture_mesh_shader_program_,
cloud->is_dense || image.empty()?cloud_shader_program_:0,
cloud->is_dense || image.empty()?0:texture_mesh_shader_program_,
cloud,
polygons,
image);

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

@@ -96,12 +96,6 @@ class Scene {
void setGraphVisible(bool visible);
void setTraceVisible(bool visible);
void addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons,
const rtabmap::Transform & pose,
const cv::Mat & image = cv::Mat());
void addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,

32
app/android/res/menu/optionmenu.xml
View File

@@ -8,24 +8,42 @@
<group android:id="@+id/group_actions">
<item android:id="@+id/open" android:title="Open"/>
<item android:id="@+id/save" android:title="Save"/>
<item android:id="@+id/export" android:title="Export (*.ply)"/>
<item android:id="@+id/export" android:title="Export...">
<menu>
<group android:id="@+id/group_export">
<item android:id="@+id/export_ply" android:title="Mesh (.ply)" />
<item android:id="@+id/export_obj" android:title="Mesh with texture (*.obj)" />
</group>
</menu>
</item>
<item android:id="@+id/reset" android:title="Reset"/>
<item android:id="@+id/about" android:title="About"/>
</group>
<item android:id="@+id/menu_settings" android:title="Options..." android:orderInCategory="2">
<item android:id="@+id/menu_rendering_settings" android:title="Rendering Options..." android:orderInCategory="2">
<menu >
<group android:id="@+id/group_visibility" android:checkableBehavior="all">
<group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
<item android:id="@+id/debug" android:checked="false" android:title="Debug" />
<item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
<item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
<item android:id="@+id/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
<item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
<item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
<item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
<item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
<item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
<item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />
</group>
</menu>
</item>
<item android:id="@+id/menu_mapping_settings" android:title="Mapping Options..." android:orderInCategory="3">
<menu >
<group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all">
<item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
<item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
<item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
<item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
<item android:id="@+id/update_rate" android:checkable="false" android:title="Map Update Rate..." />
<item android:id="@+id/time_threshold" android:checkable="false" android:title="Time Threshold..." />
<item android:id="@+id/features" android:checkable="false" android:title="Max Features Extracted..." />
<item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
</group>
</menu>
</item>
</menu>

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

@@ -75,6 +75,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private String mNewDatabasePath = "";
private String mWorkingDirectory = "";
private int mMaxDepthIndex = 5;
private int mParamUpdateRateHzIndex = 1;
private int mParamTimeThrMsIndex = 1;
private int mParamMaxFeaturesIndex = 2;
private LinearLayout mLayoutDebug;
private int mTotalLoopClosures = 0;
@@ -627,6 +633,96 @@ public class RTABMapActivity extends Activity implements OnClickListener {
item.setChecked(!item.isChecked());
RTABMapLib.setAutoExposure(item.isChecked());
}
else if(itemId == R.id.resolution)
{
item.setChecked(!item.isChecked());
RTABMapLib.setFullResolution(item.isChecked());
}
else if(itemId == R.id.max_depth)
{
// get double
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("Max Depth (m)");
final String[] values = {"1", "2", "3", "4", "5", "No Limit"};
builder.setSingleChoiceItems(values, mMaxDepthIndex, new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which) {
dialog.dismiss();
if(which >=0 && which < 6)
{
mMaxDepthIndex = which;
RTABMapLib.setMaxCloudDepth(which < 5?Integer.parseInt(values[which]):0);
}
}
});
builder.show();
}
else if(itemId == R.id.update_rate)
{
// get double
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("Update Rate (Hz)");
final String[] values = {"0.5", "1", "2", "Max"};
builder.setSingleChoiceItems(values, mParamUpdateRateHzIndex, new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which) {
dialog.dismiss();
if(which >=0 && which < 4)
{
mParamUpdateRateHzIndex = which;
if(RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", values[which]) != 0)
{
Toast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/DetectionRate\"!", Toast.LENGTH_LONG).show();
}
}
}
});
builder.show();
}
else if(itemId == R.id.time_threshold)
{
// get double
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("Time Threshold (ms, 0 means no limit)");
final String[] values = {"400", "700", "1400", "No Limit"};
builder.setSingleChoiceItems(values, mParamTimeThrMsIndex, new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which) {
dialog.dismiss();
if(which >=0 && which < 4)
{
mParamTimeThrMsIndex = which;
if(RTABMapLib.setMappingParameter("Rtabmap/TimeThr", which==3?"0":values[which]) != 0)
{
Toast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/TimeThr\"!", Toast.LENGTH_LONG).show();
}
}
}
});
builder.show();
}
else if(itemId == R.id.features)
{
// get double
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("Max Features");
final String[] values = {"Disabled", "100", "200", "300", "400", "No Limit"};
builder.setSingleChoiceItems(values, mParamMaxFeaturesIndex, new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which) {
dialog.dismiss();
if(which >=0 && which < 6)
{
mParamMaxFeaturesIndex = which;
if(RTABMapLib.setMappingParameter("Kp/MaxFeatures", which==0?"-1":which==5?"0":values[which]) != 0)
{
Toast.makeText(getActivity(),"Failed to set parameter \"Kp/MaxFeatures\"!", Toast.LENGTH_LONG).show();
}
}
}
});
builder.show();
}
else if (itemId == R.id.save)
{
if(mOpenedDatabasePath.isEmpty())
@@ -729,10 +825,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
RTABMapLib.openDatabase(mTempDatabasePath);
}
}
else if(itemId == R.id.export)
else if(itemId == R.id.export_obj || itemId == R.id.export_ply)
{
final String extension = itemId == R.id.export_ply ? ".ply" : ".obj";
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("File Name (*.obj):");
builder.setTitle(String.format("File Name (*%s):", extension));
final EditText input = new EditText(this);
input.setInputType(InputType.TYPE_CLASS_TEXT);
builder.setView(input);
@@ -744,7 +842,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
dialog.dismiss();
if(!fileName.isEmpty())
{
File newFile = new File(mWorkingDirectory + fileName + ".obj");
File newFile = new File(mWorkingDirectory + fileName + extension);
if(newFile.exists())
{
new AlertDialog.Builder(getActivity())
@@ -752,12 +850,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
.setMessage("Do you want to overwrite the existing file?")
.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
public void onClick(DialogInterface dialog, int which) {
final String path = mWorkingDirectory + fileName + ".obj";
final String path = mWorkingDirectory + fileName + extension;
mItemExport.setEnabled(false);
mProgressDialog.setTitle("Exporting");
mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+".obj"));
mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
mProgressDialog.show();
Thread exportThread = new Thread(new Runnable() {
@@ -791,10 +889,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
}
else
{
final String path = mWorkingDirectory + fileName + ".obj";
final String path = mWorkingDirectory + fileName + extension;
mItemExport.setEnabled(false);
mProgressDialog.setTitle("Exporting");
mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+".obj"));
mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
mProgressDialog.show();
Thread exportThread = new Thread(new Runnable() {
public void run() {

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

@@ -52,6 +52,9 @@ public class RTABMapLib
public static native void setGraphOptimization(boolean enabled);
public static native void setGraphVisible(boolean visible);
public static native void setAutoExposure(boolean enabled);
public static native void setFullResolution(boolean enabled);
public static native void setMaxCloudDepth(float value);
public static native int setMappingParameter(String key, String value);
public static native void resetMapping();
public static native void save();

10
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -78,6 +78,11 @@ void RTABMAP_EXP appendMesh(
std::vector<pcl::Vertices> & polygonsA,
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
const std::vector<pcl::Vertices> & polygonsB);
void RTABMAP_EXP appendMesh(
pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
std::vector<pcl::Vertices> & polygonsA,
const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
const std::vector<pcl::Vertices> & polygonsB);
// return map from new to old polygon indices
std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
@@ -85,6 +90,11 @@ std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons);
std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons);
std::vector<pcl::Vertices> RTABMAP_EXP filterCloseVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,

33
corelib/src/RtabmapThread.cpp
View File

@@ -544,14 +544,30 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
ignoreFrame = true;
}
}
if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && (odomEvent.pose().isIdentity() || odomEvent.info().variance>=9999))
if(_dataBufferMaxSize > 0 &&
((!lastPose_.isIdentity() && odomEvent.pose().isIdentity()) ||
odomEvent.info().variance>=9999 ||
odomEvent.rotVariance()>=9999 ||
odomEvent.transVariance()>=9999))
{
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomEvent.info().variance);
UWARN("Odometry is reset (identity pose or high variance (>=9999) detected). Increment map id!");
pushNewState(kStateTriggeringMap);
_rotVariance = 0;
_transVariance = 0;
}
double maxRotVar = odomEvent.rotVariance();
double maxTransVar = odomEvent.transVariance();
// FIXME: should merge the transformations/variances like Link::merge();
if(maxRotVar > _rotVariance)
{
_rotVariance = maxRotVar;
}
if(maxTransVar > _transVariance)
{
_transVariance = maxTransVar;
}
if(ignoreFrame && !_createIntermediateNodes)
{
return;
@@ -563,17 +579,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
lastPose_ = odomEvent.pose();
double maxRotVar = odomEvent.rotVariance();
double maxTransVar = odomEvent.transVariance();
// FIXME: should merge the transformations/variances like Link::merge();
if(maxRotVar > _rotVariance)
{
_rotVariance = maxRotVar;
}
if(maxTransVar > _transVariance)
{
_transVariance = maxTransVar;
}
bool notify = true;
_dataMutex.lock();
@@ -598,7 +603,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
{
_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
}
UDEBUG("Added data %d", odomEvent.data().id());
UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
_rotVariance = 0;
_transVariance = 0;

64
corelib/src/util3d_surface.cpp
View File

@@ -194,6 +194,32 @@ void appendMesh(
}
}
void appendMesh(
pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
std::vector<pcl::Vertices> & polygonsA,
const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
const std::vector<pcl::Vertices> & polygonsB)
{
UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
int sizeA = cloudA.size();
cloudA += cloudB;
int sizePolygonsA = polygonsA.size();
polygonsA.resize(sizePolygonsA+polygonsB.size());
for(unsigned int i=0; i<polygonsB.size(); ++i)
{
pcl::Vertices vertices = polygonsB[i];
for(unsigned int j=0; j<vertices.vertices.size(); ++j)
{
vertices.vertices[j] += sizeA;
}
polygonsA[i+sizePolygonsA] = vertices;
}
}
std::map<int, int> filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::vector<pcl::Vertices> & polygons,
@@ -232,6 +258,44 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
return output;
}
std::map<int, int> filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons)
{
UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
std::map<int, int> addedVertices; //<oldIndex, newIndex>
std::map<int, int> output; //<newIndex, oldIndex>
outputCloud.resize(cloud.size());
outputCloud.is_dense = true;
outputPolygons.resize(polygons.size());
int oi = 0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
pcl::Vertices & v = outputPolygons[i];
v.vertices.resize(polygons[i].vertices.size());
for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
{
std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
if(iter == addedVertices.end())
{
outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
output.insert(std::make_pair(oi, polygons[i].vertices[j]));
v.vertices[j] = oi++;
}
else
{
v.vertices[j] = iter->second;
}
}
}
outputCloud.resize(oi);
return output;
}
std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
const std::vector<pcl::Vertices> & polygons,