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

Tango: refactored shaders (to remove most "if" in them), added "Auto" to reconstruction depth option

Browse Source
This commit is contained in:
matlabbe
2017-04-18 17:30:14 -04:00
parent 286ab7a600
commit 732223e832
17 changed files with 736 additions and 633 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -104,14 +104,13 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
const float CameraTango::bilateralFilteringSigmaS = 2.0f;
const float CameraTango::bilateralFilteringSigmaR = 0.075f;
CameraTango::CameraTango(bool colorCamera, int decimation, bool autoExposure, bool publishRawScan, bool smoothing) :
CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
Camera(0),
tango_config_(0),
firstFrame_(true),
stampEpochOffset_(0.0),
colorCamera_(colorCamera),
decimation_(decimation),
autoExposure_(autoExposure),
rawScanPublished_(publishRawScan),
smoothing_(smoothing),
cloudStamp_(0),
@@ -219,31 +218,6 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
return false;
}
// disable auto exposure (disabled, seems broken on latest Tango releases)
ret = TangoConfig_setBool(tango_config_, "config_color_mode_auto", autoExposure_);
if (ret != TANGO_SUCCESS)
{
LOGE("NativeRTABMap: config_color_mode_auto() failed with error code: %d", ret);
//return false;
}
else
{
if(!autoExposure_)
{
ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
if (ret != TANGO_SUCCESS)
{
LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret);
return false;
}
}
bool verifyAutoExposureState;
int32_t verifyIso, verifyExp;
TangoConfig_getBool( tango_config_, "config_color_mode_auto", &verifyAutoExposureState );
TangoConfig_getInt32( tango_config_, "config_color_iso", &verifyIso );
TangoConfig_getInt32( tango_config_, "config_color_exp", &verifyExp );
LOGI( "NativeRTABMap: config_color autoExposure=%s %d %d", verifyAutoExposureState?"On" : "Off", verifyIso, verifyExp );
}
}
// Enable depth.

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

@@ -75,7 +75,7 @@ public:
static const float bilateralFilteringSigmaR;
public:
CameraTango(bool colorCamera, int decimation, bool autoExposure, bool publishRawScan, bool smoothing);
CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
virtual ~CameraTango();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -87,7 +87,6 @@ public:
void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
void setDecimation(int value) {decimation_ = value;}
void setSmoothing(bool enabled) {smoothing_ = enabled;}
void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
@@ -112,7 +111,6 @@ private:
double stampEpochOffset_;
bool colorCamera_;
int decimation_;
bool autoExposure_;
bool rawScanPublished_;
bool smoothing_;
cv::Mat cloud_;

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

@@ -50,13 +50,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/GainCompensator.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
#define LOW_RES_PIX 1
#define LOW_RES_PIX 2
//#define DEBUG_RENDERING_PERFORMANCE;
const int g_exportedMeshId = -100;
@@ -150,7 +151,6 @@ RTABMapApp::RTABMapApp() :
nodesFiltering_(false),
localizationMode_(false),
trajectoryMode_(false),
autoExposure_(true),
rawScanSaved_(false),
smoothing_(true),
cameraColor_(true),
@@ -260,7 +260,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
this->registerToEventsManager();
camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, autoExposure_, rawScanSaved_, smoothing_);
camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_);
}
void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
@@ -329,86 +329,113 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
createdMeshes_.clear();
int i=0;
UTimer addTime;
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end() && status==0; ++iter)
{
int id = iter->first;
if(!iter->second.isNull())
try
{
if(uContains(signatures, id))
int id = iter->first;
if(!iter->second.isNull())
{
UTimer timer;
rtabmap::SensorData data = signatures.at(id).sensorData();
cv::Mat tmpA, depth;
data.uncompressData(&tmpA, &depth);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
if(uContains(signatures, id))
{
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
if(cloud->size() && indices->size())
{
std::vector<pcl::Vertices> polygons;
std::vector<pcl::Vertices> polygonsLowRes;
if(main_scene_.isMeshRendering() && main_scene_.isMapRendering())
{
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
}
UTimer timer;
rtabmap::SensorData data = signatures.at(id).sensorData();
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering() || !main_scene_.isMapRendering())
cv::Mat tmpA, depth;
data.uncompressData(&tmpA, &depth);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
if(cloud->size() && indices->size())
{
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.polygonsLowRes = polygonsLowRes;
inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gains[0] = 1.0;
inserted.first->second.gains[1] = 1.0;
inserted.first->second.gains[2] = 1.0;
if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
std::vector<pcl::Vertices> polygons;
std::vector<pcl::Vertices> polygonsLowRes;
if(main_scene_.isMeshRendering() && main_scene_.isMapRendering())
{
if(renderingTextureDecimation_>1)
{
cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
}
else
{
inserted.first->second.texture = data.imageRaw();
}
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
}
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering() || !main_scene_.isMapRendering())
{
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.polygonsLowRes = polygonsLowRes;
inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gains[0] = 1.0;
inserted.first->second.gains[1] = 1.0;
inserted.first->second.gains[2] = 1.0;
if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
{
if(renderingTextureDecimation_>1)
{
cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
}
else
{
inserted.first->second.texture = data.imageRaw();
}
}
LOGI("Created cloud %d (%fs)", id, timer.ticks());
}
LOGI("Created cloud %d (%fs)", id, timer.ticks());
}
}
}
const rtabmap::Signature & s = signatures.at(id);
processMemoryUsedBytes += data.imageCompressed().total();
processMemoryUsedBytes += data.depthOrRightCompressed().total();
processMemoryUsedBytes += data.laserScanCompressed().total();
processMemoryUsedBytes += s.getWords().size()*4*8;
processMemoryUsedBytes += s.getWords3().size()*4*4;
if(!s.getWordsDescriptors().empty())
{
processMemoryUsedBytes +=s.getWordsDescriptors().size()*(4+s.getWordsDescriptors().begin()->second.total());
else
{
UERROR("Failed to uncompress data!");
status=-2;
}
const rtabmap::Signature & s = signatures.at(id);
processMemoryUsedBytes += data.imageCompressed().total();
processMemoryUsedBytes += data.depthOrRightCompressed().total();
processMemoryUsedBytes += data.laserScanCompressed().total();
processMemoryUsedBytes += s.getWords().size()*4*8;
processMemoryUsedBytes += s.getWords3().size()*4*4;
if(!s.getWordsDescriptors().empty())
{
processMemoryUsedBytes +=s.getWordsDescriptors().size()*(4+s.getWordsDescriptors().begin()->second.total());
}
}
}
++i;
if(addTime.elapsed() >= 4.0f)
{
UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, uFormat("Created clouds %d/%d", i, (int)poses.size())));
addTime.restart();
}
}
++i;
if(addTime.elapsed() >= 4.0f)
catch(const UException & e)
{
UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, uFormat("Created clouds %d/%d", i, (int)poses.size())));
addTime.restart();
UERROR("Exception! msg=\"%s\"", e.what());
status = -2;
}
catch (const cv::Exception & e)
{
UERROR("Exception! msg=\"%s\"", e.what());
status = -2;
}
catch (const std::exception & e)
{
UERROR("Exception! msg=\"%s\"", e.what());
status = -2;
}
}
}
if(optimize)
if(status < 0)
{
createdMeshes_.clear();
}
if(optimize && status==0)
{
UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Visual optimization..."));
gainCompensation();
@@ -1629,18 +1656,6 @@ void RTABMapApp::setGridVisible(bool visible)
main_scene_.setGridVisible(visible);
}
void RTABMapApp::setAutoExposure(bool enabled)
{
if(autoExposure_ != enabled)
{
autoExposure_ = enabled;
if(camera_)
{
camera_->setAutoExposure(autoExposure_);
}
}
}
void RTABMapApp::setRawScanSaved(bool enabled)
{
if(rawScanSaved_ != enabled)
@@ -2428,8 +2443,25 @@ bool RTABMapApp::exportMesh(
}
LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size());
if(mergedClouds->size())
if(mergedClouds->size()>=3)
{
if(optimizedDepth == 0)
{
Eigen::Vector4f min,max;
pcl::getMinMax3D(*mergedClouds, min, max);
float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
optimizedDepth = 12;
for(int i=6; i<12; ++i)
{
if(mapLength/float(1<<i) < 0.03f)
{
optimizedDepth = i;
break;
}
}
LOGI("optimizedDepth=%d (map length=%f)", optimizedDepth, mapLength);
}
// Mesh reconstruction
LOGI("Mesh reconstruction...");
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);

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

