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

LiDAR capture support in standalone library (#1264)

Browse Source 
* Working rtabmap_lidar-mapping example (live and pcap)

* finalizing merge, added some deprecated

* fixed build

* Working deskewing for Lidar + Camera/IMU (no camera pose correction yet) and Lidar + Odom Sensor in main UI.

* backward compatibility

* fixed some not used variable warnings, fixed qt build for lidar mapping example

* Refactored CameraMobile, added AREngine background support, fixed LidarVPL16 build error with PCL 1.8

* ARCoreJava: buffer last depth image in case its stamp i higher than pose stamp. CameraMobile: added pose buffer. SensorCaptureThread: to get pose, odomSensor should be explicitly set, but can be same as lidar or camera  inputs.

* Working external lidar on iOS

* util3d::commonFiltering()/adjustNormalsToViewPoint() added organized cloud support. MainWindow: updated odomSensor setup

* fixed winsock include order

* reverted camera tool

* disable imu filtering when odom sensor is used

* Updated package version

* fixed windows build

* fixing more windows build erros
This commit is contained in:
matlabbe
2024-04-14 19:06:04 -07:00
committed by GitHub
parent 6a6913c939
commit 700704bec9
131 changed files with 10585 additions and 7476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -334,11 +334,12 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR
}
}
SensorData CameraARCore::captureImage(CameraInfo * info)
SensorData CameraARCore::updateDataOnRender(Transform & pose)
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
pose.setNull();
SensorData data;
if(!arSession_)
{
@@ -370,7 +371,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
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,
BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
transformed_uvs_);
UASSERT(transformed_uvs_);
uvs_initialized_ = true;
@@ -393,7 +394,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArTrackingState camera_tracking_state;
ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
CameraModel model;
if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
{
@@ -401,24 +401,13 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
float pose_raw[7];
ArCamera_getPose(arSession_, ar_camera, arPose_);
ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
poseArCore = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
Transform poseArCore = pose;
if(pose.isNull())
if(poseArCore.isNull())
{
LOGE("CameraARCore: Pose is null");
}
else
{
this->poseReceived(pose);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
info->odomPose = pose;
}
// Get calibration parameters
float fx,fy, cx, cy;
@@ -551,6 +540,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
data.setFeatures(kpts, kpts3, cv::Mat());
if(!poseArCore.isNull())
{
pose = poseArCore;
this->poseReceived(pose, stamp);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
}
}
}
else
@@ -571,134 +571,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArCamera_release(ar_camera);
return data;
}
void CameraARCore::capturePoseOnly()
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
if(!arSession_)
{
return;
}
if(textureId_ != 0)
{
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::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;
}
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_));
// adjust origin
if(!getOriginOffset().isNull())
{
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
ArTrackingState camera_tracking_state;
ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
CameraModel model;
if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
{
// pose in OpenGL coordinates
float pose_raw[7];
ArCamera_getPose(arSession_, ar_camera, arPose_);
ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
if(!pose.isNull())
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose);
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
}
int32_t is_depth_supported = 0;
ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
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);
cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
float fx,fy, cx, cy;
int32_t rgb_width, rgb_height;
ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
float scaleX = (float)width / (float)rgb_width;
float scaleY = (float)height / (float)rgb_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);
}
}
ArCamera_release(ar_camera);
}
} /* namespace rtabmap */

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

@@ -63,23 +63,14 @@ public:
CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, 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 void close(); // close ARCore connection
virtual std::string getSerial() const;
GLuint getTextureId() const {return textureId_;}
void imageCallback(AImageReader *reader);
protected:
virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
virtual void capturePoseOnly();
virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread
private:
rtabmap::Transform getPoseAtTimestamp(double timestamp);

146
app/android/jni/CameraAREngine.cpp
View File

