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

New feature: Depth confidence (#1520)

Browse Source 
* New feature: Depth confidence

* iOS app updated to save depth confidence, added util2d::depthBleedingFiltering function

* Updated tools to show/extract depth confidence

* Android: moved smoothing in post-processing, fixed confidence registration, added depth bleeding error option.

* Fixed warning

* removed debug log

* Added new feature types, fixed rendering when exporting texture >4096 (#1469), added depth bleeding filter option to iOS

* fixed some warnings, android: added bleeding error option

* CI: try updating ros2 key

* added sudo

* antoher test

* bump ios app version
This commit is contained in:
matlabbe
2025-06-01 14:14:29 -07:00
committed by GitHub
parent 90d195237f
commit 6d4e8a4173
51 changed files with 2368 additions and 1006 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
app/android/jni/CameraARCore.cpp
View File

@@ -37,8 +37,8 @@ namespace rtabmap {
//////////////////////////////
// CameraARCore
//////////////////////////////
CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing, float upstreamRelocalizationAccThr):
CameraMobile(smoothing, upstreamRelocalizationAccThr),
CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, float upstreamRelocalizationAccThr):
CameraMobile(upstreamRelocalizationAccThr),
env_(env),
context_(context),
activity_(activity),

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

@@ -50,7 +50,7 @@ namespace rtabmap {
class CameraARCore : public CameraMobile {
public:
CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, float upstreamRelocalizationAccThr = 0.0f);
virtual ~CameraARCore();
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

4
app/android/jni/CameraAREngine.cpp
View File

@@ -40,8 +40,8 @@ namespace rtabmap {
//////////////////////////////
// CameraAREngine
//////////////////////////////
CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing, float upstreamRelocalizationAccThr):
CameraMobile(smoothing, upstreamRelocalizationAccThr),
CameraAREngine::CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr):
CameraMobile(upstreamRelocalizationAccThr),
env_(env),
context_(context),
activity_(activity),

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

@@ -46,7 +46,7 @@ namespace rtabmap {
class CameraAREngine : public CameraMobile {
public:
CameraAREngine(void* env, void* context, void* activity, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr = 0.0f);
virtual ~CameraAREngine();
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

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

@@ -41,9 +41,6 @@ namespace rtabmap {
//////////////////////////////
// CameraMobile
//////////////////////////////
const float CameraMobile::bilateralFilteringSigmaS = 2.0f;
const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
const rtabmap::Transform CameraMobile::opticalRotation = Transform(
0.0f, 0.0f, 1.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f,
@@ -53,13 +50,12 @@ 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, float upstreamRelocalizationAccThr) :
CameraMobile::CameraMobile(float upstreamRelocalizationAccThr) :
Camera(10),
deviceTColorCamera_(Transform::getIdentity()),
textureId_(0),
uvs_initialized_(false),
stampEpochOffset_(0.0),
smoothing_(smoothing),
colorCameraToDisplayRotation_(ROTATION_0),
originUpdate_(true),
upstreamRelocalizationAccThr_(upstreamRelocalizationAccThr),
@@ -394,124 +390,135 @@ SensorData CameraMobile::updateDataOnRender(Transform & pose)
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_);
}
else if(lastKnownGPS_.stamp()>0.0)
{
LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
}
if(lastEnvSensors_.size())
{
data_.setEnvSensors(lastEnvSensors_);
lastEnvSensors_.clear();
}
if(smoothing_ && !data_.depthRaw().empty())
{
//UTimer t;
data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
//LOGD("Bilateral filtering, time=%fs", t.ticks());
}
// Rotate image depending on the camera orientation
if(colorCameraToDisplayRotation_ == ROTATION_90)
{
UDEBUG("ROTATION_90");
cv::Mat rgb, depth;
cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
cv::flip(data_.imageRaw(),rgb,1);
cv::transpose(rgb,rgbt);
rgb = rgbt;
cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
cv::flip(data_.depthRaw(),depth,1);
cv::transpose(depth,deptht);
depth = deptht;
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = data_.keypoints()[i].pt.y;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
}
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_180)
{
UDEBUG("ROTATION_180");
cv::Mat rgb, depth;
cv::flip(data_.imageRaw(),rgb,1);
cv::flip(rgb,rgb,0);
cv::flip(data_.depthOrRightRaw(),depth,1);
cv::flip(depth,depth,0);
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageWidth(), model.imageHeight());
model = CameraModel(
model.fx(),
model.fy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.cy()>0?model.imageHeight()-model.cy():0,
model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
model.setImageSize(sizet);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
}
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_270)
{
UDEBUG("ROTATION_270");
cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
cv::transpose(data_.imageRaw(),rgb);
cv::flip(rgb,rgb,1);
cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
cv::transpose(data_.depthOrRightRaw(),depth);
cv::flip(depth,depth,1);
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy()>0?model.imageHeight()-model.cy():0,
model.cx(),
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
keypoints[i].pt.y = data_.keypoints()[i].pt.x;
}
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
{
// 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_);
}
else if(lastKnownGPS_.stamp()>0.0)
{
LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
}
if(lastEnvSensors_.size())
{
data_.setEnvSensors(lastEnvSensors_);
lastEnvSensors_.clear();
}
// Rotate image depending on the camera orientation
if(colorCameraToDisplayRotation_ == ROTATION_90)
{
UDEBUG("ROTATION_90");
cv::Mat rgb, depth, confidence;
cv::Mat rgbt;
cv::flip(data_.imageRaw(),rgb,1);
cv::transpose(rgb,rgbt);
rgb = rgbt;
cv::Mat deptht;
cv::flip(data_.depthRaw(),depth,1);
cv::transpose(depth,deptht);
depth = deptht;
if(!data_.depthConfidenceRaw().empty()) {
cv::Mat conft;
cv::flip(data_.depthConfidenceRaw(),confidence,1);
cv::transpose(confidence,conft);
confidence = conft;
}
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data_.setRGBDImage(rgb, depth, confidence, model);
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = data_.keypoints()[i].pt.y;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
}
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_180)
{
UDEBUG("ROTATION_180");
cv::Mat rgb, depth, confidence;
cv::flip(data_.imageRaw(),rgb,1);
cv::flip(rgb,rgb,0);
cv::flip(data_.depthOrRightRaw(),depth,1);
cv::flip(depth,depth,0);
if(!data_.depthConfidenceRaw().empty()) {
cv::flip(data_.depthConfidenceRaw(),confidence,1);
cv::flip(confidence,confidence,0);
}
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageWidth(), model.imageHeight());
model = CameraModel(
model.fx(),
model.fy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.cy()>0?model.imageHeight()-model.cy():0,
model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
model.setImageSize(sizet);
data_.setRGBDImage(rgb, depth, confidence, model);
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
}
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_270)
{
UDEBUG("ROTATION_270");
cv::Mat rgb, depth, confidence;
cv::transpose(data_.imageRaw(),rgb);
cv::flip(rgb,rgb,1);
cv::transpose(data_.depthOrRightRaw(),depth);
cv::flip(depth,depth,1);
if(!data_.depthConfidenceRaw().empty()) {
cv::transpose(data_.depthConfidenceRaw(),confidence);
cv::flip(confidence,confidence,1);
}
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy()>0?model.imageHeight()-model.cy():0,
model.cx(),
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data_.setRGBDImage(rgb, depth, confidence, model);
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
keypoints[i].pt.y = data_.keypoints()[i].pt.x;
}
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else
{
UDEBUG("ROTATION_0");
}
}
}

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

