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

Added depth/rgb registration option

Browse Source
This commit is contained in:
matlabbe
2021-06-10 11:07:25 -04:00
parent d7b5fa5d17
commit ba38d9f321
13 changed files with 842 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -56,13 +56,13 @@ CameraMobile::CameraMobile(bool smoothing) :
Camera(10),
deviceTColorCamera_(Transform::getIdentity()),
spinOncePreviousStamp_(0.0),
textureId_(9999),
uvs_initialized_(false),
previousStamp_(0.0),
stampEpochOffset_(0.0),
smoothing_(smoothing),
colorCameraToDisplayRotation_(ROTATION_0),
originUpdate_(false),
textureId_(9999),
uvs_initialized_(false)
originUpdate_(false)
{
glGenTextures(1, &textureId_);
}
@@ -156,7 +156,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
if(textureId_ != 0)
if(textureId_ != 0 && texCoord != 0)
{
cv::Mat rgbImage;
cv::cvtColor(data.imageRaw(), rgbImage, CV_BGR2RGBA);

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

@@ -916,6 +916,7 @@ void RTABMapApp::stopCamera()
camera_->close();
delete camera_;
camera_ = 0;
poseBuffer_.clear();
}
}
{
@@ -1243,7 +1244,7 @@ int RTABMapApp::Render()
if(main_scene_.background_renderer_ == 0)
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId());
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId(), true);
}
if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
{
@@ -1277,7 +1278,7 @@ int RTABMapApp::Render()
if(main_scene_.background_renderer_ == 0)
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId());
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), false);
}
if(((rtabmap::CameraMobile*)camera_)->uvsInitialized())
{
@@ -3657,7 +3658,7 @@ int RTABMapApp::postProcessing(int approach)
}
void RTABMapApp::postCameraPoseEvent(
float x, float y, float z, float qx, float qy, float qz, float qw)
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_)
@@ -3665,18 +3666,28 @@ void RTABMapApp::postCameraPoseEvent(
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);
poseBuffer_.insert(std::make_pair(stamp, pose));
if(poseBuffer_.size() > 1000)
{
poseBuffer_.erase(poseBuffer_.begin());
}
}
}
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,
rtabmap::Transform pose,
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
float depth_fx, float depth_fy, float depth_cx, float depth_cy,
const rtabmap::Transform & rgbFrame,
const rtabmap::Transform & depthFrame,
double stamp,
double depthStamp,
const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
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,
float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw,
const 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)
{
@@ -3684,7 +3695,7 @@ void RTABMapApp::postOdometryEvent(
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 && yPlane && vPlane && yPlaneLen == rgbWidth*rgbHeight)
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
//LOGD("rgb format = %d depth format =%d ", rgbFormat, depthFormat);
@@ -3770,10 +3781,87 @@ void RTABMapApp::postOdometryEvent(
if(!outputRGB.empty())
{
rtabmap::CameraModel model = rtabmap::CameraModel(fx, fy, cx, cy, camera_->getDeviceTColorCamera(), 0, cv::Size(rgbWidth, rgbHeight));
rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
// Registration depth to rgb
if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
{
UTimer time;
rtabmap::Transform motion = rtabmap::Transform::getIdentity();
if(depthStamp != stamp && !poseBuffer_.empty())
{
// Interpolate pose
if(!poseBuffer_.empty())
{
if(poseBuffer_.rbegin()->first < depthStamp)
{
UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first);
}
else
{
std::map<double, rtabmap::Transform >::const_iterator iterB = poseBuffer_.lower_bound(depthStamp);
std::map<double, rtabmap::Transform >::const_iterator iterA = iterB;
rtabmap::Transform poseDepth;
if(iterA != poseBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == poseBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && depthStamp == iterA->first)
{
poseDepth = iterA->second;
}
else if(depthStamp >= iterA->first && depthStamp <= iterB->first)
{
poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
}
else if(depthStamp < iterA->first)
{
UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first);
}
else
{
UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first);
}
if(!poseDepth.isNull())
{
#ifndef DISABLE_LOG
UDEBUG("poseRGB =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp);
UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
#endif
motion = pose.inverse()*poseDepth;
// transform in camera frame
#ifndef DISABLE_LOG
UDEBUG("motion=%s", motion.prettyPrint().c_str());
#endif
motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
#ifndef DISABLE_LOG
UDEBUG("motion=%s", motion.prettyPrint().c_str());
#endif
}
}
}
}
rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
float scale = (float)outputDepth.cols/(float)outputRGB.cols;
cv::Mat colorK = (cv::Mat_<double>(3,3) <<
rgb_fx*scale, 0, rgb_cx*scale,
0, rgb_fy*scale, rgb_cy*scale,
0, 0, 1);
cv::Mat depthK = (cv::Mat_<double>(3,3) <<
depth_fx, 0, depth_cx,
0, depth_fy, depth_cy,
0, 0, 1);
outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
#ifndef DISABLE_LOG
UDEBUG("Depth registration time: %fs", time.elapsed());
#endif
}
rtabmap::CameraModel model = rtabmap::CameraModel(rgb_fx, rgb_fy, rgb_cx, rgb_cy, camera_->getDeviceTColorCamera(), 0, cv::Size(rgbWidth, rgbHeight));
#ifndef DISABLE_LOG
//LOGI("pointCloudData size=%d", pointsLen);
#endif
@@ -3818,7 +3906,7 @@ void RTABMapApp::postOdometryEvent(
projectionMatrix[2][2] = p22;
projectionMatrix[2][3] = p32;
projectionMatrix[3][2] = p23;
glm::mat4 viewMatrix = rtabmap::glmFromTransform(rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw));
glm::mat4 viewMatrixMat = rtabmap::glmFromTransform(viewMatrix);
float texCoords[8];
texCoords[0] = t0;
texCoords[1] = t1;
@@ -3828,7 +3916,7 @@ void RTABMapApp::postOdometryEvent(
texCoords[5] = t5;
texCoords[6] = t6;
texCoords[7] = t7;
camera_->setData(data, pose, viewMatrix, projectionMatrix, texCoords);
camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
camera_->spinOnce();
}
}
@@ -3836,7 +3924,7 @@ void RTABMapApp::postOdometryEvent(
else
{
UERROR("Missing image information! fx=%f fy=%f cx=%f cy=%f stamp=%f yPlane=%d vPlane=%d yPlaneLen=%d rgbWidth=%d rgbHeight=%d",
fx, fy, cx, cy, stamp, yPlane?1:0, vPlane?1:0, yPlaneLen, rgbWidth, rgbHeight);
rgb_fx, rgb_fy, rgb_cx, rgb_cy, stamp, yPlane?1:0, vPlane?1:0, yPlaneLen, rgbWidth, rgbHeight);
}
}
#else

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

