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

android: fixed yuv conversion bug on some devices, updated default driver based on which one is available and built with it.

Browse Source
This commit is contained in:
matlabbe
2020-04-29 12:18:02 -04:00
parent 37e0860b0c
commit b3eee0d7bf
7 changed files with 166 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -575,17 +575,31 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
}
#endif
const uint8_t * plane_data;
const uint8_t * plane_uv_data;
int32_t data_length;
ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
int32_t uv_data_length;
ArImage_getPlaneData(arSession_, image, 2, &plane_uv_data, &uv_data_length);
if(plane_data != nullptr)
if(plane_data != nullptr && data_length == height*width)
{
double stamp = double(timestamp_ns)/10e8;
#ifndef DISABLE_LOG
LOGI("data_length=%d stamp=%f", data_length, stamp);
#endif
cv::Mat rgb;
cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
if((long)plane_uv_data-(long)plane_data != data_length)
{
// The uv-plane is not concatenated to y plane in memory, so concatenate them
cv::Mat yuv(height+height/2, width, CV_8UC1);
memcpy(yuv.data, plane_data, data_length);
memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
cv::cvtColor(yuv, rgb, CV_YUV2BGR_NV21);
}
else
{
cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
}
std::vector<cv::KeyPoint> kpts;
std::vector<cv::Point3f> kpts3;

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

@@ -620,6 +620,37 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
return status;
}
bool RTABMapApp::isBuiltWith(int cameraDriver) const
{
if(cameraDriver == 0)
{
#ifdef RTABMAP_TANGO
return true;
#else
return false;
#endif
}
if(cameraDriver == 1)
{
#ifdef RTABMAP_ARCORE
return true;
#else
return false;
#endif
}
if(cameraDriver == 2)
{
#ifdef RTABMAP_ARENGINE
return true;
#else
return false;
#endif
}
return false;
}
bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver)
{
cameraDriver_ = driver;
@@ -3269,6 +3300,7 @@ int RTABMapApp::postProcessing(int approach)
void RTABMapApp::postCameraPoseEvent(
float x, float y, float z, float qx, float qy, float qz, float qw)
{
boost::mutex::scoped_lock lock(cameraMutex_);
if(cameraDriver_ == 3 && camera_)
{
rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
@@ -3281,20 +3313,36 @@ void RTABMapApp::postOdometryEvent(
float x, float y, float z, float qx, float qy, float qz, float qw,
float fx, float fy, float cx, float cy,
double stamp,
void * rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
void * yPlane, void * uPlane, void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
float * points, int pointsLen)
{
#ifdef RTABMAP_ARCORE
boost::mutex::scoped_lock lock(cameraMutex_);
if(cameraDriver_ == 3 && camera_)
{
if(fx > 0.0f && fy > 0.0f && cx > 0.0f && cy > 0.0f && stamp > 0.0f && rgb)
if(fx > 0.0f && fy > 0.0f && cx > 0.0f && cy > 0.0f && stamp > 0.0f && yPlane && vPlane && yPlaneLen == rgbWidth*rgbHeight)
{
if(rgbFormat == AR_IMAGE_FORMAT_YUV_420_888 &&
depthFormat == AIMAGE_FORMAT_DEPTH16)
{
cv::Mat outputRGB;
cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, rgb), outputRGB, CV_YUV2BGR_NV21);
#ifndef DISABLE_LOG
LOGD("y=%p u=%p v=%p yLen=%d y->v=%ld", yPlane, uPlane, vPlane, yPlaneLen, (long)vPlane-(long)yPlane);
#endif
if((long)vPlane-(long)yPlane != yPlaneLen)
{
// The uv-plane is not concatenated to y plane in memory, so concatenate them
cv::Mat yuv(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1);
memcpy(yuv.data, yPlane, yPlaneLen);
memcpy(yuv.data+yPlaneLen, vPlane, rgbHeight/2*rgbWidth);
cv::cvtColor(yuv, outputRGB, CV_YUV2BGR_NV21);
}
else
{
cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, yPlane), outputRGB, CV_YUV2BGR_NV21);
}
cv::Mat outputDepth;
if(depthHeight>0 && depthWidth>0)
@@ -3344,6 +3392,10 @@ void RTABMapApp::postOdometryEvent(
}
}
}
else
{
UERROR("Missing image information!");
}
}
#else
UERROR("Not built with ARCore!");

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

@@ -55,6 +55,7 @@ class RTABMapApp : public UEventsHandler {
int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
bool isBuiltWith(int cameraDriver) const;
bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
// Allocate OpenGL resources for rendering, mainly for initializing the Scene.
@@ -153,7 +154,7 @@ class RTABMapApp : public UEventsHandler {
float x, float y, float z, float qx, float qy, float qz, float qw,
float fx, float fy, float cx, float cy,
double stamp,
void * rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
void * yPlane, void * uPlane, void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
float * points, int pointsLen);

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

@@ -123,6 +123,20 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
}
}
JNIEXPORT bool JNICALL
Java_com_introlab_rtabmap_RTABMapLib_isBuiltWith(
JNIEnv* env, jclass, jlong native_application, int cameraDriver) {
if(native_application)
{
return native(native_application)->isBuiltWith(cameraDriver);
}
else
{
UERROR("native_application is null!");
return false;
}
}
JNIEXPORT bool JNICALL
Java_com_introlab_rtabmap_RTABMapLib_startCamera(
JNIEnv* env, jclass, jlong native_application, jobject iBinder, jobject context, jobject activity, int driver) {
@@ -861,20 +875,22 @@ Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
float x, float y, float z, float qx, float qy, float qz, float qw,
float fx, float fy, float cx, float cy,
double stamp,
jobject rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
jobject yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
jobject points, int pointsLen)
{
if(native_application)
{
void *rgbPtr = env->GetDirectBufferAddress(rgb);
void *yPtr = env->GetDirectBufferAddress(yPlane);
void *uPtr = env->GetDirectBufferAddress(uPlane);
void *vPtr = env->GetDirectBufferAddress(vPlane);
void *depthPtr = env->GetDirectBufferAddress(depth);
float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
native(native_application)->postOdometryEvent(
x,y,z,qx,qy,qz,qw,
fx,fy,cx,cy,
stamp,
rgbPtr, rgbLen, rgbWidth, rgbHeight, rgbFormat,
yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
pointsPtr, pointsLen);
}