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

Added AR mapping mode (select ARCore ndk driver and set first person view)

Browse Source
This commit is contained in:
matlabbe
2020-06-29 15:24:35 -04:00
parent 542f06ec24
commit 4142ff003c
12 changed files with 376 additions and 225 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
app/android/jni/CMakeLists.txt
View File

@@ -22,6 +22,7 @@ set(sources
scene.cpp
point_cloud_drawable.cpp
graph_drawable.cpp
background_renderer.cc
tango-gl/axis.cpp
tango-gl/camera.cpp
tango-gl/conversions.cpp

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

@@ -34,29 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
#ifdef DEPTH_TEST
// Camera Callbacks
static void CameraDeviceOnDisconnected(void* context, ACameraDevice* device) {
LOGE("Camera(id: %s) is disconnected.\n", ACameraDevice_getId(device));
}
static void CameraDeviceOnError(void* context, ACameraDevice* device,
int error) {
LOGE("Error(code: %d) on Camera(id: %s).\n", error,
ACameraDevice_getId(device));
}
// Capture Callbacks
bool g_captureSessionReady = false;
static void CaptureSessionOnReady(void* context,
ACameraCaptureSession* session) {
LOGI("Session is ready.\n");
g_captureSessionReady = true;
}
static void CaptureSessionOnActive(void* context,
ACameraCaptureSession* session) {
LOGI("Session is activated.\n");
}
#endif // DEPTH_TEST
//////////////////////////////
// CameraARCore
//////////////////////////////
@@ -65,16 +42,21 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smooth
env_(env),
context_(context),
activity_(activity),
arInstallRequested_(false)
arInstallRequested_(false),
textureId_(9999),
uvs_initialized_(false)
{
glGenTextures(1, &textureId_);
}
CameraARCore::~CameraARCore() {
// Disconnect ARCore service
close();
glDeleteTextures(1, &textureId_);
if(textureId_ != 9999)
{
glDeleteTextures(1, &textureId_);
textureId_ = 9999;
}
}
@@ -146,132 +128,10 @@ std::string CameraARCore::getSerial() const
return "ARCore";
}
#ifdef DEPTH_TEST
void OnImageCallback(void *ctx, AImageReader *reader) {
reinterpret_cast<CameraARCore *>(ctx)->imageCallback(reader);
}
void CameraARCore::imageCallback(AImageReader *reader) {
int32_t format;
media_status_t status = AImageReader_getFormat(reader, &format);
UWARN("format=%d", format);
UASSERT_MSG(status == AMEDIA_OK, "Failed to get the media format");
if (format == AIMAGE_FORMAT_DEPTH16) {
// Create a thread and write out the jpeg files
AImage *image = nullptr;
media_status_t status = AImageReader_acquireNextImage(reader, &image);
UASSERT_MSG(status == AMEDIA_OK && image, "Image is not available");
int planeCount;
status = AImage_getNumberOfPlanes(image, &planeCount);
UASSERT_MSG(status == AMEDIA_OK && planeCount == 1,
uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
uint8_t *data = nullptr;
int len = 0;
int stride;
int width;
int height;
AImage_getWidth(image, &width);
AImage_getHeight(image, &height);
AImage_getPlaneRowStride(image, 0, &stride);
AImage_getPlaneData(image, 0, &data, &len);
cv::Mat output(height, width, CV_16UC1);
uint16_t *dataShort = (uint16_t *)data;
uint16_t max=0x0;
for (int y = 0; y < output.rows; ++y)
{
for (int x = 0; x < output.cols; ++x)
{
uint16_t depthSample = dataShort[y*output.cols + x];
uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
output.at<uint16_t>(y,x) = depthRange;
if(depthRange > max)
{
max = depthRange;
}
}
}
UWARN("width=%d, height=%d, bytes=%d stride=%d max=%dmm",
width, height, len, stride, (int)max);
std::string path = "/storage/emulated/0/RTAB-Map/depth.png";
cv::imwrite(path, output);
UWARN("depth image saved to %s", path.c_str());
AImage_delete(image);
}
}
#endif // DEPTH_TEST
bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
{
close();
#ifdef DEPTH_TEST
///////////////////////////
// Depth image using camera2 API
/////////////////////////////
camera_status_t cameraStatus = ACAMERA_OK;
cameraManager_ = ACameraManager_create();
deviceStateCallbacks_.onDisconnected = CameraDeviceOnDisconnected;
deviceStateCallbacks_.onError = CameraDeviceOnError;
const char * cameraId = "0";
cameraStatus = ACameraManager_openCamera(cameraManager_, cameraId, &deviceStateCallbacks_, &cameraDevice_);
UASSERT_MSG(cameraStatus == ACAMERA_OK, uFormat("Failed to open camera device (id: %s)",
cameraId).c_str());
// Currently only working resolution on Huawei P30 Pro
cv::Size size(240, 180);
int format = AIMAGE_FORMAT_DEPTH16;
media_status_t mediaStatus = AImageReader_new(size.width, size.height, format, 2, &imageReader_);
UASSERT_MSG(imageReader_ && mediaStatus == AMEDIA_OK, uFormat("Failed to create AImageReader %dx%d format=%d",
size.width, size.height, format).c_str());
AImageReader_ImageListener listener{
.context = this,
.onImageAvailable = OnImageCallback,
};
AImageReader_setImageListener(imageReader_, &listener);
//
ANativeWindow *nativeWindow;
mediaStatus = AImageReader_getWindow(imageReader_, &nativeWindow);
UASSERT_MSG(mediaStatus == AMEDIA_OK, "Could not get ANativeWindow");
outputNativeWindow_ = nativeWindow;
ACaptureSessionOutputContainer_create(&captureSessionOutputContainer_);
ANativeWindow_acquire(outputNativeWindow_);
ACaptureSessionOutput_create(outputNativeWindow_, &sessionOutput_);
ACaptureSessionOutputContainer_add(captureSessionOutputContainer_, sessionOutput_);
ACameraOutputTarget_create(outputNativeWindow_, &cameraOutputTarget_);
cameraStatus = ACameraDevice_createCaptureRequest(cameraDevice_, TEMPLATE_RECORD, &captureRequest_);
UASSERT_MSG(cameraStatus == ACAMERA_OK,
uFormat("Failed to create preview capture request (id: %s, status=%d)",
cameraId, cameraStatus).c_str());
ACaptureRequest_addTarget(captureRequest_, cameraOutputTarget_);
captureSessionStateCallbacks_.onReady = CaptureSessionOnReady;
captureSessionStateCallbacks_.onActive = CaptureSessionOnActive;
ACameraDevice_createCaptureSession(
cameraDevice_,
captureSessionOutputContainer_, // outputs
&captureSessionStateCallbacks_, // callbacks
&captureSession_);
ACameraCaptureSession_setRepeatingRequest(captureSession_, nullptr, 1,
&captureRequest_, nullptr);
// Don't start ARCore as we cannot use both at the same time
return true;
#endif // DEPTH_TEST
UScopeMutex lock(arSessionMutex_);
ArInstallStatus install_status;
@@ -302,10 +162,19 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
UASSERT(arSession_);
int32_t is_depth_supported = 0; // Disabled by default, depth is not super accurate for mapping
//ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
ArConfig_create(arSession_, &arConfig_);
UASSERT(arConfig_);
ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
if (is_depth_supported!=0) {
ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
} else {
ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
}
ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
ArFrame_create(arSession_, &arFrame_);
@@ -361,9 +230,6 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
deviceTColorCamera_ = opticalRotation;
// Required as ArSession_update does some off-screen OpenGL stuff...
ArSession_setCameraTextureName(arSession_, textureId_);
if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
{
UERROR("Cannot resume camera!");
@@ -410,44 +276,6 @@ void CameraARCore::close()
}
arPose_ = nullptr;
#ifdef DEPTH_TEST
if(captureSession_!=nullptr)
{
g_captureSessionReady = false;
ACameraCaptureSession_stopRepeating(captureSession_);
double start = UTimer::now();
while(g_captureSessionReady != true && UTimer::now()-start < 2.0){
uSleep(100);
UWARN("Waiting session to close.... max 2 seconds");
}
//ACameraCaptureSession_close(captureSession_); // FIXME: this crashes?!
captureSession_ = nullptr;
ACaptureRequest_removeTarget(captureRequest_, cameraOutputTarget_);
ACaptureRequest_free(captureRequest_);
ACameraOutputTarget_free(cameraOutputTarget_);
captureRequest_ = nullptr;
cameraOutputTarget_ = nullptr;
ACaptureSessionOutputContainer_remove(captureSessionOutputContainer_, sessionOutput_);
ANativeWindow_release(outputNativeWindow_);
ACaptureSessionOutputContainer_free(captureSessionOutputContainer_);
ACaptureSessionOutput_free(sessionOutput_);
captureSessionOutputContainer_ = nullptr;
sessionOutput_ = nullptr;
ACameraDevice_close(cameraDevice_);
cameraDevice_ = nullptr;
ACameraManager_delete(cameraManager_);
cameraManager_ = nullptr;
AImageReader_delete(imageReader_);
imageReader_ = nullptr;
}
#endif
CameraMobile::close();
}
@@ -499,6 +327,20 @@ LaserScan CameraARCore::scanFromPointCloudData(
return LaserScan();
}
void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
{
CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
if(arSession_)
{
int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
if (ret > 3) {
ret -= 4;
}
ArSession_setDisplayGeometry(arSession_, ret, width, height);
}
}
SensorData CameraARCore::captureImage(CameraInfo * info)
{
UScopeMutex lock(arSessionMutex_);
@@ -510,15 +352,44 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
return data;
}
if(textureId_ == 9999)
{
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_);
// Update session to get current frame and render camera background.
if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
LOGE("CameraARCore::captureImage() ArSession_update error");
return data;
}
// If display rotation changed (also includes view size change), we need to
// re-query the uv coordinates for the on-screen portion of the camera image.
int32_t geometry_changed = 0;
ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
if (geometry_changed != 0 || !uvs_initialized_) {
ArFrame_transformCoordinates2d(
arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
transformed_uvs_);
UASSERT(transformed_uvs_);
uvs_initialized_ = true;
}
ArCamera* ar_camera;
ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
ArCamera_getProjectionMatrix(arSession_, ar_camera,
/*near=*/0.1f, /*far=*/100.f,
glm::value_ptr(projectionMatrix_));
ArTrackingState camera_tracking_state;
ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
@@ -551,17 +422,68 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArPointCloud * pointCloud = nullptr;
ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
int32_t is_depth_supported = 0;
ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
ArImage * image = nullptr;
ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
if(status == AR_SUCCESS)
{
cv::Mat outputDepth;
if(is_depth_supported)
{
LOGD("Acquire depth image!");
ArImage * depthImage = nullptr;
ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
ArImageFormat format;
ArImage_getFormat(arSession_, depthImage, &format);
if(format == AR_IMAGE_FORMAT_DEPTH16)
{
LOGD("Depth format detected!");
int planeCount;
ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
LOGD("planeCount=%d", planeCount);
UASSERT_MSG(planeCount == 1,
uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
const uint8_t *data = nullptr;
int len = 0;
int stride;
int width;
int height;
ArImage_getWidth(arSession_, depthImage, &width);
ArImage_getHeight(arSession_, depthImage, &height);
ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
outputDepth = cv::Mat(height, width, CV_16UC1);
uint16_t *dataShort = (uint16_t *)data;
uint16_t max=0x0;
for (int y = 0; y < outputDepth.rows; ++y)
{
for (int x = 0; x < outputDepth.cols; ++x)
{
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;
if(depthRange > max)
{
max = depthRange;
}
}
}
}
ArImage_release(depthImage);
}
int64_t timestamp_ns;
ArImageFormat format;
ArImage_getTimestamp(arSession_, image, &timestamp_ns);
ArImage_getFormat(arSession_, image, &format);
if(format == AR_IMAGE_FORMAT_YUV_420_888)
{
#ifndef DISABLE_LOG
int32_t num_planes;
ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
@@ -623,7 +545,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
LOGI("pointCloud empty");
}
data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
data = SensorData(scan, rgb, outputDepth, model, 0, stamp);
data.setFeatures(kpts, kpts3, cv::Mat());
}
}
@@ -662,21 +584,54 @@ void CameraARCore::capturePoseOnly()
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
SensorData data;
if(!arSession_)
{
return;
}
if(textureId_ == 9999)
{
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_);
// Update session to get current frame and render camera background.
if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
LOGE("CameraARCore::captureImage() ArSession_update error");
LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
return;
}
// If display rotation changed (also includes view size change), we need to
// re-query the uv coordinates for the on-screen portion of the camera image.
int32_t geometry_changed = 0;
ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
if (geometry_changed != 0 || !uvs_initialized_) {
ArFrame_transformCoordinates2d(
arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
transformed_uvs_);
UASSERT(transformed_uvs_);
uvs_initialized_ = true;
}
/*ArImage* ar_image = nullptr;
ArStatus status =
ArFrame_acquireCameraImage(arSession_, arFrame_, &ar_image);
ArImage_release(ar_image);
*/
ArCamera* ar_camera;
ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
ArCamera_getProjectionMatrix(arSession_, ar_camera,
/*near=*/0.1f, /*far=*/100.f,
glm::value_ptr(projectionMatrix_));
ArTrackingState camera_tracking_state;
ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);

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

