Fix lookupTransform transform camera_to_odom

uhobeike commented 1 year ago

When I run the simulator on Gazebo simulator with ROS Noetic, I get the following error when using ros::Duration(0.0).

[ERROR] [1669975384.352619231, 25.089000000]: Transform failed: Lookup would require extrapolation 0.047000000s into the future.  Requested time 25.049000000 but the latest data is at time 25.002000000, when looking up transform from frame [odom] to frame [camera_color_optical_frame]

So I substituted transform_tolerance and it improved.

I haven't checked ROS2. If this PR is OK, create a new PR for ROS2.

ymd-stella commented 1 year ago

Would you mind showing me a rendering of rqt_tf_tree? (Or provide steps to reproduce it.)

uhobeike commented 1 year ago

About the PC specs used

12th Gen Intel(R) Core(TM) i9-12900K
GeForce RTX 2060

rqt_tf_tree

Before code modification
After code modification

uhobeike commented 1 year ago

The comment has been split up due to the lengthy explanation of how to reproduce it.

How to reproduce

Dockerfile used

FROM osrf/ros:noetic-desktop-full

ENV DEBIAN_FRONTEND noninteractive

# install dependencies via apt
ENV DEBCONF_NOWARNINGS yes
RUN set -x && \
    apt-get update -y -qq && \
    apt-get upgrade -y -qq --no-install-recommends && \
    : "basic dependencies" && \
    apt-get install -y -qq \
    build-essential \
    pkg-config \
    cmake \
    git \
    wget \
    curl \
    tar \
    unzip && \
    : "g2o dependencies" && \
    apt-get install -y -qq \
    libatlas-base-dev \
    libsuitesparse-dev \
    libglew-dev && \
    : "OpenCV dependencies" && \
    apt-get install -y -qq \
    libgtk-3-dev \
    libjpeg-dev \
    libpng++-dev \
    libtiff-dev \
    libopenexr-dev \
    libwebp-dev \
    ffmpeg \
    libavcodec-dev \
    libavformat-dev \
    libavutil-dev \
    libswscale-dev \
    libavresample-dev && \
    : "backward-cpp dependencies" && \
    apt install -y -qq binutils-dev && \
    : "other dependencies" && \
    apt-get install -y -qq \
    libyaml-cpp-dev \
    sqlite3 \
    libsqlite3-dev && \
    : "ros dependencies" && \
    apt install -y -qq ros-${ROS_DISTRO}-cv-bridge  \
    python3-catkin-tools

ARG CMAKE_INSTALL_PREFIX=/usr/local
ARG NUM_THREADS=4

ENV CPATH=${CMAKE_INSTALL_PREFIX}/include:${CPATH}
ENV C_INCLUDE_PATH=${CMAKE_INSTALL_PREFIX}/include:${C_INCLUDE_PATH}
ENV CPLUS_INCLUDE_PATH=${CMAKE_INSTALL_PREFIX}/include:${CPLUS_INCLUDE_PATH}
ENV LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/lib:${LIBRARY_PATH}
ENV LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/lib:${LD_LIBRARY_PATH}

ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all}
ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics

# Eigen
ARG EIGEN3_VERSION=3.3.7
WORKDIR /tmp
RUN set -x && \
    wget -q https://gitlab.com/libeigen/eigen/-/archive/${EIGEN3_VERSION}/eigen-${EIGEN3_VERSION}.tar.bz2 && \
    tar xf eigen-${EIGEN3_VERSION}.tar.bz2 && \
    rm -rf eigen-${EIGEN3_VERSION}.tar.bz2 && \
    cd eigen-${EIGEN3_VERSION} && \
    mkdir -p build && \
    cd build && \
    cmake \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \
    .. && \
    make -j${NUM_THREADS} && \
    make install && \
    cd /tmp && \
    rm -rf *
ENV Eigen3_DIR=${CMAKE_INSTALL_PREFIX}/share/eigen3/cmake

# g2o
ARG G2O_COMMIT=9b41a4ea5ade8e1250b9c1b279f3a9c098811b5a
WORKDIR /tmp
RUN set -x && \
    git clone https://github.com/RainerKuemmerle/g2o.git && \
    cd g2o && \
    git checkout ${G2O_COMMIT} && \
    mkdir -p build && \
    cd build && \
    cmake \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \
    -DBUILD_SHARED_LIBS=ON \
    -DBUILD_UNITTESTS=OFF \
    -DG2O_USE_CHOLMOD=OFF \
    -DG2O_USE_CSPARSE=ON \
    -DG2O_USE_OPENGL=OFF \
    -DG2O_USE_OPENMP=OFF \
    -DG2O_BUILD_APPS=OFF \
    -DG2O_BUILD_EXAMPLES=OFF \
    -DG2O_BUILD_LINKED_APPS=OFF \
    .. && \
    make -j${NUM_THREADS} && \
    make install && \
    cd /tmp && \
    rm -rf *
ENV g2o_DIR=${CMAKE_INSTALL_PREFIX}/lib/cmake/g2o

# OpenCV
ARG OPENCV_VERSION=4.2.0
WORKDIR /tmp
RUN set -x && \
    wget -q https://github.com/opencv/opencv/archive/${OPENCV_VERSION}.zip && \
    unzip -q ${OPENCV_VERSION}.zip && \
    rm -rf ${OPENCV_VERSION}.zip && \
    wget -q https://github.com/opencv/opencv_contrib/archive/refs/tags/${OPENCV_VERSION}.zip -O opencv_contrib-${OPENCV_VERSION}.zip && \
    unzip -q opencv_contrib-${OPENCV_VERSION}.zip && \
    rm -rf opencv_contrib-${OPENCV_VERSION}.zip && \
    mkdir extra && \
    mv opencv_contrib-${OPENCV_VERSION}/modules/aruco extra && \
    rm -rf opencv_contrib-${OPENCV_VERSION} && \
    cd opencv-${OPENCV_VERSION} && \
    mkdir -p build && \
    cd build && \
    cmake \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \
    -DBUILD_DOCS=OFF \
    -DBUILD_EXAMPLES=OFF \
    -DBUILD_JASPER=OFF \
    -DBUILD_OPENEXR=OFF \
    -DBUILD_PERF_TESTS=OFF \
    -DBUILD_TESTS=OFF \
    -DBUILD_PROTOBUF=OFF \
    -DBUILD_opencv_apps=OFF \
    -DBUILD_opencv_dnn=OFF \
    -DBUILD_opencv_ml=OFF \
    -DBUILD_opencv_python_bindings_generator=OFF \
    -DENABLE_CXX11=ON \
    -DENABLE_FAST_MATH=ON \
    -DWITH_EIGEN=ON \
    -DWITH_FFMPEG=ON \
    -DWITH_OPENMP=ON \
    -DOPENCV_EXTRA_MODULES_PATH=/tmp/extra \
    .. && \
    make -j${NUM_THREADS} && \
    make install && \
    cd /tmp && \
    rm -rf *
ENV OpenCV_DIR=${CMAKE_INSTALL_PREFIX}/lib/cmake/opencv4

# Pangolin
ARG PANGOLIN_COMMIT=ad8b5f83222291c51b4800d5a5873b0e90a0cf81
WORKDIR /tmp
RUN set -x && \
    git clone https://github.com/stevenlovegrove/Pangolin.git && \
    cd Pangolin && \
    git checkout ${PANGOLIN_COMMIT} && \
    sed -i -e "193,198d" ./src/utils/file_utils.cpp && \
    mkdir -p build && \
    cd build && \
    cmake \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \
    -DBUILD_EXAMPLES=OFF \
    -DBUILD_PANGOLIN_DEPTHSENSE=OFF \
    -DBUILD_PANGOLIN_FFMPEG=OFF \
    -DBUILD_PANGOLIN_LIBDC1394=OFF \
    -DBUILD_PANGOLIN_LIBJPEG=OFF \
    -DBUILD_PANGOLIN_LIBOPENEXR=OFF \
    -DBUILD_PANGOLIN_LIBPNG=OFF \
    -DBUILD_PANGOLIN_LIBREALSENSE=OFF \
    -DBUILD_PANGOLIN_LIBREALSENSE2=OFF \
    -DBUILD_PANGOLIN_LIBTIFF=OFF \
    -DBUILD_PANGOLIN_LIBUVC=OFF \
    -DBUILD_PANGOLIN_LZ4=OFF \
    -DBUILD_PANGOLIN_OPENNI=OFF \
    -DBUILD_PANGOLIN_OPENNI2=OFF \
    -DBUILD_PANGOLIN_PLEORA=OFF \
    -DBUILD_PANGOLIN_PYTHON=OFF \
    -DBUILD_PANGOLIN_TELICAM=OFF \
    -DBUILD_PANGOLIN_TOON=OFF \
    -DBUILD_PANGOLIN_UVC_MEDIAFOUNDATION=OFF \
    -DBUILD_PANGOLIN_V4L=OFF \
    -DBUILD_PANGOLIN_VIDEO=OFF \
    -DBUILD_PANGOLIN_ZSTD=OFF \
    -DBUILD_PYPANGOLIN_MODULE=OFF \
    .. && \
    make -j${NUM_THREADS} && \
    make install && \
    cd /tmp && \
    rm -rf *
ENV Pangolin_DIR=${CMAKE_INSTALL_PREFIX}/lib/cmake/Pangolin

# backward-cpp
ARG BACKWARD_CPP_COMMIT=5ffb2c879ebdbea3bdb8477c671e32b1c984beaa
WORKDIR /tmp
RUN set -x && \
    git clone https://github.com/bombela/backward-cpp.git && \
    cd backward-cpp && \
    git checkout ${BACKWARD_CPP_COMMIT} && \
    mkdir -p build && \
    cd build && \
    cmake \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \
    .. && \
    make -j${NUM_THREADS} && \
    make install && \
    cd /tmp && \
    rm -rf *

# stella_vslam
RUN  git clone -b main --recursive --depth 1 https://github.com/stella-cv/stella_vslam.git /root/stella_vslam/
WORKDIR /root/stella_vslam/
RUN set -x && \
    cd /root/stella_vslam/ && \
    mkdir -p build && \
    cd build && \
    cmake \
    -DUSE_PANGOLIN_VIEWER=ON \
    -DINSTALL_PANGOLIN_VIEWER=ON \
    -DUSE_SOCKET_PUBLISHER=OFF \
    -DBUILD_TESTS=ON \
    -DBUILD_EXAMPLES=ON \
    .. && \
    make -j${NUM_THREADS} && \
    make install && \
    chmod -R 777 ./*

# stella_vslam_ros
RUN set -x && \
    mkdir -p ~/catkin_ws/src && \
    cd ~/catkin_ws/src && \
    git clone --recursive --branch ros --depth 1 https://github.com/stella-cv/stella_vslam_ros.git && \
    cd ~/catkin_ws && \
    /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; \
    rosdep install -r -y -i --from-paths src/stella_vslam_ros; \ 
    catkin_make -DUSE_PANGOLIN_VIEWER=ON -DUSE_SOCKET_PUBLISHER=OFF -j4" && \
    echo "source /root/catkin_ws/devel/setup.bash" >> /root/.bashrc

# raspicat_sim
RUN set -x && \
    mkdir -p ~/catkin_ws/src && \
    cd ~/catkin_ws/src && \
    git clone --depth 1 https://github.com/rt-net/raspicat_sim.git && \
    git clone --depth 1 https://github.com/rt-net/raspicat_ros.git && \
    git clone --depth 1 https://github.com/rt-net/raspicat_description.git && \
    git clone --depth 1 https://github.com/rt-net/raspimouse.git && \
    cd ~/catkin_ws && \
    /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; \
    rosdep install -r -y -i --from-paths src/raspicat* src/raspimouse*; \
    catkin_make -DUSE_PANGOLIN_VIEWER=ON -DUSE_SOCKET_PUBLISHER=OFF -j4" && \
    : "remove cache" && \
    apt-get autoremove -y -qq && \
    rm -rf /var/lib/apt/lists/*

# download orb_vocab
RUN curl -sL "https://github.com/stella-cv/FBoW_orb_vocab/raw/main/orb_vocab.fbow" -o /root/orb_vocab.fbow

WORKDIR /root/catkin_ws/src
ENTRYPOINT ["/ros_entrypoint.sh"]
CMD ["bash"]

camera config file(realsense_d435.yaml)

#==============#
# Camera Model #
#==============#
Camera:
  name: "RealSense D435"
  setup: "monocular"
  model: "perspective"
  fx: 1386.4138492513919
  fy: 1386.4138492513919
  cx: 960.5
  cy: 540.5
  k1: 0.0
  k2: 0.0
  p1: 0.0
  p2: 0.0
  k3: 0.0
  fps: 10.0
  cols: 1080
  rows: 1920
  color_order: "RGB"
#=====================#
# Tracking Parameters #
#=====================#
Preprocessing:
  min_size: 500
#================#
# ORB Parameters #
#================#
Feature:
  name: "ORB feature extraction setting"
  scale_factor: 1.2
  num_levels: 8
  ini_fast_threshold: 20
  min_fast_threshold: 7
#====================#
# Mapping Parameters #
#====================#
Mapping:
  baseline_dist_thr_ratio: 0.02
  redundant_obs_ratio_thr: 0.9
  num_covisibilities_for_landmark_generation: 20
  num_covisibilities_for_landmark_fusion: 20
#===========================#
# PangolinViewer Parameters #
#===========================#
PangolinViewer:
  keyframe_size: 0.05
  keyframe_line_width: 1
  graph_line_width: 1
  point_size: 2
  camera_size: 0.08
  camera_line_width: 3
  viewpoint_x: 0
  viewpoint_y: -0.9
  viewpoint_z: -1.9
  viewpoint_f: 400

Execution command

Docker image build

docker build -t stella-vslam -f Dockerfile .

Docker run The purpose of the -v mount is to bring the realsense_d435.yaml file into the container.

docker run -it --privileged --gpus all -e DISPLAY -e QT_X11_NO_MITSHM=1 -v /tmp/.X11-unix:/tmp/.X11-unix -v /dev:/dev:ro -v /camera_config:/root/stella_vslam_config/ --name stella-vslam stella-vslam

Runs in a container


roslaunch raspicat_gazebo raspicat_with_iscas_museum.launch rviz:=false

rosrun stella_vslam_ros run_slam -v /root/orb_vocab.fbow -c /root/stella_vslam_config/realsense_d435.yaml --map-db-out /root/stella_vslam_config/map.msg /camera/image_raw:=/camera/color/image_raw base_link:=base_footprint camera_frame:=camera_link --frame-skip 3

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

ymd-stella commented 1 year ago

This error occurs when tf is never given between the time the image is captured and the end of the tracking process.

Try setting the output frequency of tf (odom to base_footprint) to 100 Hz. See https://github.com/rt-net/raspicat_description/blob/680e5add0d8c05b280fd074f758d6ca0b9084bab/urdf/wheel/wheel.gazebo.xacro#L20.

ymd-stella commented 1 year ago

I tried to execute the above steps, but the results were different.

frames

uhobeike commented 1 year ago

Thank you for your confirmation. I will confirm and get back to you by the end of this week as I am on deadline until Monday.

ymd-stella commented 1 year ago

I made the following changes and it looks like it is working correctly. However, the camera seems to show only near objects.

diff --git a/urdf/wheel/wheel.gazebo.xacro b/urdf/wheel/wheel.gazebo.xacro
index 467047a..3b2343a 100644
--- a/urdf/wheel/wheel.gazebo.xacro
+++ b/urdf/wheel/wheel.gazebo.xacro
@@ -17,7 +17,7 @@
       <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
         <robotNamespace>${robot_namespace}</robotNamespace>
         <alwaysOn>true</alwaysOn>
-        <updateRate>10</updateRate>
+        <updateRate>100</updateRate>
         <legacyMode>false</legacyMode>
         <rosDebugLevel>na</rosDebugLevel>
         <leftJoint>left_wheel_joint</leftJoint>
@@ -32,7 +32,7 @@
         <wheelTorque>50</wheelTorque>
         <wheelAcceleration>1</wheelAcceleration>
         <publishTf>${publish_tf}</publishTf>
-        <publishWheelTF>${publish_tf}</publishWheelTF>
+        <!-- <publishWheelTF>${publish_tf}</publishWheelTF> -->
         <publishOdomTF>${publish_tf}</publishOdomTF>
         <publishWheelJointState>${publish_jointstate}</publishWheelJointState>
       </plugin>

ymd-stella commented 1 year ago

I will close this pull request. Feel free to add comments or make another Issues or Discussions. I am also interested in getting stella_vslam working on gazebo.

ymd-stella commented 1 year ago

However, the camera seems to show only near objects.

Modifying the following settings will resolve the issue.

https://github.com/rt-net/raspicat_description/blob/680e5add0d8c05b280fd074f758d6ca0b9084bab/urdf/sensors/realsense_d435.gazebo.xacro#L15-L18

ymd-stella commented 1 year ago

The vertical and horizontal sizes are swapped.

-  cols: 1080
-  rows: 1920
+  cols: 1920
+  rows: 1080

uhobeike commented 1 year ago

The vertical and horizontal sizes are swapped.

It was indeed the opposite. When we modified it and ran it, we got more features than before and VSLAM is now much easier to execute!

Thank you very much.

https://user-images.githubusercontent.com/40545422/207218060-26bc8ce2-6b17-4e62-a30b-cebcf4bbf842.mp4

stella-cv / stella_vslam_ros