graph_drawable.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <sstream>
29 
30 #include "graph_drawable.h"
32 #include "util.h"
33 
34 #include <GLES2/gl2.h>
35 
37  GLuint shaderProgram,
38  const std::map<int, rtabmap::Transform> & poses,
39  const std::multimap<int, rtabmap::Link> & links) :
40  vertex_buffers_(0),
41  pose_(1.0f),
42  visible_(true),
43  lineWidth_(3.0f),
44  shader_program_(shaderProgram)
45 {
46  UASSERT(!poses.empty());
47 
48  glGenBuffers(1, &vertex_buffers_);
49 
50  if(vertex_buffers_)
51  {
52  LOGI("Creating vertex buffer %d", vertex_buffers_);
53  std::vector<float> vertices = std::vector<float>(poses.size()*3);
54  int i=0;
55  std::map<int, int> idsToIndices;
56  for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
57  {
58  vertices[i*3] = iter->second.x();
59  vertices[i*3+1] = iter->second.y();
60  vertices[i*3+2] = iter->second.z();
61  idsToIndices.insert(std::make_pair(iter->first, i));
62  ++i;
63  }
64 
65  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
66  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
67  glBindBuffer(GL_ARRAY_BUFFER, 0);
68 
69  GLint error = glGetError();
70  if(error != GL_NO_ERROR)
71  {
72  LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
73  vertex_buffers_ = 0;
74  }
75  else if(links.size())
76  {
77  neighborIndices_.resize(links.size() * 2);
78  loopClosureIndices_.resize(links.size() * 2);
79  int oiNeighbors = 0;
80  int oiLoopClosures = 0;
81  for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
82  {
83  std::map<int, int>::const_iterator jterFrom = idsToIndices.find(iter->second.from());
84  std::map<int, int>::const_iterator jterTo = idsToIndices.find(iter->second.to());
85  if(jterFrom != idsToIndices.end() && jterTo != idsToIndices.end())
86  {
87  if(iter->second.type() == rtabmap::Link::kNeighbor)
88  {
89  neighborIndices_[oiNeighbors++] = (unsigned short)jterFrom->second;
90  neighborIndices_[oiNeighbors++] = (unsigned short)jterTo->second;
91  }
92  else
93  {
94  loopClosureIndices_[oiLoopClosures++] = (unsigned short)jterFrom->second;
95  loopClosureIndices_[oiLoopClosures++] = (unsigned short)jterTo->second;
96  }
97  }
98  }
99  neighborIndices_.resize(oiNeighbors);
100  loopClosureIndices_.resize(oiLoopClosures);
101  }
102  }
103 }
104 
106 {
107  LOGI("Freeing cloud buffer %d", vertex_buffers_);
108  if (vertex_buffers_)
109  {
110  glDeleteBuffers(1, &vertex_buffers_);
111  tango_gl::util::CheckGlError("GraphDrawable::~GraphDrawable()");
112  vertex_buffers_ = 0;
113  }
114 }
115 
117 {
118  UASSERT(!pose.isNull());
119 
120  pose_ = glmFromTransform(pose);
121 }
122 
123 void GraphDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix) {
124 
125  if(vertex_buffers_ && (neighborIndices_.size() || loopClosureIndices_.size()) && visible_)
126  {
127  glUseProgram(shader_program_);
128  glLineWidth(lineWidth_);
129 
130  GLuint mvp_handle_ = glGetUniformLocation(shader_program_, "mvp");
131  glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
132  glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
133 
134  GLuint color_handle = glGetUniformLocation(shader_program_, "color");
135 
136  GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
137 
138  glEnableVertexAttribArray(attribute_vertex);
139  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
140  glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 3*sizeof(GLfloat), 0);
141 
142  if(neighborIndices_.size())
143  {
144  glUniform3f(color_handle, 1.0f, 0.0f, 0.0f); // blue for neighbors
145  glDrawElements(GL_LINES, neighborIndices_.size(), GL_UNSIGNED_SHORT, neighborIndices_.data());
146  }
147  if(loopClosureIndices_.size())
148  {
149  glUniform3f(color_handle, 0.0f, 0.0f, 1.0f); // red for loop closures
150  glDrawElements(GL_LINES, loopClosureIndices_.size(), GL_UNSIGNED_SHORT, loopClosureIndices_.data());
151  }
152 
153  glDisableVertexAttribArray(0);
154  glBindBuffer(GL_ARRAY_BUFFER, 0);
155 
156  glUseProgram(0);
157  tango_gl::util::CheckGlError("GraphDrawable::Render()");
158  }
159 }
160 
glm::mat4 glmFromTransform(const rtabmap::Transform &transform)
Definition: util.h:122
void Render(const glm::mat4 &projectionMatrix, const glm::mat4 &viewMatrix)
void setPose(const rtabmap::Transform &mapToOdom)
f
std::vector< GLushort > loopClosureIndices_
static const float vertices[]
Definition: quad.cpp:39
GLuint vertex_buffers_
glm::mat4 pose_
unsigned int GLuint
Definition: dummy.cpp:78
GLM_FUNC_DECL genType::value_type const * value_ptr(genType const &vec)
#define UASSERT(condition)
GraphDrawable(GLuint shaderProgram, const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links)
#define true
Definition: ConvertUTF.c:57
#define GL_FALSE
Definition: dummy.cpp:79
bool isNull() const
Definition: Transform.cpp:107
virtual ~GraphDrawable()
std::vector< GLushort > neighborIndices_
#define LOGI(...)
void glUniformMatrix4fv(GLuint, int, int, float *)
Definition: dummy.cpp:80
GLuint shader_program_
void CheckGlError(const char *operation)
Definition: util.cpp:43
ULogger class and convenient macros.


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58