@@ -176,17 +176,21 @@ class RTABMapApp : public UEventsHandler {
int postProcessing(int approach);
void postCameraPoseEvent(
float x, float y, float z, float qx, float qy, float qz, float qw);
float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
void 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,
rtabmap::Transform pose,
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
float depth_fx, float depth_fy, float depth_cx, float depth_cy,
const rtabmap::Transform & rgbFrame,
const rtabmap::Transform & depthFrame,
double stamp,
double depthStamp,
const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
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,
float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
const 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
@@ -265,6 +269,7 @@ class RTABMapApp : public UEventsHandler {
std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
std::list<rtabmap::OdometryEvent> odomEvents_;
std::list<rtabmap::Transform> poseEvents_;
std::map<double, rtabmap::Transform> poseBuffer_;
rtabmap::Transform mapToOdom_;

32
app/android/jni/background_renderer.cc
View File

@@ -34,17 +34,21 @@ const std::string kVertexShader =
" v_TexCoord = a_TexCoord;\n"
"}\n";
const std::string kFragmentShader =
#ifdef __ANDROID__
const std::string kFragmentShaderOES =
"#extension GL_OES_EGL_image_external : require\n"
#endif
"precision mediump float;\n"
"varying vec2 v_TexCoord;\n"
#ifdef __ANDROID__
"uniform samplerExternalOES sTexture;\n"
#else
"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";
const std::string kFragmentShader =
"precision mediump float;\n"
"varying vec2 v_TexCoord;\n"
"uniform sampler2D sTexture;\n"
#endif
"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"
@@ -73,11 +77,14 @@ const std::string kFragmentShader =
} // namespace
void BackgroundRenderer::InitializeGlContent(GLuint textureId)
void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
{
texture_id_ = textureId;
oes_ = oes;
shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
shader_program_ = tango_gl::util::CreateProgram(
kVertexShader.c_str(),
oes_?kFragmentShaderOES.c_str():kFragmentShader.c_str());
if (!shader_program_) {
LOGE("Could not create program.");
}
@@ -95,11 +102,10 @@ void BackgroundRenderer::Draw(const float * transformed_uvs) {
glEnable (GL_BLEND);
glActiveTexture(GL_TEXTURE0);
#ifdef __ANDROID__
glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
#else
glBindTexture(GL_TEXTURE_2D, texture_id_);
#endif
if(oes_)
glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
else
glBindTexture(GL_TEXTURE_2D, texture_id_);
glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);

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

@@ -55,7 +55,7 @@ public:
// Sets up OpenGL state. Must be called on the OpenGL thread and before any
// other methods below.
void InitializeGlContent(GLuint textureId);
void InitializeGlContent(GLuint textureId, bool oes);
// Draws the background image. This methods must be called for every ArFrame
// returned by ArSession_update() to catch display geometry change events.
@@ -65,6 +65,7 @@ public:
GLuint shader_program_;
GLuint texture_id_;
bool oes_ = false;
GLuint attribute_vertices_;
GLuint attribute_uvs_;

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

@@ -869,11 +869,11 @@ 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)
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);
native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw, stamp);
}
else
{
@@ -886,30 +886,36 @@ JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
JNIEnv* env, jclass, jlong native_application,
float x, float y, float z, float qx, float qy, float qz, float qw,
float fx, float fy, float cx, float cy,
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
double stamp,
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)
jobject points, int pointsLen,
float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //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
{
if(native_application)
{
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,
rtabmap::Transform(x,y,z,qx,qy,qz,qw),
rgb_fx,rgb_fy,rgb_cx,rgb_cy,
0,0,0,0,
rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
rtabmap::Transform(),
stamp,
0,
yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
0,0,0,0,0,
0,0,0,0,0, //depth
0,0,0,0,0, //conf
pointsPtr, pointsLen, 4,
0,0,0,0,0,0,0,
0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0);
rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
p00, p11, p02, p12, p22, p32, p23,
t0, t1, t2, t3, t4, t5, t6, t7);
}
else
{
@@ -918,6 +924,44 @@ Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
}
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_postOdometryEventDepth(
JNIEnv* env, jclass, jlong native_application,
float x, float y, float z, float qx, float qy, float qz, float qw,
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
float depth_fx, float depth_fy, float depth_cx, float depth_cy,
float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
double rgbStamp,
double depthStamp,
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,
float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //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)
{
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(
rtabmap::Transform(x,y,z,qx,qy,qz,qw),
rgb_fx,rgb_fy,rgb_cx,rgb_cy,
depth_fx,depth_fy,depth_cx,depth_cy,
rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
rtabmap::Transform(depthFrameX, depthFrameY, depthFrameZ, depthFrameQX, depthFrameQY, depthFrameQZ, depthFrameQW),
rgbStamp,
depthStamp,
yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
0,0,0,0,0, // conf
pointsPtr, pointsLen, 4,
rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
p00, p11, p02, p12, p22, p32, p23,
t0, t1, t2, t3, t4, t5, t6, t7);
}
#ifdef __cplusplus