@@ -117,9 +117,6 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri
deviceTColorCamera_ = opticalRotation;
// Required as ArSession_update does some off-screen OpenGL stuff...
HwArSession_setCameraTextureName(arSession_, textureId_);
if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
{
UERROR("Cannot resume camera!");
@@ -169,38 +166,87 @@ void CameraAREngine::close()
CameraMobile::close();
}
SensorData CameraAREngine::captureImage(CameraInfo * info)
void CameraAREngine::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;
}
HwArSession_setDisplayGeometry(arSession_, ret, width, height);
}
}
SensorData CameraAREngine::updateDataOnRender(Transform & pose)
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
pose.setNull();
SensorData data;
if(!arSession_)
{
return data;
}
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);
}
if(textureId_!=0)
HwArSession_setCameraTextureName(arSession_, textureId_);
// Update session to get current frame and render camera background.
if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
LOGE("CameraAREngine::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;
HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
if (geometry_changed != 0 || !uvs_initialized_) {
HwArFrame_transformDisplayUvCoords(
arSession_, arFrame_,
BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView,
transformed_uvs_);
UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)",
transformed_uvs_[0], transformed_uvs_[1],
transformed_uvs_[2], transformed_uvs_[3],
transformed_uvs_[4], transformed_uvs_[5],
transformed_uvs_[6], transformed_uvs_[7]);
UASSERT(transformed_uvs_);
uvs_initialized_ = true;
}
HwArCamera* ar_camera;
HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
HwArCamera_getProjectionMatrix(arSession_, ar_camera,
/*near=*/0.1f, /*far=*/100.f,
glm::value_ptr(projectionMatrix_));
// adjust origin
if(!getOriginOffset().isNull())
{
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
HwArTrackingState camera_tracking_state;
HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
{
// pose in OpenGL coordinates
float pose_raw[7];
HwArCamera_getPose(arSession_, ar_camera, arPose_);
HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
// Get calibration parameters
// FIXME: Hard-coded as getting intrinsics with the api fails
float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
@@ -274,6 +320,26 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
double stamp = double(timestamp_ns)/10e8;
CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
data = SensorData(outputRGB, outputDepth, model, 0, stamp);
// pose in OpenGL coordinates
float pose_raw[7];
HwArCamera_getPose(arSession_, ar_camera, arPose_);
HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
if(pose.isNull())
{
LOGE("CameraAREngine: Pose is null");
}
else
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose, stamp);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
}
}
}
else
@@ -291,66 +357,8 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
}
HwArCamera_release(ar_camera);
if(pose.isNull())
{
LOGE("CameraAREngine: Pose is null");
}
else
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
info->odomPose = pose;
}
return data;
}
void CameraAREngine::capturePoseOnly()
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
SensorData data;
if(!arSession_)
{
return;
}
// Update session to get current frame and render camera background.
if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
LOGE("CameraARCore::captureImage() ArSession_update error");
return;
}
HwArCamera* ar_camera;
HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
HwArTrackingState camera_tracking_state;
HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
CameraModel model;
if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
{
// pose in OpenGL coordinates
float pose_raw[7];
HwArCamera_getPose(arSession_, ar_camera, arPose_);
HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
if(!pose.isNull())
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose);
}
}
HwArCamera_release(ar_camera);
}
} /* namespace rtabmap */

9
app/android/jni/CameraAREngine.h
View File

@@ -38,6 +38,7 @@ 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 <huawei_arengine_interface.h>
@@ -48,13 +49,14 @@ public:
CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
virtual ~CameraAREngine();
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual void close(); // close Tango connection
virtual void close(); // close AREngine connection
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual void capturePoseOnly();
virtual SensorData updateDataOnRender(Transform & pose);
private:
rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -69,7 +71,6 @@ private:
HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
HwArPose * arPose_ = nullptr;
bool arInstallRequested_;
GLuint textureId_;
UMutex arSessionMutex_;
};

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