@@ -38,14 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UEvent.h>
#include <rtabmap/utilite/UTimer.h>
#include <boost/thread/mutex.hpp>
#include <background_renderer.h>
#include <arcore_c_api.h>
#ifdef DEPTH_TEST
#include <camera/NdkCameraDevice.h>
#include <camera/NdkCameraManager.h>
#include <media/NdkImageReader.h>
#include <android/native_window.h>
#endif
namespace rtabmap {
@@ -64,16 +63,22 @@ public:
CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
virtual ~CameraARCore();
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_;}
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
void setupGL();
virtual void close(); // close Tango connection
virtual std::string getSerial() const;
GLuint getTextureId() const {return textureId_;}
#ifdef DEPTH_TEST
void imageCallback(AImageReader *reader);
#endif // DEPTH_TEST
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
virtual void capturePoseOnly();
private:
@@ -92,22 +97,10 @@ private:
GLuint textureId_;
UMutex arSessionMutex_;
#ifdef DEPTH_TEST
// Camera variables
ACameraDevice* cameraDevice_ = nullptr;
ACaptureRequest* captureRequest_ = nullptr;
ACameraOutputTarget* cameraOutputTarget_ = nullptr;
ACaptureSessionOutput* sessionOutput_ = nullptr;
ACaptureSessionOutputContainer* captureSessionOutputContainer_ = nullptr;
ACameraCaptureSession* captureSession_ = nullptr;
ANativeWindow *outputNativeWindow_ = nullptr;
ACameraDevice_StateCallbacks deviceStateCallbacks_;
ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
ACameraManager* cameraManager_ = nullptr;
AImageReader* imageReader_ = nullptr;
#endif // DEPTH_TEST
float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
bool uvs_initialized_ = false;
glm::mat4 viewMatrix_;
glm::mat4 projectionMatrix_;
};
} /* namespace rtabmap */

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

