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

Refactored Tango and ARCore texture management. Tango: added support with First-person AR mode. ARCoreJava: Added localization filtering speed option (avoid ARCore jumps), also manage when ARCore is lost (new session is created, relocalization needed to continue mapping).

Browse Source
This commit is contained in:
matlabbe
2021-06-15 20:45:45 -04:00
parent 696216b35b
commit a9d4fc9291
14 changed files with 619 additions and 418 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -43,9 +43,6 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthF
context_(context),
activity_(activity),
arInstallRequested_(false),
textureId_(9999),
uvs_initialized_(false),
updateOcclusionImage_(false),
depthFromMotion_(depthFromMotion)
{
}
@@ -53,12 +50,6 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthF
CameraARCore::~CameraARCore() {
// Disconnect ARCore service
close();
if(textureId_ != 9999)
{
glDeleteTextures(1, &textureId_);
textureId_ = 9999;
}
}
@@ -176,7 +167,7 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
}
ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
ArFrame_create(arSession_, &arFrame_);
@@ -279,7 +270,6 @@ void CameraARCore::close()
arPose_ = nullptr;
CameraMobile::close();
occlusionImage_ = cv::Mat();
}
LaserScan CameraARCore::scanFromPointCloudData(
@@ -355,7 +345,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
return data;
}
if(textureId_ == 9999)
if(textureId_ == 0)
{
glGenTextures(1, &textureId_);
glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
@@ -364,7 +354,8 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
}
ArSession_setCameraTextureName(arSession_, textureId_);
if(textureId_!=0)
ArSession_setCameraTextureName(arSession_, textureId_);
// Update session to get current frame and render camera background.
if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
@@ -454,7 +445,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
if(status == AR_SUCCESS)
{
if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
if(is_depth_supported)
{
LOGD("Acquire depth image!");
ArImage * depthImage = nullptr;
@@ -481,11 +472,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
occlusionImage_ = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
cv::Mat occlusionImage = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
float scaleX = (float)depth_width / (float)width;
float scaleY = (float)depth_height / (float)height;
occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
this->setOcclusionImage(occlusionImage, occlusionModel);
}
ArImage_release(depthImage);
}
@@ -557,7 +549,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
LOGI("pointCloud empty");
}
data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
data.setFeatures(kpts, kpts3, cv::Mat());
}
}
@@ -592,16 +584,15 @@ void CameraARCore::capturePoseOnly()
return;
}
if(textureId_ == 9999)
if(textureId_ != 0)
{
glGenTextures(1, &textureId_);
glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
ArSession_setCameraTextureName(arSession_, textureId_);
}
ArSession_setCameraTextureName(arSession_, textureId_);
// Update session to get current frame and render camera background.
if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
@@ -662,7 +653,7 @@ void CameraARCore::capturePoseOnly()
int32_t is_depth_supported = 0;
ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
if(is_depth_supported && updateOcclusionImage_)
if(is_depth_supported)
{
LOGD("Acquire depth image!");
ArImage * depthImage = nullptr;
@@ -689,7 +680,7 @@ void CameraARCore::capturePoseOnly()
LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
float fx,fy, cx, cy;
int32_t rgb_width, rgb_height;
@@ -700,7 +691,8 @@ void CameraARCore::capturePoseOnly()
float scaleX = (float)width / (float)rgb_width;
float scaleY = (float)height / (float)rgb_height;
occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
this->setOcclusionImage(occlusionImage, occlusionModel);
}
ArImage_release(depthImage);
}

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

@@ -67,9 +67,6 @@ public:
const float* uvsTransformed() const {return transformed_uvs_;}
void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
void updateOcclusionImage(bool enabled) {updateOcclusionImage_ = enabled;}
const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -97,17 +94,8 @@ private:
ArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
ArPose * arPose_ = nullptr;
bool arInstallRequested_;
GLuint textureId_;
UMutex arSessionMutex_;
float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
bool uvs_initialized_ = false;
glm::mat4 viewMatrix_;
glm::mat4 projectionMatrix_;
bool updateOcclusionImage_;
cv::Mat occlusionImage_;
CameraModel occlusionModel_;
bool depthFromMotion_;
};

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

