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 \
    gdb \
    xterm \
    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 \
    libgflags-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-tf2-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
    
#ORB_SLAM3
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


#VINS-Fusion with Ceres

RUN cd rtabmap && \
    mkdir -p ceres && cd ceres && \
    wget http://ceres-solver.org/ceres-solver-1.14.0.tar.gz && \
    tar zxf ceres-solver-1.14.0.tar.gz && \
    cmake ./ceres-solver-1.14.0  && \
    make -j8 && \
   # make test && \
    make install

RUN rm /bin/sh && ln -s /bin/bash /bin/sh   #Required for source /opt/ros..

# VINS-Fusion

RUN source /opt/ros/noetic/setup.bash 
    
RUN cd rtabmap && \
    mkdir -p catkin_ws/src && \ 
    cd catkin_ws/src && \
    git clone https://github.com/rkuo2000/VINS-Fusion.git   #(This is fork for OpenCV4.2)
    
COPY vins-fusion.patch /rtabmap/catkin_ws/src/VINS-Fusion

RUN cd rtabmap/catkin_ws/src/VINS-Fusion && \
    git apply vins-fusion.patch && \
    source /opt/ros/noetic/setup.bash  && \
    cd /rtabmap/catkin_ws && \
    catkin_make && \
    source devel/setup.bash 

#Install OpenVINS
RUN cd rtabmap/catkin_ws/src && \
    git clone https://github.com/rpng/open_vins/ && \
    cd .. && \
    source /opt/ros/noetic/setup.bash && \
    catkin_make -j8 && \
    source devel/setup.bash

#RTAB-MAP  
       
RUN source rtabmap/catkin_ws/devel/setup.bash && \
    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_CERES=ON -DWITH_ORB_SLAM=ON -DWITH_OPENVINS=ON -DWITH_VINS=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


#RTAB-MAP-ROS
RUN source /opt/ros/noetic/setup.bash && \ 
    source rtabmap/catkin_ws/devel/setup.bash && \
    cd rtabmap && \
    cd catkin_ws/src && \
    git clone https://github.com/introlab/rtabmap_ros.git && \
    cd .. && \
    catkin_make -j8 && \
    export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH