diff --git a/app/android/jni/CMakeLists.txt b/app/android/jni/CMakeLists.txt index f00b604e..bcb5c710 100644 --- a/app/android/jni/CMakeLists.txt +++ b/app/android/jni/CMakeLists.txt @@ -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 diff --git a/app/android/jni/CameraMobile.cpp b/app/android/jni/CameraMobile.cpp index 316dd480..edb3e7f2 100644 --- a/app/android/jni/CameraMobile.cpp +++ b/app/android/jni/CameraMobile.cpp @@ -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; } diff --git a/app/android/jni/CameraMobile.h b/app/android/jni/CameraMobile.h index 6d1e2b6e..28bd85ab 100644 --- a/app/android/jni/CameraMobile.h +++ b/app/android/jni/CameraMobile.h @@ -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 previousAnchorLinearVelocity_; diff --git a/app/android/jni/Measure.h b/app/android/jni/Measure.h new file mode 100644 index 00000000..cd348710 --- /dev/null +++ b/app/android/jni/Measure.h @@ -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 + +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_ */ diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 0e846fdb..29280963 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -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), + quadSample_(new pcl::PointCloud), + 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 > > 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); - mesh.normals.reset(new pcl::PointCloud); - 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); + optMesh_.normals.reset(new pcl::PointCloud); + 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); // null pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - 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 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::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::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, diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 67a7ab6d..8fcff2ed 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#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 filterOrganizedPolygons(const std::vector & 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 measures_; // In opengl frame + bool measuresUpdated_; + bool metricSystem_; + float snapAxisThr_; + std::vector snapAxes_; + bool markerDetection_; + int measuringMode_; + bool addMeasureClicked_; + bool teleportClicked_; + bool removeMeasureClicked_; + std::vector measuringTmpPts_; // In opengl frame + std::vector measuringTmpNormals_; // In opengl frame + pcl::PointCloud::Ptr targetPoint_; + pcl::PointCloud::Ptr quadSample_; + std::vector quadSamplePolygons_; // main_scene_ includes all drawable object for visualizing Tango device's // movement and point cloud. Scene main_scene_; diff --git a/app/android/jni/quad_color.cpp b/app/android/jni/quad_color.cpp new file mode 100644 index 00000000..f3a0d1e3 --- /dev/null +++ b/app/android/jni/quad_color.cpp @@ -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); +} diff --git a/app/android/jni/quad_color.h b/app/android/jni/quad_color.h new file mode 100644 index 00000000..825ffb15 --- /dev/null +++ b/app/android/jni/quad_color.h @@ -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_ diff --git a/app/android/jni/resources/text_atlas.png b/app/android/jni/resources/text_atlas.png new file mode 100644 index 00000000..0efda713 Binary files /dev/null and b/app/android/jni/resources/text_atlas.png differ diff --git a/app/android/jni/scene.cpp b/app/android/jni/scene.cpp index 5890c684..b3f4d953 100644 --- a/app/android/jni/scene.cpp +++ b/app/android/jni/scene.cpp @@ -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::iterator iter=lines_.begin(); iter!=lines_.end(); ++iter) + { + delete iter->second; + } + lines_.clear(); +} +void Scene::clearTexts() +{ + for(std::map::iterator iter=texts_.begin(); iter!=texts_.end(); ++iter) + { + delete iter->second; + } + texts_.clear(); +} +void Scene::clearQuads() +{ + for(std::map::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::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::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::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::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::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::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::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 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::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::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::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::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) { diff --git a/app/android/jni/scene.h b/app/android/jni/scene.h index f241c12b..d217f31c 100644 --- a/app/android/jni/scene.h +++ b/app/android/jni/scene.h @@ -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 #include // 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::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 pointClouds_; + std::map lines_; + std::map texts_; + std::map quads_; rtabmap::Transform * currentPose_; diff --git a/app/android/jni/text_drawable.cpp b/app/android/jni/text_drawable.cpp new file mode 100644 index 00000000..6171a4a7 --- /dev/null +++ b/app/android/jni/text_drawable.cpp @@ -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 +#endif + +#include +#include +#include +#include "util.h" +#include "text_drawable.h" +#include "text_atlas_png.h" +#include + +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 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 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(text.size() * 12); + textureBuffer_ = std::vector(text.size() * 8); + drawListBuffer_ = std::vector(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=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 vec(12); + std::vector 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;iGetParent()->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); +} diff --git a/app/android/jni/text_drawable.h b/app/android/jni/text_drawable.h new file mode 100644 index 00000000..4a6869be --- /dev/null +++ b/app/android/jni/text_drawable.h @@ -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 +#include +#include + +// 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 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 vertexBuffer_; + std::vector textureBuffer_; + std::vector drawListBuffer_; + glm::mat4 poseGl_; + tango_gl::Color color_; +}; + +#endif // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_ diff --git a/app/ios/RTABMapApp.xcodeproj/project.pbxproj b/app/ios/RTABMapApp.xcodeproj/project.pbxproj index e2406206..9fee832a 100644 --- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj +++ b/app/ios/RTABMapApp.xcodeproj/project.pbxproj @@ -8,6 +8,8 @@ /* Begin PBXBuildFile section */ 4E0D83832621F52C00C879AC /* Settings.bundle in Resources */ = {isa = PBXBuildFile; fileRef = 4E0D83822621F52C00C879AC /* Settings.bundle */; }; + 4E1E9CF72D618E69000CB881 /* quad_color.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4E1E9CF62D618079000CB881 /* quad_color.cpp */; }; + 4E1E9CF82D618E69000CB881 /* text_drawable.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4E1E9CF52D618079000CB881 /* text_drawable.cpp */; }; 4E20B24E266AB94300316EE6 /* Images.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4E20B24D266AB94300316EE6 /* Images.xcassets */; }; 4E20B250266AB95600316EE6 /* Images.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4E20B24F266AB95600316EE6 /* Images.xcassets */; }; 4E2C516725A63119005CEDBD /* DatabaseView.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4E2C516525A63119005CEDBD /* DatabaseView.swift */; }; @@ -156,6 +158,12 @@ /* Begin PBXFileReference section */ 4E0D83822621F52C00C879AC /* Settings.bundle */ = {isa = PBXFileReference; lastKnownFileType = "wrapper.plug-in"; path = Settings.bundle; sourceTree = ""; }; + 4E1E9CF22D617D08000CB881 /* Measure.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = Measure.h; path = ../android/jni/Measure.h; sourceTree = ""; }; + 4E1E9CF32D618079000CB881 /* quad_color.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = quad_color.h; path = ../android/jni/quad_color.h; sourceTree = ""; }; + 4E1E9CF42D618079000CB881 /* text_drawable.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = text_drawable.h; path = ../android/jni/text_drawable.h; sourceTree = ""; }; + 4E1E9CF52D618079000CB881 /* text_drawable.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; name = text_drawable.cpp; path = ../android/jni/text_drawable.cpp; sourceTree = ""; }; + 4E1E9CF62D618079000CB881 /* quad_color.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; name = quad_color.cpp; path = ../android/jni/quad_color.cpp; sourceTree = ""; }; + 4E1E9CF92D619025000CB881 /* text_atlas_png.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = text_atlas_png.h; path = RTABMapApp/text_atlas_png.h; sourceTree = ""; }; 4E20B24D266AB94300316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = SOURCE_ROOT; }; 4E20B24F266AB95600316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = ""; }; 4E2C516525A63119005CEDBD /* DatabaseView.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = DatabaseView.swift; path = RTABMapApp/DatabaseView.swift; sourceTree = ""; }; @@ -679,6 +687,12 @@ 4EFD0BB0259D503200575D88 /* NativeWrapper */ = { isa = PBXGroup; children = ( + 4E1E9CF92D619025000CB881 /* text_atlas_png.h */, + 4E1E9CF62D618079000CB881 /* quad_color.cpp */, + 4E1E9CF32D618079000CB881 /* quad_color.h */, + 4E1E9CF52D618079000CB881 /* text_drawable.cpp */, + 4E1E9CF42D618079000CB881 /* text_drawable.h */, + 4E1E9CF22D617D08000CB881 /* Measure.h */, 4EFD0F6E259F847300575D88 /* background_renderer.cc */, 4EFD0F6F259F847300575D88 /* background_renderer.h */, 4EFD0F66259E38DE00575D88 /* NativeWrapper.cpp */, @@ -800,6 +814,8 @@ isa = PBXSourcesBuildPhase; buildActionMask = 2147483647; files = ( + 4E1E9CF72D618E69000CB881 /* quad_color.cpp in Sources */, + 4E1E9CF82D618E69000CB881 /* text_drawable.cpp in Sources */, 4EFD0F70259F847300575D88 /* background_renderer.cc in Sources */, 4EFD0BC0259D50A800575D88 /* scene.cpp in Sources */, 4E2C516825A63119005CEDBD /* VerticalScrollerView.swift in Sources */, diff --git a/app/ios/RTABMapApp/Base.lproj/Main.storyboard b/app/ios/RTABMapApp/Base.lproj/Main.storyboard index 3b796cb3..ef737a3a 100644 --- a/app/ios/RTABMapApp/Base.lproj/Main.storyboard +++ b/app/ios/RTABMapApp/Base.lproj/Main.storyboard @@ -18,8 +18,8 @@ - - + + + + @@ -83,17 +109,6 @@ - + + + + + - + - + + + + + @@ -196,17 +264,22 @@ + + + + + @@ -230,7 +303,7 @@