diff --git a/CMakeLists.txt b/CMakeLists.txt index b9afa1667b..3471c85c3d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 21) -SET(RTABMAP_PATCH_VERSION 6) +SET(RTABMAP_PATCH_VERSION 7) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) @@ -177,6 +177,7 @@ option(WITH_TORCH "Include Torch support (SuperPoint)" OFF) option(WITH_PYTHON "Include Python3 support (PyMatcher, PyDetector)" OFF) option(WITH_PYTHON_THREADING "Use more than one Python interpreter." OFF) option(WITH_PDAL "Include PDAL support" ON) +option(WITH_LIBLAS "Include libLAS support" OFF) option(WITH_CUDASIFT "Include CudaSift support (this fork https://github.com/matlabbe/CudaSift)" OFF) option(WITH_FREENECT "Include Freenect support" ON) option(WITH_FREENECT2 "Include Freenect2 support" ON) @@ -416,6 +417,13 @@ IF(WITH_PDAL) ENDIF(PDAL_FOUND) ENDIF(WITH_PDAL) +IF(WITH_LIBLAS) + FIND_PACKAGE(libLAS QUIET) + IF(libLAS_FOUND) + MESSAGE(STATUS "Found libLAS ${libLAS_VERSION}: ${libLAS_INCLUDE_DIRS}") + ENDIF(libLAS_FOUND) +ENDIF(WITH_LIBLAS) + IF(WITH_CUDASIFT) FIND_PACKAGE(CudaSift 3 QUIET) IF(CudaSift_FOUND) @@ -950,6 +958,9 @@ ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV) IF(NOT PDAL_FOUND) SET(PDAL "//") ENDIF(NOT PDAL_FOUND) +IF(NOT libLAS_FOUND) + SET(LIBLAS "//") +ENDIF(NOT libLAS_FOUND) IF(NOT CudaSift_FOUND) SET(CUDASIFT "//") ENDIF(NOT CudaSift_FOUND) @@ -1427,6 +1438,14 @@ ELSE() MESSAGE(STATUS " With PDAL = NO (PDAL not found)") ENDIF() +IF(libLAS_FOUND) +MESSAGE(STATUS " With libLAS ${libLAS_VERSION} = YES (License: BSD)") +ELSEIF(NOT WITH_LIBLAS) +MESSAGE(STATUS " With libLAS = NO (WITH_LIBLAS=OFF)") +ELSE() +MESSAGE(STATUS " With libLAS = NO (libLAS not found)") +ENDIF() + IF(CudaSift_FOUND) MESSAGE(STATUS " With CudaSift = YES (License: MIT)") ELSEIF(NOT WITH_CUDASIFT) diff --git a/Version.h.in b/Version.h.in index b2b9b3e89d..29feac8b27 100644 --- a/Version.h.in +++ b/Version.h.in @@ -58,6 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @FASTCV@#define RTABMAP_FASTCV @OPENGV@#define RTABMAP_OPENGV @PDAL@#define RTABMAP_PDAL +@LIBLAS@#define RTABMAP_LIBLAS @CUDASIFT@#define RTABMAP_CUDASIFT @LOAM@#define RTABMAP_LOAM @FLOAM@#define RTABMAP_FLOAM diff --git a/app/android/jni/CameraARCore.cpp b/app/android/jni/CameraARCore.cpp index a0888685c5..a670497e13 100644 --- a/app/android/jni/CameraARCore.cpp +++ b/app/android/jni/CameraARCore.cpp @@ -37,8 +37,8 @@ namespace rtabmap { ////////////////////////////// // CameraARCore ////////////////////////////// -CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing): - CameraMobile(smoothing), +CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing, float upstreamRelocalizationAccThr): + CameraMobile(smoothing, upstreamRelocalizationAccThr), env_(env), context_(context), activity_(activity), @@ -125,6 +125,8 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string { close(); + CameraMobile::init(calibrationFolder, cameraName); + UScopeMutex lock(arSessionMutex_); ArInstallStatus install_status; @@ -272,54 +274,6 @@ void CameraARCore::close() CameraMobile::close(); } -LaserScan CameraARCore::scanFromPointCloudData( - const float * pointCloudData, - int points, - const Transform & pose, - const CameraModel & model, - const cv::Mat & rgb, - std::vector * kpts, - std::vector * kpts3D) -{ - if(pointCloudData && points>0) - { - cv::Mat scanData(1, points, CV_32FC4); - float * ptr = scanData.ptr(); - for(unsigned int i=0;i(v,u).val[0]; - g=rgb.at(v,u).val[1]; - r=rgb.at(v,u).val[2]; - if(kpts) - kpts->push_back(cv::KeyPoint(u,v,3)); - if(kpts3D) - kpts3D->push_back(org); - } - *(int*)&ptr[i*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16); - - //confidence - //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16); - - } - return LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity()); - } - return LaserScan(); -} - void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) { CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height); @@ -385,12 +339,6 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose) /*near=*/0.1f, /*far=*/100.f, glm::value_ptr(projectionMatrix_)); - // adjust origin - if(!getOriginOffset().isNull()) - { - viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); - } - ArTrackingState camera_tracking_state; ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); @@ -402,11 +350,12 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose) ArCamera_getPose(arSession_, ar_camera, arPose_); ArPose_getPoseRaw(arSession_, arPose_, pose_raw); Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - poseArCore = rtabmap::rtabmap_world_T_opengl_world * poseArCore * rtabmap::opengl_world_T_rtabmap_world; + pose = rtabmap::rtabmap_world_T_opengl_world * poseArCore * rtabmap::opengl_world_T_rtabmap_world; - if(poseArCore.isNull()) + if(pose.isNull()) { LOGE("CameraARCore: Pose is null"); + return data; } // Get calibration parameters @@ -530,7 +479,11 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose) #endif if(pointCloudData && points>0) { - scan = scanFromPointCloudData(pointCloudData, points, poseArCore, model, rgb, &kpts, &kpts3); + cv::Mat pointCloudDataMat(1, points, CV_32FC4, (void *)pointCloudData); + scan = scanFromPointCloudData(pointCloudDataMat, pose, model, rgb, &kpts, &kpts3); +#ifndef DISABLE_LOG + LOGI("valid scan points = %d", scan.size()); +#endif } } else @@ -541,15 +494,9 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose) data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp); data.setFeatures(kpts, kpts3, cv::Mat()); - if(!poseArCore.isNull()) + if(!pose.isNull()) { - pose = poseArCore; this->poseReceived(pose, stamp); - // adjust origin - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } } } } diff --git a/app/android/jni/CameraARCore.h b/app/android/jni/CameraARCore.h index 429fa3fc6c..b6f1bb0798 100644 --- a/app/android/jni/CameraARCore.h +++ b/app/android/jni/CameraARCore.h @@ -50,17 +50,7 @@ namespace rtabmap { class CameraARCore : public CameraMobile { public: - static LaserScan scanFromPointCloudData( - const float * pointCloudData, - int points, - const Transform & pose, - const CameraModel & model, - const cv::Mat & rgb, - std::vector * kpts = 0, - std::vector * kpts3D = 0); - -public: - CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false); + CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f); virtual ~CameraARCore(); virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height); diff --git a/app/android/jni/CameraAREngine.cpp b/app/android/jni/CameraAREngine.cpp index 5d10ecf917..2e24737575 100644 --- a/app/android/jni/CameraAREngine.cpp +++ b/app/android/jni/CameraAREngine.cpp @@ -40,8 +40,8 @@ namespace rtabmap { ////////////////////////////// // CameraAREngine ////////////////////////////// -CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing): - CameraMobile(smoothing), +CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing, float upstreamRelocalizationAccThr): + CameraMobile(smoothing, upstreamRelocalizationAccThr), env_(env), context_(context), activity_(activity), @@ -66,6 +66,8 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri { close(); + CameraMobile::init(calibrationFolder, cameraName); + UScopeMutex lock(arSessionMutex_); HwArInstallStatus install_status; @@ -236,12 +238,6 @@ SensorData CameraAREngine::updateDataOnRender(Transform & pose) /*near=*/0.1f, /*far=*/100.f, glm::value_ptr(projectionMatrix_)); - // adjust origin - if(!getOriginOffset().isNull()) - { - viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); - } - HwArTrackingState camera_tracking_state; HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); @@ -334,11 +330,6 @@ SensorData CameraAREngine::updateDataOnRender(Transform & pose) { pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; this->poseReceived(pose, stamp); - // adjust origin - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } } } } diff --git a/app/android/jni/CameraAREngine.h b/app/android/jni/CameraAREngine.h index 03f592191e..3c221bda95 100644 --- a/app/android/jni/CameraAREngine.h +++ b/app/android/jni/CameraAREngine.h @@ -46,7 +46,7 @@ namespace rtabmap { class CameraAREngine : public CameraMobile { public: - CameraAREngine(void* env, void* context, void* activity, bool smoothing = false); + CameraAREngine(void* env, void* context, void* activity, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f); virtual ~CameraAREngine(); virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height); diff --git a/app/android/jni/CameraMobile.cpp b/app/android/jni/CameraMobile.cpp index cd7c4f140e..316dd4801b 100644 --- a/app/android/jni/CameraMobile.cpp +++ b/app/android/jni/CameraMobile.cpp @@ -18,6 +18,7 @@ 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 +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 @@ -52,7 +53,7 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform( 0.0f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f); -CameraMobile::CameraMobile(bool smoothing) : +CameraMobile::CameraMobile(bool smoothing, float upstreamRelocalizationAccThr) : Camera(10), deviceTColorCamera_(Transform::getIdentity()), textureId_(0), @@ -60,7 +61,10 @@ CameraMobile::CameraMobile(bool smoothing) : stampEpochOffset_(0.0), smoothing_(smoothing), colorCameraToDisplayRotation_(ROTATION_0), - originUpdate_(false) + originUpdate_(true), + upstreamRelocalizationAccThr_(upstreamRelocalizationAccThr), + previousAnchorStamp_(0.0), + dataGoodTracking_(true) { } @@ -72,37 +76,44 @@ CameraMobile::~CameraMobile() { bool CameraMobile::init(const std::string &, const std::string &) { deviceTColorCamera_ = opticalRotation; + // clear semaphore + if(dataReady_.value() > 0) { + dataReady_.acquire(dataReady_.value()); + } return true; } void CameraMobile::close() { + UScopeMutex lock(dataMutex_); + firstFrame_ = true; lastKnownGPS_ = GPS(); lastEnvSensors_.clear(); originOffset_ = Transform(); - originUpdate_ = false; + originUpdate_ = true; dataPose_ = Transform(); data_ = SensorData(); + dataGoodTracking_ = true; + previousAnchorPose_.setNull(); + previousAnchorLinearVelocity_.clear(); + previousAnchorStamp_ = 0.0; if(textureId_ != 0) { glDeleteTextures(1, &textureId_); textureId_ = 0; } + // in case someone is waiting on captureImage() + dataReady_.release(); } void CameraMobile::resetOrigin() { - firstFrame_ = true; - lastKnownGPS_ = GPS(); - lastEnvSensors_.clear(); - dataPose_ = Transform(); - data_ = SensorData(); originUpdate_ = true; } -bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) +bool CameraMobile::getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) { pose.setNull(); @@ -113,27 +124,27 @@ bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, { poseMutex_.lock(); int waitTry = 0; - while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs) + while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < epochStamp && waitTry < maxWaitTimeMs) { poseMutex_.unlock(); ++waitTry; uSleep(1); poseMutex_.lock(); } - if(poseBuffer_.rbegin()->first < stamp) + if(poseBuffer_.rbegin()->first < epochStamp) { if(maxWaitTimeMs > 0) { - UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first); + UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", epochStamp, maxWaitTimeMs, poseBuffer_.rbegin()->first); } else { - UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first); + UWARN("Could not find poses to interpolate at time %f (latest is %f)...", epochStamp, poseBuffer_.rbegin()->first); } } else { - std::map::const_iterator iterB = poseBuffer_.lower_bound(stamp); + std::map::const_iterator iterB = poseBuffer_.lower_bound(epochStamp); std::map::const_iterator iterA = iterB; if(iterA != poseBuffer_.begin()) { @@ -143,17 +154,17 @@ bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, { iterB = --iterB; } - if(iterA == iterB && stamp == iterA->first) + if(iterA == iterB && epochStamp == iterA->first) { pose = iterA->second; } - else if(stamp >= iterA->first && stamp <= iterB->first) + else if(epochStamp >= iterA->first && epochStamp <= iterB->first) { - pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second); + pose = iterA->second.interpolate((epochStamp-iterA->first) / (iterB->first-iterA->first), iterB->second); } else // stamp < iterA->first { - UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); + UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", epochStamp, iterA->first); } } poseMutex_.unlock(); @@ -163,25 +174,103 @@ bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, void CameraMobile::poseReceived(const Transform & pose, double deviceStamp) { + // Pose reveived is the pose of the device in rtabmap coordinate if(!pose.isNull()) { Transform p = pose; - if(originUpdate_) - { - originOffset_ = p.translation().inverse(); - originUpdate_ = false; - } if(stampEpochOffset_ == 0.0) { stampEpochOffset_ = UTimer::now() - deviceStamp; } + + if(originUpdate_) + { + firstFrame_ = true; + lastKnownGPS_ = GPS(); + lastEnvSensors_.clear(); + dataGoodTracking_ = true; + previousAnchorPose_.setNull(); + previousAnchorLinearVelocity_.clear(); + previousAnchorStamp_ = 0.0; + originOffset_ = pose.translation().inverse(); + originUpdate_ = false; + } double epochStamp = stampEpochOffset_ + deviceStamp; - if(!originOffset_.isNull()) { - p = originOffset_*p; + // Filter re-localizations from poses received + rtabmap::Transform rawPose = originOffset_ * pose.translation(); // remove rotation to keep position in fixed frame + // Remove upstream localization corrections by integrating pose from previous frame anchor + bool showLog = false; + if(upstreamRelocalizationAccThr_>0.0f && !previousAnchorPose_.isNull()) + { + float dt = epochStamp - previousAnchorStamp_; + std::vector currentLinearVelocity(3); + float dx = rawPose.x()-previousAnchorPose_.x(); + float dy = rawPose.y()-previousAnchorPose_.y(); + float dz = rawPose.z()-previousAnchorPose_.z(); + currentLinearVelocity[0] = dx / dt; + currentLinearVelocity[1] = dy / dt; + currentLinearVelocity[2] = dz / dt; + if(!previousAnchorLinearVelocity_.empty() && uNorm(dx, dy, dz)>0.02) + { + float ax = (currentLinearVelocity[0] - previousAnchorLinearVelocity_[0]) / dt; + float ay = (currentLinearVelocity[1] - previousAnchorLinearVelocity_[1]) / dt; + float az = (currentLinearVelocity[2] - previousAnchorLinearVelocity_[2]) / dt; + float acceleration = sqrt(ax*ax + ay*ay + az*az); + if(acceleration>=upstreamRelocalizationAccThr_) + { + // Only correct the translation to not lose rotation aligned + // with gravity. + + // Use constant motion model to update current pose. + rtabmap::Transform offset(previousAnchorLinearVelocity_[0] * dt, + previousAnchorLinearVelocity_[1] * dt, + previousAnchorLinearVelocity_[2] * dt, + 0, 0, 0, 1); + rtabmap::Transform newRawPose = offset * previousAnchorPose_; + currentLinearVelocity = previousAnchorLinearVelocity_; + originOffset_.x() += newRawPose.x() - rawPose.x(); + originOffset_.y() += newRawPose.y() - rawPose.y(); + originOffset_.z() += newRawPose.z() - rawPose.z(); + UERROR("Upstream re-localization has been suppressed because of " + "high acceleration detected (%f m/s^2) causing a jump!", + acceleration); + dataGoodTracking_ = false; + post(new CameraInfoEvent(0, "UpstreamRelocationFiltered", uFormat("%.1f m/s^2", acceleration).c_str())); + showLog = true; + } + } + previousAnchorLinearVelocity_ = currentLinearVelocity; + } + + p = originOffset_*pose; + previousAnchorPose_ = p; + previousAnchorStamp_ = epochStamp; + + if(upstreamRelocalizationAccThr_>0.0f) { + relocalizationDebugBuffer_.insert(std::make_pair(epochStamp, std::make_pair(pose, p))); + if(relocalizationDebugBuffer_.size() > 60) + { + relocalizationDebugBuffer_.erase(relocalizationDebugBuffer_.begin()); + } + if(showLog) { + std::stringstream stream; + for(auto iter=relocalizationDebugBuffer_.begin(); iter!=relocalizationDebugBuffer_.end(); ++iter) + { + stream << iter->first - relocalizationDebugBuffer_.begin()->first + << " " << iter->second.first.x() + << " " << iter->second.first.y() + << " " << iter->second.first.z() + << " " << iter->second.second.x() + << " " << iter->second.second.y() + << " " << iter->second.second.z() << std::endl; + } + UERROR("timestamp original_xyz corrected_xyz:\n%s", stream.str().c_str()); + } + } } { @@ -217,22 +306,16 @@ void CameraMobile::update(const SensorData & data, const Transform & pose, const { UScopeMutex lock(dataMutex_); - bool notify = !data_.isValid(); + LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp()); - LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp()); + bool notify = !data_.isValid(); + data_ = data; dataPose_ = pose; viewMatrix_ = viewMatrix; projectionMatrix_ = projectionMatrix; - - // adjust origin - if(!originOffset_.isNull()) - { - dataPose_ = originOffset_ * dataPose_; - viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); - } - + if(textureId_ == 0) { glGenTextures(1, &textureId_); @@ -272,12 +355,15 @@ void CameraMobile::update(const SensorData & data, const Transform & pose, const } } - postUpdate(); - - if(notify) - { - dataReady_.release(); - } + if(data_.isValid()) + { + postUpdate(); + + if(notify) + { + dataReady_.release(); + } + } } void CameraMobile::updateOnRender() @@ -286,7 +372,6 @@ void CameraMobile::updateOnRender() bool notify = !data_.isValid(); data_ = updateDataOnRender(dataPose_); - if(data_.isValid()) { postUpdate(); @@ -309,6 +394,14 @@ void CameraMobile::postUpdate() { if(data_.isValid()) { + // adjust origin + if(!originOffset_.isNull()) + { + dataPose_ = originOffset_ * dataPose_; + viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); + occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform()); + } + if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0) { data_.setGPS(lastKnownGPS_); @@ -424,11 +517,20 @@ void CameraMobile::postUpdate() SensorData CameraMobile::captureImage(SensorCaptureInfo * info) { SensorData data; - if(dataReady_.acquire(1, 5000)) + bool firstFrame = true; + bool dataGoodTracking = true; + rtabmap::Transform dataPose; + if(dataReady_.acquire(1, 15000)) { UScopeMutex lock(dataMutex_); data = data_; + dataPose = dataPose_; + firstFrame = firstFrame_; + dataGoodTracking = dataGoodTracking_; + firstFrame_ = false; + dataGoodTracking_ = true; data_ = SensorData(); + dataPose_.setNull(); } if(data.isValid()) { @@ -438,18 +540,26 @@ SensorData CameraMobile::captureImage(SensorCaptureInfo * info) if(info) { // linear cov = 0.0001 - info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001); - if(!firstFrame_) + info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.00001); + if(!firstFrame) { // angular cov = 0.000001 - info->odomCovariance.at(3,3) *= 0.01; - info->odomCovariance.at(4,4) *= 0.01; - info->odomCovariance.at(5,5) *= 0.01; + // roll/pitch should be fairly accurate with VIO input + info->odomCovariance.at(3,3) *= 0.01; // roll + info->odomCovariance.at(4,4) *= 0.01; // pitch + if(!dataGoodTracking) + { + UERROR("not good tracking!"); + // add slightly more error on translation + // 0.001 + info->odomCovariance.at(0,0) *= 10; // x + info->odomCovariance.at(1,1) *= 10; // y + info->odomCovariance.at(2,2) *= 10; // z + info->odomCovariance.at(5,5) *= 10; // yaw + } } - info->odomPose = dataPose_; + info->odomPose = dataPose; } - - firstFrame_ = false; } else { @@ -460,7 +570,6 @@ SensorData CameraMobile::captureImage(SensorCaptureInfo * info) LaserScan CameraMobile::scanFromPointCloudData( const cv::Mat & pointCloudData, - int points, const Transform & pose, const CameraModel & model, const cv::Mat & rgb, @@ -477,7 +586,7 @@ LaserScan CameraMobile::scanFromPointCloudData( UASSERT(pointCloudData.depth() == CV_32F && ic >= 3); int oi = 0; - for(unsigned int i=0;i previousAnchorLinearVelocity_; + double previousAnchorStamp_; + std::map > relocalizationDebugBuffer_; USemaphore dataReady_; UMutex dataMutex_; SensorData data_; Transform dataPose_; + bool dataGoodTracking_; UMutex poseMutex_; std::map poseBuffer_; // diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index 5498014dea..837b6d0e93 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -194,6 +194,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string { close(); + CameraMobile::init(calibrationFolder, cameraName); + TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics); // Connect to Tango @@ -657,12 +659,6 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) //LOGD("tango = %s", poseDevice.prettyPrint().c_str()); //LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str()); - // adjust origin - if(!getOriginOffset().isNull()) - { - odom = getOriginOffset() * odom; - } - // occlusion depth if(!depth.empty()) { @@ -832,10 +828,6 @@ SensorData CameraTango::updateDataOnRender(Transform & pose) float cy = static_cast(color_camera_intrinsics.cy); viewMatrix_ = glm::make_mat4(matrix_transform.matrix); - if(!getOriginOffset().isNull()) - { - viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); - } projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics( image_width, image_height, fx, fy, cx, cy, 0.3, 50); diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 29f14bdcaa..0e846fdb6d 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -73,9 +73,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef RTABMAP_PDAL +#include +#elif defined(RTABMAP_LIBLAS) +#include +#endif #define LOW_RES_PIX 2 -//#define DEBUG_RENDERING_PERFORMANCE +#define DEBUG_RENDERING_PERFORMANCE const int g_optMeshId = -100; @@ -134,7 +139,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false"))); - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_ && !dataRecorderMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAggressiveLoopThr(), "0.0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10")); @@ -188,10 +193,18 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemMapLabelsAdded())); if(dataRecorderMode_) { - uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("-1"))); - uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), std::string("1.0"))); // deactivate rehearsal - uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels - uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("true"))); + // Example taken from https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_launch/launch/data_recorder.launch + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), "1.0")); // deactivate rehearsal + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), "-1")); // deactivate keypoints extraction + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "0")); // deactivate global retrieval + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0")); // deactivate local retrieval + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMemoryThr(), "2")); // keep the WM empty + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1")); // STM=1 --> + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), "false")); + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), "0")); + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), "0")); + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("true"))); } return parameters; @@ -231,6 +244,8 @@ RTABMapApp::RTABMapApp() : renderingTextureDecimation_(4), backgroundColor_(0.2f), depthConfidence_(2), + upstreamRelocalizationMaxAcc_(0.0f), + exportPointCloudFormat_("ply"), dataRecorderMode_(false), clearSceneOnNextRender_(false), openingDatabase_(false), @@ -307,12 +322,14 @@ void RTABMapApp::setupSwiftCallbacks(void * classPtr, int, float, float, float, float, int, int, - float, float, float, float, float, float)) + float, float, float, float, float, float), + void(*cameraInfoEventCallback)(void *, int, const char*, const char*)) { swiftClassPtr_ = classPtr; progressionStatus_.setSwiftCallback(classPtr, progressCallback); swiftInitCallback = initCallback; swiftStatsUpdatedCallback = statsUpdatedCallback; + swiftCameraInfoEventCallback = cameraInfoEventCallback; } #endif @@ -386,6 +403,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe lastPostRenderEventTime_ = 0.0; lastPoseEventTime_ = 0.0; bufferedStatsData_.clear(); + graphOptimization_ = true; this->registerToEventsManager(); @@ -475,7 +493,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe rtabmap_ = new rtabmap::Rtabmap(); rtabmap::ParametersMap parameters = getRtabmapParameters(); - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory && !dataRecorderMode_))); LOGI("Initializing database..."); rtabmap_->init(parameters, databasePath); rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_); @@ -745,8 +763,19 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe { boost::mutex::scoped_lock lock(cameraMutex_); if(camera_) - { - camera_->resetOrigin(); + { + camera_->resetOrigin(); + if(dataRecorderMode_) + { + // Don't update faster than we record, so that we see is what is recorded + camera_->setFrameRate(rtabmapThread_->getDetectorRate()); + rtabmapThread_->setDetectorRate(0); + } + else + { + // set default 10 + camera_->setFrameRate(10); + } } } @@ -908,7 +937,7 @@ bool RTABMapApp::startCamera() else if(cameraDriver_ == 1) { #ifdef RTABMAP_ARCORE - camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_); + camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_, upstreamRelocalizationMaxAcc_); #else UERROR("RTAB-Map is not built with ARCore support!"); #endif @@ -916,14 +945,14 @@ bool RTABMapApp::startCamera() else if(cameraDriver_ == 2) { #ifdef RTABMAP_ARENGINE - camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_); + camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_, upstreamRelocalizationMaxAcc_); #else UERROR("RTAB-Map is not built with AREngine support!"); #endif } else if(cameraDriver_ == 3) { - camera_ = new rtabmap::CameraMobile(smoothing_); + camera_ = new rtabmap::CameraMobile(smoothing_, upstreamRelocalizationMaxAcc_); } if(camera_ == 0) @@ -931,6 +960,13 @@ bool RTABMapApp::startCamera() UERROR("Unknown or not supported camera driver! %d", cameraDriver_); return false; } + + if(rtabmapThread_ && dataRecorderMode_) + { + // Don't update faster than we record, so that we see is what is recorded + camera_->setFrameRate(rtabmapThread_->getDetectorRate()); + rtabmapThread_->setDetectorRate(0); + } if(camera_->init()) { @@ -967,6 +1003,7 @@ void RTABMapApp::stopCamera() boost::mutex::scoped_lock lock(cameraMutex_); if(sensorCaptureThread_!=0) { + camera_->close(); sensorCaptureThread_->join(true); delete sensorCaptureThread_; // camera_ is closed and deleted inside sensorCaptureThread_ = 0; @@ -1291,7 +1328,7 @@ int RTABMapApp::Render() camera_->updateOnRender(); } #ifdef DEBUG_RENDERING_PERFORMANCE - LOGW("Camera updateOnRender %fs", time.ticks()); + LOGD("Camera updateOnRender %fs", time.ticks()); #endif if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0) { @@ -1308,7 +1345,7 @@ int RTABMapApp::Render() arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix)); } } - if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson) + if(!visualizingMesh_ && !dataRecorderMode_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson) { rtabmap::CameraModel occlusionModel; cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel); @@ -1330,7 +1367,7 @@ int RTABMapApp::Render() } } #ifdef DEBUG_RENDERING_PERFORMANCE - LOGW("Update background and occlusion mesh %fs", time.ticks()); + LOGD("Update background and occlusion mesh %fs", time.ticks()); #endif } } @@ -1746,7 +1783,7 @@ int RTABMapApp::Render() // Transform pose in OpenGL world for(std::map::iterator iter=posesWithMarkers.begin(); iter!=posesWithMarkers.end(); ++iter) { - if(!graphOptimization_) + if(!graphOptimization_ && !dataRecorderMode_) { std::map::iterator jter = rawPoses_.find(iter->first); if(jter != rawPoses_.end()) @@ -2014,7 +2051,8 @@ int RTABMapApp::Render() } } } - else + + if(dataRecorderMode_ || !rtabmapEvents.size()) { main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0); @@ -2417,6 +2455,11 @@ void RTABMapApp::setAppendMode(bool enabled) } } +void RTABMapApp::setUpstreamRelocalizationAccThr(float value) +{ + upstreamRelocalizationMaxAcc_ = value; +} + void RTABMapApp::setDataRecorderMode(bool enabled) { if(dataRecorderMode_ != enabled) @@ -2492,6 +2535,22 @@ void RTABMapApp::setDepthConfidence(int value) } } +void RTABMapApp::setExportPointCloudFormat(const std::string & format) +{ +#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS) + if(format == "las") { + exportPointCloudFormat_ = format; + } + else +#endif + if(format != "ply") { + UERROR("Not supported point cloud format %s", format.c_str()); + } + else { + exportPointCloudFormat_ = format; + } +} + int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value) { std::string compatibleKey = key; @@ -2599,11 +2658,14 @@ void RTABMapApp::save(const std::string & databasePath) std::multimap links = rtabmap_->getLocalConstraints(); rtabmap_->close(true, databasePath); rtabmap_->init(getRtabmapParameters(), dataRecorderMode_?"":databasePath); - rtabmap_->setOptimizedPoses(poses, links); if(dataRecorderMode_) { clearSceneOnNextRender_ = true; } + else + { + rtabmap_->setOptimizedPoses(poses, links); + } } bool RTABMapApp::recover(const std::string & from, const std::string & to) @@ -3543,18 +3605,43 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str if(polygonMesh->cloud.data.size()) { - // Point cloud PLY - std::string filePath = directory + UDirectory::separator() + name + ".ply"; - LOGI("Saving ply (%d vertices, %d polygons) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, (int)polygonMesh->polygons.size(), filePath.c_str()); - success = pcl::io::savePLYFileBinary(filePath, *polygonMesh) == 0; - if(success) - { - LOGI("Saved ply to %s!", filePath.c_str()); - } - else - { - UERROR("Failed saving ply to %s!", filePath.c_str()); - } +#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS) + if(polygonMesh->polygons.empty() && exportPointCloudFormat_ == "las") { + // Point cloud LAS + std::string filePath = directory + UDirectory::separator() + name + ".las"; + LOGI("Saving las (%d vertices) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, filePath.c_str()); + pcl::PointCloud output; + pcl::fromPCLPointCloud2(polygonMesh->cloud, output); +#ifdef RTABMAP_PDAL + success = rtabmap::savePDALFile(filePath, output) == 0; +#else + success = rtabmap::saveLASFile(filePath, output) == 0; +#endif + if(success) + { + LOGI("Saved las to %s!", filePath.c_str()); + } + else + { + UERROR("Failed saving las to %s!", filePath.c_str()); + } + } + else +#endif + { + // Point cloud PLY + std::string filePath = directory + UDirectory::separator() + name + ".ply"; + LOGI("Saving ply (%d vertices, %d polygons) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, (int)polygonMesh->polygons.size(), filePath.c_str()); + success = pcl::io::savePLYFileBinary(filePath, *polygonMesh) == 0; + if(success) + { + LOGI("Saved ply to %s!", filePath.c_str()); + } + else + { + UERROR("Failed saving ply to %s!", filePath.c_str()); + } + } } else if(textureMesh->cloud.data.size()) { @@ -3712,24 +3799,6 @@ int RTABMapApp::postProcessing(int approach) return returnedValue; } -void RTABMapApp::postCameraPoseEvent( - float x, float y, float z, float qx, float qy, float qz, float qw, double stamp) -{ - boost::mutex::scoped_lock lock(cameraMutex_); - if(cameraDriver_ == 3 && camera_) - { - if(qx==0 && qy==0 && qz==0 && qw==0) - { - // Lost! clear buffer - camera_->resetOrigin(); // we are lost, create new session on next valid frame - return; - } - rtabmap::Transform pose(x,y,z,qx,qy,qz,qw); - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - camera_->poseReceived(pose, stamp); - } -} - void RTABMapApp::postOdometryEvent( rtabmap::Transform pose, float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy, @@ -3742,7 +3811,7 @@ void RTABMapApp::postOdometryEvent( const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat, const void * conf, int confLen, int confWidth, int confHeight, int confFormat, const float * points, int pointsLen, int pointsChannels, - const rtabmap::Transform & viewMatrix, + rtabmap::Transform viewMatrix, float p00, float p11, float p02, float p12, float p22, float p32, float p23, float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7) { @@ -3750,6 +3819,12 @@ void RTABMapApp::postOdometryEvent( boost::mutex::scoped_lock lock(cameraMutex_); if(cameraDriver_ == 3 && camera_) { + if(pose.isNull()) + { + // We are lost, trigger a new map on next update + camera_->resetOrigin(); + return; + } if(rgb_fx > 0.0f && rgb_fy > 0.0f && rgb_cx > 0.0f && rgb_cy > 0.0f && stamp > 0.0f && yPlane && vPlane && yPlaneLen == rgbWidth*rgbHeight) { #ifndef DISABLE_LOG @@ -3760,7 +3835,7 @@ void RTABMapApp::postOdometryEvent( (depth==0 || depthFormat == AIMAGE_FORMAT_DEPTH16)) #else //__APPLE__ if(rgbFormat == 875704422 && - (depth==0 || depthFormat == 1717855600)) + (depth==0 || depthFormat == 1717855600)) #endif { cv::Mat outputRGB; @@ -3836,13 +3911,11 @@ void RTABMapApp::postOdometryEvent( if(!outputRGB.empty()) { + // Convert in our coordinate frame pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - rtabmap::Transform poseWithOriginOffset = pose; - if(!camera_->getOriginOffset().isNull()) - { - poseWithOriginOffset = camera_->getOriginOffset() * pose; - } + // We should update the pose before querying poses for depth below (if not same stamp than rgb) + camera_->poseReceived(pose, stamp); // Registration depth to rgb if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp)) @@ -3852,19 +3925,24 @@ void RTABMapApp::postOdometryEvent( if(depthStamp != stamp) { // Interpolate pose + rtabmap::Transform poseRgb; rtabmap::Transform poseDepth; cv::Mat cov; - if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0)) + if(!camera_->getPose(camera_->getStampEpochOffset()+stamp, poseRgb, cov, 0.0)) + { + UERROR("Could not find pose at rgb stamp %f (epoch %f)!", stamp, camera_->getStampEpochOffset()+stamp); + } + else if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0)) { - UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp); + UERROR("Could not find pose at depth stamp %f (epoch %f) last rgb is %f!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp); } else { #ifndef DISABLE_LOG - UDEBUG("poseRGB =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp); + UDEBUG("poseRGB =%s (stamp=%f)", poseRgb.prettyPrint().c_str(), stamp); UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp); #endif - motion = poseWithOriginOffset.inverse()*poseDepth; + motion = poseRgb.inverse()*poseDepth; // transform in camera frame #ifndef DISABLE_LOG UDEBUG("motion=%s", motion.prettyPrint().c_str()); @@ -3910,19 +3988,19 @@ void RTABMapApp::postOdometryEvent( if(outputDepth.empty()) { int kptsSize = fullResolution_ ? 12 : 6; - scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pointsLen, pose, model, outputRGB, &kpts, &kpts3, kptsSize); + scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pose, model, outputRGB, &kpts, &kpts3, kptsSize); } else { // We will recompute features if depth is available - scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pointsLen, pose, model, outputRGB); + scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pose, model, outputRGB); } } if(!outputDepth.empty()) { rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth())); - depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform()); + depthModel.setLocalTransform(pose*model.localTransform()); camera_->setOcclusionImage(outputDepth, depthModel); } @@ -4031,6 +4109,15 @@ bool RTABMapApp::handleEvent(UEvent * event) } jvm->DetachCurrentThread(); } +#else + if(swiftClassPtr_) + { + std::function actualCallback = [&](){ + swiftCameraInfoEventCallback(swiftClassPtr_, tangoEvent->type(), tangoEvent->key().c_str(), tangoEvent->value().c_str()); + }; + actualCallback(); + success = true; + } #endif if(!success) { diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 40c9d66215..67a7ab6d62 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -70,7 +70,8 @@ class RTABMapApp : public UEventsHandler { int, float, float, float, float, int, int, - float, float, float, float, float, float)); + float, float, float, float, float, float), + void(*cameraInfoCallback)(void *, int, const char*, const char*)); #endif ~RTABMapApp(); @@ -138,6 +139,7 @@ class RTABMapApp : public UEventsHandler { void setSmoothing(bool enabled); void setDepthFromMotion(bool enabled); void setAppendMode(bool enabled); + void setUpstreamRelocalizationAccThr(float value); void setDataRecorderMode(bool enabled); void setMaxCloudDepth(float value); void setMinCloudDepth(float value); @@ -150,6 +152,7 @@ class RTABMapApp : public UEventsHandler { void setRenderingTextureDecimation(int value); void setBackgroundColor(float gray); void setDepthConfidence(int value); + void setExportPointCloudFormat(const std::string & format); int setMappingParameter(const std::string & key, const std::string & value); void setGPS(const rtabmap::GPS & gps); void addEnvSensor(int type, float value); @@ -178,9 +181,6 @@ class RTABMapApp : public UEventsHandler { bool writeExportedMesh(const std::string & directory, const std::string & name); int postProcessing(int approach); - void postCameraPoseEvent( - float x, float y, float z, float qx, float qy, float qz, float qw, double stamp); - void postOdometryEvent( rtabmap::Transform pose, float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy, @@ -193,7 +193,7 @@ class RTABMapApp : public UEventsHandler { const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat, const void * conf, int confLen, int confWidth, int confHeight, int confFormat, const float * points, int pointsLen, int pointsChannels, - const rtabmap::Transform & viewMatrix, //view matrix + rtabmap::Transform viewMatrix, //view matrix float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord @@ -239,6 +239,8 @@ class RTABMapApp : public UEventsHandler { int renderingTextureDecimation_; float backgroundColor_; int depthConfidence_; + float upstreamRelocalizationMaxAcc_; + std::string exportPointCloudFormat_; rtabmap::ParametersMap mappingParameters_; @@ -309,6 +311,7 @@ class RTABMapApp : public UEventsHandler { float, float, float, float, int, int, float, float, float, float, float, float); + void(*swiftCameraInfoEventCallback)(void *, int, const char *, const char *); #endif }; diff --git a/app/android/jni/jni_interface.cpp b/app/android/jni/jni_interface.cpp index d0f20b9e1d..53d20df098 100644 --- a/app/android/jni/jni_interface.cpp +++ b/app/android/jni/jni_interface.cpp @@ -550,6 +550,20 @@ Java_com_introlab_rtabmap_RTABMapLib_setAppendMode( UERROR("native_application is null!"); } } +JNIEXPORT void JNICALL +Java_com_introlab_rtabmap_RTABMapLib_setUpstreamRelocalizationAccThr( + JNIEnv*, jclass, jlong native_application, float value) +{ + if(native_application) + { + return native(native_application)->setUpstreamRelocalizationAccThr(value); + } + else + { + UERROR("native_application is null!"); + } +} + JNIEXPORT void JNICALL Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode( JNIEnv*, jclass, jlong native_application, bool enabled) @@ -866,22 +880,6 @@ Java_com_introlab_rtabmap_RTABMapLib_postProcessing( } } -JNIEXPORT void JNICALL -Java_com_introlab_rtabmap_RTABMapLib_postCameraPoseEvent( - JNIEnv* env, jclass, jlong native_application, - float x, float y, float z, float qx, float qy, float qz, float qw, double stamp) -{ - if(native_application) - { - native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw, stamp); - } - else - { - UERROR("native_application is null!"); - return; - } -} - JNIEXPORT void JNICALL Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent( JNIEnv* env, jclass, jlong native_application, diff --git a/app/android/res/layout/activity_settings.xml b/app/android/res/layout/activity_settings.xml index b0cb4c258f..9fb6fe06e4 100644 --- a/app/android/res/layout/activity_settings.xml +++ b/app/android/res/layout/activity_settings.xml @@ -103,12 +103,12 @@ android:summary="@string/pref_summary_depth_from_motion" android:defaultValue="@string/pref_default_depth_from_motion"/> + android:key="@string/pref_key_arcore_relocalization_acc_thr" + android:title="@string/pref_title_arcore_relocalization_acc_thr" + android:summary="@string/pref_summary_arcore_relocalization_acc_thr" + android:entries="@array/pref_arcore_relocalization_acc_thr_keys" + android:entryValues="@array/pref_arcore_relocalization_acc_thr_values" + android:defaultValue="@string/pref_default_arcore_relocalization_acc_thr"/> -1 pref_key_depth_from_motion false - pref_key_arcore_localization_filtering_speed - 0 + pref_key_arcore_relocalization_acc_thr + 58.8399 pref_key_update_rate 1 pref_key_max_speed @@ -100,7 +100,7 @@ pref_key_min_inliers 25 pref_key_opt_error - 1 + 2 pref_key_features_voc 200 pref_key_features @@ -329,8 +329,8 @@ AR sdk used for capturing 6DoF poses and images. A TOF camera is required to record a 3D model. Depth From Motion Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver. - ARCore Localization Filtering Speed - Filter ARCore\'s localizations to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set a speed threshold to detect those events. + ARCore Re-Localization Acceleration Threshold + Filter ARCore\'s re-localizations to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set an acceleration threshold to detect those events. Append Mode When resuming mapping, wait for a relocalization on the current map before starting a new map. HD Mode @@ -412,23 +412,29 @@ "1" "0.5" - + "Disabled" - "5 m/s" - "4 m/s" - "3 m/s" - "2 m/s" - "1 m/s" - "0.5 m/s" - - + "10 g" + "9 g" + "8 g" + "7 g" + "6 g" + "5 g" + "4 g" + "3 g" + "2 g" + + "0" - "5" - "4" - "3" - "2" - "1" - "0.5" + "98.0665" + "88.25985" + "78.4532" + "68.64655" + "58.8399" + "49.03325" + "39.2266" + "29.41995" + "19.6133" "No Limit" diff --git a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java index 804403bc6d..446d1a5488 100644 --- a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java +++ b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java @@ -56,9 +56,8 @@ public class ARCoreSharedCamera { }; private static RTABMapActivity mActivity; - public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSpeed) { + public ARCoreSharedCamera(RTABMapActivity c) { mActivity = c; - mARCoreLocalizationFilteringSpeed = arCoreLocalizationFilteringSpeed; } // Depth TOF Image. @@ -72,11 +71,7 @@ public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSp // ARCore session that supports camera sharing. private Session sharedSession; - private Pose previousAnchorPose = null; - private long previousAnchorTimeStamp; - private Pose arCoreCorrection = Pose.IDENTITY; private Pose odomPose = Pose.IDENTITY; - private float mARCoreLocalizationFilteringSpeed = 1.0f; // Camera capture session. Used by both non-AR and AR modes. private CameraCaptureSession captureSession; @@ -111,6 +106,7 @@ public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSp private CaptureRequest.Builder previewCaptureRequestBuilder; private int cameraTextureId = -1; + private boolean firstFrameReceived = false; // Image reader that continuously processes CPU images. public TOF_ImageReader mTOFImageReader = new TOF_ImageReader(); @@ -118,6 +114,8 @@ public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSp ByteBuffer mPreviousDepth = null; double mPreviousDepthStamp = 0.0; + ByteBuffer mPreviousDepth2 = null; + double mPreviousDepthStamp2 = 0.0; public boolean isDepthSupported() {return mTOFAvailable;} @@ -633,6 +631,7 @@ public void close() { } mTOFImageReader.close(); + firstFrameReceived = false; if(cameraTextureId>=0) { @@ -681,14 +680,15 @@ public void updateGL() throws CameraNotAvailableException { Log.e(TAG, String.format("camera is null!")); return; } + boolean lost = false; if (camera.getTrackingState() != TrackingState.TRACKING) { final String trackingState = camera.getTrackingState().toString(); Log.e(TAG, String.format("Tracking lost! state=%s", trackingState)); // This will force a new session on the next frame received - RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, 0,0,0,0,0,0,0,0); + lost = true; mActivity.runOnUiThread(new Runnable() { public void run() { - if(mToast!=null && previousAnchorPose != null) + if(mToast!=null && firstFrameReceived) { String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing."; if(mToast.getView() == null || !mToast.getView().isShown()) @@ -700,67 +700,14 @@ public void run() { { mToast.setText(msg); } - previousAnchorPose = null; - arCoreCorrection = Pose.IDENTITY; } } }); - return; } if (frame.getTimestamp() != 0) { Pose pose = camera.getPose(); - - // Remove ARCore SLAM corrections by integrating pose from previous frame anchor - if(previousAnchorPose == null || mARCoreLocalizationFilteringSpeed==0) - { - odomPose = pose; - } - else - { - float[] t = previousAnchorPose.inverse().compose(pose).getTranslation(); - final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8); - if(speed>=mARCoreLocalizationFilteringSpeed) - { - // Only correct the translation to not lose rotation aligned with gravity - arCoreCorrection = arCoreCorrection.compose(previousAnchorPose.compose(pose.inverse()).extractTranslation()); - t = arCoreCorrection.getTranslation(); - Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2])); - - t = odomPose.getTranslation(); - float[] t2 = (arCoreCorrection.compose(pose)).getTranslation(); - float[] t3 = previousAnchorPose.getTranslation(); - float[] t4 = pose.getTranslation(); - Log.e(TAG, String.format("Odom = %f %f %f -> %f %f %f ArCore= %f %f %f -> %f %f %f", t[0], t[1], t[2], t2[0], t2[1], t2[2], t3[0], t3[1], t3[2], t4[0], t4[1], t4[2])); - - mActivity.runOnUiThread(new Runnable() { - public void run() { - if(mToast!=null) - { - String msg = String.format("ARCore localization has been suppressed " - + "because of high speed detected (%f m/s) causing a jump! You can change " - + "ARCore localization filtering speed in Settings->Mapping if you are " - + "indeed moving as fast.", speed); - if(mToast.getView() == null || !mToast.getView().isShown()) - { - mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show(); - } - else - { - mToast.setText(msg); - } - } - } - }); - } - odomPose = arCoreCorrection.compose(pose); - } - previousAnchorPose = pose; - previousAnchorTimeStamp = frame.getTimestamp(); - double stamp = (double)frame.getTimestamp()/10e8; - if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f arcore %f %f %f cor= %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), pose.tx(), pose.ty(), pose.tz(), arCoreCorrection.tx(), arCoreCorrection.ty(), arCoreCorrection.tz(), stamp)); - RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp); CameraIntrinsics intrinsics = camera.getImageIntrinsics(); try{ Image image = frame.acquireCameraImage(); @@ -803,7 +750,7 @@ public void run() { camera.getProjectionMatrix(p, 0, 0.1f, 100.0f); float[] viewMatrix = new float[16]; - arCoreCorrection.compose(camera.getDisplayOrientedPose()).inverse().toMatrix(viewMatrix, 0); + camera.getDisplayOrientedPose().inverse().toMatrix(viewMatrix, 0); float[] quat = new float[4]; rotationMatrixToQuaternion(viewMatrix, quat); @@ -822,35 +769,47 @@ public void run() { mPreviousDepth = depth; mPreviousDepthStamp = depthStamp; } + if(mPreviousDepth2 == null) + { + mPreviousDepth2 = depth; + mPreviousDepthStamp2 = depthStamp; + } - if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f", - mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp)); + if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f previous=%f rgb=%f", + mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp, mPreviousDepthStamp, stamp)); RTABMapLib.postOdometryEventDepth( RTABMapActivity.nativeApplication, - odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), + lost?0:pose.tx(), lost?0:pose.ty(), lost?0:pose.tz(), lost?0:pose.qx(), lost?0:pose.qy(), lost?0:pose.qz(), lost?0:pose.qw(), fl[0], fl[1], pp[0], pp[1], depthIntrinsics[0], depthIntrinsics[1], depthIntrinsics[2], depthIntrinsics[3], rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(), depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(), stamp, - depthStamp>stamp?mPreviousDepthStamp:depthStamp, + depthStamp<=stamp?depthStamp:mPreviousDepthStamp<=stamp?mPreviousDepthStamp:mPreviousDepthStamp2, y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), - depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16, + depthStamp<=stamp?depth:mPreviousDepthStamp<=stamp?mPreviousDepth:mPreviousDepth2, + depthStamp<=stamp?depth.limit():mPreviousDepthStamp<=stamp?mPreviousDepth.limit():mPreviousDepth2.limit(), + mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16, points, points.limit()/4, viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0], p[0], p[5], p[8], p[9], p[10], p[11], p[14], texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]); - - mPreviousDepthStamp = depthStamp; - mPreviousDepth = depth; + // triple buffer in case delay of rgb frame is > 60 ms + if(depthStamp != mPreviousDepthStamp) + { + mPreviousDepthStamp2 = mPreviousDepthStamp; + mPreviousDepth2 = mPreviousDepth; + mPreviousDepthStamp = depthStamp; + mPreviousDepth = depth; + } } else { RTABMapLib.postOdometryEvent( RTABMapActivity.nativeApplication, - odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), + lost?0:pose.tx(), lost?0:pose.ty(), lost?0:pose.tz(), lost?0:pose.qx(), lost?0:pose.qy(), lost?0:pose.qz(), lost?0:pose.qw(), fl[0], fl[1], pp[0], pp[1], rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(), stamp, @@ -861,6 +820,7 @@ public void run() { texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]); } + firstFrameReceived = !lost; image.close(); cloud.close(); diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 6e0a444778..8184bfdeca 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -323,6 +323,7 @@ protected void onCreate(Bundle savedInstanceState) { // touch point. Display display = getWindowManager().getDefaultDisplay(); display.getSize(mScreenSize); + Log.i(TAG, String.format("Screen resolution: %dx%d", mScreenSize.x, mScreenSize.y)); getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON); getWindow().addFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_STATUS); @@ -1142,6 +1143,7 @@ public void run() { RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion)))); RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye)))); RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append)))); + RTABMapLib.setUpstreamRelocalizationAccThr(nativeApplication, Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_arcore_relocalization_acc_thr), getString(R.string.pref_default_arcore_relocalization_acc_thr)))); RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/DetectionRate", mUpdateRate); RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/TimeThr", mTimeThr); RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/MemoryThr", memThr); @@ -1383,9 +1385,7 @@ public void run() { if(cameraStartSucess && mCameraDriver == 3) { synchronized (this) { - SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity()); - String arCoreLocalizationFiltering = sharedPref.getString(getString(R.string.pref_key_arcore_localization_filtering_speed), getString(R.string.pref_default_arcore_localization_filtering_speed)); - mArCoreCamera = new ARCoreSharedCamera(getActivity(), Float.parseFloat(arCoreLocalizationFiltering)); + mArCoreCamera = new ARCoreSharedCamera(getActivity()); mArCoreCamera.setToast(mToast); mProgressDialog.setTitle(""); mProgressDialog.setMessage(message); @@ -1775,16 +1775,19 @@ public void onClick(DialogInterface dialog, int which) { long currentTime = System.currentTimeMillis()/1000; if(loopClosureId > 0) { + if (mToast != null && mToast.getView().isShown()) mToast.cancel(); mToast.setText(String.format("Loop closure detected! (%d/%d inliers)", inliers, matches)); mToast.show(); } else if(landmarkDetected != 0) { + if (mToast != null && mToast.getView().isShown()) mToast.cancel(); mToast.setText(String.format("Marker %d detected!", landmarkDetected)); mToast.show(); } else if(rejected > 0) { + if (mToast != null && mToast.getView().isShown()) mToast.cancel(); if(inliers >= Integer.parseInt(mMinInliers)) { if(optimizationMaxError > 0.0f) @@ -1806,6 +1809,7 @@ else if(fastMovement) { if(currentTime - mLastFastMovementNotificationStamp > 3) { + if (mToast != null && mToast.getView().isShown()) mToast.cancel(); mToast.setText("Move slower... blurry images are not added to map (\"Settings->Mapping...->Maximum Motion Speed\" is enabled)."); mToast.show(); } @@ -2057,7 +2061,14 @@ private void cameraEventUI( * "Unknown" */ String str = null; - if(key.equals("TangoServiceException")) + if(key.equals("UpstreamRelocationFiltered")) + { + if(mItemDebugVisibility != null && mItemDebugVisibility.isChecked()) { + str = String.format("%s re-localization filtered because an acceleration of %s has been detected, which is over current threshold set in the settings.", + mCameraDriver == 2?"AREngine":"ARCore", value); + } + } + else if(key.equals("TangoServiceException")) str = String.format("Tango service exception: %s", value); else if(key.equals("FisheyeOverExposed")) ;//str = String.format("The fisheye image is over exposed with average pixel value %s px.", value); @@ -3024,7 +3035,6 @@ public void onClick(DialogInterface dialog, int which) { } else { - RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true); if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING)) diff --git a/app/android/src/com/introlab/rtabmap/RTABMapLib.java b/app/android/src/com/introlab/rtabmap/RTABMapLib.java index 774bf55dc6..11af1704a0 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapLib.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapLib.java @@ -76,6 +76,7 @@ public static native void onTouchEvent(long nativeApplication, int touchCount, i public static native void setDepthFromMotion(long nativeApplication, boolean enabled); public static native void setCameraColor(long nativeApplication, boolean enabled); public static native void setAppendMode(long nativeApplication, boolean enabled); + public static native void setUpstreamRelocalizationAccThr(long nativeApplication, float value); public static native void setDataRecorderMode(long nativeApplication, boolean enabled); public static native void setMaxCloudDepth(long nativeApplication, float value); public static native void setMinCloudDepth(long nativeApplication, float value); @@ -135,7 +136,6 @@ public static native boolean exportMesh( public static native float getUpdateTime(long nativeApplication); public static native int getLoopClosureId(long nativeApplication); - public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw, double stamp); public static native void postOdometryEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw, float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy, diff --git a/app/android/src/com/introlab/rtabmap/SettingsActivity.java b/app/android/src/com/introlab/rtabmap/SettingsActivity.java index 48897c21c9..f02d74496c 100644 --- a/app/android/src/com/introlab/rtabmap/SettingsActivity.java +++ b/app/android/src/com/introlab/rtabmap/SettingsActivity.java @@ -222,7 +222,7 @@ public boolean onPreferenceClick(Preference preference) { ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color)); ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation)); - ((Preference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed)); + ((Preference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr)); ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate)); ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed)); ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr)); @@ -285,7 +285,7 @@ public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, Strin if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color)); if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation)); - if(key.compareTo(getString(R.string.pref_key_arcore_localization_filtering_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed)); + if(key.compareTo(getString(R.string.pref_key_arcore_relocalization_acc_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr)); if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate)); if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed)); if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr)); diff --git a/app/ios/RTABMapApp.xcodeproj/project.pbxproj b/app/ios/RTABMapApp.xcodeproj/project.pbxproj index ac31de3e37..5a482d787e 100644 --- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj +++ b/app/ios/RTABMapApp.xcodeproj/project.pbxproj @@ -3,7 +3,7 @@ archiveVersion = 1; classes = { }; - objectVersion = 52; + objectVersion = 54; objects = { /* Begin PBXBuildFile section */ @@ -20,6 +20,7 @@ 4EE016C3259BE464008CCE65 /* Main.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016BF259BE464008CCE65 /* Main.storyboard */; }; 4EE016C4259BE464008CCE65 /* LaunchScreen.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C1259BE464008CCE65 /* LaunchScreen.storyboard */; }; 4EE016C7259BE46F008CCE65 /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C6259BE46F008CCE65 /* Assets.xcassets */; }; + 4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFAA9422CAE4E960055DA51 /* liblas.a */; }; 4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2B259D4DE900575D88 /* libboost_filesystem.a */; }; 4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2C259D4DE900575D88 /* libboost_program_options.a */; }; 4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2D259D4DE900575D88 /* libboost_regex.a */; }; @@ -170,6 +171,7 @@ 4EE016C2259BE464008CCE65 /* Base */ = {isa = PBXFileReference; lastKnownFileType = file.storyboard; name = Base; path = RTABMapApp/Base.lproj/LaunchScreen.storyboard; sourceTree = ""; }; 4EE016C6259BE46F008CCE65 /* Assets.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Assets.xcassets; path = RTABMapApp/Assets.xcassets; sourceTree = ""; }; 4EE016E1259BE96A008CCE65 /* RTABMapApp-Bridging-Header.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = "RTABMapApp-Bridging-Header.h"; path = "RTABMapApp/RTABMapApp-Bridging-Header.h"; sourceTree = ""; }; + 4EFAA9422CAE4E960055DA51 /* liblas.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblas.a; path = RTABMapApp/Libraries/lib/liblas.a; sourceTree = ""; }; 4EFD0B2B259D4DE900575D88 /* libboost_filesystem.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_filesystem.a; path = RTABMapApp/Libraries/lib/libboost_filesystem.a; sourceTree = ""; }; 4EFD0B2C259D4DE900575D88 /* libboost_program_options.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_program_options.a; path = RTABMapApp/Libraries/lib/libboost_program_options.a; sourceTree = ""; }; 4EFD0B2D259D4DE900575D88 /* libboost_regex.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_regex.a; path = RTABMapApp/Libraries/lib/libboost_regex.a; sourceTree = ""; }; @@ -343,6 +345,7 @@ isa = PBXFrameworksBuildPhase; buildActionMask = 2147483647; files = ( + 4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */, 4EFD0F50259D67D900575D88 /* liblibwebp.a in Frameworks */, 4EFD0F51259D67D900575D88 /* liblibpng.a in Frameworks */, 4EFD0F52259D67D900575D88 /* liblibjpeg-turbo.a in Frameworks */, @@ -493,6 +496,7 @@ 4EFD0B2A259D4DE900575D88 /* Frameworks */ = { isa = PBXGroup; children = ( + 4EFAA9422CAE4E960055DA51 /* liblas.a */, 4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */, 4EFD0F4C259D67D900575D88 /* liblibpng.a */, 4EFD0F4F259D67D900575D88 /* liblibprotobuf.a */, @@ -957,6 +961,7 @@ ENABLE_STRICT_OBJC_MSGSEND = YES; GCC_C_LANGUAGE_STANDARD = gnu11; GCC_NO_COMMON_BLOCKS = YES; + GCC_PREPROCESSOR_DEFINITIONS = ""; GCC_WARN_64_TO_32_BIT_CONVERSION = YES; GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR; GCC_WARN_UNDECLARED_SELECTOR = YES; @@ -967,6 +972,7 @@ MTL_ENABLE_DEBUG_INFO = NO; MTL_FAST_MATH = YES; SDKROOT = iphoneos; + SWIFT_ACTIVE_COMPILATION_CONDITIONS = ""; SWIFT_COMPILATION_MODE = wholemodule; SWIFT_OPTIMIZATION_LEVEL = "-O"; VALIDATE_PRODUCT = YES; @@ -1007,7 +1013,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", ); - MARKETING_VERSION = 0.21.0; + MARKETING_VERSION = 0.21.7; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; PRODUCT_NAME = "$(TARGET_NAME)"; @@ -1064,7 +1070,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", ); - MARKETING_VERSION = 0.21.0; + MARKETING_VERSION = 0.21.7; ONLY_ACTIVE_ARCH = YES; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; diff --git a/app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme b/app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme index 5340a8a166..d131b750ae 100644 --- a/app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme +++ b/app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme @@ -23,7 +23,7 @@ @@ -51,6 +51,13 @@ ReferencedContainer = "container:RTABMapApp.xcodeproj"> + + + + + buildConfiguration = "Release"> - - + + - + @@ -15,14 +15,14 @@ - + - + - + @@ -147,7 +147,7 @@ - + @@ -155,8 +155,8 @@ - diff --git a/app/ios/RTABMapApp/NativeWrapper.cpp b/app/ios/RTABMapApp/NativeWrapper.cpp index 27d5ec2f2c..eb73602fce 100644 --- a/app/ios/RTABMapApp/NativeWrapper.cpp +++ b/app/ios/RTABMapApp/NativeWrapper.cpp @@ -33,11 +33,12 @@ void setupCallbacksNative(const void *object, void * classPtr, int, float, float, float, float, int, int, - float, float, float, float, float, float)) + float, float, float, float, float, float), + void(*cameraInfoEventCallback)(void *, int, const char*, const char*)) { if(object) { - native(object)->setupSwiftCallbacks(classPtr, progressCallback, initCallback, statsUpdatedCallback); + native(object)->setupSwiftCallbacks(classPtr, progressCallback, initCallback, statsUpdatedCallback, cameraInfoEventCallback); } else { @@ -283,20 +284,6 @@ void setCameraNative(const void *object, int type) { } } -void postCameraPoseEventNative(const void *object, - float x, float y, float z, float qx, float qy, float qz, float qw, double stamp) -{ - if(object) - { - native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp); - } - else - { - UERROR("object is null!"); - return; - } -} - void postOdometryEventNative(const void *object, float x, float y, float z, float qx, float qy, float qz, float qw, float fx, float fy, float cx, float cy, @@ -312,7 +299,7 @@ void postOdometryEventNative(const void *object, if(object) { native(object)->postOdometryEvent( - rtabmap::Transform(x,y,z,qx,qy,qz,qw), + (qx==0.0f && qy==0.0f && qz==0.0f && qw==0.0f)?rtabmap::Transform():rtabmap::Transform(x,y,z,qx,qy,qz,qw), fx,fy,cx,cy, 0,0,0,0, rtabmap::Transform(), rtabmap::Transform(), stamp, 0, @@ -454,6 +441,13 @@ void setLocalizationModeNative(const void *object, bool enabled) else UERROR("object is null!"); } +void setDataRecorderModeNative(const void *object, bool enabled) +{ + if(object) + native(object)->setDataRecorderMode(enabled); + else + UERROR("object is null!"); +} void setTrajectoryModeNative(const void *object, bool enabled) { if(object) @@ -510,6 +504,13 @@ void setAppendModeNative(const void *object, bool enabled) else UERROR("object is null!"); } +void setUpstreamRelocalizationAccThrNative(const void * object, float value) +{ + if(object) + native(object)->setUpstreamRelocalizationAccThr(value); + else + UERROR("object is null!"); +} void setMaxCloudDepthNative(const void *object, float value) { if(object) @@ -587,6 +588,15 @@ void setDepthConfidenceNative(const void *object, int value) else UERROR("object is null!"); } + +void setExportPointCloudFormatNative(const void *object, const char * format) +{ + if(object) + native(object)->setExportPointCloudFormat(format); + else + UERROR("object is null!"); +} + int setMappingParameterNative(const void *object, const char * key, const char * value) { if(object) diff --git a/app/ios/RTABMapApp/NativeWrapper.hpp b/app/ios/RTABMapApp/NativeWrapper.hpp index 7aa11d93af..c31e2da15d 100644 --- a/app/ios/RTABMapApp/NativeWrapper.hpp +++ b/app/ios/RTABMapApp/NativeWrapper.hpp @@ -29,7 +29,8 @@ void setupCallbacksNative(const void *object, void * classPtr, int, float, float, float, float, int, int, - float, float, float, float, float, float)); + float, float, float, float, float, float), + void(*cameraInfoEventCallback)(void *, int, const char*, const char*)); void destroyNativeApplication(const void *object); void setScreenRotationNative(const void *object, int displayRotation); int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase); @@ -66,8 +67,6 @@ int renderNative(const void *object); bool startCameraNative(const void *object); void stopCameraNative(const void *object); void setCameraNative(const void *object, int type); -void postCameraPoseEventNative(const void *object, - float x, float y, float z, float qx, float qy, float qz, float qw, double stamp); void postOdometryEventNative(const void *object, float x, float y, float z, float qx, float qy, float qz, float qw, float fx, float fy, float cx, float cy, @@ -92,6 +91,7 @@ void setLightingNative(const void *object, bool enabled); void setBackfaceCullingNative(const void *object, bool enabled); void setWireframeNative(const void *object, bool enabled); void setLocalizationModeNative(const void *object, bool enabled); +void setDataRecorderModeNative(const void *object, bool enabled); void setTrajectoryModeNative(const void *object, bool enabled); void setGraphOptimizationNative(const void *object, bool enabled); void setNodesFilteringNative(const void *object, bool enabled); @@ -100,6 +100,7 @@ void setGridVisibleNative(const void *object, bool visible); void setFullResolutionNative(const void *object, bool enabled); void setSmoothingNative(const void *object, bool enabled); void setAppendModeNative(const void *object, bool enabled); +void setUpstreamRelocalizationAccThrNative(const void *object, float value); void setMaxCloudDepthNative(const void *object, float value); void setMinCloudDepthNative(const void *object, float value); void setCloudDensityLevelNative(const void *object, int value); @@ -111,6 +112,7 @@ void setMaxGainRadiusNative(const void *object, float value); void setRenderingTextureDecimationNative(const void *object, int value); void setBackgroundColorNative(const void *object, float gray); void setDepthConfidenceNative(const void *object, int value); +void setExportPointCloudFormatNative(const void *object, const char * format); int setMappingParameterNative(const void *object, const char * key, const char * value); typedef struct ImageNative diff --git a/app/ios/RTABMapApp/RTABMap.swift b/app/ios/RTABMapApp/RTABMap.swift index dc7a602edf..733010c40d 100644 --- a/app/ios/RTABMapApp/RTABMap.swift +++ b/app/ios/RTABMapApp/RTABMap.swift @@ -85,6 +85,25 @@ class RTABMap { observer.statsUpdated(mySelf, nodes: Int(nodes), words: Int(words), points: Int(points), polygons: Int(polygons), updateTime: updateTime, loopClosureId: Int(loopClosureId), highestHypId: Int(highestHypId), databaseMemoryUsed: Int(databaseMemoryUsed), inliers: Int(inliers), matches: Int(matches), featuresExtracted: Int(featuresExtracted), hypothesis: hypothesis, nodesDrawn: Int(nodesDrawn), fps: fps, rejected: Int(rejected), rehearsalValue: rehearsalValue, optimizationMaxError: optimizationMaxError, optimizationMaxErrorRatio: optimizationMaxErrorRatio, distanceTravelled: distanceTravelled, fastMovement: Int(fastMovement), landmarkDetected: Int(landmarkDetected), x: x, y: y, z: z, roll: roll, pitch: pitch, yaw: yaw) } + }, + //cameraInfoEventCallback + {(observer, type, key, value) -> Void in + // Extract pointer to `self` from void pointer: + let mySelf = Unmanaged.fromOpaque(observer!).takeUnretainedValue() + // Call instance method: + //mySelf.TestMethod(); + for (id, observation) in mySelf.observations { + // If the observer is no longer in memory, we + // can clean up the observation for its ID + guard let observer = observation.observer else { + mySelf.observations.removeValue(forKey: id) + continue + } + + let strKey = String(cString: key!) + let strValue = String(cString: value!) + observer.cameraInfoEventReceived(mySelf, type: Int(type), key: strKey, value: strValue) + } }) } @@ -215,21 +234,7 @@ class RTABMap { func setCamera(type: Int) { setCameraNative(native_rtabmap, Int32(type)) } - - func postCameraPoseEvent(pose: simd_float4x4, stamp: TimeInterval) { - let rotation = GLKMatrix3( - m: (pose[0,0], pose[0,1], pose[0,2], - pose[1,0], pose[1,1], pose[1,2], - pose[2,0], pose[2,1], pose[2,2])) - let quat = GLKQuaternionMakeWithMatrix3(rotation) - postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, stamp) - } - - func notifyLost() { - // a null transform will make rtabmap creating a new session - postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0) - } - + func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) { let pose = frame.camera.transform // ViewMatrix let rotation = GLKMatrix3( @@ -239,12 +244,10 @@ class RTABMap { let quat = GLKQuaternionMakeWithMatrix3(rotation) - postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, frame.timestamp) - let confMap = frame.sceneDepth?.confidenceMap let depthMap = frame.sceneDepth?.depthMap let points = frame.rawFeaturePoints?.points - + if points != nil && (depthMap != nil || points!.count>0) { let v = frame.camera.viewMatrix(for: orientation) @@ -313,9 +316,22 @@ class RTABMap { if(frame.lightEstimate != nil) { addEnvSensor(type: 4, value: Float(frame.lightEstimate!.ambientIntensity)) } - + + var lost = false + switch frame.camera.trackingState { + case .normal: + lost = false + case .limited(.excessiveMotion): + lost = false + case .limited(.insufficientFeatures): + lost = false + default: + lost = true + } + // Notify lost with pose=null + postOdometryEventNative(native_rtabmap, - pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, + !lost ? pose[3,0]:0, !lost ? pose[3,1]:0, !lost ? pose[3,2]:0, !lost ? quat.x:0, !lost ? quat.y:0, !lost ? quat.z:0, !lost ? quat.w:0, frame.camera.intrinsics[0,0], // fx frame.camera.intrinsics[1,1], // fy frame.camera.intrinsics[2,0], // cx @@ -390,8 +406,8 @@ class RTABMap { func setLocalizationMode(enabled: Bool) { setLocalizationModeNative(native_rtabmap, enabled) } - func setTrajectoryMode(enabled: Bool) { - setTrajectoryModeNative(native_rtabmap, enabled) + func setDataRecorderMode(enabled: Bool) { + setDataRecorderModeNative(native_rtabmap, enabled) } func setGraphOptimization(enabled: Bool) { setGraphOptimizationNative(native_rtabmap, enabled) @@ -414,6 +430,9 @@ class RTABMap { func setAppendMode(enabled: Bool) { setAppendModeNative(native_rtabmap, enabled) } + func setUpstreamRelocalizationAccThr(value: Float) { + setUpstreamRelocalizationAccThrNative(native_rtabmap, value) + } func setMaxCloudDepth(value: Float) { setMaxCloudDepthNative(native_rtabmap, value) } @@ -447,6 +466,11 @@ class RTABMap { func setDepthConfidence(value: Int) { setDepthConfidenceNative(native_rtabmap, Int32(value)) } + func setExportPointCloudFormat(format: String) { + format.utf8CString.withUnsafeBufferPointer { bufferFormat in + return setExportPointCloudFormatNative(native_rtabmap, bufferFormat.baseAddress) + } + } func setMappingParameter(key: String, value: String) { key.utf8CString.withUnsafeBufferPointer { bufferKey in value.utf8CString.withUnsafeBufferPointer { bufferValue in @@ -523,6 +547,7 @@ protocol RTABMapObserver: class { roll: Float, pitch: Float, yaw: Float) + func cameraInfoEventReceived(_ rtabmap: RTABMap, type: Int, key: String, value: String) } extension String { diff --git a/app/ios/RTABMapApp/ViewController.swift b/app/ios/RTABMapApp/ViewController.swift index 40ebfbaca9..929cbb636f 100644 --- a/app/ios/RTABMapApp/ViewController.swift +++ b/app/ios/RTABMapApp/ViewController.swift @@ -41,6 +41,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP private var mTimeThr: Int = 0 private var mMaxFeatures: Int = 0 private var mLoopThr = 0.11 + private var mDataRecording = false private var mReviewRequested = false @@ -63,7 +64,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP case .STATE_CAMERA: return "Camera Preview" case .STATE_MAPPING: - return "Mapping" + return mDataRecording ? "Data Recording" : "Mapping" case .STATE_PROCESSING: return "Processing" case .STATE_VISUALIZING: @@ -428,6 +429,16 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP } } + func cameraInfoEventReceived(_ rtabmap: RTABMap, type: Int, key: String, value: String) { + if(self.debugShown && key == "UpstreamRelocationFiltered") + { + DispatchQueue.main.async { + self.dismiss(animated: true) + self.showToast(message: "ARKit re-localization filtered because an acceleration of \(value) has been detected, which is over current threshold set in the settings.", seconds: 3) + } + } + } + func getMemoryUsage() -> UInt64 { var taskInfo = mach_task_basic_info() var count = mach_msg_type_number_t(MemoryLayout.size)/4 @@ -460,7 +471,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP if(mMapNodes > 0 && self.openedDatabasePath == nil) { let msg = "RTAB-Map has been pushed to background while mapping. Do you want to save the map now?" - let alert = UIAlertController(title: "Mapping Stopped!", message: msg, preferredStyle: .alert) + let alert = UIAlertController(title: mDataRecording ? "Data Recording Stopped" : "Mapping Stopped!", message: msg, preferredStyle: .alert) let alertActionNo = UIAlertAction(title: "Ignore", style: .cancel) { (UIAlertAction) -> Void in } @@ -582,6 +593,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP mState = state; var actionNewScanEnabled: Bool + var actionNewDataRecording: Bool var actionSaveEnabled: Bool var actionResumeEnabled: Bool var actionExportEnabled: Bool @@ -602,7 +614,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP exportOBJPLYButton.isHidden = true orthoDistanceSlider.isHidden = cameraMode != 3 orthoGridSlider.isHidden = cameraMode != 3 - actionNewScanEnabled = true + actionNewScanEnabled = !mDataRecording + actionNewDataRecording = mDataRecording actionSaveEnabled = false actionResumeEnabled = false actionExportEnabled = false @@ -621,7 +634,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP exportOBJPLYButton.isHidden = true orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible - actionNewScanEnabled = true + actionNewScanEnabled = !mDataRecording + actionNewDataRecording = mDataRecording actionSaveEnabled = false actionResumeEnabled = false actionExportEnabled = false @@ -643,6 +657,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP orthoDistanceSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING orthoGridSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING actionNewScanEnabled = false + actionNewDataRecording = false actionSaveEnabled = false actionResumeEnabled = false actionExportEnabled = false @@ -662,6 +677,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible actionNewScanEnabled = true + actionNewDataRecording = true actionSaveEnabled = mMapNodes>0 actionResumeEnabled = mMapNodes>0 actionExportEnabled = mMapNodes>0 @@ -681,6 +697,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible actionNewScanEnabled = true + actionNewDataRecording = true actionSaveEnabled = mState != .STATE_WELCOME && mMapNodes>0 actionResumeEnabled = mState != .STATE_WELCOME && mMapNodes>0 actionExportEnabled = mState != .STATE_WELCOME && mMapNodes>0 @@ -762,9 +779,16 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP self.optimization(approach: -1) }), optimizeAdvancedMenu]) + + // Advanced menu + let advancedMenu = UIMenu(title: "Advanced...", children: [ + UIAction(title: "New Data Recording", image: UIImage(systemName: "plus.app"), attributes: actionNewDataRecording ? [] : .disabled, state: .off, handler: { _ in + self.newScan(dataRecordingMode: true) + }) + ]) var fileMenuChildren: [UIMenuElement] = [] - fileMenuChildren.append(UIAction(title: "New Scan", image: UIImage(systemName: "plus.app"), attributes: actionNewScanEnabled ? [] : .disabled, state: .off, handler: { _ in + fileMenuChildren.append(UIAction(title: "New Mapping Session", image: UIImage(systemName: "plus.app"), attributes: actionNewScanEnabled ? [] : .disabled, state: .off, handler: { _ in self.newScan() })) if(actionOptimizeEnabled) { @@ -787,6 +811,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP fileMenuChildren.append(UIAction(title: "Append Scan", image: UIImage(systemName: "play.fill"), attributes: actionResumeEnabled ? [] : .disabled, state: .off, handler: { _ in self.resumeScan() })) + fileMenuChildren.append(advancedMenu) // File menu let fileMenu = UIMenu(title: "File", options: .displayInline, children: fileMenuChildren) @@ -1001,16 +1026,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP status = "Camera Is Occluded Or Lighting Is Too Dark" } - if accept - { - if let rotation = UIApplication.shared.windows.first?.windowScene?.interfaceOrientation - { - rtabmap?.postOdometryEvent(frame: frame, orientation: rotation, viewport: self.view.frame.size) - } - } - else + if let rotation = UIApplication.shared.windows.first?.windowScene?.interfaceOrientation { - rtabmap?.notifyLost(); + rtabmap?.postOdometryEvent(frame: frame, orientation: rotation, viewport: self.view.frame.size) } if !status.isEmpty { @@ -1292,7 +1310,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP } @IBAction func singleTapped(_ gestureRecognizer: UITapGestureRecognizer) { - if gestureRecognizer.state == UIGestureRecognizer.State.recognized + if gestureRecognizer.state == .recognized { resetNoTouchTimer(!mHudVisible) @@ -1322,6 +1340,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode")); rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing")); rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode")); + rtabmap!.setUpstreamRelocalizationAccThr(value: defaults.float(forKey: "UpstreamRelocalizationFilteringAccThr")); + rtabmap!.setExportPointCloudFormat(format: defaults.string(forKey: "ExportPointCloudFormat")!); mTimeThr = (defaults.string(forKey: "TimeLimit")! as NSString).integerValue mMaxFeatures = (defaults.string(forKey: "MaxFeaturesExtractedLoopClosure")! as NSString).integerValue @@ -1379,8 +1399,10 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP let bgColor = defaults.float(forKey: "BackgroundColor"); rtabmap!.setBackgroundColor(gray: bgColor); + let format = defaults.string(forKey: "ExportPointCloudFormat")!; DispatchQueue.main.async { self.statusLabel.textColor = bgColor>=0.6 ? UIColor(white: 0.0, alpha: 1) : UIColor(white: 1.0, alpha: 1) + self.exportOBJPLYButton.setTitle("Export OBJ-\(format == "las" ? "LAS" : "PLY")", for: .normal) } rtabmap!.setClusterRatio(value: defaults.float(forKey: "NoiseFilteringRatio")); @@ -1409,22 +1431,24 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP rtabmap!.postExportation(visualize: false) } - let alertController = UIAlertController(title: "Append Mode", message: "The camera preview will not be aligned to map on start, move to a previously scanned area, then push Record. When a loop closure is detected, new scans will be appended to map.", preferredStyle: .alert) - - let okAction = UIAlertAction(title: "OK", style: .default) { (action) in + if(!mDataRecording) { + let alertController = UIAlertController(title: "Append Mode", message: "The camera preview will not be aligned to map on start, move to a previously scanned area, then push Record. When a loop closure is detected, new scans will be appended to map.", preferredStyle: .alert) + + let okAction = UIAlertAction(title: "OK", style: .default) { (action) in + } + alertController.addAction(okAction) + + present(alertController, animated: true) } - alertController.addAction(okAction) - - present(alertController, animated: true) setGLCamera(type: 0); startCamera(); } - func newScan() + func newScan(dataRecordingMode: Bool = false) { print("databases.size() = \(databases.size())") - if(databases.count >= 5 && !mReviewRequested && self.depthSupported) + if(databases.count >= 10 && !mReviewRequested && self.depthSupported) { SKStoreReviewController.requestReviewInCurrentScene() mReviewRequested = true @@ -1438,7 +1462,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP mMapNodes = 0; self.openedDatabasePath = nil let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB) - let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory") if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) && FileManager.default.fileExists(atPath: tmpDatabase.path) && tmpDatabase.fileSize > 1024*1024) // > 1MB @@ -1454,7 +1477,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP catch { print("Could not clear tmp database: \(error)") } - self.newScan() + self.newScan(dataRecordingMode: dataRecordingMode) } alert.addAction(alertActionNo) let alertActionCancel = UIAlertAction(title: "Cancel", style: .cancel) { @@ -1545,10 +1568,25 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP } else { + let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory") && !dataRecordingMode + mDataRecording = dataRecordingMode + self.rtabmap!.setDataRecorderMode(enabled: dataRecordingMode) + self.optimizedGraphShown = true // Always reset to true when opening a database self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false, clearDatabase: true) - + if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING)) { + if(mDataRecording) { + let alertController = UIAlertController(title: "Data Recording Mode", message: "This mode should be only used if you want to record raw ARKit data as long as possible without any feedback: loop closure detection and map rendering are disabled. The database size in Debug display shows how much data has been recorded so far.", preferredStyle: .alert) + + let okAction = UIAlertAction(title: "OK", style: .default) { (action) in + } + alertController.addAction(okAction) + + present(alertController, animated: true) + self.debugShown = true + } + self.setGLCamera(type: 0); self.startCamera(); } @@ -1575,6 +1613,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP alert.addAction(yes) let no = UIAlertAction(title: "No", style: .cancel) { (UIAlertAction) -> Void in + if(self.mDataRecording) { + self.save() // We cannot skip saving after data recording + } } alert.addAction(no) @@ -1583,10 +1624,17 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP self.saveDatabase(fileName: fileName); } } + else + { + self.save() + } } //Step : 3 var placeholder = Date().getFormattedDate(format: "yyMMdd-HHmmss") + if(mDataRecording) { + placeholder += "-recording" + } if self.openedDatabasePath != nil && !self.openedDatabasePath!.path.isEmpty { var components = self.openedDatabasePath!.lastPathComponent.components(separatedBy: ".") @@ -1604,7 +1652,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP //Step : 4 alert.addAction(save) //Cancel action - alert.addAction(UIAlertAction(title: "Cancel", style: .default) { (alertAction) in }) + if(!mDataRecording) { + alert.addAction(UIAlertAction(title: "Cancel", style: .default) { (alertAction) in }) + } self.present(alert, animated: true) { alert.textFields?.first?.selectAll(nil) @@ -1649,7 +1699,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP print("Could not clear tmp database: \(error)") } self.updateDatabases() - self.updateState(state: previousState) + self.updateState(state: self.mDataRecording ? .STATE_WELCOME : previousState) }) } @@ -1854,6 +1904,10 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP locationManager?.stopUpdatingLocation() rtabmap?.setPausedMapping(paused: true) rtabmap?.stopCamera() + if(mDataRecording) { + // this will show the trajectory before saving + self.rtabmap!.setGraphOptimization(enabled: false) + } setGLCamera(type: 2) if(mState == .STATE_VISUALIZING_CAMERA) { @@ -1863,34 +1917,42 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP if !ignoreSaving { - dismiss(animated: true, completion: { - var msg = "Do you want to do standard graph optimization and make a nice assembled mesh now? This can be also done later using \"Optimize\" and \"Assemble\" menus." - let depthUsed = self.depthSupported && UserDefaults.standard.bool(forKey: "LidarMode") - if !depthUsed - { - msg = "Do you want to do standard graph optimization now? This can be also done later using \"Optimize\" menu." - } - let alert = UIAlertController(title: "Mapping Stopped! Optimize Now?", message: msg, preferredStyle: .alert) - if depthUsed { - let alertActionOnlyGraph = UIAlertAction(title: "Only Optimize", style: .default) + if(mDataRecording) + { + // Go directly to save + self.save() + } + else + { + dismiss(animated: true, completion: { + var msg = "Do you want to do standard graph optimization and make a nice assembled mesh now? This can be also done later using \"Optimize\" and \"Assemble\" menus." + let depthUsed = self.depthSupported && UserDefaults.standard.bool(forKey: "LidarMode") + if !depthUsed { + msg = "Do you want to do standard graph optimization now? This can be also done later using \"Optimize\" menu." + } + let alert = UIAlertController(title: "Mapping Stopped! Optimize Now?", message: msg, preferredStyle: .alert) + if depthUsed { + let alertActionOnlyGraph = UIAlertAction(title: "Only Optimize", style: .default) + { + (UIAlertAction) -> Void in + self.optimization(withStandardMeshExport: false, approach: -1) + } + alert.addAction(alertActionOnlyGraph) + } + let alertActionNo = UIAlertAction(title: "Save First", style: .cancel) { (UIAlertAction) -> Void in - self.optimization(withStandardMeshExport: false, approach: -1) + self.save() } - alert.addAction(alertActionOnlyGraph) - } - let alertActionNo = UIAlertAction(title: "Save First", style: .cancel) { - (UIAlertAction) -> Void in - self.save() - } - alert.addAction(alertActionNo) - let alertActionYes = UIAlertAction(title: "Yes", style: .default) { - (UIAlertAction) -> Void in - self.optimization(withStandardMeshExport: depthUsed, approach: -1) - } - alert.addAction(alertActionYes) - self.present(alert, animated: true, completion: nil) - }) + alert.addAction(alertActionNo) + let alertActionYes = UIAlertAction(title: "Yes", style: .default) { + (UIAlertAction) -> Void in + self.optimization(withStandardMeshExport: depthUsed, approach: -1) + } + alert.addAction(alertActionYes) + self.present(alert, animated: true, completion: nil) + }) + } } else if(mMapNodes == 0) { @@ -1937,6 +1999,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP updateState(state: .STATE_PROCESSING); var status = 0 DispatchQueue.background(background: { + self.optimizedGraphShown = true // Always reset to true when opening a database status = self.rtabmap!.openDatabase(databasePath: self.openedDatabasePath!.path, databaseInMemory: true, optimize: false, clearDatabase: false) }, completion:{ // main thread diff --git a/app/ios/RTABMapApp/install_deps.sh b/app/ios/RTABMapApp/install_deps.sh index a33acc46be..b1964c66f2 100755 --- a/app/ios/RTABMapApp/install_deps.sh +++ b/app/ios/RTABMapApp/install_deps.sh @@ -4,17 +4,13 @@ set -euxo pipefail # Tested on Apple Silicon Mac, with cmake 3.19.2. -mkdir Libraries +mkdir -p Libraries cd Libraries pwd=$(pwd) prefix=$pwd sysroot=iphoneos #sysroot=iphonesimulator - -# Install directory for all dependencies -mkdir -p $prefix - # openmp # based on https://github.com/Homebrew/homebrew-core/blob/HEAD/Formula/libomp.rb #curl -L https://github.com/llvm/llvm-project/releases/download/llvmorg-11.1.0/openmp-11.1.0.src.tar.xz -o openmp-11.1.0.src.tar.xz @@ -30,41 +26,60 @@ mkdir -p $prefix #cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" # Boost -echo "wget boost..." -curl -L https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz -o boost_1_59_0.tar.gz -tar -xzf boost_1_59_0.tar.gz +if [ ! -e $prefix/include/boost ] +then +if [ ! -e boost_1_59_0 ] +then + echo "wget boost..." + curl -L https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz -o boost_1_59_0.tar.gz + tar -xzf boost_1_59_0.tar.gz +fi cd boost_1_59_0 curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/489ff2869eccd6f8d03ffb9090ef839108762741/BoostConfig.cmake.in -o BoostConfig.cmake.in curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/b07fe7d4e5dfe5f1d110c733e5cf660d79a26378/CMakeLists.txt -o CMakeLists.txt -mkdir build +mkdir -p build cd build cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix .. cmake --build . --config Release cmake --build . --config Release --target install cd $pwd #rm -r boost_1_59_0.tar.gz boost_1_59_0 +fi # eigen -echo "wget eigen..." -curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz -tar -xzf 3.3.9.tar.gz +if [ ! -e $prefix/include/eigen3 ] +then +if [ ! -e eigen-3.3.9 ] +then + echo "wget eigen..." + curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz + tar -xzf 3.3.9.tar.gz +fi cd eigen-3.3.9 -mkdir build +mkdir -p build cd build cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix .. cmake --build . --config Release cmake --build . --config Release --target install cd $pwd #rm -r 3.3.9.tar.gz eigen-3.3.9 +fi # FLANN -echo "wget flann..." -git clone https://github.com/flann-lib/flann.git +if [ ! -e $prefix/include/flann ] +then +if [ ! -e flann ] +then + echo "wget flann..." + git clone https://github.com/flann-lib/flann.git -b 1.8.4 +fi cd flann -git checkout 1.8.4 -curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch -o flann_ios.patch -git apply flann_ios.patch -mkdir build +if [ ! -e flann_ios.patch ] +then + curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch -o flann_ios.patch + git apply flann_ios.patch +fi +mkdir -p build cd build # comment "add_subdirectory( test )" in top CMakeLists.txt # comment "add_subdirectory( doc )" in top CMakeLists.txt @@ -73,76 +88,131 @@ cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED= cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" cd $pwd #rm -r flann +fi # GTSAM -git clone https://bitbucket.org/gtborg/gtsam.git -cd gtsam -git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5 +if [ ! -e $prefix/include/gtsam ] +then +if [ ! -e gtsam ] +then + git clone https://bitbucket.org/gtborg/gtsam.git + cd gtsam + git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5 +else + cd gtsam +fi # patch -curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch -git apply gtsam_GKlib_ios_fix.patch -mkdir build +if [ ! -e gtsam_GKlib_ios_fix.patch ] +then + curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch + git apply gtsam_GKlib_ios_fix.patch +fi +mkdir -p build cd build cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_WRAP_SERIALIZATION=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix .. cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" cd $pwd #rm -rf gtsam +fi # g2o -git clone https://github.com/RainerKuemmerle/g2o.git -cd g2o -git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e +if [ ! -e $prefix/include/g2o ] +then +if [ ! -e g2o ] +then + git clone https://github.com/RainerKuemmerle/g2o.git + cd g2o + git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e +else + cd g2o +fi # patch -curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch -git apply g2o_ios_fix.patch -mkdir build +if [ ! -e g2o_ios_fix.patch ] +then + curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch + git apply g2o_ios_fix.patch +fi +mkdir -p build cd build cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DCMAKE_FIND_ROOT_PATH=$prefix .. cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" cd $pwd #rm -rf g2o +fi # VTK -git clone https://github.com/Kitware/VTK.git -cd VTK -git checkout tags/v8.2.0 +if [ ! -e $prefix/lib/vtk.framework ] +then +if [ ! -e VTK ] +then + git clone https://github.com/Kitware/VTK.git + cd VTK + git checkout tags/v8.2.0 +else + cd VTK +fi git cherry-pick bf3ae8072df2393c7270509bae41be0776826346 -mkdir build +mkdir -p build cd build cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON .. # For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES="" cmake --build . --config Release cd $pwd #rm -rf VTK +fi # PCL -git clone https://github.com/PointCloudLibrary/pcl.git -cd pcl -git checkout tags/pcl-1.11.1 +if [ ! -e $prefix/include/pcl-1.11 ] +then +if [ ! -e pcl ] +then + git clone https://github.com/PointCloudLibrary/pcl.git + cd pcl + git checkout tags/pcl-1.11.1 +else + cd pcl +fi # patch -curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch -git apply pcl_1_11_1_vtk_ios_support.patch -mkdir build +if [ ! -e pcl_1_11_1_vtk_ios_support.patch ] +then + curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch + git apply pcl_1_11_1_vtk_ios_support.patch +fi +mkdir -p build cd build cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF -DPCL_ENABLE_SSE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix .. cmake --build . --config Release cmake --build . --config Release --target install cd $pwd #rm -rf pcl +fi # OpenCV -git clone https://github.com/opencv/opencv_contrib.git -cd opencv_contrib -git checkout tags/3.4.2 +if [ ! -e $prefix/include/opencv2 ] +then +if [ ! -e opencv_contrib ] +then + git clone https://github.com/opencv/opencv_contrib.git + cd opencv_contrib + git checkout tags/3.4.2 +fi cd $pwd -git clone https://github.com/opencv/opencv.git -cd opencv -git checkout tags/3.4.2 -curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch -git apply opencv_ios.patch -mkdir build +if [ ! -e opencv ] +then + git clone https://github.com/opencv/opencv.git + cd opencv + git checkout tags/3.4.2 +else + cd opencv +fi +if [ ! -e opencv_ios.patch ] +then + curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch + git apply opencv_ios.patch +fi +mkdir -p build cd build # add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DOPENCV_EXTRA_MODULES_PATH=$prefix/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_apps=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_stereo=OFF .. @@ -150,13 +220,31 @@ cmake --build . --config Release cmake --build . --config Release --target install cd $pwd #rm -rf opencv opencv_contrib +fi + +# LAS +if [ ! -e $prefix/include/liblas ] +then +if [ ! -e libLAS ] +then + git clone https://github.com/libLAS/libLAS.git +fi +cd libLAS +sed -i '' 's/SHARED/STATIC/g' src/CMakeLists.txt +mkdir -p build +cd build +cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_UTILITIES=OFF -DWITH_TESTS=OFF -DWITH_GEOTIFF=OFF .. +cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" +cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS="" CODE_SIGNING_ALLOWED="NO" +cd $pwd +fi -mkdir rtabmap +mkdir -p rtabmap cd rtabmap cmake -DANDROID_PREBUILD=ON ../../../../.. cmake --build . --config Release -mkdir ios +mkdir -p ios cd ios -cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF -DBUILD_EXAMPLES=OFF ../../../../../.. +cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../.. cmake --build . --config Release cmake --build . --config Release --target install diff --git a/app/ios/Settings.bundle/Assembling.plist b/app/ios/Settings.bundle/Assembling.plist index 382d6e1562..416d43c246 100644 --- a/app/ios/Settings.bundle/Assembling.plist +++ b/app/ios/Settings.bundle/Assembling.plist @@ -346,6 +346,32 @@ 0 + + Type + PSGroupSpecifier + FooterText + The format of exported point cloud data. + + + Type + PSMultiValueSpecifier + Title + Export Point Cloud Format + Key + ExportPointCloudFormat + DefaultValue + ply + Titles + + PLY + LAS + + Values + + ply + las + + diff --git a/app/ios/Settings.bundle/License.plist b/app/ios/Settings.bundle/License.plist index dc827e966a..ce1df93137 100644 --- a/app/ios/Settings.bundle/License.plist +++ b/app/ios/Settings.bundle/License.plist @@ -13,7 +13,7 @@ ======= RTAB-Map ======= RTAB-Map - https://github.com/introlab/rtabmap -Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved. +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved. Copyright (c) XXX, contributors, all rights reserved. Redistribution and use in source and binary forms, with or without @@ -165,6 +165,53 @@ 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. +======= libLAS ======= + +Copyright (c) 2007, Martin Isenburg, isenburg at cs.unc.edu + +Copyright (c) 2008, Howard Butler, hobu.inc at gmail.com + +Copyright (c) 2008, Mateusz Loskot, mateusz at loskot.net + +Copyright (c) 2008, Phil Vachon, philippe at cowpig.ca + +Copyright (c) 2008, Frank Warmerdam, warmerdam at pobox.com + +Copyright (c) 2008, Martin Rodriguez, mrodriguez at stereocarto.com + +Copyright (c) 2016, Oscar Martinez Rubi o.rubi at esciencecenter.nl + +Copyright (c) 2016, Romulo Goncalves r.goncalves at esciencecenter.nl + +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 Martin Isenburg or Iowa Department + of Natural Resources 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 OWNER 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. diff --git a/app/ios/Settings.bundle/Mapping.plist b/app/ios/Settings.bundle/Mapping.plist index 0f2dc059b6..9891b1cbc2 100644 --- a/app/ios/Settings.bundle/Mapping.plist +++ b/app/ios/Settings.bundle/Mapping.plist @@ -70,6 +70,48 @@ DefaultValue + + Type + PSGroupSpecifier + FooterText + Filter ARKit's re-localization events to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set an acceleration threshold to detect those events. + + + Type + PSMultiValueSpecifier + Title + ARKit Re-Localization Acceleration Threshold + Key + UpstreamRelocalizationFilteringAccThr + DefaultValue + 58.8399 + Titles + + Disabled + 10 g + 9 g + 8 g + 7 g + 6 g + 5 g + 4 g + 3 g + 2 g + + Values + + 0 + 98.0665 + 88.25985 + 78.4532 + 68.64655 + 58.8399 + 49.03325 + 39.2266 + 29.41995 + 19.6133 + + Type PSGroupSpecifier @@ -368,7 +410,7 @@ Key MaxOptimizationError DefaultValue - 1 + 2 Titles 4x diff --git a/app/ios/Settings.bundle/Root.plist b/app/ios/Settings.bundle/Root.plist index bc9360fe97..d7c4c48aad 100644 --- a/app/ios/Settings.bundle/Root.plist +++ b/app/ios/Settings.bundle/Root.plist @@ -60,7 +60,7 @@ PSMultiValueSpecifier Values - 0.0 + 0 5 4.5 4 @@ -74,7 +74,7 @@ DefaultValue - 0.0 + 0 Key MinDepth Title @@ -95,8 +95,8 @@ PSMultiValueSpecifier Values - 0.0 - 0.29999999999999999 + 0 + 0.3 0.5 0.75 1 @@ -130,7 +130,7 @@ FooterText - Increase minimum polygon angle to force scanning perpendicular to surfaces. Set a decimation factor to reduce the number of polygons. The higher the factor, the less polygons will remain. Note that the decimation factor is done after decreasing point cloud density. Decimation factor and texture resolution don't affect post-processing. + Increase minimum polygon angle to force scanning perpendicular to surfaces. Set a decimation factor to reduce the number of polygons. The higher the factor, the less polygons will remain. Note that the decimation factor is done after decreasing point cloud density. Decimation factor and texture resolution don't affect post-processing. Type PSGroupSpecifier @@ -196,7 +196,7 @@ DefaultValue - 0.0 + 0 Key MeshDecimationFactor Title @@ -220,18 +220,18 @@ PSMultiValueSpecifier Values - 0.98999999999999999 - 0.94999999999999996 - 0.90000000000000002 - 0.80000000000000004 - 0.69999999999999996 - 0.59999999999999998 + 0.99 + 0.95 + 0.9 + 0.8 + 0.7 + 0.6 0.5 - 0.40000000000000002 - 0.29999999999999999 - 0.20000000000000001 - 0.10000000000000001 - 0.0 + 0.4 + 0.3 + 0.2 + 0.1 + 0 @@ -266,7 +266,7 @@ DefaultValue - 0.20000000000000001 + 0.2 Key BackgroundColor Title @@ -288,16 +288,16 @@ PSMultiValueSpecifier Values - 0.90000000000000002 - 0.80000000000000004 - 0.69999999999999996 - 0.59999999999999998 + 0.9 + 0.8 + 0.7 + 0.6 0.5 - 0.40000000000000002 - 0.29999999999999999 - 0.20000000000000001 - 0.10000000000000001 - 0.0 + 0.4 + 0.3 + 0.2 + 0.1 + 0 @@ -402,19 +402,19 @@ PSMultiValueSpecifier Values - 0.29999999999999999 - 0.20000000000000001 - 0.10000000000000001 - 0.050000000000000003 - 0.025000000000000001 + 0.3 + 0.2 + 0.1 + 0.05 + 0.025 0.02 - 0.014999999999999999 + 0.015 0.01 DefaultValue - 0.050000000000000003 + 0.05 Key NoiseFilteringRatio Title @@ -439,22 +439,22 @@ Values 1 - 0.90000000000000002 - 0.80000000000000004 - 0.69999999999999996 - 0.59999999999999998 + 0.9 + 0.8 + 0.7 + 0.6 0.5 - 0.40000000000000002 - 0.29999999999999999 - 0.20000000000000001 - 0.10000000000000001 - 0.050000000000000003 + 0.4 + 0.3 + 0.2 + 0.1 + 0.05 0.01 FooterText - Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved. + Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved. Title About Type @@ -462,7 +462,7 @@ DefaultValue - 0.21.0 + 0.21.7 Key Version Title diff --git a/corelib/include/rtabmap/core/LASWriter.h b/corelib/include/rtabmap/core/LASWriter.h new file mode 100644 index 0000000000..9c29006dec --- /dev/null +++ b/corelib/include/rtabmap/core/LASWriter.h @@ -0,0 +1,43 @@ +/* +Copyright (c) 2010-2024, 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 CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ +#define CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ + +#include +#include + +namespace rtabmap { + +int saveLASFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); +int saveLASFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector(), const std::vector & intensities = std::vector()); +int saveLASFile(const std::string & filePath, const pcl::PointCloud & cloud, const std::vector & cameraIds = std::vector()); + +} + + +#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ */ diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index aadff75ae8..44741b0d6d 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -539,6 +539,22 @@ IF(PDAL_FOUND) ENDIF(PDAL_VERSION VERSION_LESS "1.7") ENDIF(PDAL_FOUND) +IF(libLAS_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${libLAS_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${libLAS_LIBRARIES} + ) + SET(SRC_FILES + ${SRC_FILES} + LASWriter.cpp + ) +ENDIF(libLAS_FOUND) + + IF(CudaSift_FOUND) #target SET(LIBRARIES diff --git a/corelib/src/LASWriter.cpp b/corelib/src/LASWriter.cpp new file mode 100644 index 0000000000..cd02b22f6b --- /dev/null +++ b/corelib/src/LASWriter.cpp @@ -0,0 +1,169 @@ +/* +Copyright (c) 2010-2024, 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. +*/ + +#include +#include +#include +#include +#include + +#include +#include + +namespace rtabmap { + +int saveLASFile(const std::string & filePath, + const pcl::PointCloud & cloud, + const std::vector & cameraIds) +{ + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + + std::ofstream ofs; + ofs.open(filePath, std::ios::out | std::ios::binary); + + liblas::Header header; + header.SetScale(0.001, 0.001, 0.001); + + header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz"); + + try { + liblas::Writer writer(ofs, header); + + for(size_t i=0; i & cloud, + const std::vector & cameraIds, + const std::vector & intensities) +{ + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(), + uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str()); + + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + + std::ofstream ofs; + ofs.open(filePath, std::ios::out | std::ios::binary); + + liblas::Header header; + + header.SetScale(0.001, 0.001, 0.001); + header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz"); + + try { + liblas::Writer writer(ofs, header); + + for(size_t i=0; i & cloud, + const std::vector & cameraIds) +{ + UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(), + uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str()); + + std::ofstream ofs; + ofs.open(filePath, std::ios::out | std::ios::binary); + + liblas::Header header; + header.SetScale(0.001, 0.001, 0.001); + + header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz"); + + try { + liblas::Writer writer(ofs, header); + + for(size_t i=0; i OptimizerGTSAM::optimize( { gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances( (gtsam::Vector(6) << - (hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), hasGPSPrior?1e-2:std::numeric_limits::min(), // roll, pitch, fixed yaw if there are no priors + (hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits::min()), // roll, pitch, fixed yaw if there are no priors (hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz ).finished()); graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); @@ -473,7 +473,7 @@ std::map OptimizerGTSAM::optimize( { Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz(); gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1)); - gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10)); + gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma())); graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1))); lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1)); } diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 43b6155b5b..69a34000d6 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -107,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_pdal->setText("No"); _ui->label_pdal_license->setEnabled(false); #endif +#ifdef RTABMAP_LIBLAS + _ui->label_liblas->setText("Yes"); + _ui->label_liblas_license->setEnabled(true); +#else + _ui->label_liblas->setText("No"); + _ui->label_liblas_license->setEnabled(false); +#endif #ifdef RTABMAP_CUDASIFT _ui->label_cudasift->setText("Yes"); _ui->label_cudasift_license->setEnabled(true); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 748322aa22..191aa131c4 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -7620,6 +7620,7 @@ void DatabaseViewer::updateGraphView() // remove intermediate nodes? if(ui_->checkBox_ignoreIntermediateNodes->isVisible() && + ui_->checkBox_ignoreIntermediateNodes->isEnabled() && ui_->checkBox_ignoreIntermediateNodes->isChecked()) { for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index a860c2f2f2..6fd82ea709 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -79,6 +79,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef RTABMAP_PDAL #include +#elif defined(RTABMAP_LIBLAS) +#include #endif namespace rtabmap { @@ -211,7 +213,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->comboBox_camProjExportCamera, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); -#ifndef RTABMAP_PDAL +#if !defined(RTABMAP_PDAL) && !defined(RTABMAP_LIBLAS) _ui->comboBox_camProjExportCamera->setEnabled(false); _ui->label_camProjExportCamera->setEnabled(false); _ui->label_camProjExportCamera->setText(_ui->label_camProjExportCamera->text() + " (PDAL dependency required)"); @@ -4046,6 +4048,8 @@ void ExportCloudsDialog::saveClouds( extensions += QString(" *.") + iter->c_str(); } extensions += ")"; +#elif defined(RTABMAP_LIBLAS) + QString extensions = tr("Point cloud data (*.ply *.pcd *.las *.laz)"); #else QString extensions = tr("Point cloud data (*.ply *.pcd)"); #endif @@ -4160,7 +4164,7 @@ void ExportCloudsDialog::saveClouds( success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0; } } -#ifdef RTABMAP_PDAL +#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS) else if(!QFileInfo(path).suffix().isEmpty()) { std::vector cameraIds(pointToPixels.size(), 0); @@ -4173,19 +4177,37 @@ void ExportCloudsDialog::saveClouds( } if(cloudIWithNormals.get()) { +#ifdef RTABMAP_PDAL success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds, binaryMode) == 0; +#else + UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation."); + success = false; +#endif } else if(cloudIWithoutNormals.get()) { +#ifdef RTABMAP_PDAL success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds, binaryMode) == 0; +#else + success = saveLASFile(path.toStdString(), *cloudIWithoutNormals, cameraIds) == 0; +#endif } else if(cloudRGBWithoutNormals.get()) { +#ifdef RTABMAP_PDAL success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds, binaryMode) == 0; +#else + success = saveLASFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds) == 0; +#endif } else { +#ifdef RTABMAP_PDAL success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds, binaryMode) == 0; +#else + UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation."); + success = false; +#endif } } #endif @@ -4230,6 +4252,8 @@ void ExportCloudsDialog::saveClouds( items.push_back(iter->c_str()); } extensions += ")..."; +#elif defined(RTABMAP_LIBLAS) + QString extensions = tr("Save clouds to (*.ply *.pcd *.las *.laz)..."); #else QString extensions = tr("Save clouds to (*.ply *.pcd)..."); #endif @@ -4343,24 +4367,42 @@ void ExportCloudsDialog::saveClouds( success = pcl::io::savePLYFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0; } } -#ifdef RTABMAP_PDAL +#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS) else if(!suffix.isEmpty()) { if(cloudIWithNormals.get()) { +#ifdef RTABMAP_PDAL success = savePDALFile(pathFile.toStdString(), *cloudIWithNormals) == 0; +#else + UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation."); + success = false; +#endif } else if(cloudIWithoutNormals.get()) { +#ifdef RTABMAP_PDAL success = savePDALFile(pathFile.toStdString(), *cloudIWithoutNormals) == 0; +#else + success = saveLASFile(pathFile.toStdString(), *cloudIWithoutNormals) == 0; +#endif } else if(cloudRGBWithoutNormals.get()) { +#ifdef RTABMAP_PDAL success = savePDALFile(pathFile.toStdString(), *cloudRGBWithoutNormals) == 0; +#else + success = saveLASFile(pathFile.toStdString(), *cloudRGBWithoutNormals) == 0; +#endif } else { +#ifdef RTABMAP_PDAL success = savePDALFile(pathFile.toStdString(), *transformedCloud) == 0; +#else + UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation."); + success = false; +#endif } } #endif diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 1fe54f3104..d78e8c7bf0 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; } 0 - 0 + -43 596 - 1142 + 1165 @@ -185,61 +185,61 @@ p, li { white-space: pre-wrap; } - - + + - OpenCV version : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With ORB Octree : + BSD true - - + + - Apache v2 + GPLv3 true - - + + - With K4W2 : + MIT true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With Open3D : true - - + + GPLv3 @@ -248,61 +248,58 @@ p, li { white-space: pre-wrap; } - - + + - PSF + BSD true - - + + - With TORO : + PCL version : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With Python3 : true - - + + - GPLv3 + With FastCV : true - - + + - Apache v2 and/or GPLv2 + MPL2 true - - + + @@ -314,7 +311,17 @@ p, li { white-space: pre-wrap; } - + + + + MIT + + + true + + + + Apache-2 @@ -324,41 +331,38 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + - LGPL + With SuperPoint Torch : true - - + + - With g2o : + Apache v2 and/or GPLv2 true - - + + @@ -370,8 +374,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -383,18 +387,18 @@ p, li { white-space: pre-wrap; } - - + + - Apache v2 + With stereo FlyCapture2 : true - - + + @@ -406,74 +410,71 @@ p, li { white-space: pre-wrap; } - - + + - With OpenNI2 : + With Zed SDK : true - - + + - GPLv3 + Apache v2 true - - + + - With stereo FlyCapture2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With RealSense : true - - + + - With Zed Open Capture : + With OpenChisel : true - - + + - <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - - Qt::RichText + With TORO : true - - + + @@ -485,8 +486,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -498,38 +499,38 @@ p, li { white-space: pre-wrap; } - - + + - Apache v2 + PSF true - - + + - Creative Commons [Attribution-NonCommercial-ShareAlike] + With DepthAI : true - - + + - GPLv3 + With AliceVision : true - - + + @@ -541,28 +542,31 @@ p, li { white-space: pre-wrap; } - - + + - With FastCV : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + With DVO : true - - + + @@ -574,71 +578,84 @@ p, li { white-space: pre-wrap; } - - + + - With Zed SDK : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + LGPL true - - + + - With loam_velodyne : + With libpointmatcher : true - - + + - With Python3 : + With MSCKF : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With CPU-TSDF : true - - + + - With CCCoreLib : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + + + With MYNTEYE S : + + + true + + + + + @@ -650,38 +667,38 @@ p, li { white-space: pre-wrap; } - - + + - With Ceres : + MIT true - - + + - With RealSense : + MIT true - - + + - Apache-2 + With g2o : true - - + + BSD @@ -690,21 +707,18 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + @@ -716,8 +730,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -729,68 +743,71 @@ p, li { white-space: pre-wrap; } - - + + - With GridMap : + GPLv2 true - - + + - BSD + With OpenNI2 : true - - + + - BSD + Apache-2 true - - + + - BSD + Qt version : true - - + + - With Octomap : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Penn Software License + With stereo dc1394 : true - - + + @@ -802,8 +819,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -815,41 +832,38 @@ p, li { white-space: pre-wrap; } - - + + - With AliceVision : + BSD true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With ORB SLAM 2 : true - - + + - MPL2 + With Ceres : true - - + + @@ -861,91 +875,88 @@ p, li { white-space: pre-wrap; } - - + + - With OpenChisel : + Apache v2 true - - + + - With cvsba : + With OKVIS : true - - + + - GPLv2 + With XVisio SDK : true - - + + - With Open3D : + VTK version : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Creative Commons [Attribution-NonCommercial-ShareAlike] true - - + + - GPLv3 + With K4A : true - - + + - MIT + BSD true - - + + - With CPU-TSDF : + BSD true - - + + @@ -957,31 +968,31 @@ p, li { white-space: pre-wrap; } - - + + - With OpenNI : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + @@ -993,151 +1004,169 @@ p, li { white-space: pre-wrap; } - - + + - GPLv2 + With GridMap : true - - + + - With Freenect2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + - Qt version : + With Viso2 : true - - + + - With RealSense2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Apache v2 and/or GPLv2 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With K4A : + With GTSAM : true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With libpointmatcher : + MIT true - - + + - With ORB SLAM 2 : + With CCCoreLib : true - - + + - BSD + Apache v2 and/or GPLv2 true - - + + @@ -1149,8 +1178,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1162,51 +1191,68 @@ p, li { white-space: pre-wrap; } - - + + - + With FOVIS : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + BSD true - - + + - With OKVIS : + BSD true - - + + - MIT + With PDAL : true - - + + - With DVO : + With Zed Open Capture : true - - + + + + With ORB Octree : + + + true + + + + + @@ -1218,81 +1264,94 @@ p, li { white-space: pre-wrap; } - - + + - Open Source or Commercial + <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> + + + Qt::RichText true - - + + - With MYNTEYE S : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - VTK version : + BSD true - - + + + + GPLv3 + + + true + + + + + - GPLv2 + With CudaSift : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv2 true - - + + - With DepthAI : + BSD true - - + + - With OpenVINS : + <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> true - - + + @@ -1304,8 +1363,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1330,18 +1389,8 @@ p, li { white-space: pre-wrap; } - - - - With FOVIS : - - - true - - - - - + + @@ -1353,8 +1402,8 @@ p, li { white-space: pre-wrap; } - - + + BSD @@ -1363,124 +1412,117 @@ p, li { white-space: pre-wrap; } - - + + - PCL version : + With cvsba : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Open Source or Commercial true - - + + - With Viso2 : + Apache v2 true - - + + - <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + With Freenect : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With OpenVINS : true - - + + - BSD + With OpenNI : true - - + + - BSD - - - true + - - - - - - MIT + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - MIT + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With GTSAM : + With Octomap : true - - + + - With Freenect : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -1492,8 +1534,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1505,31 +1547,28 @@ p, li { white-space: pre-wrap; } - - + + - With MRPT : + GPLv2 true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + @@ -1541,74 +1580,71 @@ p, li { white-space: pre-wrap; } - - + + - With XVisio SDK : + With Freenect2 : true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With CudaSift : + With VINS-Fusion : true - - + + - With stereo dc1394 : + With MRPT : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With loam_velodyne : true - - + + @@ -1620,38 +1656,38 @@ p, li { white-space: pre-wrap; } - - + + - With VINS-Fusion : + With RealSense2 : true - - + + - MIT + OpenCV version : true - - + + - With MSCKF : + With K4W2 : true - - + + @@ -1663,31 +1699,28 @@ p, li { white-space: pre-wrap; } - - + + - With SuperPoint Torch : + Penn Software License true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + @@ -1699,8 +1732,8 @@ p, li { white-space: pre-wrap; } - - + + BSD @@ -1709,18 +1742,18 @@ p, li { white-space: pre-wrap; } - - + + - With PDAL : + With libLAS : true - - + + diff --git a/package.xml b/package.xml index 3194da814b..1bc5d81fdf 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.21.6 + 0.21.7 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp index 2e29a48179..5375cf3d96 100644 --- a/tools/Export/main.cpp +++ b/tools/Export/main.cpp @@ -256,6 +256,8 @@ int main(int argc, char * argv[]) { #ifdef RTABMAP_PDAL las = true; +#elif defined(RTABMAP_LIBLAS) + printf("\"--las\" option cannot be used with libLAS because the cloud has normals, build RTAB-Map with PDAL support to export in las with normals. Will export in PLY...\n"); #else printf("\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n"); #endif