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

ios first release

Browse Source
This commit is contained in:
matlabbe
2021-06-08 00:06:49 -04:00
parent 630cb6a59f
commit 289d4afd76
88 changed files with 9260 additions and 473 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/util2d.h"
#include <glm/gtx/transform.hpp>
namespace rtabmap {
@@ -59,13 +60,22 @@ CameraMobile::CameraMobile(bool smoothing) :
stampEpochOffset_(0.0),
smoothing_(smoothing),
colorCameraToDisplayRotation_(ROTATION_0),
originUpdate_(false)
originUpdate_(false),
textureId_(9999),
uvs_initialized_(false)
{
glGenTextures(1, &textureId_);
}
CameraMobile::~CameraMobile() {
// Disconnect camera service
close();
if(textureId_ != 9999)
{
glDeleteTextures(1, &textureId_);
textureId_ = 9999;
}
}
bool CameraMobile::init(const std::string &, const std::string &)
@@ -124,11 +134,53 @@ void CameraMobile::setGPS(const GPS & gps)
lastKnownGPS_ = gps;
}
void CameraMobile::setData(const SensorData & data, const Transform & pose)
void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
{
LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
data_ = data;
pose_ = pose;
viewMatrix_ = viewMatrix;
projectionMatrix_ = projectionMatrix;
if(textureId_ == 9999)
{
glGenTextures(1, &textureId_);
}
if(texCoord)
{
memcpy(transformed_uvs_, texCoord, 8*sizeof(float));
uvs_initialized_ = true;
}
LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
if(textureId_ != 0)
{
cv::Mat rgbImage;
cv::cvtColor(data.imageRaw(), rgbImage, CV_BGR2RGBA);
glBindTexture(GL_TEXTURE_2D, textureId_);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
//glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
//glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
GLint error = glGetError();
if(error != GL_NO_ERROR)
{
LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
textureId_ = 0;
return;
}
}
}
void CameraMobile::addEnvSensor(int type, float value)
@@ -338,4 +390,60 @@ SensorData CameraMobile::captureImage(CameraInfo * info)
return data_;
}
LaserScan CameraMobile::scanFromPointCloudData(
const cv::Mat & pointCloudData,
int points,
const Transform & pose,
const CameraModel & model,
const cv::Mat & rgb,
std::vector<cv::KeyPoint> * kpts,
std::vector<cv::Point3f> * kpts3D,
int kptsSize)
{
if(!pointCloudData.empty())
{
cv::Mat scanData(1, pointCloudData.cols, CV_32FC4);
float * ptr = scanData.ptr<float>();
const float * inPtr = pointCloudData.ptr<float>();
int ic = pointCloudData.channels();
UASSERT(pointCloudData.depth() == CV_32F && ic >= 3);
int oi = 0;
for(unsigned int i=0;i<points; ++i)
{
cv::Point3f pt(inPtr[i*ic], inPtr[i*ic + 1], inPtr[i*ic + 2]);
pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
ptr[oi*4] = pt.x;
ptr[oi*4 + 1] = pt.y;
ptr[oi*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,kptsSize));
if(kpts3D)
kpts3D->push_back(org);
*(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
++oi;
}
//confidence
//*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
}
return LaserScan::backwardCompatibility(scanData.colRange(0, oi), 0, 10, rtabmap::Transform::getIdentity());
}
return LaserScan();
}
} /* namespace rtabmap */

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

@@ -75,6 +75,17 @@ public:
static const rtabmap::Transform opticalRotation;
static const rtabmap::Transform opticalRotationInv;
public:
static LaserScan scanFromPointCloudData(
const cv::Mat & 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,
int kptsSize = 3);
public:
CameraMobile(bool smoothing = false);
@@ -97,9 +108,17 @@ 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);
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_;}
void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
void setOcclusionImage(const cv::Mat & image, const CameraModel & model) {occlusionModel_ = model; occlusionImage_ = image;}
const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
@@ -113,6 +132,12 @@ protected:
Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
UTimer spinOnceFrameRateTimer_;
double spinOncePreviousStamp_;
GLuint textureId_;
glm::mat4 viewMatrix_;
glm::mat4 projectionMatrix_;
float transformed_uvs_[8];
bool uvs_initialized_ = false;
private:
Transform previousPose_;
@@ -128,6 +153,9 @@ private:
SensorData data_;
Transform pose_;
cv::Mat occlusionImage_;
CameraModel occlusionModel_;
};
} /* namespace rtabmap */

