diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index c06d02e1..7e274256 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -24,20 +24,11 @@ jobs: uses: actions/setup-python@v1 with: python-version: ${{ matrix.python-version }} - - name: Install CUDA - env: - cuda: "11.2" - run: | - source ./scripts/actions/install_cuda_ubuntu.sh - sudo ln -s /usr/local/cuda-${cuda}/ /usr/local/cuda - if [[ $? -eq 0 ]]; then - # Set paths for subsequent steps, using ${CUDA_PATH} - echo "Adding CUDA to CUDA_PATH, PATH and LD_LIBRARY_PATH" - echo "CUDA_PATH=${CUDA_PATH}" >> $GITHUB_ENV - echo "${CUDA_PATH}/bin" >> $GITHUB_PATH - echo "LD_LIBRARY_PATH=${CUDA_PATH}/lib:${LD_LIBRARY_PATH}" >> $GITHUB_ENV - fi - shell: bash + - name: Install CUDA + uses: Jimver/cuda-toolkit@v0.2.8 + id: cuda-toolkit + with: + cuda: '11.7.0' - name: Install dependencies run: | ./scripts/actions/install_deps_ubuntu.sh diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 51ffe100..7c610e5b 100644 --- a/.github/workflows/windows.yml +++ b/.github/workflows/windows.yml @@ -47,20 +47,11 @@ jobs: run: pip install -U conan - name: Install CUDA - env: - cuda: ${{ matrix.cuda }} - visual_studio: ${{ matrix.visual_studio }} - shell: powershell - run: | - # Install CUDA via a powershell script - .\scripts\actions\install_cuda_windows.ps1 - if ($?) { - # Set paths for subsequent steps, using $env:CUDA_PATH - echo "Adding CUDA to CUDA_PATH, CUDA_PATH_X_Y and PATH" - echo "CUDA_PATH=$env:CUDA_PATH" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append - echo "$env:CUDA_PATH_VX_Y=$env:CUDA_PATH" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append - echo "$env:CUDA_PATH/bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append - } + uses: Jimver/cuda-toolkit@v0.2.8 + id: cuda-toolkit + with: + cuda: '11.7.0' + - name: nvcc check shell: powershell run: | diff --git a/CMakeLists.txt b/CMakeLists.txt index e365d49d..27548003 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,7 +136,7 @@ if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) # Newer but not listed GPUs will JIT compile on launch. set(CMAKE_CUDA_ARCHITECTURES 61) endif() -target_compile_features(cupoch_flags INTERFACE cxx_std_14 $) +target_compile_features(cupoch_flags INTERFACE cxx_std_17 $) target_compile_options(cupoch_flags INTERFACE "$<$:--expt-relaxed-constexpr>" "$<$:--expt-extended-lambda>" diff --git a/README.md b/README.md index 00371a69..ded11caa 100644 --- a/README.md +++ b/README.md @@ -58,7 +58,7 @@ This repository is based on [Open3D](https://github.com/intel-isl/Open3D). ## Installation -This library is packaged under 64 Bit Ubuntu Linux 20.04 and CUDA 11.2. +This library is packaged under 64 Bit Ubuntu Linux 20.04 and CUDA 11.7. You can install cupoch using pip. ``` @@ -87,17 +87,16 @@ sudo apt-get install libxinerama-dev libxcursor-dev libglu1-mesa-dev pip3 install cupoch ``` -Or you can compile it from source. -Update your version of cmake if necessary. +Or you can compile it from source. Update your version of cmake if necessary. ``` -wget https://github.com/Kitware/CMake/releases/download/v3.16.3/cmake-3.16.3.tar.gz -tar zxvf cmake-3.16.3.tar.gz -cd cmake-3.16.3 +wget https://github.com/Kitware/CMake/releases/download/v3.18.4/cmake-3.18.4.tar.gz +tar zxvf cmake-3.18.4.tar.gz +cd cmake-3.18.4 ./bootstrap -- -DCMAKE_USE_OPENSSL=OFF make && sudo make install cd .. -git clone https://github.com/neka-nat/cupoch.git --recurse +git clone -b jetson_nano https://github.com/neka-nat/cupoch.git --recurse cd cupoch/ mkdir build cd build/ diff --git a/conanfile.py b/conanfile.py index 52e98d3c..0973ff65 100644 --- a/conanfile.py +++ b/conanfile.py @@ -10,7 +10,7 @@ class CupochConan(ConanFile): name = "cupoch" - version = "0.2.6" + version = "0.2.7.2" package_type = "library" license = "MIT" diff --git a/examples/python/advanced/laserscan_convert.py b/examples/python/advanced/laserscan_convert.py new file mode 100644 index 00000000..62929c0e --- /dev/null +++ b/examples/python/advanced/laserscan_convert.py @@ -0,0 +1,20 @@ +import cupoch as cph +import numpy as np + + +if __name__ == "__main__": + # From point cloud demo + pcd = cph.io.read_point_cloud("../../testdata/fragment.ply") + laserscan = cph.geometry.LaserScanBuffer.create_from_point_cloud(pcd, 0.01, 0, 10) + print(laserscan.num_scans) + converted_pcd = cph.geometry.PointCloud.create_from_laserscanbuffer(laserscan, 0.0, 1000.0) + cph.visualization.draw_geometries([pcd, converted_pcd]) + + # From depth image demo + pinhole_camera_intrinsic = cph.io.read_pinhole_camera_intrinsic("../../testdata/camera_primesense.json") + print(pinhole_camera_intrinsic.intrinsic_matrix) + image = cph.io.read_image("../../testdata/rgbd/depth/00000.png") + pcd = cph.geometry.PointCloud.create_from_depth_image(image, pinhole_camera_intrinsic, depth_scale=1000) + laserscan = cph.geometry.LaserScanBuffer.create_from_depth_image(image, pinhole_camera_intrinsic, 0.01, 0, 10, depth_scale=1000) + converted_pcd = cph.geometry.PointCloud.create_from_laserscanbuffer(laserscan, 0.0, 1000.0) + cph.visualization.draw_geometries([pcd, converted_pcd]) diff --git a/examples/python/ros/laserscan.py b/examples/python/ros/laserscan.py index 6d693b24..e7615a9a 100644 --- a/examples/python/ros/laserscan.py +++ b/examples/python/ros/laserscan.py @@ -1,7 +1,7 @@ import numpy as np import cupoch as cph import rosbag -import tf.transformations as tftrans +import transformations as tftrans bagfile = "../../testdata/Mapping1.bag" bag = rosbag.Bag(bagfile) @@ -30,8 +30,12 @@ if not initialize: scan = cph.geometry.LaserScanBuffer(len(msg.ranges), n_buf, msg.angle_min, msg.angle_max) initialize = True - if not scan is None: - scan.add_ranges(cph.utility.FloatVector(msg.ranges), trans, cph.utility.FloatVector(msg.intensities)) + if scan is not None: + scan.add_host_ranges( + cph.utility.HostFloatVector(msg.ranges), + trans, + cph.utility.HostFloatVector(msg.intensities), + ) if initialize and frame % n_buf == 0: temp = cph.geometry.PointCloud.create_from_laserscanbuffer(scan, 0.0, 10.0) diff --git a/examples/python/ros/laserscan_msg_convert.py b/examples/python/ros/laserscan_msg_convert.py new file mode 100644 index 00000000..b67ef41d --- /dev/null +++ b/examples/python/ros/laserscan_msg_convert.py @@ -0,0 +1,47 @@ +import numpy as np +import cupoch as cph +import rosbag +from sensor_msgs.msg import LaserScan +import transformations as tftrans + + +bagfile = "../../testdata/Mapping1.bag" +bag = rosbag.Bag(bagfile) +initialize = False +scan = None +trans = np.identity(4) +frame = 0 +n_buf = 50 +for topic, msg, t in bag.read_messages(): + if topic == "/base_pose_ground_truth": + trans = tftrans.quaternion_matrix( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + trans[0:3, 3] = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z] + frame += 1 + + if topic == "/base_scan": + if not initialize: + scan = cph.geometry.LaserScanBuffer(len(msg.ranges), n_buf, msg.angle_min, msg.angle_max) + initialize = True + if scan is not None: + scan.add_host_ranges( + cph.utility.HostFloatVector(msg.ranges), + trans, + cph.utility.HostFloatVector(msg.intensities), + ) + + new_msg = LaserScan() + new_msg.angle_min = scan.min_angle + new_msg.angle_max = scan.max_angle + new_msg.angle_increment = (scan.max_angle - scan.min_angle) / scan.num_steps + new_msg.range_min = 0.0 + new_msg.range_max = 10000.0 + new_msg.ranges, new_msg.intensities = scan.pop_host_one_scan() + print("------ Original laserscan ------ \n", msg) + print("------ Converted laserscan ------\n", new_msg) diff --git a/scripts/actions/install_cuda_ubuntu.sh b/scripts/actions/install_cuda_ubuntu.sh deleted file mode 100644 index f3fa16ef..00000000 --- a/scripts/actions/install_cuda_ubuntu.sh +++ /dev/null @@ -1,196 +0,0 @@ -# @todo - better / more robust parsing of inputs from env vars. -## ------------------- -## Constants -## ------------------- - -# List of sub-packages to install. -# @todo - pass this in from outside the script? -# @todo - check the specified subpackages exist via apt pre-install? apt-rdepends cuda-9-0 | grep "^cuda-"? - -# Ideally choose from the list of meta-packages to minimise variance between cuda versions (although it does change too). Some of these packages may not be availble pre cuda 10. -CUDA_PACKAGES_IN=( - "cuda-compiler" - "cuda-cudart-dev" - "cuda-nvtx" - "cuda-nvrtc-dev" - "libcurand-dev" # 11-0+ - "cuda-cccl" # 11.4+, provides cub and thrust. On 11.3 knwon as cuda-thrust-11-3 -) - -## ------------------- -## Bash functions -## ------------------- -# returns 0 (true) if a >= b -function version_ge() { - [ "$#" != "2" ] && echo "${FUNCNAME[0]} requires exactly 2 arguments." && exit 1 - [ "$(printf '%s\n' "$@" | sort -V | head -n 1)" == "$2" ] -} -# returns 0 (true) if a > b -function version_gt() { - [ "$#" != "2" ] && echo "${FUNCNAME[0]} requires exactly 2 arguments." && exit 1 - [ "$1" = "$2" ] && return 1 || version_ge $1 $2 -} -# returns 0 (true) if a <= b -function version_le() { - [ "$#" != "2" ] && echo "${FUNCNAME[0]} requires exactly 2 arguments." && exit 1 - [ "$(printf '%s\n' "$@" | sort -V | head -n 1)" == "$1" ] -} -# returns 0 (true) if a < b -function version_lt() { - [ "$#" != "2" ] && echo "${FUNCNAME[0]} requires exactly 2 arguments." && exit 1 - [ "$1" = "$2" ] && return 1 || version_le $1 $2 -} - -## ------------------- -## Select CUDA version -## ------------------- - -# Get the cuda version from the environment as $cuda. -CUDA_VERSION_MAJOR_MINOR=${cuda} - -# Split the version. -# We (might/probably) don't know PATCH at this point - it depends which version gets installed. -CUDA_MAJOR=$(echo "${CUDA_VERSION_MAJOR_MINOR}" | cut -d. -f1) -CUDA_MINOR=$(echo "${CUDA_VERSION_MAJOR_MINOR}" | cut -d. -f2) -CUDA_PATCH=$(echo "${CUDA_VERSION_MAJOR_MINOR}" | cut -d. -f3) -# use lsb_release to find the OS. -UBUNTU_VERSION=$(lsb_release -sr) -UBUNTU_VERSION="${UBUNTU_VERSION//.}" - -echo "CUDA_MAJOR: ${CUDA_MAJOR}" -echo "CUDA_MINOR: ${CUDA_MINOR}" -echo "CUDA_PATCH: ${CUDA_PATCH}" -# echo "UBUNTU_NAME: ${UBUNTU_NAME}" -echo "UBUNTU_VERSION: ${UBUNTU_VERSION}" - -# If we don't know the CUDA_MAJOR or MINOR, error. -if [ -z "${CUDA_MAJOR}" ] ; then - echo "Error: Unknown CUDA Major version. Aborting." - exit 1 -fi -if [ -z "${CUDA_MINOR}" ] ; then - echo "Error: Unknown CUDA Minor version. Aborting." - exit 1 -fi -# If we don't know the Ubuntu version, error. -if [ -z ${UBUNTU_VERSION} ]; then - echo "Error: Unknown Ubuntu version. Aborting." - exit 1 -fi - - -## ------------------------------- -## Select CUDA packages to install -## ------------------------------- -CUDA_PACKAGES="" -for package in "${CUDA_PACKAGES_IN[@]}" -do : - # @todo This is not perfect. Should probably provide a separate list for diff versions - # cuda-compiler-X-Y if CUDA >= 9.1 else cuda-nvcc-X-Y - if [[ "${package}" == "cuda-nvcc" ]] && version_ge "$CUDA_VERSION_MAJOR_MINOR" "9.1" ; then - package="cuda-compiler" - elif [[ "${package}" == "cuda-compiler" ]] && version_lt "$CUDA_VERSION_MAJOR_MINOR" "9.1" ; then - package="cuda-nvcc" - # CUB/Thrust are packages in cuda-thrust in 11.3, but cuda-cccl in 11.4+ - elif [[ "${package}" == "cuda-thrust" || "${package}" == "cuda-cccl" ]]; then - # CUDA cuda-thrust >= 11.4 - if version_ge "$CUDA_VERSION_MAJOR_MINOR" "11.4" ; then - package="cuda-cccl" - # Use cuda-thrust > 11.2 - elif version_ge "$CUDA_VERSION_MAJOR_MINOR" "11.3" ; then - package="cuda-thrust" - # Do not include this pacakge < 11.3 - else - continue - fi - fi - # CUDA 11+ includes lib* / lib*-dev packages, which if they existed previously where cuda-cu*- / cuda-cu*-dev- - if [[ ${package} == libcu* ]] && version_lt "$CUDA_VERSION_MAJOR_MINOR" "11.0" ; then - package="${package/libcu/cuda-cu}" - fi - # Build the full package name and append to the string. - CUDA_PACKAGES+=" ${package}-${CUDA_MAJOR}-${CUDA_MINOR}" -done -echo "CUDA_PACKAGES ${CUDA_PACKAGES}" - -## ----------------- -## Prepare to install -## ----------------- -CPU_ARCH="x86_64" -PIN_FILENAME="cuda-ubuntu${UBUNTU_VERSION}.pin" -PIN_URL="https://developer.download.nvidia.com/compute/cuda/repos/ubuntu${UBUNTU_VERSION}/${CPU_ARCH}/${PIN_FILENAME}" -# apt keyring package now available https://developer.nvidia.com/blog/updating-the-cuda-linux-gpg-repository-key/ -KERYRING_PACKAGE_FILENAME="cuda-keyring_1.0-1_all.deb" -KEYRING_PACKAGE_URL="https://developer.download.nvidia.com/compute/cuda/repos/ubuntu${UBUNTU_VERSION}/${CPU_ARCH}/${KERYRING_PACKAGE_FILENAME}" -REPO_URL="https://developer.download.nvidia.com/compute/cuda/repos/ubuntu${UBUNTU_VERSION}/${CPU_ARCH}/" - -echo "PIN_FILENAME ${PIN_FILENAME}" -echo "PIN_URL ${PIN_URL}" -echo "KEYRING_PACKAGE_URL ${KEYRING_PACKAGE_URL}" -echo "APT_KEY_URL ${APT_KEY_URL}" - -## ----------------- -## Check for root/sudo -## ----------------- - -# Detect if the script is being run as root, storing true/false in is_root. -is_root=false -if (( $EUID == 0)); then - is_root=true -fi -# Find if sudo is available -has_sudo=false -if command -v sudo &> /dev/null ; then - has_sudo=true -fi -# Decide if we can proceed or not (root or sudo is required) and if so store whether sudo should be used or not. -if [ "$is_root" = false ] && [ "$has_sudo" = false ]; then - echo "Root or sudo is required. Aborting." - exit 1 -elif [ "$is_root" = false ] ; then - USE_SUDO=sudo -else - USE_SUDO= -fi - -## ----------------- -## Install -## ----------------- -echo "Adding CUDA Repository" -wget ${PIN_URL} -$USE_SUDO mv ${PIN_FILENAME} /etc/apt/preferences.d/cuda-repository-pin-600 -wget ${KEYRING_PACKAGE_URL} && ${USE_SUDO} dpkg -i ${KERYRING_PACKAGE_FILENAME} && rm ${KERYRING_PACKAGE_FILENAME} -$USE_SUDO add-apt-repository "deb ${REPO_URL} /" -$USE_SUDO apt-get update - -echo "Installing CUDA packages ${CUDA_PACKAGES}" -$USE_SUDO apt-get -y install ${CUDA_PACKAGES} - -if [[ $? -ne 0 ]]; then - echo "CUDA Installation Error." - exit 1 -fi - -## ----------------- -## Set environment vars / vars to be propagated -## ----------------- - -CUDA_PATH=/usr/local/cuda-${CUDA_MAJOR}.${CUDA_MINOR} -echo "CUDA_PATH=${CUDA_PATH}" -export CUDA_PATH=${CUDA_PATH} -export PATH="$CUDA_PATH/bin:$PATH" -export LD_LIBRARY_PATH="$CUDA_PATH/lib:$LD_LIBRARY_PATH" -export LD_LIBRARY_PATH="$CUDA_PATH/lib64:$LD_LIBRARY_PATH" -# Check nvcc is now available. -nvcc -V - - -# If executed on github actions, make the appropriate echo statements to update the environment -if [[ $GITHUB_ACTIONS ]]; then - # Set paths for subsequent steps, using ${CUDA_PATH} - echo "Adding CUDA to CUDA_PATH, PATH and LD_LIBRARY_PATH" - echo "CUDA_PATH=${CUDA_PATH}" >> $GITHUB_ENV - echo "${CUDA_PATH}/bin" >> $GITHUB_PATH - echo "LD_LIBRARY_PATH=${CUDA_PATH}/lib:${LD_LIBRARY_PATH}" >> $GITHUB_ENV - echo "LD_LIBRARY_PATH=${CUDA_PATH}/lib64:${LD_LIBRARY_PATH}" >> $GITHUB_ENV -fi \ No newline at end of file diff --git a/scripts/actions/install_cuda_windows.ps1 b/scripts/actions/install_cuda_windows.ps1 deleted file mode 100644 index 9fc155ab..00000000 --- a/scripts/actions/install_cuda_windows.ps1 +++ /dev/null @@ -1,172 +0,0 @@ -## ------------------- -## Constants -## ------------------- - -# Dictionary of known cuda versions and thier download URLS, which do not follow a consistent pattern :( -$CUDA_KNOWN_URLS = @{ - "8.0.44" = "http://developer.nvidia.com/compute/cuda/8.0/Prod/network_installers/cuda_8.0.44_win10_network-exe"; - "8.0.61" = "http://developer.nvidia.com/compute/cuda/8.0/Prod2/network_installers/cuda_8.0.61_win10_network-exe"; - "9.0.176" = "http://developer.nvidia.com/compute/cuda/9.0/Prod/network_installers/cuda_9.0.176_win10_network-exe"; - "9.1.85" = "http://developer.nvidia.com/compute/cuda/9.1/Prod/network_installers/cuda_9.1.85_win10_network"; - "9.2.148" = "http://developer.nvidia.com/compute/cuda/9.2/Prod2/network_installers2/cuda_9.2.148_win10_network"; - "10.0.130" = "http://developer.nvidia.com/compute/cuda/10.0/Prod/network_installers/cuda_10.0.130_win10_network"; - "10.1.105" = "http://developer.nvidia.com/compute/cuda/10.1/Prod/network_installers/cuda_10.1.105_win10_network.exe"; - "10.1.168" = "http://developer.nvidia.com/compute/cuda/10.1/Prod/network_installers/cuda_10.1.168_win10_network.exe"; - "10.1.243" = "http://developer.download.nvidia.com/compute/cuda/10.1/Prod/network_installers/cuda_10.1.243_win10_network.exe"; - "10.2.89" = "http://developer.download.nvidia.com/compute/cuda/10.2/Prod/network_installers/cuda_10.2.89_win10_network.exe"; - "11.0.1" = "http://developer.download.nvidia.com/compute/cuda/11.0.1/network_installers/cuda_11.0.1_win10_network.exe"; - "11.0.2" = "http://developer.download.nvidia.com/compute/cuda/11.0.2/network_installers/cuda_11.0.2_win10_network.exe"; - "11.0.3" = "http://developer.download.nvidia.com/compute/cuda/11.0.3/network_installers/cuda_11.0.3_win10_network.exe"; - "11.1.0" = "https://developer.download.nvidia.com/compute/cuda/11.1.0/network_installers/cuda_11.1.0_win10_network.exe"; - "11.1.1" = "https://developer.download.nvidia.com/compute/cuda/11.1.1/network_installers/cuda_11.1.1_win10_network.exe"; - "11.2.0" = "https://developer.download.nvidia.com/compute/cuda/11.2.0/network_installers/cuda_11.2.0_win10_network.exe"; - "11.2.1" = "https://developer.download.nvidia.com/compute/cuda/11.2.1/network_installers/cuda_11.2.1_win10_network.exe"; - "11.2.2" = "https://developer.download.nvidia.com/compute/cuda/11.2.2/network_installers/cuda_11.2.2_win10_network.exe"; - "11.3.0" = "https://developer.download.nvidia.com/compute/cuda/11.3.0/network_installers/cuda_11.3.0_win10_network.exe"; - "11.4.0" = "https://developer.download.nvidia.com/compute/cuda/11.4.0/network_installers/cuda_11.4.0_win10_network.exe"; - "11.4.1" = "https://developer.download.nvidia.com/compute/cuda/11.4.1/network_installers/cuda_11.4.1_win10_network.exe"; - "11.4.2" = "https://developer.download.nvidia.com/compute/cuda/11.4.2/network_installers/cuda_11.4.2_win10_network.exe"; - "11.4.3" = "https://developer.download.nvidia.com/compute/cuda/11.4.3/network_installers/cuda_11.4.3_win10_network.exe"; - "11.4.4" = "https://developer.download.nvidia.com/compute/cuda/11.4.4/network_installers/cuda_11.4.4_windows_network.exe" -} - -# @todo - change this to be based on _MSC_VER intead, or invert it to be CUDA keyed instead? -$VISUAL_STUDIO_MIN_CUDA = @{ - "2019" = "10.1"; - "2017" = "10.0"; # Depends on which version of 2017! 9.0 to 10.0 depending on version - "2015" = "8.0"; # might support older, unsure. -} - -# cuda_runtime.h is in nvcc <= 10.2, but cudart >= 11.0 -# @todo - make this easier to vary per CUDA version. -$CUDA_PACKAGES_IN = @( - "nvcc"; - "visual_studio_integration"; - "curand_dev"; - "nvrtc_dev"; - "cudart"; -) - - -## ------------------- -## Select CUDA version -## ------------------- - -# Get the cuda version from the environment as env:cuda. -$CUDA_VERSION_FULL = $env:cuda -# Make sure CUDA_VERSION_FULL is set and valid, otherwise error. - -# Validate CUDA version, extracting components via regex -$cuda_ver_matched = $CUDA_VERSION_FULL -match "^(?[1-9][0-9]*)\.(?[0-9]+)\.(?[0-9]+)$" -if(-not $cuda_ver_matched){ - Write-Output "Invalid CUDA version specified, .. required. '$CUDA_VERSION_FULL'." - exit 1 -} -$CUDA_MAJOR=$Matches.major -$CUDA_MINOR=$Matches.minor -$CUDA_PATCH=$Matches.patch - -## --------------------------- -## Visual studio support check -## --------------------------- -# Exit if visual studio is too new for the cuda version. -$VISUAL_STUDIO = $env:visual_studio.trim() -if ($VISUAL_STUDIO.length -ge 4) { -$VISUAL_STUDIO_YEAR = $VISUAL_STUDIO.Substring($VISUAL_STUDIO.Length-4) - if ($VISUAL_STUDIO_YEAR.length -eq 4 -and $VISUAL_STUDIO_MIN_CUDA.containsKey($VISUAL_STUDIO_YEAR)){ - $MINIMUM_CUDA_VERSION = $VISUAL_STUDIO_MIN_CUDA[$VISUAL_STUDIO_YEAR] - if ([version]$CUDA_VERSION_FULL -lt [version]$MINIMUM_CUDA_VERSION) { - Write-Output "Error: Visual Studio $($VISUAL_STUDIO_YEAR) requires CUDA >= $($MINIMUM_CUDA_VERSION)" - exit 1 - } - } -} else { - Write-Output "Warning: Unknown Visual Studio Version. CUDA version may be insufficient." -} - -## ------------------------------------------------ -## Select CUDA packages to install from environment -## ------------------------------------------------ - -$CUDA_PACKAGES = "" - -# for CUDA >= 11 cudart is a required package. -# if([version]$CUDA_VERSION_FULL -ge [version]"11.0") { -# if(-not $CUDA_PACKAGES_IN -contains "cudart") { -# $CUDA_PACKAGES_IN += 'cudart' -# } -# } - -Foreach ($package in $CUDA_PACKAGES_IN) { - # Make sure the correct package name is used for nvcc. - if($package -eq "nvcc" -and [version]$CUDA_VERSION_FULL -lt [version]"9.1"){ - $package="compiler" - } elseif($package -eq "compiler" -and [version]$CUDA_VERSION_FULL -ge [version]"9.1") { - $package="nvcc" - } - $CUDA_PACKAGES += " $($package)_$($CUDA_MAJOR).$($CUDA_MINOR)" - -} -echo "$($CUDA_PACKAGES)" -## ----------------- -## Prepare download -## ----------------- - -# Select the download link if known, otherwise have a guess. -$CUDA_REPO_PKG_REMOTE="" -if($CUDA_KNOWN_URLS.containsKey($CUDA_VERSION_FULL)){ - $CUDA_REPO_PKG_REMOTE=$CUDA_KNOWN_URLS[$CUDA_VERSION_FULL] -} else{ - # Guess what the url is given the most recent pattern (at the time of writing, 10.1) - Write-Output "note: URL for CUDA ${$CUDA_VERSION_FULL} not known, estimating." - $CUDA_REPO_PKG_REMOTE="http://developer.download.nvidia.com/compute/cuda/$($CUDA_MAJOR).$($CUDA_MINOR)/Prod/network_installers/cuda_$($CUDA_VERSION_FULL)_win10_network.exe" -} -$CUDA_REPO_PKG_LOCAL="cuda_$($CUDA_VERSION_FULL)_win10_network.exe" - - -## ------------ -## Install CUDA -## ------------ - -# Get CUDA network installer -Write-Output "Downloading CUDA Network Installer for $($CUDA_VERSION_FULL) from: $($CUDA_REPO_PKG_REMOTE)" -Invoke-WebRequest $CUDA_REPO_PKG_REMOTE -OutFile $CUDA_REPO_PKG_LOCAL | Out-Null -if(Test-Path -Path $CUDA_REPO_PKG_LOCAL){ - Write-Output "Downloading Complete" -} else { - Write-Output "Error: Failed to download $($CUDA_REPO_PKG_LOCAL) from $($CUDA_REPO_PKG_REMOTE)" - exit 1 -} - -# Invoke silent install of CUDA (via network installer) -Write-Output "Installing CUDA $($CUDA_VERSION_FULL). Subpackages $($CUDA_PACKAGES)" -Start-Process -Wait -FilePath .\"$($CUDA_REPO_PKG_LOCAL)" -ArgumentList "-s $($CUDA_PACKAGES)" - -# Check the return status of the CUDA installer. -if (!$?) { - Write-Output "Error: CUDA installer reported error. $($LASTEXITCODE)" - exit 1 -} - -# Store the CUDA_PATH in the environment for the current session, to be forwarded in the action. -$CUDA_PATH = "C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v$($CUDA_MAJOR).$($CUDA_MINOR)" -$CUDA_PATH_VX_Y = "CUDA_PATH_V$($CUDA_MAJOR)_$($CUDA_MINOR)" -# Set environmental variables in this session -$env:CUDA_PATH = "$($CUDA_PATH)" -$env:CUDA_PATH_VX_Y = "$($CUDA_PATH_VX_Y)" -Write-Output "CUDA_PATH $($CUDA_PATH)" -Write-Output "CUDA_PATH_VX_Y $($CUDA_PATH_VX_Y)" - -# PATH needs updating elsewhere, anything in here won't persist. -# Append $CUDA_PATH/bin to path. -# Set CUDA_PATH as an environmental variable - - -# If executing on github actions, emit the appropriate echo statements to update environment variables -if (Test-Path "env:GITHUB_ACTIONS") { - # Set paths for subsequent steps, using $env:CUDA_PATH - echo "Adding CUDA to CUDA_PATH, CUDA_PATH_X_Y and PATH" - echo "CUDA_PATH=$env:CUDA_PATH" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append - echo "$env:CUDA_PATH_VX_Y=$env:CUDA_PATH" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append - echo "$env:CUDA_PATH/bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append -} \ No newline at end of file diff --git a/src/cupoch/geometry/densegrid.h b/src/cupoch/geometry/densegrid.h index 81a7dc6b..e9c37a1a 100644 --- a/src/cupoch/geometry/densegrid.h +++ b/src/cupoch/geometry/densegrid.h @@ -35,7 +35,7 @@ class DenseGrid : public GeometryBase3D { DenseGrid(Geometry::GeometryType type); DenseGrid(Geometry::GeometryType type, float voxel_size, - int resolution, + size_t resolution, const Eigen::Vector3f &origin); DenseGrid(Geometry::GeometryType type, const DenseGrid &src_grid); virtual ~DenseGrid(); @@ -53,14 +53,14 @@ class DenseGrid : public GeometryBase3D { virtual DenseGrid &Scale(const float scale, bool center = true); virtual DenseGrid &Rotate(const Eigen::Matrix3f &R, bool center = true); - virtual DenseGrid &Reconstruct(float voxel_size, int resolution); + virtual DenseGrid &Reconstruct(float voxel_size, size_t resolution); int GetVoxelIndex(const Eigen::Vector3f &point) const; thrust::tuple GetVoxel(const Eigen::Vector3f &point) const; public: float voxel_size_ = 0.0; - int resolution_ = 0; + size_t resolution_ = 0; Eigen::Vector3f origin_ = Eigen::Vector3f::Zero(); utility::device_vector voxels_; }; diff --git a/src/cupoch/geometry/densegrid.inl b/src/cupoch/geometry/densegrid.inl index ce59d584..118b07cb 100644 --- a/src/cupoch/geometry/densegrid.inl +++ b/src/cupoch/geometry/densegrid.inl @@ -31,7 +31,7 @@ DenseGrid::DenseGrid(Geometry::GeometryType type) template DenseGrid::DenseGrid(Geometry::GeometryType type, float voxel_size, - int resolution, + size_t resolution, const Eigen::Vector3f &origin) : GeometryBase3D(type), voxel_size_(voxel_size), @@ -126,7 +126,7 @@ DenseGrid &DenseGrid::Rotate(const Eigen::Matrix3f &R, template DenseGrid &DenseGrid::Reconstruct(float voxel_size, - int resolution) { + size_t resolution) { voxel_size_ = voxel_size; resolution_ = resolution; voxels_.resize(resolution_ * resolution_ * resolution_, VoxelType()); diff --git a/src/cupoch/geometry/distancetransform.cu b/src/cupoch/geometry/distancetransform.cu index 4aeb31e0..7e318f2e 100644 --- a/src/cupoch/geometry/distancetransform.cu +++ b/src/cupoch/geometry/distancetransform.cu @@ -260,6 +260,24 @@ struct compute_obstacle_cells_functor { }; }; +struct get_index_functor { + get_index_functor(float voxel_size, int resolution, const Eigen::Vector3f& origin) + : voxel_size_(voxel_size), resolution_(resolution), origin_(origin) {}; + const float voxel_size_; + const int resolution_; + const Eigen::Vector3f origin_; + __device__ int operator()(const Eigen::Vector3f& query) const { + Eigen::Vector3f qv = + (query - origin_ + + 0.5 * voxel_size_ * Eigen::Vector3f::Constant(resolution_)) / + voxel_size_; + Eigen::Vector3i idx = + Eigen::device_vectorize(qv.array()) + .cast(); + return IndexOf(idx, resolution_); + }; +}; + } // namespace template class DenseGrid; @@ -273,7 +291,7 @@ DistanceTransform::DistanceTransform() } DistanceTransform::DistanceTransform(float voxel_size, - int resolution, + size_t resolution, const Eigen::Vector3f& origin) : DenseGrid(Geometry::GeometryType::DistanceTransform, voxel_size, @@ -293,7 +311,7 @@ DistanceTransform::DistanceTransform(const DistanceTransform& other) DistanceTransform::~DistanceTransform() {} DistanceTransform& DistanceTransform::Reconstruct(float voxel_size, - int resolution) { + size_t resolution) { DenseGrid::Reconstruct(voxel_size, resolution); buffer_.resize(voxels_.size()); return *this; @@ -400,20 +418,10 @@ float DistanceTransform::GetDistance(const Eigen::Vector3f& query) const { return v.distance_; } -utility::device_vector DistanceTransform::GetDistances( +std::unique_ptr> DistanceTransform::GetDistances( const utility::device_vector& queries) const { - auto func = [voxel_size = voxel_size_, resolution = resolution_, - origin = origin_] __device__(const Eigen::Vector3f& query) { - Eigen::Vector3f qv = - (query - origin + - 0.5 * voxel_size * Eigen::Vector3f::Constant(resolution)) / - voxel_size; - Eigen::Vector3i idx = - Eigen::device_vectorize(qv.array()) - .cast(); - return IndexOf(idx, resolution); - }; - utility::device_vector dists(queries.size()); + auto func = get_index_functor(voxel_size_, resolution_, origin_); + auto dists = std::make_unique>(queries.size()); thrust::transform( thrust::make_permutation_iterator( voxels_.begin(), @@ -421,7 +429,7 @@ utility::device_vector DistanceTransform::GetDistances( thrust::make_permutation_iterator( voxels_.begin(), thrust::make_transform_iterator(queries.end(), func)), - dists.begin(), + dists->begin(), [] __device__(const DistanceVoxel& v) { return v.distance_; }); return dists; } diff --git a/src/cupoch/geometry/distancetransform.h b/src/cupoch/geometry/distancetransform.h index aab64fef..0091823f 100644 --- a/src/cupoch/geometry/distancetransform.h +++ b/src/cupoch/geometry/distancetransform.h @@ -52,12 +52,12 @@ class DistanceTransform : public DenseGrid { public: DistanceTransform(); DistanceTransform(float voxel_size, - int resolution, + size_t resolution, const Eigen::Vector3f &origin = Eigen::Vector3f::Zero()); DistanceTransform(const DistanceTransform &other); ~DistanceTransform(); - DistanceTransform &Reconstruct(float voxel_size, int resolution); + DistanceTransform &Reconstruct(float voxel_size, size_t resolution); DistanceTransform &ComputeEDT( const utility::device_vector &points); @@ -67,7 +67,7 @@ class DistanceTransform : public DenseGrid { DistanceTransform &ComputeVoronoiDiagram(const VoxelGrid &voxelgrid); float GetDistance(const Eigen::Vector3f &query) const; - utility::device_vector GetDistances( + std::unique_ptr> GetDistances( const utility::device_vector &queries) const; static std::shared_ptr CreateFromOccupancyGrid( diff --git a/src/cupoch/geometry/distancetransform_factory.cu b/src/cupoch/geometry/distancetransform_factory.cu index 403b96ae..829b3c30 100644 --- a/src/cupoch/geometry/distancetransform_factory.cu +++ b/src/cupoch/geometry/distancetransform_factory.cu @@ -24,6 +24,19 @@ namespace cupoch { namespace geometry { +namespace { + +struct get_voxel_index_functor { + get_voxel_index_functor(int resolution) + : resolution_(resolution) {} + const int resolution_; + __device__ int operator()(const OccupancyVoxel& voxel) const { + return IndexOf(voxel.grid_index_.cast(), resolution_); + } +}; + +} // namespace + std::shared_ptr DistanceTransform::CreateFromOccupancyGrid( const OccupancyGrid& input) { if (input.voxel_size_ <= 0.0) { @@ -39,12 +52,8 @@ std::shared_ptr DistanceTransform::CreateFromOccupancyGrid( output->voxels_.begin(), thrust::make_transform_iterator( occvoxels->begin(), - [res = output->resolution_] __device__( - const OccupancyVoxel& voxel) { - return IndexOf( - voxel.grid_index_.cast(), - res); - })), + get_voxel_index_functor(output->resolution_) + )), [res = output->resolution_] __device__( const OccupancyVoxel& voxel) { return DistanceVoxel(voxel.grid_index_, 0); diff --git a/src/cupoch/geometry/down_sample.cu b/src/cupoch/geometry/down_sample.cu index ab7928c0..4f9cf2b1 100644 --- a/src/cupoch/geometry/down_sample.cu +++ b/src/cupoch/geometry/down_sample.cu @@ -99,6 +99,12 @@ struct check_distance_threshold_functor { } }; +struct is_valid_index_functor { + __device__ int operator()(int idx) const { + return (int)(idx >= 0); + } +}; + } // namespace std::shared_ptr PointCloud::SelectByIndex( @@ -322,7 +328,7 @@ PointCloud::RemoveRadiusOutliers(size_t nb_points, float search_radius) const { utility::exec_policy(0), range.begin(), range.end(), thrust::make_transform_iterator( tmp_indices.begin(), - [] __device__(int idx) { return (int)(idx >= 0); }), + is_valid_index_functor()), thrust::make_discard_iterator(), counts.begin(), thrust::equal_to(), thrust::plus()); auto begin = make_tuple_iterator(indices.begin(), diff --git a/src/cupoch/geometry/graph.cu b/src/cupoch/geometry/graph.cu index 66e55dcd..1598f360 100644 --- a/src/cupoch/geometry/graph.cu +++ b/src/cupoch/geometry/graph.cu @@ -18,7 +18,11 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include #include +#include +#include #include #include @@ -257,7 +261,7 @@ Graph &Graph::ConstructGraph(bool set_edge_weights_from_distance) { template Graph &Graph::ConnectToNearestNeighbors(float max_edge_distance, - int max_num_edges) { + size_t max_num_edges) { utility::device_vector indices; utility::device_vector weights; utility::device_vector new_edges(this->points_.size() * diff --git a/src/cupoch/geometry/graph.h b/src/cupoch/geometry/graph.h index ade31045..0355b220 100644 --- a/src/cupoch/geometry/graph.h +++ b/src/cupoch/geometry/graph.h @@ -80,7 +80,7 @@ class Graph : public LineSet { Graph &ConstructGraph(bool set_edge_weights_from_distance = true); Graph &ConnectToNearestNeighbors(float max_edge_distance, - int max_num_edges = 30); + size_t max_num_edges = 30); Graph &AddNodeAndConnect(const Eigen::Matrix &point, float max_edge_distance = 0.0f, bool lazy_add = false); diff --git a/src/cupoch/geometry/image.cu b/src/cupoch/geometry/image.cu index b1307e49..da82c4fc 100644 --- a/src/cupoch/geometry/image.cu +++ b/src/cupoch/geometry/image.cu @@ -18,6 +18,7 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include #include "cupoch/geometry/boundingvolume.h" #include "cupoch/geometry/image.h" #include "cupoch/utility/console.h" diff --git a/src/cupoch/geometry/image.h b/src/cupoch/geometry/image.h index d6353dd7..3dc60685 100644 --- a/src/cupoch/geometry/image.h +++ b/src/cupoch/geometry/image.h @@ -105,10 +105,10 @@ class Image : public GeometryBaseNoTrans2D { } /// \brief Prepare Image properties and allocate Image buffer. - Image &Prepare(int width, - int height, - int num_of_channels, - int bytes_per_channel) { + Image &Prepare(unsigned int width, + unsigned int height, + unsigned int num_of_channels, + unsigned int bytes_per_channel) { width_ = width; height_ = height; num_of_channels_ = num_of_channels; @@ -211,13 +211,13 @@ class Image : public GeometryBaseNoTrans2D { public: /// Width of the image. - int width_ = 0; + unsigned int width_ = 0; /// Height of the image. - int height_ = 0; + unsigned int height_ = 0; /// Number of chanels in the image. - int num_of_channels_ = 0; + unsigned int num_of_channels_ = 0; /// Number of bytes per channel. - int bytes_per_channel_ = 0; + unsigned int bytes_per_channel_ = 0; /// Image storage buffer. utility::device_vector data_; }; diff --git a/src/cupoch/geometry/image_factory.cu b/src/cupoch/geometry/image_factory.cu index 939a6ed2..d87e886e 100644 --- a/src/cupoch/geometry/image_factory.cu +++ b/src/cupoch/geometry/image_factory.cu @@ -18,6 +18,7 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include #include "cupoch/camera/pinhole_camera_intrinsic.h" #include "cupoch/geometry/image.h" #include "cupoch/utility/console.h" diff --git a/src/cupoch/geometry/laserscanbuffer.cu b/src/cupoch/geometry/laserscanbuffer.cu index 809542d8..8facabbc 100644 --- a/src/cupoch/geometry/laserscanbuffer.cu +++ b/src/cupoch/geometry/laserscanbuffer.cu @@ -117,7 +117,11 @@ LaserScanBuffer::LaserScanBuffer(int num_steps, num_steps_(num_steps), num_max_scans_(num_max_scans), min_angle_(min_angle), - max_angle_(max_angle) {} + max_angle_(max_angle) { + ranges_.reserve(num_steps_ * num_max_scans_); + intensities_.reserve(num_steps_ * num_max_scans_); + origins_.reserve(num_max_scans_); +} LaserScanBuffer::~LaserScanBuffer(){}; @@ -186,10 +190,11 @@ LaserScanBuffer& LaserScanBuffer::Clear() { bottom_ = 0; ranges_.clear(); intensities_.clear(); + origins_.clear(); return *this; } -bool LaserScanBuffer::IsEmpty() const { return ranges_.empty(); } +bool LaserScanBuffer::IsEmpty() const { return bottom_ == top_; } Eigen::Vector3f LaserScanBuffer::GetMinBound() const { utility::LogError("LaserScanBuffer::GetMinBound is not supported"); @@ -246,10 +251,11 @@ LaserScanBuffer& LaserScanBuffer::Rotate(const Eigen::Matrix3f& R, return *this; } +template LaserScanBuffer& LaserScanBuffer::AddRanges( - const utility::device_vector& ranges, + const ContainerType& ranges, const Eigen::Matrix4f& transformation, - const utility::device_vector& intensities) { + const ContainerType& intensities) { if (ranges.size() != num_steps_) { utility::LogError("[AddRanges] Invalid size of input ranges."); return *this; @@ -276,25 +282,121 @@ LaserScanBuffer& LaserScanBuffer::AddRanges( thrust::copy_n(intensities.begin(), num_steps_, intensities_.begin() + end * num_steps_); origins_[end] = transformation; - top_++; + if (IsFull()) top_++; bottom_++; } return *this; } -LaserScanBuffer& LaserScanBuffer::AddRanges( +template LaserScanBuffer& LaserScanBuffer::AddRanges( + const utility::device_vector& range, + const Eigen::Matrix4f& transformation, + const utility::device_vector& intensities); + +template LaserScanBuffer& LaserScanBuffer::AddRanges( const utility::pinned_host_vector& ranges, const Eigen::Matrix4f& transformation, - const utility::pinned_host_vector& intensities) { - utility::device_vector d_ranges(ranges.size()); - cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(d_ranges.data()), - ranges.data(), ranges.size() * sizeof(float), - cudaMemcpyHostToDevice)); - utility::device_vector d_intensities(intensities.size()); - cudaSafeCall(cudaMemcpy( - thrust::raw_pointer_cast(d_intensities.data()), intensities.data(), - intensities.size() * sizeof(float), cudaMemcpyHostToDevice)); - return AddRanges(d_ranges, transformation, d_intensities); + const utility::pinned_host_vector& intensities); + + +class ContainerLikePtr { +public: + ContainerLikePtr(const float* data, size_t size) : data_(data), size_(size) {} + size_t size() const { return size_; } + const float* begin() const { return data_; } + const float* end() const { return data_ + size_; } + bool empty() const { return size_ == 0; } + const float* data_; + size_t size_; +}; + + +LaserScanBuffer &LaserScanBuffer::AddRanges( + const float *ranges, + const Eigen::Matrix4f &transformation, + const float *intensities) { + return AddRanges(ContainerLikePtr(ranges, num_steps_), transformation, + ContainerLikePtr(intensities, num_steps_)); +} + + +LaserScanBuffer &LaserScanBuffer::Merge(const LaserScanBuffer &other) { + if (other.IsEmpty()) { + utility::LogError("[Merge] Input buffer is empty."); + return *this; + } + if (other.num_steps_ != num_steps_) { + utility::LogError("[Merge] Input buffer has different num_steps."); + return *this; + } + if (other.HasIntensities() != HasIntensities()) { + utility::LogError( + "[Merge] Input buffer has different intensities."); + return *this; + } + if (other.min_angle_ != min_angle_ || other.max_angle_ != max_angle_) { + utility::LogError( + "[Merge] Input buffer has different angle range."); + return *this; + } + if (other.bottom_ - other.top_ + bottom_ - top_ > num_max_scans_) { + utility::LogError("[Merge] Buffer is full."); + return *this; + } + ranges_.insert(ranges_.end(), other.ranges_.begin(), other.ranges_.end()); + if (HasIntensities()) { + intensities_.insert(intensities_.end(), other.intensities_.begin(), + other.intensities_.end()); + } + origins_.insert(origins_.end(), other.origins_.begin(), + other.origins_.end()); + bottom_ += other.bottom_ - other.top_; + return *this; +} + +std::shared_ptr LaserScanBuffer::PopOneScan() { + if (IsEmpty()) { + utility::LogError("[PopRange] Buffer is empty."); + return nullptr; + } + const int start = top_ % num_max_scans_; + auto out = std::make_shared(num_steps_, num_max_scans_, + min_angle_, max_angle_); + out->ranges_.resize(num_steps_); + thrust::copy_n(ranges_.begin() + start * num_steps_, num_steps_, + out->ranges_.begin()); + if (HasIntensities()) { + out->intensities_.resize(num_steps_); + thrust::copy_n(intensities_.begin() + start * num_steps_, num_steps_, + out->intensities_.begin()); + } + out->origins_.push_back(origins_[start]); + out->top_ = 0; + out->bottom_ = 1; + top_++; + return out; +} + +std::pair>, std::unique_ptr>> LaserScanBuffer::PopHostOneScan() { + if (IsEmpty()) { + utility::LogError("[PopRange] Buffer is empty."); + return std::make_pair(std::make_unique>(), std::make_unique>()); + } + const int start = top_ % num_max_scans_; + auto out = std::make_unique>(num_steps_); + cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(out->data()), + thrust::raw_pointer_cast(ranges_.data()) + start * num_steps_, + num_steps_ * sizeof(float), cudaMemcpyDeviceToHost)); + auto out_intensities = std::make_unique>(); + if (HasIntensities()) { + out_intensities->resize(num_steps_); + cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(out_intensities->data()), + thrust::raw_pointer_cast(intensities_.data()) + start * num_steps_, + num_steps_ * sizeof(float), cudaMemcpyDeviceToHost)); + } + top_++; + cudaSafeCall(cudaDeviceSynchronize()); + return std::make_pair(std::move(out), std::move(out_intensities)); } std::shared_ptr LaserScanBuffer::RangeFilter( diff --git a/src/cupoch/geometry/laserscanbuffer.h b/src/cupoch/geometry/laserscanbuffer.h index df4c9b02..ad86f265 100644 --- a/src/cupoch/geometry/laserscanbuffer.h +++ b/src/cupoch/geometry/laserscanbuffer.h @@ -24,15 +24,25 @@ #include "cupoch/utility/eigen.h" namespace cupoch { + +namespace camera { +class PinholeCameraIntrinsic; +} // namespace camera + namespace geometry { template class AxisAlignedBoundingBox; +class Image; +class PointCloud; + + +static const unsigned int DEFAULT_NUM_MAX_SCANS = 50; class LaserScanBuffer : public GeometryBase3D { public: LaserScanBuffer(int num_steps, - int num_max_scans = 10, + int num_max_scans = DEFAULT_NUM_MAX_SCANS, float min_angle = -M_PI, float max_angle = M_PI); ~LaserScanBuffer(); @@ -59,16 +69,25 @@ class LaserScanBuffer : public GeometryBase3D { return (max_angle_ - min_angle_) / (num_steps_ - 1); }; + bool IsFull() const { return GetNumScans() == num_max_scans_; }; + int GetNumScans() const { return bottom_ - top_; }; + + template LaserScanBuffer &AddRanges( - const utility::device_vector &ranges, + const ContainerType &ranges, const Eigen::Matrix4f &transformation = Eigen::Matrix4f::Identity(), - const utility::device_vector &intensities = - utility::device_vector()); + const ContainerType &intensities = + ContainerType()); + LaserScanBuffer &AddRanges( - const utility::pinned_host_vector &ranges, + const float *ranges, const Eigen::Matrix4f &transformation = Eigen::Matrix4f::Identity(), - const utility::pinned_host_vector &intensities = - utility::pinned_host_vector()); + const float *intensities = nullptr); + + LaserScanBuffer &Merge(const LaserScanBuffer &other); + + std::shared_ptr PopOneScan(); + std::pair>, std::unique_ptr>> PopHostOneScan(); std::shared_ptr RangeFilter(float min_range, float max_range) const; @@ -79,13 +98,39 @@ class LaserScanBuffer : public GeometryBase3D { int neighbors = 0, bool remove_shadow_start_point = false) const; + static std::shared_ptr CreateFromPointCloud( + const geometry::PointCloud &pcd, + float angle_increment, + float min_height, + float max_height, + unsigned int num_vertical_divisions = 1, + float min_range = 0.0, + float max_range = std::numeric_limits::infinity(), + float min_angle = -M_PI, + float max_angle = M_PI); + + static std::shared_ptr CreateFromDepthImage( + const geometry::Image &depth, + const camera::PinholeCameraIntrinsic &intrinsic, + float angle_increment, + float min_y, + float max_y, + unsigned int num_vertical_divisions = 1, + float min_range = 0.0, + float max_range = std::numeric_limits::infinity(), + float min_angle = -M_PI, + float max_angle = M_PI, + float depth_scale = 1000.0, + float depth_trunc = 1000.0, + int stride = 1); + public: utility::device_vector ranges_; utility::device_vector intensities_; - int top_ = 0; - int bottom_ = 0; - const int num_steps_; - const int num_max_scans_; + int top_ = 0; //!< index of the oldest scan + int bottom_ = 0; //!< index of the newest scan + const int num_steps_; //!< number of steps in a scan + const int num_max_scans_; //!< maximum number of scans float min_angle_; float max_angle_; utility::device_vector origins_; diff --git a/src/cupoch/geometry/laserscanbuffer_factory.cu b/src/cupoch/geometry/laserscanbuffer_factory.cu new file mode 100644 index 00000000..269251c7 --- /dev/null +++ b/src/cupoch/geometry/laserscanbuffer_factory.cu @@ -0,0 +1,199 @@ +/** + * Copyright (c) 2023 Neka-Nat + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * IN THE SOFTWARE. + **/ +#include "cupoch/geometry/laserscanbuffer.h" +#include "cupoch/geometry/image.h" +#include "cupoch/geometry/pointcloud.h" +#include "cupoch/utility/console.h" +#include "cupoch/utility/platform.h" +#include +#include + +namespace cupoch { +namespace geometry { + +namespace { + +struct pointcloud_to_laserscan_functor { + pointcloud_to_laserscan_functor( + float* ranges, + const float min_height, + const float height_increment, + const int num_steps, + const float min_range, + const float max_range, + const float min_angle, + const float max_angle, + const float angle_increment) + : ranges_(ranges), + min_height_(min_height), + height_increment_(height_increment), + num_steps_(num_steps), + min_range_(min_range), + max_range_(max_range), + min_angle_(min_angle), + max_angle_(max_angle), + angle_increment_(angle_increment) {} + + float* ranges_; + const float min_height_; + const float height_increment_; + const int num_steps_; + const float min_range_; + const float max_range_; + const float min_angle_; + const float max_angle_; + const float angle_increment_; + + __device__ + void operator() (const Eigen::Vector3f& pt) const { + const float z = pt.z(); + const int jndex = (z - min_height_) / height_increment_; + const float range = hypotf(pt.x(), pt.y()); + if (range < min_range_ || range > max_range_) { + return; + } + const float angle = atan2f(pt.y(), pt.x()); + if (angle < min_angle_ || angle > max_angle_) { + return; + } + const int index = (angle - min_angle_) / angle_increment_; + const float range_old = ranges_[num_steps_ * jndex + index]; + if (isnan(range_old) || range < range_old) { + atomicExch(ranges_ + num_steps_ * jndex + index, range); + } + } +}; + +} + +std::shared_ptr LaserScanBuffer::CreateFromPointCloud( + const geometry::PointCloud &pcd, + float angle_increment, + float min_height, + float max_height, + unsigned int num_vertical_divisions, + float min_range, + float max_range, + float min_angle, + float max_angle) { + if (angle_increment <= 0.0) { + utility::LogError("[LaserScanBuffer::CreateFromPointCloud] angle_increment must be positive."); + return std::shared_ptr(); + } + if (min_height >= max_height) { + utility::LogError("[LaserScanBuffer::CreateFromPointCloud] min_height must be smaller than max_height."); + return std::shared_ptr(); + } + if (min_range >= max_range) { + utility::LogError("[LaserScanBuffer::CreateFromPointCloud] min_range must be smaller than max_range."); + return std::shared_ptr(); + } + if (min_angle >= max_angle) { + utility::LogError("[LaserScanBuffer::CreateFromPointCloud] min_angle must be smaller than max_angle."); + return std::shared_ptr(); + } + size_t num_steps = std::ceil((max_angle - min_angle) / angle_increment); + size_t num_max_scans = std::max(DEFAULT_NUM_MAX_SCANS, num_vertical_divisions); + auto laserscanbuffer = std::make_shared( + num_steps, + num_max_scans, + min_angle, + max_angle); + laserscanbuffer->ranges_.resize(num_steps * num_max_scans, std::numeric_limits::quiet_NaN()); + laserscanbuffer->origins_.resize(num_max_scans); + thrust::tabulate( + laserscanbuffer->origins_.begin(), + laserscanbuffer->origins_.end(), + [min_height, max_height, num_vertical_divisions] __device__ (int i) { + Eigen::Matrix4f origin = Eigen::Matrix4f::Identity(); + origin(2, 3) = min_height + (max_height - min_height) * i / num_vertical_divisions; + return origin; + }); + auto func = pointcloud_to_laserscan_functor( + thrust::raw_pointer_cast(laserscanbuffer->ranges_.data()), + min_height, + (max_height - min_height) / num_vertical_divisions, + num_steps, + min_range, + max_range, + min_angle, + max_angle, + angle_increment); + thrust::for_each(pcd.points_.begin(), pcd.points_.end(), func); + return laserscanbuffer; +} + + +std::shared_ptr LaserScanBuffer::CreateFromDepthImage( + const geometry::Image &depth, + const camera::PinholeCameraIntrinsic &intrinsic, + float angle_increment, + float min_y, + float max_y, + unsigned int num_vertical_divisions, + float min_range, + float max_range, + float min_angle, + float max_angle, + float depth_scale, + float depth_trunc, + int stride) { + if (angle_increment <= 0.0) { + utility::LogError("[LaserScanBuffer::CreateFromDepthImage] angle_increment must be positive."); + return std::shared_ptr(); + } + if (min_y >= max_y) { + utility::LogError("[LaserScanBuffer::CreateFromDepthImage] min_y must be smaller than max_y."); + return std::shared_ptr(); + } + if (min_range >= max_range) { + utility::LogError("[LaserScanBuffer::CreateFromDepthImage] min_range must be smaller than max_range."); + return std::shared_ptr(); + } + if (min_angle >= max_angle) { + utility::LogError("[LaserScanBuffer::CreateFromDepthImage] min_angle must be smaller than max_angle."); + return std::shared_ptr(); + } + Eigen::Matrix4f x_rot90 = Eigen::Matrix4f::Identity(); + x_rot90.block<3, 3>(0, 0) = Eigen::AngleAxisf(-M_PI_2, Eigen::Vector3f::UnitX()).toRotationMatrix(); + auto pcd = geometry::PointCloud::CreateFromDepthImage( + depth, intrinsic, x_rot90, depth_scale, depth_trunc, stride); + auto laserscanbuffer = CreateFromPointCloud( + *pcd, + angle_increment, + min_y, + max_y, + num_vertical_divisions, + min_range, + max_range, + min_angle, + max_angle); + thrust::for_each( + laserscanbuffer->origins_.begin(), + laserscanbuffer->origins_.end(), + [x_rot90] __device__ (Eigen::Matrix4f_u& origin) { + origin = x_rot90 * origin; + }); + return laserscanbuffer; +} + +} +} \ No newline at end of file diff --git a/src/cupoch/geometry/lineset_factory.cu b/src/cupoch/geometry/lineset_factory.cu index be2fa816..2a17cef2 100644 --- a/src/cupoch/geometry/lineset_factory.cu +++ b/src/cupoch/geometry/lineset_factory.cu @@ -18,6 +18,7 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include #include #include "cupoch/geometry/boundingvolume.h" diff --git a/src/cupoch/geometry/occupancygrid.cu b/src/cupoch/geometry/occupancygrid.cu index 2c8016c6..3122c85c 100644 --- a/src/cupoch/geometry/occupancygrid.cu +++ b/src/cupoch/geometry/occupancygrid.cu @@ -18,6 +18,9 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include +#include #include #include "cupoch/geometry/boundingvolume.h" @@ -287,7 +290,7 @@ OccupancyGrid::OccupancyGrid() min_bound_(Eigen::Vector3ui16::Constant(resolution_ / 2)), max_bound_(Eigen::Vector3ui16::Constant(resolution_ / 2)) {} OccupancyGrid::OccupancyGrid(float voxel_size, - int resolution, + size_t resolution, const Eigen::Vector3f& origin) : DenseGrid(Geometry::GeometryType::OccupancyGrid, voxel_size, diff --git a/src/cupoch/geometry/occupancygrid.h b/src/cupoch/geometry/occupancygrid.h index 1aebf052..c06cc001 100644 --- a/src/cupoch/geometry/occupancygrid.h +++ b/src/cupoch/geometry/occupancygrid.h @@ -72,7 +72,7 @@ class OccupancyGrid : public DenseGrid { public: OccupancyGrid(); OccupancyGrid(float voxel_size, - int resolution = 512, + size_t resolution = 512, const Eigen::Vector3f& origin = Eigen::Vector3f::Zero()); ~OccupancyGrid(); OccupancyGrid(const OccupancyGrid& other); diff --git a/src/cupoch/geometry/pointcloud.cu b/src/cupoch/geometry/pointcloud.cu index a9e0804b..60a67a08 100644 --- a/src/cupoch/geometry/pointcloud.cu +++ b/src/cupoch/geometry/pointcloud.cu @@ -60,7 +60,7 @@ struct gaussian_filter_functor { const int *indices, const float *dists, float sigma2, - int num_max_search_points, + size_t num_max_search_points, bool has_normal, bool has_color) : points_(points), @@ -78,7 +78,7 @@ struct gaussian_filter_functor { const int *indices_; const float *dists_; const float sigma2_; - const int num_max_search_points_; + const size_t num_max_search_points_; const bool has_normal_; const bool has_color_; __device__ thrust::tuple @@ -315,7 +315,7 @@ PointCloud &PointCloud::RemoveNoneFinitePoints(bool remove_nan, } std::shared_ptr PointCloud::GaussianFilter( - float search_radius, float sigma2, int num_max_search_points) { + float search_radius, float sigma2, size_t num_max_search_points) { auto out = std::make_shared(); if (search_radius <= 0 || sigma2 <= 0 || num_max_search_points <= 0) { utility::LogError( @@ -363,11 +363,11 @@ std::shared_ptr PointCloud::GaussianFilter( return out; } -std::shared_ptr PointCloud::PassThroughFilter(int axis_no, +std::shared_ptr PointCloud::PassThroughFilter(size_t axis_no, float min_bound, float max_bound) { auto out = std::make_shared(); - if (axis_no < 0 || axis_no >= 3) { + if (axis_no >= 3) { utility::LogError( "[PassThroughFilter] Illegal input parameters, axis_no " "must be 0, 1 or 2."); diff --git a/src/cupoch/geometry/pointcloud.h b/src/cupoch/geometry/pointcloud.h index 76282ac6..0451e865 100644 --- a/src/cupoch/geometry/pointcloud.h +++ b/src/cupoch/geometry/pointcloud.h @@ -137,9 +137,9 @@ class PointCloud : public GeometryBase3D { std::shared_ptr GaussianFilter(float search_radius, float sigma2, - int num_max_search_points = 50); + size_t num_max_search_points = 50); - std::shared_ptr PassThroughFilter(int axis_no, + std::shared_ptr PassThroughFilter(size_t axis_no, float min_bound, float max_bound); @@ -177,7 +177,7 @@ class PointCloud : public GeometryBase3D { /// in Large Spatial Databases with Noise", 1996 /// Returns a vector of point labels, -1 indicates noise according to /// the algorithm. - utility::device_vector ClusterDBSCAN( + std::unique_ptr> ClusterDBSCAN( float eps, size_t min_points, bool print_progress = false, @@ -194,8 +194,8 @@ class PointCloud : public GeometryBase3D { /// the plane inliers. std::tuple> SegmentPlane( float distance_threshold = 0.01, - int ransac_n = 3, - int num_iterations = 100) const; + size_t ransac_n = 3, + size_t num_iterations = 100) const; /// Factory function to create a pointcloud from a depth image and a camera /// model (PointCloudFactory.cpp) diff --git a/src/cupoch/geometry/pointcloud_cluster.cu b/src/cupoch/geometry/pointcloud_cluster.cu index 063dac41..bcfd5b15 100644 --- a/src/cupoch/geometry/pointcloud_cluster.cu +++ b/src/cupoch/geometry/pointcloud_cluster.cu @@ -105,7 +105,7 @@ struct set_label_functor { } // namespace // https://www.sciencedirect.com/science/article/pii/S1877050913003438 -utility::device_vector PointCloud::ClusterDBSCAN(float eps, +std::unique_ptr> PointCloud::ClusterDBSCAN(float eps, size_t min_points, bool print_progress, size_t max_edges) const { @@ -140,14 +140,14 @@ utility::device_vector PointCloud::ClusterDBSCAN(float eps, int cluster = 0; utility::device_vector visited(n_pt, 0); utility::pinned_host_vector h_visited(n_pt, 0); - utility::device_vector clusters(n_pt, -1); + auto clusters = std::make_unique>(n_pt, -1); utility::device_vector xa(n_pt); utility::device_vector fa(n_pt); for (int i = 0; i < n_pt; i++) { ++progress_bar; if (h_visited[i] != 1) { thrust::fill_n(make_tuple_iterator(visited.begin() + i, - clusters.begin() + i), + clusters->begin() + i), 1, thrust::make_tuple(1, cluster)); thrust::fill(make_tuple_begin(xa, fa), make_tuple_end(xa, fa), thrust::make_tuple(0, 0)); @@ -165,7 +165,7 @@ utility::device_vector PointCloud::ClusterDBSCAN(float eps, } set_label_functor sl_func(thrust::raw_pointer_cast(xa.data()), cluster, - thrust::raw_pointer_cast(clusters.data()), + thrust::raw_pointer_cast(clusters->data()), thrust::raw_pointer_cast(visited.data())); thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_pt), sl_func); diff --git a/src/cupoch/geometry/pointcloud_factory.cu b/src/cupoch/geometry/pointcloud_factory.cu index 037c5067..87136946 100644 --- a/src/cupoch/geometry/pointcloud_factory.cu +++ b/src/cupoch/geometry/pointcloud_factory.cu @@ -18,6 +18,8 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include #include #include @@ -372,6 +374,14 @@ std::shared_ptr PointCloud::CreateFromRGBDImage( std::shared_ptr PointCloud::CreateFromLaserScanBuffer( const LaserScanBuffer &scan, float min_range, float max_range) { + if (scan.ranges_.size() == 0) { + utility::LogError("[PointCloud::CreateFromLaserScanBuffer] Empty scan, return empty pointcloud."); + return std::make_shared(); + } + if (min_range >= max_range) { + utility::LogError("[PointCloud::CreateFromLaserScanBuffer] min_range must be smaller than max_range."); + return std::make_shared(); + } auto pointcloud = std::make_shared(); thrust::repeated_range< utility::device_vector::const_iterator> diff --git a/src/cupoch/geometry/segmentation.cu b/src/cupoch/geometry/segmentation.cu index 5bbbc3ed..b2924563 100644 --- a/src/cupoch/geometry/segmentation.cu +++ b/src/cupoch/geometry/segmentation.cu @@ -18,6 +18,9 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include +#include #include #include @@ -183,8 +186,8 @@ Eigen::Vector4f GetPlaneFromPoints( std::tuple> PointCloud::SegmentPlane(float distance_threshold /* = 0.01 */, - int ransac_n /* = 3 */, - int num_iterations /* = 100 */) const { + size_t ransac_n /* = 3 */, + size_t num_iterations /* = 100 */) const { RANSACResult result; float error = 0.0; diff --git a/src/cupoch/geometry/trianglemesh.cu b/src/cupoch/geometry/trianglemesh.cu index 67b18672..a9ae33d9 100644 --- a/src/cupoch/geometry/trianglemesh.cu +++ b/src/cupoch/geometry/trianglemesh.cu @@ -18,7 +18,11 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include +#include #include +#include #include #include diff --git a/src/cupoch/geometry/voxelgrid.cu b/src/cupoch/geometry/voxelgrid.cu index a71c92e6..a291dbda 100644 --- a/src/cupoch/geometry/voxelgrid.cu +++ b/src/cupoch/geometry/voxelgrid.cu @@ -18,6 +18,9 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include +#include #include #include diff --git a/src/cupoch/geometry/voxelgrid_factory.cu b/src/cupoch/geometry/voxelgrid_factory.cu index af976d40..ea3860d9 100644 --- a/src/cupoch/geometry/voxelgrid_factory.cu +++ b/src/cupoch/geometry/voxelgrid_factory.cu @@ -18,6 +18,9 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include +#include #include #include diff --git a/src/cupoch/integration/uniform_tsdfvolume.cu b/src/cupoch/integration/uniform_tsdfvolume.cu index ffb4f933..2e2c28ff 100644 --- a/src/cupoch/integration/uniform_tsdfvolume.cu +++ b/src/cupoch/integration/uniform_tsdfvolume.cu @@ -18,6 +18,8 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include #include #include "cupoch/integration/integrate_functor.h" diff --git a/src/cupoch/knn/lbvh_knn.inl b/src/cupoch/knn/lbvh_knn.inl index b9dee471..525d785f 100644 --- a/src/cupoch/knn/lbvh_knn.inl +++ b/src/cupoch/knn/lbvh_knn.inl @@ -18,6 +18,8 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include +#include #include "lbvh_knn.h" #include #include diff --git a/src/cupoch/registration/fast_global_registration.cu b/src/cupoch/registration/fast_global_registration.cu index b6baec1c..282ab1c2 100644 --- a/src/cupoch/registration/fast_global_registration.cu +++ b/src/cupoch/registration/fast_global_registration.cu @@ -18,9 +18,11 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include #include #include #include +#include #include "cupoch/knn/bruteforce_nn.h" #include "cupoch/geometry/pointcloud.h" diff --git a/src/cupoch/registration/kabsch.cu b/src/cupoch/registration/kabsch.cu index 5266a777..c336bd53 100644 --- a/src/cupoch/registration/kabsch.cu +++ b/src/cupoch/registration/kabsch.cu @@ -18,6 +18,7 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include #include #include #include diff --git a/src/cupoch/registration/permutohedral.inl b/src/cupoch/registration/permutohedral.inl index 7e79c1c0..86645fe8 100644 --- a/src/cupoch/registration/permutohedral.inl +++ b/src/cupoch/registration/permutohedral.inl @@ -18,6 +18,7 @@ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. **/ +#include #include "cupoch/registration/permutohedral.h" #include "cupoch/utility/platform.h" diff --git a/src/cupoch/version.txt b/src/cupoch/version.txt index 341eb466..87a5412c 100644 --- a/src/cupoch/version.txt +++ b/src/cupoch/version.txt @@ -1,4 +1,4 @@ CUPOCH_VERSION_MAJOR 0 CUPOCH_VERSION_MINOR 2 -CUPOCH_VERSION_PATCH 6 -CUPOCH_VERSION_TWEAK 0 +CUPOCH_VERSION_PATCH 7 +CUPOCH_VERSION_TWEAK 2 diff --git a/src/cupoch/visualization/shader/simple_shader.cu b/src/cupoch/visualization/shader/simple_shader.cu index d163080e..3d6f97b6 100644 --- a/src/cupoch/visualization/shader/simple_shader.cu +++ b/src/cupoch/visualization/shader/simple_shader.cu @@ -19,6 +19,7 @@ * IN THE SOFTWARE. **/ #include +#include #include #include "cupoch/geometry/boundingvolume.h" diff --git a/src/python/cupoch_pybind/geometry/distancetransform.cpp b/src/python/cupoch_pybind/geometry/distancetransform.cpp index ab41c8c8..157865ab 100644 --- a/src/python/cupoch_pybind/geometry/distancetransform.cpp +++ b/src/python/cupoch_pybind/geometry/distancetransform.cpp @@ -82,7 +82,7 @@ void pybind_distancetransform(py::module &m) { .def("get_distances", [] (const geometry::DistanceTransform& self, const wrapper::device_vector_vector3f& points) { - return wrapper::device_vector_float(self.GetDistances(points.data_)); + return wrapper::device_vector_float(*self.GetDistances(points.data_)); }) .def_static( "create_from_occupancy_grid", diff --git a/src/python/cupoch_pybind/geometry/laserscanbuffer.cpp b/src/python/cupoch_pybind/geometry/laserscanbuffer.cpp index 05c4ab95..86c017d3 100644 --- a/src/python/cupoch_pybind/geometry/laserscanbuffer.cpp +++ b/src/python/cupoch_pybind/geometry/laserscanbuffer.cpp @@ -20,6 +20,7 @@ **/ #include "cupoch/geometry/laserscanbuffer.h" +#include "cupoch/camera/pinhole_camera_intrinsic.h" #include "cupoch/geometry/pointcloud.h" #include "cupoch_pybind/docstring.h" #include "cupoch_pybind/geometry/geometry.h" @@ -36,6 +37,7 @@ void pybind_laserscanbuffer(py::module &m) { laserscan.def(py::init(), "Create a LaserScanBuffer from given a number of points and angle ranges", "num_steps"_a, "num_max_scans"_a = 10, "min_angle"_a = M_PI, "max_angle"_a = M_PI) + .def("is_full", &geometry::LaserScanBuffer::IsFull) .def("add_ranges", [](geometry::LaserScanBuffer &self, const wrapper::device_vector_float &ranges, const Eigen::Matrix4f &transformation, @@ -46,18 +48,67 @@ void pybind_laserscanbuffer(py::module &m) { py::arg("ranges"), py::arg("transformation") = Eigen::Matrix4f::Identity(), py::arg("intensities") = wrapper::device_vector_float()) + .def("add_host_ranges", [](geometry::LaserScanBuffer &self, + const cupoch::utility::pinned_host_vector &ranges, + const Eigen::Matrix4f &transformation, + const cupoch::utility::pinned_host_vector &intensities) { + return self.AddRanges( + wrapper::device_vector_float(ranges).data_, + transformation, + wrapper::device_vector_float(intensities).data_ + ); + }, + "Add host single scan ranges", + py::arg("ranges"), + py::arg("transformation") = Eigen::Matrix4f::Identity(), + py::arg("intensities") = cupoch::utility::pinned_host_vector()) + .def("merge", &geometry::LaserScanBuffer::Merge, + "Merge other LaserScanBuffer into this one") + .def("pop_one_scan", &geometry::LaserScanBuffer::PopOneScan, + "Pop one scan from the buffer") + .def("pop_host_one_scan", &geometry::LaserScanBuffer::PopHostOneScan, + "Pop host one scan from the buffer") .def("range_filter", &geometry::LaserScanBuffer::RangeFilter) .def("scan_shadow_filter", &geometry::LaserScanBuffer::ScanShadowsFilter, "This filter removes laser readings that are most likely caused" " by the veiling effect when the edge of an object is being scanned.", "min_angle"_a, "max_angle"_a, "window"_a, "neighbors"_a = 0, "remove_shadow_start_point"_a = false) + .def_static("create_from_point_cloud", &geometry::LaserScanBuffer::CreateFromPointCloud, + "Create a LaserScanBuffer from a point cloud", + "pcd"_a, + "angle_increment"_a, + "min_height"_a, + "max_height"_a, + "num_vertical_divisions"_a = 1, + "min_range"_a = 0.0, + "max_range"_a = std::numeric_limits::infinity(), + "min_angle"_a = -M_PI, + "max_angle"_a = M_PI) + .def_static("create_from_depth_image", &geometry::LaserScanBuffer::CreateFromDepthImage, + "Create a LaserScanBuffer from a depth image", + "depth"_a, + "intrinsic"_a, + "angle_increment"_a, + "min_y"_a, + "max_y"_a, + "num_vertical_divisions"_a = 1, + "min_range"_a = 0.0, + "max_range"_a = std::numeric_limits::infinity(), + "min_angle"_a = -M_PI, + "max_angle"_a = M_PI, + "depth_scale"_a = 1000.0, + "depth_trunc"_a = 1000.0, + "stride"_a = 1) .def_readonly("num_steps", &geometry::LaserScanBuffer::num_steps_, "Integer: Number of steps per scan.") .def_readonly("num_max_scans", &geometry::LaserScanBuffer::num_max_scans_, "Integer: Maximum buffer size.") .def_readwrite("min_angle", &geometry::LaserScanBuffer::min_angle_) .def_readwrite("max_angle", &geometry::LaserScanBuffer::max_angle_) + .def_property_readonly( + "num_scans", &geometry::LaserScanBuffer::GetNumScans, + "Integer: Number of scans in the buffer.") .def_property_readonly( "ranges", [](geometry::LaserScanBuffer &scan) { diff --git a/src/python/cupoch_pybind/geometry/pointcloud.cpp b/src/python/cupoch_pybind/geometry/pointcloud.cpp index 045b009d..88266bac 100644 --- a/src/python/cupoch_pybind/geometry/pointcloud.cpp +++ b/src/python/cupoch_pybind/geometry/pointcloud.cpp @@ -232,7 +232,7 @@ void pybind_pointcloud(py::module &m) { size_t max_edges) { auto res = pcd.ClusterDBSCAN(eps, min_points, print_progress, max_edges); - return wrapper::device_vector_int(std::move(res)); + return wrapper::device_vector_int(std::move(*res)); }, "Cluster PointCloud using the DBSCAN algorithm Ester et " "al., " diff --git a/src/python/pyproject.toml b/src/python/pyproject.toml index 699c9a7a..aa04d3b5 100644 --- a/src/python/pyproject.toml +++ b/src/python/pyproject.toml @@ -1,6 +1,6 @@ [tool.poetry] name = "cupoch" -version = "0.2.6.0" +version = "0.2.7.2" description = "" authors = ["nekanat "] license = "MIT" @@ -17,6 +17,10 @@ rospkg = {version = "^1.2.9", optional = true} matplotlib = {version = "^3.3.3", optional = true} pyrealsense2 = {version = "^2.41.0", optional = true} depthai = {version = "^2.10.0", optional = true} +rospy-all = {version = "^0.0.1", optional = true} +rosmaster = {version = "^1.15.11", optional = true} +rosbag = {version = "^1.15.11", optional = true} +transformations = {version = "^2022.9.26", optional = true} [tool.poetry.dev-dependencies] twine = "^3.2.0" @@ -30,5 +34,10 @@ black = "^22.6.0" requires = ["poetry-core>=1.0.0"] build-backend = "poetry.core.masonry.api" +[[tool.poetry.source]] +name = "ros" +url = "https://rospypi.github.io/simple/" +secondary = true + [tool.poetry.extras] -examples = ["open3d", "torch", "opencv-python", "networkx", "rospkg", "matplotlib", "pyrealsense2", "depthai"] +examples = ["open3d", "torch", "opencv-python", "networkx", "rospkg", "matplotlib", "pyrealsense2", "depthai", "rospy-all", "rosmaster", "rosbag", "transformations"] diff --git a/src/tests/geometry/laserscanbuffer.cpp b/src/tests/geometry/laserscanbuffer.cpp index 262ec4ae..6758a7a0 100644 --- a/src/tests/geometry/laserscanbuffer.cpp +++ b/src/tests/geometry/laserscanbuffer.cpp @@ -46,7 +46,8 @@ TEST(LaserScanBuffer, Constructor) { } TEST(LaserScanBuffer, AddRanges) { - geometry::LaserScanBuffer scan(100); + unsigned int buffer_size = 10; + geometry::LaserScanBuffer scan(100, buffer_size); EXPECT_EQ(scan.num_steps_, 100); EXPECT_EQ(scan.bottom_, 0); @@ -56,16 +57,26 @@ TEST(LaserScanBuffer, AddRanges) { EXPECT_EQ(scan.bottom_, 1); EXPECT_EQ(scan.origins_.size(), scan.bottom_); EXPECT_EQ(scan.ranges_.size(), scan.num_steps_ * scan.bottom_); + EXPECT_EQ(scan.GetNumScans(), 1); scan.AddRanges(hvec); EXPECT_EQ(scan.num_steps_, 100); EXPECT_EQ(scan.bottom_, 2); EXPECT_EQ(scan.origins_.size(), scan.bottom_); EXPECT_EQ(scan.ranges_.size(), scan.num_steps_ * scan.bottom_); + EXPECT_EQ(scan.GetNumScans(), 2); + auto latest = scan.PopOneScan(); + EXPECT_EQ(latest->num_steps_, 100); + EXPECT_EQ(scan.GetNumScans(), 1); scan.Clear(); - scan.AddRanges(hvec, Matrix4f::Identity(), hvec); - EXPECT_EQ(scan.ranges_.size(), scan.num_steps_ * scan.bottom_); - EXPECT_EQ(scan.intensities_.size(), scan.num_steps_ * scan.bottom_); + for (int i = 0; i < buffer_size; ++i) { + EXPECT_FALSE(scan.IsFull()); + scan.AddRanges(hvec, Matrix4f::Identity(), hvec); + EXPECT_EQ(scan.ranges_.size(), scan.num_steps_ * scan.GetNumScans()); + EXPECT_EQ(scan.intensities_.size(), scan.num_steps_ * scan.GetNumScans()); + EXPECT_EQ(scan.GetNumScans(), i + 1); + } + EXPECT_TRUE(scan.IsFull()); } TEST(LaserScanBuffer, RangeFilter) { diff --git a/third_party/CMakeLists.txt b/third_party/CMakeLists.txt index 9f3b6915..efbffbd7 100644 --- a/third_party/CMakeLists.txt +++ b/third_party/CMakeLists.txt @@ -44,7 +44,7 @@ set(STDGPU_SETUP_COMPILER_FLAGS OFF CACHE INTERNAL "") add_subdirectory(stdgpu) set_target_properties(stdgpu PROPERTIES POSITION_INDEPENDENT_CODE ON) target_include_directories(stdgpu INTERFACE $) -install(TARGETS stdgpu) +#install(TARGETS stdgpu) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/stdgpu/src/stdgpu DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} FILES_MATCHING PATTERN "*.cpp" EXCLUDE PATTERN "*.txt" EXCLUDE) diff --git a/third_party/lbvh/lbvh/bvh.cuh b/third_party/lbvh/lbvh/bvh.cuh index 0ee3b387..3070430a 100644 --- a/third_party/lbvh/lbvh/bvh.cuh +++ b/third_party/lbvh/lbvh/bvh.cuh @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -220,6 +221,15 @@ struct default_morton_code_calculator aabb whole; }; +template +struct aabb_merge_functor { + __device__ __host__ + inline aabb_type operator() (const aabb_type& lhs, const aabb_type& rhs) { + return merge(lhs, rhs); + } +}; + + template using bvh_device = detail::basic_device_bvh; template @@ -311,9 +321,7 @@ class bvh const auto aabb_whole = thrust::reduce( aabbs_.begin() + num_internal_nodes, aabbs_.end(), default_aabb, - [] __device__ (const aabb_type& lhs, const aabb_type& rhs) { - return merge(lhs, rhs); - }); + aabb_merge_functor()); thrust::device_vector morton(num_objects); thrust::transform(this->objects_d_.begin(), this->objects_d_.end(), diff --git a/third_party/stdgpu b/third_party/stdgpu index 4994ccf3..bc3adaeb 160000 --- a/third_party/stdgpu +++ b/third_party/stdgpu @@ -1 +1 @@ -Subproject commit 4994ccf3530e803761e19a08ba5034959940c41d +Subproject commit bc3adaeb6e50ae2225c9173db5aeaf6197ce5f80