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

Added measuring tool

Browse Source
This commit is contained in:
matlabbe
2025-02-17 20:20:19 -08:00
parent 994fad982f
commit 26d1238c8a
20 changed files with 1878 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -114,7 +114,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_ */

650
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")));
@@ -264,13 +265,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),
snapAxisThr_(0.95),
markerDetection_(false),
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 +309,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 +422,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 +439,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 +458,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;
@@ -976,6 +999,11 @@ bool RTABMapApp::startCamera()
LOGI("Cloud density level %d", cloudDensityLevel_);
LOGI("Start camera thread");
if(visualizingMesh_)
{
// We are measuring!
main_scene_.setMapRendering(false);
}
cameraJustInitialized_ = true;
if(useExternalLidar_)
{
@@ -1330,7 +1358,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 +1453,158 @@ 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);
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_.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();
int lineId = 0;
int textId = 0;
int quadId = 0;
float sphereRadius = 0.02f;
float textSize=0.05f;
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), textSize, 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]));
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);
}
}
if(!openingDatabase_)
{
@@ -1576,7 +1723,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 +1787,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 +2420,358 @@ 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());
float textSize=0.05f;
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 quadSize=0.05f;
float quadAlpha = 0.3f;
if(removeMeasureClicked_)
{
if(!measuringTmpPts_.empty())
{
main_scene_.removeLine(55555);
main_scene_.removeText(55555);
main_scene_.removeQuad(55555);
main_scene_.removeQuad(55556);
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);
main_scene_.removeLine(55555);
main_scene_.removeText(55555);
main_scene_.removeQuad(55555);
main_scene_.removeQuad(55556);
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;
// project point on line
if(measuringMode_ == 0 && measuringTmpPts_.size() == 1)
{
cv::Point3f v = pt2-measuringTmpPts_.front();
float n = v.dot(measuringTmpNormals_.front());
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;
}
}
/*std::string text = uFormat("%0.1f %0.1f %0.1f [%0.2f %0.2f %0.2f %0.2f]", v.x, v.y, v.z,
quadWidthLeft,
quadWidthRight,
quadHeightBottom,
quadHeightTop);
main_scene_.addText(55555, text, rtabmap::Transform(pt.x, pt.y, pt.z, 0,0,0), textSize, 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, quadColor);
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
{
main_scene_.addQuad(55555,
quadWidthLeft,
quadWidthRight,
quadHeightBottom,
quadHeightTop,
rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
}
}
}
else
{
main_scene_.removeText(55555);
main_scene_.removeQuad(55555);
main_scene_.removeQuad(55556);
}
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 +2839,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)
@@ -3492,7 +4003,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 +4025,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;
@@ -3799,6 +4312,57 @@ 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::postOdometryEvent(
rtabmap::Transform pose,
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,

30
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();
@@ -180,6 +181,13 @@ class RTABMapApp : public UEventsHandler {
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 postOdometryEvent(
rtabmap::Transform pose,
@@ -203,6 +211,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 +273,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 snapAxisThr_;
std::vector<cv::Vec3f> snapAxes_;
bool markerDetection_;
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_;

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

222
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);
@@ -124,6 +124,7 @@ void Scene::InitGLContent()
UASSERT(axis_ == 0);
TextDrawable::createShaderProgram();
axis_ = new tango_gl::Axis();
frustum_ = new tango_gl::Frustum();
@@ -166,6 +167,7 @@ void Scene::DeleteResources() {
background_renderer_ = 0;
}
TextDrawable::releaseShaderProgram();
PointCloudDrawable::releaseShaderPrograms();
if (graph_shader_program_) {
@@ -198,6 +200,9 @@ void Scene::clear()
{
delete iter->second;
}
clearLines();
clearQuads();
clearTexts();
if(trace_)
{
trace_->ClearVertexArray();
@@ -215,6 +220,31 @@ 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();
}
//Should only be called in OpenGL thread!
void Scene::SetupViewPort(int w, int h) {
if (h == 0) {
@@ -631,6 +661,15 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
}
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 +682,47 @@ 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(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 +880,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 +888,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 +905,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 +965,122 @@ 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::setCloudPose(int id, const rtabmap::Transform & pose)
{

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

@@ -41,12 +41,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 +71,9 @@ 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();
// Render loop.
// @param: cur_pose_transformation, latest pose's transformation.
@@ -119,11 +126,42 @@ 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 setCloudPose(int id, const rtabmap::Transform & pose);
void setCloudVisible(int id, bool visible);
@@ -188,6 +226,9 @@ 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_;
rtabmap::Transform * currentPose_;

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_