@@ -94,7 +94,7 @@ public:
const CameraModel & getCameraModel() const {return model_;}
const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
void setSmoothing(bool enabled) {smoothing_ = enabled;}
virtual void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
void setGPS(const GPS & gps);
void addEnvSensor(int type, float value);
void setData(const SensorData & data, const Transform & pose);

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

@@ -69,6 +69,7 @@ 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>
#define LOW_RES_PIX 2
//#define DEBUG_RENDERING_PERFORMANCE
@@ -263,7 +264,7 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
boost::mutex::scoped_lock lock(cameraMutex_);
if(camera_)
{
camera_->setScreenRotation(rotation);
camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
}
}
@@ -657,6 +658,10 @@ bool RTABMapApp::isBuiltWith(int cameraDriver) const
bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver)
{
//ccapp = new computer_vision::ComputerVisionApplication();
//ccapp->OnResume(env, context, activity);
//return true;
cameraDriver_ = driver;
LOGW("startCamera() camera driver=%d", cameraDriver_);
boost::mutex::scoped_lock lock(cameraMutex_);
@@ -686,7 +691,6 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
{
#ifdef RTABMAP_ARCORE
camera_ = new rtabmap::CameraARCore(env, context, activity, smoothing_);
#else
UERROR("RTAB-Map is not built with ARCore support!");
#endif
@@ -712,7 +716,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
if(camera_->init())
{
camera_->setScreenRotation(main_scene_.getScreenRotation());
camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
//update mesh decimation based on camera calibration
LOGI("Cloud density level %d", cloudDensityLevel_);
@@ -937,6 +941,11 @@ void RTABMapApp::SetViewPort(int width, int height)
{
UINFO("");
main_scene_.SetupViewPort(width, height);
boost::mutex::scoped_lock lock(cameraMutex_);
if(camera_)
{
camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
}
}
class PostRenderEvent : public UEvent
@@ -1101,12 +1110,31 @@ int RTABMapApp::Render()
}
// ARCore and AREngine capture should be done in opengl thread!
const float* uvsTransformed = 0;
glm::mat4 arProjectionMatrix(0);
glm::mat4 arViewMatrix(0);
if((cameraDriver_ == 1 || cameraDriver_ == 2) && camera_!=0)
{
boost::mutex::scoped_lock lock(cameraMutex_);
if(camera_!=0)
{
camera_->spinOnce();
if(cameraDriver_ == 1)
{
#ifdef RTABMAP_ARCORE
if(main_scene_.background_renderer_ == 0)
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId());
}
if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
{
uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
//main_scene_.background_renderer_->Draw(uvsTransformed);
}
#endif
}
}
}
@@ -1792,7 +1820,7 @@ int RTABMapApp::Render()
fpsTime.restart();
main_scene_.setFrustumVisible(camera_!=0);
lastDrawnCloudsCount_ = main_scene_.Render();
lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix);
if(renderingTime_ < fpsTime.elapsed())
{
renderingTime_ = fpsTime.elapsed();

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

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/pcl_base.h>
#include <pcl/TextureMesh.h>
// RTABMapApp handles the application lifecycle and resources.
class RTABMapApp : public UEventsHandler {
public:

92
app/android/jni/background_renderer.cc Normal file
View File

@@ -0,0 +1,92 @@
/*
* Copyright 2018 Google Inc. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
// This modules handles drawing the passthrough camera image into the OpenGL
// scene.
#include "background_renderer.h"
#include <type_traits>
namespace {
const std::string kVertexShader =
"attribute vec4 a_Position;\n"
"attribute vec2 a_TexCoord;\n"
"varying vec2 v_TexCoord;\n"
"void main() {\n"
" gl_Position = a_Position;\n"
" v_TexCoord = a_TexCoord;\n"
"}\n";
const std::string kFragmentShader =
"#extension GL_OES_EGL_image_external : require\n"
"precision mediump float;\n"
"varying vec2 v_TexCoord;\n"
"uniform samplerExternalOES sTexture;\n"
"void main() {\n"
" vec4 sample = texture2D(sTexture, v_TexCoord);\n"
" float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
" gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
"}\n";
} // namespace
void BackgroundRenderer::InitializeGlContent(GLuint textureId)
{
texture_id_ = textureId;
shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
if (!shader_program_) {
LOGE("Could not create program.");
}
glUseProgram(shader_program_);
attribute_vertices_ = glGetAttribLocation(shader_program_, "a_Position");
attribute_uvs_ = glGetAttribLocation(shader_program_, "a_TexCoord");
glUseProgram(0);
}
void BackgroundRenderer::Draw(const float * transformed_uvs) {
static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
glUseProgram(shader_program_);
glDepthMask(GL_FALSE);
glEnable (GL_BLEND);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
glEnableVertexAttribArray(attribute_vertices_);
glEnableVertexAttribArray(attribute_uvs_);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glDisableVertexAttribArray(attribute_vertices_);
glDisableVertexAttribArray(attribute_uvs_);
glUseProgram(0);
glDepthMask(GL_TRUE);
glDisable (GL_BLEND);
tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
}

59
app/android/jni/background_renderer.h Normal file
View File

@@ -0,0 +1,59 @@
/*
* Copyright 2018 Google Inc. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
#include <GLES2/gl2.h>
#include <GLES2/gl2ext.h>
#include <cstdlib>
#include <arcore_c_api.h>
#include "util.h"
static const GLfloat BackgroundRenderer_kVertices[] = {
-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
};
// This class renders the passthrough camera image into the OpenGL frame.
class BackgroundRenderer {
public:
// Positions of the quad vertices in clip space (X, Y).
static constexpr int kNumVertices = 4;
public:
BackgroundRenderer() = default;
~BackgroundRenderer() = default;
// Sets up OpenGL state. Must be called on the OpenGL thread and before any
// other methods below.
void InitializeGlContent(GLuint textureId);
// Draws the background image. This methods must be called for every ArFrame
// returned by ArSession_update() to catch display geometry change events.
void Draw(const float * transformed_uvs);
private:
GLuint shader_program_;
GLuint texture_id_;
GLuint attribute_vertices_;
GLuint attribute_uvs_;
};
#endif // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_

23
app/android/jni/scene.cpp
View File

@@ -69,6 +69,7 @@ const std::string kGraphFragmentShader =
Scene::Scene() :
background_renderer_(0),
gesture_camera_(0),
axis_(0),
frustum_(0),
@@ -160,6 +161,8 @@ void Scene::DeleteResources() {
delete trace_;
delete grid_;
delete box_;
delete background_renderer_;
background_renderer_ = 0;
}
PointCloudDrawable::releaseShaderPrograms();
@@ -364,7 +367,7 @@ bool intersectFrustumAABB(
}
//Should only be called in OpenGL thread!
int Scene::Render() {
int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix) {
UASSERT(gesture_camera_ != 0);
if(currentPose_ == 0)
@@ -395,6 +398,17 @@ int Scene::Render() {
glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
bool renderBackgroundCamera =
background_renderer_ &&
gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
uvsTransformed;
if(renderBackgroundCamera)
{
projectionMatrix = arProjectionMatrix;
viewMatrix = arViewMatrix;
}
rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
// transform in same coordinate as frustum filtering
openglCamera *= rtabmap::Transform(
@@ -495,6 +509,11 @@ int Scene::Render() {
glClearColor(r_, g_, b_, 1.0f);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
if(renderBackgroundCamera)
{
background_renderer_->Draw(uvsTransformed);
}
if(!currentPose_->isNull())
{
if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -523,7 +542,7 @@ int Scene::Render() {
}
}
if(gridVisible_)
if(gridVisible_ && !renderBackgroundCamera)
{
grid_->Render(projectionMatrix, viewMatrix);
}

5
app/android/jni/scene.h
View File

@@ -38,6 +38,7 @@
#include <point_cloud_drawable.h>
#include <graph_drawable.h>
#include <bounding_box_drawable.h>
#include <background_renderer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
@@ -71,7 +72,7 @@ class Scene {
// frame's timestamp.
// @param: point_cloud_vertices, point cloud's vertices of the current point
// frame.
int Render();
int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0));
// Set render camera's viewing angle, first person, third person or top down.
//
@@ -152,6 +153,8 @@ class Scene {
bool isLighting() const {return lighting_;}
bool isBackfaceCulling() const {return backfaceCulling_;}
BackgroundRenderer * background_renderer_;
private:
// Camera object that allows user to use touch input to interact with.
tango_gl::GestureCamera* gesture_camera_;

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

@@ -252,7 +252,7 @@ inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
// @param display: integer value of display orientation, values available
// are 0, 1, 2 ,3. Followed by Android display orientation standard:
// https://developer.android.com/reference/android/view/Display.html#getRotation()
// @param color_camera: integer value of color camera oreintation, values
// @param color_camera: integer value of color camera orientation, values
// available are 0, 90, 180, 270. Followed by Android camera orientation
// standard:
// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation