FROM arm64v8/ubuntu:20.04 as rtabmap_orb_slam3
ARG ROS_DISTRO=noetic
ENV DEBIAN_FRONTEND=noninteractive 

# Install basic packages
RUN apt-get update \
&&  apt-get upgrade -y \
&&  apt-get install -y \
    curl \
    gnupg2 \
    lsb-release \
    wget \
    locales \
    nano \
    git \
    sudo \
    build-essential \
    cmake \
    python3-vtk7 \
    libeigen3-dev

RUN apt-get update && apt-get install -y --no-install-recommends apt-utils


ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get -y install tzdata

# Install ROS1 desktop
RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \    
    && curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - \
    && apt-get update \
    && apt-get install -y   ros-${ROS_DISTRO}-desktop-full

RUN apt-get install -y  python3-rosdep \
                        python3-rosinstall \
                        python3-rosinstall-generator \
                        python3-wstool \
                        build-essential                   


RUN rosdep init && rosdep update
     
RUN apt-get update && apt-get install -y \
    cmake \
    libatlas-base-dev \
    libeigen3-dev \
    libgoogle-glog-dev \
    libsuitesparse-dev \
    python3-catkin-tools \
    python3-pybind11 \
    ros-${ROS_DISTRO}-cv-bridge \
    ros-${ROS_DISTRO}-image-transport \
    ros-${ROS_DISTRO}-message-filters \
    ros-${ROS_DISTRO}-tf \
    ros-noetic-pcl-conversions \
    ros-noetic-costmap-2d  \
    ros-noetic-image-geometry \
    ros-noetic-pcl-ros \
    ros-noetic-move-base-msgs \
    ros-noetic-stereo-image-proc \
    ros-noetic-image-transport-plugins \
    ros-noetic-libg2o \
    ros-noetic-octomap-msgs \
    ros-noetic-fiducials \
    ros-noetic-grid-map \
    ros-noetic-apriltag-ros \
    ros-noetic-find-object-2d \
    ros-noetic-octomap-rviz-plugins \
    ros-noetic-imu-tools \
    ros-noetic-gtsam && \  
    rm -rf /var/lib/apt/lists/*

RUN apt-get update && apt-get -y upgrade

#Install Pangolin
RUN mkdir rtabmap && \
    cd rtabmap && \
    git clone https://github.com/stevenlovegrove/Pangolin.git && cd Pangolin && git checkout v0.6 && \
    ls && \
    mkdir build && \
    cd build && \
    cmake .. && \
    make -j8 && \
    make install && \
    cd && \
    rm -rf Pangolin
    
    

#RUN cd rtabmap && \
#    git clone -b v0.4-beta https://github.com/UZ-SLAMLab/ORB_SLAM3.git && \
#    cd ORB_SLAM3/ && \
#    wget https://gist.githubusercontent.com/matlabbe/f5cb281304a1305b2824a6ce19792e13/raw/f8bbc796edc29b9f815cbf3c99a0c3e13e23663d/orbslam3_v4_rtabmap_fix.patch && \
#    git apply orbslam3_v4_rtabmap_fix.patch && \
#    ./build.sh


RUN cd rtabmap && \
    git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git

    
COPY orbslam3_rtabmap_fix.patch /rtabmap/ORB_SLAM3 

ENV ORB_SLAM_ROOT_DIR /rtabmap/ORB_SLAM3

RUN  cd rtabmap/ORB_SLAM3 && \
     ls && \
     sed -i 's/-march=native//g' CMakeLists.txt Thirdparty/DBoW2/CMakeLists.txt Thirdparty/g2o/CMakeLists.txt && \
     git apply orbslam3_rtabmap_fix.patch && \
     ./build.sh
     
RUN rm /bin/sh && ln -s /bin/bash /bin/sh   #Required for source /opt/ros..
 
RUN cd rtabmap && \
    git clone https://github.com/introlab/rtabmap.git rtabmap && \
    cd rtabmap/build && \
    apt remove ros-noetic-rtabmap && \
    cmake -DWITH_PYTHON=ON -DWITH_G2O=OFF -DWITH_ORB_SLAM=ON .. && \
    make -j8 && \
    sudo make install

ENV RTABMap_DIR /rtabmap/rtabmap 

RUN cp /rtabmap/ORB_SLAM3/Thirdparty/g2o/lib/libg2o.so /usr/local/lib && \
    cp /rtabmap/ORB_SLAM3/lib/libORB_SLAM3.so /usr/local/lib

#ENV INTERFACE_INCLUDE_DIRECTORIES "/opt/ros/noetic/include;/usr/include/suitesparse"

RUN source /opt/ros/noetic/setup.bash && \ 
    cd rtabmap && \
    mkdir -p catkin_ws/src && \ 
    ls && \
    cd catkin_ws/src && \
    git clone https://github.com/introlab/rtabmap_ros.git && \
    ls && \
    cd .. && \
#    catkin_make -j8
    catkin_make -j8 -DWITH_G2O=OFF -DWITH_ORB_SLAM=ON