@@ -55,10 +55,8 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
CameraMobile::CameraMobile(bool smoothing) :
Camera(10),
deviceTColorCamera_(Transform::getIdentity()),
spinOncePreviousStamp_(0.0),
textureId_(0),
uvs_initialized_(false),
previousStamp_(0.0),
stampEpochOffset_(0.0),
smoothing_(smoothing),
colorCameraToDisplayRotation_(ROTATION_0),
@@ -79,13 +77,12 @@ bool CameraMobile::init(const std::string &, const std::string &)
void CameraMobile::close()
{
previousPose_.setNull();
previousStamp_ = 0.0;
firstFrame_ = true;
lastKnownGPS_ = GPS();
lastEnvSensors_.clear();
originOffset_ = Transform();
originUpdate_ = false;
pose_ = Transform();
dataPose_ = Transform();
data_ = SensorData();
if(textureId_ != 0)
@@ -97,35 +94,107 @@ void CameraMobile::close()
void CameraMobile::resetOrigin()
{
previousPose_.setNull();
previousStamp_ = 0.0;
firstFrame_ = true;
lastKnownGPS_ = GPS();
lastEnvSensors_.clear();
pose_ = Transform();
dataPose_ = Transform();
data_ = SensorData();
originUpdate_ = true;
}
void CameraMobile::poseReceived(const Transform & pose)
bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
{
pose.setNull();
int maxWaitTimeMs = maxWaitTime * 1000;
// Interpolate pose
if(!poseBuffer_.empty())
{
poseMutex_.lock();
int waitTry = 0;
while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
{
poseMutex_.unlock();
++waitTry;
uSleep(1);
poseMutex_.lock();
}
if(poseBuffer_.rbegin()->first < stamp)
{
if(maxWaitTimeMs > 0)
{
UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
}
else
{
UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first);
}
}
else
{
std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
std::map<double, Transform>::const_iterator iterA = iterB;
if(iterA != poseBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == poseBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
pose = iterA->second;
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
}
else // stamp < iterA->first
{
UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
}
}
poseMutex_.unlock();
}
return !pose.isNull();
}
void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
{
if(!pose.isNull())
{
// send pose of the camera (without optical rotation)
Transform p = pose*deviceTColorCamera_;
Transform p = pose;
if(originUpdate_)
{
originOffset_ = p.translation().inverse();
originUpdate_ = false;
}
if(stampEpochOffset_ == 0.0)
{
stampEpochOffset_ = UTimer::now() - deviceStamp;
}
double epochStamp = stampEpochOffset_ + deviceStamp;
if(!originOffset_.isNull())
{
this->post(new PoseEvent(originOffset_*p));
p = originOffset_*p;
}
else
{
this->post(new PoseEvent(p));
UScopeMutex lock(poseMutex_);
poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p));
if(poseBuffer_.size() > 1000)
{
poseBuffer_.erase(poseBuffer_.begin());
}
}
// send pose of the camera (with optical rotation)
this->post(new PoseEvent(p * deviceTColorCamera_));
}
}
@@ -139,11 +208,20 @@ void CameraMobile::setGPS(const GPS & gps)
lastKnownGPS_ = gps;
}
void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
void CameraMobile::addEnvSensor(int type, float value)
{
LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
}
void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
{
UScopeMutex lock(dataMutex_);
bool notify = !data_.isValid();
LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
data_ = data;
pose_ = pose;
dataPose_ = pose;
viewMatrix_ = viewMatrix;
projectionMatrix_ = projectionMatrix;
@@ -151,7 +229,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
// adjust origin
if(!originOffset_.isNull())
{
pose_ = originOffset_ * pose_;
dataPose_ = originOffset_ * dataPose_;
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
@@ -166,7 +244,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
uvs_initialized_ = true;
}
LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
LOGD("CameraMobile::update textureId_=%d", (int)textureId_);
if(textureId_ != 0 && texCoord != 0)
{
@@ -193,78 +271,63 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
return;
}
}
}
void CameraMobile::addEnvSensor(int type, float value)
{
lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
}
void CameraMobile::spinOnce()
{
if(!this->isRunning())
postUpdate();
if(notify)
{
bool ignoreFrame = false;
//float rate = 10.0f; // maximum 10 FPS for image data
double now = UTimer::now();
/*if(rate>0.0f)
{
if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
{
ignoreFrame = true;
}
}*/
dataReady_.release();
}
}
if(!ignoreFrame)
void CameraMobile::updateOnRender()
{
UScopeMutex lock(dataMutex_);
bool notify = !data_.isValid();
data_ = updateDataOnRender(dataPose_);
if(data_.isValid())
{
postUpdate();
if(notify)
{
spinOnceFrameRateTimer_.start();
spinOncePreviousStamp_ = now;
mainLoop();
}
else
{
// just send pose
capturePoseOnly();
dataReady_.release();
}
}
}
void CameraMobile::mainLoopBegin()
SensorData CameraMobile::updateDataOnRender(Transform & pose)
{
double t = cameraStartedTime_.elapsed();
if(t < 5.0)
{
uSleep((5.0-t)*1000); // just to make sure that the camera is started
}
LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() "
"should be overridden by inherited classes. Returning empty data!\n");
return SensorData();
}
void CameraMobile::mainLoop()
void CameraMobile::postUpdate()
{
CameraInfo info;
SensorData data = this->captureImage(&info);
if(data.isValid() && !info.odomPose.isNull())
if(data_.isValid())
{
if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
{
data.setGPS(lastKnownGPS_);
data_.setGPS(lastKnownGPS_);
}
else if(lastKnownGPS_.stamp()>0.0)
{
LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
}
if(lastEnvSensors_.size())
{
data.setEnvSensors(lastEnvSensors_);
data_.setEnvSensors(lastEnvSensors_);
lastEnvSensors_.clear();
}
if(smoothing_ && !data.depthRaw().empty())
if(smoothing_ && !data_.depthRaw().empty())
{
//UTimer t;
data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
//LOGD("Bilateral filtering, time=%fs", t.ticks());
}
@@ -273,15 +336,15 @@ void CameraMobile::mainLoop()
{
UDEBUG("ROTATION_90");
cv::Mat rgb, depth;
cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
cv::flip(data.imageRaw(),rgb,1);
cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
cv::flip(data_.imageRaw(),rgb,1);
cv::transpose(rgb,rgbt);
rgb = rgbt;
cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
cv::flip(data.depthRaw(),depth,1);
cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
cv::flip(data_.depthRaw(),depth,1);
cv::transpose(depth,deptht);
depth = deptht;
CameraModel model = data.cameraModels()[0];
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
@@ -290,25 +353,25 @@ void CameraMobile::mainLoop()
model.cx()>0?model.imageWidth()-model.cx():0,
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data.setRGBDImage(rgb, depth, model);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data.keypoints();
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = data.keypoints()[i].pt.y;
keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
keypoints[i].pt.x = data_.keypoints()[i].pt.y;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
}
data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_180)
{
UDEBUG("ROTATION_180");
cv::Mat rgb, depth;
cv::flip(data.imageRaw(),rgb,1);
cv::flip(data_.imageRaw(),rgb,1);
cv::flip(rgb,rgb,0);
cv::flip(data.depthOrRightRaw(),depth,1);
cv::flip(data_.depthOrRightRaw(),depth,1);
cv::flip(depth,depth,0);
CameraModel model = data.cameraModels()[0];
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageWidth(), model.imageHeight());
model = CameraModel(
model.fx(),
@@ -317,26 +380,26 @@ void CameraMobile::mainLoop()
model.cy()>0?model.imageHeight()-model.cy():0,
model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
model.setImageSize(sizet);
data.setRGBDImage(rgb, depth, model);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data.keypoints();
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
}
data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_270)
{
UDEBUG("ROTATION_270");
cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
cv::transpose(data.imageRaw(),rgb);
cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
cv::transpose(data_.imageRaw(),rgb);
cv::flip(rgb,rgb,1);
cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
cv::transpose(data.depthOrRightRaw(),depth);
cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
cv::transpose(data_.depthOrRightRaw(),depth);
cv::flip(depth,depth,1);
CameraModel model = data.cameraModels()[0];
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
@@ -345,61 +408,54 @@ void CameraMobile::mainLoop()
model.cx(),
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data.setRGBDImage(rgb, depth, model);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data.keypoints();
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
keypoints[i].pt.y = data.keypoints()[i].pt.x;
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
keypoints[i].pt.y = data_.keypoints()[i].pt.x;
}
data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
rtabmap::Transform pose = info.odomPose;
data.setGroundTruth(Transform());
// convert stamp to epoch
bool firstFrame = previousPose_.isNull();
if(firstFrame)
{
stampEpochOffset_ = UTimer::now()-data.stamp();
}
data.setStamp(stampEpochOffset_ + data.stamp());
OdometryInfo info;
if(!firstFrame)
{
info.interval = data.stamp()-previousStamp_;
info.transform = previousPose_.inverse() * pose;
}
// linear cov = 0.0001
info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
if(!firstFrame)
{
// angular cov = 0.000001
info.reg.covariance.at<double>(3,3) *= 0.01;
info.reg.covariance.at<double>(4,4) *= 0.01;
info.reg.covariance.at<double>(5,5) *= 0.01;
}
LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
this->post(new OdometryEvent(data, pose, info));
previousPose_ = pose;
previousStamp_ = data.stamp();
}
else if(!this->isKilled() && info.odomPose.isNull())
{
LOGW("Odometry lost");
this->post(new OdometryEvent());
}
}
SensorData CameraMobile::captureImage(CameraInfo * info)
SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
{
if(info)
SensorData data;
if(dataReady_.acquire(1, 5000))
{
info->odomPose = pose_;
UScopeMutex lock(dataMutex_);
data = data_;
data_ = SensorData();
}
return data_;
if(data.isValid())
{
data.setGroundTruth(Transform());
data.setStamp(stampEpochOffset_ + data.stamp());
if(info)
{
// linear cov = 0.0001
info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001);
if(!firstFrame_)
{
// angular cov = 0.000001
info->odomCovariance.at<double>(3,3) *= 0.01;
info->odomCovariance.at<double>(4,4) *= 0.01;
info->odomCovariance.at<double>(5,5) *= 0.01;
}
info->odomPose = dataPose_;
}
firstFrame_ = false;
}
else
{
UWARN("CameraMobile::captureImage() invalid data!");
}
return data;
}
LaserScan CameraMobile::scanFromPointCloudData(

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

@@ -68,7 +68,7 @@ private:
Transform pose_;
};
class CameraMobile : public Camera, public UThread, public UEventsSender {
class CameraMobile : public Camera, public UEventsSender {
public:
static const float bilateralFilteringSigmaS;
static const float bilateralFilteringSigmaR;
@@ -93,14 +93,20 @@ public:
// abstract functions
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual void close(); // inherited classes should call its parent in their close().
virtual void close(); // inherited classes should call its parent at the end of their close().
virtual std::string getSerial() const {return "CameraMobile";}
void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
void updateOnRender();
const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
void resetOrigin();
virtual bool isCalibrated() const;
void poseReceived(const Transform & pose); // in rtabmap frame
virtual bool odomProvided() const { return true; }
virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time
void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
double getStampEpochOffset() const {return stampEpochOffset_;}
const CameraModel & getCameraModel() const {return model_;}
const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
@@ -108,10 +114,7 @@ public:
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, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
GLuint getTextureId() {return textureId_;}
bool uvsInitialized() const {return uvs_initialized_;}
const float* uvsTransformed() const {return transformed_uvs_;}
@@ -122,17 +125,15 @@ public:
const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual void capturePoseOnly() {}
virtual SensorData updateDataOnRender(Transform & pose);
virtual void mainLoopBegin();
virtual void mainLoop();
private:
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
void postUpdate(); // Should be called while being protected by dataMutex_
protected:
CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
UTimer spinOnceFrameRateTimer_;
double spinOncePreviousStamp_;
GLuint textureId_;
glm::mat4 viewMatrix_;
@@ -141,9 +142,7 @@ protected:
bool uvs_initialized_ = false;
private:
Transform previousPose_;
double previousStamp_;
UTimer cameraStartedTime_;
bool firstFrame_;
double stampEpochOffset_;
bool smoothing_;
ScreenRotation colorCameraToDisplayRotation_;
@@ -152,8 +151,13 @@ private:
Transform originOffset_;
bool originUpdate_;
USemaphore dataReady_;
UMutex dataMutex_;
SensorData data_;
Transform pose_;
Transform dataPose_;
UMutex poseMutex_;
std::map<double, Transform> poseBuffer_; // <stamp, Pose>
cv::Mat occlusionImage_;
CameraModel occlusionModel_;

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

@@ -101,7 +101,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
if(pose->status_code == TANGO_POSE_VALID)
{
CameraTango* app = static_cast<CameraTango*>(context);
app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp);
}
}
@@ -444,7 +444,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
UASSERT(cloud.type() == CV_32FC4);
boost::mutex::scoped_lock lock(dataMutex_);
boost::mutex::scoped_lock lock(tangoDataMutex_);
// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
// "In the current version of Project Tango Tablet RGB IR camera
@@ -463,7 +463,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
if(dt >= 0.0 && dt < 0.5)
{
bool notify = !data_.isValid();
bool notify = !tangoData_.isValid();
cv::Mat tangoImage = tangoColor_;
cv::Mat rgb;
@@ -495,7 +495,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
else
{
LOGE("Not supported color format : %d.", tangoColorType);
data_ = SensorData();
tangoData_ = SensorData();
return;
}
@@ -678,24 +678,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
if(rawScanPublished_)
{
data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
else
{
data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
data_.setGroundTruth(odom);
tangoData_.setGroundTruth(odom);
}
else
{
LOGE("Could not get depth and rgb images!?!");
data_ = SensorData();
tangoData_ = SensorData();
return;
}
if(notify)
{
dataReady_.release();
tangoDataReady_.release();
}
LOGD("process cloud received %fs", timer.ticks());
}
@@ -709,7 +709,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
{
//LOGD("RGB received! %fs", timestamp);
boost::mutex::scoped_lock lock(dataMutex_);
boost::mutex::scoped_lock lock(tangoDataMutex_);
tangoColor_ = tangoImage.clone();
tangoColorStamp_ = timestamp;
@@ -775,10 +775,11 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
return pose;
}
SensorData CameraTango::captureImage(CameraInfo * info)
SensorData CameraTango::updateDataOnRender(Transform & pose)
{
//LOGI("Capturing image...");
pose.setNull();
if(textureId_ == 0)
{
glGenTextures(1, &textureId_);
@@ -797,10 +798,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
if (status == TANGO_SUCCESS)
{
if(info)
{
info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
}
pose = getPoseAtTimestamp(video_overlay_timestamp);
int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
if (rotation > 3) {
@@ -876,16 +874,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
}
SensorData data;
if(dataReady_.acquireTry(1))
if(tangoDataReady_.acquireTry(1))
{
boost::mutex::scoped_lock lock(dataMutex_);
data = data_;
data_ = SensorData();
if(info)
{
info->odomPose = data.groundTruth();
data.setGroundTruth(Transform());
}
boost::mutex::scoped_lock lock(tangoDataMutex_);
data = tangoData_;
tangoData_ = SensorData();
pose = data.groundTruth();
data.setGroundTruth(Transform());
}
return data;

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

@@ -52,7 +52,6 @@ public:
virtual void close(); // close Tango connection
virtual std::string getSerial() const;
rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
void setDecimation(int value) {decimation_ = value;}
void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
@@ -61,7 +60,7 @@ public:
void tangoEventReceived(int type, const char * key, const char * value);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData updateDataOnRender(Transform & pose);
private:
rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -71,12 +70,12 @@ private:
bool colorCamera_;
int decimation_;
bool rawScanPublished_;
SensorData data_;
SensorData tangoData_;
cv::Mat tangoColor_;
int tangoColorType_;
double tangoColorStamp_;
boost::mutex dataMutex_;
USemaphore dataReady_;
boost::mutex tangoDataMutex_;
USemaphore tangoDataReady_;
cv::Mat fisheyeRectifyMapX_;
cv::Mat fisheyeRectifyMapY_;
};

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

@@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/GainCompensator.h>
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Recovery.h>
#include <rtabmap/core/lidar/LidarVLP16.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>
@@ -202,6 +203,7 @@ RTABMapApp::RTABMapApp() :
#endif
cameraDriver_(0),
camera_(0),
sensorCaptureThread_(0),
rtabmapThread_(0),
rtabmap_(0),
logHandler_(0),
@@ -216,6 +218,7 @@ RTABMapApp::RTABMapApp() :
cameraColor_(true),
fullResolution_(false),
appendMode_(true),
useExternalLidar_(false),
maxCloudDepth_(2.5),
minCloudDepth_(0.0),
cloudDensityLevel_(1),
@@ -537,7 +540,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
@@ -885,7 +888,7 @@ bool RTABMapApp::startCamera()
#endif
LOGW("startCamera() camera driver=%d", cameraDriver_);
boost::mutex::scoped_lock lock(cameraMutex_);
if(cameraDriver_ == 0) // Tango
{
#ifdef RTABMAP_TANGO
@@ -937,6 +940,19 @@ bool RTABMapApp::startCamera()
LOGI("Start camera thread");
cameraJustInitialized_ = true;
if(useExternalLidar_)
{
rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
lidar->init();
camera_->setImageRate(0); // if lidar, to get close camera synchronization
sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true);
}
else
{
sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_);
}
sensorCaptureThread_->start();
return true;
}
UERROR("Failed camera initialization!");
@@ -948,13 +964,12 @@ void RTABMapApp::stopCamera()
LOGI("stopCamera()");
{
boost::mutex::scoped_lock lock(cameraMutex_);
if(camera_!=0)
if(sensorCaptureThread_!=0)
{
camera_->join(true);
camera_->close();
delete camera_;
sensorCaptureThread_->join(true);
delete sensorCaptureThread_; // camera_ is closed and deleted inside
sensorCaptureThread_ = 0;
camera_ = 0;
poseBuffer_.clear();
}
}
{
@@ -1241,7 +1256,7 @@ int RTABMapApp::Render()
std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
try
{
if(camera_ == 0)
if(sensorCaptureThread_ == 0)
{
// We are not doing continous drawing, just measure single draw
fpsTime_.restart();
@@ -1272,49 +1287,45 @@ int RTABMapApp::Render()
{
if(cameraDriver_ <= 2)
{
camera_->spinOnce();
camera_->updateOnRender();
}
#ifdef DEBUG_RENDERING_PERFORMANCE
LOGW("Camera spinOnce %fs", time.ticks());
LOGW("Camera updateOnRender %fs", time.ticks());
#endif
if(cameraDriver_ != 2)
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
{
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
}
if(camera_->uvsInitialized())
{
uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
if(graphOptimization_ && !mapToOdom_.isIdentity())
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
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(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(!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());
}
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());
}
}
#ifdef DEBUG_RENDERING_PERFORMANCE
@@ -1334,14 +1345,14 @@ int RTABMapApp::Render()
}
}
rtabmap::OdometryEvent odomEvent;
rtabmap::SensorEvent sensorEvent;
{
boost::mutex::scoped_lock lock(odomMutex_);
if(odomEvents_.size())
boost::mutex::scoped_lock lock(sensorMutex_);
if(sensorEvents_.size())
{
LOGI("Process odom events");
odomEvent = odomEvents_.back();
odomEvents_.clear();
LOGI("Process sensor events");
sensorEvent = sensorEvents_.back();
sensorEvents_.clear();
if(cameraJustInitialized_)
{
notifyCameraStarted = true;
@@ -1361,7 +1372,7 @@ int RTABMapApp::Render()
{
main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl);
}
if(camera_!=0 && cameraJustInitialized_)
if(sensorCaptureThread_!=0 && cameraJustInitialized_)
{
notifyCameraStarted = true;
cameraJustInitialized_ = false;
@@ -1562,9 +1573,9 @@ int RTABMapApp::Render()
if(clearSceneOnNextRender_)
{
LOGI("Clearing all rendering data...");
odomMutex_.lock();
odomEvents_.clear();
odomMutex_.unlock();
sensorMutex_.lock();
sensorEvents_.clear();
sensorMutex_.unlock();
poseMutex_.lock();
poseEvents_.clear();
@@ -1800,7 +1811,7 @@ int RTABMapApp::Render()
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -2004,26 +2015,26 @@ int RTABMapApp::Render()
}
else
{
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && camera_!=0);
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0);
//just process the last one
if(!odomEvent.pose().isNull())
if(!sensorEvent.info().odomPose.isNull())
{
if(odomCloudShown_ && !trajectoryMode_)
{
if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) || !odomEvent.data().laserScanRaw().isEmpty())
if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()))
if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
}
else
{
//scan
cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
indices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
@@ -2034,10 +2045,10 @@ int RTABMapApp::Render()
if(cloud->size() && indices->size())
{
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows,
sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose);
main_scene_.setCloudVisible(-1, true);
}
else
@@ -2127,7 +2138,7 @@ int RTABMapApp::Render()
lastPostRenderEventTime_ = UTimer::now();
if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
{
UERROR("TangoPoseEventNotReceived");
UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
@@ -2319,7 +2330,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
void RTABMapApp::setGraphOptimization(bool enabled)
{
graphOptimization_ = enabled;
if((camera_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
{
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
@@ -3709,19 +3720,12 @@ void RTABMapApp::postCameraPoseEvent(
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);
poseBuffer_.insert(std::make_pair(stamp, pose));
if(poseBuffer_.size() > 1000)
{
poseBuffer_.erase(poseBuffer_.begin());
}
camera_->poseReceived(pose, stamp);
}
}
@@ -3833,66 +3837,41 @@ void RTABMapApp::postOdometryEvent(
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
rtabmap::Transform poseWithOriginOffset = pose;
if(!camera_->getOriginOffset().isNull())
{
poseWithOriginOffset = camera_->getOriginOffset() * pose;
}
// 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())
if(depthStamp != stamp)
{
// Interpolate pose
if(!poseBuffer_.empty())
rtabmap::Transform poseDepth;
cv::Mat cov;
if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
{
UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
}
else
{
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);
UDEBUG("poseRGB =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp);
UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
#endif
motion = pose.inverse()*poseDepth;
// transform in camera frame
motion = poseWithOriginOffset.inverse()*poseDepth;
// transform in camera frame
#ifndef DISABLE_LOG
UDEBUG("motion=%s", motion.prettyPrint().c_str());
UDEBUG("motion=%s", motion.prettyPrint().c_str());
#endif
motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
#ifndef DISABLE_LOG
UDEBUG("motion=%s", motion.prettyPrint().c_str());
UDEBUG("motion=%s", motion.prettyPrint().c_str());
#endif
}
}
}
}
rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
@@ -3941,11 +3920,6 @@ void RTABMapApp::postOdometryEvent(
if(!outputDepth.empty())
{
rtabmap::Transform poseWithOriginOffset = pose;
if(!camera_->getOriginOffset().isNull())
{
poseWithOriginOffset = camera_->getOriginOffset() * pose;
}
rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
camera_->setOcclusionImage(outputDepth, depthModel);
@@ -3971,8 +3945,7 @@ void RTABMapApp::postOdometryEvent(
texCoords[5] = t5;
texCoords[6] = t6;
texCoords[7] = t7;
camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
camera_->spinOnce();
camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
}
}
}
@@ -3989,17 +3962,17 @@ void RTABMapApp::postOdometryEvent(
bool RTABMapApp::handleEvent(UEvent * event)
{
if(camera_!=0)
if(sensorCaptureThread_!=0)
{
// called from events manager thread, so protect the data
if(event->getClassName().compare("OdometryEvent") == 0)
if(event->getClassName().compare("SensorEvent") == 0)
{
LOGI("Received OdometryEvent!");
if(odomMutex_.try_lock())
LOGI("Received SensorEvent!");
if(sensorMutex_.try_lock())
{
odomEvents_.clear();
odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
odomMutex_.unlock();
sensorEvents_.clear();
sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event)));
sensorMutex_.unlock();
}
}
if(event->getClassName().compare("RtabmapEvent") == 0)

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

@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "util.h"
#include "ProgressionStatus.h"
#include <rtabmap/core/SensorCaptureThread.h>
#include <rtabmap/core/RtabmapThread.h>
#include <rtabmap/core/SensorEvent.h>
#include <rtabmap/utilite/UEventsHandler.h>
#include <boost/thread/mutex.hpp>
#include <pcl/pcl_base.h>
@@ -209,6 +211,7 @@ class RTABMapApp : public UEventsHandler {
private:
int cameraDriver_;
rtabmap::CameraMobile * camera_;
rtabmap::SensorCaptureThread * sensorCaptureThread_;
rtabmap::RtabmapThread * rtabmapThread_;
rtabmap::Rtabmap * rtabmap_;
rtabmap::LogHandler * logHandler_;
@@ -224,6 +227,7 @@ class RTABMapApp : public UEventsHandler {
bool cameraColor_;
bool fullResolution_;
bool appendMode_;
bool useExternalLidar_;
float maxCloudDepth_;
float minCloudDepth_;
int cloudDensityLevel_;
@@ -270,16 +274,15 @@ class RTABMapApp : public UEventsHandler {
UTimer fpsTime_;
std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
std::list<rtabmap::OdometryEvent> odomEvents_;
std::list<rtabmap::SensorEvent> sensorEvents_;
std::list<rtabmap::Transform> poseEvents_;
std::map<double, rtabmap::Transform> poseBuffer_;
rtabmap::Transform mapToOdom_;
boost::mutex cameraMutex_;
boost::mutex rtabmapMutex_;
boost::mutex meshesMutex_;
boost::mutex odomMutex_;
boost::mutex sensorMutex_;
boost::mutex poseMutex_;
boost::mutex renderingMutex_;

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

@@ -155,7 +155,7 @@ void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
}
void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) {
static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
static_assert(std::extent<decltype(BackgroundRenderer_kVerticesDevice)>::value == kNumVertices * 2, "Incorrect kVertices length");
GLuint program = shaderPrograms_[depthTexture>0?1:0];
@@ -170,7 +170,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
else
#endif
glBindTexture(GL_TEXTURE_2D, texture_id_);
if(depthTexture>0)
{
// Texture activate unit 1
@@ -191,7 +191,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice);
glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
glEnableVertexAttribArray(attributeVertices);

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

@@ -28,9 +28,15 @@
#include "util.h"
static const GLfloat BackgroundRenderer_kVertices[] = {
static const GLfloat BackgroundRenderer_kVerticesDevice[] = {
-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
};
//static const GLfloat BackgroundRenderer_kVerticesView[] = {
// 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f,
//};
static const GLfloat BackgroundRenderer_kVerticesView[] = {
0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
};
static const GLfloat BackgroundRenderer_kTexCoord[] = {
1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
};