@@ -70,9 +70,6 @@ private:
class CameraMobile : public Camera, public UEventsSender {
public:
static const float bilateralFilteringSigmaS;
static const float bilateralFilteringSigmaR;
static const rtabmap::Transform opticalRotation;
static const rtabmap::Transform opticalRotationInv;
@@ -87,7 +84,7 @@ public:
int kptsSize = 3);
public:
CameraMobile(bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
CameraMobile(float upstreamRelocalizationAccThr = 0.0f);
virtual ~CameraMobile();
// abstract functions
@@ -110,7 +107,6 @@ public:
const CameraModel & getCameraModel() const {return model_;}
const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
void setSmoothing(bool enabled) {smoothing_ = enabled;}
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
void setGPS(const GPS & gps);
void addEnvSensor(int type, float value);
@@ -144,7 +140,6 @@ protected:
private:
bool firstFrame_;
double stampEpochOffset_;
bool smoothing_;
ScreenRotation colorCameraToDisplayRotation_;
GPS lastKnownGPS_;
EnvSensors lastEnvSensors_;

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

@@ -114,8 +114,7 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
//////////////////////////////
// CameraTango
//////////////////////////////
CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
CameraMobile(smoothing),
CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan) :
tango_config_(0),
colorCamera_(colorCamera),
decimation_(decimation),

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