@@ -127,7 +127,6 @@ class RTABMapApp : public UEventsHandler {
void setNodesFiltering(bool enabled);
void setGraphVisible(bool visible);
void setGridVisible(bool visible);
void setAutoExposure(bool enabled);
void setRawScanSaved(bool enabled);
void setCameraColor(bool enabled);
void setFullResolution(bool enabled);
@@ -192,7 +191,6 @@ class RTABMapApp : public UEventsHandler {
bool nodesFiltering_;
bool localizationMode_;
bool trajectoryMode_;
bool autoExposure_;
bool rawScanSaved_;
bool smoothing_;
bool cameraColor_;

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

@@ -205,12 +205,6 @@ Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
return app.setGridVisible(visible);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
JNIEnv*, jobject, bool enabled)
{
return app.setAutoExposure(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setRawScanSaved(
JNIEnv*, jobject, bool enabled)
{

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

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "point_cloud_drawable.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/imgproc/imgproc.hpp>
#include "util.h"
@@ -39,9 +40,264 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define LOW_DEC 2
#define LOWLOW_DEC 4
enum PointCloudShaders
{
kPointCloud = 0,
kPointCloudBlending = 1,
kPointCloudLighting = 2,
kPointCloudLightingBlending = 3,
kTexture = 4,
kTextureBlending = 5,
kTextureLighting = 6,
kTextureLightingBlending = 7,
kDepthPacking = 8
};
// PointCloud shaders
const std::string kPointCloudVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aColor;\n"
"uniform mat4 uMVP;\n"
"uniform float uPointSize;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" gl_PointSize = uPointSize;\n"
" vLightWeighting = 1.0;\n"
" vColor = aColor;\n"
"}\n";
const std::string kPointCloudLightingVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aNormal;\n"
"attribute vec3 aColor;\n"
"uniform mat4 uMVP;\n"
"uniform mat3 uN;\n"
"uniform vec3 uLightingDirection;\n"
"uniform float uPointSize;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" gl_PointSize = uPointSize;\n"
" vec3 transformedNormal = uN * aNormal;\n"
" vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
" if(vLightWeighting<0.5)"
" vLightWeighting=0.5;\n"
" vColor = aColor;\n"
"}\n";
const std::string kPointCloudFragmentShader =
"precision mediump float;\n"
"precision mediump int;\n"
"uniform float uGainR;\n"
"uniform float uGainG;\n"
"uniform float uGainB;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
"}\n";
const std::string kPointCloudBlendingFragmentShader =
"precision highp float;\n"
"precision mediump int;\n"
"uniform float uGainR;\n"
"uniform float uGainG;\n"
"uniform float uGainB;\n"
"uniform float uNearZ;\n"
"uniform float uFarZ;\n"
"uniform sampler2D uDepthTexture;\n"
"uniform vec2 uScreenScale;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
" float alpha = 1.0;\n"
" vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
" float depth = texture2D(uDepthTexture, coord).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 ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
" 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"
"}\n";
const std::string kPointCloudDepthPackingVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"uniform mat4 uMVP;\n"
"uniform float uPointSize;\n"
"void main() {\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" gl_PointSize = uPointSize;\n"
"}\n";
const std::string kPointCloudDepthPackingFragmentShader =
"precision highp float;\n"
"precision mediump int;\n"
"void main() {\n"
" float toFixed = 255.0/256.0;\n"
" vec4 enc = vec4(1.0, 255.0, 65025.0, 160581375.0) * toFixed * gl_FragCoord.z;\n"
" enc = fract(enc);\n"
" gl_FragColor = enc;\n"
"}\n";
// Texture shaders
const std::string kTextureMeshVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec2 aTexCoord;\n"
"uniform mat4 uMVP;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" if(aTexCoord.x < 0.0) {\n"
" vTexCoord.x = 1.0;\n"
" vTexCoord.y = 1.0;\n" // bottom right corner
" } else {\n"
" vTexCoord = aTexCoord;\n"
" }\n"
" vLightWeighting = 1.0;\n"
"}\n";
const std::string kTextureMeshLightingVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aNormal;\n"
"attribute vec2 aTexCoord;\n"
"uniform mat4 uMVP;\n"
"uniform mat3 uN;\n"
"uniform vec3 uLightingDirection;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" if(aTexCoord.x < 0.0) {\n"
" vTexCoord.x = 1.0;\n"
" vTexCoord.y = 1.0;\n" // bottom right corner
" } else {\n"
" vTexCoord = aTexCoord;\n"
" }\n"
" vec3 transformedNormal = uN * aNormal;\n"
" vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
" if(vLightWeighting<0.5) \n"
" vLightWeighting=0.5;\n"
"}\n";
const std::string kTextureMeshFragmentShader =
"precision mediump float;\n"
"precision mediump int;\n"
"uniform sampler2D uTexture;\n"
"uniform float uGainR;\n"
"uniform float uGainG;\n"
"uniform float uGainB;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
""
"void main() {\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
"}\n";
const std::string kTextureMeshBlendingFragmentShader =
"precision highp float;\n"
"precision mediump int;\n"
"uniform sampler2D uTexture;\n"
"uniform sampler2D uDepthTexture;\n"
"uniform float uGainR;\n"
"uniform float uGainG;\n"
"uniform float uGainB;\n"
"uniform vec2 uScreenScale;\n"
"uniform float uNearZ;\n"
"uniform float uFarZ;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
""
"void main() {\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
" float alpha = 1.0;\n"
" vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
" float depth = texture2D(uDepthTexture, coord).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 ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
" 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"
"}\n";
std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
void PointCloudDrawable::createShaderPrograms()
{
if(shaderPrograms_.empty())
{
shaderPrograms_.resize(9);
shaderPrograms_[kPointCloud] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
UASSERT(shaderPrograms_[kPointCloud] != 0);
shaderPrograms_[kPointCloudBlending] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
UASSERT(shaderPrograms_[kPointCloudBlending] != 0);
shaderPrograms_[kPointCloudLighting] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudFragmentShader.c_str());
UASSERT(shaderPrograms_[kPointCloudLighting] != 0);
shaderPrograms_[kPointCloudLightingBlending] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
UASSERT(shaderPrograms_[kPointCloudLightingBlending] != 0);
shaderPrograms_[kTexture] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
UASSERT(shaderPrograms_[kTexture] != 0);
shaderPrograms_[kTextureBlending] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
UASSERT(shaderPrograms_[kTextureBlending] != 0);
shaderPrograms_[kTextureLighting] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
UASSERT(shaderPrograms_[kTextureLighting] != 0);
shaderPrograms_[kTextureLightingBlending] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
UASSERT(shaderPrograms_[kTextureLightingBlending] != 0);
shaderPrograms_[kDepthPacking] = tango_gl::util::CreateProgram(kPointCloudDepthPackingVertexShader.c_str(), kPointCloudDepthPackingFragmentShader.c_str());
UASSERT(shaderPrograms_[kDepthPacking] != 0);
}
}
void PointCloudDrawable::releaseShaderPrograms()
{
for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
{
glDeleteShader(shaderPrograms_[i]);
}
shaderPrograms_.clear();
}
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float gainR,
@@ -54,8 +310,6 @@ PointCloudDrawable::PointCloudDrawable(
poseGl_(1.0f),
visible_(true),
hasNormals_(false),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram),
gainR_(gainR),
gainG_(gainG),
gainB_(gainB)
@@ -64,8 +318,6 @@ PointCloudDrawable::PointCloudDrawable(
}
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh) :
vertex_buffers_(0),
textures_(0),
@@ -74,8 +326,6 @@ PointCloudDrawable::PointCloudDrawable(
poseGl_(1.0f),
visible_(true),
hasNormals_(false),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram),
gainR_(1.0f),
gainG_(1.0f),
gainB_(1.0f)
@@ -321,9 +571,8 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
int oi_lowlow = 0;
if(textures_ && polygons.size())
{
//LOGD("Organized mesh with texture");
int items = hasNormals_?9:6;
vertices = std::vector<float>(mesh.indices->size()*9);
vertices = std::vector<float>(mesh.indices->size()*items);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
@@ -401,7 +650,6 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
}
else // assume dense mesh with texCoords set to polygons
{
totalPoints = mesh.cloud->size();
if(textures_ && polygons.size() && mesh.normals->size())
{
//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
@@ -411,8 +659,9 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
// 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);
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.texCoords.size(), -1);
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());
@@ -468,9 +717,10 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
}
else
{
totalPoints = mesh.cloud->size();
//LOGD("Dense mesh");
int items = hasNormals_?7:4;
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->size(), -1);
organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
vertices = std::vector<float>(mesh.cloud->size()*items);
for(unsigned int i=0; i<mesh.cloud->size(); ++i)
{
@@ -594,7 +844,8 @@ void PointCloudDrawable::updateAABBWorld(const rtabmap::Transform & pose)
}
void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
void PointCloudDrawable::Render(
const glm::mat4 & projectionMatrix,
const glm::mat4 & viewMatrix,
bool meshRendering,
float pointSize,
@@ -604,94 +855,171 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
const GLuint & depthTexture,
int screenWidth,
int screenHeight,
bool packDepthToColorChannel) {
if(vertex_buffers_ && nPoints_ && visible_)
float nearClipPlane,
float farClipPlane,
bool packDepthToColorChannel) const
{
if(vertex_buffers_ && nPoints_ && visible_ && !shaderPrograms_.empty())
{
if(meshRendering && textureRendering && textures_ && (verticesLowRes_.empty() || distanceToCameraSqr<50.0f))
if(packDepthToColorChannel || !hasNormals_)
{
glUseProgram(texture_shader_program_);
lighting = false;
}
GLuint mvp_handle = glGetUniformLocation(texture_shader_program_, "uMVP");
glm::mat4 mv_mat = viewMatrix * poseGl_;
glm::mat4 mvp_mat = projectionMatrix * mv_mat;
glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
GLuint n_handle = glGetUniformLocation(texture_shader_program_, "uN");
glm::mat3 normalMatrix(mv_mat);
normalMatrix = glm::inverse(normalMatrix);
normalMatrix = glm::transpose(normalMatrix);
glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
if(!hasNormals_)
{
lighting = false;
}
//lighting
GLuint lighting_handle = glGetUniformLocation(texture_shader_program_, "uUseLighting");
glUniform1i(lighting_handle, lighting?1:0);
if(packDepthToColorChannel || !(meshRendering && textureRendering && textures_))
{
textureRendering = false;
}
GLuint program;
if(packDepthToColorChannel)
{
program = shaderPrograms_[kDepthPacking];
}
else if(textureRendering)
{
if(lighting)
{
GLuint ambiant_handle = glGetUniformLocation(texture_shader_program_, "uAmbientColor");
glUniform3f(ambiant_handle,0.6,0.6,0.6);
GLuint lightingDirection_handle = glGetUniformLocation(texture_shader_program_, "uLightingDirection");
glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
program = shaderPrograms_[depthTexture>0?kTextureLightingBlending:kTextureLighting];
}
else
{
program = shaderPrograms_[depthTexture>0?kTextureBlending:kTexture];
}
}
else
{
if(lighting)
{
program = shaderPrograms_[depthTexture>0?kPointCloudLightingBlending:kPointCloudLighting];
}
else
{
program = shaderPrograms_[depthTexture>0?kPointCloudBlending:kPointCloud];
}
}
// Texture activate unit 0
glActiveTexture(GL_TEXTURE0);
// Bind the texture to this unit.
glBindTexture(GL_TEXTURE_2D, textures_);
// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "uTexture");
glUniform1i(texture_handle, 0);
glUseProgram(program);
tango_gl::util::CheckGlError("Pointcloud::Render() set program");
// Texture activate unit 1
glActiveTexture(GL_TEXTURE1);
// Bind the texture to this unit.
glBindTexture(GL_TEXTURE_2D, depthTexture);
// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
GLuint depth_texture_handle = glGetUniformLocation(texture_shader_program_, "uDepthTexture");
glUniform1i(depth_texture_handle, 1);
GLuint mvp_handle = glGetUniformLocation(program, "uMVP");
glm::mat4 mv_mat = viewMatrix * poseGl_;
glm::mat4 mvp_mat = projectionMatrix * mv_mat;
glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
GLuint gainR_handle = glGetUniformLocation(texture_shader_program_, "uGainR");
GLuint gainG_handle = glGetUniformLocation(texture_shader_program_, "uGainG");
GLuint gainB_handle = glGetUniformLocation(texture_shader_program_, "uGainB");
GLint attribute_vertex = glGetAttribLocation(program, "aVertex");
glEnableVertexAttribArray(attribute_vertex);
GLint attribute_color = 0;
GLint attribute_texture = 0;
GLint attribute_normal = 0;
if(packDepthToColorChannel || !textureRendering)
{
GLuint point_size_handle_ = glGetUniformLocation(program, "uPointSize");
glUniform1f(point_size_handle_, pointSize);
}
tango_gl::util::CheckGlError("Pointcloud::Render() vertex");
if(!packDepthToColorChannel)
{
GLuint gainR_handle = glGetUniformLocation(program, "uGainR");
GLuint gainG_handle = glGetUniformLocation(program, "uGainG");
GLuint gainB_handle = glGetUniformLocation(program, "uGainB");
glUniform1f(gainR_handle, gainR_);
glUniform1f(gainG_handle, gainG_);
glUniform1f(gainB_handle, gainB_);
GLuint blending_handle = glGetUniformLocation(texture_shader_program_, "uBlending");
glUniform1i(blending_handle, depthTexture>0?1:0);
GLuint screenScale_handle = glGetUniformLocation(texture_shader_program_, "uScreenScale");
glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "aVertex");
GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "aTexCoord");
GLint attribute_normal=0;
if(hasNormals_)
// blending
if(depthTexture > 0)
{
attribute_normal = glGetAttribLocation(texture_shader_program_, "aNormal");
// Texture activate unit 1
glActiveTexture(GL_TEXTURE1);
// Bind the texture to this unit.
glBindTexture(GL_TEXTURE_2D, depthTexture);
// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
GLuint depth_texture_handle = glGetUniformLocation(program, "uDepthTexture");
glUniform1i(depth_texture_handle, 1);
GLuint zNear_handle = glGetUniformLocation(program, "uNearZ");
GLuint zFar_handle = glGetUniformLocation(program, "uFarZ");
glUniform1f(zNear_handle, nearClipPlane);
glUniform1f(zFar_handle, farClipPlane);
GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
}
glEnableVertexAttribArray(attribute_vertex);
glEnableVertexAttribArray(attribute_texture);
if(hasNormals_)
if(lighting)
{
GLuint n_handle = glGetUniformLocation(program, "uN");
glm::mat3 normalMatrix(mv_mat);
normalMatrix = glm::inverse(normalMatrix);
normalMatrix = glm::transpose(normalMatrix);
glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
GLuint lightingDirection_handle = glGetUniformLocation(program, "uLightingDirection");
glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
attribute_normal = glGetAttribLocation(program, "aNormal");
glEnableVertexAttribArray(attribute_normal);
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
if(textureRendering)
{
// Texture activate unit 0
glActiveTexture(GL_TEXTURE0);
// Bind the texture to this unit.
glBindTexture(GL_TEXTURE_2D, textures_);
// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
GLuint texture_handle = glGetUniformLocation(program, "uTexture");
glUniform1i(texture_handle, 0);
attribute_texture = glGetAttribLocation(program, "aTexCoord");
glEnableVertexAttribArray(attribute_texture);
}
else
{
attribute_color = glGetAttribLocation(program, "aColor");
glEnableVertexAttribArray(attribute_color);
}
}
tango_gl::util::CheckGlError("Pointcloud::Render() common");
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
if(textures_)
{
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
if(hasNormals_)
if(textureRendering)
{
glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
}
else if(!packDepthToColorChannel)
{
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
}
if(lighting && hasNormals_)
{
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
}
if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
}
else
{
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
if(!packDepthToColorChannel)
{
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
}
if(lighting && hasNormals_)
{
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
}
}
tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");
UTimer drawTime;
if(textureRendering)
{
if(distanceToCameraSqr<16.0f || polygonsLowRes_.empty())
{
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
}
@@ -700,135 +1028,44 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
}
}
else // point cloud or colored mesh
else if(meshRendering && polygons_.size())
{
glUseProgram(cloud_shader_program_);
GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "uMVP");
glm::mat4 mv_mat = viewMatrix * poseGl_;
glm::mat4 mvp_mat = projectionMatrix * mv_mat;
glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
GLuint n_handle = glGetUniformLocation(cloud_shader_program_, "uN");
glm::mat3 normalMatrix(mv_mat);
normalMatrix = glm::inverse(normalMatrix);
normalMatrix = glm::transpose(normalMatrix);
glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
if(!hasNormals_)
if(distanceToCameraSqr<50.0f || polygonsLowRes_.empty())
{
lighting = false;
}
//lighting
GLuint lighting_handle = glGetUniformLocation(cloud_shader_program_, "uUseLighting");
glUniform1i(lighting_handle, lighting?1:0);
if(lighting)
{
GLuint ambiant_handle = glGetUniformLocation(cloud_shader_program_, "uAmbientColor");
glUniform3f(ambiant_handle,0.6,0.6,0.6);
GLuint lightingDirection_handle = glGetUniformLocation(cloud_shader_program_, "uLightingDirection");
glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
}
GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "uPointSize");
glUniform1f(point_size_handle_, pointSize);
// Texture activate unit 1
glActiveTexture(GL_TEXTURE0);
// Bind the texture to this unit.
glBindTexture(GL_TEXTURE_2D, depthTexture);
// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
GLuint depth_texture_handle = glGetUniformLocation(cloud_shader_program_, "uDepthTexture");
glUniform1i(depth_texture_handle, 0);
GLuint gainR_handle = glGetUniformLocation(cloud_shader_program_, "uGainR");
GLuint gainG_handle = glGetUniformLocation(cloud_shader_program_, "uGainG");
GLuint gainB_handle = glGetUniformLocation(cloud_shader_program_, "uGainB");
glUniform1f(gainR_handle, gainR_);
glUniform1f(gainG_handle, gainG_);
glUniform1f(gainB_handle, gainB_);
GLuint packing_handle = glGetUniformLocation(cloud_shader_program_, "uPackDepthToColor");
glUniform1i(packing_handle, packDepthToColorChannel?1:0);
GLuint blending_handle = glGetUniformLocation(cloud_shader_program_, "uBlending");
glUniform1i(blending_handle, depthTexture>0?1:0);
GLuint screenScale_handle = glGetUniformLocation(cloud_shader_program_, "uScreenScale");
glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "aVertex");
GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "aColor");
GLint attribute_normal=0;
if(hasNormals_)
{
attribute_normal = glGetAttribLocation(cloud_shader_program_, "aNormal");
}
glEnableVertexAttribArray(attribute_vertex);
glEnableVertexAttribArray(attribute_color);
if(hasNormals_)
{
glEnableVertexAttribArray(attribute_normal);
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
if(textures_)
{
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
if(hasNormals_)
{
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
}
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
}
else
{
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
if(hasNormals_)
{
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
}
glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
}
if(meshRendering && polygons_.size())
}
else if(!verticesLowRes_.empty())
{
if(distanceToCameraSqr>600.0f)
{
if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
{
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
}
else
{
glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
}
glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
}
else if(!verticesLowRes_.empty())
else if(distanceToCameraSqr>150.0f)
{
if(distanceToCameraSqr>600.0f)
{
glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
}
else if(distanceToCameraSqr>150.0f)
{
glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
}
else
{
glDrawArrays(GL_POINTS, 0, nPoints_);
}
glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
}
else
{
glDrawArrays(GL_POINTS, 0, nPoints_);
}
}
else
{
glDrawArrays(GL_POINTS, 0, nPoints_);
}
//UERROR("drawTime=%fs", drawTime.ticks());
tango_gl::util::CheckGlError("Pointcloud::Render() draw");
glDisableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, 0);
glUseProgram(0);
tango_gl::util::CheckGlError("Pointcloud::Render()");
tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");
}
}

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

