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

iOS feb 2025 various updates (#1451)

Browse Source 
* Using shadows on all texts and icons to see them better when background is white. Buffering database previews to show faster the Library. Updated how RAM usage is computed and now show max memory. Added warnings when RAM usage is low.

* Added measuring tool

* fixed map not shown when closing visualization

* Added measure text size setting, changed look and feel of point to point

* Fixed measuring point not showing

* Added fix for #1401

* Addressing #1407

* Export: Added colored OBJ options

* fixed index_t not existing on focal

* Fixed Settings not applied after restoring to all default settings witohut restarting the app (Marker detection not working issue #1455 )

* Marker detection: Fixing wrong depth used when rgb and depth image sizes cannot be divided exactly one from the the other #1455

* Added Marker Max Range option (default 1m)

* Added OBJ texture policy option (keep color on textureless polygons)

* Added texture/color blending option directly in the app. Added LAZ export option.

* Hiding measuring button if not mesh, dont zip if exporting to laz, updating Export XXX button based on the current context, fixed always blending texture/color on not visualization mode

* updated default marker max range to 2m

* bump ios app version

* fixing pcl 1.8.1 build

* fixed android build
This commit is contained in:
matlabbe
2025-03-15 10:58:03 -07:00
committed by GitHub
parent f329ebd42b
commit 28dbf1c98a
40 changed files with 3436 additions and 489 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
app/android/jni/CMakeLists.txt
View File

@@ -3,9 +3,10 @@ SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
${CMAKE_CURRENT_SOURCE_DIR}/third-party/include
${PROJECT_BINARY_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/utilite/include
${PROJECT_BINARY_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/utilite/include
${CMAKE_CURRENT_BINARY_DIR}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
"${ANDROID_NDK}/platforms/android-${ANDROID_NATIVE_API_LEVEL}/arch-${ANDROID_ARCH_NAME}/usr/include"
@@ -24,14 +25,19 @@ set(sources
point_cloud_drawable.cpp
graph_drawable.cpp
background_renderer.cc
text_drawable.cpp
quad_color.cpp
tango-gl/bounding_box.cpp
tango-gl/axis.cpp
tango-gl/camera.cpp
tango-gl/circle.cpp
tango-gl/conversions.cpp
tango-gl/drawable_object.cpp
tango-gl/frustum.cpp
tango-gl/gesture_camera.cpp
tango-gl/grid.cpp
tango-gl/line.cpp
tango-gl/mesh.cpp
tango-gl/shaders.cpp
tango-gl/trace.cpp
tango-gl/transform.cpp
@@ -114,7 +120,35 @@ add_definitions(${PCL_DEFINITIONS})
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
add_library(NativeRTABMap SHARED ${sources})
####################################
# Generate resources files
####################################
SET(RESOURCES
${CMAKE_CURRENT_SOURCE_DIR}/resources/text_atlas.png
)
foreach(arg ${RESOURCES})
get_filename_component(filename ${arg} NAME)
string(REPLACE "." "_" output ${filename})
set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
endforeach(arg ${RESOURCES})
find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
IF(NOT RTABMAP_RES_TOOL)
MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
ENDIF(NOT RTABMAP_RES_TOOL)
ADD_CUSTOM_COMMAND(
OUTPUT ${RESOURCES_HEADERS}
COMMAND ${RTABMAP_RES_TOOL} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
COMMENT "[Creating resources]"
DEPENDS ${RESOURCES}
)
####################################
# Generate resources files END
####################################
add_library(NativeRTABMap SHARED ${sources} ${RESOURCES_HEADERS})
target_link_libraries(NativeRTABMap ${LIBRARIES}
android
log

5
app/android/jni/CameraMobile.cpp
View File

@@ -108,8 +108,9 @@ void CameraMobile::close()
dataReady_.release();
}
void CameraMobile::resetOrigin()
void CameraMobile::resetOrigin(const rtabmap::Transform & offset)
{
manualOriginOffset_ = offset;
originUpdate_ = true;
}
@@ -193,7 +194,7 @@ void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
previousAnchorPose_.setNull();
previousAnchorLinearVelocity_.clear();
previousAnchorStamp_ = 0.0;
originOffset_ = pose.translation().inverse();
originOffset_ = manualOriginOffset_.isNull() ? pose.translation().inverse() : manualOriginOffset_;
originUpdate_ = false;
}

3
app/android/jni/CameraMobile.h
View File

@@ -99,7 +99,7 @@ public:
void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
void updateOnRender();
void resetOrigin();
void resetOrigin(const rtabmap::Transform & offset = rtabmap::Transform());
virtual bool isCalibrated() const;
virtual bool odomProvided() const { return true; }
@@ -150,6 +150,7 @@ private:
EnvSensors lastEnvSensors_;
Transform originOffset_;
bool originUpdate_;
rtabmap::Transform manualOriginOffset_;
float upstreamRelocalizationAccThr_;
rtabmap::Transform previousAnchorPose_;
std::vector<float> previousAnchorLinearVelocity_;

65
app/android/jni/Measure.h Normal file
View File

@@ -0,0 +1,65 @@
/*
Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef MEASURE_H_
#define MEASURE_H_
#include <opencv2/opencv.hpp>
class Measure
{
public:
Measure(
const cv::Point3f & pt1,
const cv::Point3f & pt2,
const cv::Vec3f & n1,
const cv::Vec3f & n2) :
pt1_(pt1),
pt2_(pt2),
n1_(n1),
n2_(n2)
{
length_ = cv::norm(pt2_ - pt1_);
}
virtual ~Measure() {}
float length() const {return length_;}
const cv::Point3f & pt1() const {return pt1_;}
const cv::Point3f & pt2() const {return pt2_;}
const cv::Vec3f & n1() const {return n1_;}
const cv::Vec3f & n2() const {return n2_;}
private:
cv::Point3f pt1_;
cv::Point3f pt2_;
cv::Vec3f n1_;
cv::Vec3f n2_;
float length_;
};
#endif /* MEASURE_H_ */

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

@@ -132,6 +132,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(cameraColor_&&fullResolution_?"2":"1")));
@@ -150,6 +151,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpParallelized(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPVarianceMedianRatio(), std::string("2")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
@@ -264,13 +266,43 @@ RTABMapApp::RTABMapApp() :
lastPoseEventTime_(0.0),
visualizingMesh_(false),
exportedMeshUpdated_(false),
optMesh_(new pcl::TextureMesh),
measuresUpdated_(false),
targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
quadSamplePolygons_(2),
metricSystem_(true),
measuringTextSize_(0.05f),
snapAxisThr_(0.95),
measuringMode_(0),
addMeasureClicked_(false),
teleportClicked_(false),
removeMeasureClicked_(false),
optTextureMesh_(new pcl::TextureMesh),
optRefId_(0),
optRefPose_(0),
mapToOdom_(rtabmap::Transform::getIdentity())
{
mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
pcl::PointXYZRGB ptWhite;
ptWhite.r = ptWhite.g = ptWhite.b = 255;
targetPoint_->push_back(ptWhite);
snapAxes_.push_back(cv::Vec3f(1,0,0));
snapAxes_.push_back(cv::Vec3f(0,1,0));
snapAxes_.push_back(cv::Vec3f(0,0,1));
float quadSize = 0.05f;
quadSample_->push_back(pcl::PointXYZ(-quadSize, -quadSize, 0.0f));
quadSample_->push_back(pcl::PointXYZ(quadSize, -quadSize, 0.0f));
quadSample_->push_back(pcl::PointXYZ(quadSize, quadSize, 0.0f));
quadSample_->push_back(pcl::PointXYZ(-quadSize, quadSize, 0.0f));
quadSamplePolygons_[0].vertices.resize(3);
quadSamplePolygons_[0].vertices[0] = 0;
quadSamplePolygons_[0].vertices[1] = 1;
quadSamplePolygons_[0].vertices[2] = 2;
quadSamplePolygons_[1].vertices.resize(3);
quadSamplePolygons_[1].vertices[0] = 0;
quadSamplePolygons_[1].vertices[1] = 2;
quadSamplePolygons_[1].vertices[2] = 3;
#ifdef __ANDROID__
env->GetJavaVM(&jvm);
@@ -278,19 +310,6 @@ RTABMapApp::RTABMapApp() :
#endif
LOGI("RTABMapApp::RTABMapApp()");
createdMeshes_.clear();
rawPoses_.clear();
clearSceneOnNextRender_ = true;
openingDatabase_ = false;
exporting_ = false;
postProcessing_=false;
totalPoints_ = 0;
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingTime_ = 0.0f;
lastPostRenderEventTime_ = 0.0;
lastPoseEventTime_ = 0.0;
bufferedStatsData_.clear();
#ifdef __ANDROID__
progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
#endif
@@ -404,13 +423,16 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
lastPoseEventTime_ = 0.0;
bufferedStatsData_.clear();
graphOptimization_ = true;
measuresUpdated_ = !measures_.empty();
measures_.clear();
this->registerToEventsManager();
int status = 0;
// Open visualization while we load (if there is an optimized mesh saved in database)
optMesh_.reset(new pcl::TextureMesh);
optTextureMesh_.reset(new pcl::TextureMesh);
optMesh_ = rtabmap::Mesh();
optTexture_ = cv::Mat();
optRefId_ = 0;
if(optRefPose_)
@@ -418,6 +440,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
delete optRefPose_;
optRefPose_ = 0;
}
visualizingMesh_ = false;
cv::Mat cloudMat;
std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -436,19 +459,20 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
if(!cloudMat.empty())
{
LOGI("Open: Found optimized mesh! Visualizing it.");
optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
optMesh_ = rtabmap::Mesh();
optTexture_ = textures;
if(!optTexture_.empty())
if(!optTexture_.empty())
{
LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
status=3;
}
else if(optMesh_->tex_polygons.size())
else if(optTextureMesh_->tex_polygons.size())
{
LOGI("Open: Polygon mesh");
status=2;
}
else if(!optMesh_->cloud.data.empty())
else if(!optTextureMesh_->cloud.data.empty())
{
LOGI("Open: Point cloud");
status=1;
@@ -1330,7 +1354,8 @@ int RTABMapApp::Render()
#ifdef DEBUG_RENDERING_PERFORMANCE
LOGD("Camera updateOnRender %fs", time.ticks());
#endif
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
// We detect if we are in measuring mode if rtabmap is not running
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -1424,40 +1449,189 @@ int RTABMapApp::Render()
{
main_scene_.clear();
exportedMeshUpdated_ = false;
measuresUpdated_ = measures_.size()>0;
}
if(!main_scene_.hasCloud(g_optMeshId))
{
LOGI("Adding optimized mesh to opengl (%d points, %d polygons, %d tex_coords, materials=%d texture=%dx%d)...",
optMesh_->cloud.point_step==0?0:(int)optMesh_->cloud.data.size()/optMesh_->cloud.point_step,
optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(),
optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(),
(int)optMesh_->tex_materials.size(),
optTextureMesh_->cloud.point_step==0?0:(int)optTextureMesh_->cloud.data.size()/optTextureMesh_->cloud.point_step,
optTextureMesh_->tex_polygons.size()!=1?0:(int)optTextureMesh_->tex_polygons[0].size(),
optTextureMesh_->tex_coordinates.size()!=1?0:(int)optTextureMesh_->tex_coordinates[0].size(),
(int)optTextureMesh_->tex_materials.size(),
optTexture_.cols, optTexture_.rows);
if(optMesh_->tex_polygons.size() && optMesh_->tex_polygons[0].size())
if(optTextureMesh_->tex_polygons.size() && optTextureMesh_->tex_polygons[0].size())
{
rtabmap::Mesh mesh;
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(optMesh_->cloud, *mesh.cloud);
pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
mesh.polygons = optMesh_->tex_polygons[0];
mesh.pose.setIdentity();
if(optMesh_->tex_coordinates.size())
optMesh_ = rtabmap::Mesh();
optMesh_.gains[0] = optMesh_.gains[1] = optMesh_.gains[2] = 1.0;
optMesh_.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
optMesh_.normals.reset(new pcl::PointCloud<pcl::Normal>);
pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.cloud);
pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.normals);
bool hasColors = false;
for(unsigned int i=0; i<optTextureMesh_->cloud.fields.size(); ++i)
{
if(optTextureMesh_->cloud.fields[i].name.compare("rgb") == 0)
{
hasColors = true;
break;
}
}
if(!hasColors)
{
std::uint8_t r = 255, g = 255, b = 255; // White
std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
for(size_t i=0; i<optMesh_.cloud->size(); ++i)
{
optMesh_.cloud->at(i).rgb = *reinterpret_cast<float*>(&rgb);
}
}
optMesh_.polygons = optTextureMesh_->tex_polygons[0];
if(optTextureMesh_->tex_coordinates.size())
{
mesh.texCoords = optMesh_->tex_coordinates[0];
mesh.texture = optTexture_;
optMesh_.texCoords = optTextureMesh_->tex_coordinates[0];
optMesh_.texture = optTexture_;
}
main_scene_.addMesh(g_optMeshId, mesh, rtabmap::opengl_world_T_rtabmap_world, true);
main_scene_.addMesh(g_optMeshId, optMesh_, rtabmap::opengl_world_T_rtabmap_world, true);
}
else
{
pcl::IndicesPtr indices(new std::vector<int>); // null
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud);
pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *cloud);
main_scene_.addCloud(g_optMeshId, cloud, indices, rtabmap::opengl_world_T_rtabmap_world);
}
if(!measures_.empty())
{
measuresUpdated_ = true;
}
}
if(camera_ != 0 && (rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
{
updateMeasuringState();
}
else
{
main_scene_.removeLine(55555);
main_scene_.removeQuad(55555);
main_scene_.removeQuad(55556);
main_scene_.removeCircle(55555);
main_scene_.removeCircle(55556);
main_scene_.removeText(55555);
main_scene_.removeCloudOrMesh(-99999);
measuringTmpPts_.clear();
measuringTmpNormals_.clear();
}
if(measuresUpdated_)
{
std::list<Measure> measures = measures_;
measuresUpdated_ = false;
main_scene_.clearLines();
main_scene_.clearTexts();
main_scene_.clearQuads();
main_scene_.clearCircles();
int lineId = 0;
int textId = 0;
int quadId = 0;
int circleId = 0;
float sphereRadius = 0.02f;
float quadSize=0.05f;
float quadAlpha = 0.3f;
tango_gl::Color color(1.0f, 0.0f, 1.0f);
tango_gl::Color xColor(1.0f, 0.0f, 0.0f);
tango_gl::Color yColor(0.0f, 1.0f, 0.0f);
tango_gl::Color zColor(0.0f, 0.0f, 1.0f);
float restrictiveSnapThr = 0.9999;
for(std::list<Measure>::iterator iter=measures.begin(); iter!=measures.end(); ++iter)
{
// Determinate color based on current snap axes
tango_gl::Color quadColor = color;
bool sameNormal = false;
if(iter->n1().dot(iter->n2()) > 0.99) // Same normal, plane to plane
{
sameNormal = true;
Eigen::Vector3f n(iter->n1()[0], iter->n1()[1], iter->n1()[2]);
float n1ProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
float n1ProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
float n1ProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
if(fabs(n1ProdX) > restrictiveSnapThr)
{
quadColor = xColor;
}
else if(fabs(n1ProdY) > restrictiveSnapThr)
{
quadColor = yColor;
}
else if(fabs(n1ProdZ) > restrictiveSnapThr)
{
quadColor = zColor;
}
}
const Measure & m = *iter;
LOGI("dist=%f, %f,%f,%f -> %f,%f,%f", m.length(),
m.pt1().x, m.pt1().y, m.pt1().z,
m.pt2().x, m.pt2().y, m.pt2().z);
cv::Point3f pt1 = rtabmap::util3d::transformPoint(m.pt1(), rtabmap::opengl_world_T_rtabmap_world);
cv::Point3f pt2 = rtabmap::util3d::transformPoint(m.pt2(), rtabmap::opengl_world_T_rtabmap_world);
main_scene_.addLine(++lineId, pt1, pt2, quadColor);
if (fabs(iter->n1()[2]) < 0.00001 && sameNormal)
{
// Add a line so that in orthogonal view, we can see better where the lines are starting/finishing
cv::Point3f n = cv::Vec3f(0,0,1).cross(iter->n1());
n = rtabmap::util3d::transformPoint(n, rtabmap::opengl_world_T_rtabmap_world) * (quadSize/2);
cv::Point3f pa = pt1 + n;
cv::Point3f pb = pt1 - n;
main_scene_.addLine(++lineId, pa, pb, quadColor);
cv::Point3f pc = pt2 + n;
cv::Point3f pd = pt2 - n;
main_scene_.addLine(++lineId, pc, pd, quadColor);
}
float diff = m.length();
std::string text = uFormat("%0.2f m", diff);
if(!metricSystem_)
{
static const double METERS_PER_FOOT = 0.3048;
static double INCHES_PER_FOOT = 12.0;
double lengthInFeet = diff / METERS_PER_FOOT;
int feet = (int)lengthInFeet;
float inches = (lengthInFeet - feet) * INCHES_PER_FOOT;
if(feet > 0)
{
text = uFormat("%d' %0.1f\"", feet, inches);
}
else
{
text = uFormat("%0.1f\"", inches);
}
}
main_scene_.addText(++textId, text, rtabmap::Transform((pt1.x+pt2.x)/2.0f, (pt1.y+pt2.y)/2.0f, (pt1.z+pt2.z)/2.0f, 0, 0,0), measuringTextSize_, quadColor);
cv::Vec3f n1 = rtabmap::util3d::transformPoint(m.n1(), rtabmap::opengl_world_T_rtabmap_world);
cv::Vec3f n2 = rtabmap::util3d::transformPoint(m.n2(), rtabmap::opengl_world_T_rtabmap_world);
Eigen::Quaternionf q1, q2;
q1.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(n1[0],n1[1],n1[2]));
q2.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(n2[0],n2[1],n2[2]));
if(sameNormal)
{
main_scene_.addQuad(++quadId, quadSize, rtabmap::Transform(pt1.x, pt1.y, pt1.z, q1.x(), q1.y(), q1.z(), q1.w()), quadColor, quadAlpha);
main_scene_.addQuad(++quadId, quadSize, rtabmap::Transform(pt2.x, pt2.y, pt2.z, q2.x(), q2.y(), q2.z(), q2.w()), quadColor, quadAlpha);
}
else
{
main_scene_.addCircle(++circleId, quadSize/2, rtabmap::Transform(pt1.x, pt1.y, pt1.z, q1.x(), q1.y(), q1.z(), q1.w()), quadColor, quadAlpha);
main_scene_.addCircle(++circleId, quadSize/2, rtabmap::Transform(pt2.x, pt2.y, pt2.z, q2.x(), q2.y(), q2.z(), q2.w()), quadColor, quadAlpha);
}
}
}
if(!openingDatabase_)
{
@@ -1576,7 +1750,8 @@ int RTABMapApp::Render()
if(main_scene_.hasCloud(g_optMeshId))
{
main_scene_.clear();
optMesh_.reset(new pcl::TextureMesh);
optTextureMesh_.reset(new pcl::TextureMesh);
optMesh_ = rtabmap::Mesh();
optTexture_ = cv::Mat();
}
@@ -1639,6 +1814,8 @@ int RTABMapApp::Render()
lastPostRenderEventTime_ = 0.0;
lastPoseEventTime_ = 0.0;
bufferedStatsData_.clear();
measuresUpdated_ = !measures_.empty();
measures_.clear();
}
// Did we lose OpenGL context? If so, recreate the context;
@@ -2270,6 +2447,395 @@ int RTABMapApp::Render()
}
}
void RTABMapApp::updateMeasuringState()
{
rtabmap::Transform openglCam = main_scene_.GetOpenGLCameraPose();
rtabmap::Transform rtabmapCam = rtabmap::rtabmap_world_T_opengl_world * openglCam * rtabmap::opengl_world_T_rtabmap_world;
Eigen::Vector3f origin(openglCam.x(), openglCam.y(), openglCam.z());
Eigen::Vector3f rtabmapOrigin(rtabmapCam.x(), rtabmapCam.y(), rtabmapCam.z());
rtabmap::Transform v = openglCam.rotation() * rtabmap::Transform(0,0,-1,0,0,0);
Eigen::Vector3f dir(v.x(), v.y(), v.z());
v = rtabmapCam.rotation() * rtabmap::Transform(1,0,0,0,0,0);
Eigen::Vector3f rtabmapDir(v.x(), v.y(), v.z());
tango_gl::Color color(1.0f, 0.0f, 1.0f);
tango_gl::Color xColor(1.0f, 0.0f, 0.0f); // in rtabmap world
tango_gl::Color yColor(0.0f, 1.0f, 0.0f); // in rtabmap world
tango_gl::Color zColor(0.0f, 0.0f, 1.0f); // in rtabmap world
float circleRadius = 0.025;
float quadSize=0.05f;
float quadAlpha = 0.3f;
main_scene_.removeQuad(55555);
main_scene_.removeQuad(55556);
main_scene_.removeCircle(55555);
main_scene_.removeCircle(55556);
main_scene_.removeLine(55555);
main_scene_.removeText(55555);
if(removeMeasureClicked_)
{
if(!measuringTmpPts_.empty())
{
measuringTmpPts_.clear();
measuringTmpNormals_.clear();
}
else
{
bool removed = false;
for(std::list<Measure>::iterator iter=measures_.begin(); iter!=measures_.end() && !removed; ++iter)
{
const Measure & m = *iter;
for(int i=0; i<2;++i)
{
// intersecting the quad?
cv::Point3f pt = i==0?m.pt1():m.pt2();
cv::Point3f n = i==0?m.n1():m.n2();
pt = rtabmap::util3d::transformPoint(pt, rtabmap::opengl_world_T_rtabmap_world);
n = rtabmap::util3d::transformPoint(n, rtabmap::opengl_world_T_rtabmap_world);
Eigen::Quaternionf q;
q.setFromTwoVectors(Eigen::Vector3f(0,0,-1), Eigen::Vector3f(n.x,n.y,n.z));
float dTmp;
Eigen::Vector3f nTmp;
int indexTmp;
if(rtabmap::util3d::intersectRayMesh(
origin,
dir,
*rtabmap::util3d::transformPointCloud(quadSample_, rtabmap::Transform(pt.x, pt.y, pt.z, q.x(), q.y(), q.z(), q.w())),
quadSamplePolygons_,
false,
dTmp,
nTmp,
indexTmp))
{
measures_.erase(iter);
removed = true;
break;
}
}
}
measuresUpdated_ |= removed;
}
}
float distance =0.0f;
Eigen::Vector3f n;
int index;
if(rtabmap::util3d::intersectRayMesh(
rtabmapOrigin,
rtabmapDir,
*optMesh_.cloud,
optMesh_.polygons,
true,
distance,
n,
index))
{
Eigen::Vector3f intersectionPt = origin + dir*distance;
cv::Point3f pt(intersectionPt[0], intersectionPt[1], intersectionPt[2]);
cv::Point3f normal(n[0], n[1], n[2]); // rtabmap world
cv::Point3f normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world); // opengl world
tango_gl::Color quadColor = color;
float normalProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
float normalProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
float normalProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
if(fabs(normal.x) > fabs(normal.z) && fabs(normal.y) > fabs(normal.z))
{
if(fabs(normalProdX) > snapAxisThr_)
{
normal.x = snapAxes_[0][0] * (normalProdX>0?1:-1);
normal.y = snapAxes_[0][1] * (normalProdX>0?1:-1);
normal.z = snapAxes_[0][2] * (normalProdX>0?1:-1);
quadColor = xColor;
}
else if(fabs(normalProdY) > snapAxisThr_)
{
normal.x = snapAxes_[1][0] * (normalProdY>0?1:-1);
normal.y = snapAxes_[1][1] * (normalProdY>0?1:-1);
normal.z = snapAxes_[1][2] * (normalProdY>0?1:-1);
quadColor = yColor;
}
else if(measuringMode_ == 0)
{
// We force to be aligned with xy plane
normal.z = 0;
float n = cv::norm(normal);
normal.x/=n;
normal.y/=n;
normal.z/=n;
}
}
else if(fabs(normalProdZ) > snapAxisThr_)
{
normal.x = snapAxes_[2][0] * (normalProdZ>0?1:-1);
normal.y = snapAxes_[2][1] * (normalProdZ>0?1:-1);
normal.z = snapAxes_[2][2] * (normalProdZ>0?1:-1);
quadColor = zColor;
}
normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world);
if(teleportClicked_)
{
camera_->resetOrigin(rtabmap::Transform(-pt.z, -pt.x, pt.y-Scene::kHeightOffset.y,0,0,0));
}
else if(addMeasureClicked_) // Add measure
{
if((measuringMode_ == 1 || measuringTmpPts_.size()==1))
{
if(measuringMode_ == 1) // Height single click
{
measuringTmpPts_.clear();
measuringTmpNormals_.clear();
normal.x=0;
normal.y=0;
normal.z=1;
normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world);
measuringTmpPts_.push_back(pt);
measuringTmpNormals_.push_back(normalGl);
pt.y = 0;
measuringTmpPts_.push_back(pt);
measuringTmpNormals_.push_back(normalGl);
}
else if(measuringMode_ == 0 || measuringMode_ == 2) // Plane to Plane or point to point
{
if(measuringMode_ == 0)
{
normalGl = measuringTmpNormals_.front();
// project point on line
float n = (pt-measuringTmpPts_.front()).dot(measuringTmpNormals_.front());
pt = measuringTmpPts_.front() + (measuringTmpNormals_.front() * n);
}
measuringTmpPts_.push_back(pt);
measuringTmpNormals_.push_back(normalGl);
}
// Add measure!
const cv::Point3f & pt1 = measuringTmpPts_.at(0);
const cv::Point3f & pt2 = measuringTmpPts_.at(1);
const cv::Vec3f & n1 = measuringTmpNormals_.at(0);
const cv::Vec3f & n2 = measuringTmpNormals_.at(1);
Measure measure(
rtabmap::util3d::transformPoint(pt1, rtabmap::rtabmap_world_T_opengl_world),
rtabmap::util3d::transformPoint(pt2, rtabmap::rtabmap_world_T_opengl_world),
rtabmap::util3d::transformPoint(n1, rtabmap::rtabmap_world_T_opengl_world),
rtabmap::util3d::transformPoint(n2, rtabmap::rtabmap_world_T_opengl_world));
if(measure.length()>=0.01f)
{
measures_.push_back(measure);
measuringTmpPts_.clear();
measuringTmpNormals_.clear();
measuresUpdated_ = true;
}
}
else
{
measuringTmpPts_.clear();
measuringTmpNormals_.clear();
// init the first point
measuringTmpPts_.push_back(pt);
measuringTmpNormals_.push_back(normalGl);
}
}
else if(measuringMode_ >= 0)
{
// move action
if(measuringMode_ == 0 && measuringTmpNormals_.size()==1)
{
normalGl = measuringTmpNormals_.front();
normal = rtabmap::util3d::transformPoint(normalGl, rtabmap::rtabmap_world_T_opengl_world);
n = Eigen::Vector3f(normal.x, normal.y, normal.z);
float normalProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
float normalProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
float normalProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
if(fabs(normalProdX) > snapAxisThr_)
{
quadColor = xColor;
}
else if(fabs(normalProdY) > snapAxisThr_)
{
quadColor = yColor;
}
else if(fabs(normalProdZ) > snapAxisThr_)
{
quadColor = zColor;
}
else
{
quadColor = color;
}
}
float quadWidthLeft = 0.05;
float quadWidthRight = quadWidthLeft;
float quadHeightBottom = quadWidthLeft;
float quadHeightTop = quadWidthLeft;
cv::Point3f pt2 = pt;
float lineLength = 0.0f;
// project point on line
if(measuringTmpPts_.size() == 1)
{
cv::Point3f v = pt2-measuringTmpPts_.front();
if(measuringMode_ == 0)
{
float n = v.dot(measuringTmpNormals_.front());
lineLength = fabs(n);
pt2 = measuringTmpPts_.front() + (measuringTmpNormals_.front() * n);
v = rtabmap::util3d::transformPoint(v, rtabmap::rtabmap_world_T_opengl_world);
if(!(fabs(normal.z) > fabs(normal.x) && fabs(normal.z) > fabs(normal.y)))
{
quadHeightTop = v.z>quadSize?v.z:quadSize;
quadHeightBottom = -v.z>quadSize?-v.z:quadSize;
if(fabs(normal.x) > fabs(normal.y))
{
cv::Point3f y = cv::Point3f(0,0,1).cross(normal);
float n = v.dot(y);
quadWidthRight = n>quadSize?n:quadSize;
quadWidthLeft = n<-quadSize?fabs(n):quadSize;
if(normal.x>0)
{
quadWidthLeft = n>quadSize?n:quadSize;
quadWidthRight = n<-quadSize?fabs(n):quadSize;
float tmp =quadHeightTop;
quadHeightTop= quadHeightBottom;
quadHeightBottom = tmp;
}
}
else
{
cv::Point3f x = normal.cross(cv::Point3f(0,0,1));
float n = v.dot(x);
quadWidthRight = n<-quadSize?fabs(n):quadSize;
quadWidthLeft = n>quadSize?n:quadSize;
}
}
else
{
if(normal.z>0)
{
quadHeightBottom = v.x<-quadSize?fabs(v.x):quadSize;
quadHeightTop = v.x>quadSize?v.x:quadSize;
quadWidthRight = v.y<-quadSize?fabs(v.y):quadSize;
quadWidthLeft = v.y>quadSize?v.y:quadSize;
}
else
{
quadHeightTop = v.x<-quadSize?fabs(v.x):quadSize;
quadHeightBottom = v.x>quadSize?v.x:quadSize;
quadWidthRight = v.y<-quadSize?fabs(v.y):quadSize;
quadWidthLeft = v.y>quadSize?v.y:quadSize;
}
}
}
else
{
lineLength = cv::norm(v);
}
std::string text = uFormat("%0.2f m", lineLength);
if(!metricSystem_)
{
static const double METERS_PER_FOOT = 0.3048;
static double INCHES_PER_FOOT = 12.0;
double lengthInFeet = lineLength / METERS_PER_FOOT;
int feet = (int)lengthInFeet;
float inches = (lengthInFeet - feet) * INCHES_PER_FOOT;
if(feet > 0)
{
text = uFormat("%d' %0.1f\"", feet, inches);
}
else
{
text = uFormat("%0.1f\"", inches);
}
}
main_scene_.addText(55555, text, rtabmap::Transform(pt.x, pt.y, pt.z, 0,0,0), 0.05f, measuringMode_ == 0?quadColor:color);
}
Eigen::Quaternionf q;
q.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(normalGl.x,normalGl.y,normalGl.z));
if(measuringTmpPts_.size() == 1 && measuringMode_ != 1)
{
const cv::Point3f & pt1 = measuringTmpPts_.at(0);
main_scene_.addLine(55555, pt1, pt2, measuringMode_ == 0?quadColor:color);
if(measuringMode_ == 2)
{
// Use respective orientation for each circle
Eigen::Quaternionf q1;
cv::Vec3f normalGL1 = measuringTmpNormals_.front();
q1.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(normalGL1[0],normalGL1[0],normalGL1[0]));
main_scene_.addCircle(55555,
circleRadius,
rtabmap::Transform(
measuringTmpPts_.front().x,
measuringTmpPts_.front().y,
measuringTmpPts_.front().z,
q1.x(),q1.y(), q1.z(), q1.w()), color, quadAlpha);
main_scene_.addCircle(55556,
circleRadius,
rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), color, quadAlpha);
}
else
{
// Use same orientation for both quads
main_scene_.addQuad(55555,
quadSize,
rtabmap::Transform(
measuringTmpPts_.front().x,
measuringTmpPts_.front().y,
measuringTmpPts_.front().z,
q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
main_scene_.addQuad(55556,
quadWidthLeft,
quadWidthRight,
quadHeightBottom,
quadHeightTop,
rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
}
}
else if(measuringMode_ == 2)
{
main_scene_.addCircle(55555,
circleRadius,
rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), color, quadAlpha);
}
else
{
main_scene_.addQuad(55555,
quadSize,
rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
}
}
}
Eigen::Vector3f target = origin+dir*(distance<100.0f?distance:0.5f);
rtabmap::Transform pose(target[0], target[1], target[2], 0,0,0);
if(main_scene_.hasCloud(-99999))
{
main_scene_.setCloudPose(-99999, pose);
}
else
{
main_scene_.addCloud(-99999, targetPoint_, pcl::IndicesPtr(), pose);
}
// reset states
removeMeasureClicked_ = false;
addMeasureClicked_ = false;
teleportClicked_ = false;
}
void RTABMapApp::SetCameraType(
tango_gl::GestureCamera::CameraType camera_type) {
main_scene_.SetCameraType(camera_type);
@@ -2337,6 +2903,15 @@ void RTABMapApp::setOrthoCropFactor(float value)
}
void RTABMapApp::setGridRotation(float value)
{
// Update measuring snap axes
rtabmap::Transform rotation(0,0, value * DEGREE_2_RADIANS);
renderingMutex_.lock();
snapAxes_[0] = rtabmap::util3d::transformPoint(cv::Vec3f(1,0,0), rotation);
snapAxes_[1] = rtabmap::util3d::transformPoint(cv::Vec3f(0,1,0), rotation);
measuresUpdated_ = true;
renderingMutex_.unlock();
// Update grid
main_scene_.setGridRotation(value);
}
void RTABMapApp::setLighting(bool enabled)
@@ -2351,6 +2926,10 @@ void RTABMapApp::setWireframe(bool enabled)
{
main_scene_.setWireframe(enabled);
}
void RTABMapApp::setTextureColorSeamsHidden(bool hidden)
{
main_scene_.setTextureColorSeamsHidden(hidden);
}
void RTABMapApp::setLocalizationMode(bool enabled)
{
@@ -2538,7 +3117,7 @@ void RTABMapApp::setDepthConfidence(int value)
void RTABMapApp::setExportPointCloudFormat(const std::string & format)
{
#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
if(format == "las") {
if(format == "las" || format == "laz") {
exportPointCloudFormat_ = format;
}
else
@@ -2710,6 +3289,7 @@ bool RTABMapApp::exportMesh(
int optimizedMinClusterSize,
float optimizedMaxTextureDistance,
int optimizedMinTextureClusterSize,
int textureVertexColorPolicy,
bool blockRendering)
{
// make sure createdMeshes_ is not modified while exporting! We don't
@@ -2998,7 +3578,7 @@ bool RTABMapApp::exportMesh(
0,
mergedClouds,
optimizedColorRadius,
textureSize == 0,
!(textureSize > 0 && textureVertexColorPolicy == 0),
optimizedCleanWhitePolygons,
optimizedMinClusterSize);
@@ -3240,7 +3820,9 @@ bool RTABMapApp::exportMesh(
textureCount,
vertexToPixels,
true, 10.0f, true ,true, 0, 0, 0, false,
&progressionStatus_);
&progressionStatus_,
255,
textureVertexColorPolicy == 1);
LOGI("Merging %d textures... globalTextures=%dx%d", (int)textureMesh->tex_materials.size(),
globalTextures.cols, globalTextures.rows);
}
@@ -3281,9 +3863,28 @@ bool RTABMapApp::exportMesh(
}
else if(textureMesh->tex_materials.size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
bool hasColors = false;
for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
{
if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
{
hasColors = true;
break;
}
}
cv::Mat cloudMat;
if(hasColors)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
}
// save in database
std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
@@ -3492,7 +4093,8 @@ bool RTABMapApp::exportMesh(
bool RTABMapApp::postExportation(bool visualize)
{
LOGI("postExportation(visualize=%d)", visualize?1:0);
optMesh_.reset(new pcl::TextureMesh);
optTextureMesh_.reset(new pcl::TextureMesh);
optMesh_= rtabmap::Mesh();
optTexture_ = cv::Mat();
exportedMeshUpdated_ = false;
@@ -3513,8 +4115,9 @@ bool RTABMapApp::postExportation(bool visualize)
if(!cloudMat.empty())
{
LOGI("postExportation: Found optimized mesh! Visualizing it.");
optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
optTexture_ = textures;
optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
optMesh_ = rtabmap::Mesh();
optTexture_ = textures;
boost::mutex::scoped_lock lock(renderingMutex_);
visualizingMesh_ = true;
@@ -3606,9 +4209,9 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
if(polygonMesh->cloud.data.size())
{
#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
if(polygonMesh->polygons.empty() && exportPointCloudFormat_ == "las") {
if(polygonMesh->polygons.empty() && (exportPointCloudFormat_ == "las" || exportPointCloudFormat_ == "laz")) {
// Point cloud LAS
std::string filePath = directory + UDirectory::separator() + name + ".las";
std::string filePath = directory + UDirectory::separator() + name + (exportPointCloudFormat_ == "las"? ".las" : ".laz");
LOGI("Saving las (%d vertices) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, filePath.c_str());
pcl::PointCloud<pcl::PointXYZRGB> output;
pcl::fromPCLPointCloud2(polygonMesh->cloud, output);
@@ -3684,7 +4287,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
totalPolygons += textureMesh->tex_polygons[i].size();
}
LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str());
success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
success = rtabmap::util3d::saveOBJFile(filePath, *textureMesh) == 0;
if(success)
{
@@ -3799,6 +4402,66 @@ int RTABMapApp::postProcessing(int approach)
return returnedValue;
}
void RTABMapApp::clearMeasures()
{
boost::mutex::scoped_lock lockRender(renderingMutex_);
measures_.clear();
measuresUpdated_ = true;
}
void RTABMapApp::setMeasuringMode(int mode)
{
if(visualizingMesh_)
{
measuringMode_ = mode;
}
}
void RTABMapApp::addMeasureButtonClicked()
{
if(visualizingMesh_)
{
boost::mutex::scoped_lock lock(renderingMutex_);
addMeasureClicked_ = true;
}
}
void RTABMapApp::teleportButtonClicked()
{
if(visualizingMesh_)
{
boost::mutex::scoped_lock lock(renderingMutex_);
teleportClicked_ = true;
}
}
void RTABMapApp::removeMeasure()
{
if(visualizingMesh_)
{
boost::mutex::scoped_lock lock(renderingMutex_);
removeMeasureClicked_ = true;
}
}
void RTABMapApp::setMetricSystem(bool enabled)
{
metricSystem_ = enabled;
if(measures_.size())
{
measuresUpdated_ = true;
}
}
void RTABMapApp::setMeasuringTextSize(float size)
{
measuringTextSize_ = size;
if(measures_.size())
{
measuresUpdated_ = true;
}
}
void RTABMapApp::postOdometryEvent(
rtabmap::Transform pose,
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,

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

@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/pcl_base.h>
#include <pcl/TextureMesh.h>
#include "Measure.h"
// RTABMapApp handles the application lifecycle and resources.
class RTABMapApp : public UEventsHandler {
@@ -84,7 +85,7 @@ class RTABMapApp : public UEventsHandler {
#ifdef __ANDROID__
bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
#else // __APPLE__
bool startCamera();
bool startCamera();
#endif
// Allocate OpenGL resources for rendering, mainly for initializing the Scene.
void InitializeGLContent();
@@ -127,6 +128,7 @@ class RTABMapApp : public UEventsHandler {
void setLighting(bool enabled);
void setBackfaceCulling(bool enabled);
void setWireframe(bool enabled);
void setTextureColorSeamsHidden(bool hidden);
void setLocalizationMode(bool enabled);
void setTrajectoryMode(bool enabled);
void setGraphOptimization(bool enabled);
@@ -176,10 +178,19 @@ class RTABMapApp : public UEventsHandler {
int optimizedMinClusterSize,
float optimizedMaxTextureDistance,
int optimizedMinTextureClusterSize,
int textureVertexColorPolicy,
bool blockRendering);
bool postExportation(bool visualize);
bool writeExportedMesh(const std::string & directory, const std::string & name);
int postProcessing(int approach);
void clearMeasures();
void showMeasures(bool x, bool y, bool z, bool custom);
void setMeasuringMode(int mode);
void addMeasureButtonClicked();
void teleportButtonClicked();
void removeMeasure();
void setMetricSystem(bool enabled);
void setMeasuringTextSize(float size);
void postOdometryEvent(
rtabmap::Transform pose,
@@ -203,6 +214,7 @@ class RTABMapApp : public UEventsHandler {
private:
int updateMeshDecimation(int width, int height);
rtabmap::ParametersMap getRtabmapParameters();
void updateMeasuringState();
bool smoothMesh(int id, rtabmap::Mesh & mesh);
void gainCompensation(bool full = false);
std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
@@ -264,11 +276,26 @@ class RTABMapApp : public UEventsHandler {
bool visualizingMesh_;
bool exportedMeshUpdated_;
pcl::TextureMesh::Ptr optMesh_;
pcl::TextureMesh::Ptr optTextureMesh_;
cv::Mat optTexture_;
rtabmap::Mesh optMesh_;
int optRefId_;
rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
std::list<Measure> measures_; // In opengl frame
bool measuresUpdated_;
bool metricSystem_;
float measuringTextSize_;
float snapAxisThr_;
std::vector<cv::Vec3f> snapAxes_;
int measuringMode_;
bool addMeasureClicked_;
bool teleportClicked_;
bool removeMeasureClicked_;
std::vector<cv::Point3f> measuringTmpPts_; // In opengl frame
std::vector<cv::Point3f> measuringTmpNormals_; // In opengl frame
pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPoint_;
pcl::PointCloud<pcl::PointXYZ>::Ptr quadSample_;
std::vector<pcl::Vertices> quadSamplePolygons_;
// main_scene_ includes all drawable object for visualizing Tango device's
// movement and point cloud.
Scene main_scene_;

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

@@ -821,6 +821,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
optimizedMinClusterSize,
optimizedMaxTextureDistance,
optimizedMinTextureClusterSize,
0,
blockRendering);
}
else

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

@@ -170,10 +170,12 @@ const std::string kTextureMeshVertexShader =
"precision highp float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aColor;\n"
"attribute vec2 aTexCoord;\n"
"uniform mat4 uMVP;\n"
"varying vec3 vColor;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
@@ -187,12 +189,14 @@ const std::string kTextureMeshVertexShader =
" vTexCoord = aTexCoord;\n"
" }\n"
" vColor = aColor;\n"
" vLightWeighting = 1.0;\n"
"}\n";
const std::string kTextureMeshLightingVertexShader =
"precision highp float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aColor;\n"
"attribute vec3 aNormal;\n"
"attribute vec2 aTexCoord;\n"
@@ -200,6 +204,7 @@ const std::string kTextureMeshLightingVertexShader =
"uniform mat3 uN;\n"
"uniform vec3 uLightingDirection;\n"
"varying vec3 vColor;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
@@ -213,6 +218,7 @@ const std::string kTextureMeshLightingVertexShader =
" vTexCoord = aTexCoord;\n"
" }\n"
" vColor = aColor;\n"
" vec3 transformedNormal = uN * aNormal;\n"
" vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
" if(vLightWeighting<0.5) \n"
@@ -225,11 +231,22 @@ const std::string kTextureMeshFragmentShader =
"uniform float uGainR;\n"
"uniform float uGainG;\n"
"uniform float uGainB;\n"
"uniform int uHideSeams;\n"
"varying vec3 vColor;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
""
"void main() {\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
" vec4 textureColor;\n"
" if(uHideSeams==1) {\n"
" if(vTexCoord.x>0.99 && vTexCoord.y>0.99) {\n"
" textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
" } else {\n"
" textureColor = texture2D(uTexture, vTexCoord);\n"
" }\n"
" } else {\n"
" textureColor = texture2D(uTexture, vTexCoord) * vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
" }\n"
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
"}\n";
const std::string kTextureMeshBlendingFragmentShader =
@@ -243,11 +260,12 @@ const std::string kTextureMeshBlendingFragmentShader =
"uniform vec2 uScreenScale;\n"
"uniform float uNearZ;\n"
"uniform float uFarZ;\n"
"varying vec3 vColor;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
""
"void main() {\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
" float alpha = 1.0;\n"
" vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
" vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
@@ -1008,7 +1026,8 @@ void PointCloudDrawable::Render(
float nearClipPlane,
float farClipPlane,
bool packDepthToColorChannel,
bool wireFrame) const
bool wireFrame,
bool hideSeams) const
{
if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
{
@@ -1127,12 +1146,16 @@ void PointCloudDrawable::Render(
attribute_texture = glGetAttribLocation(program, "aTexCoord");
glEnableVertexAttribArray(attribute_texture);
if(depthTexture == 0)
{
GLuint hideSeams_handle = glGetUniformLocation(program, "uHideSeams");
glUniform1i(hideSeams_handle, hideSeams?1:0);
}
}
else
{
attribute_color = glGetAttribLocation(program, "aColor");
glEnableVertexAttribArray(attribute_color);
}
attribute_color = glGetAttribLocation(program, "aColor");
glEnableVertexAttribArray(attribute_color);
}
tango_gl::util::CheckGlError("Pointcloud::Render() common");
@@ -1144,7 +1167,7 @@ void PointCloudDrawable::Render(
{
glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
}
else if(!packDepthToColorChannel)
if(!packDepthToColorChannel)
{
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
}

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

@@ -98,7 +98,8 @@ private:
float nearClipPlane = 0, // nonnull if depthTexture>0
float farClipPlane = 0, // nonnull if depthTexture>0
bool packDepthToColorChannel = false,
bool wireFrame = false) const;
bool wireFrame = false,
bool hideSeams = false) const;
private:
template<class PointT>

69
app/android/jni/quad_color.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*
* Copyright 2014 Google Inc. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "quad_color.h"
#include "tango-gl/util.h"
static const float vertices[] = {-1.0f, -1.0f, 1.0f, -1.0f,
-1.0f, 1.0f, 1.0f, 1.0f};
QuadColor::QuadColor(float size) {
SetShader();
vertices_.resize(8);
for(int i=0; i<8; ++i)
{
vertices_[i] = vertices[i]*size;
}
}
QuadColor::QuadColor(
float widthLeft,
float widthRight,
float heightBottom,
float heightTop) {
SetShader();
vertices_.resize(8);
vertices_[0] = vertices[0]*widthLeft;
vertices_[1] = vertices[1]*heightBottom;
vertices_[2] = vertices[2]*widthRight;
vertices_[3] = vertices[3]*heightBottom;
vertices_[4] = vertices[4]*widthLeft;
vertices_[5] = vertices[5]*heightTop;
vertices_[6] = vertices[6]*widthRight;
vertices_[7] = vertices[7]*heightTop;
}
void QuadColor::Render(const glm::mat4& projection_mat,
const glm::mat4& view_mat) const {
glUseProgram(shader_program_);
// Calculate MVP matrix and pass it to shader.
glm::mat4 model_mat = GetTransformationMatrix();
glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
// Vertice binding
glEnableVertexAttribArray(attrib_vertices_);
glVertexAttribPointer(attrib_vertices_, 2, GL_FLOAT, GL_FALSE, 0, &vertices_[0]);
glBindBuffer(GL_ARRAY_BUFFER, 0);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glUseProgram(0);
}

35
app/android/jni/quad_color.h Normal file
View File

@@ -0,0 +1,35 @@
/*
* Copyright 2014 Google Inc. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef TANGO_GL_QUADCOLOR_H_
#define TANGO_GL_QUADCOLOR_H_
#include "tango-gl/drawable_object.h"
class QuadColor : public tango_gl::DrawableObject {
public:
QuadColor(float size);
QuadColor(float widthLeft,
float widthRight,
float heightBottom,
float heightTop);
QuadColor(const QuadColor& other) = delete;
QuadColor& operator=(const QuadColor&) = delete;
virtual ~QuadColor() {}
void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
};
#endif // TANGO_GL_QUADCOLOR_H_

BIN
app/android/jni/resources/text_atlas.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 25 KiB

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

@@ -36,7 +36,7 @@
// add an offset in z to our origin. We'll set this offset to 1.3 meters based
// on the average height of a human standing with a Tango device. This allows us
// to place a grid roughly on the ground for most users.
const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
const glm::vec3 Scene::kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
// Color of the motion tracking trajectory.
const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
@@ -93,6 +93,7 @@ Scene::Scene() :
lighting_(false),
backfaceCulling_(true),
wireFrame_(false),
textureColorSeamsHidden_(true),
r_(0.0f),
g_(0.0f),
b_(0.0f),
@@ -124,6 +125,7 @@ void Scene::InitGLContent()
UASSERT(axis_ == 0);
TextDrawable::createShaderProgram();
axis_ = new tango_gl::Axis();
frustum_ = new tango_gl::Frustum();
@@ -166,6 +168,7 @@ void Scene::DeleteResources() {
background_renderer_ = 0;
}
TextDrawable::releaseShaderProgram();
PointCloudDrawable::releaseShaderPrograms();
if (graph_shader_program_) {
@@ -198,6 +201,10 @@ void Scene::clear()
{
delete iter->second;
}
clearLines();
clearQuads();
clearTexts();
clearCircles();
if(trace_)
{
trace_->ClearVertexArray();
@@ -215,6 +222,39 @@ void Scene::clear()
}
}
void Scene::clearLines()
{
for(std::map<int, tango_gl::Line*>::iterator iter=lines_.begin(); iter!=lines_.end(); ++iter)
{
delete iter->second;
}
lines_.clear();
}
void Scene::clearTexts()
{
for(std::map<int, TextDrawable*>::iterator iter=texts_.begin(); iter!=texts_.end(); ++iter)
{
delete iter->second;
}
texts_.clear();
}
void Scene::clearQuads()
{
for(std::map<int, QuadColor*>::iterator iter=quads_.begin(); iter!=quads_.end(); ++iter)
{
delete iter->second;
}
quads_.clear();
}
void Scene::clearCircles()
{
for(std::map<int, tango_gl::Circle*>::iterator iter=circles_.begin(); iter!=circles_.end(); ++iter)
{
delete iter->second;
}
circles_.clear();
}
//Should only be called in OpenGL thread!
void Scene::SetupViewPort(int w, int h) {
if (h == 0) {
@@ -444,8 +484,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
std::vector<PointCloudDrawable*> cloudsToDraw(pointClouds_.size());
int oi=0;
int positiveCloudIds = 0;
for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
{
if(iter->first > 0)
{
positiveCloudIds++;
}
if(!mapRendering_ && iter->first > 0)
{
break;
@@ -487,7 +533,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
(blending_ &&
gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho &&
mapRendering_ && meshRendering_ &&
(cloudsToDraw.size() > 1 || (renderBackgroundCamera && wireFrame_)));
(positiveCloudIds > 1 || (renderBackgroundCamera && wireFrame_)));
if(onlineBlending && fboId_)
{
@@ -629,8 +675,17 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
cloud->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_, textureColorSeamsHidden_);
}
if(quads_.find(55556)!=quads_.end())
{
glEnable(GL_BLEND);
glDisable(GL_CULL_FACE);
const QuadColor * quad = quads_.at(55556);
quad->Render(projectionMatrix, viewMatrix);
glEnable(GL_CULL_FACE);
}
if(onlineBlending)
{
@@ -643,6 +698,57 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
glDepthMask(GL_TRUE);
}
///////
glDisable (GL_DEPTH_TEST);
if(lines_.size())
{
for(std::map<int, tango_gl::Line*>::const_iterator iter=lines_.begin(); iter!=lines_.end(); ++iter)
{
const tango_gl::Line * line = iter->second;
line->Render(projectionMatrix, viewMatrix);
}
}
if(quads_.size())
{
glEnable(GL_BLEND);
glDisable(GL_CULL_FACE);
for(std::map<int, QuadColor*>::const_iterator iter=quads_.begin(); iter!=quads_.end(); ++iter)
{
if(iter->first!=55556)
{
const QuadColor * quad = iter->second;
quad->Render(projectionMatrix, viewMatrix);
}
}
}
if(circles_.size())
{
glEnable(GL_BLEND);
glDisable(GL_CULL_FACE);
for(std::map<int, tango_gl::Circle*>::const_iterator iter=circles_.begin(); iter!=circles_.end(); ++iter)
{
const tango_gl::Circle * circle = iter->second;
circle->Render(projectionMatrix, viewMatrix);
}
}
if(texts_.size())
{
glDisable(GL_CULL_FACE);
glEnable(GL_BLEND);
glm::mat4 viewMatrixRotInv = viewMatrix;
viewMatrixRotInv[3][0] = 0;
viewMatrixRotInv[3][1] = 0;
viewMatrixRotInv[3][2] = 0;
viewMatrixRotInv = glm::inverse(viewMatrixRotInv);
for(std::map<int, TextDrawable*>::const_iterator iter=texts_.begin(); iter!=texts_.end(); ++iter)
{
const TextDrawable * text = iter->second;
text->Render(projectionMatrix, viewMatrix, viewMatrixRotInv);
}
}
//draw markers on foreground
for(std::map<int, tango_gl::Axis*>::const_iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
{
@@ -800,12 +906,7 @@ void Scene::addCloud(
const rtabmap::Transform & pose)
{
LOGI("add cloud %d (%d points %d indices)", id, (int)cloud->size(), indices.get()?(int)indices->size():0);
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
delete iter->second;
pointClouds_.erase(iter);
}
removeCloudOrMesh(id);
//create
PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
@@ -813,6 +914,16 @@ void Scene::addCloud(
pointClouds_.insert(std::make_pair(id, drawable));
}
void Scene::removeCloudOrMesh(int id)
{
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
delete iter->second;
pointClouds_.erase(iter);
}
}
void Scene::addMesh(
int id,
const rtabmap::Mesh & mesh,
@@ -820,12 +931,7 @@ void Scene::addMesh(
bool createWireframe)
{
LOGI("add mesh %d", id);
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
delete iter->second;
pointClouds_.erase(iter);
}
removeCloudOrMesh(id);
//create
PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
@@ -885,6 +991,160 @@ void Scene::addMesh(
}
}
void Scene::addLine(
int id,
const cv::Point3f & pt1,
const cv::Point3f & pt2,
const tango_gl::Color & color)
{
LOGI("add line %d", id);
removeLine(id);
//create
tango_gl::Line * line = new tango_gl::Line(2.0f, GL_LINES);
line->SetShader();
std::vector<glm::vec3> vertices(2);
vertices[0].x = pt1.x;
vertices[0].y = pt1.y;
vertices[0].z = pt1.z;
vertices[1].x = pt2.x;
vertices[1].y = pt2.y;
vertices[1].z = pt2.z;
line->UpdateLineVertices(vertices);
line->SetColor(color);
lines_.insert(std::make_pair(id, line));
}
void Scene::removeLine(int id)
{
std::map<int, tango_gl::Line*>::iterator iter=lines_.find(id);
if(iter != lines_.end())
{
delete iter->second;
lines_.erase(iter);
}
}
void Scene::addText(
int id,
const std::string & text,
const rtabmap::Transform & pose,
float size,
const tango_gl::Color & color)
{
LOGI("add text %d", id);
removeText(id);
//create
TextDrawable * textD = new TextDrawable(text, pose, size, color);
texts_.insert(std::make_pair(id, textD));
}
void Scene::removeText(int id)
{
std::map<int, TextDrawable*>::iterator iter=texts_.find(id);
if(iter != texts_.end())
{
delete iter->second;
texts_.erase(iter);
}
}
void Scene::addQuad(
int id,
float size,
const rtabmap::Transform & pose,
const tango_gl::Color & color,
float alpha)
{
//LOGI("add quad %d", id);
std::map<int, QuadColor*>::iterator iter=quads_.find(id);
if(iter != quads_.end())
{
delete iter->second;
quads_.erase(iter);
}
//create
QuadColor * quad = new QuadColor(size);
quad->SetTransformationMatrix(glmFromTransform(pose));
quad->SetColor(color);
quad->SetAlpha(alpha);
quads_.insert(std::make_pair(id, quad));
}
void Scene::addQuad(
int id,
float widthLeft,
float widthRight,
float heightBottom,
float heightTop,
const rtabmap::Transform & pose,
const tango_gl::Color & color,
float alpha)
{
//LOGI("add quad %d", id);
removeQuad(id);
//create
QuadColor * quad = new QuadColor(widthLeft, widthRight, heightBottom, heightTop);
quad->SetTransformationMatrix(glmFromTransform(pose));
quad->SetColor(color);
quad->SetAlpha(alpha);
quads_.insert(std::make_pair(id, quad));
}
void Scene::removeQuad(int id)
{
std::map<int, QuadColor*>::iterator iter=quads_.find(id);
if(iter != quads_.end())
{
delete iter->second;
quads_.erase(iter);
}
}
bool Scene::hasQuad(int id) const
{
return quads_.find(id) != quads_.end();
}
void Scene::addCircle(
int id,
float radius,
const rtabmap::Transform & pose,
const tango_gl::Color & color,
float alpha)
{
//LOGI("add quad %d", id);
std::map<int, tango_gl::Circle*>::iterator iter=circles_.find(id);
if(iter != circles_.end())
{
delete iter->second;
circles_.erase(iter);
}
//create
tango_gl::Circle * circle = new tango_gl::Circle(radius, 12);
circle->SetTransformationMatrix(glmFromTransform(pose));
circle->SetColor(color);
circle->SetAlpha(alpha);
circles_.insert(std::make_pair(id, circle));
}
void Scene::removeCircle(int id)
{
std::map<int, tango_gl::Circle*>::iterator iter=circles_.find(id);
if(iter != circles_.end())
{
delete iter->second;
circles_.erase(iter);
}
}
bool Scene::hasCircle(int id) const
{
return circles_.find(id) != circles_.end();
}
void Scene::setCloudPose(int id, const rtabmap::Transform & pose)
{

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

@@ -33,6 +33,7 @@
#include <tango-gl/trace.h>
#include <tango-gl/transform.h>
#include <tango-gl/util.h>
#include <tango-gl/circle.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Link.h>
@@ -41,12 +42,16 @@
#include "graph_drawable.h"
#include "bounding_box_drawable.h"
#include "background_renderer.h"
#include "text_drawable.h"
#include "quad_color.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// Scene provides OpenGL drawable objects and renders them for visualization.
class Scene {
public:
static const glm::vec3 kHeightOffset;
public:
// Constructor and destructor.
Scene();
@@ -67,6 +72,10 @@ class Scene {
void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
void clear(); // removed all point clouds
void clearLines();
void clearTexts();
void clearQuads();
void clearCircles();
// Render loop.
// @param: cur_pose_transformation, latest pose's transformation.
@@ -119,11 +128,50 @@ class Scene {
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const rtabmap::Transform & pose);
void removeCloudOrMesh(int id);
void addMesh(
int id,
const rtabmap::Mesh & mesh,
const rtabmap::Transform & pose,
bool createWireframe = false);
void addLine(
int id,
const cv::Point3f & pt1,
const cv::Point3f & pt2,
const tango_gl::Color & color = tango_gl::Color(1.0f, 1.0f, 1.0f));
void removeLine(int id);
void addText(
int id,
const std::string & text,
const rtabmap::Transform & pose,
float size,
const tango_gl::Color & color);
void removeText(int id);
void addQuad(
int id,
float size,
const rtabmap::Transform & pose,
const tango_gl::Color & color,
float alpha = 1.0f);
void addQuad(
int id,
float widthLeft,
float widthRight,
float heightBottom,
float heightTop,
const rtabmap::Transform & pose,
const tango_gl::Color & color,
float alpha =1.0f);
void removeQuad(int id);
bool hasQuad(int id) const;
void addCircle(
int id,
float radius,
const rtabmap::Transform & pose,
const tango_gl::Color & color,
float alpha = 1.0f);
void removeCircle(int id);
bool hasCircle(int id) const;
void setCloudPose(int id, const rtabmap::Transform & pose);
void setCloudVisible(int id, bool visible);
@@ -145,6 +193,7 @@ class Scene {
void setLighting(bool enabled) {lighting_ = enabled;}
void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
void setWireframe(bool enabled) {wireFrame_ = enabled;}
void setTextureColorSeamsHidden(bool hidden) {textureColorSeamsHidden_ = hidden;}
void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
void setGridColor(float r, float g, float b);
@@ -188,6 +237,10 @@ class Scene {
rtabmap::ScreenRotation color_camera_to_display_rotation_;
std::map<int, PointCloudDrawable*> pointClouds_;
std::map<int, tango_gl::Line*> lines_;
std::map<int, TextDrawable*> texts_;
std::map<int, QuadColor*> quads_;
std::map<int, tango_gl::Circle*> circles_;
rtabmap::Transform * currentPose_;
@@ -203,6 +256,7 @@ class Scene {
bool lighting_;
bool backfaceCulling_;
bool wireFrame_;
bool textureColorSeamsHidden_;
float r_;
float g_;
float b_;

2
app/android/jni/tango-gl/circle.cpp
View File

@@ -28,8 +28,8 @@ Circle::Circle(float radius, int resolution) : Mesh(GL_TRIANGLE_FAN){
for (int i = resolution; i >= 0; i--) {
float theta = delta_theta * static_cast<float>(i);
vertices.push_back(cos(theta) * radius);
vertices.push_back(0);
vertices.push_back(sin(theta) * radius);
vertices.push_back(0);
}
SetVertices(vertices);
}

366
app/android/jni/text_drawable.cpp Normal file
View File

@@ -0,0 +1,366 @@
/*
Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifdef __ANDROID__
#include <jni.h>
#endif
#include <tango-gl/util.h>
#include <vector>
#include <rtabmap/core/Transform.h>
#include "util.h"
#include "text_drawable.h"
#include "text_atlas_png.h"
#include <rtabmap/utilite/UConversion.h>
const std::string kTextVertexShader =
"uniform mat4 uMVPMatrix;"
"attribute vec4 vPosition;"
"attribute vec2 a_texCoord;"
"varying vec2 v_texCoord;"
"void main() {"
" gl_Position = uMVPMatrix * vPosition;"
" v_texCoord = a_texCoord;"
"}";
const std::string kTextFragmentShader =
"precision mediump float;"
"uniform vec3 uColor;"
"varying vec2 v_texCoord;"
"uniform sampler2D s_texture;"
"void main() {"
" gl_FragColor = texture2D( s_texture, v_texCoord );"
" gl_FragColor.rgb = uColor;"
"}";
const int RI_TEXT_TEXTURE_SIZE = 512; // 512
const float RI_TEXT_HEIGHT_BASE = 32.0f;
const char RI_TEXT_START = ' ';
const char RI_TEXT_STOP = '~'+1;
GLuint TextDrawable::textProgram_ = 0;
GLuint TextDrawable::textTextureId_ = 0;
float TextDrawable::textUVWidth_ = 0;
float TextDrawable::textUVHeight_ = 0;
float TextDrawable::textHeight_ = 0;
std::vector<float> TextDrawable::textCharacterWidths_;
void TextDrawable::createShaderProgram()
{
releaseShaderProgram();
// hard-coded with text_atlas.png resource
textCharacterWidths_.resize(95);
textCharacterWidths_[0]=8;
textCharacterWidths_[1]=9.000000;
textCharacterWidths_[2]=10.000000;
textCharacterWidths_[3]=19.000000;
textCharacterWidths_[4]=18.000000;
textCharacterWidths_[5]=24.000000;
textCharacterWidths_[6]=21.000000;
textCharacterWidths_[7]=5.000000;
textCharacterWidths_[8]=11.000000;
textCharacterWidths_[9]=11.000000;
textCharacterWidths_[10]=15.000000;
textCharacterWidths_[11]=17.000000;
textCharacterWidths_[12]=8.000000;
textCharacterWidths_[13]=12.000000;
textCharacterWidths_[14]=9.000000;
textCharacterWidths_[15]=12.000000;
textCharacterWidths_[16]=18.000000;
textCharacterWidths_[17]=18.000000;
textCharacterWidths_[18]=18.000000;
textCharacterWidths_[19]=18.000000;
textCharacterWidths_[20]=18.000000;
textCharacterWidths_[21]=18.000000;
textCharacterWidths_[22]=18.000000;
textCharacterWidths_[23]=18.000000;
textCharacterWidths_[24]=18.000000;
textCharacterWidths_[25]=18.000000;
textCharacterWidths_[26]=9.000000;
textCharacterWidths_[27]=8.000000;
textCharacterWidths_[28]=16.000000;
textCharacterWidths_[29]=18.000000;
textCharacterWidths_[30]=17.000000;
textCharacterWidths_[31]=16.000000;
textCharacterWidths_[32]=29.000000;
textCharacterWidths_[33]=22.000000;
textCharacterWidths_[34]=20.000000;
textCharacterWidths_[35]=21.000000;
textCharacterWidths_[36]=21.000000;
textCharacterWidths_[37]=18.000000;
textCharacterWidths_[38]=18.000000;
textCharacterWidths_[39]=22.000000;
textCharacterWidths_[40]=23.000000;
textCharacterWidths_[41]=9.000000;
textCharacterWidths_[42]=18.000000;
textCharacterWidths_[43]=20.000000;
textCharacterWidths_[44]=17.000000;
textCharacterWidths_[45]=28.000000;
textCharacterWidths_[46]=23.000000;
textCharacterWidths_[47]=22.000000;
textCharacterWidths_[48]=21.000000;
textCharacterWidths_[49]=22.000000;
textCharacterWidths_[50]=20.000000;
textCharacterWidths_[51]=20.000000;
textCharacterWidths_[52]=20.000000;
textCharacterWidths_[53]=21.000000;
textCharacterWidths_[54]=21.000000;
textCharacterWidths_[55]=28.000000;
textCharacterWidths_[56]=20.000000;
textCharacterWidths_[57]=20.000000;
textCharacterWidths_[58]=19.000000;
textCharacterWidths_[59]=9.000000;
textCharacterWidths_[60]=14.000000;
textCharacterWidths_[61]=9.000000;
textCharacterWidths_[62]=14.000000;
textCharacterWidths_[63]=14.000000;
textCharacterWidths_[64]=11.000000;
textCharacterWidths_[65]=17.000000;
textCharacterWidths_[66]=18.000000;
textCharacterWidths_[67]=17.000000;
textCharacterWidths_[68]=18.000000;
textCharacterWidths_[69]=17.000000;
textCharacterWidths_[70]=11.000000;
textCharacterWidths_[71]=18.000000;
textCharacterWidths_[72]=18.000000;
textCharacterWidths_[73]=8.000000;
textCharacterWidths_[74]=8.000000;
textCharacterWidths_[75]=17.000000;
textCharacterWidths_[76]=8.000000;
textCharacterWidths_[77]=28.000000;
textCharacterWidths_[78]=18.000000;
textCharacterWidths_[79]=18.000000;
textCharacterWidths_[80]=18.000000;
textCharacterWidths_[81]=18.000000;
textCharacterWidths_[82]=12.000000;
textCharacterWidths_[83]=16.000000;
textCharacterWidths_[84]=11.000000;
textCharacterWidths_[85]=18.000000;
textCharacterWidths_[86]=16.000000;
textCharacterWidths_[87]=24.000000;
textCharacterWidths_[88]=16.000000;
textCharacterWidths_[89]=16.000000;
textCharacterWidths_[90]=16.000000;
textCharacterWidths_[91]=11.000000;
textCharacterWidths_[92]=8.000000;
textCharacterWidths_[93]=11.000000;
textCharacterWidths_[94]=21.000000;
textUVWidth_=0.062500;
textUVHeight_=0.082031;
textHeight_=42.000000;
textProgram_ = tango_gl::util::CreateProgram(kTextVertexShader.c_str(), kTextFragmentShader.c_str());
UASSERT(textProgram_ != 0);
glGenTextures(1, &textTextureId_);
UASSERT(textTextureId_);
// gen texture from image
glBindTexture(GL_TEXTURE_2D, textTextureId_);
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);
std::vector<char> data = uHex2Bytes(rtabmap::TEXT_ATLAS_PNG);
cv::Mat rgbImage = cv::imdecode(data, cv::IMREAD_UNCHANGED);
glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
GLint error = glGetError();
UASSERT(error == GL_NO_ERROR);
}
void TextDrawable::releaseShaderProgram()
{
if(textProgram_)
{
glDeleteShader(textProgram_);
textProgram_ = 0;
}
}
TextDrawable::TextDrawable(
const std::string & text,
const rtabmap::Transform & pose,
float textSize, // meters
const tango_gl::Color & color) :
poseGl_(glmFromTransform(pose)),
color_(color)
{
if(textProgram_ > 0)
{
vertexBuffer_ = std::vector<float>(text.size() * 12);
textureBuffer_ = std::vector<float>(text.size() * 8);
drawListBuffer_ = std::vector<unsigned short>(text.size() * 6);
int index_vecs = 0;
int index_indices = 0;
int index_uvs = 0;
float x=0.0f;
float y=0.0f;
float uniformscale = textSize/textHeight_;
for(unsigned int i=0; i<text.size(); ++i)
{
// get ascii value
char c = text[i];
int c_val = (int)c;
int indx = c_val-RI_TEXT_START;
if(indx<0 || indx>=textCharacterWidths_.size()) {
// unknown character, we will add a space for it to be save.
indx = 0;
}
int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
// Calculate the uv parts
int row = indx / colCount;
int col = indx % colCount;
float v = row * textUVHeight_;
float v2 = v + textUVHeight_;
float u = col * textUVWidth_;
float u2 = u + textCharacterWidths_[indx]/(float)RI_TEXT_TEXTURE_SIZE;
// Creating the triangle information
std::vector<float> vec(12);
std::vector<float> uv(8);
vec[0] = x;
vec[1] = y + (textHeight_ * uniformscale);
vec[2] = 0;
vec[3] = x;
vec[4] = y;
vec[5] = 0;
vec[6] = x + (textCharacterWidths_[indx] * uniformscale);
vec[7] = y;
vec[8] = 0;
vec[9] = x + (textCharacterWidths_[indx] * uniformscale);
vec[10] = y + (textHeight_ * uniformscale);
vec[11] = 0;
// 0.001f = texture bleeding hack/fix
uv[0] = u+0.001f;
uv[1] = v+0.001f;
uv[2] = u+0.001f;
uv[3] = v2-0.001f;
uv[4] = u2-0.001f;
uv[5] = v2-0.001f;
uv[6] = u2-0.001f;
uv[7] = v+0.001f;
unsigned short inds[6] = {0, 1, 2, 0, 2, 3};
// We need a base value because the object has indices related to
// that object and not to this collection so basicly we need to
// translate the indices to align with the vertexlocation in ou
// vecs array of vectors.
short base = (short) (index_vecs / 3);
// We should add the vec, translating the indices to our saved vector
for(int i=0;i<vec.size();i++)
{
vertexBuffer_[index_vecs] = vec[i];
index_vecs++;
}
// We should add the uvs
for(int i=0;i<uv.size();i++)
{
textureBuffer_[index_uvs] = uv[i];
index_uvs++;
}
// We handle the indices
for(int j=0;j<6;j++)
{
drawListBuffer_[index_indices] = (unsigned short) (base + inds[j]);
index_indices++;
}
// Calculate the new position
x += (textCharacterWidths_[indx] * uniformscale);
}
}
}
void TextDrawable::Render(
const glm::mat4 & projectionMatrix,
const glm::mat4 & viewMatrix,
const glm::mat4 & viewMatrixRotInv) const
{
glUseProgram(textProgram_);
// get handle to vertex shader's vPosition member
int mPositionHandle = glGetAttribLocation(textProgram_, "vPosition");
// Enable a handle to the triangle vertices
glEnableVertexAttribArray(mPositionHandle);
// Prepare the background coordinate data
glVertexAttribPointer(mPositionHandle, 3, GL_FLOAT, false, 0, &vertexBuffer_[0]);
int mTexCoordLoc = glGetAttribLocation(textProgram_, "a_texCoord" );
// Prepare the texturecoordinates
glVertexAttribPointer ( mTexCoordLoc, 2, GL_FLOAT, false, 0, &textureBuffer_[0]);
glEnableVertexAttribArray ( mPositionHandle );
glEnableVertexAttribArray ( mTexCoordLoc );
// get handle to shape's transformation matrix
int mtrxhandle = glGetUniformLocation(textProgram_, "uMVPMatrix");
// Apply the projection and view transformation
//glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
glm::mat4 mvp_mat = projectionMatrix * viewMatrix * poseGl_ * viewMatrixRotInv;//glm::toMat4(gesture_camera_->GetParent()->GetRotation());
glUniformMatrix4fv(mtrxhandle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
// get handle to color value
int colorhandle = glGetUniformLocation(textProgram_, "uColor");
glUniform3f(colorhandle, color_.r, color_.g, color_.b);
int mSamplerLoc = glGetUniformLocation (textProgram_, "s_texture" );
// Texture activate unit 0
glActiveTexture(GL_TEXTURE0);
// Bind the texture to this unit.
glBindTexture(GL_TEXTURE_2D, textTextureId_);
// Set the sampler texture unit to our selected id
glUniform1i ( mSamplerLoc, 0);
// Draw the triangle
glDrawElements(GL_TRIANGLES, drawListBuffer_.size(), GL_UNSIGNED_SHORT, &drawListBuffer_[0]);
// Disable vertex array
glDisableVertexAttribArray(mPositionHandle);
glDisableVertexAttribArray(mTexCoordLoc);
}

71
app/android/jni/text_drawable.h Normal file
View File

@@ -0,0 +1,71 @@
/*
Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef TANGO_TEXT_DRAWABLE_H_
#define TANGO_TEXT_DRAWABLE_H_
#include <vector>
#include <rtabmap/core/Transform.h>
#include <tango-gl/color.h>
// PointCloudDrawable is responsible for the point cloud rendering.
class TextDrawable {
private:
static GLuint textProgram_;
static GLuint textTextureId_;
static float textUVWidth_;
static float textUVHeight_;
static float textHeight_;
static std::vector<float> textCharacterWidths_;
public:
static void createShaderProgram();
static void releaseShaderProgram();
public:
TextDrawable(
const std::string & text,
const rtabmap::Transform & pose,
float textSize = 0.1f, // meters
const tango_gl::Color & color = tango_gl::Color(1,0,0));
virtual ~TextDrawable() {}
void Render(
const glm::mat4 & projectionMatrix,
const glm::mat4 & viewMatrix,
const glm::mat4 & viewMatrixRotInv) const;
private:
std::vector<float> vertexBuffer_;
std::vector<float> textureBuffer_;
std::vector<unsigned short> drawListBuffer_;
glm::mat4 poseGl_;
tango_gl::Color color_;
};
#endif // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_