├── .gitignore ├── CMakeLists.txt ├── Install_dependencies.sh ├── Install_dependencies_jetson.sh ├── README.md ├── compile.sh ├── compile_pointcloud.sh ├── example ├── CMakeLists.txt ├── c │ ├── CMakeLists.txt │ └── preview_depth.c ├── cpp │ ├── CMakeLists.txt │ ├── capture_raw.cpp │ ├── example │ │ ├── parse_arg.cpp │ │ ├── pid.c │ │ ├── pid.h │ │ ├── test.cpp │ │ └── test.hpp │ ├── preview_by_usb.cpp │ └── preview_depth.cpp └── python │ ├── capture_raw.py │ ├── preview_depth.py │ └── requirements.txt ├── img └── rviz_error.png ├── open3d_preview ├── CMakeLists.txt └── preview_pointcloud.cpp ├── ros2_publisher ├── README.md ├── build.sh ├── run.sh └── src │ └── arducam │ └── arducam_rclpy_tof_pointcloud │ ├── arducam_rclpy_tof_pointcloud │ ├── __init__.py │ └── tof_pointcloud.py │ ├── package.xml │ ├── resource │ └── arducam_rclpy_tof_pointcloud │ ├── setup.cfg │ └── setup.py └── setup_rock_5a.sh /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | .xmake/ 3 | 4 | py_arducam/ 5 | CMakeFiles/ 6 | 7 | build/ 8 | 9 | /ros2_publisher/install/ 10 | /ros2_publisher/log/ 11 | /ros2_publisher/*.json 12 | /example/python/*.json 13 | 14 | *.patch 15 | /*.json 16 | /cali.bin 17 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | 3 | project(TOFCameraExample) 4 | 5 | # /home/user/workspace/tof/arducam_tof_sdk/install/lib/cmake/ArducamDepthCamera/ArducamDepthCameraConfig.cmake 6 | # set(ArducamDepthCamera_DIR "/home/user/workspace/tof/arducam_tof_sdk/install/lib/cmake/ArducamDepthCamera") 7 | find_package(ArducamDepthCamera REQUIRED) 8 | 9 | set(CLIBS ArducamDepthCamera::ArducamDepthCamera4C) 10 | set(CXXLIBS ArducamDepthCamera::ArducamDepthCamera) 11 | 12 | # include_directories(${INC_DIR}) 13 | # include_directories("/usr/local/include") 14 | # link_directories("/usr/local/lib") 15 | 16 | add_subdirectory(example) 17 | add_subdirectory(open3d_preview) 18 | -------------------------------------------------------------------------------- /Install_dependencies.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | FIND_FILE="" 4 | HAS_DTOVERLAY="true" 5 | if [ -f "/boot/firmware/config.txt" ]; then # Bookworm 6 | FIND_FILE="/boot/firmware/config.txt" 7 | if [ $(grep -c -e '^dtoverlay=arducam-pivariety$' $FIND_FILE) -lt '1' ]; then 8 | HAS_DTOVERLAY="false" 9 | fi 10 | elif [ -f "/boot/config.txt" ]; then # Bullseye and earlier 11 | FIND_FILE="/boot/config.txt" 12 | if [ $(grep -c -e '^dtoverlay=arducam-pivariety$' $FIND_FILE) -lt '1' ]; then 13 | HAS_DTOVERLAY="false" 14 | fi 15 | fi 16 | 17 | if [ "$FIND_FILE" = "" ]; then 18 | echo -e "\033[31m[ERR] No config.txt file found." 19 | exit 1 20 | fi 21 | 22 | if [ "$HAS_DTOVERLAY" = "false" ]; then 23 | echo -e "\033[31m[WARN]\033[0m dtoverlay=arducam-pivariety not found in $FIND_FILE." 24 | # remove all line which has dtoverlay=arducam-pivariety 25 | sudo sed -i '/dtoverlay=arducam-pivariety/d' $FIND_FILE 26 | echo "dtoverlay=arducam-pivariety" | sudo tee -a $FIND_FILE 27 | # if "Raspberry Pi 5" in `cat /sys/firmware/devicetree/base/model` 28 | if [ -f /sys/firmware/devicetree/base/model ]; then 29 | if grep -q "Raspberry Pi 5" /sys/firmware/devicetree/base/model; then 30 | echo "dtoverlay=arducam-pivariety,cam0" | sudo tee -a $FIND_FILE 31 | fi 32 | fi 33 | echo -e "\033[32m[INFO]\033[0m dtoverlay=arducam-pivariety added to $FIND_FILE." 34 | fi 35 | 36 | if [ $(grep -c "camera_auto_detect=1" $FIND_FILE) -ne '0' ]; then 37 | sudo sed -i "s/\(^camera_auto_detect=1\)/camera_auto_detect=0/" $FIND_FILE 38 | fi 39 | if [ $(grep -c "camera_auto_detect=0" $FIND_FILE) -lt '1' ]; then 40 | sudo bash -c "echo camera_auto_detect=0 >> $FIND_FILE" 41 | fi 42 | 43 | sudo apt update 44 | if ! sudo apt-get install -y cmake curl libopencv-dev python3-pip python3-opencv python3-numpy >/dev/null 2>&1; then 45 | echo -e "\033[31m[ERR]\033[0m Failed to install dependencies." 46 | echo -e "\033[31m[ERR]\033[0m Please check your network connection." 47 | exit 1 48 | fi 49 | 50 | if [ $(dpkg -l | grep -c arducam-tof-sdk-dev) -lt 1 ]; then 51 | echo "Add Arducam_ppa repositories." 52 | curl -s --compressed "https://arducam.github.io/arducam_ppa/KEY.gpg" | sudo apt-key add - 53 | sudo curl -s --compressed -o /etc/apt/sources.list.d/arducam_list_files.list "https://arducam.github.io/arducam_ppa/arducam_list_files.list" 54 | fi 55 | 56 | # install dependency 57 | sudo apt update 58 | if ! sudo apt-get install -y arducam-config-parser-dev arducam-evk-sdk-dev arducam-tof-sdk-dev >/dev/null 2>&1; then 59 | echo -e "\033[31m[ERR]\033[0m Failed to install tof sdk." 60 | echo -e "\033[31m[ERR]\033[0m Please check your network connection." 61 | exit 1 62 | fi 63 | if ! sudo python -m pip install ArducamDepthCamera >/dev/null 2>&1; then 64 | if ! sudo python -m pip install ArducamDepthCamera --break-system-packages >/dev/null 2>&1; then 65 | echo -e "\033[31m[ERR]\033[0m Failed to install ArducamDepthCamera." 66 | fi 67 | fi 68 | # python -m pip install ArducamDepthCamera opencv-python "numpy<2.0.0" 69 | echo -e "\033[32m[INFO]\033[0m If you want to install for python venv." 70 | echo -e " please run: python -m pip install ArducamDepthCamera opencv-python \"numpy<2.0.0\"" 71 | 72 | echo "reboot now? (y/n):" 73 | read -r USER_INPUT 74 | case $USER_INPUT in 75 | 'y' | 'Y') 76 | echo "reboot" 77 | sudo reboot 78 | ;; 79 | *) 80 | echo "cancel" 81 | echo "The script settings will only take effect after restarting. Please restart yourself later." 82 | exit 1 83 | ;; 84 | esac 85 | -------------------------------------------------------------------------------- /Install_dependencies_jetson.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # install dependency 4 | sudo apt update 5 | if ! sudo apt install -y cmake curl python3-pip python3-opencv python3-numpy >/dev/null 2>&1; then 6 | echo -e "\033[31m[ERR]\033[0m Failed to install dependencies." 7 | echo -e "\033[31m[ERR]\033[0m Please check your network connection." 8 | exit 1 9 | fi 10 | 11 | if [ $(dpkg -l | grep -c arducam-tof-sdk-dev) -lt 1 ]; then 12 | echo "Add Arducam_ppa repositories." 13 | curl -s --compressed "https://arducam.github.io/arducam_ppa/KEY.gpg" | sudo apt-key add - 14 | sudo curl -s --compressed -o /etc/apt/sources.list.d/arducam_list_files.list "https://arducam.github.io/arducam_ppa/arducam_list_files.list" 15 | fi 16 | 17 | # install dependency 18 | sudo apt update 19 | if ! sudo apt install -y arducam-config-parser-dev arducam-evk-sdk-dev arducam-tof-sdk-dev >/dev/null 2>&1; then 20 | echo -e "\033[31m[ERR]\033[0m Failed to install tof sdk." 21 | echo -e "\033[31m[ERR]\033[0m Please check your network connection." 22 | exit 1 23 | fi 24 | if ! sudo python -m pip install ArducamDepthCamera >/dev/null 2>&1; then 25 | if ! sudo python -m pip install ArducamDepthCamera --break-system-packages >/dev/null 2>&1; then 26 | echo -e "\033[31m[ERR]\033[0m Failed to install ArducamDepthCamera." 27 | fi 28 | fi 29 | echo -e "\033[32m[INFO]\033[0m To install for python venv." 30 | echo -e " please run: python -m pip install ArducamDepthCamera opencv-python \"numpy<2.0.0\"" 31 | 32 | # sudo apt-get update 33 | if [ $(dpkg -l | grep libopencv-dev -c) -lt 1 ] && [ $(dpkg -l | grep nvidia-opencv -c) -lt 1 ]; then 34 | sudo apt install -y nvidia-opencv 35 | fi 36 | 37 | wget https://github.com/ArduCAM/MIPI_Camera/releases/download/v0.0.3/install_full.sh 38 | chmod +x install_full.sh 39 | ./install_full.sh -m arducam 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arducam Depth Camera 2 | 3 | ## Overview 4 | 5 | This project is a use example based on arducam's depth camera. It includes basic image rendering using opencv, displaying 3D point clouds using PCL, and publishing depth camera data through the ROS2 system. 6 | The depth camera is the depth data obtained by calculating the phase difference based on the transmitted modulated pulse. The resolution of the camera is 240*180. Currently, it has two range modes: 2 meters and 4 meters. The measurement error is within 2 cm. 7 | The depth camera supports CSI and USB two connection methods, and needs an additional 5V 2A current power supply for the camera. 8 | 9 | ## Quick Start 10 | 11 | ### Clone this repository 12 | 13 | Clone this repository and enter the directory. 14 | 15 | ```shell 16 | git clone https://github.com/ArduCAM/Arducam_tof_camera.git 17 | cd Arducam_tof_camera 18 | ``` 19 | 20 | ### Install dependencies for Raspberry Pi 21 | 22 | > Run in the Arducam_tof_camera folder 23 | > Whatever you want to run the C/C++ examples or Python examples, you need to install the dependencies. 24 | 25 | ```shell 26 | ./Install_dependencies.sh 27 | ``` 28 | 29 | > Run the setup_rock_5a.sh if on rock 5a platform. 30 | 31 | ```shell 32 | ./setup_rock_5a.sh 33 | ``` 34 | 35 | ### Install dependencies for Jetson 36 | 37 | > Run in the Arducam_tof_camera folder 38 | > Whatever you want to run the C/C++ examples or Python examples, you need to install the dependencies. 39 | 40 | ```shell 41 | ./Install_dependencies_jetson.sh 42 | ``` 43 | 44 | ## Run Examples 45 | 46 | > Platform: Raspberry Pi or Jetson 47 | 48 | ### Depth Examples 49 | 50 | #### Python 51 | 52 | ##### Run 53 | 54 | ###### Python Example 55 | 56 | > Run in the example/python folder 57 | 58 | ```shell 59 | cd example/python 60 | ``` 61 | 62 | ```shell 63 | python3 preview_depth.py 64 | #or 65 | python3 capture_raw.py 66 | ``` 67 | 68 | #### C/C++ 69 | 70 | ##### Compile 71 | 72 | > Run in the Arducam_tof_camera folder 73 | 74 | ```shell 75 | ./compile.sh 76 | ``` 77 | 78 | ##### Run 79 | 80 | ###### C Example 81 | 82 | > Run in the build/example/c folder 83 | 84 | ```shell 85 | cd build/example/c 86 | ``` 87 | 88 | ```shell 89 | ./preview_depth_c 90 | ``` 91 | 92 | ###### C++ Example 93 | 94 | > Run in the build/example/cpp folder 95 | 96 | ```shell 97 | cd build/example/cpp 98 | ``` 99 | 100 | ```shell 101 | ./preview_depth 102 | #or 103 | ./capture_raw 104 | ``` 105 | 106 | ### Point Cloud Examples 107 | 108 | 109 | 110 | #### C/C++ 111 | 112 | ##### Dependencies 113 | 114 | ```Shell 115 | sudo apt update 116 | sudo apt-get install libopen3d-dev 117 | ``` 118 | 119 | ##### Compile 120 | 121 | > Run in the Arducam_tof_camera folder 122 | 123 | ```shell 124 | ./compile_pointcloud.sh 125 | ``` 126 | 127 | ##### Run 128 | 129 | > Run in the build/open3d_preview folder 130 | > If you do not see a point cloud window, please try adding the environment variable `export MESA_GL_VERSION_OVERRIDE=4.5` before running the program. 131 | 132 | ```shell 133 | cd build/open3d_preview 134 | ``` 135 | 136 | ```shell 137 | ./preview_pointcloud 138 | ``` 139 | -------------------------------------------------------------------------------- /compile.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # compile script 3 | workpath=$(cd "$(dirname "$0")" && pwd) 4 | 5 | echo "workpath: $workpath" 6 | 7 | if ! cmake -B $workpath/build -S $workpath; then 8 | echo "== CMake failed" 9 | exit 1 10 | fi 11 | 12 | if cmake --build $workpath/build --config Release --target example -j 4 && 13 | cmake --build $workpath/build --config Release --target capture_raw -j 4 && 14 | cmake --build $workpath/build --config Release --target preview_depth -j 4 && 15 | cmake --build $workpath/build --config Release --target preview_depth_c -j 4; then 16 | echo "== Build success" 17 | echo "== Run $workpath/build/example/cpp/example" 18 | else 19 | echo "== Retry build without -j 4" 20 | if cmake --build $workpath/build --config Release --target example && 21 | cmake --build $workpath/build --config Release --target capture_raw && 22 | cmake --build $workpath/build --config Release --target preview_depth && 23 | cmake --build $workpath/build --config Release --target preview_depth_c; then 24 | echo "== Build success" 25 | echo "== Run $workpath/build/example/cpp/example" 26 | else 27 | echo "== Build failed" 28 | fi 29 | fi 30 | -------------------------------------------------------------------------------- /compile_pointcloud.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # compile script 3 | workpath=$(cd "$(dirname "$0")" && pwd) 4 | 5 | echo "workpath: $workpath" 6 | cmake -B $workpath/build -S $workpath \ 7 | && cmake --build $workpath/build --config Release --target preview_pointcloud -j 4 \ 8 | && echo "== Run $workpath/build/open3d_preview/preview_pointcloud" \ 9 | || echo "== Build failed" -------------------------------------------------------------------------------- /example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | 3 | project(example) 4 | 5 | add_subdirectory(c) 6 | add_subdirectory(cpp) 7 | -------------------------------------------------------------------------------- /example/c/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | 3 | project(c_example) 4 | 5 | add_executable(preview_depth_c preview_depth.c) 6 | target_link_libraries(preview_depth_c ${CLIBS}) 7 | -------------------------------------------------------------------------------- /example/c/preview_depth.c: -------------------------------------------------------------------------------- 1 | #include "ArducamDepthCamera.h" 2 | #include 3 | #include 4 | 5 | int main() 6 | { 7 | ArducamDepthCamera tof = createArducamDepthCamera(); 8 | ArducamFrameBuffer frame; 9 | if (arducamCameraOpen(tof, CONNECTION_CSI, 0)) { 10 | printf("Failed to open camera\n"); 11 | return -1; 12 | } 13 | 14 | if (arducamCameraStart(tof, DEPTH_FRAME)) { 15 | printf("Failed to start camera\n"); 16 | return -1; 17 | } 18 | 19 | ArducamCameraInfo info = arducamCameraGetInfo(tof); 20 | printf("open camera with (%d x %d)\n", (int)info.width, (int)info.height); 21 | 22 | uint8_t* preview_ptr = malloc(info.width * info.height * sizeof(uint8_t)); 23 | ArducamFrameFormat format; 24 | for (;;) { 25 | if ((frame = arducamCameraRequestFrame(tof, 200)) != 0x00) { 26 | format = arducamCameraGetFormat(frame, DEPTH_FRAME); 27 | float* depth_ptr = (float*)arducamCameraGetDepthData(frame); 28 | float* confidence_ptr = (float*)arducamCameraGetConfidenceData(frame); 29 | 30 | printf("frame: (%d x %d)\n", (int)format.width, (int)format.height); 31 | 32 | arducamCameraReleaseFrame(tof, frame); 33 | } 34 | } 35 | 36 | if (arducamCameraStop(tof)) { 37 | return -1; 38 | } 39 | 40 | if (arducamCameraClose(tof)) { 41 | return -1; 42 | } 43 | 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /example/cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | 3 | project(cpp_example) 4 | find_package(OpenCV REQUIRED) 5 | include_directories(${OpenCV_INCLUDE_DIRS}) 6 | 7 | add_executable(preview_depth preview_depth.cpp) 8 | target_link_libraries(preview_depth ${CXXLIBS} ${OpenCV_LIBS}) 9 | 10 | add_executable(capture_raw capture_raw.cpp) 11 | target_link_libraries(capture_raw ${CXXLIBS} ${OpenCV_LIBS}) 12 | 13 | add_executable(preview_usb preview_by_usb.cpp) 14 | target_link_libraries(preview_usb ${CXXLIBS} ${OpenCV_LIBS}) 15 | 16 | aux_source_directory(example DIR_SRCS) 17 | add_executable(example ${DIR_SRCS}) 18 | target_link_libraries(example ${CXXLIBS} ${OpenCV_LIBS}) 19 | -------------------------------------------------------------------------------- /example/cpp/capture_raw.cpp: -------------------------------------------------------------------------------- 1 | #include "ArducamTOFCamera.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace Arducam; 9 | 10 | void display_fps(void) 11 | { 12 | using std::chrono::high_resolution_clock; 13 | using namespace std::literals; 14 | static int count = 0; 15 | static auto time_beg = high_resolution_clock::now(); 16 | auto time_end = high_resolution_clock::now(); 17 | ++count; 18 | auto duration_ms = (time_end - time_beg) / 1ms; 19 | if (duration_ms >= 1000) { 20 | std::cout << "fps:" << count << std::endl; 21 | count = 0; 22 | time_beg = time_end; 23 | } 24 | } 25 | 26 | bool processKey() 27 | { 28 | int key = cv::waitKey(1); 29 | switch (key) { 30 | case 27: 31 | case 'q': 32 | return false; 33 | default: 34 | break; 35 | } 36 | return true; 37 | } 38 | 39 | int main() 40 | { 41 | ArducamTOFCamera tof; 42 | ArducamFrameBuffer* frame; 43 | if (tof.open(Connection::CSI)) { 44 | std::cerr << "initialization failed" << std::endl; 45 | return -1; 46 | } 47 | if (tof.start(FrameType::RAW_FRAME)) { 48 | std::cerr << "Failed to start camera" << std::endl; 49 | return -1; 50 | } 51 | // Modify the range also to modify the MAX_DISTANCE 52 | int max_range = 4000; 53 | // tof.setControl(Control::RANGE, 4000); 54 | tof.getControl(Control::RANGE, &max_range); 55 | 56 | CameraInfo info = tof.getCameraInfo(); 57 | std::cout << "open camera with (" << info.width << "x" << info.height << ") with range " << max_range << std::endl; 58 | 59 | cv::namedWindow("preview", cv::WINDOW_AUTOSIZE); 60 | 61 | for (;;) { 62 | frame = tof.requestFrame(2000); 63 | if (frame == nullptr) { 64 | continue; 65 | } 66 | FrameFormat format; 67 | frame->getFormat(FrameType::RAW_FRAME, format); 68 | std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl; 69 | 70 | int16_t* raw_ptr = (int16_t*)frame->getData(FrameType::RAW_FRAME); 71 | if (raw_ptr == nullptr) { 72 | tof.releaseFrame(frame); 73 | continue; 74 | } 75 | 76 | cv::Mat result_frame(format.height, format.width, CV_8U); 77 | cv::Mat raw_frame(format.width, format.height, CV_16S, raw_ptr); 78 | 79 | raw_frame.convertTo(result_frame, CV_8U, 1. / (1 << 4), 0); 80 | cv::imshow("preview", result_frame); 81 | 82 | if (!processKey()) { 83 | break; 84 | } 85 | display_fps(); 86 | tof.releaseFrame(frame); 87 | } 88 | 89 | if (tof.stop()) { 90 | return -1; 91 | } 92 | 93 | if (tof.close()) { 94 | return -1; 95 | } 96 | 97 | return 0; 98 | } 99 | -------------------------------------------------------------------------------- /example/cpp/example/parse_arg.cpp: -------------------------------------------------------------------------------- 1 | #include "test.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #define LOCAL static inline 7 | 8 | /* #region parse_opt */ 9 | 10 | LOCAL void help(const char* exec) 11 | { 12 | std::cout << "Usage: " << exec << " [OPTION]" << std::endl; 13 | std::cout << " --help Display this information" << std::endl; 14 | std::cout << " -V,--version Display the version of the program" << std::endl; 15 | std::cout << " -d,--device NUM Set the device number" << std::endl; 16 | std::cout << " --raw/--depth Display the raw or depth frame" << std::endl; 17 | std::cout << " -P,--no-preview Do not display the preview" << std::endl; 18 | std::cout << " -C,--no-confidence Do not display the confidence" << std::endl; 19 | std::cout << " -A,--no-amplitude Do not display the amplitude" << std::endl; 20 | std::cout << " --fps FPS Set the fps of the camera" << std::endl; 21 | std::cout << " --mode MODE Set the mode of the camera" << std::endl; 22 | std::cout << " 0 Near mode" << std::endl; 23 | std::cout << " 1 Far mode (default if supported)" << std::endl; 24 | std::cout << " -1 Do not set mode" << std::endl; 25 | std::cout << " --confidence NUM Set the confidence value" << std::endl; 26 | std::cout << " --exposure NUM Set the exposure time" << std::endl; 27 | std::cout << " -h,--hflip,--h-flip Enable the horizontal flip" << std::endl; 28 | std::cout << " -v,--vflip,--v-flip Enable the vertical flip" << std::endl; 29 | std::cout << " --cfg PATH The usb camera config file path" << std::endl; 30 | std::cout << " -m,--min-range NUM Set the min range of the camera (mm)" << std::endl; 31 | std::cout << " -M,--max-range NUM Set the max range of the camera (mm)" << std::endl; 32 | std::cout << " -E,--no-load-eeprom Disable loading the eeprom" << std::endl; 33 | } 34 | 35 | enum class ArgEnum { 36 | none, 37 | help, 38 | version, 39 | device, 40 | raw, 41 | depth, 42 | no_preview, 43 | no_confidence, 44 | no_amplitude, 45 | fps, 46 | mode, 47 | confidence_val, 48 | exposure_val, 49 | h_flip, 50 | v_flip, 51 | cfg, 52 | min_range, 53 | max_range, 54 | no_load_eeprom, 55 | }; 56 | 57 | template LOCAL const char* to_str() 58 | { 59 | switch (arg_enum) { 60 | case ArgEnum::help: 61 | return "help"; 62 | case ArgEnum::version: 63 | return "version"; 64 | case ArgEnum::device: 65 | return "device number"; 66 | case ArgEnum::raw: 67 | return "raw"; 68 | case ArgEnum::depth: 69 | return "depth"; 70 | case ArgEnum::no_preview: 71 | return "no preview"; 72 | case ArgEnum::no_confidence: 73 | return "no confidence"; 74 | case ArgEnum::no_amplitude: 75 | return "no amplitude"; 76 | case ArgEnum::fps: 77 | return "fps"; 78 | case ArgEnum::mode: 79 | return "mode"; 80 | case ArgEnum::confidence_val: 81 | return "confidence"; 82 | case ArgEnum::exposure_val: 83 | return "exposure"; 84 | case ArgEnum::h_flip: 85 | return "horizontal flip"; 86 | case ArgEnum::v_flip: 87 | return "vertical flip"; 88 | case ArgEnum::cfg: 89 | return "config file path"; 90 | case ArgEnum::min_range: 91 | return "min range"; 92 | case ArgEnum::max_range: 93 | return "max range"; 94 | case ArgEnum::no_load_eeprom: 95 | return "no load eeprom"; 96 | default: 97 | return "unknown"; 98 | } 99 | } 100 | 101 | LOCAL bool __parse_cfg(opt_data& data, const char* path) 102 | { 103 | std::ifstream file(path); 104 | if (!file.is_open()) { 105 | return false; 106 | } 107 | data.cfg = path; 108 | return true; 109 | } 110 | 111 | template LOCAL int __parse_opt(const char* exec, const char* curr, const char* opt, opt_data& data) 112 | { 113 | (void)curr; 114 | #define ERR_OPT() \ 115 | do { \ 116 | std::cerr << "Invalid " << to_str() << std::endl; \ 117 | return 0; \ 118 | } while (false) 119 | #define CHECK_OPT() \ 120 | do { \ 121 | if (opt == nullptr) { \ 122 | ERR_OPT(); \ 123 | return 0; \ 124 | } \ 125 | } while (false) 126 | #define ASSERT_OPT(cond) \ 127 | do { \ 128 | if (!(cond)) { \ 129 | ERR_OPT(); \ 130 | return 0; \ 131 | } \ 132 | } while (false) 133 | 134 | std::cout << "[info] process " << to_str() << std::endl; 135 | switch (arg_enum) { 136 | case ArgEnum::device: { 137 | CHECK_OPT(); 138 | data.device = atoi(opt); 139 | return 2; 140 | } break; 141 | case ArgEnum::cfg: { 142 | CHECK_OPT(); 143 | ASSERT_OPT(__parse_cfg(data, opt)); 144 | return 2; 145 | } break; 146 | case ArgEnum::raw: { 147 | data.raw = true; 148 | } break; 149 | case ArgEnum::depth: { 150 | data.raw = false; 151 | } break; 152 | case ArgEnum::no_preview: { 153 | data.no_preview = true; 154 | } break; 155 | case ArgEnum::no_confidence: { 156 | data.no_confidence = true; 157 | } break; 158 | case ArgEnum::no_amplitude: { 159 | data.no_amplitude = true; 160 | } break; 161 | case ArgEnum::no_load_eeprom: { 162 | data.no_load_cali = true; 163 | } break; 164 | case ArgEnum::h_flip: { 165 | data.h_flip = true; 166 | } break; 167 | case ArgEnum::v_flip: { 168 | data.v_flip = true; 169 | } break; 170 | case ArgEnum::confidence_val: { 171 | CHECK_OPT(); 172 | data.confidence_value = atoi(opt); 173 | ASSERT_OPT(data.confidence_value >= 0); 174 | return 2; 175 | } break; 176 | case ArgEnum::exposure_val: { 177 | CHECK_OPT(); 178 | data.exp_time = atoi(opt); 179 | ASSERT_OPT(data.exp_time >= 0); 180 | return 2; 181 | } break; 182 | case ArgEnum::min_range: { 183 | CHECK_OPT(); 184 | data.min_range = atoi(opt); 185 | ASSERT_OPT(data.min_range >= 0); 186 | return 2; 187 | } break; 188 | case ArgEnum::max_range: { 189 | CHECK_OPT(); 190 | data.max_range = atoi(opt); 191 | ASSERT_OPT(data.max_range >= 0); 192 | return 2; 193 | } break; 194 | case ArgEnum::mode: { 195 | CHECK_OPT(); 196 | data.mode = atoi(opt); 197 | ASSERT_OPT(-1 <= data.mode && data.mode <= 2); 198 | return 2; 199 | } break; 200 | case ArgEnum::fps: { 201 | CHECK_OPT(); 202 | data.fps = atoi(opt); 203 | ASSERT_OPT(data.fps >= 0); 204 | return 2; 205 | } break; 206 | case ArgEnum::help: { 207 | help(exec); 208 | exit(0); 209 | } break; 210 | case ArgEnum::version: { 211 | std::cout << "Version: 1.0.0" << std::endl; 212 | exit(0); 213 | } break; 214 | } 215 | return 1; 216 | } 217 | 218 | LOCAL int __parse_opt(int argc, char* argv[], int curr, opt_data& data) 219 | { 220 | bool is_opt, has_arg = false; 221 | std::string tmp; 222 | const char* arg = argv[curr]; 223 | const char* next = curr + 1 < argc ? argv[curr + 1] : nullptr; 224 | // if start with "--" 225 | if (arg[0] != '-') 226 | return 0; 227 | if (arg[1] == '-') { 228 | is_opt = true; 229 | arg += 2; 230 | auto eq_op = strchr(arg, '='); // find the equal sign 231 | if (eq_op != nullptr) { 232 | has_arg = true; 233 | tmp = std::string(arg, eq_op); 234 | arg = tmp.c_str(); 235 | next = eq_op + 1; 236 | } 237 | } else { 238 | is_opt = false; 239 | arg += 1; 240 | } 241 | 242 | // parse a short option 243 | #define SHORT(chr, type) \ 244 | case chr: \ 245 | if (1 != __parse_opt(argv[0], arg, next, data)) { \ 246 | return 0; \ 247 | } \ 248 | break; 249 | // parse a short option with argument 250 | #define SHORT_END(chr, type) \ 251 | case chr: \ 252 | if (arg[index + 1] != '\0') { \ 253 | return __parse_opt(argv[0], arg, arg + index + 1, data) == 0 ? 0 : 1; \ 254 | } \ 255 | return __parse_opt(argv[0], arg, next, data); \ 256 | break; 257 | // parse a long option 258 | #define LONG(chr, type) \ 259 | else if (!strcmp(arg, chr)) \ 260 | { \ 261 | auto ret = __parse_opt(argv[0], arg, next, data); \ 262 | return (has_arg && ret > 1) ? 1 : ret; \ 263 | } 264 | 265 | if (!is_opt) { 266 | for (int index = 0; arg[index] != '\0'; index++) { 267 | switch (arg[index]) { 268 | // SHORT('r', ArgEnum::raw) 269 | SHORT('h', ArgEnum::h_flip) 270 | SHORT('v', ArgEnum::v_flip) 271 | SHORT('V', ArgEnum::version) 272 | SHORT('P', ArgEnum::no_preview) 273 | SHORT('C', ArgEnum::no_confidence) 274 | SHORT('A', ArgEnum::no_amplitude) 275 | SHORT('E', ArgEnum::no_load_eeprom) 276 | SHORT_END('d', ArgEnum::device) 277 | SHORT_END('m', ArgEnum::min_range) 278 | SHORT_END('M', ArgEnum::max_range) 279 | default: 280 | return 0; 281 | } 282 | } 283 | } else { 284 | if (!strcmp(arg, "")) { 285 | return 0; 286 | } 287 | LONG("cfg", ArgEnum::cfg) 288 | LONG("raw", ArgEnum::raw) 289 | LONG("depth", ArgEnum::depth) 290 | LONG("confidence", ArgEnum::confidence_val) 291 | LONG("exposure", ArgEnum::exposure_val) 292 | LONG("hflip", ArgEnum::h_flip) 293 | LONG("h-flip", ArgEnum::h_flip) 294 | LONG("vflip", ArgEnum::v_flip) 295 | LONG("v-flip", ArgEnum::v_flip) 296 | LONG("no-preview", ArgEnum::no_preview) 297 | LONG("no-confidence", ArgEnum::no_confidence) 298 | LONG("no-amplitude", ArgEnum::no_amplitude) 299 | LONG("no-load-eeprom", ArgEnum::no_load_eeprom) 300 | LONG("min-range", ArgEnum::min_range) 301 | LONG("max-range", ArgEnum::max_range) 302 | LONG("mode", ArgEnum::mode) 303 | LONG("fps", ArgEnum::fps) 304 | LONG("help", ArgEnum::help) 305 | LONG("version", ArgEnum::version) 306 | else 307 | { 308 | return 0; 309 | } 310 | } 311 | 312 | #undef SHORT 313 | #undef SHORT_END 314 | #undef LONG 315 | 316 | return 1; 317 | } 318 | 319 | bool parse_opt(int argc, char* argv[], opt_data& opt) 320 | { 321 | for (int i = 1; i < argc;) { 322 | int tmp = __parse_opt(argc, argv, i, opt); 323 | if (tmp == 0) { 324 | std::cerr << "Invalid option: " << argv[i] << std::endl; 325 | return false; 326 | } 327 | i += tmp; 328 | } 329 | 330 | // check 331 | if (opt.no_confidence && opt.raw) { 332 | std::cerr << "Invalid option: --no-confidence and --raw are exclusive" << std::endl; 333 | return false; 334 | } 335 | if (opt.no_amplitude && opt.raw) { 336 | std::cerr << "Invalid option: --no-amplitude and --raw are exclusive" << std::endl; 337 | return false; 338 | } 339 | 340 | return true; 341 | } 342 | 343 | /* #endregion */ 344 | -------------------------------------------------------------------------------- /example/cpp/example/pid.c: -------------------------------------------------------------------------------- 1 | #ifndef _PID_SOURCE_C_ 2 | #define _PID_SOURCE_C_ 3 | 4 | #include "pid.h" 5 | 6 | #include 7 | #include 8 | 9 | struct PIDImpl { 10 | double _dt; 11 | double _max; 12 | double _min; 13 | double _Kp; 14 | double _Kd; 15 | double _Ki; 16 | double _pre_error; 17 | double _integral; 18 | }; 19 | 20 | bool pid_init(pid_ref* pid, double dt, double max, double min, double Kp, double Kd, double Ki) { 21 | pid_ref tmp = (pid_ref)malloc(sizeof(struct PIDImpl)); 22 | if (tmp == NULL) { 23 | return false; 24 | } 25 | tmp->_dt = dt; 26 | tmp->_max = max; 27 | tmp->_min = min; 28 | tmp->_Kp = Kp; 29 | tmp->_Kd = Kd; 30 | tmp->_Ki = Ki; 31 | tmp->_pre_error = 0; 32 | tmp->_integral = 0; 33 | *pid = tmp; 34 | return true; 35 | } 36 | 37 | double pid_calculate(pid_ref pid, double setpoint, double pv) { 38 | // Calculate error 39 | double error = setpoint - pv; 40 | 41 | // Proportional term 42 | double Pout = pid->_Kp * error; 43 | 44 | // Integral term 45 | pid->_integral += error * pid->_dt; 46 | double Iout = pid->_Ki * pid->_integral; 47 | 48 | // Derivative term 49 | double derivative = (error - pid->_pre_error) / pid->_dt; 50 | double Dout = pid->_Kd * derivative; 51 | 52 | // Calculate total output 53 | double output = Pout + Iout + Dout; 54 | 55 | // Restrict to max/min 56 | if (output > pid->_max) 57 | output = pid->_max; 58 | else if (output < pid->_min) 59 | output = pid->_min; 60 | 61 | // Save error to previous error 62 | pid->_pre_error = error; 63 | 64 | return output; 65 | } 66 | 67 | void pid_destroy(pid_ref pid) { free(pid); } 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /example/cpp/example/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef _PID_H_ 2 | #define _PID_H_ 3 | 4 | #include 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | typedef struct PIDImpl *pid_ref; 11 | 12 | /** 13 | * 14 | * Kp - proportional gain 15 | * Ki - Integral gain 16 | * Kd - derivative gain 17 | * dt - loop interval time 18 | * max - maximum value of manipulated variable 19 | * min - minimum value of manipulated variable 20 | */ 21 | bool pid_init(pid_ref* pid, double dt, double max, double min, double Kp, double Kd, double Ki); 22 | /// Returns the manipulated variable given a setpoint and current process value 23 | double pid_calculate(pid_ref pid, double setpoint, double pv); 24 | void pid_destroy(pid_ref pid); 25 | 26 | #ifdef __cplusplus 27 | } 28 | #endif 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /example/cpp/example/test.cpp: -------------------------------------------------------------------------------- 1 | #include "test.hpp" 2 | 3 | #include "ArducamTOFCamera.hpp" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define LOCAL static inline 14 | #define WITH_VT_100 0 15 | 16 | #if WITH_VT_100 17 | #define ESC(x) "\033" x 18 | #define NL ESC("[K\n") 19 | #else 20 | #define ESC(x) "" 21 | #define NL "\n" 22 | #endif 23 | 24 | using namespace Arducam; 25 | using namespace std::literals; 26 | 27 | #define MAX_DISTANCE 4 28 | #define SHOW_FRAME_DELAY 1 29 | #define AVG_FPS 1 30 | 31 | enum class DType { 32 | f32, 33 | u8, 34 | u16, 35 | }; 36 | 37 | LOCAL cv::Mat gamma_table(float gamma); 38 | 39 | #define DO_LATER(opt, expr) \ 40 | do { \ 41 | std::unique_lock lk(opt.process_mtx); \ 42 | opt.process.push_back([=](Arducam::ArducamTOFCamera& tof) { expr; }); \ 43 | } while (false) 44 | 45 | void on_confidence_changed(int pos, void* userdata) 46 | { 47 | opt_data& opt = *(opt_data*)userdata; 48 | opt.confidence_value = pos; 49 | } 50 | void on_amplitude_changed(int pos, void* userdata) 51 | { 52 | opt_data& opt = *(opt_data*)userdata; 53 | opt.amplitude_value = opt_data::amplitude_value_step(pos); 54 | } 55 | void on_exposure_changed(int pos, void* userdata) 56 | { 57 | opt_data& opt = *(opt_data*)userdata; 58 | opt.exp_time = pos; 59 | DO_LATER(opt, tof.setControl(Control::EXPOSURE, pos)); 60 | } 61 | void on_hflip_changed(int pos, void* userdata) 62 | { 63 | opt_data& opt = *(opt_data*)userdata; 64 | opt.h_flip = pos != 0; 65 | DO_LATER(opt, tof.setControl(Control::HFLIP, pos)); 66 | } 67 | void on_vflip_changed(int pos, void* userdata) 68 | { 69 | opt_data& opt = *(opt_data*)userdata; 70 | opt.v_flip = pos != 0; 71 | DO_LATER(opt, tof.setControl(Control::VFLIP, pos)); 72 | } 73 | void on_min_range_changed(int pos, void* userdata) 74 | { 75 | opt_data& opt = *(opt_data*)userdata; 76 | opt.min_range = pos; 77 | if (opt.min_range > opt.max_range) { 78 | opt.max_range = opt.min_range; 79 | cv::setTrackbarPos("max-range", "preview", opt.max_range); 80 | } 81 | } 82 | void on_max_range_changed(int pos, void* userdata) 83 | { 84 | opt_data& opt = *(opt_data*)userdata; 85 | opt.max_range = pos; 86 | if (opt.max_range < opt.min_range) { 87 | opt.min_range = opt.max_range; 88 | cv::setTrackbarPos("min-range", "preview", opt.min_range); 89 | } 90 | } 91 | void on_gain_changed(int pos, void* userdata) 92 | { 93 | opt_data& opt = *(opt_data*)userdata; 94 | // if (pos == 0) { 95 | // opt.gain_val = opt.gain; 96 | // } 97 | opt.gain = pos / 10.0; 98 | } 99 | void on_gamma_changed(int pos, void* userdata) 100 | { 101 | opt_data& opt = *(opt_data*)userdata; 102 | auto tmp = pos / 10.0; 103 | if (tmp != opt.gamma) { 104 | opt.gamma = tmp; 105 | opt.gamma_lut = gamma_table((float)opt.gamma); 106 | } 107 | } 108 | 109 | void onMouse(int event, int x, int y, int flags, void* userdata) 110 | { 111 | (void)flags; 112 | 113 | opt_data& opt = *(opt_data*)userdata; 114 | const auto r = opt.sel_range; 115 | if (x < r || x > (opt.max_width - r) || y < r || y > (opt.max_height - r)) 116 | return; 117 | switch (event) { 118 | case cv::EVENT_LBUTTONDOWN: 119 | break; 120 | 121 | case cv::EVENT_LBUTTONUP: 122 | opt.selectRect.x = x - r; 123 | opt.selectRect.y = y - r; 124 | opt.selectRect.width = 2 * r; 125 | opt.selectRect.height = 2 * r; 126 | break; 127 | default: 128 | opt.followRect.x = x - r; 129 | opt.followRect.y = y - r; 130 | opt.followRect.width = 2 * r; 131 | opt.followRect.height = 2 * r; 132 | break; 133 | } 134 | } 135 | 136 | LOCAL const char* to_str(DType type) 137 | { 138 | switch (type) { 139 | case DType::f32: 140 | return "f32"; 141 | case DType::u8: 142 | return "u8"; 143 | case DType::u16: 144 | return "u16"; 145 | default: 146 | return "unknown"; 147 | } 148 | } 149 | 150 | LOCAL int to_size(DType type) 151 | { 152 | switch (type) { 153 | case DType::f32: 154 | return sizeof(float); 155 | case DType::u8: 156 | return sizeof(uint8_t); 157 | case DType::u16: 158 | return sizeof(uint16_t); 159 | default: 160 | return 0; 161 | } 162 | } 163 | 164 | #if AVG_FPS 165 | LOCAL void display_fps(void) 166 | { 167 | using std::chrono::high_resolution_clock; 168 | static auto start = high_resolution_clock::now(); 169 | static float avg_duration = 0; 170 | static float alpha = 1.F / 10; 171 | static int count = 0; 172 | 173 | auto now = high_resolution_clock::now(); 174 | auto cost_ms = (now - start) / 1ms; 175 | start = now; 176 | 177 | if (count == 0) { 178 | ++count; 179 | return; 180 | } else if (count == 1) { 181 | avg_duration = (float)cost_ms; 182 | } else { 183 | float t = avg_duration * (1 - alpha) + cost_ms * alpha; 184 | if (std::isfinite(t)) { 185 | avg_duration = t; 186 | alpha = 1.F / (2000.F / t); 187 | alpha = std::min(1.F / 30, std::max(1.F / 1, alpha)); 188 | } 189 | } 190 | 191 | ++count; 192 | 193 | std::cout << "fps:" << (1000. / avg_duration) << NL; 194 | } 195 | #else 196 | LOCAL void display_fps(void) 197 | { 198 | using std::chrono::high_resolution_clock; 199 | using namespace std::literals; 200 | static int count = 0; 201 | static auto time_beg = high_resolution_clock::now(); 202 | auto time_end = high_resolution_clock::now(); 203 | ++count; 204 | auto duration_ms = (time_end - time_beg) / 1ms; 205 | if (duration_ms >= 1000) { 206 | std::cout << "fps:" << count << NL; 207 | count = 0; 208 | time_beg = time_end; 209 | } 210 | } 211 | #endif 212 | 213 | #if 0 214 | LOCAL void getPreview(cv::Mat preview_ptr, cv::Mat confidence_image_ptr, int confidence_value) 215 | { 216 | preview_ptr.setTo(255, confidence_image_ptr < confidence_value); 217 | } 218 | LOCAL void getPreviewRGB(cv::Mat preview_ptr, cv::Mat confidence_image_ptr, int confidence_value) 219 | { 220 | preview_ptr.setTo(cv::Scalar(0, 0, 0), confidence_image_ptr < confidence_value); 221 | // cv::GaussianBlur(preview_ptr, preview_ptr, cv::Size(7, 7), 0); 222 | } 223 | #endif 224 | 225 | LOCAL std::string saveData(void* data, unsigned int width, unsigned int height, const char* prefix = "depth", 226 | DType d_type = DType::f32) 227 | { 228 | using std::to_string; 229 | using std::chrono::system_clock; 230 | using namespace std::literals; 231 | // get filename 232 | // "depth_$width_$height_f32_$timestamp.raw" 233 | std::string filename = prefix + "_"s + to_string(width) + "_" + to_string(height) + "_" + to_str(d_type) + "_" + 234 | to_string(system_clock::now().time_since_epoch() / 1ms) + ".raw"; 235 | 236 | // save data 237 | auto file = std::ofstream(filename, std::ios::binary); 238 | if (!file.is_open()) { 239 | std::cerr << "save data failed" << NL; 240 | return ""; 241 | } 242 | file.write(reinterpret_cast(data), width * height * to_size(d_type)); 243 | file.close(); 244 | 245 | std::cout << "save data to " << filename << NL; 246 | return filename; 247 | } 248 | 249 | LOCAL std::string appendData(void* data, unsigned int width, unsigned int height, const std::string& filename, 250 | DType d_type = DType::f32) 251 | { 252 | using std::to_string; 253 | using std::chrono::system_clock; 254 | using namespace std::literals; 255 | // save data 256 | auto file = std::ofstream(filename, std::ios::app | std::ios::binary); 257 | if (!file.is_open()) { 258 | std::cerr << "append data failed" << NL; 259 | return ""; 260 | } 261 | file.write(reinterpret_cast(data), width * height * to_size(d_type)); 262 | file.close(); 263 | 264 | std::cout << "append data to " << filename << NL; 265 | return filename; 266 | } 267 | 268 | LOCAL bool checkExit(const opt_data& opt) 269 | { 270 | (void)opt; 271 | 272 | int key = cv::waitKey(500); 273 | switch (key) { 274 | case 27: 275 | case 'q': 276 | return false; 277 | } 278 | return true; 279 | } 280 | 281 | LOCAL bool processKey(const opt_data& opt, void* data, void* data2 = nullptr, void* data3 = nullptr) 282 | { 283 | static std::string last_filename; 284 | static int need_save_cnt = 0, need_save_input = 0; 285 | int key = 0; 286 | if (opt.fps <= 0) { 287 | key = cv::waitKey(1); 288 | } else { 289 | key = cv::waitKey(500 / opt.fps); 290 | } 291 | if (key == -1 && need_save_cnt > 0) { 292 | key = 'r'; 293 | } 294 | switch (key) { 295 | case 27: 296 | case 'q': 297 | return false; 298 | case 's': 299 | if (data2 == nullptr) { 300 | last_filename = saveData(data, opt.max_width, opt.max_height, "raw", DType::u16); 301 | } else { 302 | saveData(data, opt.max_width, opt.max_height, "depth", DType::f32); 303 | saveData(data2, opt.max_width, opt.max_height, "confidence", DType::f32); 304 | saveData(data3, opt.max_width, opt.max_height, "gray", DType::f32); 305 | } 306 | break; 307 | case 'r': 308 | if (need_save_input != 0) { 309 | need_save_cnt = need_save_input; 310 | need_save_input = 0; 311 | } 312 | if (data2 == nullptr) { 313 | if (last_filename.empty()) { 314 | last_filename = saveData(data, opt.max_width, opt.max_height, "raw", DType::u16); 315 | } else { 316 | last_filename = appendData(data, opt.max_width, opt.max_height, last_filename, DType::u16); 317 | } 318 | need_save_cnt--; 319 | if (need_save_cnt == 0) { 320 | std::cout << "save done to " << last_filename << NL; 321 | last_filename.clear(); 322 | } 323 | } 324 | break; 325 | case 'c': 326 | need_save_input = 0; 327 | std::cout << "input: " << need_save_input << NL; 328 | 329 | break; 330 | case '0': 331 | case '1': 332 | case '2': 333 | case '3': 334 | case '4': 335 | case '5': 336 | case '6': 337 | case '7': 338 | case '8': 339 | case '9': { 340 | need_save_input *= 10; 341 | need_save_input += key - '0'; 342 | std::cout << "input: " << need_save_input << NL; 343 | } 344 | default: 345 | break; 346 | } 347 | return true; 348 | } 349 | 350 | LOCAL bool raw_loop(Arducam::ArducamTOFCamera& tof, opt_data& data) 351 | { 352 | Arducam::ArducamFrameBuffer* frame = tof.requestFrame(2000); 353 | if (frame == nullptr) { 354 | return checkExit(data); 355 | } 356 | Arducam::FrameFormat format; 357 | frame->getFormat(FrameType::RAW_FRAME, format); 358 | std::cout << "frame: (" << format.width << "x" << format.height << "), time: " << format.timestamp << NL; 359 | data.max_height = format.height; 360 | data.max_width = format.width; 361 | #if SHOW_FRAME_DELAY 362 | if (1000000000000UL <= format.timestamp && format.timestamp <= 9000000000000UL) { 363 | // a timestamp in milliseconds (13 digits) 364 | uint64_t now = std::chrono::system_clock::now().time_since_epoch() / 1ms; 365 | std::cout << "timestamp: " << format.timestamp << ", now: " << now << ", diff: " << (now - format.timestamp) 366 | << "ms" << NL; 367 | } else if (1000000000UL <= format.timestamp && format.timestamp <= 9000000000UL) { 368 | // a timestamp in seconds (10 digits) 369 | uint64_t now = std::chrono::system_clock::now().time_since_epoch() / 1s; 370 | std::cout << "timestamp: " << format.timestamp << ", now: " << now << ", diff: " << (now - format.timestamp) 371 | << "s" << NL; 372 | } else { 373 | // invalid timestamp from epoch 374 | // with mono timestamp 375 | uint64_t now = std::chrono::steady_clock::now().time_since_epoch() / 1ms; 376 | std::cout << "timestamp: " << format.timestamp << ", now: " << now << ", diff: " << (now - format.timestamp) 377 | << "ms" << NL; 378 | } 379 | #endif 380 | 381 | // depth_ptr = (float*)frame->getData(FrameType::DEPTH_FRAME); 382 | uint16_t* raw_data = (uint16_t*)frame->getData(FrameType::RAW_FRAME); 383 | if (raw_data == nullptr) { 384 | tof.releaseFrame(frame); 385 | return checkExit(data); 386 | } 387 | 388 | cv::Mat result_frame(format.height, format.width, CV_8U); 389 | cv::Mat raw_frame(format.height, format.width, CV_16U, raw_data); 390 | 391 | raw_frame.convertTo(result_frame, CV_8U, 1. / (1 << 4), 0); 392 | if (!data.no_preview) { 393 | cv::imshow("preview", result_frame); 394 | } 395 | display_fps(); 396 | if (!processKey(data, raw_data)) { 397 | return false; 398 | } 399 | tof.releaseFrame(frame); 400 | return true; 401 | } 402 | 403 | LOCAL bool depth_loop(Arducam::ArducamTOFCamera& tof, opt_data& data) 404 | { 405 | // auto info = tof.getCameraInfo(); 406 | Arducam::ArducamFrameBuffer* frame = tof.requestFrame(2000); 407 | if (frame == nullptr) { 408 | return checkExit(data); 409 | } 410 | Arducam::FrameFormat format; 411 | frame->getFormat(FrameType::DEPTH_FRAME, format); 412 | std::cout << "frame: (" << format.width << "x" << format.height << "), time: " << format.timestamp << " " << NL; 413 | data.max_height = format.height; 414 | data.max_width = format.width; 415 | const int min_range = data.min_range, max_range = data.max_range; 416 | #if SHOW_FRAME_DELAY 417 | if (1000000000000UL <= format.timestamp && format.timestamp <= 9000000000000UL) { 418 | // a timestamp in milliseconds (13 digits) 419 | uint64_t now = std::chrono::system_clock::now().time_since_epoch() / 1ms; 420 | std::cout << "timestamp: " << format.timestamp << ", now: " << now << ", diff: " << (now - format.timestamp) 421 | << "ms" << NL; 422 | } else if (1000000000UL <= format.timestamp && format.timestamp <= 9000000000UL) { 423 | // a timestamp in seconds (10 digits) 424 | uint64_t now = std::chrono::system_clock::now().time_since_epoch() / 1s; 425 | std::cout << "timestamp: " << format.timestamp << ", now: " << now << ", diff: " << (now - format.timestamp) 426 | << "s" << NL; 427 | } else { 428 | // invalid timestamp from epoch 429 | // with mono timestamp 430 | uint64_t now = std::chrono::steady_clock::now().time_since_epoch() / 1ms; 431 | std::cout << "timestamp: " << format.timestamp << ", now: " << now << ", diff: " << (now - format.timestamp) 432 | << "ms" << NL; 433 | } 434 | #endif 435 | float* depth_ptr = (float*)frame->getData(FrameType::DEPTH_FRAME); 436 | float* amplitude_ptr = (float*)frame->getData(FrameType::AMPLITUDE_FRAME); 437 | float* confidence_ptr = (float*)frame->getData(FrameType::CONFIDENCE_FRAME); 438 | 439 | cv::Mat result_frame(format.height, format.width, CV_8U); 440 | cv::Mat depth_frame(format.height, format.width, CV_32F, depth_ptr); 441 | cv::Mat amplitude_frame_ori(format.height, format.width, CV_32F, amplitude_ptr); 442 | cv::Mat amplitude_frame(format.height, format.width, CV_8U); 443 | cv::Mat confidence_frame_ori(format.height, format.width, CV_32F, confidence_ptr); 444 | cv::Mat confidence_frame(format.height, format.width, CV_32F); 445 | 446 | depth_frame.convertTo(result_frame, CV_8U, 255.0 / (max_range - min_range), 447 | (-min_range * 255.0) / (max_range - min_range)); 448 | // getPreview(result_frame, confidence_frame); 449 | cv::applyColorMap(result_frame, result_frame, cv::COLORMAP_RAINBOW); 450 | result_frame.setTo(cv::Scalar(0, 0, 0), (depth_frame < min_range) | (depth_frame > max_range)); 451 | result_frame.setTo(cv::Scalar(0, 0, 0), 452 | (confidence_frame_ori < data.confidence_value) | (amplitude_frame_ori < data.amplitude_value)); 453 | 454 | confidence_frame_ori.convertTo(confidence_frame, CV_32F, 1 / 255.0, 0); 455 | // cv::normalize(amplitude_frame_ori, amplitude_frame, 255, 0, cv::NORM_MINMAX, CV_8U); 456 | if (data.gain == 0) { 457 | double min, max; 458 | // cv::minMaxLoc(amplitude_frame_ori, &min, &max); 459 | std::vector tmp; 460 | amplitude_frame_ori.reshape(1, 1).copyTo(tmp); 461 | auto min_it = tmp.begin() + tmp.size() * 0.02; 462 | std::nth_element(tmp.begin(), min_it, tmp.end()); 463 | min = *min_it; 464 | auto max_it = tmp.begin() + tmp.size() * 0.98; 465 | std::nth_element(tmp.begin(), max_it, tmp.end()); 466 | max = *max_it; 467 | // printf("= min: %f, max: %f\n", min, max); 468 | constexpr double gain_min = .05 / (1 << (12 - 8)); 469 | constexpr double gain_max = 80. / (1 << (12 - 8)); 470 | double exp_gain = 255.0 / (max - min); 471 | exp_gain = std::min(std::max(exp_gain, gain_min), gain_max); 472 | double exp_offset = -min * exp_gain; 473 | double inc_gain = pid_calculate(data.gain_pid, exp_gain, data.gain_val); 474 | double inc_offset = pid_calculate(data.gain_offset_pid, exp_offset, data.gain_offset_val); 475 | data.gain_val = data.gain_val + inc_gain; 476 | data.gain_offset_val = data.gain_offset_val + inc_offset; 477 | // printf("exp: (x%f+%f), inc: (%f, %f), real: (x%f+%f)\n", exp_gain * (1 << (12 - 8)), exp_offset, inc_gain, 478 | // inc_offset, data.gain_val * (1 << (12 - 8)), data.gain_offset_val); 479 | amplitude_frame_ori.convertTo(amplitude_frame, CV_8U, data.gain_val, data.gain_offset_val); 480 | } else { 481 | amplitude_frame_ori.convertTo(amplitude_frame, CV_8U, data.gain / (1 << (12 - 8)), 0); 482 | } 483 | if (data.gamma != 1) { 484 | cv::LUT(amplitude_frame, data.gamma_lut, amplitude_frame); 485 | } 486 | 487 | cv::rectangle(result_frame, data.selectRect, cv::Scalar(0, 0, 0), 2); 488 | cv::rectangle(result_frame, data.followRect, cv::Scalar(255, 255, 255), 1); 489 | 490 | if (!data.no_preview) { 491 | cv::imshow("preview", result_frame); 492 | std::cout << "select Rect distance: " << cv::mean(depth_frame(data.selectRect)).val[0] 493 | << "mm, pos: " << data.selectRect << NL; 494 | } 495 | if (!data.no_confidence) { 496 | cv::imshow("confidence", confidence_frame); 497 | } 498 | if (!data.no_amplitude) { 499 | cv::imshow("amplitude", amplitude_frame); 500 | } 501 | 502 | display_fps(); 503 | if (!processKey(data, depth_ptr, confidence_ptr, amplitude_ptr)) { 504 | return false; 505 | } 506 | tof.releaseFrame(frame); 507 | return true; 508 | } 509 | LOCAL void process_later(Arducam::ArducamTOFCamera& tof, opt_data& data) 510 | { 511 | std::unique_lock lk(data.process_mtx); 512 | for (auto& proc : data.process) { 513 | proc(tof); 514 | } 515 | data.process.clear(); 516 | } 517 | 518 | #define check(expr, err_msg) \ 519 | do { \ 520 | Arducam::TofErrorCode ret = (expr); \ 521 | if (ret != Arducam::TofErrorCode::ArducamSuccess) { \ 522 | std::cerr << err_msg << ": " << to_str(ret) << std::endl; \ 523 | return -1; \ 524 | } \ 525 | } while (0) 526 | #define check_nonret(expr, err_msg) \ 527 | do { \ 528 | Arducam::TofErrorCode ret = (expr); \ 529 | if (ret != Arducam::TofErrorCode::ArducamSuccess) { \ 530 | std::cerr << err_msg << ": " << to_str(ret) << std::endl; \ 531 | } \ 532 | } while (0) 533 | 534 | #define set_ctl_(tof, ctrl, val) \ 535 | check_nonret(tof.setControl(Control::ctrl, val), "set control(" #ctrl ", " #val ") failed") 536 | #define set_ctl(ctrl, val) set_ctl_(tof, ctrl, val) 537 | 538 | int main(int argc, char* argv[]) 539 | { 540 | opt_data opt; 541 | 542 | if (!parse_opt(argc, argv, opt)) { 543 | return -1; 544 | } 545 | 546 | opt.gamma_lut = gamma_table((float)opt.gamma); 547 | if (!pid_init(&opt.gain_pid, 1. / 4, 1, -1, 0.4, 0.01, 0) || 548 | !pid_init(&opt.gain_offset_pid, 1. / 4, 5, -5, 0.4, 0.01, 0.3)) { 549 | std::cerr << "pid init failed" << std::endl; 550 | return -1; 551 | } 552 | 553 | Arducam::ArducamTOFCamera tof; 554 | // if (tof.openWithFile(Arducam::DEPTH_FRAME, "imx316_mcu.cfg", 375e5)) 555 | if (opt.cfg) { 556 | check(tof.openWithFile(opt.cfg, opt.device), "open camera failed"); 557 | } else { 558 | check(tof.open(Connection::CSI, opt.device), "open camera failed"); 559 | } 560 | 561 | int option = opt.mode; 562 | auto info = tof.getCameraInfo(); 563 | if (info.device_type == Arducam::DeviceType::DEVICE_HQVGA) { 564 | if (option == -2) { 565 | option = 1; 566 | } 567 | switch (option) { 568 | case -1: 569 | std::cout << "keep default\n"; 570 | break; 571 | case 0: 572 | std::cout << "near mode\n"; 573 | break; 574 | case 1: 575 | std::cout << "far mode\n"; 576 | break; 577 | default: 578 | std::cout << "invalid mode: " << option << std::endl; 579 | return -1; 580 | } 581 | 582 | if (option == -1) { 583 | // do nothing 584 | } else { 585 | if (option & 1) { 586 | set_ctl(RANGE, 4000); 587 | } else { 588 | set_ctl(RANGE, 2000); 589 | } 590 | } 591 | } else if (info.device_type == Arducam::DeviceType::DEVICE_VGA) { 592 | if (option == -2) { 593 | option = 1; 594 | } 595 | switch (option) { 596 | case -1: 597 | std::cout << "keep default\n"; 598 | break; 599 | case 0: 600 | std::cout << "640 * 480 with single frequency\n"; 601 | break; 602 | case 1: 603 | std::cout << "640 * 480 with dual frequency\n"; 604 | break; 605 | default: 606 | std::cout << "invalid mode: " << option << std::endl; 607 | return -1; 608 | } 609 | 610 | if (option == -1) { 611 | // do nothing 612 | } else { 613 | if (option & 1) { 614 | set_ctl(MODE, to_int(TofWorkMode::DOUBLE_FREQ)); 615 | } else { 616 | set_ctl(MODE, to_int(TofWorkMode::SINGLE_FREQ)); 617 | } 618 | } 619 | } 620 | 621 | if (opt.no_load_cali) { 622 | set_ctl(LOAD_CALI_DATA, 0); 623 | } 624 | 625 | set_ctl(HFLIP, opt.h_flip); 626 | set_ctl(VFLIP, opt.v_flip); 627 | 628 | if (opt.raw) { 629 | check(tof.start(FrameType::RAW_FRAME), "start camera failed"); 630 | } else { 631 | check(tof.start(FrameType::DEPTH_FRAME), "start camera failed"); 632 | } 633 | 634 | if (opt.fps != -1) { 635 | if (opt.fps != 0) { 636 | int max_fps = 0; 637 | tof.getControl(Control::FRAME_RATE, &max_fps); 638 | if (opt.fps > max_fps) { 639 | std::cerr << "Invalid fps: " << opt.fps << ", max fps: " << max_fps << std::endl; 640 | return -1; 641 | } 642 | set_ctl(FRAME_RATE, opt.fps); 643 | } 644 | // disable auto frame rate if the camera has the control 645 | (void)tof.setControl(Control::AUTO_FRAME_RATE, 0); 646 | } 647 | tof.getControl(Control::FRAME_RATE, &opt.fps); 648 | 649 | if (opt.exp_time == 0) { 650 | tof.getControl(Control::EXPOSURE, &opt.exp_time); 651 | } else { 652 | set_ctl(EXPOSURE, opt.exp_time); 653 | } 654 | 655 | if (info.device_type == Arducam::DeviceType::DEVICE_HQVGA) { 656 | set_ctl(RANGE, opt.mode == 0 ? 2000 : 4000); 657 | } 658 | 659 | // int rang = 0; 660 | // tof.setControl(Control::RANGE, MAX_DISTANCE); 661 | // tof.getControl(Control::RANGE, &rang); 662 | // std::cout << rang << std::endl; 663 | 664 | info = tof.getCameraInfo(); 665 | int max_range; 666 | tof.getControl(Control::RANGE, &max_range); 667 | std::cout << "open camera with (" << info.width << "x" << info.height << ") with range " << max_range << std::endl; 668 | // tof.writeSensor(0x0402, 0x01301e00); 669 | tof.writeSensor(0x0402, 0x01300a00); 670 | 671 | if (opt.max_range == 0) { 672 | opt.max_range = max_range; 673 | } 674 | 675 | if (!opt.no_preview) { 676 | cv::namedWindow("preview", cv::WINDOW_NORMAL); 677 | cv::setMouseCallback("preview", onMouse, &opt); 678 | 679 | cv::createTrackbar("min-range", "preview", NULL, 6000, on_min_range_changed, &opt); 680 | cv::createTrackbar("max-range", "preview", NULL, 6000, on_max_range_changed, &opt); 681 | // cv::createTrackbar("hflip", "preview", NULL, 1, on_hflip_changed, &opt); 682 | // cv::createTrackbar("vflip", "preview", NULL, 1, on_vflip_changed, &opt); 683 | 684 | cv::setTrackbarPos("min-range", "preview", opt.min_range); 685 | cv::setTrackbarPos("max-range", "preview", opt.max_range); 686 | // cv::setTrackbarPos("hflip", "preview", opt.h_flip); 687 | // cv::setTrackbarPos("vflip", "preview", opt.v_flip); 688 | 689 | if (info.device_type == Arducam::DeviceType::DEVICE_VGA) { 690 | // exposure time 691 | cv::createTrackbar("exposure", "preview", NULL, 3000, on_exposure_changed, &opt); 692 | cv::setTrackbarPos("exposure", "preview", opt.exp_time); 693 | // only vga support confidence 694 | cv::createTrackbar("confidence", "preview", NULL, 255, on_confidence_changed, &opt); 695 | cv::setTrackbarPos("confidence", "preview", opt.confidence_value); 696 | // amplitude filter 697 | cv::createTrackbar("amplitude", "preview", NULL, opt_data::amplitude_value_range, on_amplitude_changed, 698 | &opt); 699 | cv::setTrackbarPos("amplitude", "preview", 0); 700 | } 701 | } 702 | 703 | std::cout << ESC("[?25l"); // hide cursor 704 | std::cout << ESC("7"); // save cursor position 705 | std::cout << NL << NL << NL << NL << NL << NL; // move cursor down 6 lines 706 | std::cout << ESC("8"); // restore cursor position 707 | if (opt.raw) { 708 | for (; raw_loop(tof, opt);) { 709 | process_later(tof, opt); 710 | std::cout << ESC("8"); // restore cursor position 711 | } 712 | } else { 713 | if (!opt.no_confidence) { 714 | cv::namedWindow("confidence", cv::WINDOW_NORMAL); 715 | } 716 | if (!opt.no_amplitude) { 717 | cv::namedWindow("amplitude", cv::WINDOW_NORMAL); 718 | 719 | cv::createTrackbar("gain x10", "amplitude", NULL, static_cast(80.0 * 10), on_gain_changed, &opt); 720 | cv::createTrackbar("gamma x10", "amplitude", NULL, static_cast(2.5 * 10), on_gamma_changed, &opt); 721 | cv::setTrackbarPos("gain x10", "amplitude", static_cast(opt.gain * 10)); 722 | cv::setTrackbarPos("gamma x10", "amplitude", static_cast(opt.gamma * 10)); 723 | } 724 | for (; depth_loop(tof, opt);) { 725 | process_later(tof, opt); 726 | std::cout << ESC("8"); // restore cursor position 727 | } 728 | } 729 | std::cout << ESC("[?25h"); // show cursor 730 | std::cout << ESC("[6B") << NL; // move cursor down 6 lines 731 | 732 | if (tof.stop()) { 733 | std::cerr << "stop camera failed\n"; 734 | } 735 | if (tof.close()) { 736 | std::cerr << "close camera failed\n"; 737 | } 738 | 739 | pid_destroy(opt.gain_pid); 740 | pid_destroy(opt.gain_offset_pid); 741 | return 0; 742 | } 743 | 744 | LOCAL cv::Mat gamma_table(float gamma) 745 | { 746 | cv::Mat lut(1, 256, CV_8U); 747 | float inv_gamma = 1.F / gamma; 748 | for (int i = 0; i < 256; i++) { 749 | lut.at(i) = cv::saturate_cast(pow(i / 255.0, inv_gamma) * 255.0); 750 | } 751 | return lut; 752 | } 753 | -------------------------------------------------------------------------------- /example/cpp/example/test.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ArducamTOFCamera.hpp" 4 | #include "pid.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | struct opt_data { 12 | bool raw = false; 13 | bool no_preview = false; 14 | bool no_confidence = false; 15 | bool no_amplitude = false; 16 | bool no_load_cali = false; 17 | int device = 0; 18 | int mode = -2; 19 | const char* cfg = nullptr; 20 | int fps = -1; 21 | int min_range = 0; 22 | int max_range = 0; 23 | int confidence_value = 30; 24 | static constexpr int amplitude_value_max = (1 << 12) / 20; 25 | static constexpr int amplitude_value_range = 16; 26 | static constexpr double amplitude_value_step_no_linear_base = 1.5; 27 | static constexpr int amplitude_value_step(int val) 28 | { 29 | /** 30 | * make a simple non-linear step 31 | * \alpha = 1.5 32 | * \text{range} = 16 33 | * \text{max} = \frac{1 << 12}{20} 34 | * f(val) = \frac{val^{\alpha} - 1}{\text{range}^{\alpha} - 1} \times \text{max} 35 | */ 36 | return static_cast( // 37 | (std::pow(val, amplitude_value_step_no_linear_base) - 1) / 38 | (std::pow(amplitude_value_range, amplitude_value_step_no_linear_base) - 1) * amplitude_value_max); 39 | } 40 | int amplitude_value = amplitude_value_step(0); 41 | int exp_time = 0; 42 | bool h_flip = false, v_flip = false; 43 | 44 | cv::Rect selectRect{0, 0, 0, 0}; 45 | cv::Rect followRect{0, 0, 0, 0}; 46 | int sel_range = 4; 47 | int max_width = 240; 48 | int max_height = 180; 49 | double gain = 0; 50 | double gamma = 1.7; 51 | cv::Mat gamma_lut; 52 | double gain_val = 1, gain_offset_val = 0; 53 | pid_ref gain_pid, gain_offset_pid; 54 | 55 | std::mutex process_mtx; 56 | std::vector> process; 57 | }; 58 | 59 | bool parse_opt(int argc, char* argv[], opt_data& opt); 60 | -------------------------------------------------------------------------------- /example/cpp/preview_by_usb.cpp: -------------------------------------------------------------------------------- 1 | #include "ArducamTOFCamera.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace Arducam; 10 | 11 | // MAX_DISTANCE value modifiable is 2 or 4 12 | #define MAX_DISTANCE 4 13 | 14 | cv::Rect seletRect(0, 0, 0, 0); 15 | cv::Rect followRect(0, 0, 0, 0); 16 | int max_width = 240; 17 | int max_height = 180; 18 | int max_range = 0; 19 | 20 | void display_fps(void) 21 | { 22 | using std::chrono::high_resolution_clock; 23 | using namespace std::literals; 24 | static int count = 0; 25 | static auto time_beg = high_resolution_clock::now(); 26 | auto time_end = high_resolution_clock::now(); 27 | ++count; 28 | auto duration_ms = (time_end - time_beg) / 1ms; 29 | if (duration_ms >= 1000) { 30 | std::cout << "fps:" << count << std::endl; 31 | count = 0; 32 | time_beg = time_end; 33 | } 34 | } 35 | 36 | void save_image(float* image, int width, int height) 37 | { 38 | using namespace std::literals; 39 | // filename = "depth_$width$_$height$_f32_$time.raw" 40 | auto now = std::chrono::system_clock::now().time_since_epoch() / 1ms; 41 | std::string filename = 42 | "depth_" + std::to_string(width) + "_" + std::to_string(height) + "_f32_" + std::to_string(now) + ".raw"; 43 | std::ofstream file(filename, std::ios::binary); 44 | file.write(reinterpret_cast(image), width * height * sizeof(float)); 45 | file.close(); 46 | } 47 | 48 | void getPreview(uint8_t* preview_ptr, float* phase_image_ptr, float* amplitude_image_ptr) 49 | { 50 | auto len = 240 * 180; 51 | for (auto i = 0; i < len; i++) { 52 | uint8_t mask = *(amplitude_image_ptr + i) > 30 ? 254 : 0; 53 | float depth = ((1 - (*(phase_image_ptr + i) / MAX_DISTANCE)) * 255); 54 | uint8_t pixel = depth > 255 ? 255 : depth; 55 | *(preview_ptr + i) = pixel & mask; 56 | } 57 | } 58 | 59 | void onMouse(int event, int x, int y, int flags, void* param) 60 | { 61 | if (x < 4 || x > (max_width - 4) || y < 4 || y > (max_height - 4)) 62 | return; 63 | switch (event) { 64 | case cv::EVENT_LBUTTONDOWN: 65 | 66 | break; 67 | 68 | case cv::EVENT_LBUTTONUP: 69 | seletRect.x = x - 4 ? x - 4 : 0; 70 | seletRect.y = y - 4 ? y - 4 : 0; 71 | seletRect.width = 8; 72 | seletRect.height = 8; 73 | break; 74 | default: 75 | followRect.x = x - 4 ? x - 4 : 0; 76 | followRect.y = y - 4 ? y - 4 : 0; 77 | followRect.width = 8; 78 | followRect.height = 8; 79 | break; 80 | } 81 | } 82 | 83 | int main() 84 | { 85 | ArducamTOFCamera tof; 86 | ArducamFrameBuffer* frame; 87 | if (tof.open(Connection::USB)) { 88 | std::cerr << "initialization failed" << std::endl; 89 | return -1; 90 | } 91 | 92 | if (tof.start(FrameType::DEPTH_FRAME)) { 93 | std::cerr << "Failed to start camera" << std::endl; 94 | return -1; 95 | } 96 | // Modify the range also to modify the MAX_DISTANCE 97 | tof.setControl(Control::RANGE, MAX_DISTANCE); 98 | tof.getControl(Control::RANGE, &max_range); 99 | auto info = tof.getCameraInfo(); 100 | std::cout << "open camera with (" << info.width << "x" << info.height << ")" << std::endl; 101 | 102 | float* depth_ptr; 103 | float* amplitude_ptr; 104 | uint8_t* preview_ptr = new uint8_t[info.width * info.height * 2]; 105 | cv::namedWindow("preview", cv::WINDOW_AUTOSIZE); 106 | cv::setMouseCallback("preview", onMouse); 107 | 108 | for (;;) { 109 | Arducam::FrameFormat format; 110 | frame = tof.requestFrame(200); 111 | if (frame == nullptr) { 112 | continue; 113 | } 114 | frame->getFormat(FrameType::DEPTH_FRAME, format); 115 | std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl; 116 | max_height = format.height; 117 | max_width = format.width; 118 | 119 | depth_ptr = (float*)frame->getData(FrameType::DEPTH_FRAME); 120 | amplitude_ptr = (float*)frame->getData(FrameType::CONFIDENCE_FRAME); 121 | // getPreview(preview_ptr, depth_ptr, amplitude_ptr); 122 | 123 | cv::Mat result_frame(format.height, format.width, CV_8U, preview_ptr); 124 | cv::Mat depth_frame(format.height, format.width, CV_32F, depth_ptr); 125 | cv::Mat amplitude_frame(format.height, format.width, CV_32F, amplitude_ptr); 126 | 127 | // depth_frame = matRotateClockWise180(depth_frame); 128 | // result_frame = matRotateClockWise180(result_frame); 129 | // amplitude_frame = matRotateClockWise180(amplitude_frame); 130 | depth_frame.convertTo(result_frame, CV_8U, 255.0 / 7000, 0); 131 | 132 | cv::applyColorMap(result_frame, result_frame, cv::COLORMAP_RAINBOW); 133 | amplitude_frame.convertTo(amplitude_frame, CV_8U, 255.0 / 1024, 0); 134 | 135 | cv::imshow("amplitude", amplitude_frame); 136 | 137 | cv::rectangle(result_frame, seletRect, cv::Scalar(0, 0, 0), 2); 138 | cv::rectangle(result_frame, followRect, cv::Scalar(255, 255, 255), 1); 139 | 140 | std::cout << "select Rect distance: " << cv::mean(depth_frame(seletRect)).val[0] << std::endl; 141 | 142 | cv::imshow("preview", result_frame); 143 | 144 | auto key = cv::waitKey(1); 145 | if (key == 27 || key == 'q') { 146 | break; 147 | } else if (key == 's') { 148 | save_image(depth_ptr, format.width, format.height); 149 | } 150 | display_fps(); 151 | tof.releaseFrame(frame); 152 | } 153 | 154 | if (tof.stop()) { 155 | return -1; 156 | } 157 | 158 | if (tof.close()) { 159 | return -1; 160 | } 161 | 162 | return 0; 163 | } 164 | -------------------------------------------------------------------------------- /example/cpp/preview_depth.cpp: -------------------------------------------------------------------------------- 1 | #include "ArducamTOFCamera.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace Arducam; 10 | 11 | // MAX_DISTANCE value modifiable is 2 or 4 12 | #define MAX_DISTANCE 4000 13 | 14 | cv::Rect seletRect(0, 0, 0, 0); 15 | cv::Rect followRect(0, 0, 0, 0); 16 | int max_width = 240; 17 | int max_height = 180; 18 | int max_range = 0; 19 | int confidence_value = 30; 20 | 21 | void on_confidence_changed(int pos, void* userdata) 22 | { 23 | // 24 | } 25 | 26 | void display_fps(void) 27 | { 28 | using std::chrono::high_resolution_clock; 29 | using namespace std::literals; 30 | static int count = 0; 31 | static auto time_beg = high_resolution_clock::now(); 32 | auto time_end = high_resolution_clock::now(); 33 | ++count; 34 | auto duration_ms = (time_end - time_beg) / 1ms; 35 | if (duration_ms >= 1000) { 36 | std::cout << "fps:" << count << std::endl; 37 | count = 0; 38 | time_beg = time_end; 39 | } 40 | } 41 | 42 | void save_image(float* image, int width, int height) 43 | { 44 | using namespace std::literals; 45 | // filename = "depth_$width$_$height$_f32_$time.raw" 46 | auto now = std::chrono::system_clock::now().time_since_epoch() / 1ms; 47 | std::string filename = 48 | "depth_" + std::to_string(width) + "_" + std::to_string(height) + "_f32_" + std::to_string(now) + ".raw"; 49 | std::ofstream file(filename, std::ios::binary); 50 | file.write(reinterpret_cast(image), width * height * sizeof(float)); 51 | file.close(); 52 | } 53 | 54 | cv::Mat matRotateClockWise180(cv::Mat src) 55 | { 56 | if (src.empty()) { 57 | std::cerr << "RorateMat src is empty!"; 58 | } 59 | 60 | flip(src, src, 0); 61 | flip(src, src, 1); 62 | return src; 63 | } 64 | 65 | void getPreview(cv::Mat preview_ptr, cv::Mat amplitude_image_ptr) 66 | { 67 | auto len = preview_ptr.rows * preview_ptr.cols; 68 | for (int line = 0; line < preview_ptr.rows; line++) { 69 | for (int col = 0; col < preview_ptr.cols; col++) { 70 | if (amplitude_image_ptr.at(line, col) < confidence_value) 71 | preview_ptr.at(line, col) = 255; 72 | } 73 | } 74 | } 75 | 76 | void getPreviewRGB(cv::Mat preview_ptr, cv::Mat amplitude_image_ptr) 77 | { 78 | preview_ptr.setTo(cv::Scalar(0, 0, 0), amplitude_image_ptr < confidence_value); 79 | // cv::GaussianBlur(preview_ptr, preview_ptr, cv::Size(7, 7), 0); 80 | } 81 | 82 | void onMouse(int event, int x, int y, int flags, void* param) 83 | { 84 | if (x < 4 || x > (max_width - 4) || y < 4 || y > (max_height - 4)) 85 | return; 86 | switch (event) { 87 | case cv::EVENT_LBUTTONDOWN: 88 | 89 | break; 90 | 91 | case cv::EVENT_LBUTTONUP: 92 | seletRect.x = x - 4 ? x - 4 : 0; 93 | seletRect.y = y - 4 ? y - 4 : 0; 94 | seletRect.width = 8; 95 | seletRect.height = 8; 96 | break; 97 | default: 98 | followRect.x = x - 4 ? x - 4 : 0; 99 | followRect.y = y - 4 ? y - 4 : 0; 100 | followRect.width = 8; 101 | followRect.height = 8; 102 | break; 103 | } 104 | } 105 | 106 | int main() 107 | { 108 | ArducamTOFCamera tof; 109 | ArducamFrameBuffer* frame; 110 | if (tof.open(Connection::CSI, 0)) { 111 | std::cerr << "Failed to open camera" << std::endl; 112 | return -1; 113 | } 114 | 115 | if (tof.start(FrameType::DEPTH_FRAME)) { 116 | std::cerr << "Failed to start camera" << std::endl; 117 | return -1; 118 | } 119 | // Modify the range also to modify the MAX_DISTANCE 120 | tof.setControl(Control::RANGE, MAX_DISTANCE); 121 | tof.getControl(Control::RANGE, &max_range); 122 | auto info = tof.getCameraInfo(); 123 | std::cout << "open camera with (" << info.width << "x" << info.height << ")" << std::endl; 124 | 125 | uint8_t* preview_ptr = new uint8_t[info.width * info.height * 2]; 126 | cv::namedWindow("preview", cv::WINDOW_AUTOSIZE); 127 | cv::setMouseCallback("preview", onMouse); 128 | 129 | for (;;) { 130 | Arducam::FrameFormat format; 131 | frame = tof.requestFrame(200); 132 | if (frame == nullptr) { 133 | continue; 134 | } 135 | frame->getFormat(FrameType::DEPTH_FRAME, format); 136 | std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl; 137 | max_height = format.height; 138 | max_width = format.width; 139 | 140 | float* depth_ptr = (float*)frame->getData(FrameType::DEPTH_FRAME); 141 | float* confidence_ptr = (float*)frame->getData(FrameType::CONFIDENCE_FRAME); 142 | // getPreview(preview_ptr, depth_ptr, confidence_ptr); 143 | 144 | cv::Mat result_frame(format.height, format.width, CV_8U, preview_ptr); 145 | cv::Mat depth_frame(format.height, format.width, CV_32F, depth_ptr); 146 | cv::Mat confidence_frame(format.height, format.width, CV_32F, confidence_ptr); 147 | 148 | // depth_frame = matRotateClockWise180(depth_frame); 149 | // result_frame = matRotateClockWise180(result_frame); 150 | // confidence_frame = matRotateClockWise180(confidence_frame); 151 | depth_frame.convertTo(result_frame, CV_8U, 255.0 / 7000, 0); 152 | 153 | cv::applyColorMap(result_frame, result_frame, cv::COLORMAP_RAINBOW); 154 | getPreviewRGB(result_frame, confidence_frame); 155 | 156 | confidence_frame.convertTo(confidence_frame, CV_8U, 255.0 / 1024, 0); 157 | 158 | cv::imshow("confidence", confidence_frame); 159 | 160 | cv::rectangle(result_frame, seletRect, cv::Scalar(0, 0, 0), 2); 161 | cv::rectangle(result_frame, followRect, cv::Scalar(255, 255, 255), 1); 162 | 163 | std::cout << "select Rect distance: " << cv::mean(depth_frame(seletRect)).val[0] << std::endl; 164 | 165 | cv::imshow("preview", result_frame); 166 | 167 | auto key = cv::waitKey(1); 168 | if (key == 27 || key == 'q') { 169 | break; 170 | } else if (key == 's') { 171 | save_image(depth_ptr, format.width, format.height); 172 | } 173 | display_fps(); 174 | tof.releaseFrame(frame); 175 | } 176 | 177 | if (tof.stop()) { 178 | return -1; 179 | } 180 | 181 | if (tof.close()) { 182 | return -1; 183 | } 184 | 185 | return 0; 186 | } 187 | -------------------------------------------------------------------------------- /example/python/capture_raw.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import ArducamDepthCamera as ac 4 | 5 | 6 | def main(): 7 | print("Arducam Depth Camera Demo.") 8 | print(" SDK version:", ac.__version__) 9 | 10 | cam = ac.ArducamCamera() 11 | cfg_path = None 12 | # cfg_path = "file.cfg" 13 | 14 | ret = 0 15 | if cfg_path is not None: 16 | ret = cam.openWithFile(cfg_path, 0) 17 | else: 18 | ret = cam.open(ac.Connection.CSI, 0) 19 | if ret != 0: 20 | print("initialization failed. Error code:", ret) 21 | return 22 | 23 | ret = cam.start(ac.FrameType.RAW) 24 | if ret != 0: 25 | print("Failed to start camera. Error code:", ret) 26 | cam.close() 27 | return 28 | 29 | while True: 30 | frame = cam.requestFrame(2000) 31 | if frame is not None and isinstance(frame, ac.RawData): 32 | buf = frame.raw_data 33 | cam.releaseFrame(frame) 34 | 35 | buf = (buf / (1 << 4)).astype(np.uint8) 36 | 37 | cv2.imshow("window", buf) 38 | 39 | key = cv2.waitKey(1) 40 | if key == ord("q"): 41 | break 42 | 43 | cam.stop() 44 | cam.close() 45 | 46 | 47 | if __name__ == "__main__": 48 | main() 49 | -------------------------------------------------------------------------------- /example/python/preview_depth.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import ArducamDepthCamera as ac 4 | 5 | # MAX_DISTANCE value modifiable is 2000 or 4000 6 | MAX_DISTANCE=4000 7 | 8 | 9 | class UserRect: 10 | def __init__(self) -> None: 11 | self.start_x = 0 12 | self.start_y = 0 13 | self.end_x = 0 14 | self.end_y = 0 15 | 16 | @property 17 | def rect(self): 18 | return ( 19 | self.start_x, 20 | self.start_y, 21 | self.end_x - self.start_x, 22 | self.end_y - self.start_y, 23 | ) 24 | 25 | @property 26 | def slice(self): 27 | return (slice(self.start_y, self.end_y), slice(self.start_x, self.end_x)) 28 | 29 | @property 30 | def empty(self): 31 | return self.start_x == self.end_x and self.start_y == self.end_y 32 | 33 | 34 | confidence_value = 30 35 | selectRect, followRect = UserRect(), UserRect() 36 | 37 | 38 | def getPreviewRGB(preview: np.ndarray, confidence: np.ndarray) -> np.ndarray: 39 | preview = np.nan_to_num(preview) 40 | preview[confidence < confidence_value] = (0, 0, 0) 41 | return preview 42 | 43 | 44 | def on_mouse(event, x, y, flags, param): 45 | global selectRect, followRect 46 | 47 | if event == cv2.EVENT_LBUTTONDOWN: 48 | pass 49 | 50 | elif event == cv2.EVENT_LBUTTONUP: 51 | selectRect.start_x = x - 4 52 | selectRect.start_y = y - 4 53 | selectRect.end_x = x + 4 54 | selectRect.end_y = y + 4 55 | else: 56 | followRect.start_x = x - 4 57 | followRect.start_y = y - 4 58 | followRect.end_x = x + 4 59 | followRect.end_y = y + 4 60 | 61 | 62 | def on_confidence_changed(value): 63 | global confidence_value 64 | confidence_value = value 65 | 66 | 67 | def usage(argv0): 68 | print("Usage: python " + argv0 + " [options]") 69 | print("Available options are:") 70 | print(" -d Choose the video to use") 71 | 72 | 73 | def main(): 74 | print("Arducam Depth Camera Demo.") 75 | print(" SDK version:", ac.__version__) 76 | 77 | cam = ac.ArducamCamera() 78 | cfg_path = None 79 | # cfg_path = "file.cfg" 80 | 81 | black_color = (0, 0, 0) 82 | white_color = (255, 255, 255) 83 | 84 | ret = 0 85 | if cfg_path is not None: 86 | ret = cam.openWithFile(cfg_path, 0) 87 | else: 88 | ret = cam.open(ac.Connection.CSI, 0) 89 | if ret != 0: 90 | print("Failed to open camera. Error code:", ret) 91 | return 92 | 93 | ret = cam.start(ac.FrameType.DEPTH) 94 | if ret != 0: 95 | print("Failed to start camera. Error code:", ret) 96 | cam.close() 97 | return 98 | 99 | cam.setControl(ac.Control.RANGE, MAX_DISTANCE) 100 | 101 | r = cam.getControl(ac.Control.RANGE) 102 | 103 | info = cam.getCameraInfo() 104 | print(f"Camera resolution: {info.width}x{info.height}") 105 | 106 | cv2.namedWindow("preview", cv2.WINDOW_AUTOSIZE) 107 | cv2.setMouseCallback("preview", on_mouse) 108 | 109 | if info.device_type == ac.DeviceType.VGA: 110 | # Only VGA support confidence 111 | cv2.createTrackbar( 112 | "confidence", "preview", confidence_value, 255, on_confidence_changed 113 | ) 114 | 115 | while True: 116 | frame = cam.requestFrame(2000) 117 | if frame is not None and isinstance(frame, ac.DepthData): 118 | depth_buf = frame.depth_data 119 | confidence_buf = frame.confidence_data 120 | 121 | result_image = (depth_buf * (255.0 / r)).astype(np.uint8) 122 | result_image = cv2.applyColorMap(result_image, cv2.COLORMAP_RAINBOW) 123 | result_image = getPreviewRGB(result_image, confidence_buf) 124 | 125 | cv2.normalize(confidence_buf, confidence_buf, 1, 0, cv2.NORM_MINMAX) 126 | 127 | cv2.imshow("preview_confidence", confidence_buf) 128 | 129 | cv2.rectangle(result_image, followRect.rect, white_color, 1) 130 | if not selectRect.empty: 131 | cv2.rectangle(result_image, selectRect.rect, black_color, 2) 132 | print("select Rect distance:", np.mean(depth_buf[selectRect.slice])) 133 | 134 | cv2.imshow("preview", result_image) 135 | cam.releaseFrame(frame) 136 | 137 | key = cv2.waitKey(1) 138 | if key == ord("q"): 139 | break 140 | 141 | cam.stop() 142 | cam.close() 143 | 144 | 145 | if __name__ == "__main__": 146 | main() 147 | -------------------------------------------------------------------------------- /example/python/requirements.txt: -------------------------------------------------------------------------------- 1 | ArducamDepthCamera 2 | opencv-python 3 | numpy<2.0.0 4 | -------------------------------------------------------------------------------- /img/rviz_error.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArduCAM/Arducam_tof_camera/1f4cf87c673659c384197ee8388aa561ae3999ea/img/rviz_error.png -------------------------------------------------------------------------------- /open3d_preview/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | 3 | project(PreviewPointcloud) 4 | 5 | add_compile_options(-Wno-psabi) 6 | 7 | find_package(Open3D QUIET) 8 | find_package(OpenCV REQUIRED) 9 | 10 | set(CORE_LIBS ${CXXLIBS} Open3D::Open3D) 11 | 12 | set(WITH_OPENCV_WORLD OFF CACHE BOOL "with opencv_world") 13 | if(WITH_OPENCV_WORLD) 14 | set(CORE_LIBS ${CORE_LIBS} opencv_world) 15 | else() 16 | set(CORE_LIBS ${CORE_LIBS} ${OpenCV_LIBS}) 17 | endif() 18 | 19 | if(Open3D_FOUND) 20 | add_executable(preview_pointcloud preview_pointcloud.cpp) 21 | target_include_directories(preview_pointcloud PRIVATE 22 | ${OpenCV_INCLUDE_DIRS} 23 | ) 24 | target_link_libraries(preview_pointcloud PRIVATE 25 | ${CORE_LIBS} 26 | ) 27 | else() 28 | message(WARNING "Open3D point cloud preview need libopen3d!") 29 | endif() 30 | -------------------------------------------------------------------------------- /open3d_preview/preview_pointcloud.cpp: -------------------------------------------------------------------------------- 1 | #include "ArducamTOFCamera.hpp" 2 | // #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #define LOCAL static inline 15 | // MAX_DISTANCE value modifiable is 2 or 4 16 | #define MAX_DISTANCE 4000 17 | #define WITH_VT_100 1 18 | 19 | #if WITH_VT_100 20 | #define ESC(x) "\033" x 21 | #define NL ESC("[K\n") 22 | #else 23 | #define ESC(x) "" 24 | #define NL "\n" 25 | #endif 26 | 27 | using namespace Arducam; 28 | 29 | cv::Rect seletRect(0, 0, 0, 0); 30 | cv::Rect followRect(0, 0, 0, 0); 31 | int max_width = 640; 32 | int max_height = 480; 33 | int max_range = 0; 34 | bool enable_confidence_ct = false; 35 | int confidence_value = 0; 36 | // std::atomic confidence_value{0}; 37 | 38 | void on_confidence_changed(int pos, void* userdata) 39 | { 40 | confidence_value = pos; 41 | } 42 | 43 | void onMouse(int event, int x, int y, int flags, void* param) 44 | { 45 | if (x < 4 || x > (max_width - 4) || y < 4 || y > (max_height - 4)) 46 | return; 47 | switch (event) { 48 | case cv::EVENT_LBUTTONDOWN: 49 | 50 | break; 51 | 52 | case cv::EVENT_LBUTTONUP: 53 | seletRect.x = x - 4 ? x - 4 : 0; 54 | seletRect.y = y - 4 ? y - 4 : 0; 55 | seletRect.width = 8; 56 | seletRect.height = 8; 57 | break; 58 | default: 59 | followRect.x = x - 4 ? x - 4 : 0; 60 | followRect.y = y - 4 ? y - 4 : 0; 61 | followRect.width = 8; 62 | followRect.height = 8; 63 | break; 64 | } 65 | } 66 | 67 | LOCAL void display_fps(void) 68 | { 69 | using std::chrono::high_resolution_clock; 70 | using namespace std::literals; 71 | static int count = 0; 72 | static auto time_beg = high_resolution_clock::now(); 73 | auto time_end = high_resolution_clock::now(); 74 | ++count; 75 | auto duration_ms = (time_end - time_beg) / 1ms; 76 | if (duration_ms >= 1000) { 77 | std::cout << "fps: " << count << NL; 78 | count = 0; 79 | time_beg = time_end; 80 | } 81 | #if WITH_VT_100 82 | else { 83 | std::cout << "\n"; 84 | } 85 | #endif 86 | } 87 | 88 | LOCAL void save_image(float* image, int width, int height) 89 | { 90 | using namespace std::literals; 91 | // filename = "depth_$width$_$height$_f32_$time.raw" 92 | auto now = std::chrono::system_clock::now().time_since_epoch() / 1ms; 93 | std::string filename = 94 | "depth_" + std::to_string(width) + "_" + std::to_string(height) + "_f32_" + std::to_string(now) + ".raw"; 95 | std::ofstream file(filename, std::ios::binary); 96 | file.write(reinterpret_cast(image), width * height * sizeof(float)); 97 | file.close(); 98 | } 99 | 100 | LOCAL cv::Mat matRotateClockWise180(cv::Mat src) 101 | { 102 | if (src.empty()) { 103 | std::cerr << "RorateMat src is empty!"; 104 | } 105 | 106 | flip(src, src, 0); 107 | flip(src, src, 1); 108 | return src; 109 | } 110 | 111 | LOCAL void getPreview(cv::Mat preview_ptr, cv::Mat amplitude_image_ptr) 112 | { 113 | auto len = preview_ptr.rows * preview_ptr.cols; 114 | for (int line = 0; line < preview_ptr.rows; line++) { 115 | for (int col = 0; col < preview_ptr.cols; col++) { 116 | if (amplitude_image_ptr.at(line, col) < confidence_value) 117 | preview_ptr.at(line, col) = 255; 118 | } 119 | } 120 | } 121 | 122 | LOCAL void getPreviewRGB(cv::Mat preview_ptr, cv::Mat amplitude_image_ptr) 123 | { 124 | preview_ptr.setTo(cv::Scalar(0, 0, 0), amplitude_image_ptr < confidence_value); 125 | // cv::GaussianBlur(preview_ptr, preview_ptr, cv::Size(7, 7), 0); 126 | } 127 | 128 | LOCAL bool getControl(ArducamTOFCamera& tof, Arducam::Control mode, float& val, float alpha = 1.0) 129 | { 130 | int tmp = 0; 131 | Arducam::TofErrorCode ret = tof.getControl(mode, &tmp); 132 | if (ret != 0) { 133 | std::cerr << "Failed to get control" << std::endl; 134 | return false; 135 | } 136 | val = tmp / alpha; 137 | return true; 138 | } 139 | 140 | LOCAL bool checkExit() 141 | { 142 | int key = cv::waitKey(500); 143 | switch (key) { 144 | case 27: 145 | case 'q': 146 | return false; 147 | } 148 | return true; 149 | } 150 | 151 | bool processKey(const open3d::geometry::PointCloud& pcd, float* depth_ptr, const Arducam::FrameFormat& format) 152 | { 153 | auto key = cv::waitKey(1); 154 | switch (key) { 155 | case 27: 156 | case 'q': { 157 | return false; 158 | } break; 159 | case 's': { 160 | open3d::io::WritePointCloudToPCD( // 161 | "pointcloud.pcd", pcd, 162 | { 163 | open3d::io::WritePointCloudOption::IsAscii::Ascii, 164 | open3d::io::WritePointCloudOption::Compressed::Uncompressed, 165 | }); 166 | } break; 167 | case 'r': { 168 | save_image(depth_ptr, format.width, format.height); 169 | } break; 170 | case '=': 171 | case '.': { 172 | if (enable_confidence_ct) { 173 | confidence_value += 1; 174 | if (confidence_value > 255) { 175 | confidence_value = 255; 176 | } 177 | std::cout << "confidence value: " << confidence_value << NL; 178 | } 179 | } break; 180 | case '-': 181 | case ',': { 182 | if (enable_confidence_ct) { 183 | confidence_value -= 1; 184 | if (confidence_value < 0) { 185 | confidence_value = 0; 186 | } 187 | std::cout << "confidence value: " << confidence_value << NL; 188 | } 189 | } break; 190 | case '>': 191 | case ']': { 192 | if (enable_confidence_ct) { 193 | confidence_value += 5; 194 | if (confidence_value > 255) { 195 | confidence_value = 255; 196 | } 197 | std::cout << "confidence value: " << confidence_value << NL; 198 | } 199 | } break; 200 | case '<': 201 | case '[': { 202 | if (enable_confidence_ct) { 203 | confidence_value -= 5; 204 | if (confidence_value < 0) { 205 | confidence_value = 0; 206 | } 207 | std::cout << "confidence value: " << confidence_value << NL; 208 | } 209 | } break; 210 | default: 211 | break; 212 | } 213 | return true; 214 | } 215 | 216 | bool pc_loop(ArducamTOFCamera& tof, std::shared_ptr& pcd, Eigen::Matrix4d transfrom) 217 | { 218 | Arducam::FrameFormat format; 219 | ArducamFrameBuffer* frame = tof.requestFrame(500); 220 | if (frame == nullptr) { 221 | return checkExit(); 222 | } 223 | frame->getFormat(FrameType::DEPTH_FRAME, format); 224 | std::cout << "frame: (" << format.width << "x" << format.height << ")" << NL; 225 | max_height = format.height; 226 | max_width = format.width; 227 | 228 | float* depth_ptr = (float*)frame->getData(FrameType::DEPTH_FRAME); 229 | float* confidence_ptr = (float*)frame->getData(FrameType::CONFIDENCE_FRAME); 230 | 231 | cv::Mat result_frame(format.height, format.width, CV_8U); 232 | cv::Mat depth_frame(format.height, format.width, CV_32F, depth_ptr); 233 | cv::Mat confidence_frame(format.height, format.width, CV_32F, confidence_ptr); 234 | 235 | // depth_frame = matRotateClockWise180(depth_frame); 236 | // result_frame = matRotateClockWise180(result_frame); 237 | // confidence_frame = matRotateClockWise180(confidence_frame); 238 | depth_frame.convertTo(result_frame, CV_8U, 255.0 / 7000, 0); 239 | 240 | cv::applyColorMap(result_frame, result_frame, cv::COLORMAP_RAINBOW); 241 | getPreviewRGB(result_frame, confidence_frame); 242 | 243 | // confidence_frame.convertTo(confidence_frame, CV_8U, 255.0 / 1024, 0); 244 | 245 | // cv::imshow("confidence", confidence_frame); 246 | 247 | cv::rectangle(result_frame, seletRect, cv::Scalar(0, 0, 0), 2); 248 | cv::rectangle(result_frame, followRect, cv::Scalar(255, 255, 255), 1); 249 | 250 | std::cout << "select Rect distance: " << cv::mean(depth_frame(seletRect)).val[0] << NL; 251 | 252 | cv::imshow("preview", result_frame); 253 | depth_frame.setTo(0, confidence_frame < confidence_value); 254 | auto depth_image = open3d::geometry::Image(); 255 | depth_image.Prepare(max_width, max_height, 1, 4); 256 | 257 | memcpy(depth_image.data_.data(), depth_frame.data, depth_image.data_.size()); 258 | 259 | float fx, fy, cx, cy; 260 | 261 | getControl(tof, Control::INTRINSIC_FX, fx, 100); 262 | getControl(tof, Control::INTRINSIC_FY, fy, 100); 263 | getControl(tof, Control::INTRINSIC_CX, cx, 100); 264 | getControl(tof, Control::INTRINSIC_CY, cy, 100); 265 | 266 | std::cout << "fx: " << fx << " fy: " << fy << " cx: " << cx << " cy: " << cy << NL; 267 | 268 | auto curr = open3d::geometry::PointCloud::CreateFromDepthImage( // 269 | depth_image, {max_width, max_height, fx, fy, cx, cy}, Eigen::Matrix4d::Identity(), 1000.0, 5000.0); 270 | 271 | curr->Transform(transfrom); 272 | 273 | // auto voxel_down_pcd = pcd->VoxelDownSample(20); 274 | // auto tuple = voxel_down_pcd->RemoveStatisticalOutliers(20, 2.0); 275 | // auto inlier_cloud = voxel_down_pcd->SelectByIndex(std::get>(tuple)); 276 | // pcd->points_ = inlier_cloud->points_; 277 | 278 | display_fps(); 279 | if (!processKey(*curr, depth_ptr, format)) { 280 | return false; 281 | } 282 | pcd->points_ = curr->points_; 283 | tof.releaseFrame(frame); 284 | return true; 285 | } 286 | 287 | int main(int argc, char* argv[]) 288 | { 289 | ArducamTOFCamera tof; 290 | const char* cfg_path = nullptr; 291 | 292 | if (argc > 2) { 293 | confidence_value = atoi(argv[2]); 294 | } 295 | if (argc > 1) { 296 | if (strcmp(argv[1], "-") != 0) { 297 | cfg_path = argv[1]; 298 | } 299 | } 300 | if (cfg_path != nullptr) { 301 | if (tof.openWithFile(cfg_path)) { 302 | std::cerr << "Failed to open camera with cfg: " << cfg_path << std::endl; 303 | return -1; 304 | } 305 | } else { 306 | if (tof.open(Connection::CSI)) { 307 | std::cerr << "Failed to open camera" << std::endl; 308 | return -1; 309 | } 310 | } 311 | 312 | if (tof.start(FrameType::DEPTH_FRAME)) { 313 | std::cerr << "Failed to start camera" << std::endl; 314 | return -1; 315 | } 316 | // Modify the range also to modify the MAX_DISTANCE 317 | tof.setControl(Control::RANGE, MAX_DISTANCE); 318 | tof.getControl(Control::RANGE, &max_range); 319 | auto info = tof.getCameraInfo(); 320 | std::cout << "open camera with (" << info.width << "x" << info.height << ")" << std::endl; 321 | 322 | cv::namedWindow("preview", cv::WINDOW_NORMAL); 323 | cv::setMouseCallback("preview", onMouse); 324 | if (info.device_type == Arducam::DeviceType::DEVICE_VGA) { 325 | // only vga support confidence 326 | enable_confidence_ct = true; 327 | cv::createTrackbar("confidence", "preview", NULL, 255, on_confidence_changed); 328 | cv::setTrackbarPos("confidence", "preview", confidence_value); 329 | } 330 | 331 | Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); 332 | // clang-format off 333 | m << 1, 0, 0, 0, 334 | 0, -1, 0, 0, 335 | 0, 0, -1, 0, 336 | 0, 0, 0, 1; 337 | // clang-format on 338 | 339 | auto pcd = std::make_shared(); 340 | open3d::visualization::Visualizer vis; 341 | vis.CreateVisualizerWindow("Point Cloud", max_width, max_height); 342 | bool first = true; 343 | 344 | std::cout << ESC("[?25l"); // hide cursor 345 | std::cout << ESC("7"); // save cursor position 346 | for (; pc_loop(tof, pcd, m);) { 347 | if (first) { 348 | vis.AddGeometry(pcd); 349 | first = false; 350 | } 351 | vis.UpdateGeometry(pcd); 352 | vis.PollEvents(); 353 | vis.UpdateRender(); 354 | std::cout << ESC("8"); // restore cursor position 355 | } 356 | std::cout << ESC("[?25h"); // show cursor 357 | std::cout << ESC("[6B") << NL; // move cursor down 6 lines 358 | 359 | vis.DestroyVisualizerWindow(); 360 | 361 | if (tof.stop()) { 362 | return -1; 363 | } 364 | 365 | if (tof.close()) { 366 | return -1; 367 | } 368 | 369 | return 0; 370 | } 371 | -------------------------------------------------------------------------------- /ros2_publisher/README.md: -------------------------------------------------------------------------------- 1 | # ros2_publisher 2 | 3 | ## Dependencies 4 | 5 | **Raspberry Pi:** 6 | 7 | > Please refer to the instructions above for _*a. Driver Install*_, _*b. Configuration*_, and _*c. SDK Install*_ which run on Raspberry Pi. 8 | 9 | **Jetson:** 10 | 11 | > Please refer to the instructions above for _*project dependencies*_ and _*SDK Install*_ which run on Jetson. 12 | 13 | ### ROS2 Installation 14 | 15 | #### On Raspberry Pi 16 | 17 | Install ROS2 on Raspberry Pi. See [ROS2 Installation](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) for details. 18 | Alternatively, you can use a third-party installation script to install ROS2 on Raspberry Pi. See [this repository](https://github.com/Ar-Ray-code/rpi-bullseye-ros2) for details. 19 | 20 | #### On Jetson 21 | 22 | The example is based on ROS2. Please refer to [ROS2 Installation](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) and install it yourself. 23 | 24 | ```bash 25 | # Install the compilation tool colcon 26 | sudo apt install python3-colcon-common-extensions 27 | ``` 28 | 29 | ### ROS2 post-installation 30 | 31 | Then export the environment variables to the bashrc file. 32 | 33 | ----- 34 | > for humble version 35 | 36 | ```Shell 37 | echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc 38 | echo "export ROS_DOMAIN_ID=7" >> ~/.bashrc 39 | source ~/.bashrc 40 | ``` 41 | 42 | ----- 43 | > for jazzy version 44 | 45 | ```Shell 46 | echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc 47 | echo "export ROS_DOMAIN_ID=7" >> ~/.bashrc 48 | source ~/.bashrc 49 | ``` 50 | 51 | ## Compilation 52 | 53 | ```Shell 54 | cd Arducam_tof_camera/ros2_publisher 55 | colcon build --merge-install 56 | ``` 57 | 58 | ### Run C++ examples 59 | 60 | ```Shell 61 | . install/setup.bash 62 | ros2 run arducam_rclcpp_tof_pointcloud tof_pointcloud 63 | ``` 64 | 65 | ### Run Python examples 66 | 67 | ```shell 68 | . install/setup.bash 69 | ros2 run arducam_rclpy_tof_pointcloud tof_pointcloud 70 | ``` 71 | 72 | >You can preview by running rviz2 on the host in the LAN 73 | 74 | ### RViz2 75 | 76 | > If you encounter an error like this: 77 | > ![rviz::RenderSystem: error](../img/rviz_error.png) 78 | > Make sure to set the Display Server to X11. 79 | 80 | ```Shell 81 | rviz2 82 | ``` 83 | -------------------------------------------------------------------------------- /ros2_publisher/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | colcon build --merge-install 3 | -------------------------------------------------------------------------------- /ros2_publisher/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source install/setup.bash 3 | ros2 run arducam_rclpy_tof_pointcloud tof_pointcloud 4 | -------------------------------------------------------------------------------- /ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/arducam_rclpy_tof_pointcloud/__init__.py: -------------------------------------------------------------------------------- 1 | from .tof_pointcloud import main 2 | 3 | if __name__ == "__main__": 4 | main() 5 | -------------------------------------------------------------------------------- /ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/arducam_rclpy_tof_pointcloud/tof_pointcloud.py: -------------------------------------------------------------------------------- 1 | from argparse import ArgumentParser 2 | from typing import Optional 3 | import rclpy 4 | from rclpy.node import Node 5 | from sensor_msgs.msg import PointCloud2 6 | from sensor_msgs_py import point_cloud2 7 | from std_msgs.msg import Float32MultiArray, Header 8 | import numpy as np 9 | from threading import Thread 10 | 11 | 12 | from ArducamDepthCamera import ( 13 | ArducamCamera, 14 | Connection, 15 | DeviceType, 16 | FrameType, 17 | Control, 18 | DepthData, 19 | ) 20 | 21 | class Option: 22 | cfg: Optional[str] 23 | 24 | 25 | 26 | class TOFPublisher(Node): 27 | def __init__(self, options: Option): 28 | super().__init__("arducam") 29 | 30 | tof = self.__init_camera(options) 31 | if tof is None: 32 | raise Exception("Failed to initialize camera") 33 | 34 | self.tof_ = tof 35 | self.pointsize_ = self.width_ * self.height_ 36 | self.frame_id = "sensor_frame" 37 | self.depth_msg_ = Float32MultiArray() 38 | self.publisher_ = self.create_publisher(PointCloud2, "point_cloud", 10) 39 | self.publisher_depth_ = self.create_publisher( 40 | Float32MultiArray, "depth_frame", 10 41 | ) 42 | self.fx = tof.getControl(Control.INTRINSIC_FX) / 100 43 | self.fy = tof.getControl(Control.INTRINSIC_FY) / 100 44 | self.header = Header() 45 | self.header.frame_id = "map" 46 | self.points = None 47 | self.running_ = True 48 | self.timer_ = self.create_timer(1 / 30, self.update) 49 | self.process_point_cloud_thr = Thread( 50 | target=self.__generateSensorPointCloud, daemon=True 51 | ) 52 | self.process_point_cloud_thr.start() 53 | 54 | def __init_camera(self, options: Option): 55 | print("pointcloud publisher init") 56 | tof = ArducamCamera() 57 | ret = 0 58 | 59 | if options.cfg is not None: 60 | ret = tof.openWithFile(options.cfg, 0) 61 | else: 62 | ret = tof.open(Connection.CSI, 0) 63 | if ret != 0: 64 | print("Failed to open camera. Error code:", ret) 65 | return 66 | 67 | ret = tof.start(FrameType.DEPTH) 68 | if ret != 0: 69 | print("Failed to start camera. Error code:", ret) 70 | tof.close() 71 | return 72 | 73 | info = tof.getCameraInfo() 74 | if info.device_type == DeviceType.HQVGA: 75 | self.width_ = info.width 76 | self.height_ = info.height 77 | tof.setControl(Control.RANGE, 4) 78 | elif info.device_type == DeviceType.VGA: 79 | self.width_ = info.width 80 | self.height_ = info.height // 10 - 1 81 | print(f"Open camera success, width: {self.width_}, height: {self.height_}") 82 | 83 | print("Pointcloud publisher start") 84 | return tof 85 | 86 | def __generateSensorPointCloud(self): 87 | while self.running_: 88 | frame = self.tof_.requestFrame(200) 89 | if frame is not None and isinstance(frame, DepthData): 90 | self.fx = self.tof_.getControl(Control.INTRINSIC_FX) / 100 91 | self.fy = self.tof_.getControl(Control.INTRINSIC_FY) / 100 92 | depth_buf = frame.depth_data 93 | confidence_buf = frame.confidence_data 94 | 95 | depth_buf[confidence_buf < 30] = 0 96 | 97 | self.depth_msg_.data = depth_buf.flatten() / 1000 98 | 99 | # Convert depth values ​​from millimeters to meters 100 | z = depth_buf / 1000.0 101 | z[z <= 0] = np.nan # Handling invalid depth values 102 | 103 | # Calculate x and y coordinates 104 | u = np.arange(self.width_) 105 | v = np.arange(self.height_) 106 | u, v = np.meshgrid(u, v) 107 | 108 | # Calculate point cloud coordinates 109 | x = (u - self.width_ / 2) * z / self.fx 110 | y = (v - self.height_ / 2) * z / self.fy 111 | 112 | # Combined point cloud 113 | points = np.stack((x, y, z), axis=-1) 114 | self.points = points[ 115 | ~np.isnan(points).any(axis=-1) 116 | ] # Filter invalid points 117 | 118 | self.tof_.releaseFrame(frame) 119 | 120 | def update(self): 121 | # self.__generateSensorPointCloud() 122 | if self.points is None: 123 | return 124 | self.header.stamp = self.get_clock().now().to_msg() 125 | 126 | pc2_msg_ = point_cloud2.create_cloud_xyz32(self.header, self.points) 127 | 128 | self.publisher_.publish(pc2_msg_) 129 | self.publisher_depth_.publish(self.depth_msg_) 130 | 131 | def stop(self): 132 | self.running_ = False 133 | self.process_point_cloud_thr.join() 134 | self.tof_.stop() 135 | self.tof_.close() 136 | 137 | 138 | def main(args=None): 139 | rclpy.init(args=args) 140 | parser = ArgumentParser() 141 | parser.add_argument("--cfg", type=str, help="Path to camera configuration file") 142 | 143 | ns = parser.parse_args() 144 | 145 | options = Option() 146 | options.cfg = ns.cfg 147 | 148 | tof_publisher = TOFPublisher(options) 149 | 150 | rclpy.spin(tof_publisher) 151 | rclpy.shutdown() 152 | tof_publisher.stop() 153 | 154 | 155 | if __name__ == "__main__": 156 | main() 157 | -------------------------------------------------------------------------------- /ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | arducam_rclpy_tof_pointcloud 5 | 0.0.1 6 | Arducam Tof Camera Examples. 7 | pi 8 | Apache License, Version 2.0 9 | 10 | ament_python 11 | 12 | rclpy 13 | std_msgs 14 | sensor_msgs 15 | 16 | 17 | ament_python 18 | 19 | 20 | -------------------------------------------------------------------------------- /ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/resource/arducam_rclpy_tof_pointcloud: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ArduCAM/Arducam_tof_camera/1f4cf87c673659c384197ee8388aa561ae3999ea/ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/resource/arducam_rclpy_tof_pointcloud -------------------------------------------------------------------------------- /ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/arducam_rclpy_tof_pointcloud 3 | [install] 4 | install_scripts=$base/lib/arducam_rclpy_tof_pointcloud 5 | -------------------------------------------------------------------------------- /ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'arducam_rclpy_tof_pointcloud' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.1', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | author='arducam', 17 | author_email='dennis@arducam.com', 18 | maintainer='arducam', 19 | maintainer_email='dennis@arducam.com', 20 | keywords=['ROS'], 21 | classifiers=[ 22 | 'Intended Audience :: Developers', 23 | 'License :: OSI Approved :: Apache Software License', 24 | 'Programming Language :: Python', 25 | 'Topic :: Software Development', 26 | ], 27 | description='Arducam Tof Camera Examples.', 28 | license='Apache License, Version 2.0', 29 | tests_require=['pytest'], 30 | entry_points={ 31 | 'console_scripts': [ 32 | 'tof_pointcloud = ' + package_name + '.tof_pointcloud:main', 33 | ], 34 | }, 35 | ) 36 | -------------------------------------------------------------------------------- /setup_rock_5a.sh: -------------------------------------------------------------------------------- 1 | #/bin/bash 2 | # if "Radxa ROCK" in `cat /sys/firmware/devicetree/base/model` 3 | if [ -f /sys/firmware/devicetree/base/model ]; then 4 | if grep -q "Radxa ROCK" /sys/firmware/devicetree/base/model; then 5 | echo 0 0 0 0 | sudo tee /sys/devices/platform/rkcif-mipi-lvds2/compact_test 6 | fi 7 | fi 8 | --------------------------------------------------------------------------------