@@ -40,18 +40,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// PointCloudDrawable is responsible for the point cloud rendering.
class PointCloudDrawable {
public:
static void createShaderPrograms();
static void releaseShaderPrograms();
private:
static std::vector<GLuint> shaderPrograms_;
public:
PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float gainR = 1.0f,
float gainG = 1.0f,
float gainB = 1.0f);
PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh);
virtual ~PointCloudDrawable();
@@ -77,7 +80,8 @@ class PointCloudDrawable {
// @param view_mat: view matrix from current render camera.
// @param model_mat: model matrix for this point cloud frame.
// @param vertices: all vertices in this point cloud frame.
void Render(const glm::mat4 & projectionMatrix,
void Render(
const glm::mat4 & projectionMatrix,
const glm::mat4 & viewMatrix,
bool meshRendering = true,
float pointSize = 3.0f,
@@ -85,9 +89,11 @@ class PointCloudDrawable {
bool lighting = true,
float distanceToCamSqr = 0.0f,
const GLuint & depthTexture = 0,
int screenWidth = 0,
int screenHeight = 0,
bool packDepthToColorChannel = false);
int screenWidth = 0, // nonnull if depthTexture>0
int screenHeight = 0, // nonnull if depthTexture>0
float nearClipPlane = 0, // nonnull if depthTexture>0
float farClipPlane = 0, // nonnull if depthTexture>0
bool packDepthToColorChannel = false) const;
private:
template<class PointT>
@@ -117,9 +123,6 @@ class PointCloudDrawable {
bool hasNormals_;
std::vector<unsigned int> organizedToDenseIndices_;
GLuint cloud_shader_program_;
GLuint texture_shader_program_;
float gainR_;
float gainG_;
float gainB_;

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

@@ -43,141 +43,6 @@ const tango_gl::Color kGridColor(0.85f, 0.85f, 0.85f);
// Frustum scale.
const glm::vec3 kFrustumScale = glm::vec3(0.4f, 0.3f, 0.5f);
const std::string kPointCloudVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aNormal;\n"
"attribute vec3 aColor;\n"
"uniform mat4 uMVP;\n"
"uniform mat3 uN;\n"
"uniform vec3 uAmbientColor;\n"
"uniform vec3 uLightingDirection;\n"
"uniform bool uUseLighting;\n"
"uniform float uPointSize;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" gl_PointSize = uPointSize;\n"
" if (!uUseLighting) {\n"
" vLightWeighting = 1.0;\n"
" } else {\n"
" vec3 transformedNormal = uN * aNormal;\n"
" vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
" if(vLightWeighting<0.5) vLightWeighting=0.5;\n"
" }\n"
" vColor = aColor;\n"
"}\n";
const std::string kPointCloudFragmentShader =
"precision highp float;\n"
"precision mediump int;\n"
"uniform float uGainR;\n"
"uniform float uGainG;\n"
"uniform float uGainB;\n"
"uniform sampler2D uDepthTexture;\n"
"uniform bool uBlending;\n"
"uniform vec2 uScreenScale;\n"
"uniform bool uPackDepthToColor;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
" float alpha = 1.0;\n"
" if(uBlending) {\n"
" vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
" float depth = texture2D(uDepthTexture, coord).r;\n"
// " alpha = 0.5;\n"
" float zNear = 0.2;\n"
" float zFar = 1000.0;\n"
" float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
" float linearDepth = (2.0 * zNear * zFar) / (zFar + zNear - ndcDepth * (zFar - zNear));\n"
" float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
" float linearFragz = (2.0 * zNear * zFar) / (zFar + zNear - ndcFragz * (zFar - zNear));\n"
" if(linearFragz > linearDepth + 0.05)\n"
" alpha=0.0;\n"
" }\n"
" if(uPackDepthToColor) {\n"
" float toFixed = 255.0/256.0;\n"
" vec4 enc = vec4(1.0, 255.0, 65025.0, 160581375.0) * toFixed * gl_FragCoord.z;\n"
" enc = fract(enc);\n"
" gl_FragColor = enc;\n"
" }\n"
" else {"
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
" }\n"
"}\n";
const std::string kTextureMeshVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aNormal;\n"
"attribute vec2 aTexCoord;\n"
"uniform mat4 uMVP;\n"
"uniform mat3 uN;\n"
"uniform vec3 uAmbientColor;\n"
"uniform vec3 uLightingDirection;\n"
"uniform bool uUseLighting;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" if(aTexCoord.x < 0.0) {\n"
" vTexCoord.x = 1.0;\n"
" vTexCoord.y = 1.0;\n" // bottom right corner
" } else {\n"
" vTexCoord = aTexCoord;\n"
" }\n"
" if (!uUseLighting) {\n"
" vLightWeighting = 1.0;\n"
" } else {\n"
" vec3 transformedNormal = uN * aNormal;\n"
" vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
" if(vLightWeighting<0.5) vLightWeighting=0.5;\n"
" }\n"
"}\n";
const std::string kTextureMeshFragmentShader =
"precision highp float;\n"
"precision mediump int;\n"
"uniform sampler2D uTexture;\n"
"uniform sampler2D uDepthTexture;\n"
"uniform float uGainR;\n"
"uniform float uGainG;\n"
"uniform float uGainB;\n"
"uniform bool uBlending;\n"
"uniform vec2 uScreenScale;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
""
"void main() {\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
" float alpha = 1.0;\n"
" if(uBlending) {\n"
" vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
" float depth = texture2D(uDepthTexture, coord).r;\n"
// " alpha = 0.5;\n"
//Linearize depth: http://stackoverflow.com/questions/6652253/getting-the-true-z-value-from-the-depth-buffer
" float zNear = 0.2;\n"
" float zFar = 1000.0;\n"
" float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
" float linearDepth = (2.0 * zNear * zFar) / (zFar + zNear - ndcDepth * (zFar - zNear));\n"
" float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
" float linearFragz = (2.0 * zNear * zFar) / (zFar + zNear - ndcFragz * (zFar - zNear));\n"
" if(linearFragz > linearDepth + 0.05)\n"
" alpha=0.0;\n"
" }\n"
" gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
"}\n";
const std::string kGraphVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
@@ -212,8 +77,6 @@ Scene::Scene() :
traceVisible_(true),
color_camera_to_display_rotation_(ROTATION_0),
currentPose_(0),
cloud_shader_program_(0),
texture_mesh_shader_program_(0),
graph_shader_program_(0),
blending_(true),
mapRendering_(true),
@@ -253,38 +116,30 @@ void Scene::InitGLContent()
UASSERT(axis_ == 0);
axis_ = new tango_gl::Axis();
frustum_ = new tango_gl::Frustum();
trace_ = new tango_gl::Trace();
grid_ = new tango_gl::Grid();
box_ = new BoundingBoxDrawable();
currentPose_ = new rtabmap::Transform();
axis_ = new tango_gl::Axis();
frustum_ = new tango_gl::Frustum();
trace_ = new tango_gl::Trace();
grid_ = new tango_gl::Grid();
box_ = new BoundingBoxDrawable();
currentPose_ = new rtabmap::Transform();
axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
frustum_->SetColor(kTraceColor);
trace_->ClearVertexArray();
trace_->SetColor(kTraceColor);
grid_->SetColor(kGridColor);
grid_->SetPosition(-kHeightOffset);
box_->SetShader();
box_->SetColor(1,0,0);
axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
frustum_->SetColor(kTraceColor);
trace_->ClearVertexArray();
trace_->SetColor(kTraceColor);
grid_->SetColor(kGridColor);
grid_->SetPosition(-kHeightOffset);
box_->SetShader();
box_->SetColor(1,0,0);
if(cloud_shader_program_ == 0)
{
cloud_shader_program_ = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
UASSERT(cloud_shader_program_ != 0);
}
if(texture_mesh_shader_program_ == 0)
{
texture_mesh_shader_program_ = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
UASSERT(texture_mesh_shader_program_ != 0);
}
if(graph_shader_program_ == 0)
{
graph_shader_program_ = tango_gl::util::CreateProgram(kGraphVertexShader.c_str(), kGraphFragmentShader.c_str());
UASSERT(graph_shader_program_ != 0);
}
PointCloudDrawable::createShaderPrograms();
if(graph_shader_program_ == 0)
{
graph_shader_program_ = tango_gl::util::CreateProgram(kGraphVertexShader.c_str(), kGraphFragmentShader.c_str());
UASSERT(graph_shader_program_ != 0);
}
}
//Should only be called in OpenGL thread!
@@ -302,14 +157,8 @@ void Scene::DeleteResources() {
delete box_;
}
if (cloud_shader_program_) {
glDeleteShader(cloud_shader_program_);
cloud_shader_program_ = 0;
}
if (texture_mesh_shader_program_) {
glDeleteShader(texture_mesh_shader_program_);
texture_mesh_shader_program_ = 0;
}
PointCloudDrawable::releaseShaderPrograms();
if (graph_shader_program_) {
glDeleteShader(graph_shader_program_);
graph_shader_program_ = 0;
@@ -535,7 +384,28 @@ int Scene::Render() {
0.0f, 1.0f, 0.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f);
//Culling
std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
std::vector<PointCloudDrawable*> cloudsToDraw(pointClouds_.size());
int oi=0;
for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
{
if(!mapRendering_ && iter->first > 0)
{
break;
}
if(iter->second->isVisible())
{
if(intersectFrustumAABB(planes,
iter->second->aabbMinWorld(),
iter->second->aabbMaxWorld()))
{
cloudsToDraw[oi++] = iter->second;
}
}
}
cloudsToDraw.resize(oi);
// First rendering to get depth texture
glEnable(GL_DEPTH_TEST);
@@ -556,8 +426,7 @@ int Scene::Render() {
UTimer timer;
bool onlineBlending = blending_ && mapRendering_ && meshRendering_ && pointClouds_.size()>1;
std::set<int> usedForDepth;
bool onlineBlending = blending_ && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
if(onlineBlending && fboId_)
{
// set the rendering destination to FBO
@@ -568,23 +437,10 @@ int Scene::Render() {
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
// Draw scene
for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
{
if(iter->second->isVisible())
{
if(intersectFrustumAABB(planes,
iter->second->aabbMinWorld(),
iter->second->aabbMaxWorld()))
{
usedForDepth.insert(iter->first);
Eigen::Vector3f cloudToCamera(
iter->second->getPose().x() - openglCamera.x(),
iter->second->getPose().y() - openglCamera.y(),
iter->second->getPose().z() - openglCamera.z());
float distanceToCameraSqr = 999.0f; // set it large to use low res polygons for fast processing
iter->second->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr);
}
}
// set large distance to cam to use low res polygons for fast processing
(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
}
// back to normal window-system-provided framebuffer
@@ -597,17 +453,10 @@ int Scene::Render() {
glClearColor(0, 0, 0, 0);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
{
if((onlineBlending && usedForDepth.find(iter->first) != usedForDepth.end()) ||
(!onlineBlending && iter->second->isVisible() &&
intersectFrustumAABB(planes,
iter->second->aabbMinWorld(),
iter->second->aabbMaxWorld())))
{
float distanceToCameraSqr = 999.0f; // set it large to use low res polygons for fast processing
iter->second->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, distanceToCameraSqr, 0, 0, 0, true);
}
// 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);
}
GLubyte zValue[4];
@@ -661,75 +510,39 @@ int Scene::Render() {
graph_->Render(projectionMatrix, viewMatrix);
}
int cloudDrawn=0;
if(mapRendering_)
if(onlineBlending)
{
if(onlineBlending)
{
glEnable (GL_BLEND);
glDepthMask(GL_FALSE);
}
for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
{
if((onlineBlending && usedForDepth.find(iter->first) != usedForDepth.end()) ||
(!onlineBlending && iter->second->isVisible() &&
intersectFrustumAABB(planes,
iter->second->aabbMinWorld(),
iter->second->aabbMaxWorld())))
{
if(boundingBoxRendering_)
{
box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
box_->Render(projectionMatrix, viewMatrix);
}
Eigen::Vector3f cloudToCamera(
iter->second->getPose().x() - openglCamera.x(),
iter->second->getPose().y() - openglCamera.y(),
iter->second->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
iter->second->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_);
++cloudDrawn;
}
}
if(onlineBlending)
{
glDisable (GL_BLEND);
glDepthMask(GL_TRUE);
}
}
else
{
for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
{
if(iter->first > 0)
{
break;
}
if(iter->second->isVisible())
{
if(boundingBoxRendering_)
{
box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
box_->Render(projectionMatrix, viewMatrix);
}
++cloudDrawn;
Eigen::Vector3f cloudToCamera(
iter->second->getPose().x() - openglCamera.x(),
iter->second->getPose().y() - openglCamera.y(),
iter->second->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
iter->second->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_);
}
}
glEnable (GL_BLEND);
glDepthMask(GL_FALSE);
}
return 1;
for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
{
PointCloudDrawable * cloud = *iter;
if(boundingBoxRendering_)
{
box_->updateVertices(cloud->aabbMinWorld(), cloud->aabbMaxWorld());
box_->Render(projectionMatrix, viewMatrix);
}
Eigen::Vector3f cloudToCamera(
cloud->getPose().x() - openglCamera.x(),
cloud->getPose().y() - openglCamera.y(),
cloud->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane());
}
if(onlineBlending)
{
glDisable (GL_BLEND);
glDepthMask(GL_TRUE);
}
return (int)cloudsToDraw.size();
}
void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {
@@ -824,12 +637,7 @@ void Scene::addCloud(
}
//create
UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
PointCloudDrawable * drawable = new PointCloudDrawable(
cloud_shader_program_,
texture_mesh_shader_program_,
cloud,
indices);
PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable));
}
@@ -848,11 +656,7 @@ void Scene::addMesh(
}
//create
UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
PointCloudDrawable * drawable = new PointCloudDrawable(
cloud_shader_program_,
texture_mesh_shader_program_,
mesh);
PointCloudDrawable * drawable = new PointCloudDrawable(mesh);
drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable));
}

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

@@ -167,8 +167,6 @@ class Scene {
rtabmap::Transform * currentPose_;
// Shader to display point cloud.
GLuint cloud_shader_program_;
GLuint texture_mesh_shader_program_;
GLuint graph_shader_program_;
bool blending_;

2
app/android/jni/tango-gl/include/tango-gl/camera.h
View File

@@ -33,6 +33,8 @@ class Camera : public Transform {
glm::mat4 GetViewMatrix();
glm::mat4 GetProjectionMatrix();
float getNearClipPlane() const {return near_clip_plane_;}
float getFarClipPlane() const {return far_clip_plane_;}
/**
* Create an OpenGL perspective matrix from window size, camera intrinsics, and clip settings.