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

ARCore: when tof camera is not available, use arcore point cloud as keypoints

Browse Source
This commit is contained in:
matlabbe
2020-04-26 14:48:48 -04:00
parent 3abdd22031
commit 06482d2378
8 changed files with 174 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -451,6 +451,54 @@ 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();
}
SensorData CameraARCore::captureImage(CameraInfo * info)
{
UScopeMutex lock(arSessionMutex_);
@@ -492,7 +540,9 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &width, &height);
UINFO("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
#ifndef DISABLE_LOG
LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
#endif
if(fx > 0 && fy > 0 && width > 0 && height > 0 && cx > 0 && cy > 0)
{
@@ -513,13 +563,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
{
int32_t num_planes;
int32_t pixel_stride;
int32_t row_stride;
const uint8_t * plane_data;
int32_t data_length;
ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
for(int i=0;i<num_planes; ++i)
{
ArImage_getPlanePixelStride(arSession_, image, 0, &pixel_stride);
//LOGI("Plane %d/%d: w=%d h=%d stride=%d", i+1, num_planes, width, height, pixel_stride);
ArImage_getPlanePixelStride(arSession_, image, i, &pixel_stride);
ArImage_getPlaneRowStride(arSession_, image, i, &row_stride);
#ifndef DISABLE_LOG
LOGI("Plane %d/%d: pixel stride=%d, row stride=%d", i+1, num_planes, pixel_stride, row_stride);
#endif
}
ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
@@ -527,10 +581,14 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
if(plane_data != nullptr)
{
double stamp = double(timestamp_ns)/10e8;
//LOGI("data_length=%d stamp=%f", data_length, stamp);
#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);
std::vector<cv::KeyPoint> kpts;
std::vector<cv::Point3f> kpts3;
LaserScan scan;
if(pointCloud)
{
@@ -538,38 +596,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArPointCloud_getNumberOfPoints(arSession_, pointCloud, &points);
const float * pointCloudData = 0;
ArPointCloud_getData(arSession_, pointCloud, &pointCloudData);
#ifndef DISABLE_LOG
LOGI("pointCloudData=%d size=%d", pointCloudData?1:0, points);
#endif
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];
}
*(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);
}
scan = LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3);
}
}
else
@@ -578,6 +610,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
}
data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
data.setFeatures(kpts, kpts3, cv::Mat());
}
}
else

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

@@ -50,6 +50,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 smoothing = false);
virtual ~CameraARCore();

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

@@ -70,7 +70,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
#define LOW_RES_PIX 2
#define DEBUG_RENDERING_PERFORMANCE
//#define DEBUG_RENDERING_PERFORMANCE
const int g_optMeshId = -100;
@@ -1436,7 +1436,7 @@ int RTABMapApp::Render()
}
#ifdef DEBUG_RENDERING_PERFORMANCE
LOGW("Looking for data to load (%d) %fs", bufferedSensorData.size(), time.ticks());
LOGW("Looking for data to load (%d) %fs", (int)bufferedSensorData.size(), time.ticks());
#endif
std::map<int, rtabmap::Transform> posesWithMarkers = rtabmapEvents.back()->getStats().poses();
@@ -3282,12 +3282,13 @@ void RTABMapApp::postOdometryEvent(
float fx, float fy, float cx, float cy,
double stamp,
void * rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat)
void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
float * points, int pointsLen)
{
#ifdef RTABMAP_ARCORE
if(cameraDriver_ == 3 && camera_)
{
if(fx > 0.0f && fy > 0.0f && cx > 0.0f && cy > 0.0f && stamp > 0.0f && rgb && depth)
if(fx > 0.0f && fy > 0.0f && cx > 0.0f && cy > 0.0f && stamp > 0.0f && rgb)
{
if(rgbFormat == AR_IMAGE_FORMAT_YUV_420_888 &&
depthFormat == AIMAGE_FORMAT_DEPTH16)
@@ -3295,7 +3296,11 @@ void RTABMapApp::postOdometryEvent(
cv::Mat outputRGB;
cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, rgb), outputRGB, CV_YUV2BGR_NV21);
cv::Mat outputDepth(depthHeight, depthWidth, CV_16UC1);
cv::Mat outputDepth;
if(depthHeight>0 && depthWidth>0)
{
outputDepth = cv::Mat(depthHeight, depthWidth, CV_16UC1);
}
uint16_t *dataShort = (uint16_t *)depth;
for (int y = 0; y < outputDepth.rows; ++y)
{
@@ -3307,12 +3312,33 @@ void RTABMapApp::postOdometryEvent(
}
}
if(!outputRGB.empty() && !outputDepth.empty())
if(!outputRGB.empty())
{
rtabmap::CameraModel model = rtabmap::CameraModel(fx, fy, cx, cy, camera_->getDeviceTColorCamera(), 0, cv::Size(rgbWidth, rgbHeight));
rtabmap::SensorData data(outputRGB, outputDepth, model, 0, stamp);
rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
#ifndef DISABLE_LOG
LOGI("pointCloudData size=%d", pointsLen);
#endif
std::vector<cv::KeyPoint> kpts;
std::vector<cv::Point3f> kpts3;
rtabmap::LaserScan scan;
if(points && pointsLen>0)
{
if(outputDepth.empty())
{
scan = rtabmap::CameraARCore::scanFromPointCloudData(points, pointsLen, pose, model, outputRGB, &kpts, &kpts3);
}
else
{
// We will recompute features if depth is available
scan = rtabmap::CameraARCore::scanFromPointCloudData(points, pointsLen, pose, model, outputRGB);
}
}
rtabmap::SensorData data(scan, outputRGB, outputDepth, model, 0, stamp);
data.setFeatures(kpts, kpts3, cv::Mat());
camera_->setData(data, pose);
camera_->spinOnce();
}

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

@@ -154,7 +154,8 @@ class RTABMapApp : public UEventsHandler {
float fx, float fy, float cx, float cy,
double stamp,
void * rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat);
void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
float * points, int pointsLen);
protected:
virtual bool handleEvent(UEvent * event);

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

@@ -862,18 +862,21 @@ Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
float fx, float fy, float cx, float cy,
double stamp,
jobject rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat)
jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
jobject points, int pointsLen)
{
if(native_application)
{
void *rgbPtr = env->GetDirectBufferAddress(rgb);
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,
depthPtr, depthLen, depthWidth, depthHeight, depthFormat);
depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
pointsPtr, pointsLen);
}
else
{