diff --git a/.github/workflows/ubuntu-dep-src.yml b/.github/workflows/ubuntu-dep-src.yml index 02126f9b77..8132a7216d 100644 --- a/.github/workflows/ubuntu-dep-src.yml +++ b/.github/workflows/ubuntu-dep-src.yml @@ -13,6 +13,10 @@ concurrency: group: ${{ github.workflow }}-${{ github.ref || github.run_id }} cancel-in-progress: true +env: + # Function to get the most recent tag of a remote repository without having to clone it, excluding tag with 'pr' as in Pre Release + FUNCTION_GET_LATEST: 'git -c "versionsort.suffix=-" ls-remote --exit-code --refs --sort="version:refname" --tags ${GIT_ADDRESS} "*.*.*" | cut --delimiter="/" --fields=3 | grep -v -e pr | tail --lines=1' + jobs: build-ubuntu-dep-src: runs-on: ${{ matrix.os }} @@ -59,7 +63,10 @@ jobs: run: | pwd echo $GITHUB_WORKSPACE - git clone --depth 1 https://github.com/xianyi/OpenBLAS.git ${HOME}/OpenBLAS + GIT_ADDRESS=https://github.com/xianyi/OpenBLAS.git + LATEST_TAG=`eval ${FUNCTION_GET_LATEST}` + echo "Newest tag is ${LATEST_TAG}" + git clone --depth 1 --branch ${LATEST_TAG} ${GIT_ADDRESS} ${HOME}/OpenBLAS cd ${HOME}/OpenBLAS mkdir install make -j$(nproc) @@ -74,7 +81,10 @@ jobs: pwd echo "GIT_CLONE_PROTECTION_ACTIVE=false" >> $GITHUB_ENV export GIT_CLONE_PROTECTION_ACTIVE=false - git clone --recursive --depth 1 --branch v9.3.0 https://github.com/Kitware/VTK.git ${HOME}/VTK + GIT_ADDRESS="https://github.com/Kitware/VTK.git" + LATEST_TAG=`eval ${FUNCTION_GET_LATEST}` + echo "Newest tag is ${LATEST_TAG}" + git clone --recursive --depth 1 --branch ${LATEST_TAG} ${GIT_ADDRESS} ${HOME}/VTK cd ${HOME}/VTK mkdir build && cd build && mkdir install cmake .. -DVTK_ANDROID_BUILD=OFF -DVTK_BUILD_DOCUMENTATION=OFF -DVTK_BUILD_EXAMPLES=OFF -DVTK_BUILD_EXAMPLES=OFF -DCMAKE_BUILD_TYPE=Release \ @@ -86,7 +96,10 @@ jobs: - name: Build OpenCV from source run: | pwd - git clone --depth 1 https://github.com/opencv/opencv.git ${HOME}/opencv + GIT_ADDRESS="https://github.com/opencv/opencv.git" + LATEST_TAG=`eval ${FUNCTION_GET_LATEST}` + echo "Newest tag is ${LATEST_TAG}" + git clone --depth 1 --branch ${LATEST_TAG} ${GIT_ADDRESS} ${HOME}/opencv cd ${HOME}/opencv mkdir build && cd build && mkdir install cmake .. -DBUILD_EXAMPLES=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$(pwd)/install @@ -97,7 +110,10 @@ jobs: - name: Build librealsense2 from source run: | pwd - git clone --depth 1 https://github.com/IntelRealSense/librealsense.git ${HOME}/librealsense + GIT_ADDRESS="https://github.com/IntelRealSense/librealsense.git" + LATEST_TAG=`eval ${FUNCTION_GET_LATEST}` + echo "Newest tag is ${LATEST_TAG}" + git clone --depth 1 --branch ${LATEST_TAG} ${GIT_ADDRESS} ${HOME}/librealsense cd ${HOME}/librealsense mkdir build && cd build && mkdir install cmake .. -DBUILD_EXAMPLES=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$(pwd)/install @@ -108,7 +124,10 @@ jobs: - name: Build PCL from source run: | pwd - git clone --depth 1 https://github.com/PointCloudLibrary/pcl.git ${HOME}/pcl + GIT_ADDRESS="https://github.com/PointCloudLibrary/pcl.git" + LATEST_TAG=`eval ${FUNCTION_GET_LATEST}` + echo "Newest tag is ${LATEST_TAG}" + git clone --depth 1 --branch ${LATEST_TAG} ${GIT_ADDRESS} ${HOME}/pcl cd ${HOME}/pcl mkdir build && cd build && mkdir install cmake .. -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DPCL_DISABLE_GPU_TESTS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$(pwd)/install diff --git a/ChangeLog.txt b/ChangeLog.txt index 926f81569c..6af835006b 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -38,7 +38,8 @@ ViSP 3.x.x (Version in development) . New tutorials in tutorial/segmentation/color folder to show how to use HSV color segmentation to extract the corresponding point cloud . New scene renderer based on Panda3D. See inheritance diagram for vpPanda3DBaseRenderer class and corresponding - tutorial. + tutorial + . New docker image for Ubuntu 24.04 in docker folder - Applications . Migrate eye-to-hand tutorials in apps - Tutorials diff --git a/ci/docker/ubuntu-18.04/Dockerfile b/ci/docker/ubuntu-18.04/Dockerfile index 0e8c23a443..468a3cb008 100644 --- a/ci/docker/ubuntu-18.04/Dockerfile +++ b/ci/docker/ubuntu-18.04/Dockerfile @@ -1,50 +1,88 @@ FROM ubuntu:18.04 -MAINTAINER Fabien Spindler ARG DEBIAN_FRONTEND=noninteractive -ARG VISPCI_USER_UID=1001 -ARG DOCKER_GROUP_GID=130 +ARG USER_UID=1001 ENV TZ=Europe/Paris -# Update aptitude with new repo -RUN apt-get update +# Update aptitude with default packages +RUN apt-get update \ + && apt-get install -y \ + build-essential \ + cmake \ + cmake-curses-gui \ + curl \ + gedit \ + gdb \ + git \ + iputils-ping \ + locales \ + locate \ + lsb-release \ + mercurial \ + nano \ + net-tools \ + python3 \ + python3-dev \ + python3-pip \ + python3-dbg \ + python3-empy \ + python3-numpy \ + python3-pip \ + python3-psutil \ + python3-venv \ + software-properties-common \ + sudo \ + tzdata \ + vim \ + wget \ + && apt-get clean -# Install packages -RUN apt-get install -y \ - sudo \ - build-essential \ - cmake \ - git \ - net-tools \ - iputils-ping \ - # Recommended ViSP 3rd parties - libopencv-dev \ - libx11-dev \ - liblapack-dev \ - libeigen3-dev \ - libdc1394-22-dev \ - libv4l-dev \ - libzbar-dev \ - libpthread-stubs0-dev \ - # Other optional 3rd parties - libpcl-dev \ - libcoin80-dev \ - libjpeg-dev \ - libpng-dev \ - libogre-1.9-dev \ - libois-dev \ - libdmtx-dev \ - libgsl-dev +# Update aptitude with recommended ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libdc1394-22-dev \ + libeigen3-dev \ + liblapack-dev \ + libopencv-dev \ + libv4l-dev \ + libx11-dev \ + libzbar-dev \ + && apt-get clean -RUN adduser --disabled-password --gecos "" --uid $VISPCI_USER_UID vispci \ - && groupadd docker --gid $DOCKER_GROUP_GID \ - && usermod -aG sudo vispci \ - && usermod -aG docker vispci \ - && echo "%sudo ALL=(ALL:ALL) NOPASSWD:ALL" > /etc/sudoers \ - && echo "Defaults env_keep += \"DEBIAN_FRONTEND\"" >> /etc/sudoers \ - && adduser vispci video +# Update aptitude with other optional ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libpcl-dev \ + libcoin80-dev \ + libjpeg-dev \ + libpng-dev \ + libogre-1.9-dev \ + libois-dev \ + libdmtx-dev \ + libgsl-dev \ + && apt-get clean -ENV HOME=/home/vispci +# Set Locale +RUN locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 && \ + export LANG=en_US.UTF-8 + +ENV USERNAME vispci + +RUN useradd -U --uid $USER_UID -ms /bin/bash ${USERNAME} \ + && echo "${USERNAME}:${USERNAME}" | chpasswd \ + && adduser ${USERNAME} sudo \ + && echo "$USERNAME ALL=NOPASSWD: ALL" >> /etc/sudoers.d/${USERNAME} \ + && adduser ${USERNAME} video + +# Commands below are now run as normal user +USER ${USERNAME} + +# When running a container start in the home folder +WORKDIR /home/$USERNAME + +# Create visp workspace +ENV VISP_WS /home/$USERNAME/visp-ws # Install visp-images RUN mkdir -p ${HOME}/visp-ws \ @@ -54,16 +92,14 @@ RUN mkdir -p ${HOME}/visp-ws \ && echo "export VISP_INPUT_IMAGE_PATH=${HOME}/visp-ws/visp-images" >> ${HOME}/.bashrc # Get visp -RUN cd ${HOME}/visp-ws \ +RUN cd ${VISP_WS} \ && git clone https://github.com/lagadic/visp # Build visp -RUN cd ${HOME}/visp-ws \ +RUN cd ${VISP_WS} \ && mkdir visp-build \ && cd visp-build \ && cmake ../visp \ - && make -j4 - -USER vispci + && make -j$(nproc) -WORKDIR /home/vispci +CMD ["/bin/bash"] diff --git a/ci/docker/ubuntu-20.04/Dockerfile b/ci/docker/ubuntu-20.04/Dockerfile index d9de38da6f..3a20a67327 100644 --- a/ci/docker/ubuntu-20.04/Dockerfile +++ b/ci/docker/ubuntu-20.04/Dockerfile @@ -1,50 +1,92 @@ FROM ubuntu:20.04 -MAINTAINER Fabien Spindler ARG DEBIAN_FRONTEND=noninteractive -ARG VISPCI_USER_UID=1001 -ARG DOCKER_GROUP_GID=130 +ARG USER_UID=1001 ENV TZ=Europe/Paris -# Update aptitude with new repo -RUN apt-get update +# Update aptitude with default packages +RUN apt-get update \ + && apt-get install -y \ + build-essential \ + cmake \ + cmake-curses-gui \ + curl \ + gedit \ + gdb \ + git \ + iputils-ping \ + locales \ + locate \ + lsb-release \ + mercurial \ + nano \ + net-tools \ + python3 \ + python3-dev \ + python3-pip \ + python3-dbg \ + python3-empy \ + python3-numpy \ + python3-pip \ + python3-psutil \ + python3-venv \ + software-properties-common \ + sudo \ + tzdata \ + vim \ + wget \ + && apt-get clean -# Install packages -RUN apt-get install -y \ - sudo \ - build-essential \ - cmake \ - git \ - net-tools \ - iputils-ping \ - # Recommended ViSP 3rd parties - libopencv-dev \ - libx11-dev \ - liblapack-dev \ - libeigen3-dev \ - libdc1394-22-dev \ - libv4l-dev \ - libzbar-dev \ - libpthread-stubs0-dev \ - # Other optional 3rd parties - libpcl-dev \ - libcoin-dev \ - libjpeg-dev \ - libpng-dev \ - libogre-1.9-dev \ - libois-dev \ - libdmtx-dev \ - libgsl-dev +# Update aptitude with recommended ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libdc1394-22-dev \ + libeigen3-dev \ + liblapack-dev \ + libopencv-dev \ + libv4l-dev \ + libx11-dev \ + libzbar-dev \ + nlohmann-json3-dev \ + && apt-get clean -RUN adduser --disabled-password --gecos "" --uid $VISPCI_USER_UID vispci \ - && groupadd docker --gid $DOCKER_GROUP_GID \ - && usermod -aG sudo vispci \ - && usermod -aG docker vispci \ - && echo "%sudo ALL=(ALL:ALL) NOPASSWD:ALL" > /etc/sudoers \ - && echo "Defaults env_keep += \"DEBIAN_FRONTEND\"" >> /etc/sudoers \ - && adduser vispci video +# Update aptitude with other optional ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libcoin-dev \ + libdmtx-dev \ + libgsl-dev \ + libjpeg-dev \ + libogre-1.9-dev \ + libois-dev \ + libpcl-dev \ + libpng-dev \ + && apt-get clean -ENV HOME=/home/vispci +# Set Locale +RUN locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 && \ + export LANG=en_US.UTF-8 + +ENV USERNAME vispci + +RUN useradd -U --uid $USER_UID -ms /bin/bash ${USERNAME} \ + && echo "${USERNAME}:${USERNAME}" | chpasswd \ + && adduser ${USERNAME} sudo \ + && echo "$USERNAME ALL=NOPASSWD: ALL" >> /etc/sudoers.d/${USERNAME} \ + && adduser ${USERNAME} video + +# Commands below are now run as normal user +USER ${USERNAME} + +# When running a container start in the home folder +WORKDIR /home/$USERNAME + +# Some apps don't show controls without this +ENV QT_X11_NO_MITSHM 1 + +# Create visp workspace +ENV VISP_WS /home/$USERNAME/visp-ws # Install visp-images RUN mkdir -p ${HOME}/visp-ws \ @@ -54,16 +96,14 @@ RUN mkdir -p ${HOME}/visp-ws \ && echo "export VISP_INPUT_IMAGE_PATH=${HOME}/visp-ws/visp-images" >> ${HOME}/.bashrc # Get visp -RUN cd ${HOME}/visp-ws \ +RUN cd ${VISP_WS} \ && git clone https://github.com/lagadic/visp # Build visp -RUN cd ${HOME}/visp-ws \ +RUN cd ${VISP_WS} \ && mkdir visp-build \ && cd visp-build \ && cmake ../visp \ - && make -j4 - -USER vispci + && make -j$(nproc) -WORKDIR /home/vispci +CMD ["/bin/bash"] diff --git a/ci/docker/ubuntu-22.04/Dockerfile b/ci/docker/ubuntu-22.04/Dockerfile index dfe5a9da72..e0a12e87fd 100644 --- a/ci/docker/ubuntu-22.04/Dockerfile +++ b/ci/docker/ubuntu-22.04/Dockerfile @@ -1,49 +1,89 @@ FROM ubuntu:22.04 -MAINTAINER Fabien Spindler ARG DEBIAN_FRONTEND=noninteractive -ARG VISPCI_USER_UID=1001 -ARG DOCKER_GROUP_GID=121 +ARG USER_UID=1001 ENV TZ=Europe/Paris -# Update aptitude with new repo -RUN apt-get update +# Update aptitude with default packages +RUN apt-get update \ + && apt-get install -y \ + build-essential \ + cmake \ + cmake-curses-gui \ + curl \ + gedit \ + gdb \ + git \ + iputils-ping \ + locales \ + locate \ + lsb-release \ + mercurial \ + nano \ + net-tools \ + python3 \ + python3-dev \ + python3-pip \ + python3-dbg \ + python3-empy \ + python3-numpy \ + python3-pip \ + python3-psutil \ + python3-venv \ + software-properties-common \ + sudo \ + tzdata \ + vim \ + wget \ + && apt-get clean -# Install packages -RUN apt-get install -y \ - sudo \ - build-essential \ - cmake \ - git \ - net-tools \ - iputils-ping \ - # Recommended ViSP 3rd parties - libopencv-dev \ - libx11-dev \ - liblapack-dev \ - libeigen3-dev \ - libdc1394-dev \ - libv4l-dev \ - libzbar-dev \ - # Other optional 3rd parties - libpcl-dev \ - libcoin-dev \ - libjpeg-turbo8-dev \ - libpng-dev \ - libogre-1.9-dev \ - libois-dev \ - libdmtx-dev \ - libgsl-dev +# Update aptitude with recommended ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libdc1394-dev \ + libeigen3-dev \ + liblapack-dev \ + libopencv-dev \ + libv4l-dev \ + libx11-dev \ + libzbar-dev \ + nlohmann-json3-dev \ + && apt-get clean -RUN adduser --disabled-password --gecos "" --uid $VISPCI_USER_UID vispci \ - && groupadd docker --gid $DOCKER_GROUP_GID \ - && usermod -aG sudo vispci \ - && usermod -aG docker vispci \ - && echo "%sudo ALL=(ALL:ALL) NOPASSWD:ALL" > /etc/sudoers \ - && echo "Defaults env_keep += \"DEBIAN_FRONTEND\"" >> /etc/sudoers \ - && adduser vispci video +# Update aptitude with other optional ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libcoin-dev \ + libdmtx-dev \ + libgsl-dev \ + libjpeg-turbo8-dev \ + libpcl-dev \ + libpng-dev \ + libogre-1.9-dev \ + libois-dev \ + && apt-get clean -ENV HOME=/home/vispci +# Set Locale +RUN locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 && \ + export LANG=en_US.UTF-8 + +ENV USERNAME vispci + +RUN useradd -U --uid $USER_UID -ms /bin/bash ${USERNAME} \ + && echo "${USERNAME}:${USERNAME}" | chpasswd \ + && adduser ${USERNAME} sudo \ + && echo "$USERNAME ALL=NOPASSWD: ALL" >> /etc/sudoers.d/${USERNAME} \ + && adduser ${USERNAME} video + +# Commands below are now run as normal user +USER ${USERNAME} + +# When running a container start in the home folder +WORKDIR /home/$USERNAME + +# Some apps don't show controls without this +ENV QT_X11_NO_MITSHM 1 # Install visp-images RUN mkdir -p ${HOME}/visp-ws \ @@ -53,16 +93,14 @@ RUN mkdir -p ${HOME}/visp-ws \ && echo "export VISP_INPUT_IMAGE_PATH=${HOME}/visp-ws/visp-images" >> ${HOME}/.bashrc # Get visp -RUN cd ${HOME}/visp-ws \ +RUN cd ${VISP_WS} \ && git clone https://github.com/lagadic/visp # Build visp -RUN cd ${HOME}/visp-ws \ +RUN cd ${VISP_WS} \ && mkdir visp-build \ && cd visp-build \ && cmake ../visp \ && make -j$(nproc) -USER vispci - -WORKDIR /home/vispci +CMD ["/bin/bash"] diff --git a/ci/docker/ubuntu-24.04/Dockerfile b/ci/docker/ubuntu-24.04/Dockerfile new file mode 100644 index 0000000000..08b76e39db --- /dev/null +++ b/ci/docker/ubuntu-24.04/Dockerfile @@ -0,0 +1,106 @@ +FROM ubuntu:24.04 + +ARG DEBIAN_FRONTEND=noninteractive +ARG USER_UID=1001 +ENV TZ=Europe/Paris + +## Update aptitude with default packages +RUN apt-get update \ + && apt-get install -y \ + build-essential \ + cmake \ + cmake-curses-gui \ + curl \ + gedit \ + gdb \ + git \ + iputils-ping \ + locales \ + locate \ + lsb-release \ + mercurial \ + nano \ + net-tools \ + python3 \ + python3-dev \ + python3-pip \ + python3-dbg \ + python3-empy \ + python3-numpy \ + python3-pip \ + python3-psutil \ + python3-venv \ + software-properties-common \ + sudo \ + tzdata \ + vim \ + wget \ + && apt-get clean + +# Update aptitude with recommended ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libdc1394-dev \ + libeigen3-dev \ + liblapack-dev \ + libopencv-dev \ + libv4l-dev \ + libx11-dev \ + libzbar-dev \ + nlohmann-json3-dev \ + && apt-get clean + +# Update aptitude with other optional ViSP 3rd parties +RUN apt-get update \ + && apt-get install -y \ + libcoin-dev \ + libdmtx-dev \ + libgsl-dev \ + libjpeg-turbo8-dev \ + libpng-dev \ + libogre-1.9-dev \ + libois-dev \ + libpcl-dev \ + && apt-get clean + +# Set Locale +RUN locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 && \ + export LANG=en_US.UTF-8 + +ENV USERNAME vispci + +RUN useradd -U --uid $USER_UID -ms /bin/bash ${USERNAME} \ + && echo "${USERNAME}:${USERNAME}" | chpasswd \ + && adduser ${USERNAME} sudo \ + && echo "$USERNAME ALL=NOPASSWD: ALL" >> /etc/sudoers.d/${USERNAME} \ + && adduser ${USERNAME} video + +# Commands below are now run as normal user +USER ${USERNAME} + +# When running a container start in the home folder +WORKDIR /home/$USERNAME + +# Some apps don't show controls without this +ENV QT_X11_NO_MITSHM 1 + +# Install visp-images +RUN mkdir -p ${HOME}/visp-ws \ + && cd ${HOME}/visp-ws \ + && git clone https://github.com/lagadic/visp-images.git \ + && echo "export VISP_WS=${HOME}/visp-ws" >> ${HOME}/.bashrc \ + && echo "export VISP_INPUT_IMAGE_PATH=${HOME}/visp-ws/visp-images" >> ${HOME}/.bashrc + +# Get visp +RUN cd ${HOME}/visp-ws \ + && git clone https://github.com/lagadic/visp + +# Build visp +RUN cd ${HOME}/visp-ws \ + && mkdir visp-build \ + && cd visp-build \ + && cmake ../visp \ + && make -j$(nproc) + +CMD ["/bin/bash"] diff --git a/ci/docker/ubuntu-dep-src/Dockerfile b/ci/docker/ubuntu-dep-src/Dockerfile new file mode 100644 index 0000000000..63ab7d1914 --- /dev/null +++ b/ci/docker/ubuntu-dep-src/Dockerfile @@ -0,0 +1,175 @@ +FROM ubuntu:22.04 + +ARG DEBIAN_FRONTEND=noninteractive +ARG USER_UID=1001 +ENV TZ=Europe/Paris + +## Variable containing the URL of the GIT repository that needs to be tested. +## By default GIT_URL is set to https://githb.com/lagadic/visp and GIT_BRANCH_NAME is set to master branch +## To build this container use : +## +## docker build . -t ubuntu-dep-src:v1 --build-arg GIT_URL="${YOUR_URL}" [--build-arg GIT_BRANCH_NAME="{BRANCH_NAME}"] +## +ARG GIT_URL=https://github.com/lagadic/visp +ARG GIT_BRANCH_NAME=master +RUN ["/bin/bash", "-c", ": ${GIT_URL:?Build argument GIT_URL needs to be set and not null.}"] +ENV GIT_URL="$GIT_URL" +ENV GIT_BRANCH_CMD="${GIT_BRANCH_NAME}" +ENV GIT_BRANCH_CMD=${GIT_BRANCH_CMD:+"--branch $GIT_BRANCH_NAME --depth 1"} +ENV FUNCTION_GET_LATEST='git -c "versionsort.suffix=-" ls-remote --exit-code --refs --sort="version:refname" --tags ${GIT_ADDRESS} "*.*.*" | cut --delimiter="/" --fields=3 | grep -v -e pr | tail --lines=1' + +# Update aptitude with default packages +RUN apt-get update \ + && apt-get install -y \ + build-essential \ + cmake \ + cmake-curses-gui \ + curl \ + gedit \ + git \ + locales \ + lsb-release \ + iputils-ping \ + nano \ + sudo \ + && apt-get clean + +# Install common dependencies +RUN apt update \ + && apt install -y \ + gfortran \ + freeglut3-dev \ + mesa-common-dev \ + mesa-utils \ + nlohmann-json3-dev \ + libboost-all-dev \ + libdc1394-dev \ + libeigen3-dev \ + libflann-dev \ + libgl1-mesa-dev \ + libglfw3-dev \ + libglu1-mesa-dev \ + libgtk-3-dev \ + liblapack-dev \ + libssl-dev \ + libusb-1.0-0-dev \ + libv4l-dev \ + libx11-dev \ + pkg-config \ + && apt-get clean + +# Set Locale +RUN locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 && \ + export LANG=en_US.UTF-8 + +ENV USERNAME=vispci + +RUN useradd -U --uid $USER_UID -ms /bin/bash ${USERNAME} \ + && echo "${USERNAME}:${USERNAME}" | chpasswd \ + && adduser ${USERNAME} sudo \ + && echo "$USERNAME ALL=NOPASSWD: ALL" >> /etc/sudoers.d/${USERNAME} \ + && adduser ${USERNAME} video + +# Commands below are now run as normal user +USER ${USERNAME} + +# When running a container start in the home folder +WORKDIR /home/$USERNAME +ENV HOME=/home/$USERNAME + +# Some apps don't show controls without this +ENV QT_X11_NO_MITSHM=1 + +# Create folder for 3rd parties +RUN mkdir -p ${HOME}/visp-ws/3rdparty + +# Install OpenBLAS from source +RUN cd ${HOME}/visp-ws/3rdparty \ + && GIT_ADDRESS="https://github.com/xianyi/OpenBLAS.git" \ + && LATEST_TAG=`eval $FUNCTION_GET_LATEST` \ + && git clone --depth 1 --branch $LATEST_TAG $GIT_ADDRESS \ + && cd OpenBLAS \ + && mkdir install \ + && make -j$(nproc) \ + && make -j$(nproc) install PREFIX=$(pwd)/install + +ENV OpenBLAS_HOME=${HOME}/visp-ws/3rdparty/OpenBLAS/install + +# Install VTK from source +ENV GIT_CLONE_PROTECTION_ACTIVE=false +RUN cd ${HOME}/visp-ws/3rdparty \ + && GIT_ADDRESS="https://github.com/Kitware/VTK.git" \ + && LATEST_TAG=`eval $FUNCTION_GET_LATEST` \ + && git clone --recursive --depth 1 --branch $LATEST_TAG $GIT_ADDRESS \ + && cd VTK \ + && mkdir build && cd build && mkdir install \ + && cmake .. -DVTK_ANDROID_BUILD=OFF -DVTK_BUILD_DOCUMENTATION=OFF -DVTK_BUILD_EXAMPLES=OFF -DVTK_BUILD_EXAMPLES=OFF \ + -DCMAKE_BUILD_TYPE=Release -DVTK_GROUP_ENABLE_Imaging=DONT_WANT -DVTK_GROUP_ENABLE_MPI=DONT_WANT \ + -DVTK_GROUP_ENABLE_Web=DONT_WANT -DCMAKE_INSTALL_PREFIX=${HOME}/visp-ws/3rdparty/VTK/build/install \ + && make -j$(nproc) install + +ENV VTK_DIR=${HOME}/visp-ws/3rdparty/VTK/build/install + +# Install OpenCV from source +RUN cd ${HOME}/visp-ws/3rdparty \ + && GIT_ADDRESS="https://github.com/opencv/opencv.git" \ + && LATEST_TAG=`eval $FUNCTION_GET_LATEST` \ + && git clone --depth 1 --branch $LATEST_TAG $GIT_ADDRESS \ + && mkdir opencv/build \ + && cd opencv/build \ + && mkdir install \ + && cmake .. -DBUILD_EXAMPLES=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=${HOME}/visp-ws/3rdparty/opencv/build/install \ + && make -j$(nproc) install + +ENV OpenCV_DIR=${HOME}/visp-ws/3rdparty/opencv/build/install + +# Install Intel® RealSense™ SDK +RUN cd ${HOME}/visp-ws/3rdparty \ + && GIT_ADDRESS="https://github.com/IntelRealSense/librealsense.git" \ + && LATEST_TAG=`eval $FUNCTION_GET_LATEST` \ + && git clone --depth 1 --branch $LATEST_TAG $GIT_ADDRESS \ + && mkdir librealsense/build \ + && cd librealsense/build \ + && mkdir install \ + && cmake .. -DBUILD_EXAMPLES=OFF -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=${HOME}/visp-ws/3rdparty/librealsense/build/install \ + && make -j$(nproc) install + +ENV REALSENSE2_DIR=${HOME}/visp-ws/3rdparty/librealsense/build/install + +# Build PCL from source +RUN cd ${HOME}/visp-ws/3rdparty \ + && GIT_ADDRESS="https://github.com/PointCloudLibrary/pcl.git" \ + && LATEST_TAG=`eval $FUNCTION_GET_LATEST` \ + && git clone --depth 1 --branch $LATEST_TAG $GIT_ADDRESS \ + && mkdir pcl/build \ + && cd pcl/build \ + && mkdir install \ + && cmake .. -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DPCL_DISABLE_GPU_TESTS=ON -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=${HOME}/visp-ws/3rdparty/pcl/build/install \ + && make -j10 install + +ENV PCL_DIR=${HOME}/visp-ws/3rdparty/pcl/build/install + +# Install visp-images +RUN mkdir -p ${HOME}/visp-ws \ + && cd ${HOME}/visp-ws \ + && git clone https://github.com/lagadic/visp-images.git \ + && echo "export VISP_WS=${HOME}/visp-ws" >> ${HOME}/.bashrc \ + && echo "export VISP_INPUT_IMAGE_PATH=${HOME}/visp-ws/visp-images" >> ${HOME}/.bashrc + +# Download ViSP fork +RUN cd ${HOME}/visp-ws \ + && git clone ${GIT_URL} ${GIT_BRANCH_CMD} + +# Build visp +RUN cd ${HOME}/visp-ws \ + && mkdir visp-build \ + && cd visp-build \ + && cmake ../visp -DCMAKE_INSTALL_PREFIX=${HOME}/visp-ws/visp-build/install \ + && make -j$(nproc) developer_scripts \ + && make -j$(nproc) install + +CMD ["/bin/bash"] diff --git a/doc/tutorial/detection_dnn/tutorial-detection-tensorrt.dox b/doc/tutorial/detection_dnn/tutorial-detection-tensorrt.dox index aff7c62955..2660c603e7 100644 --- a/doc/tutorial/detection_dnn/tutorial-detection-tensorrt.dox +++ b/doc/tutorial/detection_dnn/tutorial-detection-tensorrt.dox @@ -201,7 +201,7 @@ Include TensorRT header files. \subsection preprocessing Pre-processing Prepare input image for inference with OpenCV. First, upload image to GPU, resize it to match model's input dimensions, normalize with `meanR` `meanG` `meanB` being -the values used for mean substraction. +the values used for mean subtraction. Transform data to tensor (copy data to channel by channel to `gpu_input`). In the case of `ssd_mobilenet.onnx`, the input dimension is 1x3x300x300. \snippet tutorial-dnn-tensorrt-live.cpp Preprocess image diff --git a/doc/tutorial/docker/tutorial-install-docker.dox b/doc/tutorial/docker/tutorial-install-docker.dox index e7ced95c6f..0faf3f137f 100644 --- a/doc/tutorial/docker/tutorial-install-docker.dox +++ b/doc/tutorial/docker/tutorial-install-docker.dox @@ -77,10 +77,10 @@ In [Docker Hub](https://hub.docker.com/repository/docker/vispci/vispci), we prov images that can be used on an Ubuntu or macOS host with ViSP already built. Instead, there is also the possibility to build docker images from a `Dockerfile` following instruction given in \ref docker_visp_build section. -\subsection docker_visp_pull_ubuntu_18_04 Pull Ubuntu 18.04 image +\subsection docker_visp_pull_ubuntu_22_04 Pull Ubuntu 22.04 image \verbatim -$ docker pull vispci/vispci:ubuntu-18.04 +$ docker pull vispci/vispci:ubuntu-22.04 \endverbatim \subsection docker_visp_pull_ubuntu_20_04 Pull Ubuntu 20.04 image @@ -89,10 +89,10 @@ $ docker pull vispci/vispci:ubuntu-18.04 $ docker pull vispci/vispci:ubuntu-20.04 \endverbatim -\subsection docker_visp_pull_ubuntu_22_04 Pull Ubuntu 22.04 image +\subsection docker_visp_pull_ubuntu_18_04 Pull Ubuntu 18.04 image \verbatim -$ docker pull vispci/vispci:ubuntu-22.04 +$ docker pull vispci/vispci:ubuntu-18.04 \endverbatim \section docker_visp_build Build docker image from Dockerfile @@ -101,25 +101,32 @@ We suppose here that you cloned ViSP from github in your workspace. Change directory to access the `Dockerfile` and build the corresponding docker image -\subsection docker_visp_build_ubuntu_18_04 Build Ubuntu 18.04 image +\subsection docker_visp_build_ubuntu_24_04 Build Ubuntu 24.04 image \verbatim -$ cd $VISP_WS/visp/ci/docker/ubuntu-18.04 -$ docker build -t vispci/vispci:ubuntu-18.04 . +$ cd $VISP_WS/visp/docker/ubuntu-24.04 +$ docker build -t vispci/vispci:ubuntu-24.04 . +\endverbatim + +\subsection docker_visp_build_ubuntu_22_04 Build Ubuntu 22.04 image + +\verbatim +$ cd $VISP_WS/visp/docker/ubuntu-22.04 +$ docker build -t vispci/vispci:ubuntu-22.04 . \endverbatim \subsection docker_visp_build_ubuntu_20_04 Build Ubuntu 20.04 image \verbatim -$ cd $VISP_WS/visp/ci/docker/ubuntu-20.04 +$ cd $VISP_WS/visp/docker/ubuntu-20.04 $ docker build -t vispci/vispci:ubuntu-20.04 . \endverbatim -\subsection docker_visp_build_ubuntu_22_04 Build Ubuntu 22.04 image +\subsection docker_visp_build_ubuntu_18_04 Build Ubuntu 18.04 image \verbatim -$ cd $VISP_WS/visp/ci/docker/ubuntu-22.04 -$ docker build -t vispci/vispci:ubuntu-22.04 . +$ cd $VISP_WS/visp/docker/ubuntu-18.04 +$ docker build -t vispci/vispci:ubuntu-18.04 . \endverbatim \section docker_visp_start Start ViSP container diff --git a/doc/tutorial/imgproc/tutorial-imgproc-count-coins.dox b/doc/tutorial/imgproc/tutorial-imgproc-count-coins.dox index 123dd01bdb..cc8431e3a6 100644 --- a/doc/tutorial/imgproc/tutorial-imgproc-count-coins.dox +++ b/doc/tutorial/imgproc/tutorial-imgproc-count-coins.dox @@ -71,7 +71,7 @@ The fill holes algorithm is basic: \image html img-tutorial-count-coins-mask.png "Left: binary image, right: result of the flood fill operation" -- substract the flood fill image from a white image to get only the holes +- subtract the flood fill image from a white image to get only the holes \image html img-tutorial-count-coins-white-holes.png "Top left: white image, bottom left: flood fill image, right: I_holes = I_white - I_flood_fill" diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox index f8e0bb1b8e..9725d28c1c 100644 --- a/doc/tutorial/misc/tutorial-ukf.dox +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -357,17 +357,17 @@ Finally, we create the UKF and initialize its state: \snippet tutorial-ukf.cpp Init_UKF -If the internal state cannot use the standard addition and substraction, it would be necessary to write other methods: +If the internal state cannot use the standard addition and subtraction, it would be necessary to write other methods: - a method permitting to add two state vectors (see vpUnscentedKalman::setStateAddFunction), -- a method permitting to substract two state vectors (see vpUnscentedKalman::setStateResidualFunction), +- a method permitting to subtract two state vectors (see vpUnscentedKalman::setStateResidualFunction), - and a method permitting to compute the mean of several state vectors (see vpUnscentedKalman::setStateMeanFunction). If some commands are known to have an effect on the internal state, it would be necessary to write other methods: - a method for the effects of the commands on the state, without knowing the state (see vpUnscentedKalman::setCommandOnlyFunction), - and/or a method for the effects of the commands on the state, knowing the state (see vpUnscentedKalman::setCommandStateFunction). -If the measurement space cannot use the standard addition and substraction, it would be necessary to write other methods: -- a method permitting to substract two measurement vectors (see vpUnscentedKalman::setMeasurementResidualFunction), +If the measurement space cannot use the standard addition and subtraction, it would be necessary to write other methods: +- a method permitting to subtract two measurement vectors (see vpUnscentedKalman::setMeasurementResidualFunction), - and a method permitting to compute the mean of several measurement vectors (see vpUnscentedKalman::setMeasurementMeanFunction). \subsubsection tuto-ukf-tutorial-explained-initgui Details on the initialization of the Graphical User Interface diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp index adbdb4ef2a..4b5d06ee92 100644 --- a/example/kalman/ukf-nonlinear-complex-example.cpp +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -28,7 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. */ -/** \example ukf-nonlinear-complex-example.cpp +/** + * \example ukf-nonlinear-complex-example.cpp * Example of a complex non-linear use-case of the Unscented Kalman Filter (UKF). * The system we are interested in is a 4-wheel robot, moving at a low velocity. * As such, it can be modeled using a bicycle model. @@ -118,7 +119,7 @@ double normalizeAngle(const double &angle) */ vpColVector measurementMean(const std::vector &measurements, const std::vector &wm) { - const unsigned int nbPoints = measurements.size(); + const unsigned int nbPoints = static_cast(measurements.size()); const unsigned int sizeMeasurement = measurements[0].size(); const unsigned int nbLandmarks = sizeMeasurement / 2; vpColVector mean(sizeMeasurement, 0.); @@ -138,16 +139,16 @@ vpColVector measurementMean(const std::vector &measurements, const } /** - * \brief Compute the substraction between two vectors expressed in the measurement space, + * \brief Compute the subtraction between two vectors expressed in the measurement space, * such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... * - * \param[in] meas Measurement to which we must substract something. - * \param[in] toSubstract The something we must substract. - * \return vpColVector \b meas - \b toSubstract . + * \param[in] meas Measurement to which we must subtract something. + * \param[in] toSubtract The something we must subtract. + * \return vpColVector \b meas - \b toSubtract . */ -vpColVector measurementResidual(const vpColVector &meas, const vpColVector &toSubstract) +vpColVector measurementResidual(const vpColVector &meas, const vpColVector &toSubtract) { - vpColVector res = meas - toSubstract; + vpColVector res = meas - toSubtract; unsigned int nbMeasures = res.size(); for (unsigned int i = 1; i < nbMeasures; i += 2) { res[i] = normalizeAngle(res[i]); @@ -180,7 +181,7 @@ vpColVector stateAdd(const vpColVector &state, const vpColVector &toAdd) vpColVector stateMean(const std::vector &states, const std::vector &wm) { vpColVector mean(3, 0.); - unsigned int nbPoints = states.size(); + unsigned int nbPoints = static_cast(states.size()); double sumCos = 0.; double sumSin = 0.; for (unsigned int i = 0; i < nbPoints; ++i) { @@ -194,16 +195,16 @@ vpColVector stateMean(const std::vector &states, const std::vector< } /** - * \brief Compute the substraction between two vectors expressed in the state space, + * \brief Compute the subtraction between two vectors expressed in the state space, * such as v[0] = x ; v[1] = y; v[2] = heading . * - * \param[in] state State to which we must substract something. - * \param[in] toSubstract The something we must substract. - * \return vpColVector \b state - \b toSubstract . + * \param[in] state State to which we must subtract something. + * \param[in] toSubtract The something we must subtract. + * \return vpColVector \b state - \b toSubtract . */ -vpColVector stateResidual(const vpColVector &state, const vpColVector &toSubstract) +vpColVector stateResidual(const vpColVector &state, const vpColVector &toSubtract) { - vpColVector res = state - toSubstract; + vpColVector res = state - toSubtract; res[2] = normalizeAngle(res[2]); return res; } @@ -474,7 +475,7 @@ class vpLandmarksGrid */ vpColVector state_to_measurement(const vpColVector &chi) { - unsigned int nbLandmarks = m_landmarks.size(); + unsigned int nbLandmarks = static_cast(m_landmarks.size()); vpColVector measurements(2*nbLandmarks); for (unsigned int i = 0; i < nbLandmarks; ++i) { vpColVector landmarkMeas = m_landmarks[i].state_to_measurement(chi); @@ -494,7 +495,7 @@ class vpLandmarksGrid */ vpColVector measureGT(const vpColVector &pos) { - unsigned int nbLandmarks = m_landmarks.size(); + unsigned int nbLandmarks = static_cast(m_landmarks.size()); vpColVector measurements(2*nbLandmarks); for (unsigned int i = 0; i < nbLandmarks; ++i) { vpColVector landmarkMeas = m_landmarks[i].measureGT(pos); @@ -514,7 +515,7 @@ class vpLandmarksGrid */ vpColVector measureWithNoise(const vpColVector &pos) { - unsigned int nbLandmarks = m_landmarks.size(); + unsigned int nbLandmarks = static_cast(m_landmarks.size()); vpColVector measurements(2*nbLandmarks); for (unsigned int i = 0; i < nbLandmarks; ++i) { vpColVector landmarkMeas = m_landmarks[i].measureWithNoise(pos); @@ -560,10 +561,10 @@ int main(const int argc, const char *argv[]) , vpLandmarkMeasurements(20, 5, sigmaRange, sigmaBearing) , vpLandmarkMeasurements(0, 30, sigmaRange, sigmaBearing) , vpLandmarkMeasurements(50, 30, sigmaRange, sigmaBearing) - , vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) }; // Vector of landmarks constituing the grid - const unsigned int nbLandmarks = landmarks.size(); // Number of landmarks constituing the grid + , vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) }; // Vector of landmarks constituting the grid + const unsigned int nbLandmarks = static_cast(landmarks.size()); // Number of landmarks constituting the grid std::vector cmds = generateCommands(); - const unsigned int nbCmds = cmds.size(); + const unsigned int nbCmds = static_cast(cmds.size()); // Initialize the attributes of the UKF std::shared_ptr drawer = std::make_shared(3, 0.1, 2., 0, stateResidual, stateAdd); diff --git a/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp index 067ad1b6ad..e6dce7377f 100644 --- a/example/kalman/ukf-nonlinear-example.cpp +++ b/example/kalman/ukf-nonlinear-example.cpp @@ -75,7 +75,7 @@ where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. - Additionnally, the addition and substraction of angles must be carefully done, as the result + Additionnally, the addition and subtraction of angles must be carefully done, as the result must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use the interval \f$[- \pi ; \pi ]\f$ . */ @@ -143,7 +143,7 @@ double normalizeAngle(const double &angle) */ vpColVector measurementMean(const std::vector &measurements, const std::vector &wm) { - const unsigned int nbPoints = measurements.size(); + const unsigned int nbPoints = static_cast(measurements.size()); const unsigned int sizeMeasurement = measurements[0].size(); vpColVector mean(sizeMeasurement, 0.); double sumCos(0.); @@ -160,16 +160,16 @@ vpColVector measurementMean(const std::vector &measurements, const } /** - * \brief Compute the substraction between two vectors expressed in the measurement space, + * \brief Compute the subtraction between two vectors expressed in the measurement space, * such as v[0] = range ; v[1] = elevation_angle * - * \param[in] meas Measurement to which we must substract something. - * \param[in] toSubstract The something we must substract. - * \return vpColVector \b meas - \b toSubstract . + * \param[in] meas Measurement to which we must subtract something. + * \param[in] toSubtract The something we must subtract. + * \return vpColVector \b meas - \b toSubtract . */ -vpColVector measurementResidual(const vpColVector &meas, const vpColVector &toSubstract) +vpColVector measurementResidual(const vpColVector &meas, const vpColVector &toSubtract) { - vpColVector res = meas - toSubstract; + vpColVector res = meas - toSubtract; res[1] = normalizeAngle(res[1]); return res; } diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index f373011ff9..4039856615 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -204,21 +204,22 @@ class VISP_EXPORT vpImageFilter // between 0 and 255 for an vpImage float scaleX = 1.f; float scaleY = 1.f; + const unsigned int val2 = 2U; if (filteringType == CANNY_GBLUR_SOBEL_FILTERING) { if (computeDx) { - scaleX = static_cast(vpImageFilter::getSobelKernelX(gradientFilterX.data, (apertureGradient - 1) / 2)); + scaleX = static_cast(vpImageFilter::getSobelKernelX(gradientFilterX.data, (apertureGradient - 1) / val2)); } if (computeDy) { - scaleY = static_cast(vpImageFilter::getSobelKernelY(gradientFilterY.data, (apertureGradient - 1) / 2)); + scaleY = static_cast(vpImageFilter::getSobelKernelY(gradientFilterY.data, (apertureGradient - 1) / val2)); } } else if (filteringType == CANNY_GBLUR_SCHARR_FILTERING) { if (computeDx) { - scaleX = static_cast(vpImageFilter::getScharrKernelX(gradientFilterX.data, (apertureGradient - 1) / 2)); + scaleX = static_cast(vpImageFilter::getScharrKernelX(gradientFilterX.data, (apertureGradient - 1) / val2)); } if (computeDy) { - scaleY = static_cast(vpImageFilter::getScharrKernelY(gradientFilterY.data, (apertureGradient - 1) / 2)); + scaleY = static_cast(vpImageFilter::getScharrKernelY(gradientFilterY.data, (apertureGradient - 1) / val2)); } } @@ -375,13 +376,16 @@ class VISP_EXPORT vpImageFilter float accu = 0; float t = upperThresholdRatio * totalNbPixels; float bon = 0; - for (unsigned int i = 0; i < nbBins; ++i) { + unsigned int i = 0; + bool notFound = true; + while ((i < nbBins) && notFound) { float tf = static_cast(hist[i]); accu = accu + tf; if (accu > t) { bon = static_cast(i); - break; + notFound = false; } + ++i; } float upperThresh = std::max(bon, 1.f); lowerThresh = lowerThresholdRatio * bon; @@ -397,8 +401,9 @@ class VISP_EXPORT vpImageFilter */ template static double derivativeFilterX(const vpImage &I, unsigned int r, unsigned int c) { - return ((2047.0 * static_cast(I[r][c + 1] - I[r][c - 1])) + (913.0 * static_cast(I[r][c + 2] - I[r][c - 2])) + - (112.0 * static_cast(I[r][c + 3] - I[r][c - 3]))) / 8418.0; + const int val1 = 1, val2 = 2, val3 = 3; + return ((2047.0 * static_cast(I[r][c + val1] - I[r][c - val1])) + (913.0 * static_cast(I[r][c + val2] - I[r][c - val2])) + + (112.0 * static_cast(I[r][c + val3] - I[r][c - val3]))) / 8418.0; } /*! @@ -410,8 +415,9 @@ class VISP_EXPORT vpImageFilter */ template static double derivativeFilterY(const vpImage &I, unsigned int r, unsigned int c) { - return ((2047.0 * static_cast(I[r + 1][c] - I[r - 1][c])) + (913.0 * static_cast(I[r + 2][c] - I[r - 2][c])) + - (112.0 * static_cast(I[r + 3][c] - I[r - 3][c]))) / 8418.0; + const int val1 = 1, val2 = 2, val3 = 3; + return ((2047.0 * static_cast(I[r + val1][c] - I[r - val1][c])) + (913.0 * static_cast(I[r + val2][c] - I[r - val2][c])) + + (112.0 * static_cast(I[r + val3][c] - I[r - val3][c]))) / 8418.0; } /*! @@ -674,11 +680,13 @@ class VISP_EXPORT vpImageFilter static inline unsigned char filterGaussXPyramidal(const vpImage &I, unsigned int i, unsigned int j) { - return static_cast(((1. * I[i][j - 2]) + (4. * I[i][j - 1]) + (6. * I[i][j]) + (4. * I[i][j + 1]) + (1. * I[i][j + 2])) / 16.); + const int val2 = 2; + return static_cast(((1. * I[i][j - val2]) + (4. * I[i][j - 1]) + (6. * I[i][j]) + (4. * I[i][j + 1]) + (1. * I[i][j + val2])) / 16.); } static inline unsigned char filterGaussYPyramidal(const vpImage &I, unsigned int i, unsigned int j) { - return static_cast(((1. * I[i - 2][j]) + (4. * I[i - 1][j]) + (6. * I[i][j]) + (4. * I[i + 1][j]) + (1. * I[i + 2][j])) / 16.); + const int val2 = 2; + return static_cast(((1. * I[i - val2][j]) + (4. * I[i - 1][j]) + (6. * I[i][j]) + (4. * I[i + 1][j]) + (1. * I[i + val2][j])) / 16.); } template @@ -774,13 +782,14 @@ class VISP_EXPORT vpImageFilter const unsigned int stop = (size - 1) / 2; const unsigned int width = I.getWidth(); FilterType result = static_cast(0.); + const unsigned int twice = 2; for (unsigned int i = 1; i <= stop; ++i) { if ((c + i) < width) { result += filter[i] * static_cast(I[r][c + i] + I[r][c - i]); } else { - result += filter[i] * static_cast(I[r][((2 * width) - c) - i - 1] + I[r][c - i]); + result += filter[i] * static_cast(I[r][((twice * width) - c) - i - 1] + I[r][c - i]); } } return result + (filter[0] * static_cast(I[r][c])); @@ -882,13 +891,13 @@ class VISP_EXPORT vpImageFilter const unsigned int height = I.getHeight(); const unsigned int stop = (size - 1) / 2; FilterType result = static_cast(0.); - + const unsigned int twiceHeight = 2 * height; for (unsigned int i = 1; i <= stop; ++i) { if ((r + i) < height) { result += filter[i] * static_cast(I[r + i][c] + I[r - i][c]); } else { - result += filter[i] * static_cast(I[((2 * height) - r) - i - 1][c] + I[r - i][c]); + result += filter[i] * static_cast(I[(twiceHeight - r) - i - 1][c] + I[r - i][c]); } } return result + (filter[0] * static_cast(I[r][c])); @@ -934,13 +943,14 @@ class VISP_EXPORT vpImageFilter */ template static double gaussianFilter(const vpImage &fr, unsigned int r, unsigned int c) { + const int val2 = 2; return ((15.0 * fr[r][c]) + (12.0 * (fr[r - 1][c] + fr[r][c - 1] + fr[r + 1][c] + fr[r][c + 1])) + (9.0 * (fr[r - 1][c - 1] + fr[r + 1][c - 1] + fr[r - 1][c + 1] + fr[r + 1][c + 1])) + - (5.0 * (fr[r - 2][c] + fr[r][c - 2] + fr[r + 2][c] + fr[r][c + 2])) + - (4.0 * (fr[r - 2][c + 1] + fr[r - 2][c - 1] + fr[r - 1][c - 2] + fr[r + 1][c - 2] + fr[r + 2][c - 1] + - fr[r + 2][c + 1] + fr[r - 1][c + 2] + fr[r + 1][c + 2])) + - (2.0 * (fr[r - 2][c - 2] + fr[r + 2][c - 2] + fr[r - 2][c + 2] + fr[r + 2][c + 2]))) / 159.0; + (5.0 * (fr[r - val2][c] + fr[r][c - val2] + fr[r + val2][c] + fr[r][c + val2])) + + (4.0 * (fr[r - val2][c + 1] + fr[r - val2][c - 1] + fr[r - 1][c - val2] + fr[r + 1][c - val2] + fr[r + val2][c - 1] + + fr[r + val2][c + 1] + fr[r - 1][c + val2] + fr[r + 1][c + val2])) + + (2.0 * (fr[r - val2][c - val2] + fr[r + val2][c - val2] + fr[r - val2][c + val2] + fr[r + val2][c + val2]))) / 159.0; } // Gaussian pyramid operation static void getGaussPyramidal(const vpImage &I, vpImage &GI); @@ -966,7 +976,8 @@ class VISP_EXPORT vpImageFilter template static void getGaussianKernel(FilterType *filter, unsigned int size, FilterType sigma = 0., bool normalize = true) { - if ((size % 2) != 1) { + const unsigned int mod2 = 2; + if ((size % mod2) != 1) { throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size")); } @@ -984,8 +995,9 @@ class VISP_EXPORT vpImageFilter if (normalize) { // renormalization FilterType sum = 0; + const unsigned int val2 = 2U; for (int i = 1; i <= middle; ++i) { - sum += 2 * filter[i]; + sum += val2 * filter[i]; } sum += filter[0]; @@ -1012,7 +1024,8 @@ class VISP_EXPORT vpImageFilter template static void getGaussianDerivativeKernel(FilterType *filter, unsigned int size, FilterType sigma = 0., bool normalize = true) { - if ((size % 2) != 1) { + const unsigned int mod2 = 2; + if ((size % mod2) != 1) { throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size")); } @@ -1020,7 +1033,8 @@ class VISP_EXPORT vpImageFilter sigma = static_cast((size - 1) / 6.0); } - int middle = (static_cast(size) - 1) / 2; + const int half = 2; + int middle = (static_cast(size) - 1) / half; FilterType sigma2 = static_cast(vpMath::sqr(sigma)); FilterType coef_1 = static_cast(1. / (sigma * sqrt(2. * M_PI))); FilterType coef_1_over_2 = coef_1 / static_cast(2.); @@ -1243,14 +1257,15 @@ class VISP_EXPORT vpImageFilter template inline static FilterType getScharrKernelX(FilterType *filter, unsigned int size) { + const unsigned int actualKernelSize = (size * 2) + 1; if (size != 1) { // Size = 1 => kernel_size = 2*1 + 1 = 3 std::stringstream errMsg; - errMsg << "Cannot get Scharr kernel of size " << ((size * 2) + 1) << " != 3"; + errMsg << "Cannot get Scharr kernel of size " << actualKernelSize << " != 3"; throw vpException(vpException::dimensionError, errMsg.str()); } - vpArray2D ScharrY((size * 2) + 1, (size * 2) + 1); + vpArray2D ScharrY(actualKernelSize, actualKernelSize); FilterType norm = getScharrKernelY(ScharrY.data, size); memcpy(filter, ScharrY.t().data, ScharrY.getRows() * ScharrY.getCols() * sizeof(FilterType)); return norm; @@ -1277,7 +1292,8 @@ class VISP_EXPORT vpImageFilter } const unsigned int kernel_size = (size * 2) + 1; - if (kernel_size == 3) { + const unsigned int kernel3 = 3; + if (kernel_size == kernel3) { memcpy(filter, ScharrY3x3, kernel_size * kernel_size * sizeof(FilterType)); return static_cast(1.0 / 32.0); } @@ -1295,14 +1311,16 @@ class VISP_EXPORT vpImageFilter template inline static FilterType getSobelKernelX(FilterType *filter, unsigned int size) { + const unsigned int maxSize = 20; if (size == 0) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size 0!"); } - if (size > 20) { + if (size > maxSize) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size > 20!"); } - vpArray2D SobelY((size * 2) + 1, (size * 2) + 1); + const unsigned int kernel_size = (size * 2) + 1; + vpArray2D SobelY(kernel_size, kernel_size); FilterType norm = getSobelKernelY(SobelY.data, size); memcpy(filter, SobelY.t().data, SobelY.getRows() * SobelY.getCols() * sizeof(FilterType)); return norm; @@ -1339,26 +1357,28 @@ class VISP_EXPORT vpImageFilter smoothingKernel[index_2][index_1] = 2.0; smoothingKernel[index_2][index_2] = 1.0; + const unsigned int maxSize = 20; if (size == 0) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size 0!"); } - if (size > 20) { + if (size > maxSize) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size > 20!"); } const unsigned int kernel_size = (size * 2) + 1; FilterType scale = static_cast(1. / 8.); // Scale to normalize Sobel3x3 - if (kernel_size == 3) { + const unsigned int kernel3 = 3, kernel5 = 5, kernel7 = 7; + if (kernel_size == kernel3) { memcpy(filter, SobelY3x3, kernel_size * kernel_size * sizeof(FilterType)); return scale; } scale *= static_cast(1. / 16.); // Sobel5x5 is the convolution of smoothingKernel, which needs 1/16 scale factor, with Sobel3x3 - if (kernel_size == 5) { + if (kernel_size == kernel5) { memcpy(filter, SobelY5x5, kernel_size * kernel_size * sizeof(FilterType)); return scale; } scale *= static_cast(1. / 16.); // Sobel7x7 is the convolution of smoothingKernel, which needs 1/16 scale factor, with Sobel5x5 - if (kernel_size == 7) { + if (kernel_size == kernel7) { memcpy(filter, SobelY7x7, kernel_size * kernel_size * sizeof(FilterType)); return scale; } diff --git a/modules/core/include/visp3/core/vpImageMorphology.h b/modules/core/include/visp3/core/vpImageMorphology.h index dab0ee754b..b73848a3c9 100644 --- a/modules/core/include/visp3/core/vpImageMorphology.h +++ b/modules/core/include/visp3/core/vpImageMorphology.h @@ -137,6 +137,39 @@ class VISP_EXPORT vpImageMorphology #endif private: + template + class vpPixelOperation + { + public: + vpPixelOperation() { } + + virtual T operator()(const T &, const T &) = 0; + }; + + template + class vpPixelOperationMax : public vpPixelOperation + { + public: + vpPixelOperationMax() { } + + virtual T operator()(const T &a, const T &b) VP_OVERRIDE + { + return std::max(a, b); + } + }; + + template + class vpPixelOperationMin : public vpPixelOperation + { + public: + vpPixelOperationMin() { } + + T operator()(const T &a, const T &b) VP_OVERRIDE + { + return std::min(a, b); + } + }; + /** * \brief Modify the image by applying the \b operation on each of its elements on a 3x3 * grid. @@ -149,7 +182,7 @@ class VISP_EXPORT vpImageMorphology * and vertical neighbors, or a 8-connexity, if we want to also take into account the diagonal neighbors. */ template - static void imageOperation(vpImage &I, const T &null_value, const T &(*operation)(const T &, const T &), const vpConnexityType &connexity = CONNEXITY_4); + static void imageOperation(vpImage &I, const T &null_value, vpPixelOperation *operation, const vpConnexityType &connexity = CONNEXITY_4); /** * \brief Modify the image by applying the \b operation on each of its elements on a \b size x \b size @@ -161,7 +194,7 @@ class VISP_EXPORT vpImageMorphology * \param[in] size Size of the kernel of the operation. */ template - static void imageOperation(vpImage &I, const T &(*operation)(const T &, const T &), const int &size = 3); + static void imageOperation(vpImage &I, vpPixelOperation *operation, const int &size = 3); }; @@ -323,7 +356,7 @@ void vpImageMorphology::dilatation(vpImage &I, Type value, Type value_out, } template -void vpImageMorphology::imageOperation(vpImage &I, const T &null_value, const T &(*operation)(const T &, const T &), const vpConnexityType &connexity) +void vpImageMorphology::imageOperation(vpImage &I, const T &null_value, vpPixelOperation *operation, const vpConnexityType &connexity) { const int width_in = I.getWidth(); const int height_in = I.getHeight(); @@ -343,7 +376,7 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &null_value, const for (int j = 0; j < width_in; ++j) { T value = null_value; for (int k = 0; k < nbOffset; ++k) { - value = operation(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]); + value = (*operation)(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]); } I[i][j] = value; @@ -359,7 +392,7 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &null_value, const for (int j = 0; j < width_in; ++j) { T value = null_value; for (int k = 0; k < nbOffset; ++k) { - value = operation(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]); + value = (*operation)(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]); } I[i][j] = value; @@ -394,8 +427,8 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &null_value, const template void vpImageMorphology::erosion(vpImage &I, const vpConnexityType &connexity) { - const T &(*operation)(const T & a, const T & b) = std::min; - vpImageMorphology::imageOperation(I, std::numeric_limits::max(), operation, connexity); + vpPixelOperationMin operation; + vpImageMorphology::imageOperation(I, std::numeric_limits::max(), &operation, connexity); } /*! @@ -424,12 +457,12 @@ void vpImageMorphology::erosion(vpImage &I, const vpConnexityType &connexity) template void vpImageMorphology::dilatation(vpImage &I, const vpConnexityType &connexity) { - const T &(*operation)(const T & a, const T & b) = std::max; - vpImageMorphology::imageOperation(I, std::numeric_limits::min(), operation, connexity); + vpPixelOperationMax operation; + vpImageMorphology::imageOperation(I, std::numeric_limits::min(), &operation, connexity); } template -void vpImageMorphology::imageOperation(vpImage &I, const T &(*operation)(const T &, const T &), const int &size) +void vpImageMorphology::imageOperation(vpImage &I, vpPixelOperation *operation, const int &size) { if ((size % 2) != 1) { throw(vpException(vpException::badValue, "Dilatation/erosion kernel must be odd.")); @@ -461,7 +494,7 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &(*operation)(cons } for (int r_iterator = r_iterator_start; r_iterator < r_iterator_stop; ++r_iterator) { for (int c_iterator = c_iterator_start; c_iterator < c_iterator_stop; ++c_iterator) { - value = operation(value, J[r + r_iterator][c + c_iterator]); + value = (*operation)(value, J[r + r_iterator][c + c_iterator]); } } I[r][c] = value; @@ -498,8 +531,8 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &(*operation)(cons template void vpImageMorphology::erosion(vpImage &I, const int &size) { - const T &(*operation)(const T & a, const T & b) = std::min; - vpImageMorphology::imageOperation(I, operation, size); + vpPixelOperationMin operation; + vpImageMorphology::imageOperation(I, &operation, size); } /** @@ -530,8 +563,8 @@ void vpImageMorphology::erosion(vpImage &I, const int &size) template void vpImageMorphology::dilatation(vpImage &I, const int &size) { - const T &(*operation)(const T & a, const T & b) = std::max; - vpImageMorphology::imageOperation(I, operation, size); + vpPixelOperationMax operation; + vpImageMorphology::imageOperation(I, &operation, size); } END_VISP_NAMESPACE #endif diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index fa4b5dac55..f7961ef99f 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -510,8 +510,9 @@ inline void vpImageTools::binarise(vpImage &I, unsigned char thre { if (useLUT) { // Construct the LUT - unsigned char lut[256]; - for (unsigned int i = 0; i < 256; ++i) { + const unsigned int sizeLut = 256; + unsigned char lut[sizeLut]; + for (unsigned int i = 0; i < sizeLut; ++i) { lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2); } @@ -876,7 +877,8 @@ template void vpImageTools::flip(vpImage &I) vpImage Ibuf; Ibuf.resize(1, width); - for (unsigned int i = 0; i < (height / 2); ++i) { + const unsigned int halfHeight = height / 2; + for (unsigned int i = 0; i < halfHeight; ++i) { memcpy(Ibuf.bitmap, I.bitmap + (i * width), width * sizeof(Type)); memcpy(I.bitmap + (i * width), I.bitmap + ((height - 1 - i) * width), width * sizeof(Type)); @@ -960,7 +962,8 @@ inline void vpImageTools::resizeBicubic(const vpImage &I, vpImage(reinterpret_cast(&p00)[c]), static_cast(reinterpret_cast(&p01)[c]), static_cast(reinterpret_cast(&p02)[c]), @@ -1022,7 +1025,8 @@ inline void vpImageTools::resizeBilinear(const vpImage &I, vpImage(reinterpret_cast(&I[v0][u0])[c]), static_cast(reinterpret_cast(&I[v1][u1])[c]), xFrac); float col1 = lerp(static_cast(reinterpret_cast(&I[v2][u2])[c]), @@ -1092,7 +1096,8 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpI #endif ) { - if ((I.getWidth() < 2) || (I.getHeight() < 2) || (Ires.getWidth() < 2) || (Ires.getHeight() < 2)) { + const unsigned int minWidth = 2, minHeight = 2; + if ((I.getWidth() < minWidth) || (I.getHeight() < minHeight) || (Ires.getWidth() < minWidth) || (Ires.getHeight() < minHeight)) { std::cerr << "Input or output image is too small!" << std::endl; return; } @@ -1105,17 +1110,14 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpI const float scaleY = I.getHeight() / static_cast(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; - + const int ires_height = static_cast(Ires.getHeight()); #if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } #pragma omp parallel for schedule(dynamic) #endif - /* - // int ires_height = static_cast(Ires.getHeight()); - */ - for (int i = 0; i < static_cast(Ires.getHeight()); ++i) { + for (int i = 0; i < ires_height; ++i) { const float v = ((i + half) * scaleY) - half; const float v0 = std::floor(v); const float yFrac = v - v0; @@ -1149,7 +1151,9 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &I, vpImage(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; - + const int ires_height = static_cast(Ires.getHeight()); #if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } #pragma omp parallel for schedule(dynamic) #endif - /* - // int ires_height = static_cast(Ires.getHeight()); - */ - for (int i = 0; i < static_cast(Ires.getHeight()); ++i) { + for (int i = 0; i < ires_height; ++i) { float v = ((i + half) * scaleY) - half; float yFrac = v - static_cast(v); @@ -1203,7 +1204,9 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &Ires #endif ) { - if ((I.getWidth() < 2) || (I.getHeight() < 2) || (Ires.getWidth() < 2) || (Ires.getHeight() < 2)) { + const unsigned int minWidth = 2, minHeight = 2; + + if ((I.getWidth() < minWidth) || (I.getHeight() < minHeight) || (Ires.getWidth() < minWidth) || (Ires.getHeight() < minHeight)) { std::cerr << "Input or output image is too small!" << std::endl; return; } @@ -1218,18 +1221,15 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &Ires const float scaleY = I.getHeight() / static_cast(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; - + const int ires_height = static_cast(Ires.getHeight()); #if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } #pragma omp parallel for schedule(dynamic) #endif - /* - // int ires_height = static_cast(Ires.getHeight()); - */ - for (int i = 0; i < static_cast(Ires.getHeight()); ++i) { - float v = (i + half) * scaleY - half; + for (int i = 0; i < ires_height; ++i) { + float v = ((i + half) * scaleY) - half; float yFrac = v - static_cast(v); unsigned int ires_width = static_cast(Ires.getWidth()); diff --git a/modules/core/include/visp3/core/vpImageTools_warp.h b/modules/core/include/visp3/core/vpImageTools_warp.h index e5d02a8655..7c4d0f14d6 100644 --- a/modules/core/include/visp3/core/vpImageTools_warp.h +++ b/modules/core/include/visp3/core/vpImageTools_warp.h @@ -55,7 +55,8 @@ template void vpImageTools::warpImage(const vpImage &src, const vpMatrix &T, vpImage &dst, const vpImageInterpolationType &interpolation, bool fixedPointArithmetic, bool pixelCenter) { - if (((T.getRows() != 2) && (T.getRows() != 3)) || (T.getCols() != 3)) { + const unsigned int expectedNbCols = 3, expectedNbRows1stOpt = 2, expectedNbRows2ndOpt = 3; + if (((T.getRows() != expectedNbRows1stOpt) && (T.getRows() != expectedNbRows2ndOpt)) || (T.getCols() != expectedNbCols)) { std::cerr << "Input transformation must be a (2x3) or (3x3) matrix." << std::endl; return; } @@ -474,6 +475,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s unsigned int dst_width = dst.getWidth(); int src_height = static_cast(src.getHeight()); int src_width = static_cast(src.getWidth()); + const unsigned char aChannelVal = 255; for (unsigned int i = 0; i < dst_height; ++i) { int64_t xi = a2_i64; int64_t yi = a5_i64; @@ -509,12 +511,12 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = (interpB_i64 >> (nbits * 2)) + ((interpB_i64 & 0xFFFFFFFFU) * precision_2); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (y_ < (src_height - 1)) { const vpRGBa val00 = src[y_][x_]; const vpRGBa val10 = src[y_ + 1][x_]; - const int64_t interpR_i64 = static_cast(t_1 * val00.R + t * val10.R); + const int64_t interpR_i64 = static_cast((t_1 * val00.R) + (t * val10.R)); const float interpR = (interpR_i64 >> nbits) + ((interpR_i64 & 0xFFFF) * precision_1); const int64_t interpG_i64 = static_cast((t_1 * val00.G) + (t * val10.G)); @@ -524,7 +526,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = (interpB_i64 >> nbits) + ((interpB_i64 & 0xFFFF) * precision_1); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (x_ < (src_width - 1)) { const vpRGBa val00 = src[y_][x_]; @@ -539,7 +541,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = (interpB_i64 >> nbits) + ((interpB_i64 & 0xFFFF) * precision_1); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else { dst[i][j] = src[y_][x_]; @@ -559,6 +561,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s unsigned int dst_width = dst.getWidth(); int src_height = static_cast(src.getHeight()); int src_width = static_cast(src.getWidth()); + const unsigned char aChannelVal = 255; for (unsigned int i = 0; i < dst_height; ++i) { int64_t xi = a2_i64; int64_t yi = a5_i64; @@ -595,7 +598,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = lerp(colB0, colB1, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (y_ < (src_height - 1)) { const vpRGBa val00 = src[y_][x_]; @@ -605,7 +608,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = lerp(val00.B, val10.B, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (x_ < (src_width - 1)) { const vpRGBa val00 = src[y_][x_]; @@ -615,7 +618,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = lerp(val00.B, val01.B, s); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else { dst[i][j] = src[y_][x_]; @@ -658,6 +661,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix unsigned int dst_width = dst.getWidth(); int src_height = static_cast(src.getHeight()); int src_width = static_cast(src.getWidth()); + const unsigned char aChannelVal = 255; for (unsigned int i = 0; i < dst_height; ++i) { for (unsigned int j = 0; j < dst_width; ++j) { double x = (a0 * (centerCorner ? (j + 0.5) : j)) + (a1 * (centerCorner ? (i + 0.5) : i)) + a2; @@ -696,7 +700,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const double interpB = lerp(colB0, colB1, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (y_lower < (src_height - 1)) { const vpRGBa val00 = src[y_lower][x_lower]; @@ -706,7 +710,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const double interpB = lerp(val00.B, val10.B, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (x_lower < (src_width - 1)) { const vpRGBa val00 = src[y_lower][x_lower]; @@ -716,7 +720,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const double interpB = lerp(val00.B, val01.B, s); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else { dst[i][j] = src[y_lower][x_lower]; diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h index c4561b5c0b..adb42b7c0f 100644 --- a/modules/core/include/visp3/core/vpMath.h +++ b/modules/core/include/visp3/core/vpMath.h @@ -356,9 +356,11 @@ class VISP_EXPORT vpMath } static std::vector > computeRegularPointsOnSphere(unsigned int maxPoints); + + typedef vpHomogeneousMatrix(*LongLattToHomogeneous)(double lonDeg, double latDeg, double radius); static std::vector getLocalTangentPlaneTransformations(const std::vector > &lonlatVec, double radius, - vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius)); + LongLattToHomogeneous func); static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp); diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h index bf35979321..f14e76d531 100644 --- a/modules/core/include/visp3/core/vpMatrix.h +++ b/modules/core/include/visp3/core/vpMatrix.h @@ -581,6 +581,8 @@ class VISP_EXPORT vpMatrix : public vpArray2D int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const; int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const; #endif + + vpMatrix dampedInverse(const double &ratioOfMaxSvd = 1e-4) const; //@} //------------------------------------------------- diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index ae3d1fb9a6..7dc179896e 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -175,14 +175,26 @@ class VISP_EXPORT vpUnscentedKalman /** * \brief Function that computes either the equivalent of an addition or the equivalent - * of a substraction in the state space or in the measurement space. - * The first argument is the vector to which we must add/substract something - * and the second argument is the thing to be added/substracted. The return is the + * of a subtraction in the state space or in the measurement space. + * The first argument is the vector to which we must add/subtract something + * and the second argument is the thing to be added/subtracted. The return is the * result of this operation. */ typedef std::function vpAddSubFunction; /** +<<<<<<< HEAD +======= + * \brief Residual function, which computes either the equivalent of the subtraction in the + * state space or the equivalent of the subtraction in the measurement space. + * The first argument is the vector to which we must subtract something + * and the second argument is the thing to be subtracted. The return is the + * result of this "subtraction". + */ + typedef std::function vpAddFunction; + + /** +>>>>>>> master * \brief Construct a new vpUnscentedKalman object. * * \param[in] Q The covariance introduced by performing the prediction step. @@ -237,7 +249,7 @@ class VISP_EXPORT vpUnscentedKalman } /** - * \brief Set the measurement residual function to use when computing a substraction + * \brief Set the measurement residual function to use when computing a subtraction * in the measurement space. * * \param measResFunc The residual function to use. @@ -270,7 +282,7 @@ class VISP_EXPORT vpUnscentedKalman } /** - * \brief Set the state residual function to use when computing a substraction + * \brief Set the state residual function to use when computing a subtraction * in the state space. * * \param stateResFunc The residual function to use. @@ -384,15 +396,15 @@ class VISP_EXPORT vpUnscentedKalman } /** - * \brief Simple function to compute a residual, which just does \f$ \textbf{res} = \textbf{a} - \textbf{toSubstract} \f$ + * \brief Simple function to compute a residual, which just does \f$ \textbf{res} = \textbf{a} - \textbf{toSubtract} \f$ * - * \param[in] a Vector to which we must substract something. - * \param[in] toSubstract The something we must substract to \b a . - * \return vpColVector \f$ \textbf{res} = \textbf{a} - \textbf{toSubstract} \f$ + * \param[in] a Vector to which we must subtract something. + * \param[in] toSubtract The something we must subtract to \b a . + * \return vpColVector \f$ \textbf{res} = \textbf{a} - \textbf{toSubtract} \f$ */ - inline static vpColVector simpleResidual(const vpColVector &a, const vpColVector &toSubstract) + inline static vpColVector simpleResidual(const vpColVector &a, const vpColVector &toSubtract) { - vpColVector res = a - toSubstract; + vpColVector res = a - toSubtract; return res; } @@ -439,10 +451,10 @@ class VISP_EXPORT vpUnscentedKalman vpCommandOnlyFunction m_b; /*!< Function that permits to compute the effect of the commands on the prior, without knowledge of the state.*/ vpCommandStateFunction m_bx; /*!< Function that permits to compute the effect of the commands on the prior, with knowledge of the state.*/ vpMeanFunction m_measMeanFunc; /*!< Function to compute a weighted mean in the measurement space.*/ - vpAddSubFunction m_measResFunc; /*!< Function to compute a substraction in the measurement space.*/ + vpAddSubFunction m_measResFunc; /*!< Function to compute a subtraction in the measurement space.*/ vpAddSubFunction m_stateAddFunction; /*!< Function to compute an addition in the state space.*/ vpMeanFunction m_stateMeanFunc; /*!< Function to compute a weighted mean in the state space.*/ - vpAddSubFunction m_stateResFunc; /*!< Function to compute a substraction in the state space.*/ + vpAddSubFunction m_stateResFunc; /*!< Function to compute a subtraction in the state space.*/ /** * \brief Structure that stores the results of the unscented transform. diff --git a/modules/core/src/camera/vpCameraParameters.cpp b/modules/core/src/camera/vpCameraParameters.cpp index 75f83a825a..f8fdab20d0 100644 --- a/modules/core/src/camera/vpCameraParameters.cpp +++ b/modules/core/src/camera/vpCameraParameters.cpp @@ -497,8 +497,11 @@ bool vpCameraParameters::operator==(const vpCameraParameters &c) const } std::vector::const_iterator it1 = m_fovNormals.begin(); + std::vector::const_iterator it1_end = m_fovNormals.end(); std::vector::const_iterator it2 = c.m_fovNormals.begin(); - for (; (it1 != m_fovNormals.end()) && (it2 != c.m_fovNormals.end()); ++it1, ++it2) { + std::vector::const_iterator it2_end = c.m_fovNormals.end(); + + for (; (it1 != it1_end) && (it2 != it2_end); ++it1, ++it2) { if (*it1 != *it2) { return false; } diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp index b8d0b8751b..1e1e75cadd 100644 --- a/modules/core/src/camera/vpXmlParserCamera.cpp +++ b/modules/core/src/camera/vpXmlParserCamera.cpp @@ -308,9 +308,10 @@ class vpXmlParserCamera::Impl // if same name && same projection model && same image size camera already exists, we return SEQUENCE_OK // otherwise it is a new camera that need to be updated and we return SEQUENCE_OK bool same_name = (cam_name.empty() || (cam_name == camera_name_tmp)); - bool same_img_size = (abs(static_cast(im_width) - static_cast(image_width_tmp)) < allowedPixelDiffOnImageSize || im_width == 0) && - (abs(static_cast(im_height) - static_cast(image_height_tmp)) < allowedPixelDiffOnImageSize || im_height == 0) && - (test_subsampling_width) && (test_subsampling_height); + bool imWidthOk = (abs(static_cast(im_width) - static_cast(image_width_tmp)) < allowedPixelDiffOnImageSize) || (im_width == 0); + bool imHeightOk = (abs(static_cast(im_height) - static_cast(image_height_tmp)) < allowedPixelDiffOnImageSize) || (im_height == 0); + bool imSizeOk = imWidthOk && imHeightOk; + bool same_img_size = imSizeOk && test_subsampling_width && test_subsampling_height; if (same_name && same_img_size && same_proj_model) { back = SEQUENCE_OK; // Camera exists camera = cam_tmp; @@ -761,10 +762,13 @@ class vpXmlParserCamera::Impl } } } - if (!((cam_name == camera_name_tmp) && (im_width == image_width_tmp || im_width == 0) && - (im_height == image_height_tmp || im_height == 0) && - (subsampl_width == subsampling_width_tmp || subsampl_width == 0) && - (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) { + bool imHeightOK = (im_height == image_height_tmp) || (im_height == 0); + bool imWidthOK = (im_width == image_width_tmp) || (im_width == 0); + bool imSizeEqual = imHeightOK && imWidthOK; + bool subsampleHeightOK = (subsampl_height == subsampling_height_tmp) || (subsampl_height == 0); + bool subsampleWidthOK = (subsampl_width == subsampling_width_tmp) || (subsampl_width == 0); + bool subsampleOK = subsampleHeightOK && subsampleWidthOK; + if (!((cam_name == camera_name_tmp) && imSizeEqual && subsampleOK)) { back = SEQUENCE_ERROR; } return back; @@ -1134,7 +1138,6 @@ class vpXmlParserCamera::Impl node_tmp = node_model.append_child(LABEL_XML_V0); node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0(); - //, , , , std::vector distortion_coefs = camera.getKannalaBrandtDistortionCoefficients(); if (distortion_coefs.size() != requiredNbCoeff) { diff --git a/modules/core/src/display/vpDisplay_impl.h b/modules/core/src/display/vpDisplay_impl.h index 65f91861f2..3ba46b3fe4 100644 --- a/modules/core/src/display/vpDisplay_impl.h +++ b/modules/core/src/display/vpDisplay_impl.h @@ -450,13 +450,14 @@ void vp_display_display_polygon(const vpImage &I, const std::vectordisplayLine(vip[i], vip[(i + 1) % vip.size()], color, thickness); } } else { - for (unsigned int i = 1; i < vip.size(); ++i) { + for (unsigned int i = 1; i < vip_size; ++i) { (I.display)->displayLine(vip[i - 1], vip[i], color, thickness); } } @@ -571,8 +572,9 @@ template void vp_display_display_roi(const vpImage &I, const double roiwidth = floor(roi.getWidth()); double iheight = static_cast(I.getHeight()); double iwidth = static_cast(I.getWidth()); - - if (top < 0 || top > iheight || left < 0 || left > iwidth || top + roiheight > iheight || left + roiwidth > iwidth) { + bool vertNOK = (top < 0) || (top > iheight) || ((top + roiheight) > iheight); + bool horNOK = (left < 0) || (left > iwidth) || ((left + roiwidth) > iwidth); + if (vertNOK || horNOK) { throw(vpException(vpException::dimensionError, "Region of interest outside of the image")); } diff --git a/modules/core/src/display/vpDisplay_uchar.cpp b/modules/core/src/display/vpDisplay_uchar.cpp index 092ecc53b6..ed4deea4bb 100644 --- a/modules/core/src/display/vpDisplay_uchar.cpp +++ b/modules/core/src/display/vpDisplay_uchar.cpp @@ -286,9 +286,10 @@ void vpDisplay::displayDotLine(const vpImage &I, const std::list< } std::list::const_iterator it = ips.begin(); + std::list::const_iterator it_end = ips.end(); vpImagePoint ip_prev = *(++it); - for (; it != ips.end(); ++it) { + for (; it != it_end; ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_dot_line(I, ip_prev, *it, color, thickness); ip_prev = *it; @@ -541,9 +542,10 @@ void vpDisplay::displayLine(const vpImage &I, const std::list::const_iterator it = ips.begin(); + std::list::const_iterator it_end = ips.end(); vpImagePoint ip_prev = *(++it); - for (; it != ips.end(); ++it) { + for (; it != it_end; ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_line(I, ip_prev, *it, color, thickness); ip_prev = *it; diff --git a/modules/core/src/image/private/vpImageConvert_impl.h b/modules/core/src/image/private/vpImageConvert_impl.h index da2188b86a..493fa91074 100644 --- a/modules/core/src/image/private/vpImageConvert_impl.h +++ b/modules/core/src/image/private/vpImageConvert_impl.h @@ -123,7 +123,7 @@ void vp_createDepthHistogram(const vpImage &src_depth, vpImage(src_depth.getSize()); + const int src_depth_size = static_cast(src_depth.getSize()); #if defined(VISP_HAVE_OPENMP) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) int nThreads = omp_get_max_threads(); @@ -144,7 +144,7 @@ void vp_createDepthHistogram(const vpImage &src_depth, vpImage(src_depth.getSize()); ++i) { + for (int i = 0; i < src_depth_size; ++i) { ++histogram[static_cast(src_depth.bitmap[i])]; } #endif diff --git a/modules/core/src/image/vpImageCircle.cpp b/modules/core/src/image/vpImageCircle.cpp index 9ea2714dad..1e3ce0d3c7 100644 --- a/modules/core/src/image/vpImageCircle.cpp +++ b/modules/core/src/image/vpImageCircle.cpp @@ -577,11 +577,14 @@ void computeIntersTopRightBottom(const float &u_c, const float &v_c, const float float theta_u_max_bottom = crossing_theta_u_max.first; float u_umin_bottom = crossing_theta_u_min.second; float u_umax_bottom = crossing_theta_u_max.second; + bool crossOnceTopHor = (u_umin_top <= umax_roi) && (u_umax_top > umax_roi); + bool dontCrossVert = (v_vmin <= vmin_roi) && (v_vmax >= vmax_roi); + bool crossOnceBotHor = (u_umin_bottom <= umax_roi) && (u_umax_bottom > umax_roi); if ((u_umax_top <= umax_roi) && (u_umax_bottom <= umax_roi) && (v_vmin >= vmin_roi) && (v_vmax <= vmax_roi)) { // case intersection top + right + bottom twice delta_theta = (2.f * M_PI_FLOAT) - ((theta_u_min_top - theta_u_max_top) + (theta_v_min - theta_v_max) + (theta_u_max_bottom - theta_u_min_bottom)); } - else if ((u_umin_top <= umax_roi) && (u_umax_top > umax_roi) && (v_vmin <= vmin_roi) && (u_umin_bottom <= umax_roi) && (u_umax_bottom > umax_roi) && (v_vmax >= vmax_roi)) { + else if (crossOnceTopHor && crossOnceBotHor && dontCrossVert) { // case intersection top and bottom delta_theta = (theta_u_max_top - theta_u_max_bottom); } diff --git a/modules/core/src/image/vpImageConvert_yuv.cpp b/modules/core/src/image/vpImageConvert_yuv.cpp index 5206a557dc..3bd4ab23db 100644 --- a/modules/core/src/image/vpImageConvert_yuv.cpp +++ b/modules/core/src/image/vpImageConvert_yuv.cpp @@ -44,11 +44,12 @@ namespace { void vpSAT(int &c) { + const int val_255 = 255; if (c < 0) { c = 0; } - else { - const unsigned int val_255 = 255; + else if (c > val_255) { + c = val_255; } } @@ -73,13 +74,13 @@ void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsign unsigned char *d; int w, h; int r, g, b, cr, cg, cb, y1, y2; - const unsigned int val_2 = 2; - const unsigned int val_88 = 88; - const unsigned int val_128 = 128; - const unsigned int val_183 = 183; - const unsigned int val_256 = 256; - const unsigned int val_359 = 359; - const unsigned int val_454 = 454; + const int val_2 = 2; + const int val_88 = 88; + const int val_128 = 128; + const int val_183 = 183; + const int val_256 = 256; + const int val_359 = 359; + const int val_454 = 454; h = static_cast(height); w = static_cast(width); @@ -142,13 +143,13 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned unsigned char *d; int h, w; int r, g, b, cr, cg, cb, y1, y2; - const unsigned int val_2 = 2; - const unsigned int val_88 = 88; - const unsigned int val_128 = 128; - const unsigned int val_183 = 183; - const unsigned int val_256 = 256; - const unsigned int val_359 = 359; - const unsigned int val_454 = 454; + const int val_2 = 2; + const int val_88 = 88; + const int val_128 = 128; + const int val_183 = 183; + const int val_256 = 256; + const int val_359 = 359; + const int val_454 = 454; h = static_cast(height); w = static_cast(width); diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp index cb495deea5..ad9886cc0f 100644 --- a/modules/core/src/image/vpImageTools.cpp +++ b/modules/core/src/image/vpImageTools.cpp @@ -895,11 +895,12 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D & const vpArray2D &mapDu, const vpArray2D &mapDv, vpImage &Iundist) { Iundist.resize(I.getHeight(), I.getWidth()); + const int I_height = static_cast(I.getHeight()); #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif - for (int i_ = 0; i_ < static_cast(I.getHeight()); ++i_) { + for (int i_ = 0; i_ < I_height; ++i_) { const unsigned int i = static_cast(i_); unsigned int i_width = I.getWidth(); for (unsigned int j = 0; j < i_width; ++j) { @@ -911,7 +912,7 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D & float dv = mapDv[i][j]; if ((0 <= u_round) && (0 <= v_round) && (u_round < (static_cast(I.getWidth()) - 1)) && - (v_round < (static_cast(I.getHeight()) - 1))) { + (v_round < (I_height - 1))) { // process interpolation float col0 = lerp(I[v_round][u_round], I[v_round][u_round + 1], du); float col1 = lerp(I[v_round + 1][u_round], I[v_round + 1][u_round + 1], du); @@ -940,11 +941,11 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D &mapU, c const vpArray2D &mapDu, const vpArray2D &mapDv, vpImage &Iundist) { Iundist.resize(I.getHeight(), I.getWidth()); - + const int I_height = static_cast(I.getHeight()); #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif - for (int i = 0; i < static_cast(I.getHeight()); ++i) { + for (int i = 0; i < I_height; ++i) { #if defined(VISP_HAVE_SIMDLIB) SimdRemap(reinterpret_cast(I.bitmap), 4, I.getWidth(), I.getHeight(), i * I.getWidth(), mapU.data, mapV.data, mapDu.data, mapDv.data, reinterpret_cast(Iundist.bitmap)); @@ -960,7 +961,7 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D &mapU, c float dv = mapDv[i_][j]; if ((0 <= u_round) && (0 <= v_round) && (u_round < (static_cast(I.getWidth()) - 1)) - && (v_round < (static_cast(I.getHeight()) - 1))) { + && (v_round < (I_height - 1))) { // process interpolation float col0 = lerp(I[v_round][u_round].R, I[v_round][u_round + 1].R, du); float col1 = lerp(I[v_round + 1][u_round].R, I[v_round + 1][u_round + 1].R, du); diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp index cd5e66195b..6c9fc1738e 100644 --- a/modules/core/src/math/matrix/vpColVector.cpp +++ b/modules/core/src/math/matrix/vpColVector.cpp @@ -244,16 +244,16 @@ vpColVector::vpColVector(const vpMatrix &M) : vpArray2D(M.getRows(), 1) vpColVector::vpColVector(const std::vector &v) : vpArray2D(static_cast(v.size()), 1) { - size_t v_size = v.size(); - for (size_t i = 0; i < v_size; ++i) { + unsigned int v_size = static_cast(v.size()); + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; } } vpColVector::vpColVector(const std::vector &v) : vpArray2D(static_cast(v.size()), 1) { - size_t v_size = v.size(); - for (size_t i = 0; i < v_size; ++i) { + unsigned int v_size = static_cast(v.size()); + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = static_cast(v[i]); } } @@ -350,9 +350,9 @@ vpColVector &vpColVector::operator=(const vpMatrix &M) vpColVector &vpColVector::operator=(const std::vector &v) { - size_t v_size = v.size(); + unsigned int v_size = static_cast(v.size()); resize(v_size, false); - for (size_t i = 0; i < v_size; ++i) { + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; } return *this; @@ -360,9 +360,9 @@ vpColVector &vpColVector::operator=(const std::vector &v) vpColVector &vpColVector::operator=(const std::vector &v) { - size_t v_size = v.size(); + unsigned int v_size = static_cast(v.size()); resize(v_size, false); - for (size_t i = 0; i < v_size; ++i) { + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = static_cast(v[i]); } return *this; diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index a69882ce28..5111e52303 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -1455,7 +1455,8 @@ unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const kerAt.resize(nbcol - rank, nbcol); if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; ++j) { + unsigned int k = 0; + for (unsigned int j = 0; j < nbcol; ++j) { // if( v.col(j) in kernel and non zero ) if ((sv[j] <= (maxsv * svThreshold)) && (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { @@ -1524,7 +1525,8 @@ unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const kerA.resize(nbcol, nbcol - rank); if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; ++j) { + unsigned int k = 0; + for (unsigned int j = 0; j < nbcol; ++j) { // if( v.col(j) in kernel and non zero ) if (sv[j] <= (maxsv * svThreshold)) { for (unsigned int i = 0; i < nbcol; ++i) { diff --git a/modules/core/src/math/matrix/vpMatrix_pseudo_inverse.cpp b/modules/core/src/math/matrix/vpMatrix_pseudo_inverse.cpp index 4edbd3ed75..d6eee1f0f2 100644 --- a/modules/core/src/math/matrix/vpMatrix_pseudo_inverse.cpp +++ b/modules/core/src/math/matrix/vpMatrix_pseudo_inverse.cpp @@ -1010,4 +1010,29 @@ int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix #endif } +/*! + * Permits to compute an approximated inverse of a matrix that is ill-conditionned. + * If the matrix is well-conditionned, the damped inverse is close to the Moore-Penrose pseudo-inverse. + * + * The corresponding equation is the following, assuming that \f$ \textbf{A} \f$ + * is the matrix we want to compute the damped inverse: + * + * \f$ \textbf{A} \approx ( \lambda \textbf{I} + \textbf{A}^T \textbf{A})^{-1} \textbf{A}^T \f$ + * + * \param[in] ratioOfMaxSvd The ratio of the maximum singular value of \f$ M \f$ we want to set \f$ \lambda \f$. + * @return vpMatrix The damped inverse. + */ +vpMatrix vpMatrix::dampedInverse(const double &ratioOfMaxSvd) const +{ + vpColVector w; + vpMatrix V, Mcpy(*this); + Mcpy.svd(w, V); + double maxSingularValue = w.getMaxValue(); + vpMatrix I; + I.eye(this->colNum); + double lambda = ratioOfMaxSvd * maxSingularValue; + vpMatrix dampedInv = (lambda * I + this->transpose() * *this).inverseByLU()* this->transpose(); + return dampedInv; +} + END_VISP_NAMESPACE diff --git a/modules/core/src/math/matrix/vpRowVector.cpp b/modules/core/src/math/matrix/vpRowVector.cpp index 6b3d9bd6ab..ef1d0bcb67 100644 --- a/modules/core/src/math/matrix/vpRowVector.cpp +++ b/modules/core/src/math/matrix/vpRowVector.cpp @@ -98,9 +98,9 @@ vpRowVector &vpRowVector::operator=(const vpMatrix &M) */ vpRowVector &vpRowVector::operator=(const std::vector &v) { - size_t v_size = v.size(); + unsigned int v_size = static_cast(v.size()); resize(v_size); - for (size_t i = 0; i < v_size; ++i) { + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; } return *this; @@ -110,9 +110,9 @@ vpRowVector &vpRowVector::operator=(const std::vector &v) */ vpRowVector &vpRowVector::operator=(const std::vector &v) { - size_t v_size = v.size(); + unsigned int v_size = static_cast(v.size()); resize(v_size); - for (size_t i = 0; i < v_size; ++i) { + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = static_cast(v[i]); } return *this; @@ -579,18 +579,19 @@ vpRowVector::vpRowVector(const vpMatrix &M) : vpArray2D(1, M.getCols()) */ vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, static_cast(v.size())) { - size_t v_size = v.size(); - for (size_t j = 0; j < v_size; ++j) { + unsigned int v_size = static_cast(v.size()); + for (unsigned int j = 0; j < v_size; ++j) { (*this)[j] = v[j]; } } + /*! Constructor that creates a row vector from a std vector of float. */ vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, static_cast(v.size())) { - size_t v_size = v.size(); - for (size_t j = 0; j < v_size; ++j) { + unsigned int v_size = static_cast(v.size()); + for (unsigned int j = 0; j < v_size; ++j) { (*this)[j] = static_cast(v[j]); } } diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp index a9a3480042..83be4b418c 100644 --- a/modules/core/src/math/misc/vpMath.cpp +++ b/modules/core/src/math/misc/vpMath.cpp @@ -640,7 +640,7 @@ std::vector > vpMath::computeRegularPointsOnSphere(uns \sa enu2ecef(), ned2ecef() */ std::vector vpMath::getLocalTangentPlaneTransformations(const std::vector > &lonlatVec, double radius, - vpHomogeneousMatrix(*toECEF)(double lonDeg_, double latDeg_, double radius_)) + LongLattToHomogeneous toECEF) { std::vector ecef_M_local_vec; ecef_M_local_vec.reserve(lonlatVec.size()); diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp index f246f9f416..20cfcc6edf 100644 --- a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp @@ -995,8 +995,9 @@ bool vpHomogeneousMatrix::isAnHomogeneousMatrix(double threshold) const const unsigned int index_2 = 2; const unsigned int index_3 = 3; const double epsilon = std::numeric_limits::epsilon(); - return R.isARotationMatrix(threshold) && vpMath::nul((*this)[index_3][index_0], epsilon) && vpMath::nul((*this)[index_3][index_1], epsilon) && - vpMath::nul((*this)[index_3][index_2], epsilon) && vpMath::equal((*this)[index_3][index_3], 1.0, epsilon); + bool isLastRowOK = vpMath::nul((*this)[index_3][index_0], epsilon) && vpMath::nul((*this)[index_3][index_1], epsilon) && + vpMath::nul((*this)[index_3][index_2], epsilon); + return R.isARotationMatrix(threshold) && isLastRowOK && vpMath::equal((*this)[index_3][index_3], 1.0, epsilon); } /*! diff --git a/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp index 1cb6be9e37..eaf1eebe71 100644 --- a/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp +++ b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp @@ -43,7 +43,7 @@ BEGIN_VISP_NAMESPACE namespace vpCPUFeatures { // TODO: try to refactor to keep only SimdCpuInfo code and remove cpu_x86 code? -static const FeatureDetector::cpu_x86 cpu_features; +static const FeatureDetector::cpuX86 cpu_features; bool checkSSE2() { return cpu_features.HW_SSE2; } diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp index bcdfd083d1..06b80f741b 100644 --- a/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp +++ b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp @@ -80,7 +80,7 @@ using std::memset; //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -void cpu_x86::print(const char *label, bool yes) +void cpuX86::print(const char *label, bool yes) { cout << label; cout << (yes ? "Yes" : "No") << endl; @@ -89,12 +89,12 @@ void cpu_x86::print(const char *label, bool yes) //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -cpu_x86::cpu_x86() +cpuX86::cpuX86() { memset(this, 0, sizeof(*this)); detect_host(); } -bool cpu_x86::detect_OS_AVX() +bool cpuX86::detect_OS_AVX() { #ifndef UNKNOWN_ARCH // Copied from: http://stackoverflow.com/a/22521619/922184 @@ -120,7 +120,7 @@ bool cpu_x86::detect_OS_AVX() return false; #endif } -bool cpu_x86::detect_OS_AVX512() +bool cpuX86::detect_OS_AVX512() { #ifndef UNKNOWN_ARCH if (!detect_OS_AVX()) { @@ -133,7 +133,7 @@ bool cpu_x86::detect_OS_AVX512() return false; #endif } -std::string cpu_x86::get_vendor_string() +std::string cpuX86::get_vendor_string() { #ifndef UNKNOWN_ARCH uint32_t CPUInfo[4]; @@ -158,7 +158,7 @@ std::string cpu_x86::get_vendor_string() //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -void cpu_x86::detect_host() +void cpuX86::detect_host() { #ifndef UNKNOWN_ARCH // OS Features @@ -256,7 +256,7 @@ void cpu_x86::detect_host() } #endif } -void cpu_x86::print() const +void cpuX86::print() const { cout << "CPU Vendor:" << endl; print(" AMD = ", Vendor_AMD); diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86.h b/modules/core/src/tools/cpu-features/x86/cpu_x86.h index 2559a9f331..6591fad53b 100644 --- a/modules/core/src/tools/cpu-features/x86/cpu_x86.h +++ b/modules/core/src/tools/cpu-features/x86/cpu_x86.h @@ -53,9 +53,9 @@ namespace FeatureDetector //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -struct cpu_x86 +struct cpuX86 { -// Vendor + // Vendor bool Vendor_AMD; bool Vendor_Intel; @@ -105,7 +105,7 @@ struct cpu_x86 bool HW_AVX512_VBMI; public: - cpu_x86(); + cpuX86(); void print() const; diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86_Linux.ipp b/modules/core/src/tools/cpu-features/x86/cpu_x86_Linux.ipp index 9272e6e4a8..b91fe507d6 100644 --- a/modules/core/src/tools/cpu-features/x86/cpu_x86_Linux.ipp +++ b/modules/core/src/tools/cpu-features/x86/cpu_x86_Linux.ipp @@ -1,9 +1,9 @@ /* cpu_x86_Linux.ipp - * + * * Author : Alexander J. Yee * Date Created : 04/12/2014 * Last Modified : 04/12/2014 - * + * */ //////////////////////////////////////////////////////////////////////////////// @@ -13,26 +13,34 @@ // Dependencies #include #include "cpu_x86.h" -namespace FeatureDetector{ +namespace FeatureDetector +{ //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -void cpu_x86::cpuid(uint32_t out[4], uint32_t x){ - __cpuid_count(x, 0, out[0], out[1], out[2], out[3]); +void cpuX86::cpuid(uint32_t out[4], uint32_t x) +{ + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + __cpuid_count(x, 0, out[index_0], out[index_1], out[index_2], out[index_3]); } -uint64_t xgetbv(unsigned int index){ - uint32_t eax, edx; - __asm__ __volatile__("xgetbv" : "=a"(eax), "=d"(edx) : "c"(index)); - return ((uint64_t)edx << 32) | eax; +uint64_t xgetbv(unsigned int index) +{ + uint32_t eax, edx; + __asm__ __volatile__("xgetbv" : "=a"(eax), "=d"(edx) : "c"(index)); + return ((uint64_t)edx << 32) | eax; } #define _XCR_XFEATURE_ENABLED_MASK 0 //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// // Detect 64-bit -bool cpu_x86::detect_OS_x64(){ - // We only support x64 on Linux. - return true; +bool cpuX86::detect_OS_x64() +{ +// We only support x64 on Linux. + return true; } //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86_Windows.ipp b/modules/core/src/tools/cpu-features/x86/cpu_x86_Windows.ipp index b8440eed97..2f2069ec80 100644 --- a/modules/core/src/tools/cpu-features/x86/cpu_x86_Windows.ipp +++ b/modules/core/src/tools/cpu-features/x86/cpu_x86_Windows.ipp @@ -71,13 +71,17 @@ unsigned __int64 _xgetbv(unsigned int index) #if defined(__MINGW32__) void __cpuidex(unsigned int CPUInfo[4], unsigned int function_id, unsigned int subfunction_id) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; __asm__ __volatile__( "cpuid" - : "=a" (CPUInfo[0]), "=b" (CPUInfo[1]), "=c" (CPUInfo[2]), "=d" (CPUInfo[3]) + : "=a" (CPUInfo[index_0]), "=b" (CPUInfo[index_1]), "=c" (CPUInfo[index_2]), "=d" (CPUInfo[index_3]) : "a" (function_id), "c" (subfunction_id)); } #endif -void cpu_x86::cpuid(uint32_t out[4], uint32_t x) +void cpuX86::cpuid(uint32_t out[4], uint32_t x) { #if defined(__MINGW32__) __cpuidex(out, x, 0U); @@ -119,7 +123,7 @@ BOOL IsWow64() #endif return bIsWow64; } -bool cpu_x86::detect_OS_x64() +bool cpuX86::detect_OS_x64() { #ifdef _M_X64 return true; diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index d476228808..39f84cb7f6 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -489,7 +489,8 @@ int vpIoTools::mkdir_p(const std::string &path, int mode) // Iterate over the string std::string cpy_path = path; std::string sub_path; - for (size_t pos = 0; (pos = cpy_path.find(vpIoTools::separator)) != std::string::npos;) { + size_t pos = 0; + while (pos != std::string::npos) { sub_path += cpy_path.substr(0, pos + 1); // Continue if sub_path = separator bool stop_for_loop = false; @@ -508,10 +509,11 @@ int vpIoTools::mkdir_p(const std::string &path, int mode) if (errno != EEXIST) { return -1; } - } - cpy_path.erase(0, pos + 1); } + cpy_path.erase(0, pos + 1); } + pos = cpy_path.find(vpIoTools::separator); +} if (!cpy_path.empty()) { sub_path += cpy_path; @@ -525,7 +527,7 @@ int vpIoTools::mkdir_p(const std::string &path, int mode) if (errno != EEXIST) { return -1; } - } + } } return 0; @@ -577,7 +579,7 @@ void vpIoTools::makeDirectory(const std::string &dirname) if (vpIoTools::mkdir_p(v_dirname, 0755) != 0) { throw(vpIoException(vpIoException::cantCreateDirectory, "Unable to create directory '%s'", dirname.c_str())); } - } +} if (checkDirectory(dirname) == false) { throw(vpIoException(vpIoException::cantCreateDirectory, "Unable to create directory '%s'", dirname.c_str())); @@ -801,7 +803,7 @@ bool vpIoTools::checkFilename(const std::string &filename) #endif { return false; - } +} if ((stbuf.st_mode & S_IFREG) == 0) { return false; } @@ -904,18 +906,18 @@ bool vpIoTools::copy(const std::string &src, const std::string &dst) std::cout << "Cannot copy: " << src << " in " << dst << std::endl; return false; } -} + } -/*! + /*! - Remove a file or a directory. + Remove a file or a directory. - \param file_or_dir : File name or directory to remove. + \param file_or_dir : File name or directory to remove. - \return true if the file or the directory was removed, false otherwise. + \return true if the file or the directory was removed, false otherwise. - \sa makeDirectory(), makeTempDirectory() -*/ + \sa makeDirectory(), makeTempDirectory() + */ bool vpIoTools::remove(const std::string &file_or_dir) { // Check if we have to consider a file or a directory @@ -969,17 +971,17 @@ bool vpIoTools::remove(const std::string &file_or_dir) std::cout << "Cannot remove: " << file_or_dir << std::endl; return false; } -} + } -/*! + /*! - Rename an existing file \e oldfilename in \e newfilename. + Rename an existing file \e oldfilename in \e newfilename. - \param oldfilename : File to rename. - \param newfilename : New file name. + \param oldfilename : File to rename. + \param newfilename : New file name. - \return true if the file was renamed, false otherwise. -*/ + \return true if the file was renamed, false otherwise. + */ bool vpIoTools::rename(const std::string &oldfilename, const std::string &newfilename) { if (::rename(oldfilename.c_str(), newfilename.c_str()) != 0) { @@ -1338,44 +1340,48 @@ std::string vpIoTools::toLowerCase(const std::string &input) { std::string out; #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98 - for (std::string::const_iterator it = input.cbegin(); it != input.cend(); ++it) { + const std::string::const_iterator it_end = input.cend(); + for (std::string::const_iterator it = input.cbegin(); it != it_end; ++it) { #else - for (std::string::const_iterator it = input.begin(); it != input.end(); ++it) { + const std::string::const_iterator it_end = input.end(); + for (std::string::const_iterator it = input.begin(); it != it_end; ++it) { #endif out += std::tolower(*it); } return out; -} + } -/** - * @brief Return a upper-case version of the string \b input . - * Numbers and special characters stay the same - * - * @param input The input string for which we want to ensure that all the characters are in upper case. - * @return std::string A upper-case version of the string \b input, where - * numbers and special characters stay the same - */ + /** + * @brief Return a upper-case version of the string \b input . + * Numbers and special characters stay the same + * + * @param input The input string for which we want to ensure that all the characters are in upper case. + * @return std::string A upper-case version of the string \b input, where + * numbers and special characters stay the same + */ std::string vpIoTools::toUpperCase(const std::string &input) { std::string out; #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98 - for (std::string::const_iterator it = input.cbegin(); it != input.cend(); ++it) { + const std::string::const_iterator it_end = input.cend(); + for (std::string::const_iterator it = input.cbegin(); it != it_end; ++it) { #else - for (std::string::const_iterator it = input.begin(); it != input.end(); ++it) { + const std::string::const_iterator it_end = input.end(); + for (std::string::const_iterator it = input.begin(); it != it_end; ++it) { #endif out += std::toupper(*it); } return out; -} + } -/*! - Returns the absolute path using realpath() on Unix systems or - GetFullPathName() on Windows systems. \return According to realpath() - manual, returns an absolute pathname that names the same file, whose - resolution does not involve '.', '..', or symbolic links for Unix systems. - According to GetFullPathName() documentation, retrieves the full path of the - specified file for Windows systems. - */ + /*! + Returns the absolute path using realpath() on Unix systems or + GetFullPathName() on Windows systems. \return According to realpath() + manual, returns an absolute pathname that names the same file, whose + resolution does not involve '.', '..', or symbolic links for Unix systems. + According to GetFullPathName() documentation, retrieves the full path of the + specified file for Windows systems. + */ std::string vpIoTools::getAbsolutePathname(const std::string &pathname) { diff --git a/modules/core/src/tools/geometry/vpPolygon.cpp b/modules/core/src/tools/geometry/vpPolygon.cpp index 2283401203..5e10d986ac 100644 --- a/modules/core/src/tools/geometry/vpPolygon.cpp +++ b/modules/core/src/tools/geometry/vpPolygon.cpp @@ -499,7 +499,8 @@ bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &met // comment: default bool oddNodes = false; size_t v_corners_size = _corners.size(); - for (size_t i = 0, j = (v_corners_size - 1); i < v_corners_size; ++i) { + size_t j = v_corners_size - 1; + for (size_t i = 0; i < v_corners_size; ++i) { if (((_corners[i].get_v() < ip.get_v()) && (_corners[j].get_v() >= ip.get_v())) || ((_corners[j].get_v() < ip.get_v()) && (_corners[i].get_v() >= ip.get_v()))) { oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u()); @@ -526,7 +527,8 @@ void vpPolygon::precalcValuesPnPoly() m_PnPolyMultiples.resize(_corners.size()); size_t v_corners_size = _corners.size(); - for (size_t i = 0, j = (v_corners_size - 1); i < v_corners_size; ++i) { + size_t j = v_corners_size - 1; + for (size_t i = 0; i < v_corners_size; ++i) { if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits::epsilon())) { m_PnPolyConstants[i] = _corners[i].get_u(); m_PnPolyMultiples[i] = 0.0; @@ -629,7 +631,7 @@ void vpPolygon::updateBoundingBox() std::set setI; std::set setJ; - unsigned int v_corners_size = _corners.size(); + unsigned int v_corners_size = static_cast(_corners.size()); for (unsigned int i = 0; i < v_corners_size; ++i) { setI.insert(_corners[i].get_i()); setJ.insert(_corners[i].get_j()); diff --git a/modules/core/test/image-with-dataset/testIoPGM.cpp b/modules/core/test/image-with-dataset/testIoPGM.cpp index 20ec119e58..4498a69892 100644 --- a/modules/core/test/image-with-dataset/testIoPGM.cpp +++ b/modules/core/test/image-with-dataset/testIoPGM.cpp @@ -268,6 +268,7 @@ int main(int argc, const char **argv) catch (const vpException &e) { std::cout << "Catch an exception due to a non existing file: " << e << std::endl; } + std::cout << "Test succeed" << std::endl; return EXIT_SUCCESS; } catch (const vpException &e) { diff --git a/modules/core/test/image-with-dataset/testIoPPM.cpp b/modules/core/test/image-with-dataset/testIoPPM.cpp index 5c74740336..f8c76ace9f 100644 --- a/modules/core/test/image-with-dataset/testIoPPM.cpp +++ b/modules/core/test/image-with-dataset/testIoPPM.cpp @@ -306,6 +306,7 @@ int main(int argc, const char **argv) catch (const vpException &e) { std::cout << "Catch an exception due to a non existing file: " << e << std::endl; } + std::cout << "Test succeed" << std::endl; return EXIT_SUCCESS; } catch (const vpException &e) { diff --git a/modules/core/test/tools/io/testNPZ.cpp b/modules/core/test/tools/io/testNPZ.cpp index f8cde42b80..90d9da4ef3 100644 --- a/modules/core/test/tools/io/testNPZ.cpp +++ b/modules/core/test/tools/io/testNPZ.cpp @@ -104,7 +104,7 @@ TEST_CASE("Test visp::cnpy::npy_load/npz_save", "[visp::cnpy I/O]") { std::vector save_vec; save_vec.reserve(height*width*channels); - for (size_t i = 0; i < height*width*channels; i++) { + for (int i = 0; i < static_cast(height*width*channels); ++i) { save_vec.push_back(i); } @@ -120,7 +120,7 @@ TEST_CASE("Test visp::cnpy::npy_load/npz_save", "[visp::cnpy I/O]") std::vector read_vec = arr_vec_data.as_vec(); REQUIRE(save_vec_copy.size() == read_vec.size()); - for (size_t i = 0; i < read_vec.size(); i++) { + for (size_t i = 0; i < read_vec.size(); ++i) { CHECK(save_vec_copy[i] == read_vec[i]); } } @@ -256,7 +256,7 @@ TEST_CASE("Test visp::cnpy::npy_load/npz_save", "[visp::cnpy I/O]") { std::vector vec_cMo_save; for (size_t i = 0; i < 5; i++) { - vpHomogeneousMatrix cMo_save(vpTranslationVector(1+10*i, 2+20*i, 3+30*i), vpThetaUVector(0.1+i, 0.2+i, 0.3+i)); + vpHomogeneousMatrix cMo_save(vpTranslationVector(1. +10.*i, 2. +20.*i, 3. +30.*i), vpThetaUVector(0.1+i, 0.2+i, 0.3+i)); vec_cMo_save_copy.push_back(cMo_save); vec_cMo_save.insert(vec_cMo_save.end(), cMo_save.data, cMo_save.data+cMo_save.size()); // std::cout << "cMo_save:\n" << cMo_save << std::endl; diff --git a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h index 17e10b61be..676b68125d 100644 --- a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h +++ b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h @@ -1195,6 +1195,19 @@ class VISP_EXPORT vpCircleHoughTransform static const unsigned char edgeMapOff; protected: + +#ifndef DOXYGEN_SHOULD_SKIP_THIS + /** + * \brief Data storage for the computation of the barycenter of center candidates. + */ + typedef struct vpCentersBarycenter + { + std::pair m_position; /*!< The position of the barycenter.*/ + float m_totalVotes; /*!< The cumulated number of votes accross all the centers voting for this barycenter.*/ + float m_nbElectors; /*!< The number of center candidates that have voted for this barycenter.*/ + }vpCentersBarycenter; +#endif + /** * \brief Initialize the Gaussian filters used to blur the image and * compute the gradient images. @@ -1241,6 +1254,19 @@ class VISP_EXPORT vpCircleHoughTransform */ virtual void filterCenterCandidates(const std::vector &peak_positions_votes); + /** + * \brief Look in the list containing the raw center candidates if one is closed to the center candidate + * that is currently of interest. + * + * \param[in] idPeak The ID of the center candidate that is currently of interest. + * \param[in] nbPeaks The number of center candidates. + * \param[in] squared_distance_max The maximum squared distance between to center candidates to merge them. + * \param[in] peak_positions_votes The list containing the raw center candidates. + * \param[out] has_been_merged Vector indicating if the center candidates have already been merged. + * \return vpCentersBarycenter The barycenter between the center candidate that is currently of interest and similar center candidates. + */ + virtual vpCentersBarycenter mergeSimilarCenters(const unsigned int &idPeak, const unsigned int &nbPeaks, const float &squared_distance_max, const std::vector &peak_positions_votes, std::vector &has_been_merged); + /** * \brief Compute the probability of \b circle given the number of pixels voting for * it \b nbVotes. diff --git a/modules/imgproc/include/visp3/imgproc/vpContours.h b/modules/imgproc/include/visp3/imgproc/vpContours.h index 6c59a0dce8..fd11a5abea 100644 --- a/modules/imgproc/include/visp3/imgproc/vpContours.h +++ b/modules/imgproc/include/visp3/imgproc/vpContours.h @@ -144,7 +144,7 @@ class vpDirection { vpDirection direction; int directionSize = LAST_DIRECTION; - direction.m_direction = vpDirectionType((static_cast(m_direction) + 1) % directionSize); + direction.m_direction = static_cast((static_cast(m_direction) + 1) % directionSize); return direction; } @@ -158,7 +158,7 @@ class vpDirection vpDirection direction; int directionSize = static_cast(LAST_DIRECTION); int idx = VISP_NAMESPACE_ADDRESSING vpMath::modulo(static_cast(m_direction) - 1, directionSize); - direction.m_direction = vpDirectionType(idx); + direction.m_direction = static_cast(idx); return direction; } diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp deleted file mode 100644 index a37cfda7b8..0000000000 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ /dev/null @@ -1,1210 +0,0 @@ -/* - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2024 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - */ - -#include -#include - -#include - -BEGIN_VISP_NAMESPACE - -#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) -namespace -{ - -// Sorting by decreasing probabilities -bool hasBetterProba(std::pair a, std::pair b) -{ - return (a.second > b.second); -} - -void updateAccumulator(const float &x_orig, const float &y_orig, - const int &x, const int &y, - const int &offsetX, const int &offsetY, - const int &nbCols, const int &nbRows, - vpImage &accum, bool &hasToStop) -{ - if (((x - offsetX) < 0) || - ((x - offsetX) >= nbCols) || - ((y - offsetY) < 0) || - ((y - offsetY) >= nbRows) - ) { - hasToStop = true; - } - else { - float dx = (x_orig - static_cast(x)); - float dy = (y_orig - static_cast(y)); - accum[y - offsetY][x - offsetX] += std::abs(dx) + std::abs(dy); - } -} - -bool sortingCenters(const vpCircleHoughTransform::vpCenterVotes &position_vote_a, - const vpCircleHoughTransform::vpCenterVotes &position_vote_b) -{ - return position_vote_a.m_votes > position_vote_b.m_votes; -} - -float computeEffectiveRadius(const float &votes, const float &weigthedSumRadius) -{ - float r_effective = -1.f; - if (votes > std::numeric_limits::epsilon()) { - r_effective = weigthedSumRadius / votes; - } - return r_effective; -} - -void -scaleFilter(vpArray2D &filter, const float &scale) -{ - unsigned int nbRows = filter.getRows(); - unsigned int nbCols = filter.getCols(); - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - filter[r][c] = filter[r][c] * scale; - } - } -} -} -#endif - -// Static variables -const unsigned char vpCircleHoughTransform::edgeMapOn = 255; -const unsigned char vpCircleHoughTransform::edgeMapOff = 0; - -vpCircleHoughTransform::vpCircleHoughTransform() - : m_algoParams() - , mp_mask(nullptr) -{ - initGaussianFilters(); - initGradientFilters(); -} - -vpCircleHoughTransform::vpCircleHoughTransform(const vpCircleHoughTransformParams &algoParams) - : m_algoParams(algoParams) - , mp_mask(nullptr) -{ - initGaussianFilters(); - initGradientFilters(); -} - -void -vpCircleHoughTransform::init(const vpCircleHoughTransformParams &algoParams) -{ - m_algoParams = algoParams; - initGaussianFilters(); - initGradientFilters(); -} - -vpCircleHoughTransform::~vpCircleHoughTransform() -{ } - -#ifdef VISP_HAVE_NLOHMANN_JSON -using json = nlohmann::json; - -vpCircleHoughTransform::vpCircleHoughTransform(const std::string &jsonPath) -{ - initFromJSON(jsonPath); -} - -void -vpCircleHoughTransform::initFromJSON(const std::string &jsonPath) -{ - std::ifstream file(jsonPath); - if (!file.good()) { - std::stringstream ss; - ss << "Problem opening file " << jsonPath << ". Make sure it exists and is readable" << std::endl; - throw vpException(vpException::ioError, ss.str()); - } - json j; - try { - j = json::parse(file); - } - catch (json::parse_error &e) { - std::stringstream msg; - msg << "Could not parse JSON file : \n"; - - msg << e.what() << std::endl; - msg << "Byte position of error: " << e.byte; - throw vpException(vpException::ioError, msg.str()); - } - m_algoParams = j; // Call from_json(const json& j, vpDetectorDNN& *this) to read json - file.close(); - initGaussianFilters(); - initGradientFilters(); -} - -void -vpCircleHoughTransform::saveConfigurationInJSON(const std::string &jsonPath) const -{ - m_algoParams.saveConfigurationInJSON(jsonPath); -} -#endif - -void -vpCircleHoughTransform::initGaussianFilters() -{ - const int filterHalfSize = (m_algoParams.m_gaussianKernelSize + 1) / 2; - m_fg.resize(1, filterHalfSize); - vpImageFilter::getGaussianKernel(m_fg.data, m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev, true); - m_cannyVisp.setGaussianFilterParameters(m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev); -} - -void -vpCircleHoughTransform::initGradientFilters() -{ -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) // Check if cxx11 or higher - // Helper to apply the scale to the raw values of the filters - auto scaleFilter = [](vpArray2D &filter, const float &scale) { - const unsigned int nbRows = filter.getRows(); - const unsigned int nbCols = filter.getCols(); - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - filter[r][c] = filter[r][c] * scale; - } - } - }; -#endif - - const int moduloCheckForOddity = 2; - if ((m_algoParams.m_gradientFilterKernelSize % moduloCheckForOddity) != 1) { - throw vpException(vpException::badValue, "Gradient filters Kernel size should be odd."); - } - m_gradientFilterX.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); - m_gradientFilterY.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); - m_cannyVisp.setGradientFilterAperture(m_algoParams.m_gradientFilterKernelSize); - - float scaleX = 1.f; - float scaleY = 1.f; - unsigned int filterHalfSize = (m_algoParams.m_gradientFilterKernelSize - 1) / 2; - - if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) { - // Compute the Sobel filters - scaleX = vpImageFilter::getSobelKernelX(m_gradientFilterX.data, filterHalfSize); - scaleY = vpImageFilter::getSobelKernelY(m_gradientFilterY.data, filterHalfSize); - } - else if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING) { - // Compute the Scharr filters - scaleX = vpImageFilter::getScharrKernelX(m_gradientFilterX.data, filterHalfSize); - scaleY = vpImageFilter::getScharrKernelY(m_gradientFilterY.data, filterHalfSize); - } - else { - std::string errMsg = "[vpCircleHoughTransform::initGradientFilters] Error: gradient filtering method \""; - errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); - errMsg += "\" has not been implemented yet\n"; - throw vpException(vpException::notImplementedError, errMsg); - } - scaleFilter(m_gradientFilterX, scaleX); - scaleFilter(m_gradientFilterY, scaleY); -} - -std::vector -vpCircleHoughTransform::detect(const vpImage &I) -{ - vpImage I_gray; - vpImageConvert::convert(I, I_gray); - return detect(I_gray); -} - -#ifdef HAVE_OPENCV_CORE -std::vector -vpCircleHoughTransform::detect(const cv::Mat &cv_I) -{ - vpImage I_gray; - vpImageConvert::convert(cv_I, I_gray); - return detect(I_gray); -} -#endif - -std::vector -vpCircleHoughTransform::detect(const vpImage &I, const int &nbCircles) -{ - std::vector detections = detect(I); - size_t nbDetections = detections.size(); - - // Prepare vector of tuple to sort by decreasing probabilities - std::vector > v_id_proba; - for (size_t i = 0; i < nbDetections; ++i) { - std::pair id_proba(i, m_finalCirclesProbabilities[i]); - v_id_proba.push_back(id_proba); - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - // Sorting by decreasing probabilities - auto hasBetterProba - = [](std::pair a, std::pair b) { - return (a.second > b.second); - }; -#endif - std::sort(v_id_proba.begin(), v_id_proba.end(), hasBetterProba); - - // Clearing the storages containing the detection results - // to have it sorted by decreasing probabilities - size_t limitMin; - if (nbCircles < 0) { - limitMin = nbDetections; - } - else { - limitMin = std::min(nbDetections, static_cast(nbCircles)); - } - - std::vector bestCircles; - std::vector copyFinalCircles = m_finalCircles; - std::vector copyFinalCirclesVotes = m_finalCircleVotes; - std::vector copyFinalCirclesProbas = m_finalCirclesProbabilities; - std::vector > > copyFinalCirclesVotingPoints = m_finalCirclesVotingPoints; - for (size_t i = 0; i < nbDetections; ++i) { - size_t id = v_id_proba[i].first; - m_finalCircles[i] = copyFinalCircles[id]; - m_finalCircleVotes[i] = copyFinalCirclesVotes[id]; - m_finalCirclesProbabilities[i] = copyFinalCirclesProbas[id]; - if (m_algoParams.m_recordVotingPoints) { - m_finalCirclesVotingPoints[i] = copyFinalCirclesVotingPoints[id]; - } - if (i < limitMin) { - bestCircles.push_back(m_finalCircles[i]); - } - } - - return bestCircles; -} - -std::vector -vpCircleHoughTransform::detect(const vpImage &I) -{ - // Cleaning results of potential previous detection - m_centerCandidatesList.clear(); - m_centerVotes.clear(); - m_edgePointsList.clear(); - m_circleCandidates.clear(); - m_circleCandidatesVotes.clear(); - m_circleCandidatesProbabilities.clear(); - m_finalCircles.clear(); - m_finalCircleVotes.clear(); - - // Ensuring that the difference between the max and min radii is big enough to take into account - // the pixelization of the image - const float minRadiusDiff = 3.f; - if ((m_algoParams.m_maxRadius - m_algoParams.m_minRadius) < minRadiusDiff) { - if (m_algoParams.m_minRadius > (minRadiusDiff / 2.f)) { - m_algoParams.m_maxRadius += minRadiusDiff / 2.f; - m_algoParams.m_minRadius -= minRadiusDiff / 2.f; - } - else { - m_algoParams.m_maxRadius += minRadiusDiff - m_algoParams.m_minRadius; - m_algoParams.m_minRadius = 0.f; - } - } - - // Ensuring that the difference between the max and min center position is big enough to take into account - // the pixelization of the image - const float minCenterPositionDiff = 3.f; - if ((m_algoParams.m_centerXlimits.second - m_algoParams.m_centerXlimits.first) < minCenterPositionDiff) { - m_algoParams.m_centerXlimits.second += static_cast(minCenterPositionDiff / 2.f); - m_algoParams.m_centerXlimits.first -= static_cast(minCenterPositionDiff / 2.f); - } - if ((m_algoParams.m_centerYlimits.second - m_algoParams.m_centerYlimits.first) < minCenterPositionDiff) { - m_algoParams.m_centerYlimits.second += static_cast(minCenterPositionDiff / 2.f); - m_algoParams.m_centerYlimits.first -= static_cast(minCenterPositionDiff / 2.f); - } - - // First thing, we need to apply a Gaussian filter on the image to remove some spurious noise - // Then, we need to compute the image gradients in order to be able to perform edge detection - computeGradients(I); - - // Using the gradients, it is now possible to perform edge detection - // We rely on the Canny edge detector - // It will also give us the connected edged points - edgeDetection(I); - - // From the edge map and gradient information, it is possible to compute - // the center point candidates - computeCenterCandidates(); - - // From the edge map and center point candidates, we can compute candidate - // circles. These candidate circles are circles whose center belong to - // the center point candidates and whose radius is a "radius bin" that got - // enough votes by computing the distance between each point of the edge map - // and the center point candidate - computeCircleCandidates(); - - // Finally, we perform a merging operation that permits to merge circles - // respecting similarity criteria (distance between centers and similar radius) - mergeCircleCandidates(); - - return m_finalCircles; -} - -bool -operator==(const vpImageCircle &a, const vpImageCircle &b) -{ - vpImagePoint aCenter = a.getCenter(); - vpImagePoint bCenter = b.getCenter(); - bool haveSameCenter = (std::abs(aCenter.get_u() - bCenter.get_u()) - + std::abs(aCenter.get_v() - bCenter.get_v())) <= (2. * std::numeric_limits::epsilon()); - bool haveSameRadius = std::abs(a.getRadius() - b.getRadius()) <= (2.f * std::numeric_limits::epsilon()); - return (haveSameCenter && haveSameRadius); -} - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) -void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, - std::optional< vpImage > &mask, std::optional>>> &opt_votingPoints) const -#else -void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, - vpImage **mask, std::vector > > **opt_votingPoints) const -#endif -{ - if (!m_algoParams.m_recordVotingPoints) { - // We weren't asked to remember the voting points -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - mask = std::nullopt; - opt_votingPoints = std::nullopt; -#else - *mask = nullptr; - *opt_votingPoints = nullptr; -#endif - return; - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - mask = vpImage(I.getHeight(), I.getWidth(), false); - opt_votingPoints = std::vector>>(); -#else - *mask = new vpImage(I.getHeight(), I.getWidth(), false); - *opt_votingPoints = new std::vector > >(); -#endif - -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - for (const auto &detection : detections) -#else - const size_t nbDetections = detections.size(); - for (size_t i = 0; i < nbDetections; ++i) -#endif - { - bool hasFoundSimilarCircle = false; - unsigned int nbPreviouslyDetected = static_cast(m_finalCircles.size()); - unsigned int id = 0; - // Looking for a circle that was detected and is similar to the one given to the function - while ((id < nbPreviouslyDetected) && (!hasFoundSimilarCircle)) { - vpImageCircle previouslyDetected = m_finalCircles[id]; -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - if (previouslyDetected == detection) -#else - if (previouslyDetected == detections[i]) -#endif - { - hasFoundSimilarCircle = true; - // We found a circle that is similar to the one given to the function => updating the mask - const unsigned int nbVotingPoints = static_cast(m_finalCirclesVotingPoints[id].size()); - for (unsigned int idPoint = 0; idPoint < nbVotingPoints; ++idPoint) { - const std::pair &votingPoint = m_finalCirclesVotingPoints[id][idPoint]; -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - (*mask)[votingPoint.first][votingPoint.second] = true; -#else - (**mask)[votingPoint.first][votingPoint.second] = true; -#endif - } -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - opt_votingPoints->push_back(m_finalCirclesVotingPoints[id]); -#else - (**opt_votingPoints).push_back(m_finalCirclesVotingPoints[id]); -#endif - } - ++id; - } - } -} - -void -vpCircleHoughTransform::computeGradients(const vpImage &I) -{ - if ((m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) - || (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING)) { - // Computing the Gaussian blurr - vpImage Iblur, GIx; - vpImageFilter::filterX(I, GIx, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); - vpImageFilter::filterY(GIx, Iblur, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); - - // Computing the gradients - vpImageFilter::filter(Iblur, m_dIx, m_gradientFilterX, true, mp_mask); - vpImageFilter::filter(Iblur, m_dIy, m_gradientFilterY, true, mp_mask); - } - else { - std::string errMsg("[computeGradients] The filtering + gradient operators \""); - errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); - errMsg += "\" is not implemented (yet)."; - throw(vpException(vpException::notImplementedError, errMsg)); - } -} - -void -vpCircleHoughTransform::edgeDetection(const vpImage &I) -{ - if (m_algoParams.m_cannyBackendType == vpImageFilter::CANNY_VISP_BACKEND) { - // This is done to increase the time performances, because it avoids to - // recompute the gradient in the vpImageFilter::canny method - m_cannyVisp.setFilteringAndGradientType(m_algoParams.m_filteringAndGradientType); - m_cannyVisp.setCannyThresholds(m_algoParams.m_lowerCannyThresh, m_algoParams.m_upperCannyThresh); - m_cannyVisp.setCannyThresholdsRatio(m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio); - m_cannyVisp.setGradients(m_dIx, m_dIy); - m_cannyVisp.setMask(mp_mask); - m_edgeMap = m_cannyVisp.detect(I); - } - else { - if (mp_mask != nullptr) { - // Delete pixels that fall outside the mask - vpImage I_masked(I); - unsigned int nbRows = I_masked.getHeight(); - unsigned int nbCols = I_masked.getWidth(); - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - if (!((*mp_mask)[r][c])) { - I_masked[r][c] = 0; - } - } - } - - // We will have to recompute the gradient in the desired backend format anyway so we let - // the vpImageFilter::canny method take care of it - vpImageFilter::canny(I_masked, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, - m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, - m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, - m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); - } - else { - vpImageFilter::canny(I, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, - m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, - m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, - m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); - } - } - - for (int i = 0; i < m_algoParams.m_edgeMapFilteringNbIter; ++i) { - filterEdgeMap(); - } -} - -void -vpCircleHoughTransform::filterEdgeMap() -{ - vpImage J = m_edgeMap; - const unsigned int height = J.getHeight(); - const unsigned int width = J.getWidth(); - const int minNbContiguousPts = 2; - - for (unsigned int i = 1; i < (height - 1); ++i) { - for (unsigned int j = 1; j < (width - 1); ++j) { - if (J[i][j] == vpCircleHoughTransform::edgeMapOn) { - // Consider 8 neighbors - int topLeftPixel = static_cast(J[i - 1][j - 1]); - int topPixel = static_cast(J[i - 1][j]); - int topRightPixel = static_cast(J[i - 1][j + 1]); - int botLeftPixel = static_cast(J[i + 1][j - 1]); - int bottomPixel = static_cast(J[i + 1][j]); - int botRightPixel = static_cast(J[i + 1][j + 1]); - int leftPixel = static_cast(J[i][j - 1]); - int rightPixel = static_cast(J[i][j + 1]); - if ((topLeftPixel + topPixel + topRightPixel - + botLeftPixel + bottomPixel + botRightPixel - + leftPixel + rightPixel - ) >= (minNbContiguousPts * static_cast(vpCircleHoughTransform::edgeMapOn))) { - // At least minNbContiguousPts of the 8-neighbor points are also an edge point - // so we keep the edge point - m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOn; - } - else { - // The edge point is isolated => we erase it - m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOff; - } - } - } - } -} - -void -vpCircleHoughTransform::computeCenterCandidates() -{ - // For each edge point EP_i, check the image gradient at EP_i - // Then, for each image point in the direction of the gradient, - // increment the accumulator - // We can perform bilinear interpolation in order not to vote for a "line" of - // points, but for an "area" of points - unsigned int nbRows = m_edgeMap.getRows(), nbCols = m_edgeMap.getCols(); - - // Computing the minimum and maximum horizontal position of the center candidates - // The miminum horizontal position of the center is at worst -maxRadius outside the image - // The maxinum horizontal position of the center is at worst +maxRadiusoutside the image - // The width of the accumulator is the difference between the max and the min - int minimumXposition = std::max(m_algoParams.m_centerXlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); - int maximumXposition = std::min(m_algoParams.m_centerXlimits.second, static_cast(m_algoParams.m_maxRadius + nbCols)); - minimumXposition = std::min(minimumXposition, maximumXposition - 1); - float minimumXpositionFloat = static_cast(minimumXposition); - float maximumXpositionFloat = static_cast(maximumXposition); - int offsetX = minimumXposition; - int accumulatorWidth = (maximumXposition - minimumXposition) + 1; - if (accumulatorWidth <= 0) { - throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator width <= 0!")); - } - - // Computing the minimum and maximum vertical position of the center candidates - // The miminum vertical position of the center is at worst -maxRadius outside the image - // The maxinum vertical position of the center is at worst +maxRadiusoutside the image - // The height of the accumulator is the difference between the max and the min - int minimumYposition = std::max(m_algoParams.m_centerYlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); - int maximumYposition = std::min(m_algoParams.m_centerYlimits.second, static_cast(m_algoParams.m_maxRadius + nbRows)); - minimumYposition = std::min(minimumYposition, maximumYposition - 1); - float minimumYpositionFloat = static_cast(minimumYposition); - float maximumYpositionFloat = static_cast(maximumYposition); - int offsetY = minimumYposition; - int accumulatorHeight = (maximumYposition - minimumYposition) + 1; - if (accumulatorHeight <= 0) { - throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator height <= 0!")); - } - - vpImage centersAccum(accumulatorHeight, accumulatorWidth + 1, 0.); /*!< Votes for the center candidates.*/ - - const int nbDirections = 2; - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - if (m_edgeMap[r][c] == vpCircleHoughTransform::edgeMapOn) { - // Voting for points in both direction of the gradient - // Step from min_radius to max_radius in both directions of the gradient - float mag = std::sqrt((m_dIx[r][c] * m_dIx[r][c]) + (m_dIy[r][c] * m_dIy[r][c])); - - float sx = 0.f, sy = 0.f; - if (std::abs(mag) >= std::numeric_limits::epsilon()) { - sx = m_dIx[r][c] / mag; - sy = m_dIy[r][c] / mag; - - // Saving the edge point for further use - m_edgePointsList.push_back(std::pair(r, c)); - - for (int k1 = 0; k1 < nbDirections; ++k1) { - bool hasToStopLoop = false; - int x_low_prev = std::numeric_limits::max(), y_low_prev, y_high_prev; - int x_high_prev = (y_low_prev = (y_high_prev = x_low_prev)); - - float rstart = m_algoParams.m_minRadius, rstop = m_algoParams.m_maxRadius; - float min_minus_c = minimumXpositionFloat - static_cast(c); - float min_minus_r = minimumYpositionFloat - static_cast(r); - float max_minus_c = maximumXpositionFloat - static_cast(c); - float max_minus_r = maximumYpositionFloat - static_cast(r); - if (sx > 0) { - float rmin = min_minus_c / sx; - rstart = std::max(rmin, m_algoParams.m_minRadius); - float rmax = max_minus_c / sx; - rstop = std::min(rmax, m_algoParams.m_maxRadius); - } - else if (sx < 0) { - float rmin = max_minus_c / sx; - rstart = std::max(rmin, m_algoParams.m_minRadius); - float rmax = min_minus_c / sx; - rstop = std::min(rmax, m_algoParams.m_maxRadius); - } - - if (sy > 0) { - float rmin = min_minus_r / sy; - rstart = std::max(rmin, rstart); - float rmax = max_minus_r / sy; - rstop = std::min(rmax, rstop); - } - else if (sy < 0) { - float rmin = max_minus_r / sy; - rstart = std::max(rmin, rstart); - float rmax = min_minus_r / sy; - rstop = std::min(rmax, rstop); - } - - float deltar_x = 1.f / std::abs(sx), deltar_y = 1.f / std::abs(sy); - float deltar = std::min(deltar_x, deltar_y); - - float rad = rstart; - while ((rad <= rstop) && (!hasToStopLoop)) { - float x1 = static_cast(c) + (rad * sx); - float y1 = static_cast(r) + (rad * sy); - rad += deltar; // Update rad that is not used below not to forget it - - if ((x1 < minimumXpositionFloat) || (y1 < minimumYpositionFloat) - || (x1 > maximumXpositionFloat) || (y1 > maximumYpositionFloat)) { - continue; // It means that the center is outside the search region. - } - - int x_low, x_high, y_low, y_high; - - if (x1 > 0.) { - x_low = static_cast(std::floor(x1)); - x_high = static_cast(std::ceil(x1)); - } - else { - x_low = -(static_cast(std::ceil(-x1))); - x_high = -(static_cast(std::floor(-x1))); - } - - if (y1 > 0.) { - y_low = static_cast(std::floor(y1)); - y_high = static_cast(std::ceil(y1)); - } - else { - y_low = -(static_cast(std::ceil(-1. * y1))); - y_high = -(static_cast(std::floor(-1. * y1))); - } - - if ((x_low_prev == x_low) && (x_high_prev == x_high) - && (y_low_prev == y_low) && (y_high_prev == y_high)) { - // Avoid duplicated votes to the same center candidate - continue; - } - else { - x_low_prev = x_low; - x_high_prev = x_high; - y_low_prev = y_low; - y_high_prev = y_high; - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - auto updateAccumulator = - [](const float &x_orig, const float &y_orig, const int &x, const int &y, - const int &offsetX, const int &offsetY, const int &nbCols, const int &nbRows, - vpImage &accum, bool &hasToStop) { - if (((x - offsetX) < 0) || ((x - offsetX) >= nbCols) || - ((y - offsetY) < 0) || ((y - offsetY) >= nbRows) - ) { - hasToStop = true; - } - else { - float dx = (x_orig - static_cast(x)); - float dy = (y_orig - static_cast(y)); - accum[y - offsetY][x - offsetX] += std::abs(dx) + std::abs(dy); - } - }; -#endif - - updateAccumulator(x1, y1, x_low, y_low, - offsetX, offsetY, - accumulatorWidth, accumulatorHeight, - centersAccum, hasToStopLoop - ); - - updateAccumulator(x1, y1, x_high, y_high, - offsetX, offsetY, - accumulatorWidth, accumulatorHeight, - centersAccum, hasToStopLoop - ); - } - - sx = -sx; - sy = -sy; - } - } - } - } - } - - // Use dilatation with large kernel in order to determine the - // accumulator maxima - vpImage centerCandidatesMaxima = centersAccum; - int dilatationKernelSize = std::max(m_algoParams.m_dilatationKernelSize, 3); // Ensure at least a 3x3 dilatation operation is performed - vpImageMorphology::dilatation(centerCandidatesMaxima, dilatationKernelSize); - - // Look for the image points that correspond to the accumulator maxima - // These points will become the center candidates - // find the possible circle centers - int nbColsAccum = centersAccum.getCols(); - int nbRowsAccum = centersAccum.getRows(); - int nbVotes = -1; - std::vector peak_positions_votes; - - for (int y = 0; y < nbRowsAccum; ++y) { - int left = -1; - for (int x = 0; x < nbColsAccum; ++x) { - if ((centersAccum[y][x] >= m_algoParams.m_centerMinThresh) - && (vpMath::equal(centersAccum[y][x], centerCandidatesMaxima[y][x])) - && (centersAccum[y][x] > centersAccum[y][x + 1]) - ) { - if (left < 0) { - left = x; - } - nbVotes = std::max(nbVotes, static_cast(centersAccum[y][x])); - } - else if (left >= 0) { - int cx = static_cast(((left + x) - 1) * 0.5f); - float sumVotes = 0., x_avg = 0., y_avg = 0.; - int averagingWindowHalfSize = m_algoParams.m_averagingWindowSize / 2; - int startingRow = std::max(0, y - averagingWindowHalfSize); - int startingCol = std::max(0, cx - averagingWindowHalfSize); - int endRow = std::min(accumulatorHeight, y + averagingWindowHalfSize + 1); - int endCol = std::min(accumulatorWidth, cx + averagingWindowHalfSize + 1); - for (int r = startingRow; r < endRow; ++r) { - for (int c = startingCol; c < endCol; ++c) { - sumVotes += centersAccum[r][c]; - x_avg += centersAccum[r][c] * c; - y_avg += centersAccum[r][c] * r; - } - } - float avgVotes = sumVotes / static_cast(m_algoParams.m_averagingWindowSize * m_algoParams.m_averagingWindowSize); - if (avgVotes > m_algoParams.m_centerMinThresh) { - x_avg /= static_cast(sumVotes); - y_avg /= static_cast(sumVotes); - std::pair position(y_avg + static_cast(offsetY), x_avg + static_cast(offsetX)); - vpCenterVotes position_vote; - position_vote.m_position = position; - position_vote.m_votes = avgVotes; - peak_positions_votes.push_back(position_vote); - } - if (nbVotes < 0) { - std::stringstream errMsg; - errMsg << "nbVotes (" << nbVotes << ") < 0, thresh = " << m_algoParams.m_centerMinThresh; - throw(vpException(vpException::badValue, errMsg.str())); - } - left = -1; - nbVotes = -1; - } - } - } - filterCenterCandidates(peak_positions_votes); -} - -void -vpCircleHoughTransform::filterCenterCandidates(const std::vector &peak_positions_votes) -{ - unsigned int nbPeaks = static_cast(peak_positions_votes.size()); - if (nbPeaks > 0) { - std::vector has_been_merged(nbPeaks, false); - std::vector merged_peaks_position_votes; - float squared_distance_max = m_algoParams.m_centerMinDist * m_algoParams.m_centerMinDist; - for (unsigned int idPeak = 0; idPeak < nbPeaks; ++idPeak) { - float votes = peak_positions_votes[idPeak].m_votes; - if (has_been_merged[idPeak]) { - // Ignoring peak that has already been merged - continue; - } - else if (votes < m_algoParams.m_centerMinThresh) { - // Ignoring peak whose number of votes is lower than the threshold - has_been_merged[idPeak] = true; - continue; - } - std::pair position = peak_positions_votes[idPeak].m_position; - std::pair barycenter; - barycenter.first = position.first * peak_positions_votes[idPeak].m_votes; - barycenter.second = position.second * peak_positions_votes[idPeak].m_votes; - float total_votes = peak_positions_votes[idPeak].m_votes; - float nb_electors = 1.f; - // Looking for potential similar peak in the following peaks - for (unsigned int idCandidate = idPeak + 1; idCandidate < nbPeaks; ++idCandidate) { - float votes_candidate = peak_positions_votes[idCandidate].m_votes; - if (has_been_merged[idCandidate]) { - continue; - } - else if (votes_candidate < m_algoParams.m_centerMinThresh) { - // Ignoring peak whose number of votes is lower than the threshold - has_been_merged[idCandidate] = true; - continue; - } - // Computing the distance with the peak of insterest - std::pair position_candidate = peak_positions_votes[idCandidate].m_position; - float squared_distance = ((position.first - position_candidate.first) * (position.first - position_candidate.first)) - + ((position.second - position_candidate.second) * (position.second - position_candidate.second)); - - // If the peaks are similar, update the barycenter peak between them and corresponding votes - if (squared_distance < squared_distance_max) { - barycenter.first += position_candidate.first * votes_candidate; - barycenter.second += position_candidate.second * votes_candidate; - total_votes += votes_candidate; - nb_electors += 1.f; - has_been_merged[idCandidate] = true; - } - } - - float avg_votes = total_votes / nb_electors; - // Only the centers having enough votes are considered - if (avg_votes > m_algoParams.m_centerMinThresh) { - barycenter.first /= total_votes; - barycenter.second /= total_votes; - vpCenterVotes barycenter_votes; - barycenter_votes.m_position = barycenter; - barycenter_votes.m_votes = avg_votes; - merged_peaks_position_votes.push_back(barycenter_votes); - } - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - auto sortingCenters = [](const vpCenterVotes &position_vote_a, - const vpCenterVotes &position_vote_b) { - return position_vote_a.m_votes > position_vote_b.m_votes; - }; -#endif - - std::sort(merged_peaks_position_votes.begin(), merged_peaks_position_votes.end(), sortingCenters); - - nbPeaks = static_cast(merged_peaks_position_votes.size()); - int nbPeaksToKeep = (m_algoParams.m_expectedNbCenters > 0 ? m_algoParams.m_expectedNbCenters : static_cast(nbPeaks)); - nbPeaksToKeep = std::min(nbPeaksToKeep, static_cast(nbPeaks)); - for (int i = 0; i < nbPeaksToKeep; ++i) { - m_centerCandidatesList.push_back(merged_peaks_position_votes[i].m_position); - m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].m_votes)); - } - } -} - -void -vpCircleHoughTransform::computeCircleCandidates() -{ - size_t nbCenterCandidates = m_centerCandidatesList.size(); - int nbBins = static_cast(((m_algoParams.m_maxRadius - m_algoParams.m_minRadius) + 1) / m_algoParams.m_mergingRadiusDiffThresh); - nbBins = std::max(static_cast(1), nbBins); // Avoid having 0 bins, which causes segfault - std::vector radiusAccumList; // Radius accumulator for each center candidates. - std::vector radiusActualValueList; // Vector that contains the actual distance between the edge points and the center candidates. - std::vector > > votingPoints(nbBins); // Vectors that contain the points voting for each radius bin - - float rmin2 = m_algoParams.m_minRadius * m_algoParams.m_minRadius; - float rmax2 = m_algoParams.m_maxRadius * m_algoParams.m_maxRadius; - float circlePerfectness2 = m_algoParams.m_circlePerfectness * m_algoParams.m_circlePerfectness; - - for (size_t i = 0; i < nbCenterCandidates; ++i) { - std::pair centerCandidate = m_centerCandidatesList[i]; - // Initialize the radius accumulator of the candidate with 0s - radiusAccumList.clear(); - radiusAccumList.resize(nbBins, 0); - radiusActualValueList.clear(); - radiusActualValueList.resize(nbBins, 0.); - -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - for (auto edgePoint : m_edgePointsList) -#else - const size_t nbEdgePoints = m_edgePointsList.size(); - for (size_t e = 0; e < nbEdgePoints; ++e) -#endif - { - // For each center candidate CeC_i, compute the distance with each edge point EP_j d_ij = dist(CeC_i; EP_j) -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - float rx = edgePoint.second - centerCandidate.second; - float ry = edgePoint.first - centerCandidate.first; -#else - float rx = m_edgePointsList[e].second - centerCandidate.second; - float ry = m_edgePointsList[e].first - centerCandidate.first; -#endif - float r2 = (rx * rx) + (ry * ry); - if ((r2 > rmin2) && (r2 < rmax2)) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - float gx = m_dIx[edgePoint.first][edgePoint.second]; - float gy = m_dIy[edgePoint.first][edgePoint.second]; -#else - float gx = m_dIx[m_edgePointsList[e].first][m_edgePointsList[e].second]; - float gy = m_dIy[m_edgePointsList[e].first][m_edgePointsList[e].second]; -#endif - float grad2 = (gx * gx) + (gy * gy); - - float scalProd = (rx * gx) + (ry * gy); - float scalProd2 = scalProd * scalProd; - if (scalProd2 >= (circlePerfectness2 * r2 * grad2)) { - // Look for the Radius Candidate Bin RCB_k to which d_ij is "the closest" will have an additionnal vote - float r = static_cast(std::sqrt(r2)); - int r_bin = static_cast(std::floor((r - m_algoParams.m_minRadius) / m_algoParams.m_mergingRadiusDiffThresh)); - r_bin = std::min(r_bin, nbBins - 1); - if ((r < (m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * 0.5f))) - || (r >(m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * (static_cast(nbBins - 1) + 0.5f))))) { - // If the radius is at the very beginning of the allowed radii or at the very end, we do not span the vote - radiusAccumList[r_bin] += 1.f; - radiusActualValueList[r_bin] += r; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[r_bin].push_back(edgePoint); -#else - votingPoints[r_bin].push_back(m_edgePointsList[e]); -#endif - } - } - else { - float midRadiusPrevBin = m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * ((r_bin - 1.f) + 0.5f)); - float midRadiusCurBin = m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * (r_bin + 0.5f)); - float midRadiusNextBin = m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * (r_bin + 1.f + 0.5f)); - - if ((r >= midRadiusCurBin) && (r <= midRadiusNextBin)) { - // The radius is at the end of the current bin or beginning of the next, we span the vote with the next bin - float voteCurBin = (midRadiusNextBin - r) / m_algoParams.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the current bin - float voteNextBin = 1.f - voteCurBin; - radiusAccumList[r_bin] += voteCurBin; - radiusActualValueList[r_bin] += r * voteCurBin; - radiusAccumList[r_bin + 1] += voteNextBin; - radiusActualValueList[r_bin + 1] += r * voteNextBin; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[r_bin].push_back(edgePoint); - votingPoints[r_bin + 1].push_back(edgePoint); -#else - votingPoints[r_bin].push_back(m_edgePointsList[e]); - votingPoints[r_bin + 1].push_back(m_edgePointsList[e]); -#endif - } - } - else { - // The radius is at the end of the previous bin or beginning of the current, we span the vote with the previous bin - float votePrevBin = (r - midRadiusPrevBin) / m_algoParams.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the previous bin - float voteCurBin = 1.f - votePrevBin; - radiusAccumList[r_bin] += voteCurBin; - radiusActualValueList[r_bin] += r * voteCurBin; - radiusAccumList[r_bin - 1] += votePrevBin; - radiusActualValueList[r_bin - 1] += r * votePrevBin; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[r_bin].push_back(edgePoint); - votingPoints[r_bin - 1].push_back(edgePoint); -#else - votingPoints[r_bin].push_back(m_edgePointsList[e]); - votingPoints[r_bin - 1].push_back(m_edgePointsList[e]); -#endif - } - } - } - } - } - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - // Lambda to compute the effective radius (i.e. barycenter) of each radius bin - auto computeEffectiveRadius = [](const float &votes, const float &weigthedSumRadius) { - float r_effective = -1.f; - if (votes > std::numeric_limits::epsilon()) { - r_effective = weigthedSumRadius / votes; - } - return r_effective; - }; -#endif - - // Merging similar candidates - std::vector v_r_effective; // Vector of radius of each candidate after the merge step - std::vector v_votes_effective; // Vector of number of votes of each candidate after the merge step - std::vector > > v_votingPoints_effective; // Vector of voting points after the merge step - std::vector v_hasMerged_effective; // Vector indicating if merge has been performed for the different candidates - for (int idBin = 0; idBin < nbBins; ++idBin) { - float r_effective = computeEffectiveRadius(radiusAccumList[idBin], radiusActualValueList[idBin]); - float votes_effective = radiusAccumList[idBin]; - std::vector > votingPoints_effective = votingPoints[idBin]; - bool is_r_effective_similar = (r_effective > 0.f); - // Looking for potential similar radii in the following bins - // If so, compute the barycenter radius between them - int idCandidate = idBin + 1; - bool hasMerged = false; - while ((idCandidate < nbBins) && is_r_effective_similar) { - float r_effective_candidate = computeEffectiveRadius(radiusAccumList[idCandidate], radiusActualValueList[idCandidate]); - if (std::abs(r_effective_candidate - r_effective) < m_algoParams.m_mergingRadiusDiffThresh) { - r_effective = ((r_effective * votes_effective) + (r_effective_candidate * radiusAccumList[idCandidate])) / (votes_effective + radiusAccumList[idCandidate]); - votes_effective += radiusAccumList[idCandidate]; - radiusAccumList[idCandidate] = -.1f; - radiusActualValueList[idCandidate] = -1.f; - is_r_effective_similar = true; - if (m_algoParams.m_recordVotingPoints) { - // Move elements from votingPoints[idCandidate] to votingPoints_effective. - // votingPoints[idCandidate] is left in undefined but safe-to-destruct state. -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints_effective.insert( - votingPoints_effective.end(), - std::make_move_iterator(votingPoints[idCandidate].begin()), - std::make_move_iterator(votingPoints[idCandidate].end()) - ); -#else - votingPoints_effective.insert( - votingPoints_effective.end(), - votingPoints[idCandidate].begin(), - votingPoints[idCandidate].end() - ); -#endif - hasMerged = true; - } - } - else { - is_r_effective_similar = false; - } - ++idCandidate; - } - - if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= (m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_PI_FLOAT * r_effective))) { - // Only the circles having enough votes and being visible enough are considered - v_r_effective.push_back(r_effective); - v_votes_effective.push_back(votes_effective); - if (m_algoParams.m_recordVotingPoints) { - v_votingPoints_effective.push_back(votingPoints_effective); - v_hasMerged_effective.push_back(hasMerged); - } - } - } - - unsigned int nbCandidates = static_cast(v_r_effective.size()); - for (unsigned int idBin = 0; idBin < nbCandidates; ++idBin) { - // If the circle of center CeC_i and radius RCB_k has enough votes, it is added to the list - // of Circle Candidates - float r_effective = v_r_effective[idBin]; - vpImageCircle candidateCircle(vpImagePoint(centerCandidate.first, centerCandidate.second), r_effective); - float proba = computeCircleProbability(candidateCircle, static_cast(v_votes_effective[idBin])); - if (proba > m_algoParams.m_circleProbaThresh) { - m_circleCandidates.push_back(candidateCircle); - m_circleCandidatesProbabilities.push_back(proba); - m_circleCandidatesVotes.push_back(static_cast(v_votes_effective[idBin])); - if (m_algoParams.m_recordVotingPoints) { - if (v_hasMerged_effective[idBin]) { - // Remove potential duplicated points - std::sort(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()); - v_votingPoints_effective[idBin].erase(std::unique(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()), v_votingPoints_effective[idBin].end()); - } - // Save the points - m_circleCandidatesVotingPoints.push_back(v_votingPoints_effective[idBin]); - } - } - } - } -} - -float -vpCircleHoughTransform::computeCircleProbability(const vpImageCircle &circle, const unsigned int &nbVotes) -{ - float proba(0.f); - float visibleArc(static_cast(nbVotes)); - float theoreticalLenght; - if (mp_mask != nullptr) { - theoreticalLenght = static_cast(circle.computePixelsInMask(*mp_mask)); - } - else { - theoreticalLenght = circle.computeArcLengthInRoI(vpRect(vpImagePoint(0, 0), m_edgeMap.getWidth(), m_edgeMap.getHeight())); - } - if (theoreticalLenght < std::numeric_limits::epsilon()) { - proba = 0.f; - } - else { - proba = std::min(visibleArc / theoreticalLenght, 1.f); - } - return proba; -} - -void -vpCircleHoughTransform::mergeCircleCandidates() -{ - std::vector circleCandidates = m_circleCandidates; - std::vector circleCandidatesVotes = m_circleCandidatesVotes; - std::vector circleCandidatesProba = m_circleCandidatesProbabilities; - std::vector > > circleCandidatesVotingPoints = m_circleCandidatesVotingPoints; - // First iteration of merge - mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); - - // Second iteration of merge - mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); - - // Saving the results - m_finalCircles = circleCandidates; - m_finalCircleVotes = circleCandidatesVotes; - m_finalCirclesProbabilities = circleCandidatesProba; - m_finalCirclesVotingPoints = circleCandidatesVotingPoints; -} - -void -vpCircleHoughTransform::mergeCandidates(std::vector &circleCandidates, std::vector &circleCandidatesVotes, - std::vector &circleCandidatesProba, std::vector > > &votingPoints) -{ - size_t nbCandidates = circleCandidates.size(); - size_t i = 0; - while (i < nbCandidates) { - vpImageCircle cic_i = circleCandidates[i]; - bool hasPerformedMerge = false; - // // For each other circle candidate CiC_j do: - size_t j = i + 1; - while (j < nbCandidates) { - vpImageCircle cic_j = circleCandidates[j]; - // // // Compute the similarity between CiC_i and CiC_j - double distanceBetweenCenters = vpImagePoint::distance(cic_i.getCenter(), cic_j.getCenter()); - double radiusDifference = std::abs(cic_i.getRadius() - cic_j.getRadius()); - bool areCirclesSimilar = ((distanceBetweenCenters < m_algoParams.m_centerMinDist) - && (radiusDifference < m_algoParams.m_mergingRadiusDiffThresh) - ); - - if (areCirclesSimilar) { - hasPerformedMerge = true; - // // // If the similarity exceeds a threshold, merge the circle candidates CiC_i and CiC_j and remove CiC_j of the list - unsigned int totalVotes = circleCandidatesVotes[i] + circleCandidatesVotes[j]; - float totalProba = circleCandidatesProba[i] + circleCandidatesProba[j]; - float newProba = 0.5f * totalProba; - float newRadius = ((cic_i.getRadius() * circleCandidatesProba[i]) + (cic_j.getRadius() * circleCandidatesProba[j])) / totalProba; - vpImagePoint newCenter = ((cic_i.getCenter() * circleCandidatesProba[i]) + (cic_j.getCenter() * circleCandidatesProba[j])) / totalProba; - cic_i = vpImageCircle(newCenter, newRadius); - circleCandidates[j] = circleCandidates[nbCandidates - 1]; - const unsigned int var2 = 2; - circleCandidatesVotes[i] = totalVotes / var2; // Compute the mean vote - circleCandidatesVotes[j] = circleCandidatesVotes[nbCandidates - 1]; - circleCandidatesProba[i] = newProba; - circleCandidatesProba[j] = circleCandidatesProba[nbCandidates - 1]; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[i].insert( - votingPoints[i].end(), - std::make_move_iterator(votingPoints[j].begin()), - std::make_move_iterator(votingPoints[j].end()) - ); -#else - votingPoints[i].insert( - votingPoints[i].end(), - votingPoints[j].begin(), - votingPoints[j].end() - ); -#endif - votingPoints.pop_back(); - } - circleCandidates.pop_back(); - circleCandidatesVotes.pop_back(); - circleCandidatesProba.pop_back(); - --nbCandidates; - // We do not update j because the new j-th candidate has not been evaluated yet - } - else { - // We will evaluate the next candidate - ++j; - } - } - // // Add the circle candidate CiC_i, potentially merged with other circle candidates, to the final list of detected circles - circleCandidates[i] = cic_i; - if (hasPerformedMerge && m_algoParams.m_recordVotingPoints) { - // Remove duplicated points - std::sort(votingPoints[i].begin(), votingPoints[i].end()); - votingPoints[i].erase(std::unique(votingPoints[i].begin(), votingPoints[i].end()), votingPoints[i].end()); - } - ++i; - } -} - -std::string -vpCircleHoughTransform::toString() const -{ - return m_algoParams.toString(); -} - -std::ostream & -operator<<(std::ostream &os, const vpCircleHoughTransform &detector) -{ - os << detector.toString(); - return os; -} - -END_VISP_NAMESPACE diff --git a/modules/imgproc/src/vpCircleHoughTransform_centers.cpp b/modules/imgproc/src/vpCircleHoughTransform_centers.cpp new file mode 100644 index 0000000000..62a09d7c79 --- /dev/null +++ b/modules/imgproc/src/vpCircleHoughTransform_centers.cpp @@ -0,0 +1,569 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + +namespace +{ + /** + * \brief Data required to update the center candidates accumulator along the + * gradient direction. + */ +typedef struct vpDataForAccumLoop +{ + unsigned int r; /*!< The row of the edge-point of interest.*/ + unsigned int c; /*!< The column of the edge-point of interest.*/ + float minRadius; /*!< The minimum radius of the searched circle.s.*/ + float maxRadius; /*!< The maximum radius of the searched circle.s.*/ + float minimumXpositionFloat; /*!< The minimum x-axis position of the center in the image.*/ + float minimumYpositionFloat; /*!< The minimum y-axis position of the center in the image.*/ + float maximumXpositionFloat; /*!< The maximum x-axis position of the center in the image.*/ + float maximumYpositionFloat; /*!< The maximum y-axis position of the center in the image.*/ + int offsetX; /*!< The offset to map the accumulator indices with the minimum x-axis value.*/ + int offsetY; /*!< The offset to map the accumulator indices with the minimum y-axis value.*/ + int accumulatorWidth; /*!< The width of the accumulator.*/ + int accumulatorHeight; /*!< The height of the accumulator.*/ +}vpDataForAccumLoop; + +/** + * \brief Store the coordinates for single step of update of the center candidates accumulator. + */ +typedef struct vpCoordinatesForAccumStep +{ + float x_orig; + float y_orig; + int x; + int y; +}vpCoordinatesForAccumStep; + +#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) +void updateAccumulator(const vpCoordinatesForAccumStep &coord, + const vpDataForAccumLoop &data, + vpImage &accum, bool &hasToStop) +{ + if (((coord.x - data.offsetX) < 0) || + ((coord.x - data.offsetX) >= data.accumulatorWidth) || + ((coord.y - data.offsetY) < 0) || + ((coord.y - data.offsetY) >= data.accumulatorHeight) + ) { + hasToStop = true; + } + else { + float dx = (coord.x_orig - static_cast(coord.x)); + float dy = (coord.y_orig - static_cast(coord.y)); + accum[coord.y - data.offsetY][coord.x - data.offsetX] += std::abs(dx) + std::abs(dy); + } +} + +bool sortingCenters(const vpCircleHoughTransform::vpCenterVotes &position_vote_a, + const vpCircleHoughTransform::vpCenterVotes &position_vote_b) +{ + return position_vote_a.m_votes > position_vote_b.m_votes; +} +#endif + +/** + * \brief Update the center accumulator along the positive and negative gradient direction + * starting from an edge-point of interest. + * + * \param[in] data The data required for the algorithm. + * \param[out] sx The gradient along x. + * \param[out] sy The gradient along y. + * \param[out] centersAccum The center candidates accumulator. + */ +void +updateAccumAlongGradientDir(const vpDataForAccumLoop &data, float &sx, float &sy, vpImage ¢ersAccum) +{ + static const int nbDirections = 2; + for (int k1 = 0; k1 < nbDirections; ++k1) { + bool hasToStopLoop = false; + int x_low_prev = std::numeric_limits::max(), y_low_prev, y_high_prev; + int x_high_prev = (y_low_prev = (y_high_prev = x_low_prev)); + + float rstart = data.minRadius, rstop = data.maxRadius; + float min_minus_c = data.minimumXpositionFloat - static_cast(data.c); + float min_minus_r = data.minimumYpositionFloat - static_cast(data.r); + float max_minus_c = data.maximumXpositionFloat - static_cast(data.c); + float max_minus_r = data.maximumYpositionFloat - static_cast(data.r); + if (sx > 0) { + float rmin = min_minus_c / sx; + rstart = std::max(rmin, data.minRadius); + float rmax = max_minus_c / sx; + rstop = std::min(rmax, data.maxRadius); + } + else if (sx < 0) { + float rmin = max_minus_c / sx; + rstart = std::max(rmin, data.minRadius); + float rmax = min_minus_c / sx; + rstop = std::min(rmax, data.maxRadius); + } + + if (sy > 0) { + float rmin = min_minus_r / sy; + rstart = std::max(rmin, rstart); + float rmax = max_minus_r / sy; + rstop = std::min(rmax, rstop); + } + else if (sy < 0) { + float rmin = max_minus_r / sy; + rstart = std::max(rmin, rstart); + float rmax = min_minus_r / sy; + rstop = std::min(rmax, rstop); + } + + float deltar_x = 1.f / std::abs(sx), deltar_y = 1.f / std::abs(sy); + float deltar = std::min(deltar_x, deltar_y); + + float rad = rstart; + while ((rad <= rstop) && (!hasToStopLoop)) { + float x1 = static_cast(data.c) + (rad * sx); + float y1 = static_cast(data.r) + (rad * sy); + rad += deltar; // Update rad that is not used below not to forget it + + bool xOutsideRoI = (x1 < data.minimumXpositionFloat) || (x1 > data.maximumXpositionFloat); + bool yOutsideRoI = (y1 < data.minimumYpositionFloat) || (y1 > data.maximumYpositionFloat); + // Continue only if the center is inside the search region. + if (!(xOutsideRoI || yOutsideRoI)) { + int x_low, x_high, y_low, y_high; + + if (x1 > 0.) { + x_low = static_cast(std::floor(x1)); + x_high = static_cast(std::ceil(x1)); + } + else { + x_low = -(static_cast(std::ceil(-x1))); + x_high = -(static_cast(std::floor(-x1))); + } + + if (y1 > 0.) { + y_low = static_cast(std::floor(y1)); + y_high = static_cast(std::ceil(y1)); + } + else { + y_low = -(static_cast(std::ceil(-1. * y1))); + y_high = -(static_cast(std::floor(-1. * y1))); + } + + bool xHasNotChanged = (x_low_prev == x_low) && (x_high_prev == x_high); + bool yHasNotChanged = (y_low_prev == y_low) && (y_high_prev == y_high); + + // Avoid duplicated votes to the same center candidate + if (!(xHasNotChanged && yHasNotChanged)) { + x_low_prev = x_low; + x_high_prev = x_high; + y_low_prev = y_low; + y_high_prev = y_high; + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + auto updateAccumulator = + [](const vpCoordinatesForAccumStep &coord, + const vpDataForAccumLoop &data, + vpImage &accum, bool &hasToStop) + { + if (((coord.x - data.offsetX) < 0) || + ((coord.x - data.offsetX) >= data.accumulatorWidth) || + ((coord.y - data.offsetY) < 0) || + ((coord.y - data.offsetY) >= data.accumulatorHeight) + ) { + hasToStop = true; + } + else { + float dx = (coord.x_orig - static_cast(coord.x)); + float dy = (coord.y_orig - static_cast(coord.y)); + accum[coord.y - data.offsetY][coord.x - data.offsetX] += std::abs(dx) + std::abs(dy); + } + }; +#endif + vpCoordinatesForAccumStep coords; + coords.x_orig = x1; + coords.y_orig = y1; + coords.x = x_low; + coords.y = y_low; + updateAccumulator(coords, data, centersAccum, hasToStopLoop); + + coords.x = x_high; + coords.y = y_high; + updateAccumulator(coords, data, centersAccum, hasToStopLoop); + } + } + } + sx = -sx; + sy = -sy; +} +} +} + + +// Static variables +const unsigned char vpCircleHoughTransform::edgeMapOn = 255; +const unsigned char vpCircleHoughTransform::edgeMapOff = 0; + +void +vpCircleHoughTransform::computeGradients(const vpImage &I) +{ + if ((m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) + || (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING)) { + // Computing the Gaussian blurr + vpImage Iblur, GIx; + vpImageFilter::filterX(I, GIx, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); + vpImageFilter::filterY(GIx, Iblur, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); + + // Computing the gradients + vpImageFilter::filter(Iblur, m_dIx, m_gradientFilterX, true, mp_mask); + vpImageFilter::filter(Iblur, m_dIy, m_gradientFilterY, true, mp_mask); + } + else { + std::string errMsg("[computeGradients] The filtering + gradient operators \""); + errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); + errMsg += "\" is not implemented (yet)."; + throw(vpException(vpException::notImplementedError, errMsg)); + } +} + +void +vpCircleHoughTransform::edgeDetection(const vpImage &I) +{ + if (m_algoParams.m_cannyBackendType == vpImageFilter::CANNY_VISP_BACKEND) { + // This is done to increase the time performances, because it avoids to + // recompute the gradient in the vpImageFilter::canny method + m_cannyVisp.setFilteringAndGradientType(m_algoParams.m_filteringAndGradientType); + m_cannyVisp.setCannyThresholds(m_algoParams.m_lowerCannyThresh, m_algoParams.m_upperCannyThresh); + m_cannyVisp.setCannyThresholdsRatio(m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio); + m_cannyVisp.setGradients(m_dIx, m_dIy); + m_cannyVisp.setMask(mp_mask); + m_edgeMap = m_cannyVisp.detect(I); + } + else { + if (mp_mask != nullptr) { + // Delete pixels that fall outside the mask + vpImage I_masked(I); + unsigned int nbRows = I_masked.getHeight(); + unsigned int nbCols = I_masked.getWidth(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + if (!((*mp_mask)[r][c])) { + I_masked[r][c] = 0; + } + } + } + + // We will have to recompute the gradient in the desired backend format anyway so we let + // the vpImageFilter::canny method take care of it + vpImageFilter::canny(I_masked, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, + m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, + m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, + m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); + } + else { + vpImageFilter::canny(I, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, + m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, + m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, + m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); + } + } + + for (int i = 0; i < m_algoParams.m_edgeMapFilteringNbIter; ++i) { + filterEdgeMap(); + } +} + +void +vpCircleHoughTransform::filterEdgeMap() +{ + vpImage J = m_edgeMap; + const unsigned int height = J.getHeight(); + const unsigned int width = J.getWidth(); + const int minNbContiguousPts = 2; + + for (unsigned int i = 1; i < (height - 1); ++i) { + for (unsigned int j = 1; j < (width - 1); ++j) { + if (J[i][j] == vpCircleHoughTransform::edgeMapOn) { + // Consider 8 neighbors + int topLeftPixel = static_cast(J[i - 1][j - 1]); + int topPixel = static_cast(J[i - 1][j]); + int topRightPixel = static_cast(J[i - 1][j + 1]); + int botLeftPixel = static_cast(J[i + 1][j - 1]); + int bottomPixel = static_cast(J[i + 1][j]); + int botRightPixel = static_cast(J[i + 1][j + 1]); + int leftPixel = static_cast(J[i][j - 1]); + int rightPixel = static_cast(J[i][j + 1]); + if ((topLeftPixel + topPixel + topRightPixel + + botLeftPixel + bottomPixel + botRightPixel + + leftPixel + rightPixel + ) >= (minNbContiguousPts * static_cast(vpCircleHoughTransform::edgeMapOn))) { + // At least minNbContiguousPts of the 8-neighbor points are also an edge point + // so we keep the edge point + m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOn; + } + else { + // The edge point is isolated => we erase it + m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOff; + } + } + } + } +} + +void +vpCircleHoughTransform::computeCenterCandidates() +{ + // For each edge point EP_i, check the image gradient at EP_i + // Then, for each image point in the direction of the gradient, + // increment the accumulator + // We can perform bilinear interpolation in order not to vote for a "line" of + // points, but for an "area" of points + unsigned int nbRows = m_edgeMap.getRows(), nbCols = m_edgeMap.getCols(); + + // Computing the minimum and maximum horizontal position of the center candidates + // The miminum horizontal position of the center is at worst -maxRadius outside the image + // The maxinum horizontal position of the center is at worst +maxRadiusoutside the image + // The width of the accumulator is the difference between the max and the min + int minimumXposition = std::max(m_algoParams.m_centerXlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); + int maximumXposition = std::min(m_algoParams.m_centerXlimits.second, static_cast(m_algoParams.m_maxRadius + nbCols)); + minimumXposition = std::min(minimumXposition, maximumXposition - 1); + float minimumXpositionFloat = static_cast(minimumXposition); + float maximumXpositionFloat = static_cast(maximumXposition); + int offsetX = minimumXposition; + int accumulatorWidth = (maximumXposition - minimumXposition) + 1; + if (accumulatorWidth <= 0) { + throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator width <= 0!")); + } + + // Computing the minimum and maximum vertical position of the center candidates + // The miminum vertical position of the center is at worst -maxRadius outside the image + // The maxinum vertical position of the center is at worst +maxRadiusoutside the image + // The height of the accumulator is the difference between the max and the min + int minimumYposition = std::max(m_algoParams.m_centerYlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); + int maximumYposition = std::min(m_algoParams.m_centerYlimits.second, static_cast(m_algoParams.m_maxRadius + nbRows)); + minimumYposition = std::min(minimumYposition, maximumYposition - 1); + float minimumYpositionFloat = static_cast(minimumYposition); + float maximumYpositionFloat = static_cast(maximumYposition); + int offsetY = minimumYposition; + int accumulatorHeight = (maximumYposition - minimumYposition) + 1; + if (accumulatorHeight <= 0) { + throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator height <= 0!")); + } + + vpImage centersAccum(accumulatorHeight, accumulatorWidth + 1, 0.); /*!< Votes for the center candidates.*/ + + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + if (m_edgeMap[r][c] == vpCircleHoughTransform::edgeMapOn) { + // Voting for points in both direction of the gradient + // Step from min_radius to max_radius in both directions of the gradient + float mag = std::sqrt((m_dIx[r][c] * m_dIx[r][c]) + (m_dIy[r][c] * m_dIy[r][c])); + + float sx = 0.f, sy = 0.f; + if (std::abs(mag) >= std::numeric_limits::epsilon()) { + sx = m_dIx[r][c] / mag; + sy = m_dIy[r][c] / mag; + + // Saving the edge point for further use + m_edgePointsList.push_back(std::pair(r, c)); + + vpDataForAccumLoop data; + data.accumulatorHeight = accumulatorHeight; + data.accumulatorWidth = accumulatorWidth; + data.c = c; + data.maximumXpositionFloat = maximumXpositionFloat; + data.maximumYpositionFloat = maximumYpositionFloat; + data.maxRadius = m_algoParams.m_maxRadius; + data.minimumXpositionFloat = minimumXpositionFloat; + data.minimumYpositionFloat = minimumYpositionFloat; + data.minRadius = m_algoParams.m_minRadius; + data.offsetX = offsetX; + data.offsetY = offsetY; + data.r = r; + updateAccumAlongGradientDir(data, sx, sy, centersAccum); + } + } + } + } + + // Use dilatation with large kernel in order to determine the + // accumulator maxima + vpImage centerCandidatesMaxima = centersAccum; + int dilatationKernelSize = std::max(m_algoParams.m_dilatationKernelSize, 3); // Ensure at least a 3x3 dilatation operation is performed + vpImageMorphology::dilatation(centerCandidatesMaxima, dilatationKernelSize); + + // Look for the image points that correspond to the accumulator maxima + // These points will become the center candidates + // find the possible circle centers + int nbColsAccum = centersAccum.getCols(); + int nbRowsAccum = centersAccum.getRows(); + int nbVotes = -1; + std::vector peak_positions_votes; + + for (int y = 0; y < nbRowsAccum; ++y) { + int left = -1; + for (int x = 0; x < nbColsAccum; ++x) { + if ((centersAccum[y][x] >= m_algoParams.m_centerMinThresh) + && (vpMath::equal(centersAccum[y][x], centerCandidatesMaxima[y][x])) + && (centersAccum[y][x] > centersAccum[y][x + 1]) + ) { + if (left < 0) { + left = x; + } + nbVotes = std::max(nbVotes, static_cast(centersAccum[y][x])); + } + else if (left >= 0) { + int cx = static_cast(((left + x) - 1) * 0.5f); + float sumVotes = 0., x_avg = 0., y_avg = 0.; + int averagingWindowHalfSize = m_algoParams.m_averagingWindowSize / 2; + int startingRow = std::max(0, y - averagingWindowHalfSize); + int startingCol = std::max(0, cx - averagingWindowHalfSize); + int endRow = std::min(accumulatorHeight, y + averagingWindowHalfSize + 1); + int endCol = std::min(accumulatorWidth, cx + averagingWindowHalfSize + 1); + for (int r = startingRow; r < endRow; ++r) { + for (int c = startingCol; c < endCol; ++c) { + sumVotes += centersAccum[r][c]; + x_avg += centersAccum[r][c] * c; + y_avg += centersAccum[r][c] * r; + } + } + float avgVotes = sumVotes / static_cast(m_algoParams.m_averagingWindowSize * m_algoParams.m_averagingWindowSize); + if (avgVotes > m_algoParams.m_centerMinThresh) { + x_avg /= static_cast(sumVotes); + y_avg /= static_cast(sumVotes); + std::pair position(y_avg + static_cast(offsetY), x_avg + static_cast(offsetX)); + vpCenterVotes position_vote; + position_vote.m_position = position; + position_vote.m_votes = avgVotes; + peak_positions_votes.push_back(position_vote); + } + if (nbVotes < 0) { + std::stringstream errMsg; + errMsg << "nbVotes (" << nbVotes << ") < 0, thresh = " << m_algoParams.m_centerMinThresh; + throw(vpException(vpException::badValue, errMsg.str())); + } + left = -1; + nbVotes = -1; + } + } + } + filterCenterCandidates(peak_positions_votes); +} + +void +vpCircleHoughTransform::filterCenterCandidates(const std::vector &peak_positions_votes) +{ + unsigned int nbPeaks = static_cast(peak_positions_votes.size()); + if (nbPeaks > 0) { + std::vector has_been_merged(nbPeaks, false); + std::vector merged_peaks_position_votes; + float squared_distance_max = m_algoParams.m_centerMinDist * m_algoParams.m_centerMinDist; + for (unsigned int idPeak = 0; idPeak < nbPeaks; ++idPeak) { + float votes = peak_positions_votes[idPeak].m_votes; + // Ignoring peak that has already been merged + if (!has_been_merged[idPeak]) { + if (votes < m_algoParams.m_centerMinThresh) { + // Ignoring peak whose number of votes is lower than the threshold + has_been_merged[idPeak] = true; + } + else { + vpCentersBarycenter barycenter = mergeSimilarCenters(idPeak, nbPeaks, squared_distance_max, peak_positions_votes, has_been_merged); + + float avg_votes = barycenter.m_totalVotes / barycenter.m_nbElectors; + // Only the centers having enough votes are considered + if (avg_votes > m_algoParams.m_centerMinThresh) { + barycenter.m_position.first /= barycenter.m_totalVotes; + barycenter.m_position.second /= barycenter.m_totalVotes; + vpCenterVotes barycenter_votes; + barycenter_votes.m_position = barycenter.m_position; + barycenter_votes.m_votes = avg_votes; + merged_peaks_position_votes.push_back(barycenter_votes); + } + } + } + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + auto sortingCenters = [](const vpCenterVotes &position_vote_a, + const vpCenterVotes &position_vote_b) { + return position_vote_a.m_votes > position_vote_b.m_votes; + }; +#endif + + std::sort(merged_peaks_position_votes.begin(), merged_peaks_position_votes.end(), sortingCenters); + + nbPeaks = static_cast(merged_peaks_position_votes.size()); + int nbPeaksToKeep = (m_algoParams.m_expectedNbCenters > 0 ? m_algoParams.m_expectedNbCenters : static_cast(nbPeaks)); + nbPeaksToKeep = std::min(nbPeaksToKeep, static_cast(nbPeaks)); + for (int i = 0; i < nbPeaksToKeep; ++i) { + m_centerCandidatesList.push_back(merged_peaks_position_votes[i].m_position); + m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].m_votes)); + } + } + } + +vpCircleHoughTransform::vpCentersBarycenter +vpCircleHoughTransform::mergeSimilarCenters(const unsigned int &idPeak, const unsigned int &nbPeaks, const float &squared_distance_max, const std::vector &peak_positions_votes, std::vector &has_been_merged) +{ + std::pair position = peak_positions_votes[idPeak].m_position; + vpCentersBarycenter barycenter; + barycenter.m_position.first = position.first * peak_positions_votes[idPeak].m_votes; + barycenter.m_position.second = position.second * peak_positions_votes[idPeak].m_votes; + barycenter.m_totalVotes = peak_positions_votes[idPeak].m_votes; + barycenter.m_nbElectors = 1.f; + // Looking for potential similar peak in the following peaks + for (unsigned int idCandidate = idPeak + 1; idCandidate < nbPeaks; ++idCandidate) { + float votes_candidate = peak_positions_votes[idCandidate].m_votes; + // Ignoring peaks that have already been merged + if (!has_been_merged[idCandidate]) { + if (votes_candidate < m_algoParams.m_centerMinThresh) { + // Ignoring peak whose number of votes is lower than the threshold + has_been_merged[idCandidate] = true; + } + else { + // Computing the distance with the peak of insterest + std::pair position_candidate = peak_positions_votes[idCandidate].m_position; + float squared_distance = ((position.first - position_candidate.first) * (position.first - position_candidate.first)) + + ((position.second - position_candidate.second) * (position.second - position_candidate.second)); + + // If the peaks are similar, update the barycenter peak between them and corresponding votes + if (squared_distance < squared_distance_max) { + barycenter.m_position.first += position_candidate.first * votes_candidate; + barycenter.m_position.second += position_candidate.second * votes_candidate; + barycenter.m_totalVotes += votes_candidate; + barycenter.m_nbElectors += 1.f; + has_been_merged[idCandidate] = true; + } + } + } + } + return barycenter; +} + +END_VISP_NAMESPACE diff --git a/modules/imgproc/src/vpCircleHoughTransform_circles.cpp b/modules/imgproc/src/vpCircleHoughTransform_circles.cpp new file mode 100644 index 0000000000..844f853027 --- /dev/null +++ b/modules/imgproc/src/vpCircleHoughTransform_circles.cpp @@ -0,0 +1,387 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + +namespace +{ +#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) +float computeEffectiveRadius(const float &votes, const float &weigthedSumRadius) +{ + float r_effective = -1.f; + if (votes > std::numeric_limits::epsilon()) { + r_effective = weigthedSumRadius / votes; + } + return r_effective; +} +#endif + +typedef struct vpDataUpdateRadAccum +{ + int m_nbBins; + std::pair m_centerCandidate; + std::pair m_edgePoint; + const vpImage &m_dIx; + const vpImage &m_dIy; + float m_rx; + float m_ry; + float m_circlePerfectness2; + float m_r2; + float m_minRadius; + float m_mergingRadiusDiffThresh; + bool m_recordVotingPoints; + + vpDataUpdateRadAccum(const vpImage &dIx, const vpImage &dIy) + : m_dIx(dIx) + , m_dIy(dIy) + { } +} vpDataUpdateRadAccum; + +void +updateRadiusAccumulator(const vpDataUpdateRadAccum &data, std::vector &radiusAccumList, + std::vector &radiusActualValueList, + std::vector > > &votingPoints) +{ + float gx = data.m_dIx[data.m_edgePoint.first][data.m_edgePoint.second]; + float gy = data.m_dIy[data.m_edgePoint.first][data.m_edgePoint.second]; + float grad2 = (gx * gx) + (gy * gy); + float scalProd = (data.m_rx * gx) + (data.m_ry * gy); + float scalProd2 = scalProd * scalProd; + if (scalProd2 >= (data.m_circlePerfectness2 * data.m_r2 * grad2)) { + // Look for the Radius Candidate Bin RCB_k to which d_ij is "the closest" will have an additionnal vote + float r = static_cast(std::sqrt(data.m_r2)); + int r_bin = static_cast(std::floor((r - data.m_minRadius) / data.m_mergingRadiusDiffThresh)); + r_bin = std::min(r_bin, data.m_nbBins - 1); + if ((r < (data.m_minRadius + (data.m_mergingRadiusDiffThresh * 0.5f))) + || (r >(data.m_minRadius + (data.m_mergingRadiusDiffThresh * (static_cast(data.m_nbBins - 1) + 0.5f))))) { + // If the radius is at the very beginning of the allowed radii or at the very end, we do not span the vote + radiusAccumList[r_bin] += 1.f; + radiusActualValueList[r_bin] += r; + if (data.m_recordVotingPoints) { + votingPoints[r_bin].push_back(data.m_edgePoint); + } + } + else { + float midRadiusPrevBin = data.m_minRadius + (data.m_mergingRadiusDiffThresh * ((r_bin - 1.f) + 0.5f)); + float midRadiusCurBin = data.m_minRadius + (data.m_mergingRadiusDiffThresh * (r_bin + 0.5f)); + float midRadiusNextBin = data.m_minRadius + (data.m_mergingRadiusDiffThresh * (r_bin + 1.f + 0.5f)); + + if ((r >= midRadiusCurBin) && (r <= midRadiusNextBin)) { + // The radius is at the end of the current bin or beginning of the next, we span the vote with the next bin + float voteCurBin = (midRadiusNextBin - r) / data.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the current bin + float voteNextBin = 1.f - voteCurBin; + radiusAccumList[r_bin] += voteCurBin; + radiusActualValueList[r_bin] += r * voteCurBin; + radiusAccumList[r_bin + 1] += voteNextBin; + radiusActualValueList[r_bin + 1] += r * voteNextBin; + if (data.m_recordVotingPoints) { + votingPoints[r_bin].push_back(data.m_edgePoint); + votingPoints[r_bin + 1].push_back(data.m_edgePoint); + } + } + else { + // The radius is at the end of the previous bin or beginning of the current, we span the vote with the previous bin + float votePrevBin = (r - midRadiusPrevBin) / data.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the previous bin + float voteCurBin = 1.f - votePrevBin; + radiusAccumList[r_bin] += voteCurBin; + radiusActualValueList[r_bin] += r * voteCurBin; + radiusAccumList[r_bin - 1] += votePrevBin; + radiusActualValueList[r_bin - 1] += r * votePrevBin; + if (data.m_recordVotingPoints) { + votingPoints[r_bin].push_back(data.m_edgePoint); + votingPoints[r_bin - 1].push_back(data.m_edgePoint); + } + } + } + } +} +} + +void +vpCircleHoughTransform::computeCircleCandidates() +{ + size_t nbCenterCandidates = m_centerCandidatesList.size(); + int nbBins = static_cast(((m_algoParams.m_maxRadius - m_algoParams.m_minRadius) + 1) / m_algoParams.m_mergingRadiusDiffThresh); + nbBins = std::max(static_cast(1), nbBins); // Avoid having 0 bins, which causes segfault + std::vector radiusAccumList; // Radius accumulator for each center candidates. + std::vector radiusActualValueList; // Vector that contains the actual distance between the edge points and the center candidates. + std::vector > > votingPoints(nbBins); // Vectors that contain the points voting for each radius bin + + float rmin2 = m_algoParams.m_minRadius * m_algoParams.m_minRadius; + float rmax2 = m_algoParams.m_maxRadius * m_algoParams.m_maxRadius; + float circlePerfectness2 = m_algoParams.m_circlePerfectness * m_algoParams.m_circlePerfectness; + + vpDataUpdateRadAccum data(m_dIx, m_dIy); + data.m_nbBins = nbBins; + data.m_circlePerfectness2 = circlePerfectness2; + data.m_minRadius = m_algoParams.m_minRadius; + data.m_mergingRadiusDiffThresh = m_algoParams.m_mergingRadiusDiffThresh; + data.m_recordVotingPoints = m_algoParams.m_recordVotingPoints; + + for (size_t i = 0; i < nbCenterCandidates; ++i) { + std::pair centerCandidate = m_centerCandidatesList[i]; + // Initialize the radius accumulator of the candidate with 0s + radiusAccumList.clear(); + radiusAccumList.resize(nbBins, 0); + radiusActualValueList.clear(); + radiusActualValueList.resize(nbBins, 0.); + + const unsigned int nbEdgePoints = m_edgePointsList.size(); + for (unsigned int e = 0; e < nbEdgePoints; ++e) { + const std::pair &edgePoint = m_edgePointsList[e]; + + // For each center candidate CeC_i, compute the distance with each edge point EP_j d_ij = dist(CeC_i; EP_j) + float rx = edgePoint.second - centerCandidate.second; + float ry = edgePoint.first - centerCandidate.first; + float r2 = (rx * rx) + (ry * ry); + if ((r2 > rmin2) && (r2 < rmax2)) { + data.m_centerCandidate = centerCandidate; + data.m_edgePoint = edgePoint; + data.m_rx = rx; + data.m_ry = ry; + data.m_r2 = r2; + updateRadiusAccumulator(data, radiusAccumList, radiusActualValueList, votingPoints); + } + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + // Lambda to compute the effective radius (i.e. barycenter) of each radius bin + auto computeEffectiveRadius = [](const float &votes, const float &weigthedSumRadius) { + float r_effective = -1.f; + if (votes > std::numeric_limits::epsilon()) { + r_effective = weigthedSumRadius / votes; + } + return r_effective; + }; +#endif + + // Merging similar candidates + std::vector v_r_effective; // Vector of radius of each candidate after the merge step + std::vector v_votes_effective; // Vector of number of votes of each candidate after the merge step + std::vector > > v_votingPoints_effective; // Vector of voting points after the merge step + std::vector v_hasMerged_effective; // Vector indicating if merge has been performed for the different candidates + for (int idBin = 0; idBin < nbBins; ++idBin) { + float r_effective = computeEffectiveRadius(radiusAccumList[idBin], radiusActualValueList[idBin]); + float votes_effective = radiusAccumList[idBin]; + std::vector > votingPoints_effective = votingPoints[idBin]; + bool is_r_effective_similar = (r_effective > 0.f); + // Looking for potential similar radii in the following bins + // If so, compute the barycenter radius between them + int idCandidate = idBin + 1; + bool hasMerged = false; + while ((idCandidate < nbBins) && is_r_effective_similar) { + float r_effective_candidate = computeEffectiveRadius(radiusAccumList[idCandidate], radiusActualValueList[idCandidate]); + if (std::abs(r_effective_candidate - r_effective) < m_algoParams.m_mergingRadiusDiffThresh) { + r_effective = ((r_effective * votes_effective) + (r_effective_candidate * radiusAccumList[idCandidate])) / (votes_effective + radiusAccumList[idCandidate]); + votes_effective += radiusAccumList[idCandidate]; + radiusAccumList[idCandidate] = -.1f; + radiusActualValueList[idCandidate] = -1.f; + is_r_effective_similar = true; + if (m_algoParams.m_recordVotingPoints) { + // Move elements from votingPoints[idCandidate] to votingPoints_effective. + // votingPoints[idCandidate] is left in undefined but safe-to-destruct state. +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + votingPoints_effective.insert( + votingPoints_effective.end(), + std::make_move_iterator(votingPoints[idCandidate].begin()), + std::make_move_iterator(votingPoints[idCandidate].end()) + ); +#else + votingPoints_effective.insert( + votingPoints_effective.end(), + votingPoints[idCandidate].begin(), + votingPoints[idCandidate].end() + ); +#endif + hasMerged = true; + } + } + else { + is_r_effective_similar = false; + } + ++idCandidate; + } + + if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= (m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_PI_FLOAT * r_effective))) { + // Only the circles having enough votes and being visible enough are considered + v_r_effective.push_back(r_effective); + v_votes_effective.push_back(votes_effective); + if (m_algoParams.m_recordVotingPoints) { + v_votingPoints_effective.push_back(votingPoints_effective); + v_hasMerged_effective.push_back(hasMerged); + } + } + } + + unsigned int nbCandidates = static_cast(v_r_effective.size()); + for (unsigned int idBin = 0; idBin < nbCandidates; ++idBin) { + // If the circle of center CeC_i and radius RCB_k has enough votes, it is added to the list + // of Circle Candidates + float r_effective = v_r_effective[idBin]; + vpImageCircle candidateCircle(vpImagePoint(centerCandidate.first, centerCandidate.second), r_effective); + float proba = computeCircleProbability(candidateCircle, static_cast(v_votes_effective[idBin])); + if (proba > m_algoParams.m_circleProbaThresh) { + m_circleCandidates.push_back(candidateCircle); + m_circleCandidatesProbabilities.push_back(proba); + m_circleCandidatesVotes.push_back(static_cast(v_votes_effective[idBin])); + if (m_algoParams.m_recordVotingPoints) { + if (v_hasMerged_effective[idBin]) { + // Remove potential duplicated points + std::sort(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()); + v_votingPoints_effective[idBin].erase(std::unique(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()), v_votingPoints_effective[idBin].end()); + } + // Save the points + m_circleCandidatesVotingPoints.push_back(v_votingPoints_effective[idBin]); + } + } + } + } +} + +float +vpCircleHoughTransform::computeCircleProbability(const vpImageCircle &circle, const unsigned int &nbVotes) +{ + float proba(0.f); + float visibleArc(static_cast(nbVotes)); + float theoreticalLenght; + if (mp_mask != nullptr) { + theoreticalLenght = static_cast(circle.computePixelsInMask(*mp_mask)); + } + else { + theoreticalLenght = circle.computeArcLengthInRoI(vpRect(vpImagePoint(0, 0), m_edgeMap.getWidth(), m_edgeMap.getHeight())); + } + if (theoreticalLenght < std::numeric_limits::epsilon()) { + proba = 0.f; + } + else { + proba = std::min(visibleArc / theoreticalLenght, 1.f); + } + return proba; +} + +void +vpCircleHoughTransform::mergeCircleCandidates() +{ + std::vector circleCandidates = m_circleCandidates; + std::vector circleCandidatesVotes = m_circleCandidatesVotes; + std::vector circleCandidatesProba = m_circleCandidatesProbabilities; + std::vector > > circleCandidatesVotingPoints = m_circleCandidatesVotingPoints; + // First iteration of merge + mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); + + // Second iteration of merge + mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); + + // Saving the results + m_finalCircles = circleCandidates; + m_finalCircleVotes = circleCandidatesVotes; + m_finalCirclesProbabilities = circleCandidatesProba; + m_finalCirclesVotingPoints = circleCandidatesVotingPoints; +} + +void +vpCircleHoughTransform::mergeCandidates(std::vector &circleCandidates, std::vector &circleCandidatesVotes, + std::vector &circleCandidatesProba, std::vector > > &votingPoints) +{ + size_t nbCandidates = circleCandidates.size(); + size_t i = 0; + while (i < nbCandidates) { + vpImageCircle cic_i = circleCandidates[i]; + bool hasPerformedMerge = false; + // // For each other circle candidate CiC_j do: + size_t j = i + 1; + while (j < nbCandidates) { + vpImageCircle cic_j = circleCandidates[j]; + // // // Compute the similarity between CiC_i and CiC_j + double distanceBetweenCenters = vpImagePoint::distance(cic_i.getCenter(), cic_j.getCenter()); + double radiusDifference = std::abs(cic_i.getRadius() - cic_j.getRadius()); + bool areCirclesSimilar = ((distanceBetweenCenters < m_algoParams.m_centerMinDist) + && (radiusDifference < m_algoParams.m_mergingRadiusDiffThresh) + ); + + if (areCirclesSimilar) { + hasPerformedMerge = true; + // // // If the similarity exceeds a threshold, merge the circle candidates CiC_i and CiC_j and remove CiC_j of the list + unsigned int totalVotes = circleCandidatesVotes[i] + circleCandidatesVotes[j]; + float totalProba = circleCandidatesProba[i] + circleCandidatesProba[j]; + float newProba = 0.5f * totalProba; + float newRadius = ((cic_i.getRadius() * circleCandidatesProba[i]) + (cic_j.getRadius() * circleCandidatesProba[j])) / totalProba; + vpImagePoint newCenter = ((cic_i.getCenter() * circleCandidatesProba[i]) + (cic_j.getCenter() * circleCandidatesProba[j])) / totalProba; + cic_i = vpImageCircle(newCenter, newRadius); + circleCandidates[j] = circleCandidates[nbCandidates - 1]; + const unsigned int var2 = 2; + circleCandidatesVotes[i] = totalVotes / var2; // Compute the mean vote + circleCandidatesVotes[j] = circleCandidatesVotes[nbCandidates - 1]; + circleCandidatesProba[i] = newProba; + circleCandidatesProba[j] = circleCandidatesProba[nbCandidates - 1]; + if (m_algoParams.m_recordVotingPoints) { +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + votingPoints[i].insert( + votingPoints[i].end(), + std::make_move_iterator(votingPoints[j].begin()), + std::make_move_iterator(votingPoints[j].end()) + ); +#else + votingPoints[i].insert( + votingPoints[i].end(), + votingPoints[j].begin(), + votingPoints[j].end() + ); +#endif + votingPoints.pop_back(); + } + circleCandidates.pop_back(); + circleCandidatesVotes.pop_back(); + circleCandidatesProba.pop_back(); + --nbCandidates; + // We do not update j because the new j-th candidate has not been evaluated yet + } + else { + // We will evaluate the next candidate + ++j; + } + } + // // Add the circle candidate CiC_i, potentially merged with other circle candidates, to the final list of detected circles + circleCandidates[i] = cic_i; + if (hasPerformedMerge && m_algoParams.m_recordVotingPoints) { + // Remove duplicated points + std::sort(votingPoints[i].begin(), votingPoints[i].end()); + votingPoints[i].erase(std::unique(votingPoints[i].begin(), votingPoints[i].end()), votingPoints[i].end()); + } + ++i; + } +} + +END_VISP_NAMESPACE diff --git a/modules/imgproc/src/vpCircleHoughTransform_common.cpp b/modules/imgproc/src/vpCircleHoughTransform_common.cpp new file mode 100644 index 0000000000..11114df4e0 --- /dev/null +++ b/modules/imgproc/src/vpCircleHoughTransform_common.cpp @@ -0,0 +1,419 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + +#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) +namespace +{ + +// Sorting by decreasing probabilities +bool hasBetterProba(std::pair a, std::pair b) +{ + return (a.second > b.second); +} + + +void +scaleFilter(vpArray2D &filter, const float &scale) +{ + unsigned int nbRows = filter.getRows(); + unsigned int nbCols = filter.getCols(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + filter[r][c] = filter[r][c] * scale; + } + } +} +} +#endif + +vpCircleHoughTransform::vpCircleHoughTransform() + : m_algoParams() + , mp_mask(nullptr) +{ + initGaussianFilters(); + initGradientFilters(); +} + +vpCircleHoughTransform::vpCircleHoughTransform(const vpCircleHoughTransformParams &algoParams) + : m_algoParams(algoParams) + , mp_mask(nullptr) +{ + initGaussianFilters(); + initGradientFilters(); +} + +void +vpCircleHoughTransform::init(const vpCircleHoughTransformParams &algoParams) +{ + m_algoParams = algoParams; + initGaussianFilters(); + initGradientFilters(); +} + +vpCircleHoughTransform::~vpCircleHoughTransform() +{ } + +#ifdef VISP_HAVE_NLOHMANN_JSON +using json = nlohmann::json; + +vpCircleHoughTransform::vpCircleHoughTransform(const std::string &jsonPath) +{ + initFromJSON(jsonPath); +} + +void +vpCircleHoughTransform::initFromJSON(const std::string &jsonPath) +{ + std::ifstream file(jsonPath); + if (!file.good()) { + std::stringstream ss; + ss << "Problem opening file " << jsonPath << ". Make sure it exists and is readable" << std::endl; + throw vpException(vpException::ioError, ss.str()); + } + json j; + try { + j = json::parse(file); + } + catch (json::parse_error &e) { + std::stringstream msg; + msg << "Could not parse JSON file : \n"; + + msg << e.what() << std::endl; + msg << "Byte position of error: " << e.byte; + throw vpException(vpException::ioError, msg.str()); + } + m_algoParams = j; // Call from_json(const json& j, vpDetectorDNN& *this) to read json + file.close(); + initGaussianFilters(); + initGradientFilters(); +} + +void +vpCircleHoughTransform::saveConfigurationInJSON(const std::string &jsonPath) const +{ + m_algoParams.saveConfigurationInJSON(jsonPath); +} +#endif + +void +vpCircleHoughTransform::initGaussianFilters() +{ + const int filterHalfSize = (m_algoParams.m_gaussianKernelSize + 1) / 2; + m_fg.resize(1, filterHalfSize); + vpImageFilter::getGaussianKernel(m_fg.data, m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev, true); + m_cannyVisp.setGaussianFilterParameters(m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev); +} + +void +vpCircleHoughTransform::initGradientFilters() +{ +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) // Check if cxx11 or higher + // Helper to apply the scale to the raw values of the filters + auto scaleFilter = [](vpArray2D &filter, const float &scale) { + const unsigned int nbRows = filter.getRows(); + const unsigned int nbCols = filter.getCols(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + filter[r][c] = filter[r][c] * scale; + } + } + }; +#endif + + const int moduloCheckForOddity = 2; + if ((m_algoParams.m_gradientFilterKernelSize % moduloCheckForOddity) != 1) { + throw vpException(vpException::badValue, "Gradient filters Kernel size should be odd."); + } + m_gradientFilterX.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); + m_gradientFilterY.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); + m_cannyVisp.setGradientFilterAperture(m_algoParams.m_gradientFilterKernelSize); + + float scaleX = 1.f; + float scaleY = 1.f; + unsigned int filterHalfSize = (m_algoParams.m_gradientFilterKernelSize - 1) / 2; + + if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) { + // Compute the Sobel filters + scaleX = vpImageFilter::getSobelKernelX(m_gradientFilterX.data, filterHalfSize); + scaleY = vpImageFilter::getSobelKernelY(m_gradientFilterY.data, filterHalfSize); + } + else if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING) { + // Compute the Scharr filters + scaleX = vpImageFilter::getScharrKernelX(m_gradientFilterX.data, filterHalfSize); + scaleY = vpImageFilter::getScharrKernelY(m_gradientFilterY.data, filterHalfSize); + } + else { + std::string errMsg = "[vpCircleHoughTransform::initGradientFilters] Error: gradient filtering method \""; + errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); + errMsg += "\" has not been implemented yet\n"; + throw vpException(vpException::notImplementedError, errMsg); + } + scaleFilter(m_gradientFilterX, scaleX); + scaleFilter(m_gradientFilterY, scaleY); +} + +std::vector +vpCircleHoughTransform::detect(const vpImage &I) +{ + vpImage I_gray; + vpImageConvert::convert(I, I_gray); + return detect(I_gray); +} + +#ifdef HAVE_OPENCV_CORE +std::vector +vpCircleHoughTransform::detect(const cv::Mat &cv_I) +{ + vpImage I_gray; + vpImageConvert::convert(cv_I, I_gray); + return detect(I_gray); +} +#endif + +std::vector +vpCircleHoughTransform::detect(const vpImage &I, const int &nbCircles) +{ + std::vector detections = detect(I); + size_t nbDetections = detections.size(); + + // Prepare vector of tuple to sort by decreasing probabilities + std::vector > v_id_proba; + for (size_t i = 0; i < nbDetections; ++i) { + std::pair id_proba(i, m_finalCirclesProbabilities[i]); + v_id_proba.push_back(id_proba); + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + // Sorting by decreasing probabilities + auto hasBetterProba + = [](std::pair a, std::pair b) { + return (a.second > b.second); + }; +#endif + std::sort(v_id_proba.begin(), v_id_proba.end(), hasBetterProba); + + // Clearing the storages containing the detection results + // to have it sorted by decreasing probabilities + size_t limitMin; + if (nbCircles < 0) { + limitMin = nbDetections; + } + else { + limitMin = std::min(nbDetections, static_cast(nbCircles)); + } + + std::vector bestCircles; + std::vector copyFinalCircles = m_finalCircles; + std::vector copyFinalCirclesVotes = m_finalCircleVotes; + std::vector copyFinalCirclesProbas = m_finalCirclesProbabilities; + std::vector > > copyFinalCirclesVotingPoints = m_finalCirclesVotingPoints; + for (size_t i = 0; i < nbDetections; ++i) { + size_t id = v_id_proba[i].first; + m_finalCircles[i] = copyFinalCircles[id]; + m_finalCircleVotes[i] = copyFinalCirclesVotes[id]; + m_finalCirclesProbabilities[i] = copyFinalCirclesProbas[id]; + if (m_algoParams.m_recordVotingPoints) { + m_finalCirclesVotingPoints[i] = copyFinalCirclesVotingPoints[id]; + } + if (i < limitMin) { + bestCircles.push_back(m_finalCircles[i]); + } + } + + return bestCircles; +} + +std::vector +vpCircleHoughTransform::detect(const vpImage &I) +{ + // Cleaning results of potential previous detection + m_centerCandidatesList.clear(); + m_centerVotes.clear(); + m_edgePointsList.clear(); + m_circleCandidates.clear(); + m_circleCandidatesVotes.clear(); + m_circleCandidatesProbabilities.clear(); + m_finalCircles.clear(); + m_finalCircleVotes.clear(); + + // Ensuring that the difference between the max and min radii is big enough to take into account + // the pixelization of the image + const float minRadiusDiff = 3.f; + if ((m_algoParams.m_maxRadius - m_algoParams.m_minRadius) < minRadiusDiff) { + if (m_algoParams.m_minRadius > (minRadiusDiff / 2.f)) { + m_algoParams.m_maxRadius += minRadiusDiff / 2.f; + m_algoParams.m_minRadius -= minRadiusDiff / 2.f; + } + else { + m_algoParams.m_maxRadius += minRadiusDiff - m_algoParams.m_minRadius; + m_algoParams.m_minRadius = 0.f; + } + } + + // Ensuring that the difference between the max and min center position is big enough to take into account + // the pixelization of the image + const float minCenterPositionDiff = 3.f; + if ((m_algoParams.m_centerXlimits.second - m_algoParams.m_centerXlimits.first) < minCenterPositionDiff) { + m_algoParams.m_centerXlimits.second += static_cast(minCenterPositionDiff / 2.f); + m_algoParams.m_centerXlimits.first -= static_cast(minCenterPositionDiff / 2.f); + } + if ((m_algoParams.m_centerYlimits.second - m_algoParams.m_centerYlimits.first) < minCenterPositionDiff) { + m_algoParams.m_centerYlimits.second += static_cast(minCenterPositionDiff / 2.f); + m_algoParams.m_centerYlimits.first -= static_cast(minCenterPositionDiff / 2.f); + } + + // First thing, we need to apply a Gaussian filter on the image to remove some spurious noise + // Then, we need to compute the image gradients in order to be able to perform edge detection + computeGradients(I); + + // Using the gradients, it is now possible to perform edge detection + // We rely on the Canny edge detector + // It will also give us the connected edged points + edgeDetection(I); + + // From the edge map and gradient information, it is possible to compute + // the center point candidates + computeCenterCandidates(); + + // From the edge map and center point candidates, we can compute candidate + // circles. These candidate circles are circles whose center belong to + // the center point candidates and whose radius is a "radius bin" that got + // enough votes by computing the distance between each point of the edge map + // and the center point candidate + computeCircleCandidates(); + + // Finally, we perform a merging operation that permits to merge circles + // respecting similarity criteria (distance between centers and similar radius) + mergeCircleCandidates(); + + return m_finalCircles; +} + +bool +operator==(const vpImageCircle &a, const vpImageCircle &b) +{ + vpImagePoint aCenter = a.getCenter(); + vpImagePoint bCenter = b.getCenter(); + bool haveSameCenter = (std::abs(aCenter.get_u() - bCenter.get_u()) + + std::abs(aCenter.get_v() - bCenter.get_v())) <= (2. * std::numeric_limits::epsilon()); + bool haveSameRadius = std::abs(a.getRadius() - b.getRadius()) <= (2.f * std::numeric_limits::epsilon()); + return (haveSameCenter && haveSameRadius); +} + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, + std::optional< vpImage > &mask, std::optional>>> &opt_votingPoints) const +#else +void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, + vpImage **mask, std::vector > > **opt_votingPoints) const +#endif +{ + if (!m_algoParams.m_recordVotingPoints) { + // We weren't asked to remember the voting points +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + mask = std::nullopt; + opt_votingPoints = std::nullopt; +#else + *mask = nullptr; + *opt_votingPoints = nullptr; +#endif + return; + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + mask = vpImage(I.getHeight(), I.getWidth(), false); + opt_votingPoints = std::vector>>(); +#else + *mask = new vpImage(I.getHeight(), I.getWidth(), false); + *opt_votingPoints = new std::vector > >(); +#endif + +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + for (const auto &detection : detections) +#else + const size_t nbDetections = detections.size(); + for (size_t i = 0; i < nbDetections; ++i) +#endif + { + bool hasFoundSimilarCircle = false; + unsigned int nbPreviouslyDetected = static_cast(m_finalCircles.size()); + unsigned int id = 0; + // Looking for a circle that was detected and is similar to the one given to the function + while ((id < nbPreviouslyDetected) && (!hasFoundSimilarCircle)) { + vpImageCircle previouslyDetected = m_finalCircles[id]; +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + if (previouslyDetected == detection) +#else + if (previouslyDetected == detections[i]) +#endif + { + hasFoundSimilarCircle = true; + // We found a circle that is similar to the one given to the function => updating the mask + const unsigned int nbVotingPoints = static_cast(m_finalCirclesVotingPoints[id].size()); + for (unsigned int idPoint = 0; idPoint < nbVotingPoints; ++idPoint) { + const std::pair &votingPoint = m_finalCirclesVotingPoints[id][idPoint]; +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + (*mask)[votingPoint.first][votingPoint.second] = true; +#else + (**mask)[votingPoint.first][votingPoint.second] = true; +#endif + } +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + opt_votingPoints->push_back(m_finalCirclesVotingPoints[id]); +#else + (**opt_votingPoints).push_back(m_finalCirclesVotingPoints[id]); +#endif + } + ++id; + } + } +} + +std::string +vpCircleHoughTransform::toString() const +{ + return m_algoParams.toString(); +} + +std::ostream & +operator<<(std::ostream &os, const vpCircleHoughTransform &detector) +{ + os << detector.toString(); + return os; +} + +END_VISP_NAMESPACE diff --git a/modules/python/examples/ukf-linear-example.py b/modules/python/examples/ukf-linear-example.py index b27b7dd7ef..8f105ccec7 100644 --- a/modules/python/examples/ukf-linear-example.py +++ b/modules/python/examples/ukf-linear-example.py @@ -115,12 +115,12 @@ def add_state_vectors(a, b) -> ColVector: def residual_state_vectors(a, b) -> ColVector: """ - Method that permits to substract a state vector to another. + Method that permits to subtract a state vector to another. - :param a: The first state vector to which another state vector must be substracted. - :param b: The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be subtracted. + :param b: The other state vector that must be subtracted to a. - :return ColVector: The substraction a - b. + :return ColVector: The subtraction a - b. """ return a - b diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index 29403ef912..b204a014b3 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -73,7 +73,7 @@ where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. - Additionnally, the addition and substraction of angles must be carefully done, as the result + Additionnally, the addition and subtraction of angles must be carefully done, as the result must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use the interval \f$[- \pi ; \pi ]\f$ . """ @@ -126,17 +126,17 @@ def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVecto mean[1::2] = orientations return ColVector(mean) -def measurement_residual(meas: ColVector, to_substract: ColVector) -> ColVector: +def measurement_residual(meas: ColVector, to_subtract: ColVector) -> ColVector: """ - Compute the substraction between two vectors expressed in the measurement space, + Compute the subtraction between two vectors expressed in the measurement space, such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... - :param meas: Measurement to which we must substract something. - :param toSubstract: The something we must substract. + :param meas: Measurement to which we must subtract something. + :param toSubtract: The something we must subtract. - :return ColVector: \b meas - \b toSubstract . + :return ColVector: \b meas - \b toSubtract . """ - res = meas.numpy() - to_substract.numpy() + res = meas.numpy() - to_subtract.numpy() res[1::2] = [normalize_angle(angle) for angle in res[1::2]] return ColVector(res) @@ -175,13 +175,13 @@ def state_mean_vectors(states: List[ColVector], wm: List[float]) -> ColVector: def state_residual_vectors(a, b) -> ColVector: """ - Compute the substraction between two vectors expressed in the state space, + Compute the subtraction between two vectors expressed in the state space, such as v[0] = x ; v[1] = y; v[2] = heading . - :param a: The first state vector to which another state vector must be substracted. - :param b: The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be subtracted. + :param b: The other state vector that must be subtracted to a. - :return ColVector: The substraction a - b. + :return ColVector: The subtraction a - b. """ res = a - b return ColVector([res[0], res[1], normalize_angle(res[2])]) @@ -453,9 +453,9 @@ def measure_with_noise(self, pos: ColVector) -> ColVector: sigma_bearing = Math.rad(0.5) # Standard deviation of the bearing angle: 0.5deg wheelbase = 0.5 # Wheelbase of 0.5m process_variance = 0.0001 - positions = [ (5, 10) , (10, 5), (15, 15), (20, 5), (0, 30), (50, 30), (40, 10)] # Positions of the landmarks constituing the grid - landmarks = [LandmarkMeasurements(x, y, sigma_range, sigma_bearing) for x,y in positions] # Vector of landmarks constituing the grid - nbLandmarks = len(landmarks) # Number of landmarks constituing the grid + positions = [ (5, 10) , (10, 5), (15, 15), (20, 5), (0, 30), (50, 30), (40, 10)] # Positions of the landmarks constituting the grid + landmarks = [LandmarkMeasurements(x, y, sigma_range, sigma_bearing) for x,y in positions] # Vector of landmarks constituting the grid + nbLandmarks = len(landmarks) # Number of landmarks constituting the grid cmds = generate_commands() nb_cmds = len(cmds) diff --git a/modules/python/examples/ukf-nonlinear-example.py b/modules/python/examples/ukf-nonlinear-example.py index bd7062f3a8..d795827069 100644 --- a/modules/python/examples/ukf-nonlinear-example.py +++ b/modules/python/examples/ukf-nonlinear-example.py @@ -107,17 +107,17 @@ def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVecto return ColVector([meanRange, mean_angle]) -def measurementResidual(meas: ColVector, to_substract: ColVector) -> ColVector: +def measurementResidual(meas: ColVector, to_subtract: ColVector) -> ColVector: """ - Compute the substraction between two vectors expressed in the measurement space, + Compute the subtraction between two vectors expressed in the measurement space, such as v[0] = range ; v[1] = elevation_angle - :param meas: Measurement to which we must substract something. - :param toSubstract: The something we must substract. + :param meas: Measurement to which we must subtract something. + :param toSubtract: The something we must subtract. - :return vpColVector: \b meas - \b toSubstract . + :return vpColVector: \b meas - \b toSubtract . """ - res_temp = meas - to_substract + res_temp = meas - to_subtract return ColVector([res_temp[0], normalize_angle(res_temp[1])]) def state_add_vectors(a, b) -> ColVector: @@ -133,12 +133,12 @@ def state_add_vectors(a, b) -> ColVector: def state_residual_vectors(a, b) -> ColVector: """ - Method that permits to substract a state vector to another. + Method that permits to subtract a state vector to another. - :param a: The first state vector to which another state vector must be substracted. - :param b: The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be subtracted. + :param b: The other state vector that must be subtracted to a. - :return ColVector: The substraction a - b. + :return ColVector: The subtraction a - b. """ return a - b diff --git a/modules/python/examples/yolo-centering-task-afma6.py b/modules/python/examples/yolo-centering-task-afma6.py index 8221727309..4844b185c2 100644 --- a/modules/python/examples/yolo-centering-task-afma6.py +++ b/modules/python/examples/yolo-centering-task-afma6.py @@ -74,12 +74,12 @@ def add_state_vectors(a, b) -> ColVector: def residual_state_vectors(a, b) -> ColVector: """ - @brief Method that permits to substract a state vector to another. + @brief Method that permits to subtract a state vector to another. - @param a The first state vector to which another state vector must be substracted. - @param b The other state vector that must be substracted to a. + @param a The first state vector to which another state vector must be subtracted. + @param b The other state vector that must be subtracted to a. - @return ColVector The substraction a - b. + @return ColVector The subtraction a - b. """ return a - b diff --git a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp index 7d013bc268..ed2657017f 100644 --- a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp +++ b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp @@ -724,8 +724,9 @@ void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, const vpImageTools::crop(bitmap, width, height, roi, I); break; case V4L2_RGB24_FORMAT: // tested - if (roi == vpRect()) + if (roi == vpRect()) { vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height); + } else { vpImage tmp(height, width); vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width * height); diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h index 8af6989702..752c6f8ce1 100644 --- a/modules/vision/include/visp3/vision/vpPose.h +++ b/modules/vision/include/visp3/vision/vpPose.h @@ -117,7 +117,7 @@ class VISP_EXPORT vpPose public: // Typedef a function that checks if a pose is valid. - typedef bool (*funcCheckValidityPose)(const vpHomogeneousMatrix &); + typedef bool (*FuncCheckValidityPose)(const vpHomogeneousMatrix &); /*! * Default constructor. @@ -183,7 +183,7 @@ class VISP_EXPORT vpPose * has the smallest residual. * - vpPose::RANSAC: Robust Ransac aproach (doesn't need an initialization) */ - bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, funcCheckValidityPose func = nullptr); + bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr); /*! * @brief Method that first computes the pose \b cMo using the linear approaches of Dementhon and Lagrange @@ -326,7 +326,7 @@ class VISP_EXPORT vpPose * The number of threads used can then be set with setNbParallelRansacThreads(). * Filter flag can be used with setRansacFilterFlag(). */ - bool poseRansac(vpHomogeneousMatrix &cMo, funcCheckValidityPose func = nullptr); + bool poseRansac(vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr); /*! * Compute the pose using virtual visual servoing approach and @@ -646,7 +646,7 @@ class VISP_EXPORT vpPose const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0, - funcCheckValidityPose func = nullptr); + FuncCheckValidityPose func = nullptr); #ifdef VISP_HAVE_HOMOGRAPHY /*! @@ -833,7 +833,7 @@ class VISP_EXPORT vpPose */ vpRansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_, double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_, - const std::vector &listOfUniquePoints_, funcCheckValidityPose func_) + const std::vector &listOfUniquePoints_, FuncCheckValidityPose func_) : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false), m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_), @@ -870,7 +870,7 @@ class VISP_EXPORT vpPose bool m_checkDegeneratePoints; //!< Flag to check for degenerate points vpHomogeneousMatrix m_cMo; //!< Estimated pose bool m_foundSolution; //!< Solution found - funcCheckValidityPose m_func; //!< Pointer to ransac function + FuncCheckValidityPose m_func; //!< Pointer to ransac function std::vector m_listOfUniquePoints; //!< List of unique points unsigned int m_nbInliers; //!< Number of inliers int m_ransacMaxTrials; //!< Ransac max trial number diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp index 6597e80974..9b42523239 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp @@ -643,7 +643,7 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub return 0; } -bool lmderMostInnerLoop(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n, +bool lmderMostInnerLoop(ComputeFunctionAndJacobian ptr_fcn, int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, unsigned int maxfev, double *diag, int nprint, int *info, unsigned int *nfev, int *ipvt, double *qtf, double *wa1, double *wa2, double *wa3, double *wa4, const double &gnorm, int &iter, double &delta, double &par, double &pnorm, @@ -810,11 +810,14 @@ bool lmderMostInnerLoop(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, } if (delta <= (xtol * xnorm)) { - *info = 2; + const int flagInfo = 2; + *info = flagInfo; } - if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0) && (*info == 2)) { - *info = 3; + const int info2 = 2; + if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0) && (*info == info2)) { + const int flagInfo = 3; + *info = flagInfo; } if (*info != 0) { @@ -839,19 +842,23 @@ bool lmderMostInnerLoop(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, */ if (*nfev >= maxfev) { - *info = 5; + const int flagInfo = 5; + *info = flagInfo; } if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && ((tol5 * ratio) <= 1.0)) { - *info = 6; + const int flagInfo = 6; + *info = flagInfo; } if (delta <= (epsmch * xnorm)) { - *info = 7; + const int flagInfo = 7; + *info = flagInfo; } if (gnorm <= epsmch) { - *info = 8; + const int flagInfo = 8; + *info = flagInfo; } if (*info != 0) { @@ -874,7 +881,7 @@ bool lmderMostInnerLoop(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, return false; } -int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n, +int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, double gtol, unsigned int maxfev, double *diag, int mode, const double factor, int nprint, int *info, unsigned int *nfev, int *njev, int *ipvt, double *qtf, double *wa1, double *wa2, double *wa3, double *wa4) @@ -934,7 +941,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, return count; } - if (mode == 2) { + const int mode2 = 2; + if (mode == mode2) { for (j = 0; j < n; ++j) { if (diag[j] <= 0.0) { /* @@ -991,6 +999,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, par = 0.0; iter = 1; + const int iflag2 = 2; /* * debut de la boucle la plus externe. @@ -1001,7 +1010,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * calcul de la matrice jacobienne. */ - iflag = 2; + iflag = iflag2; (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); @@ -1062,7 +1071,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, */ if (iter == 1) { - if (mode != 2) { + if (mode != mode2) { for (j = 0; j < n; ++j) { diag[j] = wa2[j]; if (std::fabs(wa2[j]) <= std::numeric_limits::epsilon()) { @@ -1142,7 +1151,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, */ if (gnorm <= gtol) { - *info = 4; + const int info4 = 4; + *info = info4; } if (*info != 0) { @@ -1166,7 +1176,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * remise a l'echelle si necessaire. */ - if (mode != 2) { + if (mode != mode2) { for (j = 0; j < n; ++j) { diag[j] = vpMath::maximum(diag[j], wa2[j]); } @@ -1183,7 +1193,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, return 0; } -int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n, +int lmder1(ComputeFunctionAndJacobian ptr_fcn, int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa) { const double factor = 100.0; @@ -1194,26 +1204,28 @@ int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, *info = 0; /* verification des parametres en entree qui causent des erreurs */ - - if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) || (lwa < ((5 * n) + m))) { - printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0), (lwa < ((5 * n) + m))); + const int lwaThresh = ((5 * n) + m); + if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) || (lwa < lwaThresh)) { + printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0), (lwa < lwaThresh)); return (-1); } /* appel a lmder */ - - maxfev = static_cast(100 * (n + 1)); + const int factor100 = 100; + maxfev = static_cast(factor100 * (n + 1)); ftol = tol; xtol = tol; gtol = 0.0; mode = 1; nprint = 0; + const int factor2 = 2, factor3 = 3, factor4 = 4, factor5 = 5; lmder(ptr_fcn, m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa, mode, factor, nprint, info, &nfev, &njev, - ipvt, &wa[n], &wa[2 * n], &wa[3 * n], &wa[4 * n], &wa[5 * n]); + ipvt, &wa[n], &wa[factor2 * n], &wa[factor3 * n], &wa[factor4 * n], &wa[factor5 * n]); - if (*info == 8) { - *info = 4; + const int info8 = 8, info4 = 4; + if (*info == info8) { + *info = info4; } return 0; diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h index b4a4008373..1a693cd13c 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h @@ -267,6 +267,11 @@ double VISP_EXPORT pythag(double a, double b); int VISP_EXPORT qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int lipvt, double *rdiag, double *acnorm, double *wa); +/** + * \brief Function pointer towards a method that evaluates a function and its Jacobian. + */ +typedef void (*ComputeFunctionAndJacobian)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag); + /* * PROCEDURE : lmder * @@ -372,7 +377,7 @@ int VISP_EXPORT qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, i * En cas de succes, la valeur zero est retournee. * Sinon la valeur -1 est retournee. */ -int VISP_EXPORT lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), +int VISP_EXPORT lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, double gtol, unsigned int maxfev, double *diag, int mode, const double factor, int nprint, int *info, unsigned int *nfev, int *njev, int *ipvt, double *qtf, double *wa1, double *wa2, @@ -457,7 +462,7 @@ int VISP_EXPORT lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, d * En cas de succes, la valeur zero est retournee. * Sinon, la valeur -1. */ -int VISP_EXPORT lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), +int VISP_EXPORT lmder1(ComputeFunctionAndJacobian ptr_fcn, int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa); diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index 6bee3a938f..d237d63869 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -206,13 +206,14 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double z2 = P2.get_oZ(); z3 = P3.get_oZ(); + const int idX = 0, idY = 1, idZ = 2; vpColVector a_b(3), b_c(3), cross_prod; - a_b[0] = x1 - x2; - a_b[1] = y1 - y2; - a_b[2] = z1 - z2; - b_c[0] = x2 - x3; - b_c[1] = y2 - y3; - b_c[2] = z2 - z3; + a_b[idX] = x1 - x2; + a_b[idY] = y1 - y2; + a_b[idZ] = z1 - z2; + b_c[idX] = x2 - x3; + b_c[idY] = y2 - y3; + b_c[idZ] = z2 - z3; cross_prod = vpColVector::crossProd(a_b, b_c); if (cross_prod.sumSquare() <= std::numeric_limits::epsilon()) { @@ -381,7 +382,7 @@ void vpPose::callLagrangePose(vpHomogeneousMatrix &cMo) } } -bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, funcCheckValidityPose func) +bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func) { const int minNbPtDementhon = 4; const int minNbPtRansac = 4; diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index 41349a9b98..feee63caea 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -198,9 +198,9 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ++cpt; } // go back to the initial frame - cMo[idX][3] -= ((cdg[idX] * cMo[idX][idX]) + (cdg[idY] * cMo[idX][idY]) + (cdg[idZ] * cMo[idX][idZ])); - cMo[idY][3] -= ((cdg[idX] * cMo[idY][idX]) + (cdg[idY] * cMo[idY][idY]) + (cdg[idZ] * cMo[idY][idZ])); - cMo[idZ][3] -= ((cdg[idX] * cMo[idZ][idX]) + (cdg[idY] * cMo[idZ][idY]) + (cdg[idZ] * cMo[idZ][idZ])); + cMo[idX][idTranslation] -= ((cdg[idX] * cMo[idX][idX]) + (cdg[idY] * cMo[idX][idY]) + (cdg[idZ] * cMo[idX][idZ])); + cMo[idY][idTranslation] -= ((cdg[idX] * cMo[idY][idX]) + (cdg[idY] * cMo[idY][idY]) + (cdg[idZ] * cMo[idY][idZ])); + cMo[idZ][idTranslation] -= ((cdg[idX] * cMo[idZ][idX]) + (cdg[idY] * cMo[idZ][idY]) + (cdg[idZ] * cMo[idZ][idZ])); } static void calculRTheta(double s, double c, double &r, double &theta) @@ -385,9 +385,9 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) std::list::const_iterator listp_end_decl2 = listP.end(); for (std::list::const_iterator it = listP.begin(); it != listp_end_decl2; ++it) { P = (*it); - P.set_oX(P.get_oX() - cdg[0]); - P.set_oY(P.get_oY() - cdg[1]); - P.set_oZ(P.get_oZ() - cdg[2]); + P.set_oX(P.get_oX() - cdg[idX]); + P.set_oY(P.get_oY() - cdg[idY]); + P.set_oZ(P.get_oZ() - cdg[idZ]); c3d.push_back(P); } diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp index c8885c9bf6..2ef27c04e4 100644 --- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp @@ -259,13 +259,14 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * unsigned int k = 0; unsigned int nl = npt * 2; - const unsigned int nbColsA = 3, nbColsB = 6; + const unsigned int nbColsA = 3U, nbColsB = 6U; vpMatrix A(nl, nbColsA); vpMatrix B(nl, nbColsB); vpPoint P; std::list::const_iterator listp_end = listP.end(); - const unsigned int idHomogeneous = 3, sizeHomogeneous = 4; + const unsigned int idHomogeneous = 3U, sizeHomogeneous = 4U; + const unsigned int offsetk = 2U; for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; @@ -299,7 +300,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * B[k + 1][index_4] = -1.0; B[k + 1][index_5] = P.get_y(); - k += 2; + k += offsetk; } const unsigned int sizeX1 = nbColsA, sizeX2 = nbColsB, lastX2 = sizeX2 - 1; // X1 is of the size of A^T A and X2 of B^T B vpColVector X1(sizeX1); @@ -349,7 +350,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * for (unsigned int i = 0; i < val_3; ++i) { cMf[i][index_0] = X1[i]; cMf[i][index_1] = X2[i]; - cMf[i][index_3] = X2[i + 3]; + cMf[i][index_3] = X2[i + index_3]; } // Apply the transform to go back to object frame @@ -383,6 +384,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) const unsigned int id0 = 0, id1 = 1, id2 = 2; const unsigned int id3 = 3, id4 = 4, id5 = 5; const unsigned int id6 = 6, id7 = 7, id8 = 8; + const unsigned int offsetk = 2U; for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; a[k][id0] = -P.get_oX(); @@ -417,7 +419,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) b[k + 1][id7] = -1.0; b[k + 1][id8] = P.get_y(); - k += 2; + k += offsetk; } vpColVector X1(nbColsA); // X1 is of size A^T A vpColVector X2(nbColsB); // X2 is of size B^T B @@ -444,7 +446,8 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) } s = 1.0 / sqrt(s); - for (i = 0; i < 3; ++i) { + const unsigned int istop = 3; + for (i = 0; i < istop; ++i) { X2[i] *= s; } /* X2^T X2 = 1 */ @@ -455,7 +458,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) const unsigned int nc1 = 3, nc3 = 6; calculTranslation(a, b, nl, nc1, nc3, X1, X2); - for (i = 0; i < 3; ++i) { + for (i = 0; i < id3; ++i) { cMo[i][id0] = X1[i]; cMo[i][id1] = X2[i]; cMo[i][id2] = X2[i + id3]; diff --git a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp index 651de31321..47be6fae92 100644 --- a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp @@ -202,7 +202,8 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con unsigned int nb_points_3d = static_cast(points_3d.size() / 3); - if (nb_points_3d > 4) { + const unsigned int minNbPoints = 4; + if (nb_points_3d > minNbPoints) { std::vector p, q; // Plane equation diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index a3641e850a..eefbe8cb92 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -68,16 +68,17 @@ struct CompareObjectPointDegenerate const unsigned int index_0 = 0; const unsigned int index_1 = 1; const unsigned int index_2 = 2; + const double val3 = 3.; bool rc = false; const double dist1 = (point1.get_oX() * point1.get_oX()) + (point1.get_oY() * point1.get_oY()) + (point1.get_oZ() * point1.get_oZ()); const double dist2 = (point2.get_oX() * point2.get_oX()) + (point2.get_oY() * point2.get_oY()) + (point2.get_oZ() * point2.get_oZ()); - if ((dist1 - dist2) < (-3 * EPS * EPS)) { + if ((dist1 - dist2) < (-val3 * EPS * EPS)) { return true; } - if ((dist1 - dist2) > (3 * EPS * EPS)) { + if ((dist1 - dist2) > (val3 * EPS * EPS)) { return false; } @@ -114,10 +115,11 @@ struct CompareImagePointDegenerate bool rc = false; const double dist1 = (point1.get_x() * point1.get_x()) + (point1.get_y() * point1.get_y()); const double dist2 = (point2.get_x() * point2.get_x()) + (point2.get_y() * point2.get_y()); - if ((dist1 - dist2) < (-2 * EPS * EPS)) { + const double val2 = 2.; + if ((dist1 - dist2) < (-val2 * EPS * EPS)) { return true; } - if ((dist1 - dist2) > (2 * EPS * EPS)) { + if ((dist1 - dist2) > (val2 * EPS * EPS)) { return false; } @@ -191,43 +193,46 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() std::vector usedPt(size, false); vpPose poseMin; - for (unsigned int i = 0; i < nbMinRandom;) { + unsigned int i = 0; + bool stop_loop = false; + while ((i < nbMinRandom) && (stop_loop == false)) { if (static_cast(std::count(usedPt.begin(), usedPt.end(), true)) == usedPt.size()) { - // All points was picked once, break otherwise we stay in an infinite loop - break; + // All points were picked once, break otherwise we stay in an infinite loop + stop_loop = true; } + if (!stop_loop) { + // Pick a point randomly + unsigned int r_ = m_uniRand.uniform(0, size); - // Pick a point randomly - unsigned int r_ = m_uniRand.uniform(0, size); - - while (usedPt[r_]) { - // If already picked, pick another point randomly - r_ = m_uniRand.uniform(0, size); - } - // Mark this point as already picked - usedPt[r_] = true; - vpPoint pt = m_listOfUniquePoints[r_]; - - bool degenerate = false; - if (m_checkDegeneratePoints) { - if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) != - poseMin.listOfPoints.end()) { - degenerate = true; + while (usedPt[r_]) { + // If already picked, pick another point randomly + r_ = m_uniRand.uniform(0, size); + } + // Mark this point as already picked + usedPt[r_] = true; + vpPoint pt = m_listOfUniquePoints[r_]; + + bool degenerate = false; + if (m_checkDegeneratePoints) { + if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) != + poseMin.listOfPoints.end()) { + degenerate = true; + } } - } - if (!degenerate) { - poseMin.addPoint(pt); - cur_randoms.push_back(r_); - // Increment the number of points picked - ++i; + if (!degenerate) { + poseMin.addPoint(pt); + cur_randoms.push_back(r_); + // Increment the number of points picked + ++i; + } } } bool stop_for_loop = false; if (poseMin.npt < nbMinRandom) { ++nbTrials; - stop_for_loop = true;; + stop_for_loop = true; } if (!stop_for_loop) { @@ -323,7 +328,7 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() return foundSolution; } -bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &)) +bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, FuncCheckValidityPose func) { // Check only for adding / removing problem // Do not take into account problem with element modification here @@ -572,7 +577,7 @@ void vpPose::findMatch(std::vector &p2D, std::vector &p3D, const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials, bool useParallelRansac, unsigned int nthreads, - bool (*func)(const vpHomogeneousMatrix &)) + FuncCheckValidityPose func) { vpPose pose;