@@ -56,7 +56,7 @@ CameraMobile::CameraMobile(bool smoothing) :
Camera(10),
deviceTColorCamera_(Transform::getIdentity()),
spinOncePreviousStamp_(0.0),
textureId_(9999),
textureId_(0),
uvs_initialized_(false),
previousStamp_(0.0),
stampEpochOffset_(0.0),
@@ -64,18 +64,11 @@ CameraMobile::CameraMobile(bool smoothing) :
colorCameraToDisplayRotation_(ROTATION_0),
originUpdate_(false)
{
glGenTextures(1, &textureId_);
}
CameraMobile::~CameraMobile() {
// Disconnect camera service
close();
if(textureId_ != 9999)
{
glDeleteTextures(1, &textureId_);
textureId_ = 9999;
}
}
bool CameraMobile::init(const std::string &, const std::string &)
@@ -94,10 +87,22 @@ void CameraMobile::close()
originUpdate_ = false;
pose_ = Transform();
data_ = SensorData();
if(textureId_ != 0)
{
glDeleteTextures(1, &textureId_);
textureId_ = 0;
}
}
void CameraMobile::resetOrigin()
{
previousPose_.setNull();
previousStamp_ = 0.0;
lastKnownGPS_ = GPS();
lastEnvSensors_.clear();
pose_ = Transform();
data_ = SensorData();
originUpdate_ = true;
}
@@ -150,9 +155,9 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
if(textureId_ == 9999)
if(textureId_ == 0)
{
glGenTextures(1, &textureId_);
glGenTextures(1, &textureId_);
}
if(texCoord)
@@ -381,7 +386,7 @@ void CameraMobile::mainLoop()
previousPose_ = pose;
previousStamp_ = data.stamp();
}
else if(!this->isKilled())
else if(!this->isKilled() && info.odomPose.isNull())
{
LOGW("Odometry lost");
this->post(new OdometryEvent());

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

@@ -116,6 +116,7 @@ public:
bool uvsInitialized() const {return uvs_initialized_;}
const float* uvsTransformed() const {return transformed_uvs_;}
void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
ScreenRotation getScreenRotation() const {return colorCameraToDisplayRotation_;}
void setOcclusionImage(const cv::Mat & image, const CameraModel & model) {occlusionModel_ = model; occlusionImage_ = image;}
const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }

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

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util2d.h"
#include <tango_client_api.h>
#include <tango_support_api.h>
#include "tango-gl/camera.h"
namespace rtabmap {
@@ -42,11 +43,22 @@ const int holeSize = 5;
const float maxDepthError = 0.10;
const int scanDownsampling = 1;
//android phone
//11 10 01 00 // portrait
//01 11 00 10 // left
//10 00 11 01 // right
//00 01 10 11 // down
const float kTextureCoords0[] = {1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
const float kTextureCoords90[] = {0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0};
const float kTextureCoords180[] = {0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0};
const float kTextureCoords270[] = {1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0};
// Callbacks
void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
{
CameraTango* app = static_cast<CameraTango*>(context);
if(app->isRunning() && point_cloud->num_points>0)
if(point_cloud->num_points>0)
{
app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
}
@@ -55,34 +67,32 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
{
CameraTango* app = static_cast<CameraTango*>(context);
if(app->isRunning())
{
cv::Mat tangoImage;
if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
{
tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
}
else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
{
tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
}
else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
{
tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
}
else if(color->format == 35)
{
tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
}
else
{
LOGE("Not supported color format : %d.", color->format);
}
if(!tangoImage.empty())
{
app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
}
cv::Mat tangoImage;
if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
{
tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
}
else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
{
tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
}
else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
{
tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
}
else if(color->format == 35)
{
tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
}
else
{
LOGE("Not supported color format : %d.", color->format);
}
if(!tangoImage.empty())
{
app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
}
}
@@ -110,7 +120,6 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan,
colorCamera_(colorCamera),
decimation_(decimation),
rawScanPublished_(publishRawScan),
cloudStamp_(0),
tangoColorType_(0),
tangoColorStamp_(0)
{
@@ -430,7 +439,7 @@ void CameraTango::close()
void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
{
if(this->isRunning() && !cloud.empty())
if(!cloud.empty())
{
//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
@@ -447,20 +456,248 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
// So, synchronize with the last RGB frame before the Depth is acquired
if(!tangoColor_.empty())
{
UTimer timer;
double dt = fabs(timestamp - tangoColorStamp_);
//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
if(dt >= 0.0 && dt < 0.5)
{
bool notify = cloud_.empty();
cloud_ = cloud.clone();
cloudStamp_ = timestamp;
bool notify = !data_.isValid();
cv::Mat tangoImage = tangoColor_;
cv::Mat rgb;
double cloudStamp = timestamp;
double rgbStamp = tangoColorStamp_;
int tangoColorType = tangoColorType_;
tangoColor_ = cv::Mat();
tangoColorStamp_ = 0.0;
tangoColorType_ = 0;
LOGD("tangoColorType=%d", tangoColorType);
if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
{
cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
}
else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
{
cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
}
else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
{
cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
}
else if(tangoColorType == 35)
{
cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
}
else
{
LOGE("Not supported color format : %d.", tangoColorType);
data_ = SensorData();
return;
}
//for(int i=0; i<rgb.cols; ++i)
//{
// UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
//}
CameraModel model = model_;
if(colorCamera_)
{
if(decimation_ > 1)
{
rgb = util2d::decimate(rgb, decimation_);
model = model.scaled(1.0/double(decimation_));
}
}
else
{
//UTimer t;
cv::Mat rgbRect;
cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
rgb = rgbRect;
//LOGD("Rectification time=%fs", t.ticks());
}
// Querying the depth image's frame transformation based on the depth image's
// timestamp.
cv::Mat depth;
// Calculate the relative pose from color camera frame at timestamp
// color_timestamp t1 and depth
// camera frame at depth_timestamp t0.
Transform colorToDepth;
TangoPoseData pose_color_image_t1_T_depth_image_t0;
if (TangoSupport_calculateRelativePose(
rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
&pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
{
colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
}
else
{
LOGE(
"SynchronizationApplication: Could not find a valid relative pose at "
"time for color and "
" depth cameras.");
}
if(colorToDepth.getNormSquared() > 100000)
{
LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
colorToDepth.setNull();
}
cv::Mat scan;
if(!colorToDepth.isNull())
{
// The Color Camera frame at timestamp t0 with respect to Depth
// Camera frame at timestamp t1.
//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
int pixelsSet = 0;
int depthSizeDec = colorCamera_?8:1;
depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
int oi=0;
int closePoints = 0;
float closeROI[4];
closeROI[0] = depth.cols/4;
closeROI[1] = 3*(depth.cols/4);
closeROI[2] = depth.rows/4;
closeROI[3] = 3*(depth.rows/4);
unsigned short minDepthValue=10000;
for(unsigned int i=0; i<cloud.total(); ++i)
{
const float * p = cloud.ptr<float>(0,i);
cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
{
scanData.at(oi++) = pt;
}
int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
// get the coordinate on image plane.
pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
unsigned short depth_value(pt.z * 1000.0f);
if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
depth_value < 600)
{
++closePoints;
if(depth_value < minDepthValue)
{
minDepthValue = depth_value;
}
}
bool pixelSet = false;
if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
depth_value)
{
unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
if(depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
pixelSet = true;
}
}
if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
depth_value)
{
unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
if(depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
pixelSet = true;
}
}
if(pixelSet)
{
pixelsSet += 1;
}
}
if(closePoints > 100)
{
this->post(new CameraInfoEvent(0, "TooClose", ""));
}
if(oi)
{
scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
}
//LOGD("pixels depth set= %d", pixelsSet);
}
else
{
LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
}
if(!rgb.empty() && !depth.empty())
{
depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
Transform odom = getPoseAtTimestamp(rgbStamp);
//LOGD("Local = %s", model.localTransform().prettyPrint().c_str());
//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())
{
rtabmap::CameraModel depthModel = model.scaled(float(depth.cols) / float(model.imageWidth()));
depthModel.setLocalTransform(odom*model.localTransform());
this->setOcclusionImage(depth, depthModel);
}
//LOGD("rtabmap = %s", odom.prettyPrint().c_str());
//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
Transform scanLocalTransform = model.localTransform();
if(rawScanPublished_)
{
data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
else
{
data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
data_.setGroundTruth(odom);
}
else
{
LOGE("Could not get depth and rgb images!?!");
data_ = SensorData();
return;
}
if(notify)
{
//LOGD("Cloud: Release semaphore");
dataReady_.release();
}
LOGD("process cloud received %fs", timer.ticks());
}
}
}
@@ -468,7 +705,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
{
if(this->isRunning() && !tangoImage.empty())
if(!tangoImage.empty())
{
//LOGD("RGB received! %fs", timestamp);
@@ -532,7 +769,6 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
}
else
{
pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
}
@@ -543,254 +779,112 @@ SensorData CameraTango::captureImage(CameraInfo * info)
{
//LOGI("Capturing image...");
SensorData data;
if(!dataReady_.acquire(1, 2000))
if(textureId_ == 0)
{
if(this->isRunning())
{
LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
this->post(new CameraInfoEvent(0, "CameraTango", "No frames received since 2 seconds."));
boost::mutex::scoped_lock lock(dataMutex_);
if(!cloud_.empty() && !tangoColor_.empty())
{
UERROR("cloud and image were set!?");
}
}
cloud_ = cv::Mat();
cloudStamp_ = 0.0;
tangoColor_ = cv::Mat();
tangoColorStamp_ = 0.0;
tangoColorType_ = 0;
glGenTextures(1, &textureId_);
glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
}
else
// Update Texture (optional, just for first-view rendering)
if(colorCamera_ && textureId_)
{
cv::Mat cloud;
cv::Mat tangoImage;
cv::Mat rgb;
double cloudStamp = 0.0;
double rgbStamp = 0.0;
int tangoColorType = 0;
double video_overlay_timestamp;
TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);
if (status == TANGO_SUCCESS)
{
boost::mutex::scoped_lock lock(dataMutex_);
cloud = cloud_;
cloudStamp = cloudStamp_;
cloud_ = cv::Mat();
cloudStamp_ = 0.0;
tangoImage = tangoColor_;
rgbStamp = tangoColorStamp_;
tangoColorType = tangoColorType_;
tangoColor_ = cv::Mat();
tangoColorStamp_ = 0.0;
tangoColorType_ = 0;
}
LOGD("tangoColorType=%d", tangoColorType);
if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
{
cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
}
else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
{
cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
}
else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
{
cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
}
else if(tangoColorType == 35)
{
cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
}
else
{
LOGE("Not supported color format : %d.", tangoColorType);
return data;
}
//for(int i=0; i<rgb.cols; ++i)
//{
// UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
//}
CameraModel model = model_;
if(colorCamera_)
{
if(decimation_ > 1)
if(info)
{
rgb = util2d::decimate(rgb, decimation_);
model = model.scaled(1.0/double(decimation_));
info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
}
}
else
{
//UTimer t;
cv::Mat rgbRect;
cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
rgb = rgbRect;
//LOGD("Rectification time=%fs", t.ticks());
}
// Querying the depth image's frame transformation based on the depth image's
// timestamp.
cv::Mat depth;
int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
if (rotation > 3) {
rotation -= 4;
}
// Calculate the relative pose from color camera frame at timestamp
// color_timestamp t1 and depth
// camera frame at depth_timestamp t0.
Transform colorToDepth;
TangoPoseData pose_color_image_t1_T_depth_image_t0;
if (TangoSupport_calculateRelativePose(
rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
&pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
{
colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
}
else
{
LOGE(
"SynchronizationApplication: Could not find a valid relative pose at "
"time for color and "
" depth cameras.");
}
if(colorToDepth.getNormSquared() > 100000)
{
LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
colorToDepth.setNull();
}
cv::Mat scan;
if(!colorToDepth.isNull())
{
// The Color Camera frame at timestamp t0 with respect to Depth
// Camera frame at timestamp t1.
//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
int pixelsSet = 0;
int depthSizeDec = colorCamera_?8:1;
depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
int oi=0;
int closePoints = 0;
float closeROI[4];
closeROI[0] = depth.cols/4;
closeROI[1] = 3*(depth.cols/4);
closeROI[2] = depth.rows/4;
closeROI[3] = 3*(depth.rows/4);
unsigned short minDepthValue=10000;
for(unsigned int i=0; i<cloud.total(); ++i)
TangoDoubleMatrixTransformData matrix_transform;
status = TangoSupport_getDoubleMatrixTransformAtTime(
video_overlay_timestamp,
TANGO_COORDINATE_FRAME_CAMERA_COLOR,
TANGO_COORDINATE_FRAME_START_OF_SERVICE,
TANGO_SUPPORT_ENGINE_OPENGL,
TANGO_SUPPORT_ENGINE_OPENGL,
static_cast<TangoSupportRotation>(rotation),
&matrix_transform);
if (matrix_transform.status_code == TANGO_POSE_VALID)
{
float * p = cloud.ptr<float>(0,i);
cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
// Get projection matrix
TangoCameraIntrinsics color_camera_intrinsics;
int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
TANGO_CAMERA_COLOR,
static_cast<TangoSupportRotation>(rotation),
&color_camera_intrinsics);
if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
{
scanData.at(oi++) = pt;
}
if (ret == TANGO_SUCCESS) {
float image_width = static_cast<float>(color_camera_intrinsics.width);
float image_height = static_cast<float>(color_camera_intrinsics.height);
float fx = static_cast<float>(color_camera_intrinsics.fx);
float fy = static_cast<float>(color_camera_intrinsics.fy);
float cx = static_cast<float>(color_camera_intrinsics.cx);
float cy = static_cast<float>(color_camera_intrinsics.cy);
int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
// get the coordinate on image plane.
pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
unsigned short depth_value(pt.z * 1000.0f);
if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
depth_value < 600)
{
++closePoints;
if(depth_value < minDepthValue)
viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
if(!getOriginOffset().isNull())
{
minDepthValue = depth_value;
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
}
bool pixelSet = false;
if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
depth_value)
{
unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
if(depthPixel == 0 || depthPixel > depth_value)
projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
image_width, image_height, fx, fy, cx, cy, 0.3, 50);
switch(rotation)
{
depthPixel = depth_value;
pixelSet = true;
case ROTATION_90:
memcpy(transformed_uvs_, kTextureCoords90, 8*sizeof(float));
break;
case ROTATION_180:
memcpy(transformed_uvs_, kTextureCoords180, 8*sizeof(float));
break;
case ROTATION_270:
memcpy(transformed_uvs_, kTextureCoords270, 8*sizeof(float));
break;
case ROTATION_0:
default:
memcpy(transformed_uvs_, kTextureCoords0, 8*sizeof(float));
}
uvs_initialized_ = true;
}
if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
depth_value)
else
{
unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
if(depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
pixelSet = true;
}
UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
}
if(pixelSet)
{
pixelsSet += 1;
}
}
if(closePoints > 100)
{
this->post(new CameraInfoEvent(0, "TooClose", ""));
}
if(oi)
{
scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
}
//LOGD("pixels depth set= %d", pixelsSet);
}
else
{
LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
}
if(!rgb.empty() && !depth.empty())
{
depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
Transform odom = getPoseAtTimestamp(rgbStamp);
//LOGD("Local = %s", model.localTransform().prettyPrint().c_str());
//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;
}
//LOGD("rtabmap = %s", odom.prettyPrint().c_str());
//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
Transform scanLocalTransform = model.localTransform();
if(rawScanPublished_)
{
data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
else
{
data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
}
info->odomPose = odom;
}
else
{
LOGE("Could not get depth and rgb images!?!");
UERROR("TangoService_updateTextureExternalOes failed!");
}
}
SensorData data;
if(dataReady_.acquireTry(1))
{
boost::mutex::scoped_lock lock(dataMutex_);
data = data_;
data_ = SensorData();
if(info)
{
info->odomPose = data.groundTruth();
data.setGroundTruth(Transform());
}
}
return data;

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

@@ -71,8 +71,7 @@ private:
bool colorCamera_;
int decimation_;
bool rawScanPublished_;
cv::Mat cloud_;
double cloudStamp_;
SensorData data_;
cv::Mat tangoColor_;
int tangoColorType_;
double tangoColorStamp_;

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

@@ -73,7 +73,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define LOW_RES_PIX 2
#define DEBUG_RENDERING_PERFORMANCE
//#define DEBUG_RENDERING_PERFORMANCE
const int g_optMeshId = -100;
@@ -894,10 +894,6 @@ bool RTABMapApp::startCamera()
LOGI("Cloud density level %d", cloudDensityLevel_);
LOGI("Start camera thread");
if(cameraDriver_ == 0)
{
camera_->start();
}
cameraJustInitialized_ = true;
return true;
}
@@ -1222,36 +1218,30 @@ int RTABMapApp::Render()
glm::mat4 arProjectionMatrix(0);
glm::mat4 arViewMatrix(0);
rtabmap::Mesh occlusionMesh;
if((cameraDriver_ == 1 || cameraDriver_ == 2 || cameraDriver_ == 3) && camera_!=0)
{
boost::mutex::scoped_lock lock(cameraMutex_);
if(camera_!=0)
{
#ifdef RTABMAP_ARCORE
if(cameraDriver_ == 1)
if(cameraDriver_ <= 2)
{
((rtabmap::CameraARCore*)camera_)->updateOcclusionImage(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson);
camera_->spinOnce();
}
#ifdef DEBUG_RENDERING_PERFORMANCE
LOGW("Camera spinOnce %fs", time.ticks());
#endif
if(cameraDriver_ == 1 || cameraDriver_ == 2)
{
camera_->spinOnce();
}
#ifdef RTABMAP_ARCORE
if(cameraDriver_ == 1)
if(cameraDriver_ != 2)
{
if(main_scene_.background_renderer_ == 0 &&
((rtabmap::CameraARCore*)camera_)->getTextureId() != 9999 &&
((rtabmap::CameraARCore*)camera_)->getTextureId() != 0)
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId(), true);
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
}
if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
if(camera_->uvsInitialized())
{
uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
if(graphOptimization_ && !mapToOdom_.isIdentity())
{
rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
@@ -1261,12 +1251,12 @@ int RTABMapApp::Render()
if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
{
rtabmap::CameraModel occlusionModel;
cv::Mat occlusionImage = ((rtabmap::CameraARCore*)camera_)->getOcclusionImage(&occlusionModel);
cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
if(occlusionModel.isValidForProjection())
{
pcl::IndicesPtr indices(new std::vector<int>);
int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
@@ -1274,54 +1264,15 @@ int RTABMapApp::Render()
occlusionMesh.indices = indices;
occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
}
else
else if(!occlusionImage.empty())
{
UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
}
}
}
#ifdef DEBUG_RENDERING_PERFORMANCE
LOGW("Update background and occlusion mesh %fs", time.ticks());
#endif
if(cameraDriver_ == 3)
{
if(main_scene_.background_renderer_ == 0 &&
((rtabmap::CameraMobile*)camera_)->getTextureId() != 9999 &&
((rtabmap::CameraMobile*)camera_)->getTextureId() != 0)
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), false);
}
if(((rtabmap::CameraMobile*)camera_)->uvsInitialized())
{
uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
if(graphOptimization_ && !mapToOdom_.isIdentity())
{
rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
}
}
if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
{
rtabmap::CameraModel occlusionModel;
cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
if(occlusionModel.isValidForProjection())
{
pcl::IndicesPtr indices(new std::vector<int>);
int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
occlusionMesh.indices = indices;
occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
}
else if(!occlusionImage.empty())
{
UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
}
}
}
}
}
@@ -3691,6 +3642,13 @@ void RTABMapApp::postCameraPoseEvent(
boost::mutex::scoped_lock lock(cameraMutex_);
if(cameraDriver_ == 3 && camera_)
{
if(qx==0 && qy==0 && qz==0 && qw==0)
{
// Lost! clear buffer
poseBuffer_.clear();
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);