[ { "path": ".github/workflows/build.yml", "content": "name: Build\n\non:\n push:\n branches: [ master ]\n pull_request:\n branches: [ master ]\n\n # Allows you to run this workflow manually from the Actions tab\n workflow_dispatch:\n\njobs:\n build:\n runs-on: ubuntu-latest\n strategy:\n matrix:\n ROS_DISTRO: [noetic]\n\n steps:\n - uses: actions/checkout@v2\n with:\n submodule: true\n\n - name: Docker login\n uses: docker/login-action@v1\n with:\n username: ${{ secrets.DOCKER_USERNAME }}\n password: ${{ secrets.DOCKER_TOKEN }}\n\n - name: Docker build\n uses: docker/build-push-action@v2\n with:\n file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile\n context: .\n push: false\n" }, { "path": ".travis.yml", "content": "sudo: required\nlanguage: generic\ndist: xenial\n\nservices:\n - docker\n\nmatrix:\n include:\n - name: \"Xenial kinetic\"\n env: ROS_DISTRO=kinetic\n - name: \"Bionic melodic\"\n env: ROS_DISTRO=melodic\n\nscript:\n - docker build -f ./docker/$ROS_DISTRO/Dockerfile .\n" }, { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(monocular_person_following)\n\nadd_compile_options(-std=c++11)\n\nfind_package(catkin REQUIRED\n tf\n rospy\n roscpp\n cv_bridge\n message_filters\n image_transport\n eigen_conversions\n message_generation\n ccf_person_identification\n monocular_people_tracking\n)\n\n########################\n## service generation ##\n########################\nadd_service_files(FILES\n Imprint.srv\n)\n\n########################\n## message generation ##\n########################\nadd_message_files(FILES\n BoundingBox2D.msg\n FaceDetection.msg\n FaceDetectionArray.msg\n Target.msg\n)\n\ngenerate_messages(DEPENDENCIES std_msgs sensor_msgs)\n\n###################################\n## catkin specific configuration ##\n###################################\ncatkin_package(\n # INCLUDE_DIRS include\n # LIBRARIES monocular_person_following\n # CATKIN_DEPENDS other_catkin_pkg\n # DEPENDS system_lib\n )\n\n###########\n## Build ##\n###########\n\ninclude_directories(\n include\n ${catkin_INCLUDE_DIRS}\n)\n\nadd_executable(monocular_person_following_node\n src/monocular_person_following/state/initial_state.cpp\n src/monocular_person_following/state/initial_training_state.cpp\n src/monocular_person_following/state/tracking_state.cpp\n src/monocular_person_following/state/reid_state.cpp\n src/monocular_person_following/context.cpp\n src/monocular_person_following_node.cpp\n)\nadd_dependencies(monocular_person_following_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\ntarget_link_libraries(monocular_person_following_node\n ${catkin_LIBRARIES}\n)\n" }, { "path": "README.md", "content": "# monocular_person_following\n\nThis package provides a monocular vision-based person tracking and identification framework for person following robots. It first detects people using *tf-pose-estimation*, and then track them in the robot space with Unscented Kalman filter with the ground plane information. The combination of Convolutional Channel Features and Online Boosting runs on the top of the tracking module to keep tracking the target person with a re-identification capability. The entire system is designed so that it can be run on a Jetson TX2/Xavier, and it can easily be reproduced and reused on a new mobile robot platform.\n\n![system](data/imgs/system.png)\n\n[[video]](https://www.youtube.com/watch?v=SsIrXxnOgaQ)\n\n[![Codacy Badge](https://api.codacy.com/project/badge/Grade/c7664fce1722461db5ffdc27eae59e9c)](https://www.codacy.com/app/koide3/monocular_person_following?utm_source=github.com&utm_medium=referral&utm_content=koide3/monocular_person_following&utm_campaign=Badge_Grade)　[![Build](https://github.com/koide3/monocular_person_following/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/monocular_person_following/actions/workflows/build.yml)\n\n## Dependencies\n\n- dlib\n- flann\n- tensorflow\n- tf-pose-estimation\n\n## Quick Test\n\n- [Quick test on a sample rosbag (Desktop PC with CUDA >= 10.2)](https://github.com/koide3/monocular_person_following/wiki/Quick-test-with-sample-rosbag)\n- [Quick test using a webcam (Desktop PC with CUDA >= 10.2 / Jetson Xavier with Jetpack 4.5.1)](https://github.com/koide3/monocular_person_following/wiki/Quick-test-with-USB-cam)\n\n![screenshot](data/imgs/screenshot.jpg)\n\n## Install\n\nWe tested this package on Xavier/Jetpack 4.5.1 \nNote: We recommend using docker to avod the painful installation process\n\n- [[Install from source (Jetson Xavier with Jetpack 4.5.1)]](https://github.com/koide3/monocular_person_following/wiki/%5BJetpack-4.5%5D-Install-from-source)\n\n\n## Setup your own person following robot\n\n[[Setup your robot]](https://github.com/koide3/monocular_person_following/wiki/Setup-your-own-person-following-robot)\n\n## Related packages\n\n- [ccf_person_identification](https://github.com/koide3/ccf_person_identification)\n- [monocular_people_tracking](https://github.com/koide3/monocular_people_tracking)\n- [monocular_person_following](https://github.com/koide3/monocular_person_following)\n\n\n## Papers\n- Kenji Koide, Jun Miura, and Emanuele Menegatti, Monocular Person Tracking and Identification with Online Deep Feature Selection for Person Following Robots, Robotics and Autonomous Systems [[link]](https://www.researchgate.net/publication/336871285_Monocular_Person_Tracking_and_Identification_with_On-line_Deep_Feature_Selection_for_Person_Following_Robots).\n\n\n- Kenji Koide and Jun Miura, Convolutional Channel Features-based Person Identification for Person Following Robots, 15th International Conference IAS-15, Baden-Baden, Germany, 2018 [[link]](https://www.researchgate.net/publication/325854919_Convolutional_Channel_Features-Based_Person_Identification_for_Person_Following_Robots_Proceedings_of_the_15th_International_Conference_IAS-15).\n" }, { "path": "docker/jp45/Dockerfile", "content": "FROM koide3/jetson-ros-noetic-desktop:jp45\n\n# editors\nRUN apt-get update && \\\n apt-get install -y --no-install-recommends \\\n vim nano tmux \\\n && rm -rf /var/lib/apt/lists/*\n\n# flann & openblas\nRUN apt-get update && \\\n apt-get install -y --no-install-recommends \\\n libflann-dev libopenblas-base libopenblas-dev \\\n && rm -rf /var/lib/apt/lists/*\n\n# tensorflow\nRUN apt-get update && \\\n apt-get install -y --no-install-recommends \\\n libhdf5-serial-dev hdf5-tools libhdf5-dev zlib1g-dev \\\n zip libjpeg8-dev liblapack-dev libblas-dev gfortran \\\n python3-pip \\\n && rm -rf /var/lib/apt/lists/*\n\nRUN pip3 install -U pip testresources setuptools\nRUN pip3 install --pre --extra-index-url https://developer.download.nvidia.com/compute/redist/jp/v45 \"tensorflow<2\"\n\n# tf-pose-estimation\nRUN apt-get update && \\\n apt-get install -y --no-install-recommends \\\n llvm-10 llvm-10-dev swig \\\n && rm -rf /var/lib/apt/lists/*\n\nRUN update-alternatives --install /usr/bin/llvm-config llvm-config /usr/bin/llvm-config-10 50\nRUN pip3 install numba==0.45.0\n\nWORKDIR /root/catkin_ws/src\nRUN git clone https://github.com/koide3/tf-pose-estimation\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation\nRUN pip3 install -r requirements.txt\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation/tf_pose/pafprocess\nRUN swig -python -c++ pafprocess.i && python3 setup.py build_ext --inplace\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation/models/graph/cmu\nRUN bash download.sh\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation\nRUN pip3 install -U matplotlib\nRUN python3 setup.py install\n\n# dlib\nWORKDIR /root\nRUN wget http://dlib.net/files/dlib-19.22.tar.bz2\nRUN tar xvf dlib-19.22.tar.bz2\nENV DLIB_ROOT=\"/root/dlib-19.22\"\n\n# other ros packages\nWORKDIR /root/catkin_ws/src\nRUN git clone https://github.com/ros-drivers/usb_cam.git\nRUN git clone https://github.com/ros-perception/image_transport_plugins.git\nRUN git clone https://github.com/ros-perception/vision_opencv.git\nRUN git clone https://github.com/ros-perception/image_common.git\nRUN git clone https://github.com/ros-perception/image_pipeline.git\n\nRUN touch /root/catkin_ws/src/image_transport_plugins/theora_image_transport/CATKIN_IGNORE\nRUN touch /root/catkin_ws/src/image_pipeline/stereo_image_proc/CATKIN_IGNORE\n\nWORKDIR /root/catkin_ws\nRUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make -DCMAKE_BUILD_TYPE=Release'\n\n# WORKDIR /root/catkin_ws/src\n# RUN https://github.com/ros/geometry.git\n\n# WORKDIR /root/catkin_ws\n# RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make -DCMAKE_BUILD_TYPE=Release'\n\n\n# monocular_person_following\nWORKDIR /root/catkin_ws/src\nRUN git clone https://github.com/koide3/open_face_recognition.git\nRUN git clone https://github.com/koide3/ccf_person_identification.git\nRUN git clone https://github.com/koide3/monocular_people_tracking.git --recursive\nRUN git clone https://github.com/koide3/monocular_person_following.git\n\nWORKDIR /root/catkin_ws\nRUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make -DCMAKE_BUILD_TYPE=Release'\nRUN sed -i \"7i source \\\"/root/catkin_ws/devel/setup.bash\\\"\" /ros_entrypoint.sh\n\nWORKDIR /root/catkin_ws/src\n\nENTRYPOINT [\"/ros_entrypoint.sh\"]\nCMD [\"bash\"]\n" }, { "path": "docker/jp45/Dockerfile.ros.noetic", "content": "# original dockerfile can be found at: https://github.com/dusty-nv/jetson-containers\r\n#\r\n# this dockerfile roughly follows the 'Installing from source' from:\r\n# http://wiki.ros.org/noetic/Installation/Source\r\n#\r\nARG BASE_IMAGE=nvcr.io/nvidia/l4t-base:r32.5.0\r\nFROM ${BASE_IMAGE}\r\n\r\nARG ROS_PKG=desktop\r\nENV ROS_DISTRO=noetic\r\nENV ROS_ROOT=/opt/ros/${ROS_DISTRO}\r\nENV ROS_PYTHON_VERSION=3\r\n\r\nENV DEBIAN_FRONTEND=noninteractive\r\n\r\nWORKDIR /workspace\r\n\r\n# add the ROS deb repo to the apt sources list\r\nRUN apt-get update && \\\r\n apt-get install -y --no-install-recommends \\\r\n git \\\r\n\t\tcmake \\\r\n\t\tbuild-essential \\\r\n\t\tcurl \\\r\n\t\twget \\\r\n\t\tgnupg2 \\\r\n\t\tlsb-release \\\r\n\t\tca-certificates \\\r\n && rm -rf /var/lib/apt/lists/*\r\n\r\nRUN sh -c 'echo \"deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main\" > /etc/apt/sources.list.d/ros-latest.list'\r\nRUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add -\r\n\r\n# install bootstrap dependencies\r\nRUN apt-get update && \\\r\n apt-get install -y --no-install-recommends \\\r\n libpython3-dev \\\r\n python3-rosdep \\\r\n python3-rosinstall-generator \\\r\n python3-vcstool \\\r\n build-essential && \\\r\n rosdep init && \\\r\n rosdep update && \\\r\n rm -rf /var/lib/apt/lists/*\r\n\r\nRUN ln -s /usr/lib/aarch64-linux-gnu/libboost_python3.a /usr/lib/aarch64-linux-gnu/libboost_python37.a\r\nRUN ln -s /usr/lib/aarch64-linux-gnu/libboost_python3.so /usr/lib/aarch64-linux-gnu/libboost_python37.so\r\n\r\n# download/build the ROS source\r\nRUN mkdir ros_catkin_ws && \\\r\n cd ros_catkin_ws && \\\r\n rosinstall_generator ${ROS_PKG} vision_msgs --rosdistro ${ROS_DISTRO} --deps --tar > ${ROS_DISTRO}-${ROS_PKG}.rosinstall && \\\r\n mkdir src && \\\r\n vcs import --input ${ROS_DISTRO}-${ROS_PKG}.rosinstall ./src && \\\r\n apt-get update && \\\r\n rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro ${ROS_DISTRO} --skip-keys python3-pykdl -y && \\\r\n python3 ./src/catkin/bin/catkin_make_isolated --install --install-space ${ROS_ROOT} -DCMAKE_BUILD_TYPE=Release && \\\r\n rm -rf /var/lib/apt/lists/*\r\n\r\nRUN rm -rf /workspace\r\n\r\n# setup entrypoint\r\nCOPY ros_entrypoint.sh /ros_entrypoint.sh\r\nRUN echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> /root/.bashrc\r\nENTRYPOINT [\"/ros_entrypoint.sh\"]\r\nCMD [\"bash\"]\r\nWORKDIR /\r\n" }, { "path": "docker/jp45/build.sh", "content": "#!/bin/bash\n# sudo docker build --tag koide3/jetson-ros-noetic-desktop:jp45 -f Dockerfile.ros.noetic .\nsudo docker build --tag koide3/monocular_person_following:jp45 -f Dockerfile .\n" }, { "path": "docker/jp45/ros_entrypoint.sh", "content": "#!/bin/bash\nset -e\n\nros_env_setup=\"/opt/ros/$ROS_DISTRO/setup.bash\"\necho \"sourcing $ros_env_setup\"\nsource \"$ros_env_setup\"\n\necho \"ROS_ROOT $ROS_ROOT\"\necho \"ROS_DISTRO $ROS_DISTRO\"\n\nexec \"$@\"\n" }, { "path": "docker/jp45/run.sh", "content": "#/bin/bash\nsudo docker run -it --rm \\\n --net host \\\n --gpus all \\\n --device /dev/video0:/dev/video0 \\\n -v $(realpath ~/.ros/camera_info):/root/.ros/camera_info \\\n koide3/monocular_person_following:jp45 $@\n" }, { "path": "docker/noetic/Dockerfile", "content": "FROM koide3/jetson-ros-noetic-desktop:noetic\n\nRUN apt-get update && \\\n apt-get install -y --no-install-recommends \\\n # editors\n vim nano tmux \\\n # flann & openblas\n libflann-dev libopenblas-base libopenblas-dev \\\n # tf-pose-estimation\n llvm-10 llvm-10-dev swig \\\n && rm -rf /var/lib/apt/lists/*\n\nRUN update-alternatives --install /usr/bin/llvm-config llvm-config /usr/bin/llvm-config-10 50\n\nWORKDIR /root/catkin_ws/src\nRUN git clone https://github.com/koide3/tf-pose-estimation\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation\nRUN pip3 install numba==0.45.0\nRUN pip3 install -r requirements.txt\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation/tf_pose/pafprocess\nRUN swig -python -c++ pafprocess.i && python3 setup.py build_ext --inplace\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation/models/graph/cmu\nRUN bash download.sh\n\nWORKDIR /root/catkin_ws/src/tf-pose-estimation\nRUN pip3 install -U matplotlib\nRUN python3 setup.py install\n\n# dlib\nWORKDIR /root\nRUN wget http://dlib.net/files/dlib-19.22.tar.bz2\nRUN tar xvf dlib-19.22.tar.bz2\nENV DLIB_ROOT=\"/root/dlib-19.22\"\n\n# other ros packages\nWORKDIR /root/catkin_ws/src\nRUN git clone https://github.com/ros-drivers/usb_cam.git\nRUN git clone https://github.com/ros-perception/image_transport_plugins.git\nRUN git clone https://github.com/ros-perception/vision_opencv.git\nRUN git clone https://github.com/ros-perception/image_common.git\nRUN git clone https://github.com/ros-perception/image_pipeline.git\n\nRUN touch /root/catkin_ws/src/image_transport_plugins/theora_image_transport/CATKIN_IGNORE\nRUN touch /root/catkin_ws/src/image_pipeline/stereo_image_proc/CATKIN_IGNORE\n\nWORKDIR /root/catkin_ws\nRUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make -DCMAKE_BUILD_TYPE=Release'\n\n# monocular_person_following\nWORKDIR /root/catkin_ws/src\nRUN git clone https://github.com/koide3/open_face_recognition.git\nRUN git clone https://github.com/koide3/ccf_person_identification.git\nRUN git clone https://github.com/koide3/monocular_people_tracking.git --recursive\nCOPY . /root/catkin_ws/monocular_person_following\n\nWORKDIR /root/catkin_ws\nRUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make -DCMAKE_BUILD_TYPE=Release'\nRUN sed -i \"7i source \\\"/root/catkin_ws/devel/setup.bash\\\"\" /ros_entrypoint.sh\n\nWORKDIR /root/catkin_ws/src\n\nENTRYPOINT [\"/ros_entrypoint.sh\"]\nCMD [\"bash\"]\n" }, { "path": "docker/noetic/Dockerfile.ros.noetic", "content": "# original dockerfile can be found at: https://github.com/dusty-nv/jetson-containers\r\n#\r\n# this dockerfile roughly follows the 'Installing from source' from:\r\n# http://wiki.ros.org/noetic/Installation/Source\r\n#\r\nFROM tensorflow/tensorflow:1.15.5-gpu-py3\r\n\r\nARG ROS_PKG=desktop\r\nENV ROS_DISTRO=noetic\r\nENV ROS_ROOT=/opt/ros/${ROS_DISTRO}\r\nENV ROS_PYTHON_VERSION=3\r\n\r\nENV DEBIAN_FRONTEND=noninteractive\r\n\r\nWORKDIR /workspace\r\n\r\n# add the ROS deb repo to the apt sources list\r\nRUN apt-get update && \\\r\n apt-get install -y --no-install-recommends \\\r\n git cmake build-essential curl wget \\\r\n\t\tgnupg2 lsb-release ca-certificates \\\r\n && rm -rf /var/lib/apt/lists/*\r\n\r\nRUN sh -c 'echo \"deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main\" > /etc/apt/sources.list.d/ros-latest.list'\r\nRUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add -\r\n\r\n# install bootstrap dependencies\r\nRUN apt-get update && \\\r\n apt-get install -y --no-install-recommends \\\r\n libpython3-dev python3-rosdep python3-rosinstall-generator \\\r\n python3-vcstool build-essential && \\\r\n rosdep init && \\\r\n rosdep update && \\\r\n rm -rf /var/lib/apt/lists/*\r\n\r\nRUN ln -s /usr/lib/x86_64-linux-gnu/libboost_python3.a /usr/lib/x86_64-linux-gnu/libboost_python37.a\r\nRUN ln -s /usr/lib/x86_64-linux-gnu/libboost_python3.so /usr/lib/x86_64-linux-gnu/libboost_python37.so\r\n\r\n# download/build the ROS source\r\nRUN mkdir ros_catkin_ws && \\\r\n cd ros_catkin_ws && \\\r\n rosinstall_generator ${ROS_PKG} vision_msgs --rosdistro ${ROS_DISTRO} --deps --tar > ${ROS_DISTRO}-${ROS_PKG}.rosinstall && \\\r\n mkdir src && \\\r\n vcs import --input ${ROS_DISTRO}-${ROS_PKG}.rosinstall ./src && \\\r\n apt-get update && \\\r\n rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro ${ROS_DISTRO} --skip-keys python3-pykdl -y && \\\r\n python ./src/catkin/bin/catkin_make_isolated --install --install-space ${ROS_ROOT} -DCMAKE_BUILD_TYPE=Release && \\\r\n rm -rf /var/lib/apt/lists/* && \\\r\n rm -rf /workspace\r\n\r\n# setup entrypoint\r\nCOPY ros_entrypoint.sh /ros_entrypoint.sh\r\nRUN echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> /root/.bashrc\r\nENTRYPOINT [\"/ros_entrypoint.sh\"]\r\nCMD [\"bash\"]\r\nWORKDIR /\r\n" }, { "path": "docker/noetic/build.sh", "content": "#!/bin/bash\n# sudo docker build --tag koide3/jetson-ros-noetic-desktop:noetic -f Dockerfile.ros.noetic .\nsudo docker build --tag koide3/monocular_person_following:noetic -f Dockerfile .\n" }, { "path": "docker/noetic/ros_entrypoint.sh", "content": "#!/bin/bash\nset -e\n\nros_env_setup=\"/opt/ros/$ROS_DISTRO/setup.bash\"\necho \"sourcing $ros_env_setup\"\nsource \"$ros_env_setup\"\n\necho \"ROS_ROOT $ROS_ROOT\"\necho \"ROS_DISTRO $ROS_DISTRO\"\n\nexec \"$@\"\n" }, { "path": "docker/noetic/run.sh", "content": "#/bin/bash\nsudo docker run -it --rm \\\n --net host \\\n --gpus all \\\n --device /dev/video0:/dev/video0 \\\n -v $(realpath ~/.ros/camera_info):/root/.ros/camera_info \\\n koide3/monocular_person_following:noetic $@\n" }, { "path": "include/monocular_person_following/context.hpp", "content": "#ifndef MONOCULAR_PERSON_FOLLOWING_CONTEXT_HPP\n#define MONOCULAR_PERSON_FOLLOWING_CONTEXT_HPP\n\n#include \n#include \n#include \n#include \n\n#include \n#include \n\nnamespace ccf_person_classifier {\n\nclass PersonInput;\nclass PersonFeatures;\nclass PersonClassifier;\n\n}\n\n\nnamespace monocular_person_following {\n\nclass Context {\npublic:\n using PersonInput = ccf_person_classifier::PersonInput;\n using PersonFeatures = ccf_person_classifier::PersonFeatures;\n using PersonClassifier = ccf_person_classifier::PersonClassifier;\n\n Context(ros::NodeHandle& nh);\n ~Context();\n\npublic:\n void extract_features(const cv::Mat& bgr_image, std::unordered_map& tracks);\n bool update_classifier(double label, const Tracklet::Ptr& track);\n boost::optional predict(const Tracklet::Ptr& track);\n\n std::vector classifier_names() const;\n cv::Mat visualize_body_features();\n\nprivate:\n std::mt19937 mt;\n std::unique_ptr classifier;\n std::unordered_map> classifier_confidences;\n\n boost::circular_buffer> pos_feature_bank;\n boost::circular_buffer> neg_feature_bank;\n};\n\n}\n\n#endif\n" }, { "path": "include/monocular_person_following/state/initial_state.hpp", "content": "#ifndef MONOCULAR_PERSON_FOLLOWING_INITIAL_STATE_HPP\n#define MONOCULAR_PERSON_FOLLOWING_INITIAL_STATE_HPP\n\n#include \n\nnamespace monocular_person_following {\n\nclass InitialState : public State {\npublic:\n InitialState() {}\n\n virtual ~InitialState() override {}\n\n virtual std::string state_name() const override {\n return \"initial\";\n }\n\n virtual State* update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) override;\n\nprivate:\n long select_target(ros::NodeHandle& nh, const std::unordered_map& tracks);\n};\n\n}\n\n#endif // INITIAL_STATE_HPP\n" }, { "path": "include/monocular_person_following/state/initial_training_state.hpp", "content": "#ifndef MONOCULAR_PERSON_FOLLOWING_INITIAL_TRAINING_STATE_HPP\n#define MONOCULAR_PERSON_FOLLOWING_INITIAL_TRAINING_STATE_HPP\n\n#include \n\nnamespace monocular_person_following {\n\nclass InitialTrainingState : public State {\npublic:\n InitialTrainingState(long target_id)\n : target_id(target_id),\n num_pos_samples(0)\n {}\n\n virtual ~InitialTrainingState() override {}\n\n virtual long target() const override {\n return target_id;\n }\n\n virtual std::string state_name() const override {\n return \"initial_training\";\n }\n\n virtual State* update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) override;\n\nprivate:\n long target_id;\n long num_pos_samples;\n};\n\n}\n\n#endif // INITIAL_TRAINING_STATE_HPP\n" }, { "path": "include/monocular_person_following/state/reid_state.hpp", "content": "#ifndef REID_STATE_HPP\n#define REID_STATE_HPP\n\n#include \n\nnamespace monocular_person_following {\n\nclass ReidState : public State {\npublic:\n ReidState() {}\n virtual ~ReidState() override {}\n\n virtual std::string state_name() const override {\n return \"re-identification\";\n }\n\n virtual State* update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) override;\n\nprivate:\n std::unordered_map positive_count;\n};\n\n}\n\n#endif // REID_STATE_HPP\n" }, { "path": "include/monocular_person_following/state/state.hpp", "content": "#ifndef MONOCULAR_PERSON_FOLLOWING_STATE_HPP\n#define MONOCULAR_PERSON_FOLLOWING_STATE_HPP\n\n#include \n\nnamespace monocular_person_following {\n\nclass State {\npublic:\n State() {}\n virtual ~State() {}\n\n virtual long target() const { return -1; }\n\n virtual std::string state_name() const = 0;\n\n virtual State* update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) = 0;\nprivate:\n\n};\n\n}\n\n#endif\n" }, { "path": "include/monocular_person_following/state/tracking_state.hpp", "content": "#ifndef TRACKING_STATE_HPP\n#define TRACKING_STATE_HPP\n\n#include \n\nnamespace monocular_person_following {\n\nclass TrackingState : public State {\npublic:\n TrackingState(long target_id)\n : target_id(target_id)\n {}\n\n virtual ~TrackingState() override {}\n\n virtual long target() const override {\n return target_id;\n }\n\n virtual std::string state_name() const override {\n return \"tracking\";\n }\n\n virtual State* update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) override;\n\nprivate:\n long target_id;\n};\n\n}\n\n#endif // TRACKING_STATE_HPP\n" }, { "path": "include/monocular_person_following/tracklet.hpp", "content": "#ifndef TRACKLET_HPP\n#define TRACKLET_HPP\n\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\nnamespace monocular_person_following {\n\nstruct Tracklet {\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n using Ptr = std::shared_ptr;\n\n Tracklet(tf::TransformListener& tf_listener, const std_msgs::Header& header, const monocular_people_tracking::Track& track_msg)\n : confidence(boost::none),\n track_msg(&track_msg)\n {\n geometry_msgs::PointStamped point;\n point.header = header;\n point.point = track_msg.pos;\n\n geometry_msgs::PointStamped transformed;\n tf_listener.transformPoint(\"base_link\", point, transformed);\n\n pos_in_baselink = Eigen::Vector2f(transformed.point.x, transformed.point.y);\n }\n\npublic:\n boost::optional confidence;\n std::vector classifier_confidences;\n\n cv::Mat face_image;\n\n boost::optional person_region;\n ccf_person_classifier::Input::Ptr input;\n ccf_person_classifier::Features::Ptr features;\n\n Eigen::Vector2f pos_in_baselink;\n const monocular_people_tracking::Track* track_msg;\n};\n\n}\n\n#endif // TRACKLET_HPP\n" }, { "path": "launch/jetson_person_following.launch", "content": "\n\n \n\n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "launch/jetson_person_following_sim.launch", "content": "\n\n \n\n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "launch/monocular_person_following.launch", "content": "\n\n \n\n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n" }, { "path": "launch/start_robot.launch", "content": "\n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n\n" }, { "path": "msg/BoundingBox2D.msg", "content": "int32 x\nint32 y\nint32 width\nint32 height\n" }, { "path": "msg/FaceDetection.msg", "content": "int64 track_id\n\nBoundingBox2D[] face_roi\nBoundingBox2D[] face_region\nsensor_msgs/Image[] face_image\n" }, { "path": "msg/FaceDetectionArray.msg", "content": "std_msgs/Header header\n\nuint32 image_width\nuint32 image_height\n\nFaceDetection[] faces\n" }, { "path": "msg/Target.msg", "content": "std_msgs/Header header\n\nstd_msgs/String state\nint64 target_id\n\nint64[] track_ids\nfloat32[] confidences\n\nstd_msgs/String[] classifier_names\nfloat32[] classifier_confidences\n" }, { "path": "package.xml", "content": "\n\n monocular_person_following\n 0.0.0\n The monocular_person_following package\n\n koide\n\n BSD\n\n catkin\n tf\n rospy\n roscpp\n cv_bridge\n image_transport\n message_filters\n message_generation\n eigen_conversions\n ccf_person_identification\n monocular_people_tracking\n\n \n \n\n" }, { "path": "rviz/monocular_person_following.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 84\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /TF1/Frames1\n Splitter Ratio: 0.5\n Tree Height: 100\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: \"\"\nPreferences:\n PromptSaveOnExit: true\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 13068\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: LaserScan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /scan_tf\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 12958\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: LaserScan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /scan_tr\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 15634\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: LaserScan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /scan_bf\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 11609\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: LaserScan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /scan_br\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Class: rviz/TF\n Enabled: true\n Frame Timeout: 15\n Frames:\n All Enabled: false\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4101\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: LaserScan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /scan_bottom\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 11846\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: LaserScan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /scan_top\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 892\n Min Color: 0; 0; 0\n Min Intensity: 148\n Name: LaserScan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Flat Squares\n Topic: /scan\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /monocular_people_tracking/markers\n Name: MarkerArray\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /monocular_person_following/features\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /visualization_node/visualize\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Default Light: true\n Fixed Frame: base_link\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Topic: /initialpose\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 12.880599975585938\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 1.4971981048583984\n Y: -0.11331728100776672\n Z: -1.1117677688598633\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.9347967505455017\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 2.420490026473999\n Saved: ~\nWindow Geometry:\n \"&Displays\":\n collapsed: false\n \"&Image\":\n collapsed: false\n \"&Time\":\n collapsed: false\n \"&Views\":\n collapsed: false\n Height: 1021\n Hide Left Dock: false\n Hide Right Dock: false\n I&mage:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Tool Properties:\n collapsed: false\n Width: 1855\n X: 57\n Y: 0\n" }, { "path": "scripts/eval/convert_dataset2bag.py", "content": "#!/usr/bin/python\nimport os\nimport sys\nimport cv2\nimport numpy\n\nimport rospy\nimport rosbag\n\nfrom sensor_msgs.msg import *\n\n\ndef main():\n\tinput_dir = sys.argv[1]\n\toutput_bag = sys.argv[2]\n\n\tcamera_info = CameraInfo()\n\tcamera_info.K = [300, 0, 320, 0, 300, 240, 0, 0, 1]\n\tcamera_info.D = [0, 0, 0, 0]\n\n\timage_files = sorted([input_dir + '/' + x for x in os.listdir(input_dir) if '.jpg' in x])\n\n\twith rosbag.Bag(output_bag, 'w') as dst_bag:\n\t\tstamp = rospy.Time(0)\n\t\tfor image_file in image_files:\n\t\t\tprint image_file\n\t\t\timage = cv2.imread(image_file)\n\t\t\tif image is None:\n\t\t\t\tprint 'failed to read image...'\n\t\t\t\tcontinue\n\n\t\t\tstamp += rospy.Duration(1.0 / 14.2)\n\n\t\t\tcompressed_msg = CompressedImage()\n\t\t\tcompressed_msg.header.stamp = stamp\n\t\t\tcompressed_msg.header.frame_id = 'camera_optical_frame'\n\t\t\tcamera_info.header = compressed_msg.header\n\n\t\t\tret, encoded = cv2.imencode('.jpg', image)\n\t\t\tbuf = numpy.vstack([encoded]).tostring()\n\n\t\t\tcompressed_msg.data = buf\n\n\t\t\tdst_bag.write('/top_front_camera/camera_info', camera_info, stamp)\n\t\t\tdst_bag.write('/top_front_camera/image_raw/compressed', compressed_msg, stamp)\n\n\t\t\tif rospy.is_shutdown():\n\t\t\t\tbreak\n\n\nif __name__ == '__main__':\n\tmain()\n" }, { "path": "scripts/eval/generate_bb.py", "content": "#!/usr/bin/python\nimport sys\nimport numpy\nimport rosbag\n\n\ndef main():\n\timages_bag = sys.argv[1]\n\tresults_bag = images_bag[:images_bag.find('.bag')] + '_results.bag'\n\n\tdst_file = sys.argv[2]\n\n\t'read image timestamps'\n\timage_stamps = []\n\twith rosbag.Bag(images_bag, 'r') as bag:\n\t\tfor topic, msg, stamp in bag.read_messages():\n\t\t\tif 'compressed' not in topic:\n\t\t\t\tcontinue\n\n\t\t\timage_stamps.append(msg.header.stamp)\n\n\ttimestamp_map = {}\n\tfor i, stamp in enumerate(image_stamps):\n\t\ttimestamp_map[stamp] = i\n\n\tinit_bbox = None\n\twith rosbag.Bag(results_bag, 'r') as bag:\n\t\tprint 'read results'\n\t\tall_msgs = sorted([x for x in bag.read_messages()], key=lambda x: x[2])\n\n\t\ttarget_id = -1\n\n\t\tbbs = {}\n\t\tfor topic, msg, stamp in all_msgs:\n\t\t\tif 'target' in topic:\n\t\t\t\ttarget_id = msg.target_id\n\t\t\tif 'tracks' in topic:\n\t\t\t\ttarget = [x for x in msg.tracks if x.id == target_id]\n\n\t\t\t\tif not len(target):\n\t\t\t\t\tcontinue\n\n\t\t\t\tif False:\n\t\t\t\t\tneck_ankle = target[0].expected_measurement_mean\n\t\t\t\t\tneck = numpy.float32(neck_ankle[:2])\n\t\t\t\t\tankle = numpy.float32(neck_ankle[2:])\n\t\t\t\telse:\n\t\t\t\t\tif not len(target[0].associated_neck_ankle):\n\t\t\t\t\t\tcontinue\n\n\t\t\t\t\tneck_ankle = target[0].associated_neck_ankle\n\t\t\t\t\tneck = numpy.float32([neck_ankle[0].x, neck_ankle[0].y])\n\t\t\t\t\tankle = numpy.float32([neck_ankle[1].x, neck_ankle[1].y])\n\n\t\t\t\tcenter = (neck + ankle) / 2.0\n\t\t\t\theight = (ankle[1] - neck[1])\n\t\t\t\twidth = height * 0.25\n\n\t\t\t\tcenter[1] -= height * 0.1\n\t\t\t\theight = height * 1.1\n\n\t\t\t\tif width > 1e6 or height > 1e6:\n\t\t\t\t\tcontinue\n\n\t\t\t\ttl = numpy.int32((center - (width / 2, height / 2)))\n\t\t\t\twh = numpy.int32((width, height))\n\n\t\t\t\timage_id = timestamp_map[msg.header.stamp]\n\t\t\t\tbbs[image_id] = tl.tolist() + wh.tolist()\n\n\t\t\t\tif init_bbox is None:\n\t\t\t\t\tinit_bbox = tl.tolist() + wh.tolist()\n\n\tprint init_bbox\n\twith open(dst_file, 'w') as file:\n\t\tbbox = init_bbox\n\t\tfor i, timestamp in enumerate(image_stamps):\n\t\t\tif i in bbs:\n\t\t\t\tbbox = bbs[i]\n\n\t\t\tprint >> sys.stdout, '%d\\t%d\\t%d\\t%d\\t%d' % tuple([i] + bbox)\n\t\t\tprint >> file, '%d\\t%d\\t%d\\t%d\\t%d' % tuple([i] + bbox)\n\n\nif __name__ == '__main__':\n\tmain()\n" }, { "path": "scripts/face_detector_node.py", "content": "#!/usr/bin/python\nimport cv2\nimport numpy\nimport dlib\nimport rospy\nimport cv_bridge\nimport message_filters\nfrom tfpose_ros.msg import *\nfrom sensor_msgs.msg import *\nfrom monocular_people_tracking.msg import *\nfrom monocular_person_following.msg import *\n\n\nclass FaceDetectorNode:\n\tdef __init__(self):\n\t\tself.cv_bridge = cv_bridge.CvBridge()\n\t\tself.face_detector = dlib.get_frontal_face_detector()\n\t\tself.roi_scale = rospy.get_param('~roi_scale', 1.0)\n\t\tself.face_detection_upscale_pyramid = rospy.get_param('~face_detection_upscale_pyramid', 1)\n\t\tself.confidence_thresh = rospy.get_param('~confidence_thresh', -0.5)\n\t\tself.face_expantion_scale = rospy.get_param('~face_expantion_scale', 1.5)\n\n\t\tsubs = [\n\t\t\tmessage_filters.Subscriber('image_rect', Image),\n\t\t\tmessage_filters.Subscriber('/monocular_people_tracking/tracks', TrackArray)\n\t\t]\n\t\tself.sync = message_filters.TimeSynchronizer(subs, 30)\n\t\tself.sync.registerCallback(self.callback)\n\n\t\tself.faces_pub = rospy.Publisher('~faces', FaceDetectionArray, queue_size=5)\n\n\tdef callback(self, image_msg, tracks_msg):\n\t\timage = self.cv_bridge.imgmsg_to_cv2(image_msg, 'bgr8')\n\n\t\tfaces_msg = FaceDetectionArray()\n\t\tfaces_msg.header = image_msg.header\n\t\tfaces_msg.image_width = image_msg.width\n\t\tfaces_msg.image_height = image_msg.height\n\n\t\tfor person in tracks_msg.tracks:\n\t\t\tface_msg = FaceDetection()\n\t\t\tface_msg.track_id = person.id\n\t\t\tfaces_msg.faces.append(face_msg)\n\n\t\t\troi = self.calc_roi(image.shape[1], image.shape[0], person)\n\t\t\tif roi is None:\n\t\t\t\tcontinue\n\n\t\t\tface_msg.face_roi.append(self.tlbr2bb(roi[0], roi[1]))\n\n\t\t\tface = self.detect_face(image, roi)\n\t\t\tif face is None:\n\t\t\t\tcontinue\n\n\t\t\tface_msg.face_region.append(self.tlbr2bb(face[0], face[1]))\n\t\t\tface_image = self.extract_roi(image, face[0], face[1])\n\t\t\tface_msg.face_image.append(self.cv_bridge.cv2_to_imgmsg(face_image, 'bgr8'))\n\n\t\tself.faces_pub.publish(faces_msg)\n\n\tdef calc_roi(self, width, height, track_msg):\n\t\tif not len(track_msg.associated):\n\t\t\treturn None\n\n\t\tpose = track_msg.associated[0]\n\t\thead = [x for x in pose.body_part if x.part_id == 0]\n\t\tneck = [x for x in pose.body_part if x.part_id == 1]\n\n\t\tif not len(head) or not len(neck):\n\t\t\treturn None\n\n\t\timage_size = numpy.float32([width, height])\n\t\thead = numpy.float32([head[0].x, head[0].y]) * image_size\n\t\tneck = numpy.float32([neck[0].x, neck[0].y]) * image_size\n\n\t\tneck2head = (neck[1] - head[1])\n\n\t\ttl = head - neck2head * self.roi_scale\n\t\tbr = head + neck2head * self.roi_scale\n\n\t\ttl = max(0, min(width, tl[0])), max(0, min(height, tl[1]))\n\t\tbr = max(0, min(width, br[0])), max(0, min(height, br[1]))\n\n\t\treturn tuple(numpy.int32(tl)), tuple(numpy.int32(br))\n\n\tdef detect_face(self, image, roi):\n\t\tif roi[1][0] - roi[0][0] < 10 or roi[1][1] - roi[0][1] < 10:\n\t\t\tprint 'too small roi!!'\n\t\t\treturn None\n\n\t\ttl, br = roi\n\n\t\troi_image = self.extract_roi(image, tl, br)\n\t\tdets, confidences, labels = self.face_detector.run(roi_image, self.face_detection_upscale_pyramid, self.confidence_thresh)\n\n\t\tif not len(dets):\n\t\t\treturn None\n\n\t\tlargest = sorted(dets, key=lambda x: -x.area())[0]\n\n\t\tcenter = largest.dcenter()\n\t\tface_left = int(center.x - (center.x - largest.left()) * self.face_expantion_scale)\n\t\tface_top = int(center.y - (center.y - largest.top()) * self.face_expantion_scale)\n\t\tface_right = int(center.x + (center.x - largest.left()) * self.face_expantion_scale)\n\t\tface_bottom = int(center.y + (center.y - largest.top()) * self.face_expantion_scale)\n\n\t\tface_tl = (tl[0] + face_left, tl[1] + face_top)\n\t\tface_br = (tl[0] + face_right, tl[1] + face_bottom)\n\t\treturn face_tl, face_br\n\n\tdef extract_roi(self, image, tl, br):\n\t\treturn image[tl[1]: br[1], tl[0]: br[0], :]\n\n\tdef tlbr2bb(self, tl, br):\n\t\tbb = BoundingBox2D()\n\t\tbb.x = tl[0]\n\t\tbb.y = tl[1]\n\t\tbb.width = br[0] - tl[0]\n\t\tbb.height = br[1] - tl[1]\n\n\t\treturn bb\n\n\ndef main():\n\tprint '--- face_detector_node ---'\n\trospy.init_node('face_detector_node')\n\tnode = FaceDetectorNode()\n\n\tprint 'ready'\n\trospy.spin()\n\nif __name__ == '__main__':\n\tmain()\n" }, { "path": "scripts/robot_controller.py", "content": "#!/usr/bin/python\nimport tf\nimport math\nimport rospy\n\nfrom geometry_msgs.msg import *\nfrom monocular_people_tracking.msg import *\nfrom monocular_person_following.msg import *\n\n\nclass RobotControllerNode:\n\tdef __init__(self):\n\t\tself.tf_listener = tf.TransformListener()\n\n\t\tself.enable_back = rospy.get_param('~enable_back', True)\n\t\tself.max_vx = rospy.get_param('~max_vx', 0.2)\n\t\tself.max_va = rospy.get_param('~max_va', 0.5)\n\t\tself.gain_vx = rospy.get_param('~gain_vx', 0.3)\n\t\tself.gain_va = rospy.get_param('~gain_va', 0.3)\n\t\tself.distance = rospy.get_param('~distance', 3.25)\n\t\tself.timeout = rospy.get_param('~timeout', 1.0)\n\n\t\tself.last_time = rospy.Time(0)\n\t\tself.target_id = -1\n\n\t\tself.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)\n\t\tself.target_sub = rospy.Subscriber('/monocular_person_following/target', Target, self.target_callback)\n\t\tself.tracks_sub = rospy.Subscriber('/monocular_people_tracking/tracks', TrackArray, self.tracks_callback)\n\n\tdef target_callback(self, target_msg):\n\t\tself.target_id = target_msg.target_id\n\n\tdef tracks_callback(self, tracks_msg):\n\t\ttarget = [x for x in tracks_msg.tracks if x.id == self.target_id]\n\n\t\tif not len(target):\n\t\t\tself.cmd_vel_pub.publish(Twist())\n\t\t\treturn\n\n\t\tpoint = PointStamped()\n\t\tpoint.header = tracks_msg.header\n\t\tpoint.point = target[0].pos\n\n\t\ttry:\n\t\t\tself.tf_listener.waitForTransform('base_link', tracks_msg.header.frame_id, tracks_msg.header.stamp, rospy.Duration(0.5))\n\t\t\ttarget_pos = self.tf_listener.transformPoint('base_link', point)\n\t\texcept:\n\t\t\tprint 'failed to lookup transform between base_link and', tracks_msg.header.frame_id\n\t\t\tself.cmd_vel_pub.publish(Twist())\n\t\t\treturn\n\n\t\ttheta = math.atan2(target_pos.point.y, target_pos.point.x)\n\t\tprint 'x, y, theta:', target_pos.point.x, target_pos.point.y, theta, math.radians(45)\n\n\t\tva = min(self.max_va, max(-self.max_va, theta * self.gain_va))\n\t\tvx = 0.0\n\t\tif abs(theta) < math.radians(45):\n\t\t\tvx = (target_pos.point.x - self.distance) * self.gain_vx\n\t\t\tprint 'raw vx', vx\n\t\t\tmin_vx = -self.max_vx if self.enable_back else 0.0\n\t\t\tvx = min(self.max_vx, max(min_vx, vx))\n\t\telse:\n\t\t\tprint 'rotation too big'\n\n\n\t\ttwist = Twist()\n\t\ttwist.linear.x = vx\n\t\ttwist.angular.z = va\n\n\t\tself.cmd_vel_pub.publish(twist)\n\t\tself.last_time = tracks_msg.header.stamp\n\n\tdef spin(self):\n\t\tif (rospy.Time.now() - self.last_time).to_sec() > self.timeout:\n\t\t\tprint 'timeout!!'\n\t\t\tself.cmd_vel_pub.publish(Twist())\n\n\ndef main():\n\trospy.init_node('robot_controller_node')\n\tnode = RobotControllerNode()\n\n\trate = rospy.Rate(10)\n\twhile not rospy.is_shutdown():\n\t\tnode.spin()\n\t\trate.sleep()\n\nif __name__ == '__main__':\n\tmain()\n" }, { "path": "scripts/save_bag.sh", "content": "#!/bin/bash\n\nrosbag record -O $1 \"/visualization_node/visualize/compressed\" \"/tf\" \"/scan\" -e \".*/markers\" -e \".*/tracks\" -e \".*/target\" -e \".*/features/compressed\" -e \".*/pose\"\n" }, { "path": "scripts/simple_gesture_recognition.py", "content": "#!/usr/bin/env python3\nimport rospy\nfrom monocular_people_tracking.msg import *\nfrom monocular_person_following.srv import *\n\n\nclass GestureRecognizer:\n\tdef __init__(self, track_id):\n\t\tself.track_id = track_id\n\t\tself.last_stamp = rospy.Time.now()\n\n\tdef callback(self, track_msg, imprint):\n\t\tif not len(track_msg.associated):\n\t\t\tself.last_stamp = rospy.Time.now()\n\t\t\treturn\n\n\t\tskeleton = track_msg.associated[0]\n\n\t\tneck = [x for x in skeleton.body_part if x.part_id == 1]\n\t\tr_elbow = [x for x in skeleton.body_part if x.part_id == 3]\n\t\tr_hand = [x for x in skeleton.body_part if x.part_id == 4]\n\n\t\tif not len(neck) or not len(r_elbow) or not len(r_hand):\n\t\t\tself.last_stamp = rospy.Time.now()\n\t\t\treturn\n\n\t\tif r_elbow[0].y < neck[0].y and r_hand[0].y < neck[0].y:\n\t\t\tprint(self.track_id, rospy.Time.now() - self.last_stamp)\n\t\t\tif rospy.Time.now() - self.last_stamp > rospy.Duration(5.0):\n\t\t\t\timprint(track_msg.id)\n\t\t\t\tself.last_stamp = rospy.Time.now()\n\n\nclass SimpleGestureRecognitionNode:\n\tdef __init__(self):\n\t\tprint('--- simple_gesture_recognition ---')\n\t\tself.recognizers = {}\n\t\tprint('wait for service')\n\t\trospy.wait_for_service('/monocular_person_following/imprint')\n\t\tself.imprint_service = rospy.ServiceProxy('/monocular_person_following/imprint', Imprint)\n\n\t\tself.sub = rospy.Subscriber('/monocular_people_tracking/tracks', TrackArray, self.callback)\n\t\tprint('done')\n\n\tdef callback(self, track_msg):\n\t\tfor track in track_msg.tracks:\n\t\t\tif track.id not in self.recognizers:\n\t\t\t\tself.recognizers[track.id] = GestureRecognizer(track.id)\n\n\t\t\tself.recognizers[track.id].callback(track, self.imprint)\n\n\tdef imprint(self, target_id):\n\t\tprint('reset target', target_id)\n\t\treq = ImprintRequest()\n\t\treq.target_id = target_id\n\n\t\tself.imprint_service(req)\n\n\ndef main():\n\trospy.init_node('simple_gesture_recognition')\n\tnode = SimpleGestureRecognitionNode()\n\trospy.spin()\n\n\nif __name__ == '__main__':\n\tmain()\n" }, { "path": "scripts/visualization.py", "content": "#!/usr/bin/env python3\nimport cv2\nimport math\nimport numpy\n\nimport tf\nimport rospy\nimport cv_bridge\nimport message_filters\nfrom tfpose_ros.msg import *\nfrom sensor_msgs.msg import *\nfrom monocular_people_tracking.msg import *\nfrom monocular_person_following.msg import *\n\nfrom tf_pose import common\nfrom tf_pose.estimator import Human, BodyPart\n\n\nclass VisualizationNode:\n\tdef __init__(self):\n\t\tself.tf_listener = tf.TransformListener()\n\t\tself.image_pub = rospy.Publisher('~visualize', Image, queue_size=1)\n\n\t\tcolor_palette = numpy.uint8([(180 / 10 * i, 255, 255) for i in range(10)]).reshape(-1, 1, 3)\n\t\tself.color_palette = cv2.cvtColor(color_palette, cv2.COLOR_HSV2BGR).reshape(-1, 3)\n\n\t\tself.target_id = 0\n\t\tself.state_name = \"NONE\"\n\t\tself.confidences = {}\n\t\tself.use_face = rospy.get_param('~use_face', True)\n\t\tself.target_sub = rospy.Subscriber('/monocular_person_following/target', Target, self.target_callback)\n\n\t\tself.image = numpy.zeros((128, 128, 3), dtype=numpy.uint8)\n\t\tsubs = [\n\t\t\tmessage_filters.Subscriber('image_rect', Image),\n\t\t\tmessage_filters.Subscriber('/pose_estimator/pose', Persons),\n\t\t\tmessage_filters.Subscriber('/monocular_people_tracking/tracks', TrackArray)\n\t\t]\n\n\t\tif self.use_face:\n\t\t\tsubs.append(message_filters.Subscriber('/face_detector/faces', FaceDetectionArray))\n\t\t\tself.sync = message_filters.TimeSynchronizer(subs, 50)\n\t\t\tself.sync.registerCallback(self.callback)\n\t\telse:\n\t\t\tself.sync = message_filters.TimeSynchronizer(subs, 50)\n\t\t\tself.sync.registerCallback(self.callback_wo_face)\n\n\tdef target_callback(self, target_msg):\n\t\tself.state_name = target_msg.state.data\n\t\tself.target_id = target_msg.target_id\n\n\t\tfor i in range(len(target_msg.track_ids)):\n\t\t\ttrack_id = target_msg.track_ids[i]\n\t\t\tconfidence = target_msg.confidences[i]\n\t\t\tif track_id not in self.confidences:\n\t\t\t\tself.confidences[track_id] = {}\n\n\t\t\tself.confidences[track_id]['conf'] = confidence\n\n\t\t\tnum_classifiers = len(target_msg.classifier_names)\n\t\t\tfor j in range(num_classifiers):\n\t\t\t\tself.confidences[track_id][target_msg.classifier_names[j].data] = target_msg.classifier_confidences[num_classifiers * i + j]\n\n\tdef callback_wo_face(self, image_msg, poses_msg, tracks_msg):\n\t\tself.callback(image_msg, poses_msg, tracks_msg, None)\n\n\tdef callback(self, image_msg, poses_msg, tracks_msg, faces_msg):\n\t\timage = cv_bridge.CvBridge().imgmsg_to_cv2(image_msg, 'bgr8')\n\n\t\thumans = []\n\t\tfor p_idx, person in enumerate(poses_msg.persons):\n\t\t\thuman = Human([])\n\t\t\tfor body_part in person.body_part:\n\t\t\t\tpart = BodyPart('', body_part.part_id, body_part.x, body_part.y, body_part.confidence)\n\t\t\t\thuman.body_parts[body_part.part_id] = part\n\n\t\t\thumans.append(human)\n\n\t\timage = self.draw_humans(image, humans, imgcopy=False)\n\n\t\tfor track in tracks_msg.tracks:\n\t\t\tself.draw_expected_measurement(image, track)\n\t\t\tself.draw_bounding_box(image, track)\n\n\t\t\tif track.id == self.target_id:\n\t\t\t\tself.draw_target_icon(image, track)\n\n\t\tif faces_msg is not None:\n\t\t\tface_scale = image_msg.width / float(faces_msg.image_width)\n\n\t\t\tdef draw_face(bb, color, line_width):\n\t\t\t\ttl = numpy.float32((bb.x, bb.y)) * face_scale\n\t\t\t\tbr = numpy.float32((bb.x + bb.width, bb.y + bb.height)) * face_scale\n\n\t\t\t\tcv2.rectangle(image, tuple(tl.astype(numpy.int32)), tuple(br.astype(numpy.int32)), color, line_width)\n\n\t\t\tfor face in faces_msg.faces:\n\t\t\t\tif not len(face.face_roi):\n\t\t\t\t\tcontinue\n\t\t\t\tdraw_face(face.face_roi[0], (255, 0, 0), 1)\n\n\t\t\t\tif not len(face.face_region):\n\t\t\t\t\tcontinue\n\n\t\t\t\tface_confidence = 0.0\n\t\t\t\tif face.track_id in self.confidences:\n\t\t\t\t\tface_confidence = self.confidences[face.track_id]['face']\n\t\t\t\tface_confidence = (face_confidence + 1.0) / 2.0\n\n\t\t\t\tcolor = (0, int(face_confidence * 255), int((1.0 - face_confidence) * 255))\n\t\t\t\tdraw_face(face.face_region[0], color, 2)\n\n\t\tcv2.putText(image, self.state_name, (15, 30), cv2.FONT_HERSHEY_PLAIN, 1.5, (0, 0, 0), 3)\n\t\tcv2.putText(image, self.state_name, (15, 30), cv2.FONT_HERSHEY_PLAIN, 1.5, (255, 255, 255), 1)\n\n\t\tself.image = image\n\n\t# taken from tfpose_ros/tf_pose/estimator.py\n\tdef draw_humans(self, npimg, humans, imgcopy=False):\n\t\tif imgcopy:\n\t\t\tnpimg = np.copy(npimg)\n\n\t\tcanvas = npimg.copy()\n\t\timage_h, image_w = npimg.shape[:2]\n\t\tcenters = {}\n\t\tfor human in humans:\n\t\t\t# draw point\n\t\t\tfor i in range(common.CocoPart.Background.value):\n\t\t\t\tif i not in human.body_parts.keys():\n\t\t\t\t\tcontinue\n\n\t\t\t\tbody_part = human.body_parts[i]\n\t\t\t\tcenter = (int(body_part.x * image_w + 0.5), int(body_part.y * image_h + 0.5))\n\t\t\t\tcenters[i] = center\n\t\t\t\tcv2.circle(canvas, center, 3, common.CocoColors[i], thickness=3, lineType=8, shift=0)\n\n\t\t\t# draw line\n\t\t\tfor pair_order, pair in enumerate(common.CocoPairsRender):\n\t\t\t\tif pair[0] not in human.body_parts.keys() or pair[1] not in human.body_parts.keys():\n\t\t\t\t\tcontinue\n\n\t\t\t\t# npimg = cv2.line(npimg, centers[pair[0]], centers[pair[1]], common.CocoColors[pair_order], 3)\n\t\t\t\tcv2.line(canvas, centers[pair[0]], centers[pair[1]], common.CocoColors[pair_order], 3)\n\n\t\tnpimg = npimg / 2 + canvas / 2\n\t\treturn npimg\n\n\tdef draw_expected_measurement(self, image, track):\n\t\tmeas_mean = numpy.float32(track.expected_measurement_mean).flatten()\n\t\tmeas_cov = numpy.float32(track.expected_measurement_cov).reshape(4, 4)\n\n\t\tdef error_ellipse(cov, kai):\n\t\t\tw, v = numpy.linalg.eig(cov)\n\n\t\t\textents = numpy.sqrt(kai * kai * w)\n\t\t\tangle = math.atan2(v[0, 1], v[1, 1])\n\n\t\t\treturn (extents[0], extents[1], angle)\n\n\t\tneck_ellipse = error_ellipse(meas_cov[:2, :2], 3.0)\n\t\tankle_ellipse = error_ellipse(meas_cov[2:, 2:], 3.0)\n\t\tneck_pos = tuple(meas_mean[:2].astype(numpy.int32))\n\t\tankle_pos = tuple(meas_mean[2:].astype(numpy.int32))\n\n\t\tcolor = self.color_palette[track.id % len(self.color_palette)]\n\t\tcolor = tuple(int(x) for x in color)\n\n\t\tcv2.ellipse(image, neck_pos, neck_ellipse[:2], neck_ellipse[-1], 0, 360, color, 2)\n\t\tcv2.ellipse(image, ankle_pos, ankle_ellipse[:2], neck_ellipse[-1], 0, 360, color, 2)\n\t\tcv2.line(image, neck_pos, ankle_pos, color, 2)\n\n\tdef draw_bounding_box(self, image, track):\n\t\tneck_ankle = numpy.float32(track.expected_measurement_mean).flatten()\n\t\tcenter = (neck_ankle[:2] + neck_ankle[2:]) / 2.0\n\t\theight = (neck_ankle[-1] - neck_ankle[1]) * 1.5\n\t\twidth = height * 0.25\n\t\thalf_extents = (width / 2.0, height / 2.0)\n\n\t\ttl = tuple(numpy.int32(center - half_extents))\n\t\tbr = tuple(numpy.int32(center + half_extents))\n\n\t\tcv2.putText(image, \"id:%d\" % track.id, (tl[0] + 5, tl[1] + 15), cv2.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 0), 2)\n\t\tcv2.putText(image, \"id:%d\" % track.id, (tl[0] + 5, tl[1] + 15), cv2.FONT_HERSHEY_PLAIN, 1.0, (255, 255, 255), 1)\n\n\t\tbody_confidence = 0.0\n\t\tif track.id in self.confidences:\n\t\t\tbody_confidence = self.confidences[track.id]['body']\n\n\t\t\tdef order(name):\n\t\t\t\treturn {'conf': 0, 'body': 1, 'face': 2}[name]\n\t\t\tkeys = sorted(self.confidences[track.id].keys(), key=order)\n\n\t\t\tfor i, key in enumerate(keys):\n\t\t\t\tconf = self.confidences[track.id][key]\n\n\t\t\t\tcv2.putText(image, \"%s:%.2f\" % (key, conf), (tl[0] + 5, br[1] - 5 - 12 * i), cv2.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 0), 2)\n\t\t\t\tcv2.putText(image, \"%s:%.2f\" % (key, conf), (tl[0] + 5, br[1] - 5 - 12 * i), cv2.FONT_HERSHEY_PLAIN, 1.0, (255, 255, 255), 1)\n\n\t\tbody_confidence = body_confidence + 0.5\n\t\tcolor = (0, int(255 * body_confidence), int(255 * (1 - body_confidence)))\n\t\tcv2.rectangle(image, tl, br, color, 2)\n\n\tdef draw_target_icon(self, image, track):\n\t\tneck_ankle = numpy.float32(track.expected_measurement_mean).flatten()\n\t\theight = neck_ankle[-1] - neck_ankle[1]\n\n\t\tpt = (neck_ankle[:2] + (0, -height * 0.33)).astype(numpy.int32)\n\t\tpts = numpy.array([pt, pt + (15, -15), pt + (-15, -15)]).reshape(-1, 1, 2)\n\t\tcv2.polylines(image, [pts], True, (0, 0, 255), 2)\n\n\tdef spin(self):\n\t\tif rospy.get_param('~show', True):\n\t\t\tcv2.imshow('image', self.image)\n\t\t\tcv2.waitKey(10)\n\n\t\tif self.image_pub.get_num_connections():\n\t\t\timg_msg = cv_bridge.CvBridge().cv2_to_imgmsg(self.image.astype(numpy.uint8), 'bgr8')\n\t\t\tself.image_pub.publish(img_msg)\n\n\ndef main():\n\trospy.init_node('visualization_node')\n\tnode = VisualizationNode()\n\n\trate = rospy.Rate(15)\n\twhile not rospy.is_shutdown():\n\t\tnode.spin()\n\t\trate.sleep()\n\n\nif __name__ == '__main__':\n\tmain()\n" }, { "path": "src/monocular_person_following/context.cpp", "content": "#include \n\n#include \n#include \n\nnamespace monocular_person_following {\n\nContext::Context(ros::NodeHandle& nh) {\n classifier.reset(new PersonClassifier(nh));\n\n pos_feature_bank.resize(nh.param(\"feature_bank_size\", 32));\n neg_feature_bank.resize(nh.param(\"feature_bank_size\", 32));\n}\n\nContext::~Context() {}\n\n\nvoid Context::extract_features(const cv::Mat& bgr_image, std::unordered_map& tracks) {\n for(auto& track: tracks) {\n if(!track.second->person_region) {\n continue;\n }\n\n if(track.second->person_region->width < 20 || track.second->person_region->height < 20) {\n continue;\n }\n\n track.second->input.reset(new PersonInput());\n track.second->features.reset(new PersonFeatures());\n\n std::unordered_map images;\n images[\"body\"] = cv::Mat(bgr_image, *track.second->person_region);\n images[\"face\"] = track.second->face_image;\n\n if(!classifier->extractInput(track.second->input, images)) {\n ROS_WARN_STREAM(\"failed to extract input data\");\n continue;\n }\n\n if(!classifier->extractFeatures(track.second->features, track.second->input)) {\n ROS_WARN_STREAM(\"failed to extract input features\");\n continue;\n }\n }\n}\n\n\nbool Context::update_classifier(double label, const Tracklet::Ptr& track) {\n auto pred = classifier->predict(track->features, classifier_confidences[track->track_msg->id]);\n if(pred) {\n track->confidence = *pred;\n }\n track->classifier_confidences = classifier_confidences[track->track_msg->id];\n\n auto& p_bank = label > 0.0 ? pos_feature_bank : neg_feature_bank;\n auto& n_bank = label > 0.0 ? neg_feature_bank : pos_feature_bank;\n\n if(!n_bank.empty()) {\n size_t i = std::uniform_int_distribution<>(0, n_bank.size())(mt);\n classifier->update(-label, n_bank[i]);\n }\n\n if(!p_bank.full()) {\n p_bank.push_back(track->features);\n } else {\n size_t i = std::uniform_int_distribution<>(0, p_bank.size())(mt);\n p_bank[i] = track->features;\n }\n\n return classifier->update(label, track->features);\n}\n\nboost::optional Context::predict(const Tracklet::Ptr& track) {\n auto pred = classifier->predict(track->features, classifier_confidences[track->track_msg->id]);\n if(pred) {\n track->confidence = *pred;\n }\n track->classifier_confidences = classifier_confidences[track->track_msg->id];\n\n return pred;\n}\n\nstd::vector Context::classifier_names() const {\n return classifier->classifierNames();\n}\n\n\ncv::Mat Context::visualize_body_features() {\n ccf_person_classifier::BodyClassifier::Ptr body_classifier = classifier->getClassifier(\"body\");\n if(body_classifier) {\n cv::Mat feature_map = body_classifier->visualize();\n return feature_map;\n }\n\n return cv::Mat();\n}\n\n\n}\n\n\n" }, { "path": "src/monocular_person_following/state/initial_state.cpp", "content": "#include \n\n#include \n\nnamespace monocular_person_following {\n\nState* InitialState::update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) {\n long target_id = select_target(nh, tracks);\n\n if(target_id < 0) {\n return this;\n }\n\n return new InitialTrainingState(target_id);\n}\n\nlong InitialState::select_target(ros::NodeHandle& nh, const std::unordered_map& tracks) {\n long target_id = -1;\n double distance = 0.0;\n\n for(const auto& track: tracks) {\n const auto& pos = track.second->pos_in_baselink;\n if(pos.x() > nh.param(\"imprinting_max_dist\", 4.0) || std::abs(pos.y()) > pos.x()) {\n continue;\n }\n\n if(target_id == -1 || distance > pos.norm()) {\n target_id = track.first;\n distance = pos.norm();\n }\n }\n\n return target_id;\n}\n\n}\n" }, { "path": "src/monocular_person_following/state/initial_training_state.cpp", "content": "#include \n\n#include \n#include \n\nnamespace monocular_person_following {\n\nState* InitialTrainingState::update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) {\n auto found = tracks.find(target_id);\n if(found == tracks.end()) {\n ROS_INFO_STREAM(\"lost target during the initial training!!\");\n return new InitialState();\n }\n\n for(const auto& track: tracks) {\n double label = track.first == target_id ? 1.0 : -1.0;\n boost::optional pred = context.predict(track.second);\n\n context.update_classifier(label, track.second);\n\n if(label > 0.0) {\n num_pos_samples ++;\n }\n }\n\n if(num_pos_samples >= nh.param(\"initial_training_num_samples\", 10)) {\n return new TrackingState(target_id);\n }\n\n return this;\n}\n\n}\n" }, { "path": "src/monocular_person_following/state/reid_state.cpp", "content": "#include \n\n#include \n\nnamespace monocular_person_following {\n\nState* ReidState::update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) {\n for(const auto& track: tracks) {\n boost::optional pred = context.predict(track.second);\n\n if(!pred || pred < nh.param(\"reid_confidence_thresh\", 0.2)) {\n continue;\n }\n\n auto found = positive_count.find(track.first);\n if(found == positive_count.end()) {\n positive_count[track.first] = 0;\n }\n positive_count[track.first] ++;\n\n if(positive_count[track.first] >= nh.param(\"reid_positive_count\", 5)) {\n return new TrackingState(track.first);\n }\n }\n\n return this;\n}\n\n}\n" }, { "path": "src/monocular_person_following/state/tracking_state.cpp", "content": "#include \n\n#include \n\nnamespace monocular_person_following {\n\nState* TrackingState::update(ros::NodeHandle& nh, Context& context, const std::unordered_map& tracks) {\n auto found = tracks.find(target_id);\n if(found == tracks.end()) {\n ROS_INFO_STREAM(\"target lost!!\");\n return new ReidState();\n }\n\n boost::optional pred = context.predict(found->second);\n if(pred && *pred < nh.param(\"id_switch_detection_thresh\", -0.1)) {\n ROS_INFO_STREAM(\"ID switch detected!!\");\n return new ReidState();\n }\n\n if(!pred || *pred < nh.param(\"min_target_cofidence\", 0.1)) {\n ROS_INFO_STREAM(\"do not update\");\n return this;\n }\n\n for(const auto& track: tracks) {\n double label = track.first == target_id ? 1.0 : -1.0;\n context.update_classifier(label, track.second);\n }\n\n return this;\n}\n\n\n}\n" }, { "path": "src/monocular_person_following_node.cpp", "content": "#include \n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\n\nnamespace monocular_person_following {\n\nclass MonocularPersonFollowingNode {\npublic:\n MonocularPersonFollowingNode()\n : nh(),\n private_nh(\"~\"),\n target_pub(nh.advertise(\"/monocular_person_following/target\", 1)),\n image_trans(nh),\n features_pub(image_trans.advertise(\"/monocular_person_following/features\", 1)),\n image_sub(nh, \"image\", 10),\n tracks_sub(nh, \"/monocular_people_tracking/tracks\", 10),\n faces_sub(nh, \"/face_detector/faces\", 10),\n sync(image_sub, tracks_sub, 30),\n sync_w_face(image_sub, tracks_sub, faces_sub, 30),\n reset_sub(private_nh.subscribe(\"reset\", 10, &MonocularPersonFollowingNode::reset_callback, this)),\n reset_service_server(private_nh.advertiseService(\"reset\", &MonocularPersonFollowingNode::reset_service, this)),\n imprint_service_server(private_nh.advertiseService(\"imprint\", &MonocularPersonFollowingNode::imprint_service, this))\n {\n state.reset(new InitialState());\n context.reset(new Context(private_nh));\n\n if(private_nh.param(\"use_face\", true)) {\n sync_w_face.registerCallback(boost::bind(&MonocularPersonFollowingNode::callback, this, _1, _2, _3));\n } else {\n sync.registerCallback(boost::bind(&MonocularPersonFollowingNode::callback, this, _1, _2, nullptr));\n }\n }\n\n void callback(const sensor_msgs::ImageConstPtr& image_msg, const monocular_people_tracking::TrackArrayConstPtr& tracks_msg, const monocular_person_following::FaceDetectionArrayConstPtr& faces_msg) {\n auto cv_image = cv_bridge::toCvCopy(image_msg, \"bgr8\");\n\n std::unordered_map tracks;\n\n std::unordered_map face_msgs;\n if(faces_msg != nullptr) {\n for(const auto& face : faces_msg->faces) {\n face_msgs[face.track_id] = &face;\n }\n }\n\n for(const auto& track : tracks_msg->tracks) {\n tracks[track.id].reset(new Tracklet(tf_listener, tracks_msg->header, track));\n\n if(track.associated_neck_ankle.empty()) {\n continue;\n }\n\n cv::Rect person_region = calc_person_region(track, cv_image->image.size());\n tracks[track.id]->person_region = person_region;\n\n auto face = face_msgs.find(track.id);\n if(face != face_msgs.end() && !face->second->face_image.empty()) {\n auto face_image = cv_bridge::toCvCopy(face->second->face_image[0], \"bgr8\");\n tracks[track.id]->face_image = face_image->image;\n }\n }\n\n std::lock_guard lock(context_mutex);\n context->extract_features(cv_image->image, tracks);\n\n State* next_state = state->update(private_nh, *context, tracks);\n if(next_state != state.get()) {\n state.reset(next_state);\n }\n\n if(target_pub.getNumSubscribers()) {\n Target target;\n target.header = image_msg->header;\n target.state.data = state->state_name();\n target.target_id = state->target();\n\n target.track_ids.reserve(tracks_msg->tracks.size());\n target.confidences.reserve(tracks_msg->tracks.size());\n target.classifier_confidences.reserve(tracks_msg->tracks.size() * 2);\n\n std::vector classifier_names = context->classifier_names();\n for(const auto& name: classifier_names) {\n std_msgs::String classifier_name;\n classifier_name.data = name;\n target.classifier_names.push_back(classifier_name);\n }\n\n for(const auto& track : tracks) {\n if(track.second->confidence) {\n target.track_ids.push_back(track.first);\n target.confidences.push_back(*track.second->confidence);\n\n if(track.second->classifier_confidences.size() != target.classifier_names.size()) {\n ROS_ERROR_STREAM(\"num_classifiers did not match!!\");\n ROS_ERROR_STREAM(track.second->classifier_confidences.size() << \" : \" << target.classifier_names.size());\n }\n std::copy(track.second->classifier_confidences.begin(), track.second->classifier_confidences.end(), std::back_inserter(target.classifier_confidences));\n }\n }\n\n target_pub.publish(target);\n }\n\n if(features_pub.getNumSubscribers()) {\n cv::Mat features = context->visualize_body_features();\n if(features.data) {\n cv_bridge::CvImage cv_image(image_msg->header, \"bgr8\", features);\n features_pub.publish(cv_image.toImageMsg());\n }\n }\n }\n\n bool imprint_service(ImprintRequest& req, ImprintResponse& res) {\n reset(req.target_id);\n res.success = true;\n\n return true;\n }\n\n bool reset_service(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) {\n reset();\n\n return true;\n }\n\n void reset_callback(const std_msgs::EmptyConstPtr& empty_msg) {\n reset();\n }\n\nprivate:\n cv::Rect2f calc_person_region(const monocular_people_tracking::Track& track, const cv::Size& image_size) {\n Eigen::Vector2f neck(track.associated_neck_ankle[0].x, track.associated_neck_ankle[0].y);\n Eigen::Vector2f ankle(track.associated_neck_ankle[1].x, track.associated_neck_ankle[1].y);\n\n Eigen::Vector2f center = (neck + ankle) / 2.0f;\n float height = (ankle.y() - neck.y()) * 1.25f;\n float width = height * 0.25f;\n\n cv::Rect rect(center.x() - width / 2.0f, center.y() - height / 2.0f, width, height);\n\n cv::Point tl = rect.tl();\n cv::Point br = rect.br();\n\n tl.x = std::min(image_size.width, std::max(0, tl.x));\n tl.y = std::min(image_size.height, std::max(0, tl.y));\n br.x = std::min(image_size.width, std::max(0, br.x));\n br.y = std::min(image_size.height, std::max(0, br.y));\n\n return cv::Rect(tl, br);\n }\n\n void reset(long target_id = -1) {\n ROS_INFO_STREAM(\"reset identification!!\");\n std::lock_guard lock(context_mutex);\n\n if(target_id < 0) {\n state.reset(new InitialState());\n } else {\n state.reset(new InitialTrainingState(target_id));\n }\n context.reset(new Context(private_nh));\n }\n\nprivate:\n ros::NodeHandle nh;\n ros::NodeHandle private_nh;\n\n tf::TransformListener tf_listener;\n\n ros::Publisher target_pub;\n\n image_transport::ImageTransport image_trans;\n image_transport::Publisher features_pub;\n\n message_filters::Subscriber image_sub;\n message_filters::Subscriber tracks_sub;\n message_filters::Subscriber faces_sub;\n message_filters::TimeSynchronizer sync;\n message_filters::TimeSynchronizer sync_w_face;\n\n // reset service callbacks\n ros::Subscriber reset_sub;\n ros::ServiceServer reset_service_server;\n ros::ServiceServer imprint_service_server;\n\n std::mutex context_mutex;\n std::shared_ptr state;\n std::unique_ptr context;\n};\n\n}\n\nint main(int argc, char** argv) {\n ros::init(argc, argv, \"monocular_person_following\");\n std::unique_ptr node(new monocular_person_following::MonocularPersonFollowingNode());\n ros::spin();\n\n return 0;\n}\n" }, { "path": "srv/Imprint.srv", "content": "int64 target_id\n---\nbool success\n" } ]