├── FoxyDockerfile ├── JetsonDockerfile ├── Makefile ├── NoeticDockerfile ├── README.md ├── ncore ├── CMakeLists.txt ├── data │ └── floor │ │ ├── GTMap │ │ ├── 0 │ │ │ ├── Map.png │ │ │ ├── Map.yaml │ │ │ ├── Map_Empty.png │ │ │ ├── SemMaps │ │ │ │ ├── board.png │ │ │ │ ├── box.png │ │ │ │ ├── cabinet.png │ │ │ │ ├── chair.png │ │ │ │ ├── desk.png │ │ │ │ ├── door.png │ │ │ │ ├── drawers.png │ │ │ │ ├── fire extinguisher.png │ │ │ │ ├── oven.png │ │ │ │ ├── person.png │ │ │ │ ├── potted plant.png │ │ │ │ ├── sink.png │ │ │ │ ├── sofa.png │ │ │ │ └── table.png │ │ │ ├── floor.config │ │ │ ├── global_variance.csv │ │ │ ├── global_variance.json │ │ │ └── roomseg.png │ │ ├── building.config │ │ └── nmcl.config │ │ ├── ICPMap │ │ ├── 0 │ │ │ ├── Map.png │ │ │ ├── Map.yaml │ │ │ ├── Map_Empty.png │ │ │ ├── SemMaps │ │ │ │ ├── board.png │ │ │ │ ├── box.png │ │ │ │ ├── cabinet.png │ │ │ │ ├── chair.png │ │ │ │ ├── desk.png │ │ │ │ ├── door.png │ │ │ │ ├── drawers.png │ │ │ │ ├── fire extinguisher.png │ │ │ │ ├── oven.png │ │ │ │ ├── person.png │ │ │ │ ├── potted plant.png │ │ │ │ ├── sink.png │ │ │ │ ├── sofa.png │ │ │ │ └── table.png │ │ │ ├── floor.config │ │ │ ├── global_variance.csv │ │ │ ├── global_variance.json │ │ │ └── roomseg.png │ │ ├── building.config │ │ └── nmcl.config │ │ └── KPMap │ │ ├── 0 │ │ ├── Map.png │ │ ├── Map.yaml │ │ ├── Map_Empty.png │ │ ├── SemMaps │ │ │ ├── board.png │ │ │ ├── box.png │ │ │ ├── cabinet.png │ │ │ ├── chair.png │ │ │ ├── desk.png │ │ │ ├── door.png │ │ │ ├── drawers.png │ │ │ ├── fire extinguisher.png │ │ │ ├── oven.png │ │ │ ├── person.png │ │ │ ├── potted plant.png │ │ │ ├── sink.png │ │ │ ├── sofa.png │ │ │ └── table.png │ │ ├── floor.config │ │ ├── global_variance.csv │ │ ├── global_variance.json │ │ └── roomseg.png │ │ ├── building.config │ │ └── nmcl.config ├── nmap │ ├── CMakeLists.txt │ ├── include │ │ ├── BuildingMap.h │ │ ├── FloorMap.h │ │ ├── GMap.h │ │ ├── IMap2D.h │ │ ├── Lift.h │ │ ├── Object.h │ │ └── Room.h │ ├── src │ │ ├── BuildingMap.cpp │ │ ├── CMakeLists.txt │ │ ├── FloorMap.cpp │ │ ├── GMap.cpp │ │ ├── Lift.cpp │ │ ├── Object.cpp │ │ ├── Room.cpp │ │ └── RoomSegmentation.cpp │ └── tst │ │ ├── CMakeLists.txt │ │ └── NMapUnitTests.cpp ├── nmcl │ ├── CMakeLists.txt │ ├── include │ │ ├── MixedFSR.h │ │ ├── NMCLFactory.h │ │ ├── Particle.h │ │ ├── ParticleFilter.h │ │ ├── ReNMCL.h │ │ ├── Resampling.h │ │ ├── Semantic3DData.h │ │ ├── SemanticOverlap.h │ │ └── SetStatistics.h │ ├── src │ │ ├── CMakeLists.txt │ │ ├── MixedFSR.cpp │ │ ├── NMCLFactory.cpp │ │ ├── Particle.cpp │ │ ├── ParticleFilter.cpp │ │ ├── ReNMCL.cpp │ │ ├── Resampling.cpp │ │ ├── SemanticOverlap.cpp │ │ ├── SemanticVisibility (copy).cpp │ │ └── SetStatistics.cpp │ └── tst │ │ ├── Analysis.cpp │ │ ├── Analysis.h │ │ ├── BenchMark.cpp │ │ ├── CMakeLists.txt │ │ ├── NMCLSanityTests.cpp │ │ ├── NMCLUnitTests.cpp │ │ └── verifyInit.py └── nsensors │ ├── CMakeLists.txt │ ├── include │ ├── Camera.h │ ├── Lidar2D.h │ ├── OptiTrack.h │ └── Utils.h │ ├── src │ ├── CMakeLists.txt │ ├── Camera.cpp │ ├── Lidar2D.cpp │ ├── OptiTrack.cpp │ └── Utils.cpp │ └── tst │ ├── CMakeLists.txt │ └── NSensorsUnitTests.cpp ├── resources ├── 3dmap.png ├── OmniExample.png ├── traj.png └── trajectory.pdf ├── ros1_ws └── src │ ├── CMakeLists.txt │ ├── data_processing │ ├── CMakeLists.txt │ ├── package.xml │ └── srv │ │ └── NoArguments.srv │ ├── nmcl_msgs │ ├── CMakeLists.txt │ ├── msg │ │ ├── Omni3D.msg │ │ └── Omni3DArray.msg │ └── package.xml │ ├── nmcl_ros │ ├── CMakeLists.txt │ ├── include │ │ └── RosUtils.h │ ├── launch │ │ ├── confignmcl.launch │ │ ├── demo.launch │ │ ├── dingo_localization.launch │ │ ├── youbot_eval.launch │ │ └── youbot_localization.launch │ ├── package.xml │ ├── rviz │ │ ├── Omni.rviz │ │ ├── confignmcl.rviz │ │ ├── demo.rviz │ │ ├── nmcl.rviz │ │ └── smap.rviz │ └── src │ │ ├── CMakeLists.txt │ │ ├── ConfigNMCLNode.cpp │ │ ├── GMAP.py │ │ ├── RosUtils.cpp │ │ └── SemanticMapNode.py │ └── omni3d_ros │ ├── CMakeLists.txt │ ├── configs │ ├── Base.yaml │ ├── Map.png │ ├── Map.yaml │ ├── args.json │ ├── category_meta.json │ ├── omnidataset.yaml │ ├── roomseg.png │ ├── trained_Omni3D.yaml │ └── variance.csv │ ├── launch │ ├── broadcast.launch │ ├── dingo_omni3d.launch │ ├── gt.launch │ └── omni3d.launch │ ├── package.xml │ ├── rviz │ ├── broadcast.rviz │ ├── mapping.rviz │ └── omni.rviz │ └── src │ ├── BroadcastMapNode.py │ ├── ComputeStatsOnPred.py │ ├── DatasetUtils.py │ ├── GMAP.py │ ├── GTNode.py │ ├── InterTest.py │ ├── JSONUtils.py │ ├── Lab3DObject.py │ ├── MapObjectTracker.py │ ├── NewAutomaticLabeling.py │ ├── ObjectUtils.py │ ├── Omni3DCategory.py │ ├── Omni3DDataset.py │ ├── Omni3DImage.py │ ├── Omni3DInfo.py │ ├── Omni3DMappingNode.py │ ├── Omni3DNode.py │ ├── Omni3DObject.py │ ├── SIMP.py │ ├── SIMPNode.py │ └── VisUtils.py ├── ros2_entrypoint.sh ├── ros2_ws └── src │ ├── nmcl_msgs │ ├── CMakeLists.txt │ ├── msg │ │ ├── Omni3D.msg │ │ └── Omni3DArray.msg │ ├── package.xml │ └── srv │ │ └── NoArguments.srv │ ├── nmcl_ros │ ├── CMakeLists.txt │ ├── include │ │ └── RosUtils.h │ ├── launch │ │ └── confignmcl.launch │ ├── package.xml │ ├── rviz │ │ └── config.rviz │ └── src │ │ ├── ConfigNMCLNode.cpp │ │ └── RosUtils.cpp │ └── omni3d_ros │ ├── configs │ ├── Base.yaml │ ├── Map.png │ ├── Map.yaml │ ├── args.json │ ├── category_meta.json │ ├── omnidataset.yaml │ ├── roomseg.png │ ├── trained_Omni3D.yaml │ └── variance.csv │ ├── launch │ ├── gt.launch │ └── omni3d.launch │ ├── omni3d_ros │ ├── BroadcastMapNode.py │ ├── ComputeStatsOnPred.py │ ├── DatasetUtils.py │ ├── GMAP.py │ ├── GTNode.py │ ├── InterTest.py │ ├── JSONUtils.py │ ├── Lab3DObject.py │ ├── MapObjectTracker.py │ ├── NewAutomaticLabeling.py │ ├── ObjectUtils.py │ ├── Omni3DCategory.py │ ├── Omni3DDataset.py │ ├── Omni3DImage.py │ ├── Omni3DInfo.py │ ├── Omni3DMappingNode.py │ ├── Omni3DNode.py │ ├── Omni3DObject.py │ ├── SIMP.py │ ├── SIMPNode.py │ ├── SemanticMapNode.py │ ├── VisUtils.py │ └── __init__.py │ ├── package.xml │ ├── resource │ └── omni3d_ros │ ├── rviz │ ├── broadcast.rviz │ ├── mapping.rviz │ └── omni.rviz │ ├── setup.cfg │ └── setup.py └── ros_entrypoint.sh /FoxyDockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:11.4.0-devel-ubuntu20.04 2 | 3 | ENV DEBIAN_FRONTEND noninteractive 4 | ENV NVIDIA_VISIBLE_DEVICES all 5 | ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute 6 | 7 | 8 | # Install some basic utilities 9 | RUN apt-get update && apt-get install -y \ 10 | curl \ 11 | ca-certificates \ 12 | sudo \ 13 | git \ 14 | bzip2 \ 15 | libx11-6 \ 16 | python3 \ 17 | python3-dev \ 18 | python3.9 \ 19 | python3.9-dev \ 20 | python3-pip \ 21 | cmake \ 22 | wget \ 23 | tar \ 24 | libx11-dev \ 25 | xorg-dev \ 26 | libssl-dev \ 27 | build-essential \ 28 | tmux \ 29 | libusb-1.0-0-dev && apt-get clean && rm -rf /var/lib/apt/lists/* 30 | 31 | 32 | 33 | # Install miniconda 34 | #ENV CONDA_DIR /opt/conda 35 | #RUN wget --quiet https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O ~/miniconda.sh && \ 36 | # /bin/bash ~/miniconda.sh -b -p /opt/conda 37 | 38 | # Put conda in path so we can use conda activate 39 | #ENV PATH=$CONDA_DIR/bin:$PATH 40 | 41 | 42 | RUN apt-get update && \ 43 | apt-get install -y software-properties-common && \ 44 | rm -rf /var/lib/apt/lists/* 45 | RUN apt-add-repository universe 46 | RUN apt-add-repository multiverse 47 | RUN apt-add-repository restricted 48 | 49 | RUN pip install torch==1.12.1+cu113 torchvision==0.13.1+cu113 torchaudio==0.12.1 --extra-index-url https://download.pytorch.org/whl/cu113 50 | RUN pip install 'git+https://github.com/cocodataset/cocoapi.git#subdirectory=PythonAPI' 51 | RUN pip install opencv-python 52 | 53 | RUN git clone https://github.com/facebookresearch/detectron2.git && cd detectron2 && ls && pip install -e . 54 | RUN pip install fvcore iopath 55 | RUN curl -LO https://github.com/NVIDIA/cub/archive/1.10.0.tar.gz 56 | RUN tar xzf 1.10.0.tar.gz 57 | ENV CUB_HOME=$PWD/cub-1.10.0 58 | RUN pip install --no-index --no-cache-dir pytorch3d -f https://dl.fbaipublicfiles.com/pytorch3d/packaging/wheels/py38_cu113_pyt1120/download.html 59 | RUN pip install cython scipy seaborn open3d 60 | 61 | 62 | # Install ROS 63 | ARG ROS_DISTRO=foxy 64 | ARG INSTALL_PACKAGE=desktop 65 | 66 | RUN apt-get update -q && \ 67 | apt-get install -y curl gnupg2 lsb-release && \ 68 | curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ 69 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ 70 | apt-get update -q && \ 71 | apt-get install -y gosu ros-${ROS_DISTRO}-${INSTALL_PACKAGE} \ 72 | python3-argcomplete \ 73 | python3-colcon-common-extensions \ 74 | python3-rosdep python3-vcstool \ 75 | ros-${ROS_DISTRO}-navigation2 \ 76 | ros-${ROS_DISTRO}-nav2-bringup \ 77 | '~ros-${ROS_DISTRO}-turtlebot3-.*' \ 78 | ros-${ROS_DISTRO}-gazebo-ros-pkgs && \ 79 | rosdep init && \ 80 | rm -rf /var/lib/apt/lists/* 81 | 82 | 83 | 84 | # Install our code 85 | 86 | RUN apt-get update && \ 87 | apt-get install -y nlohmann-json3-dev libgtest-dev libeigen3-dev vim ros-foxy-tf-transformations \ 88 | ros-foxy-pcl-conversions ros-foxy-pcl-msgs ros-foxy-pcl-ros && \ 89 | rm -rf /var/lib/apt/lists/* 90 | 91 | RUN pip install transforms3d 92 | WORKDIR /SIMP 93 | COPY ncore ncore 94 | COPY ros2_ws ros2_ws 95 | SHELL ["/bin/bash", "-c"] 96 | COPY ros2_entrypoint.sh / 97 | RUN chmod +x /ros2_entrypoint.sh 98 | ENTRYPOINT ["/ros2_entrypoint.sh"] 99 | CMD ["bash"] -------------------------------------------------------------------------------- /JetsonDockerfile: -------------------------------------------------------------------------------- 1 | FROM dustynv/ros:noetic-pytorch-l4t-r35.1.0 2 | #RUN apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 42D5A192B819C5DA 3 | RUN apt-get update && \ 4 | 5 | #-> Install general usage dependencies 6 | echo "Installing general usage dependencies ..." && \ 7 | apt-get install -y \ 8 | git \ 9 | cmake \ 10 | wget \ 11 | tar \ 12 | libx11-dev \ 13 | xorg-dev \ 14 | libssl-dev \ 15 | build-essential \ 16 | libusb-1.0-0-dev 17 | ENV NVIDIA_VISIBLE_DEVICES all 18 | ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute 19 | 20 | WORKDIR /packages 21 | RUN pip install 'git+https://github.com/cocodataset/cocoapi.git#subdirectory=PythonAPI' 22 | 23 | #RUN git clone https://github.com/facebookresearch/detectron2.git && \ 24 | COPY detectron2 detectron2 25 | RUN cd detectron2 && ls && \ 26 | pip install -e . 27 | 28 | 29 | #RUN git clone https://github.com/facebookresearch/pytorch3d.git && \ 30 | COPY pytorch3d pytorch3d 31 | RUN cd pytorch3d && FORCE_CUDA=1 pip install -e . 32 | RUN pip install cython scipy seaborn 33 | WORKDIR /omni3d_ws 34 | 35 | RUN pip install open3d 36 | RUN apt-get install -y tmux 37 | 38 | 39 | #RUN git clone https://github.com/facebookresearch/omni3d.git 40 | 41 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | build: 2 | ifdef foxy 3 | docker build -f FoxyDockerfile -t simpfoxy . 4 | endif 5 | ifdef noetic 6 | docker build -f NoeticDockerfile -t simpnoetic . 7 | endif 8 | ifdef jetson 9 | docker build -f JetsonDockerfile -t simpjetson . 10 | endif 11 | run: 12 | ifdef foxy 13 | docker run --rm -it --init --gpus=all --network host -e "DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 14 | -v $(CURDIR):/SIMP simpfoxy 15 | endif 16 | ifdef noetic 17 | docker run --rm -it --init --gpus=all -e "DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 18 | -v $(CURDIR):/SIMP simpnoetic 19 | endif 20 | ifdef jetson 21 | docker run --rm -it --init --gpus=all --network host -e "DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 22 | -e ROS_IP=192.168.131.69 \ 23 | -e ROS_MASTER_URI=http://192.168.131.1:11311 \ 24 | -e ROS_HOSTNAME=jetson \ 25 | -v $(CURDIR):/SIMP simpjetson 26 | endif -------------------------------------------------------------------------------- /NoeticDockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:11.4.0-devel-ubuntu20.04 2 | 3 | ENV DEBIAN_FRONTEND noninteractive 4 | ENV NVIDIA_VISIBLE_DEVICES all 5 | ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute 6 | 7 | 8 | # Install some basic utilities 9 | RUN apt-get update && apt-get install -y \ 10 | curl \ 11 | ca-certificates \ 12 | sudo \ 13 | git \ 14 | bzip2 \ 15 | libx11-6 \ 16 | python3 \ 17 | python3-dev \ 18 | python3.9 \ 19 | python3.9-dev \ 20 | python3-pip \ 21 | cmake \ 22 | wget \ 23 | tar \ 24 | libx11-dev \ 25 | xorg-dev \ 26 | libssl-dev \ 27 | build-essential \ 28 | tmux \ 29 | libusb-1.0-0-dev && apt-get clean && rm -rf /var/lib/apt/lists/* 30 | 31 | 32 | 33 | # Install miniconda 34 | #ENV CONDA_DIR /opt/conda 35 | #RUN wget --quiet https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O ~/miniconda.sh && \ 36 | # /bin/bash ~/miniconda.sh -b -p /opt/conda 37 | 38 | # Put conda in path so we can use conda activate 39 | #ENV PATH=$CONDA_DIR/bin:$PATH 40 | 41 | 42 | RUN apt-get update && \ 43 | apt-get install -y software-properties-common && \ 44 | rm -rf /var/lib/apt/lists/* 45 | RUN apt-add-repository universe 46 | RUN apt-add-repository multiverse 47 | RUN apt-add-repository restricted 48 | 49 | 50 | RUN pip install torch==1.12.1+cu113 torchvision==0.13.1+cu113 torchaudio==0.12.1 --extra-index-url https://download.pytorch.org/whl/cu113 51 | RUN pip install 'git+https://github.com/cocodataset/cocoapi.git#subdirectory=PythonAPI' 52 | RUN pip install opencv-python 53 | 54 | RUN git clone https://github.com/facebookresearch/detectron2.git && cd detectron2 && ls && pip install -e . 55 | RUN pip install fvcore iopath 56 | RUN curl -LO https://github.com/NVIDIA/cub/archive/1.10.0.tar.gz 57 | RUN tar xzf 1.10.0.tar.gz 58 | ENV CUB_HOME=$PWD/cub-1.10.0 59 | #RUN git clone https://github.com/facebookresearch/pytorch3d.git 60 | #RUN cd pytorch3d && FORCE_CUDA=1 pip install -e . 61 | #RUN pip install cython scipy seaborn 62 | #RUN git clone https://github.com/facebookresearch/omni3d.git 63 | RUN pip install --no-index --no-cache-dir pytorch3d -f https://dl.fbaipublicfiles.com/pytorch3d/packaging/wheels/py38_cu113_pyt1120/download.html 64 | RUN pip install cython scipy seaborn 65 | 66 | 67 | # Install ROS 68 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 69 | RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 70 | RUN apt-get update && apt-get install -y --allow-downgrades --allow-remove-essential --allow-change-held-packages \ 71 | libpcap-dev \ 72 | libopenblas-dev \ 73 | gstreamer1.0-tools libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-good1.0-dev \ 74 | ros-noetic-desktop-full python3-rosdep python3-rosinstall-generator python3-vcstool build-essential && \ 75 | apt-get clean && rm -rf /var/lib/apt/lists/* 76 | # Configure ROS 77 | RUN rosdep init && rosdep update 78 | RUN echo "source /opt/ros/noetic/setup.bash" >> /root/.bashrc 79 | 80 | 81 | # Install our code 82 | 83 | RUN apt-get update && \ 84 | apt-get install -y nlohmann-json3-dev libgtest-dev libeigen3-dev ros-noetic-map-server vim && \ 85 | rm -rf /var/lib/apt/lists/* 86 | 87 | RUN pip install open3d 88 | WORKDIR /SIMP 89 | COPY ncore ncore 90 | COPY ros1_ws ros1_ws 91 | SHELL ["/bin/bash", "-c"] 92 | 93 | COPY ros_entrypoint.sh / 94 | RUN chmod +x /ros_entrypoint.sh 95 | ENTRYPOINT ["/ros_entrypoint.sh"] 96 | CMD ["bash"] 97 | -------------------------------------------------------------------------------- /ncore/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | project(ncore) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -fopenmp -Wall ${CMAKE_CXX_FLAGS}") 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | find_package(OpenMP) 8 | if (OPENMP_FOUND) 9 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 10 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 11 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 12 | endif() 13 | 14 | 15 | find_package(Eigen3 REQUIRED) 16 | find_package(OpenCV 4 REQUIRED) 17 | if(OpenCV_FOUND) 18 | message(STATUS "Found OpenCV version ${OpenCV_VERSION}") 19 | message(STATUS "OpenCV directories: ${OpenCV_INCLUDE_DIRS}") 20 | include_directories(${OpenCV_INCLUDE_DIRS}) 21 | else() 22 | message(FATAL_ERROR "OpenCV not found, please read the README.md") 23 | endif(OpenCV_FOUND) 24 | 25 | 26 | find_package(Boost COMPONENTS serialization system filesystem) 27 | if(Boost_FOUND) 28 | message(STATUS "Found Boost version ${Boost_VERSION}") 29 | message(STATUS "Boost directories: ${Boost_INCLUDE_DIRS}") 30 | include_directories(${Boost_INCLUDE_DIRS}) 31 | else() 32 | message(FATAL_ERROR "Boost not found, please read the README.md") 33 | endif(Boost_FOUND) 34 | 35 | find_package(nlohmann_json 3.2.0 REQUIRED) 36 | if(nlohmann_json_FOUND) 37 | message(STATUS "Found nlohmann_json") 38 | else() 39 | message(FATAL_ERROR "nlohmann_json not found, please read the README.md") 40 | endif(nlohmann_json_FOUND) 41 | 42 | if(BUILD_TESTING) 43 | enable_testing() 44 | find_package(GTest REQUIRED) 45 | if(GTEST_FOUND) 46 | message(STATUS "GTest directories: ${GTEST_INCLUDE_DIRS}") 47 | include_directories(${GTEST_INCLUDE_DIRS}) 48 | else() 49 | message(FATAL_ERROR "GTEST_FOUND not found, please read the README.md") 50 | endif(GTEST_FOUND) 51 | endif() 52 | 53 | add_subdirectory(nsensors) 54 | add_subdirectory(nmap) 55 | add_subdirectory(nmcl) 56 | -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/Map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/Map.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/Map.yaml: -------------------------------------------------------------------------------- 1 | image: Map.png 2 | resolution: 0.05 3 | origin: 4 | - -13.9155 5 | - -11.04537 6 | - 0.0 7 | negate: 0 8 | occupied_thresh: 0.65 9 | free_thresh: 0.196 10 | -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/Map_Empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/Map_Empty.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/board.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/board.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/box.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/box.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/cabinet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/cabinet.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/chair.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/chair.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/desk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/desk.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/door.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/door.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/drawers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/drawers.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/fire extinguisher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/fire extinguisher.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/oven.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/oven.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/person.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/person.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/potted plant.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/potted plant.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/sink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/sink.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/sofa.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/sofa.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/SemMaps/table.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/SemMaps/table.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/0/roomseg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/GTMap/0/roomseg.png -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/building.config: -------------------------------------------------------------------------------- 1 | { 2 | "name" : "building", 3 | "floors": [0] 4 | } 5 | -------------------------------------------------------------------------------- /ncore/data/floor/GTMap/nmcl.config: -------------------------------------------------------------------------------- 1 | { 2 | "buildingMapPath": "building.config", 3 | "motionModel": "MixedFSR", 4 | "numParticles": 5000, 5 | "predictStrategy": "Uniform", 6 | "resampling": { 7 | "lowVarianceTH": 10000000000 8 | }, 9 | 10 | "omni":{ 11 | "mode" : true, 12 | "classes" : ["sink", "door", "oven", "board", "table", "box", "potted plant", "drawers", "sofa", "cabinet", "chair", "fire extinguisher", "person", "desk"], 13 | "confidence" : [0.7, 0.5, 0.7, 0.7, 0.6, 0.7, 0.7, 0.4, 0.8, 0.7, 0.7, 0.7, 0.6, 0.7] 14 | }, 15 | "tracking": 16 | { 17 | "mode" : false, 18 | "x" : [-9.07765], 19 | "y" : [ -7.11326], 20 | "yaw" : [-3.04342], 21 | "cov_x" : [0.01], 22 | "cov_y" : [0.01], 23 | "cov_yaw" : [0.01], 24 | "floorID" : [0] 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/Map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/Map.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/Map.yaml: -------------------------------------------------------------------------------- 1 | image: Map.png 2 | resolution: 0.05 3 | origin: 4 | - -13.9155 5 | - -11.04537 6 | - 0.0 7 | negate: 0 8 | occupied_thresh: 0.65 9 | free_thresh: 0.196 10 | -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/Map_Empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/Map_Empty.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/board.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/board.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/box.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/box.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/cabinet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/cabinet.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/chair.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/chair.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/desk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/desk.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/door.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/door.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/drawers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/drawers.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/fire extinguisher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/fire extinguisher.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/oven.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/oven.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/person.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/person.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/potted plant.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/potted plant.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/sink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/sink.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/sofa.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/sofa.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/SemMaps/table.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/SemMaps/table.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/0/roomseg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/ICPMap/0/roomseg.png -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/building.config: -------------------------------------------------------------------------------- 1 | { 2 | "name" : "building", 3 | "floors": [0] 4 | } 5 | -------------------------------------------------------------------------------- /ncore/data/floor/ICPMap/nmcl.config: -------------------------------------------------------------------------------- 1 | { 2 | "buildingMapPath": "building.config", 3 | "motionModel": "MixedFSR", 4 | "numParticles": 5000, 5 | "predictStrategy": "Uniform", 6 | "resampling": { 7 | "lowVarianceTH": 10000000000 8 | }, 9 | 10 | "omni":{ 11 | "mode" : true, 12 | "classes" : ["sink", "door", "oven", "board", "table", "box", "potted plant", "drawers", "sofa", "cabinet", "chair", "fire extinguisher", "person", "desk"], 13 | "confidence" : [0.7, 0.5, 0.7, 0.7, 0.6, 0.7, 0.7, 0.4, 0.8, 0.7, 0.7, 0.7, 0.6, 0.7] 14 | }, 15 | "tracking": 16 | { 17 | "mode" : false, 18 | "x" : [-9.07765], 19 | "y" : [ -7.11326], 20 | "yaw" : [-3.04342], 21 | "cov_x" : [0.01], 22 | "cov_y" : [0.01], 23 | "cov_yaw" : [0.01], 24 | "floorID" : [0] 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/Map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/Map.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/Map.yaml: -------------------------------------------------------------------------------- 1 | image: Map.png 2 | resolution: 0.05 3 | origin: 4 | - -13.9155 5 | - -11.04537 6 | - 0.0 7 | negate: 0 8 | occupied_thresh: 0.65 9 | free_thresh: 0.196 10 | -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/Map_Empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/Map_Empty.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/board.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/board.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/box.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/box.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/cabinet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/cabinet.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/chair.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/chair.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/desk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/desk.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/door.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/door.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/drawers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/drawers.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/fire extinguisher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/fire extinguisher.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/oven.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/oven.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/person.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/person.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/potted plant.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/potted plant.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/sink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/sink.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/sofa.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/sofa.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/SemMaps/table.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/SemMaps/table.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/0/roomseg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ncore/data/floor/KPMap/0/roomseg.png -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/building.config: -------------------------------------------------------------------------------- 1 | { 2 | "name" : "building", 3 | "floors": [0] 4 | } 5 | -------------------------------------------------------------------------------- /ncore/data/floor/KPMap/nmcl.config: -------------------------------------------------------------------------------- 1 | { 2 | "buildingMapPath": "building.config", 3 | "motionModel": "MixedFSR", 4 | "numParticles": 5000, 5 | "predictStrategy": "Uniform", 6 | "resampling": { 7 | "lowVarianceTH": 10000000000 8 | }, 9 | 10 | "omni":{ 11 | "mode" : true, 12 | "classes" : ["sink", "door", "oven", "board", "table", "box", "potted plant", "drawers", "sofa", "cabinet", "chair", "fire extinguisher", "person", "desk"], 13 | "confidence" : [0.7, 0.5, 0.7, 0.7, 0.6, 0.7, 0.7, 0.4, 0.8, 0.7, 0.7, 0.7, 0.6, 0.7] 14 | }, 15 | "tracking": 16 | { 17 | "mode" : false, 18 | "x" : [-9.07765], 19 | "y" : [ -7.11326], 20 | "yaw" : [-3.04342], 21 | "cov_x" : [0.01], 22 | "cov_y" : [0.01], 23 | "cov_yaw" : [0.01], 24 | "floorID" : [0] 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /ncore/nmap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | 3 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) 4 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/) 5 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) 6 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../nsensors/include/) 7 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../nlohmann_json/include/) 8 | 9 | set(RESULTS_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/build) 10 | file(MAKE_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}) 11 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 12 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 13 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/bin) 14 | 15 | 16 | 17 | add_subdirectory("src") 18 | #add_library(NSENSORS STATIC IMPORTED) 19 | #set_target_properties(NSENSORS PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../build/lib/libNSENSORS.a) 20 | 21 | 22 | # After all are setup is done, we can go to our src/ directory to build our 23 | # files 24 | if(BUILD_TESTING) 25 | add_subdirectory("tst") 26 | endif() 27 | -------------------------------------------------------------------------------- /ncore/nmap/include/BuildingMap.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: BuildingMap.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef BUILDINGMAP 13 | #define BUILDINGMAP 14 | #pragma once 15 | 16 | #include 17 | #include 18 | #include "FloorMap.h" 19 | 20 | class BuildingMap 21 | { 22 | public: 23 | 24 | BuildingMap(std::string jsonPath); 25 | 26 | const std::vector>& GetFloors() 27 | { 28 | return o_floors; 29 | } 30 | 31 | 32 | private: 33 | 34 | std::vector> o_floors; 35 | 36 | 37 | }; 38 | 39 | #endif -------------------------------------------------------------------------------- /ncore/nmap/include/FloorMap.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: FloorMap.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef FLOORMAP 13 | #define FLOORMAP 14 | #pragma once 15 | 16 | #include 17 | #include 18 | 19 | #include "GMap.h" 20 | #include "Room.h" 21 | #include "Lift.h" 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | class FloorMap 28 | { 29 | public: 30 | 31 | FloorMap(std::string jsonPath); 32 | 33 | FloorMap(nlohmann::json config, std::string folderPath); 34 | 35 | 36 | int GetRoomID(float x, float y); 37 | 38 | int GetRoomID(Eigen::Vector3f pose); 39 | 40 | std::vector CreateSemMaps(); 41 | 42 | std::string Name() const 43 | { 44 | return o_name; 45 | } 46 | 47 | Room& GetRoom(int id) 48 | { 49 | return o_rooms[id]; 50 | } 51 | 52 | std::vector& GetRooms() 53 | { 54 | return o_rooms; 55 | } 56 | 57 | int GetRoomsNum() const 58 | { 59 | return o_rooms.size(); 60 | } 61 | 62 | std::vector GetRoomNames(); 63 | 64 | const std::vector>& Neighbors() 65 | { 66 | return o_neighbors; 67 | } 68 | 69 | // returned seeds in world coordinates 70 | const std::vector& Seeds() const 71 | { 72 | return o_seeds; 73 | } 74 | 75 | // returned seed in world coordinates 76 | const Eigen::Vector3f& Seed(int id) const 77 | { 78 | return o_seeds[id]; 79 | } 80 | 81 | // input seed in world coordinates 82 | void Seed(int id, const Eigen::Vector3f& seed) 83 | { 84 | o_seeds[id] = seed; 85 | } 86 | 87 | // input seed in map ccordinates 88 | void Seed(int id, const Eigen::Vector2f& seed); 89 | 90 | // input seeds in world coordinates 91 | void Seeds(std::vector seeds) 92 | { 93 | o_seeds = seeds; 94 | } 95 | 96 | const std::shared_ptr& Map() const 97 | { 98 | return o_map; 99 | } 100 | 101 | const std::vector>& Maps() const 102 | { 103 | return o_maps; 104 | } 105 | 106 | const std::vector& Classes() const 107 | { 108 | return o_classes; 109 | } 110 | 111 | 112 | 113 | cv::Mat ColorizeRoomSeg(); 114 | void findNeighbours(); 115 | 116 | 117 | private: 118 | 119 | friend class boost::serialization::access; 120 | template 121 | void serialize(Archive & ar, const unsigned int version) 122 | { 123 | ar & o_rooms; 124 | ar & o_seeds; 125 | } 126 | 127 | void extractRoomSegmentation(); 128 | std::vector extractRoomIDs(); 129 | cv::Mat augmentGMap(const cv::Mat& img, const std::vector& augmentedClasses); 130 | //void fingNeighbours(); 131 | 132 | 133 | std::string o_name = "0"; 134 | std::shared_ptr o_map; 135 | std::vector> o_maps; 136 | cv::Mat o_roomSeg; 137 | std::vector o_rooms; 138 | //std::vector o_lifts; 139 | std::vector> o_neighbors; 140 | std::vector o_seeds; 141 | std::vector o_classes; 142 | std::vector o_categories; 143 | std::string o_folderPath; 144 | 145 | 146 | }; 147 | 148 | #endif // !FLOORMAP 149 | 150 | -------------------------------------------------------------------------------- /ncore/nmap/include/GMap.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: GMap.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef GMAP 14 | #define GMAP 15 | 16 | #include "opencv2/opencv.hpp" 17 | #include 18 | #include "IMap2D.h" 19 | 20 | class GMap : public IMap2D 21 | { 22 | public: 23 | 24 | //! A constructor for handling the output of Gmapping, which include a metadata yaml and a .pgm map 25 | /*! 26 | \param origin is the 2D pose of the bottom right corner of the map (found in the yaml) 27 | \param resolution is the map resolution - distance in meters corresponding to 1 pixel (found in the yaml) 28 | \param gridMap is occupancy map built according to the scans 29 | */ 30 | GMap(cv::Mat& gridMap, Eigen::Vector3f origin, float resolution); 31 | 32 | 33 | //! A constructor for handling the output of Gmapping, which include a metadata yaml and a .pgm map 34 | /*! 35 | \param yamlPath is the path to the metadata yaml file produced by gmapping 36 | */ 37 | GMap(const std::string& mapFolder, const std::string& yamlName = "YouBotMap.yaml"); 38 | 39 | 40 | //! A getter top left corner of the actual occupied area, as the map usually has wide empty margins 41 | /*! 42 | \return Eigen::Vector2f = (u, v) pixel coordinates for the gridmap 43 | */ 44 | Eigen::Vector2f TopLeft() 45 | { 46 | return o_topLeft; 47 | } 48 | 49 | 50 | //! A getter bottom right corner of the actual occupied area, as the map usually has wide empty margins 51 | /*! 52 | \return Eigen::Vector2f = (u, v) pixel coordinates for the gridmap 53 | */ 54 | Eigen::Vector2f BottomRight() 55 | { 56 | return o_bottomRight; 57 | } 58 | 59 | 60 | //! Converts (x, y) from the map frame to the pixels coordinates 61 | /*! 62 | \param (x, y) position in map frame 63 | \return (u, v) pixel coordinates for the gridmap 64 | */ 65 | Eigen::Vector2f World2Map(Eigen::Vector2f xy) const; 66 | 67 | 68 | //! Converts (u, v) pixel coordinates to map frame (x, y) 69 | /*! 70 | \param (u, v) pixel coordinates for the gridmap 71 | \return (x, y) position in map frame 72 | */ 73 | Eigen::Vector2f Map2World(Eigen::Vector2f uv) const; 74 | 75 | 76 | bool IsValid(Eigen::Vector3f pose) const; 77 | 78 | bool IsValid2D(Eigen::Vector2f mp) const; 79 | 80 | 81 | const cv::Mat& Map() const 82 | { 83 | return o_gridmap; 84 | } 85 | 86 | 87 | private: 88 | 89 | //std::shared_ptr o_gridmap; 90 | cv::Mat o_gridmap; 91 | float o_resolution = 0; 92 | int o_maxy = 0; 93 | Eigen::Vector3f o_origin; 94 | Eigen::Vector2f o_bottomRight; 95 | Eigen::Vector2f o_topLeft; 96 | 97 | void getBorders(); 98 | 99 | }; 100 | 101 | #endif //GMap 102 | 103 | -------------------------------------------------------------------------------- /ncore/nmap/include/IMap2D.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: IMap2D.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef IMAP2D_H 13 | #define IMAP2D_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | class IMap2D 21 | { 22 | public: 23 | 24 | 25 | //! Converts (x, y) from the map frame to the pixels coordinates 26 | /*! 27 | \param (x, y) position in map frame 28 | \return (u, v) pixel coordinates for the gridmap 29 | */ 30 | virtual Eigen::Vector2f World2Map(Eigen::Vector2f xy) const = 0; 31 | 32 | 33 | //! Converts (u, v) pixel coordinates to map frame (x, y) 34 | /*! 35 | \param (u, v) pixel coordinates for the gridmap 36 | \return (x, y) position in map frame 37 | */ 38 | virtual Eigen::Vector2f Map2World(Eigen::Vector2f uv) const = 0 ; 39 | 40 | 41 | virtual bool IsValid(Eigen::Vector3f pose) const = 0 ; 42 | 43 | virtual bool IsValid2D(Eigen::Vector2f mp) const = 0 ; 44 | 45 | 46 | virtual ~IMap2D() {}; 47 | 48 | 49 | virtual const cv::Mat& Map() const = 0; 50 | 51 | 52 | }; 53 | 54 | #endif // IMap2D -------------------------------------------------------------------------------- /ncore/nmap/include/Lift.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | # ############################################################################## 4 | # Copyright (c) 2021- University of Bonn # 5 | # All rights reserved. # 6 | # # 7 | # Author: Nicky Zimmerman # 8 | # # 9 | # File: Lift.h # 10 | # ############################################################################## 11 | **/ 12 | 13 | 14 | #ifndef LIFT 15 | #define LIFT 16 | 17 | 18 | #pragma once 19 | class Lift 20 | { 21 | }; 22 | 23 | #endif // !LIFT 24 | 25 | -------------------------------------------------------------------------------- /ncore/nmap/include/Object.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Object.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef OBJECT 14 | #define OBJECT 15 | 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #pragma once 24 | class Object 25 | { 26 | 27 | public: 28 | 29 | 30 | Object(int semLabel, Eigen::Vector3f pose = Eigen::Vector3f(), std::string modelPath = ""); 31 | 32 | Object(); 33 | 34 | Object(nlohmann::json config); 35 | 36 | 37 | int ID() const 38 | { 39 | return o_id; 40 | } 41 | 42 | int SemLabel() const 43 | { 44 | return o_semLabel; 45 | } 46 | 47 | void SemLabel(int semLabel) 48 | { 49 | o_semLabel = semLabel; 50 | } 51 | 52 | Eigen::Vector3f Pose() const 53 | { 54 | return o_pose; 55 | } 56 | 57 | Eigen::Vector4f Position() const 58 | { 59 | return o_posistion; 60 | } 61 | 62 | void Pose(Eigen::Vector3f pose) 63 | { 64 | o_pose = pose; 65 | } 66 | 67 | std::string ModelPath() const 68 | { 69 | return o_modelPath; 70 | } 71 | 72 | void ModelPath(std::string modelPath) 73 | { 74 | o_modelPath = modelPath; 75 | //loadModel(); 76 | } 77 | 78 | //void Json(json config); 79 | 80 | 81 | 82 | private: 83 | 84 | friend class boost::serialization::access; 85 | template 86 | void serialize(Archive & ar, const unsigned int version) 87 | { 88 | ar & o_id; 89 | ar & o_semLabel; 90 | ar & o_pose; 91 | ar & o_modelPath; 92 | } 93 | 94 | static int generateID(); 95 | void loadModel(); 96 | 97 | int o_id; 98 | int o_semLabel; 99 | Eigen::Vector3f o_pose; 100 | Eigen::Vector4f o_posistion; 101 | std::string o_modelPath; 102 | }; 103 | 104 | 105 | namespace boost { 106 | namespace serialization { 107 | 108 | template 109 | void serialize(Archive & ar, Eigen::Vector3f & pose, const unsigned int version) 110 | { 111 | ar & pose(0); 112 | ar & pose(1); 113 | ar & pose(2); 114 | } 115 | 116 | } // namespace serialization 117 | } // namespace boost 118 | 119 | 120 | #endif // !OBJECT 121 | 122 | 123 | -------------------------------------------------------------------------------- /ncore/nmap/include/Room.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Room.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef ROOM 13 | #define ROOM 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include "Object.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | class Room 27 | { 28 | public: 29 | 30 | Room(std::string name, int id, int purpose = 0); 31 | 32 | Room() {}; 33 | 34 | Room(nlohmann::json config); 35 | 36 | std::string Name() const 37 | { 38 | return o_name; 39 | } 40 | 41 | 42 | void Name(std::string name) 43 | { 44 | o_name = name; 45 | } 46 | 47 | int Purpose() const 48 | { 49 | return o_purpose; 50 | } 51 | 52 | void Purpose(int purpose) 53 | { 54 | o_purpose = purpose; 55 | } 56 | 57 | 58 | int ID() const 59 | { 60 | return o_id; 61 | } 62 | 63 | const std::vector Objects() const 64 | { 65 | return o_objects; 66 | } 67 | 68 | Object& GetObject(int id); 69 | 70 | 71 | int AddObject(Object& obj); 72 | 73 | //NOT IMPLEMENTED YET!!!! 74 | int RemoveObject(int id); 75 | 76 | private: 77 | 78 | 79 | friend class boost::serialization::access; 80 | template 81 | void serialize(Archive & ar, const unsigned int version) 82 | { 83 | ar & o_name; 84 | ar & o_purpose; 85 | ar & o_id; 86 | ar & o_objects; 87 | } 88 | 89 | 90 | std::string o_name = "room"; 91 | int o_purpose = 0; 92 | int o_id = -1; 93 | std::vector o_objects; 94 | 95 | }; 96 | 97 | #endif // ROOM 98 | -------------------------------------------------------------------------------- /ncore/nmap/src/BuildingMap.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: BuildingMap.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "BuildingMap.h" 14 | #include 15 | #include 16 | #include 17 | 18 | BuildingMap::BuildingMap(std::string jsonPath) 19 | { 20 | using json = nlohmann::json; 21 | 22 | std::string folderPath = boost::filesystem::path(jsonPath).parent_path().string() + "/"; 23 | 24 | std::ifstream file(jsonPath); 25 | json config; 26 | file >> config; 27 | 28 | std::vector floorIDs = config["floors"]; 29 | for(int i = 0; i < floorIDs.size(); ++i) 30 | { 31 | //std::cout << folderPath + std::to_string(i) + "/floor.config" << std::endl; 32 | FloorMap floormap = FloorMap(folderPath + std::to_string(i) + "/floor.config"); 33 | std::shared_ptr fp = std::make_shared(floormap); 34 | o_floors.push_back(fp); 35 | } 36 | 37 | 38 | } -------------------------------------------------------------------------------- /ncore/nmap/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #add_executable(RoomSegmentation RoomSegmentation.cpp GMap.cpp FloorMap.cpp Room.cpp Lift.cpp Object.cpp BuildingMap.cpp) 2 | #target_link_libraries(RoomSegmentation ${OpenCV_LIBS} NSENSORS ${Boost_LIBRARIES}) 3 | add_library(NMAP GMap.cpp BuildingMap.cpp FloorMap.cpp Room.cpp Lift.cpp Object.cpp) 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ncore/nmap/src/GMap.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: GMap.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "GMap.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "Utils.h" 18 | 19 | GMap::GMap(cv::Mat& gridMap, Eigen::Vector3f origin, float resolution) 20 | { 21 | o_resolution = resolution; 22 | o_origin = origin; 23 | if (gridMap.channels() == 3) 24 | { 25 | cv::cvtColor(gridMap, o_gridmap, cv::COLOR_BGR2GRAY); 26 | } 27 | else 28 | { 29 | gridMap.copyTo(o_gridmap); 30 | } 31 | 32 | o_gridmap = 255 - o_gridmap; 33 | o_maxy = o_gridmap.rows; 34 | 35 | //compute the borders 36 | getBorders(); 37 | } 38 | 39 | 40 | GMap::GMap(const std::string& mapFolder, const std::string& yamlName) 41 | { 42 | 43 | std::vector fields = File2Lines(mapFolder + yamlName); 44 | 45 | // "image: " - 7 46 | fields[0].erase(0,7); 47 | // "iresolution: " - 12 48 | fields[1].erase(0,12); 49 | // "origin: " - 8 50 | fields[2].erase(0,8); 51 | // "occupied_thresh: " - 17 52 | fields[4].erase(0,17); 53 | // "free_thresh: " - 13 54 | fields[5].erase(0,13); 55 | 56 | std::string imgPath = mapFolder + fields[0]; 57 | o_resolution = std::stof(fields[1]); 58 | 59 | std::vector vec = StringToVec(fields[2]); 60 | 61 | o_origin = Eigen::Vector3f(vec[0], vec[1], vec[2]); 62 | o_gridmap = cv::imread(imgPath, cv::IMREAD_GRAYSCALE); 63 | //cv::cvtColor( gridMap_, map, cv::COLOR_BGR2GRAY); 64 | o_gridmap = 255 - o_gridmap; 65 | o_maxy = o_gridmap.rows; 66 | 67 | //compute the borders 68 | getBorders(); 69 | } 70 | 71 | 72 | void GMap::getBorders() 73 | { 74 | cv::Mat binMap; 75 | cv::threshold(o_gridmap, binMap, 127, 255, 0); 76 | std::vector locations; 77 | cv::findNonZero(binMap, locations); 78 | cv::Rect rect = cv::boundingRect(locations); 79 | 80 | // remember: cv::Point = (col, row) - might consider inverting this 81 | // or using Eigen 82 | cv::Point tl = rect.tl(); 83 | cv::Point br = rect.br(); 84 | o_topLeft = Eigen::Vector2f(tl.x, tl.y); 85 | o_bottomRight = Eigen::Vector2f((br.x < o_gridmap.cols) ? br.x : o_gridmap.cols - 1, (br.y < o_maxy) ? br.y : (o_maxy - 1)); 86 | } 87 | 88 | Eigen::Vector2f GMap::World2Map(Eigen::Vector2f xy) const 89 | { 90 | int u = round((xy(0) - o_origin(0)) / o_resolution); 91 | int v = o_maxy - round((xy(1) - o_origin(1)) / o_resolution); 92 | return Eigen::Vector2f(u, v); 93 | } 94 | 95 | 96 | 97 | Eigen::Vector2f GMap::Map2World(Eigen::Vector2f uv) const 98 | { 99 | float x = uv(0) * o_resolution + o_origin(0); 100 | float y = (o_maxy - uv(1)) * o_resolution + o_origin(1); 101 | return Eigen::Vector2f(x, y); 102 | } 103 | 104 | 105 | bool GMap::IsValid(Eigen::Vector3f pose) const 106 | { 107 | Eigen::Vector2f xy = Eigen::Vector2f(pose(0), pose(1)); 108 | Eigen::Vector2f mp = World2Map(xy); 109 | 110 | return IsValid2D(mp); 111 | } 112 | 113 | 114 | bool GMap::IsValid2D(Eigen::Vector2f mp) const 115 | { 116 | Eigen::Vector2f br = o_bottomRight; 117 | if ((mp(0) < 0) || (mp(1) < 0) || (mp(0) > br(0)) || (mp(1) > br(1))) return false; 118 | 119 | int val = o_gridmap.at(mp(1) ,mp(0)); 120 | 121 | if (val > 1) return false; 122 | 123 | return true; 124 | } -------------------------------------------------------------------------------- /ncore/nmap/src/Lift.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Lift.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "Lift.h" 13 | -------------------------------------------------------------------------------- /ncore/nmap/src/Object.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Object.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "Object.h" 13 | #include 14 | #include 15 | #include 16 | 17 | Object::Object(int semLabel, Eigen::Vector3f pose, std::string modelPath) 18 | { 19 | o_id = Object::generateID(); 20 | o_semLabel = semLabel; 21 | o_pose = pose; 22 | o_modelPath = modelPath; 23 | } 24 | 25 | Object::Object() 26 | { 27 | o_id = Object::generateID(); 28 | o_pose = Eigen::Vector3f(0, 0, 0); 29 | o_semLabel = 0; 30 | o_modelPath = ""; 31 | } 32 | 33 | int Object::generateID() 34 | { 35 | //srand(); 36 | int id = (rand()%10000)+1; 37 | 38 | return id; 39 | } 40 | 41 | void Object::loadModel() 42 | { 43 | 44 | } 45 | 46 | 47 | Object::Object(nlohmann::json config) 48 | { 49 | //o_id = Object::generateID(); 50 | o_id = config["id"]; 51 | o_semLabel = config["semLabel"]; 52 | std::vector pos = config["position"]; 53 | o_posistion = Eigen::Vector4f(pos[0], pos[1], pos[2], pos[3]); 54 | } 55 | 56 | 57 | -------------------------------------------------------------------------------- /ncore/nmap/src/Room.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Room.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "Room.h" 13 | #include 14 | #include 15 | #include 16 | 17 | Room::Room(std::string name, int id, int purpose) 18 | { 19 | o_name = name; 20 | o_purpose = purpose; 21 | o_id = id; 22 | } 23 | 24 | 25 | Room::Room(nlohmann::json config) 26 | { 27 | o_name = config["name"]; 28 | o_purpose = config["purpose"]; 29 | o_id = config["id"]; 30 | 31 | auto objects = config["objects"]; 32 | 33 | for(auto cfg : objects) 34 | { 35 | Object obj(cfg); 36 | o_objects.push_back(obj); 37 | } 38 | } 39 | 40 | Object& Room::GetObject(int id) 41 | { 42 | auto it = find_if(o_objects.begin(), o_objects.end(), [&id](const Object& obj) {return obj.ID() == id;}); 43 | auto index = std::distance(o_objects.begin(), it); 44 | return o_objects[index]; 45 | } 46 | 47 | 48 | int Room::AddObject(Object& obj) 49 | { 50 | o_objects.push_back(obj); 51 | return 0; 52 | } 53 | 54 | int Room::RemoveObject(int id) 55 | { 56 | auto it = find_if(o_objects.begin(), o_objects.end(), [&id](const Object& obj) {return obj.ID() == id;}); 57 | if (it != o_objects.end()) 58 | { 59 | auto index = std::distance(o_objects.begin(), it); 60 | o_objects.erase (o_objects.begin() + index); 61 | 62 | return 0; 63 | } 64 | 65 | return -1; 66 | } 67 | -------------------------------------------------------------------------------- /ncore/nmap/src/RoomSegmentation.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: RoomSegmentation.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | // RoomSegmentation.cpp : This file contains the 'main' function. Program execution begins and ends there. 13 | // 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "opencv2/opencv.hpp" 21 | 22 | #include "GMap.h" 23 | #include "FloorMap.h" 24 | 25 | 26 | int main() 27 | { 28 | cv::Mat grid = cv::imread("/home/nickybones/Code/YouBotMCL/ncore/data/floor/2021_10_27/YouBotMap.pgm"); 29 | cv::Mat roomSeg = cv::imread("/home/nickybones/Code/YouBotMCL/ncore/data/floor/2021_10_27/YouBotMapRoomSeg.png"); 30 | 31 | //int id = o_roomSeg.at(440, 300); 32 | //std::cout << id << std::endl; 33 | 34 | // cv::Mat split[3]; 35 | // cv::split(roomSeg3, split); 36 | // cv::Mat roomSeg = split[2]; 37 | 38 | GMap map = GMap(grid, Eigen::Vector3f(-17.000000, -12.200000, 0.000000), 0.05); 39 | FloorMap fm = FloorMap(std::make_shared(map), roomSeg); 40 | cv::Mat roomSegRGB = fm.ColorizeRoomSeg(); 41 | 42 | cv::imshow("room segmentation", roomSegRGB); 43 | cv::imwrite("colorized_room_seg.png", roomSegRGB); 44 | cv::waitKey(); 45 | 46 | } 47 | 48 | -------------------------------------------------------------------------------- /ncore/nmap/tst/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | add_executable(NMapUnitTests NMapUnitTests.cpp ) 4 | target_link_libraries(NMapUnitTests ${OpenCV_LIBS} ${catkin_LIBRARIES} NMAP NSENSORS GTest::GTest gtest_main ${Boost_LIBRARIES}) 5 | target_compile_definitions(NMapUnitTests PRIVATE PROJECT_TEST_DATA_DIR="${PROJECT_SOURCE_DIR}/data/") 6 | add_test(AllTestsInTests NMapUnitTests) 7 | -------------------------------------------------------------------------------- /ncore/nmcl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | 3 | ## Specify additional locations of header files 4 | ## Your package locations should be listed before other locations 5 | 6 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) 7 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/) 8 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) 9 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../nsensors/include/) 10 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../nmap/include/) 11 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../nlohmann_json/include/) 12 | 13 | set(RESULTS_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/build) 14 | file(MAKE_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}) 15 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 16 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 17 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/bin) 18 | 19 | 20 | 21 | add_subdirectory("src") 22 | #add_library(NSENSORS STATIC IMPORTED) 23 | set_target_properties(NSENSORS PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../build/lib/libNSENSORS.a) 24 | set_target_properties(NMAP PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../build/lib/libNMAP.a) 25 | 26 | 27 | 28 | # After all are setup is done, we can go to our src/ directory to build our 29 | # files 30 | if(BUILD_TESTING) 31 | add_subdirectory("tst") 32 | endif() 33 | -------------------------------------------------------------------------------- /ncore/nmcl/include/MixedFSR.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: MixedFSR.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef MIXEDFSR_H 14 | #define MIXEDFSR_H 15 | 16 | #include 17 | #include 18 | 19 | class MixedFSR 20 | { 21 | public: 22 | 23 | Eigen::Vector3f SampleMotion(const Eigen::Vector3f& p1, const std::vector& command, const std::vector& weights, const Eigen::Vector3f& noise); 24 | 25 | Eigen::Vector3f Forward(Eigen::Vector3f p1, Eigen::Vector3f u); 26 | 27 | Eigen::Vector3f Backward(Eigen::Vector3f p1, Eigen::Vector3f p2); 28 | 29 | 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/include/NMCLFactory.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: NMCLFactory.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef NMCLFACTORY 13 | #define NMCLFACTORY 14 | 15 | #include "ReNMCL.h" 16 | #include 17 | 18 | 19 | class NMCLFactory 20 | { 21 | public: 22 | 23 | static std::shared_ptr Create(const std::string& configPath); 24 | 25 | static void Dump(const std::string& configPath); 26 | 27 | 28 | private: 29 | 30 | }; 31 | 32 | 33 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/include/Particle.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Particle.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef PARTICLE_H 13 | #define PARTICLE_H 14 | 15 | 16 | #include 17 | #include 18 | 19 | 20 | class Particle 21 | { 22 | public: 23 | 24 | Particle(Eigen::Vector3f p = Eigen::Vector3f(0, 0, 0), double w = 0, unsigned int id = 0); 25 | 26 | Eigen::Vector3f pose; 27 | unsigned int floorID; 28 | double weight; 29 | 30 | }; 31 | 32 | 33 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/include/ParticleFilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: ParticleFilter.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef PARTICLEFILTER_H 14 | #define PARTICLEFILTER_H 15 | 16 | #include "Particle.h" 17 | #include "GMap.h" 18 | #include "SetStatistics.h" 19 | #include "FloorMap.h" 20 | #include "BuildingMap.h" 21 | 22 | class ParticleFilter 23 | { 24 | public: 25 | 26 | //multi-map 27 | ParticleFilter(std::shared_ptr buildingMap); 28 | 29 | void InitByRoomType(std::vector& particles, int n_particles, const std::vector& roomProbabilities); 30 | 31 | //multi-map 32 | void InitUniform(std::vector& particles, int n_particles); 33 | //multi-map 34 | void InitGaussian(std::vector& particles, int n_particles, const std::vector& initGuess, const std::vector& covariances); 35 | //multi-map 36 | void RemoveWeakest(std::vector& particles, int n_particles); 37 | 38 | 39 | void AddBoundingBox(std::vector& particles, int n_particles, const std::vector& tls, const std::vector& brs, const std::vector& yaws); 40 | 41 | //multi-map 42 | SetStatistics ComputeStatistics(const std::vector& particles); 43 | 44 | //multi-map 45 | void NormalizeWeights(std::vector& particles); 46 | 47 | //multi-map 48 | std::vector GetFloorParticles(const std::vector& particles, unsigned int floorID); 49 | 50 | std::vector& Particles() 51 | { 52 | return o_particles; 53 | } 54 | 55 | void SetParticle(int id, Particle p) 56 | { 57 | o_particles[id] = p; 58 | } 59 | 60 | 61 | SetStatistics Statistics() 62 | { 63 | return o_stats; 64 | } 65 | 66 | 67 | 68 | private: 69 | 70 | 71 | 72 | std::shared_ptr o_buildingMap; 73 | std::shared_ptr o_floorMap; 74 | std::shared_ptr o_gmap; 75 | std::vector> o_gmaps; 76 | std::vector o_particles; 77 | std::vector o_floorStats; 78 | SetStatistics o_stats; 79 | std::vector o_floorWeights; 80 | }; 81 | 82 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/include/Resampling.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: LowVarianceResampling.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef RESAMPLING_H 14 | #define RESAMPLING_H 15 | 16 | 17 | #include 18 | #include 19 | #include "Particle.h" 20 | 21 | class Resampling 22 | { 23 | public: 24 | 25 | void Resample(std::vector& particles); 26 | 27 | void SetTH(float th) 28 | { 29 | o_th = th; 30 | } 31 | 32 | 33 | private: 34 | 35 | float o_th = 0.5; 36 | 37 | }; 38 | 39 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/include/Semantic3DData.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Semantic3DData.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef SEMANTIC3DDATA_H 14 | #define SEMANTIC3DDATA_H 15 | 16 | #include 17 | #include 18 | 19 | 20 | class Semantic3DData 21 | { 22 | 23 | public: 24 | 25 | Semantic3DData(const std::vector& labels, const std::vector>& vertices, const std::vector& confidences) 26 | { 27 | o_labels = labels; 28 | o_vertices = vertices; 29 | o_confidences = confidences; 30 | } 31 | 32 | //virtual ~LidarData(){}; 33 | 34 | const std::vector>& Vertices() const 35 | { 36 | return o_vertices; 37 | } 38 | 39 | const std::vector& Label() const 40 | { 41 | return o_labels; 42 | } 43 | 44 | const std::vector& Confidence() const 45 | { 46 | return o_confidences; 47 | } 48 | 49 | private: 50 | 51 | std::vector> o_vertices; 52 | std::vector o_labels; 53 | std::vector o_confidences; 54 | 55 | }; 56 | 57 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/include/SemanticOverlap.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: SemanticOverlap.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef SEMANTICOVERLAP_H 14 | #define SEMANTICOVERLAP_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include "GMap.h" 20 | #include "BuildingMap.h" 21 | #include "Semantic3DData.h" 22 | 23 | 24 | class SemanticOverlap 25 | { 26 | public: 27 | 28 | 29 | 30 | //! A constructor 31 | /*! 32 | \param buildingMap is a ptr to a Building object, which holds the buiding map 33 | \param classes is a float that determine how forgiving the model is (small sigma will give a very peaked likelihood) 34 | \param confidences is a float specifying up to what distance from the sensor a reading is valid 35 | \param globalVariancevPath is a float specifying up to what distance from the sensor a reading is valid 36 | */ 37 | 38 | SemanticOverlap(std::shared_ptr buildingMap, const std::vector& classes, const std::vector& confidences, const std::string& globalVariancevPath); 39 | 40 | 41 | //! Computes weights for all particles based on how well the observation matches the map 42 | /*! 43 | \param particles is a vector of Particle elements 44 | \param SensorData is an abstract container for sensor data. This function expects LidarData type 45 | */ 46 | 47 | void ComputeWeights(std::vector& particles, std::shared_ptr data); 48 | 49 | 50 | 51 | private: 52 | 53 | 54 | bool isTraced(const cv::Mat& currMap, Eigen::Vector2f pose, Eigen::Vector2f bearing, unsigned int floorID); 55 | 56 | double geometric(const Particle& particle, std::shared_ptr data); 57 | std::pair computeLikelihood(Eigen::Vector2f uv, int label, int floorID); 58 | void createObjectMaps(); 59 | 60 | 61 | //void createVisibilityMap(unsigned int floorID); 62 | 63 | std::vector o_classNames; 64 | std::vector> o_gmaps; 65 | cv::Size o_mapSize; 66 | std::vector o_confidenceTH; 67 | std::vector o_sigmas; 68 | std::vector o_classConsistency; 69 | std::shared_ptr o_buildingMap; 70 | float o_maxRange = 10.0; 71 | 72 | std::vector o_categories; 73 | std::vector o_uid; 74 | std::vector o_mx; 75 | std::vector o_my; 76 | std::vector o_sx; 77 | std::vector o_sy; 78 | std::vector o_b; 79 | std::vector> o_corners; 80 | std::vector o_objectMaps; 81 | 82 | }; 83 | 84 | 85 | 86 | 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /ncore/nmcl/include/SetStatistics.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: SetStatistics.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef SETSTATISTICS_H 14 | #define SETSTATISTICS_H 15 | 16 | #include 17 | #include 18 | #include "Particle.h" 19 | 20 | class SetStatistics 21 | { 22 | public: 23 | 24 | SetStatistics(Eigen::Vector3d m = Eigen::Vector3d::Zero(), Eigen::Matrix3d c = Eigen::Matrix3d::Zero(), unsigned int id = 0) 25 | { 26 | mean = m; 27 | cov = c; 28 | floorId = id; 29 | } 30 | 31 | Eigen::Vector3d Mean() 32 | { 33 | return mean; 34 | } 35 | 36 | Eigen::Matrix3d Cov() 37 | { 38 | return cov; 39 | } 40 | 41 | unsigned int FloorID() 42 | { 43 | return floorId; 44 | } 45 | 46 | void FloorID(unsigned int id) 47 | { 48 | floorId = id; 49 | } 50 | 51 | static SetStatistics ComputeParticleSetStatistics(const std::vector& particles); 52 | 53 | private: 54 | 55 | Eigen::Vector3d mean; 56 | Eigen::Matrix3d cov; 57 | unsigned int floorId; 58 | 59 | }; 60 | 61 | 62 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | add_library(NMCL MixedFSR.cpp Particle.cpp SetStatistics.cpp Resampling.cpp ReNMCL.cpp NMCLFactory.cpp ParticleFilter.cpp SemanticOverlap.cpp) 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ncore/nmcl/src/MixedFSR.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: MixedFSR.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "MixedFSR.h" 13 | #include 14 | #include 15 | #include "Utils.h" 16 | #include 17 | 18 | 19 | 20 | Eigen::Vector3f MixedFSR::SampleMotion(const Eigen::Vector3f& p1, const std::vector& command, const std::vector& weights, const Eigen::Vector3f& noise) 21 | { 22 | Eigen::Vector3f u(0, 0, 0); 23 | float choose = drand48(); 24 | float w = 0.0; 25 | 26 | for(long unsigned int i = 0; i < command.size(); ++i) 27 | { 28 | w += weights[i]; 29 | if(choose <= w) 30 | { 31 | u = command[i]; 32 | break; 33 | } 34 | } 35 | 36 | float f = u(0); 37 | float s = u(1); 38 | float r = u(2); 39 | 40 | float f_h = f - SampleGuassian(noise(0) * fabs(f)); 41 | float s_h = s - SampleGuassian(noise(1) * fabs(s)); 42 | float r_h = r - SampleGuassian(noise(2) * fabs(r)); 43 | 44 | 45 | Eigen::Vector3f new_p = Forward(p1, Eigen::Vector3f(f_h, s_h, r_h)); 46 | 47 | return new_p; 48 | 49 | } 50 | 51 | 52 | Eigen::Vector3f MixedFSR::Backward(Eigen::Vector3f p1, Eigen::Vector3f p2) 53 | { 54 | Eigen::Vector3f dp = p2 - p1; 55 | 56 | float a = cos(p1(2)); 57 | float b = sin(p1(2)); 58 | 59 | float f = (dp.x() * a + dp.y() * b) / (pow(a, 2.0) + pow(b, 2.0)); 60 | float s = (dp.y() - f * b) / a; 61 | float r = dp(2); 62 | 63 | 64 | return Eigen::Vector3f(f, s, r); 65 | 66 | } 67 | 68 | Eigen::Vector3f MixedFSR::Forward(Eigen::Vector3f p1, Eigen::Vector3f u) 69 | { 70 | float f = u(0); 71 | float s = u(1); 72 | float r = u(2); 73 | 74 | float x = p1(0) + f * cos(p1(2)) - s * sin(p1(2)); 75 | float y = p1(1) + f * sin(p1(2)) + s * cos(p1(2)); 76 | float theta = Wrap2Pi(r + p1(2)); 77 | 78 | return Eigen::Vector3f(x, y, theta); 79 | 80 | } 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /ncore/nmcl/src/Particle.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Particle.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "Particle.h" 14 | 15 | Particle::Particle(Eigen::Vector3f p, double w, unsigned int id) 16 | { 17 | pose = p; 18 | weight = w; 19 | floorID = id; 20 | } -------------------------------------------------------------------------------- /ncore/nmcl/src/Resampling.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: LowVarianceResampling.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "Resampling.h" 14 | 15 | #include 16 | #include 17 | 18 | void Resampling::Resample(std::vector& particles) 19 | { 20 | int n_particles = particles.size(); 21 | std::vector transWeights(n_particles); 22 | std::vector new_particles(n_particles); 23 | 24 | 25 | std::transform(particles.begin(), particles.end(), transWeights.begin(), [](Particle &p){ return p.weight * p.weight; }); 26 | double sumWeights = std::accumulate(transWeights.begin(), transWeights.end(), 0.0); 27 | 28 | double effN = 1.0 / sumWeights; 29 | 30 | if (effN < o_th * n_particles) 31 | { 32 | double unitW = 1.0 / n_particles; 33 | //std::cout << "resample" << std::endl; 34 | double r = drand48() * 1.0 / n_particles; 35 | double acc = particles[0].weight; 36 | int i = 0; 37 | 38 | for(int j = 0; j < n_particles; ++j) 39 | { 40 | double U = r + j * 1.0 / n_particles; 41 | while((U > acc) && (i < n_particles)) 42 | { 43 | ++i; 44 | acc += particles[i].weight; 45 | } 46 | particles[i].weight = unitW; 47 | new_particles[j] = particles[i]; 48 | } 49 | particles = new_particles; 50 | } 51 | } -------------------------------------------------------------------------------- /ncore/nmcl/src/SetStatistics.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: SetStatistics.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include "SetStatistics.h" 13 | #include 14 | 15 | SetStatistics SetStatistics::ComputeParticleSetStatistics(const std::vector& particles) 16 | { 17 | 18 | Eigen::Vector4d m = Eigen::Vector4d(0, 0, 0, 0); 19 | Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(3, 3); 20 | int n = particles.size(); 21 | double tot_w = 0.0; 22 | 23 | for(int i = 0; i < n; ++i) 24 | { 25 | Eigen::Vector3f p = particles[i].pose; 26 | double w = particles[i].weight; 27 | 28 | m(0) += p(0) * w; // mean x 29 | m(1) += p(1) * w; // mean y 30 | m(2) += cos(p(2)) * w; // theta 31 | m(3) += sin(p(2)) * w; // theta 32 | 33 | tot_w += w; 34 | 35 | // linear components cov 36 | for(int j = 0; j < 2; ++j) 37 | { 38 | for(int k = 0; k < 2; ++k) 39 | { 40 | cov(j,k) += w * p(j) * p(k); 41 | } 42 | } 43 | } 44 | 45 | Eigen::Vector3d mean; 46 | mean(0) = m(0) / tot_w; 47 | mean(1) = m(1) / tot_w; 48 | mean(2) = atan2(m(3), m(2)); 49 | 50 | 51 | // normalize linear components cov 52 | for(int j = 0; j < 2; ++j) 53 | { 54 | for(int k = 0; k < 2; ++k) 55 | { 56 | cov(j, k) = cov(j, k) /tot_w - mean(j) * mean(k); 57 | } 58 | } 59 | 60 | // angular covariance 61 | double R = sqrt(m(2) * m(2) + m(3) * m(3)); 62 | 63 | // https://github.com/ros-planning/navigation/blob/2b807bd312fac1b476851800c84cb962559cbc53/amcl/src/amcl/pf/pf.c#L690 64 | //cov(2, 2) = -2 * log(R); 65 | 66 | // https://www.ebi.ac.uk/thornton-srv/software/PROCHECK/nmr_manual/man_cv.html 67 | cov(2, 2) = 1 - R / tot_w; 68 | 69 | 70 | SetStatistics stats = SetStatistics(mean, cov); 71 | 72 | return stats; 73 | 74 | } -------------------------------------------------------------------------------- /ncore/nmcl/tst/Analysis.cpp: -------------------------------------------------------------------------------- 1 | #include "Analysis.h" 2 | #include 3 | 4 | 5 | float CosineDistance(float t1, float t2) 6 | { 7 | Eigen::Vector2f p1 = Eigen::Vector2f(cos(t1), sin(t1)); 8 | Eigen::Vector2f p2 = Eigen::Vector2f(cos(t2), sin(t2)); 9 | float dist = 1 - ( (p1.dot(p2))/ (p1.norm() * p2.norm()) ); 10 | 11 | return dist; 12 | } 13 | 14 | 15 | Eigen::Vector3f RMSE(std::vector predictions, std::vector groundTruth) 16 | { 17 | double x = 0.0; 18 | double y = 0.0; 19 | double theta = 0.0; 20 | 21 | int cnt = 0; 22 | 23 | for(int i = 10; i < predictions.size(); ++i) 24 | { 25 | Eigen::Vector3f p = predictions[i]; 26 | Eigen::Vector3f g = groundTruth[i]; 27 | Eigen::Vector3f delta = p - g; 28 | x += delta(0) * delta(0); 29 | y += delta(1) * delta(1); 30 | theta += CosineDistance(p(2), g(2)); 31 | ++cnt; 32 | } 33 | 34 | x = sqrt(x / cnt); 35 | y = sqrt(y / cnt); 36 | theta = theta / cnt; 37 | 38 | //std::cout << "rmse: " << x << ", " << y << ", " << theta << std::endl; 39 | 40 | return Eigen::Vector3f(x, y, theta); 41 | } -------------------------------------------------------------------------------- /ncore/nmcl/tst/Analysis.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef ANALYSIS_H 3 | #define ANALYSIS_H 4 | 5 | #include 6 | #include 7 | 8 | float CosineDistance(float t1, float t2); 9 | 10 | 11 | Eigen::Vector3f RMSE(std::vector predictions, std::vector groundTruth); 12 | 13 | #endif -------------------------------------------------------------------------------- /ncore/nmcl/tst/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | add_executable(NMCLUnitTests NMCLUnitTests.cpp Analysis.cpp) 5 | target_link_libraries(NMCLUnitTests ${OpenCV_LIBS} ${catkin_LIBRARIES} NMCL NSENSORS NMAP nlohmann_json::nlohmann_json GTest::GTest gtest_main ${Boost_LIBRARIES}) 6 | target_compile_definitions(NMCLUnitTests PRIVATE PROJECT_TEST_DATA_DIR="${PROJECT_SOURCE_DIR}/data") 7 | add_test(AllTestsInTests NMCLUnitTests) 8 | 9 | 10 | -------------------------------------------------------------------------------- /ncore/nmcl/tst/verifyInit.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import pandas as pd 4 | 5 | 6 | for a in range(4): 7 | 8 | df = pd.read_csv("/home/nickybones/Code/YouBotMCL/ncore/build/" + str(90 * a) + "particles.csv") 9 | x = df.iloc[:, 0] 10 | y = df.iloc[:, 1] 11 | yaw = df.iloc[:, 2] 12 | 13 | plt.gca().invert_yaxis() 14 | 15 | plt.scatter(x, y, c='k') 16 | plt.scatter([0], [0], c='r') 17 | 18 | for i in range(len(yaw)): 19 | ang = yaw[i] 20 | dx = np.cos(ang) 21 | dy = np.sin(ang) 22 | plt.arrow(x[i], y[i], 0.005*dx, 0.005*dy) 23 | 24 | plt.savefig("/home/nickybones/Code/YouBotMCL/ncore/build/" + str(90 * a) + "particles.png") 25 | plt.close() 26 | #plt.show() 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /ncore/nsensors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | 3 | ## Specify additional locations of header files 4 | ## Your package locations should be listed before other locations 5 | 6 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) 7 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/) 8 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) 9 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../nlohmann_json/include/) 10 | 11 | set(RESULTS_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/build) 12 | file(MAKE_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}) 13 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 14 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 15 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/bin) 16 | 17 | 18 | # After all are setup is done, we can go to our src/ directory to build our 19 | # files 20 | add_subdirectory("src") 21 | if(BUILD_TESTING) 22 | add_subdirectory("tst") 23 | endif() 24 | -------------------------------------------------------------------------------- /ncore/nsensors/include/Camera.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Camera.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef CAMERA_H 13 | #define CAMERA_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | class Camera 22 | { 23 | public: 24 | //! A constructor for Camera object, that maps from image coordinates to 3D in camera frame 25 | /*! 26 | \param k 27 | \param tc 28 | */ 29 | Camera(Eigen::Matrix3d k, Eigen::Matrix3d t); 30 | 31 | Camera(std::string jsonPath); 32 | 33 | std::pair UV2CameraFrame(Eigen::Vector2d q1, Eigen::Vector2d q2); 34 | 35 | Eigen::Vector3d UV2CameraFrame(Eigen::Vector2f q1); 36 | 37 | std::pair ComputeOccAngles(Eigen::Vector2d q1, Eigen::Vector2d q2); 38 | 39 | int ID() const 40 | { 41 | return o_id; 42 | } 43 | 44 | 45 | float Yaw() const 46 | { 47 | return o_yaw; 48 | } 49 | 50 | 51 | 52 | 53 | private: 54 | 55 | Eigen::Matrix3d o_K; 56 | Eigen::Matrix3d o_invK; 57 | Eigen::Matrix3d o_T; 58 | int o_id = 0; 59 | float o_yaw = 0.0; 60 | }; 61 | 62 | 63 | #endif -------------------------------------------------------------------------------- /ncore/nsensors/include/Lidar2D.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Lidar2D.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef LIDAR2D_H 13 | #define LIDAR2D_H 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | class Lidar2D 21 | { 22 | public: 23 | 24 | Lidar2D(std::string name_, Eigen::Vector3f origin, int numBeams, float maxAngle, float minAngle); 25 | 26 | Lidar2D(std::string name_, std::string yamlFolder); 27 | 28 | Lidar2D(std::string jsonPath); 29 | 30 | //const std::vector& Heading() const 31 | std::vector Heading() 32 | { 33 | return o_heading; 34 | } 35 | 36 | std::string Name() 37 | { 38 | return o_name; 39 | } 40 | 41 | std::vector Center(std::vector& homoPoints); 42 | 43 | 44 | 45 | private: 46 | 47 | std::vector o_heading; 48 | std::string o_name; 49 | Eigen::Matrix3f o_trans; 50 | 51 | }; 52 | 53 | 54 | 55 | std::vector MergeScans(const std::vector& f_ranges, Lidar2D laser_front, 56 | const std::vector& r_ranges, Lidar2D laser_rear, int dsFactor = 10, float maxRange = 15); 57 | 58 | std::vector MergeScansSimple(const std::vector& f_ranges, Lidar2D laser_front, 59 | const std::vector& r_ranges, Lidar2D laser_rear); 60 | 61 | #endif -------------------------------------------------------------------------------- /ncore/nsensors/include/OptiTrack.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: OptiTrack.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef OPTITRACK_H 13 | #define OPTITRACK_H 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | class OptiTrack 20 | { 21 | public: 22 | 23 | //! A constructor 24 | /*! 25 | \param origin is the (x, y, theta) location of the robot in the OptiTrack frame when the robot begins mapping 26 | So it is (0, 0, 0) in the map frame. 27 | */ 28 | OptiTrack(Eigen::Vector3f origin); 29 | 30 | //! A constructor 31 | /*! 32 | \param yamlPath is a string of the path of the folder of a yaml file containing the origin location 33 | */ 34 | OptiTrack(std::string yamlFolder); 35 | 36 | //! Converts 2D pose (x, y, theta) in the OptiTrack frame to 2D pose in the map frame. 37 | /*! 38 | \param p is 2D pose from the OptiTrack motion capture 39 | \return 2D pose in the map frame 40 | */ 41 | Eigen::Vector3f OptiTrack2World(Eigen::Vector3f p); 42 | 43 | 44 | private: 45 | Eigen::Matrix3f o_invTrans; 46 | Eigen::Vector3f o_origin; 47 | }; 48 | 49 | #endif -------------------------------------------------------------------------------- /ncore/nsensors/include/Utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Utils.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef UTILS_H 13 | #define UTILS_H 14 | 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | 22 | Eigen::Matrix3f Vec2Trans(Eigen::Vector3f v); 23 | 24 | float Wrap2Pi(float angle); 25 | 26 | float GetYaw(float qz, float qw); 27 | 28 | std::vector Ranges2Points(const std::vector& ranges, const std::vector& angles); 29 | 30 | std::vector Downsample(const std::vector& ranges, int N); 31 | 32 | std::vector Downsample(const std::vector& ranges, int N); 33 | 34 | std::vector StringToVec(std::string seq); 35 | 36 | std::vector File2Lines(std::string filePath); 37 | 38 | float SampleGuassian(float sigma); 39 | 40 | #endif -------------------------------------------------------------------------------- /ncore/nsensors/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #target_link_libraries(Driver ${OpenCV_LIBS}) 2 | add_library(NSENSORS Utils.cpp Camera.cpp OptiTrack.cpp Lidar2D.cpp) 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ncore/nsensors/src/Camera.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Camera.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "Camera.h" 14 | #include "Utils.h" 15 | #include 16 | #include 17 | #include 18 | 19 | Camera::Camera(Eigen::Matrix3d k, Eigen::Matrix3d t) 20 | { 21 | o_K = k; 22 | o_invK = o_K.inverse(); 23 | o_T = t; 24 | } 25 | 26 | Camera::Camera(std::string jsonPath) 27 | { 28 | using json = nlohmann::json; 29 | 30 | std::ifstream file(jsonPath); 31 | json config; 32 | file >> config; 33 | o_id = config["id"]; 34 | o_yaw = config["yaw"]; 35 | std::vector k = config["k"]; 36 | std::vector t = config["t"]; 37 | 38 | o_K = Eigen::Matrix(k.data()); 39 | o_invK = o_K.inverse(); 40 | o_T = Eigen::Matrix(t.data()); 41 | } 42 | 43 | 44 | 45 | std::pair Camera::UV2CameraFrame(Eigen::Vector2d q1, Eigen::Vector2d q2) 46 | { 47 | Eigen::Vector3d p1(q1(0), q1(1), 1); 48 | Eigen::Vector3d p2(q2(0), q2(1), 1); 49 | 50 | 51 | // multiply by inverse calibration matrix 52 | Eigen::Vector3d p1k_ = o_invK * p1; 53 | Eigen::Vector3d p2k_ = o_invK * p2; 54 | 55 | // divide by z component to homogenize it 56 | Eigen::Vector3d p1k = p1k_ / p1k_(2); 57 | Eigen::Vector3d p2k = p2k_ / p2k_(2); 58 | 59 | // go from image frame to camera frame 60 | Eigen::Vector3d p1c = o_T * p1k; 61 | Eigen::Vector3d p2c = o_T * p2k; 62 | 63 | std::pair pc{p1c, p2c}; 64 | 65 | return pc; 66 | } 67 | 68 | Eigen::Vector3d Camera::UV2CameraFrame(Eigen::Vector2f q1) 69 | { 70 | Eigen::Vector3d p1(q1(0), q1(1), 1); 71 | 72 | // multiply by inverse calibration matrix 73 | Eigen::Vector3d p1k_ = o_invK * p1; 74 | 75 | // divide by z component to homogenize it 76 | Eigen::Vector3d p1k = p1k_ / p1k_(2); 77 | 78 | // go from image frame to camera frame 79 | Eigen::Vector3d p1c = o_T * p1k; 80 | 81 | return p1c; 82 | } 83 | 84 | 85 | std::pair Camera::ComputeOccAngles(Eigen::Vector2d q1, Eigen::Vector2d q2) 86 | { 87 | std::pair pc = UV2CameraFrame(q1, q2); 88 | Eigen::Vector3d p1c = pc.first; 89 | Eigen::Vector3d p2c = pc.second; 90 | float t1 = Wrap2Pi(atan2(p1c(1), p1c(0)) + o_yaw); 91 | float t2 = Wrap2Pi(atan2(p2c(1), p2c(0)) + o_yaw); 92 | 93 | return std::pair (t1, t2); 94 | } -------------------------------------------------------------------------------- /ncore/nsensors/src/OptiTrack.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: OptiTrack.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #include "OptiTrack.h" 14 | #include "Utils.h" 15 | 16 | 17 | OptiTrack::OptiTrack(Eigen::Vector3f origin_) 18 | { 19 | Eigen::Matrix3f trans = Vec2Trans(origin_); 20 | o_invTrans = trans.inverse(); 21 | o_origin = origin_; 22 | } 23 | 24 | OptiTrack::OptiTrack(std::string yamlFolder) 25 | { 26 | std::vector fields = File2Lines(yamlFolder + "optitrack.yaml"); 27 | // "origin:" - 8 28 | fields[0].erase(0,8); 29 | std::vector vec = StringToVec(fields[0]); 30 | 31 | o_origin = Eigen::Vector3f(vec[0], vec[1], vec[2]); 32 | Eigen::Matrix3f trans = Vec2Trans(o_origin); 33 | o_invTrans = trans.inverse(); 34 | } 35 | 36 | 37 | 38 | Eigen::Vector3f OptiTrack::OptiTrack2World(Eigen::Vector3f p) 39 | { 40 | Eigen::Vector3f xy1 = Eigen::Vector3f(p(0), p(1), 1); 41 | Eigen::Vector3f p_trans = o_invTrans * xy1; 42 | p_trans(2) = Wrap2Pi(p(2) - o_origin(2)); 43 | 44 | return p_trans; 45 | } -------------------------------------------------------------------------------- /ncore/nsensors/src/Utils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: Utils.cpp # 9 | # ############################################################################## 10 | **/ 11 | 12 | #include 13 | #include "Utils.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | 25 | Eigen::Matrix3f Vec2Trans(Eigen::Vector3f v) 26 | { 27 | float c = cos(v(2)); 28 | float s = sin(v(2)); 29 | Eigen::Matrix3f trans; 30 | trans << c, -s, v(0), s, c, v(1), 0, 0, 1; 31 | 32 | return trans; 33 | } 34 | 35 | float Wrap2Pi(float angle) 36 | { 37 | float wAngle = angle; 38 | while (wAngle < -M_PI) wAngle += 2 * M_PI; 39 | 40 | while (wAngle > M_PI) wAngle -= 2 * M_PI; 41 | 42 | return wAngle; 43 | } 44 | 45 | float GetYaw(float qz, float qw) 46 | { 47 | float yaw = 2 * atan2(qz, qw); 48 | yaw = Wrap2Pi(yaw); 49 | 50 | return yaw; 51 | } 52 | 53 | std::vector Ranges2Points(const std::vector& ranges, const std::vector& angles) 54 | { 55 | int n = ranges.size(); 56 | std::vector homoPoints(n); 57 | 58 | for(int i = 0; i < n; ++i) 59 | { 60 | float r = ranges[i]; 61 | float a = angles[i]; 62 | Eigen::Vector3f p = Eigen::Vector3f(r * cos(a), r * sin(a), 1); 63 | homoPoints[i] = p; 64 | } 65 | 66 | // consider using a matrix instead of a list for later stuff 67 | return homoPoints; 68 | } 69 | 70 | 71 | 72 | std::vector Downsample(const std::vector& ranges, int N) 73 | { 74 | int n = ranges.size() / N; 75 | std::vector downsampled(n); 76 | 77 | for(int i = 0; i < n; ++i) 78 | { 79 | downsampled[i] = ranges[i * N]; 80 | } 81 | 82 | return downsampled; 83 | } 84 | 85 | std::vector Downsample(const std::vector& ranges, int N) 86 | { 87 | int n = ranges.size() / N; 88 | std::vector downsampled(n); 89 | 90 | for(int i = 0; i < n; ++i) 91 | { 92 | downsampled[i] = ranges[i * N]; 93 | } 94 | 95 | return downsampled; 96 | } 97 | 98 | 99 | 100 | std::vector StringToVec(std::string seq) 101 | { 102 | std::vector vec; 103 | 104 | seq.erase(std::remove(seq.begin(), seq.end(), ','), seq.end()); 105 | seq.erase(std::remove(seq.begin(), seq.end(), '['), seq.end()); 106 | seq.erase(std::remove(seq.begin(), seq.end(), ']'), seq.end()); 107 | 108 | size_t pos = 0; 109 | std::string space_delimiter = " "; 110 | std::vector words; 111 | while ((pos = seq.find(space_delimiter)) != std::string::npos) 112 | { 113 | words.push_back(seq.substr(0, pos)); 114 | seq.erase(0, pos + space_delimiter.length()); 115 | } 116 | words.push_back(seq.substr(0, pos)); 117 | 118 | for(long unsigned int i = 0; i < words.size(); ++i) 119 | { 120 | //std::cout << words[i] << std::endl; 121 | vec.push_back(std::stof(words[i])); 122 | } 123 | 124 | return vec; 125 | } 126 | 127 | 128 | std::vector File2Lines(std::string filePath) 129 | { 130 | std::ifstream file(filePath); 131 | 132 | std::vector fields; 133 | 134 | if (file.is_open()) 135 | { 136 | std::string line; 137 | while (std::getline(file, line)) 138 | { 139 | fields.push_back(line); 140 | } 141 | file.close(); 142 | } 143 | 144 | return fields; 145 | } 146 | 147 | 148 | float SampleGuassian(float sigma) 149 | { 150 | float sample = 0; 151 | 152 | for(int i = 0; i < 12; ++i) 153 | { 154 | sample += drand48() * 2 * sigma - sigma; 155 | } 156 | sample *= 0.5; 157 | 158 | return sample; 159 | 160 | } -------------------------------------------------------------------------------- /ncore/nsensors/tst/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | add_executable(NSensorsUnitTests NSensorsUnitTests.cpp ) 4 | target_link_libraries(NSensorsUnitTests ${OpenCV_LIBS} ${catkin_LIBRARIES} NSENSORS GTest::GTest gtest_main) 5 | target_compile_definitions(NSensorsUnitTests PRIVATE PROJECT_TEST_DATA_DIR="${PROJECT_SOURCE_DIR}/data/") 6 | add_test(AllTestsInTests NSensorsUnitTests) 7 | -------------------------------------------------------------------------------- /resources/3dmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/resources/3dmap.png -------------------------------------------------------------------------------- /resources/OmniExample.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/resources/OmniExample.png -------------------------------------------------------------------------------- /resources/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/resources/traj.png -------------------------------------------------------------------------------- /resources/trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/resources/trajectory.pdf -------------------------------------------------------------------------------- /ros1_ws/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /ros1_ws/src/data_processing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | data_processing 4 | 0.0.0 5 | The data_processing package 6 | 7 | 8 | 9 | 10 | nickybones 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | std_msgs 54 | message_generation 55 | message_runtime 56 | std_msgs 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /ros1_ws/src/data_processing/srv/NoArguments.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string return_value -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_msgs/msg/Omni3D.msg: -------------------------------------------------------------------------------- 1 | float32[] center 2 | float32[] dim 3 | float32[] rot 4 | int64 category 5 | float32 confidence 6 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_msgs/msg/Omni3DArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Omni3D[] detections -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nmcl_msgs 4 | 0.0.0 5 | The nmcl_msgs package 6 | 7 | 8 | 9 | 10 | nickybones 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | sensor_msgs 56 | roscpp 57 | rospy 58 | std_msgs 59 | roscpp 60 | rospy 61 | std_msgs 62 | 63 | 64 | 65 | message_generation 66 | message_runtime 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(nmcl_ros) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | #set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -Wall ${CMAKE_CXX_FLAGS}") 8 | 9 | #set(nlohmann_json_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/build/nlohmann_json) 10 | 11 | 12 | ## Specify additional locations of header files 13 | ## Your package locations should be listed before other locations 14 | 15 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) 16 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/) 17 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) 18 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/nmcl/include/) 19 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/nsensors/include/) 20 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/nmap/include/) 21 | 22 | 23 | ## Declare a C++ library 24 | # add_library(${PROJECT_NAME} 25 | # src/${PROJECT_NAME}/nmcl.cpp 26 | # ) 27 | 28 | set(RESULTS_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/results) 29 | file(MAKE_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}) 30 | 31 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 32 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/lib) 33 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${RESULTS_OUTPUT_DIRECTORY}/bin) 34 | link_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/build/lib) 35 | 36 | 37 | # Attempt to find OpenCV4 on your system, for more details please read 38 | # /usr/share/OpenCV/OpenCVConfig.cmake 39 | 40 | find_package(OpenMP) 41 | if (OPENMP_FOUND) 42 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 43 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 44 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 45 | endif() 46 | 47 | find_package( 48 | catkin REQUIRED COMPONENTS roscpp sensor_msgs message_filters geometry_msgs nav_msgs tf2 tf cv_bridge pcl_ros nmcl_msgs) 49 | include_directories(${catkin_INCLUDE_DIRS}) 50 | find_package(Eigen3 REQUIRED) 51 | find_package(OpenCV 4 REQUIRED) 52 | if(OpenCV_FOUND) 53 | message(STATUS "Found OpenCV version ${OpenCV_VERSION}") 54 | message(STATUS "OpenCV directories: ${OpenCV_INCLUDE_DIRS}") 55 | include_directories(${OpenCV_INCLUDE_DIRS}) 56 | else() 57 | message(FATAL_ERROR "OpenCV not found, please read the README.md") 58 | endif(OpenCV_FOUND) 59 | 60 | find_package(Boost COMPONENTS serialization system filesystem) 61 | if(Boost_FOUND) 62 | message(STATUS "Found Boost version ${Boost_VERSION}") 63 | message(STATUS "Boost directories: ${Boost_INCLUDE_DIRS}") 64 | include_directories(${Boost_INCLUDE_DIRS}) 65 | else() 66 | message(FATAL_ERROR "Boost not found, please read the README.md") 67 | endif(Boost_FOUND) 68 | 69 | #find_package(Torch REQUIRED) 70 | #if(Torch_FOUND) 71 | # message(STATUS "Found Torch version ${Torch_VERSION}") 72 | # message(STATUS "Torch directories: ${Torch_INCLUDE_DIRS}") 73 | # include_directories(${Torch_INCLUDE_DIRS}) 74 | #else() 75 | # message(FATAL_ERROR "OpenCV not found, please read the README.md") 76 | #endif(Torch_FOUND) 77 | 78 | find_package(nlohmann_json 3.2.0 REQUIRED) 79 | if(nlohmann_json_FOUND) 80 | message(STATUS "Found nlohmann_json") 81 | else() 82 | message(FATAL_ERROR "nlohmann_json not found, please read the README.md") 83 | endif(nlohmann_json_FOUND) 84 | 85 | # After all are setup is done, we can go to our src/ directory to build our 86 | # files 87 | add_subdirectory("src") 88 | message(STATUS "Nicky ${CMAKE_CURRENT_SOURCE_DIR}") 89 | add_library(NMCL STATIC IMPORTED) 90 | add_library(NSENSORS STATIC IMPORTED) 91 | add_library(NMAP STATIC IMPORTED) 92 | 93 | 94 | # Provide the full path to the library, so CMake knows where to find it. 95 | set_target_properties(NMCL PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/build/lib/libNMCL.a) 96 | set_target_properties(NSENSORS PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/build/lib/libNSENSORS.a) 97 | set_target_properties(NMAP PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../../../ncore/build/lib/libNMAP.a) 98 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/include/RosUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: RosUtils.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | #ifndef ROSUTILS_H 13 | #define ROSUTILS_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "opencv2/opencv.hpp" 24 | #include 25 | 26 | 27 | Eigen::Vector3f OdomMsg2Pose2D(const nav_msgs::OdometryConstPtr& odom); 28 | 29 | Eigen::Vector3f PoseMsg2Pose2D(const geometry_msgs::PoseStampedConstPtr& poseMsg); 30 | 31 | std::vector OdomMsg2Pose3D(const nav_msgs::OdometryConstPtr& odom); 32 | 33 | std::vector PoseMsg2Pose3D(const geometry_msgs::PoseStampedConstPtr& odom); 34 | 35 | geometry_msgs::PoseStamped Pose2D2PoseMsg(Eigen::Vector3f pose2d); 36 | 37 | 38 | pcl::PointCloud Vec2PointCloud(const std::vector& points3d, Eigen::Vector3f rgb); 39 | 40 | pcl::PointCloud Vec2PointCloud(const std::vector& points3d); 41 | 42 | pcl::PointCloud Vec2RGBPointCloud(const std::vector& points3d, std::vector rgb); 43 | 44 | 45 | pcl::PointCloud ManyVec2PointCloud(const std::vector>& points3d, std::vector rgb); 46 | 47 | 48 | geometry_msgs::PoseWithCovarianceStamped Pred2PoseWithCov(Eigen::Vector3d pred, Eigen::Matrix3d cov); 49 | 50 | sensor_msgs::Image CVMat2ImgMsg(const cv::Mat& img, std_msgs::Header header, const std::string& type); 51 | 52 | 53 | 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/launch/confignmcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | [0.15, 0.15, 0.15] 15 | [1.0] 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | [0.15, 0.15, 0.15] 29 | [1.0] 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/launch/dingo_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | [0.15, 0.15, 0.15] 13 | [1.0] 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | ["/camera0/color/image_raw", "/camera1/color/image_raw", "/camera2/color/image_raw", "/camera3/color/image_raw"] 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/launch/youbot_eval.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | [0.15, 0.15, 0.15] 23 | [1.0] 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | ["/camera0/color/image_raw", "/camera1/color/image_raw", "/camera2/color/image_raw", "/camera3/color/image_raw"] 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/launch/youbot_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | [0.15, 0.15, 0.15] 13 | [1.0] 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | ["/camera0/color/image_raw", "/camera1/color/image_raw", "/camera2/color/image_raw", "/camera3/color/image_raw"] 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nmcl_ros 4 | 0.0.0 5 | The nmcl_ros package 6 | 7 | 8 | 9 | 10 | nickybones 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | catkin 53 | roscpp 54 | rospy 55 | std_msgs 56 | sensor_msgs 57 | nmcl_msgs 58 | 59 | 60 | roscpp 61 | rospy 62 | std_msgs 63 | roscpp 64 | rospy 65 | std_msgs 66 | sensor_msgs 67 | nmcl_msgs 68 | 69 | catkin 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | add_executable(ConfigNMCLNode ConfigNMCLNode.cpp RosUtils.cpp) 9 | target_link_libraries(ConfigNMCLNode ${OpenCV_LIBS} ${catkin_LIBRARIES} NMCL NSENSORS NMAP ${Boost_LIBRARIES} nlohmann_json::nlohmann_json) 10 | 11 | -------------------------------------------------------------------------------- /ros1_ws/src/nmcl_ros/src/GMAP.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | 4 | class GMAP(): 5 | 6 | def __init__(self, yaml, gridmap): 7 | 8 | self.map = gridmap 9 | self.resolution = yaml['resolution'] 10 | self.origin = yaml['origin'] 11 | self.max_y = gridmap.shape[0] 12 | self.ComputeBorders() 13 | 14 | 15 | def __init__(self, gridmap, resolution, origin): 16 | 17 | self.map = gridmap 18 | self.resolution = resolution 19 | self.origin = origin 20 | self.max_y = gridmap.shape[0] 21 | self.ComputeBorders() 22 | 23 | # the (0,0) of the gmapping map is where the robot started from. To relate this to the gridmap image 24 | # We need to know the real world coordinates of some image point. 25 | # the map origin in the bottom left corner of the image, and its real world coordinates are 26 | # specified in the metadata yaml. 27 | 28 | def world2map(self, p): 29 | u = np.round((p[0] - self.origin[0]) / self.resolution) 30 | v = self.max_y - np.round((p[1] - self.origin[1]) / self.resolution) 31 | return np.array([u, v]).astype(int) 32 | 33 | def map2world(self, uv): 34 | x = uv[0] * self.resolution + self.origin[0] 35 | y = (self.max_y - uv[1]) * self.resolution + self.origin[1] 36 | 37 | return np.array([x, y]) 38 | 39 | def ComputeBorders(self): 40 | self.map = 255 - cv2.cvtColor(self.map, cv2.COLOR_BGR2GRAY) 41 | white_pixels = np.array(np.where(self.map == 255)) 42 | min_x = min(white_pixels[1]) 43 | max_x = max(white_pixels[1]) 44 | min_y = min(white_pixels[0]) 45 | max_y = max(white_pixels[0]) 46 | self.map_border = {"min_x": min_x, "min_y": min_y, "max_x": max_x, "max_y": max_y} 47 | 48 | def TopLeft(self): 49 | return np.array([self.map_border["min_x"], self.map_border["min_y"]]) 50 | 51 | def BottomRight(self): 52 | return np.array([self.map_border["max_x"], self.map_border["max_y"]]) 53 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/Base.yaml: -------------------------------------------------------------------------------- 1 | SOLVER: 2 | TYPE: "sgd" 3 | IMS_PER_BATCH: 32 4 | BASE_LR: 0.02 5 | STEPS: (19200, 25600) 6 | MAX_ITER: 32000 7 | WEIGHT_DECAY: 0.0001 8 | LR_SCHEDULER_NAME: "WarmupMultiStepLR" 9 | INPUT: 10 | MIN_SIZE_TRAIN: (256, 272, 288, 304, 320, 336, 352, 368, 384, 400, 416, 432, 448, 464, 480, 496, 512, 528, 544, 560, 576, 592, 608, 624, 640,) 11 | MIN_SIZE_TEST: 512 12 | MAX_SIZE_TRAIN: 4096 13 | MAX_SIZE_TEST: 4096 14 | TEST: 15 | VISIBILITY_THRES: 0.33333333 16 | TRUNCATION_THRES: 0.33333333 17 | EVAL_PERIOD: 16000 18 | DATASETS: 19 | TRAIN: ('KITTI_train', 'KITTI_val') 20 | TEST: ('KITTI_test',) 21 | CATEGORY_NAMES: ('pedestrian', 'car', 'cyclist', 'van', 'truck', 'tram', 'person') 22 | IGNORE_NAMES: "['dontcare', 'ignore', 'void']" 23 | MIN_HEIGHT_THRES: 0.05 24 | TRUNCATION_THRES: 0.75 25 | VISIBILITY_THRES: 0.25 26 | TRUNC_2D_BOXES: True 27 | VIS_PERIOD: 640 28 | DATALOADER: 29 | SAMPLER_TRAIN: "RepeatFactorTrainingSampler" 30 | REPEAT_THRESHOLD: 0.1 31 | MODEL: 32 | PIXEL_MEAN: [103.530, 116.280, 123.675] 33 | PIXEL_STD: [57.375, 57.120, 58.395] 34 | META_ARCHITECTURE: "RCNN3D" 35 | MASK_ON: False 36 | STABILIZE: 0.02 37 | USE_BN: True 38 | BACKBONE: 39 | FREEZE_AT: 0 40 | NAME: 'build_dla_from_vision_fpn_backbone' 41 | DLA: 42 | TYPE: 'dla34' 43 | FPN: 44 | IN_FEATURES: ['p2', 'p3', 'p4', 'p5', 'p6'] 45 | ANCHOR_GENERATOR: 46 | SIZES: [[32], [64], [128], [256], [512]] # One size for each in feature map 47 | ASPECT_RATIOS: [[0.5, 1.0, 2.0]] # Three aspect ratios (same for all in feature maps) 48 | RPN: 49 | HEAD_NAME: "StandardRPNHead" 50 | IN_FEATURES: ['p2', 'p3', 'p4', 'p5', 'p6'] 51 | PRE_NMS_TOPK_TRAIN: 2000 # Per FPN level 52 | PRE_NMS_TOPK_TEST: 1000 # Per FPN level 53 | POST_NMS_TOPK_TRAIN: 1000 54 | POST_NMS_TOPK_TEST: 1000 55 | BOUNDARY_THRESH: -1 56 | OBJECTNESS_UNCERTAINTY: "IoUness" 57 | IOU_THRESHOLDS: [0.05, 0.05] 58 | POSITIVE_FRACTION: 1.0 59 | PROPOSAL_GENERATOR: 60 | NAME: "RPNWithIgnore" 61 | ROI_HEADS: 62 | NAME: "ROIHeads3D" 63 | IN_FEATURES: ["p2", "p3", "p4", "p5", 'p6'] 64 | BATCH_SIZE_PER_IMAGE: 512 65 | SCORE_THRESH_TEST: 0.01 66 | NUM_CLASSES: 43 67 | ROI_BOX_HEAD: 68 | NAME: "FastRCNNConvFCHead" 69 | NUM_FC: 2 70 | POOLER_RESOLUTION: 7 71 | ROI_CUBE_HEAD: 72 | NAME: 'CubeHead' 73 | Z_TYPE: 'direct' 74 | POSE_TYPE: '6d' 75 | NUM_FC: 2 76 | SHARED_FC: True 77 | USE_CONFIDENCE: 1.0 78 | LOSS_W_3D: 1.0 79 | POOLER_TYPE: 'ROIAlignV2' 80 | POOLER_RESOLUTION: 7 81 | DIMS_PRIORS_ENABLED: True 82 | DISENTANGLED_LOSS: True 83 | ALLOCENTRIC_POSE: True 84 | VIRTUAL_FOCAL: 512.0 85 | VIRTUAL_DEPTH: True 86 | CHAMFER_POSE: True 87 | VERSION: 2 -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/Map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ros1_ws/src/omni3d_ros/configs/Map.png -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/Map.yaml: -------------------------------------------------------------------------------- 1 | image: Map.png 2 | resolution: 0.05 3 | origin: 4 | - -13.9155 5 | - -11.04537 6 | - 0.0 7 | negate: 0 8 | occupied_thresh: 0.65 9 | free_thresh: 0.196 10 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/args.json: -------------------------------------------------------------------------------- 1 | { 2 | "gmap" :"/SIMP/ros1_ws/src/omni3d_ros/configs/Map.yaml", 3 | "roomseg" : "/SIMP/ros1_ws/src/omni3d_ros/configs/roomseg.png", 4 | "categories" : ["sink", "door", "oven", "board", "table", "box", "potted plant", "drawers", "sofa", "cabinet", "chair", "fire extinguisher", "person", "desk"], 5 | "yolo_conf" : [0.7, 0.5, 0.7, 0.7, 0.6, 0.7, 0.7, 0.7, 0.8, 0.7, 0.7, 0.7, 0.6, 0.7], 6 | "omni_conf" : [0.7, 0.0, 0.8, 0.9, 0.7, 0.0, 0.95, 0.8, 0.7, 0.9, 0.0, 0.9, 0.0, 0.8], 7 | "min_dist" : [0.5, 0.0, 0.0, 1.5, 1.0, 0.0, 0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0, 0.5], 8 | "max_dist" : [3.0, 0.0, 4.0, 4.5, 4.5, 0.0, 4.5, 3.5, 3.0, 5.0, 0.0, 3.0, 0.0, 4.0], 9 | "width" : 640, 10 | "height" : 480, 11 | "2DvisTest" : true, 12 | "3DvisTest" : true, 13 | "YOLOvisTest" : true, 14 | "odomTrigger" : true, 15 | "odom_trigerTH" : [0.2, 0.1], 16 | "totalSeenTH" : 1, 17 | "costTH" : 0.3, 18 | "truncTH" : 0.5, 19 | "skipTH" : 20, 20 | "variancePath" : "/SIMP/ros1_ws/src/omni3d_ros/configs/variance.csv", 21 | "K" : [380.07025146484375, 0.0, 324.4729919433594, 0.0, 379.66119384765625, 237.78517150878906, 0.0, 0.0, 1.0], 22 | "cam_poses" : [0.1, 0, 0, 0, 0, -0.1, 0, -1.5707963267948966, -0.1, 0, 0, 3.141592653589793, 0, 0.1, 0, 1.5707963267948966], 23 | "angles" : [ 0, -1.5707963267948966,3.141592653589793, 1.5707963267948966], 24 | "min_z" : -1.55708286, 25 | "camera_z" : 0.63 26 | } -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/category_meta.json: -------------------------------------------------------------------------------- 1 | {"thing_classes": ["sink", "door", "oven", "board", "table", "box", "potted plant", "drawers", "sofa", "cabinet", "chair", "fire extinguisher", "person", "desk"], "thing_dataset_id_to_contiguous_id": {"0": 0, "1": 1, "2": 2, "3": 3, "4": 4, "5": 5, "6": 6, "7": 7, "8": 8, "9": 9, "10": 10, "11": 11, "12": 12, "13": 13}} -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/omnidataset.yaml: -------------------------------------------------------------------------------- 1 | train: data/images/training/ 2 | val: data/images/validation/ 3 | 4 | # number of classes 5 | nc: 14 6 | 7 | # class names 8 | names: ['sink', 'door', 'oven', 'board', 'table', 'box', 'potted plant', 'drawers', 'sofa', 'cabinet', 'chair', 'fire extinguisher', 'person', 'desk'] -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/roomseg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ros1_ws/src/omni3d_ros/configs/roomseg.png -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/trained_Omni3D.yaml: -------------------------------------------------------------------------------- 1 | _BASE_: "Base.yaml" 2 | SOLVER: 3 | TYPE: "sgd" 4 | IMS_PER_BATCH: 12 5 | BASE_LR: 0.0015 6 | STEPS: (3340800, 4454400) 7 | MAX_ITER: 5568000 8 | WARMUP_ITERS: 174000 9 | TEST: 10 | EVAL_PERIOD: 2500 11 | VIS_PERIOD: 2320 12 | DATASETS: 13 | TRAIN: ('Hypersim_train', 'Hypersim_val') 14 | TEST: ('Hypersim_test',) 15 | CATEGORY_NAMES: ('sink', 'door', 'oven', 'board', 'table', 'box', 'potted plant', 'drawers', 'sofa', 'cabinet', 'chair', 'fire extinguisher', 'person', 'desk') # ('chair', 'table', 'cabinet', 'car', 'lamp', 'books', 'sofa', 'pedestrian', 'picture', 'window', 'pillow', 'truck', 'door', 'blinds', 'sink', 'shelves', 'television', 'shoes', 'cup', 'bottle', 'bookcase', 'laptop', 'desk', 'cereal box', 'floor mat', 'traffic cone', 'mirror', 'barrier', 'counter', 'camera', 'bicycle', 'toilet', 'bus', 'bed', 'refrigerator', 'trailer', 'box', 'oven', 'clothes', 'van', 'towel', 'motorcycle', 'night stand', 'stove', 'fire extinguisher', 'chair', 'stationery', 'bathtub', 'cyclist', 'curtain', 'bin', 'poop') 16 | MODEL: 17 | ROI_HEADS: 18 | NUM_CLASSES: 14 19 | WEIGHTS: 'cubercnn_DLA34_FPN.pth' 20 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/configs/variance.csv: -------------------------------------------------------------------------------- 1 | category,mx,my,sx,sy,b 2 | 0,-1.2059629422293427,-0.21596509127955354,3.5598299922803824,4.752782645150083,2.0 3 | 1,,,,, 4 | 2,-0.2719821124479019,-0.09356356143728917,5.2496109773482,5.927369127588248,4.0 5 | 3,-0.917821683643759,1.0360433732917682,2.952211852916148,12.65687793596594,2.0 6 | 4,-1.4398911670302559,-0.9773037293126856,7.69218980951617,16.189173686580585,4.0 7 | 5,,,,, 8 | 6,-3.2148634474083,-1.5600138203264704,2.945795021310433,3.269590787245149,2.0 9 | 7,-0.3996214251000274,-0.5297622941886962,5.494713481959431,3.8353573775340055,2.0 10 | 8,-0.7996663557464608,2.304188897078266,7.152909344647871,22.032779109063913,2.0 11 | 9,-0.9854608379604701,0.7808889332308545,4.694373920974595,11.558330395794945,2.0 12 | 10,,,,, 13 | 11,0.1639741907493567,-0.37827609098200454,1.895822092187304,2.198324715132525,2.0 14 | 12,,,,, 15 | 13,5.287808297665473,6.392892568902049,5.417909228561944,20.390211704373392,2.0 16 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/launch/broadcast.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/launch/dingo_omni3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/launch/gt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/launch/omni3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | omni3d_ros 4 | 0.0.0 5 | The omni3d_ros package 6 | 7 | 8 | 9 | 10 | nickybones 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | std_msgs 54 | sensor_msgs 55 | message_generation 56 | message_runtime 57 | std_msgs 58 | sensor_msgs 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/BroadcastMapNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import logging 4 | import os 5 | import argparse 6 | import sys 7 | import numpy as np 8 | from collections import OrderedDict 9 | import torch 10 | import cv2 11 | import rospy 12 | from sensor_msgs.msg import Image 13 | from cv_bridge import CvBridge 14 | from message_filters import ApproximateTimeSynchronizer, Subscriber 15 | from std_msgs.msg import UInt16, Float32MultiArray 16 | from nmcl_msgs.msg import Omni3D, Omni3DArray 17 | from visualization_msgs.msg import Marker 18 | from visualization_msgs.msg import MarkerArray 19 | from geometry_msgs.msg import Point 20 | import time 21 | import open3d as o3d 22 | import copy 23 | from DatasetUtils import get_cuboid_verts_faces, convert_3d_box_to_2d, getTrunc2Dbbox 24 | from matplotlib import cm 25 | from scipy.spatial import ConvexHull 26 | from MapObjectTracker import MapObjectTracker 27 | from scipy.spatial.transform import Rotation as R 28 | from ObjectUtils import * 29 | 30 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 31 | 32 | all_classes = ['sink', 'door', 'oven', 'board', 'table', 'box', 'potted plant', 'drawers', 'sofa', 'cabinet', 'chair', 'fire extinguisher', 'person', 'desk'] 33 | camere_z = 0.63 34 | min_z = -1.55708286 35 | 36 | 37 | class BroadcastMapDNode(): 38 | 39 | 40 | def __init__(self): 41 | 42 | self.marker_pub = rospy.Publisher("omni3dMarkerTopic", MarkerArray, queue_size=10) 43 | 44 | self.clr = cm.rainbow(np.linspace(0, 1, 14)) 45 | 46 | rospy.loginfo("BroadcastMapDNode::Ready!") 47 | 48 | mapObjects = LoadTrackedObjectsFromPickle("/home/nickybones/Code/OmniNMCL/ros1_ws/src/omni3d_ros/", "mapObjects_ICP2.pickle") 49 | markers = self.createMarkers(mapObjects) 50 | 51 | while not rospy.is_shutdown(): 52 | 53 | markerArray = MarkerArray() 54 | markerArray.markers = markers 55 | # Renumber the marker IDs 56 | id = 0 57 | for m in markerArray.markers: 58 | m.id = id 59 | id += 1 60 | 61 | # Publish the MarkerArray 62 | self.marker_pub .publish(markerArray) 63 | 64 | rospy.sleep(0.01) 65 | 66 | 67 | 68 | def createMarkers(self, mapObjs): 69 | 70 | markers = [] 71 | 72 | for obj in mapObjs: 73 | marker = Marker() 74 | marker.header.frame_id = "map" 75 | marker.action = marker.ADD 76 | marker.type = marker.CUBE; 77 | marker.pose.position.x = obj.center[0] 78 | marker.pose.position.y = obj.center[1] 79 | marker.pose.position.z = obj.center[2] - min_z 80 | 81 | r = R.from_matrix(obj.rot) 82 | q = r.as_quat() 83 | color = self.clr[obj.category] 84 | 85 | marker.pose.orientation.x = q[0] 86 | marker.pose.orientation.y = q[1] 87 | marker.pose.orientation.z = q[2] 88 | marker.pose.orientation.w = q[3] 89 | marker.scale.x = obj.dim[2] 90 | marker.scale.y = obj.dim[1] 91 | marker.scale.z = obj.dim[0] 92 | marker.color.a = 0.8 93 | marker.color.r = color[2] 94 | marker.color.g = color[1] 95 | marker.color.b = color[0] 96 | marker.lifetime = rospy.Duration(3) 97 | 98 | markers.append(marker) 99 | 100 | return markers 101 | 102 | 103 | 104 | 105 | def main(): 106 | 107 | 108 | rospy.init_node('BroadcastMapDNode', anonymous=True) 109 | omn = BroadcastMapDNode() 110 | rospy.spin() 111 | 112 | if __name__ == "__main__": 113 | 114 | main() 115 | 116 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/GMAP.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import cv2 4 | import yaml 5 | import os 6 | 7 | class GMAP(): 8 | 9 | 10 | 11 | def __init__(self, *args): 12 | 13 | 14 | if len(args) == 1: 15 | yamlPath = args[0] 16 | folderPath, _ = os.path.split(yamlPath) 17 | print(folderPath) 18 | with open(yamlPath, 'r') as stream: 19 | data = yaml.safe_load(stream) 20 | 21 | self.map = cv2.imread(folderPath + "/" + data['image']) 22 | self.resolution = data['resolution'] 23 | self.origin = data['origin'] 24 | self.max_y = self.map.shape[0] 25 | self.ComputeBorders() 26 | 27 | elif len(args) == 2: 28 | yamlConf = args[0] 29 | gridmap = args[1] 30 | self.map = gridmap 31 | self.resolution = yamlConf['resolution'] 32 | self.origin = yamlConf['origin'] 33 | self.max_y = gridmap.shape[0] 34 | self.ComputeBorders() 35 | 36 | elif len(args) == 3: 37 | gridmap = args[0] 38 | resolution = args[1] 39 | origin = args[2] 40 | self.map = gridmap 41 | self.resolution = resolution 42 | self.origin = origin 43 | self.max_y = gridmap.shape[0] 44 | self.ComputeBorders() 45 | 46 | 47 | # the (0,0) of the gmapping map is where the robot started from. To relate this to the gridmap image 48 | # We need to know the real world coordinates of some image point. 49 | # the map origin in the bottom left corner of the image, and its real world coordinates are 50 | # specified in the metadata yaml. 51 | 52 | def world2map(self, p): 53 | u = np.round((p[0] - self.origin[0]) / self.resolution) 54 | v = self.max_y - np.round((p[1] - self.origin[1]) / self.resolution) 55 | return np.array([u, v]).astype(int) 56 | 57 | def map2world(self, uv): 58 | x = uv[0] * self.resolution + self.origin[0] 59 | y = (self.max_y - uv[1]) * self.resolution + self.origin[1] 60 | 61 | return np.array([x, y]) 62 | 63 | def ComputeBorders(self): 64 | self.map = 255 - cv2.cvtColor(self.map, cv2.COLOR_BGR2GRAY) 65 | white_pixels = np.array(np.where(self.map == 255)) 66 | min_x = min(white_pixels[1]) 67 | max_x = max(white_pixels[1]) 68 | min_y = min(white_pixels[0]) 69 | max_y = max(white_pixels[0]) 70 | self.map_border = {"min_x": min_x, "min_y": min_y, "max_x": max_x, "max_y": max_y} 71 | 72 | def TopLeft(self): 73 | return np.array([self.map_border["min_x"], self.map_border["min_y"]]) 74 | 75 | def BottomRight(self): 76 | return np.array([self.map_border["max_x"], self.map_border["max_y"]]) 77 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/GTNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import cv2 4 | import json 5 | import rospy 6 | import pandas as pd 7 | from sensor_msgs.msg import Image 8 | from geometry_msgs.msg import PoseStamped 9 | import tf 10 | import numpy as np 11 | from scipy.spatial.transform import Rotation as R 12 | 13 | 14 | class GTNode(): 15 | 16 | def __init__(self): 17 | 18 | csvPath = rospy.get_param('~csvPath') 19 | gtTopic = rospy.get_param('gtTopic') 20 | self.df = pd.read_csv(csvPath) 21 | jsonPath = rospy.get_param('jsonPath') 22 | with open(jsonPath) as f: 23 | self.args = json.load(f) 24 | self.camera_z = self.args['camera_z'] 25 | cameraTopic = rospy.get_param('cameraTopic') 26 | 27 | 28 | self.sub = rospy.Subscriber(cameraTopic, Image, self.callback) 29 | 30 | self.pose_pub = rospy.Publisher(gtTopic, PoseStamped, queue_size=1) 31 | self.tf_pub = tf.TransformBroadcaster() 32 | 33 | rospy.loginfo("GTNode::Ready!") 34 | 35 | 36 | def callback(self, cam0_msg): 37 | 38 | t_img = cam0_msg.header.stamp.to_nsec() 39 | row = self.df.iloc[(self.df['t']-t_img).abs().argsort()[:1]] 40 | x = row['gt_x'].to_numpy()[0] 41 | y = row['gt_y'].to_numpy()[0] 42 | z = self.camera_z 43 | yaw = row['gt_yaw'].to_numpy()[0] 44 | #print(x, y, yaw, t_img) 45 | 46 | quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw) 47 | pose_msg = PoseStamped() 48 | pose_msg.pose.orientation.x = quaternion[0] 49 | pose_msg.pose.orientation.y = quaternion[1] 50 | pose_msg.pose.orientation.z = quaternion[2] 51 | pose_msg.pose.orientation.w = quaternion[3] 52 | pose_msg.pose.position.x = x 53 | pose_msg.pose.position.y = y 54 | pose_msg.pose.position.z = z 55 | pose_msg.header.stamp = cam0_msg.header.stamp 56 | pose_msg.header.frame_id = "map" 57 | 58 | self.pose_pub.publish(pose_msg) 59 | 60 | 61 | 62 | 63 | def main(): 64 | 65 | 66 | rospy.init_node('GTNode', anonymous=True) 67 | #rate = rospy.Rate(10) 68 | gt = GTNode() 69 | rospy.spin() 70 | 71 | if __name__ == "__main__": 72 | 73 | main() 74 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/InterTest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import cv2 4 | import rospy 5 | import pandas as pd 6 | from message_filters import ApproximateTimeSynchronizer, Subscriber 7 | from sensor_msgs.msg import Image 8 | from geometry_msgs.msg import PoseStamped, Transform 9 | import tf 10 | from tf import transformations as ts 11 | import numpy as np 12 | import math 13 | from scipy.interpolate import interp1d 14 | from bisect import insort, bisect_left 15 | from collections import deque 16 | from itertools import islice 17 | from scipy.spatial.transform import Rotation as R 18 | 19 | camera_z = 0.63 20 | min_z = -1.55708286 21 | 22 | def running_median_insort(seq, window_size): 23 | """Contributed by Peter Otten""" 24 | seq = iter(seq) 25 | d = deque() 26 | s = [] 27 | result = [] 28 | for item in islice(seq, window_size): 29 | d.append(item) 30 | insort(s, item) 31 | result.append(s[len(d)//2]) 32 | m = window_size // 2 33 | for item in seq: 34 | old = d.popleft() 35 | d.append(item) 36 | del s[bisect_left(s, old)] 37 | insort(s, item) 38 | result.append(s[m]) 39 | return result 40 | 41 | 42 | def interpolate(t, poses, kind='nearest', window=2, tol=0.5): 43 | """Contributed by Jerome Guzzi and adapted to our data""" 44 | 45 | n = len(poses) 46 | data = np.zeros((n, 4)) 47 | 48 | for i in range(n): 49 | data[i, 0] = t[i] 50 | data[i, 1:] = poses[i] 51 | 52 | 53 | # Filter outliers in yaw 54 | data = data[np.abs(data[:, 3] - running_median_insort(data[:, 3], window)) < tol] 55 | data[0, -1] = np.fmod(data[0, -1], 2 * np.pi) 56 | 57 | return interp1d(data[:, 0], data[:, 1:], axis=0, fill_value='extrapolate', assume_sorted=True, kind=kind) 58 | 59 | 60 | 61 | 62 | def main(): 63 | 64 | 65 | df = pd.read_csv("/home/nickybones/data/MCL/omni3d/Map3/raw_mcl.csv") 66 | gt_t = [] 67 | gt_poses = [] 68 | n = len(df['gt_x']) 69 | t = df['t'] 70 | x = df['gt_x'].to_numpy() 71 | y = df['gt_y'].to_numpy() 72 | yaw = df['gt_yaw'].to_numpy() 73 | 74 | 75 | for i in range(n): 76 | 77 | gt_poses.append(np.array([x[i], y[i], yaw[i]])) 78 | gt_t.append(t[i]) 79 | #print(gt_poses[i], gt_t[i]) 80 | 81 | 82 | t1 = 1673979858458452341 83 | t2 = 1673979858505056416 84 | t3 = int((t1 + t2) / 2) 85 | print("[ 7.02680039 -2.95074724 3.90965687] 1673979858458452341") 86 | print("[ 7.02663729 -2.94933241 3.84571609] 1673979858505056416") 87 | intr = interpolate(gt_t, gt_poses) 88 | 89 | 90 | pose = intr(t3) 91 | print(pose, t3) 92 | 93 | if __name__ == "__main__": 94 | 95 | main() -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/JSONUtils.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import json 4 | 5 | def FindGTByImageID(images, image_id): 6 | 7 | for im in images: 8 | if im['id'] == image_id: 9 | return im['gt'] 10 | 11 | 12 | return None 13 | 14 | 15 | def FindLastImageFrameNum(images): 16 | 17 | im = images[-1] 18 | imOldPath = im['file_path'] 19 | camPath = os.path.split(imOldPath)[1].replace('.png', '') 20 | 21 | return int(camPath) 22 | 23 | 24 | 25 | return None 26 | 27 | def FindImageByImageID(images, image_id): 28 | 29 | for im in images: 30 | if im['id'] == image_id: 31 | return im 32 | 33 | 34 | return None 35 | 36 | def FindAllImagseBySeqID(images, seq_id): 37 | 38 | seq_images = [] 39 | for im in images: 40 | if im['seq_id'] == seq_id: 41 | seq_images.append(im) 42 | 43 | 44 | return seq_images 45 | 46 | def FindAllImagseByFrameID(images, frame_id): 47 | 48 | frame_images = [] 49 | for im in images: 50 | if "/{:05d}.png".format(frame_id) in im['file_path']: 51 | frame_images.append(im) 52 | 53 | 54 | return frame_images 55 | 56 | def GetAllSequences(images): 57 | 58 | seq_ids = set() 59 | 60 | for im in images: 61 | seq_ids.add(im['seq_id']) 62 | 63 | return list(seq_ids) 64 | 65 | 66 | def FindAllObjectsByImageID(objects, image_id): 67 | 68 | matches = [] 69 | 70 | for o in objects: 71 | if o['image_id'] == image_id: 72 | matches.append(o) 73 | 74 | 75 | return matches 76 | 77 | def LoadObjectsFromJSON(jsonPath): 78 | 79 | with open(jsonPath) as f: 80 | data = json.load(f) 81 | 82 | 83 | images = data['images'] 84 | objects = data['annotations'] 85 | 86 | return images, objects 87 | 88 | 89 | 90 | def getCamNmeFromImage(im): 91 | 92 | seq = im['seq_id'] 93 | imOldPath = im['file_path'] 94 | c = getCamNme(imOldPath) 95 | 96 | return c 97 | 98 | 99 | def getCamNme(imOldPath): 100 | 101 | camPath = os.path.split(imOldPath)[0] 102 | camPath = os.path.split(camPath)[1] 103 | 104 | if '00' in camPath: 105 | return 0 106 | 107 | elif '01' in camPath: 108 | return 1 109 | 110 | elif '02' in camPath: 111 | return 2 112 | 113 | elif '03' in camPath: 114 | return 3 115 | 116 | else: 117 | return -1 118 | 119 | def retrieveOrigImagePath(im): 120 | 121 | seq = im['seq_id'] 122 | imOldPath = im['file_path'] 123 | imName = os.path.split(imOldPath)[1] 124 | c = getCamNme(imOldPath) 125 | imPath = "R{}/camera{}/color/{}".format(seq,c,imName) 126 | 127 | return imPath 128 | 129 | 130 | def getCategories(predJsonPath): 131 | 132 | with open(predJsonPath) as f: 133 | data = json.load(f) 134 | 135 | cats = data['categories'] 136 | 137 | classes = [] 138 | 139 | for cat in cats: 140 | classes.append(cat['name']) 141 | 142 | return classes -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/Lab3DObject.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | 5 | 6 | class Lab3DObject(): 7 | 8 | gid = 0 9 | 10 | def __init__(self, oid, category, pos, dim, yaw): 11 | self.oid = oid 12 | self.category = category 13 | self.pos = np.array([pos['x'], pos['y'], pos['z']], dtype=np.float64) 14 | self.dim = np.array([dim['x'], dim['y'], dim['z']], dtype=np.float64) 15 | self.yaw = yaw 16 | self.gid = Lab3DObject.gid 17 | 18 | Lab3DObject.gid += 1 19 | 20 | 21 | def __init__(self, jObj): 22 | 23 | self.oid = jObj['id'] 24 | self.category = jObj['category_id'] 25 | pos = jObj['position'] 26 | dim = jObj['dimensions'] 27 | self.pos = np.array([pos['x'], pos['y'], pos['z']], dtype=np.float64) 28 | self.dim = np.array([dim['x'], dim['y'], dim['z']], dtype=np.float64) 29 | self.yaw = jObj['yaw'] 30 | self.gid = Lab3DObject.gid 31 | 32 | Lab3DObject.gid += 1 -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/MapObjectTracker.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | import pandas as pd 4 | from scipy.spatial.transform import Rotation as R 5 | import open3d as o3d 6 | 7 | class MapObjectTracker(): 8 | 9 | uid = 0 10 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 11 | 12 | 13 | def __init__(self, category, center, dim, rot, conf, room=0, static=False, seen=1, skip=0, times_matched=1, times_skipped=0): 14 | self.category = category 15 | self.center = center 16 | self.dim = np.asarray(dim) 17 | self.rot = rot 18 | #self.rot = MapObjectTracker.origin.get_rotation_matrix_from_xyz((0, 0, yaw_mu)) 19 | self.seen = seen 20 | self.times_matched = times_matched 21 | self.times_skipped = times_skipped 22 | self.skip = skip 23 | self.uid = MapObjectTracker.uid 24 | self.conf = conf 25 | self.static = static 26 | self.active = 1 27 | self.inactive = 0 28 | self.room = room 29 | self.volume = self.dim[0] * self.dim[1] * self.dim[2] 30 | 31 | 32 | MapObjectTracker.uid += 1 33 | 34 | 35 | 36 | 37 | 38 | def predict(self, isActive): 39 | 40 | if isActive: 41 | self.active += 1 42 | self.skip += 1 43 | self.times_skipped += 1 44 | self.inactive = 0 45 | else: 46 | self.active = 0 47 | self.inactive += 1 48 | 49 | 50 | 51 | def mergeObject(self, mo): 52 | 53 | self.center = (self.center * self.times_matched + mo.center * mo.times_matched) / ( self.times_matched + mo.times_matched) 54 | 55 | test_rot = np.linalg.inv(self.rot) @ mo.rot 56 | r = R.from_matrix(test_rot) 57 | x, y, z = r.as_euler('xyz', degrees=False) 58 | nrm1 = np.linalg.norm(np.array([0, 0, z])) 59 | 60 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 61 | 62 | rot_180 = origin.get_rotation_matrix_from_xyz((0, 0,np.pi)) 63 | test_rot = np.linalg.inv(self.rot) @ rot_180 @ mo.rot 64 | r = R.from_matrix(test_rot) 65 | x, y, z = r.as_euler('xyz', degrees=False) 66 | nrm2 = np.linalg.norm(np.array([0, 0, z])) 67 | 68 | best_rot = mo.rot 69 | if nrm2 < nrm1: 70 | best_rot = rot_180 @ mo.rot 71 | 72 | rot_avg = (self.rot * self.times_matched + best_rot * mo.times_matched) / ( self.times_matched + mo.times_matched) 73 | u, s, vh = np.linalg.svd(rot_avg, full_matrices=True) 74 | self.rot = u @ vh 75 | 76 | #self.dim = np.maximum(self.dim, np.asarray(mo.dim)) 77 | self.dim = (self.dim * self.times_matched + mo.dim * mo.times_matched) / ( self.times_matched + mo.times_matched) 78 | self.volume = self.dim[0] * self.dim[1] * self.dim[2] 79 | 80 | self.times_matched += mo.times_matched 81 | self.times_skipped = 0 82 | self.skip = 0 83 | self.inactive = 0 84 | 85 | 86 | 87 | def avgRotation(self, mo): 88 | 89 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 90 | 91 | test_rot = np.linalg.inv(self.rot) @ mo.rot 92 | r = R.from_matrix(test_rot) 93 | x, y, z = r.as_euler('xyz', degrees=False) 94 | nrmz1 = np.linalg.norm(np.array([0, 0, z])) 95 | 96 | rot_180 = origin.get_rotation_matrix_from_xyz((0, 0,np.pi)) 97 | test_rot = np.linalg.inv(self.rot) @ rot_180 @ mo.rot 98 | r = R.from_matrix(test_rot) 99 | x, y, z = r.as_euler('xyz', degrees=False) 100 | nrmz2 = np.linalg.norm(np.array([0, 0, z])) 101 | 102 | 103 | # nrmy1 = np.linalg.norm(np.array([0, y, 0])) 104 | 105 | # rot_180 = origin.get_rotation_matrix_from_xyz((0, np.pi,0)) 106 | # test_rot = np.linalg.inv(self.rot) @ rot_180 @ mo.rot 107 | # r = R.from_matrix(test_rot) 108 | # x, y, z = r.as_euler('xyz', degrees=False) 109 | # nrmy2 = np.linalg.norm(np.array([0, y, 0])) 110 | 111 | best_rot = mo.rot 112 | if nrmz2 < nrmz1: 113 | best_rot = rot_180 @ mo.rot 114 | 115 | rot_avg = (self.rot * self.times_matched + best_rot * mo.times_matched) / ( self.times_matched + mo.times_matched) 116 | u, s, vh = np.linalg.svd(rot_avg, full_matrices=True) 117 | self.rot = u @ vh 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/Omni3DCategory.py: -------------------------------------------------------------------------------- 1 | 2 | import json 3 | 4 | class Omni3DCategory(): 5 | 6 | def __init__(self, id_, name, supercategory=""): 7 | 8 | self.id = id_ 9 | self.name = name 10 | self.supercategory = supercategory 11 | 12 | def toJSON(self): 13 | return json.dumps(self, default=vars, sort_keys=True, indent=4) -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/Omni3DDataset.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | class Omni3DDataset(): 4 | 5 | def __init__(self, info, images, categories, annotations): 6 | self.info = info 7 | self.images = images 8 | self.categories = categories 9 | self.annotations = annotations 10 | 11 | 12 | def toJSON(self): 13 | return json.dumps(self, default=vars, sort_keys=True, indent=4) 14 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/Omni3DImage.py: -------------------------------------------------------------------------------- 1 | 2 | import json 3 | 4 | class Omni3DImage(): 5 | 6 | def __init__(self, id_, dataset_id, seq_id, width, height, file_path, K, gt, t, src_90_rotate=0, src_flagged=0): 7 | 8 | self.id = id_ 9 | self.dataset_id = dataset_id 10 | self.seq_id = seq_id 11 | self.width = width 12 | self.height = height 13 | self.file_path = file_path 14 | self.K = K 15 | self.src_90_rotate = src_90_rotate 16 | self.src_flagged = src_flagged 17 | self.gt = gt 18 | self.t = t 19 | 20 | def toJSON(self): 21 | return json.dumps(self, default=vars, indent=4) 22 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/Omni3DInfo.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | class Omni3DInfo(): 4 | def __init__(self, id_, source, name, split, version, url): 5 | self.id = id_ 6 | self.source = source 7 | self.name = name 8 | self.split = split 9 | self.version = version 10 | self.url = url 11 | 12 | 13 | def toJSON(self): 14 | return json.dumps(self, default=vars, sort_keys=True, indent=4) 15 | -------------------------------------------------------------------------------- /ros1_ws/src/omni3d_ros/src/Omni3DObject.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | import json 4 | 5 | 6 | class Omni3DObject(): 7 | 8 | uid = 0 9 | 10 | 11 | def __init__(self, dataset_id, image_id, seq_id, category_id, category_name, \ 12 | valid3D, bbox2D_tight, bbox2D_proj, bbox2D_trunc, bbox3D_cam, center_cam, dimensions, \ 13 | R_cam, behind_camera=-1, visibility=-1, truncation=-1, segmentation_pts=-1, lidar_pts=-1, depth_error=-1): 14 | 15 | self.id = Omni3DObject.uid 16 | Omni3DObject.uid += 1 17 | self.dataset_id = dataset_id 18 | self.image_id = image_id 19 | self.seq_id = seq_id 20 | self.category_id = category_id 21 | self.category_name = category_name 22 | self.valid3D = valid3D 23 | self.bbox2D_tight = bbox2D_tight 24 | self.bbox2D_proj = bbox2D_proj 25 | self.bbox2D_trunc = bbox2D_trunc 26 | self.bbox3D_cam = bbox3D_cam 27 | self.center_cam = center_cam 28 | self.dimensions = dimensions 29 | self.R_cam = R_cam 30 | self.behind_camera = behind_camera 31 | self.visibility = visibility 32 | self.truncation = truncation 33 | self.segmentation_pts = segmentation_pts 34 | self.lidar_pts = lidar_pts 35 | self.depth_error = depth_error 36 | 37 | def toJSON(self): 38 | return json.dumps(self, default=vars, sort_keys=True, indent=4) 39 | -------------------------------------------------------------------------------- /ros2_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | source /opt/ros/foxy/setup.bash 5 | 6 | exec "$@" -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nmcl_msgs) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | find_package(std_msgs REQUIRED) 25 | find_package(rosidl_default_generators REQUIRED) 26 | 27 | rosidl_generate_interfaces(${PROJECT_NAME} 28 | "msg/Omni3D.msg" 29 | "msg/Omni3DArray.msg" 30 | "srv/NoArguments.srv" 31 | DEPENDENCIES builtin_interfaces std_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg 32 | ) 33 | 34 | 35 | 36 | if(BUILD_TESTING) 37 | find_package(ament_lint_auto REQUIRED) 38 | # the following line skips the linter which checks for copyrights 39 | # uncomment the line when a copyright and license is not present in all source files 40 | #set(ament_cmake_copyright_FOUND TRUE) 41 | # the following line skips cpplint (only works in a git repo) 42 | # uncomment the line when this package is not in a git repo 43 | #set(ament_cmake_cpplint_FOUND TRUE) 44 | ament_lint_auto_find_test_dependencies() 45 | endif() 46 | 47 | ament_package() 48 | -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_msgs/msg/Omni3D.msg: -------------------------------------------------------------------------------- 1 | float32[] center 2 | float32[] dim 3 | float32[] rot 4 | int64 category 5 | float32 confidence 6 | -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_msgs/msg/Omni3DArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Omni3D[] detections -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nmcl_msgs 5 | 0.0.0 6 | TODO: Package description 7 | nickybones 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rosidl_default_generators 16 | 17 | rosidl_default_runtime 18 | 19 | rosidl_interface_packages 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_msgs/srv/NoArguments.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | string return_value -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_ros/include/RosUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | # ############################################################################## 3 | # Copyright (c) 2021- University of Bonn # 4 | # All rights reserved. # 5 | # # 6 | # Author: Nicky Zimmerman # 7 | # # 8 | # File: RosUtils.h # 9 | # ############################################################################## 10 | **/ 11 | 12 | 13 | #ifndef ROSUTILS_H 14 | #define ROSUTILS_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | //#include 23 | //#include 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | // Eigen::Vector3f OdomMsg2Pose2D(const nav_msgs::msg::Odometry::ConstSharedPtr odom); 30 | 31 | // Eigen::Vector3f PoseMsg2Pose2D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& poseMsg); 32 | 33 | // std::vector OdomMsg2Pose3D(const nav_msgs::msg::Odometry::ConstSharedPtr odom); 34 | 35 | // std::vector PoseMsg2Pose3D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& odom); 36 | 37 | // geometry_msgs::msg::PoseStamped Pose2D2PoseMsg(Eigen::Vector3f pose2d); 38 | 39 | 40 | // //pcl::PointCloud Vec2PointCloud(const std::vector& points3d, Eigen::Vector3f rgb); 41 | 42 | 43 | // geometry_msgs::msg::PoseWithCovarianceStamped Pred2PoseWithCov(Eigen::Vector3d pred, Eigen::Matrix3d cov); 44 | 45 | 46 | Eigen::Vector3f OdomMsg2Pose2D(const nav_msgs::msg::Odometry::ConstSharedPtr& odom); 47 | 48 | Eigen::Vector3f PoseMsg2Pose2D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& poseMsg); 49 | 50 | std::vector OdomMsg2Pose3D(const nav_msgs::msg::Odometry::ConstSharedPtr& odom); 51 | 52 | std::vector PoseMsg2Pose3D(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& odom); 53 | 54 | geometry_msgs::msg::PoseStamped Pose2D2PoseMsg(Eigen::Vector3f pose2d); 55 | 56 | 57 | pcl::PointCloud Vec2PointCloud(const std::vector& points3d, Eigen::Vector3f rgb); 58 | 59 | pcl::PointCloud Vec2PointCloud(const std::vector& points3d); 60 | 61 | pcl::PointCloud Vec2RGBPointCloud(const std::vector& points3d, std::vector rgb); 62 | 63 | 64 | pcl::PointCloud ManyVec2PointCloud(const std::vector>& points3d, std::vector rgb); 65 | 66 | 67 | geometry_msgs::msg::PoseWithCovarianceStamped Pred2PoseWithCov(Eigen::Vector3d pred, Eigen::Matrix3d cov); 68 | 69 | //sensor_msgs::msg::Image CVMat2ImgMsg(const cv::Mat& img, std_msgs::Header header, const std::string& type); 70 | 71 | #endif -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_ros/launch/confignmcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ros2_ws/src/nmcl_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nmcl_ros 5 | 0.0.0 6 | TODO: Package description 7 | root 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/Base.yaml: -------------------------------------------------------------------------------- 1 | SOLVER: 2 | TYPE: "sgd" 3 | IMS_PER_BATCH: 32 4 | BASE_LR: 0.02 5 | STEPS: (19200, 25600) 6 | MAX_ITER: 32000 7 | WEIGHT_DECAY: 0.0001 8 | LR_SCHEDULER_NAME: "WarmupMultiStepLR" 9 | INPUT: 10 | MIN_SIZE_TRAIN: (256, 272, 288, 304, 320, 336, 352, 368, 384, 400, 416, 432, 448, 464, 480, 496, 512, 528, 544, 560, 576, 592, 608, 624, 640,) 11 | MIN_SIZE_TEST: 512 12 | MAX_SIZE_TRAIN: 4096 13 | MAX_SIZE_TEST: 4096 14 | TEST: 15 | VISIBILITY_THRES: 0.33333333 16 | TRUNCATION_THRES: 0.33333333 17 | EVAL_PERIOD: 16000 18 | DATASETS: 19 | TRAIN: ('KITTI_train', 'KITTI_val') 20 | TEST: ('KITTI_test',) 21 | CATEGORY_NAMES: ('pedestrian', 'car', 'cyclist', 'van', 'truck', 'tram', 'person') 22 | IGNORE_NAMES: "['dontcare', 'ignore', 'void']" 23 | MIN_HEIGHT_THRES: 0.05 24 | TRUNCATION_THRES: 0.75 25 | VISIBILITY_THRES: 0.25 26 | TRUNC_2D_BOXES: True 27 | VIS_PERIOD: 640 28 | DATALOADER: 29 | SAMPLER_TRAIN: "RepeatFactorTrainingSampler" 30 | REPEAT_THRESHOLD: 0.1 31 | MODEL: 32 | PIXEL_MEAN: [103.530, 116.280, 123.675] 33 | PIXEL_STD: [57.375, 57.120, 58.395] 34 | META_ARCHITECTURE: "RCNN3D" 35 | MASK_ON: False 36 | STABILIZE: 0.02 37 | USE_BN: True 38 | BACKBONE: 39 | FREEZE_AT: 0 40 | NAME: 'build_dla_from_vision_fpn_backbone' 41 | DLA: 42 | TYPE: 'dla34' 43 | FPN: 44 | IN_FEATURES: ['p2', 'p3', 'p4', 'p5', 'p6'] 45 | ANCHOR_GENERATOR: 46 | SIZES: [[32], [64], [128], [256], [512]] # One size for each in feature map 47 | ASPECT_RATIOS: [[0.5, 1.0, 2.0]] # Three aspect ratios (same for all in feature maps) 48 | RPN: 49 | HEAD_NAME: "StandardRPNHead" 50 | IN_FEATURES: ['p2', 'p3', 'p4', 'p5', 'p6'] 51 | PRE_NMS_TOPK_TRAIN: 2000 # Per FPN level 52 | PRE_NMS_TOPK_TEST: 1000 # Per FPN level 53 | POST_NMS_TOPK_TRAIN: 1000 54 | POST_NMS_TOPK_TEST: 1000 55 | BOUNDARY_THRESH: -1 56 | OBJECTNESS_UNCERTAINTY: "IoUness" 57 | IOU_THRESHOLDS: [0.05, 0.05] 58 | POSITIVE_FRACTION: 1.0 59 | PROPOSAL_GENERATOR: 60 | NAME: "RPNWithIgnore" 61 | ROI_HEADS: 62 | NAME: "ROIHeads3D" 63 | IN_FEATURES: ["p2", "p3", "p4", "p5", 'p6'] 64 | BATCH_SIZE_PER_IMAGE: 512 65 | SCORE_THRESH_TEST: 0.01 66 | NUM_CLASSES: 43 67 | ROI_BOX_HEAD: 68 | NAME: "FastRCNNConvFCHead" 69 | NUM_FC: 2 70 | POOLER_RESOLUTION: 7 71 | ROI_CUBE_HEAD: 72 | NAME: 'CubeHead' 73 | Z_TYPE: 'direct' 74 | POSE_TYPE: '6d' 75 | NUM_FC: 2 76 | SHARED_FC: True 77 | USE_CONFIDENCE: 1.0 78 | LOSS_W_3D: 1.0 79 | POOLER_TYPE: 'ROIAlignV2' 80 | POOLER_RESOLUTION: 7 81 | DIMS_PRIORS_ENABLED: True 82 | DISENTANGLED_LOSS: True 83 | ALLOCENTRIC_POSE: True 84 | VIRTUAL_FOCAL: 512.0 85 | VIRTUAL_DEPTH: True 86 | CHAMFER_POSE: True 87 | VERSION: 2 -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/Map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ros2_ws/src/omni3d_ros/configs/Map.png -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/Map.yaml: -------------------------------------------------------------------------------- 1 | image: Map.png 2 | resolution: 0.05 3 | origin: 4 | - -13.9155 5 | - -11.04537 6 | - 0.0 7 | negate: 0 8 | occupied_thresh: 0.65 9 | free_thresh: 0.196 10 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/args.json: -------------------------------------------------------------------------------- 1 | { 2 | "gmap" :"//SIMP/ros2_ws/src/omni3d_ros/configs/Map.yaml", 3 | "roomseg" : "/SIMP/ros2_ws/src/omni3d_ros/configs/roomseg.png", 4 | "categories" : ["sink", "door", "oven", "board", "table", "box", "potted plant", "drawers", "sofa", "cabinet", "chair", "fire extinguisher", "person", "desk"], 5 | "yolo_conf" : [0.7, 0.5, 0.7, 0.7, 0.6, 0.7, 0.7, 0.7, 0.8, 0.7, 0.7, 0.7, 0.6, 0.7], 6 | "omni_conf" : [0.7, 0.0, 0.8, 0.9, 0.7, 0.0, 0.95, 0.8, 0.7, 0.9, 0.0, 0.9, 0.0, 0.8], 7 | "min_dist" : [0.5, 0.0, 0.0, 1.5, 1.0, 0.0, 0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0, 0.5], 8 | "max_dist" : [3.0, 0.0, 4.0, 4.5, 4.5, 0.0, 4.5, 3.5, 3.0, 5.0, 0.0, 3.0, 0.0, 4.0], 9 | "width" : 640, 10 | "height" : 480, 11 | "2DvisTest" : true, 12 | "3DvisTest" : true, 13 | "YOLOvisTest" : true, 14 | "odomTrigger" : true, 15 | "odom_trigerTH" : [0.2, 0.1], 16 | "totalSeenTH" : 1, 17 | "costTH" : 0.3, 18 | "truncTH" : 0.5, 19 | "skipTH" : 20, 20 | "variancePath" : "/SIMP/ros2_ws/src/omni3d_ros/configs/variance.csv", 21 | "K" : [380.07025146484375, 0.0, 324.4729919433594, 0.0, 379.66119384765625, 237.78517150878906, 0.0, 0.0, 1.0], 22 | "cam_poses" : [0.1, 0, 0, 0, 0, -0.1, 0, -1.5707963267948966, -0.1, 0, 0, 3.141592653589793, 0, 0.1, 0, 1.5707963267948966], 23 | "angles" : [ 0, -1.5707963267948966,3.141592653589793, 1.5707963267948966], 24 | "min_z" : -1.55708286, 25 | "camera_z" : 0.63 26 | } -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/category_meta.json: -------------------------------------------------------------------------------- 1 | {"thing_classes": ["sink", "door", "oven", "board", "table", "box", "potted plant", "drawers", "sofa", "cabinet", "chair", "fire extinguisher", "person", "desk"], "thing_dataset_id_to_contiguous_id": {"0": 0, "1": 1, "2": 2, "3": 3, "4": 4, "5": 5, "6": 6, "7": 7, "8": 8, "9": 9, "10": 10, "11": 11, "12": 12, "13": 13}} -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/omnidataset.yaml: -------------------------------------------------------------------------------- 1 | train: data/images/training/ 2 | val: data/images/validation/ 3 | 4 | # number of classes 5 | nc: 14 6 | 7 | # class names 8 | names: ['sink', 'door', 'oven', 'board', 'table', 'box', 'potted plant', 'drawers', 'sofa', 'cabinet', 'chair', 'fire extinguisher', 'person', 'desk'] -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/roomseg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ros2_ws/src/omni3d_ros/configs/roomseg.png -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/trained_Omni3D.yaml: -------------------------------------------------------------------------------- 1 | _BASE_: "Base.yaml" 2 | SOLVER: 3 | TYPE: "sgd" 4 | IMS_PER_BATCH: 12 5 | BASE_LR: 0.0015 6 | STEPS: (3340800, 4454400) 7 | MAX_ITER: 5568000 8 | WARMUP_ITERS: 174000 9 | TEST: 10 | EVAL_PERIOD: 2500 11 | VIS_PERIOD: 2320 12 | DATASETS: 13 | TRAIN: ('Hypersim_train', 'Hypersim_val') 14 | TEST: ('Hypersim_test',) 15 | CATEGORY_NAMES: ('sink', 'door', 'oven', 'board', 'table', 'box', 'potted plant', 'drawers', 'sofa', 'cabinet', 'chair', 'fire extinguisher', 'person', 'desk') # ('chair', 'table', 'cabinet', 'car', 'lamp', 'books', 'sofa', 'pedestrian', 'picture', 'window', 'pillow', 'truck', 'door', 'blinds', 'sink', 'shelves', 'television', 'shoes', 'cup', 'bottle', 'bookcase', 'laptop', 'desk', 'cereal box', 'floor mat', 'traffic cone', 'mirror', 'barrier', 'counter', 'camera', 'bicycle', 'toilet', 'bus', 'bed', 'refrigerator', 'trailer', 'box', 'oven', 'clothes', 'van', 'towel', 'motorcycle', 'night stand', 'stove', 'fire extinguisher', 'chair', 'stationery', 'bathtub', 'cyclist', 'curtain', 'bin', 'poop') 16 | MODEL: 17 | ROI_HEADS: 18 | NUM_CLASSES: 14 19 | WEIGHTS: 'cubercnn_DLA34_FPN.pth' 20 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/configs/variance.csv: -------------------------------------------------------------------------------- 1 | category,mx,my,sx,sy,b 2 | 0,-1.2059629422293427,-0.21596509127955354,3.5598299922803824,4.752782645150083,2.0 3 | 1,,,,, 4 | 2,-0.2719821124479019,-0.09356356143728917,5.2496109773482,5.927369127588248,4.0 5 | 3,-0.917821683643759,1.0360433732917682,2.952211852916148,12.65687793596594,2.0 6 | 4,-1.4398911670302559,-0.9773037293126856,7.69218980951617,16.189173686580585,4.0 7 | 5,,,,, 8 | 6,-3.2148634474083,-1.5600138203264704,2.945795021310433,3.269590787245149,2.0 9 | 7,-0.3996214251000274,-0.5297622941886962,5.494713481959431,3.8353573775340055,2.0 10 | 8,-0.7996663557464608,2.304188897078266,7.152909344647871,22.032779109063913,2.0 11 | 9,-0.9854608379604701,0.7808889332308545,4.694373920974595,11.558330395794945,2.0 12 | 10,,,,, 13 | 11,0.1639741907493567,-0.37827609098200454,1.895822092187304,2.198324715132525,2.0 14 | 12,,,,, 15 | 13,5.287808297665473,6.392892568902049,5.417909228561944,20.390211704373392,2.0 16 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/launch/gt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/launch/omni3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/BroadcastMapNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import logging 4 | import os 5 | import argparse 6 | import sys 7 | import numpy as np 8 | from collections import OrderedDict 9 | import torch 10 | import cv2 11 | import rospy 12 | from sensor_msgs.msg import Image 13 | from cv_bridge import CvBridge 14 | from message_filters import ApproximateTimeSynchronizer, Subscriber 15 | from std_msgs.msg import UInt16, Float32MultiArray 16 | from nmcl_msgs.msg import Omni3D, Omni3DArray 17 | from visualization_msgs.msg import Marker 18 | from visualization_msgs.msg import MarkerArray 19 | from geometry_msgs.msg import Point 20 | import time 21 | import open3d as o3d 22 | import copy 23 | from DatasetUtils import get_cuboid_verts_faces, convert_3d_box_to_2d, getTrunc2Dbbox 24 | from matplotlib import cm 25 | from scipy.spatial import ConvexHull 26 | from MapObjectTracker import MapObjectTracker 27 | from scipy.spatial.transform import Rotation as R 28 | from ObjectUtils import * 29 | 30 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 31 | 32 | all_classes = ['sink', 'door', 'oven', 'board', 'table', 'box', 'potted plant', 'drawers', 'sofa', 'cabinet', 'chair', 'fire extinguisher', 'person', 'desk'] 33 | camere_z = 0.63 34 | min_z = -1.55708286 35 | 36 | 37 | class BroadcastMapDNode(): 38 | 39 | 40 | def __init__(self): 41 | 42 | self.marker_pub = rospy.Publisher("omni3dMarkerTopic", MarkerArray, queue_size=10) 43 | 44 | self.clr = cm.rainbow(np.linspace(0, 1, 14)) 45 | 46 | rospy.loginfo("BroadcastMapDNode::Ready!") 47 | 48 | mapObjects = LoadTrackedObjectsFromPickle("/home/nickybones/Code/OmniNMCL/ros1_ws/src/omni3d_ros/", "mapObjects_ICP2.pickle") 49 | markers = self.createMarkers(mapObjects) 50 | 51 | while not rospy.is_shutdown(): 52 | 53 | markerArray = MarkerArray() 54 | markerArray.markers = markers 55 | # Renumber the marker IDs 56 | id = 0 57 | for m in markerArray.markers: 58 | m.id = id 59 | id += 1 60 | 61 | # Publish the MarkerArray 62 | self.marker_pub .publish(markerArray) 63 | 64 | rospy.sleep(0.01) 65 | 66 | 67 | 68 | def createMarkers(self, mapObjs): 69 | 70 | markers = [] 71 | 72 | for obj in mapObjs: 73 | marker = Marker() 74 | marker.header.frame_id = "map" 75 | marker.action = marker.ADD 76 | marker.type = marker.CUBE; 77 | marker.pose.position.x = obj.center[0] 78 | marker.pose.position.y = obj.center[1] 79 | marker.pose.position.z = obj.center[2] - min_z 80 | 81 | r = R.from_matrix(obj.rot) 82 | q = r.as_quat() 83 | color = self.clr[obj.category] 84 | 85 | marker.pose.orientation.x = q[0] 86 | marker.pose.orientation.y = q[1] 87 | marker.pose.orientation.z = q[2] 88 | marker.pose.orientation.w = q[3] 89 | marker.scale.x = obj.dim[2] 90 | marker.scale.y = obj.dim[1] 91 | marker.scale.z = obj.dim[0] 92 | marker.color.a = 0.8 93 | marker.color.r = color[2] 94 | marker.color.g = color[1] 95 | marker.color.b = color[0] 96 | marker.lifetime = rospy.Duration(3) 97 | 98 | markers.append(marker) 99 | 100 | return markers 101 | 102 | 103 | 104 | 105 | def main(): 106 | 107 | 108 | rospy.init_node('BroadcastMapDNode', anonymous=True) 109 | omn = BroadcastMapDNode() 110 | rospy.spin() 111 | 112 | if __name__ == "__main__": 113 | 114 | main() 115 | 116 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/GMAP.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import cv2 4 | import yaml 5 | import os 6 | 7 | class GMAP(): 8 | 9 | 10 | 11 | def __init__(self, *args): 12 | 13 | 14 | if len(args) == 1: 15 | yamlPath = args[0] 16 | folderPath, _ = os.path.split(yamlPath) 17 | print(folderPath) 18 | with open(yamlPath, 'r') as stream: 19 | data = yaml.safe_load(stream) 20 | 21 | self.map = cv2.imread(folderPath + "/" + data['image']) 22 | self.resolution = data['resolution'] 23 | self.origin = data['origin'] 24 | self.max_y = self.map.shape[0] 25 | self.ComputeBorders() 26 | 27 | elif len(args) == 2: 28 | yamlConf = args[0] 29 | gridmap = args[1] 30 | self.map = gridmap 31 | self.resolution = yamlConf['resolution'] 32 | self.origin = yamlConf['origin'] 33 | self.max_y = gridmap.shape[0] 34 | self.ComputeBorders() 35 | 36 | elif len(args) == 3: 37 | gridmap = args[0] 38 | resolution = args[1] 39 | origin = args[2] 40 | self.map = gridmap 41 | self.resolution = resolution 42 | self.origin = origin 43 | self.max_y = gridmap.shape[0] 44 | self.ComputeBorders() 45 | 46 | 47 | # the (0,0) of the gmapping map is where the robot started from. To relate this to the gridmap image 48 | # We need to know the real world coordinates of some image point. 49 | # the map origin in the bottom left corner of the image, and its real world coordinates are 50 | # specified in the metadata yaml. 51 | 52 | def world2map(self, p): 53 | u = np.round((p[0] - self.origin[0]) / self.resolution) 54 | v = self.max_y - np.round((p[1] - self.origin[1]) / self.resolution) 55 | return np.array([u, v]).astype(int) 56 | 57 | def map2world(self, uv): 58 | x = uv[0] * self.resolution + self.origin[0] 59 | y = (self.max_y - uv[1]) * self.resolution + self.origin[1] 60 | 61 | return np.array([x, y]) 62 | 63 | def ComputeBorders(self): 64 | self.map = 255 - cv2.cvtColor(self.map, cv2.COLOR_BGR2GRAY) 65 | white_pixels = np.array(np.where(self.map == 255)) 66 | min_x = min(white_pixels[1]) 67 | max_x = max(white_pixels[1]) 68 | min_y = min(white_pixels[0]) 69 | max_y = max(white_pixels[0]) 70 | self.map_border = {"min_x": min_x, "min_y": min_y, "max_x": max_x, "max_y": max_y} 71 | 72 | def TopLeft(self): 73 | return np.array([self.map_border["min_x"], self.map_border["min_y"]]) 74 | 75 | def BottomRight(self): 76 | return np.array([self.map_border["max_x"], self.map_border["max_y"]]) 77 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/GTNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import cv2 4 | import json 5 | import rclpy 6 | from rclpy.node import Node 7 | from std_msgs.msg import String 8 | import pandas as pd 9 | from sensor_msgs.msg import Image 10 | from geometry_msgs.msg import PoseStamped 11 | import numpy as np 12 | from scipy.spatial.transform import Rotation as R 13 | import tf_transformations 14 | from rclpy.time import Time 15 | 16 | 17 | class GTNode(Node): 18 | 19 | def __init__(self): 20 | super().__init__('GTNode') 21 | 22 | self.declare_parameter('csvPath') 23 | csvPath = self.get_parameter('csvPath').value 24 | self.get_logger().info("csvPath: %s" % (str(csvPath),)) 25 | self.declare_parameter('gtTopic') 26 | gtTopic = self.get_parameter('gtTopic').value 27 | self.get_logger().info("gtTopic: %s" % (str(gtTopic),)) 28 | self.declare_parameter('jsonPath') 29 | jsonPath = self.get_parameter('jsonPath').value 30 | self.get_logger().info("jsonPath: %s" % (str(jsonPath),)) 31 | self.declare_parameter('cameraTopic') 32 | cameraTopic = self.get_parameter('cameraTopic').value 33 | self.get_logger().info("cameraTopic: %s" % (str(cameraTopic),)) 34 | 35 | self.df = pd.read_csv(csvPath) 36 | with open(jsonPath) as f: 37 | self.args = json.load(f) 38 | self.camera_z = self.args['camera_z'] 39 | 40 | self.sub = self.create_subscription(Image, cameraTopic, self.callback, 1) 41 | self.pose_pub = self.create_publisher(PoseStamped, gtTopic, 1) 42 | 43 | self.get_logger().info("GTNode::Ready!") 44 | 45 | 46 | def callback(self, cam0_msg): 47 | 48 | t_img = Time.from_msg(cam0_msg.header.stamp).nanoseconds 49 | row = self.df.iloc[(self.df['t']-t_img).abs().argsort()[:1]] 50 | x = row['gt_x'].to_numpy()[0] 51 | y = row['gt_y'].to_numpy()[0] 52 | z = self.camera_z 53 | yaw = row['gt_yaw'].to_numpy()[0] 54 | 55 | quaternion = tf_transformations.quaternion_from_euler(0, 0, yaw) 56 | pose_msg = PoseStamped() 57 | pose_msg.pose.orientation.x = quaternion[0] 58 | pose_msg.pose.orientation.y = quaternion[1] 59 | pose_msg.pose.orientation.z = quaternion[2] 60 | pose_msg.pose.orientation.w = quaternion[3] 61 | pose_msg.pose.position.x = x 62 | pose_msg.pose.position.y = y 63 | pose_msg.pose.position.z = z 64 | pose_msg.header.stamp = cam0_msg.header.stamp 65 | pose_msg.header.frame_id = "map" 66 | 67 | self.pose_pub.publish(pose_msg) 68 | 69 | 70 | 71 | 72 | def main(args=None): 73 | 74 | 75 | # rospy.init_node('GTNode', anonymous=True) 76 | #rate = rospy.Rate(10) 77 | #gt = GTNode() 78 | #rospy.spin() 79 | 80 | rclpy.init(args=args) 81 | 82 | gt_node = GTNode() 83 | 84 | rclpy.spin(gt_node) 85 | 86 | # Destroy the node explicitly 87 | # (optional - otherwise it will be done automatically 88 | # when the garbage collector destroys the node object) 89 | gt_node.destroy_node() 90 | rclpy.shutdown() 91 | 92 | 93 | if __name__ == "__main__": 94 | 95 | main() 96 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/InterTest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import cv2 4 | import rospy 5 | import pandas as pd 6 | from message_filters import ApproximateTimeSynchronizer, Subscriber 7 | from sensor_msgs.msg import Image 8 | from geometry_msgs.msg import PoseStamped, Transform 9 | import tf 10 | from tf import transformations as ts 11 | import numpy as np 12 | import math 13 | from scipy.interpolate import interp1d 14 | from bisect import insort, bisect_left 15 | from collections import deque 16 | from itertools import islice 17 | from scipy.spatial.transform import Rotation as R 18 | 19 | camera_z = 0.63 20 | min_z = -1.55708286 21 | 22 | def running_median_insort(seq, window_size): 23 | """Contributed by Peter Otten""" 24 | seq = iter(seq) 25 | d = deque() 26 | s = [] 27 | result = [] 28 | for item in islice(seq, window_size): 29 | d.append(item) 30 | insort(s, item) 31 | result.append(s[len(d)//2]) 32 | m = window_size // 2 33 | for item in seq: 34 | old = d.popleft() 35 | d.append(item) 36 | del s[bisect_left(s, old)] 37 | insort(s, item) 38 | result.append(s[m]) 39 | return result 40 | 41 | 42 | def interpolate(t, poses, kind='nearest', window=2, tol=0.5): 43 | """Contributed by Jerome Guzzi and adapted to our data""" 44 | 45 | n = len(poses) 46 | data = np.zeros((n, 4)) 47 | 48 | for i in range(n): 49 | data[i, 0] = t[i] 50 | data[i, 1:] = poses[i] 51 | 52 | 53 | # Filter outliers in yaw 54 | data = data[np.abs(data[:, 3] - running_median_insort(data[:, 3], window)) < tol] 55 | data[0, -1] = np.fmod(data[0, -1], 2 * np.pi) 56 | 57 | return interp1d(data[:, 0], data[:, 1:], axis=0, fill_value='extrapolate', assume_sorted=True, kind=kind) 58 | 59 | 60 | 61 | 62 | def main(): 63 | 64 | 65 | df = pd.read_csv("/home/nickybones/data/MCL/omni3d/Map3/raw_mcl.csv") 66 | gt_t = [] 67 | gt_poses = [] 68 | n = len(df['gt_x']) 69 | t = df['t'] 70 | x = df['gt_x'].to_numpy() 71 | y = df['gt_y'].to_numpy() 72 | yaw = df['gt_yaw'].to_numpy() 73 | 74 | 75 | for i in range(n): 76 | 77 | gt_poses.append(np.array([x[i], y[i], yaw[i]])) 78 | gt_t.append(t[i]) 79 | #print(gt_poses[i], gt_t[i]) 80 | 81 | 82 | t1 = 1673979858458452341 83 | t2 = 1673979858505056416 84 | t3 = int((t1 + t2) / 2) 85 | print("[ 7.02680039 -2.95074724 3.90965687] 1673979858458452341") 86 | print("[ 7.02663729 -2.94933241 3.84571609] 1673979858505056416") 87 | intr = interpolate(gt_t, gt_poses) 88 | 89 | 90 | pose = intr(t3) 91 | print(pose, t3) 92 | 93 | if __name__ == "__main__": 94 | 95 | main() -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/JSONUtils.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import json 4 | 5 | def FindGTByImageID(images, image_id): 6 | 7 | for im in images: 8 | if im['id'] == image_id: 9 | return im['gt'] 10 | 11 | 12 | return None 13 | 14 | 15 | def FindLastImageFrameNum(images): 16 | 17 | im = images[-1] 18 | imOldPath = im['file_path'] 19 | camPath = os.path.split(imOldPath)[1].replace('.png', '') 20 | 21 | return int(camPath) 22 | 23 | 24 | 25 | return None 26 | 27 | def FindImageByImageID(images, image_id): 28 | 29 | for im in images: 30 | if im['id'] == image_id: 31 | return im 32 | 33 | 34 | return None 35 | 36 | def FindAllImagseBySeqID(images, seq_id): 37 | 38 | seq_images = [] 39 | for im in images: 40 | if im['seq_id'] == seq_id: 41 | seq_images.append(im) 42 | 43 | 44 | return seq_images 45 | 46 | def FindAllImagseByFrameID(images, frame_id): 47 | 48 | frame_images = [] 49 | for im in images: 50 | if "/{:05d}.png".format(frame_id) in im['file_path']: 51 | frame_images.append(im) 52 | 53 | 54 | return frame_images 55 | 56 | def GetAllSequences(images): 57 | 58 | seq_ids = set() 59 | 60 | for im in images: 61 | seq_ids.add(im['seq_id']) 62 | 63 | return list(seq_ids) 64 | 65 | 66 | def FindAllObjectsByImageID(objects, image_id): 67 | 68 | matches = [] 69 | 70 | for o in objects: 71 | if o['image_id'] == image_id: 72 | matches.append(o) 73 | 74 | 75 | return matches 76 | 77 | def LoadObjectsFromJSON(jsonPath): 78 | 79 | with open(jsonPath) as f: 80 | data = json.load(f) 81 | 82 | 83 | images = data['images'] 84 | objects = data['annotations'] 85 | 86 | return images, objects 87 | 88 | 89 | 90 | def getCamNmeFromImage(im): 91 | 92 | seq = im['seq_id'] 93 | imOldPath = im['file_path'] 94 | c = getCamNme(imOldPath) 95 | 96 | return c 97 | 98 | 99 | def getCamNme(imOldPath): 100 | 101 | camPath = os.path.split(imOldPath)[0] 102 | camPath = os.path.split(camPath)[1] 103 | 104 | if '00' in camPath: 105 | return 0 106 | 107 | elif '01' in camPath: 108 | return 1 109 | 110 | elif '02' in camPath: 111 | return 2 112 | 113 | elif '03' in camPath: 114 | return 3 115 | 116 | else: 117 | return -1 118 | 119 | def retrieveOrigImagePath(im): 120 | 121 | seq = im['seq_id'] 122 | imOldPath = im['file_path'] 123 | imName = os.path.split(imOldPath)[1] 124 | c = getCamNme(imOldPath) 125 | imPath = "R{}/camera{}/color/{}".format(seq,c,imName) 126 | 127 | return imPath 128 | 129 | 130 | def getCategories(predJsonPath): 131 | 132 | with open(predJsonPath) as f: 133 | data = json.load(f) 134 | 135 | cats = data['categories'] 136 | 137 | classes = [] 138 | 139 | for cat in cats: 140 | classes.append(cat['name']) 141 | 142 | return classes -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/Lab3DObject.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | 5 | 6 | class Lab3DObject(): 7 | 8 | gid = 0 9 | 10 | def __init__(self, oid, category, pos, dim, yaw): 11 | self.oid = oid 12 | self.category = category 13 | self.pos = np.array([pos['x'], pos['y'], pos['z']], dtype=np.float64) 14 | self.dim = np.array([dim['x'], dim['y'], dim['z']], dtype=np.float64) 15 | self.yaw = yaw 16 | self.gid = Lab3DObject.gid 17 | 18 | Lab3DObject.gid += 1 19 | 20 | 21 | def __init__(self, jObj): 22 | 23 | self.oid = jObj['id'] 24 | self.category = jObj['category_id'] 25 | pos = jObj['position'] 26 | dim = jObj['dimensions'] 27 | self.pos = np.array([pos['x'], pos['y'], pos['z']], dtype=np.float64) 28 | self.dim = np.array([dim['x'], dim['y'], dim['z']], dtype=np.float64) 29 | self.yaw = jObj['yaw'] 30 | self.gid = Lab3DObject.gid 31 | 32 | Lab3DObject.gid += 1 -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/MapObjectTracker.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | import pandas as pd 4 | from scipy.spatial.transform import Rotation as R 5 | import open3d as o3d 6 | 7 | class MapObjectTracker(): 8 | 9 | uid = 0 10 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 11 | 12 | 13 | def __init__(self, category, center, dim, rot, conf, room=0, static=False, seen=1, skip=0, times_matched=1, times_skipped=0): 14 | self.category = category 15 | self.center = center 16 | self.dim = np.asarray(dim) 17 | self.rot = rot 18 | #self.rot = MapObjectTracker.origin.get_rotation_matrix_from_xyz((0, 0, yaw_mu)) 19 | self.seen = seen 20 | self.times_matched = times_matched 21 | self.times_skipped = times_skipped 22 | self.skip = skip 23 | self.uid = MapObjectTracker.uid 24 | self.conf = conf 25 | self.static = static 26 | self.active = 1 27 | self.inactive = 0 28 | self.room = room 29 | self.volume = self.dim[0] * self.dim[1] * self.dim[2] 30 | 31 | 32 | MapObjectTracker.uid += 1 33 | 34 | 35 | 36 | 37 | 38 | def predict(self, isActive): 39 | 40 | if isActive: 41 | self.active += 1 42 | self.skip += 1 43 | self.times_skipped += 1 44 | self.inactive = 0 45 | else: 46 | self.active = 0 47 | self.inactive += 1 48 | 49 | 50 | 51 | def mergeObject(self, mo): 52 | 53 | self.center = (self.center * self.times_matched + mo.center * mo.times_matched) / ( self.times_matched + mo.times_matched) 54 | 55 | test_rot = np.linalg.inv(self.rot) @ mo.rot 56 | r = R.from_matrix(test_rot) 57 | x, y, z = r.as_euler('xyz', degrees=False) 58 | nrm1 = np.linalg.norm(np.array([0, 0, z])) 59 | 60 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 61 | 62 | rot_180 = origin.get_rotation_matrix_from_xyz((0, 0,np.pi)) 63 | test_rot = np.linalg.inv(self.rot) @ rot_180 @ mo.rot 64 | r = R.from_matrix(test_rot) 65 | x, y, z = r.as_euler('xyz', degrees=False) 66 | nrm2 = np.linalg.norm(np.array([0, 0, z])) 67 | 68 | best_rot = mo.rot 69 | if nrm2 < nrm1: 70 | best_rot = rot_180 @ mo.rot 71 | 72 | rot_avg = (self.rot * self.times_matched + best_rot * mo.times_matched) / ( self.times_matched + mo.times_matched) 73 | u, s, vh = np.linalg.svd(rot_avg, full_matrices=True) 74 | self.rot = u @ vh 75 | 76 | #self.dim = np.maximum(self.dim, np.asarray(mo.dim)) 77 | self.dim = (self.dim * self.times_matched + mo.dim * mo.times_matched) / ( self.times_matched + mo.times_matched) 78 | self.volume = self.dim[0] * self.dim[1] * self.dim[2] 79 | 80 | self.times_matched += mo.times_matched 81 | self.times_skipped = 0 82 | self.skip = 0 83 | self.inactive = 0 84 | 85 | 86 | 87 | def avgRotation(self, mo): 88 | 89 | origin = o3d.geometry.TriangleMesh.create_coordinate_frame() 90 | 91 | test_rot = np.linalg.inv(self.rot) @ mo.rot 92 | r = R.from_matrix(test_rot) 93 | x, y, z = r.as_euler('xyz', degrees=False) 94 | nrmz1 = np.linalg.norm(np.array([0, 0, z])) 95 | 96 | rot_180 = origin.get_rotation_matrix_from_xyz((0, 0,np.pi)) 97 | test_rot = np.linalg.inv(self.rot) @ rot_180 @ mo.rot 98 | r = R.from_matrix(test_rot) 99 | x, y, z = r.as_euler('xyz', degrees=False) 100 | nrmz2 = np.linalg.norm(np.array([0, 0, z])) 101 | 102 | 103 | # nrmy1 = np.linalg.norm(np.array([0, y, 0])) 104 | 105 | # rot_180 = origin.get_rotation_matrix_from_xyz((0, np.pi,0)) 106 | # test_rot = np.linalg.inv(self.rot) @ rot_180 @ mo.rot 107 | # r = R.from_matrix(test_rot) 108 | # x, y, z = r.as_euler('xyz', degrees=False) 109 | # nrmy2 = np.linalg.norm(np.array([0, y, 0])) 110 | 111 | best_rot = mo.rot 112 | if nrmz2 < nrmz1: 113 | best_rot = rot_180 @ mo.rot 114 | 115 | rot_avg = (self.rot * self.times_matched + best_rot * mo.times_matched) / ( self.times_matched + mo.times_matched) 116 | u, s, vh = np.linalg.svd(rot_avg, full_matrices=True) 117 | self.rot = u @ vh 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/Omni3DCategory.py: -------------------------------------------------------------------------------- 1 | 2 | import json 3 | 4 | class Omni3DCategory(): 5 | 6 | def __init__(self, id_, name, supercategory=""): 7 | 8 | self.id = id_ 9 | self.name = name 10 | self.supercategory = supercategory 11 | 12 | def toJSON(self): 13 | return json.dumps(self, default=vars, sort_keys=True, indent=4) -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/Omni3DDataset.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | class Omni3DDataset(): 4 | 5 | def __init__(self, info, images, categories, annotations): 6 | self.info = info 7 | self.images = images 8 | self.categories = categories 9 | self.annotations = annotations 10 | 11 | 12 | def toJSON(self): 13 | return json.dumps(self, default=vars, sort_keys=True, indent=4) 14 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/Omni3DImage.py: -------------------------------------------------------------------------------- 1 | 2 | import json 3 | 4 | class Omni3DImage(): 5 | 6 | def __init__(self, id_, dataset_id, seq_id, width, height, file_path, K, gt, t, src_90_rotate=0, src_flagged=0): 7 | 8 | self.id = id_ 9 | self.dataset_id = dataset_id 10 | self.seq_id = seq_id 11 | self.width = width 12 | self.height = height 13 | self.file_path = file_path 14 | self.K = K 15 | self.src_90_rotate = src_90_rotate 16 | self.src_flagged = src_flagged 17 | self.gt = gt 18 | self.t = t 19 | 20 | def toJSON(self): 21 | return json.dumps(self, default=vars, indent=4) 22 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/Omni3DInfo.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | class Omni3DInfo(): 4 | def __init__(self, id_, source, name, split, version, url): 5 | self.id = id_ 6 | self.source = source 7 | self.name = name 8 | self.split = split 9 | self.version = version 10 | self.url = url 11 | 12 | 13 | def toJSON(self): 14 | return json.dumps(self, default=vars, sort_keys=True, indent=4) 15 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/Omni3DObject.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | import json 4 | 5 | 6 | class Omni3DObject(): 7 | 8 | uid = 0 9 | 10 | 11 | def __init__(self, dataset_id, image_id, seq_id, category_id, category_name, \ 12 | valid3D, bbox2D_tight, bbox2D_proj, bbox2D_trunc, bbox3D_cam, center_cam, dimensions, \ 13 | R_cam, behind_camera=-1, visibility=-1, truncation=-1, segmentation_pts=-1, lidar_pts=-1, depth_error=-1): 14 | 15 | self.id = Omni3DObject.uid 16 | Omni3DObject.uid += 1 17 | self.dataset_id = dataset_id 18 | self.image_id = image_id 19 | self.seq_id = seq_id 20 | self.category_id = category_id 21 | self.category_name = category_name 22 | self.valid3D = valid3D 23 | self.bbox2D_tight = bbox2D_tight 24 | self.bbox2D_proj = bbox2D_proj 25 | self.bbox2D_trunc = bbox2D_trunc 26 | self.bbox3D_cam = bbox3D_cam 27 | self.center_cam = center_cam 28 | self.dimensions = dimensions 29 | self.R_cam = R_cam 30 | self.behind_camera = behind_camera 31 | self.visibility = visibility 32 | self.truncation = truncation 33 | self.segmentation_pts = segmentation_pts 34 | self.lidar_pts = lidar_pts 35 | self.depth_error = depth_error 36 | 37 | def toJSON(self): 38 | return json.dumps(self, default=vars, sort_keys=True, indent=4) 39 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/omni3d_ros/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ros2_ws/src/omni3d_ros/omni3d_ros/__init__.py -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | omni3d_ros 5 | 0.0.0 6 | TODO: Package description 7 | nickybones 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | rclpy 16 | 17 | 18 | ament_python 19 | 20 | 21 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/resource/omni3d_ros: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/SIMP/1fad62398d9288f65e667d3f597ffc3fda020ec2/ros2_ws/src/omni3d_ros/resource/omni3d_ros -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/omni3d_ros 3 | [install] 4 | install-scripts=$base/lib/omni3d_ros 5 | -------------------------------------------------------------------------------- /ros2_ws/src/omni3d_ros/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from glob import glob 3 | from setuptools import setup 4 | 5 | package_name = 'omni3d_ros' 6 | 7 | data_files=[ 8 | ('share/ament_index/resource_index/packages', 9 | ['resource/' + package_name]), 10 | ('share/' + package_name, ['package.xml']), 11 | (os.path.join('lib', package_name), glob('omni3d_ros/*')), 12 | ] 13 | 14 | def package_files(data_files, directory_list, dest_list): 15 | 16 | paths_dict = {} 17 | 18 | for i, directory in enumerate(directory_list): 19 | 20 | for (path, directories, filenames) in os.walk(directory): 21 | 22 | for filename in filenames: 23 | 24 | file_path = os.path.join(path, filename) 25 | install_path = os.path.join(dest_list[i], package_name, path) 26 | 27 | if install_path in paths_dict.keys(): 28 | paths_dict[install_path].append(file_path) 29 | 30 | else: 31 | paths_dict[install_path] = [file_path] 32 | 33 | for key in paths_dict.keys(): 34 | data_files.append((key, paths_dict[key])) 35 | 36 | return data_files 37 | 38 | 39 | setup( 40 | name=package_name, 41 | version='0.0.0', 42 | packages=[package_name], 43 | data_files=package_files(data_files, ['models/', 'launch/', 'configs/', 'rviz/', 'omni3d/' ], ['share', 'share', 'share', 'share', 'lib' ]), 44 | install_requires=['setuptools'], 45 | zip_safe=True, 46 | maintainer='nickybones', 47 | maintainer_email='nickyfullmetal@gmail.com', 48 | description='TODO: Package description', 49 | license='TODO: License declaration', 50 | tests_require=['pytest'], 51 | entry_points={ 52 | 'console_scripts': [ 53 | 'GTNode = omni3d_ros.GTNode:main', 54 | 'SIMPNode = omni3d_ros.SIMPNode:main', 55 | 'Omni3DNode = omni3d_ros.Omni3DNode:main', 56 | 'Omni3DMappingNode = omni3d_ros.Omni3DMappingNode:main', 57 | 'SemanticMapNode = omni3d_ros.SemanticMapNode:main', 58 | ], 59 | }, 60 | ) 61 | 62 | 63 | -------------------------------------------------------------------------------- /ros_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | source /opt/ros/noetic/setup.bash 5 | 6 | exec "$@" --------------------------------------------------------------------------------