33
app/android/jni/ProgressionStatus.h
View File

@@ -11,7 +11,9 @@
#include <rtabmap/core/ProgressState.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
#ifdef __ANDROID__
#include <jni.h>
#endif
namespace rtabmap {
@@ -27,16 +29,29 @@ public:
class ProgressionStatus: public ProgressState, public UEventsHandler
{
public:
ProgressionStatus() : count_(0), max_(100), jvm_(0), rtabmap_(0)
ProgressionStatus() : count_(0), max_(100)
#ifdef __ANDROID__
, jvm_(0), rtabmap_(0)
#else
, swiftClassPtr_(0)
#endif
{
registerToEventsManager();
}
#ifdef __ANDROID__
void setJavaObjects(JavaVM * jvm, jobject rtabmap)
{
jvm_ = jvm;
rtabmap_ = rtabmap;
}
#else
void setSwiftCallback(void * classPtr, void(*callback)(void *, int, int))
{
swiftClassPtr_ = classPtr;
swiftCallback = callback;
}
#endif
void reset(int max)
{
@@ -82,6 +97,7 @@ protected:
count_ += ((ProgressEvent*)event)->count_;
// Call JAVA callback
bool success = false;
#ifdef __ANDROID__
if(jvm_ && rtabmap_)
{
JNIEnv *env = 0;
@@ -103,6 +119,16 @@ protected:
}
jvm_->DetachCurrentThread();
}
#else // APPLE
if(swiftClassPtr_)
{
std::function<void()> actualCallback = [&](){
swiftCallback(swiftClassPtr_, count_, max_);
};
actualCallback();
success = true;
}
#endif
if(!success)
{
UERROR("Failed to call rtabmap::updateProgressionCallback");
@@ -114,8 +140,13 @@ protected:
private:
int count_;
int max_;
#ifdef __ANDROID__
JavaVM *jvm_;
jobject rtabmap_;
#else
void * swiftClassPtr_;
void(*swiftCallback)(void *, int, int);
#endif
};
}

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

File diff suppressed because it is too large Load Diff

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

@@ -28,7 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef RTABMAP_APP_H_
#define RTABMAP_APP_H_
#ifdef __ANDROID__
#include <jni.h>
#endif
#include <memory>
#include <tango-gl/util.h>
@@ -49,7 +51,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
class RTABMapApp : public UEventsHandler {
public:
// Constructor and deconstructor.
#ifdef __ANDROID__
RTABMapApp(JNIEnv* env, jobject caller_activity);
#else // __APPLE__
RTABMapApp();
void setupSwiftCallbacks(void * classPtr,
void(*progressCallback)(void *, int, int),
void(*initCallback)(void *, int, const char*),
void(*statsUpdatedCallback)(void *,
int, int, int, int,
float,
int, int, int, int, int ,int,
float,
int,
float,
int,
float, float, float, float,
int, int,
float, float, float, float, float, float));
#endif
~RTABMapApp();
void setScreenRotation(int displayRotation, int cameraRotation);
@@ -57,8 +78,11 @@ 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;
#ifdef __ANDROID__
bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
#else // __APPLE__
bool startCamera();
#endif
// Allocate OpenGL resources for rendering, mainly for initializing the Scene.
void InitializeGLContent();
@@ -117,11 +141,13 @@ class RTABMapApp : public UEventsHandler {
void setMinCloudDepth(float value);
void setCloudDensityLevel(int value);
void setMeshAngleTolerance(float value);
void setMeshDecimationFactor(float value);
void setMeshTriangleSize(int value);
void setClusterRatio(float value);
void setMaxGainRadius(float value);
void setRenderingTextureDecimation(int value);
void setBackgroundColor(float gray);
void setDepthConfidence(int value);
int setMappingParameter(const std::string & key, const std::string & value);
void setGPS(const rtabmap::GPS & gps);
void addEnvSensor(int type, float value);
@@ -156,14 +182,19 @@ 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 * 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);
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
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
protected:
virtual bool handleEvent(UEvent * event);
private:
int updateMeshDecimation(int width, int height);
rtabmap::ParametersMap getRtabmapParameters();
bool smoothMesh(int id, rtabmap::Mesh & mesh);
void gainCompensation(bool full = false);
@@ -188,15 +219,18 @@ class RTABMapApp : public UEventsHandler {
bool cameraColor_;
bool fullResolution_;
bool appendMode_;
bool proximityDetection_;
float maxCloudDepth_;
float minCloudDepth_;
int cloudDensityLevel_;
int meshTrianglePix_;
float meshAngleToleranceDeg_;
float meshDecimationFactor_;
float clusterRatio_;
float maxGainRadius_;
int renderingTextureDecimation_;
float backgroundColor_;
int depthConfidence_;
rtabmap::ParametersMap mappingParameters_;
@@ -210,7 +244,6 @@ class RTABMapApp : public UEventsHandler {
bool bilateralFilteringOnNextRender_;
bool takeScreenshotOnNextRender_;
bool cameraJustInitialized_;
int meshDecimation_;
int totalPoints_;
int totalPolygons_;
int lastDrawnCloudsCount_;
@@ -251,6 +284,23 @@ class RTABMapApp : public UEventsHandler {
std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
rtabmap::ProgressionStatus progressionStatus_;
#ifndef __ANDROID__
void * swiftClassPtr_;
void(*swiftInitCallback)(void *, int, const char *);
void(*swiftStatsUpdatedCallback)(void *,
int, int, int, int,
float,
int, int, int, int, int ,int,
float,
int,
float,
int,
float, float, float, float,
int, int,
float, float, float, float, float, float);
#endif
};
#endif // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

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

@@ -35,18 +35,42 @@ const std::string kVertexShader =
"}\n";
const std::string kFragmentShader =
#ifdef __ANDROID__
"#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
"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"
" gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
"}\n";
/* To debug depth texture
const std::string kFragmentShader =
"precision mediump float;\n"
"varying vec2 v_TexCoord;\n"
"uniform sampler2D sTexture;\n"
"void main() {\n"
" float uNearZ = 0.2;\n"
" float uFarZ = 1000.0;\n"
" float depth = texture2D(sTexture, v_TexCoord).r;\n"
" float num = (2.0 * uNearZ * uFarZ);\n"
" float diff = (uFarZ - uNearZ);\n"
" float add = (uFarZ + uNearZ);\n"
" float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
" float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
" float grey = linearDepth/3.0;\n"
" gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
"}\n";
*/
} // namespace
void BackgroundRenderer::InitializeGlContent(GLuint textureId)
@@ -71,10 +95,14 @@ 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
glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
glEnableVertexAttribArray(attribute_vertices_);
glEnableVertexAttribArray(attribute_uvs_);

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

@@ -17,8 +17,13 @@
#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
#ifdef __ANDROID__
#include <GLES2/gl2.h>
#include <GLES2/gl2ext.h>
#else // __APPLE__
#include <OpenGLES/ES2/gl.h>
#include <OpenGLES/ES2/glext.h>
#endif
#include <cstdlib>
#include "util.h"
@@ -26,6 +31,16 @@
static const GLfloat BackgroundRenderer_kVertices[] = {
-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
};
static const GLfloat BackgroundRenderer_kTexCoord[] = {
1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
};
//android phone
//11 10 01 00 // portrait
//01 11 00 10 // left
//10 00 11 01 // right
//00 01 10 11 // down
// This class renders the passthrough camera image into the OpenGL frame.
class BackgroundRenderer {

4
app/android/jni/graph_drawable.cpp
View File

@@ -31,7 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/ULogger.h"
#include "util.h"
#ifdef __ANDROID__
#include <GLES2/gl2.h>
#else //__APPLE__
#include <OpenGLES/ES2/gl.h>
#endif
GraphDrawable::GraphDrawable(
GLuint shaderProgram,

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

@@ -28,8 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef GRAPH_DRAWABLE_H_
#define GRAPH_DRAWABLE_H_
#ifdef __ANDROID__
#include <jni.h>
#endif
#include <tango-gl/util.h>
#include <vector>
#include <pcl/point_cloud.h>

20
app/android/jni/point_cloud_drawable.cpp
View File

@@ -35,7 +35,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "util.h"
#include "pcl/common/transforms.h"
#ifdef __ANDROID__
#include <GLES2/gl2.h>
#else // __APPLE__
#include <OpenGLES/ES2/gl.h>
#endif
#define LOW_DEC 2
#define LOWLOW_DEC 4
@@ -257,7 +261,7 @@ const std::string kTextureMeshBlendingFragmentShader =
" float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
" if(linearFragz > linearDepth + 0.05)\n"
" alpha=0.0;\n"
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
"}\n";
std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
@@ -356,7 +360,7 @@ PointCloudDrawable::~PointCloudDrawable()
void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
{
LOGD("Update polygons");
//LOGD("Update polygons");
polygons_.clear();
polygonLines_.clear();
polygonsLowRes_.clear();
@@ -580,13 +584,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
int totalPoints = 0;
std::vector<pcl::Vertices> polygons = mesh.polygons;
std::vector<pcl::Vertices> polygonsLowRes;
hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
if(mesh.cloud->isOrganized()) // assume organized mesh
{
polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
totalPoints = mesh.indices->size();
totalPoints = (int)mesh.indices->size();
verticesLowRes_.resize(totalPoints);
verticesLowLowRes_.resize(totalPoints);
int oi_low = 0;
@@ -672,7 +676,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
}
else // assume dense mesh with texCoords set to polygons
{
if(textures_ && polygons.size() && mesh.normals->size())
if(textures_ && polygons.size())
{
//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
// (int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
@@ -681,14 +685,14 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
// tex_coordinates should be linked to points, not
// polygon vertices. Points linked to multiple different texCoords (different textures) should
// be duplicated.
totalPoints = mesh.texCoords.size();
vertices = std::vector<float>(mesh.texCoords.size()*9);
totalPoints = (int)mesh.texCoords.size();
int items = hasNormals_?9:6;
vertices = std::vector<float>(mesh.texCoords.size()*items);
organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
int items = hasNormals_?9:6;
unsigned int oi=0;
for(unsigned int i=0; i<polygons.size(); ++i)
{

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

@@ -28,7 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
#define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
#ifdef __ANDROID__
#include <jni.h>
#endif
#include <tango-gl/util.h>
#include <vector>

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

@@ -88,7 +88,7 @@ Scene::Scene() :
mapRendering_(true),
meshRendering_(true),
meshRenderingTexture_(true),
pointSize_(5.0f),
pointSize_(10.0f),
boundingBoxRendering_(false),
lighting_(false),
backfaceCulling_(true),
@@ -220,11 +220,14 @@ void Scene::SetupViewPort(int w, int h) {
if (h == 0) {
LOGE("Setup graphic height not valid");
}
UASSERT(gesture_camera_ != 0);
gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
glViewport(0, 0, w, h);
if(screenWidth_ != w || screenHeight_ != h || fboId_ == 0)
{
UINFO("Setup viewport OpenGL: %dx%d", w, h);
if(fboId_>0)
{
glDeleteFramebuffers(1, &fboId_);
@@ -235,6 +238,9 @@ void Scene::SetupViewPort(int w, int h) {
depthTexture_ = 0;
}
GLint originid = 0;
glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
// regenerate fbo texture
// create a framebuffer object, you need to delete them when program exits.
glGenFramebuffers(1, &fboId_);
@@ -260,12 +266,9 @@ void Scene::SetupViewPort(int w, int h) {
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
if ( status != GL_FRAMEBUFFER_COMPLETE)
{
LOGE("Frame buffer cannot be generated! Status: %in", status);
}
glBindFramebuffer(GL_FRAMEBUFFER,0);
}
UASSERT ( status == GL_FRAMEBUFFER_COMPLETE);
glBindFramebuffer(GL_FRAMEBUFFER, originid);
}
screenWidth_ = w;
screenHeight_ = h;
}
@@ -411,13 +414,21 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
bool renderBackgroundCamera =
background_renderer_ &&
gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
uvsTransformed;
gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
uvsTransformed;
if(renderBackgroundCamera)
{
projectionMatrix = arProjectionMatrix;
viewMatrix = arViewMatrix;
if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.2)
{
projectionMatrix = arProjectionMatrix;
viewMatrix = arViewMatrix;
}
else
{
renderBackgroundCamera = false;
}
}
rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
@@ -472,13 +483,16 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
if(onlineBlending && fboId_)
{
GLint originid = 0;
glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
// set the rendering destination to FBO
glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
glClearColor(0, 0, 0, 0);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
if(renderBackgroundCamera)
if(renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
{
PointCloudDrawable drawable(occlusionMesh);
drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f, 0, 0, 0, 0, 0, true);
@@ -488,13 +502,17 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
// Draw scene
for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
{
// set large distance to cam to use low res polygons for fast processing
(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f, 0, 0, 0, 0, 0, true);
Eigen::Vector3f cloudToCamera(
(*iter)->getPose().x() - openglCamera.x(),
(*iter)->getPose().y() - openglCamera.y(),
(*iter)->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
}
}
// back to normal window-system-provided framebuffer
glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
glBindFramebuffer(GL_FRAMEBUFFER, originid); // unbind
}
if(doubleTapOn_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -502,10 +520,16 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
glClearColor(0, 0, 0, 0);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
// FIXME: we could use the depthTexture if already computed!
for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
{
// set large distance to cam to use low res polygons for fast processing
(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, 999.0f, 0, 0, 0, 0, 0, true);
Eigen::Vector3f cloudToCamera(
(*iter)->getPose().x() - openglCamera.x(),
(*iter)->getPose().y() - openglCamera.y(),
(*iter)->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
}
GLubyte zValue[4];
@@ -680,18 +704,10 @@ void Scene::updateGraph(
const std::multimap<int, rtabmap::Link> & links)
{
LOGI("updateGraph");
if(graph_)
{
delete graph_;
graph_ = 0;
}
//create
if(graphVisible_)
{
UASSERT(graph_shader_program_ != 0);
graph_ = new GraphDrawable(graph_shader_program_, poses, links);
}
UASSERT(graph_shader_program_ != 0);
delete graph_;
graph_ = new GraphDrawable(graph_shader_program_, poses, links);
}
void Scene::setGraphVisible(bool visible)

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

@@ -17,7 +17,9 @@
#ifndef TANGO_POINT_CLOUD_SCENE_H_
#define TANGO_POINT_CLOUD_SCENE_H_
#ifdef __ANDROID__
#include <jni.h>
#endif
#include <memory>
#include <set>
@@ -35,10 +37,10 @@
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Link.h>
#include <point_cloud_drawable.h>
#include <graph_drawable.h>
#include <bounding_box_drawable.h>
#include <background_renderer.h>
#include "point_cloud_drawable.h"
#include "graph_drawable.h"
#include "bounding_box_drawable.h"
#include "background_renderer.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

4
app/android/jni/tango-gl/camera.cpp
View File

@@ -24,8 +24,8 @@ Camera::Camera() {
aspect_ratio_ = 4.0f / 3.0f;
width_ = 800.0f;
height_ = 600.0f;
near_clip_plane_ = 0.2f;
far_clip_plane_ = 1000.0f;
near_clip_plane_ = 0.5f;
far_clip_plane_ = 50.0f;
ortho_ = false;
orthoScale_ = 2.0f;
orthoCropFactor_ = -1.0f;

25
app/android/jni/tango-gl/gesture_camera.cpp
View File

@@ -43,9 +43,11 @@ const float kCamViewMaxDist = 100.f;
// FOV set up values.
// Third and top down camera's FOV is 65 degrees.
// First person camera's FOV is 45 degrees.
const float kHighFov = 65.0f;
// First person camera's FOV is 85 degrees.
const float kHighestFov = 120.0f;
const float kHighFov = 85.0f;
const float kLowFov = 65.0f;
const float kLowestFov = 40.0f;
}
namespace tango_gl {
@@ -111,7 +113,7 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
if(camera_type_ == kFirstPerson)
{
this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, kLowestFov, kHighestFov));
}
else
{
@@ -179,8 +181,9 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
camera_type_ = camera_index;
switch (camera_index) {
case kFirstPerson:
SetOrthoMode(false);
SetFieldOfView(kLowFov);
SetOrthoMode(false);
SetFieldOfView(kLowestFov);
SetNearFarClipPlanes(0.5, 50);
SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
cam_cur_dist_ = 0.0f;
@@ -193,11 +196,12 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
break;
case kThirdPerson:
case kThirdPersonFollow:
SetOrthoMode(false);
SetFieldOfView(kHighFov);
SetOrthoMode(false);
SetFieldOfView(kLowFov);
SetNearFarClipPlanes(0.5, 50);
SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
cam_cur_dist_ = camera_index==kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
cam_cur_angle_.x = -M_PI / 12.0f;
cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
@@ -208,7 +212,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
SetOrthoMode(false);
SetFieldOfView(kHighFov);
SetFieldOfView(kLowFov);
SetNearFarClipPlanes(0.5, 50);
cam_cur_dist_ = kTopDownCameraDist;
anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
cam_cur_angle_.x = -M_PI / 2.0f;
@@ -222,6 +227,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
SetOrthoMode(true);
SetOrthoScale(kTopDownCameraDist);
SetOrthoCropFactor(-1.0f);
SetFieldOfView(kLowFov);
SetNearFarClipPlanes(0.5, 50);
cam_cur_dist_ = kTopDownCameraDist;
anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
cam_cur_angle_.x = -M_PI / 2.0f;

7
app/android/jni/tango-gl/include/tango-gl/texture.h
View File

@@ -18,7 +18,10 @@
#define TANGO_GL_TEXTURE_H_
#include <errno.h>
#ifdef __ANDROID__
#include <png.h>
#endif
#include "tango-gl/util.h"
@@ -30,11 +33,15 @@ class Texture {
Texture& operator=(const Texture&) = delete;
~Texture();
#ifdef __ANDROID__
bool LoadFromPNG(const char* file_path);
#endif
GLuint GetTextureID() const;
private:
#ifdef __ANDROID__
png_uint_32 width_, height_;
#endif
int bit_depth_, color_type_;
char* byte_data_;
GLuint texture_id_;

19
app/android/jni/tango-gl/include/tango-gl/util.h
View File

@@ -21,10 +21,16 @@
#define GL_VERTEX_PROGRAM_POINT_SIZE 0x8642
#include <stdlib.h>
#ifdef __ANDROID__
#include <jni.h>
#include <android/log.h>
#include <GLES2/gl2.h>
#include <GLES2/gl2ext.h>
#else // __APPLE__
#include <OpenGLES/ES2/gl.h>
#include <OpenGLES/ES2/glext.h>
#include <syslog.h>
#endif
#include "glm/glm.hpp"
#include "glm/gtc/matrix_transform.hpp"
@@ -33,17 +39,28 @@
#include "glm/gtx/matrix_decompose.hpp"
#define LOG_TAG "rtabmap"
#ifdef DISABLE_LOG
#if defined(DISABLE_LOG)
#define LOGD(...) ;
#define LOGI(...) ;
#define LOGW(...) ;
#else
#ifdef __APPLE__
#define LOGD(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
#define LOGI(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
#define LOGW(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
#else
#define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
#define LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
#endif
#endif
#ifdef __APPLE__
#define LOGE(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
#define LOGF(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
#else
#define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
#define LOGF(...) __android_log_print(ANDROID_LOG_FATAL,LOG_TAG,__VA_ARGS__)
#endif
#ifndef M_PI
#define M_PI 3.1415926f

2
app/android/jni/tango-gl/quad.cpp
View File

@@ -57,7 +57,7 @@ Quad::Quad() {
Quad::~Quad() { glDeleteShader(shader_program_); }
void Quat::SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }
void Quad::SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }
void Quad::Render(const glm::mat4& projection_mat,
const glm::mat4& view_mat) const {

6
app/android/jni/tango-gl/texture.cpp
View File

@@ -34,11 +34,13 @@ static int RoundUpPowerOfTwo(int w) {
}
Texture::Texture(const char* file_path) {
#ifdef __ANDROID__
if (!LoadFromPNG(file_path)) {
LOGE("Texture initialing error");
}
#endif
}
#ifdef __ANDROID__
bool Texture::LoadFromPNG(const char* file_path) {
FILE* file = fopen(file_path, "rb");
@@ -94,7 +96,7 @@ bool Texture::LoadFromPNG(const char* file_path) {
return true;
}
#endif
GLuint Texture::GetTextureID() const { return texture_id_; }
Texture::~Texture() {

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

@@ -28,8 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef UTIL_H_
#define UTIL_H_
#include <android/log.h>
#include <rtabmap/utilite/UEventsHandler.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/CameraModel.h>
@@ -46,8 +44,13 @@ class LogHandler : public UEventsHandler
public:
LogHandler()
{
ULogger::setLevel(ULogger::kDebug);
ULogger::setEventLevel(ULogger::kDebug);
#ifdef DISABLE_LOG
ULogger::setLevel(ULogger::kWarning);
ULogger::setEventLevel(ULogger::kWarning);
#else
ULogger::setLevel(ULogger::kDebug);
ULogger::setEventLevel(ULogger::kDebug);
#endif
ULogger::setPrintThreadId(true);
registerToEventsManager();
@@ -55,7 +58,7 @@ public:
protected:
virtual bool handleEvent(UEvent * event)
{
if(event->getClassName().compare("ULogEvent") == 0)
if(event->getClassName().compare("ULogEvent") == 0)
{
ULogEvent * logEvent = (ULogEvent*)event;
if(logEvent->getCode() == ULogger::kDebug)
@@ -78,7 +81,6 @@ protected:
{
LOGF("%s", logEvent->getMsg().c_str());
}
}
return false;
}