├── .gitignore ├── README.md ├── image ├── s1.png ├── s2.png ├── s3.png ├── s4.png ├── s5.png └── s6.png ├── install.sh ├── run.sh ├── src ├── CMakeLists.txt └── rviz_cloud_annotation │ ├── CMakeLists.txt │ ├── icons │ └── classes │ │ └── ic_launcher.png │ ├── launch │ └── annotation.launch │ ├── msg │ ├── RectangleSelectionViewport.msg │ └── UndoRedoState.msg │ ├── package.xml │ ├── pc_utils │ ├── CMakeLists.txt │ ├── include │ │ └── pc_utils │ │ │ ├── bound │ │ │ ├── OBB.h │ │ │ └── box_extract.h │ │ │ ├── common │ │ │ ├── common.h │ │ │ ├── detail │ │ │ │ ├── factory.h │ │ │ │ └── type_util.h │ │ │ ├── lidar_device.h │ │ │ ├── parameter.h │ │ │ ├── point_type.h │ │ │ └── utils.h │ │ │ ├── filter │ │ │ └── cloud_filter.h │ │ │ └── seg │ │ │ ├── cluster.h │ │ │ └── ground_estimator.h │ └── lib │ │ └── libpc_utils.so │ ├── pcl_include │ ├── colors.cpp │ └── pcl │ │ └── common │ │ └── colors.h │ ├── rviz │ └── annotation.rviz │ ├── rviz_plugin │ └── cloud_annotation.xml │ └── src │ ├── point_cloud_plane_curves_extract.cpp │ ├── point_cloud_plane_curves_extract.h │ ├── point_cloud_plane_params.cpp │ ├── point_cloud_plane_params.h │ ├── point_neighborhood.cpp │ ├── point_neighborhood.h │ ├── point_neighborhood_search.cpp │ ├── point_neighborhood_search.h │ ├── rviz_cloud_annotation.cpp │ ├── rviz_cloud_annotation.h │ ├── rviz_cloud_annotation_class.cpp │ ├── rviz_cloud_annotation_class.h │ ├── rviz_cloud_annotation_params.h │ ├── rviz_cloud_annotation_plugin.cpp │ ├── rviz_cloud_annotation_plugin.h │ ├── rviz_cloud_annotation_point_plane.cpp │ ├── rviz_cloud_annotation_point_plane.h │ ├── rviz_cloud_annotation_points.cpp │ ├── rviz_cloud_annotation_points.h │ ├── rviz_cloud_annotation_points_io.cpp │ ├── rviz_cloud_annotation_undo.cpp │ ├── rviz_cloud_annotation_undo.h │ ├── rviz_select_tool.cpp │ └── rviz_select_tool.h ├── tools ├── clear_annotation.sh ├── install_annotation.sh ├── launch_annotation.sh ├── pcd_play ├── raw2pcd ├── ros_debug.sh └── uninstall_annotation.sh ├── uninstall.sh └── 示例pcd文件 └── pcd ├── 16line.pcd └── 64line.pcd /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | .ros/ 3 | .catkin* 4 | .idea/ 5 | devel/ 6 | build/ 7 | **/cmake-build-debug 8 | cmake-build-debug/ 9 | 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | PCAT点云标注工具-使用手册 2 | --------------------- 3 | 4 | - Demo项目，请自行魔改 5 | 6 | - This is the open source version: 7 | 8 | `Author: WenwenDu` 9 | `TEL: 18355180339` 10 | `E-mail: 1455112695@qq.com` 11 | 12 | - Video tutorial: 13 | 1. `https://v.youku.com/v_show/id_XNDYxNjY4MDExMg==.html?spm=a2h0k.11417342.soresults.dtitle` 14 | 15 | 2. `https://v.youku.com/v_show/id_XNDYxNjY4MDI5Mg==.html?spm=a2hzp.8244740.0.0` 16 | 17 | ## **I. 配置使用环境及安装** 18 | 19 | - `配置要求：ubuntu16.04 + ROS Kinetic full` 20 | - `注意：请务必保证系统使用原生python2.7，在使用Anaconda2的情况下，请在~/.bashrc环境变量中临时关闭Anaconda2,避免冲突。（如果你长期使用ROS，强烈建议在虚拟环境下使用anaconda，避免冲突。）` 21 | 22 | ### 1. 安装ROS-Kinetic 23 | 参考[ROS WiKi-安装说明](http://http://wiki.ros.org/kinetic/Installation/Ubuntu), 安装步骤如下： 24 | ``` 25 | 添加ROS源： 26 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 27 | 添加ROS源秘钥： 28 | sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 29 | 更新源 30 | sudo apt-get update 31 | ``` 32 | ``` 33 | 安装ROS完整版：（由于使用Rviz，PCL等模块，请务必安装完整版） 34 | sudo apt-get install ros-kinetic-desktop-full 35 | sudo apt-cache search ros-kinetic 36 | 初始化ROS： 37 | sudo rosdep init 38 | rosdep update 39 | ``` 40 | 41 | ``` 42 | 添加环境变量 43 | echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc 44 | source ~/.bashrc 45 | 更新ROS环境变量 46 | source /opt/ros/kinetic/setup.bash 47 | ``` 48 | 49 | ``` 50 | 测试ROS是否成功安装: 51 | 开启一个新的Teminnal，输入： 52 | roscore 53 | 测试Rviz 54 | 开启一个新的Teminnal，输入： 55 | rviz 56 | ``` 57 | 成功显示rviz界面如下： 58 | ![图片](https://github.com/halostorm/PCAT/blob/master/image/s1.png) 59 | 60 | ### 2. 安装PCAT标注工具 61 | ``` 62 | (1) 进入文件夹PCAT 63 | (2) 开启终端，运行安装命令： sh install.sh 64 | (3) 显示 install successful 后，home文件夹下出现lidar_annotation文件夹，安装成功 65 | ``` 66 | ------------------------- 67 | ## **II. 导入pcd文件** 68 | 1. **导入待标注点云pcd文件** 69 | ``` 70 | Copy 待标注的点云.pcd格式文件到 lidar_annotation/pcd/ 文件夹下 71 | 72 | 注意：标注工具默认支持激光雷达pcd格式点云，Field为[x,y,z,intensity],如果使用XYZRGB等其他pcd format，请在src/rviz_cloud_annotation/launch/annotation.launch中更改pcd_type参数的value. 73 | 74 | 常见issue 75 | 76 | [1] 如何支持其他类型pcd或其他3Dpoints？ 修改以下code... 77 | // src/rviz_cloud_annotation/src/rviz_cloud_annotation_class.cpp 78 | void RVizCloudAnnotation::LoadCloud(const std::string &filename, 79 | const std::string &normal_source, 80 | PointXYZRGBNormalCloud &cloud); 81 | 82 | ``` 83 | 84 | 2. **开始标注** 85 | ``` 86 | 打开 Teminnal, 运行: sh run.sh 87 | ``` 88 | 显示标注界面如下： 89 | ![图片](https://github.com/halostorm/PCAT/blob/master/image/s2.png) 90 | 91 | ------------------------------- 92 | ## **III. 标注手册正篇** 93 | `首次使用请务必仔细阅读` 94 | ### 1. 标注面板详解 95 | **下面就上图中 A, B, C, D, E 5个模块做详细说明：** 96 | - [ ] **A. 标注菜单栏** 97 | 98 | ``` 99 | 标注菜单栏由 [文件]， [编辑]，[视图]，[标记]，[选择] 5部分组成 100 | 文件：(1)切换新文件，(2)清除当前帧标记，(3)保存 101 | 编辑：(1)取消，(2)恢复 102 | 视图：(1)增加点的尺寸，(2)减小点的尺寸，(3)重置点的尺寸 103 | 标记：(1)清除当前物体的标记，(2)切换颜色，(3)设置障碍物BBox遮挡系数，(4)调节障碍物BBox方位，(5)调节障碍物BBox尺寸 104 | 选择：(1)跳转至下一物体，(2)跳转至上一物体 105 | ``` 106 | ``` 107 | 特别说明： 108 | 1.切换新文件会自动保存当前文件的标注信息 109 | 2.取消/恢复开销较大，尽量避免使用 110 | 3.标记完成一个物体后，需要切换到下一个物体进行标注，否则会覆盖当前标记；选择新的颜色会自动切换到下一物体；物体ID显示在面板上 111 | 4.标记障碍物时，颜色 1～5，6~10，11~15，16~20 分别对应标签： 小车，大车，行人，骑行； 112 | 5.标记障碍物时，需要设置方位角和遮挡系数，请以实际为准标注，0--不遮挡，1--完全遮挡 113 | 尽量使用简洁的方式完成标注，熟练使用快捷键可以有效提高标注速度。 114 | ``` 115 | ![图片](https://github.com/halostorm/PCAT/blob/master/image/s3.png) 116 | 特别说明 117 | 1.点云被重复标记为 障碍物，路沿，车道线，地面时，标签优先级为 （障碍物 > 路沿/车道线 > 地面） 118 | 119 | ### 2.标注步骤 120 | `在看标注说明之前请务必观看视频教程` 121 | - [ ] 标注请按照: 【障碍物--> 路沿-->车道线-->地面】 的顺序。 122 | 123 | ``` 124 | (1) 障碍物 125 | 障碍物包括 小车（轿车），大车（卡车、有轨电车等），行人，骑行（电动车）4类。 126 | 在该数据集中主要包含 小车和行人，及少量的大车和骑行。请在标注`颜色面板`选择不同的按钮，对应不同的障碍物。 127 | 颜色面板分为4大块，颜色 1～5，6~10，11~15，16~20 分别对应： 小车，大车，行人，骑行，代表不同的障碍物。 128 | 对每一帧的点云，障碍物存在则标注，不存在则不标注；每标注完一个障碍物，需要==切换至下一个障碍物进行新的标注。 129 | (比如：标完第一辆小车，需要按`Shitf+N` 切换至下一小车，或者按`Shift+P`切换至上一障碍物进行修改)。 130 | 选择新的颜色会自动切换至新的下一障碍物。 131 | 每个障碍物，需要标注人员自己判断大致的朝向，并进行方位调节（R、F键）。 132 | 受到遮挡的障碍物请设置`遮挡系数`，默认为 0，即不遮挡，大多数障碍物不存在遮挡。 133 | ``` 134 | ![图片](https://github.com/halostorm/PCAT/blob/master/image/s4.png) 135 | 136 | ``` 137 | (2) 路沿 138 | 路沿指道路中地面的边界，如上图显示；标记路沿只能使用点选的方式标注（具体操作可以参考标注视频教程） 139 | 一般一帧点云中有多条路沿，每标记一条，需要切换至下一路沿进行标注，切换方式与障碍物切换相同。 140 | (3) 车道线 141 | 车道线指道路中颜色明显突出的线段，一般出现的频率比较低，没有出现或者看不清楚则不用标注；车道线的标注方式与路沿完全相同。 142 | (4) 地面 143 | 地面是一帧点云中比较关键的部分，一般选择使用多边形进行选择标注，边界为之前标注的路沿。 144 | 地面可以分多次标注，拼接生成；如果一次选点过多，地面生成时间会较长。 145 | *在2.4.0版本之后，标注工具增加了地面辅助标记功能：用户每次选择`地面(F2)`按钮时，系统会自动生成95%的地面，用户在此基础上进行细节修改， 146 | 得到最终的地面标注。 147 | ``` 148 | 149 | ### 3.标注结果 150 | #### Result路径说明 151 | ![图片](https://github.com/halostorm/PCAT/blob/master/image/s6.png) 152 | #### 3D框label 153 | ![图片](https://github.com/halostorm/PCAT/blob/master/image/s5.png) 154 | 155 | ------------------------ 156 | 157 | **IV、注意事项** 158 | ----------------------- 159 | 1. 标注工具使用过程中如果遇见问题，或者代码部分有疑问，编辑需要，联系 @杜文文(18355180339 / 1455112695@qq.com) 160 | 2. 视频教程: 161 | A`https://v.youku.com/v_show/id_XNDYxNjY4MDExMg==.html?spm=a2h0k.11417342.soresults.dtitle` 162 | B`https://v.youku.com/v_show/id_XNDYxNjY4MDI5Mg==.html?spm=a2hzp.8244740.0.0` 163 | 164 | ----------------------- 165 | 166 | **V、版权说明** 167 | ----------------------- 168 | 1. **软件版权** 169 | 本标注工具的版权归WenwenDu所有 170 | 2. **其他版权** 171 | 本标注工具在 RIMLab 开源标注工具 rviz_cloud_annotation 上改进完成： 172 | `https://github.com/RMonica/rviz_cloud_annotation` 173 | 174 | ``` 175 | 原始版权说明： 176 | Original Copyright: 177 | /* 178 | * Copyright (c) 2016-2017, Riccardo Monica 179 | * RIMLab, Department of Engineering and Architecture 180 | * University of Parma, Italy 181 | * http://www.rimlab.ce.unipr.it/ 182 | * 183 | * Redistribution and use in source and binary forms, with or without 184 | * modification, are permitted provided that the following conditions are met: 185 | * 186 | * 1. Redistributions of source code must retain the above copyright notice, 187 | * this list of conditions and the following disclaimer. 188 | * 189 | * 2. Redistributions in binary form must reproduce the above copyright notice, 190 | * this list of conditions and the following disclaimer in the documentation 191 | * and/or other materials provided with the distribution. 192 | * 193 | * 3. Neither the name of the copyright holder nor the names of its 194 | * contributors may be used to endorse or promote products derived from this 195 | * software without specific prior written permission. 196 | * 197 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 198 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 199 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 201 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 202 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 203 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 204 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 205 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 206 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 207 | * POSSIBILITY OF SUCH DAMAGE. 208 | */ 209 | ``` 210 | 211 | -------------------------------------------------------------------------------- /image/s1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/image/s1.png -------------------------------------------------------------------------------- /image/s2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/image/s2.png -------------------------------------------------------------------------------- /image/s3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/image/s3.png -------------------------------------------------------------------------------- /image/s4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/image/s4.png -------------------------------------------------------------------------------- /image/s5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/image/s5.png -------------------------------------------------------------------------------- /image/s6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/image/s6.png -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | mkdir $HOME/lidar_annotation/ 2 | 3 | cd tools/ 4 | 5 | bash install_annotation.sh 6 | 7 | echo "install successful!" 8 | -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | cd tools/ 2 | 3 | bash launch_annotation.sh 4 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(rviz_cloud_annotation) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cv_bridge 6 | interactive_markers 7 | pcl_conversions 8 | roscpp 9 | tf 10 | visualization_msgs 11 | geometry_msgs 12 | eigen_conversions 13 | cmake_modules 14 | std_msgs 15 | rviz 16 | ) 17 | find_package(OpenCV) 18 | find_package(Eigen3 REQUIRED) 19 | find_package(Boost REQUIRED) 20 | find_package(PCL ) 21 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/pc_utils) 22 | message(WARNING "PCL 1.8 found") 23 | 24 | set(MAYBE_PCL_COMMON_COLORS "") 25 | if (NOT ${PCL_FOUND}) 26 | message(WARNING "PCL 1.8 not found, attempting 1.7...") 27 | find_package(PCL 1.7 REQUIRED) 28 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/pcl_include) 29 | set(MAYBE_PCL_COMMON_COLORS "${CMAKE_CURRENT_SOURCE_DIR}/pcl_include/colors.cpp") 30 | endif() 31 | 32 | find_package(Qt5Widgets REQUIRED) 33 | set(CMAKE_CXX_STANDARD 17) # C++11... 34 | set(CMAKE_AUTOMOC ON) 35 | add_definitions(-DQT_NO_KEYWORDS) 36 | 37 | add_definitions(${PCL_DEFINITIONS}) 38 | link_directories(${PCL_LIBRARY_DIRS}) 39 | 40 | ################################################ 41 | ## Declare ROS messages, services and actions ## 42 | ################################################ 43 | 44 | add_message_files( 45 | FILES 46 | UndoRedoState.msg 47 | RectangleSelectionViewport.msg 48 | ) 49 | 50 | generate_messages( 51 | DEPENDENCIES 52 | std_msgs 53 | geometry_msgs 54 | ) 55 | 56 | ################################### 57 | ## catkin specific configuration ## 58 | ################################### 59 | 60 | include_directories( 61 | ${catkin_INCLUDE_DIRS} 62 | ${PCL_INCLUDE_DIRS} 63 | ${Boost_INCLUDE_DIRS} 64 | ${Eigen3_INCLUDE_DIRS} 65 | ${OpenCV_INCLUDE_DIRS} 66 | ${PC_UTILS_INCLUDE_DIRS} 67 | ) 68 | message(WARNING " ${PC_UTILS_INCLUDE_DIRS}") 69 | add_executable(rviz_cloud_annotation_node 70 | src/rviz_cloud_annotation.cpp 71 | ${MAYBE_PCL_COMMON_COLORS} 72 | ) 73 | 74 | add_dependencies(rviz_cloud_annotation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 75 | 76 | target_link_libraries( 77 | rviz_cloud_annotation_node 78 | rviz_cloud_annotation_com 79 | ${PCL_LIBRARIES} 80 | ${Boost_LIBRARIES} 81 | ${Eigen3_LIBRARIES} 82 | ${catkin_LIBRARIES} 83 | ${PC_UTILS_LIBRARIES} 84 | ) 85 | 86 | 87 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 88 | 89 | add_library(rviz_cloud_annotation_plugin 90 | src/rviz_cloud_annotation_plugin.cpp 91 | src/rviz_select_tool.cpp 92 | ${MAYBE_PCL_COMMON_COLORS} 93 | ) 94 | 95 | add_dependencies(rviz_cloud_annotation_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 96 | 97 | target_link_libraries(rviz_cloud_annotation_plugin 98 | ${PCL_LIBRARIES} 99 | ${Boost_LIBRARIES} 100 | ${Eigen3_LIBRARIES} 101 | ${catkin_LIBRARIES} 102 | ${OpenCV_LIBRARIES} 103 | Qt5::Widgets 104 | ) 105 | 106 | 107 | add_library(rviz_cloud_annotation_com 108 | src/rviz_cloud_annotation_points.cpp 109 | src/rviz_cloud_annotation_points_io.cpp 110 | src/rviz_cloud_annotation_point_plane.cpp 111 | src/rviz_cloud_annotation_class.cpp 112 | src/rviz_cloud_annotation_undo.cpp 113 | src/point_neighborhood.cpp 114 | src/point_neighborhood_search.cpp 115 | src/point_cloud_plane_curves_extract.cpp 116 | src/point_cloud_plane_params.cpp 117 | ${MAYBE_PCL_COMMON_COLORS} 118 | ) 119 | 120 | 121 | add_dependencies(rviz_cloud_annotation_com ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | target_link_libraries(rviz_cloud_annotation_com 124 | ${PCL_LIBRARIES} 125 | ${Boost_LIBRARIES} 126 | ${Eigen3_LIBRARIES} 127 | ${catkin_LIBRARIES} 128 | ${OpenCV_LIBRARIES} 129 | Qt5::Widgets 130 | ) 131 | 132 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/icons/classes/ic_launcher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/src/rviz_cloud_annotation/icons/classes/ic_launcher.png -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/launch/annotation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/msg/RectangleSelectionViewport.msg: -------------------------------------------------------------------------------- 1 | uint32 start_x 2 | uint32 start_y 3 | uint32 end_x 4 | uint32 end_y 5 | 6 | uint32 viewport_height 7 | uint32 viewport_width 8 | 9 | float32 focal_length 10 | 11 | float32[16] projection_matrix 12 | geometry_msgs/Pose camera_pose 13 | 14 | bool is_deep_selection 15 | 16 | int32[] polyline_x 17 | int32[] polyline_y 18 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/msg/UndoRedoState.msg: -------------------------------------------------------------------------------- 1 | bool redo_enabled 2 | string redo_description 3 | 4 | bool undo_enabled 5 | string undo_description 6 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rviz_cloud_annotation 4 | 3.1.0 5 | The rviz_cloud_annotation package 6 | 7 | wenwendu 8 | wenwendu 9 | 10 | catkin 11 | interactive_markers 12 | rviz 13 | pcl_conversions 14 | roscpp 15 | tf 16 | visualization_msgs 17 | cmake_modules 18 | std_msgs 19 | geometry_msgs 20 | eigen_conversions 21 | opencv2 22 | opencv2 23 | 24 | interactive_markers 25 | pcl_conversions 26 | roscpp 27 | tf 28 | visualization_msgs 29 | rviz 30 | std_msgs 31 | geometry_msgs 32 | eigen_conversions 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(PC_UTILS_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include PARENT_SCOPE) 2 | set(PC_UTILS_LIBRARIES ${CMAKE_CURRENT_SOURCE_DIR}/lib/libpc_utils.so yaml-cpp PARENT_SCOPE) -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/bound/OBB.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ou on 2021/12/7. 3 | // 4 | 5 | #ifndef PC_UTILS_BOUNDING_OBB_H 6 | #define PC_UTILS_BOUNDING_OBB_H 7 | #include 8 | namespace pc_utils{ 9 | 10 | class BoundingBox { 11 | public: 12 | Eigen::Isometry3f pose{Eigen::Isometry3f::Identity()}; 13 | Eigen::Vector3f dxyz{1, 1, 1}; 14 | 15 | static void Corner3d(const BoundingBox &_box, Eigen::Vector3f (&_corner3d)[8]) { 16 | /** 17 | ** Box Corner 下/上 18 | ** 2/3 __________ 6/7 19 | ** | y | 20 | ** | | | 21 | ** | o - x | 22 | ** | | 23 | ** |__________| 24 | ** 0/1 4/5 25 | **********************************************/ 26 | static Eigen::Array3f corner3d_unit[8] = {{-1, -1, -1}, 27 | {-1, -1, 1}, 28 | {-1, 1, -1}, 29 | {-1, 1, 1}, 30 | {1, -1, -1}, 31 | {1, -1, 1}, 32 | {1, 1, -1}, 33 | {1, 1, 1}}; 34 | 35 | for (int i = 0; i < 8; i++) { 36 | _corner3d[i] = _box.pose * (0.5f * corner3d_unit[i] * _box.dxyz.array()); 37 | } 38 | } 39 | 40 | 41 | static void LineList(const BoundingBox &_box, Eigen::Vector3f (&_lines)[24]) { 42 | static int table[] = {0, 1, 1, 3, 3, 2, 2, 0, 4, 5, 5, 7, 7, 6, 6, 4, 0, 4, 1, 5, 2, 6, 3, 7}; 43 | Eigen::Vector3f corners[8]; 44 | Corner3d(_box, corners); 45 | for (int i = 0; i < 24; i++) { 46 | _lines[i][0] = corners[table[i]][0]; 47 | _lines[i][1] = corners[table[i]][1]; 48 | _lines[i][2] = corners[table[i]][2]; 49 | } 50 | } 51 | }; 52 | } 53 | 54 | #endif //PC_UTILS_BOUNDING_OBB_H 55 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/bound/box_extract.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by nrsl on 2021/10/11. 3 | // 4 | 5 | #ifndef PERCEPTION3D_BOX_EXTRACT_H 6 | #define PERCEPTION3D_BOX_EXTRACT_H 7 | 8 | #include "pc_utils/bound/OBB.h" 9 | #include "pc_utils/common/common.h" 10 | 11 | 12 | /** 13 | * @brief support type of box extractor 14 | * @typedef OrientedBBoxExtractor 15 | * @typedef OrientedBBox2p5DExtractor 16 | * @typedef AxiallyAlignedBBoxExtractor 17 | */ 18 | #define PC_UTILS_BBOX_TYPE \ 19 | define( AxiallyAlignedBBoxExtractor ) \ 20 | define( OrientedBBox3DExtractor ) \ 21 | define( OrientedBBox2p5DExtractor ) 22 | 23 | namespace pc_utils { 24 | 25 | template 26 | class BoundingExtractor { 27 | public: 28 | virtual void extract(const typename PC::Ptr &input, BoundingBox &output) = 0; 29 | }; 30 | 31 | } // namespace pc_utils 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | PC_UTILS_LINK_HELPER_HEADER(BoundingExtractor) 44 | #endif //PERCEPTION3D_BOX_EXTRACT_H 45 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/common/common.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ou on 2021/11/17. 3 | // 4 | 5 | #ifndef SRC_COMMON_H 6 | #define SRC_COMMON_H 7 | 8 | #include "utils.h" 9 | 10 | namespace pc_utils { 11 | double inline to_rad(double degree) { return degree / 180 * M_PI; } 12 | 13 | Eigen::Isometry3f inline fromXYZRPY(const Eigen::Vector3f &xyz, const Eigen::Vector3f &rpy) { 14 | Eigen::Isometry3f ret = Eigen::Isometry3f::Identity(); 15 | Eigen::AngleAxisf 16 | r(rpy.x(), Eigen::Vector3f::UnitX()), 17 | p(rpy.y(), Eigen::Vector3f::UnitY()), 18 | y(rpy.z(), Eigen::Vector3f::UnitZ()); 19 | ret.translate(xyz); 20 | ret.rotate(Eigen::Quaternionf{y * p * r}); 21 | return ret; 22 | } 23 | 24 | Eigen::Isometry3f inline fromXYZRPY(float x, float y, float z, float r, float p, float yaw) { 25 | return fromXYZRPY(Eigen::Vector3f(x, y, z), Eigen::Vector3f(r, p, yaw)); 26 | } 27 | } 28 | 29 | 30 | 31 | 32 | PC_UTILS_LINK_HELPER_HEADER(common) 33 | #endif //SRC_COMMON_H 34 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/common/detail/factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FACTORY_H 2 | #define FACTORY_H 3 | 4 | #include "type_util.h" 5 | #include 6 | #include 7 | //#define BASE_UTILS_VERBOSE 8 | /*** 9 | * @brief Factory class definition 10 | * @example 11 | * struct Base{} 12 | * struct Der{ Der(int){}} 13 | * static bool register_1 = Factory::Register(); 14 | */ 15 | template 16 | struct Factory { 17 | private: 18 | template 19 | static auto *BuildImpl(const std::string &name, T &&...t) { 20 | if (GetTable().find(name) == GetTable().end()) { 21 | fprintf(stderr, "build fail: %s no exist in %s.\n", name.c_str(), 22 | Type>::c_str()); 23 | return (Base *) nullptr; 24 | } else { 25 | return GetTable()[name](std::forward(t)...); 26 | } 27 | } 28 | 29 | static inline auto &GetTable() { 30 | static std::unordered_map table; 31 | return table; 32 | } 33 | 34 | template 35 | struct raw_ptr; 36 | public: 37 | 38 | template 39 | static bool Register() { 40 | constexpr bool validation = std::is_default_constructible_v || std::is_constructible_v; 41 | static_assert(validation, "no Der(Args...) constructor function exist\n"); 42 | if constexpr (validation) { 43 | static_assert(std::is_base_of_v, "no derivative relationship between this two types.\n"); 44 | std::string hash_code; 45 | if constexpr (has_member_name_v) 46 | hash_code = std::string(Der::name()); 47 | else 48 | hash_code = std::string(Type::name); 49 | 50 | if (GetTable().find(hash_code) == GetTable().end()) { 51 | #ifdef BASE_UTILS_VERBOSE 52 | printf("register [√]: %s -> %s\n", 53 | Type::c_str(), 54 | Type>::c_str()); 55 | #endif 56 | GetTable()[hash_code] = [](Args ...args) -> Base * { return new Der(args...); }; 57 | } else { 58 | fprintf(stderr, "register [x]: %s -> %s\n", 59 | Type::c_str(), 60 | Type>::c_str()); 61 | } 62 | return true; 63 | } else 64 | return false; 65 | } 66 | 67 | template class PtrType=raw_ptr, class ...T> 68 | static auto Build(const std::string &name, T &&...t) { 69 | if constexpr (std::is_same_v, raw_ptr>) return BuildImpl(name, std::forward(t)...); 70 | else return PtrType(BuildImpl(name, std::forward(t)...)); 71 | } 72 | 73 | template class PtrType=std::shared_ptr, class ...T> 74 | static auto BuildT(const std::string &name, T &&...t) { 75 | return Build(TTypeTrait::with_template_arg(name), std::forward(t)...); 76 | } 77 | }; 78 | 79 | /*** 80 | * @brief auto register (support construction overload) 81 | * @example 82 | * struct Base{} 83 | * struct Der: AutoRegister::template OverLoad<>, 84 | * AutoRegister::template OverLoad{ 85 | * Der(){} 86 | * Der(int){} 87 | * } 88 | */ 89 | template 90 | struct AutoRegister { 91 | template 92 | struct OverLoad { 93 | OverLoad() { (void) registered; /*必不可少，显式调用才会生成代码*/} 94 | 95 | static bool registered; 96 | }; 97 | 98 | struct Default { 99 | Default() { 100 | (void) registered; 101 | Factory::template Register(); /*必不可少，显式调用才会生成代码*/} 102 | 103 | static bool registered; 104 | }; 105 | }; 106 | template 107 | template 108 | bool AutoRegister::OverLoad::registered = Factory::template Register(); 109 | 110 | template 111 | bool AutoRegister::Default::registered = Factory::template Register(); 112 | 113 | 114 | 115 | /*** 116 | * @brief for manually register 117 | * @example 118 | * struct Base{} 119 | * struct Der{ 120 | * Der(){} 121 | * Der(int){} 122 | * } 123 | * REGISTER_TO_FACTORY(Base,Der) 124 | * REGISTER_TO_FACTORY(Base,Der, int) 125 | */ 126 | 127 | 128 | #define REGISTER_TO_FACTORY(der, ...) \ 129 | namespace{static bool REGISTER_TO_FACTORY_UNIQUE_ID(register_der) = Factory<__VA_ARGS__>::Register();} 130 | 131 | /*** 132 | * @brief define namespace decoration 133 | */ 134 | namespace pc_utils { REGISTER_DEF_NAMESPACE_DECORATE} 135 | 136 | 137 | #endif -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/common/detail/type_util.h: -------------------------------------------------------------------------------- 1 | #ifndef FACTORY_TYPE_UTILS_H 2 | #define FACTORY_TYPE_UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /** 9 | * @brief get type name from template arg 10 | * @example 11 | * Type::name -> constexpr std::string_view("int") 12 | * Type::str() -> std::string("int") 13 | * Type::c_str() -> std::string("int").c_str() 14 | */ 15 | template 16 | struct Type { 17 | private: 18 | static constexpr auto type_name_impl() noexcept { 19 | std::string_view name, prefix, suffix; 20 | #ifdef __clang__ 21 | name = __PRETTY_FUNCTION__; 22 | prefix = "static auto Type::type_name_impl() [T = "; 23 | suffix = "]"; 24 | #elif defined(__GNUC__) 25 | name = __PRETTY_FUNCTION__; 26 | prefix = "static constexpr auto Type::type_name_impl() [with T = "; 27 | suffix = "]"; 28 | #elif defined(_MSC_VER) 29 | name = __FUNCSIG__; 30 | prefix = "static auto __cdecl Type::type_name_impl<"; 31 | suffix = ">(void) noexcept"; 32 | #endif 33 | name.remove_prefix(prefix.size()); 34 | name.remove_suffix(suffix.size()); 35 | return name; 36 | } 37 | 38 | public: 39 | static constexpr auto name = type_name_impl(); 40 | 41 | static std::string &str() { 42 | static std::string n(name); 43 | return n; 44 | } 45 | 46 | static auto *c_str() { return str().c_str(); } 47 | }; 48 | 49 | /** 50 | * @brief get type name from template args 51 | * @example 52 | * Types::name<1> -> constexpr std::string_view("double") 53 | * Types::names -> constexpr std::string_view("") 54 | * Types::raw_names -> constexpr std::string_view("int, double") 55 | */ 56 | template 57 | struct Types { 58 | private: 59 | static constexpr auto type_names_impl() noexcept { 60 | std::string_view name = Type>::name, prefix = "Types"; 61 | name.remove_prefix(prefix.size()); 62 | return name; 63 | } 64 | 65 | static constexpr auto raw_type_names_impl() noexcept { 66 | std::string_view name = type_names_impl(), prefix = "<", suffix = ">"; 67 | name.remove_prefix(prefix.size()); 68 | name.remove_suffix(suffix.size()); 69 | return name; 70 | } 71 | 72 | public: 73 | 74 | static constexpr size_t size = sizeof...(T); 75 | template 76 | static constexpr auto name = Type >::type>::name; 77 | 78 | static constexpr auto names = type_names_impl(); 79 | static constexpr auto raw_names = raw_type_names_impl(); 80 | }; 81 | 82 | /** 83 | * @brief decorate template class name with template arg 84 | * @example 85 | * DECORATE(TemplateClass,int,double) -> std::string("TemplateClass") 86 | * decorate(TemplateClass) -> std::string("TemplateClass") 87 | */ 88 | #define DECORATE(name, ...) decorate<__VA_ARGS__>(name) 89 | 90 | template 91 | inline std::string decorate(const std::string &name) { return name + std::string(Types::names); } 92 | 93 | template<> 94 | inline std::string decorate(const std::string &name) { return name; } 95 | 96 | /** 97 | * @brief template class type trait 98 | * @example 99 | * TTypeTrait>::size -> size_t(2) 100 | * TTypeTrait>::arg_type<1> -> double 101 | * TTypeTrait>::with_template_arg(NewClass) -> std::string("NewClass") 102 | */ 103 | 104 | template 105 | struct TTypeTrait { 106 | constexpr static size_t size = 0; 107 | 108 | static std::string with_template_arg(const std::string &name) { return name; } 109 | }; 110 | 111 | template class TType, class ...T> 112 | struct TTypeTrait> { 113 | private: 114 | template 115 | static std::string 116 | with_template_arg_helper(const std::string &name, std::index_sequence) { 117 | return decorate...>(name); 118 | } 119 | 120 | public: 121 | constexpr static size_t size = sizeof ...(T); 122 | template 123 | using arg_type = typename std::tuple_element>::type; 124 | 125 | static std::string with_template_arg(const std::string &name) { 126 | return with_template_arg_helper(name, std::make_index_sequence()); 127 | } 128 | }; 129 | 130 | /*** 131 | * @brief SFINAE to detect if CLASS have static function called "name" 132 | * @example 133 | * struct A { 134 | * static void name() {} 135 | * }; 136 | * struct B {} 137 | * 138 | * has_member_name_v -> constexpr bool(true) 139 | * has_member_name_v -> constexpr bool(false) 140 | */ 141 | #define PC_UTILS_HAS_MEMBER(XXX) \ 142 | template\ 143 | struct has_member_##XXX \ 144 | { \ 145 | private: \ 146 | template static auto Check(int) -> decltype(std::declval().XXX(std::declval()...), std::true_type()); \ 147 | template static std::false_type Check(...); \ 148 | public: \ 149 | static constexpr auto value = decltype(Check(0))::value; \ 150 | };\ 151 | template inline constexpr bool has_member_##XXX##_v{has_member_##XXX::value}; 152 | 153 | PC_UTILS_HAS_MEMBER(name) 154 | 155 | /*** 156 | * @brief get unique symbol 157 | * @example 158 | * REGISTER_TO_FACTORY_UNIQUE_ID(ABC) ABC1 159 | * REGISTER_TO_FACTORY_UNIQUE_ID(ABC) ABC2 160 | */ 161 | #define REGISTER_TO_FACTORY_UNIQUE_ID_MERGE_IMPL(a, b) a ## b //合并用的主体 162 | #define REGISTER_TO_FACTORY_UNIQUE_ID_MERGE(a, b) REGISTER_TO_FACTORY_UNIQUE_ID_MERGE_IMPL(a, b) //中间层 163 | #define REGISTER_TO_FACTORY_UNIQUE_ID(name) REGISTER_TO_FACTORY_UNIQUE_ID_MERGE(name, __COUNTER__) 164 | 165 | #define REGISTER_REMOVE_PARENTHESES_IMPL(X) X 166 | #define REGISTER_REMOVE_PARENTHESES(X) REGISTER_REMOVE_PARENTHESES_IMPL X 167 | 168 | /*** 169 | * @brief define namespace addition 170 | */ 171 | 172 | #define REGISTER_DEF_NAMESPACE_DECORATE inline std::string ns(const std::string &name) { \ 173 | struct Ns { \ 174 | static constexpr auto name_space() noexcept { \ 175 | auto name = Type::name, \ 176 | suffix = std::string_view("ns(const string&)::Ns"); \ 177 | name.remove_suffix(suffix.size()); \ 178 | return name; \ 179 | } \ 180 | }; \ 181 | return std::string(Ns::name_space()) + name; \ 182 | } 183 | 184 | #endif -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/common/lidar_device.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by nrsl on 2021/10/11. 3 | // 4 | 5 | #ifndef PERCEPTION3D_LIDAR_H 6 | #define PERCEPTION3D_LIDAR_H 7 | 8 | #include "pc_utils/common/common.h" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace pc_utils { 16 | struct LidarDevice { 17 | int index; 18 | std::string name; 19 | std::string type; 20 | std::string frame_id; 21 | std::string ros_topic; 22 | std::string relative_to; 23 | Eigen::Isometry3f pose; 24 | bool enable; 25 | bool fuse_relative; 26 | bool fuse_global; 27 | 28 | static void from_yaml(const YAML::Node &config, std::vector &devices) { 29 | /*** 30 | * 读取雷达配置 31 | */ 32 | for (int i = 0; i < config.size(); i++) { 33 | auto device = config[i]; 34 | auto lidar = device["device"]; 35 | auto init = lidar["calibration"]["init_pose"].as>(); 36 | 37 | devices.push_back({ 38 | .index=i, 39 | .name=lidar["name"].as(), 40 | .type=lidar["type"].as(), 41 | .frame_id=lidar["frame_id"].as(), 42 | .ros_topic=lidar["ros_topic"].as(), 43 | .relative_to=lidar["calibration"]["relative_to"].as(), 44 | .pose=fromXYZRPY(init[0], init[1], init[2], init[3], init[4], init[5]), 45 | .enable=lidar["calibration"]["enable"].as(), 46 | .fuse_relative=lidar["calibration"]["fuse_relative"].as(), 47 | .fuse_global=lidar["fuse_global"].as() 48 | }); 49 | } 50 | } 51 | 52 | static void from_yaml(const std::string &config_file, std::vector &devices) { 53 | if (not boost::filesystem::exists(config_file)) { 54 | throw std::runtime_error(config_file + std::string("no exist")); 55 | } else { 56 | auto config = YAML::LoadFile(config_file)["lidar"]; 57 | from_yaml(config, devices); 58 | } 59 | } 60 | }; 61 | 62 | } 63 | 64 | #endif //PERCEPTION3D_LIDAR_H 65 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/common/parameter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ou on 2021/12/2. 3 | // 4 | 5 | #ifndef PC_UTILS_PARAMETER_H 6 | #define PC_UTILS_PARAMETER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using Param = std::any; 14 | using Params = std::map; 15 | 16 | template 17 | inline ValueType any_lexical_cast(const std::any &a) { 18 | if constexpr(std::is_scalar::value) { 19 | ValueType result = ValueType(); 20 | if (a.type() == typeid(std::string) 21 | && boost::conversion::try_lexical_convert(std::any_cast(a), result)) { 22 | return boost::lexical_cast(std::any_cast(a)); 23 | } else if (a.type() == typeid(const char *) 24 | && boost::conversion::try_lexical_convert(std::any_cast(a), result)) { 25 | return boost::lexical_cast(std::any_cast(a)); 26 | } 27 | } 28 | return std::any_cast(a); 29 | } 30 | 31 | 32 | #endif //PC_UTILS_PARAMETER_H 33 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/common/point_type.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by nrsl on 2021/10/10. 3 | // 4 | 5 | #ifndef PC_UTILS_POINT_TYPE_H 6 | #define PC_UTILS_POINT_TYPE_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | /*** 13 | * @brief supported point type 14 | */ 15 | #define PC_UTILS_POINT_TYPE \ 16 | ((pcl::PointXYZ, XYZ)) \ 17 | ((pcl::PointXYZI, XYZI)) \ 18 | ((pcl::PointXYZL, XYZL)) \ 19 | ((pcl::PointNormal, XYZN)) \ 20 | ((pcl::PointXYZRGBNormal,XYZRGBN)) 21 | 22 | 23 | 24 | #endif //PC_UTILS_POINT_TYPE_H 25 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/common/utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ou on 2021/11/16. 3 | // 4 | 5 | #ifndef SRC_UTILS_H 6 | #define SRC_UTILS_H 7 | 8 | #include "point_type.h" 9 | 10 | /** 11 | * @brief: point cloud alias 12 | * @example: PCXYZ -> pcl::PointCloud 13 | */ 14 | #define PC_UTILS_DEF_CLOUD_TYPE(_, data, elem) \ 15 | using BOOST_PP_CAT(P,BOOST_PP_TUPLE_ELEM(1,elem)) = BOOST_PP_TUPLE_ELEM(0,elem); \ 16 | using BOOST_PP_CAT(PC,BOOST_PP_TUPLE_ELEM(1,elem)) = PC; \ 17 | using BOOST_PP_CAT(BOOST_PP_CAT(PC,BOOST_PP_TUPLE_ELEM(1,elem)),Ptr) = PC::Ptr;\ 18 | using BOOST_PP_CAT(BOOST_PP_CAT(PC,BOOST_PP_TUPLE_ELEM(1,elem)),ConstPtr) = PC::ConstPtr; 19 | 20 | namespace pc_utils { 21 | template 22 | using PC = pcl::PointCloud; 23 | BOOST_PP_SEQ_FOR_EACH(PC_UTILS_DEF_CLOUD_TYPE, _, PC_UTILS_POINT_TYPE) 24 | } // namespace pc_utils 25 | 26 | 27 | /*** 28 | * @brief for template specialization 29 | * @example: 30 | * PC_UTILS_TEMPLATE_SPECIALIZATIONS(CurvedVoxelCluster) 31 | * -> template class CurvedVoxelCluster; 32 | * -> template class CurvedVoxelCluster; 33 | * -> ... 34 | */ 35 | #define PC_UTILS_TEMPLATE_SPECIALIZATIONS_ONE_TYPE(_, CLASS, ELEM)\ 36 | template class CLASS; 37 | 38 | #define PC_UTILS_TEMPLATE_SPECIALIZATIONS(CLASS)\ 39 | BOOST_PP_SEQ_FOR_EACH(PC_UTILS_TEMPLATE_SPECIALIZATIONS_ONE_TYPE, CLASS, PC_UTILS_POINT_TYPE) 40 | 41 | 42 | /*** 43 | * @brief auto set params from yaml-node 44 | */ 45 | #define PC_UTILS_CLASS_MEMBER_0(_0, _1, _2) _0 46 | #define PC_UTILS_CLASS_MEMBER_1(_0, _1, _2) _1 47 | #define PC_UTILS_CLASS_MEMBER_2(_0, _1, _2) _2 48 | #define PC_UTILS_CLASS_MEMBER_TYPE(pack) PC_UTILS_CLASS_MEMBER_0 pack 49 | #define PC_UTILS_CLASS_MEMBER_NAME(pack) PC_UTILS_CLASS_MEMBER_1 pack 50 | #define PC_UTILS_CLASS_MEMBER_VALUE(pack) PC_UTILS_CLASS_MEMBER_2 pack 51 | 52 | /** 53 | @example #undef PC_UTILS_CLASS_MEMBER_LIST 54 | @example #define PC_UTILS_CLASS_MEMBER_LIST \ 55 | @example ((float, min_dis , )) \ 56 | @example ((float, min_points, )) 57 | @example PC_UTILS_DEF_CLASS_MEMBER 58 | */ 59 | /* define class member variable from arg list*/ 60 | #define PC_UTILS_DEF_CLASS_MEMBER_IMPL(_, __, elem) \ 61 | PC_UTILS_CLASS_MEMBER_TYPE(elem) PC_UTILS_CLASS_MEMBER_NAME(elem){PC_UTILS_CLASS_MEMBER_VALUE(elem)}; 62 | 63 | #define PC_UTILS_DEF_CLASS_MEMBER \ 64 | BOOST_PP_SEQ_FOR_EACH(PC_UTILS_DEF_CLASS_MEMBER_IMPL, _, PC_UTILS_CLASS_MEMBER_LIST) 65 | 66 | /* set class member variable from yaml-node */ 67 | #define PC_UTILS_SET_CLASS_MEMBER_FROM_YAML_IMPL(_, data, elem) \ 68 | PC_UTILS_CLASS_MEMBER_NAME(elem) = params[BOOST_PP_STRINGIZE(PC_UTILS_CLASS_MEMBER_NAME(elem))].as(); 69 | 70 | #define PC_UTILS_SET_CLASS_MEMBER_FROM_YAML \ 71 | BOOST_PP_SEQ_FOR_EACH(PC_UTILS_SET_CLASS_MEMBER_FROM_YAML_IMPL, _, PC_UTILS_CLASS_MEMBER_LIST) 72 | 73 | #define PC_UTILS_CONSTRUCTOR_YAML_NODE(CLASS) explicit CLASS(const YAML::Node ¶ms) {PC_UTILS_SET_CLASS_MEMBER_FROM_YAML} 74 | 75 | /*** 76 | * @brief quickly auto register 77 | */ 78 | #define AUTO_REGISTER(base, der, ...) \ 79 | public AutoRegister::template OverLoad<__VA_ARGS__> 80 | 81 | /*** 82 | * 83 | */ 84 | #define PC_UTILS_CLASS_DERIVATION_1(DER, _) DER 85 | #define PC_UTILS_CLASS_DERIVATION(_) PC_UTILS_CLASS_DERIVATION_1 _ 86 | #define PC_UTILS_CLASS_BASE_1(_, BASE) BASE 87 | #define PC_UTILS_CLASS_BASE(_) PC_UTILS_CLASS_BASE_1 _ 88 | 89 | #define PC_UTILS_CLASS_CONSTRUCTION_I(_, ctx, elem) public AutoRegister< \ 90 | PC_UTILS_CLASS_BASE_TYPE, \ 91 | ctx \ 92 | >::template OverLoad, 93 | 94 | 95 | #define PC_UTILS_BASE_LIST(DER) BOOST_PP_SEQ_FOR_EACH( \ 96 | PC_UTILS_CLASS_CONSTRUCTION_I, \ 97 | DER, \ 98 | PC_UTILS_CLASS_CONSTRUCTION) \ 99 | public PC_UTILS_CLASS_BASE_TYPE 100 | 101 | /*** 102 | * @brief for static link 103 | */ 104 | #define PC_UTILS_LINK_HELPER_HEADER(name) \ 105 | namespace pc_utils::_link { \ 106 | extern bool link_##name; \ 107 | static struct Link_##name##_Helper{ \ 108 | Link_##name##_Helper(){if(link_##name) std::cout<<"";} \ 109 | }helper##name; \ 110 | } 111 | 112 | #define PC_UTILS_LINK_HELPER_CPP(name) \ 113 | namespace pc_utils::_link {bool link_##name = true;} 114 | 115 | 116 | 117 | /** 118 | * class pre-declare 119 | */ 120 | namespace pc_utils { 121 | 122 | class BoundingBox; 123 | 124 | template 125 | class BoundingExtractor; 126 | 127 | template 128 | class CloudFilter; 129 | 130 | template 131 | class Cluster; 132 | 133 | template 134 | class GroundEstimator; 135 | 136 | }// namespace pc_utils 137 | 138 | 139 | #endif //SRC_UTILS_H 140 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/filter/cloud_filter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ou on 2021/12/2. 3 | // 4 | 5 | #ifndef PC_UTILS_FILTER_H 6 | #define PC_UTILS_FILTER_H 7 | 8 | #include "pc_utils/common/common.h" 9 | 10 | #define PC_UTILS_FILTER_TYPE \ 11 | define( PassThroughFilter ) \ 12 | define( CropAABoxFilter ) \ 13 | define( CropOBoxFilter ) \ 14 | define( RegionOfInterestFilter ) \ 15 | define( ApproximateVoxelFilter ) \ 16 | define( SelfFilter ) \ 17 | define( MaxPointCountFilter ) \ 18 | define( RandomSamplingFilter ) \ 19 | define( RemoveNaNFilter ) \ 20 | define( Filters ) 21 | 22 | namespace pc_utils { 23 | template 24 | class CloudFilter { 25 | public: 26 | virtual void 27 | filter(const typename PC::Ptr &input, typename PC::Ptr &output, void *data = nullptr) = 0; 28 | 29 | virtual std::string class_name() = 0; 30 | }; // namespace pc_utils 31 | 32 | 33 | } 34 | 35 | 36 | PC_UTILS_LINK_HELPER_HEADER(CloudFilter) 37 | 38 | #endif //PC_UTILS_FILTER_H 39 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/seg/cluster.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by nrsl on 2021/10/11. 3 | // 4 | 5 | #ifndef PERCEPTION3D_CLUSTER_H 6 | #define PERCEPTION3D_CLUSTER_H 7 | 8 | #include "pc_utils/common/common.h" 9 | /** 10 | * @brief 11 | * @typedef EuclideanCluster 12 | * @typedef CurvedVoxelCluster 13 | */ 14 | #define PC_UTILS_CLUSTER_TYPE \ 15 | define( EuclideanCluster ) \ 16 | define( CurvedVoxelCluster ) 17 | 18 | 19 | namespace pc_utils { 20 | 21 | template 22 | class Cluster { 23 | public: 24 | virtual void extract(const typename PC::Ptr &cloud_in, 25 | std::vector &cluster_indices, 26 | std::vector &cluster_id) = 0; 27 | }; 28 | 29 | 30 | } // namespace pc_utils 31 | 32 | 33 | 34 | 35 | 36 | 37 | PC_UTILS_LINK_HELPER_HEADER(Cluster) 38 | #endif //PERCEPTION3D_CLUSTER_H 39 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/include/pc_utils/seg/ground_estimator.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ou on 2021/10/10. 3 | // 4 | 5 | #ifndef PERCEPTION3D_GROUND_ESTIMATE_H 6 | #define PERCEPTION3D_GROUND_ESTIMATE_H 7 | 8 | #include "pc_utils/common/common.h" 9 | /** 10 | * @typedef RansacGroundEstimator 11 | * @typedef PatchWorkGroundEstimator 12 | * @typedef RingShapedElevationConjunctionMap 13 | */ 14 | 15 | 16 | namespace pc_utils { 17 | 18 | template 19 | class GroundEstimator { 20 | public: 21 | virtual void estimate(const typename pcl::PointCloud::Ptr &cloud_in, 22 | typename pcl::PointCloud::Ptr &cloud_ground, 23 | typename pcl::PointCloud::Ptr &cloud_no_ground) = 0; 24 | 25 | virtual void estimate(const typename pcl::PointCloud::Ptr &cloud_in, 26 | std::vector &ind_ground, std::vector &ind_non_ground) = 0; 27 | }; 28 | 29 | } // namespace pc_utils 30 | 31 | 32 | PC_UTILS_LINK_HELPER_HEADER(GroundEstimator) 33 | #endif //PERCEPTION3D_GROUND_ESTIMATE_H 34 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pc_utils/lib/libpc_utils.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/src/rviz_cloud_annotation/pc_utils/lib/libpc_utils.so -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pcl_include/colors.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | /// Lookup table 5 | static const unsigned char GLASBEY_LUT[] = 6 | { 7 | 228, 26 , 28 , 8 | 77 , 175, 74 , 9 | 55 , 126, 184, 10 | 152, 78 , 163, 11 | 255, 127, 0 , 12 | 255, 255, 51 , 13 | 166, 86 , 40 , 14 | 247, 129, 191, 15 | 153, 153, 153, 16 | 0 , 0 , 255, 17 | 255, 0 , 255, 18 | 0 , 255, 248, 19 | 0 , 255, 0 , 20 | 0 , 101, 255, 21 | 255, 255, 180, 22 | 52 , 68 , 1 , 23 | 0 , 0 , 68 , 24 | 96 , 0 , 41 , 25 | 158, 147, 0 , 26 | 116, 0 , 185, 27 | 255, 0 , 114, 28 | 0 , 149, 125, 29 | 209, 186, 255, 30 | 255, 183, 156, 31 | 240, 0 , 174, 32 | 208, 255, 245, 33 | 0 , 255, 176, 34 | 170, 255, 93 , 35 | 0 , 207, 255, 36 | 255, 190, 1 , 37 | 241, 117, 255, 38 | 52 , 74 , 167, 39 | 150, 166, 103, 40 | 255, 114, 114, 41 | 171, 100, 109, 42 | 161, 0 , 41 , 43 | 160, 135, 255, 44 | 105, 86 , 121, 45 | 178, 21 , 105, 46 | 0 , 3 , 123, 47 | 255, 221, 236, 48 | 160, 238, 173, 49 | 237, 161, 77 , 50 | 0 , 141, 255, 51 | 0 , 97 , 109, 52 | 1 , 238, 98 , 53 | 81 , 0 , 78 , 54 | 128, 103, 66 , 55 | 0 , 108, 44 , 56 | 209, 224, 94 , 57 | 155, 0 , 255, 58 | 0 , 45 , 223, 59 | 88 , 28 , 0 , 60 | 166, 2 , 162, 61 | 165, 205, 239, 62 | 84 , 121, 0 , 63 | 76 , 109, 80 , 64 | 122, 180, 0 , 65 | 104, 204, 204, 66 | 145, 95 , 255, 67 | 214, 208, 177, 68 | 185, 130, 176, 69 | 130, 120, 194, 70 | 128, 96 , 0 , 71 | 247, 161, 255, 72 | 10 , 65 , 119, 73 | 232, 102, 54 , 74 | 7 , 191, 131, 75 | 105, 54 , 171, 76 | 10 , 177, 0 , 77 | 215, 191, 105, 78 | 198, 66 , 249, 79 | 140, 255, 145, 80 | 135, 60 , 105, 81 | 254, 170, 191, 82 | 130, 173, 255, 83 | 161, 35 , 0 , 84 | 197, 255, 0 , 85 | 40 , 153, 180, 86 | 215, 83 , 185, 87 | 255, 77 , 161, 88 | 128, 175, 147, 89 | 216, 91 , 124, 90 | 193, 144, 91 , 91 | 210, 196, 0 , 92 | 232, 39 , 73 , 93 | 76 , 52 , 116, 94 | 159, 206, 110, 95 | 138, 147, 187, 96 | 140, 5 , 114, 97 | 0 , 56 , 183, 98 | 191, 105, 0 , 99 | 83 , 58 , 0 , 100 | 94 , 224, 0 , 101 | 121, 99 , 99 , 102 | 212, 123, 104, 103 | 89 , 160, 99 , 104 | 146, 58 , 54 , 105 | 231, 46 , 215, 106 | 93 , 245, 200, 107 | 191, 147, 133, 108 | 255, 211, 89 , 109 | 171, 77 , 214, 110 | 0 , 121, 0 , 111 | 60 , 14 , 107, 112 | 255, 82 , 1 , 113 | 112, 115, 43 , 114 | 0 , 172, 245, 115 | 255, 184, 240, 116 | 1 , 210, 111, 117 | 203, 151, 0 , 118 | 95 , 114, 129, 119 | 183, 215, 17 , 120 | 80 , 110, 231, 121 | 201, 25 , 87 , 122 | 218, 250, 203, 123 | 255, 148, 103, 124 | 255, 217, 163, 125 | 198, 172, 199, 126 | 78 , 139, 135, 127 | 197, 255, 134, 128 | 38 , 0 , 165, 129 | 197, 208, 211, 130 | 193, 117, 225, 131 | 111, 0 , 128, 132 | 147, 255, 238, 133 | 125, 62 , 254, 134 | 112, 213, 78 , 135 | 198, 76 , 61 , 136 | 155, 48 , 82 , 137 | 0 , 199, 176, 138 | 118, 6 , 0 , 139 | 2 , 106, 192, 140 | 140, 167, 49 , 141 | 189, 81 , 145, 142 | 254, 162, 38 , 143 | 134, 138, 106, 144 | 0 , 68 , 17 , 145 | 122, 73 , 61 , 146 | 255, 251, 239, 147 | 127, 94 , 193, 148 | 181, 140, 48 , 149 | 66 , 235, 255, 150 | 189, 140, 218, 151 | 190, 0 , 138, 152 | 132, 177, 185, 153 | 90 , 54 , 202, 154 | 0 , 35 , 131, 155 | 251, 139, 149, 156 | 74 , 0 , 225, 157 | 0 , 106, 90 , 158 | 106, 199, 155, 159 | 104, 169, 217, 160 | 255, 239, 134, 161 | 44 , 94 , 130, 162 | 126, 0 , 78 , 163 | 196, 214, 145, 164 | 160, 238, 255, 165 | 222, 215, 255, 166 | 255, 87 , 126, 167 | 170, 161, 255, 168 | 81 , 120, 60 , 169 | 255, 242, 91 , 170 | 168, 130, 145, 171 | 158, 153, 64 , 172 | 211, 123, 156, 173 | 255, 0 , 3 , 174 | 210, 118, 197, 175 | 0 , 41 , 111, 176 | 198, 178, 125, 177 | 211, 255, 169, 178 | 109, 215, 130, 179 | 41 , 90 , 0 , 180 | 235, 193, 183, 181 | 114, 58 , 0 , 182 | 140, 96 , 155, 183 | 163, 223, 193, 184 | 255, 142, 63 , 185 | 66 , 155, 1 , 186 | 83 , 96 , 152, 187 | 106, 133, 230, 188 | 255, 85 , 72 , 189 | 141, 216, 0 , 190 | 162, 102, 73 , 191 | 79 , 0 , 146, 192 | 190, 0 , 30 , 193 | 165, 0 , 193, 194 | 81 , 84 , 255, 195 | 0 , 148, 74 , 196 | 203, 0 , 255, 197 | 121, 54 , 71 , 198 | 215, 255, 97 , 199 | 163, 178, 0 , 200 | 111, 154, 68 , 201 | 120, 93 , 222, 202 | 254, 187, 126, 203 | 112, 0 , 27 , 204 | 204, 59 , 0 , 205 | 0 , 165, 167, 206 | 151, 255, 0 , 207 | 104, 41 , 124, 208 | 138, 89 , 113, 209 | 255, 94 , 224, 210 | 86 , 91 , 48 , 211 | 75 , 255, 76 , 212 | 204, 190, 67 , 213 | 255, 224, 0 , 214 | 49 , 126, 85 , 215 | 145, 196, 135, 216 | 187, 64 , 79 , 217 | 255, 130, 233, 218 | 205, 127, 68 , 219 | 105, 195, 223, 220 | 161, 213, 81 , 221 | 165, 183, 240, 222 | 125, 255, 180, 223 | 126, 255, 111, 224 | 67 , 255, 145, 225 | 154, 138, 83 , 226 | 46 , 145, 231, 227 | 118, 121, 0 , 228 | 154, 2 , 222, 229 | 185, 119, 255, 230 | 255, 0 , 62 , 231 | 131, 28 , 170, 232 | 177, 70 , 183, 233 | 217, 0 , 115, 234 | 186, 196, 95 , 235 | 97 , 46 , 97 , 236 | 84 , 134, 167, 237 | 81 , 54 , 145, 238 | 107, 117, 107, 239 | 51 , 15 , 80 , 240 | 215, 139, 143, 241 | 255, 247, 203, 242 | 255, 86 , 192, 243 | 153, 91 , 0 , 244 | 255, 1 , 156, 245 | 183, 79 , 19 , 246 | 235, 255, 146, 247 | 211, 1 , 224, 248 | 178, 167, 144, 249 | 217, 97 , 0 , 250 | 134, 91 , 38 , 251 | 175, 151, 206, 252 | 0 , 182, 63 , 253 | 210, 40 , 179, 254 | 2 , 213, 42 , 255 | 94 , 84 , 160, 256 | 136, 48 , 0 , 257 | 255, 110, 163, 258 | 144, 121, 157, 259 | 153, 61 , 225, 260 | 237, 87 , 255, 261 | 87 , 24 , 206, 262 | 117, 143, 207, 263 | }; 264 | 265 | /// Number of colors in Glasbey lookup table 266 | static const unsigned int GLASBEY_LUT_SIZE = sizeof (GLASBEY_LUT) / (sizeof (GLASBEY_LUT[0]) * 3); 267 | 268 | pcl::RGB 269 | pcl::GlasbeyLUT::at (unsigned int color_id) 270 | { 271 | assert (color_id < GLASBEY_LUT_SIZE); 272 | pcl::RGB color; 273 | color.r = GLASBEY_LUT[color_id * 3 + 0]; 274 | color.g = GLASBEY_LUT[color_id * 3 + 1]; 275 | color.b = GLASBEY_LUT[color_id * 3 + 2]; 276 | return (color); 277 | } 278 | 279 | size_t 280 | pcl::GlasbeyLUT::size () 281 | { 282 | return GLASBEY_LUT_SIZE; 283 | } 284 | 285 | const unsigned char* 286 | pcl::GlasbeyLUT::data () 287 | { 288 | return GLASBEY_LUT; 289 | } 290 | 291 | pcl::RGB 292 | pcl::getRandomColor (double min, double max) 293 | { 294 | double sum; 295 | static unsigned stepRGBA = 100; 296 | double r, g, b; 297 | do 298 | { 299 | sum = 0; 300 | r = (rand () % stepRGBA) / static_cast (stepRGBA); 301 | while ((g = (rand () % stepRGBA) / static_cast (stepRGBA)) == r) {} 302 | while (((b = (rand () % stepRGBA) / static_cast (stepRGBA)) == r) && (b == g)) {} 303 | sum = r + g + b; 304 | } 305 | while (sum <= min || sum >= max); 306 | pcl::RGB color; 307 | color.r = uint8_t (r * 255.0); 308 | color.g = uint8_t (g * 255.0); 309 | color.b = uint8_t (b * 255.0); 310 | return (color); 311 | } 312 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/pcl_include/pcl/common/colors.h: -------------------------------------------------------------------------------- 1 | #ifndef PCL_COMMON_COLORS_H 2 | #define PCL_COMMON_COLORS_H 3 | 4 | #include 5 | 6 | namespace pcl 7 | { 8 | 9 | struct RGB; 10 | 11 | PCL_EXPORTS RGB 12 | getRandomColor (double min = 0.2, double max = 2.8); 13 | 14 | class PCL_EXPORTS GlasbeyLUT 15 | { 16 | 17 | public: 18 | 19 | /** Get a color from the lookup table with a given id. 20 | * 21 | * The id should be less than the size of the LUT (see size()). */ 22 | static RGB at (unsigned int color_id); 23 | 24 | /** Get the number of colors in the lookup table. 25 | * 26 | * Note: the number of colors is different from the number of elements 27 | * in the lookup table (each color is defined by three bytes). */ 28 | static size_t size (); 29 | 30 | /** Get a raw pointer to the lookup table. */ 31 | static const unsigned char* data (); 32 | 33 | }; 34 | 35 | } 36 | 37 | #endif /* PCL_COMMON_COLORS_H */ 38 | 39 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/rviz/annotation.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Grid1 9 | Splitter Ratio: 0.5267665982246399 10 | Tree Height: 353 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: ~ 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | - Class: rviz/Time 23 | Experimental: false 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: "" 27 | - Class: rviz_cloud_annotation/Annotation 28 | Name: Annotation 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 100 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/InteractiveMarkers 55 | Enable Transparency: true 56 | Enabled: true 57 | Name: points 58 | Show Axes: false 59 | Show Descriptions: true 60 | Show Visual Aids: false 61 | Update Topic: /point_cloud/update 62 | Value: true 63 | - Class: rviz/Marker 64 | Enabled: true 65 | Marker Topic: /rviz_cloud_annotation_node/bbox_marker 66 | Name: bbox 67 | Namespaces: 68 | {} 69 | Queue Size: 100 70 | Value: true 71 | - Class: rviz/Marker 72 | Enabled: true 73 | Marker Topic: /rviz_cloud_annotation_node/kerb_marker 74 | Name: kerb 75 | Namespaces: 76 | {} 77 | Queue Size: 100 78 | Value: true 79 | - Class: rviz/Marker 80 | Enabled: true 81 | Marker Topic: /rviz_cloud_annotation_node/lane_marker 82 | Name: lane 83 | Namespaces: 84 | {} 85 | Queue Size: 100 86 | Value: true 87 | - Class: rviz/Marker 88 | Enabled: true 89 | Marker Topic: /rviz_cloud_annotation_node/plane_marker 90 | Name: plane 91 | Namespaces: 92 | {} 93 | Queue Size: 100 94 | Value: true 95 | - Class: rviz/Axes 96 | Enabled: true 97 | Length: 1 98 | Name: Axes 99 | Radius: 0.10000000149011612 100 | Reference Frame: 101 | Value: true 102 | - Class: rviz/Marker 103 | Enabled: true 104 | Marker Topic: /rviz_cloud_annotation_node/bbox_head_marker 105 | Name: bbox_head 106 | Namespaces: 107 | {} 108 | Queue Size: 100 109 | Value: true 110 | - Class: rviz/Image 111 | Enabled: false 112 | Image Topic: /rviz_cloud_annotation/image_label 113 | Max Value: 1 114 | Median window: 5 115 | Min Value: 0 116 | Name: Image 117 | Normalize Range: true 118 | Queue Size: 2 119 | Transport Hint: raw 120 | Unreliable: false 121 | Value: false 122 | Enabled: true 123 | Global Options: 124 | Background Color: 48; 48; 48 125 | Default Light: true 126 | Fixed Frame: base_link 127 | Frame Rate: 30 128 | Name: root 129 | Tools: 130 | - Class: rviz_cloud_annotation/Annotation Tool 131 | - Class: rviz/Interact 132 | Hide Inactive Objects: true 133 | - Class: rviz/MoveCamera 134 | - Class: rviz_plugin_tutorials/PlantFlag 135 | Flags: ~ 136 | Value: true 137 | Views: 138 | Current: 139 | Class: rviz/Orbit 140 | Distance: 74.5174560546875 141 | Enable Stereo Rendering: 142 | Stereo Eye Separation: 0.05999999865889549 143 | Stereo Focal Distance: 1 144 | Swap Stereo Eyes: false 145 | Value: false 146 | Focal Point: 147 | X: -1.3472211360931396 148 | Y: -0.4886458218097687 149 | Z: 1.9714674949645996 150 | Focal Shape Fixed Size: true 151 | Focal Shape Size: 0.05000000074505806 152 | Invert Z Axis: false 153 | Name: Current View 154 | Near Clip Distance: 0.009999999776482582 155 | Pitch: 0.5147964954376221 156 | Target Frame: 157 | Value: Orbit (rviz) 158 | Yaw: 2.0945746898651123 159 | Saved: ~ 160 | Window Geometry: 161 | Annotation: 162 | collapsed: false 163 | Displays: 164 | collapsed: false 165 | Height: 1052 166 | Hide Left Dock: false 167 | Hide Right Dock: true 168 | Image: 169 | collapsed: false 170 | QMainWindow State: 000000ff00000000fd00000004000000000000025a0000037efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000019e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a00500061006e0065006c00000001da000000790000000000000000fb000000140041006e006e006f0074006100740069006f006e01000001e1000001da000001da00fffffffb0000000a0049006d00610067006502000005000000025800000280000001e0fb0000000a0049006d0061006700650300000661000002e70000010d000000ed000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000002a9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005200000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 171 | Selection: 172 | collapsed: false 173 | Time: 174 | collapsed: false 175 | Tool Properties: 176 | collapsed: false 177 | Views: 178 | collapsed: true 179 | Width: 1920 180 | X: 1080 181 | Y: 438 182 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/rviz_plugin/cloud_annotation.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Panel to control the rviz_cloud_annotation node from the RViz interface. 7 | 8 | 9 | 12 | 13 | Annotation tool for rviz_cloud_annotation 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_cloud_plane_curves_extract.cpp: -------------------------------------------------------------------------------- 1 | #include "point_cloud_plane_curves_extract.h" 2 | 3 | void PointCloudPlaneCurvesExtract::SearchCurves(const PointXYZRGBNormalCloud &PointCloud) 4 | { 5 | for (int i = 0; i < _numOfRings; i++) 6 | { 7 | mCurvesVector[i].reserve(_defaultCurveSize); 8 | mCurvesId[i].reserve(_defaultCurveSize); 9 | } 10 | for (uint64 i = 0; i < PointCloud.size(); i++) 11 | { 12 | PointXYZRGBNormal point = PointCloud[i]; 13 | float angle = std::atan(point.z / m_sqrt(point.x * point.x + point.y * point.y)) / (1.0f * M_PI) * 180.0f; 14 | int64 ringID = GetScanringID(angle); 15 | if (ringID < _numOfRings && ringID >= 0) 16 | { 17 | mCurvesVector[ringID].push_back(point); 18 | mCurvesId[ringID].push_back(i); 19 | } 20 | } 21 | 22 | for (int i = 0; i < _planeRings; i++) 23 | { 24 | mScanringRadius[i] = GetScanringRadius(i); 25 | } 26 | // DensityFilter 27 | for (int i = 0; i < _planeRings; i++) 28 | { 29 | if (mCurvesVector[i].size() > _curveSizeThreshold) 30 | { 31 | CurveDensityFilter(mCurvesVector[i], i, mCurvesId[i], mDensityCurvesVector[i]); 32 | } 33 | } 34 | // RadiusFilter 35 | CurvesRadiusFilter(mDensityCurvesVector, mDensityCurvesId); 36 | // SizeFilter 37 | for (int i = 0; i < _planeRings; i++) 38 | { 39 | if (mRadiusCurvesVector[i].size() > _curveSizeThreshold) 40 | { 41 | CurveSizeFilter(mRadiusCurvesVector[i], i, mRadiusCurvesId[i], mSizeCurvesVector[i]); 42 | } 43 | } 44 | } 45 | 46 | void PointCloudPlaneCurvesExtract::CurveDensityFilter(const PointXYZRGBNormalCloud &Curve, const int64 ringID, 47 | const Uint64Vector &curveId, PointXYZRGBNormalCloud &outCurve) 48 | { 49 | outCurve.reserve(Curve.size()); 50 | mDensityCurvesId[ringID].reserve(curveId.size()); 51 | float TrueRadius = mScanringRadius[ringID]; 52 | if (TrueRadius < 0) 53 | { 54 | return; 55 | } 56 | float arcLen = 0; 57 | float radiusMean = 0; 58 | float radius = TrueRadius; 59 | float radius_1 = TrueRadius; 60 | int64 arcNum = 0; 61 | for (int i = 1; i < Curve.size(); i += 1) 62 | { 63 | float dx = Curve[i].x - Curve[i - 1].x; 64 | float dy = Curve[i].y - Curve[i - 1].y; 65 | float dz = Curve[i].z - Curve[i - 1].z; 66 | arcNum++; 67 | radius = m_sqrt(Curve[i].x * Curve[i].x + Curve[i].y * Curve[i].y); 68 | radiusMean += radius; 69 | arcLen += (m_sqrt(dx * dx + dy * dy + dz * dz)) * (2 * _basicRadius / (radius + radius_1)); 70 | radius_1 = radius; 71 | if (arcLen >= _srcLenThreshold) 72 | { 73 | radiusMean /= arcNum; 74 | if (arcNum > _arcNumThreshold) 75 | { 76 | for (int k = i - arcNum + 1; k <= i; k++) 77 | { 78 | mDensityCurvesId[ringID].push_back(curveId[k]); 79 | outCurve.push_back(Curve[k]); 80 | } 81 | } 82 | arcLen = 0; 83 | arcNum = 0; 84 | radiusMean = 0; 85 | } 86 | } 87 | } 88 | 89 | void *PointCloudPlaneCurvesExtract::CurvesRadiusFilter(PointXYZRGBNormalCloud *CurvesVector, 90 | const Uint64Vector *CurvesId) 91 | { 92 | for (int i = 0; i < _planeRings; i++) 93 | { 94 | mSentorLabelVector[i].reserve(CurvesVector[i].size()); 95 | for (int j = 0; j < CurvesVector[i].size(); j++) 96 | { 97 | float x = CurvesVector[i][j].x; 98 | float y = CurvesVector[i][j].y; 99 | int AngleGridId = (int)(((atan2f(y, x) / M_PI * 180 + 180) / _AngleGridResolution)); 100 | if (AngleGridId < 0 || AngleGridId >= _numOfAngleGrid) 101 | { 102 | continue; 103 | } 104 | CurvesVector[i][j].rgba = (uint)AngleGridId; 105 | float radius = m_sqrt(x * x + y * y); 106 | CurvesVector[i][j].curvature = radius; // / mScanringRadius[i]; 107 | 108 | mSentorIds[i][AngleGridId].push_back(j); 109 | mSentorLabelVector[i].push_back(0); 110 | mSentorMeanRadius[i][AngleGridId] = 111 | (mSentorMeanRadius[i][AngleGridId] * mSentorIds[i][AngleGridId].size() + radius) / 112 | (mSentorIds[i][AngleGridId].size() + 1); 113 | } 114 | } 115 | 116 | for (int i = 0; i < _planeRings - 1; i++) 117 | { 118 | for (int j = 0; j < _numOfAngleGrid; j++) 119 | { 120 | if (fabs(mSentorMeanRadius[i][j] - mSentorMeanRadius[i + 1][j]) < 121 | _radiusScaleThreshold * fabs(mScanringRadius[i + 1] - mScanringRadius[i])) 122 | { 123 | if (i == 0) 124 | { 125 | for (int k = 0; k < mSentorIds[i][j].size(); k++) 126 | { 127 | uint64 id = mSentorIds[i][j][k]; 128 | mSentorLabelVector[i][id] = -1; 129 | } 130 | mSentorIds[i][j].clear(); 131 | } 132 | for (int k = 0; k < mSentorIds[i + 1][j].size(); k++) 133 | { 134 | uint64 id = mSentorIds[i + 1][j][k]; 135 | mSentorLabelVector[i + 1][id] = -1; 136 | } 137 | mSentorIds[i + 1][j].clear(); 138 | } 139 | } 140 | } 141 | for (int i = 0; i < _planeRings; i++) 142 | { 143 | for (int j = 0; j < CurvesVector[i].size(); j++) 144 | { 145 | if (mSentorLabelVector[i][j] != -1) 146 | { 147 | mRadiusCurvesId[i].push_back(CurvesId[i][j]); 148 | mRadiusCurvesVector[i].push_back(CurvesVector[i][j]); 149 | } 150 | } 151 | } 152 | } 153 | 154 | void PointCloudPlaneCurvesExtract::CurveSizeFilter(const PointXYZRGBNormalCloud &Curve, const int64 ringID, 155 | const Uint64Vector &curveId, PointXYZRGBNormalCloud &outCurve) 156 | { 157 | outCurve.reserve(Curve.size()); 158 | mSizeCurvesId[ringID].reserve(curveId.size()); 159 | float TrueRadius = mScanringRadius[ringID]; 160 | float ratio = _basicRadius / TrueRadius; 161 | if (TrueRadius < 0 || Curve.size() < 1) 162 | { 163 | return; 164 | } 165 | int LabelArray[Curve.size()]; 166 | int64 begin = 0; 167 | int64 end = 0; 168 | float meanZ = 0; 169 | for (uint64 i = 1; i < Curve.size(); i += 1) 170 | { 171 | LabelArray[i] = 0; 172 | float dx = Curve[i].x - Curve[i - 1].x; 173 | float dy = Curve[i].y - Curve[i - 1].y; 174 | float dz = Curve[i].z - Curve[i - 1].z; 175 | meanZ += dz; 176 | float dis = (m_sqrt(dx * dx + dy * dy + dz * dz)) * (ratio); 177 | if (dis > _breakingDistanceThreshold) 178 | { 179 | end = i; 180 | meanZ /= (end - begin); 181 | if (end - begin < _breakingSizeThreshold) 182 | { 183 | for (uint64 k = begin; k < end; k++) 184 | { 185 | LabelArray[k] = -1; 186 | } 187 | } 188 | begin = end; 189 | } 190 | } 191 | for (uint64 i = 0; i < Curve.size(); i += 1) 192 | { 193 | if (LabelArray[i] != -1) 194 | { 195 | mSizeCurvesId[ringID].push_back(curveId[i]); 196 | outCurve.push_back(Curve[i]); 197 | } 198 | } 199 | } 200 | 201 | int64 PointCloudPlaneCurvesExtract::GetScanringID(const float &angle) 202 | { 203 | return static_cast((angle - _lowerBound) / (1.0f * (_upperBound - _lowerBound)) * (_numOfRings - 1) + 0.5); 204 | } 205 | 206 | float PointCloudPlaneCurvesExtract::GetScanringRadius(const int64 ID) 207 | { 208 | float TrueRadius = -1; 209 | float angle = (_upperBound - _lowerBound) * 1.0 / _numOfRings * ID + _lowerBound; 210 | if (angle < -2) 211 | { 212 | TrueRadius = _basicRadius * fabs(tan(_lowerBound * 1.0 / 180 * M_PI)) * fabs(tan((90 + angle) * 1.0 / 180 * M_PI)); 213 | } 214 | return TrueRadius; 215 | } -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_cloud_plane_curves_extract.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_CLOUD_PLANE_CURVES 2 | #define POINT_CLOUD_PLANE_CURVES 3 | 4 | #include "point_cloud_plane_params.h" 5 | 6 | class PointCloudPlaneCurvesExtract 7 | { 8 | public: 9 | PointXYZRGBNormalCloud mCurvesVector[_numOfRings]; 10 | PointXYZRGBNormalCloud mDensityCurvesVector[_numOfRings]; 11 | PointXYZRGBNormalCloud mRadiusCurvesVector[_numOfRings]; 12 | PointXYZRGBNormalCloud mSizeCurvesVector[_numOfRings]; 13 | 14 | Uint64Vector mCurvesId[_numOfRings]; 15 | Uint64Vector mDensityCurvesId[_numOfRings]; 16 | Uint64Vector mSizeCurvesId[_numOfRings]; 17 | Uint64Vector mRadiusCurvesId[_numOfRings]; 18 | 19 | Uint64Vector mAnglePointId[_numOfRings][_numOfAngleGrid]; 20 | PointXYZRGBNormalCloud mAnglePointVector[_numOfRings][_numOfAngleGrid]; 21 | 22 | float mSentorMeanRadius[_numOfRings][_numOfAngleGrid] = {{0}}; 23 | Uint64Vector mSentorIds[_numOfRings][_numOfAngleGrid]; 24 | Uint64Vector mSentorLabelVector[_numOfRings]; 25 | 26 | float mScanringRadius[_numOfRings]; 27 | 28 | void SearchCurves(const PointXYZRGBNormalCloud &PointCloud); 29 | void CurveDensityFilter(const PointXYZRGBNormalCloud &Curve, const int64 ringID, const Uint64Vector &curveId, 30 | PointXYZRGBNormalCloud &outCurve); 31 | void *CurvesRadiusFilter(PointXYZRGBNormalCloud *CurvesVector, const Uint64Vector *CurvesId); 32 | void CurveSizeFilter(const PointXYZRGBNormalCloud &Curve, const int64 ringID, const Uint64Vector &curveId, 33 | PointXYZRGBNormalCloud &outCurve); 34 | 35 | int64 GetScanringID(const float &angle); 36 | float GetScanringRadius(const int64 ID); 37 | }; 38 | #endif -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_cloud_plane_params.cpp: -------------------------------------------------------------------------------- 1 | #include "point_cloud_plane_params.h" 2 | 3 | float m_sqrt(float x) 4 | { 5 | float half_x = 0.5 * x; 6 | int i = *((int *)&x); 7 | i = 0x5f3759df - (i >> 1); 8 | x = *((float *)&i); 9 | x = x * (1.5 - (half_x * x * x)); 10 | return 1 / x; 11 | } 12 | float getVar(float x[], int len) 13 | { 14 | int m = len; 15 | float sum = 0; 16 | for (int i = 0; i < m; i++) 17 | { 18 | sum += x[i]; 19 | } 20 | float dAve = sum / m; 21 | float dVar = 0; 22 | for (int i = 0; i < m; i++) 23 | { 24 | dVar += (x[i] - dAve) * (x[i] - dAve); 25 | } 26 | return dVar / m; 27 | } -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_cloud_plane_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef POINT_CLOUD_PLANE_PARAMS 3 | #define POINT_CLOUD_PLANE_PARAMS 4 | // STL 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | // PCL 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // System Params 20 | #define _lowerBound -15 21 | #define _upperBound 15 22 | #define _numOfRings 16 23 | #define _horizontalAngleResolution 0.4 24 | //Filter Params 25 | #define _basicRadius 6.9 //基准圆半径 26 | #define _planeRings 6 //Ground ring Size 27 | #define _defaultCurveSize 2000 28 | #define _curveSizeThreshold 50 // Curve点数阈值 29 | // Density Filter Params 30 | #define _srcLenThreshold 0.2 31 | #define _arcNumThreshold 7 32 | // Radius Filter Params 33 | #define _AngleGridResolution 2.0 34 | #define _numOfAngleGrid 180 // = 360 / _AngleGridResolution 35 | #define _radiusScaleThreshold 0.2 36 | // Size Filter Params 37 | #define _breakingDistanceThreshold 0.2 38 | #define _breakingSizeThreshold 30 39 | 40 | #define _max(a, b) (((a) > (b)) ? (a) : (b)) 41 | #define _min(a, b) (((a) > (b)) ? (b) : (a)) 42 | 43 | typedef int64_t int64; 44 | typedef uint64_t uint64; 45 | typedef unsigned short ushort; 46 | typedef std::vector Uint64Vector; 47 | typedef std::vector Int64Vector; 48 | 49 | typedef pcl::PointXYZRGBNormal PointXYZRGBNormal; 50 | typedef pcl::PointCloud PointXYZRGBNormalCloud; 51 | 52 | float m_sqrt(float x); //Fast Sqrt 53 | float getVar(float x[], int len); 54 | #endif -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_neighborhood.cpp: -------------------------------------------------------------------------------- 1 | #include "point_neighborhood.h" 2 | #include 3 | #include 4 | 5 | PointNeighborhood::PointNeighborhood(PointXYZRGBNormalCloud::ConstPtr cloudptr, const Conf &conf) 6 | { 7 | const PointXYZRGBNormalCloud &cloud = *cloudptr; 8 | const uint64 cloud_size = cloud.size(); 9 | 10 | m_conf = conf; 11 | 12 | m_index.resize(cloud_size); 13 | 14 | Uint64Vector temporary_indices_vector(cloud_size); 15 | 16 | KdTree::Ptr kdtree(new KdTree); 17 | 18 | kdtree->setInputCloud(cloudptr); 19 | 20 | uint64 counter = 0; 21 | 22 | std::vector idxs; 23 | std::vector dsts; 24 | for (uint64 i = 0; i < cloud_size; i++) 25 | { 26 | const PointXYZRGBNormal &pt = cloud[i]; 27 | conf.searcher->Search(*kdtree, pt, idxs, dsts); 28 | const uint64 size = idxs.size(); 29 | 30 | m_index[i].size = 0; 31 | 32 | if (size == 0) 33 | continue; 34 | m_neighbors.resize(counter + size); 35 | m_total_dists.resize(counter + size); 36 | m_position_dists.resize(counter + size); 37 | uint64 inc = 0; 38 | for (uint64 h = 0; h < size; h++) 39 | if (uint64(idxs[h]) != i) // do not add self 40 | { 41 | m_neighbors[counter + inc] = idxs[h]; 42 | m_total_dists[counter + inc] = TotalDistance(cloud[i], cloud[idxs[h]], conf); 43 | m_position_dists[counter + inc] = std::sqrt(dsts[h]); 44 | inc++; 45 | } 46 | 47 | temporary_indices_vector[i] = counter; 48 | counter += inc; 49 | m_index[i].size = inc; 50 | } 51 | 52 | // if requested by the searcher, remove all non-symmetric links 53 | if (conf.searcher->IsPostProcessingRequired(PointNeighborhoodSearch::PPTYPE_REMOVE_UNIDIRECTIONAL_LINKS)) 54 | RemoveUnidirectionalLinks(temporary_indices_vector); 55 | 56 | if (conf.searcher->IsPostProcessingRequired(PointNeighborhoodSearch::PPTYPE_COMPLETE_UNIDIRECTIONAL_LINKS)) 57 | AddUnidirectionalLinks(temporary_indices_vector); 58 | 59 | // update pointers 60 | for (uint64 i = 0; i < cloud_size; i++) 61 | { 62 | m_index[i].neighbors = &(m_neighbors[temporary_indices_vector[i]]); 63 | m_index[i].total_dists = &(m_total_dists[temporary_indices_vector[i]]); 64 | m_index[i].position_dists = &(m_position_dists[temporary_indices_vector[i]]); 65 | } 66 | } 67 | 68 | void PointNeighborhood::RemoveUnidirectionalLinks(Uint64Vector &temporary_indices_vector) 69 | { 70 | const uint64 cloud_size = temporary_indices_vector.size(); 71 | Uint64Vector temporary_indices_vector_in(cloud_size); 72 | NeighsVector index_in(cloud_size); 73 | Uint64Vector neighbors_in; 74 | FloatVector total_dists_in; 75 | FloatVector position_dists_in; 76 | temporary_indices_vector.swap(temporary_indices_vector_in); 77 | m_index.swap(index_in); 78 | m_neighbors.swap(neighbors_in); 79 | m_total_dists.swap(total_dists_in); 80 | m_position_dists.swap(position_dists_in); 81 | 82 | m_neighbors.reserve(neighbors_in.size()); 83 | m_total_dists.reserve(total_dists_in.size()); 84 | m_position_dists.reserve(position_dists_in.size()); 85 | 86 | uint64 counter = 0; 87 | for (uint64 i = 0; i < cloud_size; i++) 88 | { 89 | const uint64 size = index_in[i].size; 90 | const uint64 start = temporary_indices_vector_in[i]; 91 | 92 | uint64 inc = 0; 93 | 94 | for (uint64 h = 0; h < size; h++) 95 | { 96 | const uint64 i2 = neighbors_in[start + h]; 97 | const float td = total_dists_in[start + h]; 98 | const float pd = position_dists_in[start + h]; 99 | 100 | const uint64 size2 = index_in[i2].size; 101 | const uint64 start2 = temporary_indices_vector_in[i2]; 102 | 103 | for (uint64 k = 0; k < size2; k++) 104 | if (neighbors_in[start2 + k] == i) 105 | { 106 | m_neighbors.push_back(i2); 107 | m_total_dists.push_back(td); 108 | m_position_dists.push_back(pd); 109 | inc++; 110 | break; 111 | } 112 | } 113 | 114 | temporary_indices_vector[i] = counter; 115 | counter += inc; 116 | m_index[i].size = inc; 117 | } 118 | } 119 | 120 | void PointNeighborhood::AddUnidirectionalLinks(Uint64Vector &temporary_indices_vector) 121 | { 122 | const uint64 cloud_size = temporary_indices_vector.size(); 123 | Uint64Vector temporary_indices_vector_in(cloud_size); 124 | NeighsVector index_in(cloud_size); 125 | Uint64Vector neighbors_in; 126 | FloatVector total_dists_in; 127 | FloatVector position_dists_in; 128 | temporary_indices_vector.swap(temporary_indices_vector_in); 129 | m_index.swap(index_in); 130 | m_neighbors.swap(neighbors_in); 131 | m_total_dists.swap(total_dists_in); 132 | m_position_dists.swap(position_dists_in); 133 | 134 | Uint64VectorVector dv_neighbors(cloud_size); 135 | FloatVectorVector dv_total_dists(cloud_size); 136 | FloatVectorVector dv_position_dists(cloud_size); 137 | 138 | uint64 counter = 0; 139 | for (uint64 i = 0; i < cloud_size; i++) 140 | { 141 | const uint64 size = index_in[i].size; 142 | const uint64 start = temporary_indices_vector_in[i]; 143 | 144 | Uint64Vector &v_neighbors = dv_neighbors[i]; 145 | FloatVector &v_total_dists = dv_total_dists[i]; 146 | FloatVector &v_position_dists = dv_position_dists[i]; 147 | 148 | for (uint64 h = 0; h < size; h++) 149 | { 150 | const uint64 i2 = neighbors_in[start + h]; 151 | const float td = total_dists_in[start + h]; 152 | const float pd = position_dists_in[start + h]; 153 | 154 | if (std::find(v_neighbors.begin(), v_neighbors.end(), i2) == v_neighbors.end()) 155 | { 156 | v_neighbors.push_back(i2); 157 | v_total_dists.push_back(td); 158 | v_position_dists.push_back(pd); 159 | counter++; 160 | } 161 | 162 | Uint64Vector &v2_neighbors = dv_neighbors[i2]; 163 | FloatVector &v2_total_dists = dv_total_dists[i2]; 164 | FloatVector &v2_position_dists = dv_position_dists[i2]; 165 | 166 | if (std::find(v2_neighbors.begin(), v2_neighbors.end(), i) == v2_neighbors.end()) 167 | { 168 | v2_neighbors.push_back(i); 169 | v2_total_dists.push_back(td); 170 | v2_position_dists.push_back(pd); 171 | counter++; 172 | } 173 | } 174 | } 175 | 176 | m_neighbors.resize(counter); 177 | m_total_dists.resize(counter); 178 | m_position_dists.resize(counter); 179 | 180 | counter = 0; 181 | for (uint64 i = 0; i < cloud_size; i++) 182 | { 183 | const uint64 size = dv_neighbors[i].size(); 184 | m_index[i].size = size; 185 | const uint64 start = counter; 186 | temporary_indices_vector[i] = counter; 187 | for (uint64 h = 0; h < size; h++) 188 | { 189 | m_neighbors[start + h] = dv_neighbors[i][h]; 190 | m_total_dists[start + h] = dv_total_dists[i][h]; 191 | m_position_dists[start + h] = dv_position_dists[i][h]; 192 | } 193 | counter += size; 194 | } 195 | } 196 | 197 | float PointNeighborhood::TotalDistance(const PointXYZRGBNormal &a, const PointXYZRGBNormal &b, const Conf &conf) const 198 | { 199 | const Eigen::Vector3f epta(a.x, a.y, a.z); 200 | const Eigen::Vector3f eptb(b.x, b.y, b.z); 201 | const Eigen::Vector3f eca(a.r, a.g, a.b); 202 | const Eigen::Vector3f ecb(b.r, b.g, b.b); 203 | const Eigen::Vector3f ena(a.normal_x, a.normal_y, a.normal_z); 204 | const Eigen::Vector3f enb(b.normal_x, b.normal_y, b.normal_z); 205 | 206 | float spatial_dist = (epta - eptb).norm() / conf.max_distance; 207 | float color_dist = (eca - ecb).norm() / (255.0f * std::sqrt(3.0f)); 208 | float cos_angle_normal = 1.0f - ena.dot(enb); 209 | return spatial_dist * conf.position_importance + cos_angle_normal * conf.normal_importance + color_dist * conf.color_importance; 210 | } 211 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_neighborhood.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_NEIGHBORHOOD_H 2 | #define POINT_NEIGHBORHOOD_H 3 | 4 | // STL 5 | #include 6 | #include 7 | 8 | // PCL 9 | #include 10 | #include 11 | #include 12 | 13 | #include "point_neighborhood_search.h" 14 | 15 | class PointNeighborhood 16 | { 17 | public: 18 | typedef uint64_t uint64; 19 | typedef std::vector Uint64Vector; 20 | typedef std::vector Uint64VectorVector; 21 | typedef std::vector FloatVector; 22 | typedef std::vector FloatVectorVector; 23 | 24 | typedef pcl::PointXYZRGBNormal PointXYZRGBNormal; 25 | typedef pcl::PointCloud PointXYZRGBNormalCloud; 26 | typedef pcl::KdTreeFLANN KdTree; 27 | 28 | typedef boost::shared_ptr Ptr; 29 | typedef boost::shared_ptr ConstPtr; 30 | 31 | struct Conf 32 | { 33 | float color_importance; 34 | float position_importance; 35 | float normal_importance; 36 | 37 | float max_distance; 38 | 39 | PointNeighborhoodSearch::Searcher::ConstPtr searcher; 40 | }; 41 | 42 | struct Neighs 43 | { 44 | uint64 size; 45 | const uint64 * neighbors; 46 | const float * total_dists; 47 | const float * position_dists; 48 | }; 49 | 50 | typedef std::vector NeighsVector; 51 | 52 | PointNeighborhood(PointXYZRGBNormalCloud::ConstPtr cloudptr,const Conf & conf); 53 | virtual ~PointNeighborhood() {} 54 | 55 | float TotalDistance(const PointXYZRGBNormal & a,const PointXYZRGBNormal & b,const Conf & conf) const; 56 | 57 | uint64 GetNeighborhoodAsPointer(const uint64 i,const uint64 * & neighbors, 58 | const float * & total_dists,const float * & position_dists) const 59 | { 60 | neighbors = m_index[i].neighbors; 61 | total_dists = m_index[i].total_dists; 62 | position_dists = m_index[i].position_dists; 63 | return m_index[i].size; 64 | } 65 | 66 | // conf used at creation 67 | const Conf & GetConf() const {return m_conf; } 68 | 69 | private: 70 | void RemoveUnidirectionalLinks(Uint64Vector & temporary_indices_vector); 71 | void AddUnidirectionalLinks(Uint64Vector & temporary_indices_vector); 72 | 73 | NeighsVector m_index; 74 | Uint64Vector m_neighbors; 75 | FloatVector m_total_dists; 76 | FloatVector m_position_dists; 77 | 78 | Conf m_conf; 79 | }; 80 | 81 | #endif // POINT_NEIGHBORHOOD_H 82 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_neighborhood_search.cpp: -------------------------------------------------------------------------------- 1 | #include "point_neighborhood_search.h" 2 | 3 | #include 4 | 5 | #include "rviz_cloud_annotation.h" 6 | 7 | class PointNeighborhoodSearch::FixedDistanceSearcher: public PointNeighborhoodSearch::Searcher 8 | { 9 | public: 10 | explicit FixedDistanceSearcher(const float search_distance): m_search_distance(search_distance) {} 11 | 12 | void Search(const KdTree & kdtree, 13 | const PointXYZRGBNormal & center, 14 | IntVector & indices, 15 | FloatVector & distances) const 16 | { 17 | kdtree.radiusSearch(center,m_search_distance,indices,distances); 18 | } 19 | 20 | void Serialize(std::ostream & ofile) const 21 | { 22 | const uint64 id = PARAM_VALUE_NEIGH_SEARCH_FIXED_DISTANCE; 23 | ofile.write((const char *)&id,sizeof(id)); 24 | ofile.write((const char *)&m_search_distance,sizeof(m_search_distance)); 25 | } 26 | 27 | std::string ToString() const 28 | { 29 | return boost::lexical_cast(m_search_distance); 30 | } 31 | 32 | bool ApproxEquals(const Searcher & other) const 33 | { 34 | const FixedDistanceSearcher * const fds = dynamic_cast(&other); 35 | return fds && (std::abs(fds->m_search_distance - m_search_distance) < 1e-5); 36 | } 37 | 38 | uint64 GetId() const {return PARAM_VALUE_NEIGH_SEARCH_FIXED_DISTANCE; } 39 | 40 | bool IsPostProcessingRequired(const PostProcessingType) const {return false; } 41 | 42 | private: 43 | float m_search_distance; 44 | }; 45 | 46 | class PointNeighborhoodSearch::KNearestNeighborsSearcher: public PointNeighborhoodSearch::Searcher 47 | { 48 | public: 49 | explicit KNearestNeighborsSearcher(const uint32 knn): m_knn(knn) {} 50 | 51 | void Search(const KdTree & kdtree, 52 | const PointXYZRGBNormal & center, 53 | IntVector & indices, 54 | FloatVector & distances) const 55 | { 56 | indices.clear(); 57 | distances.clear(); 58 | indices.reserve(m_knn); 59 | distances.reserve(m_knn); 60 | 61 | if (m_knn == 0) 62 | return; // nothing to do 63 | 64 | const uint32 n = kdtree.nearestKSearch(center,m_knn,indices,distances); 65 | indices.resize(n); 66 | distances.resize(n); 67 | } 68 | 69 | void Serialize(std::ostream & ofile) const 70 | { 71 | const uint64 id = GetId(); 72 | ofile.write((const char *)&id,sizeof(id)); 73 | ofile.write((const char *)&m_knn,sizeof(m_knn)); 74 | } 75 | 76 | std::string ToString() const 77 | { 78 | return boost::lexical_cast(m_knn); 79 | } 80 | 81 | bool ApproxEquals(const Searcher & other) const 82 | { 83 | const KNearestNeighborsSearcher * const fds = dynamic_cast(&other); 84 | return fds && (fds->m_knn == m_knn) && (fds->GetId() == GetId()); 85 | } 86 | 87 | private: 88 | uint32 m_knn; 89 | }; 90 | 91 | class PointNeighborhoodSearch::KNearestNeighborsSearcherAtleast: public PointNeighborhoodSearch::KNearestNeighborsSearcher 92 | { 93 | public: 94 | explicit KNearestNeighborsSearcherAtleast(const uint32 knn): KNearestNeighborsSearcher(knn) {} 95 | uint64 GetId() const {return PARAM_VALUE_NEIGH_SEARCH_KNN_ATLEAST; } 96 | bool IsPostProcessingRequired(const PostProcessingType type) const {return type == PPTYPE_COMPLETE_UNIDIRECTIONAL_LINKS; } 97 | }; 98 | 99 | class PointNeighborhoodSearch::KNearestNeighborsSearcherAtmost: public PointNeighborhoodSearch::KNearestNeighborsSearcher 100 | { 101 | public: 102 | explicit KNearestNeighborsSearcherAtmost(const uint32 knn): KNearestNeighborsSearcher(knn) {} 103 | uint64 GetId() const {return PARAM_VALUE_NEIGH_SEARCH_KNN_ATMOST; } 104 | bool IsPostProcessingRequired(const PostProcessingType type) const {return type == PPTYPE_REMOVE_UNIDIRECTIONAL_LINKS; } 105 | }; 106 | 107 | PointNeighborhoodSearch::Searcher::ConstPtr PointNeighborhoodSearch::CreateFromString(const uint64 id,const std::string & param) 108 | { 109 | switch (id) 110 | { 111 | case PARAM_VALUE_NEIGH_SEARCH_FIXED_DISTANCE: 112 | { 113 | try 114 | { 115 | const float fixed_distance = boost::lexical_cast(param); 116 | return Searcher::Ptr(new FixedDistanceSearcher(fixed_distance)); 117 | } 118 | catch (const boost::bad_lexical_cast &) 119 | { 120 | throw ParserException(std::string("Parameter for fixed distance search must be a float, it is \"") + 121 | param + std::string("\" instead.")); 122 | } 123 | } 124 | case PARAM_VALUE_NEIGH_SEARCH_KNN_ATMOST: 125 | { 126 | try 127 | { 128 | const uint32 knn = boost::lexical_cast(param); 129 | return Searcher::Ptr(new KNearestNeighborsSearcherAtmost(knn)); 130 | } 131 | catch (const boost::bad_lexical_cast &) 132 | { 133 | throw ParserException(std::string("Parameter for knn search must be an integer, it is \"") + 134 | param + std::string("\" instead.")); 135 | } 136 | } 137 | case PARAM_VALUE_NEIGH_SEARCH_KNN_ATLEAST: 138 | { 139 | try 140 | { 141 | const uint32 knn = boost::lexical_cast(param); 142 | return Searcher::Ptr(new KNearestNeighborsSearcherAtleast(knn)); 143 | } 144 | catch (const boost::bad_lexical_cast &) 145 | { 146 | throw ParserException(std::string("Parameter for knn search must be an integer, it is \"") + 147 | param + std::string("\" instead.")); 148 | } 149 | } 150 | default: 151 | throw ParserException(std::string("Unknown search type: ") + 152 | boost::lexical_cast(id) + "."); 153 | } 154 | } 155 | 156 | PointNeighborhoodSearch::Searcher::ConstPtr PointNeighborhoodSearch::CreateFromIstream(std::istream & ifile) 157 | { 158 | uint64 id; 159 | ifile.read((char *)&id,sizeof(id)); 160 | if (!ifile) 161 | throw ParserException("Unexpected EOF while reading neighborhood searcher id."); 162 | 163 | switch (id) 164 | { 165 | case PARAM_VALUE_NEIGH_SEARCH_FIXED_DISTANCE: 166 | { 167 | float distance; 168 | ifile.read((char *)&distance,sizeof(distance)); 169 | if (!ifile) 170 | throw ParserException("Unexpected EOF while reading fixed distance search parameter."); 171 | 172 | return Searcher::Ptr(new FixedDistanceSearcher(distance)); 173 | } 174 | case PARAM_VALUE_NEIGH_SEARCH_KNN_ATMOST: 175 | { 176 | uint32 knn; 177 | ifile.read((char *)&knn,sizeof(knn)); 178 | if (!ifile) 179 | throw ParserException("Unexpected EOF while reading knn search parameter."); 180 | 181 | return Searcher::Ptr(new KNearestNeighborsSearcherAtmost(knn)); 182 | } 183 | case PARAM_VALUE_NEIGH_SEARCH_KNN_ATLEAST: 184 | { 185 | uint32 knn; 186 | ifile.read((char *)&knn,sizeof(knn)); 187 | if (!ifile) 188 | throw ParserException("Unexpected EOF while reading knn search parameter."); 189 | 190 | return Searcher::Ptr(new KNearestNeighborsSearcherAtleast(knn)); 191 | } 192 | default: 193 | throw ParserException(std::string("Unknown search type: ") + 194 | boost::lexical_cast(id) + std::string(".")); 195 | } 196 | } 197 | 198 | 199 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/point_neighborhood_search.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_NEIGHBORHOOD_SEARCH_H 2 | #define POINT_NEIGHBORHOOD_SEARCH_H 3 | 4 | // Boost 5 | #include 6 | #include 7 | 8 | // PCL 9 | #include 10 | #include 11 | #include 12 | 13 | // STL 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace PointNeighborhoodSearch 20 | { 21 | typedef uint64_t uint64; 22 | typedef uint32_t uint32; 23 | 24 | typedef pcl::PointXYZRGBNormal PointXYZRGBNormal; 25 | typedef pcl::PointCloud PointXYZRGBNormalCloud; 26 | typedef pcl::KdTreeFLANN KdTree; 27 | 28 | typedef std::vector IntVector; 29 | typedef std::vector FloatVector; 30 | 31 | enum PostProcessingType 32 | { 33 | // solve non-symmetric graph edges by removing them 34 | PPTYPE_REMOVE_UNIDIRECTIONAL_LINKS, 35 | // make non-symmetric graph edges symmetric by adding the opposite edge 36 | PPTYPE_COMPLETE_UNIDIRECTIONAL_LINKS 37 | }; 38 | 39 | class Searcher 40 | { 41 | public: 42 | typedef boost::shared_ptr Ptr; 43 | typedef boost::shared_ptr ConstPtr; 44 | virtual ~Searcher() {} 45 | 46 | virtual void Search(const KdTree & kdtree,const PointXYZRGBNormal & center, 47 | IntVector & indices,FloatVector & distances) const = 0; 48 | virtual void Serialize(std::ostream & ofile) const = 0; 49 | virtual std::string ToString() const = 0; 50 | virtual uint64 GetId() const = 0; 51 | 52 | virtual bool ApproxEquals(const Searcher & other) const = 0; 53 | 54 | virtual bool IsPostProcessingRequired(const PostProcessingType type) const = 0; 55 | }; 56 | 57 | struct ParserException 58 | { 59 | ParserException(const std::string m): message(m) {} 60 | std::string message; 61 | }; 62 | 63 | // from ROS parameter 64 | Searcher::ConstPtr CreateFromString(const uint64 id,const std::string & param); 65 | // from binary stream 66 | Searcher::ConstPtr CreateFromIstream(std::istream & ifile); 67 | 68 | class FixedDistanceSearcher; 69 | class KNearestNeighborsSearcher; 70 | class KNearestNeighborsSearcherAtleast; 71 | class KNearestNeighborsSearcherAtmost; 72 | } 73 | 74 | #endif // POINT_NEIGHBORHOOD_SEARCH_H 75 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 版权说明 3 | ---------------------------------------------------- 4 | 1. **软件版权** 5 | 本标注工具的版权归Duwenwen|Nullmax 所有 6 | 2. **其他版权** 7 | 本标注工具在 RIMLab 开源标注工具 rviz_cloud_annotation 上改进完成： 8 | `https://github.com/RMonica/rviz_cloud_annotation` 9 | ``` 10 | 原始版权说明： 11 | Original Copyright : 12 | / 13 | * Copyright (c) 2016-2017, Riccardo Monica 14 | * RIMLab, Department of Engineering and Architecture 15 | * University of Parma, Italy 16 | * http://www.rimlab.ce.unipr.it/ 17 | * 18 | * Redistribution and use in source and binary forms, with or without 19 | * modification, are permitted provided that the following conditions are met: 20 | * 21 | * 1. Redistributions of source code must retain the above copyright notice, 22 | * this list of conditions and the following disclaimer. 23 | * 24 | * 2. Redistributions in binary form must reproduce the above copyright notice, 25 | * this list of conditions and the following disclaimer in the documentation 26 | * and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its 29 | * contributors may be used to endorse or promote products derived from this 30 | * software without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 35 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 36 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 37 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 38 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 39 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 40 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 41 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 | * POSSIBILITY OF SUCH DAMAGE. 43 | */ 44 | 45 | #include "rviz_cloud_annotation_class.h" 46 | int main(int argc, char** argv) 47 | { 48 | ros::init(argc, argv, "rviz_cloud_annotation"); 49 | ros::NodeHandle nh("~"); 50 | 51 | RVizCloudAnnotation rvca(nh); 52 | 53 | ros::spin(); 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_CLOUD_ANNOTATION_H 2 | #define RVIZ_CLOUD_ANNOTATION_H 3 | 4 | #define PARAM_NAME_UPDATE_TOPIC "update_topic" 5 | #define PARAM_DEFAULT_UPDATE_TOPIC "/rviz_cloud_annotation/update_topic" 6 | 7 | #define PARAM_NAME_PCD_TYPE "pcd_type" 8 | #define PARAM_DEFAULT_PCD_TYPE "/rviz_cloud_annotation/pcd_type" 9 | 10 | #define PARAM_NAME_NORMAL_SOURCE "normal_source" 11 | #define PARAM_VALUE_NORMAL_SOURCE_CLOUD "cloud" // from PARAM_NAME_CLOUD_FILENAME itself 12 | #define PARAM_VALUE_NORMAL_SOURCE_OTHER_CLOUD "other_cloud:" // example: "other_cloud:normal_cloud.pcd" 13 | #define PARAM_DEFAULT_NORMAL_SOURCE PARAM_VALUE_NORMAL_SOURCE_CLOUD 14 | 15 | #define PARAM_NAME_AUTOSAVE_TIME "autosave_time" 16 | #define PARAM_DEFAULT_AUTOSAVE_TIME (0.0) // seconds, < 1.0 to disable 17 | 18 | #define PARAM_NAME_AUTOSAVE_TIMESTAMP "autosave_append_timestamp" 19 | #define PARAM_DEFAULT_AUTOSAVE_TIMESTAMP (false) 20 | 21 | #define PARAM_NAME_FRAME_ID "frame_id" 22 | #define PARAM_DEFAULT_FRAME_ID "base_link" 23 | 24 | #define PARAM_NAME_POINT_SIZE "point_size" 25 | #define PARAM_DEFAULT_POINT_SIZE (0.05) 26 | 27 | // size of the current label for each point 28 | #define PARAM_NAME_LABEL_SIZE "label_size" 29 | #define PARAM_DEFAULT_LABEL_SIZE (0.1) 30 | 31 | // size of the current label for control points 32 | #define PARAM_NAME_CONTROL_LABEL_SIZE "control_label_size" 33 | #define PARAM_DEFAULT_CONTROL_LABEL_SIZE (0.1) 34 | 35 | // shows labels in the back of the points as well 36 | #define PARAM_NAME_SHOW_POINTS_BACK_LABELS "show_labels_back" 37 | #define PARAM_DEFAULT_SHOW_POINTS_BACK_LABELS (true) 38 | 39 | #define PARAM_NAME_CONTROL_POINT_VISUAL "control_point_visual" 40 | #define PARAM_VALUE_CONTROL_POINT_VISUAL_LINE "line" 41 | #define PARAM_VALUE_CONTROL_POINT_VISUAL_SPHERE "sphere" 42 | #define PARAM_VALUE_CONTROL_POINT_VISUAL_THREE_SPHERES "three_spheres" 43 | #define PARAM_DEFAULT_CONTROL_POINT_VISUAL PARAM_VALUE_CONTROL_POINT_VISUAL_SPHERE 44 | 45 | // [0..1] 0: size not affected by weight - 1: whole size affected 46 | #define PARAM_NAME_CP_WEIGHT_SCALE_FRACTION "control_point_weight_scale_fraction" 47 | #define PARAM_DEFAULT_CP_WEIGHT_SCALE_FRACTION (0.5) 48 | 49 | #define PARAM_NAME_ZERO_WEIGHT_CP_SHOW "show_zero_weight_control_points" 50 | #define PARAM_DEFAULT_ZERO_WEIGHT_CP_SHOW (true) 51 | 52 | // from interface to backend 53 | #define PARAM_NAME_SET_EDIT_MODE_TOPIC "rviz_cloud_annotation/set_edit_mode_topic" 54 | #define PARAM_DEFAULT_SET_EDIT_MODE_TOPIC "/rviz_cloud_annotation/set_edit_mode" 55 | 56 | // from backend to interface 57 | #define PARAM_NAME_SET_EDIT_MODE_TOPIC2 "rviz_cloud_annotation/set_edit_mode_topic2" 58 | #define PARAM_DEFAULT_SET_EDIT_MODE_TOPIC2 "/rviz_cloud_annotation/set_edit_mode2" 59 | 60 | #define PARAM_NAME_COLORS_COLS_PER_PAGE "rviz_cloud_annotation/color_columns_per_page" 61 | #define PARAM_DEFAULT_COLOR_COLS_PER_PAGE (10) 62 | 63 | #define PARAM_NAME_COLORS_ROWS_PER_PAGE "rviz_cloud_annotation/color_rows_per_page" 64 | #define PARAM_DEFAULT_COLOR_ROWS_PER_PAGE (2) 65 | 66 | #define PARAM_NAME_POINT_SIZE_CHANGE_MULT "rviz_cloud_annotation/point_change_size_multiplier" 67 | #define PARAM_DEFAULT_POINT_SIZE_CHANGE_MULT (0.2) 68 | 69 | #define PARAM_NAME_SET_CURRENT_LABEL_TOPIC "rviz_cloud_annotation/set_current_label_topic" 70 | #define PARAM_DEFAULT_SET_CURRENT_LABEL_TOPIC "/rviz_cloud_annotation/set_current_label" 71 | 72 | #define PARAM_NAME_CURRENT_LABEL_TOPIC "rviz_cloud_annotation/set_current_label_topic2" 73 | #define PARAM_DEFAULT_CURRENT_LABEL_TOPIC "/rviz_cloud_annotation/set_current_label2" 74 | 75 | #define PARAM_NAME_SAVE_TOPIC "rviz_cloud_annotation/save_topic" 76 | #define PARAM_DEFAULT_SAVE_TOPIC "/rviz_cloud_annotation/save" 77 | 78 | #define PARAM_NAME_RESTORE_TOPIC "rviz_cloud_annotation/restore_topic" 79 | #define PARAM_DEFAULT_RESTORE_TOPIC "/rviz_cloud_annotation/restore" 80 | 81 | #define PARAM_NAME_CLEAR_TOPIC "rviz_cloud_annotation/clear_topic" 82 | #define PARAM_DEFAULT_CLEAR_TOPIC "/rviz_cloud_annotation/clear" 83 | 84 | #define PARAM_NAME_NEW_TOPIC "rviz_cloud_annotation/new_topic" 85 | #define PARAM_DEFAULT_NEW_TOPIC "/rviz_cloud_annotation/new" 86 | 87 | // from RViz to backend 88 | #define PARAM_NAME_SET_NAME_TOPIC "rviz_cloud_annotation/set_name_topic" 89 | #define PARAM_DEFAULT_SET_NAME_TOPIC "/rviz_cloud_annotation/set_name" 90 | 91 | // from backend to RViz 92 | #define PARAM_NAME_SET_NAME_TOPIC2 "rviz_cloud_annotation/set_name_topic2" 93 | #define PARAM_DEFAULT_SET_NAME_TOPIC2 "/rviz_cloud_annotation/set_name2" 94 | 95 | // from backend to RViz 96 | #define PARAM_NAME_POINT_COUNT_UPDATE_TOPIC "rviz_cloud_annotation/point_count_update" 97 | #define PARAM_DEFAULT_POINT_COUNT_UPDATE_TOPIC "/rviz_cloud_annotation/point_count_update" 98 | 99 | #define PARAM_NAME_VIEW_GROUND_TOPIC "rviz_cloud_annotation/view_ground_topic" 100 | #define PARAM_DEFAULT_VIEW_GROUND_TOPIC "/rviz_cloud_annotation/view_ground" 101 | 102 | 103 | #define PARAM_NAME_VIEW_LABEL_TOPIC "rviz_cloud_annotation/view_labels_topic" 104 | #define PARAM_DEFAULT_VIEW_LABEL_TOPIC "/rviz_cloud_annotation/view_labels" 105 | 106 | #define PARAM_NAME_VIEW_CONTROL_POINTS_TOPIC "rviz_cloud_annotation/view_control_points_topic" 107 | #define PARAM_DEFAULT_VIEW_CONTROL_POINTS_TOPIC "/rviz_cloud_annotation/view_control_points" 108 | 109 | #define PARAM_NAME_VIEW_CLOUD_TOPIC "rviz_cloud_annotation/view_cloud_topic" 110 | #define PARAM_DEFAULT_VIEW_CLOUD_TOPIC "/rviz_cloud_annotation/view_cloud" 111 | 112 | #define PARAM_NAME_UNDO_TOPIC "rviz_cloud_annotation/undo_topic" 113 | #define PARAM_DEFAULT_UNDO_TOPIC "/rviz_cloud_annotation/undo" 114 | 115 | #define PARAM_NAME_REDO_TOPIC "rviz_cloud_annotation/redo_topic" 116 | #define PARAM_DEFAULT_REDO_TOPIC "/rviz_cloud_annotation/redo" 117 | 118 | #define PARAM_NAME_NEXT_TOPIC "rviz_cloud_annotation/next_topic" 119 | #define PARAM_DEFAULT_NEXT_TOPIC "/rviz_cloud_annotation/next" 120 | 121 | #define PARAM_NAME_PRE_TOPIC "rviz_cloud_annotation/pre_topic" 122 | #define PARAM_DEFAULT_PRE_TOPIC "/rviz_cloud_annotation/pre" 123 | 124 | #define PARAM_NAME_UNDO_REDO_STATE_TOPIC "rviz_cloud_annotation/undo_redo_state_topic" 125 | #define PARAM_DEFAULT_UNDO_REDO_STATE_TOPIC "/rviz_cloud_annotation/undo_redo_state" 126 | 127 | #define PARAM_NAME_POINT_SIZE_CHANGE_TOPIC "rviz_cloud_annotation/point_size_change_topic" 128 | #define PARAM_DEFAULT_POINT_SIZE_CHANGE_TOPIC "/rviz_cloud_annotation/point_size_change" 129 | 130 | #define PARAM_NAME_CONTROL_POINT_WEIGHT_TOPIC "rviz_cloud_annotation/point_weight_topic" 131 | #define PARAM_DEFAULT_CONTROL_POINT_WEIGHT_TOPIC "/rviz_cloud_annotation/point_weight" 132 | 133 | #define PARAM_NAME_CONTROL_POINT_MAX_WEIGHT_TOPIC "rviz_cloud_annotation/point_max_weight_topic" 134 | #define PARAM_DEFAULT_CONTROL_POINT_MAX_WEIGHT_TOPIC "/rviz_cloud_annotation/point_max_weight" 135 | 136 | #define PARAM_NAME_YAW_TOPIC "rviz_cloud_annotation/yaw_topic" 137 | #define PARAM_DEFAULT_YAW_TOPIC "/rviz_cloud_annotation/yaw" 138 | 139 | #define PARAM_NAME_OCCLUDED_TOPIC "rviz_cloud_annotation/occluded_topic" 140 | #define PARAM_DEFAULT_OCCLUDED_TOPIC "/rviz_cloud_annotation/occluded" 141 | 142 | #define PARAM_NAME_YAW_MAX_TOPIC "rviz_cloud_annotation/yaw_max_topic" 143 | #define PARAM_DEFAULT_YAW_MAX_TOPIC "/rviz_cloud_annotation/yaw_max" 144 | 145 | #define PARAM_NAME_YAW_MIN_TOPIC "rviz_cloud_annotation/yaw_min_topic" 146 | #define PARAM_DEFAULT_YAW_MIN_TOPIC "/rviz_cloud_annotation/yaw_min" 147 | 148 | #define PARAM_NAME_BIAS_TOPIC "rviz_cloud_annotation/bias_topic" 149 | #define PARAM_DEFAULT_BIAS_TOPIC "/rviz_cloud_annotation/bias" 150 | 151 | #define PARAM_NAME_BIAS_ZERO_TOPIC "rviz_cloud_annotation/bias_zero_topic" 152 | #define PARAM_DEFAULT_BIAS_ZERO_TOPIC "/rviz_cloud_annotation/bias_zero" 153 | 154 | #define PARAM_NAME_GOTO_FIRST_UNUSED_TOPIC "rviz_cloud_annotation/goto_first_unused_topic" 155 | #define PARAM_DEFAULT_GOTO_FIRST_UNUSED_TOPIC "/rviz_cloud_annotation/goto_first_unused" 156 | 157 | #define PARAM_NAME_GOTO_LAST_UNUSED_TOPIC "rviz_cloud_annotation/goto_last_unused_topic" 158 | #define PARAM_DEFAULT_GOTO_LAST_UNUSED_TOPIC "/rviz_cloud_annotation/goto_last_unused" 159 | 160 | #define PARAM_NAME_GOTO_NEXT_UNUSED_TOPIC "rviz_cloud_annotation/goto_next_unused_topic" 161 | #define PARAM_DEFAULT_GOTO_NEXT_UNUSED_TOPIC "/rviz_cloud_annotation/goto_next_unused" 162 | 163 | #define PARAM_NAME_GOTO_FIRST_TOPIC "rviz_cloud_annotation/goto_first_topic" 164 | #define PARAM_DEFAULT_GOTO_FIRST_TOPIC "/rviz_cloud_annotation/goto_first" 165 | 166 | #define PARAM_NAME_RECT_SELECTION_TOPIC "rviz_cloud_annotation/rect_selection_topic" 167 | #define PARAM_DEFAULT_RECT_SELECTION_TOPIC "/rviz_cloud_annotation/rect_selection" 168 | 169 | #define PARAM_NAME_TOOL_TYPE_TOPIC "rviz_cloud_annotation/tool_type_topic" 170 | #define PARAM_DEFAULT_TOOL_TYPE_TOPIC "/rviz_cloud_annotation/tool_type" 171 | 172 | #define PARAM_NAME_ANNOTATION_TYPE_TOPIC "rviz_cloud_annotation/annotation_type_topic" 173 | #define PARAM_DEFAULT_ANNOTATION_TYPE_TOPIC "/rviz_cloud_annotation/annotation_type" 174 | 175 | #define PARAM_NAME_OBJECT_ID_TOPIC "rviz_cloud_annotation/object_id_topic" 176 | #define PARAM_DEFAULT_OBJECT_ID_TOPIC "/rviz_cloud_annotation/object_id" 177 | 178 | #define PARAM_NAME_TOGGLE_NONE_TOPIC "rviz_cloud_annotation/toggle_none_topic" 179 | #define PARAM_DEFAULT_TOGGLE_NONE_TOPIC "/rviz_cloud_annotation/toggle_none" 180 | 181 | #define PARAM_IMAGE_LABEL_TOPIC "rviz_cloud_annotation/image_label_topic" 182 | #define PARAM_DEFAULT_IMAGE_LABEL_TOPIC "/rviz_cloud_annotation/image_label" 183 | 184 | #define PARAM_AUTO_PLANE_TOPIC "rviz_cloud_annotation/auto_plane_topic" 185 | #define PARAM_DEFAULT_AUTO_PLANE_TOPIC "/rviz_cloud_annotation/auto_plane" 186 | 187 | #define PARAM_GROUND_TOPIC "rviz_cloud_annotation/ground_topic" 188 | #define PARAM_DEFAULT_GROUND_TOPIC "/rviz_cloud_annotation/ground" 189 | 190 | 191 | // parameters for smart labeling 192 | // neighborhood graph distance 193 | #define PARAM_NAME_NEIGH_SEARCH_DISTANCE "neighborhood_search_distance" // DEPRECATED 194 | #define PARAM_DEFAULT_NEIGH_SEARCH_DISTANCE (0.0) 195 | 196 | #define PARAM_NAME_NEIGH_SEARCH_TYPE "neigh_search_type" 197 | #define PARAM_DEFAULT_NEIGH_SEARCH_TYPE (0) 198 | #define PARAM_VALUE_NEIGH_SEARCH_FIXED_DISTANCE (0) // neigh_search_params is the distance (float) 199 | #define PARAM_VALUE_NEIGH_SEARCH_KNN_ATMOST (1) // neigh_search_params is the maximum number of neighbors (integer) 200 | #define PARAM_VALUE_NEIGH_SEARCH_KNN_ATLEAST (2) // neigh_search_params is the minimum number of neighbors (integer) 201 | 202 | // this is always a string, content depends on neigh_search_type 203 | #define PARAM_NAME_NEIGH_SEARCH_PARAMS "neigh_search_params" 204 | #define PARAM_DEFAULT_NEIGH_SEARCH_PARAMS "" 205 | 206 | // max label size per control point 207 | #define PARAM_NAME_MAX_DISTANCE "max_label_distance" 208 | #define PARAM_DEFAULT_MAX_DISTANCE (0.0) 209 | 210 | #define PARAM_NAME_COLOR_IMPORTANCE "color_importance" 211 | #define PARAM_DEFAULT_COLOR_IMPORTANCE (0.0) 212 | 213 | #define PARAM_NAME_POSITION_IMPORTANCE "position_importance" 214 | #define PARAM_DEFAULT_POSITION_IMPORTANCE (1.0) 215 | 216 | #define PARAM_NAME_NORMAL_IMPORTANCE "normal_importance" 217 | #define PARAM_DEFAULT_NORMAL_IMPORTANCE (0.0) 218 | // end parameters for smart labeling 219 | 220 | // this number of weight steps, plus the step 0 221 | #define PARAM_NAME_WEIGHT_STEPS "weight_steps" 222 | #define PARAM_DEFAULT_WEIGHT_STEPS (100) 223 | 224 | #define PARAM_NAME_YAW_MAX "yaw_max" 225 | #define PARAM_DEFAULT_YAW_MAX (180) 226 | 227 | #define PARAM_NAME_YAW_MIN "yaw_min" 228 | #define PARAM_DEFAULT_YAW_MIN (-180) 229 | 230 | #define EDIT_MODE_NONE (0) 231 | #define EDIT_MODE_CONTROL_POINT (1) 232 | #define EDIT_MODE_ERASER (2) 233 | #define EDIT_MODE_COLOR_PICKER (3) 234 | #define EDIT_MODE_MAX (4) 235 | 236 | #define ANNOTATION_TYPE_BBOX (0) 237 | #define ANNOTATION_TYPE_PLANE (1) 238 | #define ANNOTATION_TYPE_KERB (2) 239 | #define ANNOTATION_TYPE_LANE (3) 240 | 241 | #define TOOL_TYPE_SINGLE_PICK (0) 242 | #define TOOL_TYPE_DEEP_SQUARE (1) 243 | #define TOOL_TYPE_SHALLOW_SQUARE (2) 244 | #define TOOL_TYPE_SHALLOW_POLY (3) 245 | #define TOOL_TYPE_MAX (4) 246 | 247 | #define POINT_SIZE_CHANGE_BIGGER (1) 248 | #define POINT_SIZE_CHANGE_SMALLER (-1) 249 | #define POINT_SIZE_CHANGE_RESET (0) 250 | 251 | #define LOG_FILE "LogFile" 252 | #define LOG_FILE_DEFAULT "/LogFile/" 253 | 254 | #define DATASET_FOLDER "cloud_file_folder" 255 | #define IMAGE_FOLDER "image_file_folder" 256 | #define ANNOTATION_FILE_FOLDER "annotation_file_folder" 257 | #define ANNOTATION_CLOUD_FOLDER "annotation_cloud_folder" 258 | #define LABEL_NAME_FOLDER "label_names_file_folder" 259 | #define PATH "my_path" 260 | #define LINE_NAME_FOLDER "line_names_file_folder" 261 | #define BBOX_NAME_FOLDER "bbox_names_file_folder" 262 | 263 | #define DATASET_FOLDER_DEFAULT "/cloud_file_folder/" 264 | #define IMAGE_FOLDER_DEFAULT "/image_file_folder/" 265 | #define ANNOTATION_FILE_FOLDER_DEFAULT "/annotation_file_folder/" 266 | #define ANNOTATION_CLOUD_FOLDER_DEFAULT "/annotation_cloud_folder/" 267 | #define LABEL_NAME_FOLDER_DEFAULT "/label_names_file_folder/" 268 | #define PATH_DEFAULT "/my_path/" 269 | #define LINE_NAME_FOLDER_DEFAULT "/line_names_file_folder/" 270 | #define BBOX_NAME_FOLDER_DEFAULT "/bbox_names_file_folder/" 271 | 272 | #define SAVE_BBOX_NAME "Bbox" 273 | #define SAVE_BBOX_DEFAULT_NAME "Bbox.txt" 274 | 275 | #endif // RVIZ_CLOUD_ANNOTATION_H 276 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_params.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #define BBOXNUMBER_LINEPOINTNUMBER 100 11 | 12 | #define LINENUMBER 30 13 | 14 | #define DISTANCE_LIMMIT 30 15 | #define HEIGHT_LIMMIT 1.5 16 | 17 | #define _max(a, b) (((a) > (b)) ? (a) : (b)) 18 | #define _min(a, b) (((a) > (b)) ? (b) : (a)) 19 | 20 | #define _M_PI 3.141592653 21 | 22 | namespace po = boost::program_options; 23 | namespace fs = boost::filesystem; 24 | 25 | class PointCloudFilesTool 26 | { 27 | typedef std::vector StringVector; 28 | 29 | public: 30 | struct ScanFileComp 31 | { 32 | bool operator()(const fs::path &p1, const fs::path &p2) 33 | { 34 | return p1.stem().string() < p2.stem().string(); 35 | } 36 | }; 37 | 38 | //数据集按名称排序 39 | int getFilesList(const std::string &path_name, StringVector &m_pcd_files) 40 | { 41 | std::vector files_list; 42 | fs::path path(path_name); 43 | 44 | if (!fs::is_directory(path)) 45 | { 46 | return 0; 47 | } 48 | 49 | files_list.clear(); 50 | for (fs::directory_iterator file(path), f_end; file != f_end; ++file) 51 | { 52 | files_list.push_back(file->path()); 53 | } 54 | 55 | sort(files_list.begin(), files_list.end(), ScanFileComp()); 56 | 57 | for (std::vector::const_iterator iter = files_list.begin(); iter != files_list.end(); iter++) 58 | { 59 | m_pcd_files.push_back((iter->filename()).c_str()); 60 | } 61 | return 1; 62 | } 63 | }; 64 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_CLOUD_ANNOTATION_PLUGIN_H 2 | #define RVIZ_CLOUD_ANNOTATION_PLUGIN_H 3 | 4 | #include 5 | #include "rviz_cloud_annotation.h" 6 | // QT 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // STL 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | // ROS 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | // PCL 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | // OpenCv 54 | #include 55 | #include 56 | #include "cv_bridge/cv_bridge.h" 57 | #include "image_transport/image_transport.h" 58 | #include "sensor_msgs/image_encodings.h" 59 | 60 | #include 61 | 62 | class QLabel; 63 | 64 | class QPushButton; 65 | 66 | class QButtonGroup; 67 | 68 | class QLineEdit; 69 | 70 | class QAction; 71 | 72 | class QToolButton; 73 | 74 | class QSlider; 75 | 76 | class QMenu; 77 | 78 | namespace pcl { 79 | class RGB; 80 | } 81 | 82 | namespace rviz_cloud_annotation { 83 | class QRVizCloudAnnotation : public rviz::Panel { 84 | Q_OBJECT; 85 | 86 | typedef uint64_t uint64; 87 | typedef int64_t int64; 88 | typedef uint32_t uint32; 89 | typedef int32_t int32; 90 | typedef std::vector Uint64Vector; 91 | typedef std::vector FloatVector; 92 | typedef std::vector Int64Vector; 93 | typedef std::vector BoolVector; 94 | typedef std::vector PQPushButtonVector; 95 | 96 | public: 97 | QRVizCloudAnnotation(QWidget *parent = NULL); 98 | 99 | virtual ~QRVizCloudAnnotation(); 100 | 101 | private Q_SLOTS: 102 | 103 | void onSetEditMode(int mode); 104 | 105 | void onSetToolType(int type); 106 | 107 | void onSetAnnotationType(uint32 annotation_type); 108 | 109 | void onLabelButtonSelected(int id); 110 | 111 | void onPlusLabel(); 112 | 113 | void onMinusLabel(); 114 | 115 | void onPageUp(); 116 | 117 | void onPageDown(); 118 | 119 | void onAutoPlane(const bool checked); 120 | 121 | void onGround();// add by ou1ong 122 | 123 | void onSave(); 124 | 125 | void onRestore(); 126 | 127 | void onClear(); 128 | 129 | void onNew(); 130 | 131 | void onClearCurrent(); 132 | 133 | void onUndo(); 134 | 135 | void onRedo(); 136 | 137 | void onSendName(); 138 | 139 | void onViewCloudToggled(const bool checked); 140 | 141 | void onViewControlPointsToggled(const bool checked); 142 | 143 | void onViewLabelsToggled(const bool checked); 144 | 145 | void onViewGround(const bool checked); 146 | 147 | void onGotoFirstUnused(); 148 | 149 | void onGotoLastUnused(); 150 | 151 | void onGotoFirst(); 152 | 153 | void onGotoNextUnused(); 154 | 155 | void onSmallerPoints(); 156 | 157 | void onBiggerPoints(); 158 | 159 | void onResetPointsSize(); 160 | 161 | void onControlYawSliderMoved(int new_value); 162 | 163 | void onControlYawSliderSet(int new_value); 164 | 165 | void onSetControlMaxYaw(const std_msgs::Int32 &msg); 166 | 167 | void onSetControlMinYaw(const std_msgs::Int32 &msg); 168 | 169 | void onYawInc(); 170 | 171 | void onYawDec(); 172 | 173 | void onBiasX1(); 174 | 175 | void onBiasX2(); 176 | 177 | void onBiasX3(); 178 | 179 | void onBiasX4(); 180 | 181 | void onBiasY1(); 182 | 183 | void onBiasY2(); 184 | 185 | void onBiasY3(); 186 | 187 | void onBiasY4(); 188 | 189 | void onBiasZ1(); 190 | 191 | void onBiasZ2(); 192 | 193 | void onBiasZ3(); 194 | 195 | void onBiasZ4(); 196 | 197 | void onSetBiasZero(const std_msgs::Empty &msg); 198 | 199 | void onSetObjectId(const std_msgs::Int32 &msg); 200 | 201 | void onChangeObjectId(); 202 | 203 | void onControlPointWeightSliderMoved(int new_value); 204 | 205 | void onControlPointWeightSliderSet(int new_value); 206 | 207 | void onControlPointWeightInc(); 208 | 209 | void onControlPointWeightDec(); 210 | 211 | void onControlPointWeightMax(); 212 | 213 | void onControlPointWeightMin(); 214 | 215 | void onSetControlPointMaxWeight(const std_msgs::Int32 &msg); 216 | 217 | private: 218 | void SetCurrentEditMode(const uint64 mode); 219 | 220 | void FillColorPageButtons(); 221 | 222 | void FillPointCounts(uint type); 223 | 224 | void FillColorPageButtonStylesheet(); 225 | 226 | void SetCurrentLabel(const uint64 label, const uint64 page); 227 | 228 | void onSetCurrentLabel(const std_msgs::UInt32 &label); 229 | 230 | void onSetEditMode2(const std_msgs::UInt32 &mode); 231 | 232 | void onPointCountUpdate(const std_msgs::UInt64MultiArray &counters); 233 | 234 | void onUndoRedoState(const rviz_cloud_annotation::UndoRedoState &msg); 235 | 236 | void onSetName(const std_msgs::String &name); 237 | 238 | uint64 GetPageForLabel(const uint64 label) const; 239 | 240 | uint64 GetLabelFromPageAndId(const uint64 page, const int id) const; 241 | 242 | uint64 GetFirstLabelForPage(const uint64 page) const; 243 | 244 | uint64 GetLastLabelForPage(const uint64 page) const; 245 | 246 | void SetUndoText(const std::string &text); 247 | 248 | void SetRedoText(const std::string &text); 249 | 250 | void onNextObject(); 251 | 252 | void onPreObject(); 253 | 254 | void showImageLabel(std::string imagePath); 255 | 256 | void FirstUsedTime(time_t &begin) { 257 | const char *timefile_name = "time.txt"; 258 | std::ifstream infile; 259 | std::ofstream outfile; 260 | infile.open(timefile_name); 261 | outfile.open(timefile_name, std::ios::app); 262 | std::string timeS; 263 | if (infile) { 264 | std::getline(infile, timeS); 265 | begin = atoi(timeS.c_str()); 266 | } else { 267 | outfile << begin << std::endl; 268 | outfile.close(); 269 | } 270 | } 271 | 272 | static void ColorToHex(const pcl::RGB &color, char hex[7]); 273 | 274 | uint64 m_current_edit_mode; 275 | 276 | int32 bbox_id = 0; 277 | int32 lane_id = 0; 278 | int32 kerb_id = 0; 279 | 280 | // 0 for the eraser 281 | uint64 m_current_label; 282 | uint64 m_current_page; 283 | 284 | ros::NodeHandle m_nh; 285 | ros::Publisher m_set_edit_mode_pub; 286 | ros::Publisher m_set_current_label_pub; 287 | 288 | ros::Publisher m_save_pub; 289 | ros::Publisher m_restore_pub; 290 | ros::Publisher m_clear_pub; 291 | ros::Publisher m_new_pub; 292 | 293 | ros::Publisher m_goto_first_unused_pub; 294 | ros::Publisher m_goto_last_unused_pub; 295 | ros::Publisher m_goto_first_pub; 296 | ros::Publisher m_goto_next_unused_pub; 297 | 298 | ros::Subscriber m_set_edit_mode_sub; 299 | ros::Subscriber m_set_current_label_sub; 300 | 301 | ros::Publisher m_set_name_pub; 302 | ros::Subscriber m_set_name_sub; 303 | 304 | ros::Publisher m_view_cloud_pub; 305 | ros::Publisher m_view_labels_pub; 306 | ros::Publisher m_view_control_points_pub; 307 | ros::Publisher m_view_ground_pub; 308 | 309 | ros::Publisher m_next_pub; 310 | ros::Publisher m_pre_pub; 311 | 312 | ros::Publisher m_redo_pub; 313 | ros::Publisher m_undo_pub; 314 | ros::Subscriber m_undo_redo_state_sub; 315 | 316 | ros::Publisher m_point_size_change_pub; 317 | 318 | ros::Publisher m_control_points_weight_pub; 319 | ros::Subscriber m_control_point_max_weight_sub; 320 | 321 | ros::Publisher m_bias_pub; 322 | ros::Subscriber m_bias_zero_sub; 323 | 324 | ros::Publisher m_yaw_pub; 325 | ros::Subscriber m_yaw_max_sub; 326 | ros::Subscriber m_yaw_min_sub; 327 | 328 | ros::Publisher m_bbox_occluded_pub; 329 | 330 | ros::Publisher m_tool_type_pub; 331 | 332 | ros::Publisher m_annotation_type_pub; 333 | ros::Subscriber m_object_id_sub; 334 | 335 | ros::Publisher m_auto_plane_pub; 336 | 337 | 338 | image_transport::Publisher m_image_label_pub; 339 | 340 | QPushButton *m_edit_none_button; 341 | QPushButton *m_edit_control_point_button; 342 | QPushButton *m_edit_eraser_button; 343 | QPushButton *m_edit_color_picker_button; 344 | QButtonGroup *m_toolbar_group; 345 | 346 | QPushButton *m_kerb_button; 347 | QPushButton *m_lane_button; 348 | QPushButton *m_plane_button; 349 | QPushButton *m_bbox_button; 350 | QButtonGroup *m_annotation_type_group; 351 | 352 | QPushButton *m_tool_single_button; 353 | QPushButton *m_tool_shallow_square_button; 354 | QPushButton *m_tool_deep_square_button; 355 | QPushButton *m_tool_shallow_poly_button; 356 | QButtonGroup *m_tooltype_group; 357 | 358 | QPushButton *m_ground_button; 359 | 360 | QAction *m_prev_page_action; 361 | QAction *m_next_page_action; 362 | QAction *m_next_label_action; 363 | QAction *m_prev_label_action; 364 | 365 | QAction *m_prev_weight_action; 366 | QAction *m_next_weight_action; 367 | QAction *m_min_weight_action; 368 | QAction *m_max_weight_action; 369 | QMenu *m_weight_menu; 370 | 371 | QAction *m_auto_plane_action; 372 | 373 | QAction *m_X1_bias_action; 374 | QAction *m_X2_bias_action; 375 | QAction *m_X3_bias_action; 376 | QAction *m_X4_bias_action; 377 | QAction *m_Y1_bias_action; 378 | QAction *m_Y2_bias_action; 379 | QAction *m_Y3_bias_action; 380 | QAction *m_Y4_bias_action; 381 | QAction *m_Z1_bias_action; 382 | QAction *m_Z2_bias_action; 383 | QAction *m_Z3_bias_action; 384 | QAction *m_Z4_bias_action; 385 | QMenu *m_bias_menu; 386 | 387 | QAction *m_prev_yaw_action; 388 | QAction *m_next_yaw_action; 389 | QAction *m_max_yaw_action; 390 | QAction *m_min_yaw_action; 391 | QMenu *m_yaw_menu; 392 | 393 | QAction *m_undo_action; 394 | QAction *m_redo_action; 395 | 396 | QLabel *m_current_page_label; 397 | 398 | QLabel *m_object_id; 399 | 400 | QLabel *m_current_control_point_weight_label; 401 | QSlider *m_current_control_point_weight_slider; 402 | uint32 m_control_point_weight_max; 403 | 404 | QLabel *m_current_yaw_label; 405 | QSlider *m_current_yaw_slider; 406 | int32 m_yaw_max; 407 | int32 m_yaw_min; 408 | 409 | PQPushButtonVector m_page_buttons; 410 | QButtonGroup *m_page_button_group; 411 | 412 | Uint64Vector m_point_counters; 413 | ros::Subscriber m_point_count_update_sub; 414 | 415 | QLineEdit *m_set_name_edit; 416 | 417 | uint64 m_color_cols_per_page; 418 | uint64 m_color_rows_per_page; 419 | 420 | FloatVector m_bias; 421 | const float bias_step = 0.05; 422 | 423 | const uint BBOX = 0u; 424 | const uint PLANE = 1u; 425 | const uint KERB = 2u; 426 | const uint LANE = 3u; 427 | 428 | uint ANNOTATION_TYPE = BBOX; 429 | // add by ou1ong 430 | ros::Publisher m_ground_pub; 431 | QLineEdit *m_ground_params_edit[7]; 432 | QLineEdit *m_set_height_edit; 433 | QLineEdit *m_set_sensor_height_edit; 434 | QLineEdit *m_set_min_range_edit; 435 | QLineEdit *m_set_max_range_edit; 436 | QLineEdit *m_set_th_g_edit; 437 | QLineEdit *m_set_max_slope_edit; 438 | QLineEdit *m_set_num_scan_edit; 439 | QLineEdit *m_set_delta_ring_edit; 440 | }; 441 | 442 | } // namespace rviz_cloud_annotation 443 | 444 | #endif // RVIZ_CLOUD_ANNOTATION_PLUGIN_H 445 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_point_plane.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz_cloud_annotation_point_plane.h" 2 | 3 | #include 4 | 5 | RVizCloudAnnotationPointsPointPlane::RVizCloudAnnotationPointsPointPlane(const uint64 cloud_size, 6 | const PointNeighborhood & point_neighborhood,const float multiplier): m_point_neighborhood(point_neighborhood) 7 | { 8 | m_multiplier = multiplier; 9 | m_cloud_size = cloud_size; 10 | 11 | m_labels_assoc.resize(cloud_size,0); 12 | m_last_generated_tot_dists.resize(cloud_size,0.0); 13 | } 14 | 15 | void RVizCloudAnnotationPointsPointPlane::Clear() 16 | { 17 | m_labels_assoc.assign(m_cloud_size,0); 18 | m_last_generated_tot_dists.assign(m_cloud_size,0.0); 19 | } 20 | 21 | void RVizCloudAnnotationPointsPointPlane::RemoveLabel(const uint64 label_id, 22 | const uint64 point_id, 23 | BoolVector & touched_labels, 24 | Uint64Set & touched_points) 25 | { 26 | Uint64Set seeds_set; 27 | 28 | Uint64Queue queue; 29 | queue.push(point_id); 30 | 31 | m_labels_assoc[point_id] = 0; 32 | m_last_generated_tot_dists[point_id] = 0.0; 33 | touched_points.insert(point_id); 34 | 35 | while (!queue.empty()) 36 | { 37 | const uint64 curr_point_id = queue.front(); 38 | queue.pop(); 39 | 40 | const float * neigh_dists; 41 | const float * neigh_tot_dists; 42 | const uint64 * neighs; 43 | const uint64 neighs_size = 44 | m_point_neighborhood.GetNeighborhoodAsPointer(curr_point_id,neighs,neigh_tot_dists,neigh_dists); 45 | 46 | for (uint64 h = 0; h < neighs_size; h++) 47 | { 48 | const uint64 next_point_id = neighs[h]; 49 | 50 | const uint32 label = m_labels_assoc[next_point_id]; 51 | if (label == 0) 52 | continue; 53 | 54 | if (label != label_id) 55 | seeds_set.insert(next_point_id); 56 | if (label == label_id) 57 | { 58 | queue.push(next_point_id); 59 | 60 | m_labels_assoc[next_point_id] = 0; 61 | m_last_generated_tot_dists[next_point_id] = 0.0; 62 | touched_points.insert(next_point_id); 63 | } 64 | } 65 | } 66 | 67 | const Uint64Vector seeds(seeds_set.begin(),seeds_set.end()); 68 | 69 | UpdateRegionGrowing(seeds,touched_labels,touched_points); 70 | touched_labels[label_id - 1] = true; 71 | } 72 | 73 | void RVizCloudAnnotationPointsPointPlane::UpdateRegionGrowing(const Uint64Vector & seeds, 74 | BoolVector & touched_labels, 75 | Uint64Set & touched_points) 76 | { 77 | Uint64Queue queue; 78 | BoolVector in_queue(m_cloud_size,false); 79 | for (uint64 i = 0; i < seeds.size(); i++) 80 | { 81 | const uint64 first = seeds[i]; 82 | queue.push(first); 83 | in_queue[first] = true; 84 | touched_points.insert(first); 85 | } 86 | 87 | if (!std::isfinite(m_multiplier)) 88 | return; // do not expand if multiplier is infinite (i.e. weight is zero) 89 | 90 | while (!queue.empty()) 91 | { 92 | const uint64 current = queue.front(); 93 | queue.pop(); 94 | in_queue[current] = false; 95 | 96 | const float current_tot_dist = m_last_generated_tot_dists[current]; 97 | const uint64 current_label = m_labels_assoc[current]; 98 | 99 | const float * neigh_dists; 100 | const float * neigh_tot_dists; 101 | const uint64 * neighs; 102 | const uint64 neighs_size = m_point_neighborhood.GetNeighborhoodAsPointer(current,neighs,neigh_tot_dists,neigh_dists); 103 | 104 | for (uint64 i = 0; i < neighs_size; i++) 105 | { 106 | const uint64 next = neighs[i]; 107 | const float neigh_tot_dist = neigh_tot_dists[i] * m_multiplier; 108 | const float next_tot_dist = neigh_tot_dist + current_tot_dist; 109 | const uint64 next_label = m_labels_assoc[next]; 110 | 111 | if (next_tot_dist > 1.0) 112 | continue; 113 | 114 | if (next_label != 0 && m_last_generated_tot_dists[next] <= next_tot_dist) 115 | continue; 116 | 117 | if (next_label != 0) 118 | touched_labels[next_label - 1] = true; 119 | touched_labels[current_label - 1] = true; 120 | 121 | m_last_generated_tot_dists[next] = next_tot_dist; 122 | m_labels_assoc[next] = current_label; 123 | touched_points.insert(next); 124 | 125 | if (!in_queue[next]) 126 | { 127 | in_queue[next] = true; 128 | queue.push(next); 129 | } 130 | } 131 | } 132 | } 133 | 134 | void RVizCloudAnnotationPointsPointPlane::SetSeed(const uint64 point_id,const uint32 label_id) 135 | { 136 | m_labels_assoc[point_id] = label_id; 137 | m_last_generated_tot_dists[point_id] = 0.0; 138 | } 139 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_point_plane.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_CLOUD_ANNOTATION_POINT_PLANE_H 2 | #define RVIZ_CLOUD_ANNOTATION_POINT_PLANE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "point_neighborhood.h" 12 | 13 | class RVizCloudAnnotationPointsPointPlane 14 | { 15 | public: 16 | typedef uint64_t uint64; 17 | typedef uint32_t uint32; 18 | typedef std::vector Uint64Vector; 19 | typedef std::vector FloatVector; 20 | typedef std::vector BoolVector; 21 | typedef std::queue Uint64Queue; 22 | typedef std::set Uint64Set; 23 | typedef boost::shared_ptr Ptr; 24 | typedef boost::shared_ptr ConstPtr; 25 | 26 | RVizCloudAnnotationPointsPointPlane(const uint64 cloud_size, 27 | const PointNeighborhood & point_neighborhood, 28 | const float multiplier); 29 | 30 | void UpdateRegionGrowing(const Uint64Vector & seeds, 31 | BoolVector & touched_labels, 32 | Uint64Set & touched_points); 33 | void RemoveLabel(const uint64 label_id, 34 | const uint64 point_id, 35 | BoolVector & touched_labels, 36 | Uint64Set & touched_points); 37 | 38 | void Clear(); 39 | 40 | void SetSeed(const uint64 point_id,const uint32 label_id); 41 | 42 | uint32 GetLabel(const uint64 point_id) const {return m_labels_assoc[point_id]; } 43 | float GetTotDist(const uint64 point_id) const {return m_last_generated_tot_dists[point_id]; } 44 | 45 | private: 46 | Uint64Vector m_labels_assoc; 47 | FloatVector m_last_generated_tot_dists; 48 | 49 | uint64 m_cloud_size; 50 | 51 | const PointNeighborhood & m_point_neighborhood; 52 | 53 | float m_multiplier; 54 | }; 55 | 56 | #endif // RVIZ_CLOUD_ANNOTATION_POINT_PLANE_H 57 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_points.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz_cloud_annotation_points.h" 2 | #include "rviz_cloud_annotation.h" 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | RVizCloudAnnotationPoints::RVizCloudAnnotationPoints(const uint64 cloud_size, 10 | const uint32 weight_steps, 11 | const PointNeighborhood::ConstPtr neighborhood) 12 | { 13 | m_cloud_size = cloud_size; 14 | 15 | m_control_points_assoc.resize(m_cloud_size,0); 16 | m_labels_assoc.resize(m_cloud_size,0); 17 | m_last_generated_tot_dists.resize(m_cloud_size,0.0); 18 | 19 | m_weight_steps_count = weight_steps; 20 | m_weight_steps_planes.resize(m_weight_steps_count + 1); 21 | m_control_points_for_weight.resize(m_weight_steps_count + 1); 22 | 23 | m_point_neighborhood = neighborhood; 24 | } 25 | 26 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::Clear() 27 | { 28 | Uint64Vector result; 29 | result.resize(m_control_points_for_label.size()); 30 | for (uint64 i = 0; i < result.size(); i++) 31 | result[i] = i + 1; 32 | 33 | m_control_points.clear(); 34 | m_erased_control_points.clear(); 35 | m_ext_label_names.clear(); 36 | m_control_points_for_label.clear(); 37 | 38 | m_weight_steps_planes.assign(m_weight_steps_count + 1,PointPlane::Ptr()); 39 | m_control_points_for_weight.assign(m_weight_steps_count + 1,Uint64Vector()); 40 | 41 | m_control_points_assoc.assign(m_cloud_size,0); 42 | m_labels_assoc.assign(m_cloud_size,0); 43 | m_last_generated_tot_dists.assign(m_cloud_size,0.0); 44 | 45 | return result; 46 | } 47 | 48 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::SetControlPointVector(const Uint64Vector & ids, 49 | const Uint32Vector & weight_steps, 50 | const Uint64Vector & labels) 51 | { 52 | for (uint64 i = 0; i < labels.size(); i++) 53 | ExpandLabelsUntil(labels[i]); 54 | BoolVector touched(GetMaxLabel() + 1,false); 55 | 56 | // call all the SetControlPoint s and merge the results 57 | for (uint64 i = 0; i < ids.size(); i++) 58 | { 59 | const uint64 point_id = ids[i]; 60 | const uint64 label = labels[i]; 61 | const uint32 weight_step = weight_steps[i]; 62 | 63 | Uint64Vector touched_vec; 64 | touched_vec = SetControlPoint(point_id,weight_step,label); 65 | 66 | for (uint64 h = 0; h < touched_vec.size(); h++) 67 | touched[touched_vec[h]] = true; 68 | } 69 | 70 | Uint64Vector result; 71 | for (uint64 i = 0; i < touched.size(); i++) 72 | if (touched[i]) 73 | result.push_back(i); 74 | 75 | return result; 76 | } 77 | 78 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::SetControlPoint(const uint64 point_id, 79 | const uint32 weight_step, 80 | const uint64 label) 81 | { 82 | ExpandLabelsUntil(label); 83 | 84 | const uint64 prev_control_point_id = m_control_points_assoc[point_id]; 85 | 86 | // ADDING 87 | if (prev_control_point_id == 0) 88 | { 89 | if (label == 0) 90 | return Uint64Vector(); // nothing to do 91 | 92 | const uint64 new_id = InternalSetControlPoint(point_id,weight_step,label); 93 | BoolVector touched; 94 | UpdateLabelAssocAdded(new_id,weight_step,touched); 95 | 96 | return TouchedBoolVectorToExtLabel(touched); 97 | } 98 | 99 | // DELETING 100 | if (prev_control_point_id != 0 && label == 0) 101 | { 102 | BoolVector touched; 103 | UpdateLabelAssocDeleted(prev_control_point_id,touched); 104 | touched[prev_control_point_id - 1] = true; 105 | const Uint64Vector result = TouchedBoolVectorToExtLabel(touched); 106 | 107 | InternalSetControlPoint(point_id,0,0); 108 | 109 | return result; 110 | } 111 | 112 | const ControlPoint & control_point = m_control_points[prev_control_point_id - 1]; 113 | const uint64 prev_weight_step = control_point.weight_step_id; 114 | const uint64 prev_label = control_point.label_id; 115 | 116 | // CHANGE LABEL, SAME WEIGHT - INTERNAL ONLY 117 | if (prev_weight_step == weight_step && prev_label != label) 118 | { 119 | InternalSetControlPoint(point_id,weight_step,label); 120 | 121 | Uint64Vector result; 122 | result.push_back(prev_label); 123 | result.push_back(label); 124 | return result; 125 | } 126 | 127 | // CHANGE WEIGHT: delete and re-create 128 | if (prev_weight_step != weight_step) 129 | { 130 | BoolVector touched; 131 | UpdateLabelAssocDeleted(prev_control_point_id,touched); 132 | touched[prev_control_point_id - 1] = true; 133 | const Uint64Vector result1 = TouchedBoolVectorToExtLabel(touched); 134 | 135 | const uint64 new_id = InternalSetControlPoint(point_id,weight_step,label); 136 | 137 | UpdateLabelAssocAdded(new_id,weight_step,touched); 138 | const Uint64Vector result2 = TouchedBoolVectorToExtLabel(touched); 139 | 140 | Uint64Set result_set(result1.begin(),result1.end()); 141 | result_set.insert(result2.begin(),result2.end()); 142 | return Uint64Vector(result_set.begin(),result_set.end()); 143 | } 144 | 145 | return Uint64Vector(); 146 | } 147 | 148 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::SetControlPointList( 149 | const CPDataVector & control_points_data,const uint64 label) 150 | { 151 | CPDataVector cpdatas = control_points_data; 152 | for (uint64 i = 0; i < cpdatas.size(); i++) 153 | cpdatas[i].label_id = label; 154 | return SetControlPointList(cpdatas); 155 | } 156 | 157 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::SetControlPointList(const CPDataVector & control_points_data) 158 | { 159 | Uint64Set touched; 160 | 161 | const uint64 size = control_points_data.size(); 162 | for (uint64 i = 0; i < size; i++) 163 | { 164 | const CPData & data = control_points_data[i]; 165 | const Uint64Vector local_touched = SetControlPoint(data.point_id,data.weight_step_id,data.label_id); 166 | touched.insert(local_touched.begin(),local_touched.end()); 167 | } 168 | 169 | return Uint64Vector(touched.begin(),touched.end()); 170 | } 171 | 172 | RVizCloudAnnotationPoints::CPDataVector RVizCloudAnnotationPoints::GetControlPointList(const uint64 label) const 173 | { 174 | if (label > GetMaxLabel()) 175 | return CPDataVector(); 176 | 177 | const Uint64Vector & control_point_ids = m_control_points_for_label[label - 1]; 178 | CPDataVector result; 179 | result.reserve(control_point_ids.size()); 180 | for (uint64 i = 0; i < control_point_ids.size(); i++) 181 | result.push_back(m_control_points[control_point_ids[i] - 1].ToCPData()); 182 | 183 | return result; 184 | } 185 | 186 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::GetLabelPointList(const uint64 label) const 187 | { 188 | Uint64Vector result; 189 | 190 | if (label > GetMaxLabel()) 191 | return result; 192 | 193 | if (label == 0) 194 | { 195 | for (uint64 i = 0; i < m_cloud_size; i++) 196 | if (m_labels_assoc[i] == 0) 197 | result.push_back(i); 198 | return result; 199 | } 200 | 201 | for (uint64 i = 0; i < m_cloud_size; i++) 202 | if (m_labels_assoc[i] && m_control_points[m_labels_assoc[i] - 1].label_id == label) 203 | result.push_back(i); 204 | 205 | return result; 206 | } 207 | 208 | void RVizCloudAnnotationPoints::UpdateMainWeightPlane(const Uint64Set & touched_points, 209 | BoolVector & touched) 210 | { 211 | touched.resize(m_control_points.size(),false); 212 | 213 | for (Uint64Set::const_iterator iter = touched_points.begin(); iter != touched_points.end(); iter++) 214 | { 215 | const uint64 i = *iter; 216 | 217 | m_last_generated_tot_dists[i] = 0.0; 218 | m_labels_assoc[i] = 0; 219 | 220 | for (uint64 h = 0; h <= m_weight_steps_count; h++) 221 | { 222 | if (!m_weight_steps_planes[h]) 223 | continue; 224 | 225 | const PointPlane & plane = *(m_weight_steps_planes[h]); 226 | const uint32 label_id = plane.GetLabel(i); 227 | const float weight = plane.GetTotDist(i); 228 | const uint32 prev_label = m_labels_assoc[i]; 229 | const float prev_weight = m_last_generated_tot_dists[i]; 230 | if (label_id != 0 && (weight < prev_weight || prev_label == 0)) 231 | { 232 | if (m_labels_assoc[i]) 233 | touched[m_labels_assoc[i] - 1] = true; 234 | if (label_id) 235 | touched[label_id - 1] = true; 236 | m_last_generated_tot_dists[i] = weight; 237 | m_labels_assoc[i] = label_id; 238 | } 239 | } 240 | } 241 | } 242 | 243 | void RVizCloudAnnotationPoints::RebuildMainWeightPlane(BoolVector & touched) 244 | { 245 | m_labels_assoc.assign(m_cloud_size,0); 246 | m_last_generated_tot_dists.assign(m_cloud_size,0.0); 247 | touched.assign(m_control_points.size(),false); 248 | 249 | for (uint64 i = 0; i < m_cloud_size; i++) 250 | for (uint64 h = 0; h <= m_weight_steps_count; h++) 251 | { 252 | if (!m_weight_steps_planes[h]) 253 | continue; 254 | 255 | const PointPlane & plane = *(m_weight_steps_planes[h]); 256 | const uint32 label_id = plane.GetLabel(i); 257 | const float weight = plane.GetTotDist(i); 258 | const uint32 prev_label = m_labels_assoc[i]; 259 | const float prev_weight = m_last_generated_tot_dists[i]; 260 | if (label_id != 0 && (weight < prev_weight || prev_label == 0)) 261 | { 262 | if (m_labels_assoc[i]) 263 | touched[m_labels_assoc[i] - 1] = true; 264 | if (label_id) 265 | touched[label_id - 1] = true; 266 | m_last_generated_tot_dists[i] = weight; 267 | m_labels_assoc[i] = label_id; 268 | } 269 | } 270 | } 271 | 272 | void RVizCloudAnnotationPoints::RegenerateLabelAssoc(BoolVector & touched) 273 | { 274 | touched.assign(m_control_points.size(),false); 275 | 276 | Uint64Set touched_points; 277 | 278 | for (uint64 wi = 0; wi <= m_weight_steps_count; wi++) 279 | { 280 | if (m_control_points_for_weight[wi].empty() && !m_weight_steps_planes[wi]) 281 | continue; // do not create this plane 282 | 283 | ExpandPointPlane(wi); 284 | PointPlane & plane = *(m_weight_steps_planes[wi]); 285 | plane.Clear(); 286 | 287 | Uint64Vector seeds; 288 | const Uint64Vector & control_points = m_control_points_for_weight[wi]; 289 | for (uint64 i = 0; i < control_points.size(); i++) 290 | { 291 | const uint64 cp_i = control_points[i]; 292 | const ControlPoint & first = m_control_points[cp_i - 1]; 293 | seeds.push_back(first.point_id); 294 | plane.SetSeed(first.point_id,cp_i); 295 | } 296 | 297 | plane.UpdateRegionGrowing(seeds,touched,touched_points); 298 | } 299 | 300 | RebuildMainWeightPlane(touched); 301 | } 302 | 303 | void RVizCloudAnnotationPoints::UpdateLabelAssocAddedPlane(const uint64 added_index, 304 | const uint32 added_weight, 305 | BoolVector & touched, 306 | Uint64Set & touched_points) 307 | { 308 | touched.assign(m_control_points.size(),false); 309 | Uint64Vector seeds; 310 | 311 | touched[added_index - 1] = true; // we are adding it 312 | const uint64 point_id = m_control_points[added_index - 1].point_id; 313 | seeds.push_back(point_id); 314 | 315 | ExpandPointPlane(added_weight); 316 | 317 | PointPlane & plane = *(m_weight_steps_planes[added_weight]); 318 | plane.SetSeed(point_id,added_index); 319 | plane.UpdateRegionGrowing(seeds,touched,touched_points); 320 | } 321 | 322 | void RVizCloudAnnotationPoints::UpdateLabelAssocAdded(const uint64 added_index, 323 | const uint32 added_weight, 324 | BoolVector & touched) 325 | { 326 | Uint64Set touched_points; 327 | UpdateLabelAssocAddedPlane(added_index,added_weight,touched,touched_points); 328 | UpdateMainWeightPlane(touched_points,touched); 329 | } 330 | 331 | void RVizCloudAnnotationPoints::UpdateLabelAssocDeleted(const uint64 removed_index,BoolVector & touched_labels) 332 | { 333 | Uint64Vector removed_indices; 334 | removed_indices.push_back(removed_index); 335 | UpdateLabelAssocDeletedVector(removed_indices,touched_labels); 336 | } 337 | 338 | void RVizCloudAnnotationPoints::UpdateLabelAssocDeletedVector(const Uint64Vector & removed_indices,BoolVector & touched_labels) 339 | { 340 | touched_labels.assign(m_control_points.size(),false); 341 | Uint64Set touched_points; 342 | 343 | for (uint64 i = 0; i < removed_indices.size(); i++) 344 | { 345 | const uint64 removed_index = removed_indices[i]; 346 | touched_labels[removed_index - 1] = true; // we are removing it 347 | 348 | const uint32 removed_weight = m_control_points[removed_index - 1].weight_step_id; 349 | if (!m_weight_steps_planes[removed_weight]) 350 | return; 351 | const uint64 point_id = m_control_points[removed_index - 1].point_id; 352 | 353 | PointPlane & plane = *(m_weight_steps_planes[removed_weight]); 354 | 355 | plane.RemoveLabel(removed_index,point_id,touched_labels,touched_points); 356 | } 357 | 358 | UpdateMainWeightPlane(touched_points,touched_labels); 359 | } 360 | 361 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::ClearLabel(const uint64 label) 362 | { 363 | if (label == 0) 364 | return Uint64Vector(); 365 | if (label > GetMaxLabel()) 366 | return Uint64Vector(); 367 | 368 | const Uint64Vector control_points_ids = m_control_points_for_label[label - 1]; 369 | if (control_points_ids.empty()) 370 | return Uint64Vector(); 371 | 372 | BoolVector touched; 373 | UpdateLabelAssocDeletedVector(control_points_ids,touched); 374 | const Uint64Vector result = TouchedBoolVectorToExtLabel(touched); 375 | 376 | for (uint64 i = 0; i < control_points_ids.size(); i++) 377 | InternalSetControlPoint(m_control_points[control_points_ids[i] - 1].point_id,0,0); 378 | 379 | return result; 380 | } 381 | 382 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::SetNameForLabel(const uint64 label,const std::string & name) 383 | { 384 | ExpandLabelsUntil(label); 385 | m_ext_label_names[label - 1] = name; 386 | return Uint64Vector(1,label); 387 | } 388 | 389 | void RVizCloudAnnotationPoints::ExpandLabelsUntil(const uint64 label) 390 | { 391 | if (label <= GetMaxLabel()) 392 | return; 393 | 394 | m_ext_label_names.resize(label); 395 | m_control_points_for_label.resize(label); 396 | } 397 | 398 | void RVizCloudAnnotationPoints::ExpandPointPlane(const uint32 weight_id) 399 | { 400 | PointPlane::Ptr & plane_ptr = m_weight_steps_planes[weight_id]; 401 | if (!plane_ptr) 402 | { 403 | const float multiplier = (weight_id ? (float(m_weight_steps_count) / float(weight_id)) : NAN); 404 | plane_ptr = PointPlane::Ptr(new PointPlane(m_cloud_size,*m_point_neighborhood,multiplier)); 405 | } 406 | } 407 | 408 | RVizCloudAnnotationPoints::uint64 RVizCloudAnnotationPoints::InternalSetControlPoint(const uint64 point_id, 409 | const uint32 weight_step, 410 | const uint32 label) 411 | { 412 | ExpandLabelsUntil(label); 413 | 414 | const uint64 prev_control_point_id = m_control_points_assoc[point_id]; 415 | if (prev_control_point_id != 0) 416 | { 417 | const uint32 prev_label = m_control_points[prev_control_point_id - 1].label_id; 418 | const uint32 prev_weight = m_control_points[prev_control_point_id - 1].weight_step_id; 419 | 420 | if (label == 0) 421 | { 422 | // remove control point 423 | m_control_points_assoc[point_id] = 0; 424 | m_control_points[prev_control_point_id - 1].Invalidate(); 425 | m_erased_control_points.push_back(prev_control_point_id); 426 | EraseFromVector(m_control_points_for_label[prev_label - 1],prev_control_point_id); 427 | EraseFromVector(m_control_points_for_weight[prev_weight],prev_control_point_id); 428 | 429 | while (!m_control_points.empty() && m_control_points.back().Invalid()) 430 | { 431 | EraseFromVector(m_erased_control_points,uint64(m_control_points.size())); 432 | m_control_points.pop_back(); 433 | } 434 | 435 | return 0; 436 | } 437 | 438 | if (label == prev_label) 439 | { 440 | if (prev_weight != weight_step) 441 | { 442 | m_control_points[prev_control_point_id - 1].weight_step_id = weight_step; 443 | EraseFromVector(m_control_points_for_weight[prev_weight],prev_control_point_id); 444 | m_control_points_for_weight[weight_step].push_back(prev_control_point_id); 445 | } 446 | 447 | return prev_control_point_id; 448 | } 449 | 450 | // change label of control point 451 | EraseFromVector(m_control_points_for_label[prev_label - 1],prev_control_point_id); 452 | m_control_points[prev_control_point_id - 1].label_id = label; 453 | m_control_points_for_label[label - 1].push_back(prev_control_point_id); 454 | 455 | if (prev_weight != weight_step) 456 | { 457 | m_control_points[prev_control_point_id - 1].weight_step_id = weight_step; 458 | EraseFromVector(m_control_points_for_weight[prev_weight],prev_control_point_id); 459 | m_control_points_for_weight[weight_step].push_back(prev_control_point_id); 460 | } 461 | 462 | return prev_control_point_id; 463 | } 464 | 465 | // ok, create new control point 466 | uint64 new_id; 467 | if (m_erased_control_points.empty()) 468 | { 469 | m_control_points.push_back(ControlPoint(point_id,weight_step,label)); 470 | new_id = m_control_points.size(); 471 | } 472 | else // if an erased control point is present, reuse it 473 | { 474 | new_id = m_erased_control_points.back(); 475 | m_control_points[new_id - 1] = ControlPoint(point_id,weight_step,label); 476 | m_erased_control_points.pop_back(); 477 | } 478 | m_control_points_for_label[label - 1].push_back(new_id); 479 | m_control_points_for_weight[weight_step].push_back(new_id); 480 | m_control_points_assoc[point_id] = new_id; 481 | 482 | return new_id; 483 | } 484 | 485 | template 486 | void RVizCloudAnnotationPoints::EraseFromVector(std::vector & vector,const T value) 487 | { 488 | typename std::vector::iterator iter = std::find(vector.begin(),vector.end(),value); 489 | if (iter != vector.end()) 490 | vector.erase(iter); 491 | } 492 | 493 | RVizCloudAnnotationPoints::Uint64Vector RVizCloudAnnotationPoints::TouchedBoolVectorToExtLabel(const BoolVector & touched) const 494 | { 495 | BoolVector touched_labels(m_control_points_for_label.size(),false); 496 | Uint64Vector result; 497 | 498 | for (uint64 i = 0; i < touched.size(); i++) 499 | if (touched[i]) 500 | touched_labels[m_control_points[i].label_id - 1] = true; 501 | 502 | for (uint64 i = 0; i < touched_labels.size(); i++) 503 | if (touched_labels[i]) 504 | result.push_back(i + 1); 505 | 506 | return result; 507 | } 508 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_points.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_CLOUD_ANNOTATION_POINTS_H 2 | #define RVIZ_CLOUD_ANNOTATION_POINTS_H 3 | 4 | // STL 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // PCL 14 | #include 15 | #include 16 | #include 17 | 18 | // boost 19 | #include 20 | 21 | #include "point_neighborhood.h" 22 | #include "rviz_cloud_annotation_point_plane.h" 23 | 24 | class RVizCloudAnnotationPoints 25 | { 26 | public: 27 | typedef uint64_t uint64; 28 | typedef uint32_t uint32; 29 | typedef uint8_t uint8; 30 | typedef std::vector Uint64Vector; 31 | typedef std::vector Uint64VectorVector; 32 | typedef std::vector Uint32Vector; 33 | typedef std::vector Uint8Vector; 34 | typedef std::vector FloatVector; 35 | typedef std::queue Uint64Queue; 36 | typedef std::vector BoolVector; 37 | typedef std::vector StringVector; 38 | typedef std::set Uint64Set; 39 | struct CPData 40 | { 41 | uint64 point_id; 42 | uint32 weight_step_id; 43 | uint32 label_id; 44 | 45 | CPData(uint64 pi,uint32 wsi,uint32 li): point_id(pi), weight_step_id(wsi), label_id(li) {} 46 | CPData(): point_id(0), weight_step_id(0), label_id(0) {} 47 | }; 48 | typedef std::vector CPDataVector; 49 | typedef RVizCloudAnnotationPointsPointPlane PointPlane; 50 | typedef std::vector PointPlanePtrVector; 51 | 52 | typedef boost::shared_ptr ConstPtr; 53 | typedef boost::shared_ptr Ptr; 54 | 55 | explicit RVizCloudAnnotationPoints(const uint64 cloud_size, 56 | const uint32 weight_steps, 57 | const PointNeighborhood::ConstPtr neighborhood); 58 | 59 | struct IOE // IO exception 60 | { 61 | IOE(const std::string & d): description(d) {} 62 | std::string description; 63 | }; 64 | 65 | static RVizCloudAnnotationPoints::Ptr Deserialize(std::istream & ifile, 66 | const uint32 weight_steps, 67 | PointNeighborhood::ConstPtr neighborhood); 68 | void Serialize(std::ostream & ofile) const; 69 | 70 | // returns the list of affected labels 71 | Uint64Vector SetControlPoint(const uint64 point_id,const uint32 weight_step,const uint64 label); 72 | Uint64Vector SetControlPointVector(const Uint64Vector & ids, 73 | const Uint32Vector & weight_steps, 74 | const Uint64Vector & labels); 75 | Uint64Vector SetControlPoint(const CPData & control_point_data); 76 | Uint64Vector SetControlPointList(const CPDataVector & control_points_data,const uint64 label); 77 | Uint64Vector SetControlPointList(const CPDataVector & control_points_data); 78 | 79 | Uint64Vector Clear(); 80 | Uint64Vector ClearLabel(const uint64 label); // clear label, returns list of affected labels 81 | Uint64Vector SetNameForLabel(const uint64 label,const std::string & name); 82 | 83 | CPDataVector GetControlPointList(const uint64 label) const; 84 | Uint64Vector GetLabelPointList(const uint64 label) const; 85 | 86 | uint64 GetWeightStepsCount() const {return m_weight_steps_count; } 87 | 88 | // 0 if none 89 | uint64 GetLabelForPoint(const uint64 idx) const 90 | { 91 | if (!m_labels_assoc[idx]) 92 | return 0; 93 | return m_control_points[m_labels_assoc[idx] - 1].label_id; 94 | } 95 | 96 | CPData GetControlPointForPoint(const uint64 idx) const 97 | { 98 | if (!m_control_points_assoc[idx]) 99 | return CPData(); 100 | return m_control_points[m_control_points_assoc[idx] - 1].ToCPData(); 101 | } 102 | 103 | std::string GetNameForLabel(const uint64 label) const 104 | { 105 | if (label > GetMaxLabel()) 106 | return ""; 107 | return m_ext_label_names[label - 1]; 108 | } 109 | 110 | uint64 GetNextLabel() const {return m_control_points_for_label.size() + 1; } 111 | uint64 GetMaxLabel() const {return m_control_points_for_label.size(); } 112 | uint64 GetCloudSize() const {return m_cloud_size; } 113 | 114 | uint64 GetLabelPointCount(const uint64 label) const 115 | { 116 | if (label > GetMaxLabel()) 117 | return 0; 118 | return m_control_points_for_label[label - 1].size(); 119 | } 120 | 121 | template 122 | void LabelCloud(pcl::PointCloud & cloud) const; 123 | template 124 | void LabelCloudWithColor(pcl::PointCloud & cloud) const; 125 | 126 | private: 127 | struct ControlPoint 128 | { 129 | uint32 label_id; 130 | uint32 weight_step_id; 131 | uint64 point_id; 132 | 133 | ControlPoint(const uint64 point_id,const uint32 weight_step,const uint32 label_id): 134 | label_id(label_id), weight_step_id(weight_step), point_id(point_id) {} 135 | void Invalidate() {label_id = 0; } 136 | bool Valid() const {return label_id; } 137 | bool Invalid() const {return !label_id; } 138 | 139 | CPData ToCPData() const {return CPData(point_id,weight_step_id,label_id); } 140 | }; 141 | typedef std::vector ControlPointVector; 142 | 143 | void ExpandLabelsUntil(const uint64 label); 144 | void ExpandPointPlane(const uint32 weight_id); 145 | 146 | void RegenerateLabelAssoc(BoolVector & touched); 147 | void UpdateLabelAssocAdded(const uint64 added_index, 148 | const uint32 added_weight, 149 | BoolVector & touched); 150 | void UpdateLabelAssocAddedPlane(const uint64 added_index, 151 | const uint32 added_weight, 152 | BoolVector & touched, 153 | Uint64Set & touched_plane); 154 | void UpdateLabelAssocDeleted(const uint64 removed_index,BoolVector & touched_labels); 155 | void UpdateLabelAssocDeletedVector(const Uint64Vector & removed_indices,BoolVector & touched); 156 | 157 | void UpdateMainWeightPlane(const Uint64Set & touched_points,BoolVector & touched); 158 | void RebuildMainWeightPlane(BoolVector & touched); 159 | 160 | uint64 InternalSetControlPoint(const uint64 point_id, 161 | const uint32 weight_step, 162 | const uint32 label); 163 | template 164 | static void EraseFromVector(std::vector & vector,const T value); 165 | Uint64Vector TouchedBoolVectorToExtLabel(const BoolVector & touched) const; 166 | 167 | // assoc from cloud point to control point, 0 otherwise 168 | Uint64Vector m_control_points_assoc; 169 | // assoc from cloud point to region grown control point, 0 otherwise 170 | Uint64Vector m_labels_assoc; 171 | // control points 172 | ControlPointVector m_control_points; 173 | Uint64Vector m_erased_control_points; 174 | 175 | // from external label to list of control points with that label 176 | Uint64VectorVector m_control_points_for_label; 177 | Uint64VectorVector m_control_points_for_weight; 178 | 179 | uint64 m_cloud_size; 180 | 181 | FloatVector m_last_generated_tot_dists; 182 | 183 | StringVector m_ext_label_names; 184 | 185 | PointPlanePtrVector m_weight_steps_planes; 186 | uint32 m_weight_steps_count; 187 | 188 | PointNeighborhood::ConstPtr m_point_neighborhood; 189 | }; 190 | 191 | template 192 | void RVizCloudAnnotationPoints::LabelCloud(pcl::PointCloud & cloud) const 193 | { 194 | for (uint64 i = 0; i < m_cloud_size; i++) 195 | cloud[i].label = (m_labels_assoc[i] ? (m_control_points[m_labels_assoc[i] - 1].label_id) : 0); 196 | } 197 | 198 | template 199 | void RVizCloudAnnotationPoints::LabelCloudWithColor(pcl::PointCloud & cloud) const 200 | { 201 | LabelCloud(cloud); 202 | for (uint64 i = 0; i < m_cloud_size; i++) 203 | { 204 | const uint32 label = cloud[i].label; 205 | if (!label) 206 | { 207 | cloud[i].r = 0; 208 | cloud[i].g = 0; 209 | cloud[i].b = 0; 210 | continue; 211 | } 212 | const pcl::RGB color = pcl::GlasbeyLUT::at((label - 1) % 256); 213 | cloud[i].r = color.r; 214 | cloud[i].g = color.g; 215 | cloud[i].b = color.b; 216 | } 217 | } 218 | 219 | #endif // RVIZ_CLOUD_ANNOTATION_POINTS_H 220 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_points_io.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz_cloud_annotation_points.h" 2 | #include "rviz_cloud_annotation.h" 3 | 4 | #define MAGIC_STRING "ANNOTATION" 5 | #define MAGIC_MIN_VERSION (1) 6 | #define MAGIC_MAX_VERSION (4) 7 | 8 | RVizCloudAnnotationPoints::Ptr RVizCloudAnnotationPoints::Deserialize(std::istream & ifile, 9 | const uint32 weight_steps, 10 | PointNeighborhood::ConstPtr neighborhood) 11 | { 12 | if (!ifile) 13 | throw IOE("Invalid file stream."); 14 | 15 | const std::string magic_string = MAGIC_STRING; 16 | Uint8Vector maybe_magic_string(magic_string.size() + 1); 17 | ifile.read((char *)(maybe_magic_string.data()),magic_string.size() + 1); 18 | if (!ifile) 19 | throw IOE("Unexpected EOF while reading magic string."); 20 | if (std::memcmp(magic_string.c_str(),maybe_magic_string.data(),magic_string.size() + 1) != 0) 21 | throw IOE("Invalid magic string."); 22 | 23 | uint64 version; 24 | ifile.read((char *)&version,sizeof(version)); 25 | if (!ifile) 26 | throw IOE("Unexpected EOF while reading version."); 27 | if (version < MAGIC_MIN_VERSION || version > MAGIC_MAX_VERSION) 28 | throw IOE(std::string("Invalid version: ") + boost::lexical_cast(version)); 29 | 30 | uint64 cloud_size; 31 | ifile.read((char *)&cloud_size,sizeof(cloud_size)); 32 | if (!ifile) 33 | throw IOE("Unexpected EOF while reading cloud size."); 34 | 35 | { 36 | const PointNeighborhood::Conf & conf = neighborhood->GetConf(); 37 | float position_importance; 38 | ifile.read((char *)&position_importance,sizeof(position_importance)); 39 | float color_importance; 40 | ifile.read((char *)&color_importance,sizeof(color_importance)); 41 | float normal_importance; 42 | ifile.read((char *)&normal_importance,sizeof(normal_importance)); 43 | float max_distance; 44 | ifile.read((char *)&max_distance,sizeof(max_distance)); 45 | 46 | PointNeighborhoodSearch::Searcher::ConstPtr searcher; 47 | if (version <= 2) 48 | { 49 | float search_distance; 50 | ifile.read((char *)&search_distance,sizeof(search_distance)); 51 | searcher = PointNeighborhoodSearch::CreateFromString(0,boost::lexical_cast(search_distance)); 52 | } 53 | else 54 | { 55 | try 56 | { 57 | searcher = PointNeighborhoodSearch::CreateFromIstream(ifile); 58 | } 59 | catch (const PointNeighborhoodSearch::ParserException & ex) 60 | { 61 | throw IOE(std::string("Invalid neighborhood configuration parameters: ") + ex.message); 62 | } 63 | } 64 | 65 | if (!ifile) 66 | throw IOE("Unexpected EOF while reading neighborhood configuration parameters."); 67 | 68 | if (position_importance != conf.position_importance || 69 | color_importance != conf.color_importance || 70 | normal_importance != conf.normal_importance || 71 | max_distance != conf.max_distance || 72 | !searcher->ApproxEquals(*conf.searcher)) 73 | { 74 | const uint64 w = 30; 75 | std::ostringstream msg; 76 | msg << "Loaded neighborhood configuration parameters do not match: \n" 77 | << std::setw(w) << "Name" << std::setw(w) << "ROS param" << std::setw(w) << "File" << "\n" 78 | << std::setw(w) << PARAM_NAME_POSITION_IMPORTANCE 79 | << std::setw(w) << conf.position_importance << std::setw(w) << position_importance << "\n" 80 | << std::setw(w) << PARAM_NAME_COLOR_IMPORTANCE 81 | << std::setw(w) << conf.color_importance << std::setw(w) << color_importance << "\n" 82 | << std::setw(w) << PARAM_NAME_NORMAL_IMPORTANCE 83 | << std::setw(w) << conf.normal_importance << std::setw(w) << normal_importance << "\n" 84 | << std::setw(w) << PARAM_NAME_MAX_DISTANCE 85 | << std::setw(w) << conf.max_distance << std::setw(w) << max_distance << "\n" 86 | << std::setw(w) << PARAM_NAME_NEIGH_SEARCH_TYPE 87 | << std::setw(w) << conf.searcher->GetId() << std::setw(w) << searcher->GetId() << "\n" 88 | << std::setw(w) << PARAM_NAME_NEIGH_SEARCH_PARAMS 89 | << std::setw(w) << conf.searcher->ToString() << std::setw(w) << searcher->ToString() << "\n" 90 | ; 91 | throw IOE(msg.str()); 92 | } 93 | 94 | } 95 | 96 | { 97 | uint32 file_weight_steps; 98 | if (version < 4) 99 | file_weight_steps = weight_steps; 100 | else 101 | ifile.read((char *)&file_weight_steps,sizeof(file_weight_steps)); 102 | 103 | if (weight_steps != file_weight_steps) 104 | { 105 | const uint64 w = 30; 106 | std::ostringstream msg; 107 | msg << "Weight configuration does not match: \n" 108 | << std::setw(w) << "Name" << std::setw(w) << "ROS param" << std::setw(w) << "File" << "\n" 109 | << std::setw(w) << PARAM_NAME_WEIGHT_STEPS 110 | << std::setw(w) << weight_steps << std::setw(w) << file_weight_steps << "\n"; 111 | throw IOE(msg.str()); 112 | } 113 | } 114 | 115 | RVizCloudAnnotationPoints::Ptr resultptr(new RVizCloudAnnotationPoints(cloud_size,weight_steps,neighborhood)); 116 | RVizCloudAnnotationPoints & result = *resultptr; 117 | 118 | uint64 control_points_size; 119 | ifile.read((char *)&control_points_size,sizeof(control_points_size)); 120 | if (!ifile) 121 | throw IOE("Unexpected EOF while reading control point size."); 122 | result.ExpandLabelsUntil(control_points_size); 123 | 124 | for (uint64 i = 0; i < control_points_size; i++) 125 | { 126 | uint64 control_point_size; 127 | ifile.read((char *)&control_point_size,sizeof(control_point_size)); 128 | if (!ifile) 129 | throw IOE("Unexpected EOF while reading size of control point " + boost::lexical_cast(i) + "."); 130 | 131 | for (uint64 h = 0; h < control_point_size; h++) 132 | { 133 | uint64 point_index; 134 | ifile.read((char *)&point_index,sizeof(point_index)); 135 | if (!ifile) 136 | throw IOE("Unexpected EOF while reading point index of control point " + boost::lexical_cast(i) + "."); 137 | 138 | uint32 weight_step; 139 | if (version >= 4) 140 | ifile.read((char *)&weight_step,sizeof(weight_step)); 141 | else 142 | weight_step = weight_steps; 143 | if (!ifile) 144 | throw IOE("Unexpected EOF while reading weight of control point " + boost::lexical_cast(i) + "."); 145 | if (weight_step > weight_steps) 146 | throw IOE("Weight step " + boost::lexical_cast(weight_step) + " is out of range (max is " + 147 | boost::lexical_cast(weight_steps) + " ) " + "while reading control point " + 148 | boost::lexical_cast(i) + "."); 149 | 150 | result.InternalSetControlPoint(point_index,weight_step,i + 1); 151 | } 152 | } 153 | 154 | if (version >= 2) 155 | { 156 | for (uint64 i = 0; i < control_points_size; i++) 157 | { 158 | uint32 string_size; 159 | ifile.read((char *)&string_size,sizeof(string_size)); 160 | if (!ifile) 161 | throw IOE("Unexpected EOF while reading text label size " + boost::lexical_cast(i) + "."); 162 | Uint8Vector data(string_size + 1,0); // this is 0-terminated for sure 163 | ifile.read((char *)(data.data()),string_size); 164 | if (!ifile) 165 | throw IOE("Unexpected EOF while reading text label content " + boost::lexical_cast(i) + "."); 166 | std::string str((const char *)(data.data())); 167 | result.m_ext_label_names[i] = str; 168 | } 169 | } 170 | 171 | BoolVector touched; 172 | result.RegenerateLabelAssoc(touched); 173 | 174 | return resultptr; 175 | } 176 | 177 | void RVizCloudAnnotationPoints::Serialize(std::ostream & ofile) const 178 | { 179 | if (!ofile) 180 | throw IOE("Invalid file stream."); 181 | 182 | const std::string magic_string = MAGIC_STRING; 183 | ofile.write(magic_string.c_str(),magic_string.size() + 1); 184 | const uint64 version = MAGIC_MAX_VERSION; 185 | ofile.write((char *)&version,sizeof(version)); 186 | const uint64 cloud_size = m_cloud_size; 187 | ofile.write((char *)&cloud_size,sizeof(cloud_size)); 188 | 189 | { 190 | const PointNeighborhood::Conf & conf = m_point_neighborhood->GetConf(); 191 | const float position_importance = conf.position_importance; 192 | ofile.write((char *)&position_importance,sizeof(position_importance)); 193 | const float color_importance = conf.color_importance; 194 | ofile.write((char *)&color_importance,sizeof(color_importance)); 195 | const float normal_importance = conf.normal_importance; 196 | ofile.write((char *)&normal_importance,sizeof(normal_importance)); 197 | const float max_distance = conf.max_distance; 198 | ofile.write((char *)&max_distance,sizeof(max_distance)); 199 | conf.searcher->Serialize(ofile); 200 | 201 | const uint32 weight_steps = GetWeightStepsCount(); 202 | ofile.write((char *)&weight_steps,sizeof(weight_steps)); 203 | } 204 | 205 | const uint64 control_points_size = m_control_points_for_label.size(); 206 | ofile.write((char *)&control_points_size,sizeof(control_points_size)); 207 | 208 | for (uint64 i = 0; i < control_points_size; i++) 209 | { 210 | const uint64 control_point_size = m_control_points_for_label[i].size(); 211 | ofile.write((char *)&control_point_size,sizeof(control_point_size)); 212 | for (uint64 h = 0; h < control_point_size; h++) 213 | { 214 | const uint64 control_point_index = m_control_points_for_label[i][h]; 215 | const ControlPoint & cp = m_control_points[control_point_index - 1]; 216 | const uint64 point_index = cp.point_id; 217 | ofile.write((char *)&point_index,sizeof(point_index)); 218 | const uint32 weight_step = cp.weight_step_id; 219 | ofile.write((char *)&weight_step,sizeof(weight_step)); 220 | } 221 | } 222 | 223 | for (uint64 i = 0; i < control_points_size; i++) 224 | { 225 | uint32 string_size = m_ext_label_names[i].size(); 226 | ofile.write((char *)&string_size,sizeof(string_size)); 227 | ofile.write(m_ext_label_names[i].c_str(),string_size); 228 | } 229 | 230 | if (!ofile) 231 | throw IOE("Write error."); 232 | } 233 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_undo.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz_cloud_annotation_undo.h" 2 | 3 | #include 4 | 5 | typedef RVizCloudAnnotationUndo::SetControlPointAction SetControlPointAction; 6 | typedef RVizCloudAnnotationUndo::Action Action; 7 | typedef RVizCloudAnnotationUndo::SetNameForLabelAction SetNameForLabelAction; 8 | typedef RVizCloudAnnotationUndo::ClearLabelAction ClearLabelAction; 9 | typedef RVizCloudAnnotationUndo::RestoreLabelAction RestoreLabelAction; 10 | typedef RVizCloudAnnotationUndo::ClearAction ClearAction; 11 | typedef RVizCloudAnnotationUndo::RestoreAction RestoreAction; 12 | typedef RVizCloudAnnotationUndo::SetControlPointVectorAction SetControlPointVectorAction; 13 | 14 | #define QUEUE_SIZE_SANITY (10000) 15 | 16 | RVizCloudAnnotationUndo::RVizCloudAnnotationUndo() 17 | { 18 | m_undone_count = 0; 19 | } 20 | 21 | void RVizCloudAnnotationUndo::SetAnnotation(RVizCloudAnnotationPoints::Ptr annotation) 22 | { 23 | m_annotation = annotation; 24 | Reset(); 25 | } 26 | 27 | void RVizCloudAnnotationUndo::Reset() 28 | { 29 | m_actions.clear(); 30 | m_undone_count = 0; 31 | } 32 | 33 | RVizCloudAnnotationUndo::Uint64Vector RVizCloudAnnotationUndo::SetControlPoint(const uint64 idx, 34 | const uint32 weight_step, 35 | const uint32 next_label) 36 | { 37 | const ControlPointData prev_cp = m_annotation->GetControlPointForPoint(idx); 38 | if ((prev_cp.label_id == next_label) && (prev_cp.weight_step_id == weight_step)) 39 | return Uint64Vector(); // nothing to do 40 | 41 | const Action::Ptr action(new SetControlPointAction(idx,prev_cp.label_id,prev_cp.weight_step_id,next_label,weight_step)); 42 | PushAction(action); 43 | return action->Execute(*m_annotation); 44 | } 45 | 46 | RVizCloudAnnotationUndo::Uint64Vector RVizCloudAnnotationUndo::SetControlPointVector(const Uint64Vector ids, 47 | const uint32 weight_step, 48 | const uint32 next_label) 49 | { 50 | const Uint32Vector weight_steps(ids.size(),weight_step); 51 | const Uint64Vector next_labels(ids.size(),next_label); 52 | 53 | Uint32Vector prev_weight_steps(ids.size(),0); 54 | Uint64Vector prev_labels(ids.size(),0); 55 | 56 | for (uint64 i = 0; i < ids.size(); i++) 57 | { 58 | const ControlPointData prev_cp = m_annotation->GetControlPointForPoint(ids[i]); 59 | prev_weight_steps[i] = prev_cp.weight_step_id; 60 | prev_labels[i] = prev_cp.label_id; 61 | } 62 | 63 | const Action::Ptr action(new SetControlPointVectorAction(ids,prev_labels,prev_weight_steps,next_labels,weight_steps)); 64 | PushAction(action); 65 | return action->Execute(*m_annotation); 66 | } 67 | 68 | void RVizCloudAnnotationUndo::PushAction(Action::Ptr action) 69 | { 70 | for ( ; m_undone_count > 0; m_undone_count--) 71 | m_actions.pop_front(); // bring m_undone_count to 0 by popping all actions 72 | 73 | m_actions.push_front(action); 74 | while (m_actions.size() > QUEUE_SIZE_SANITY) 75 | m_actions.pop_back(); 76 | } 77 | 78 | RVizCloudAnnotationUndo::Uint64Vector RVizCloudAnnotationUndo::Undo() 79 | { 80 | if (m_actions.size() == 0) 81 | return Uint64Vector(); // nothing to undo 82 | 83 | if (m_actions.size() == m_undone_count) 84 | return Uint64Vector(); // everything is undone 85 | 86 | const Action::Ptr to_be_undone = m_actions[m_undone_count]; 87 | m_undone_count++; 88 | return to_be_undone->Inverse()->Execute(*m_annotation); 89 | } 90 | 91 | RVizCloudAnnotationUndo::Uint64Vector RVizCloudAnnotationUndo::Redo() 92 | { 93 | if (m_undone_count == 0) 94 | return Uint64Vector(); // everything is done 95 | 96 | m_undone_count--; 97 | const Action::Ptr to_be_done = m_actions[m_undone_count]; 98 | return to_be_done->Execute(*m_annotation); 99 | } 100 | 101 | bool RVizCloudAnnotationUndo::IsUndoEnabled() const 102 | { 103 | return m_undone_count < m_actions.size(); 104 | } 105 | 106 | bool RVizCloudAnnotationUndo::IsRedoEnabled() const 107 | { 108 | return m_undone_count > 0; 109 | } 110 | 111 | std::string RVizCloudAnnotationUndo::GetUndoDescription() const 112 | { 113 | if (!IsUndoEnabled()) 114 | return ""; 115 | return m_actions[m_undone_count]->GetDescription(); 116 | } 117 | 118 | std::string RVizCloudAnnotationUndo::GetRedoDescription() const 119 | { 120 | if (!IsRedoEnabled()) 121 | return ""; 122 | return m_actions[m_undone_count - 1]->GetDescription(); 123 | } 124 | 125 | RVizCloudAnnotationUndo::Uint64Vector RVizCloudAnnotationUndo::Clear() 126 | { 127 | ControlPointDataVector prev_control_points; 128 | StringVector names; 129 | const uint64 max_label = m_annotation->GetMaxLabel(); 130 | names.resize(max_label); 131 | for (uint64 i = 0; i < max_label; i++) 132 | { 133 | names[i] = m_annotation->GetNameForLabel(i + 1); 134 | 135 | const ControlPointDataVector cp = m_annotation->GetControlPointList(i + 1); 136 | prev_control_points.insert(prev_control_points.end(),cp.begin(),cp.end()); 137 | } 138 | 139 | const Action::Ptr action(new ClearAction(prev_control_points,names)); 140 | PushAction(action); 141 | return m_annotation->Clear(); 142 | } 143 | 144 | RVizCloudAnnotationUndo::Uint64Vector RVizCloudAnnotationUndo::ClearLabel(const uint32 label) 145 | { 146 | const ControlPointDataVector prev_control_points = m_annotation->GetControlPointList(label); 147 | if (prev_control_points.empty()) 148 | return Uint64Vector(); 149 | const Action::Ptr action(new ClearLabelAction(label,prev_control_points)); 150 | PushAction(action); 151 | return m_annotation->ClearLabel(label); 152 | } 153 | 154 | RVizCloudAnnotationUndo::Uint64Vector RVizCloudAnnotationUndo::SetNameForLabel(const uint32 label,const std::string & new_name) 155 | { 156 | const std::string prev_name = m_annotation->GetNameForLabel(label); 157 | if (prev_name == new_name) 158 | return Uint64Vector(); // nothing to do 159 | 160 | const Action::Ptr action(new SetNameForLabelAction(label,prev_name,new_name)); 161 | PushAction(action); 162 | return action->Execute(*m_annotation); 163 | } 164 | 165 | // ----- 166 | 167 | RVizCloudAnnotationUndo::Uint64Vector SetControlPointAction::Execute(RVizCloudAnnotationPoints & annotation) const 168 | { 169 | return annotation.SetControlPoint(m_idx,m_next_weight_step,m_next_label); 170 | } 171 | 172 | Action::Ptr SetControlPointAction::Inverse() const 173 | { 174 | return Action::Ptr(new SetControlPointAction(m_idx,m_next_label,m_next_weight_step, 175 | m_prev_label,m_prev_weight_step)); 176 | } 177 | 178 | std::string SetControlPointAction::GetDescription() const 179 | { 180 | std::ostringstream oss; 181 | if (m_next_label && !m_prev_label) 182 | oss << "Set " << m_next_label; 183 | else if (m_prev_label && !m_next_label) 184 | oss << "Del " << m_prev_label; 185 | else if (m_prev_label == m_next_label) 186 | oss << "Weight " << m_prev_label << " (" << m_prev_weight_step << " -> " << m_next_weight_step << ")"; 187 | else 188 | oss << "Set " << m_prev_label << " -> " << m_next_label; 189 | return oss.str(); 190 | } 191 | 192 | SetControlPointAction::SetControlPointAction(const uint64 idx,const uint32 prev_label,const uint32 prev_weight_step, 193 | const uint32 next_label, const uint32 next_weight_step) 194 | { 195 | m_idx = idx; 196 | m_prev_label = prev_label; 197 | m_next_label = next_label; 198 | m_prev_weight_step = prev_weight_step; 199 | m_next_weight_step = next_weight_step; 200 | } 201 | 202 | // ----- 203 | 204 | RVizCloudAnnotationUndo::Uint64Vector SetControlPointVectorAction::Execute(RVizCloudAnnotationPoints & annotation) const 205 | { 206 | return annotation.SetControlPointVector(m_idxs,m_next_weight_steps,m_next_labels); 207 | } 208 | 209 | Action::Ptr SetControlPointVectorAction::Inverse() const 210 | { 211 | return Action::Ptr(new SetControlPointVectorAction(m_idxs,m_next_labels,m_next_weight_steps, 212 | m_prev_labels,m_prev_weight_steps)); 213 | } 214 | 215 | std::string SetControlPointVectorAction::GetDescription() const 216 | { 217 | std::ostringstream oss; 218 | oss << "Change pts"; 219 | for (uint64 i = 0; i < m_idxs.size() && i < 3; i++) 220 | oss << " " << m_idxs[i]; 221 | if (m_idxs.size() > 3) 222 | oss << "..."; 223 | return oss.str(); 224 | } 225 | 226 | SetControlPointVectorAction::SetControlPointVectorAction(const Uint64Vector & idxs, 227 | const Uint64Vector & prev_labels,const Uint32Vector & prev_weight_steps, 228 | const Uint64Vector & next_labels,const Uint32Vector & next_weight_steps) 229 | { 230 | m_idxs = idxs; 231 | m_prev_labels = prev_labels; 232 | m_next_labels = next_labels; 233 | m_prev_weight_steps = prev_weight_steps; 234 | m_next_weight_steps = next_weight_steps; 235 | } 236 | 237 | // ----- 238 | 239 | SetNameForLabelAction::SetNameForLabelAction(const uint32 label,const std::string & prev_name,const std::string & new_name) 240 | { 241 | m_label = label; 242 | m_prev_name = prev_name; 243 | m_new_name = new_name; 244 | } 245 | 246 | RVizCloudAnnotationUndo::Uint64Vector SetNameForLabelAction::Execute(RVizCloudAnnotationPoints & annotation) const 247 | { 248 | annotation.SetNameForLabel(m_label,m_new_name); 249 | Uint64Vector result; 250 | result.push_back(m_label); 251 | return result; 252 | } 253 | 254 | Action::Ptr SetNameForLabelAction::Inverse() const 255 | { 256 | return Action::Ptr(new SetNameForLabelAction(m_label,m_new_name,m_prev_name)); 257 | } 258 | 259 | std::string SetNameForLabelAction::GetDescription() const 260 | { 261 | std::ostringstream oss; 262 | oss << "Name " << m_label; 263 | return oss.str(); 264 | } 265 | 266 | // ----- 267 | 268 | RVizCloudAnnotationUndo::Uint64Vector ClearLabelAction::Execute(RVizCloudAnnotationPoints & annotation) const 269 | { 270 | return annotation.ClearLabel(m_label); 271 | } 272 | 273 | Action::Ptr ClearLabelAction::Inverse() const 274 | { 275 | return Action::Ptr(new RestoreLabelAction(m_label,m_prev_control_points)); 276 | } 277 | 278 | std::string ClearLabelAction::GetDescription() const 279 | { 280 | std::ostringstream oss; 281 | oss << "Clear " << m_label; 282 | return oss.str(); 283 | } 284 | 285 | ClearLabelAction::ClearLabelAction(const uint32 label,const ControlPointDataVector & prev_control_points) 286 | { 287 | m_label = label; 288 | m_prev_control_points = prev_control_points; 289 | } 290 | 291 | RVizCloudAnnotationUndo::Uint64Vector RestoreLabelAction::Execute(RVizCloudAnnotationPoints & annotation) const 292 | { 293 | return annotation.SetControlPointList(m_control_points,m_label); 294 | } 295 | 296 | Action::Ptr RestoreLabelAction::Inverse() const 297 | { 298 | return Action::Ptr(new ClearLabelAction(m_label,m_control_points)); 299 | } 300 | 301 | std::string RestoreLabelAction::GetDescription() const 302 | { 303 | std::ostringstream oss; 304 | oss << "Restore " << m_label; 305 | return oss.str(); 306 | } 307 | 308 | RestoreLabelAction::RestoreLabelAction(const uint32 label,const ControlPointDataVector & control_points) 309 | { 310 | m_label = label; 311 | m_control_points = control_points; 312 | } 313 | 314 | // ----- 315 | 316 | RVizCloudAnnotationUndo::Uint64Vector ClearAction::Execute(RVizCloudAnnotationPoints & annotation) const 317 | { 318 | return annotation.Clear(); 319 | } 320 | 321 | Action::Ptr ClearAction::Inverse() const 322 | { 323 | return Action::Ptr(new RestoreAction(m_prev_control_points,m_names)); 324 | } 325 | 326 | std::string ClearAction::GetDescription() const 327 | { 328 | return "Clear all"; 329 | } 330 | 331 | ClearAction::ClearAction(const ControlPointDataVector & control_points,const StringVector & names) 332 | { 333 | m_prev_control_points = control_points; 334 | m_names = names; 335 | } 336 | 337 | RVizCloudAnnotationUndo::Uint64Vector RestoreAction::Execute(RVizCloudAnnotationPoints & annotation) const 338 | { 339 | const uint64 size = m_names.size(); 340 | Uint64Vector all(size); 341 | for (uint64 i = 0; i < m_names.size(); i++) 342 | { 343 | annotation.SetNameForLabel(i + 1,m_names[i]); 344 | all[i] = i + 1; 345 | } 346 | 347 | annotation.SetControlPointList(m_control_points); 348 | 349 | return all; 350 | } 351 | 352 | Action::Ptr RestoreAction::Inverse() const 353 | { 354 | return Action::Ptr(new ClearAction(m_control_points,m_names)); 355 | } 356 | 357 | std::string RestoreAction::GetDescription() const 358 | { 359 | return "Restore all"; 360 | } 361 | 362 | RestoreAction::RestoreAction(const ControlPointDataVector & control_points,const StringVector & names) 363 | { 364 | m_control_points = control_points; 365 | m_names = names; 366 | } 367 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_cloud_annotation_undo.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_CLOUD_ANNOTATION_UNDO_H 2 | #define RVIZ_CLOUD_ANNOTATION_UNDO_H 3 | 4 | #include "rviz_cloud_annotation_points.h" 5 | 6 | // Boost 7 | #include 8 | #include 9 | 10 | // STL 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | class RVizCloudAnnotationUndo 17 | { 18 | public: 19 | typedef uint64_t uint64; 20 | typedef uint32_t uint32; 21 | typedef std::vector Uint64Vector; 22 | typedef std::vector Uint32Vector; 23 | typedef std::vector StringVector; 24 | typedef RVizCloudAnnotationPoints::CPData ControlPointData; 25 | typedef RVizCloudAnnotationPoints::CPDataVector ControlPointDataVector; 26 | 27 | RVizCloudAnnotationUndo(); 28 | 29 | // interface 30 | class Action 31 | { 32 | public: 33 | typedef boost::shared_ptr Ptr; 34 | typedef boost::shared_ptr ConstPtr; 35 | 36 | virtual ~Action() {} 37 | 38 | // returns a list of affected markers 39 | virtual Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const = 0; 40 | virtual Ptr Inverse() const = 0; // get the inverse action from the current one 41 | virtual std::string GetDescription() const = 0; 42 | }; 43 | 44 | typedef std::deque ActionPtrDeque; 45 | 46 | class SetControlPointAction: public Action 47 | { 48 | public: 49 | Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const; 50 | Action::Ptr Inverse() const; 51 | std::string GetDescription() const; 52 | 53 | SetControlPointAction(const uint64 idx,const uint32 prev_label,const uint32 prev_weight_step, 54 | const uint32 next_label,const uint32 next_weight_step); 55 | 56 | private: 57 | uint64 m_idx; 58 | uint32 m_prev_label; 59 | uint32 m_next_label; 60 | uint32 m_prev_weight_step; 61 | uint32 m_next_weight_step; 62 | }; 63 | 64 | class SetControlPointVectorAction: public Action 65 | { 66 | public: 67 | Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const; 68 | Action::Ptr Inverse() const; 69 | std::string GetDescription() const; 70 | 71 | SetControlPointVectorAction(const Uint64Vector & idxs, 72 | const Uint64Vector & prev_labels,const Uint32Vector & prev_weight_steps, 73 | const Uint64Vector & next_labels,const Uint32Vector & next_weight_steps); 74 | 75 | private: 76 | Uint64Vector m_idxs; 77 | Uint64Vector m_prev_labels; 78 | Uint32Vector m_prev_weight_steps; 79 | Uint64Vector m_next_labels; 80 | Uint32Vector m_next_weight_steps; 81 | }; 82 | 83 | class SetNameForLabelAction: public Action 84 | { 85 | public: 86 | Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const; 87 | Action::Ptr Inverse() const; 88 | std::string GetDescription() const; 89 | 90 | SetNameForLabelAction(const uint32 label,const std::string & prev_name,const std::string & new_name); 91 | 92 | private: 93 | std::string m_new_name; 94 | std::string m_prev_name; 95 | uint32 m_label; 96 | }; 97 | 98 | class ClearLabelAction: public Action 99 | { 100 | public: 101 | Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const; 102 | Action::Ptr Inverse() const; 103 | std::string GetDescription() const; 104 | 105 | ClearLabelAction(const uint32 label,const ControlPointDataVector & prev_control_points); 106 | 107 | private: 108 | ControlPointDataVector m_prev_control_points; 109 | uint32 m_label; 110 | }; 111 | 112 | class RestoreLabelAction: public Action 113 | { 114 | public: 115 | Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const; 116 | Action::Ptr Inverse() const; 117 | std::string GetDescription() const; 118 | 119 | RestoreLabelAction(const uint32 label,const ControlPointDataVector & control_points); 120 | 121 | private: 122 | ControlPointDataVector m_control_points; 123 | uint32 m_label; 124 | }; 125 | 126 | class ClearAction: public Action 127 | { 128 | public: 129 | Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const; 130 | Action::Ptr Inverse() const; 131 | std::string GetDescription() const; 132 | 133 | ClearAction(const ControlPointDataVector & control_points,const StringVector & names); 134 | 135 | private: 136 | ControlPointDataVector m_prev_control_points; 137 | StringVector m_names; 138 | }; 139 | 140 | class RestoreAction: public Action 141 | { 142 | public: 143 | Uint64Vector Execute(RVizCloudAnnotationPoints & annotation) const; 144 | Action::Ptr Inverse() const; 145 | std::string GetDescription() const; 146 | 147 | RestoreAction(const ControlPointDataVector & control_points,const StringVector & names); 148 | 149 | private: 150 | ControlPointDataVector m_control_points; 151 | StringVector m_names; 152 | }; 153 | 154 | Uint64Vector SetControlPoint(const uint64 idx,const uint32 weight_step,const uint32 next_label); 155 | Uint64Vector SetControlPointVector(const Uint64Vector ids,const uint32 weight_step,const uint32 next_label); 156 | Uint64Vector Clear(); 157 | Uint64Vector ClearLabel(const uint32 label); 158 | Uint64Vector SetNameForLabel(const uint32 label,const std::string & name); 159 | 160 | Uint64Vector Undo(); 161 | Uint64Vector Redo(); 162 | 163 | bool IsUndoEnabled() const; 164 | bool IsRedoEnabled() const; 165 | std::string GetUndoDescription() const; 166 | std::string GetRedoDescription() const; 167 | 168 | RVizCloudAnnotationPoints::Ptr GetAnnotation() const {return m_annotation; } 169 | 170 | void SetAnnotation(RVizCloudAnnotationPoints::Ptr annotation); 171 | void Reset(); 172 | 173 | private: 174 | void PushAction(Action::Ptr action); 175 | 176 | RVizCloudAnnotationPoints::Ptr m_annotation; 177 | 178 | ActionPtrDeque m_actions; 179 | uint64 m_undone_count; 180 | }; 181 | 182 | #endif // RVIZ_CLOUD_ANNOTATION_UNDO_H 183 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_select_tool.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz_select_tool.h" 2 | #include "rviz_cloud_annotation.h" 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | 20 | namespace rviz_cloud_annotation 21 | { 22 | AnnotationSelectionTool::AnnotationSelectionTool(): m_nh("~") 23 | { 24 | access_all_keys_ = true; 25 | 26 | std::string param_string; 27 | 28 | m_nh.param(PARAM_NAME_RECT_SELECTION_TOPIC,param_string,PARAM_DEFAULT_RECT_SELECTION_TOPIC); 29 | m_annotation_selection_pub = m_nh.advertise(param_string,1); 30 | 31 | m_nh.param(PARAM_NAME_SET_EDIT_MODE_TOPIC2,param_string,PARAM_DEFAULT_SET_EDIT_MODE_TOPIC2); 32 | m_on_set_mode_sub = m_nh.subscribe(param_string,1,&AnnotationSelectionTool::onSetEditMode,this); 33 | 34 | m_nh.param(PARAM_NAME_TOOL_TYPE_TOPIC,param_string,PARAM_DEFAULT_TOOL_TYPE_TOPIC); 35 | m_on_set_tool_type_sub = m_nh.subscribe(param_string,1,&AnnotationSelectionTool::onSetToolType,this); 36 | 37 | m_nh.param(PARAM_NAME_TOGGLE_NONE_TOPIC,param_string,PARAM_DEFAULT_TOGGLE_NONE_TOPIC); 38 | m_toggle_none_pub = m_nh.advertise(param_string,1); 39 | 40 | m_move_tool = new rviz::MoveTool; 41 | m_interaction_tool = new rviz::InteractionTool; 42 | 43 | m_edit_mode = EDIT_MODE_NONE; 44 | m_edit_mode_selectable = false; 45 | m_tool_type = TOOL_TYPE_SINGLE_PICK; 46 | 47 | m_active = false; 48 | m_selecting = false; 49 | m_polyline_selecting = false; 50 | m_sel_start_x = 0; 51 | m_sel_start_y = 0; 52 | 53 | m_selection_rectangle = NULL; 54 | m_selection_rectangle_node = NULL; 55 | m_selection_viewport = NULL; 56 | } 57 | 58 | AnnotationSelectionTool::~AnnotationSelectionTool() 59 | { 60 | delete m_move_tool; 61 | delete m_interaction_tool; 62 | } 63 | 64 | void AnnotationSelectionTool::onInitialize() 65 | { 66 | m_move_tool->initialize(context_); 67 | m_interaction_tool->initialize(context_); 68 | 69 | InitRectangleSelection(); 70 | InitPolylineSelection(); 71 | } 72 | 73 | void AnnotationSelectionTool::InitRectangleSelection() 74 | { 75 | std::stringstream ss; 76 | ss << "RVizCloudAnnotationSelectionTool" << std::hex << intptr_t(this); 77 | const std::string base_name = ss.str(); 78 | 79 | m_selection_rectangle_node = scene_manager_->getRootSceneNode()->createChildSceneNode(); 80 | 81 | m_selection_rectangle = new Ogre::Rectangle2D(true); 82 | 83 | const static uint32_t texture_data[1] = { 0xffff0080 }; 84 | Ogre::DataStreamPtr pixel_stream; 85 | pixel_stream.bind(new Ogre::MemoryDataStream( (void*)&texture_data[0], 4 )); 86 | 87 | Ogre::TexturePtr tex = Ogre::TextureManager::getSingleton().loadRawData(base_name + "Texture", 88 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 89 | pixel_stream, 1, 1, Ogre::PF_R8G8B8A8, 90 | Ogre::TEX_TYPE_2D, 0); 91 | 92 | Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create(base_name, 93 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 94 | material->setLightingEnabled(false); 95 | m_selection_rectangle->setMaterial(material->getName()); 96 | Ogre::AxisAlignedBox aabInf; 97 | aabInf.setInfinite(); 98 | m_selection_rectangle->setBoundingBox(aabInf); 99 | m_selection_rectangle->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1); 100 | material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 101 | material->setCullingMode(Ogre::CULL_NONE); 102 | 103 | Ogre::TextureUnitState* tex_unit = material->getTechnique(0)->getPass(0)->createTextureUnitState(); 104 | tex_unit->setTextureName(tex->getName()); 105 | tex_unit->setTextureFiltering(Ogre::TFO_NONE); 106 | 107 | m_selection_rectangle_node->attachObject(m_selection_rectangle); 108 | } 109 | 110 | void AnnotationSelectionTool::InitPolylineSelection() 111 | { 112 | std::stringstream ss; 113 | ss << "RVizCloudAnnotationSelectionToolPoly" << std::hex << intptr_t(this); 114 | const std::string base_name = ss.str(); 115 | 116 | const static uint32_t texture_data[1] = { 0xff000080 }; 117 | Ogre::DataStreamPtr pixel_stream; 118 | pixel_stream.bind(new Ogre::MemoryDataStream( (void*)&texture_data[0], 4 )); 119 | 120 | Ogre::TexturePtr tex = Ogre::TextureManager::getSingleton().loadRawData(base_name + "Texture", 121 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 122 | pixel_stream, 1, 1, Ogre::PF_R8G8B8A8, 123 | Ogre::TEX_TYPE_2D, 0); 124 | 125 | Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create(base_name, 126 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 127 | material->setLightingEnabled(false); 128 | material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 129 | material->setCullingMode(Ogre::CULL_NONE); 130 | 131 | Ogre::TextureUnitState* tex_unit = material->getTechnique(0)->getPass(0)->createTextureUnitState(); 132 | tex_unit->setTextureName(tex->getName()); 133 | tex_unit->setTextureFiltering(Ogre::TFO_NONE); 134 | 135 | m_selection_polyline_node = scene_manager_->getRootSceneNode()->createChildSceneNode(); 136 | 137 | m_selection_polyline = scene_manager_->createManualObject(base_name + "manual"); 138 | m_selection_polyline->setUseIdentityProjection(true); 139 | m_selection_polyline->setUseIdentityView(true); 140 | 141 | Ogre::AxisAlignedBox aabInf; 142 | aabInf.setInfinite(); 143 | m_selection_polyline->setBoundingBox(aabInf); 144 | m_selection_polyline->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1); 145 | 146 | m_selection_polyline->begin(base_name,Ogre::RenderOperation::OT_TRIANGLE_LIST); 147 | 148 | m_selection_polyline->position(0.0,0.0,0.0); 149 | m_selection_polyline->position(1.0,1.0,0.0); 150 | m_selection_polyline->position(-1.0,1.0,0.0); 151 | 152 | m_selection_polyline->end(); 153 | 154 | m_selection_polyline_node->attachObject(m_selection_polyline); 155 | m_selection_polyline_node->setVisible(false); 156 | } 157 | 158 | void AnnotationSelectionTool::UpdatePolylineSelection() 159 | { 160 | const uint32 poly_size = m_sel_polyline.size(); 161 | 162 | FloatVector conv_x(poly_size); 163 | FloatVector conv_y(poly_size); 164 | 165 | const float viewport_width = m_selection_viewport->getActualWidth(); 166 | const float viewport_height = m_selection_viewport->getActualHeight(); 167 | 168 | const float LINE_WIDTH_PX = 1.0; 169 | const float wx = (LINE_WIDTH_PX / viewport_width) * 2.0f; 170 | const float wy = (LINE_WIDTH_PX / viewport_height) * 2.0f; 171 | 172 | for (uint32 i = 0; i < poly_size; i++) 173 | { 174 | const Coords2D & c = m_sel_polyline[i]; 175 | float nx = (c.x / viewport_width) * 2.0f - 1.0f; 176 | float ny = (c.y / viewport_height) * 2.0f - 1.0f; 177 | 178 | nx = nx < -1.0f ? -1.0f : (nx > 1.0f ? 1.0f : nx); 179 | ny = ny < -1.0f ? -1.0f : (ny > 1.0f ? 1.0f : ny); 180 | ny = -ny; 181 | 182 | conv_x[i] = nx; 183 | conv_y[i] = ny; 184 | } 185 | 186 | m_selection_polyline->beginUpdate(0); 187 | 188 | for (uint32 i = 1; i <= poly_size; i++) 189 | { 190 | const float cx = conv_x[i % poly_size]; 191 | const float cy = conv_y[i % poly_size]; 192 | const float px = conv_x[i - 1]; 193 | const float py = conv_y[i - 1]; 194 | const float ny = (cx - px); 195 | const float nx = -(cy - py); 196 | const float nnorm = std::sqrt(ny*ny + nx*nx); 197 | if (nnorm < 0.01) 198 | continue; 199 | const float dx = wx * nx / nnorm; 200 | const float dy = wy * ny / nnorm; 201 | m_selection_polyline->position(px - dx,py - dy,0.0); 202 | m_selection_polyline->position(px + dx,py + dy,0.0); 203 | m_selection_polyline->position(cx + dx,cy + dy,0.0); 204 | 205 | m_selection_polyline->position(px - dx,py - dy,0.0); 206 | m_selection_polyline->position(cx + dx,cy + dy,0.0); 207 | m_selection_polyline->position(cx - dx,cy - dy,0.0); 208 | } 209 | 210 | m_selection_polyline->end(); 211 | } 212 | 213 | void AnnotationSelectionTool::ClearSelection() 214 | { 215 | m_selecting = false; 216 | m_polyline_selecting = false; 217 | } 218 | 219 | void AnnotationSelectionTool::activate() 220 | { 221 | setStatus("Annotation tool."); 222 | 223 | ClearSelection(); 224 | m_active = true; 225 | UpdateCursor(); 226 | } 227 | 228 | void AnnotationSelectionTool::deactivate() 229 | { 230 | m_active = false; 231 | ClearSelection(); 232 | } 233 | 234 | void AnnotationSelectionTool::update(float wall_dt, float ros_dt) 235 | { 236 | m_selection_rectangle_node->setVisible(m_selecting); 237 | m_selection_polyline_node->setVisible(m_polyline_selecting); 238 | 239 | if (m_selecting) 240 | { 241 | float nx1 = ((float)m_sel_start_x / m_selection_viewport->getActualWidth()) * 2 - 1; 242 | float nx2 = ((float)m_sel_end_x / m_selection_viewport->getActualWidth()) * 2 - 1; 243 | float ny1 = -(((float)m_sel_start_y / m_selection_viewport->getActualHeight()) * 2 - 1); 244 | float ny2 = -(((float)m_sel_end_y / m_selection_viewport->getActualHeight()) * 2 - 1); 245 | 246 | nx1 = nx1 < -1 ? -1 : (nx1 > 1 ? 1 : nx1); 247 | ny1 = ny1 < -1 ? -1 : (ny1 > 1 ? 1 : ny1); 248 | nx2 = nx2 < -1 ? -1 : (nx2 > 1 ? 1 : nx2); 249 | ny2 = ny2 < -1 ? -1 : (ny2 > 1 ? 1 : ny2); 250 | 251 | m_selection_rectangle->setCorners(nx1,ny1,nx2,ny2); 252 | } 253 | 254 | if (m_polyline_selecting) 255 | UpdatePolylineSelection(); 256 | } 257 | 258 | void AnnotationSelectionTool::onSetEditMode(const std_msgs::UInt32 & mode) 259 | { 260 | if (m_edit_mode != mode.data) 261 | ClearSelection(); 262 | 263 | m_edit_mode = mode.data; 264 | m_edit_mode_selectable = 265 | m_edit_mode == EDIT_MODE_CONTROL_POINT || 266 | m_edit_mode == EDIT_MODE_ERASER; 267 | 268 | if (m_active) 269 | UpdateCursor(); 270 | } 271 | 272 | void AnnotationSelectionTool::onSetToolType(const std_msgs::UInt32 & type) 273 | { 274 | if (m_tool_type != type.data) 275 | ClearSelection(); 276 | 277 | m_tool_type = type.data; 278 | 279 | if (m_active) 280 | UpdateCursor(); 281 | } 282 | 283 | int AnnotationSelectionTool::processKeyEvent(QKeyEvent * event,rviz::RenderPanel * panel) 284 | { 285 | if (event->key() == Qt::Key_Insert) 286 | { 287 | m_toggle_none_pub.publish(std_msgs::Empty()); 288 | return 0; 289 | } 290 | 291 | if (event->key() == Qt::Key_Escape && 292 | m_polyline_selecting) 293 | { 294 | ClearSelection(); 295 | return Render; 296 | } 297 | 298 | return 0; 299 | } 300 | 301 | void AnnotationSelectionTool::UpdateCursor() 302 | { 303 | if (m_edit_mode_selectable && 304 | (m_tool_type == TOOL_TYPE_SHALLOW_SQUARE || 305 | m_tool_type == TOOL_TYPE_DEEP_SQUARE || 306 | m_tool_type == TOOL_TYPE_SHALLOW_POLY)) 307 | { 308 | setCursor(Qt::CrossCursor); 309 | } 310 | else if (m_edit_mode == EDIT_MODE_NONE) 311 | { 312 | setCursor(m_move_tool->getCursor()); 313 | } 314 | else 315 | { 316 | setCursor(Qt::PointingHandCursor); 317 | } 318 | } 319 | 320 | int AnnotationSelectionTool::processMouseEvent(rviz::ViewportMouseEvent& event) 321 | { 322 | if ((m_tool_type == TOOL_TYPE_SINGLE_PICK || !m_edit_mode_selectable) && 323 | (event.leftDown() || event.leftUp())) 324 | { 325 | return m_interaction_tool->processMouseEvent(event); 326 | } 327 | 328 | if (event.wheel_delta != 0 && !m_selecting && !m_polyline_selecting) 329 | { 330 | return m_move_tool->processMouseEvent(event); 331 | } 332 | 333 | if (event.rightDown() && !m_selecting && !m_polyline_selecting) 334 | { 335 | m_toggle_none_pub.publish(std_msgs::Empty()); 336 | return 0; 337 | } 338 | 339 | if (m_edit_mode == EDIT_MODE_NONE) 340 | { 341 | return m_move_tool->processMouseEvent(event); 342 | } 343 | 344 | if (event.leftDown() && 345 | (m_tool_type == TOOL_TYPE_DEEP_SQUARE || m_tool_type == TOOL_TYPE_SHALLOW_SQUARE)) 346 | { 347 | StartRectangleSelection(event.x,event.y,event.viewport); 348 | 349 | return Render; 350 | } 351 | 352 | if (event.leftDown() && 353 | (m_tool_type == TOOL_TYPE_SHALLOW_POLY) && 354 | !m_polyline_selecting) 355 | { 356 | StartPolylineSelection(event.x,event.y,event.viewport); 357 | 358 | return Render; 359 | } 360 | 361 | if (m_selecting) //点选操作 362 | { 363 | m_sel_end_x = event.x; 364 | m_sel_end_y = event.y; 365 | if(event.leftUp()) 366 | { 367 | SendViewportData(m_sel_start_x,m_sel_start_y,m_sel_end_x,m_sel_end_y,m_selection_viewport); 368 | ClearSelection(); 369 | } 370 | return Render; 371 | } 372 | 373 | if (m_polyline_selecting) // 多边形选择操作 374 | { 375 | m_sel_polyline.back() = Coords2D(event.x,event.y); 376 | 377 | if (event.leftDown()) 378 | { 379 | m_sel_polyline.push_back(Coords2D(event.x,event.y)); 380 | } 381 | 382 | if (event.rightDown()) 383 | { 384 | SendViewportPolylineData(m_selection_viewport,m_sel_polyline); 385 | ClearSelection(); 386 | } 387 | 388 | return Render; 389 | } 390 | 391 | return 0; 392 | } 393 | 394 | void AnnotationSelectionTool::StartRectangleSelection(const int32 start_x,const int32 start_y,Ogre::Viewport * viewport) 395 | { 396 | m_selecting = true; 397 | m_sel_start_x = start_x; 398 | m_sel_start_y = start_y; 399 | m_sel_end_x = start_x; 400 | m_sel_end_y = start_y; 401 | m_selection_viewport = viewport; 402 | } 403 | 404 | void AnnotationSelectionTool::StartPolylineSelection(const int32 start_x,const int32 start_y,Ogre::Viewport * viewport) 405 | { 406 | m_sel_polyline.clear(); 407 | m_sel_polyline.push_back(Coords2D(start_x,start_y)); 408 | m_sel_polyline.push_back(Coords2D(start_x,start_y)); 409 | m_polyline_selecting = true; 410 | m_polyline_self_intersect = false; 411 | m_selection_viewport = viewport; 412 | } 413 | 414 | void AnnotationSelectionTool::SendViewportPolylineData(const Ogre::Viewport * const viewport, 415 | const Coords2DVector & polyline) 416 | { 417 | const uint32 polyline_size = polyline.size(); 418 | if (polyline_size < 3) 419 | return; 420 | 421 | int32 x1 = polyline[0].x; 422 | int32 x2 = polyline[0].x; 423 | int32 y1 = polyline[0].y; 424 | int32 y2 = polyline[0].y; 425 | 426 | for (uint32 i = 1; i < polyline_size; i++) 427 | { 428 | x1 = std::min(x1,polyline[i].x); 429 | x2 = std::max(x2,polyline[i].x); 430 | y1 = std::min(y1,polyline[i].y); 431 | y2 = std::max(y2,polyline[i].y); 432 | } 433 | 434 | if (x1 < 0) x1 = 0; 435 | if (y1 < 0) y1 = 0; 436 | 437 | if (x1 > viewport->getActualWidth() - 1) x1 = viewport->getActualWidth() - 1; 438 | if (y1 > viewport->getActualHeight() - 1) y1 = viewport->getActualHeight() - 1; 439 | 440 | if (x2 < 0) x2 = 0; 441 | if (y2 < 0) y2 = 0; 442 | if (x2 > viewport->getActualWidth() - 1) x2 = viewport->getActualWidth() - 1; 443 | if (y2 > viewport->getActualHeight() - 1) y2 = viewport->getActualHeight() - 1; 444 | 445 | const Ogre::Matrix4 proj_matrix = viewport->getCamera()->getProjectionMatrix(); 446 | const Ogre::Vector3 camera_position = viewport->getCamera()->getDerivedPosition(); 447 | const Ogre::Quaternion camera_orientation = viewport->getCamera()->getDerivedOrientation(); 448 | 449 | const float fovy = viewport->getCamera()->getFOVy().valueRadians(); 450 | const float focal_length_px = viewport->getActualHeight() * std::tan((M_PI / 2) - (fovy / 2)); 451 | 452 | rviz_cloud_annotation::RectangleSelectionViewport msg; 453 | 454 | for (uint32 i = 0; i < polyline_size; i++) 455 | { 456 | msg.polyline_x.push_back(polyline[i].x); 457 | msg.polyline_y.push_back(polyline[i].y); 458 | } 459 | 460 | for (uint32 y = 0; y < 4; y++) 461 | for (uint32 x = 0; x < 4; x++) 462 | msg.projection_matrix[x + y * 4] = proj_matrix[y][x]; 463 | 464 | msg.camera_pose.orientation.x = camera_orientation.x; 465 | msg.camera_pose.orientation.y = camera_orientation.y; 466 | msg.camera_pose.orientation.z = camera_orientation.z; 467 | msg.camera_pose.orientation.w = camera_orientation.w; 468 | 469 | msg.camera_pose.position.x = camera_position.x; 470 | msg.camera_pose.position.y = camera_position.y; 471 | msg.camera_pose.position.z = camera_position.z; 472 | 473 | msg.viewport_width = viewport->getActualWidth(); 474 | msg.viewport_height = viewport->getActualHeight(); 475 | 476 | msg.focal_length = focal_length_px; 477 | 478 | msg.is_deep_selection = false; 479 | 480 | msg.start_x = x1; 481 | msg.end_x = x2; 482 | msg.start_y = y1; 483 | msg.end_y = y2; 484 | 485 | m_annotation_selection_pub.publish(msg); 486 | } 487 | 488 | void AnnotationSelectionTool::SendViewportData(const int32 start_x,const int32 start_y, 489 | const int32 end_x,const int32 end_y, 490 | const Ogre::Viewport * const viewport) 491 | { 492 | int32 x1 = std::min(start_x,end_x + 1); 493 | int32 x2 = std::max(start_x,end_x + 1); 494 | int32 y1 = std::min(start_y,end_y + 1); 495 | int32 y2 = std::max(start_y,end_y + 1); 496 | 497 | if (x1 < 0) x1 = 0; 498 | if (y1 < 0) y1 = 0; 499 | 500 | if (x1 > viewport->getActualWidth() - 1) x1 = viewport->getActualWidth() - 1; 501 | if (y1 > viewport->getActualHeight() - 1) y1 = viewport->getActualHeight() - 1; 502 | 503 | if (x2 < 0) x2 = 0; 504 | if (y2 < 0) y2 = 0; 505 | if (x2 > viewport->getActualWidth() - 1) x2 = viewport->getActualWidth() - 1; 506 | if (y2 > viewport->getActualHeight() - 1) y2 = viewport->getActualHeight() - 1; 507 | 508 | const Ogre::Matrix4 proj_matrix = viewport->getCamera()->getProjectionMatrix(); 509 | const Ogre::Vector3 camera_position = viewport->getCamera()->getDerivedPosition(); 510 | const Ogre::Quaternion camera_orientation = viewport->getCamera()->getDerivedOrientation(); 511 | 512 | const float fovy = viewport->getCamera()->getFOVy().valueRadians(); 513 | const float focal_length_px = viewport->getActualHeight() * std::tan((M_PI / 2) - (fovy / 2)); 514 | 515 | rviz_cloud_annotation::RectangleSelectionViewport msg; 516 | 517 | for (uint32 y = 0; y < 4; y++) 518 | for (uint32 x = 0; x < 4; x++) 519 | msg.projection_matrix[x + y * 4] = proj_matrix[y][x]; 520 | 521 | msg.camera_pose.orientation.x = camera_orientation.x; 522 | msg.camera_pose.orientation.y = camera_orientation.y; 523 | msg.camera_pose.orientation.z = camera_orientation.z; 524 | msg.camera_pose.orientation.w = camera_orientation.w; 525 | 526 | msg.camera_pose.position.x = camera_position.x; 527 | msg.camera_pose.position.y = camera_position.y; 528 | msg.camera_pose.position.z = camera_position.z; 529 | 530 | msg.viewport_width = viewport->getActualWidth(); 531 | msg.viewport_height = viewport->getActualHeight(); 532 | 533 | msg.focal_length = focal_length_px; 534 | 535 | msg.is_deep_selection = (m_tool_type == TOOL_TYPE_DEEP_SQUARE); 536 | 537 | msg.start_x = x1; 538 | msg.end_x = x2; 539 | msg.start_y = y1; 540 | msg.end_y = y2; 541 | 542 | m_annotation_selection_pub.publish(msg); 543 | } 544 | 545 | } // end namespace rviz 546 | 547 | #include 548 | PLUGINLIB_EXPORT_CLASS(rviz_cloud_annotation::AnnotationSelectionTool,rviz::Tool) 549 | -------------------------------------------------------------------------------- /src/rviz_cloud_annotation/src/rviz_select_tool.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_SELECT_TOOL_H 2 | #define RVIZ_SELECT_TOOL_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace Ogre 13 | { 14 | class Viewport; 15 | class Rectangle2D; 16 | class SceneNode; 17 | class ManualObject; 18 | } 19 | 20 | namespace rviz 21 | { 22 | class MoveTool; 23 | class ViewportMouseEvent; 24 | class InteractionTool; 25 | class RenderPanel; 26 | } 27 | 28 | namespace rviz_cloud_annotation 29 | { 30 | class AnnotationSelectionTool: public rviz::Tool 31 | { 32 | public: 33 | typedef int32_t int32; 34 | typedef uint32_t uint32; 35 | typedef std::vector FloatVector; 36 | 37 | struct Coords2D 38 | { 39 | int32 x; 40 | int32 y; 41 | Coords2D(const int32 px,const int32 py): x(px), y(py) {} 42 | }; 43 | typedef std::vector Coords2DVector; 44 | 45 | AnnotationSelectionTool(); 46 | virtual ~AnnotationSelectionTool(); 47 | 48 | virtual void onInitialize(); 49 | 50 | virtual void activate(); 51 | virtual void deactivate(); 52 | 53 | int processMouseEvent(rviz::ViewportMouseEvent & event); 54 | int processKeyEvent(QKeyEvent * event,rviz::RenderPanel * panel); 55 | 56 | virtual void update(float wall_dt,float ros_dt); 57 | 58 | private: 59 | 60 | void SendViewportPolylineData(const Ogre::Viewport * const viewport, 61 | const Coords2DVector & polyline); 62 | 63 | void SendViewportData(const int32 start_x, const int32 start_y, 64 | const int32 end_x, const int32 end_y, 65 | const Ogre::Viewport * const viewport); 66 | 67 | void onSetEditMode(const std_msgs::UInt32 & mode); 68 | void onSetToolType(const std_msgs::UInt32 & type); 69 | void onSetAnnotationType(const std_msgs::UInt32 & type); 70 | 71 | void UpdateCursor(); 72 | 73 | void StartRectangleSelection(const int32 start_x,const int32 start_y,Ogre::Viewport * viewport); 74 | void InitRectangleSelection(); 75 | 76 | void StartPolylineSelection(const int32 start_x,const int32 start_y,Ogre::Viewport * viewport); 77 | void InitPolylineSelection(); 78 | void UpdatePolylineSelection(); 79 | 80 | void ClearSelection(); 81 | 82 | rviz::MoveTool * m_move_tool; 83 | rviz::InteractionTool * m_interaction_tool; 84 | 85 | uint32 m_edit_mode; 86 | bool m_edit_mode_selectable; 87 | uint32 m_tool_type; 88 | uint32 m_annotation_type; 89 | 90 | ros::NodeHandle m_nh; 91 | ros::Publisher m_annotation_selection_pub; 92 | ros::Subscriber m_on_set_mode_sub; 93 | ros::Subscriber m_on_set_tool_type_sub; 94 | ros::Publisher m_toggle_none_pub; 95 | 96 | bool m_selecting; 97 | int32 m_sel_start_x; 98 | int32 m_sel_start_y; 99 | int32 m_sel_end_x; 100 | int32 m_sel_end_y; 101 | 102 | bool m_polyline_selecting; 103 | bool m_polyline_self_intersect; 104 | Coords2DVector m_sel_polyline; 105 | 106 | bool m_active; 107 | 108 | Ogre::Rectangle2D * m_selection_rectangle; 109 | Ogre::ManualObject * m_selection_polyline; 110 | Ogre::SceneNode * m_selection_rectangle_node; 111 | Ogre::SceneNode * m_selection_polyline_node; 112 | Ogre::Viewport * m_selection_viewport; 113 | 114 | //My View 115 | 116 | }; 117 | 118 | } 119 | 120 | #endif // RVIZ_SELECT_TOOL_H 121 | -------------------------------------------------------------------------------- /tools/clear_annotation.sh: -------------------------------------------------------------------------------- 1 | cd $HOME/lidar_annotation/ 2 | 3 | rm -rf _annotation/ 4 | rm -rf _bbox/ 5 | rm -rf _label/ 6 | rm -rf _pcd/ 7 | rm -f annotation.log 8 | -------------------------------------------------------------------------------- /tools/install_annotation.sh: -------------------------------------------------------------------------------- 1 | cd $pwd .. 2 | 3 | catkin_make 4 | 5 | source devel/setup.bash 6 | -------------------------------------------------------------------------------- /tools/launch_annotation.sh: -------------------------------------------------------------------------------- 1 | cd $pwd .. 2 | 3 | cd devel/ 4 | 5 | source setup.bash 6 | 7 | cd $pwd .. 8 | 9 | roslaunch rviz_cloud_annotation annotation.launch 10 | -------------------------------------------------------------------------------- /tools/pcd_play: -------------------------------------------------------------------------------- 1 | rosrun pcl_ros pcd_to_pointcloud a.pcd 0.1 _frame_id:=/base_link 2 | -------------------------------------------------------------------------------- /tools/raw2pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/tools/raw2pcd -------------------------------------------------------------------------------- /tools/ros_debug.sh: -------------------------------------------------------------------------------- 1 | sudo cp -r .ros/ ~/ 2 | 3 | sudo chmod -R +777 ~/.ros/ 4 | 5 | 6 | -------------------------------------------------------------------------------- /tools/uninstall_annotation.sh: -------------------------------------------------------------------------------- 1 | cd $pwd .. 2 | 3 | rm -r build/ 4 | 5 | rm -r devel/ 6 | -------------------------------------------------------------------------------- /uninstall.sh: -------------------------------------------------------------------------------- 1 | cd tools/ 2 | 3 | bash uninstall_annotation.sh 4 | 5 | echo "uninstall successful!" 6 | -------------------------------------------------------------------------------- /示例pcd文件/pcd/16line.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/示例pcd文件/pcd/16line.pcd -------------------------------------------------------------------------------- /示例pcd文件/pcd/64line.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/halostorm/PCAT_open_source/fdc9272f0a14c59469435b9444906b51e849a2ab/示例pcd文件/pcd/64line.pcd --------------------------------------------------------------------------------