feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,241 @@
|
||||
/*
|
||||
* Copyright 2014 Google Inc. All Rights Reserved.
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "tango-gl/util.h"
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
namespace tango_gl {
|
||||
|
||||
namespace {
|
||||
int NormalizedColorCameraRotation(int camera_rotation) {
|
||||
int camera_n = 0;
|
||||
switch (camera_rotation) {
|
||||
case 90:
|
||||
camera_n = 1;
|
||||
break;
|
||||
case 180:
|
||||
camera_n = 2;
|
||||
break;
|
||||
case 270:
|
||||
camera_n = 3;
|
||||
break;
|
||||
default:
|
||||
camera_n = 0;
|
||||
break;
|
||||
}
|
||||
return camera_n;
|
||||
}
|
||||
} // annonymous namespace
|
||||
|
||||
void util::CheckGlError(const char* operation) {
|
||||
for (GLint error = glGetError(); error; error = glGetError()) {
|
||||
LOGE("after %s() glError (0x%x)\n", operation, error);
|
||||
}
|
||||
}
|
||||
|
||||
// Convenience function used in CreateProgram below.
|
||||
static GLuint LoadShader(GLenum shader_type, const char* shader_source) {
|
||||
GLuint shader = glCreateShader(shader_type);
|
||||
if (shader) {
|
||||
glShaderSource(shader, 1, &shader_source, NULL);
|
||||
glCompileShader(shader);
|
||||
GLint compiled = 0;
|
||||
glGetShaderiv(shader, GL_COMPILE_STATUS, &compiled);
|
||||
if (!compiled) {
|
||||
GLint info_len = 0;
|
||||
glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &info_len);
|
||||
if (info_len) {
|
||||
char* buf = (char*) malloc(info_len);
|
||||
if (buf) {
|
||||
glGetShaderInfoLog(shader, info_len, NULL, buf);
|
||||
LOGE("Could not compile shader %d:\n%s\n", shader_type, buf);
|
||||
free(buf);
|
||||
}
|
||||
glDeleteShader(shader);
|
||||
shader = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
return shader;
|
||||
}
|
||||
|
||||
GLuint util::CreateProgram(const char* vertex_source,
|
||||
const char* fragment_source) {
|
||||
GLuint vertexShader = LoadShader(GL_VERTEX_SHADER, vertex_source);
|
||||
if (!vertexShader) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
GLuint fragment_shader = LoadShader(GL_FRAGMENT_SHADER, fragment_source);
|
||||
if (!fragment_shader) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
GLuint program = glCreateProgram();
|
||||
if (program) {
|
||||
glAttachShader(program, vertexShader);
|
||||
CheckGlError("glAttachShader");
|
||||
glAttachShader(program, fragment_shader);
|
||||
CheckGlError("glAttachShader");
|
||||
glLinkProgram(program);
|
||||
GLint link_status = GL_FALSE;
|
||||
glGetProgramiv(program, GL_LINK_STATUS, &link_status);
|
||||
if (link_status != GL_TRUE) {
|
||||
GLint buf_length = 0;
|
||||
glGetProgramiv(program, GL_INFO_LOG_LENGTH, &buf_length);
|
||||
if (buf_length) {
|
||||
char* buf = (char*) malloc(buf_length);
|
||||
if (buf) {
|
||||
glGetProgramInfoLog(program, buf_length, NULL, buf);
|
||||
LOGE("Could not link program:\n%s\n", buf);
|
||||
free(buf);
|
||||
}
|
||||
}
|
||||
glDeleteProgram(program);
|
||||
program = 0;
|
||||
}
|
||||
}
|
||||
CheckGlError("CreateProgram");
|
||||
return program;
|
||||
}
|
||||
|
||||
void util::DecomposeMatrix (const glm::mat4& transform_mat,
|
||||
glm::vec3& translation,
|
||||
glm::quat& rotation,
|
||||
glm::vec3& scale) {
|
||||
float scale_x = glm::length( glm::vec3( transform_mat[0][0], transform_mat[1][0], transform_mat[2][0] ) );
|
||||
float scale_y = glm::length( glm::vec3( transform_mat[0][1], transform_mat[1][1], transform_mat[2][1] ) );
|
||||
float scale_z = glm::length( glm::vec3( transform_mat[0][2], transform_mat[1][2], transform_mat[2][2] ) );
|
||||
|
||||
|
||||
float determinant = glm::determinant( transform_mat );
|
||||
if( determinant < 0.0 )
|
||||
scale_x = -scale_x;
|
||||
|
||||
translation.x = transform_mat[3][0];
|
||||
translation.y = transform_mat[3][1];
|
||||
translation.z = transform_mat[3][2];
|
||||
|
||||
float inverse_scale_x = 1.0 / scale_x;
|
||||
float inverse_scale_y = 1.0 / scale_y;
|
||||
float inverse_scale_z = 1.0 / scale_z;
|
||||
|
||||
glm::mat4 transform_unscaled = transform_mat;
|
||||
|
||||
transform_unscaled[0][0] *= inverse_scale_x;
|
||||
transform_unscaled[1][0] *= inverse_scale_x;
|
||||
transform_unscaled[2][0] *= inverse_scale_x;
|
||||
|
||||
transform_unscaled[0][1] *= inverse_scale_y;
|
||||
transform_unscaled[1][1] *= inverse_scale_y;
|
||||
transform_unscaled[2][1] *= inverse_scale_y;
|
||||
|
||||
transform_unscaled[0][2] *= inverse_scale_z;
|
||||
transform_unscaled[1][2] *= inverse_scale_z;
|
||||
transform_unscaled[2][2] *= inverse_scale_z;
|
||||
|
||||
rotation = glm::quat_cast( transform_mat );
|
||||
|
||||
scale.x = scale_x;
|
||||
scale.y = scale_y;
|
||||
scale.z = scale_z;
|
||||
}
|
||||
|
||||
glm::vec3 util::GetColumnFromMatrix(const glm::mat4& mat, const int col) {
|
||||
return glm::vec3(mat[col][0], mat[col][1], mat[col][2]);
|
||||
}
|
||||
|
||||
glm::vec3 util::GetTranslationFromMatrix(const glm::mat4& mat) {
|
||||
return glm::vec3(mat[3][0], mat[3][1], mat[3][2]);
|
||||
}
|
||||
|
||||
float util::Clamp(float value, float min, float max) {
|
||||
return value < min ? min : (value > max ? max : value);
|
||||
}
|
||||
|
||||
// Print out a column major matrix.
|
||||
void util::PrintMatrix(const glm::mat4& matrix) {
|
||||
int i;
|
||||
for (i = 0; i < 4; i++) {
|
||||
LOGI("[ %f, %f, %f, %f ]", matrix[0][i], matrix[1][i], matrix[2][i],
|
||||
matrix[3][i]);
|
||||
}
|
||||
LOGI(" ");
|
||||
}
|
||||
|
||||
void util::PrintVector(const glm::vec3& vector) {
|
||||
LOGI("[ %f, %f, %f ]", vector[0], vector[1], vector[2]);
|
||||
LOGI(" ");
|
||||
}
|
||||
|
||||
void util::PrintQuaternion(const glm::quat& quat) {
|
||||
LOGI("[ %f, %f, %f, %f ]", quat[0], quat[1], quat[2], quat[3]);
|
||||
LOGI(" ");
|
||||
}
|
||||
|
||||
glm::vec3 util::LerpVector(const glm::vec3& x, const glm::vec3& y, float a) {
|
||||
return x * (1.0f - a) + y * a;
|
||||
}
|
||||
|
||||
float util::DistanceSquared(const glm::vec3& v1, const glm::vec3& v2) {
|
||||
glm::vec3 delta = v2 - v1;
|
||||
return glm::dot(delta, delta);
|
||||
}
|
||||
|
||||
bool util::SegmentAABBIntersect(const glm::vec3& aabb_min,
|
||||
const glm::vec3& aabb_max,
|
||||
const glm::vec3& start,
|
||||
const glm::vec3& end) {
|
||||
float tmin, tmax, tymin, tymax, tzmin, tzmax;
|
||||
glm::vec3 direction = end - start;
|
||||
if (direction.x >= 0) {
|
||||
tmin = (aabb_min.x - start.x) / direction.x;
|
||||
tmax = (aabb_max.x - start.x) / direction.x;
|
||||
} else {
|
||||
tmin = (aabb_max.x - start.x) / direction.x;
|
||||
tmax = (aabb_min.x - start.x) / direction.x;
|
||||
}
|
||||
if (direction.y >= 0) {
|
||||
tymin = (aabb_min.y - start.y) / direction.y;
|
||||
tymax = (aabb_max.y - start.y) / direction.y;
|
||||
} else {
|
||||
tymin = (aabb_max.y - start.y) / direction.y;
|
||||
tymax = (aabb_min.y - start.y) / direction.y;
|
||||
}
|
||||
if ((tmin > tymax) || (tymin > tmax)) return false;
|
||||
|
||||
if (tymin > tmin) tmin = tymin;
|
||||
if (tymax < tmax) tmax = tymax;
|
||||
if (direction.z >= 0) {
|
||||
tzmin = (aabb_min.z - start.z) / direction.z;
|
||||
tzmax = (aabb_max.z - start.z) / direction.z;
|
||||
} else {
|
||||
tzmin = (aabb_max.z - start.z) / direction.z;
|
||||
tzmax = (aabb_min.z - start.z) / direction.z;
|
||||
}
|
||||
if ((tmin > tzmax) || (tzmin > tmax)) return false;
|
||||
|
||||
if (tzmin > tmin) tmin = tzmin;
|
||||
if (tzmax < tmax) tmax = tzmax;
|
||||
// Use the full length of the segment.
|
||||
return ((tmin < 1.0f) && (tmax > 0));
|
||||
}
|
||||
|
||||
glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
|
||||
return glm::vec3(mat * glm::vec4(vec, 1.0f));
|
||||
}
|
||||
|
||||
} // namespace tango_gl
|
||||
Reference in New Issue
Block a user