feat(slam): add rtabmap_ros

2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
@@ -0,0 +1,75 @@
+/*
+ * 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/axis.h"
+#include "tango-gl/shaders.h"
+
+namespace tango_gl {
+
+static const float float_vertices[] = {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+                                       0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+                                       0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
+
+static const float float_colors[] = {
+    1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f,
+    0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f};
+
+Axis::Axis() : Line(3.0f, GL_LINES) {
+  // Implement SetShader here, not using the dedault one.
+  shader_program_ =
+      util::CreateProgram(shaders::GetColorVertexShader().c_str(),
+                          shaders::GetBasicFragmentShader().c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  uniform_mvp_mat_ = glGetUniformLocation(shader_program_, "mvp");
+  attrib_colors_ = glGetAttribLocation(shader_program_, "color");
+  attrib_vertices_ = glGetAttribLocation(shader_program_, "vertex");
+
+  size_t size = sizeof(float_vertices) / (sizeof(float) * 3);
+  for (size_t i = 0; i < size; i++) {
+    vec_vertices_.push_back(glm::vec3(float_vertices[i * 3],
+                                      float_vertices[i * 3 + 1],
+                                      float_vertices[i * 3 + 2]));
+    vec_colors_.push_back(
+        glm::vec4(float_colors[i * 4], float_colors[i * 4 + 1],
+                  float_colors[i * 4 + 2], float_colors[i * 4 + 3]));
+  }
+}
+
+void Axis::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+  glLineWidth(line_width_);
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
+                        sizeof(glm::vec3), &vec_vertices_[0]);
+
+  glEnableVertexAttribArray(attrib_colors_);
+  glVertexAttribPointer(attrib_colors_, 4, GL_FLOAT, GL_FALSE,
+                        sizeof(glm::vec4), &vec_colors_[0]);
+
+  glDrawArrays(render_mode_, 0, vec_vertices_.size());
+
+  glDisableVertexAttribArray(attrib_vertices_);
+  glDisableVertexAttribArray(attrib_colors_);
+  glUseProgram(0);
+}
+}  // namespace tango_gl
@@ -0,0 +1,149 @@
+/*
+ * 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/band.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+
+// Set band resolution to 0.01m(1cm) when using UpdateVertexArray()
+static const float kMinDistanceSquared = 0.0001f;
+
+Band::Band(const unsigned int max_length)
+    : band_width_(0.2), max_length_(max_length) {
+  SetShader();
+  vertices_v_.reserve(max_length);
+  pivot_left = glm::vec3(0, 0, 0);
+  pivot_right = glm::vec3(0, 0, 0);
+}
+
+void Band::SetWidth(const float width) {
+  band_width_ = width;
+}
+
+void Band::UpdateVertexArray(const glm::mat4 m, BandMode mode) {
+  // First 2 vertices of a band + 3 arrow head vertices.
+  bool need_to_initialize = (vertices_v_.size() < 5);
+
+  bool sufficient_delta = false;
+  if (!need_to_initialize) {
+    // Band head is the first two vertices after arrow head.
+    glm::vec3 band_front = 0.5f * (vertices_v_[vertices_v_.size() - 4] +
+                                   vertices_v_[vertices_v_.size() - 5]);
+    sufficient_delta = kMinDistanceSquared <
+        util::DistanceSquared(band_front, util::GetTranslationFromMatrix(m));
+  }
+
+  if (need_to_initialize || sufficient_delta) {
+    glm::vec3 left  = glm::vec3(-band_width_ * 0.5f, 0, 0);
+    glm::vec3 right = glm::vec3(band_width_ * 0.5f, 0, 0);
+    glm::vec3 arrow_left = glm::vec3(-band_width_ * 0.75f, 0, 0);
+    glm::vec3 arrow_right = glm::vec3(band_width_ * 0.75f, 0, 0);
+    glm::vec3 arrow_front = glm::vec3(0, 0, -band_width_ * 0.75f);
+
+    // If keep right pivot point, or normal mode,
+    // then only update left pivot point.
+    if (mode == BandMode::kNormal || mode == BandMode::kKeepRight) {
+      pivot_left = util::ApplyTransform(m, left);
+    }
+    // If keep left pivot point, or normal mode,
+    // then only update right pivot point.
+    if (mode == BandMode::kNormal || mode == BandMode::kKeepLeft) {
+      pivot_right = util::ApplyTransform(m, right);
+    }
+
+    glm::mat4 head_m = m;
+
+    if (mode != BandMode::kNormal) {
+      glm::vec3 up = glm::vec3(0, 1.0f, 0);
+      glm::vec3 position = 0.5f * (pivot_left + pivot_right);
+      glm::vec3 heading = glm::cross(up, pivot_right-pivot_left);
+      head_m = glm::inverse(glm::lookAt(glm::vec3(0, 0, 0), heading, up));
+      head_m[3][0] = position.x;
+      head_m[3][1] = position.y;
+      head_m[3][2] = position.z;
+    }
+
+    if (need_to_initialize) {
+      vertices_v_.resize(5);
+    } else {
+      vertices_v_.resize(vertices_v_.size() + 2);
+    }
+
+    size_t insertion_start = vertices_v_.size() - 5;
+    vertices_v_[insertion_start + 0] = pivot_left;
+    vertices_v_[insertion_start + 1] = pivot_right;
+    vertices_v_[insertion_start + 2] = util::ApplyTransform(head_m, arrow_left);
+    vertices_v_[insertion_start + 3] = util::ApplyTransform(head_m, arrow_right);
+    vertices_v_[insertion_start + 4] = util::ApplyTransform(head_m, arrow_front);
+
+    if (vertices_v_.size() > max_length_) {
+      vertices_v_.erase(vertices_v_.begin(), vertices_v_.begin() + 2);
+    }
+  }
+}
+
+void Band::UpdateVertexArray(const glm::mat4 m) {
+  // Defualt to call update with normal mode.
+  UpdateVertexArray(m, BandMode::kNormal);
+}
+
+void Band::SetVertexArray(const std::vector<glm::vec3>& v,
+                          const glm::vec3& up) {
+  vertices_v_.clear();
+  vertices_v_.reserve(2 * v.size());
+  if (v.size() < 2)
+    return;
+
+  for (size_t i = 0; i < v.size() - 1; ++i) {
+    glm::vec3 gl_p_world_a = v[i];
+    glm::vec3 gl_p_world_b = v[i + 1];
+    glm::vec3 dir = glm::normalize(gl_p_world_b - gl_p_world_a);
+    glm::vec3 left = glm::cross(up, dir);
+    glm::normalize(left);
+
+    vertices_v_.push_back(gl_p_world_a + (band_width_ / 2.0f * left));
+    vertices_v_.push_back(gl_p_world_a - (band_width_ / 2.0f * left));
+
+    // Cap the end of the path.
+    if (i == v.size() - 2) {
+      vertices_v_.push_back(gl_p_world_b + (band_width_ / 2.0f * left));
+      vertices_v_.push_back(gl_p_world_b - (band_width_ / 2.0f * left));
+    }
+  }
+
+}
+
+void Band::ClearVertexArray() { vertices_v_.clear(); }
+
+void Band::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
+
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
+                        sizeof(glm::vec3), &vertices_v_[0]);
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, vertices_v_.size());
+  glDisableVertexAttribArray(attrib_vertices_);
+  glUseProgram(0);
+}
+
+}  // namespace tango_gl
@@ -0,0 +1,62 @@
+#include "tango-gl/bounding_box.h"
+
+namespace tango_gl {
+BoundingBox::BoundingBox(const std::vector<float>& vertices) {
+  // Set min and max to the first vertice.
+  bounding_min_ = glm::vec3(vertices[0], vertices[1], vertices[2]);
+  bounding_max_ = bounding_min_;
+  size_t vertices_count = vertices.size() / 3;
+  for (size_t i = 1; i < vertices_count; i += 3) {
+    bounding_min_.x = std::min(vertices[i * 3], bounding_min_.x);
+    bounding_min_.y = std::min(vertices[i * 3 + 1], bounding_min_.y);
+    bounding_min_.z = std::min(vertices[i * 3 + 2], bounding_min_.z);
+
+    bounding_max_.x = std::max(vertices[i * 3], bounding_max_.x);
+    bounding_max_.y = std::max(vertices[i * 3 + 1], bounding_max_.y);
+    bounding_max_.z = std::max(vertices[i * 3 + 2], bounding_max_.z);
+  }
+}
+
+bool BoundingBox::IsIntersecting(const Segment& segment,
+                                 const glm::quat& rotation,
+                                 const glm::mat4& transformation) {
+  // The current bounding box.
+  glm::vec3 min, max;
+
+  // If the mesh has been rotated, we need to derive a new bounding box
+  // based on the original one, if it just been translated or scaled,
+  // we can still use the original one with current model matrix applied.
+  if (rotation == glm::quat(1.0f, 0.0f, 0.0f, 0.0f)) {
+    min = util::ApplyTransform(transformation, bounding_min_);
+    max = util::ApplyTransform(transformation, bounding_max_);
+  } else {
+    std::vector<glm::vec3> box;
+    // Derive 8 vertices of the new bounding box from original min and max.
+    box.push_back(bounding_min_);
+    box.push_back(bounding_max_);
+
+    box.push_back(glm::vec3(bounding_min_.x, bounding_max_.y, bounding_max_.z));
+    box.push_back(glm::vec3(bounding_max_.x, bounding_min_.y, bounding_min_.z));
+
+    box.push_back(glm::vec3(bounding_min_.x, bounding_min_.y, bounding_max_.z));
+    box.push_back(glm::vec3(bounding_max_.x, bounding_max_.y, bounding_min_.z));
+
+    box.push_back(glm::vec3(bounding_max_.x, bounding_min_.y, bounding_max_.z));
+    box.push_back(glm::vec3(bounding_min_.x, bounding_max_.y, bounding_min_.z));
+
+    min = util::ApplyTransform(transformation, bounding_min_);
+    max = min;
+    for (size_t i = 1; i < box.size(); i++) {
+      glm::vec3 temp = util::ApplyTransform(transformation, box[i]);
+      min.x = std::min(temp.x, min.x);
+      min.y = std::min(temp.y, min.y);
+      min.z = std::min(temp.z, min.z);
+
+      max.x = std::max(temp.x, max.x);
+      max.y = std::max(temp.y, max.y);
+      max.z = std::max(temp.z, max.z);
+    }
+  }
+  return util::SegmentAABBIntersect(min, max, segment.start, segment.end);
+}
+}  // namespace tango_gl
@@ -0,0 +1,83 @@
+/*
+ * 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/camera.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+
+Camera::Camera() {
+  field_of_view_ = 45.0f * DEGREE_2_RADIANS;
+  aspect_ratio_ = 4.0f / 3.0f;
+  width_ = 800.0f;
+  height_ = 600.0f;
+  near_clip_plane_ = 0.5f;
+  far_clip_plane_ = 50.0f;
+  ortho_ = false;
+  orthoScale_ = 2.0f;
+  orthoCropFactor_ = -1.0f;
+}
+
+glm::mat4 Camera::GetViewMatrix() {
+  return glm::inverse(GetTransformationMatrix());
+}
+
+glm::mat4 Camera::GetProjectionMatrix() {
+	if(ortho_)
+	{
+		return glm::ortho(-orthoScale_*aspect_ratio_, orthoScale_*aspect_ratio_, -orthoScale_, orthoScale_, orthoScale_ + orthoCropFactor_, far_clip_plane_);
+	}
+  return glm::perspective(field_of_view_, aspect_ratio_, near_clip_plane_, far_clip_plane_);
+}
+
+void Camera::SetWindowSize(float width, float height) {
+	width_ = width;
+	height_ = height;
+  aspect_ratio_ = width/height;
+}
+
+void Camera::SetFieldOfView(float fov) {
+  field_of_view_ = fov * DEGREE_2_RADIANS;
+}
+
+void Camera::SetNearFarClipPlanes(const float near, const float far)
+{
+	near_clip_plane_ = near;
+	far_clip_plane_ = far;
+}
+
+Camera::~Camera() {
+}
+
+glm::mat4 Camera::ProjectionMatrixForCameraIntrinsics(float width, float height,
+                                                      float fx, float fy,
+                                                      float cx, float cy,
+                                                      float near, float far) {
+  const float xscale = near / fx;
+  const float yscale = near / fy;
+
+  const float xoffset =  (cx - (width  / 2.0)) * xscale;
+  // Color camera's coordinates has y pointing downwards so we negate this term.
+  const float yoffset = -(cy - (height / 2.0)) * yscale;
+
+  return  glm::frustum(xscale * -width  / 2.0f - xoffset,
+                       xscale *  width  / 2.0f - xoffset,
+                       yscale * -height / 2.0f - yoffset,
+                       yscale *  height / 2.0f - yoffset,
+                       near, far);
+}
+
+}  // namespace tango_gl
@@ -0,0 +1,36 @@
+/*
+ * 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/circle.h"
+
+namespace tango_gl {
+Circle::Circle(float radius, int resolution) : Mesh(GL_TRIANGLE_FAN){
+  SetShader();
+  std::vector<GLfloat> vertices;
+  vertices.reserve(3 * (resolution + 2));
+  vertices.push_back(0);
+  vertices.push_back(0);
+  vertices.push_back(0);
+  float delta_theta = M_PI * 2.0f / static_cast<float>(resolution);
+  for (int i = resolution; i >= 0; i--) {
+    float theta = delta_theta * static_cast<float>(i);
+    vertices.push_back(cos(theta) * radius);
+    vertices.push_back(sin(theta) * radius);
+    vertices.push_back(0);
+  }
+  SetVertices(vertices);
+}
+}  // namespace tango_gl
@@ -0,0 +1,52 @@
+/*
+ * 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/conversions.h"
+
+namespace tango_gl {
+namespace conversions {
+
+glm::mat4 opengl_world_T_tango_world() {
+  // Note glm is column-wise.
+  return glm::mat4(1.0f, 0.0f,  0.0f, 0.0f,
+                   0.0f, 0.0f, -1.0f, 0.0f,
+                   0.0f, 1.0f,  0.0f, 0.0f,
+                   0.0f, 0.0f,  0.0f, 1.0f);
+}
+
+glm::mat4 color_camera_T_opengl_camera() {
+  // Note glm is column-wise.
+  return glm::mat4(1.0f,  0.0f,  0.0f, 0.0f,
+                   0.0f, -1.0f,  0.0f, 0.0f,
+                   0.0f,  0.0f, -1.0f, 0.0f,
+                   0.0f,  0.0f,  0.0f, 1.0f);
+}
+
+glm::mat4 depth_camera_T_opengl_camera() {
+  // Note glm is column-wise.
+  return glm::mat4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+}
+
+glm::quat QuatTangoToGl(const glm::quat& tango_q_frame) {
+  const float kSqrt2Over2 = std::sqrt(2.0) / 2.0f;
+  // Tango frame is a -90 degree rotation about +X from the GL frame.
+  glm::quat gl_q_tango = glm::quat(kSqrt2Over2, -kSqrt2Over2, 0.0f, 0.0f);
+  return gl_q_tango * tango_q_frame;
+}
+
+}  // namespace gl_tango_conversions
+}  // namespace tango_gl
@@ -0,0 +1,54 @@
+/*
+ * 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/cube.h"
+
+namespace tango_gl {
+
+static const GLfloat const_vertices[] = {
+    -1.0f, 1.0f,  1.0f,  -1.0f, -1.0f, 1.0f,  1.0f,  1.0f,  1.0f,  -1.0f, -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,
+    -1.0f, 1.0f,  1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f, -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f, 1.0f,
+    -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f, -1.0f,
+    1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  1.0f,  -1.0f, -1.0f, -1.0f,
+    -1.0f, -1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,
+    1.0f,  1.0f,  1.0f,  -1.0f, -1.0f, 1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,
+    1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f,
+    1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f};
+
+static const GLfloat const_normals[] = {
+    0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,
+    1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  1.0f,  0.0f,  0.0f,  1.0f,
+    0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,
+    1.0f,  0.0f,  0.0f,  0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,
+    -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, -1.0f,
+    0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,
+    -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,
+    0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,
+    1.0f,  0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,
+    0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f};
+
+Cube::Cube() {
+  SetShader(true);
+  std::vector<GLfloat> vertices(
+      const_vertices,
+      const_vertices + sizeof(const_vertices) / sizeof(GLfloat));
+  std::vector<GLfloat> normals(
+      const_normals, const_normals + sizeof(const_normals) / sizeof(GLfloat));
+  SetVertices(vertices, normals);
+}
+}  // namespace tango_gl
@@ -0,0 +1,66 @@
+/*
+ * 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/drawable_object.h"
+#include "tango-gl/shaders.h"
+
+namespace tango_gl {
+
+void DrawableObject::SetShader() {
+  shader_program_ =
+      util::CreateProgram(shaders::GetBasicVertexShader().c_str(),
+                          shaders::GetBasicFragmentShader().c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  uniform_mvp_mat_ = glGetUniformLocation(shader_program_, "mvp");
+  attrib_vertices_ = glGetAttribLocation(shader_program_, "vertex");
+  uniform_color_ = glGetUniformLocation(shader_program_, "color");
+}
+
+void DrawableObject::DeleteGlResources() {
+  if (shader_program_) {
+    glDeleteShader(shader_program_);
+  }
+}
+
+void DrawableObject::SetColor(float red, float green, float blue) {
+  red_ = red;
+  green_ = green;
+  blue_ = blue;
+}
+void DrawableObject::SetColor(const Color& color) {
+  SetColor(color.r, color.g, color.b);
+}
+
+void DrawableObject::SetAlpha(const float alpha) { alpha_ = alpha; }
+
+void DrawableObject::SetVertices(const std::vector<GLfloat>& vertices) {
+  vertices_ = vertices;
+}
+
+void DrawableObject::SetVertices(const std::vector<GLfloat>& vertices,
+                                 const std::vector<GLushort>& indices) {
+  vertices_ = vertices;
+  indices_ = indices;
+}
+
+void DrawableObject::SetVertices(const std::vector<GLfloat>& vertices,
+                                 const std::vector<GLfloat>& normals) {
+  vertices_ = vertices;
+  normals_ = normals;
+}
+}  // namespace tango_gl
@@ -0,0 +1,38 @@
+/*
+ * 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/frustum.h"
+
+namespace tango_gl {
+
+static const float float_vertices[] = {
+    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f,  -1.0f, -1.0f,
+	-1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f,
+	1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f,
+	-1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
+
+Frustum::Frustum() : Line(3.0f, GL_LINES) {
+  SetShader();
+  size_t size = sizeof(float_vertices) / sizeof(float);
+  for (size_t i = 0; i < size; i += 3) {
+    vec_vertices_.push_back(glm::vec3(float_vertices[i], float_vertices[i + 1],
+                                      float_vertices[i + 2]));
+  }
+}
+}  // namespace tango_gl
@@ -0,0 +1,254 @@
+/*
+ * 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/gesture_camera.h"
+#include "tango-gl/util.h"
+#include "glm/gtx/quaternion.hpp"
+
+namespace {
+// Render camera observation distance in third person camera mode.
+const float kThirdPersonCameraDist = 7.0f;
+
+// Render camera observation distance in third person camera mode.
+const float kThirdPersonFollowCameraDist = 2.0f;
+
+// Render camera observation distance in top down camera mode.
+const float kTopDownCameraDist = 5.0f;
+
+// Zoom in speed.
+const float kZoomSpeed = 10.0f;
+
+// Move speed
+const float kMoveSpeed = 10.0f;
+
+// Rotation speed
+const float kRotationSpeed = 2.0f;
+
+// Min/max clamp value of camera observation distance.
+const float kCamViewMinDist = .1f;
+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 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 {
+
+GestureCamera::GestureCamera() :
+		cam_cur_target_rot_(1,0,0,0),
+		start_touch_dist_(0.0f),
+		cur_touch_dist_(0.0f)
+{
+  cam_parent_transform_ = new Transform();
+  SetParent(cam_parent_transform_);
+}
+
+GestureCamera::~GestureCamera() { delete cam_parent_transform_; }
+
+void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
+		float y0, float x1, float y1) {
+
+	if (camera_type_!=kFirstPerson && touch_count == 1) {
+		switch (event) {
+		case kTouch0Down: {
+			cam_start_angle_ = cam_cur_angle_;
+
+			touch0_start_position_.x = x0;
+			touch0_start_position_.y = y0;
+			break;
+		}
+		case kTouchMove: {
+			glm::vec2 offset;
+
+			float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
+			float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
+
+			if(camera_type_!=kTopOrtho)
+				cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
+			cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
+
+			StartCameraToCurrentTransform();
+
+			break;
+		}
+		default: { break; }
+		}
+	}
+	if (touch_count == 2) {
+		switch (event) {
+		case kTouch1Down: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
+			cam_start_dist_ = GetPosition().z;
+			cam_start_fov_ = this->getFOV();
+
+			// center touch
+			touch0_start_position_.x = (x0+x1)/2.0f;
+			touch0_start_position_.y = (y0+y1)/2.0f;
+			break;
+		}
+		case kTouchMove: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
+
+			if(camera_type_ == kFirstPerson)
+			{
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, kLowestFov, kHighestFov));
+			}
+			else
+			{
+				cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+								kCamViewMinDist, kCamViewMaxDist);
+
+				this->SetOrthoMode(camera_type_ == kTopOrtho);
+				if(camera_type_ == kTopOrtho)
+				{
+					this->SetOrthoScale(cam_cur_dist_);
+				}
+
+				glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
+				glm::vec2 offset;
+				offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
+				offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
+				touch0_start_position_ = touch_center_position;
+
+				StartCameraToCurrentTransform();
+
+				anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			}
+			break;
+		}
+		default: { break; }
+		}
+	}
+}
+
+Segment GestureCamera::GetSegmentFromTouch(float normalized_x,
+                                           float normalized_y,
+                                           float touch_range) {
+  float screen_height = touch_range * (2.0f * glm::tan(field_of_view_ * 0.5f));
+  float screen_width = screen_height * aspect_ratio_;
+  // normalized_x and normalized_x are from OnTouchEvent, top-left corner of the
+  // screen
+  // is [0, 0], transform it to opengl frame.
+  normalized_x = normalized_x - 0.5f;
+  normalized_y = 0.5f - normalized_y;
+  glm::vec3 start =
+      util::ApplyTransform(GetTransformationMatrix(), glm::vec3(0, 0, 0));
+  glm::vec3 end = util::ApplyTransform(GetTransformationMatrix(), 
+      glm::vec3(normalized_x * screen_width, normalized_y * screen_height,
+                -touch_range));
+  Segment segment(start, end);
+  return segment;
+}
+
+void GestureCamera::SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation) {
+	// Anchor position
+	cam_parent_transform_->SetPosition(pos+anchor_offset_);
+
+	// Anchor rotation
+	if(camera_type_ == kThirdPersonFollow)
+	{
+		cam_cur_target_rot_ = rotation;
+		cam_cur_target_rot_.x = 0;
+		cam_cur_target_rot_.z = 0;
+		cam_cur_target_rot_ = glm::normalize(cam_cur_target_rot_);
+		StartCameraToCurrentTransform();
+	}
+}
+
+void GestureCamera::SetCameraType(CameraType camera_index) {
+  camera_type_ = camera_index;
+  switch (camera_index) {
+    case kFirstPerson:
+      SetOrthoMode(false);
+      SetFieldOfView(kLowestFov);
+      SetNearFarClipPlanes(0.25, 25);
+      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;
+      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+      cam_cur_angle_.x = 0.0f;
+      cam_cur_angle_.y = 0.0f;
+      cam_cur_target_rot_ = glm::quat(1,0,0,0);
+      cam_parent_transform_->SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+      StartCameraToCurrentTransform();
+      break;
+    case kThirdPerson:
+    case kThirdPersonFollow:
+      SetOrthoMode(false);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
+      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      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;
+      cam_cur_target_rot_ = glm::quat(1,0,0,0);
+      StartCameraToCurrentTransform();
+      break;
+    case kTopDown:
+      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      SetOrthoMode(false);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
+      cam_cur_dist_ = kTopDownCameraDist;
+      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+      cam_cur_angle_.x = -M_PI / 2.0f;
+      cam_cur_angle_.y = 0.0f;
+      cam_cur_target_rot_ = glm::quat(1,0,0,0);
+      StartCameraToCurrentTransform();
+      break;
+    case kTopOrtho:
+	  SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+	  SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	  SetOrthoMode(true);
+	  SetOrthoScale(kTopDownCameraDist);
+	  SetOrthoCropFactor(-1.0f);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
+	  cam_cur_dist_ = kTopDownCameraDist;
+	  anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+	  cam_cur_angle_.x = -M_PI / 2.0f;
+	  cam_cur_angle_.y = 0.0f;
+	  cam_cur_target_rot_ = glm::quat(1,0,0,0);
+	  StartCameraToCurrentTransform();
+	  break;
+    default:
+      break;
+  }
+}
+
+void GestureCamera::StartCameraToCurrentTransform()
+{
+  //Anchor rotation
+  glm::quat parent_cam_rot = glm::rotate(cam_cur_target_rot_, cam_cur_angle_.y, glm::vec3(0, 1, 0));
+  parent_cam_rot = glm::rotate(parent_cam_rot, cam_cur_angle_.x, glm::vec3(1, 0, 0));
+  cam_parent_transform_->SetRotation(parent_cam_rot);
+
+  //Camera position
+  SetPosition(glm::vec3(0, 0, cam_cur_dist_));
+}
+}  // namespace tango_gl
@@ -0,0 +1,59 @@
+
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ * Distributed under the Project Tango Preview Development Kit (PDK) Agreement.
+ * CONFIDENTIAL. AUTHORIZED USE ONLY. DO NOT REDISTRIBUTE.
+ */
+
+#include "tango-gl/goal_marker.h"
+
+namespace tango_gl {
+
+static const GLfloat const_vertices[] = {
+    -4.5298f,  -0.2676f,  0.0,       -4.3209f,  -1.3857f,  0.0,      -3.7242f,
+    -11.1445f, 0.0,       -5.6799f,  -8.9811f,  0.0,       -4.4540f, 0.8673f,
+    0.0,       -7.8376f,  -6.4508f,  0.0,       -9.5223f,  -3.0538f, 0.0,
+    -9.9826f,  -0.5898f,  0.0,       -9.8157f,  1.9113f,   0.0,      -9.0320f,
+    4.2923f,   0.0,       -7.6808f,  6.4036f,   0.0,       -5.8469f, 8.1125f,
+    0.0,       -3.6457f,  9.3117f,   0.0,       -4.0984f,  1.9477f,  0.0,
+    -1.2155f,  9.9259f,   0.0,       -3.4853f,  2.9057f,   0.0,      1.2912f,
+    9.9163f,   0.0,       -2.6532f,  3.6812f,   0.0,       3.7167f,  9.2837f,
+    0.0,       -1.6543f,  4.2254f,   0.0,       5.9087f,   8.0677f,  0.0,
+    -0.5515f,  4.5040f,   0.0,       7.7294f,   6.3448f,   0.0,      0.5859f,
+    4.4997f,   0.0,       9.0645f,   4.2232f,   0.0,       1.6865f,  4.2126f,
+    0.0,       9.8300f,   1.8363f,   0.0,       2.6812f,   3.6609f,  0.0,
+    9.9778f,   -0.6660f,  0.0,       3.5074f,   2.8791f,   0.0,      9.4987f,
+    -3.1264f,  0.0,       4.1132f,   1.9164f,   0.0,       7.7964f,  -6.5204f,
+    0.0,       4.4605f,   0.8333f,   0.0,       5.6245f,   -9.0444f, 0.0,
+    4.5276f,   -0.3022f,  0.0,       3.6572f,   -11.1925f, 0.0,      4.3102f,
+    -1.4187f,  0.0,       1.5165f,   -13.5960f, 0.0,       3.8220f,  -2.4460f,
+    0.0,       -0.0382f,  -16.4245f, 0.0,       3.0936f,   -3.3197f, 0.0,
+    -1.5902f,  -13.5657f, 0.0,       2.1709f,   -3.9847f,  0.0,      -3.8405f,
+    -2.4168f,  0.0,       -3.1189f,  -3.2959f,  0.0,       -2.2012f, -3.9680f,
+    0.0,       -1.1452f,  -4.3908f,  0.0,       -0.0173f,  -4.5377f, 0.0,
+    1.1117f,   -4.3994f,  0.0};
+
+static const GLushort const_indices[] = {
+    1,  2,  3,  1,  3,  4,  5,  1,  4,  4,  6,  7,  5,  4,  7,  7,  8,  9,  9,
+    10, 11, 7,  9,  11, 5,  7,  11, 5,  11, 12, 5,  12, 13, 14, 5,  13, 14, 13,
+    15, 16, 14, 15, 16, 15, 17, 18, 16, 17, 18, 17, 19, 20, 18, 19, 20, 19, 21,
+    22, 20, 21, 22, 21, 23, 24, 22, 23, 24, 23, 25, 26, 24, 25, 26, 25, 27, 28,
+    26, 27, 28, 27, 29, 30, 28, 29, 30, 29, 31, 32, 30, 31, 32, 31, 33, 34, 32,
+    33, 34, 33, 35, 36, 34, 35, 36, 35, 37, 38, 36, 37, 38, 37, 39, 40, 38, 39,
+    40, 39, 41, 42, 40, 41, 42, 41, 43, 44, 42, 43, 44, 43, 3,  3,  2,  45, 3,
+    45, 46, 3,  46, 47, 3,  47, 48, 3,  48, 49, 3,  49, 50, 44, 3,  50};
+
+GoalMarker::GoalMarker() {
+  SetShader();
+  std::vector<GLfloat> vertices(
+      const_vertices,
+      const_vertices + sizeof(const_vertices) / sizeof(GLfloat));
+  std::vector<GLushort> indices(
+      const_indices, const_indices + sizeof(const_indices) / sizeof(GLushort));
+  // Change indices so they are zero-indexed.
+  for (size_t i = 0; i < indices.size(); ++i) {
+    indices[i] = indices[i] - 1;
+  }
+  SetVertices(vertices, indices);
+}
+}  // namespace tango_gl
@@ -0,0 +1,47 @@
+/*
+ * 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/grid.h"
+
+namespace tango_gl {
+
+// Initialize Grid with x and y grid count,
+// qx, quantity in x
+// qy, quantity in y.
+Grid::Grid(float density, int qx, int qy) : Line(1.0f, GL_LINES) {
+  SetShader();
+
+  // 3 float in 1 vertex, 2 vertices form a line.
+  // Horizontal line and vertical line forms the grid.
+  float width = density * qx / 2;
+  float height = density * qy / 2;
+
+  // Horizontal line.
+  for (int i = 0; i < (qy + 1); i++) {
+	  for (int j = 0; j < (qx + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + j*density, 0.0f, -height + i * density));
+		vec_vertices_.push_back(glm::vec3(-width+ + (j+1)*density, 0.0f, -height + i * density));
+	  }
+  }
+
+  for (int i = 0; i < (qx + 1); i++) {
+	  for (int j = 0; j < (qy + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + j*density));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + (j+1)*density));
+	  }
+  }
+}
+}  // namespace tango_gl
@@ -0,0 +1,32 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_AXIS_H_
+#define TANGO_GL_AXIS_H_
+
+#include "tango-gl/line.h"
+
+namespace tango_gl {
+class Axis : public Line {
+ public:
+  Axis();
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+ private:
+  GLuint attrib_colors_;
+  std::vector<glm::vec4> vec_colors_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_AXIS_H_
@@ -0,0 +1,55 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_BAND_H_
+#define TANGO_GL_BAND_H_
+
+#include <stdlib.h>
+#include <vector>
+
+#include "tango-gl/drawable_object.h"
+
+namespace tango_gl {
+class Band : public DrawableObject {
+ public:
+  enum BandMode {
+    kNormal = 0,
+    kKeepLeft = 1,
+    kKeepRight = 2
+  };
+
+  Band(const unsigned int max_legnth);
+
+  void SetWidth(const float width);
+  // Render a Band with arrow head, pass in the mode for rendering,
+  // kKeepLeft is left turn, kKeepRight is right turn,
+  // when making a turn, vertices only get updated in one side to avoid overlapping.
+  void UpdateVertexArray(const glm::mat4 m, BandMode mode);
+  void UpdateVertexArray(const glm::mat4 m);
+  void SetVertexArray(const std::vector<glm::vec3>& v, const glm::vec3& up);
+  void ClearVertexArray();
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+
+ private:
+  float band_width_;
+  unsigned int max_length_;
+  std::vector<glm::vec3> vertices_v_;
+  // Current band head's left and right position in world frame.
+  glm::vec3 pivot_left;
+  glm::vec3 pivot_right;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_BAND_H_
@@ -0,0 +1,42 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_BOUNDING_BOX_H_
+#define TANGO_GL_BOUNDING_BOX_H_
+
+#include <vector>
+
+#include "tango-gl/segment.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+class BoundingBox {
+ public:
+  BoundingBox()
+      : bounding_min_(glm::vec3(0, 0, 0)), bounding_max_(glm::vec3(0, 0, 0)) {}
+  BoundingBox(const std::vector<float>& vertices);
+  BoundingBox(const glm::vec3& min, const glm::vec3& max)
+      : bounding_min_(min), bounding_max_(max) {}
+  bool IsIntersecting(const Segment& segment, const glm::quat& rotation,
+                      const glm::mat4& transformation);
+
+ private:
+  // Axis-aligned bounding box minimum and maximum point.
+  glm::vec3 bounding_min_;
+  glm::vec3 bounding_max_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_BOUNDING_BOX_H_
@@ -0,0 +1,69 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_CAMERA_H_
+#define TANGO_GL_CAMERA_H_
+
+#include "tango-gl/transform.h"
+
+namespace tango_gl {
+class Camera : public Transform {
+ public:
+  Camera();
+  Camera(const Camera& other) = delete;
+  Camera& operator=(const Camera&) = delete;
+  ~Camera();
+
+  void SetWindowSize(const float width, const float height);
+  void SetFieldOfView(const float fov);
+  void SetOrthoMode(bool enabled) {ortho_ = enabled;}
+  void SetOrthoScale(float scale) {orthoScale_ = scale;}
+  void SetOrthoCropFactor(float value) {orthoCropFactor_ = value;}
+  void SetNearFarClipPlanes(const float near, const float far);
+
+  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.
+   *
+   * @param width  - The width of the camera image.
+   * @param height - The height of the camera image.
+   * @param fx     - The x-axis focal length of the camera.
+   * @param fy     - The y-axis focal length of the camera.
+   * @param cx     - The x-coordinate principal point in pixels.
+   * @param cy     - The y-coordinate principal point in pixels.
+   * @param near   - The desired near z-clipping plane.
+   * @param far    - The desired far z-clipping plane.
+   */
+  static glm::mat4 ProjectionMatrixForCameraIntrinsics(float width, float height,
+                                                       float fx,    float fy,
+                                                       float cx,    float cy,
+                                                       float near,  float far);
+ protected:
+  float field_of_view_;
+  float aspect_ratio_;
+  float width_;
+  float height_;
+  float near_clip_plane_, far_clip_plane_;
+  bool ortho_;
+  float orthoScale_;
+  float orthoCropFactor_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_CAMERA_H_
@@ -0,0 +1,28 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_CIRCLE_H_
+#define TANGO_GL_CIRCLE_H_
+
+#include "tango-gl/mesh.h"
+
+namespace tango_gl {
+class Circle : public Mesh {
+ public:
+  Circle(float radius, int resolution);
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_CIRCLE_H_
@@ -0,0 +1,33 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_COLOR_H_
+#define TANGO_GL_COLOR_H_
+
+namespace tango_gl {
+class Color {
+ public:
+  Color() : r(0), g(0), b(0) {}
+  Color(float red, float green, float blue) : r(red), g(green), b(blue) {}
+  Color(const Color&) = default;
+  Color& operator=(const Color&) = default;
+
+  float r;
+  float g;
+  float b;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_COLOR_H_
@@ -0,0 +1,132 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_GL_TANGO_CONVERSIONS_H_
+#define TANGO_GL_GL_TANGO_CONVERSIONS_H_
+
+#define GLM_FORCE_RADIANS
+
+#include "glm/glm.hpp"
+#include "glm/gtc/matrix_transform.hpp"
+#include "glm/gtc/quaternion.hpp"
+
+namespace tango_gl {
+namespace conversions {
+
+/**
+ * @brief Creates a glm::vec3 from double[3] = {x, y, z}. This is designed to
+ * work with the TangoPoseData.translation field.
+ */
+inline glm::vec3 Vec3FromArray(const double* array) {
+  return glm::vec3(array[0], array[1], array[2]);
+}
+
+/**
+ * @brief Creates a glm::quat from double[4] = {x, y, z, w}. This is designed to
+ * work with the TangoPoseData.orientation field.
+ */
+inline glm::quat QuatFromArray(const double* array) {
+  // Note GLM expects arguments in order {w, x, y, z}.
+  return glm::quat(array[3], array[0], array[1], array[2]);
+}
+
+/**
+ * @brief Creates a glm::mat4 rigid-frame transformation matrix from two arrays.
+ * This is designed for the TangoPoseData translation and orientation fields.
+ * @param A_p_B Position [x, y, z] of B_origin from A_origin, expressed in A.
+ * @param A_q_B The quaternion representation [x, y, z, w] of the rotation
+ * matrix A_R_B.
+ * @return The transformation matrix A_T_B.
+ */
+inline glm::mat4 TransformFromArrays(const double* A_p_B, const double* A_q_B) {
+  glm::vec3 glm_A_p_B = Vec3FromArray(A_p_B);
+  glm::quat glm_A_q_B = QuatFromArray(A_q_B);
+  return glm::translate(glm::mat4(1.0f), glm_A_p_B) * glm::mat4_cast(glm_A_q_B);
+}
+
+/**
+ * @brief Creates a glm::mat4 rigid-frame transformation matrix from glm::vec3 and glm::quat.
+ * This is designed for the TangoPoseData translation and orientation fields.
+ * @param A_p_B A position vector of B_origin from A_origin, expressed in A.
+ * @param A_q_B A quaternion representation of the rotation.
+ * matrix A_R_B.
+ * @return The transformation matrix A_T_B.
+ */
+inline glm::mat4 TransformFromVecAndQuat(const glm::vec3& A_p_B, const glm::quat& A_q_B) {
+  return glm::translate(glm::mat4(1.0f), A_p_B) * glm::mat4_cast(A_q_B);
+}
+
+/**
+ * @brief Convert (re-express, or rotate) a vector from the Tango ADF (or start-
+ * of-service) frame convention [right, forward, up] to the typical OpenGl world
+ * frame convention [right, up, backward]. Note this assumes the two frames are
+ * coincident, and it doesn't know about any additional offsets between a
+ * particular OpenGl scene and the Tango service frames.
+ * @param tango_vec A vector expressed in the Tango ADF frame convention.
+ * @return The same vector expressed using the Opengl frame convention.
+ */
+inline glm::vec3 Vec3TangoToGl(const glm::vec3& tango_vec) {
+  return glm::vec3(tango_vec.x, tango_vec.z, -tango_vec.y);
+}
+
+/**
+ * @brief Convert (re-express, or rotate) a vector from the typical OpenGl world
+ * frame convention [right, up, backward] to the Tango ADF (or start-of-service)
+ * frame convention [right, forward, up]. Note this assumes the two frames are
+ * coincident, and it doesn't know about any additional offsets between a
+ * particular OpenGl scene and the Tango service frames.
+ * @param gl_vec A vector expressed in the Opengl world frame convention.
+ * @return The same vector expressed using the Tango ADF frame convention.
+ */
+inline glm::vec3 Vec3GlToTango(const glm::vec3& gl_vec) {
+  return glm::vec3(gl_vec.x, -gl_vec.z, gl_vec.y);
+}
+
+/**
+ * @brief Given a quaternion representing the rotation matrix tango_R_any,
+ * returns the quaternion representing gl_R_any, where "any" is an arbitrary
+ * frame. Note the gl base frame is rotated by 90-degrees about +X from the
+ * Tango ADF/start-of-service frame, so this is equivalent to applying such a
+ * rotation to the quaternion.
+ * @param tango_q_any A quaternion representing rotation matrix tango_R_any.
+ * @return The quaternion representing gl_R_any.
+ */
+glm::quat QuatTangoToGl(const glm::quat& tango_q_any);
+
+/**
+ * Get the fixed transformation matrix relating the opengl frame convention
+ * (with Y-up, X-right) and the tango frame convention for the start-of-service
+ * and ADF frames (with Z-up, X-right), termed "world" here.
+ */
+glm::mat4 opengl_world_T_tango_world();
+
+/**
+ * Get the fixed transformation matrix relating the frame convention of the
+ * device's color camera frame (with Z-forward, X-right) and the opengl camera
+ * frame (with Z-backward, X-right).
+ */
+glm::mat4 color_camera_T_opengl_camera();
+
+/**
+ * Get the fixed transformation matrix relating the frame convention of the
+ * device's depth camera frame (with Z-forward, X-right) and the opengl camera
+ * frame (with Z-backward, X-right).
+ */
+glm::mat4 depth_camera_T_opengl_camera();
+
+}  // namespace conversions
+}  // namespace tango_gl
+#endif  // TANGO_GL_GL_TANGO_CONVERSIONS_H_
@@ -0,0 +1,28 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_CUBE_H_
+#define TANGO_GL_CUBE_H_
+
+#include "tango-gl/mesh.h"
+
+namespace tango_gl {
+class Cube : public Mesh {
+ public:
+  Cube();
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_CUBE_H_
@@ -0,0 +1,63 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_DRAWABLE_OBJECT_H_
+#define TANGO_GL_DRAWABLE_OBJECT_H_
+
+#include <vector>
+
+#include "tango-gl/color.h"
+#include "tango-gl/transform.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+class DrawableObject : public Transform {
+ public:
+  DrawableObject() : red_(0), green_(0), blue_(0), alpha_(1.0f) {};
+  DrawableObject(const DrawableObject& other) = delete;
+  const DrawableObject& operator=(const DrawableObject&) = delete;
+
+  void DeleteGlResources();
+  void SetShader();
+  void SetColor(const Color& color);
+  void SetColor(const float red, const float green, const float blue);
+  void SetAlpha(const float alpha);
+  void SetVertices(const std::vector<GLfloat>& vertices);
+  void SetVertices(const std::vector<GLfloat>& vertices,
+                   const std::vector<GLushort>& indices);
+  void SetVertices(const std::vector<GLfloat>& vertices,
+                   const std::vector<GLfloat>& normals);
+  virtual void Render(const glm::mat4& projection_mat,
+                      const glm::mat4& view_mat) const = 0;
+
+ protected:
+  float red_;
+  float green_;
+  float blue_;
+  float alpha_;
+  std::vector<GLushort> indices_;
+  std::vector<GLfloat> vertices_;
+  std::vector<GLfloat> normals_;
+
+  GLenum render_mode_;
+  GLuint shader_program_;
+  GLuint uniform_color_;
+  GLuint uniform_mvp_mat_;
+  GLuint attrib_vertices_;
+  GLuint attrib_normals_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_DRAWABLE_OBJECT_H_
@@ -0,0 +1,28 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_FRUSTUM_H_
+#define TANGO_GL_FRUSTUM_H_
+
+#include "tango-gl/line.h"
+
+namespace tango_gl {
+class Frustum : public Line {
+ public:
+  Frustum();
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_FRUSTUM_H_
@@ -0,0 +1,94 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_GESTURE_CAMERA_H_
+#define TANGO_GL_GESTURE_CAMERA_H_
+
+#include "tango-gl/camera.h"
+#include "tango-gl/segment.h"
+#include "tango-gl/transform.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+class GestureCamera : public Camera {
+ public:
+  enum CameraType {
+    kFirstPerson = 0,
+    kThirdPersonFollow = 1,
+    kTopDown = 2,
+	kTopOrtho = 3,
+    kThirdPerson = 4
+  };
+
+  enum TouchEvent {
+    kTouch0Down = 0,
+    kTouch0Up = 1,
+    kTouchMove = 2,
+    kTouch1Down = 5,
+    kTouch1Up = 6,
+    kTouchNone = -1
+  };
+
+  GestureCamera();
+  ~GestureCamera();
+
+  void OnTouchEvent(int touch_count, TouchEvent event, float x0, float y0,
+                    float x1, float y1);
+
+  // Get the ray in opengl world frame given the 2d touch position on screen,
+  // normalized touch_x and normalized touch_y should be the same value get from
+  // OnTouchEvent, x0 and y0, touch_range is the depth of the touch in
+  // camera frame.
+  Segment GetSegmentFromTouch(float normalized_x, float normalized_y,
+                              float touch_range);
+
+  void SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation);
+  void SetAnchorOffset(const glm::vec3& pos) {anchor_offset_ = pos;}
+  const glm::vec3& GetAnchorOffset() const {return anchor_offset_;}
+
+  void SetCameraDistance(float cameraDistance) {cam_cur_dist_ = cameraDistance;}
+  float GetCameraDistance() const {return cam_cur_dist_;}
+
+  // Set camera type, set render camera's parent position and rotation.
+  void SetCameraType(CameraType camera_index);
+
+  CameraType GetCameraType() const { return camera_type_; }
+  float getFOV() const {return field_of_view_ * RADIAN_2_DEGREE;}
+
+ private:
+  void StartCameraToCurrentTransform();
+
+  // Render camera's parent transformation.
+  Transform* cam_parent_transform_;
+
+  CameraType camera_type_;
+
+  glm::vec2 cam_start_angle_;
+  glm::vec2 cam_cur_angle_;
+  glm::quat cam_cur_target_rot_;
+
+  float cam_start_dist_;
+  float cam_start_fov_;
+  float cam_cur_dist_;
+  glm::vec3 anchor_offset_;
+
+  float start_touch_dist_;
+  float cur_touch_dist_;
+
+  glm::vec2 touch0_start_position_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_GESTURE_CAMERA_H_
@@ -0,0 +1,18 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ * Distributed under the Project Tango Preview Development Kit (PDK) Agreement.
+ * CONFIDENTIAL. AUTHORIZED USE ONLY. DO NOT REDISTRIBUTE.
+ */
+
+#ifndef TANGO_GL_GOAL_MARKER_H_
+#define TANGO_GL_GOAL_MARKER_H_
+
+#include <tango-gl/mesh.h>
+
+namespace tango_gl {
+class GoalMarker : public Mesh {
+ public:
+  GoalMarker();
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_GOAL_MARKER_H_
@@ -0,0 +1,28 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_GRID_H_
+#define TANGO_GL_GRID_H_
+
+#include "tango-gl/line.h"
+
+namespace tango_gl {
+class Grid : public Line {
+ public:
+  Grid(float density = 1.0f, int qx = 50, int qy = 50);
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_GRID_H_
@@ -0,0 +1,37 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_LINE_H_
+#define TANGO_GL_LINE_H_
+
+#include "tango-gl/drawable_object.h"
+
+namespace tango_gl {
+class Line : public DrawableObject {
+ public:
+  Line(float line_width, GLenum render_mode);
+  void SetLineWidth(const float pixels);
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+  void UpdateLineVertices(const std::vector<glm::vec3>& vec_vertices) {
+    vec_vertices_ = vec_vertices;
+  }
+
+ protected:
+  float line_width_;
+  std::vector<glm::vec3> vec_vertices_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_LINE_H_
@@ -0,0 +1,45 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_MESH_H_
+#define TANGO_GL_MESH_H_
+
+#include "tango-gl/bounding_box.h"
+#include "tango-gl/drawable_object.h"
+#include "tango-gl/segment.h"
+
+namespace tango_gl {
+class Mesh : public DrawableObject {
+ public:
+  Mesh();
+  Mesh(GLenum render_mode);
+  void SetShader();
+  void SetShader(bool is_lighting_on);
+  void SetBoundingBox();
+  void SetLightDirection(const glm::vec3& light_direction);
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+  bool IsIntersecting(const Segment& segment);
+
+ protected:
+  BoundingBox* bounding_box_;
+  bool is_lighting_on_;
+  bool is_bounding_box_on_;
+  glm::vec3 light_direction_;
+  GLuint uniform_mv_mat_;
+  GLuint uniform_light_vec_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_MESH_H_
@@ -0,0 +1,61 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_OBJ_LOADER_H
+#define TANGO_GL_OBJ_LOADER_H
+
+#include <vector>
+
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+namespace obj_loader {
+//  Load standard .obj file into vertices, indices or normals vectors,
+//  OBJ file can be exported from 3D tools like 3ds Max or Blender.
+//  A readable file with only vertices should look like
+//  "v 1.00 2.00 3.00
+//   ...
+//   f 1 2 3
+//   f 1 2 3 4
+//   ..."
+//
+//  If exported with normals, file should look like
+//  "v 1.00 2.00 3.00
+//   ...
+//   f 1//1 2//3 3//4
+//   f 1//1 2//3 3//4 4//6
+//   ...
+//   vn 1.00 2.00 3.00
+//   ..."
+//  this can be used with Mesh:
+//
+//  std::vector<GLfloat> vertices;
+//  std::vector<GLushort> indices;
+//  std::vector<GLfloat> normals;
+//  tango_gl::obj_loader::LoadOBJData("/sdcard/model.obj", vertices, indices);
+//  mesh->SetVertices(vertices, indices);
+//  or
+//  tango_gl::obj_loader::LoadOBJData("/sdcard/model.obj", vertices, normals);
+//  mesh->SetVertices(vertices, normals);
+
+bool LoadOBJData(const char* path, std::vector<GLfloat>& vertices,
+                 std::vector<GLushort>& indices);
+
+bool LoadOBJData(const char* path, std::vector<GLfloat>& vertices,
+                 std::vector<GLfloat>& normals);
+}  // namespace obj_loader
+}  // namespace tango_gl
+#endif  // TANGO_GL_OBJ_LOADER
@@ -0,0 +1,44 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_QUAD_H_
+#define TANGO_GL_QUAD_H_
+
+#include "tango-gl/drawable_object.h"
+
+namespace tango_gl {
+class Quad : public DrawableObject {
+ public:
+  Quad();
+  Quad(const Quad& other) = delete;
+  Quad& operator=(const Quad&) = delete;
+  ~Quad();
+
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+  void SetTextureId(GLuint texture_id);
+
+ private:
+  GLuint vertex_buffer_;
+  GLuint shader_program_;
+  GLuint attrib_vertices_;
+  GLuint texture_coords_;
+  GLuint texture_handle;
+  GLuint uniform_mvp_mat_;
+
+  GLuint texture_id_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_QUAD_H_
@@ -0,0 +1,35 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_SEGMENT_H_
+#define TANGO_GL_SEGMENT_H_
+
+#include "glm/glm.hpp"
+
+namespace tango_gl {
+class Segment {
+ public:
+  Segment() : start(glm::vec3(0, 0, 0)), end(glm::vec3(0, 0, 0)) {}
+  Segment(const glm::vec3& segment_start, const glm::vec3& segment_end)
+      : start(segment_start), end(segment_end) {}
+  Segment(const Segment&) = default;
+  Segment& operator=(const Segment&) = default;
+
+  glm::vec3 start;
+  glm::vec3 end;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_SEGMENT_H_
@@ -0,0 +1,30 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_SEGMENT_DRAWABLE_H_
+#define TANGO_GL_SEGMENT_DRAWABLE_H_
+
+#include "tango-gl/line.h"
+#include "tango-gl/segment.h"
+
+namespace tango_gl {
+class SegmentDrawable : public Line {
+ public:
+  SegmentDrawable();
+  void UpdateSegment(const Segment& segment);
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_SEGMENT_DRAWABLE_H_
@@ -0,0 +1,32 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_SHADERS_H_
+#define TANGO_GL_SHADERS_H_
+
+#include <string>
+
+namespace tango_gl {
+namespace shaders {
+std::string GetBasicVertexShader();
+std::string GetBasicFragmentShader();
+std::string GetColorVertexShader();
+std::string GetVideoOverlayVertexShader();
+std::string GetVideoOverlayFragmentShader();
+std::string GetShadedVertexShader();
+}  // namespace shaders
+}  // namespace tango_gl
+#endif  // TANGO_GL_SHADERS_H_
@@ -0,0 +1,50 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_TEXTURE_H_
+#define TANGO_GL_TEXTURE_H_
+
+#include <errno.h>
+
+#ifdef __ANDROID__
+#include <png.h>
+#endif
+
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+class Texture {
+ public:
+  Texture(const char* file_path);
+  Texture(const Texture& other) = delete;
+  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_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_TEXTURE_H_
@@ -0,0 +1,30 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_TRACE_H_
+#define TANGO_GL_TRACE_H_
+
+#include "tango-gl/line.h"
+
+namespace tango_gl {
+class Trace : public Line {
+ public:
+  Trace();
+  void UpdateVertexArray(const glm::vec3& v);
+  void ClearVertexArray();
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_TRACE_H_
@@ -0,0 +1,59 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_TRANSFORM_H_
+#define TANGO_GL_TRANSFORM_H_
+
+#include "glm/glm.hpp"
+#include "glm/gtc/quaternion.hpp"
+
+namespace tango_gl {
+class Transform {
+ public:
+  Transform();
+  virtual ~Transform();
+
+  Transform(const Transform& other) = delete;
+  const Transform& operator=(const Transform& rhs) = delete;
+
+  void SetPosition(const glm::vec3& position);
+  glm::vec3 GetPosition() const;
+
+  void SetRotation(const glm::quat& rotation);
+  glm::quat GetRotation() const;
+
+  void SetScale(const glm::vec3& scale);
+  glm::vec3 GetScale() const;
+
+  void Translate(const glm::vec3& translation);
+
+  void SetTransformationMatrix(const glm::mat4& transform_mat);
+  glm::mat4 GetTransformationMatrix() const;
+
+  void SetParent(Transform* transform);
+
+  const Transform* GetParent() const ;
+  Transform* GetParent() ;
+
+ private:
+  Transform* parent_;
+
+  glm::vec3 position_;
+  glm::quat rotation_;
+  glm::vec3 scale_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_TRANSFORM_H_
@@ -0,0 +1,28 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_TRIANGLE_H_
+#define TANGO_GL_TRIANGLE_H_
+
+#include "tango-gl/mesh.h"
+
+namespace tango_gl {
+class Triangle : public Mesh {
+ public:
+  Triangle();
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_TRIANGLE_H_
@@ -0,0 +1,106 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_GL_UTIL_H_
+#define TANGO_GL_GL_UTIL_H_
+#define GLM_FORCE_RADIANS
+
+#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"
+#include "glm/gtc/quaternion.hpp"
+#include "glm/gtc/type_ptr.hpp"
+#include "glm/gtx/matrix_decompose.hpp"
+
+#define LOG_TAG "rtabmap"
+#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
+#endif
+
+#define RADIAN_2_DEGREE 57.2957795f
+#define DEGREE_2_RADIANS 0.0174532925f
+
+namespace tango_gl {
+namespace util {
+  void CheckGlError(const char* operation);
+
+  GLuint CreateProgram(const char* vertex_source,
+                       const char* fragment_source);
+
+  void DecomposeMatrix(const glm::mat4& transform_mat,
+                       glm::vec3& translation,
+                       glm::quat& rotation,
+                       glm::vec3& scale);
+
+  // Get a 3x1 column from the upper 3x4 of a transformation matrix. Columns
+  // 0, 1, 2 are the rotation/scale portion, and column 3 is the translation.
+  glm::vec3 GetColumnFromMatrix(const glm::mat4& mat, const int col);
+
+  // Get the translation component of a transformation matrix.
+  glm::vec3 GetTranslationFromMatrix(const glm::mat4& mat);
+
+  float Clamp(float value, float min, float max);
+  void PrintMatrix(const glm::mat4& matrix);
+  void PrintVector(const glm::vec3& vector);
+  void PrintQuaternion(const glm::quat& quat);
+
+  glm::vec3 LerpVector(const glm::vec3& x, const glm::vec3& y, float a);
+  float DistanceSquared(const glm::vec3& v1, const glm::vec3& v2);
+
+  bool SegmentAABBIntersect(const glm::vec3& aabb_min, const glm::vec3& aabb_max,
+                        const glm::vec3& start, const glm::vec3& end);
+
+  glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec);
+
+}  // namespace util
+}  // namespace tango_gl
+#endif  // TANGO_GL_RENDERER_GL_UTIL
@@ -0,0 +1,39 @@
+/*
+ * 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.
+ */
+
+#ifndef TANGO_GL_RENDERER_VIDEO_OVERLAY_H_
+#define TANGO_GL_RENDERER_VIDEO_OVERLAY_H_
+
+#include "tango-gl/drawable_object.h"
+
+namespace tango_gl {
+class VideoOverlay : public DrawableObject {
+ public:
+  VideoOverlay();
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+  GLuint GetTextureId() const { return texture_id_; }
+  void SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }
+
+ private:
+  // This id is populated on construction, and is passed to the tango service.
+  GLuint texture_id_;
+
+  GLuint attrib_texture_coords_;
+  GLuint uniform_texture_;
+  GLuint vertex_buffers_[3];
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_RENDERER_VIDEO_OVERLAY_H_
@@ -0,0 +1,44 @@
+/*
+ * 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/line.h"
+
+namespace tango_gl {
+Line::Line(float line_width, GLenum render_mode) {
+  line_width_ = line_width;
+  render_mode_ = render_mode;
+}
+void Line::SetLineWidth(const float pixels) { line_width_ = pixels; }
+void Line::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+  glLineWidth(line_width_);
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
+
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
+                        sizeof(glm::vec3), &vec_vertices_[0]);
+  glDrawArrays(render_mode_, 0, vec_vertices_.size());
+
+  glDisableVertexAttribArray(attrib_vertices_);
+  glUseProgram(0);
+}
+
+}  // namespace tango_gl
@@ -0,0 +1,124 @@
+/*
+ * 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/mesh.h"
+#include "tango-gl/shaders.h"
+
+namespace tango_gl {
+Mesh::Mesh() {
+  render_mode_ = GL_TRIANGLES;
+}
+Mesh::Mesh(GLenum render_mode) {
+  render_mode_ = render_mode;
+}
+
+void Mesh::SetShader() {
+  DrawableObject::SetShader();
+  // Default mode set to no lighting.
+  is_lighting_on_ = false;
+  // Default mode set to without bounding box detection.
+  is_bounding_box_on_ = false;
+}
+
+void Mesh::SetShader(bool is_lighting_on) {
+  if (is_lighting_on) {
+    shader_program_ =
+        util::CreateProgram(shaders::GetShadedVertexShader().c_str(),
+                            shaders::GetBasicFragmentShader().c_str());
+    if (!shader_program_) {
+      LOGE("Could not create program.");
+    }
+    uniform_mvp_mat_ = glGetUniformLocation(shader_program_, "mvp");
+    uniform_mv_mat_ = glGetUniformLocation(shader_program_, "mv");
+    uniform_light_vec_ = glGetUniformLocation(shader_program_, "lightVec");
+    uniform_color_ = glGetUniformLocation(shader_program_, "color");
+
+    attrib_vertices_ = glGetAttribLocation(shader_program_, "vertex");
+    attrib_normals_ = glGetAttribLocation(shader_program_, "normal");
+    is_lighting_on_ = true;
+    // Set a defualt direction for directional light.
+    light_direction_ = glm::vec3(-1.0f, -3.0f, -1.0f);
+    light_direction_ = glm::normalize(light_direction_);
+  } else {
+    SetShader();
+  }
+}
+
+void Mesh::SetBoundingBox(){
+  // Traverse all the vertices to define an axis-aligned
+  // bounding box for this mesh, needs to be called after SetVertices().
+  if(vertices_.size()==0){
+    LOGE("Please set up vertices first!");
+    return;
+  }
+  is_bounding_box_on_ = true;
+  bounding_box_ = new BoundingBox(vertices_);
+}
+
+void Mesh::SetLightDirection(const glm::vec3& light_direction) {
+  light_direction_ = light_direction;
+}
+
+bool Mesh::IsIntersecting(const Segment& segment) {
+  // If there is no bounding box defined based on all vertices,
+  // we can not calculate intersection.
+  if (!is_bounding_box_on_) {
+    LOGE("Mesh::IsIntersecting, bounding box is not available.");
+    return false;
+  }
+  return bounding_box_->IsIntersecting(segment, GetRotation(),
+                                       GetTransformationMatrix());
+}
+
+void Mesh::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mv_mat = view_mat * model_mat;
+  glm::mat4 mvp_mat = projection_mat * mv_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+  glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
+
+  if (is_lighting_on_) {
+    glUniformMatrix4fv(uniform_mv_mat_, 1, GL_FALSE, glm::value_ptr(mv_mat));
+
+    glEnableVertexAttribArray(attrib_normals_);
+    glVertexAttribPointer(attrib_normals_, 3, GL_FLOAT, GL_FALSE,
+                          3 * sizeof(GLfloat), &normals_[0]);
+    glm::vec3 light_direction = glm::mat3(view_mat) * light_direction_;
+    glUniform3fv(uniform_light_vec_, 1, glm::value_ptr(light_direction));
+  }
+
+  glEnableVertexAttribArray(attrib_vertices_);
+
+  if (!indices_.empty()) {
+    glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
+                          3 * sizeof(GLfloat), vertices_.data());
+    glDrawElements(render_mode_, indices_.size(), GL_UNSIGNED_SHORT,
+                   indices_.data());
+  } else {
+    glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
+                          3 * sizeof(GLfloat), &vertices_[0]);
+    glDrawArrays(render_mode_, 0, vertices_.size() / 3);
+  }
+
+  glDisableVertexAttribArray(attrib_vertices_);
+  if (is_lighting_on_) {
+    glDisableVertexAttribArray(attrib_normals_);
+  }
+  glUseProgram(0);
+}
+}  // namespace tango_gl
@@ -0,0 +1,156 @@
+/*
+ * 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 <string>
+
+#include "tango-gl/obj_loader.h"
+
+namespace tango_gl {
+bool obj_loader::LoadOBJData(const char* path, std::vector<GLfloat>& vertices,
+                             std::vector<GLushort>& indices) {
+  FILE *file = fopen(path, "r");
+  if (file == NULL) {
+    LOGE("Failed to open file: %s", path);
+    return false;
+  }
+
+  while (1) {
+    char lineHeader[128];
+    int res = fscanf(file, "%s", lineHeader);
+    if (res == EOF) break;
+    if (strcmp(lineHeader, "v") == 0) {
+      GLfloat vertex[3];
+      int matches =
+          fscanf(file, "%f %f %f\n", &vertex[0], &vertex[1], &vertex[2]);
+      if (matches != 3) {
+        LOGE("Format of 'v float float float' required for each vertice line");
+        return false;
+      }
+      vertices.push_back(vertex[0]);
+      vertices.push_back(vertex[1]);
+      vertices.push_back(vertex[2]);
+    } else if (strcmp(lineHeader, "f") == 0) {
+      GLushort vertexIndex[3];
+      int matches = fscanf(file, "%hu %hu %hu\n", &vertexIndex[0],
+                           &vertexIndex[1], &vertexIndex[2]);
+      if (matches != 3) {
+        LOGE("Format of 'f int int int' required for each face line");
+        return false;
+      }
+      indices.push_back(vertexIndex[0] - 1);
+      indices.push_back(vertexIndex[1] - 1);
+      indices.push_back(vertexIndex[2] - 1);
+    }
+    else {
+      char comments_buffer[1000];
+      fgets(comments_buffer, 1000, file);
+    }
+  }
+  fclose(file);
+  return true;
+}
+
+bool obj_loader::LoadOBJData(const char* path, std::vector<GLfloat>& vertices,
+                             std::vector<GLfloat>& normals) {
+  std::vector<unsigned int> vertexIndices, normalIndices;
+  std::vector<GLfloat> temp_vertices, temp_normals;
+
+  FILE* file = fopen(path, "r");
+  if (file == NULL) {
+    LOGE("Failed to open file: %s", path);
+    return false;
+  }
+
+  while (1) {
+    char lineHeader[128];
+    int res = fscanf(file, "%s", lineHeader);
+    if (res == EOF) break;
+    if (strcmp(lineHeader, "v") == 0) {
+      GLfloat vertex[3];
+      int matches =
+          fscanf(file, "%f %f %f\n", &vertex[0], &vertex[1], &vertex[2]);
+      if (matches != 3) {
+        LOGE("Format of 'v float float float' required for each vertice line");
+        return false;
+      }
+      temp_vertices.push_back(vertex[0]);
+      temp_vertices.push_back(vertex[1]);
+      temp_vertices.push_back(vertex[2]);
+    } else if (strcmp(lineHeader, "vn") == 0) {
+      GLfloat normal[3];
+      int matches =
+          fscanf(file, "%f %f %f\n", &normal[0], &normal[1], &normal[2]);
+      if (matches != 3) {
+        LOGE("Format of 'vn float float float' required for each normal line");
+        return false;
+      }
+      temp_normals.push_back(normal[0]);
+      temp_normals.push_back(normal[1]);
+      temp_normals.push_back(normal[2]);
+    } else if (strcmp(lineHeader, "f") == 0) {
+      GLushort vertexIndex[4];
+      GLushort normalIndex[4];
+      int matches = fscanf(file, "%hu//%hu %hu//%hu %hu//%hu %hu//%hu\n",
+                           &vertexIndex[0], &normalIndex[0], &vertexIndex[1], &normalIndex[1],
+                           &vertexIndex[2], &normalIndex[2], &vertexIndex[3], &normalIndex[3]);
+
+      // .obj file is 1-indexed, so subtract 1 from all indices.
+      if (matches == 6) {
+        // If triangles provided.
+        vertexIndices.push_back(vertexIndex[0] - 1);
+        vertexIndices.push_back(vertexIndex[1] - 1);
+        vertexIndices.push_back(vertexIndex[2] - 1);
+        normalIndices.push_back(normalIndex[0] - 1);
+        normalIndices.push_back(normalIndex[1] - 1);
+        normalIndices.push_back(normalIndex[2] - 1);
+      } else if(matches == 8) {
+        // If quads provided.
+        vertexIndices.push_back(vertexIndex[0] - 1);
+        vertexIndices.push_back(vertexIndex[1] - 1);
+        vertexIndices.push_back(vertexIndex[2] - 1);
+        vertexIndices.push_back(vertexIndex[0] - 1);
+        vertexIndices.push_back(vertexIndex[2] - 1);
+        vertexIndices.push_back(vertexIndex[3] - 1);
+        normalIndices.push_back(normalIndex[0] - 1);
+        normalIndices.push_back(normalIndex[1] - 1);
+        normalIndices.push_back(normalIndex[2] - 1);
+        normalIndices.push_back(normalIndex[0] - 1);
+        normalIndices.push_back(normalIndex[2] - 1);
+        normalIndices.push_back(normalIndex[3] - 1);
+      } else {
+        LOGE("Format of 'f int//int int//int int//int' required for each face");
+        return false;
+      }
+    } else {
+      char comments_buffer[1000];
+      fgets(comments_buffer, 1000, file);
+    }
+  }
+
+  for (unsigned int i = 0; i < vertexIndices.size(); i++) {
+    unsigned int vertexIndex = vertexIndices[i];
+    unsigned int normalIndex = normalIndices[i];
+
+    vertices.push_back(temp_vertices[vertexIndex * 3]);
+    vertices.push_back(temp_vertices[vertexIndex * 3 + 1]);
+    vertices.push_back(temp_vertices[vertexIndex * 3 + 2]);
+    normals.push_back(temp_normals[normalIndex * 3]);
+    normals.push_back(temp_normals[normalIndex * 3 + 1]);
+    normals.push_back(temp_normals[normalIndex * 3 + 2]);
+  }
+  fclose(file);
+  return true;
+}
+}  // namespace tango_gl
@@ -0,0 +1,91 @@
+/*
+ * 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/quad.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+
+static const char kVertexShader[] =
+    "attribute vec4 vertex;\n"
+    "attribute vec2 inputTextureCoordinate;\n"
+    "varying vec2 textureCoordinate;\n"
+    "uniform mat4 mvp;\n"
+    "void main() {\n"
+    "  gl_Position = mvp*vertex;\n"
+    "  textureCoordinate = inputTextureCoordinate.xy;\n"
+    "}\n";
+
+static const char kFragmentShader[] =
+    "varying vec2 textureCoordinate;\n"
+    "uniform sampler2D inputTexture;\n"
+    "void main() {\n"
+    "  gl_FragColor = texture2D(inputTexture, textureCoordinate);\n"
+    "}\n";
+
+static const float vertices[] = {-0.5f, -0.5f, 0.5f, -0.5f,
+                                 -0.5f, 0.5f,  0.5f, 0.5f};
+
+static const GLfloat texture_coords[] = {0.0f, 1.0f, 1.0f, 1.0f,
+                                         0.0f, 0.0f, 1.0f, 0.0f, };
+
+Quad::Quad() {
+  shader_program_ = util::CreateProgram(kVertexShader, kFragmentShader);
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  uniform_mvp_mat_ = glGetUniformLocation(shader_program_, "mvp");
+  attrib_vertices_ = glGetAttribLocation(shader_program_, "vertex");
+  texture_coords_ =
+      glGetAttribLocation(shader_program_, "inputTextureCoordinate");
+  texture_handle = glGetUniformLocation(shader_program_, "inputTexture");
+  glGenBuffers(1, &vertex_buffer_);
+}
+
+Quad::~Quad() { glDeleteShader(shader_program_); }
+
+void Quad::SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }
+
+void Quad::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glEnable(GL_CULL_FACE);
+  glEnable(GL_BLEND);
+  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+  glUseProgram(shader_program_);
+
+  glBindTexture(GL_TEXTURE_2D, texture_id_);
+  glUniform1i(texture_handle, 0);
+
+  // Calculate MVP matrix and pass it to shader.
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  // Vertice binding
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 2, GL_FLOAT, GL_FALSE, 0, vertices);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  glEnableVertexAttribArray(texture_coords_);
+  glVertexAttribPointer(texture_coords_, 2, GL_FLOAT, GL_FALSE, 0,
+                        texture_coords);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+  glUseProgram(0);
+  glBindTexture(GL_TEXTURE_2D, 0);
+}
+
+}  // namespace tango_gl
@@ -0,0 +1,29 @@
+/*
+ * 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/segment_drawable.h"
+
+namespace tango_gl {
+SegmentDrawable::SegmentDrawable() : tango_gl::Line(5.0f, GL_LINES) {
+  SetShader();
+  vec_vertices_.push_back(glm::vec3(0, 0, 0));
+  vec_vertices_.push_back(glm::vec3(1.f, 1.f, 1.f));
+}
+void SegmentDrawable::UpdateSegment(const Segment& segment) {
+  vec_vertices_[0] = segment.start;
+  vec_vertices_[1] = segment.end;
+}
+}  // namespace tango_gl
@@ -0,0 +1,95 @@
+/*
+ * 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/shaders.h"
+
+namespace tango_gl {
+namespace shaders {
+std::string GetBasicVertexShader() {
+  return "precision mediump float;\n"
+         "precision mediump int;\n"
+         "attribute vec4 vertex;\n"
+         "uniform mat4 mvp;\n"
+         "uniform vec4 color;\n"
+         "varying vec4 v_color;\n"
+         "void main() {\n"
+         "  gl_Position = mvp*vertex;\n"
+         "  v_color = color;\n"
+         "}\n";
+}
+
+std::string GetBasicFragmentShader() {
+  return "precision mediump float;\n"
+         "varying vec4 v_color;\n"
+         "void main() {\n"
+         "  gl_FragColor = v_color;\n"
+         "}\n";
+}
+
+std::string GetColorVertexShader() {
+  return "precision mediump float;\n"
+         "precision mediump int;\n"
+         "attribute vec4 vertex;\n"
+         "attribute vec4 color;\n"
+         "uniform mat4 mvp;\n"
+         "varying vec4 v_color;\n"
+         "void main() {\n"
+         "  gl_Position = mvp*vertex;\n"
+         "  v_color = color;\n"
+         "}\n";
+}
+
+std::string GetVideoOverlayVertexShader() {
+  return "precision highp float;\n"
+         "precision highp int;\n"
+         "attribute vec4 vertex;\n"
+         "attribute vec2 textureCoords;\n"
+         "varying vec2 f_textureCoords;\n"
+         "uniform mat4 mvp;\n"
+         "void main() {\n"
+         "  f_textureCoords = textureCoords;\n"
+         "  gl_Position = mvp * vertex;\n"
+         "}\n";
+}
+
+std::string GetVideoOverlayFragmentShader() {
+  return "#extension GL_OES_EGL_image_external : require\n"
+         "precision highp float;\n"
+         "precision highp int;\n"
+         "uniform samplerExternalOES texture;\n"
+         "varying vec2 f_textureCoords;\n"
+         "void main() {\n"
+         "  gl_FragColor = texture2D(texture, f_textureCoords);\n"
+         "}\n";
+}
+
+std::string GetShadedVertexShader() {
+  return "attribute vec4 vertex;\n"
+         "attribute vec3 normal;\n"
+         "uniform mat4 mvp;\n"
+         "uniform mat4 mv;\n"
+         "uniform vec4 color;\n"
+         "uniform vec3 lightVec;\n"
+         "varying vec4 v_color;\n"
+         "void main() {\n"
+         "  vec3 mvNormal = vec3(mv * vec4(normal, 0.0));\n"
+         "  float diffuse = max(-dot(mvNormal, lightVec), 0.0);\n"
+         "  v_color.a = color.a;\n"
+         "  v_color.xyz = color.xyz * diffuse + color.xyz * 0.3;\n"
+         "  gl_Position = mvp*vertex;\n"
+         "}\n";
+}
+}  // namespace shaders
+}  // namespace tango_gl
@@ -0,0 +1,109 @@
+/*
+ * 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/texture.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+
+static const int kMaxExponentiation = 12;
+
+static int RoundUpPowerOfTwo(int w) {
+  int start = 2;
+  for (int i = 0; i <= kMaxExponentiation; ++i) {
+    if (w < start) {
+      w = start;
+      break;
+    } else {
+      start = start << 1;
+    }
+  }
+  return 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");
+
+  if (file == NULL) {
+    LOGE("fp not loaded: %s", strerror(errno));
+    return false;
+  }
+
+  fseek(file, 8, SEEK_CUR);
+
+  png_structp png_ptr =
+      png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+  png_infop info_ptr = png_create_info_struct(png_ptr);
+
+  png_init_io(png_ptr, file);
+  png_set_sig_bytes(png_ptr, 8);
+  png_read_info(png_ptr, info_ptr);
+  png_get_IHDR(png_ptr, info_ptr, &width_, &height_, &bit_depth_, &color_type_,
+               NULL, NULL, NULL);
+
+  width_ = RoundUpPowerOfTwo(width_);
+  height_ = RoundUpPowerOfTwo(height_);
+  int row = width_ * (color_type_ == PNG_COLOR_TYPE_RGBA ? 4 : 3);
+  byte_data_ = new char[row * height_];
+
+  png_bytep* row_pointers = new png_bytep[height_];
+  for (uint i = 0; i < height_; ++i) {
+    row_pointers[i] = (png_bytep)(byte_data_ + i * row);
+  }
+  png_read_image(png_ptr, row_pointers);
+  png_destroy_read_struct(&png_ptr, &info_ptr, 0);
+
+  glGenTextures(1, &texture_id_);
+  glBindTexture(GL_TEXTURE_2D, texture_id_);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+  util::CheckGlError("glBindTexture");
+  if (color_type_ == PNG_COLOR_TYPE_RGBA) {
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width_, height_, 0, GL_RGBA,
+                 GL_UNSIGNED_BYTE, byte_data_);
+  } else {
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB,
+                 GL_UNSIGNED_BYTE, byte_data_);
+  }
+  util::CheckGlError("glTexImage2D");
+  glBindTexture(GL_TEXTURE_2D, 0);
+
+  fclose(file);
+  delete[] row_pointers;
+  delete[] byte_data_;
+
+  return true;
+}
+#endif
+GLuint Texture::GetTextureID() const { return texture_id_; }
+
+Texture::~Texture() {
+  if (byte_data_ != NULL) {
+    delete[] byte_data_;
+  }
+  byte_data_ = NULL;
+}
+
+}  // namespace tango_gl
@@ -0,0 +1,38 @@
+/*
+ * 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/trace.h"
+
+namespace tango_gl {
+
+static const int kMaxTraceLength = 5000;
+static const float kDistanceCheck = 0.05f;
+
+Trace::Trace() : Line(3.0f, GL_LINE_STRIP) { SetShader(); }
+
+void Trace::UpdateVertexArray(const glm::vec3& v) {
+  if (vec_vertices_.size() == 0) {
+    vec_vertices_.push_back(v);
+  } else {
+    float dist = glm::distance(vec_vertices_[vec_vertices_.size() - 1], v);
+    if (dist >= kDistanceCheck) {
+      vec_vertices_.push_back(v);
+    }
+  }
+}
+
+void Trace::ClearVertexArray() { vec_vertices_.clear(); }
+}  // namespace tango_gl
@@ -0,0 +1,92 @@
+/*
+ * 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/transform.h"
+#include "tango-gl/util.h"
+
+#define nullptr 0
+
+namespace tango_gl {
+
+Transform::Transform()
+  : parent_(nullptr),
+    position_(0.0f, 0.0f, 0.0f),
+    rotation_(1.0f, 0.0f, 0.0f, 0.0f),
+    scale_(1.0f, 1.0f, 1.0f) {
+}
+
+Transform::~Transform() {
+  // Objects are not responsible for deleting their parents.
+}
+
+void Transform::SetPosition(const glm::vec3& position) {
+  position_ = position;
+}
+
+glm::vec3 Transform::GetPosition() const {
+  return position_;
+}
+
+void Transform::SetRotation(const glm::quat& rotation) {
+  rotation_ = rotation;
+}
+
+glm::quat Transform::GetRotation() const {
+  return rotation_;
+}
+
+void Transform::SetScale(const glm::vec3& scale) {
+  scale_ = scale;
+}
+
+glm::vec3 Transform::GetScale() const {
+  return scale_;
+}
+
+void Transform::Translate(const glm::vec3& translation) {
+  position_ += translation;
+}
+
+void Transform::SetTransformationMatrix(const glm::mat4& transform_mat) {
+  util::DecomposeMatrix(transform_mat, position_, rotation_, scale_);
+}
+
+glm::mat4 Transform::GetTransformationMatrix() const {
+  glm::mat4 trans_mat = glm::scale(glm::mat4_cast(rotation_), scale_);
+  trans_mat[3][0] = position_.x;
+  trans_mat[3][1] = position_.y;
+  trans_mat[3][2] = position_.z;
+  glm::mat4 parent_mat = glm::mat4(1.0f);
+  if (parent_ != NULL) {
+    parent_mat = parent_->GetTransformationMatrix();
+    trans_mat = parent_mat * trans_mat;
+  }
+  return  trans_mat;
+}
+
+void Transform::SetParent(Transform* transform) {
+  parent_ = transform;
+}
+
+const Transform* Transform::GetParent() const {
+  return parent_;
+}
+
+Transform* Transform::GetParent() {
+  return parent_;
+}
+
+}  // namespace tango_gl
@@ -0,0 +1,35 @@
+/*
+ * 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/triangle.h"
+
+namespace tango_gl {
+
+static const GLfloat const_vertices[]
+  = {-0.15f, 0.0f, 0.0f, 0.15f, 0.0f, 0.0f, 0.0f, 0.0f, -0.2f};
+
+static const GLushort const_indices[] = {0, 1, 2};
+
+Triangle::Triangle() {
+  SetShader();
+  std::vector<GLfloat> vertices(
+      const_vertices,
+      const_vertices + sizeof(const_vertices) / sizeof(GLfloat));
+  std::vector<GLushort> indices(
+      const_indices, const_indices + sizeof(const_indices) / sizeof(GLushort));
+  SetVertices(vertices, indices);
+}
+}  // namespace tango_gl
@@ -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
@@ -0,0 +1,121 @@
+/*
+ * 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/video_overlay.h"
+#include "tango-gl/shaders.h"
+
+namespace tango_gl {
+
+static const GLfloat kVertices[] =
+  {-1.0,  1.0, 0.0,
+   -1.0, -1.0, 0.0,
+    1.0,  1.0, 0.0,
+    1.0, -1.0, 0.0};
+
+static const GLushort kIndices[] =
+  {0, 1, 2, 2, 1, 3};
+
+static const GLfloat kTextureCoords[] =
+  {0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0};
+
+VideoOverlay::VideoOverlay() {
+  glEnable(GL_VERTEX_PROGRAM_POINT_SIZE);
+  shader_program_ =
+      util::CreateProgram(shaders::GetVideoOverlayVertexShader().c_str(),
+                          shaders::GetVideoOverlayFragmentShader().c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+
+  glGenTextures(1, &texture_id_);
+  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+  uniform_texture_ = glGetUniformLocation(shader_program_, "texture");
+
+  glGenBuffers(3, vertex_buffers_);
+  // Allocate vertices buffer.
+  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_[0]);
+  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * 3 * 4, kVertices,
+               GL_STATIC_DRAW);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  // Allocate triangle indices buffer.
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vertex_buffers_[1]);
+  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(GLushort) * 6, kIndices,
+               GL_STATIC_DRAW);
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+
+  // Allocate texture coordinates buufer.
+  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_[2]);
+  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * 2 * 4, kTextureCoords,
+               GL_STATIC_DRAW);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  // Assign the vertices attribute data.
+  attrib_vertices_ = glGetAttribLocation(shader_program_, "vertex");
+  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_[0]);
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  // Assign the texture coordinates attribute data.
+  attrib_texture_coords_ = glGetAttribLocation(shader_program_, "textureCoords");
+  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_[2]);
+  glEnableVertexAttribArray(attrib_texture_coords_);
+  glVertexAttribPointer(attrib_texture_coords_, 2, GL_FLOAT, GL_FALSE, 0,
+                        nullptr);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  uniform_mvp_mat_ = glGetUniformLocation(shader_program_, "mvp");
+}
+
+void VideoOverlay::Render(const glm::mat4& projection_mat,
+                          const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+
+  glUniform1i(uniform_texture_, 0);
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  // Bind vertices buffer.
+  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_[0]);
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  // Bind texture coordinates buffer.
+  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_[2]);
+  glEnableVertexAttribArray(attrib_texture_coords_);
+  glVertexAttribPointer(attrib_texture_coords_, 2, GL_FLOAT, GL_FALSE, 0,
+                        nullptr);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  // Bind element array buffer.
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vertex_buffers_[1]);
+  glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_SHORT, 0);
+  util::CheckGlError("glDrawElements");
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+
+  glUseProgram(0);
+  util::CheckGlError("glUseProgram()");
+}
+
+}  // namespace tango_gl