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

iOS various updates (#1340)

Browse Source 
* Added Data Recording Mode. Added option to filter ARKit localization jumps.

* Implemented max acc relocalization filtering (working on iOS)

* Fixed android build, added re-localization max acceleration parameter

* ARCore Java: Fixed pose of depth not available at stamp requested

* Android log fix

* Added libLAS support

* ios: added LAS support

* updated dep install script to skip libraries already installed

* CameraMobile: Fixed origin not updated if updateOnRender() is used

* Default max opt error increased to 2x to reduce number of loop closures rejected. OptimizerGTSAM: updated gravity noise model to use same sigma for both parameters.

* Updated license

* bump 0.21.7

* updated license date
This commit is contained in:
matlabbe
2024-10-06 17:16:22 -07:00
committed by GitHub
parent 409ef73e56
commit 595f200a89
41 changed files with 2420 additions and 1664 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
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):
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<cv::KeyPoint> * kpts,
std::vector<cv::Point3f> * kpts3D)
{
if(pointCloudData && points>0)
{
cv::Mat scanData(1, points, CV_32FC4);
float * ptr = scanData.ptr<float>();
for(unsigned int i=0;i<points; ++i)
{
cv::Point3f pt(pointCloudData[i*4], pointCloudData[i*4 + 1], pointCloudData[i*4 + 2]);
pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
ptr[i*4] = pt.x;
ptr[i*4 + 1] = pt.y;
ptr[i*4 + 2] = pt.z;
//get color from rgb image
cv::Point3f org= pt;
pt = util3d::transformPoint(pt, opticalRotationInv);
int u,v;
model.reproject(pt.x, pt.y, pt.z, u, v);
unsigned char r=255,g=255,b=255;
if(model.inFrame(u, v))
{
b=rgb.at<cv::Vec3b>(v,u).val[0];
g=rgb.at<cv::Vec3b>(v,u).val[1];
r=rgb.at<cv::Vec3b>(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;
}
}
}
}

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

@@ -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<cv::KeyPoint> * kpts = 0,
std::vector<cv::Point3f> * 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);

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

@@ -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;
}
}
}
}

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);
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);

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

@@ -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<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(epochStamp);
std::map<double, Transform>::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<float> 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<double>(3,3) *= 0.01;
info->odomCovariance.at<double>(4,4) *= 0.01;
info->odomCovariance.at<double>(5,5) *= 0.01;
// roll/pitch should be fairly accurate with VIO input
info->odomCovariance.at<double>(3,3) *= 0.01; // roll
info->odomCovariance.at<double>(4,4) *= 0.01; // pitch
if(!dataGoodTracking)
{
UERROR("not good tracking!");
// add slightly more error on translation
// 0.001
info->odomCovariance.at<double>(0,0) *= 10; // x
info->odomCovariance.at<double>(1,1) *= 10; // y
info->odomCovariance.at<double>(2,2) *= 10; // z
info->odomCovariance.at<double>(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<points; ++i)
for(unsigned int i=0;i<pointCloudData.cols; ++i)
{
cv::Point3f pt(inPtr[i*ic], inPtr[i*ic + 1], inPtr[i*ic + 2]);
pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);

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

@@ -79,7 +79,6 @@ public:
public:
static LaserScan scanFromPointCloudData(
const cv::Mat & pointCloudData,
int points,
const Transform & pose,
const CameraModel & model,
const cv::Mat & rgb,
@@ -88,7 +87,7 @@ public:
int kptsSize = 3);
public:
CameraMobile(bool smoothing = false);
CameraMobile(bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
virtual ~CameraMobile();
// abstract functions
@@ -96,16 +95,17 @@ public:
virtual void close(); // inherited classes should call its parent at the end of their close().
virtual std::string getSerial() const {return "CameraMobile";}
// original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch), viewMatrix in opengl frame (without origin offset)
void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
void updateOnRender();
const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
void resetOrigin();
virtual bool isCalibrated() const;
virtual bool odomProvided() const { return true; }
virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time
void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
// original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
void poseReceived(const Transform & pose, double deviceStamp);
double getStampEpochOffset() const {return stampEpochOffset_;}
const CameraModel & getCameraModel() const {return model_;}
@@ -150,11 +150,17 @@ private:
EnvSensors lastEnvSensors_;
Transform originOffset_;
bool originUpdate_;
float upstreamRelocalizationAccThr_;
rtabmap::Transform previousAnchorPose_;
std::vector<float> previousAnchorLinearVelocity_;
double previousAnchorStamp_;
std::map<double, std::pair<rtabmap::Transform, rtabmap::Transform> > relocalizationDebugBuffer_;
USemaphore dataReady_;
UMutex dataMutex_;
SensorData data_;
Transform dataPose_;
bool dataGoodTracking_;
UMutex poseMutex_;
std::map<double, Transform> poseBuffer_; // <stamp, Pose>

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

@@ -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<float>(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);

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

@@ -73,9 +73,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/surface/poisson.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
#ifdef RTABMAP_PDAL
#include <rtabmap/core/PDALWriter.h>
#elif defined(RTABMAP_LIBLAS)
#include <rtabmap/core/LASWriter.h>
#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<int, rtabmap::Transform>::iterator iter=posesWithMarkers.begin(); iter!=posesWithMarkers.end(); ++iter)
{
if(!graphOptimization_)
if(!graphOptimization_ && !dataRecorderMode_)
{
std::map<int, rtabmap::Transform>::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<int, rtabmap::Link> 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<pcl::PointXYZRGB> 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 depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
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) 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<void()> actualCallback = [&](){
swiftCameraInfoEventCallback(swiftClassPtr_, tangoEvent->type(), tangoEvent->key().c_str(), tangoEvent->value().c_str());
};
actualCallback();
success = true;
}
#endif
if(!success)
{

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

@@ -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
};

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

@@ -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,