@@ -45,7 +45,7 @@ namespace rtabmap {
class CameraTango : public CameraMobile {
public:
CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
CameraTango(bool colorCamera, int decimation, bool publishRawScan);
virtual ~CameraTango();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -84,6 +84,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
const int g_optMeshId = -100;
const float g_bilateralFilteringSigmaS = 2.0f;
const float g_bilateralFilteringSigmaR = 0.075f;
#ifdef __ANDROID__
static JavaVM *jvm;
static jobject RTABMapActivity = 0;
@@ -230,6 +233,7 @@ RTABMapApp::RTABMapApp() :
trajectoryMode_(false),
rawScanSaved_(false),
smoothing_(true),
depthBleedingError_(0.0f),
depthFromMotion_(false),
cameraColor_(true),
fullResolution_(false),
@@ -245,7 +249,11 @@ RTABMapApp::RTABMapApp() :
maxGainRadius_(0.02f),
renderingTextureDecimation_(4),
backgroundColor_(0.2f),
depthConfidence_(2),
#ifndef RTABMAP_ARCORE
depthConfidence_(100), // iOS
#else
depthConfidence_(0),
#endif
upstreamRelocalizationMaxAcc_(0.0f),
exportPointCloudFormat_("ply"),
dataRecorderMode_(false),
@@ -266,20 +274,20 @@ RTABMapApp::RTABMapApp() :
lastPoseEventTime_(0.0),
visualizingMesh_(false),
exportedMeshUpdated_(false),
optTextureMesh_(new pcl::TextureMesh),
optRefId_(0),
optRefPose_(0),
measuresUpdated_(false),
targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
quadSamplePolygons_(2),
metricSystem_(true),
metricSystem_(true),
measuringTextSize_(0.05f),
snapAxisThr_(0.95),
measuringMode_(0),
addMeasureClicked_(false),
teleportClicked_(false),
removeMeasureClicked_(false),
optTextureMesh_(new pcl::TextureMesh),
optRefId_(0),
optRefPose_(0),
targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
quadSamplePolygons_(2),
mapToOdom_(rtabmap::Transform::getIdentity())
{
@@ -461,7 +469,13 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
LOGI("Open: Found optimized mesh! Visualizing it.");
optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
optMesh_ = rtabmap::Mesh();
optTexture_ = textures;
if(textures.rows > 4096) // Limitation iOS, just for rendering on device
{
cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
}
else {
optTexture_ = textures;
}
if(!optTexture_.empty())
{
LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
@@ -569,8 +583,8 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
rtabmap::SensorData data = signatures.at(id).sensorData();
rawPoses_.insert(std::make_pair(id, signatures.at(id).getPose()));
cv::Mat tmpA, depth;
data.uncompressData(&tmpA, &depth);
cv::Mat tmpA, tmpB, tmpC;
data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
{
@@ -586,8 +600,30 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
if(smoothing_ || depthBleedingError_>0.0f)
{
cv::Mat depth = data.depthRaw();
if(depthBleedingError_ > 0.0f)
{
rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
}
if(smoothing_)
{
depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
}
data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
}
cloud = rtabmap::util3d::cloudRGBFromSensorData(
data,
meshDecimation,
maxCloudDepth_,
minCloudDepth_,
indices.get(),
rtabmap::ParametersMap(),
std::vector<float>(),
depthConfidence_);
}
else
{
@@ -946,7 +982,7 @@ bool RTABMapApp::startCamera()
if(cameraDriver_ == 0) // Tango
{
#ifdef RTABMAP_TANGO
camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_);
camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_);
if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
UERROR("TangoHandler::ConnectTango, TangoService_setBinder error");
@@ -961,7 +997,7 @@ bool RTABMapApp::startCamera()
else if(cameraDriver_ == 1)
{
#ifdef RTABMAP_ARCORE
camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_, upstreamRelocalizationMaxAcc_);
camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, upstreamRelocalizationMaxAcc_);
#else
UERROR("RTAB-Map is not built with ARCore support!");
#endif
@@ -969,14 +1005,14 @@ bool RTABMapApp::startCamera()
else if(cameraDriver_ == 2)
{
#ifdef RTABMAP_ARENGINE
camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_, upstreamRelocalizationMaxAcc_);
camera_ = new rtabmap::CameraAREngine(env, context, activity, upstreamRelocalizationMaxAcc_);
#else
UERROR("RTAB-Map is not built with AREngine support!");
#endif
}
else if(cameraDriver_ == 3)
{
camera_ = new rtabmap::CameraMobile(smoothing_, upstreamRelocalizationMaxAcc_);
camera_ = new rtabmap::CameraMobile(upstreamRelocalizationMaxAcc_);
}
if(camera_ == 0)
@@ -1358,8 +1394,8 @@ int RTABMapApp::Render()
#ifdef DEBUG_RENDERING_PERFORMANCE
LOGD("Camera updateOnRender %fs", time.ticks());
#endif
// We detect if we are in measuring mode if rtabmap is not running
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
// We check if we are in measuring mode: not visualizing mesh or rtabmap is not started (localization mode)
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && (!visualizingMesh_ || !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning())))
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -1540,7 +1576,6 @@ int RTABMapApp::Render()
int textId = 0;
int quadId = 0;
int circleId = 0;
float sphereRadius = 0.02f;
float quadSize=0.05f;
float quadAlpha = 0.3f;
@@ -2014,8 +2049,8 @@ int RTABMapApp::Render()
{
rtabmap::SensorData data = bufferedSensorData.at(id);
cv::Mat tmpA, depth;
data.uncompressData(&tmpA, &depth);
cv::Mat tmpA, tmpB, tmpC;
data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
{
rtabmap::LaserScan scan;
@@ -2033,7 +2068,20 @@ int RTABMapApp::Render()
if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
if(smoothing_ || depthBleedingError_>0.0f)
{
cv::Mat depth = data.depthRaw();
if(depthBleedingError_ > 0.0f)
{
rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
}
if(smoothing_)
{
depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
}
data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
}
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
}
else
{
@@ -2249,7 +2297,7 @@ int RTABMapApp::Render()
if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
}
else
{
@@ -3019,6 +3067,11 @@ void RTABMapApp::setSmoothing(bool enabled)
}
}
void RTABMapApp::setDepthBleedingError(float value)
{
depthBleedingError_ = value;
}
void RTABMapApp::setDepthFromMotion(bool enabled)
{
if(depthFromMotion_ != enabled)
@@ -3111,10 +3164,10 @@ void RTABMapApp::setBackgroundColor(float gray)
void RTABMapApp::setDepthConfidence(int value)
{
depthConfidence_ = value;
if(depthConfidence_>2)
depthConfidence_ = value*50; // [0,2] -> [0,100]
if(depthConfidence_>100)
{
depthConfidence_ = 2;
depthConfidence_ = 100;
}
}
@@ -3414,6 +3467,19 @@ bool RTABMapApp::exportMesh(
if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
if(smoothing_ || depthBleedingError_>0.0f)
{
cv::Mat depth = data.depthRaw();
if(depthBleedingError_ > 0.0f)
{
rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
}
if(smoothing_)
{
depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
}
data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
}
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
model = data.cameraModels()[0];
depth = data.depthRaw();
@@ -3681,7 +3747,20 @@ bool RTABMapApp::exportMesh(
if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_);
if(smoothing_ || depthBleedingError_>0.0f)
{
cv::Mat depth = data.depthRaw();
if(depthBleedingError_ > 0.0f)
{
rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
}
if(smoothing_)
{
depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
}
data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
}
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, 0, rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
}
}
@@ -3939,7 +4018,20 @@ bool RTABMapApp::exportMesh(
if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
// full resolution
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
if(smoothing_ || depthBleedingError_>0.0f)
{
cv::Mat depth = data.depthRaw();
if(depthBleedingError_ > 0.0f)
{
rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
}
if(smoothing_)
{
depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
}
data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
}
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
}
else if(!data.laserScanRaw().empty())
{
@@ -3969,7 +4061,20 @@ bool RTABMapApp::exportMesh(
if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
if(smoothing_ || depthBleedingError_>0.0f)
{
cv::Mat depth = data.depthRaw();
if(depthBleedingError_ > 0.0f)
{
rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
}
if(smoothing_)
{
depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
}
data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
}
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
}
else if(!data.laserScanRaw().empty())
{
@@ -4121,7 +4226,13 @@ bool RTABMapApp::postExportation(bool visualize)
LOGI("postExportation: Found optimized mesh! Visualizing it.");
optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
optMesh_ = rtabmap::Mesh();
optTexture_ = textures;
if(textures.rows > 4096) // Limitation iOS, just for rendering on device
{
cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
}
else {
optTexture_ = textures;
}
boost::mutex::scoped_lock lock(renderingMutex_);
visualizingMesh_ = true;
@@ -4528,6 +4639,7 @@ void RTABMapApp::postOdometryEvent(
cv::Mat outputDepth;
cv::Mat outputDepthConfidence;
if(depth && depthHeight>0 && depthWidth>0)
{
#ifndef DISABLE_LOG
@@ -4537,32 +4649,21 @@ void RTABMapApp::postOdometryEvent(
{
// IOS
outputDepth = cv::Mat(depthHeight, depthWidth, CV_32FC1, (void*)depth).clone();
if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488 && depthConfidence_>0)
if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488)
{
const unsigned char * confPtr = (const unsigned char *)conf;
float * depthPtr = outputDepth.ptr<float>();
int i=0;
for (int y = 0; y < outputDepth.rows; ++y)
{
for (int x = 0; x < outputDepth.cols; ++x)
{
// https://developer.apple.com/documentation/arkit/arconfidencelevel
// 0 = low
// 1 = medium
// 2 = high
if(confPtr[y*outputDepth.cols + x] < depthConfidence_)
{
depthPtr[y*outputDepth.cols + x] = 0.0f;
++i;
}
}
}
// https://developer.apple.com/documentation/arkit/arconfidencelevel
// 0 = low
// 1 = medium
// 2 = high
// Re-scale confidence from [0,2] to [0,100]
cv::Mat(depthHeight, depthWidth, CV_8UC1, (void*)conf).convertTo(outputDepthConfidence, CV_8UC1, 50, 0);
}
}
else if(depthLen == 2*depthWidth*depthHeight)
{
// ANDROID
outputDepth = cv::Mat(depthHeight, depthWidth, CV_16UC1);
outputDepthConfidence = cv::Mat(depthHeight, depthWidth, CV_8UC1);
uint16_t *dataShort = (uint16_t *)depth;
for (int y = 0; y < outputDepth.rows; ++y)
{
@@ -4571,6 +4672,13 @@ void RTABMapApp::postOdometryEvent(
uint16_t depthSample = dataShort[y*outputDepth.cols + x];
uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
outputDepth.at<uint16_t>(y,x) = depthRange;
// https://developer.android.com/reference/android/graphics/ImageFormat#DEPTH16
// The confidence value is an estimate of correctness for this sample. It
// is encoded in the 3 most significant bits of the sample, with a value of
// 0 representing 100% confidence, a value of 1 representing 0% confidence, a
// value of 2 representing 1/7, a value of 3 representing 2/7, and so on.
uint8_t depthConfidence = uint8_t((depthSample >> 13) & 0x7);
outputDepthConfidence.at<uint8_t>(y,x) = depthConfidence == 0 ? 100 : (depthConfidence - 1)*100 / 7;
}
}
}
@@ -4630,7 +4738,9 @@ void RTABMapApp::postOdometryEvent(
depth_fx, 0, depth_cx,
0, depth_fy, depth_cy,
0, 0, 1);
outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
cv::Mat regConfidence;
outputDepth = rtabmap::util2d::registerDepth(outputDepth, outputDepthConfidence, depthK, outputDepth.size(), colorK, rgbToDepth, regConfidence);
outputDepthConfidence = regConfidence;
#ifndef DISABLE_LOG
UDEBUG("Depth registration time: %fs", time.elapsed());
#endif
@@ -4670,8 +4780,8 @@ void RTABMapApp::postOdometryEvent(
depthModel.setLocalTransform(pose*model.localTransform());
camera_->setOcclusionImage(outputDepth, depthModel);
}
rtabmap::SensorData data(scan, outputRGB, outputDepth, model, 0, stamp);
rtabmap::SensorData data(scan, outputRGB, outputDepth, outputDepthConfidence, model, 0, stamp);
data.setFeatures(kpts, kpts3, cv::Mat());
glm::mat4 projectionMatrix(0);
projectionMatrix[0][0] = p00;

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

@@ -139,6 +139,7 @@ class RTABMapApp : public UEventsHandler {
void setCameraColor(bool enabled);
void setFullResolution(bool enabled);
void setSmoothing(bool enabled);
void setDepthBleedingError(float value);
void setDepthFromMotion(bool enabled);
void setAppendMode(bool enabled);
void setUpstreamRelocalizationAccThr(float value);
@@ -235,6 +236,7 @@ class RTABMapApp : public UEventsHandler {
bool trajectoryMode_;
bool rawScanSaved_;
bool smoothing_;
float depthBleedingError_;
bool depthFromMotion_;
bool cameraColor_;
bool fullResolution_;
@@ -250,7 +252,7 @@ class RTABMapApp : public UEventsHandler {
float maxGainRadius_;
int renderingTextureDecimation_;
float backgroundColor_;
int depthConfidence_;
unsigned char depthConfidence_;
float upstreamRelocalizationMaxAcc_;
std::string exportPointCloudFormat_;

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

@@ -512,6 +512,19 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
}
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setDepthBleedingError(
JNIEnv*, jclass, jlong native_application, float value)
{
if(native_application)
{
return native(native_application)->setDepthBleedingError(value);
}
else
{
UERROR("native_application is null!");
}
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
JNIEnv*, jclass, jlong native_application, bool enabled)
{