├── CMakeLists.txt ├── README.md ├── __resources ├── 1stEngineering_With_Camera_OUtliers3.png └── fr101_local_loop_enabled.png ├── include └── ColorMap.h ├── msg ├── EdgeSE2.msg ├── GraphSE2.msg └── VertexSE2.msg ├── package.xml └── src └── se2_graph_visualizer_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(graph_slam_visualizer) 3 | 4 | 5 | # check c++11 / c++0x(https://svn.boost.org/trac/boost/ticket/9240) 6 | include(CheckCXXCompilerFlag) 7 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 8 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 9 | if(COMPILER_SUPPORTS_CXX11) 10 | set(CMAKE_CXX_FLAGS "-std=c++11 -fext-numeric-literals") 11 | #'-fext-numeric-literals' : https://svn.boost.org/trac/boost/ticket/9240 12 | elseif(COMPILER_SUPPORTS_CXX0X) 13 | set(CMAKE_CXX_FLAGS "-std=c++0x") 14 | else() 15 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 16 | endif() 17 | 18 | find_package(catkin REQUIRED COMPONENTS 19 | roscpp 20 | std_msgs 21 | visualization_msgs 22 | message_generation 23 | ) 24 | 25 | add_message_files( 26 | FILES 27 | EdgeSE2.msg 28 | VertexSE2.msg 29 | GraphSE2.msg 30 | ) 31 | 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | ) 36 | 37 | catkin_package( 38 | # INCLUDE_DIRS include 39 | # LIBRARIES graph_visualizer 40 | CATKIN_DEPENDS roscpp std_msgs visualization_msgs message_runtime 41 | # DEPENDS system_lib 42 | ) 43 | 44 | include_directories( 45 | include 46 | ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | 50 | add_executable(se2_graph_visualizer src/se2_graph_visualizer_node.cpp) 51 | target_link_libraries(se2_graph_visualizer ${catkin_LIBRARIES}) 52 | 53 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | ## Fileds 3 | ``` txt 4 | std_msgs/Header header 5 | EdgeSE2[] edges 6 | float32[] edge_weights 7 | VertexSE2[] vertices 8 | ``` 9 | 10 | ## 2D Pose Graph 11 | A 2d pose graph can be visualized by publishing `GraphSE2` message. 12 | `header, edges, vertices` are always required for visualization. 13 | But `edge_weights` is optional and should be left empty. 14 | It is required only when the graph is robustified. 15 | 16 | ![][2d pose graph] 17 | 18 | ## Robustified Pose Graph 19 | A robustified pose graph can be visualized by setting `edge_weights` field. 20 | Each i-th `edge_weights` must map into i-th `edges` in `GraphSE2` message. 21 | Each `edge_weights` field must have a value between **0.0f~1.0f**. 22 | - **0.0f** means the edge has no influence in graph optimization. 23 | - **1.0f** means the edge influnces the graph as much as possible. 24 | 25 | Fully unweighted edges are greyed out. 26 | 27 | ![robustified][robustified pose graph] 28 | 29 | 30 | [2d pose graph]: __resources/fr101_local_loop_enabled.png 31 | [robustified pose graph]: __resources/1stEngineering_With_Camera_OUtliers3.png -------------------------------------------------------------------------------- /__resources/1stEngineering_With_Camera_OUtliers3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jaejunlee0538/graph_slam_visualizer/50d5f7e3209279e1b2f0172ef5ef3d6ac32bb09d/__resources/1stEngineering_With_Camera_OUtliers3.png -------------------------------------------------------------------------------- /__resources/fr101_local_loop_enabled.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jaejunlee0538/graph_slam_visualizer/50d5f7e3209279e1b2f0172ef5ef3d6ac32bb09d/__resources/fr101_local_loop_enabled.png -------------------------------------------------------------------------------- /include/ColorMap.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ub1404 on 16. 6. 19. 3 | // 4 | 5 | #ifndef GRAPH_VISUALIZER_COLORMAP_H 6 | #define GRAPH_VISUALIZER_COLORMAP_H 7 | struct MyRGB{ 8 | MyRGB(){} 9 | MyRGB(float r, float g, float b):r(r), g(g), b(b){} 10 | void print(std::ostream& os){ 11 | os<<"["< 1.0f || g < 0.0f || g>1.0f || b<0.0f || b>1.0f) 15 | return false; 16 | return true; 17 | } 18 | float r,g,b; 19 | }; 20 | struct ColorMap{ 21 | ColorMap(const std::vector& colors){ 22 | this->colors = colors; 23 | n_seg = colors.size() - 1; 24 | w = 1.0 / n_seg; 25 | seg.clear(); 26 | seg.push_back(0.0); 27 | for(int i=1;i<=n_seg;i++){ 28 | seg.push_back(i*w); 29 | } 30 | } 31 | 32 | MyRGB getRGB(const double& pos){ 33 | MyRGB rgb; 34 | 35 | int i = colors.size()-1; 36 | for(;i>=0;i--){ 37 | if(pos >= seg[i]) 38 | break; 39 | } 40 | if(i < 0) 41 | i=0; 42 | double ww = (pos - seg[i]) / w; 43 | 44 | rgb.r = colors[i].r + ww*(colors[i+1].r - colors[i].r); 45 | rgb.g = colors[i].g + ww*(colors[i+1].g - colors[i].g); 46 | rgb.b = colors[i].b + ww*(colors[i+1].b - colors[i].b); 47 | return rgb; 48 | } 49 | 50 | std::vector colors; 51 | int n_seg; 52 | std::vector seg; 53 | double w; 54 | }; 55 | 56 | 57 | #endif //GRAPH_VISUALIZER_COLORMAP_H 58 | -------------------------------------------------------------------------------- /msg/EdgeSE2.msg: -------------------------------------------------------------------------------- 1 | int32 vi_idx # index of a vertex from which the edge stems 2 | int32 vj_idx # index of a vertex to which the edge goes -------------------------------------------------------------------------------- /msg/GraphSE2.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | EdgeSE2[] edges # set of edges 3 | float32[] edge_weights # leave empty if the graph is not robustified(each edges are weighted) 4 | VertexSE2[] vertices # set of vertices. The index of vertices must be same with the indices defined in edges -------------------------------------------------------------------------------- /msg/VertexSE2.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graph_slam_visualizer 4 | 0.0.0 5 | The graph_slam_visualizer package 6 | 7 | 8 | 9 | 10 | ub1404 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | visualization_msgs 46 | message_generation 47 | roscpp 48 | std_msgs 49 | visualization_msgs 50 | message_runtime 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /src/se2_graph_visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ub1404 on 16. 5. 7. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | ros::Publisher graph_vis_pub; 11 | 12 | struct Triangle2D{ 13 | Triangle2D(const double& width, const double& height){ 14 | x[0] = 0; 15 | y[0] = 0.5 * width; 16 | 17 | x[1] = 0; 18 | y[1] = -0.5 * width; 19 | 20 | x[2] = height; 21 | y[2] = 0; 22 | } 23 | 24 | Triangle2D(const double& x1, const double& x2, const double& x3, const double& y1, const double& y2, const double& y3){ 25 | x[0] = x1; y[0] = y1; 26 | x[1] = x2; y[1] = y2; 27 | x[2] = x3; y[2] = y3; 28 | } 29 | 30 | Triangle2D transformed(const double& dx, const double& dy, const double& dtheta){ 31 | double c=cos(dtheta),s=sin(dtheta); 32 | return Triangle2D(c*x[0]-s*y[0]+dx, c*x[1]-s*y[1]+dx, c*x[2]-s*y[2]+dx, 33 | s*x[0]+c*y[0]+dy, s*x[1]+c*y[1]+dy, s*x[2]+c*y[2]+dy); 34 | } 35 | 36 | double x[3]; 37 | double y[3]; 38 | }; 39 | 40 | ros::NodeHandle* p_nh; 41 | double vertex_scale; 42 | double odom_edge_width; 43 | double loop_edge_width; 44 | double loop_edge_alpha; 45 | double odom_edge_alpha; 46 | bool pose_nose_coloring; 47 | double graph_z_position; 48 | double graph_heights; 49 | 50 | constexpr float normalizedRGB(const int& color){ 51 | return color/255.f; 52 | } 53 | constexpr float normalizedDistance(const float& when0, const float& when1){ 54 | return 1.0f / (when1 - when0); 55 | } 56 | 57 | ColorMap nodeColors({MyRGB(1.0f,0.0f,0.0f), MyRGB(0.0f,1.0f,0.0f), MyRGB(0.69f,0.4f,0.8f), MyRGB(0.2f,0.2f,1.0f)}); 58 | 59 | 60 | void graphse2_callback(const graph_slam_visualizer::GraphSE2ConstPtr& graph){ 61 | p_nh->param("vertex_scale",vertex_scale, 10.0); 62 | p_nh->param("odom_edge_width", odom_edge_width, 0.1); 63 | p_nh->param("loop_edge_width", loop_edge_width, 0.1); 64 | p_nh->param("loop_edge_alpha", loop_edge_alpha, 0.8); 65 | p_nh->param("odom_edge_alpha", odom_edge_alpha, 0.8); 66 | p_nh->param("pose_nose_coloring", pose_nose_coloring, false); 67 | p_nh->param("graph_heights", graph_heights, 0.0); 68 | p_nh->param("graph_z_position", graph_z_position, 0.0); 69 | Triangle2D tri(vertex_scale*0.1,vertex_scale*0.15); 70 | 71 | visualization_msgs::MarkerArray::Ptr array(new visualization_msgs::MarkerArray()); 72 | geometry_msgs::Point p1, p2, p3; 73 | visualization_msgs::Marker marker_vertex; 74 | 75 | int cnt = 0; 76 | //////////////////////////////////////////////////////////////////// 77 | //Converting vertices into MarkerArray 78 | int v_size = graph->vertices.size(); 79 | std::vector vertexZvalue(v_size); 80 | double elevation = 0.0; 81 | if(v_size) elevation = graph_heights / v_size; 82 | if(graph->vertices.size()) { 83 | marker_vertex.action = visualization_msgs::Marker::MODIFY; 84 | marker_vertex.header = graph->header; 85 | marker_vertex.ns = "vertex_se2"; 86 | marker_vertex.type = visualization_msgs::Marker::TRIANGLE_LIST; 87 | marker_vertex.scale.x = 1; 88 | marker_vertex.scale.y = 1; 89 | marker_vertex.scale.z = 1; 90 | if(!pose_nose_coloring) { 91 | marker_vertex.color.r = 1.0; 92 | marker_vertex.color.g = 0.0; 93 | marker_vertex.color.b = 0.0; 94 | marker_vertex.color.a = 1.0; 95 | } 96 | marker_vertex.id = 1; 97 | 98 | marker_vertex.points.resize(3 * graph->vertices.size()); 99 | if(pose_nose_coloring) 100 | marker_vertex.colors.resize(3*v_size); 101 | for(int i=0;ivertices[i].x, graph->vertices[i].y, graph->vertices[i].theta); 103 | vertexZvalue[i] = graph_z_position + elevation * i; 104 | p1.x = tmp.x[0]; p1.y = tmp.y[0]; p1.z = vertexZvalue[i]; 105 | p2.x = tmp.x[1]; p2.y = tmp.y[1]; p2.z = vertexZvalue[i]; 106 | p3.x = tmp.x[2]; p3.y = tmp.y[2]; p3.z = vertexZvalue[i]; 107 | marker_vertex.points[i*3]= p1; 108 | marker_vertex.points[i*3+1]= p2; 109 | marker_vertex.points[i*3+2]= p3; 110 | if(pose_nose_coloring) { 111 | std_msgs::ColorRGBA rgba; 112 | auto rgb = nodeColors.getRGB((double) i / v_size); 113 | if (!rgb.validate()) 114 | ROS_ERROR("Invalid color. %.3f %.3f %.3f(%d - %d)", rgb.r, rgb.g, rgb.b, i, v_size); 115 | rgba.r = rgb.r; 116 | rgba.g = rgb.g; 117 | rgba.b = rgb.b; 118 | rgba.a = 1.0f; 119 | marker_vertex.colors[i * 3] = rgba; 120 | marker_vertex.colors[i * 3 + 1] = rgba; 121 | marker_vertex.colors[i * 3 + 2] = rgba; 122 | } 123 | } 124 | array->markers.push_back(marker_vertex); 125 | } 126 | 127 | //////////////////////////////////////////////////////////////////// 128 | //Converting edges into MarkerArray 129 | if(graph->edges.size()) { 130 | visualization_msgs::Marker marker_edge; 131 | visualization_msgs::Marker marker_edge_loop; 132 | marker_edge.action = visualization_msgs::Marker::MODIFY; 133 | marker_edge.header = graph->header; 134 | marker_edge.ns = "edges_se2_odom"; 135 | marker_edge.type = visualization_msgs::Marker::LINE_LIST; 136 | marker_edge.scale.x = odom_edge_width; 137 | marker_edge.id = 0; 138 | marker_edge.colors.clear(); 139 | 140 | marker_edge_loop.action = visualization_msgs::Marker::MODIFY; 141 | marker_edge_loop.header = graph->header; 142 | marker_edge_loop.ns = "edges_se2_loop"; 143 | marker_edge_loop.type = visualization_msgs::Marker::LINE_LIST; 144 | marker_edge_loop.scale.x = loop_edge_width; 145 | marker_edge_loop.id = 0; 146 | marker_edge_loop.colors.clear(); 147 | // marker_edge.points.reserve(2 * graph->edges.size()); 148 | std_msgs::ColorRGBA rgba; 149 | std::vector weights; 150 | if(graph->edge_weights.empty() || graph->edge_weights.size()!=graph->edges.size()){ 151 | std::fill_n(std::back_inserter(weights), graph->edges.size(), 1.0f); 152 | }else{ 153 | std::copy(graph->edge_weights.begin(),graph->edge_weights.end(), std::back_inserter(weights)); 154 | } 155 | 156 | for (size_t i=0;iedges.size();i++) { 157 | p1.x = graph->vertices[graph->edges[i].vi_idx].x; 158 | p1.y = graph->vertices[graph->edges[i].vi_idx].y; 159 | p1.z = vertexZvalue[graph->edges[i].vi_idx]; 160 | p2.x = graph->vertices[graph->edges[i].vj_idx].x; 161 | p2.y = graph->vertices[graph->edges[i].vj_idx].y; 162 | p2.z = vertexZvalue[graph->edges[i].vj_idx]; 163 | 164 | if(abs(graph->edges[i].vj_idx-graph->edges[i].vi_idx) > 1){ 165 | //loop closing edges 166 | marker_edge_loop.points.push_back(p1); 167 | marker_edge_loop.points.push_back(p2); 168 | rgba.a = loop_edge_alpha; 169 | rgba.r = normalizedRGB(120) + weights[i]*(normalizedRGB(127)- normalizedRGB(120)); 170 | rgba.g = normalizedRGB(120) + weights[i]*(normalizedRGB(0) - normalizedRGB(120)); 171 | rgba.b = normalizedRGB(120) + weights[i]*(normalizedRGB(255) - normalizedRGB(120)); 172 | 173 | marker_edge_loop.colors.push_back(rgba); 174 | marker_edge_loop.colors.push_back(rgba); 175 | }else{ 176 | //odometry edges 177 | marker_edge.points.push_back(p1); 178 | marker_edge.points.push_back(p2); 179 | rgba.a = odom_edge_alpha; 180 | rgba.r = normalizedRGB(100) - weights[i]*normalizedRGB(100); 181 | rgba.g = normalizedRGB(155) + weights[i]*normalizedRGB(50); 182 | rgba.b = normalizedRGB(100) - weights[i]*normalizedRGB(100); 183 | 184 | marker_edge.colors.push_back(rgba); 185 | marker_edge.colors.push_back(rgba); 186 | } 187 | } 188 | array->markers.push_back(marker_edge); 189 | array->markers.push_back(marker_edge_loop); 190 | } 191 | //////////////////////////////////////////////////////////////////// 192 | graph_vis_pub.publish(array); 193 | } 194 | 195 | int main(int argc, char** argv){ 196 | 197 | ros::init(argc, argv, "graph_visualization_node"); 198 | ros::NodeHandle nh; 199 | ros::NodeHandle pnh("~"); 200 | p_nh = &pnh; 201 | 202 | graph_vis_pub = nh.advertise("graph_se2_vis", 10); 203 | 204 | ros::Subscriber sub_robust = nh.subscribe("g2o_graph_se2",10, graphse2_callback); 205 | ros::spin(); 206 | } --------------------------------------------------------------------------------