├── .devcontainer └── devcontainer.json ├── .github └── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md ├── .gitignore ├── CMakeLists.txt ├── CODE_OF_CONDUCT.md ├── LICENSE ├── README.md ├── docker ├── Dockerfile ├── docker-compose.yml └── download.bash ├── docs ├── add-new-annotation-tool.gif ├── align-bounding-box.gif ├── labeling.md ├── new-annotation.gif ├── rviz-full.png ├── set-label.gif ├── shortcut-editing.gif └── track.png ├── icons ├── README.md ├── auto-fit-points.svg ├── automations.svg ├── classes │ ├── Annotated PointCloud2.svg │ └── New Annotation.svg ├── commit-annotation.svg ├── keyboard.svg ├── labels.svg ├── open.svg ├── rotate-anti-clockwise.svg ├── rotate-clockwise.svg ├── save.svg ├── shrink-to-points.svg ├── tags.svg ├── toggle-pause.svg └── undo.svg ├── include └── annotate │ ├── annotate_display.h │ ├── annotate_tool.h │ ├── annotation_marker.h │ ├── file_dialog_property.h │ └── shortcut_property.h ├── launch ├── annotate.launch ├── demo.launch └── demo.rviz ├── package.xml ├── plugin_description.xml └── src ├── annotate_display.cpp ├── annotate_tool.cpp ├── annotation_marker.cpp ├── file_dialog_property.cpp └── shortcut_property.cpp /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "ROS", 3 | "dockerComposeFile": [ 4 | "../docker/docker-compose.yml" 5 | ], 6 | "service": "workspace", 7 | "workspaceFolder": "/workspace", 8 | "shutdownAction": "stopCompose", 9 | "extensions": [ 10 | "ms-vscode.cpptools", 11 | "ms-python.python", 12 | "ajshort.msg", 13 | "twxs.cmake", 14 | "redhat.vscode-xml" 15 | ], 16 | "remoteUser": "user" 17 | } 18 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: bug 6 | assignees: Earthwings 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. Ubuntu 18.04] 28 | - ROS Distribution [e.g. Melodic] 29 | - Annotate Version [e.g. git revision number] 30 | 31 | **Additional context** 32 | Add any other context about the problem here. 33 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: Earthwings 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(annotate) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | #set(CMAKE_BUILD_TYPE Debug) 8 | 9 | ## Find catkin macros and libraries 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | geometry_msgs 13 | interactive_markers 14 | roscpp 15 | rviz 16 | tf 17 | visualization_msgs 18 | ) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | # LIBRARIES ${PROJECT_NAME} 26 | CATKIN_DEPENDS 27 | geometry_msgs 28 | interactive_markers 29 | roscpp 30 | rviz 31 | tf 32 | visualization_msgs 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | include_directories( 39 | include 40 | ${catkin_INCLUDE_DIRS} 41 | ${rviz_INCLUDE_DIRS} 42 | ) 43 | 44 | set(CMAKE_AUTOMOC ON) 45 | find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets Gui) 46 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 47 | set(QT_LIBRARIES Qt5::Widgets Qt5::Gui) 48 | add_definitions(-DQT_NO_KEYWORDS) 49 | 50 | # add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp 51 | # target_link_libraries(${PROJECT_NAME}_node ${QT_LIBRARIES} ${catkin_LIBRARIES} yaml-cpp) 52 | 53 | add_library(${PROJECT_NAME} 54 | src/${PROJECT_NAME}_display.cpp 55 | src/${PROJECT_NAME}_tool.cpp 56 | src/annotation_marker.cpp 57 | src/file_dialog_property.cpp 58 | src/shortcut_property.cpp 59 | include/${PROJECT_NAME}/${PROJECT_NAME}_display.h 60 | include/${PROJECT_NAME}/${PROJECT_NAME}_tool.h 61 | include/${PROJECT_NAME}/file_dialog_property.h 62 | include/${PROJECT_NAME}/shortcut_property.h 63 | ) 64 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES} yaml-cpp) 65 | 66 | ############# 67 | ## Install ## 68 | ############# 69 | #install(TARGETS ${PROJECT_NAME}_node 70 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 71 | #) 72 | 73 | install(TARGETS ${PROJECT_NAME} 74 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 77 | ) 78 | 79 | install(DIRECTORY include/${PROJECT_NAME}/ 80 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 81 | FILES_MATCHING PATTERN "*.h" 82 | ) 83 | 84 | install(FILES 85 | plugin_description.xml 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 87 | ) 88 | 89 | install(DIRECTORY 90 | icons 91 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 92 | ) 93 | -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | In the interest of fostering an open and welcoming environment, we as 6 | contributors and maintainers pledge to making participation in our project and 7 | our community a harassment-free experience for everyone, regardless of age, body 8 | size, disability, ethnicity, sex characteristics, gender identity and expression, 9 | level of experience, education, socio-economic status, nationality, personal 10 | appearance, race, religion, or sexual identity and orientation. 11 | 12 | ## Our Standards 13 | 14 | Examples of behavior that contributes to creating a positive environment 15 | include: 16 | 17 | * Using welcoming and inclusive language 18 | * Being respectful of differing viewpoints and experiences 19 | * Gracefully accepting constructive criticism 20 | * Focusing on what is best for the community 21 | * Showing empathy towards other community members 22 | 23 | Examples of unacceptable behavior by participants include: 24 | 25 | * The use of sexualized language or imagery and unwelcome sexual attention or 26 | advances 27 | * Trolling, insulting/derogatory comments, and personal or political attacks 28 | * Public or private harassment 29 | * Publishing others' private information, such as a physical or electronic 30 | address, without explicit permission 31 | * Other conduct which could reasonably be considered inappropriate in a 32 | professional setting 33 | 34 | ## Our Responsibilities 35 | 36 | Project maintainers are responsible for clarifying the standards of acceptable 37 | behavior and are expected to take appropriate and fair corrective action in 38 | response to any instances of unacceptable behavior. 39 | 40 | Project maintainers have the right and responsibility to remove, edit, or 41 | reject comments, commits, code, wiki edits, issues, and other contributions 42 | that are not aligned to this Code of Conduct, or to ban temporarily or 43 | permanently any contributor for other behaviors that they deem inappropriate, 44 | threatening, offensive, or harmful. 45 | 46 | ## Scope 47 | 48 | This Code of Conduct applies both within project spaces and in public spaces 49 | when an individual is representing the project or its community. Examples of 50 | representing a project or community include using an official project e-mail 51 | address, posting via an official social media account, or acting as an appointed 52 | representative at an online or offline event. Representation of a project may be 53 | further defined and clarified by project maintainers. 54 | 55 | ## Enforcement 56 | 57 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 58 | reported by contacting the project team at opensource@nienhueser.de. All 59 | complaints will be reviewed and investigated and will result in a response that 60 | is deemed necessary and appropriate to the circumstances. The project team is 61 | obligated to maintain confidentiality with regard to the reporter of an incident. 62 | Further details of specific enforcement policies may be posted separately. 63 | 64 | Project maintainers who do not follow or enforce the Code of Conduct in good 65 | faith may face temporary or permanent repercussions as determined by other 66 | members of the project's leadership. 67 | 68 | ## Attribution 69 | 70 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, 71 | available at https://www.contributor-covenant.org/version/1/4/code-of-conduct.html 72 | 73 | [homepage]: https://www.contributor-covenant.org 74 | 75 | For answers to common questions about this code of conduct, see 76 | https://www.contributor-covenant.org/faq 77 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Dennis Nienhüser 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # annotate 2 | 3 | ![GitHub](https://img.shields.io/github/license/Earthwings/annotate) 4 | ![GitHub top language](https://img.shields.io/github/languages/top/Earthwings/annotate) 5 | ![GitHub last commit](https://img.shields.io/github/last-commit/Earthwings/annotate) 6 | 7 | The evaluation of perception algorithms requires labeled ground truth data. Supervised machine learning algorithms need labeled training data. Annotate is a tool to create 3D labeled bounding boxes in ROS/RViz. Its labeled data is useful both as training data for machine learning algorithms as well as ground truth for evaluation. 8 | 9 | ## Requirements 10 | 11 | Annotate is useful in the following contexts: 12 | 13 | * You are using ROS. Annotate was tested with ROS Kinetic (Ubuntu 16.04) and ROS Melodic (Ubuntu 18.04). 14 | * Your data is available in (or can easily be converted to) [3D point clouds](http://wiki.ros.org/pcl). 15 | 16 | ## Overview 17 | 18 | Create annotations (labeled data) using RViz. Annotate provides RViz extensions for labeling. You are responsible to provide labeling data using a ROS topic with [sensor_msgs/PointCloud2](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html) data. 19 | 20 | * Use ```rosbag play``` or similar tooling to provide point cloud data to be annotated in ```sensor_msgs/PointCloud2``` format 21 | * Start ```rviz``` from a ROS workspace including this repository to create and edit annotation tracks 22 | * Labeled data is stored in a YAML file of your choice 23 | See [labeling](docs/labeling.md) for a detailled description of label creation. 24 | 25 | ## Getting Started 26 | 27 | Clone this repository in a ROS workspace to build and run annotate. For a successful build please install the dependency pcl-ros as well. You can also use rosdep to pick it up for you. 28 | 29 | ```bash 30 | cd /path/to/your/workspace/src 31 | git clone https://github.com/Earthwings/annotate.git 32 | catkin build # or catkin_make or your other favorite build method 33 | source /path/to/your/workspace/devel/setup.bash # or setup.zsh for ZSH users 34 | ``` 35 | 36 | Use the ```demo.launch``` launch file shipped with annotate to see it in action quickly. Here is a sample call: 37 | 38 | ```bash 39 | roslaunch annotate demo.launch \ 40 | bag:="/kitti/2011_09_26_drive_0005_sync_pointcloud.bag --pause-topics velodyne_points" 41 | ``` 42 | 43 | The above call will run ```rosbag play``` and RViz using a configuration file that includes the annotate RViz tool and the annotate RViz display. In the annotate display set the topic with to be labeled data to ```/velodyne_points```. The RViz window will look similar to this: 44 | 45 | ![RViz/Annotate screenshot](docs/rviz-full.png "RViz screenshot with annotate") 46 | 47 | Please see [labeling](docs/labeling.md) for a detailed description of label creation. 48 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=melodic 2 | FROM ros:${ROS_DISTRO}-ros-base 3 | 4 | #COPY NVIDIA-DRIVER.run /tmp/NVIDIA-DRIVER.run 5 | 6 | ARG USERNAME=user 7 | ARG USER_UID=1000 8 | ARG USER_GID=$USER_UID 9 | 10 | # Avoid warnings by switching to noninteractive 11 | ENV DEBIAN_FRONTEND=noninteractive 12 | 13 | # Configure apt and install packages 14 | RUN apt-get update \ 15 | && apt-get -y install --no-install-recommends apt-utils dialog 2>&1 \ 16 | # 17 | # Install Nvidia drivers related tools 18 | && apt-get -y install kmod mesa-utils binutils \ 19 | # 20 | # Verify git, process tools, lsb-release (useful for CLI installs) installed 21 | && apt-get -y install git iproute2 procps lsb-release \ 22 | # 23 | # Install C++ tools 24 | && apt-get -y install build-essential cmake cppcheck valgrind \ 25 | # Install ROS packages 26 | && apt-get -y install ros-${ROS_DISTRO}-pcl-ros ros-${ROS_DISTRO}-rviz \ 27 | # 28 | # Create a non-root user to use if preferred - see https://aka.ms/vscode-remote/containers/non-root-user. 29 | && groupadd --gid $USER_GID $USERNAME \ 30 | && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ 31 | # [Optional] Add sudo support for the non-root user 32 | && apt-get install -y sudo \ 33 | && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ 34 | && chmod 0440 /etc/sudoers.d/$USERNAME \ 35 | && echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> /home/user/.bashrc 36 | # 37 | # 38 | # Install nvidia driver. Run download.bash before. 39 | #&& sh /tmp/NVIDIA-DRIVER.run -a -N --ui=none --no-kernel-module && rm /tmp/NVIDIA-DRIVER.run \ 40 | #&& ln -s /usr/lib/libGL.so.1 /usr/lib/x86_64-linux-gnu/libGL.so 41 | 42 | RUN if [ "$ROS_DISTRO" = "noetic" ] \ 43 | ; then \ 44 | # Install catkin tools. See https://answers.ros.org/question/353113/catkin-build-in-ubuntu-2004-noetic/?answer=353115#post-id-353115 45 | apt-get -y install python3-pip \ 46 | && pip3 install git+https://github.com/catkin/catkin_tools.git \ 47 | ; else \ 48 | apt-get -y install python-catkin-tools \ 49 | ; fi 50 | 51 | # Clean up 52 | RUN apt-get autoremove -y \ 53 | && apt-get clean -y \ 54 | && rm -rf /var/lib/apt/lists/* 55 | 56 | # Switch back to dialog for any ad-hoc use of apt-get 57 | ENV DEBIAN_FRONTEND=dialog 58 | 59 | USER user 60 | ENTRYPOINT ["/ros_entrypoint.sh"] 61 | -------------------------------------------------------------------------------- /docker/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '3' 2 | services: 3 | workspace: 4 | build: '.' 5 | environment: 6 | - DISPLAY 7 | - QT_X11_NO_MITSHM=1 8 | privileged: true 9 | command: sleep infinity 10 | network_mode: host 11 | volumes: 12 | - "../../..:/workspace" 13 | - "/tmp/.X11-unix:/tmp/.X11-unix:rw" 14 | - "$HOME/.Xauthority:/home/user/.Xauthority:rw" 15 | -------------------------------------------------------------------------------- /docker/download.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | version="$(glxinfo | grep "OpenGL version string" | rev | cut -d" " -f1 | rev)" 3 | wget "http://us.download.nvidia.com/XFree86/Linux-x86_64/${version}/NVIDIA-Linux-x86_64-${version}.run" -O NVIDIA-DRIVER.run 4 | -------------------------------------------------------------------------------- /docs/add-new-annotation-tool.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Earthwings/annotate/15f32b2c6598144e36816ab148237ca96b80255f/docs/add-new-annotation-tool.gif -------------------------------------------------------------------------------- /docs/align-bounding-box.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Earthwings/annotate/15f32b2c6598144e36816ab148237ca96b80255f/docs/align-bounding-box.gif -------------------------------------------------------------------------------- /docs/labeling.md: -------------------------------------------------------------------------------- 1 | # Labeling with annotate 2 | 3 | This guide teaches basic annotation concepts used in annotate. Read on to learn how to 4 | 5 | * setup your ROS environment to start annotating 6 | * create the first annotation for a physical object 7 | * create subsequent annotations for the same object 8 | * save time with keyboard shortcuts and linked actions. 9 | 10 | The guide uses a real-world example. Setup your local ROS environment as described in the guide to follow in parallel using a hands-on approach. 11 | 12 | ## Preparation 13 | 14 | The example described in the rest of this document uses a real-world labeling example taken from the [KITTI Vision Benchmark Suite](http://www.cvlibs.net/datasets/kitti/). The example bag file is a 16 seconds long traffic sequence called 2011_09_26_drive_0005.bag created using [kitti_to_rosbag](https://github.com/ethz-asl/kitti_to_rosbag). It provides lidar data from a Velodyne HDL-64 in the velodyne_points topic. 15 | 16 | Do you have a different ROS bag file with point cloud data at hand that should be annotated? You can also follow the guide using that bag file. Some sections need slight adjustments; the guide will point out these differences. 17 | 18 | The annotation environment in ROS consists of two parts: 19 | 20 | * Point cloud data on a ROS topic in `sensor_msgs/PointCloud2` format. Usually it comes from `rosbag play` — either directly or after some conversion or filtering. 21 | * RViz running with the annotate plugins installed. 22 | 23 | Creating annotations takes some time. Hence point cloud data should only change when you are ready for it. For `rosbag play`, its `--pause-topics` parameter works well to achieve this: Instruct `rosbag play` to pause after each new data on the point cloud topic, and resume playback when you are done creating annotations for the current data. 24 | 25 | Use the ```demo.launch``` launch file that ships with annotate to start both `rosbag play` and RViz at once: 26 | 27 | ```bash 28 | roslaunch annotate demo.launch \ 29 | bag:="/workspace/kitti/2011_09_26/2011_09_26_drive_0005_sync_pointcloud.bag --pause-topics velodyne_points" 30 | ``` 31 | 32 | For a custom bag file, please adjust the path to the bag file and the point cloud topic to pause on. In case you are not using `demo.launch`, but start RViz manually, you have to configure RViz for annotations: 33 | 34 | * In the `Tools` panel, use the `+` icon to add the `New Annotation` tool. 35 | * In the `Displays` panel, use the `Add` button to add an `Annotated PointCloud2` display for the point cloud topic you want to annotate. 36 | 37 | Starting `demo.launch` from annotate will open an RViz window for data annotation. Initially you will not see any point cloud data, because playback has paused directly. Press space once or twice to resume playback until point cloud data appears. 38 | 39 | You are ready to create the first annotation now. The next section explains that in detail. It uses the cyclist in the beginning of the scene as its annotation object. Do you want to follow up in parallel? Then please spot the cyclist in the scene and focus the RViz view on it before reading on. 40 | 41 | ## Creating a new annotation 42 | 43 | After you have spotted a new object for annotation, create a new annotation for it in two steps: 44 | 45 | * Click on the **New Annotation** button in the RViz tools panel. 46 | * Click on the object in the pointcloud. 47 | 48 | Annotate will create a new annotation at the point you clicked in the scene. It has a default size and no label attached yet like in the following video: 49 | 50 | ![RViz screenshot for creating a new annotation](new-annotation.gif "Creating a new annotation") 51 | 52 | ### Assigning a label 53 | 54 | Assign a label for the new annotation using the context menu. 55 | 56 | * Right-click on the annotation cube (bounding box). 57 | * Select a label in the **Label** menu of the context menu. 58 | * Note how the annotation description above the annotation cube now includes the chosen label. 59 | 60 | In the following video the bicyclist is labeled ```bicycle```: 61 | 62 | ![Assign a label to an annotation](set-label.gif "Assign a label to an annotation") 63 | 64 | A label is carried over to subsequent annotations of the same object. You can, however, change an annotation's label at any time. This is useful, for example, to mark special situations like partial occlusions of objects. Consult your teams annotation guidelines for instructions on special situations. 65 | 66 | ### Bounding box alignment 67 | 68 | The position and size of an annotation's bounding box is crucial information. A carefully aligned bounding box: 69 | 70 | * Contains all points of the object. 71 | * Tightly fits all object points such that the distance between the outmost points and the annotation box walls is small in all dimensions. 72 | * Has the red axis point towards the primary direction of the object. 73 | 74 | The primary direction of an object can mean different things. For example, for a car it could mean its front. But it could also mean its driving direction — its front for forward driving and standstill and its rear for reverse driving. Consult your teams annotation guidelines to learn about the meaning of the primary direction of each object type. 75 | 76 | Follow these steps to get a carefully aligned bounding box: 77 | 78 | 1. Change the annotation's **box mode** to **Move**. A black circle around the annotation box appears. 79 | 2. Change the RViz view to see the object from top (birds-eye view). 80 | 3. Drag the black ring to move the annotation box. Align its center with the object's center 81 | 4. Click on the object's annotation box to switch from move to rotation mode. The formerly black ring becomes blue. 82 | 5. Drag the blue ring to align the red axis with the object's front 83 | 6. Click on the object's annotation box once again to switch from rotation mode to resize mode. The blue ring is replaced with six colored resize handles (arrows). 84 | 7. Use the resize handles to roughly align the objects' annotation box size with the size of the object. 85 | 8. Right-click on the annotation box and choose **Auto-fit Box** from the **Edit** menu. 86 | 87 | While the list of steps may seem daunting at first sight, the process is actually a quick one as the following video shows: 88 | 89 | ![Bounding box alignment](align-bounding-box.gif "Align the bounding box of an annotation") 90 | 91 | Please note the annotation's description text above the box in the end of the video. It reads 92 | 93 | > ```bicycle #1```
94 | > ```1.80 (+0.05) x 0.60 (+0.05) x 1.66 (+0.05)```
95 | > ```841 points inside, 0 nearby``` 96 | 97 | The second line tells us that our box now has a length of 1.80 m, a width of 0.60 m and a height of 1.66 m. It also tells us that for each of those dimensions, the box could be shrinked by just 0.05 m. This is because the *auto-fit* feature (just like the *shrink to points* feature) leaves a margin of 0.025 m (2.5 cm) to every side. This means the box is a tight fit around all points inside. 98 | 99 | The third line tells us that there are 841 points inside the annotation box, and zero points in a distance of up to 0.25 m to it. Visual inspection also shows that we did not forget any point in the vicinity of the object. 100 | 101 | You can conclude that an annotation box is a well-aligned box if the following holds: 102 | 103 | * The second line of the annotation description reads ```(+0.05)``` for all three dimensions — we have a tight fit. 104 | * Visual inspection shows that there are no object points outside of the annotation box. 105 | 106 | ### Committing 107 | 108 | ### Summary 109 | 110 | In order to create a new annotation for an object that has not been annotated yet in the scene, follow these steps: 111 | 112 | 1. Spot the object in RViz and focus the view on it. 113 | 2. Click on the '''New Annotation''' button. 114 | 3. Click on the object in the RViz view. 115 | 4. Right-click on the object's annotation box and assign a label. 116 | 5. Change the RViz view to see the object from top (birds-eye view). 117 | 6. Drag the black ring to move the annotation box. Align its center with the object's center 118 | 7. Click on the object's annotation box to switch from move to rotation mode. The formerly black ring becomes blue. 119 | 8. Drag the blue ring to align the red axis with the object's front 120 | 9. Click on the object's annotation box once again to switch from rotation mode to resize mode. The blue ring is replaced with six colored resize handles (arrows). 121 | 10. Use the resize handles to roughly align the objects' annotation box size with the size of the object. 122 | 11. Right-click on the annotation box and choose **Auto-fit Box** from the **Edit** menu. 123 | 12. Perform a sanity check of the annotation box: The red axis should be aligned with the object front. All object points have to be within the annotation box. The annotation box should be tight around the object points. 124 | 13. If the sanity check fails, repeat the steps above to correct the found problem. 125 | 14. If the sanity check succeeds, right-click on the annotation box and choose **Commit**. The annotation box turns green. 126 | 127 | Most objects change their position and shape in the scene only gradually. Accordingly subsequent annotations are similar in shape and position. Annotate uses this fact to facilitate creating so-called annotation tracks, the change of an object's position and shape over time. Continue reading the next section to learn about annotation tracks in detail. 128 | 129 | ## Creating annotation tracks 130 | 131 | In the previous section you created a new annotation for the cyclist. The new annotation automatically created an *annotation track* with a unique identifier (`#15`). The first commit created the first *annotation instance* for that annotation track. An annotation instance holds all annotation properties for a given point in time: The bounding box (position and shape), the assigned label and the assigned tags. Only the identifier is not stored with an annotation instance, but with its track. 132 | 133 | You are now ready to create the second *annotation instance* for the cyclist. To do that, first move forward a bit in time to the next point cloud data. Press space to accomplish this. The point cloud changes slightly and the cyclist moves forward a bit. Note how the previously created annotation for the cyclist stays around at its earlier position, but changes color from green (committed) to gray. 134 | 135 | Create the second annotation instance for the cyclist by following steps 5. (change to birds-eye view) to 14. in the previous section. The commit in step 14 turns the bounding box green again, but another change occurs: A line is shown connecting the earlier bounding box center to the new center. This is a visual aid to see the object's movement over time. Over time it will become a path like in the following screenshot: 136 | 137 | ![Annotation Track ](track.png "Annotation Track") 138 | 139 | Move on to create the third annotation instance: Press space again. Note that this time the old annotation instance does not stay at its previous position, but moves with the object. Annotate calculates the new position based on the previous two annotation instances. The estimated position is accurate as long as the object keeps its speed and direction. This helps to reduce the effort needed to align the bounding box perfectly. Often a call of the auto-fit action is all that is needed. 140 | 141 | Before committing the annotation instance, check that its label and tags are still accurate. Annotate will carry on the label and tags of the previous instance, but occasionally they change. For example, the cyclist could dismount the bike and become a pedestrian. 142 | 143 | Continue the track annotation process for as long as the cyclist is visible in the scene. You will end up with an entire annotation track for the cyclist, which is all that is needed to annotate him. 144 | 145 | To summarize, the following process creates an entire annotation track for a new object that becomes visible in the scene: 146 | 147 | 1. Create a new annotation as described in the previous section. 148 | 2. Press space to move forward in time. 149 | 3. If the object is not visible anymore, you are done. 150 | 4. Otherwise, align the bounding box perfectly. 151 | 5. Adjust the label, if needed. 152 | 6. Adjust tags, if needed. 153 | 7. Commit the annotation instance. 154 | 8. Go to step 2. 155 | 156 | The cyclist is not the only object in the scene that should be annotated. The annotation process for the other objects follows the same principle, however. When annotating multiple objects, there are two basic strategies: 157 | 158 | * Annotating one object after the other, restarting playback several times. 159 | * Annotating all visible objects per time step, and only move forward in time afterwards. 160 | 161 | Which strategy you want to follow is up to you. If you are unsure, go for the first one (annotating one object after the other). Focussing on one object at a time is usually faster and leads to more accurate annotations. 162 | 163 | ## Using Keyboard Shortcuts 164 | 165 | Creating annotations becomes quick once you are familiar with the different edit modes and the available actions. But toggling between edit modes and selecting actions from the context will limit your speed. Use keyboard shortcuts to quickly activate actions. 166 | 167 | The following table shows the available actions and their default keyboard shortcuts. 168 | 169 | | Action | Default Shortcut | Description | 170 | |--------------------------------------------------------------------------------------------------------------------|-----------------------------------|--------------------------------------------------------------| 171 | | undo | Ctrl+Z | Revert the last action | 172 | | toggle pause | space | Toggle play and pause state in `rosbag play` | 173 | | rotate clockwise | ▶ (right arrow) | Rotate the bounding box clockwise (from birds-eye view) | 174 | | rotate anti-clockwise | ◀ (left arrow) | Rotate the bounding box anti-clockwise (from birds-eye view) | 175 | | shrink to points | Ctrl+B | Shrink bounding box to points inside | 176 | | auto-fit points | Ctrl+F | Fit bounding box to nearby points | 177 | | commit annotation | Ctrl+C | Save current annotation | 178 | 179 | The actions *toggle pause*, *rotate clockwise*, and *rotate anti-clockwise* have no counterpart in the context menu of annotations. Rotations can alternatively be performed in the box mode *Rotate*. Toggling playback and pause state is only available via a keyboard shortcut. 180 | 181 | Change the keyboard shortcuts to your liking in the Annotated PointCloud2 display. You can specify any key sequence understood by [Qt's QKeySequence](https://doc.qt.io/qt-5/qkeysequence.html). Combine modifiers like Ctrl, Shift, Alt with character keys like A, B, C, but also other keys including 182 | 183 | * ▲ up 184 | * ▶ right 185 | * ◀ left 186 | * ▼ down 187 | * ⏎ return 188 | * ⟵ backspace. 189 | 190 | Annotate will warn you in case you're entering an invalid shortcut. It also warns you against specifying shortcuts already used somewhere else like the following video demonstrates: 191 | 192 | ![Editing Shortcuts](shortcut-editing.gif) 193 | 194 | ## Linking Actions 195 | 196 | Once you memorize keyboard shortcuts you will become very fast to create annotation tracks for most objects. A typical track annotation sequence repeats the following default keyboard shortcuts with only little manual intervention in between: 197 | 198 | 1. Ctrl+F to fit the bounding box to nearby points 199 | 2. Ctrl+C to commit the annotation 200 | 3. space to move to the next point in time 201 | 202 | Shorten sequences with linked actions. The one above, for example, can be shortened to a single press of Ctrl+C by activating 203 | 204 | * Auto-fit points after change 205 | * Resume playback after commit 206 | 207 | in the *Linked Actions* section in the Annotated Pointcloud2 display. 208 | 209 | The following table lists all actions that can be linked and their description: 210 | 211 | | Linked Action | Description | 212 | |------------------------------------|-------------| 213 | | Shrink after resize | After resizing the annotation bounding box using the resize handles in resize mode, shrink the annotation bounding box to the points now inside. | 214 | | Shrink before commit | When the commit action is executed via the context menu or a keyboard shortcut, shrink the bounding box to the points inside before committing. | 215 | | Auto-fit after points change | When new point data arrives (time changes, objects move) fit the bounding box to nearby points. | 216 | | Pause playback after points change | When new point data arrives (time changes, objects move), pause playback. This can be used as an alternative to the `--pause-topics` parameter of `rosbag play`. | 217 | | Resume playback after commit | After the commit action is executed via the context menu or a keyboard shortcut, set `rosbag play` to play mode to resume playback. | 218 | -------------------------------------------------------------------------------- /docs/new-annotation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Earthwings/annotate/15f32b2c6598144e36816ab148237ca96b80255f/docs/new-annotation.gif -------------------------------------------------------------------------------- /docs/rviz-full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Earthwings/annotate/15f32b2c6598144e36816ab148237ca96b80255f/docs/rviz-full.png -------------------------------------------------------------------------------- /docs/set-label.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Earthwings/annotate/15f32b2c6598144e36816ab148237ca96b80255f/docs/set-label.gif -------------------------------------------------------------------------------- /docs/shortcut-editing.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Earthwings/annotate/15f32b2c6598144e36816ab148237ca96b80255f/docs/shortcut-editing.gif -------------------------------------------------------------------------------- /docs/track.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Earthwings/annotate/15f32b2c6598144e36816ab148237ca96b80255f/docs/track.png -------------------------------------------------------------------------------- /icons/README.md: -------------------------------------------------------------------------------- 1 | # Icon Origins 2 | This page lists the origin of icons used within this project. 3 | 4 | ## Original Icons 5 | * ```classes/New Annotation.svg``` by Dennis Nienhüser 6 | 7 | ## The Noun Project 8 | * ```keyboard.svg``` by Deemak Daksina from [the Noun Project](https://thenounproject.com/browse/?i=1630214). 9 | 10 | ## KDE Breeze 11 | The following icons originate from KDE Breeze. They are available under [GNU LGPL 3](http://www.gnu.org/licenses/). 12 | 13 | * ```open.svg``` from [document-open.svg](https://github.com/KDE/breeze-icons/blob/master/icons/actions/22/document-open.svg) 14 | * ```save.svg``` from [document-save.svg](https://github.com/KDE/breeze-icons/blob/master/icons/actions/22/document-save.svg) 15 | 16 | ## Material Design 17 | The following icons originate from [Material Design](https://material.io). They are available under [Apache license version 2.0](https://www.apache.org/licenses/LICENSE-2.0.html). They have been modified for their respective use and to fit the target display size. 18 | 19 | * ```auto-fit-points.svg``` from [open_with](https://material.io/resources/icons/?icon=open_with&style=outline) 20 | * ```shrink-to-points.svg``` from [open_with](https://material.io/resources/icons/?icon=open_with&style=outline) 21 | * ```commit-annotation.svg``` from [check](https://material.io/resources/icons/?icon=check&style=outline) 22 | * ```tags.svg``` from [label](https://material.io/resources/icons/?icon=label&style=outline) 23 | * ```labels.svg``` from [category](https://material.io/resources/icons/?icon=category&style=outline) 24 | * ```toggle-pause.svg``` from [pause](https://material.io/resources/icons/?icon=pause&style=outline) 25 | * ```undo.svg``` from [undo](https://material.io/resources/icons/?icon=undo&style=outline) 26 | * ```link.svg``` from [link](https://material.io/resources/icons/?icon=link&style=outline) 27 | -------------------------------------------------------------------------------- /icons/auto-fit-points.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 53 | 56 | 57 | 61 | 66 | 71 | 76 | 81 | 82 | -------------------------------------------------------------------------------- /icons/automations.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 51 | 54 | 55 | 59 | 63 | 64 | -------------------------------------------------------------------------------- /icons/classes/Annotated PointCloud2.svg: -------------------------------------------------------------------------------- 1 | New Annotation.svg -------------------------------------------------------------------------------- /icons/classes/New Annotation.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 19 | 21 | 28 | 29 | 50 | 52 | 53 | 55 | image/svg+xml 56 | 58 | 59 | 60 | 61 | 62 | 67 | 70 | 87 | 92 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /icons/commit-annotation.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 53 | 56 | 57 | 61 | 66 | 67 | -------------------------------------------------------------------------------- /icons/keyboard.svg: -------------------------------------------------------------------------------- 1 | 2 | image/svg+xml 67 | -------------------------------------------------------------------------------- /icons/labels.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 52 | 55 | 56 | 60 | 66 | 74 | 79 | 80 | -------------------------------------------------------------------------------- /icons/open.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 49 | 52 | 53 | 55 | 62 | 63 | 68 | 69 | -------------------------------------------------------------------------------- /icons/rotate-anti-clockwise.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 52 | 55 | 56 | 60 | 65 | 66 | -------------------------------------------------------------------------------- /icons/rotate-clockwise.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 52 | 55 | 56 | 60 | 65 | 66 | -------------------------------------------------------------------------------- /icons/save.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 50 | 53 | 54 | 56 | 63 | 64 | 72 | 73 | -------------------------------------------------------------------------------- /icons/shrink-to-points.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 54 | 57 | 58 | 62 | 67 | 72 | 77 | 82 | 83 | -------------------------------------------------------------------------------- /icons/tags.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 53 | 56 | 57 | 61 | 68 | 69 | -------------------------------------------------------------------------------- /icons/toggle-pause.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 54 | 57 | 58 | 62 | 67 | 68 | -------------------------------------------------------------------------------- /icons/undo.svg: -------------------------------------------------------------------------------- 1 | 2 | 17 | 19 | 20 | 22 | image/svg+xml 23 | 25 | 26 | 27 | 28 | 29 | 31 | 58 | 61 | 62 | 66 | 71 | 72 | -------------------------------------------------------------------------------- /include/annotate/annotate_display.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 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 "annotation_marker.h" 22 | #include "file_dialog_property.h" 23 | #include "shortcut_property.h" 24 | 25 | namespace annotate 26 | { 27 | class CloudDisplay; 28 | class MarkerDisplay; 29 | class TrackDisplay; 30 | 31 | class AnnotateDisplay : public rviz::DisplayGroup 32 | { 33 | Q_OBJECT 34 | public: 35 | AnnotateDisplay(); 36 | void onInitialize() override; 37 | void setTopic(const QString& topic, const QString& datatype) override; 38 | void load(const rviz::Config& config) override; 39 | void setCurrentMarker(AnnotationMarker* marker); 40 | 41 | bool save(); 42 | void publishTrackMarkers(); 43 | sensor_msgs::PointCloud2ConstPtr cloud() const; 44 | tf::TransformListener& transformListener(); 45 | 46 | bool shrinkAfterResize() const; 47 | bool shrinkBeforeCommit() const; 48 | bool autoFitAfterPointsChange() const; 49 | QStringList toolShortcuts() const; 50 | double preTime() const; 51 | double postTime() const; 52 | 53 | private Q_SLOTS: 54 | void updateTopic(); 55 | void updateLabels(); 56 | void updateTags(); 57 | void openFile(); 58 | void updateAnnotationFile(); 59 | void updatePadding(); 60 | void updateMargin(); 61 | void updateIgnoreGround(); 62 | void shrinkToPoints(); 63 | void autoFitPoints(); 64 | void undo(); 65 | void commit(); 66 | void rotateClockwise(); 67 | void rotateAntiClockwise(); 68 | void togglePlayPause(); 69 | void updateShortcuts(); 70 | 71 | private: 72 | enum PlaybackCommand 73 | { 74 | Play, 75 | Pause, 76 | Toggle 77 | }; 78 | 79 | template 80 | void modifyChild(rviz::Property* parent, QString const& name, std::function modifier); 81 | void adjustView(); 82 | bool load(std::string const& file); 83 | void createNewAnnotation(const geometry_msgs::PointStamped::ConstPtr& message); 84 | void handlePointcloud(const sensor_msgs::PointCloud2ConstPtr& cloud); 85 | void sendPlaybackCommand(PlaybackCommand command); 86 | 87 | ros::NodeHandle node_handle_; 88 | ros::Subscriber new_annotation_subscriber_; 89 | ros::Subscriber pointcloud_subscriber_; 90 | ros::Publisher track_marker_publisher_; 91 | std::shared_ptr server_; 92 | size_t current_marker_id_{ 0 }; 93 | std::vector markers_; 94 | std::vector labels_; 95 | std::vector tags_; 96 | std::string filename_; 97 | ros::Time time_; 98 | ros::Time last_track_publish_time_; 99 | sensor_msgs::PointCloud2ConstPtr cloud_; 100 | tf::TransformListener transform_listener_; 101 | bool ignore_ground_{ false }; 102 | rviz::RosTopicProperty* topic_property_{ nullptr }; 103 | rviz::BoolProperty* ignore_ground_property_{ nullptr }; 104 | rviz::StringProperty* labels_property_{ nullptr }; 105 | rviz::StringProperty* tags_property_{ nullptr }; 106 | FileDialogProperty* open_file_property_{ nullptr }; 107 | FileDialogProperty* annotation_file_property_{ nullptr }; 108 | rviz::Display* cloud_display_{ nullptr }; 109 | rviz::Display* marker_display_{ nullptr }; 110 | rviz::Display* track_display_{ nullptr }; 111 | AnnotationMarker* current_marker_{ nullptr }; 112 | rviz::BoolProperty* shortcuts_property_{ nullptr }; 113 | ros::ServiceClient playback_client_; 114 | rviz::BoolProperty* shrink_after_resize_{ nullptr }; 115 | rviz::BoolProperty* shrink_before_commit_{ nullptr }; 116 | rviz::BoolProperty* auto_fit_after_points_change_{ nullptr }; 117 | rviz::BoolProperty* play_after_commit_{ nullptr }; 118 | rviz::BoolProperty* pause_after_data_change_{ nullptr }; 119 | rviz::FloatProperty* padding_property_{ nullptr }; 120 | rviz::FloatProperty* margin_property_{ nullptr }; 121 | rviz::FloatProperty* pre_time_property_{ nullptr }; 122 | rviz::FloatProperty* post_time_property_{ nullptr }; 123 | }; 124 | } // namespace annotate 125 | -------------------------------------------------------------------------------- /include/annotate/annotate_tool.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace annotate 7 | { 8 | class AnnotateTool : public rviz::Tool 9 | { 10 | Q_OBJECT 11 | public: 12 | void onInitialize() override; 13 | void activate() override; 14 | void deactivate() override; 15 | int processMouseEvent(rviz::ViewportMouseEvent& event) override; 16 | void save(rviz::Config config) const override; 17 | 18 | private: 19 | ros::NodeHandle node_handle_; 20 | ros::Publisher publisher_; 21 | }; 22 | 23 | } // namespace annotate 24 | -------------------------------------------------------------------------------- /include/annotate/annotation_marker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "annotation_marker.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace annotate 18 | { 19 | void setRotation(geometry_msgs::Quaternion& quaternion, double x, double y, double z); 20 | 21 | struct TrackInstance 22 | { 23 | std::string label; 24 | std::vector tags; 25 | tf::StampedTransform center; 26 | tf::Vector3 box_size; 27 | ros::Duration time_offset; 28 | 29 | double timeTo(ros::Time const& time) const; 30 | }; 31 | 32 | using Track = std::vector; 33 | 34 | class AnnotateDisplay; 35 | 36 | class AnnotationMarker 37 | { 38 | public: 39 | using Ptr = std::shared_ptr; 40 | 41 | AnnotationMarker(AnnotateDisplay* markers, 42 | const std::shared_ptr& server, 43 | const TrackInstance& trackInstance, int marker_id); 44 | 45 | int id() const; 46 | Track const& track() const; 47 | void setTrack(const Track& track); 48 | void setPadding(double padding); 49 | void setMargin(double margin); 50 | void setIgnoreGround(bool enabled); 51 | void setLabels(const std::vector& labels); 52 | void setTags(const std::vector& tags); 53 | 54 | void setTime(const ros::Time& time); 55 | void shrinkToPoints(); 56 | void autoFit(); 57 | void undo(); 58 | void commit(); 59 | void rotateYaw(double delta_rad); 60 | 61 | private: 62 | using MenuHandler = interactive_markers::MenuHandler; 63 | 64 | enum Mode 65 | { 66 | Locked, 67 | Move, 68 | Rotate, 69 | Resize 70 | }; 71 | 72 | enum State 73 | { 74 | Hidden, 75 | New, 76 | Committed, 77 | Modified 78 | }; 79 | 80 | struct UndoState 81 | { 82 | QTime time; 83 | std::string undo_description; 84 | 85 | geometry_msgs::Pose pose; 86 | tf::Vector3 box_size; 87 | std::string label; 88 | std::vector tags; 89 | State state; 90 | }; 91 | 92 | struct PointContext 93 | { 94 | ros::Time time; 95 | ros::Duration time_offset; 96 | size_t points_inside{ 0u }; 97 | size_t points_nearby{ 0u }; 98 | tf::Vector3 minimum{ std::numeric_limits::max(), std::numeric_limits::max(), 99 | std::numeric_limits::max() }; 100 | tf::Vector3 maximum{ std::numeric_limits::min(), std::numeric_limits::min(), 101 | std::numeric_limits::min() }; 102 | }; 103 | 104 | void updateMenu(const PointContext& context); 105 | void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 106 | void nextMode(); 107 | void changeSize(const tf::Pose& new_pose); 108 | void lock(); 109 | void lock(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 110 | void enableResizeControl(); 111 | void enableResizeControl(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 112 | void enableMoveControl(); 113 | void enableMoveControl(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 114 | void enableRotationControl(); 115 | void enableRotationControl(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 116 | void createMarker(const TrackInstance& instance); 117 | void setLabel(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 118 | void setTag(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 119 | void commit(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 120 | void updateDescription(const PointContext& context); 121 | void updateState(State state); 122 | bool hasMoved(geometry_msgs::Pose const& a, geometry_msgs::Pose const& b) const; 123 | void saveMove(); 124 | void saveForUndo(const std::string& description); 125 | void undo(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 126 | void resize(double offset); 127 | PointContext analyzePoints() const; 128 | void shrinkTo(const PointContext& context); 129 | void shrink(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 130 | bool fitNearbyPoints(); 131 | void autoFit(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 132 | void pull(); 133 | void push(); 134 | void removeControls(); 135 | void createCubeControl(); 136 | void createMoveControl(); 137 | void createRotationControl(); 138 | void createResizeControl(); 139 | void setBoxSize(const tf::Vector3& box_size); 140 | tf::Vector3 boxSize() const; 141 | 142 | visualization_msgs::InteractiveMarker marker_; 143 | MenuHandler menu_handler_; 144 | std::shared_ptr server_; 145 | Mode mode_{ Move }; 146 | bool can_change_size_{ false }; 147 | tf::Pose last_pose_; 148 | bool button_click_active_{ false }; 149 | tf::Point last_mouse_point_; 150 | int id_{ -1 }; 151 | std::vector label_keys_; 152 | std::map labels_; 153 | std::string label_; 154 | std::vector tag_keys_; 155 | std::map tags_menu_; 156 | std::vector tags_; 157 | Track track_; 158 | AnnotateDisplay* annotate_display_; 159 | ros::Time time_; 160 | State state_{ Hidden }; 161 | std::stack undo_stack_; 162 | bool ignore_ground_{ false }; 163 | double padding_ { 0.05 }; 164 | double margin_ { 0.25 }; 165 | }; 166 | 167 | } // namespace annotate 168 | -------------------------------------------------------------------------------- /include/annotate/file_dialog_property.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "rviz/properties/line_edit_with_button.h" 5 | 6 | namespace annotate 7 | { 8 | class FileDialogProperty : public rviz::Property 9 | { 10 | Q_OBJECT 11 | public: 12 | enum Mode 13 | { 14 | ExistingDirectory, 15 | OpenFileName, 16 | SaveFileName 17 | }; 18 | 19 | FileDialogProperty(const QString& name = QString(), const QString& default_value = QString(), 20 | const QString& description = QString(), rviz::Property* parent = 0, const char* changed_slot = 0, 21 | QObject* receiver = 0); 22 | 23 | void setMode(Mode mode); 24 | QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option); 25 | 26 | private: 27 | Mode mode_{ OpenFileName }; 28 | }; 29 | 30 | class FileDialogEditor : public rviz::LineEditWithButton 31 | { 32 | Q_OBJECT 33 | public: 34 | FileDialogEditor(FileDialogProperty* property, QWidget* parent, FileDialogProperty::Mode mode); 35 | void onButtonClick() override; 36 | 37 | private: 38 | FileDialogProperty* property_; 39 | FileDialogProperty::Mode const mode_; 40 | }; 41 | 42 | } // namespace annotate 43 | -------------------------------------------------------------------------------- /include/annotate/shortcut_property.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace annotate 7 | { 8 | class AnnotateDisplay; 9 | 10 | class ShortcutProperty : public rviz::StringProperty 11 | { 12 | Q_OBJECT 13 | public: 14 | ShortcutProperty(const QString& name = QString(), const QString& default_value = QString(), 15 | const QString& description = QString(), rviz::Property* parent = 0, const char* changed_slot = 0, 16 | QObject* receiver = 0); 17 | 18 | /** 19 | * Install a shortcut in target that calls trigger_slot in receiver. The shortcut is derived from getName(), 20 | * which should be a shortcut string that Qt understands (cf QKeySequence). If a shortcut is invalid or used 21 | * already, a warning status is set in display. 22 | */ 23 | void createShortcut(AnnotateDisplay* display, QWidget* target, QObject* receiver, const char* trigger_slot); 24 | void setEnabled(bool enabled); 25 | 26 | private Q_SLOTS: 27 | void updateShortcut(); 28 | void handleAmbiguousShortcut(); 29 | 30 | private: 31 | QString statusName() const; 32 | QShortcut* shortcut_{ nullptr }; 33 | AnnotateDisplay* display_{ nullptr }; 34 | }; 35 | 36 | } // namespace annotate 37 | -------------------------------------------------------------------------------- /launch/annotate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Base Link1/Frames1 8 | - /Base Link1/Tree1 9 | - /Annotated PointCloud21 10 | - /Annotated PointCloud21/Settings1 11 | - /Annotated PointCloud21/Linked Actions1 12 | - /Annotated PointCloud21/Keyboard Shortcuts1 13 | - /Annotated PointCloud21/Point Cloud1/Autocompute Value Bounds1 14 | Splitter Ratio: 0.6272493600845337 15 | Tree Height: 511 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: ~ 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5724906921386719 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 2 31 | SyncSource: Point Cloud 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Class: rviz/TF 40 | Enabled: false 41 | Frame Timeout: 15 42 | Frames: 43 | All Enabled: false 44 | Marker Scale: 1 45 | Name: Base Link 46 | Show Arrows: false 47 | Show Axes: true 48 | Show Names: true 49 | Tree: 50 | {} 51 | Update Interval: 0 52 | Value: false 53 | - Alpha: 0.5 54 | Cell Size: 1 55 | Class: rviz/Grid 56 | Color: 160; 160; 164 57 | Enabled: true 58 | Line Style: 59 | Line Width: 0.029999999329447746 60 | Value: Lines 61 | Name: Grid 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: 0 67 | Plane: XY 68 | Plane Cell Count: 1000 69 | Reference Frame: 70 | Value: true 71 | - Class: annotate/Annotated PointCloud2 72 | Displays: 73 | - Alpha: 1 74 | Autocompute Intensity Bounds: true 75 | Autocompute Value Bounds: 76 | Max Value: 4.2914934158325195 77 | Min Value: -1 78 | Value: false 79 | Axis: Z 80 | Channel Name: intensity 81 | Class: rviz/PointCloud2 82 | Color: 255; 255; 255 83 | Color Transformer: AxisColor 84 | Decay Time: 0 85 | Enabled: true 86 | Invert Rainbow: false 87 | Max Color: 255; 255; 255 88 | Max Intensity: 0.9900000095367432 89 | Min Color: 0; 0; 0 90 | Min Intensity: 0 91 | Name: Point Cloud 92 | Position Transformer: XYZ 93 | Queue Size: 10 94 | Selectable: true 95 | Size (Pixels): 3 96 | Size (m): 0.03999999910593033 97 | Style: Spheres 98 | Topic: /velodyne_points 99 | Unreliable: false 100 | Use Fixed Frame: true 101 | Use rainbow: true 102 | Value: true 103 | - Class: rviz/InteractiveMarkers 104 | Enable Transparency: true 105 | Enabled: true 106 | Name: Annotations 107 | Show Axes: true 108 | Show Descriptions: true 109 | Show Visual Aids: false 110 | Update Topic: /annotate_node/update 111 | Value: true 112 | - Class: rviz/MarkerArray 113 | Enabled: true 114 | Marker Topic: /tracks 115 | Name: Tracks 116 | Namespaces: 117 | Path: true 118 | Positions: true 119 | Queue Size: 100 120 | Value: true 121 | Enabled: true 122 | Keyboard Shortcuts: 123 | Value: true 124 | auto fit points: Ctrl+F 125 | commit annotation: Ctrl+C 126 | rotate anti-clockwise: left 127 | rotate clockwise: right 128 | shrink to points: Ctrl+B 129 | toggle pause: space 130 | undo: Ctrl+Z 131 | Linked Actions: 132 | Auto-fit after points change: true 133 | Pause playback after points change: false 134 | Resume playback after commit: false 135 | Shrink after resize: true 136 | Shrink before commit: true 137 | Name: Annotated PointCloud2 138 | Open: "" 139 | Settings: 140 | Annotation File: /workspace/kitti/annotate3.yaml 141 | Ignore Ground: false 142 | Labels: person, car, bicycle 143 | Margin: 0.25 144 | Padding: 0.05000000074505806 145 | Tags: moving, standing 146 | Topic: /velodyne_points 147 | Value: true 148 | Enabled: true 149 | Global Options: 150 | Background Color: 48; 48; 48 151 | Default Light: true 152 | Fixed Frame: world 153 | Frame Rate: 30 154 | Name: root 155 | Tools: 156 | - Class: rviz/Interact 157 | Hide Inactive Objects: true 158 | - Class: rviz/MoveCamera 159 | - Class: rviz/Select 160 | - Class: rviz/FocusCamera 161 | - Class: rviz/Measure 162 | - Class: annotate/New Annotation 163 | Value: true 164 | Views: 165 | Current: 166 | Class: rviz/ThirdPersonFollower 167 | Distance: 9.62160587310791 168 | Enable Stereo Rendering: 169 | Stereo Eye Separation: 0.05999999865889549 170 | Stereo Focal Distance: 1 171 | Swap Stereo Eyes: false 172 | Value: false 173 | Focal Point: 174 | X: 19.214397430419922 175 | Y: 3.8923299312591553 176 | Z: 3.0994415283203125e-6 177 | Focal Shape Fixed Size: false 178 | Focal Shape Size: 0.05000000074505806 179 | Invert Z Axis: false 180 | Name: Current View 181 | Near Clip Distance: 0.009999999776482582 182 | Pitch: 0.5403982996940613 183 | Target Frame: world 184 | Value: ThirdPersonFollower (rviz) 185 | Yaw: 0.3472052812576294 186 | Saved: 187 | - Angle: 2.2950024604797363 188 | Class: rviz/TopDownOrtho 189 | Enable Stereo Rendering: 190 | Stereo Eye Separation: 0.05999999865889549 191 | Stereo Focal Distance: 1 192 | Swap Stereo Eyes: false 193 | Value: false 194 | Invert Z Axis: false 195 | Name: Birds Eye 196 | Near Clip Distance: 0.009999999776482582 197 | Scale: 164.28306579589844 198 | Target Frame: current_annotation 199 | Value: TopDownOrtho (rviz) 200 | X: 0.3209964632987976 201 | Y: 0.13343486189842224 202 | - Class: rviz/ThirdPersonFollower 203 | Distance: 10.419363021850586 204 | Enable Stereo Rendering: 205 | Stereo Eye Separation: 0.05999999865889549 206 | Stereo Focal Distance: 1 207 | Swap Stereo Eyes: false 208 | Value: false 209 | Focal Point: 210 | X: 1.4881457090377808 211 | Y: 1.0724236965179443 212 | Z: 0 213 | Focal Shape Fixed Size: false 214 | Focal Shape Size: 0.05000000074505806 215 | Invert Z Axis: false 216 | Name: 3D 217 | Near Clip Distance: 0.009999999776482582 218 | Pitch: 0.3966536521911621 219 | Target Frame: current_annotation 220 | Value: ThirdPersonFollower (rviz) 221 | Yaw: 0.7090426087379456 222 | Window Geometry: 223 | Displays: 224 | collapsed: false 225 | Height: 656 226 | Hide Left Dock: false 227 | Hide Right Dock: false 228 | QMainWindow State: 000000ff00000000fd0000000400000000000001870000023afc0200000008fb0000001200530065006c0065006300740069006f006e000000003b0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000023a000000c700fffffffb0000000a005600690065007700730000000223000000b5000000a000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dcfc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003b000000af0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000270000001a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004cf0000004ffc0100000002fb0000000800540069006d00650000000000000004cf0000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000002880000023a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 229 | Selection: 230 | collapsed: false 231 | Time: 232 | collapsed: false 233 | Tool Properties: 234 | collapsed: false 235 | Views: 236 | collapsed: false 237 | Width: 1045 238 | X: 319 239 | Y: 260 240 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | annotate 4 | 1.0.0 5 | Create 3D labelled bounding boxes in RViz 6 | 7 | Dennis Nienhüser 8 | Dennis Nienhüser 9 | BSD-3 10 | https://github.com/Earthwings/annotate 11 | 12 | catkin 13 | qtbase5-dev 14 | libqt5-core 15 | libqt5-gui 16 | libqt5-widgets 17 | rviz 18 | geometry_msgs 19 | interactive_markers 20 | roscpp 21 | sensor_msgs 22 | visualization_msgs 23 | tf 24 | yaml-cpp 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Create annotated objects in point clouds using labels and bounding boxes. 7 | 8 | 9 | 12 | 13 | Create annotated objects in point clouds using labels and bounding boxes. 14 | 15 | sensor_msgs/PointCloud2 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/annotate_display.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace visualization_msgs; 19 | using namespace interactive_markers; 20 | using namespace tf; 21 | using namespace std; 22 | 23 | namespace annotate 24 | { 25 | namespace internal 26 | { 27 | std_msgs::ColorRGBA createColor(int id) 28 | { 29 | // Cache color such that subsequent calls return the same one 30 | static map colors; 31 | auto const iter = colors.find(id); 32 | if (iter != colors.end()) 33 | { 34 | return iter->second; 35 | } 36 | 37 | // Reverse the ID bits to spread them far over the hue range of 0..360 38 | // 0 => 0, 1 => 180, 2 => 90, 3 => 270, ... 39 | uchar h = uchar(id); 40 | h = (h & 0xF0) >> 4 | (h & 0x0F) << 4; 41 | h = (h & 0xCC) >> 2 | (h & 0x33) << 2; 42 | h = (h & 0xAA) >> 1 | (h & 0x55) << 1; 43 | int const hue = int(h * 360.0 / 256); 44 | 45 | // Vary saturation and value slightly 46 | random_device rd; 47 | mt19937 mt(rd()); 48 | uniform_int_distribution dist(210, 240); 49 | QColor color; 50 | color.setHsv(hue, dist(mt), dist(mt)); 51 | 52 | std_msgs::ColorRGBA result; 53 | qreal r, g, b; 54 | color.getRgbF(&r, &g, &b); 55 | result.r = r; 56 | result.g = g; 57 | result.b = b; 58 | colors[id] = result; 59 | return result; 60 | } 61 | 62 | Marker createTrackLine(float scale, const std_msgs::ColorRGBA& color) 63 | { 64 | Marker marker; 65 | marker.type = Marker::LINE_STRIP; 66 | setRotation(marker.pose.orientation, 0.0, 0.0, 0.0); 67 | marker.color = color; 68 | marker.color.a = 0.7; 69 | marker.scale.x = scale; 70 | return marker; 71 | } 72 | 73 | Marker createTrackSpheres(float scale, const std_msgs::ColorRGBA& color) 74 | { 75 | Marker marker; 76 | marker.type = Marker::SPHERE_LIST; 77 | setRotation(marker.pose.orientation, 0.0, 0.0, 0.0); 78 | marker.color = color; 79 | marker.color.a = 0.7; 80 | marker.scale.x = scale; 81 | marker.scale.y = scale; 82 | marker.scale.z = scale; 83 | return marker; 84 | } 85 | 86 | } // namespace internal 87 | 88 | void AnnotateDisplay::createNewAnnotation(const geometry_msgs::PointStamped::ConstPtr& message) 89 | { 90 | Transform transform; 91 | transform.setOrigin({ message->point.x, message->point.y, message->point.z }); 92 | TrackInstance instance; 93 | instance.center = StampedTransform(transform, time_, message->header.frame_id, "current_annotation"); 94 | instance.label = "unknown"; 95 | instance.box_size.setValue(1.0, 1.0, 1.0); 96 | ++current_marker_id_; 97 | auto marker = make_shared(this, server_, instance, current_marker_id_); 98 | marker->setLabels(labels_); 99 | marker->setTags(tags_); 100 | marker->setPadding(padding_property_->getFloat()); 101 | marker->setMargin(margin_property_->getFloat()); 102 | marker->setIgnoreGround(ignore_ground_property_->getBool()); 103 | marker->setTime(time_); 104 | markers_.push_back(marker); 105 | } 106 | 107 | void AnnotateDisplay::handlePointcloud(const sensor_msgs::PointCloud2ConstPtr& cloud) 108 | { 109 | cloud_ = cloud; 110 | time_ = cloud->header.stamp; 111 | if (pause_after_data_change_->getBool()) 112 | { 113 | sendPlaybackCommand(Pause); 114 | } 115 | for (auto& marker : markers_) 116 | { 117 | marker->setTime(time_); 118 | } 119 | publishTrackMarkers(); 120 | } 121 | 122 | AnnotateDisplay::AnnotateDisplay() 123 | { 124 | server_ = make_shared("annotate_node", "", false); 125 | track_marker_publisher_ = node_handle_.advertise("tracks", 10, true); 126 | new_annotation_subscriber_ = 127 | node_handle_.subscribe("/new_annotation", 10, &AnnotateDisplay::createNewAnnotation, this); 128 | } 129 | 130 | template 131 | void AnnotateDisplay::modifyChild(rviz::Property* parent, QString const& name, std::function modifier) 132 | { 133 | for (int i = 0; i < parent->numChildren(); ++i) 134 | { 135 | auto* property = dynamic_cast(parent->childAt(i)); 136 | if (property && (name.isEmpty() || property->getName() == name)) 137 | { 138 | modifier(property); 139 | } 140 | } 141 | } 142 | 143 | void AnnotateDisplay::shrinkToPoints() 144 | { 145 | if (current_marker_) 146 | { 147 | current_marker_->shrinkToPoints(); 148 | } 149 | } 150 | 151 | void AnnotateDisplay::autoFitPoints() 152 | { 153 | if (current_marker_) 154 | { 155 | current_marker_->autoFit(); 156 | } 157 | } 158 | 159 | void AnnotateDisplay::undo() 160 | { 161 | if (current_marker_) 162 | { 163 | current_marker_->undo(); 164 | } 165 | } 166 | 167 | void AnnotateDisplay::commit() 168 | { 169 | if (current_marker_) 170 | { 171 | current_marker_->commit(); 172 | if (play_after_commit_->getBool()) 173 | { 174 | sendPlaybackCommand(Play); 175 | } 176 | } 177 | } 178 | 179 | void AnnotateDisplay::rotateClockwise() 180 | { 181 | if (current_marker_) 182 | { 183 | current_marker_->rotateYaw(-0.01); 184 | } 185 | } 186 | 187 | void AnnotateDisplay::rotateAntiClockwise() 188 | { 189 | if (current_marker_) 190 | { 191 | current_marker_->rotateYaw(0.01); 192 | } 193 | } 194 | 195 | void AnnotateDisplay::sendPlaybackCommand(PlaybackCommand command) 196 | { 197 | if (!playback_client_.exists() || !playback_client_.isValid()) 198 | { 199 | QStringList services; 200 | XmlRpc::XmlRpcValue args, result, payload; 201 | args[0] = "/annotate"; 202 | 203 | if (!ros::master::execute("getSystemState", args, result, payload, true)) 204 | { 205 | return; 206 | } 207 | 208 | for (int i = 0; i < payload.size(); ++i) 209 | { 210 | for (int j = 0; j < payload[i].size(); ++j) 211 | { 212 | XmlRpc::XmlRpcValue val = payload[i][j]; 213 | if (val.size() > 0) 214 | { 215 | string const ending = "/pause_playback"; 216 | string const value = val[0]; 217 | auto const v = value.length(); 218 | auto const e = ending.length(); 219 | bool const ends_with = v >= e && 0 == value.compare(v - e, e, ending); 220 | if (ends_with) 221 | { 222 | services.push_back(QString::fromStdString(value)); 223 | } 224 | } 225 | } 226 | } 227 | 228 | QString const status_name = "rosbag play service"; 229 | if (services.empty()) 230 | { 231 | setStatus(rviz::StatusProperty::Warn, status_name, 232 | "Found no pause_playback ROS service to toggle playback. Maybe 'rosbag play' is not running?"); 233 | } 234 | else if (services.size() == 1) 235 | { 236 | ros::NodeHandle node_handle; 237 | playback_client_ = node_handle.serviceClient(services.front().toStdString()); 238 | deleteStatus(status_name); 239 | } 240 | else 241 | { 242 | QString message = "Found multiple pause_playback ROS services to toggle playback: %1"; 243 | setStatus(rviz::StatusProperty::Warn, status_name, message.arg(services.join(", "))); 244 | } 245 | } 246 | 247 | if (playback_client_.isValid()) 248 | { 249 | std_srvs::SetBool value; 250 | value.request.data = command == Toggle || command == Pause; 251 | if (playback_client_.call(value)) 252 | { 253 | if (command == Toggle && !value.response.success) 254 | { 255 | value.request.data = !value.request.data; 256 | if (playback_client_.call(value)) 257 | { 258 | if (!value.response.success) 259 | { 260 | ROS_WARN_STREAM("Play/pause toggle failed."); 261 | } 262 | } 263 | } 264 | } 265 | else 266 | { 267 | playback_client_.shutdown(); 268 | } 269 | } 270 | } 271 | 272 | void AnnotateDisplay::togglePlayPause() 273 | { 274 | sendPlaybackCommand(Toggle); 275 | } 276 | 277 | void AnnotateDisplay::updateShortcuts() 278 | { 279 | auto const enabled = shortcuts_property_->getBool(); 280 | for (int i = 0; i < shortcuts_property_->numChildren(); ++i) 281 | { 282 | auto* child = shortcuts_property_->childAt(i); 283 | ShortcutProperty* property = qobject_cast(child); 284 | if (property) 285 | { 286 | property->setEnabled(enabled); 287 | } 288 | } 289 | } 290 | 291 | void AnnotateDisplay::onInitialize() 292 | { 293 | cloud_display_ = createDisplay("rviz/PointCloud2"); 294 | addDisplay(cloud_display_); 295 | cloud_display_->initialize(context_); 296 | cloud_display_->setName("Point Cloud"); 297 | cloud_display_->setEnabled(true); 298 | 299 | marker_display_ = createDisplay("rviz/InteractiveMarkers"); 300 | addDisplay(marker_display_); 301 | marker_display_->initialize(context_); 302 | marker_display_->setName("Annotations"); 303 | marker_display_->setTopic("/annotate_node/update", "visualization_msgs/InteractiveMarkerUpdate"); 304 | marker_display_->setEnabled(true); 305 | modifyChild(marker_display_, "Show Axes", 306 | [](rviz::BoolProperty* property) { property->setBool(true); }); 307 | 308 | track_display_ = createDisplay("rviz/MarkerArray"); 309 | addDisplay(track_display_); 310 | track_display_->initialize(context_); 311 | track_display_->setName("Tracks"); 312 | track_display_->setTopic("/tracks", "visualization_msgs/MarkerArray"); 313 | track_display_->setEnabled(true); 314 | 315 | open_file_property_ = new FileDialogProperty("Open", QString(), "Open an existing annotation file for editing", this, 316 | SLOT(openFile()), this); 317 | open_file_property_->setIcon(rviz::loadPixmap(QString("package://annotate/icons/open.svg"))); 318 | open_file_property_->setMode(FileDialogProperty::OpenFileName); 319 | 320 | auto* settings = new rviz::Property("Settings", QVariant(), "Configure annotation properties", this); 321 | topic_property_ = new rviz::RosTopicProperty("Topic", QString(), "sensor_msgs/PointCloud2", "Point cloud to annotate", 322 | settings, SLOT(updateTopic()), this); 323 | annotation_file_property_ = new FileDialogProperty("Annotation File", "annotate.yaml", "Annotation storage file", 324 | settings, SLOT(updateAnnotationFile()), this); 325 | annotation_file_property_->setIcon(rviz::loadPixmap(QString("package://annotate/icons/save.svg"))); 326 | annotation_file_property_->setMode(FileDialogProperty::SaveFileName); 327 | labels_property_ = new rviz::StringProperty("Labels", "object, unknown", "Available labels (separated by comma)", 328 | settings, SLOT(updateLabels()), this); 329 | labels_property_->setIcon(rviz::loadPixmap(QString("package://annotate/icons/labels.svg"))); 330 | tags_property_ = new rviz::StringProperty("Tags", "easy, moderate, hard", "Available tags (separated by comma)", 331 | settings, SLOT(updateTags()), this); 332 | tags_property_->setIcon(rviz::loadPixmap(QString("package://annotate/icons/tags.svg"))); 333 | padding_property_ = new rviz::FloatProperty("Padding", 0.05, 334 | "Minimum distance between annotation bounding box and inner points for " 335 | "actions " 336 | "like shrink to points and auto-fit points.", 337 | settings, SLOT(updatePadding()), this); 338 | padding_property_->setMin(0.0f); 339 | margin_property_ = new rviz::FloatProperty("Margin", 0.25, 340 | "Maximum distance between annotation bounding box and outer points to be " 341 | "considered nearby.", 342 | settings, SLOT(updateMargin()), this); 343 | margin_property_->setMin(0.0f); 344 | pre_time_property_ = new rviz::FloatProperty( 345 | "Pre Time", 1.0, "Seconds to show annotation marker before first track instance", settings); 346 | pre_time_property_->setMin(0.0f); 347 | post_time_property_ = new rviz::FloatProperty( 348 | "Post Time", 1.0, "Seconds to show annotation marker after last track instance", settings); 349 | post_time_property_->setMin(0.0f); 350 | 351 | ignore_ground_property_ = new rviz::BoolProperty("Ignore Ground", false, 352 | "Enable to ignore the ground direction (negative z) when " 353 | "shrinking " 354 | "or fitting boxes. This is useful if the point cloud contains " 355 | "ground points that should not be included in annotations.", 356 | settings, SLOT(updateIgnoreGround()), this); 357 | 358 | auto* automations = 359 | new rviz::Property("Linked Actions", QVariant(), "Configure the interaction of related actions.", this); 360 | automations->setIcon(rviz::loadPixmap("package://annotate/icons/automations.svg")); 361 | shrink_after_resize_ = new rviz::BoolProperty("Shrink after resize", false, 362 | "Shrink annotation box to fit points after adjusting its size with the " 363 | "handles.", 364 | automations); 365 | shrink_before_commit_ = new rviz::BoolProperty( 366 | "Shrink before commit", true, "Shrink annotation box to fit points when committing an annotation.", automations); 367 | auto_fit_after_points_change_ = new rviz::BoolProperty( 368 | "Auto-fit after points change", false, "Auto-fit annotation boxes when the point cloud changes.", automations); 369 | pause_after_data_change_ = new rviz::BoolProperty("Pause playback after points change", false, 370 | "Pause playback when the point cloud changes.", automations); 371 | play_after_commit_ = new rviz::BoolProperty("Resume playback after commit", false, 372 | "Resume playback after committing an annotation", automations); 373 | 374 | auto* render_panel = context_->getViewManager()->getRenderPanel(); 375 | shortcuts_property_ = 376 | new BoolProperty("Keyboard Shortcuts", true, "Keyboard shortcuts that affect the currently selected annotation", 377 | this, SLOT(updateShortcuts()), this); 378 | shortcuts_property_->setDisableChildrenIfFalse(true); 379 | QIcon icon = rviz::loadPixmap("package://annotate/icons/keyboard.svg"); 380 | shortcuts_property_->setIcon(icon); 381 | 382 | auto* undo = new ShortcutProperty("undo", "Ctrl+Z", "Undo last action", shortcuts_property_); 383 | undo->createShortcut(this, render_panel, this, SLOT(undo())); 384 | 385 | auto* play_pause = 386 | new ShortcutProperty("toggle pause", "space", "Toggle play and pause state of rosbag play", shortcuts_property_); 387 | play_pause->createShortcut(this, render_panel, this, SLOT(togglePlayPause())); 388 | 389 | auto* rotate_clockwise_ = 390 | new ShortcutProperty("rotate clockwise", "right", "Rotate the current annotation clockwise", shortcuts_property_); 391 | rotate_clockwise_->createShortcut(this, render_panel, this, SLOT(rotateClockwise())); 392 | auto* rotate_anti_clockwise_ = new ShortcutProperty( 393 | "rotate anti-clockwise", "left", "Rotate the current annotation anti-clockwise", shortcuts_property_); 394 | rotate_anti_clockwise_->createShortcut(this, render_panel, this, SLOT(rotateAntiClockwise())); 395 | 396 | auto* shrink = 397 | new ShortcutProperty("shrink to points", "Ctrl+B", "Shrink annotation box to fit points", shortcuts_property_); 398 | shrink->createShortcut(this, render_panel, this, SLOT(shrinkToPoints())); 399 | auto* auto_fit = 400 | new ShortcutProperty("auto fit points", "Ctrl+F", "Auto-fit annotation to nearby points", shortcuts_property_); 401 | auto_fit->createShortcut(this, render_panel, this, SLOT(autoFitPoints())); 402 | auto* commit = 403 | new ShortcutProperty("commit annotation", "Ctrl+C", "Commit current annotation and save", shortcuts_property_); 404 | commit->createShortcut(this, render_panel, this, SLOT(commit())); 405 | 406 | adjustView(); 407 | expand(); 408 | } 409 | 410 | QStringList AnnotateDisplay::toolShortcuts() const 411 | { 412 | QStringList shortcuts; 413 | auto* tool_manager = context_->getToolManager(); 414 | if (tool_manager) 415 | { 416 | for (int i = 0; i < tool_manager->numTools(); ++i) 417 | { 418 | auto* tool = tool_manager->getTool(i); 419 | shortcuts << QString(tool->getShortcutKey()); 420 | } 421 | } 422 | return shortcuts; 423 | } 424 | 425 | void AnnotateDisplay::load(const rviz::Config& config) 426 | { 427 | /* 428 | Display::load(config); 429 | 430 | auto const display_list_config = config.mapGetChild("Displays"); 431 | for (int i = 0; i < display_list_config.listLength(); ++i) 432 | { 433 | auto const display_config = display_list_config.listChildAt(i); 434 | QString display_class = "(no class name found)"; 435 | display_config.mapGetString("Class", &display_class); 436 | QString display_name; 437 | display_config.mapGetString("Name", &display_name); 438 | if (display_class == "rviz/InteractiveMarkers") 439 | { 440 | marker_display_->load(display_config); 441 | marker_display_->setObjectName(display_name); 442 | } 443 | else if (display_class == "rviz/MarkerArray") 444 | { 445 | track_display_->load(display_config); 446 | track_display_->setObjectName(display_name); 447 | } 448 | else if (display_class == "rviz/PointCloud2") 449 | { 450 | cloud_display_->load(display_config); 451 | cloud_display_->setObjectName(display_name); 452 | } 453 | } 454 | adjustView(); 455 | */ 456 | 457 | cloud_display_ = nullptr; 458 | track_display_ = nullptr; 459 | marker_display_ = nullptr; 460 | DisplayGroup::load(config); 461 | modifyChild(this, QString(), 462 | [this](rviz::PointCloud2Display* property) { cloud_display_ = property; }); 463 | modifyChild(this, QString(), 464 | [this](rviz::MarkerArrayDisplay* property) { track_display_ = property; }); 465 | modifyChild( 466 | this, QString(), [this](rviz::InteractiveMarkerDisplay* property) { marker_display_ = property; }); 467 | adjustView(); 468 | } 469 | 470 | void AnnotateDisplay::adjustView() 471 | { 472 | for (auto display : { cloud_display_, marker_display_, track_display_ }) 473 | { 474 | modifyChild(display, QString(), 475 | [](rviz::RosTopicProperty* property) { property->setHidden(true); }); 476 | } 477 | } 478 | 479 | void AnnotateDisplay::setTopic(const QString& topic, const QString& datatype) 480 | { 481 | if (topic_property_) 482 | { 483 | topic_property_->setString(topic); 484 | } 485 | pointcloud_subscriber_ = node_handle_.subscribe(topic.toStdString(), 10, &AnnotateDisplay::handlePointcloud, this); 486 | if (cloud_display_) 487 | { 488 | cloud_display_->setTopic(topic, datatype); 489 | } 490 | } 491 | 492 | void AnnotateDisplay::updateTopic() 493 | { 494 | if (topic_property_) 495 | { 496 | setTopic(topic_property_->getTopic(), topic_property_->getMessageType()); 497 | } 498 | } 499 | 500 | void AnnotateDisplay::updateLabels() 501 | { 502 | labels_.clear(); 503 | auto const labels = labels_property_->getString().split(',', QString::SkipEmptyParts); 504 | for (auto const& label : labels) 505 | { 506 | labels_.push_back(label.trimmed().toStdString()); 507 | } 508 | for (auto const& marker : markers_) 509 | { 510 | marker->setLabels(labels_); 511 | } 512 | } 513 | 514 | void AnnotateDisplay::updateTags() 515 | { 516 | tags_.clear(); 517 | auto const tags = tags_property_->getString().split(',', QString::SkipEmptyParts); 518 | for (auto const& tag : tags) 519 | { 520 | tags_.push_back(tag.trimmed().toStdString()); 521 | } 522 | for (auto const& marker : markers_) 523 | { 524 | marker->setTags(tags_); 525 | } 526 | } 527 | 528 | void AnnotateDisplay::openFile() 529 | { 530 | auto const file = open_file_property_->getValue().toString(); 531 | if (!file.isEmpty()) 532 | { 533 | server_->clear(); 534 | server_->applyChanges(); 535 | markers_.clear(); 536 | open_file_property_->setValue(QString()); 537 | if (load(file.toStdString())) 538 | { 539 | annotation_file_property_->blockSignals(true); 540 | annotation_file_property_->setValue(file); 541 | filename_ = file.toStdString(); 542 | annotation_file_property_->blockSignals(false); 543 | } 544 | } 545 | } 546 | 547 | void AnnotateDisplay::updateAnnotationFile() 548 | { 549 | auto const file = annotation_file_property_->getValue().toString(); 550 | if (filename_.empty() && QFileInfo::exists(file)) 551 | { 552 | if (load(file.toStdString())) 553 | { 554 | filename_ = file.toStdString(); 555 | } 556 | } 557 | else 558 | { 559 | filename_ = file.toStdString(); 560 | save(); 561 | } 562 | } 563 | 564 | void AnnotateDisplay::updatePadding() 565 | { 566 | auto const padding = padding_property_->getFloat(); 567 | for (auto const& marker : markers_) 568 | { 569 | marker->setPadding(padding); 570 | } 571 | } 572 | 573 | void AnnotateDisplay::updateMargin() 574 | { 575 | auto const margin = margin_property_->getFloat(); 576 | for (auto const& marker : markers_) 577 | { 578 | marker->setMargin(margin); 579 | } 580 | } 581 | 582 | double AnnotateDisplay::preTime() const 583 | { 584 | return pre_time_property_->getFloat(); 585 | } 586 | 587 | double AnnotateDisplay::postTime() const 588 | { 589 | return post_time_property_->getFloat(); 590 | } 591 | 592 | void AnnotateDisplay::updateIgnoreGround() 593 | { 594 | auto const ignore_ground = ignore_ground_property_->getBool(); 595 | for (auto const& marker : markers_) 596 | { 597 | marker->setIgnoreGround(ignore_ground); 598 | } 599 | } 600 | 601 | bool AnnotateDisplay::load(string const& file) 602 | { 603 | using namespace YAML; 604 | Node node; 605 | try 606 | { 607 | node = LoadFile(file); 608 | } 609 | catch (Exception const& e) 610 | { 611 | stringstream stream; 612 | stream << "Failed to open " << file << ": " << e.msg; 613 | ROS_DEBUG_STREAM(stream.str()); 614 | setStatusStd(rviz::StatusProperty::Error, "Annotation File", stream.str()); 615 | return false; 616 | } 617 | labels_.clear(); 618 | Node labels = node["labels"]; 619 | string joined_labels; 620 | for (size_t i = 0; i < labels.size(); ++i) 621 | { 622 | auto const value = labels[i].as(); 623 | labels_.push_back(value); 624 | if (joined_labels.empty()) 625 | { 626 | joined_labels = value; 627 | } 628 | else 629 | { 630 | joined_labels += ", " + value; 631 | } 632 | } 633 | labels_property_->setStdString(joined_labels); 634 | tags_.clear(); 635 | Node tags = node["tags"]; 636 | string joined_tags; 637 | for (size_t i = 0; i < tags.size(); ++i) 638 | { 639 | auto const value = tags[i].as(); 640 | tags_.push_back(value); 641 | if (joined_tags.empty()) 642 | { 643 | joined_tags = value; 644 | } 645 | else 646 | { 647 | joined_tags += ", " + value; 648 | } 649 | } 650 | tags_property_->setStdString(joined_tags); 651 | Node tracks = node["tracks"]; 652 | size_t annotations = 0; 653 | for (size_t i = 0; i < tracks.size(); ++i) 654 | { 655 | Track track; 656 | Node annotation = tracks[i]; 657 | auto const id = annotation["id"].as(); 658 | Node t = annotation["track"]; 659 | for (size_t j = 0; j < t.size(); ++j) 660 | { 661 | Node inst = t[j]; 662 | TrackInstance instance; 663 | instance.label = inst["label"].as(); 664 | if (inst["tags"]) 665 | { 666 | for (auto const& tag : inst["tags"].as>()) 667 | { 668 | instance.tags.push_back(tag); 669 | } 670 | } 671 | 672 | Node header = inst["header"]; 673 | instance.center.frame_id_ = header["frame_id"].as(); 674 | instance.center.stamp_.sec = header["stamp"]["secs"].as(); 675 | instance.center.stamp_.nsec = header["stamp"]["nsecs"].as(); 676 | 677 | if (inst["time_offset"]) 678 | { 679 | Node time_offset = inst["time_offset"]; 680 | instance.time_offset.sec = time_offset["secs"].as(); 681 | instance.time_offset.nsec = time_offset["nsecs"].as(); 682 | } 683 | 684 | Node origin = inst["translation"]; 685 | instance.center.setOrigin({ origin["x"].as(), origin["y"].as(), origin["z"].as() }); 686 | Node rotation = inst["rotation"]; 687 | instance.center.setRotation({ rotation["x"].as(), rotation["y"].as(), rotation["z"].as(), 688 | rotation["w"].as() }); 689 | 690 | Node box = inst["box"]; 691 | instance.box_size.setX(box["length"].as()); 692 | instance.box_size.setY(box["width"].as()); 693 | instance.box_size.setZ(box["height"].as()); 694 | 695 | track.push_back(instance); 696 | ++annotations; 697 | } 698 | 699 | if (!track.empty()) 700 | { 701 | current_marker_id_ = max(current_marker_id_, id); 702 | auto marker = make_shared(this, server_, track.front(), id); 703 | marker->setLabels(labels_); 704 | marker->setTags(tags_); 705 | marker->setTrack(track); 706 | auto const padding = padding_property_ ? padding_property_->getFloat() : 0.05f; 707 | marker->setPadding(padding); 708 | auto const margin = margin_property_ ? margin_property_->getFloat() : 0.25f; 709 | marker->setMargin(margin); 710 | bool ignore_ground = ignore_ground_property_ ? ignore_ground_property_->getBool() : false; 711 | marker->setIgnoreGround(ignore_ground); 712 | marker->setTime(time_); 713 | markers_.push_back(marker); 714 | } 715 | } 716 | stringstream stream; 717 | stream << "Loaded " << markers_.size() << " tracks with " << annotations << " annotations"; 718 | setStatusStd(rviz::StatusProperty::Ok, "Annotation File", stream.str()); 719 | publishTrackMarkers(); 720 | return true; 721 | } 722 | 723 | bool AnnotateDisplay::save() 724 | { 725 | using namespace YAML; 726 | Node node; 727 | size_t annotations = 0; 728 | if (!labels_.empty()) 729 | { 730 | for (auto const& label : labels_) 731 | { 732 | node["labels"].push_back(label); 733 | } 734 | } 735 | if (!tags_.empty()) 736 | { 737 | for (auto const& tag : tags_) 738 | { 739 | node["tags"].push_back(tag); 740 | } 741 | } 742 | for (auto const& marker : markers_) 743 | { 744 | Node annotation; 745 | annotation["id"] = marker->id(); 746 | for (auto const& instance : marker->track()) 747 | { 748 | Node i; 749 | i["label"] = instance.label; 750 | 751 | if (!instance.tags.empty()) 752 | { 753 | Node tags; 754 | for (auto const& tag : instance.tags) 755 | { 756 | tags.push_back(tag); 757 | } 758 | i["tags"] = tags; 759 | } 760 | 761 | Node header; 762 | header["frame_id"] = instance.center.frame_id_; 763 | Node stamp; 764 | stamp["secs"] = instance.center.stamp_.sec; 765 | stamp["nsecs"] = instance.center.stamp_.nsec; 766 | header["stamp"] = stamp; 767 | i["header"] = header; 768 | 769 | if (!instance.time_offset.isZero()) 770 | { 771 | Node time_offset; 772 | time_offset["secs"] = instance.time_offset.sec; 773 | time_offset["nsecs"] = instance.time_offset.nsec; 774 | i["time_offset"] = time_offset; 775 | } 776 | 777 | Node origin; 778 | auto const o = instance.center.getOrigin(); 779 | origin["x"] = o.x(); 780 | origin["y"] = o.y(); 781 | origin["z"] = o.z(); 782 | i["translation"] = origin; 783 | 784 | Node rotation; 785 | auto const q = instance.center.getRotation(); 786 | rotation["x"] = q.x(); 787 | rotation["y"] = q.y(); 788 | rotation["z"] = q.z(); 789 | rotation["w"] = q.w(); 790 | i["rotation"] = rotation; 791 | 792 | Node box; 793 | box["length"] = instance.box_size.x(); 794 | box["width"] = instance.box_size.y(); 795 | box["height"] = instance.box_size.z(); 796 | i["box"] = box; 797 | 798 | annotation["track"].push_back(i); 799 | ++annotations; 800 | } 801 | node["tracks"].push_back(annotation); 802 | } 803 | 804 | ofstream stream(filename_); 805 | if (stream.is_open()) 806 | { 807 | stream << node; 808 | if (stream.bad()) 809 | { 810 | stringstream status_stream; 811 | status_stream << "Failed to write annotations to " << filename_; 812 | setStatusStd(rviz::StatusProperty::Error, "Annotation File", status_stream.str()); 813 | ROS_WARN_STREAM(status_stream.str()); 814 | return false; 815 | } 816 | } 817 | else 818 | { 819 | stringstream status_stream; 820 | status_stream << "Failed to open " << filename_ << " for writing. Annotations will not be saved."; 821 | setStatusStd(rviz::StatusProperty::Error, "Annotation File", status_stream.str()); 822 | ROS_WARN_STREAM(status_stream.str()); 823 | return false; 824 | } 825 | stream.close(); 826 | stringstream status_stream; 827 | status_stream << "Saved " << markers_.size() << " tracks with " << annotations << " annotations"; 828 | setStatusStd(rviz::StatusProperty::Ok, "Annotation File", status_stream.str()); 829 | return true; 830 | } 831 | 832 | void AnnotateDisplay::publishTrackMarkers() 833 | { 834 | visualization_msgs::MarkerArray message; 835 | 836 | Marker delete_all; 837 | delete_all.action = Marker::DELETEALL; 838 | message.markers.push_back(delete_all); 839 | 840 | for (auto const& marker : markers_) 841 | { 842 | auto const color = internal::createColor(marker->id()); 843 | Marker line = internal::createTrackLine(0.02, color); 844 | line.id = marker->id(); 845 | line.ns = "Path"; 846 | Marker dots = internal::createTrackSpheres(0.1, color); 847 | dots.id = marker->id() << 16; 848 | dots.ns = "Positions"; 849 | 850 | for (auto const& instance : marker->track()) 851 | { 852 | if (instance.center.stamp_ <= time_ && instance.timeTo(time_) <= 5.0) 853 | { 854 | geometry_msgs::Point point; 855 | auto const p = instance.center.getOrigin(); 856 | pointTFToMsg(p, point); 857 | line.points.push_back(point); 858 | line.header.frame_id = instance.center.frame_id_; 859 | dots.points.push_back(point); 860 | dots.header.frame_id = instance.center.frame_id_; 861 | } 862 | } 863 | 864 | line.action = line.points.size() < 2 ? Marker::DELETE : Marker::ADD; 865 | message.markers.push_back(line); 866 | dots.action = dots.points.empty() ? Marker::DELETE : Marker::ADD; 867 | message.markers.push_back(dots); 868 | } 869 | 870 | track_marker_publisher_.publish(message); 871 | } 872 | 873 | sensor_msgs::PointCloud2ConstPtr AnnotateDisplay::cloud() const 874 | { 875 | return cloud_; 876 | } 877 | 878 | TransformListener& AnnotateDisplay::transformListener() 879 | { 880 | return transform_listener_; 881 | } 882 | 883 | void AnnotateDisplay::setCurrentMarker(AnnotationMarker* marker) 884 | { 885 | current_marker_ = marker; 886 | } 887 | 888 | bool AnnotateDisplay::shrinkAfterResize() const 889 | { 890 | return shrink_after_resize_ && shrink_after_resize_->getBool(); 891 | } 892 | 893 | bool AnnotateDisplay::shrinkBeforeCommit() const 894 | { 895 | return shrink_before_commit_ && shrink_before_commit_->getBool(); 896 | } 897 | 898 | bool AnnotateDisplay::autoFitAfterPointsChange() const 899 | { 900 | return auto_fit_after_points_change_ && auto_fit_after_points_change_->getBool(); 901 | } 902 | 903 | } // namespace annotate 904 | 905 | #include 906 | PLUGINLIB_EXPORT_CLASS(annotate::AnnotateDisplay, rviz::Display) 907 | -------------------------------------------------------------------------------- /src/annotate_tool.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace annotate 11 | { 12 | void AnnotateTool::onInitialize() 13 | { 14 | publisher_ = node_handle_.advertise("/new_annotation", 1); 15 | } 16 | 17 | int AnnotateTool::processMouseEvent(rviz::ViewportMouseEvent& event) 18 | { 19 | Ogre::Vector3 intersection; 20 | Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); 21 | if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, event.y, intersection)) 22 | { 23 | if (event.leftDown()) 24 | { 25 | geometry_msgs::PointStamped message; 26 | message.header.stamp = ros::Time::now(); 27 | message.header.frame_id = context_->getFixedFrame().toStdString(); 28 | message.point.x = intersection.x; 29 | message.point.y = intersection.y; 30 | message.point.z = intersection.z; 31 | publisher_.publish(message); 32 | return Render | Finished; 33 | } 34 | } 35 | return Render; 36 | } 37 | 38 | void AnnotateTool::save(rviz::Config config) const 39 | { 40 | // Required to restore tool button visibility by ToolManager 41 | config.mapSetValue("Class", getClassId()); 42 | } 43 | 44 | void AnnotateTool::activate() 45 | { 46 | // does nothing 47 | } 48 | 49 | void AnnotateTool::deactivate() 50 | { 51 | // does nothing 52 | } 53 | 54 | } // namespace annotate 55 | 56 | #include 57 | PLUGINLIB_EXPORT_CLASS(annotate::AnnotateTool, rviz::Tool) 58 | -------------------------------------------------------------------------------- /src/annotation_marker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace visualization_msgs; 12 | using namespace interactive_markers; 13 | using namespace tf; 14 | using namespace std; 15 | 16 | namespace annotate 17 | { 18 | namespace internal 19 | { 20 | template 21 | int sgn(T val) 22 | { 23 | return (T(0) < val) - (val < T(0)); 24 | } 25 | 26 | Marker createCube(float scale) 27 | { 28 | Marker marker; 29 | marker.type = Marker::CUBE; 30 | marker.scale.x = scale; 31 | marker.scale.y = scale; 32 | marker.scale.z = scale; 33 | marker.color.r = 0.5; 34 | marker.color.g = 0.5; 35 | marker.color.b = 0.5; 36 | marker.color.a = 0.7; 37 | return marker; 38 | } 39 | 40 | } // namespace internal 41 | 42 | void setRotation(geometry_msgs::Quaternion& quaternion, double x, double y, double z) 43 | { 44 | tf::Quaternion orientation(x, y, z, 1.0); 45 | orientation.normalize(); 46 | quaternionTFToMsg(orientation, quaternion); 47 | } 48 | 49 | double TrackInstance::timeTo(ros::Time const& time) const 50 | { 51 | return fabs((time - center.stamp_).toSec()); 52 | } 53 | 54 | void AnnotationMarker::removeControls() 55 | { 56 | marker_.controls.resize(1); 57 | } 58 | 59 | void AnnotationMarker::createCubeControl() 60 | { 61 | InteractiveMarkerControl control; 62 | control.always_visible = true; 63 | control.markers.push_back(internal::createCube(marker_.scale - 0.2)); 64 | control.interaction_mode = InteractiveMarkerControl::BUTTON; 65 | marker_.controls.push_back(control); 66 | } 67 | 68 | void AnnotationMarker::createMoveControl() 69 | { 70 | removeControls(); 71 | InteractiveMarkerControl control; 72 | setRotation(control.orientation, 0.0, 1.0, 0.0); 73 | control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; 74 | marker_.controls.push_back(control); 75 | } 76 | 77 | void AnnotationMarker::createRotationControl() 78 | { 79 | removeControls(); 80 | InteractiveMarkerControl control; 81 | setRotation(control.orientation, 0.0, 1.0, 0.0); 82 | control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 83 | marker_.controls.push_back(control); 84 | } 85 | 86 | void AnnotationMarker::createResizeControl() 87 | { 88 | removeControls(); 89 | InteractiveMarkerControl control; 90 | 91 | setRotation(control.orientation, 1.0, 0.0, 0.0); 92 | control.name = "move_x"; 93 | control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 94 | marker_.controls.push_back(control); 95 | 96 | setRotation(control.orientation, 0.0, 1.0, 0.0); 97 | control.name = "move_z"; 98 | control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 99 | marker_.controls.push_back(control); 100 | 101 | setRotation(control.orientation, 0.0, 0.0, 1.0); 102 | control.name = "move_y"; 103 | control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 104 | marker_.controls.push_back(control); 105 | } 106 | 107 | Vector3 AnnotationMarker::boxSize() const 108 | { 109 | Vector3 box_size; 110 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 111 | { 112 | auto& box = marker_.controls.front().markers.front(); 113 | vector3MsgToTF(box.scale, box_size); 114 | } 115 | return box_size; 116 | } 117 | 118 | void AnnotationMarker::setBoxSize(const Vector3& box_size) 119 | { 120 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 121 | { 122 | auto& box = marker_.controls.front().markers.front(); 123 | vector3TFToMsg(box_size, box.scale); 124 | } 125 | marker_.scale = 0.2 + box_size[box_size.maxAxis()]; 126 | } 127 | 128 | AnnotationMarker::AnnotationMarker(AnnotateDisplay* annotate_display, const shared_ptr& server, 129 | const TrackInstance& trackInstance, int marker_id) 130 | : server_(server), id_(marker_id), annotate_display_(annotate_display) 131 | { 132 | time_ = trackInstance.center.stamp_; 133 | createMarker(trackInstance); 134 | } 135 | 136 | void AnnotationMarker::setLabels(const std::vector& labels) 137 | { 138 | label_keys_ = labels; 139 | if (state_ != Hidden) 140 | { 141 | pull(); 142 | push(); 143 | } 144 | } 145 | 146 | void AnnotationMarker::setTags(const std::vector& tags) 147 | { 148 | tag_keys_ = tags; 149 | if (state_ != Hidden) 150 | { 151 | pull(); 152 | push(); 153 | } 154 | } 155 | 156 | void AnnotationMarker::updateMenu(const PointContext& context) 157 | { 158 | menu_handler_ = MenuHandler(); 159 | 160 | MenuHandler::EntryHandle mode_menu = menu_handler_.insert("Box Mode"); 161 | auto handle = menu_handler_.insert(mode_menu, "Locked", boost::bind(&AnnotationMarker::lock, this, _1)); 162 | menu_handler_.setCheckState(handle, mode_ == Locked ? MenuHandler::CHECKED : MenuHandler::NO_CHECKBOX); 163 | handle = menu_handler_.insert(mode_menu, "Move", boost::bind(&AnnotationMarker::enableMoveControl, this, _1)); 164 | menu_handler_.setCheckState(handle, mode_ == Move ? MenuHandler::CHECKED : MenuHandler::NO_CHECKBOX); 165 | handle = menu_handler_.insert(mode_menu, "Rotate", boost::bind(&AnnotationMarker::enableRotationControl, this, _1)); 166 | menu_handler_.setCheckState(handle, mode_ == Rotate ? MenuHandler::CHECKED : MenuHandler::NO_CHECKBOX); 167 | handle = menu_handler_.insert(mode_menu, "Resize", boost::bind(&AnnotationMarker::enableResizeControl, this, _1)); 168 | menu_handler_.setCheckState(handle, mode_ == Resize ? MenuHandler::CHECKED : MenuHandler::NO_CHECKBOX); 169 | 170 | labels_.clear(); 171 | if (!label_keys_.empty()) 172 | { 173 | MenuHandler::EntryHandle label_menu = menu_handler_.insert("Label"); 174 | for (auto const& label : label_keys_) 175 | { 176 | auto const handle = menu_handler_.insert(label_menu, label, boost::bind(&AnnotationMarker::setLabel, this, _1)); 177 | labels_[handle] = label; 178 | menu_handler_.setCheckState(handle, label_ == label ? MenuHandler::CHECKED : MenuHandler::NO_CHECKBOX); 179 | } 180 | } 181 | 182 | tags_menu_.clear(); 183 | if (!tag_keys_.empty()) 184 | { 185 | MenuHandler::EntryHandle tags_menu = menu_handler_.insert("Tags"); 186 | for (auto const& tag : tag_keys_) 187 | { 188 | auto const handle = menu_handler_.insert(tags_menu, tag, boost::bind(&AnnotationMarker::setTag, this, _1)); 189 | tags_menu_[handle] = tag; 190 | auto const has_tag = find(tags_.cbegin(), tags_.cend(), tag) != tags_.cend(); 191 | menu_handler_.setCheckState(handle, has_tag ? MenuHandler::CHECKED : MenuHandler::NO_CHECKBOX); 192 | } 193 | } 194 | 195 | MenuHandler::EntryHandle edit_menu = menu_handler_.insert("Actions"); 196 | if (!undo_stack_.empty()) 197 | { 198 | menu_handler_.insert(edit_menu, "Undo " + undo_stack_.top().undo_description, 199 | boost::bind(&AnnotationMarker::undo, this, _1)); 200 | } 201 | menu_handler_.insert(edit_menu, "Shrink to Points", boost::bind(&AnnotationMarker::shrink, this, _1)); 202 | menu_handler_.insert(edit_menu, "Auto-fit Box", boost::bind(&AnnotationMarker::autoFit, this, _1)); 203 | 204 | string commit_title = "Commit"; 205 | if (context.points_nearby) 206 | { 207 | commit_title += " (despite " + to_string(context.points_nearby) + " nearby points)"; 208 | } 209 | menu_handler_.insert(commit_title, boost::bind(&AnnotationMarker::commit, this, _1)); 210 | 211 | menu_handler_.apply(*server_, marker_.name); 212 | } 213 | 214 | void AnnotationMarker::processFeedback(const InteractiveMarkerFeedbackConstPtr& feedback) 215 | { 216 | switch (feedback->event_type) 217 | { 218 | case InteractiveMarkerFeedback::POSE_UPDATE: 219 | if (!button_click_active_ && mode_ == Move) 220 | { 221 | saveMove(); 222 | } 223 | break; 224 | case InteractiveMarkerFeedback::BUTTON_CLICK: 225 | button_click_active_ = true; 226 | annotate_display_->setCurrentMarker(this); 227 | return; 228 | break; 229 | case InteractiveMarkerFeedback::MOUSE_DOWN: 230 | if (mode_ == Resize && feedback->mouse_point_valid) 231 | { 232 | poseMsgToTF(feedback->pose, last_pose_); 233 | pointMsgToTF(feedback->mouse_point, last_mouse_point_); 234 | can_change_size_ = true; 235 | } 236 | break; 237 | case InteractiveMarkerFeedback::MOUSE_UP: 238 | if (button_click_active_) 239 | { 240 | nextMode(); 241 | button_click_active_ = false; 242 | can_change_size_ = false; 243 | } 244 | else if (mode_ == Resize && can_change_size_) 245 | { 246 | Pose pose; 247 | poseMsgToTF(feedback->pose, pose); 248 | UndoState state; 249 | state.time.start(); 250 | state.undo_description = "change size"; 251 | poseTFToMsg(last_pose_, state.pose); 252 | state.box_size = boxSize(); 253 | state.state = state_; 254 | state.label = label_; 255 | state.tags = tags_; 256 | undo_stack_.push(state); 257 | changeSize(pose); 258 | can_change_size_ = false; 259 | return; 260 | } 261 | break; 262 | } 263 | } 264 | 265 | void AnnotationMarker::changeSize(const Pose& new_pose) 266 | { 267 | Pose last_mouse_pose = new_pose; 268 | last_mouse_pose.setOrigin(last_mouse_point_); 269 | auto const mouse_diff = last_pose_.inverseTimes(last_mouse_pose).getOrigin(); 270 | auto const diff = last_pose_.inverseTimes(new_pose).getOrigin(); 271 | array const components({ fabs(diff.x()), fabs(diff.y()), fabs(diff.z()) }); 272 | size_t const max_component = distance(components.cbegin(), max_element(components.cbegin(), components.cend())); 273 | auto const change = internal::sgn(mouse_diff.m_floats[max_component]) * diff.m_floats[max_component]; 274 | 275 | pull(); 276 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 277 | { 278 | auto& box = marker_.controls.front().markers.front(); 279 | Vector3 scale; 280 | vector3MsgToTF(box.scale, scale); 281 | scale.m_floats[max_component] = max(padding_, scale[max_component] + change); 282 | setBoxSize(scale); 283 | Pose new_center = new_pose; 284 | new_center.setOrigin(last_pose_ * (0.5 * diff)); 285 | poseTFToMsg(new_center, marker_.pose); 286 | 287 | if (annotate_display_->shrinkAfterResize()) 288 | { 289 | auto const context = analyzePoints(); 290 | if (context.points_inside) 291 | { 292 | saveForUndo("shrink to points"); 293 | shrinkTo(context); 294 | } 295 | } 296 | 297 | updateState(Modified); 298 | push(); 299 | } 300 | } 301 | 302 | void AnnotationMarker::pull() 303 | { 304 | if (!server_->get(marker_.name, marker_)) 305 | { 306 | push(); 307 | } 308 | } 309 | 310 | void AnnotationMarker::push() 311 | { 312 | auto const context = analyzePoints(); 313 | updateDescription(context); 314 | server_->insert(marker_, boost::bind(&AnnotationMarker::processFeedback, this, _1)); 315 | updateMenu(context); 316 | server_->applyChanges(); 317 | } 318 | 319 | void AnnotationMarker::nextMode() 320 | { 321 | if (mode_ == Move) 322 | { 323 | can_change_size_ = false; 324 | enableRotationControl(); 325 | } 326 | else if (mode_ == Rotate) 327 | { 328 | enableResizeControl(); 329 | } 330 | else if (mode_ == Resize) 331 | { 332 | enableMoveControl(); 333 | } 334 | } 335 | 336 | void AnnotationMarker::createMarker(const TrackInstance& instance) 337 | { 338 | marker_.header.frame_id = instance.center.frame_id_; 339 | marker_.header.stamp = time_; 340 | auto const center = instance.center.getOrigin(); 341 | pointTFToMsg(center, marker_.pose.position); 342 | Quaternion rotation; 343 | rotation.setRPY(0.0, 0.0, 0.0); 344 | quaternionTFToMsg(rotation, marker_.pose.orientation); 345 | marker_.scale = 1; 346 | marker_.name = string("annotation_") + to_string(id_); 347 | createCubeControl(); 348 | createMoveControl(); 349 | } 350 | 351 | void AnnotationMarker::updateDescription(const PointContext& context) 352 | { 353 | stringstream stream; 354 | stream << label_ << " #" << id_; 355 | if (!tags_.empty()) 356 | { 357 | stream << " ("; 358 | auto iter = tags_.begin(); 359 | stream << *iter; 360 | ++iter; 361 | for (; iter != tags_.cend(); ++iter) 362 | { 363 | stream << ", "; 364 | stream << *iter; 365 | } 366 | stream << ")"; 367 | } 368 | 369 | stream << setiosflags(ios::fixed) << setprecision(2); 370 | if (!context.time_offset.isZero()) 371 | { 372 | stream << "\ntime offset: " << context.time_offset.toSec() * 1000 << " ms"; 373 | } 374 | 375 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 376 | { 377 | auto const& box = marker_.controls.front().markers.front(); 378 | Vector3 diff; 379 | if (context.points_inside) 380 | { 381 | Vector3 box_min; 382 | vector3MsgToTF(box.scale, box_min); 383 | box_min = -0.5 * box_min; 384 | Vector3 const box_max = -box_min; 385 | diff = (box_min - context.minimum).absolute() + (box_max - context.maximum).absolute(); 386 | } 387 | 388 | bool const fits_tight = (diff - Vector3(padding_, padding_, padding_)).fuzzyZero(); 389 | if (fits_tight) 390 | { 391 | stream << "\n" << box.scale.x; 392 | stream << " x " << box.scale.y; 393 | stream << " x " << box.scale.z; 394 | stream << " (well-fitting)"; 395 | } 396 | else 397 | { 398 | stream << "\n" << box.scale.x << " (+" << diff.x() << ")"; 399 | stream << " x " << box.scale.y << " (+" << diff.y() << ")"; 400 | stream << " x " << box.scale.z << " (+" << diff.z() << ")"; 401 | } 402 | } 403 | 404 | stream << "\n" << context.points_inside << " points inside, "; 405 | stream << context.points_nearby << " nearby"; 406 | 407 | marker_.description = stream.str(); 408 | } 409 | 410 | void AnnotationMarker::enableMoveControl() 411 | { 412 | mode_ = Move; 413 | pull(); 414 | createMoveControl(); 415 | push(); 416 | } 417 | 418 | void AnnotationMarker::enableMoveControl(const InteractiveMarkerFeedbackConstPtr& feedback) 419 | { 420 | enableMoveControl(); 421 | } 422 | 423 | void AnnotationMarker::enableResizeControl() 424 | { 425 | mode_ = Resize; 426 | pull(); 427 | createResizeControl(); 428 | push(); 429 | } 430 | 431 | void AnnotationMarker::enableResizeControl(const InteractiveMarkerFeedbackConstPtr& feedback) 432 | { 433 | enableResizeControl(); 434 | } 435 | 436 | void AnnotationMarker::enableRotationControl() 437 | { 438 | mode_ = Rotate; 439 | pull(); 440 | createRotationControl(); 441 | push(); 442 | } 443 | 444 | void AnnotationMarker::enableRotationControl(const InteractiveMarkerFeedbackConstPtr& feedback) 445 | { 446 | enableRotationControl(); 447 | } 448 | 449 | void AnnotationMarker::lock() 450 | { 451 | mode_ = Locked; 452 | pull(); 453 | removeControls(); 454 | push(); 455 | } 456 | 457 | void AnnotationMarker::lock(const InteractiveMarkerFeedbackConstPtr& feedback) 458 | { 459 | lock(); 460 | } 461 | 462 | void AnnotationMarker::setLabel(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 463 | { 464 | if (feedback->event_type == InteractiveMarkerFeedback::MENU_SELECT) 465 | { 466 | pull(); 467 | if (label_ != labels_[feedback->menu_entry_id]) 468 | { 469 | saveForUndo("label change"); 470 | label_ = labels_[feedback->menu_entry_id]; 471 | updateState(Modified); 472 | push(); 473 | } 474 | } 475 | } 476 | 477 | void AnnotationMarker::setTag(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 478 | { 479 | if (feedback->event_type == InteractiveMarkerFeedback::MENU_SELECT) 480 | { 481 | pull(); 482 | MenuHandler::CheckState check_state; 483 | menu_handler_.getCheckState(feedback->menu_entry_id, check_state); 484 | auto const tag = tags_menu_[feedback->menu_entry_id]; 485 | if (check_state == MenuHandler::CHECKED) 486 | { 487 | saveForUndo("remove tag " + tag); 488 | tags_.erase(remove(tags_.begin(), tags_.end(), tag), tags_.end()); 489 | } 490 | else 491 | { 492 | saveForUndo("add tag " + tag); 493 | tags_.push_back(tag); 494 | } 495 | updateState(Modified); 496 | push(); 497 | } 498 | } 499 | 500 | void AnnotationMarker::shrinkTo(const PointContext& context) 501 | { 502 | if (!context.points_inside) 503 | { 504 | return; 505 | } 506 | 507 | Transform transform; 508 | poseMsgToTF(marker_.pose, transform); 509 | Point input(0.5 * (context.maximum + context.minimum)); 510 | Point output = transform * input; 511 | pointTFToMsg(output, marker_.pose.position); 512 | Vector3 const margin(padding_, padding_, padding_); 513 | setBoxSize(margin + context.maximum - context.minimum); 514 | } 515 | 516 | void AnnotationMarker::shrink(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 517 | { 518 | shrinkToPoints(); 519 | } 520 | 521 | bool AnnotationMarker::fitNearbyPoints() 522 | { 523 | auto const pose = marker_.pose; 524 | auto const box_size = boxSize(); 525 | 526 | { 527 | auto const context = analyzePoints(); 528 | if (context.points_nearby == 0) 529 | { 530 | shrinkTo(context); 531 | return true; 532 | } 533 | } 534 | 535 | for (int i = 0; i < 4; ++i) 536 | { 537 | resize(margin_); 538 | auto const context = analyzePoints(); 539 | if (context.points_nearby == 0) 540 | { 541 | shrinkTo(context); 542 | return true; 543 | } 544 | } 545 | 546 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 547 | { 548 | auto& box = marker_.controls.front().markers.front(); 549 | vector3TFToMsg(box_size, box.scale); 550 | marker_.pose = pose; 551 | } 552 | return false; 553 | } 554 | 555 | void AnnotationMarker::shrinkToPoints() 556 | { 557 | pull(); 558 | auto const context = analyzePoints(); 559 | if (context.points_inside) 560 | { 561 | saveForUndo("shrink to points"); 562 | shrinkTo(context); 563 | updateState(Modified); 564 | push(); 565 | } 566 | } 567 | 568 | void AnnotationMarker::autoFit() 569 | { 570 | pull(); 571 | if (fitNearbyPoints()) 572 | { 573 | updateState(Modified); 574 | push(); 575 | } 576 | } 577 | 578 | void AnnotationMarker::autoFit(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 579 | { 580 | autoFit(); 581 | } 582 | 583 | void AnnotationMarker::saveMove() 584 | { 585 | bool const merge_with_previous = 586 | !undo_stack_.empty() && undo_stack_.top().undo_description == "move" && undo_stack_.top().time.elapsed() < 800; 587 | if (merge_with_previous) 588 | { 589 | undo_stack_.top().time.restart(); 590 | } 591 | else 592 | { 593 | auto const pose = marker_.pose; 594 | pull(); 595 | if (hasMoved(pose, marker_.pose)) 596 | { 597 | saveForUndo("move"); 598 | } 599 | push(); 600 | } 601 | } 602 | 603 | bool AnnotationMarker::hasMoved(geometry_msgs::Pose const& one, geometry_msgs::Pose const& two) const 604 | { 605 | Transform a; 606 | poseMsgToTF(one, a); 607 | Transform b; 608 | poseMsgToTF(two, b); 609 | return !(a.getOrigin() - b.getOrigin()).fuzzyZero(); 610 | } 611 | 612 | void AnnotationMarker::saveForUndo(const string& description) 613 | { 614 | if (!undo_stack_.empty()) 615 | { 616 | auto const& last_state = undo_stack_.top(); 617 | if (last_state.state == state_ && last_state.label == label_ && last_state.tags == tags_ && 618 | last_state.box_size == boxSize() && !hasMoved(last_state.pose, marker_.pose)) 619 | { 620 | return; 621 | } 622 | } 623 | 624 | UndoState state; 625 | state.time.start(); 626 | state.undo_description = description; 627 | state.pose = marker_.pose; 628 | state.box_size = boxSize(); 629 | state.state = state_; 630 | state.label = label_; 631 | state.tags = tags_; 632 | undo_stack_.push(state); 633 | } 634 | 635 | void AnnotationMarker::undo() 636 | { 637 | if (undo_stack_.empty()) 638 | { 639 | return; 640 | } 641 | 642 | auto const state = undo_stack_.top(); 643 | marker_.pose = state.pose; 644 | label_ = state.label; 645 | tags_ = state.tags; 646 | undo_stack_.pop(); 647 | setBoxSize(state.box_size); 648 | updateState(state.state); 649 | push(); 650 | } 651 | 652 | void AnnotationMarker::undo(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 653 | { 654 | undo(); 655 | } 656 | 657 | void AnnotationMarker::resize(double offset) 658 | { 659 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 660 | { 661 | auto& box = marker_.controls.front().markers.front(); 662 | if (ignore_ground_) 663 | { 664 | marker_.pose.position.z += offset / 4.0; 665 | setBoxSize({ box.scale.x + offset, box.scale.y + offset, box.scale.z + offset / 2.0 }); 666 | } 667 | else 668 | { 669 | setBoxSize({ box.scale.x + offset, box.scale.y + offset, box.scale.z + offset }); 670 | } 671 | } 672 | } 673 | 674 | AnnotationMarker::PointContext AnnotationMarker::analyzePoints() const 675 | { 676 | auto const cloud = annotate_display_->cloud(); 677 | PointContext context; 678 | if (!cloud || cloud->width == 0 || cloud->height == 0) 679 | { 680 | return context; 681 | } 682 | context.time = min(time_, cloud->header.stamp); 683 | auto& transform_listener = annotate_display_->transformListener(); 684 | auto time = ros::Time(); 685 | string error; 686 | for (int i = 0; i < 25; ++i) 687 | { 688 | if (transform_listener.canTransform(marker_.header.frame_id, cloud->header.frame_id, context.time, &error)) 689 | { 690 | time = context.time; 691 | break; 692 | } 693 | QThread::msleep(10); 694 | } 695 | using namespace sensor_msgs; 696 | if (transform_listener.canTransform(marker_.header.frame_id, cloud->header.frame_id, time, &error)) 697 | { 698 | StampedTransform cloud_to_fixed; 699 | transform_listener.lookupTransform(marker_.header.frame_id, cloud->header.frame_id, time, cloud_to_fixed); 700 | auto const field_iter = find_if(cloud->fields.begin(), cloud->fields.end(), [](PointField const& field) { 701 | return field.name == "timestamp" && field.count == 1 && field.datatype == PointField::FLOAT64; 702 | }); 703 | bool const has_timestamp = field_iter != cloud->fields.end(); 704 | auto const num_points = cloud->width * cloud->height; 705 | 706 | Transform fixed_to_marker; 707 | poseMsgToTF(marker_.pose, fixed_to_marker); 708 | Transform const trafo = fixed_to_marker.inverse() * cloud_to_fixed; 709 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 710 | { 711 | auto& box = marker_.controls.front().markers.front(); 712 | Vector3 box_min; 713 | vector3MsgToTF(box.scale, box_min); 714 | box_min = -0.5 * box_min; 715 | Vector3 const box_max = -box_min; 716 | Vector3 offset(margin_, margin_, margin_); 717 | Vector3 const nearby_max = box_max + offset; 718 | Vector3 nearby_min = box_min - offset; 719 | if (ignore_ground_) 720 | { 721 | nearby_min.setZ(box_min.z()); 722 | } 723 | PointCloud2ConstIterator iter_x(*cloud, "x"); 724 | PointCloud2ConstIterator iter_y(*cloud, "y"); 725 | PointCloud2ConstIterator iter_z(*cloud, "z"); 726 | shared_ptr> iter_timestamp; 727 | if (has_timestamp) 728 | { 729 | iter_timestamp = make_shared>(*cloud, "timestamp"); 730 | } 731 | ros::Duration time_offsets; 732 | for (size_t i = 0; i < num_points; ++i) 733 | { 734 | auto const point = trafo * Vector3(*iter_x, *iter_y, *iter_z); 735 | Vector3 alien = point; 736 | alien.setMax(nearby_min); 737 | alien.setMin(nearby_max); 738 | if (alien == point) 739 | { 740 | Vector3 canary = point; 741 | canary.setMax(box_min); 742 | canary.setMin(box_max); 743 | if (canary == point) 744 | { 745 | ++context.points_inside; 746 | context.minimum.setMin(point); 747 | context.maximum.setMax(point); 748 | if (has_timestamp) 749 | { 750 | ros::Time point_time; 751 | point_time.fromNSec(**iter_timestamp); 752 | time_offsets += point_time - context.time; 753 | } 754 | } 755 | else 756 | { 757 | ++context.points_nearby; 758 | } 759 | } 760 | 761 | ++iter_x; 762 | ++iter_y; 763 | ++iter_z; 764 | if (has_timestamp) 765 | { 766 | ++*iter_timestamp; 767 | } 768 | } 769 | if (has_timestamp && context.points_inside > 0) 770 | { 771 | auto const mean = time_offsets.toSec() / context.points_inside; 772 | context.time_offset.fromSec(mean); 773 | } 774 | } 775 | } 776 | else 777 | { 778 | ROS_WARN_STREAM("Transformation failed: " << error); 779 | } 780 | return context; 781 | } 782 | 783 | void AnnotationMarker::rotateYaw(double delta_rad) 784 | { 785 | pull(); 786 | Quaternion rotation; 787 | quaternionMsgToTF(marker_.pose.orientation, rotation); 788 | Quaternion delta; 789 | delta.setRPY(0.0, 0.0, delta_rad); 790 | auto rotated = delta * rotation; 791 | rotated.normalize(); 792 | quaternionTFToMsg(rotated, marker_.pose.orientation); 793 | push(); 794 | } 795 | 796 | void AnnotationMarker::commit() 797 | { 798 | pull(); 799 | auto const context = analyzePoints(); 800 | if (annotate_display_->shrinkBeforeCommit()) 801 | { 802 | if (context.points_inside) 803 | { 804 | saveForUndo("shrink to points"); 805 | shrinkTo(context); 806 | updateState(Modified); 807 | } 808 | } 809 | 810 | TrackInstance instance; 811 | instance.label = label_; 812 | instance.tags = tags_; 813 | instance.time_offset = context.time_offset; 814 | 815 | Transform transform; 816 | poseMsgToTF(marker_.pose, transform); 817 | instance.center = StampedTransform(transform, time_, marker_.header.frame_id, "current_annotation"); 818 | 819 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 820 | { 821 | auto& box = marker_.controls.front().markers.front(); 822 | vector3MsgToTF(box.scale, instance.box_size); 823 | } 824 | 825 | track_.erase( 826 | remove_if(track_.begin(), track_.end(), [this](const TrackInstance& t) { return t.center.stamp_ == time_; }), 827 | track_.end()); 828 | track_.push_back(instance); 829 | sort(track_.begin(), track_.end(), 830 | [](TrackInstance const& a, TrackInstance const& b) -> bool { return a.center.stamp_ < b.center.stamp_; }); 831 | if (annotate_display_->save()) 832 | { 833 | updateState(Committed); 834 | } 835 | annotate_display_->publishTrackMarkers(); 836 | push(); 837 | } 838 | 839 | void AnnotationMarker::commit(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) 840 | { 841 | if (feedback->event_type == InteractiveMarkerFeedback::MENU_SELECT) 842 | { 843 | commit(); 844 | } 845 | } 846 | 847 | void AnnotationMarker::updateState(State state) 848 | { 849 | if (state_ == state) 850 | { 851 | return; 852 | } 853 | 854 | state_ = state; 855 | if (!marker_.controls.empty() && !marker_.controls.front().markers.empty()) 856 | { 857 | auto& box = marker_.controls.front().markers.front(); 858 | 859 | switch (state_) 860 | { 861 | case Hidden: 862 | case New: 863 | box.color.r = 0.5; 864 | box.color.g = 0.5; 865 | box.color.b = 0.5; 866 | break; 867 | case Committed: 868 | box.color.r = 60 / 255.0; 869 | box.color.g = 180 / 255.0; 870 | box.color.b = 75 / 255.0; 871 | break; 872 | case Modified: 873 | box.color.r = 245 / 255.0; 874 | box.color.g = 130 / 255.0; 875 | box.color.b = 48 / 255.0; 876 | break; 877 | } 878 | } 879 | } 880 | 881 | int AnnotationMarker::id() const 882 | { 883 | return id_; 884 | } 885 | 886 | Track const& AnnotationMarker::track() const 887 | { 888 | return track_; 889 | } 890 | 891 | StampedTransform estimatePose(StampedTransform const& a, StampedTransform const& b, ros::Time const& time) 892 | { 893 | auto const time_diff = (b.stamp_ - a.stamp_).toSec(); 894 | if (fabs(time_diff) < 0.001) 895 | { 896 | // Avoid division by zero and measurement noise affecting interpolation results 897 | return a; 898 | } 899 | auto const ratio = (time - a.stamp_).toSec() / time_diff; 900 | Transform transform; 901 | transform.setOrigin(a.getOrigin().lerp(b.getOrigin(), ratio)); 902 | transform.setRotation(a.getRotation().slerp(b.getRotation(), ratio)); 903 | return StampedTransform(transform, time, a.frame_id_, a.child_frame_id_); 904 | } 905 | 906 | void AnnotationMarker::setTime(const ros::Time& time) 907 | { 908 | time_ = time; 909 | if (!track_.empty()) 910 | { 911 | auto const prune_before_track_start = 912 | time < track_.front().center.stamp_ && track_.front().timeTo(time) > annotate_display_->preTime(); 913 | auto const prune_after_track_end = 914 | time > track_.back().center.stamp_ && track_.back().timeTo(time) > annotate_display_->postTime(); 915 | if (prune_before_track_start || prune_after_track_end) 916 | { 917 | server_->erase(marker_.name); 918 | server_->applyChanges(); 919 | updateState(Hidden); 920 | return; 921 | } 922 | } 923 | 924 | pull(); 925 | while (!undo_stack_.empty()) 926 | { 927 | undo_stack_.pop(); 928 | } 929 | marker_.header.stamp = time; 930 | 931 | // Find an existing annotation for this point in time, if any 932 | for (auto const& instance : track_) 933 | { 934 | if (instance.timeTo(time) < 0.01) 935 | { 936 | label_ = instance.label; 937 | tags_ = instance.tags; 938 | updateState(Committed); 939 | poseTFToMsg(instance.center, marker_.pose); 940 | setBoxSize(instance.box_size); 941 | push(); 942 | return; 943 | } 944 | } 945 | 946 | // Estimate a suitable pose from nearby annotations 947 | Track track = track_; 948 | sort(track.begin(), track.end(), 949 | [time](TrackInstance const& a, TrackInstance const& b) -> bool { return a.timeTo(time) < b.timeTo(time); }); 950 | double const extrapolation_limit = 2.0; 951 | if (track.size() > 1 && track[1].timeTo(time) < extrapolation_limit) 952 | { 953 | auto const transform = estimatePose(track[0].center, track[1].center, time); 954 | poseTFToMsg(transform, marker_.pose); 955 | } 956 | 957 | updateState(New); 958 | if (annotate_display_->autoFitAfterPointsChange()) 959 | { 960 | if (fitNearbyPoints()) 961 | { 962 | updateState(Modified); 963 | } 964 | } 965 | 966 | push(); 967 | } 968 | 969 | void AnnotationMarker::setTrack(const Track& track) 970 | { 971 | track_ = track; 972 | } 973 | 974 | void AnnotationMarker::setPadding(double padding) 975 | { 976 | padding_ = padding; 977 | pull(); 978 | push(); 979 | } 980 | 981 | void AnnotationMarker::setMargin(double margin) 982 | { 983 | margin_ = margin; 984 | pull(); 985 | push(); 986 | } 987 | 988 | void AnnotationMarker::setIgnoreGround(bool enabled) 989 | { 990 | ignore_ground_ = enabled; 991 | } 992 | 993 | } // namespace annotate 994 | -------------------------------------------------------------------------------- /src/file_dialog_property.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace annotate 7 | { 8 | FileDialogProperty::FileDialogProperty(const QString& name, const QString& default_value, const QString& description, 9 | rviz::Property* parent, const char* changed_slot, QObject* receiver) 10 | : rviz::Property(name, default_value, description, parent, changed_slot, receiver) 11 | { 12 | // does nothing 13 | } 14 | 15 | QWidget* FileDialogProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& option) 16 | { 17 | FileDialogEditor* editor = new FileDialogEditor(this, parent, mode_); 18 | editor->setFrame(false); 19 | return editor; 20 | } 21 | 22 | void FileDialogProperty::setMode(Mode mode) 23 | { 24 | mode_ = mode; 25 | } 26 | 27 | FileDialogEditor::FileDialogEditor(FileDialogProperty* property, QWidget* parent, FileDialogProperty::Mode mode) 28 | : rviz::LineEditWithButton(parent), property_(property), mode_(mode) 29 | { 30 | // does nothing 31 | } 32 | 33 | void FileDialogEditor::onButtonClick() 34 | { 35 | auto property = property_; // same hack as in ColorEditor::onButtonClick() 36 | QString file; 37 | switch (mode_) 38 | { 39 | case FileDialogProperty::ExistingDirectory: 40 | file = QFileDialog::getExistingDirectory(parentWidget(), "Open Existing Directory", QString()); 41 | break; 42 | case FileDialogProperty::OpenFileName: 43 | file = QFileDialog::getOpenFileName(parentWidget(), "Open Existing Annotation File", QString(), 44 | "Annotation Files (*.yaml *.yml *.annotate)"); 45 | break; 46 | case FileDialogProperty::SaveFileName: 47 | file = QFileDialog::getSaveFileName(parentWidget(), "Save Annotation File", QString(), 48 | "Annotation Files (*.yaml *.yml *.annotate)"); 49 | break; 50 | } 51 | if (!file.isEmpty()) 52 | { 53 | property->setValue(file); 54 | } 55 | } 56 | 57 | } // namespace annotate 58 | -------------------------------------------------------------------------------- /src/shortcut_property.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace annotate 9 | { 10 | ShortcutProperty::ShortcutProperty(const QString& name, const QString& default_value, const QString& description, 11 | rviz::Property* parent, const char* changed_slot, QObject* receiver) 12 | : rviz::StringProperty(name, default_value, description, parent, changed_slot, receiver) 13 | { 14 | QString const icon_name = QString(name).replace(' ', '-'); 15 | setIcon(rviz::loadPixmap(QString("package://annotate/icons/%1.svg").arg(icon_name))); 16 | connect(this, SIGNAL(changed()), this, SLOT(updateShortcut())); 17 | } 18 | 19 | void ShortcutProperty::createShortcut(AnnotateDisplay* display, QWidget* target, QObject* receiver, 20 | const char* trigger_slot) 21 | { 22 | display_ = display; 23 | shortcut_ = new QShortcut(target); 24 | connect(shortcut_, SIGNAL(activated()), receiver, trigger_slot); 25 | connect(shortcut_, SIGNAL(activatedAmbiguously()), this, SLOT(handleAmbiguousShortcut())); 26 | updateShortcut(); 27 | } 28 | 29 | QString ShortcutProperty::statusName() const 30 | { 31 | return QString("Shortcut %1").arg(getName()); 32 | } 33 | 34 | void ShortcutProperty::setEnabled(bool enabled) 35 | { 36 | if (shortcut_) 37 | { 38 | shortcut_->setEnabled(enabled); 39 | } 40 | } 41 | 42 | void ShortcutProperty::updateShortcut() 43 | { 44 | if (shortcut_) 45 | { 46 | auto const key_sequence = QKeySequence(getString()); 47 | if (key_sequence.isEmpty()) 48 | { 49 | if (display_) 50 | { 51 | display_->setStatus(rviz::StatusProperty::Warn, statusName(), 52 | QString("The keyboard shortcut '%1' is invalid. Please choose a valid shortcut like " 53 | "'Ctrl+D'.") 54 | .arg(getString())); 55 | } 56 | } 57 | else 58 | { 59 | shortcut_->setKey(key_sequence); 60 | if (display_) 61 | { 62 | if (display_->toolShortcuts().contains(getString())) 63 | { 64 | display_->setStatus(rviz::StatusProperty::Warn, statusName(), 65 | QString("The keyboard shortcut '%1' hides a Tool shortcut.").arg(getString())); 66 | } 67 | else 68 | { 69 | display_->deleteStatus(statusName()); 70 | } 71 | } 72 | } 73 | } 74 | } 75 | 76 | void ShortcutProperty::handleAmbiguousShortcut() 77 | { 78 | if (display_) 79 | { 80 | display_->setStatus( 81 | rviz::StatusProperty::Warn, statusName(), 82 | QString("The keyboard shortcut '%1' is already used. Please choose a different one.").arg(getString())); 83 | } 84 | } 85 | 86 | } // namespace annotate 87 | --------------------------------------------------------------------------------