├── .gitignore ├── .dockerignore ├── scripts ├── modify_entrypoint.sh └── build.sh ├── thirdparty └── image_proc │ ├── test │ ├── test_rectify.xml │ ├── CMakeLists.txt │ ├── test_bayer.xml │ ├── rostest.cpp │ └── test_rectify.cpp │ ├── mainpage.dox │ ├── cfg │ ├── Rectify.cfg │ ├── Debayer.cfg │ ├── Resize.cfg │ └── CropDecimate.cfg │ ├── launch │ └── image_proc.launch │ ├── nodelet_plugins.xml │ ├── package.xml │ ├── src │ ├── nodelets │ │ ├── edge_aware.h │ │ ├── crop_non_zero.cpp │ │ ├── rectify.cpp │ │ ├── resize.cpp │ │ ├── debayer.cpp │ │ ├── crop_decimate.cpp │ │ └── edge_aware.cpp │ ├── libimage_proc │ │ ├── advertisement_checker.cpp │ │ └── processor.cpp │ └── nodes │ │ └── image_proc.cpp │ ├── include │ └── image_proc │ │ ├── advertisement_checker.h │ │ └── processor.h │ ├── CMakeLists.txt │ └── CHANGELOG.rst ├── .gitmodules ├── launch └── play_bag_viz.launch ├── LICENSE.md ├── run_all_rosario_sequences.sh ├── Dockerfile ├── calc_t_imu_camrect.py ├── run_rosario_sequence.sh ├── run.sh ├── README.md └── rviz └── rviz_config.rviz /.gitignore: -------------------------------------------------------------------------------- 1 | outputs/ 2 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | .git 2 | outputs/ 3 | basalt/doc/ 4 | **/*.png 5 | **/*.jpg 6 | **/*.jpeg 7 | **/*.pdf 8 | -------------------------------------------------------------------------------- /scripts/modify_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sed -i '/exec "$@"/i source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh 3 | -------------------------------------------------------------------------------- /scripts/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /opt/ros/melodic/setup.bash 3 | catkin config -DCMAKE_BUILD_TYPE=Release -DSAVE_TIMES=ON 4 | catkin build 5 | -------------------------------------------------------------------------------- /thirdparty/image_proc/test/test_rectify.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /thirdparty/image_proc/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | @mainpage 3 | @htmlinclude manifest.html 4 | 5 | @b image_proc provides a node for performing single image rectification and 6 | color processing on the raw images produced by a camera. The outputs of 7 | image_proc are suitable for visual processing by other nodes. See 8 | http://www.ros.org/wiki/image_proc for documentation. 9 | 10 | Currently this package has no public code API. 11 | 12 | */ 13 | -------------------------------------------------------------------------------- /thirdparty/image_proc/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(rostest REQUIRED) 2 | find_package(catkin REQUIRED camera_calibration_parsers image_transport cv_bridge) 3 | #catkin_add_gtest(image_proc_rostest rostest.cpp) 4 | #target_link_libraries(image_proc_rostest ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 5 | include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 6 | add_rostest_gtest(image_proc_test_rectify test_rectify.xml test_rectify.cpp) 7 | target_link_libraries(image_proc_test_rectify ${catkin_LIBRARIES}) 8 | -------------------------------------------------------------------------------- /thirdparty/image_proc/test/test_bayer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "basalt_vio_ros2_msgs"] 2 | path = basalt_vio_ros2_msgs 3 | url = https://github.com/berndpfrommer/basalt_vio_ros2_msgs.git 4 | [submodule "basalt_vio_ros1_msgs"] 5 | path = basalt_vio_ros1_msgs 6 | url = https://github.com/berndpfrommer/basalt_vio_ros1_msgs.git 7 | [submodule "basalt_ros1"] 8 | path = basalt_ros1 9 | url = git@github.com:CIFASIS/basalt_ros1.git 10 | [submodule "basalt_ros2"] 11 | path = basalt_ros2 12 | url = https://github.com/berndpfrommer/basalt_ros2.git 13 | [submodule "basalt-mirror"] 14 | path = basalt 15 | url = git@github.com:CIFASIS/basalt-mirror.git 16 | -------------------------------------------------------------------------------- /launch/play_bag_viz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /thirdparty/image_proc/cfg/Rectify.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='image_proc' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | interpolate_enum = gen.enum([ gen.const("NN", int_t, 0, "Nearest neighbor"), 10 | gen.const("Linear", int_t, 1, "Linear"), 11 | gen.const("Cubic", int_t, 2, "Cubic"), 12 | gen.const("Lanczos4", int_t, 4, "Lanczos4")], 13 | "interpolation type") 14 | 15 | gen.add("interpolation", int_t, 0, 16 | "Interpolation algorithm between source image pixels", 17 | 1, 0, 4, edit_method = interpolate_enum) 18 | 19 | # First string value is node name, used only for generating documentation 20 | # Second string value ("Rectify") is name of class and generated 21 | # .h file, with "Config" added, so class RectifyConfig 22 | exit(gen.generate(PACKAGE, "image_proc", "Rectify")) 23 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Bernd Pfrommer 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /thirdparty/image_proc/launch/image_proc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 20 | 21 | 22 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /thirdparty/image_proc/cfg/Debayer.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='image_proc' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | debayer_enum = gen.enum([ gen.const("Bilinear", int_t, 0, 10 | "Fast algorithm using bilinear interpolation"), 11 | gen.const("EdgeAware", int_t, 1, 12 | "Edge-aware algorithm"), 13 | gen.const("EdgeAwareWeighted", int_t, 2, 14 | "Weighted edge-aware algorithm"), 15 | gen.const("VNG", int_t, 3, 16 | "Slow but high quality Variable Number of Gradients algorithm")], 17 | "Debayering algorithm") 18 | 19 | gen.add("debayer", int_t, 0, 20 | "Debayering algorithm", 21 | 0, 0, 3, edit_method = debayer_enum) 22 | 23 | # First string value is node name, used only for generating documentation 24 | # Second string value ("Debayer") is name of class and generated 25 | # .h file, with "Config" added, so class DebayerConfig 26 | exit(gen.generate(PACKAGE, "image_proc", "Debayer")) 27 | -------------------------------------------------------------------------------- /thirdparty/image_proc/cfg/Resize.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import * 4 | 5 | 6 | PACKAGE = 'image_proc' 7 | ID = 'Resize' 8 | 9 | gen = ParameterGenerator() 10 | 11 | interpolate_enum = gen.enum([gen.const('NN', int_t, 0, 'Nearest neighbor'), 12 | gen.const('Linear', int_t, 1, 'Linear'), 13 | gen.const('Cubic', int_t, 2, 'Cubic'), 14 | gen.const('Lanczos4', int_t, 4, 'Lanczos4')], 15 | 'interpolation type') 16 | gen.add('interpolation', int_t, level=0, 17 | description='Interpolation algorithm between source image pixels', 18 | default=1, min=0, max=4, edit_method=interpolate_enum) 19 | 20 | gen.add('use_scale', bool_t, level=0, 21 | description='Flag to use scale instead of static size.', 22 | default=True) 23 | gen.add('scale_height', double_t, level=0, 24 | description='Scale of height.', 25 | default=1, min=0, max=10) 26 | gen.add('scale_width', double_t, level=0, 27 | description='Scale of width', 28 | default=1, min=0, max=10) 29 | 30 | gen.add('height', int_t, level=0, 31 | description='Destination height. Ignored if negative.', 32 | default=-1, min=-1) 33 | gen.add('width', int_t, level=0, 34 | description='Destination width. Ignored if negative.', 35 | default=-1, min=-1) 36 | 37 | exit(gen.generate(PACKAGE, PACKAGE, ID)) 38 | -------------------------------------------------------------------------------- /thirdparty/image_proc/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | Nodelet to debayer (if needed) a raw camera image stream. 8 | 9 | 10 | 11 | 14 | 15 | Nodelet to rectify an unrectified camera image stream. 16 | 17 | 18 | 19 | 22 | 23 | Nodelet to resize image and camera_info. 24 | 25 | 26 | 27 | 30 | 31 | Nodelet to apply decimation (software binning) and ROI to a raw camera 32 | image post-capture. 33 | 34 | 35 | 36 | 37 | 40 | 43 | 44 | Nodelet to crop the largest valid (Non Zero) area of the image. 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /thirdparty/image_proc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | image_proc 3 | 1.16.0 4 | Single image rectification and color processing. 5 | Patrick Mihelich 6 | Kurt Konolige 7 | Jeremy Leibs 8 | Vincent Rabaud 9 | Autonomoustuff team 10 | BSD 11 | http://www.ros.org/wiki/image_proc 12 | 13 | 14 | 15 | 16 | 17 | catkin 18 | 19 | rostest 20 | camera_calibration_parsers 21 | 22 | boost 23 | cv_bridge 24 | dynamic_reconfigure 25 | image_geometry 26 | image_transport 27 | nodelet 28 | nodelet_topic_tools 29 | roscpp 30 | sensor_msgs 31 | 32 | cv_bridge 33 | dynamic_reconfigure 34 | image_geometry 35 | image_transport 36 | nodelet 37 | nodelet_topic_tools 38 | roscpp 39 | sensor_msgs 40 | 41 | -------------------------------------------------------------------------------- /run_all_rosario_sequences.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Run Rosario dataset (from sequence 01 to 06) 4 | # Parameter: 5 | # -Path of folder containing rosbags (files must be named sequence0*.bag) 6 | 7 | # Get full directory name of the script no matter where it is being called from 8 | CURRENT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 9 | 10 | if [ -z "${SEQUENCES}" ] ; then 11 | SEQUENCES=($(seq 1 6)) 12 | fi 13 | 14 | function echoUsage() 15 | { 16 | echo -e "Usage: ./run_all_rosario_sequences.sh [-p] ROSBAGS_PATH\n\ 17 | \t -p \t Plot and show results. Even if the flag is not set, a\n\ 18 | \t\t script to automatically plot the results will be\n\ 19 | \t\t generated with outputs. Evo should be installed. \n\ 20 | \t -h \t Help." >&2 21 | } 22 | 23 | PLOT=0 24 | while getopts "hp" opt; do 25 | case "$opt" in 26 | h) echoUsage 27 | exit 0 28 | ;; 29 | p) PLOT=1 30 | ;; 31 | *) 32 | echoUsage 33 | exit 1 34 | ;; 35 | esac 36 | done 37 | 38 | shift $((OPTIND - 1)) 39 | DATASET_DIR=$1 40 | 41 | dt=$(date '+%Y%m%d_%H%M%S') 42 | OUTPUT_DIR=$CURRENT_DIR/outputs/rosario_${dt} 43 | mkdir -p $OUTPUT_DIR 44 | echo "#!/bin/bash" > $OUTPUT_DIR/plot_results.sh 45 | 46 | trap "exit 1" INT 47 | 48 | for i in ${SEQUENCES[@]} ; do 49 | $CURRENT_DIR/run_rosario_sequence.sh -r -b -o $OUTPUT_DIR/trajectory_rosario_0$i.txt $DATASET_DIR/sequence0$i.bag 50 | echo "evo_traj tum $OUTPUT_DIR/trajectory_rosario_0$i.txt --ref $DATASET_DIR/sequence0${i}_gt.txt --align --plot --t_max_diff 0.02" >> $OUTPUT_DIR/plot_results.sh 51 | done 52 | 53 | chmod +x $OUTPUT_DIR/plot_results.sh 54 | 55 | if [ $PLOT -eq 1 ] ; then 56 | $OUTPUT_DIR/plot_results.sh 57 | fi 58 | -------------------------------------------------------------------------------- /thirdparty/image_proc/cfg/CropDecimate.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='image_proc' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | # Decimation parameters 10 | gen.add("decimation_x", int_t, 0, "Number of pixels to decimate to one horizontally", 1, 1, 16) 11 | gen.add("decimation_y", int_t, 0, "Number of pixels to decimate to one vertically", 1, 1, 16) 12 | 13 | # ROI parameters 14 | # Maximums are arbitrary set to the resolution of a 5Mp Prosilica, since we can't set 15 | # the dynamically. 16 | gen.add("x_offset", int_t, 0, "X offset of the region of interest", 0, 0, 2447) 17 | gen.add("y_offset", int_t, 0, "Y offset of the region of interest", 0, 0, 2049) 18 | gen.add("width", int_t, 0, "Width of the region of interest", 0, 0, 2448) 19 | gen.add("height", int_t, 0, "Height of the region of interest", 0, 0, 2050) 20 | 21 | interpolate_enum = gen.enum([ gen.const("NN", int_t, 0, "Nearest-neighbor sampling"), 22 | gen.const("Linear", int_t, 1, "Bilinear interpolation"), 23 | gen.const("Cubic", int_t, 2, "Bicubic interpolation over 4x4 neighborhood"), 24 | gen.const("Area", int_t, 3, "Resampling using pixel area relation"), 25 | gen.const("Lanczos4", int_t, 4, "Lanczos interpolation over 8x8 neighborhood")], 26 | "interpolation type") 27 | 28 | gen.add("interpolation", int_t, 0, 29 | "Sampling algorithm", 30 | 0, 0, 4, edit_method = interpolate_enum) 31 | 32 | # First string value is node name, used only for generating documentation 33 | # Second string value ("CropDecimate") is name of class and generated 34 | # .h file, with "Config" added, so class CropDecimateConfig 35 | exit(gen.generate(PACKAGE, "image_proc", "CropDecimate")) 36 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic-perception 2 | 3 | RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 4 | 5 | RUN apt-get update && apt-get install -y wget gnupg software-properties-common 6 | 7 | RUN wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key| apt-key add - 8 | RUN echo "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-10 main" > /etc/apt/sources.list.d/llvm10.list 9 | 10 | RUN apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE 11 | RUN add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" 12 | #RUN echo "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" > /etc/apt/sources.list.d/realsense.list 13 | 14 | RUN apt-get update && \ 15 | DEBIAN_FRONTEND=noninteractive apt-get install --no-install-recommends -y \ 16 | cmake \ 17 | libtbb-dev \ 18 | libeigen3-dev \ 19 | libglew-dev \ 20 | ccache \ 21 | libjpeg-dev \ 22 | libpng-dev \ 23 | openssh-client \ 24 | liblz4-dev \ 25 | libbz2-dev \ 26 | libboost-regex-dev \ 27 | libboost-filesystem-dev \ 28 | libboost-date-time-dev \ 29 | libboost-program-options-dev \ 30 | libopencv-dev \ 31 | libpython2.7-dev \ 32 | libgtest-dev \ 33 | lsb-core \ 34 | gcovr \ 35 | ggcov \ 36 | lcov \ 37 | librealsense2-dev \ 38 | librealsense2-gl-dev \ 39 | librealsense2-dkms \ 40 | librealsense2-utils \ 41 | doxygen \ 42 | graphviz \ 43 | libsuitesparse-dev \ 44 | clang-10 \ 45 | clang-format-10 \ 46 | build-essential \ 47 | python-catkin-tools && \ 48 | rm -rf /var/lib/apt/lists/* 49 | 50 | # if you want to reference a previously defined env variable in another definition, use multiple ENV 51 | ENV CATKIN_WS=/root/catkin_ws BASALT_ROOT=/root/catkin_ws/src/basalt_ros BASALT_ROS1_ROOT=/root/catkin_ws/src/basalt_ros/basalt_ros1 52 | 53 | COPY ./ $BASALT_ROOT 54 | WORKDIR $CATKIN_WS 55 | COPY ./scripts/ $CATKIN_WS 56 | 57 | RUN ["/bin/bash", "-c", "chmod +x build.sh && chmod +x modify_entrypoint.sh && sync && ./build.sh && ./modify_entrypoint.sh"] 58 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodelets/edge_aware.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #ifndef IMAGE_PROC_EDGE_AWARE 35 | #define IMAGE_PROC_EDGE_AWARE 36 | 37 | #include 38 | 39 | // Edge-aware debayering algorithms, intended for eventual inclusion in OpenCV. 40 | 41 | namespace image_proc { 42 | 43 | void debayerEdgeAware(const cv::Mat& bayer, cv::Mat& color); 44 | 45 | void debayerEdgeAwareWeighted(const cv::Mat& bayer, cv::Mat& color); 46 | 47 | } // namespace image_proc 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /calc_t_imu_camrect.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import evo.core.transformations as tf 3 | 4 | # Calibration (Kalibr) 5 | T_imu_cam0 = np.array([[0.009471441780975032, -0.9939081748565041, 0.10980360533243941, -0.030950056130583600], 6 | [0.9984567420628084, 0.0033909183804303744, -0.055431362078334365, -0.077136433704428381], 7 | [0.05472134884952806, 0.11015916496574298, 0.9924064350628417, 0.026512487799308242], 8 | [0.0, 0.0, 0.0, 1.0]]) 9 | 10 | T_imu_cam1 = np.array([[0.010486169744957197, -0.9940873423040585, 0.10807588128224391, -0.029702271983652186], 11 | [0.9972728488827708, 0.002500626397312078, -0.07376050263429253, 0.041641180069859347], 12 | [0.07305412462908822, 0.10855460717295605, 0.9914025378907414, 0.033393889310081332], 13 | [0.0, 0.0, 0.0, 1.0]]) 14 | 15 | # Rectification matrix (from camera_info) 16 | R_cam0rect_cam0 = np.array([0.999873448071739, 0.0007366880139693224, 0.015891668631757446, -0.0007117643758229365, 0.9999985080430129, -0.0015739451138449111, -0.01589280442857141, 0.0015624348044513143, 0.9998724806518465]).reshape(3,3) 17 | R_cam0_cam0rect = R_cam0rect_cam0.transpose() 18 | 19 | R_cam1rect_cam1 = np.array([0.9999973307670281, -0.00043956732938318916, 0.0022683120115662946, 0.0004360095298107782, 0.9999986744820188, 0.0015687351258854836, -0.0022689985695877315, -0.0015677419329123293, 0.9999961969081298]).reshape(3,3) 20 | R_cam1_cam1rect = R_cam1rect_cam1.transpose() 21 | 22 | 23 | def get_T_imu_camrect(T_imu_cam, R_cam_camrect): 24 | T_cam_camrect = np.eye(4) 25 | T_cam_camrect[:3,:3] = R_cam_camrect 26 | return T_imu_cam.dot(T_cam_camrect) 27 | 28 | 29 | def quaternion_from_transformation(T): 30 | return tf.quaternion_from_matrix(T) 31 | 32 | 33 | def traslation_from_transformation(T): 34 | return T[:3,3] 35 | 36 | def print_p_q_from_transformation(T): 37 | print("**** From Transformation:") 38 | print(T) 39 | 40 | print("\nTraslation vector:") 41 | p = traslation_from_transformation(T) 42 | print(p) 43 | print("\nQuaternion: (format: qw, qx, qy, qz)") 44 | q = quaternion_from_transformation(T) 45 | print(q) 46 | print("****") 47 | 48 | T_imu_cam0rect = get_T_imu_camrect(T_imu_cam0, R_cam0_cam0rect) 49 | T_imu_cam1rect = get_T_imu_camrect(T_imu_cam1, R_cam1_cam1rect) 50 | 51 | print_p_q_from_transformation(T_imu_cam0rect) 52 | print_p_q_from_transformation(T_imu_cam1rect) 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /thirdparty/image_proc/include/image_proc/advertisement_checker.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #ifndef IMAGE_PROC_ADVERTISEMENT_CHECKER_H 35 | #define IMAGE_PROC_ADVERTISEMENT_CHECKER_H 36 | 37 | #include 38 | 39 | namespace image_proc { 40 | 41 | class AdvertisementChecker 42 | { 43 | ros::NodeHandle nh_; 44 | std::string name_; 45 | ros::WallTimer timer_; 46 | ros::V_string topics_; 47 | 48 | void timerCb(); 49 | 50 | public: 51 | AdvertisementChecker(const ros::NodeHandle& nh = ros::NodeHandle(), 52 | const std::string& name = std::string()); 53 | 54 | void start(const ros::V_string& topics, double duration); 55 | 56 | void stop(); 57 | }; 58 | 59 | } // namespace image_proc 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /thirdparty/image_proc/include/image_proc/processor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #ifndef IMAGE_PROC_PROCESSOR_H 35 | #define IMAGE_PROC_PROCESSOR_H 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | namespace image_proc { 42 | 43 | struct ImageSet 44 | { 45 | std::string color_encoding; 46 | cv::Mat mono; 47 | cv::Mat rect; 48 | cv::Mat color; 49 | cv::Mat rect_color; 50 | }; 51 | 52 | class Processor 53 | { 54 | public: 55 | Processor() 56 | : interpolation_(cv::INTER_LINEAR) 57 | { 58 | } 59 | 60 | int interpolation_; 61 | 62 | enum { 63 | MONO = 1 << 0, 64 | RECT = 1 << 1, 65 | COLOR = 1 << 2, 66 | RECT_COLOR = 1 << 3, 67 | ALL = MONO | RECT | COLOR | RECT_COLOR 68 | }; 69 | 70 | bool process(const sensor_msgs::ImageConstPtr& raw_image, 71 | const image_geometry::PinholeCameraModel& model, 72 | ImageSet& output, int flags = ALL) const; 73 | }; 74 | 75 | } //namespace image_proc 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /thirdparty/image_proc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(image_proc) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | find_package(catkin REQUIRED cv_bridge dynamic_reconfigure image_geometry image_transport nodelet nodelet_topic_tools roscpp sensor_msgs) 7 | find_package(OpenCV REQUIRED) 8 | find_package(Boost REQUIRED COMPONENTS thread) 9 | 10 | option(SAVE_TIMES "Enable option to save tracking times" OFF) 11 | if(SAVE_TIMES) 12 | message(STATUS "Building with SAVE_TIMES") 13 | add_definitions(-DSAVE_TIMES) 14 | endif() 15 | 16 | if(cv_bridge_VERSION VERSION_GREATER "1.12.0") 17 | add_compile_options(-std=c++11) 18 | endif() 19 | 20 | # Dynamic reconfigure support 21 | generate_dynamic_reconfigure_options(cfg/CropDecimate.cfg cfg/Debayer.cfg cfg/Rectify.cfg cfg/Resize.cfg) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS image_geometry roscpp sensor_msgs 25 | DEPENDS OpenCV 26 | INCLUDE_DIRS include 27 | LIBRARIES ${PROJECT_NAME} 28 | ) 29 | 30 | include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 31 | 32 | # Temporary fix for DataType deprecation in OpenCV 3.3.1. We continue to use the deprecated form for now because 33 | # the new one is not available in OpenCV 2.4 (on Trusty). 34 | add_definitions(-DOPENCV_TRAITS_ENABLE_DEPRECATED) 35 | 36 | # Nodelet library 37 | add_library(${PROJECT_NAME} src/libimage_proc/processor.cpp 38 | src/nodelets/debayer.cpp 39 | src/nodelets/rectify.cpp 40 | src/nodelets/resize.cpp 41 | src/nodelets/crop_decimate.cpp 42 | src/libimage_proc/advertisement_checker.cpp 43 | src/nodelets/edge_aware.cpp 44 | src/nodelets/crop_non_zero.cpp 45 | ) 46 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 47 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES}) 48 | 49 | install(TARGETS ${PROJECT_NAME} 50 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 53 | ) 54 | install(DIRECTORY include/${PROJECT_NAME}/ 55 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 56 | ) 57 | install(FILES nodelet_plugins.xml 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 59 | ) 60 | 61 | # Standalone node 62 | add_executable(image_proc_exe src/nodes/image_proc.cpp) 63 | target_link_libraries(image_proc_exe ${PROJECT_NAME} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES}) 64 | SET_TARGET_PROPERTIES(image_proc_exe PROPERTIES OUTPUT_NAME image_proc) 65 | install(TARGETS image_proc_exe 66 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 67 | ) 68 | 69 | # install the launch file 70 | install(DIRECTORY launch 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ 72 | ) 73 | 74 | if(CATKIN_ENABLE_TESTING) 75 | add_subdirectory(test) 76 | endif() 77 | -------------------------------------------------------------------------------- /run_rosario_sequence.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Run rosbag play, visualization and save trajectory in a text file. 4 | # It requires a catkin workspace containing pose_listener. 5 | 6 | set -eE # Any subsequent commands which fail will cause the shell script to exit immediately 7 | OUTPUT_TOPIC="/vio_back_end/odom" 8 | if [ -z "${CATKIN_WS_DIR}" ] ; then 9 | CATKIN_WS_DIR=$HOME/catkin_ws/ 10 | fi 11 | 12 | # Get full directory name of the script no matter where it is being called from 13 | CURRENT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 14 | 15 | function echoUsage() 16 | { 17 | echo -e "Usage: ./run_rosario_sequence.sh [FLAG] ROSBAG\n\ 18 | \t -r run method in a detached docker container \n\ 19 | \t -o path to output file \n\ 20 | \t -b do not open rviz visualization \n\ 21 | \t -h help" >&2 22 | } 23 | 24 | VISUALIZE=true 25 | RUN_CONTAINER=0 26 | OUTPUT_FILE=$CURRENT_DIR/trajectory_$(date '+%Y%m%d_%H%M%S').txt 27 | while getopts "hrbo:" opt; do 28 | case "$opt" in 29 | h) echoUsage 30 | exit 0 31 | ;; 32 | r) RUN_CONTAINER=1 33 | ;; 34 | b) VISUALIZE=false 35 | ;; 36 | o) case $OPTARG in 37 | -*) echo "ERROR: a path to output file must be provided"; echoUsage; exit 1 ;; 38 | *) OUTPUT_FILE=$OPTARG ;; 39 | esac 40 | ;; 41 | *) 42 | echoUsage 43 | exit 1 44 | ;; 45 | esac 46 | done 47 | 48 | shift $((OPTIND -1)) 49 | BAG=$1 50 | 51 | function cleanup() { 52 | if [ -n "${CID}" ] ; then 53 | printf "\e[31m%s %s\e[m\n" "Cleaning" 54 | docker stop $CID > /dev/null 55 | docker logs $CID > $(dirname $OUTPUT_FILE)/$(basename $OUTPUT_FILE .txt)_log.txt 56 | docker rm $CID > /dev/null 57 | unset CID 58 | fi 59 | # rosnode kill -a 60 | } 61 | 62 | trap cleanup INT 63 | trap cleanup ERR 64 | 65 | function wait_docker() { 66 | TOPIC=$1 67 | attempts=0 68 | max_attempts=30 69 | output=$(rostopic list $TOPIC 2> /dev/null || :) # force the command to exit successfully (i.e. $? == 0) to avoid trap 70 | while [ "$attempts" -lt "$max_attempts" ] && [ "$output" != $TOPIC ]; do 71 | sleep 1 72 | output=$(rostopic list $TOPIC 2> /dev/null || :) 73 | attempts=$(( attempts + 1 )) 74 | done 75 | if [ "$attempts" -eq "$max_attempts" ] ; then 76 | echo "ERROR: System seems not to start" 77 | cleanup 78 | exit 1 79 | fi 80 | } 81 | 82 | source /opt/ros/$ROS_DISTRO/setup.bash 83 | 84 | if [ $RUN_CONTAINER -eq 1 ] ; then 85 | echo "Starting docker container (detached mode)" 86 | CID=$($CURRENT_DIR/run.sh -v detached) 87 | fi 88 | 89 | wait_docker $OUTPUT_TOPIC 90 | 91 | source ${CATKIN_WS_DIR}/devel/setup.bash 92 | roslaunch $CURRENT_DIR/launch/play_bag_viz.launch \ 93 | config_rviz:=$CURRENT_DIR/rviz/rviz_config.rviz \ 94 | type:=O \ 95 | topic:=$OUTPUT_TOPIC \ 96 | save_to_file:=true \ 97 | visualize:=$VISUALIZE \ 98 | output_file:=$OUTPUT_FILE \ 99 | bagfile:=$BAG 100 | 101 | cleanup 102 | echo "END" 103 | -------------------------------------------------------------------------------- /thirdparty/image_proc/test/rostest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | class ImageProcTest : public testing::Test 11 | { 12 | protected: 13 | virtual void SetUp() 14 | { 15 | ros::NodeHandle local_nh("~"); 16 | 17 | // Determine topic names 18 | std::string camera_ns = nh.resolveName("camera") + "/"; 19 | if (camera_ns == "/camera") 20 | throw "Must remap 'camera' to the camera namespace."; 21 | topic_raw = camera_ns + "image_raw"; 22 | topic_mono = camera_ns + "image_mono"; 23 | topic_rect = camera_ns + "image_rect"; 24 | topic_color = camera_ns + "image_color"; 25 | topic_rect_color = camera_ns + "image_rect_color"; 26 | 27 | // Load raw image and cam info 28 | /// @todo Make these cmd-line args instead? 29 | std::string raw_image_file, cam_info_file; 30 | if (!local_nh.getParam("raw_image_file", raw_image_file)) 31 | throw "Must set parameter ~raw_image_file."; 32 | if (!local_nh.getParam("camera_info_file", cam_info_file)) 33 | throw "Must set parameter ~camera_info_file."; 34 | 35 | /// @todo Test variety of encodings for raw image (bayer, mono, color) 36 | cv::Mat img = cv::imread(raw_image_file, 0); 37 | raw_image = cv_bridge::CvImage(std_msgs::Header(), "mono8", img).toImageMsg(); 38 | std::string cam_name; 39 | if (!camera_calibration_parsers::readCalibration(cam_info_file, cam_name, cam_info)) 40 | throw "Failed to read camera info file."; 41 | 42 | // Create raw camera publisher 43 | image_transport::ImageTransport it(nh); 44 | cam_pub = it.advertiseCamera(topic_raw, 1); 45 | 46 | // Wait for image_proc to be operational 47 | ros::master::V_TopicInfo topics; 48 | while (true) { 49 | if (ros::master::getTopics(topics)) { 50 | BOOST_FOREACH(ros::master::TopicInfo& topic, topics) { 51 | if (topic.name == topic_rect_color) 52 | return; 53 | } 54 | } 55 | ros::Duration(0.5).sleep(); 56 | } 57 | } 58 | 59 | ros::NodeHandle nh; 60 | std::string topic_raw; 61 | std::string topic_mono; 62 | std::string topic_rect; 63 | std::string topic_color; 64 | std::string topic_rect_color; 65 | 66 | sensor_msgs::ImagePtr raw_image; 67 | sensor_msgs::CameraInfo cam_info; 68 | image_transport::CameraPublisher cam_pub; 69 | 70 | void publishRaw() 71 | { 72 | cam_pub.publish(*raw_image, cam_info); 73 | } 74 | }; 75 | 76 | void callback(const sensor_msgs::ImageConstPtr& msg) 77 | { 78 | ROS_FATAL("Got an image"); 79 | ros::shutdown(); 80 | } 81 | 82 | TEST_F(ImageProcTest, monoSubscription) 83 | { 84 | ROS_INFO("In test. Subscribing."); 85 | ros::Subscriber mono_sub = nh.subscribe(topic_mono, 1, callback); 86 | ROS_INFO("Publishing."); 87 | publishRaw(); 88 | 89 | ROS_INFO("Spinning."); 90 | ros::spin(); 91 | ROS_INFO("Done."); 92 | } 93 | 94 | int main(int argc, char** argv) 95 | { 96 | ros::init(argc, argv, "imageproc_rostest"); 97 | testing::InitGoogleTest(&argc, argv); 98 | return RUN_ALL_TESTS(); 99 | } 100 | -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Perform several Basalt operations in a Docker container: 4 | # 1-Build docker image (compilation) 5 | # 2-Run method in a docker container (dev mode) 6 | # 3-Run method in a docker container (vis mode) 7 | 8 | DEV_MODE=0 9 | VIS_MODE=0 10 | BUILD=0 11 | DETACHED=0 12 | MUTUALLY_EXCLUSIVE_OPTS=0 13 | LAUNCH_FILE=vio_rosario_nodelet.launch 14 | 15 | # Get full directory name of the script no matter where it is being called from 16 | CURRENT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 17 | 18 | function echoUsage() 19 | { 20 | echo -e "Usage: ./run.sh -d | -v [detached] [-l LAUNCHFILE] | -b [-h] \n\ 21 | \t -d dev mode \n\ 22 | \t -v vis mode \n\ 23 | \t\t 'detached' to run in background \n\ 24 | \t\t -l to run a specific launch file. \n\ 25 | \t\t\t It must be placed in basalt_ros1/launch/ \n\ 26 | \t -b build image \n\ 27 | \t -h help" >&2 28 | } 29 | 30 | function mutually_exclusive_opts() { 31 | if [ $MUTUALLY_EXCLUSIVE_OPTS -eq 1 ] ; then 32 | >&2 echo "ERROR: Only one option is allowed" 33 | echoUsage 34 | exit 1 35 | fi 36 | } 37 | 38 | unset SOME_OPT 39 | while getopts "hdvl:b" opt; do 40 | SOME_OPT=1 41 | case "$opt" in 42 | h) 43 | echoUsage 44 | exit 0 45 | ;; 46 | d) DEV_MODE=1 47 | mutually_exclusive_opts 48 | MUTUALLY_EXCLUSIVE_OPTS=1 49 | ;; 50 | v) VIS_MODE=1 51 | mutually_exclusive_opts 52 | MUTUALLY_EXCLUSIVE_OPTS=1 53 | case $2 in 54 | "detached") DETACHED=1; shift ;; 55 | -* | "") ;; 56 | *) echoUsage; exit 1 ;; 57 | esac 58 | ;; 59 | b) BUILD=1 60 | mutually_exclusive_opts 61 | MUTUALLY_EXCLUSIVE_OPTS=1 62 | ;; 63 | l) if [ $VIS_MODE -eq 1 ] ; then 64 | case $OPTARG in 65 | -*) echo "ERROR: a path to launchfile must be provided"; echoUsage; exit 1 ;; 66 | *) LAUNCH_FILE=$OPTARG ;; 67 | esac 68 | else 69 | echoUsage; exit 1 70 | fi 71 | ;; 72 | *) 73 | echoUsage 74 | exit 1 75 | ;; 76 | esac 77 | done 78 | 79 | if [ -z "$SOME_OPT" ]; then 80 | echoUsage 81 | exit 1 82 | fi 83 | 84 | if [ $DEV_MODE -eq 1 ] ; then 85 | docker run --rm -it --net=host -v $CURRENT_DIR:/root/catkin_ws/src/basalt_ros/ basalt:ros_melodic 86 | fi 87 | 88 | if [ $VIS_MODE -eq 1 ] ; then 89 | if [ $DETACHED -eq 1 ] ; then 90 | docker run -d --net=host \ 91 | -v $CURRENT_DIR/basalt_ros1/config/:/root/catkin_ws/src/basalt_ros/basalt_ros1/config/ \ 92 | -v $CURRENT_DIR/basalt_ros1/launch/:/root/catkin_ws/src/basalt_ros/basalt_ros1/launch/ \ 93 | basalt:ros_melodic \ 94 | roslaunch basalt_ros1 $LAUNCH_FILE 95 | else 96 | docker run --net=host \ 97 | -v $CURRENT_DIR/basalt_ros1/config/:/root/catkin_ws/src/basalt_ros/basalt_ros1/config/ \ 98 | -v $CURRENT_DIR/basalt_ros1/launch/:/root/catkin_ws/src/basalt_ros/basalt_ros1/launch/ \ 99 | basalt:ros_melodic \ 100 | roslaunch basalt_ros1 $LAUNCH_FILE 101 | fi 102 | fi 103 | 104 | if [ $BUILD -eq 1 ] ; then 105 | docker build --rm -t "basalt:ros_melodic" $CURRENT_DIR 106 | fi -------------------------------------------------------------------------------- /thirdparty/image_proc/src/libimage_proc/advertisement_checker.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include "image_proc/advertisement_checker.h" 35 | #include 36 | 37 | namespace image_proc { 38 | 39 | AdvertisementChecker::AdvertisementChecker(const ros::NodeHandle& nh, 40 | const std::string& name) 41 | : nh_(nh), 42 | name_(name) 43 | { 44 | } 45 | 46 | void AdvertisementChecker::timerCb() 47 | { 48 | ros::master::V_TopicInfo topic_info; 49 | if (!ros::master::getTopics(topic_info)) return; 50 | 51 | ros::V_string::iterator topic_it = topics_.begin(); 52 | while (topic_it != topics_.end()) 53 | { 54 | // Should use std::find_if 55 | bool found = false; 56 | ros::master::V_TopicInfo::iterator info_it = topic_info.begin(); 57 | while (!found && info_it != topic_info.end()) 58 | { 59 | found = (*topic_it == info_it->name); 60 | ++info_it; 61 | } 62 | if (found) 63 | topic_it = topics_.erase(topic_it); 64 | else 65 | { 66 | ROS_WARN_NAMED(name_, "The input topic '%s' is not yet advertised", topic_it->c_str()); 67 | ++topic_it; 68 | } 69 | } 70 | 71 | if (topics_.empty()) 72 | stop(); 73 | } 74 | 75 | void AdvertisementChecker::start(const ros::V_string& topics, double duration) 76 | { 77 | topics_.clear(); 78 | BOOST_FOREACH(const std::string& topic, topics) 79 | topics_.push_back(nh_.resolveName(topic)); 80 | 81 | ros::NodeHandle nh; 82 | timer_ = nh.createWallTimer(ros::WallDuration(duration), 83 | boost::bind(&AdvertisementChecker::timerCb, this)); 84 | timerCb(); 85 | } 86 | 87 | void AdvertisementChecker::stop() 88 | { 89 | timer_.stop(); 90 | } 91 | 92 | } // namespace image_proc 93 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Basalt 2 | 3 | ## Original repository 4 | This repository is a modified version of [basalt_ros](https://github.com/berndpfrommer/basalt_ros). We facilitate the installation process and the use of Docker. 5 | 6 | ## Docker support 7 | 8 | In order to facilitate the installation process, the system is wrapped up using Docker. 9 | We provide scripts to create a Docker image, build the system and run it in a Docker container. 10 | 11 | ### Dependencies 12 | * Docker 13 | * ROS 14 | * [`pose_listener`](https://github.com/CIFASIS/pose_listener) (if you use `run_rosario_sequence.sh`, see below) 15 | 16 | ### Building the system 17 | Run: 18 | ``` 19 | ./run.sh -b 20 | ``` 21 | 22 | This command creates a Docker image, installs all the dependencies and builds the system. The resulting image contains a version of the system ready to be run. 23 | 24 | ### Running the system in VIS mode 25 | If you are not interested in making changes in the source code, you should run the system in VIS mode. Run: 26 | ``` 27 | ./run.sh -v 28 | ``` 29 | The system is launched in a Docker container based on the previously built image. By default, this command executes a launch file which is configured to run the Rosario dataset. If you want to run your own dataset, **write a launch file and placed it in** `basalt_ros1/launch/`. Configuration files must be placed in `basalt_ros1/config/`. Then, run the script with the option `-l `. For example, if you are testing EuRoC, write `euroc_dataset.launch`, move it into `basalt_ros1/launch/` and type: 30 | ``` 31 | ./run.sh -v -l euroc_dataset.launch 32 | ``` 33 | Making changes in launch/configuration files in the host is possible because these folders are mounted into the Docker container. It is not necessary to access the container through a bash shell to modify these files. 34 | 35 | See below for information about input data and visualization. 36 | 37 | ### Running the system in DEV mode 38 | DEV mode allows developers to make changes in the source code, recompile the system and run it with the modifications. To do this, the whole repository is mounted in a container. Run: 39 | ``` 40 | ./run.sh -d 41 | ``` 42 | This opens a bash shell in a docker container. You can edit source files in the host and after that you can use this shell to recompile the system. When the compilation process finishes, you can run the method using `roslaunch`. 43 | 44 | See below for information about input data and visualization. 45 | 46 | ### Input data and visualization 47 | 48 | At this point, the system is waiting for input data. Either you can run `rosbag play` or you can use `run_rosario_sequence.sh`. 49 | If you choose the latter, open a second terminal and run: 50 | ``` 51 | ./run_rosario_sequence.sh -o 52 | ``` 53 | In contrast to what `run.sh` does, `run_rosario_sequence.sh` executes commands in the host (you can modify it to use a Docker container). 54 | 55 | `ROSBAG_FILE` is played using `rosbag`. Also, make sure you have cloned and built `pose_listener` in your catkin workspace. Default path for the workspace is `${HOME}/catkin_ws`, set `CATKIN_WS_DIR` if the workspace is somewhere else (e.g.: `export CATKIN_WS_DIR=$HOME/foo_catkin_ws`). `pose_listener` saves the estimated trajectory in `` (use absolute path). You can edit `run_rosario_sequence.sh` if you prefer to save the trajectory using your own methods. Additionally, `run_rosario_sequence.sh` launches `rviz` to display visual information during the execution of the system. 56 | 57 | Alternatively, if you are not interested in development but in testing or visualization, instead of running `run.sh` and `run_rosario_sequence.sh` in two different terminals, you can just run: 58 | ``` 59 | ./run_rosario_sequence.sh -r -o 60 | ``` 61 | This launches a Docker container and executes the default launch file (see `LAUNCH_FILE` in `run.sh`). After that, the bagfile is played and `rviz` and `pose_listener` are launched. Add `-b` if you want to turn off the visualization. 62 | 63 | 64 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodes/image_proc.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | int main(int argc, char **argv) 40 | { 41 | ros::init(argc, argv, "image_proc"); 42 | 43 | // Check for common user errors 44 | if (ros::names::remap("camera") != "camera") 45 | { 46 | ROS_WARN("Remapping 'camera' has no effect! Start image_proc in the " 47 | "camera namespace instead.\nExample command-line usage:\n" 48 | "\t$ ROS_NAMESPACE=%s rosrun image_proc image_proc", 49 | ros::names::remap("camera").c_str()); 50 | } 51 | if (ros::this_node::getNamespace() == "/") 52 | { 53 | ROS_WARN("Started in the global namespace! This is probably wrong. Start image_proc " 54 | "in the camera namespace.\nExample command-line usage:\n" 55 | "\t$ ROS_NAMESPACE=my_camera rosrun image_proc image_proc"); 56 | } 57 | 58 | // Shared parameters to be propagated to nodelet private namespaces 59 | ros::NodeHandle private_nh("~"); 60 | XmlRpc::XmlRpcValue shared_params; 61 | int queue_size; 62 | if (private_nh.getParam("queue_size", queue_size)) 63 | shared_params["queue_size"] = queue_size; 64 | 65 | nodelet::Loader manager(false); // Don't bring up the manager ROS API 66 | nodelet::M_string remappings; 67 | nodelet::V_string my_argv; 68 | 69 | // Debayer nodelet, image_raw -> image_mono, image_color 70 | std::string debayer_name = ros::this_node::getName() + "_debayer"; 71 | manager.load(debayer_name, "image_proc/debayer", remappings, my_argv); 72 | 73 | // Rectify nodelet, image_mono -> image_rect 74 | std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono"; 75 | if (shared_params.valid()) 76 | ros::param::set(rectify_mono_name, shared_params); 77 | manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv); 78 | 79 | // Rectify nodelet, image_color -> image_rect_color 80 | // NOTE: Explicitly resolve any global remappings here, so they don't get hidden. 81 | remappings["image_mono"] = ros::names::resolve("image_color"); 82 | remappings["image_rect"] = ros::names::resolve("image_rect_color"); 83 | std::string rectify_color_name = ros::this_node::getName() + "_rectify_color"; 84 | if (shared_params.valid()) 85 | ros::param::set(rectify_color_name, shared_params); 86 | manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv); 87 | 88 | // Check for only the original camera topics 89 | ros::V_string topics; 90 | topics.push_back(ros::names::resolve("image_raw")); 91 | topics.push_back(ros::names::resolve("camera_info")); 92 | image_proc::AdvertisementChecker check_inputs(ros::NodeHandle(), ros::this_node::getName()); 93 | check_inputs.start(topics, 60.0); 94 | 95 | ros::spin(); 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/libimage_proc/processor.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include "image_proc/processor.h" 35 | #include 36 | #include 37 | 38 | namespace image_proc { 39 | 40 | namespace enc = sensor_msgs::image_encodings; 41 | 42 | bool Processor::process(const sensor_msgs::ImageConstPtr& raw_image, 43 | const image_geometry::PinholeCameraModel& model, 44 | ImageSet& output, int flags) const 45 | { 46 | static const int MONO_EITHER = MONO | RECT; 47 | static const int COLOR_EITHER = COLOR | RECT_COLOR; 48 | if (!(flags & ALL)) return true; 49 | 50 | // Check if raw_image is color 51 | const std::string& raw_encoding = raw_image->encoding; 52 | int raw_type = CV_8UC1; 53 | if (raw_encoding == enc::BGR8 || raw_encoding == enc::RGB8) { 54 | raw_type = CV_8UC3; 55 | output.color_encoding = raw_encoding; 56 | } 57 | // Construct cv::Mat pointing to raw_image data 58 | const cv::Mat raw(raw_image->height, raw_image->width, raw_type, 59 | const_cast(&raw_image->data[0]), raw_image->step); 60 | 61 | /////////////////////////////////////////////////////// 62 | // Construct colorized (unrectified) images from raw // 63 | /////////////////////////////////////////////////////// 64 | 65 | // Bayer case 66 | if (raw_encoding.find("bayer") != std::string::npos) { 67 | // Convert to color BGR 68 | /// @todo Faster to convert directly to mono when color is not requested, but OpenCV doesn't support 69 | int code = 0; 70 | if (raw_encoding == enc::BAYER_RGGB8) 71 | code = cv::COLOR_BayerBG2BGR; 72 | else if (raw_encoding == enc::BAYER_BGGR8) 73 | code = cv::COLOR_BayerRG2BGR; 74 | else if (raw_encoding == enc::BAYER_GBRG8) 75 | code = cv::COLOR_BayerGR2BGR; 76 | else if (raw_encoding == enc::BAYER_GRBG8) 77 | code = cv::COLOR_BayerGB2BGR; 78 | else { 79 | ROS_ERROR("[image_proc] Unsupported encoding '%s'", raw_encoding.c_str()); 80 | return false; 81 | } 82 | cv::cvtColor(raw, output.color, code); 83 | output.color_encoding = enc::BGR8; 84 | 85 | if (flags & MONO_EITHER) 86 | cv::cvtColor(output.color, output.mono, cv::COLOR_BGR2GRAY); 87 | } 88 | // Color case 89 | else if (raw_type == CV_8UC3) { 90 | output.color = raw; 91 | if (flags & MONO_EITHER) { 92 | int code = (raw_encoding == enc::BGR8) ? cv::COLOR_BGR2GRAY : cv::COLOR_RGB2GRAY; 93 | cv::cvtColor(output.color, output.mono, code); 94 | } 95 | } 96 | // Mono case 97 | else if (raw_encoding == enc::MONO8) { 98 | output.mono = raw; 99 | if (flags & COLOR_EITHER) { 100 | output.color_encoding = enc::MONO8; 101 | output.color = raw; 102 | } 103 | } 104 | // 8UC3 does not specify a color encoding. Is it BGR, RGB, HSV, XYZ, LUV...? 105 | else if (raw_encoding == enc::TYPE_8UC3) { 106 | ROS_ERROR("[image_proc] Ambiguous encoding '8UC3'. The camera driver " 107 | "should set the encoding to 'bgr8' or 'rgb8'."); 108 | return false; 109 | } 110 | // Something else we can't handle 111 | else { 112 | ROS_ERROR("[image_proc] Unsupported encoding '%s'", raw_encoding.c_str()); 113 | return false; 114 | } 115 | 116 | ////////////////////////////////////////////////////// 117 | // Construct rectified images from colorized images // 118 | ////////////////////////////////////////////////////// 119 | 120 | /// @todo If no distortion, could just point to the colorized data. But copy is 121 | /// already way faster than remap. 122 | if (flags & RECT) 123 | model.rectifyImage(output.mono, output.rect, interpolation_); 124 | if (flags & RECT_COLOR) 125 | model.rectifyImage(output.color, output.rect_color, interpolation_); 126 | 127 | return true; 128 | } 129 | 130 | } //namespace image_proc 131 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodelets/crop_non_zero.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include 35 | #include 36 | #include 37 | #include 38 | //#include // for std::max_element 39 | 40 | namespace image_proc { 41 | 42 | namespace enc = sensor_msgs::image_encodings; 43 | 44 | class CropNonZeroNodelet : public nodelet::Nodelet 45 | { 46 | // Subscriptions 47 | boost::shared_ptr it_; 48 | image_transport::Subscriber sub_raw_; 49 | 50 | // Publications 51 | boost::mutex connect_mutex_; 52 | image_transport::Publisher pub_; 53 | 54 | virtual void onInit(); 55 | 56 | void connectCb(); 57 | 58 | void imageCb(const sensor_msgs::ImageConstPtr& raw_msg); 59 | }; 60 | 61 | void CropNonZeroNodelet::onInit() 62 | { 63 | ros::NodeHandle& nh = getNodeHandle(); 64 | it_.reset(new image_transport::ImageTransport(nh)); 65 | 66 | // Monitor whether anyone is subscribed to the output 67 | image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropNonZeroNodelet::connectCb, this); 68 | // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ 69 | boost::lock_guard lock(connect_mutex_); 70 | pub_ = it_->advertise("image", 1, connect_cb, connect_cb); 71 | } 72 | 73 | // Handles (un)subscribing when clients (un)subscribe 74 | void CropNonZeroNodelet::connectCb() 75 | { 76 | boost::lock_guard lock(connect_mutex_); 77 | if (pub_.getNumSubscribers() == 0) 78 | { 79 | sub_raw_.shutdown(); 80 | } 81 | else if (!sub_raw_) 82 | { 83 | image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); 84 | sub_raw_ = it_->subscribe("image_raw", 1, &CropNonZeroNodelet::imageCb, this, hints); 85 | } 86 | } 87 | 88 | void CropNonZeroNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg) 89 | { 90 | cv_bridge::CvImagePtr cv_ptr; 91 | try 92 | { 93 | cv_ptr = cv_bridge::toCvCopy(raw_msg); 94 | } 95 | catch (cv_bridge::Exception& e) 96 | { 97 | ROS_ERROR("cv_bridge exception: %s", e.what()); 98 | return; 99 | } 100 | 101 | // Check the number of channels 102 | if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){ 103 | NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str()); 104 | return; 105 | } 106 | 107 | std::vector >cnt; 108 | cv::Mat1b m(raw_msg->width, raw_msg->height); 109 | 110 | if (raw_msg->encoding == enc::TYPE_8UC1){ 111 | m = cv_ptr->image; 112 | }else{ 113 | double minVal, maxVal; 114 | cv::minMaxIdx(cv_ptr->image, &minVal, &maxVal, 0, 0, cv_ptr->image != 0.); 115 | double ra = maxVal - minVal; 116 | 117 | cv_ptr->image.convertTo(m, CV_8U, 255./ra, -minVal*255./ra); 118 | } 119 | 120 | cv::findContours(m, cnt, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); 121 | 122 | // search the largest area 123 | /* // -std=c++11 124 | std::vector >::iterator it = std::max_element(cnt.begin(), cnt.end(), [](std::vector a, std::vector b) { 125 | return a.size() < b.size(); 126 | }); 127 | */ 128 | std::vector >::iterator it = cnt.begin(); 129 | for(std::vector >::iterator i=cnt.begin();i!=cnt.end();++i){ 130 | it = (*it).size() < (*i).size() ? i : it; 131 | } 132 | cv::Rect r = cv::boundingRect(cnt[std::distance(cnt.begin(), it)]); 133 | 134 | cv_bridge::CvImage out_msg; 135 | out_msg.header = raw_msg->header; 136 | out_msg.encoding = raw_msg->encoding; 137 | out_msg.image = cv_ptr->image(r); 138 | 139 | pub_.publish(out_msg.toImageMsg()); 140 | } 141 | 142 | } // namespace image_proc 143 | 144 | // Register as nodelet 145 | #include 146 | PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet,nodelet::Nodelet); 147 | -------------------------------------------------------------------------------- /thirdparty/image_proc/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 1.16.0 (2021-11-12) 2 | ------------------- 3 | 4 | 1.15.3 (2020-12-11) 5 | ------------------- 6 | * remove email blasts from steve macenski (`#595 `_) 7 | * Contributors: Steve Macenski 8 | 9 | 1.15.2 (2020-05-19) 10 | ------------------- 11 | 12 | 1.15.1 (2020-05-18) 13 | ------------------- 14 | 15 | 1.15.0 (2020-05-14) 16 | ------------------- 17 | * Python 3 compatibility (`#530 `_) 18 | * cmake_minimum_required to 3.0.2 19 | * Adapted to OpenCV4 20 | * import setup from setuptools instead of distutils-core 21 | * updated install locations for better portability. (`#500 `_) 22 | * Contributors: Joshua Whitley, Sean Yen 23 | 24 | 1.14.0 (2020-01-12) 25 | ------------------- 26 | * resize.cpp: fix memory leak (`#489 `_) 27 | * Try catch around cvtColor to avoid demosaicing src.empty() error (`#463 `_) 28 | * Merge pull request `#436 `_ from ros-perception/throttle_warnings 29 | * adding throttled warnings to not blast the users 30 | * Merge pull request `#423 `_ from lucasw/crop_decimate_resolution_change 31 | Avoid crashing when the x or y offset is too large 32 | * Merge pull request `#435 `_ from ros-perception/patch_resize_copy 33 | * patch extra copy for nodelet users of resize 34 | * Merge pull request `#411 `_ from Tuebel/fix_409 35 | Fix 409 based on melodic branch 36 | * Need to look at x and y offset 37 | * Simplified copying of the camera_info message. 38 | * Independent resize of image and camera_info 39 | * removed unused infoCb 40 | * Rename infoCb to cameraCb matching subscribeCamera 41 | * replaced boost mutex & shared_ptr with std 42 | * Removed hard coded image encoding. 43 | Using toCvCopy instead of toCvShared (copy is needed anyway). 44 | * Contributors: Joshua Whitley, Lucas Walter, Tim Übelhör, Yuki Furuta, stevemacenski 45 | 46 | 1.13.0 (2019-06-12) 47 | ------------------- 48 | * Merge pull request `#395 `_ from ros-perception/steve_maintain 49 | * adding autonomoustuff mainainer 50 | * adding stevemacenski as maintainer to get emails 51 | * Contributors: Joshua Whitley, Yoshito Okada, stevemacenski 52 | 53 | 1.12.23 (2018-05-10) 54 | -------------------- 55 | 56 | 1.12.22 (2017-12-08) 57 | -------------------- 58 | * Merge pull request `#311 `_ from knorth55/revert-299 59 | Revert "Fix image_resize nodelet (`#299 `_)" 60 | This reverts commit 32e19697ebce47101b063c6a02b95dfa2c5dbc52. 61 | * Contributors: Shingo Kitagawa, Tully Foote 62 | 63 | 1.12.21 (2017-11-05) 64 | -------------------- 65 | * Fix image_resize nodelet (`#299 `_) 66 | Update interpolation types 67 | Add arguments to enable disable each nodelet 68 | Add default arguments for image_resize and image_rect 69 | Use toCVShare instead of toCVCopy 70 | Include image_resize in image_proc 71 | * Updated fix for traits change. (`#303 `_) 72 | * Fix C++11 compilation 73 | This fixes `#292 `_ and `#291 `_ 74 | * [image_proc][crop_decimate] support changing target image frame_id (`#276 `_) 75 | * Contributors: Furushchev, Mike Purvis, Vincent Rabaud, bikramak 76 | 77 | 1.12.20 (2017-04-30) 78 | -------------------- 79 | * Add nodelet to resize image and camera_info (`#273 `_) 80 | * Add nodelet to resize image and camera_info 81 | * Depends on nodelet_topic_tools 82 | * Use recursive_mutex for mutex guard for dynamic reconfiguring 83 | * Fix nodelet name: crop_nonZero -> crop_non_zero (`#269 `_) 84 | Fix https://github.com/ros-perception/image_pipeline/issues/217 85 | * Fix permission of executable files unexpectedly (`#260 `_) 86 | * address gcc6 build error 87 | With gcc6, compiling fails with `stdlib.h: No such file or directory`, 88 | as including '-isystem /usr/include' breaks with gcc6, cf., 89 | https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129. 90 | This commit addresses this issue for this package in the same way 91 | it was addressed in various other ROS packages. A list of related 92 | commits and pull requests is at: 93 | https://github.com/ros/rosdistro/issues/12783 94 | Signed-off-by: Lukas Bulwahn 95 | * Contributors: Kentaro Wada, Lukas Bulwahn 96 | 97 | 1.12.19 (2016-07-24) 98 | -------------------- 99 | 100 | 1.12.18 (2016-07-12) 101 | -------------------- 102 | 103 | 1.12.17 (2016-07-11) 104 | -------------------- 105 | 106 | 1.12.16 (2016-03-19) 107 | -------------------- 108 | * clean OpenCV dependency in package.xml 109 | * issue `#180 `_ Check if all distortion coefficients are zero. 110 | Test with: 111 | rostest --reuse-master --text image_proc test_rectify.xml 112 | Can also test interactively with vimjay image_rect.launch, which brings up an rqt gui and camera info distortion coefficients can be dynamically reconfigured. 113 | * Add a feature to crop the largest valid (non zero) area 114 | Remove unnecessary headers 115 | change a filename to fit for the ROS convention 116 | * Contributors: Kenta Yonekura, Lucas Walter, Vincent Rabaud 117 | 118 | 1.12.15 (2016-01-17) 119 | -------------------- 120 | * simplify OpenCV3 conversion 121 | * Contributors: Vincent Rabaud 122 | 123 | 1.12.14 (2015-07-22) 124 | -------------------- 125 | 126 | 1.12.13 (2015-04-06) 127 | -------------------- 128 | * fix dependencies 129 | * Contributors: Vincent Rabaud 130 | 131 | 1.12.12 (2014-12-31) 132 | -------------------- 133 | 134 | 1.12.11 (2014-10-26) 135 | -------------------- 136 | 137 | 1.12.10 (2014-09-28) 138 | -------------------- 139 | 140 | 1.12.9 (2014-09-21) 141 | ------------------- 142 | * get code to compile with OpenCV3 143 | fixes `#96 `_ 144 | * Contributors: Vincent Rabaud 145 | 146 | 1.12.8 (2014-08-19) 147 | ------------------- 148 | 149 | 1.12.6 (2014-07-27) 150 | ------------------- 151 | 152 | 1.12.4 (2014-04-28) 153 | ------------------- 154 | 155 | 1.12.3 (2014-04-12) 156 | ------------------- 157 | 158 | 1.12.2 (2014-04-08) 159 | ------------------- 160 | 161 | 1.12.1 (2014-04-06) 162 | ------------------- 163 | * get proper opencv dependency 164 | * Contributors: Vincent Rabaud 165 | 166 | 1.11.7 (2014-03-28) 167 | ------------------- 168 | 169 | 1.11.6 (2014-01-29 00:38:55 +0100) 170 | ---------------------------------- 171 | - fix bad OpenCV linkage (#53) 172 | -------------------------------------------------------------------------------- /thirdparty/image_proc/test/test_rectify.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class ImageProcRectifyTest : public testing::Test 11 | { 12 | protected: 13 | virtual void SetUp() 14 | { 15 | // Determine topic names 16 | std::string camera_ns = nh_.resolveName("camera") + "/"; 17 | if (camera_ns == "/camera") 18 | throw "Must remap 'camera' to the camera namespace."; 19 | topic_raw_ = camera_ns + "image_raw"; 20 | topic_mono_ = camera_ns + "image_mono"; 21 | topic_rect_ = camera_ns + "image_rect"; 22 | topic_color_ = camera_ns + "image_color"; 23 | topic_rect_color_ = camera_ns + "image_rect_color"; 24 | 25 | // Taken from vision_opencv/image_geometry/test/utest.cpp 26 | double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0}; 27 | double K[] = {430.15433020105519, 0.0, 311.71339830549732, 28 | 0.0, 430.60920415473657, 221.06824942698509, 29 | 0.0, 0.0, 1.0}; 30 | double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904, 31 | -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664, 32 | -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516}; 33 | double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0, 34 | 0.0, 295.53402059708782, 223.29617881774902, 0.0, 35 | 0.0, 0.0, 1.0, 0.0}; 36 | 37 | cam_info_.header.frame_id = "tf_frame"; 38 | cam_info_.height = 480; 39 | cam_info_.width = 640; 40 | // No ROI 41 | cam_info_.D.resize(5); 42 | std::copy(D, D+5, cam_info_.D.begin()); 43 | std::copy(K, K+9, cam_info_.K.begin()); 44 | std::copy(R, R+9, cam_info_.R.begin()); 45 | std::copy(P, P+12, cam_info_.P.begin()); 46 | cam_info_.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; 47 | 48 | distorted_image_ = cv::Mat(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3); 49 | // draw a grid 50 | const cv::Scalar color = cv::Scalar(255, 255, 255); 51 | // draw the lines thick so the proportion of error due to 52 | // interpolation is reduced 53 | const int thickness = 7; 54 | const int type = 8; 55 | for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10) 56 | { 57 | cv::line(distorted_image_, 58 | cv::Point(0, y), cv::Point(cam_info_.width, y), 59 | color, type, thickness); 60 | } 61 | for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10) 62 | { 63 | // draw the lines thick so the prorportion of interpolation error is reduced 64 | cv::line(distorted_image_, 65 | cv::Point(x, 0), cv::Point(x, cam_info_.height), 66 | color, type, thickness); 67 | } 68 | 69 | raw_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8", 70 | distorted_image_).toImageMsg(); 71 | 72 | // Create raw camera subscriber and publisher 73 | image_transport::ImageTransport it(nh_); 74 | cam_pub_ = it.advertiseCamera(topic_raw_, 1); 75 | } 76 | 77 | ros::NodeHandle nh_; 78 | std::string topic_raw_; 79 | std::string topic_mono_; 80 | std::string topic_rect_; 81 | std::string topic_color_; 82 | std::string topic_rect_color_; 83 | 84 | cv::Mat distorted_image_; 85 | sensor_msgs::ImagePtr raw_image_; 86 | bool has_new_image_; 87 | cv::Mat received_image_; 88 | sensor_msgs::CameraInfo cam_info_; 89 | image_transport::CameraPublisher cam_pub_; 90 | image_transport::Subscriber cam_sub_; 91 | 92 | public: 93 | void imageCallback(const sensor_msgs::ImageConstPtr& msg) 94 | { 95 | cv_bridge::CvImageConstPtr cv_ptr; 96 | try 97 | { 98 | cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8); 99 | } 100 | catch (cv_bridge::Exception& e) 101 | { 102 | ROS_FATAL("cv_bridge exception: %s", e.what()); 103 | return; 104 | } 105 | received_image_ = cv_ptr->image.clone(); 106 | has_new_image_ = true; 107 | } 108 | 109 | void publishRaw() 110 | { 111 | has_new_image_ = false; 112 | cam_pub_.publish(*raw_image_, cam_info_); 113 | } 114 | }; 115 | 116 | TEST_F(ImageProcRectifyTest, rectifyTest) 117 | { 118 | ROS_INFO("In test. Subscribing."); 119 | image_transport::ImageTransport it(nh_); 120 | cam_sub_ = it.subscribe(topic_rect_, 1, &ImageProcRectifyTest::imageCallback, 121 | dynamic_cast(this)); 122 | // Wait for image_proc to be operational 123 | bool wait_for_topic = true; 124 | while (wait_for_topic) 125 | { 126 | // @todo this fails without the additional 0.5 second sleep after the 127 | // publisher comes online, which means on a slower or more heavily 128 | // loaded system it may take longer than 0.5 seconds, and the test 129 | // would hang until the timeout is reached and fail. 130 | if (cam_sub_.getNumPublishers() > 0) 131 | wait_for_topic = false; 132 | ros::Duration(0.5).sleep(); 133 | } 134 | 135 | // All the tests are the same as from 136 | // vision_opencv/image_geometry/test/utest.cpp 137 | // default cam info 138 | 139 | // Just making this number up, maybe ought to be larger 140 | // since a completely different image would be on the order of 141 | // width * height * 255 = 78e6 142 | const double diff_threshold = 10000.0; 143 | double error; 144 | 145 | // use original cam_info 146 | publishRaw(); 147 | while (!has_new_image_) 148 | { 149 | ros::spinOnce(); 150 | ros::Duration(0.5).sleep(); 151 | } 152 | // Test that rectified image is sufficiently different 153 | // using default distortion 154 | error = cv::norm(distorted_image_, received_image_, cv::NORM_L1); 155 | // Just making this number up, maybe ought to be larger 156 | EXPECT_GT(error, diff_threshold); 157 | 158 | // Test that rectified image is sufficiently different 159 | // using default distortion but with first element zeroed 160 | // out. 161 | sensor_msgs::CameraInfo cam_info_orig = cam_info_; 162 | cam_info_.D[0] = 0.0; 163 | publishRaw(); 164 | while (!has_new_image_) 165 | { 166 | ros::spinOnce(); 167 | ros::Duration(0.5).sleep(); 168 | } 169 | error = cv::norm(distorted_image_, received_image_, cv::NORM_L1); 170 | EXPECT_GT(error, diff_threshold); 171 | 172 | // Test that rectified image is the same using zero distortion 173 | cam_info_.D.assign(cam_info_.D.size(), 0); 174 | publishRaw(); 175 | while (!has_new_image_) 176 | { 177 | ros::spinOnce(); 178 | ros::Duration(0.5).sleep(); 179 | } 180 | error = cv::norm(distorted_image_, received_image_, cv::NORM_L1); 181 | EXPECT_EQ(error, 0); 182 | 183 | 184 | // Test that rectified image is the same using empty distortion 185 | cam_info_.D.clear(); 186 | publishRaw(); 187 | while (!has_new_image_) 188 | { 189 | ros::spinOnce(); 190 | ros::Duration(0.5).sleep(); 191 | } 192 | error = cv::norm(distorted_image_, received_image_, cv::NORM_L1); 193 | 194 | EXPECT_EQ(error, 0); 195 | 196 | // restore the original cam_info for other tests added in the future 197 | cam_info_ = cam_info_orig; 198 | } 199 | 200 | int main(int argc, char** argv) 201 | { 202 | ros::init(argc, argv, "image_proc_test_rectify"); 203 | testing::InitGoogleTest(&argc, argv); 204 | return RUN_ALL_TESTS(); 205 | } 206 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodelets/rectify.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include 35 | #if ((BOOST_VERSION / 100) % 1000) >= 53 36 | #include 37 | #endif 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | namespace image_proc { 51 | 52 | class RectifyNodelet : public nodelet::Nodelet 53 | { 54 | // ROS communication 55 | boost::shared_ptr it_; 56 | image_transport::CameraSubscriber sub_camera_; 57 | int queue_size_; 58 | 59 | boost::mutex connect_mutex_; 60 | image_transport::Publisher pub_rect_; 61 | 62 | // Dynamic reconfigure 63 | boost::recursive_mutex config_mutex_; 64 | typedef image_proc::RectifyConfig Config; 65 | typedef dynamic_reconfigure::Server ReconfigureServer; 66 | boost::shared_ptr reconfigure_server_; 67 | Config config_; 68 | 69 | // Processing state (note: only safe because we're using single-threaded NodeHandle!) 70 | image_geometry::PinholeCameraModel model_; 71 | 72 | #ifdef SAVE_TIMES 73 | std::ofstream f_track_times_; 74 | int num_tracked_frames_; 75 | ~RectifyNodelet(); 76 | #endif 77 | 78 | virtual void onInit(); 79 | 80 | void connectCb(); 81 | 82 | void imageCb(const sensor_msgs::ImageConstPtr& image_msg, 83 | const sensor_msgs::CameraInfoConstPtr& info_msg); 84 | 85 | void configCb(Config &config, uint32_t level); 86 | }; 87 | 88 | void RectifyNodelet::onInit() 89 | { 90 | ros::NodeHandle &nh = getNodeHandle(); 91 | ros::NodeHandle &private_nh = getPrivateNodeHandle(); 92 | it_.reset(new image_transport::ImageTransport(nh)); 93 | 94 | // Read parameters 95 | private_nh.param("queue_size", queue_size_, 5); 96 | #ifdef SAVE_TIMES 97 | std::string node_name = ros::names::clean(getName()); 98 | if(*node_name.begin() == '/') { 99 | node_name.erase(0,1); 100 | } 101 | const std::string prefix = "tracking_times_start_"; 102 | const std::string extension = ".txt"; 103 | f_track_times_.open(prefix + node_name + extension); 104 | f_track_times_ << std::fixed; 105 | f_track_times_ << std::setprecision(6); 106 | num_tracked_frames_ = 0; 107 | #endif 108 | 109 | // Set up dynamic reconfigure 110 | reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh)); 111 | ReconfigureServer::CallbackType f = boost::bind(&RectifyNodelet::configCb, this, _1, _2); 112 | reconfigure_server_->setCallback(f); 113 | 114 | // Monitor whether anyone is subscribed to the output 115 | image_transport::SubscriberStatusCallback connect_cb = boost::bind(&RectifyNodelet::connectCb, this); 116 | // Make sure we don't enter connectCb() between advertising and assigning to pub_rect_ 117 | boost::lock_guard lock(connect_mutex_); 118 | pub_rect_ = it_->advertise("image_rect", 1, connect_cb, connect_cb); 119 | } 120 | 121 | #ifdef SAVE_TIMES 122 | RectifyNodelet::~RectifyNodelet() { 123 | f_track_times_.close(); 124 | } 125 | #endif 126 | 127 | // Handles (un)subscribing when clients (un)subscribe 128 | void RectifyNodelet::connectCb() 129 | { 130 | boost::lock_guard lock(connect_mutex_); 131 | if (pub_rect_.getNumSubscribers() == 0) 132 | sub_camera_.shutdown(); 133 | else if (!sub_camera_) 134 | { 135 | image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); 136 | sub_camera_ = it_->subscribeCamera("image_mono", queue_size_, &RectifyNodelet::imageCb, this, hints); 137 | } 138 | } 139 | 140 | void RectifyNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg, 141 | const sensor_msgs::CameraInfoConstPtr& info_msg) 142 | { 143 | 144 | #ifdef SAVE_TIMES 145 | num_tracked_frames_++; 146 | auto const t = std::chrono::system_clock::now().time_since_epoch().count(); 147 | f_track_times_ << num_tracked_frames_ << " " << t / 1e09 << std::endl; 148 | #endif 149 | 150 | // Verify camera is actually calibrated 151 | if (info_msg->K[0] == 0.0) { 152 | NODELET_ERROR_THROTTLE(30, "Rectified topic '%s' requested but camera publishing '%s' " 153 | "is uncalibrated", pub_rect_.getTopic().c_str(), 154 | sub_camera_.getInfoTopic().c_str()); 155 | return; 156 | } 157 | 158 | // If zero distortion, just pass the message along 159 | bool zero_distortion = true; 160 | for (size_t i = 0; i < info_msg->D.size(); ++i) 161 | { 162 | if (info_msg->D[i] != 0.0) 163 | { 164 | zero_distortion = false; 165 | break; 166 | } 167 | } 168 | // This will be true if D is empty/zero sized 169 | if (zero_distortion) 170 | { 171 | pub_rect_.publish(image_msg); 172 | return; 173 | } 174 | 175 | // Update the camera model 176 | model_.fromCameraInfo(info_msg); 177 | 178 | // Create cv::Mat views onto both buffers 179 | const cv::Mat image = cv_bridge::toCvShare(image_msg)->image; 180 | cv::Mat rect; 181 | 182 | // Rectify and publish 183 | int interpolation; 184 | { 185 | boost::lock_guard lock(config_mutex_); 186 | interpolation = config_.interpolation; 187 | } 188 | model_.rectifyImage(image, rect, interpolation); 189 | 190 | // Allocate new rectified image message 191 | sensor_msgs::ImagePtr rect_msg = cv_bridge::CvImage(image_msg->header, image_msg->encoding, rect).toImageMsg(); 192 | pub_rect_.publish(rect_msg); 193 | } 194 | 195 | void RectifyNodelet::configCb(Config &config, uint32_t level) 196 | { 197 | config_ = config; 198 | } 199 | 200 | } // namespace image_proc 201 | 202 | // Register nodelet 203 | #include 204 | PLUGINLIB_EXPORT_CLASS( image_proc::RectifyNodelet, nodelet::Nodelet) 205 | -------------------------------------------------------------------------------- /rviz/rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /VIOGroup1 9 | Splitter Ratio: 0.4651159942150116 10 | Tree Height: 319 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | - Class: rviz/Displays 31 | Help Height: 104 32 | Name: Displays 33 | Property Tree Widget: 34 | Expanded: ~ 35 | Splitter Ratio: 0.5 36 | Tree Height: 318 37 | Preferences: 38 | PromptSaveOnExit: true 39 | Toolbars: 40 | toolButtonStyle: 2 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Alpha: 0.5 45 | Cell Size: 1 46 | Class: rviz/Grid 47 | Color: 160; 160; 164 48 | Enabled: true 49 | Line Style: 50 | Line Width: 0.029999999329447746 51 | Value: Lines 52 | Name: Grid 53 | Normal Cell Count: 0 54 | Offset: 55 | X: 0 56 | Y: 0 57 | Z: 0 58 | Plane: XY 59 | Plane Cell Count: 10 60 | Reference Frame: 61 | Value: true 62 | - Alpha: 1 63 | Class: rviz/Axes 64 | Enabled: true 65 | Length: 0.5 66 | Name: Axes 67 | Radius: 0.05000000074505806 68 | Reference Frame: 69 | Value: true 70 | - Class: rviz/Group 71 | Displays: 72 | - Angle Tolerance: 0.10000000149011612 73 | Class: rviz/Odometry 74 | Covariance: 75 | Orientation: 76 | Alpha: 0.5 77 | Color: 255; 255; 127 78 | Color Style: Unique 79 | Frame: Local 80 | Offset: 1 81 | Scale: 1 82 | Value: true 83 | Position: 84 | Alpha: 0.30000001192092896 85 | Color: 204; 51; 204 86 | Scale: 1 87 | Value: true 88 | Value: true 89 | Enabled: true 90 | Keep: 100 91 | Name: VIOPath 92 | Position Tolerance: 0.10000000149011612 93 | Queue Size: 10 94 | Shape: 95 | Alpha: 1 96 | Axes Length: 1 97 | Axes Radius: 0.10000000149011612 98 | Color: 255; 25; 0 99 | Head Length: 0.30000001192092896 100 | Head Radius: 0.10000000149011612 101 | Shaft Length: 1 102 | Shaft Radius: 0.05000000074505806 103 | Value: Arrow 104 | Topic: /vio_back_end/odom 105 | Unreliable: false 106 | Value: true 107 | - Class: rviz/Image 108 | Enabled: true 109 | Image Topic: /viz_flow/flow_image 110 | Max Value: 1 111 | Median window: 5 112 | Min Value: 0 113 | Name: track_image 114 | Normalize Range: true 115 | Queue Size: 2 116 | Transport Hint: raw 117 | Unreliable: false 118 | Value: true 119 | Enabled: true 120 | Name: VIOGroup 121 | Enabled: true 122 | Global Options: 123 | Background Color: 0; 0; 0 124 | Default Light: true 125 | Fixed Frame: world 126 | Frame Rate: 30 127 | Name: root 128 | Tools: 129 | - Class: rviz/MoveCamera 130 | - Class: rviz/Select 131 | - Class: rviz/FocusCamera 132 | - Class: rviz/Measure 133 | - Class: rviz/SetInitialPose 134 | Theta std deviation: 0.2617993950843811 135 | Topic: /initialpose 136 | X std deviation: 0.5 137 | Y std deviation: 0.5 138 | - Class: rviz/SetGoal 139 | Topic: /move_base_simple/goal 140 | - Class: rviz/PublishPoint 141 | Single click: true 142 | Topic: /clicked_point 143 | Value: true 144 | Views: 145 | Current: 146 | Class: rviz/ThirdPersonFollower 147 | Distance: 24.51221466064453 148 | Enable Stereo Rendering: 149 | Stereo Eye Separation: 0.05999999865889549 150 | Stereo Focal Distance: 1 151 | Swap Stereo Eyes: false 152 | Value: false 153 | Field of View: 0.7853981852531433 154 | Focal Point: 155 | X: 9.147403717041016 156 | Y: -2.7492141723632812 157 | Z: 0.38143303990364075 158 | Focal Shape Fixed Size: true 159 | Focal Shape Size: 0.05000000074505806 160 | Invert Z Axis: false 161 | Name: Current View 162 | Near Clip Distance: 0.009999999776482582 163 | Pitch: 0.44979721307754517 164 | Target Frame: world 165 | Yaw: 5.662466049194336 166 | Saved: ~ 167 | Window Geometry: 168 | Displays: 169 | collapsed: false 170 | Height: 1001 171 | Hide Left Dock: false 172 | Hide Right Dock: true 173 | QMainWindow State: 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 174 | Selection: 175 | collapsed: false 176 | Time: 177 | collapsed: false 178 | Tool Properties: 179 | collapsed: false 180 | Views: 181 | collapsed: true 182 | Width: 1848 183 | X: 72 184 | Y: 41 185 | track_image: 186 | collapsed: false 187 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodelets/resize.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Kentaro Wada. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Kentaro Wada nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include "image_proc/ResizeConfig.h" 44 | 45 | namespace image_proc 46 | { 47 | class ResizeNodelet : public nodelet::Nodelet 48 | { 49 | protected: 50 | // ROS communication 51 | std::shared_ptr nh_; 52 | std::shared_ptr pnh_; 53 | image_transport::Publisher pub_image_; 54 | ros::Publisher pub_info_; 55 | image_transport::Subscriber sub_image_; 56 | ros::Subscriber sub_info_; 57 | 58 | std::shared_ptr it_, private_it_; 59 | std::mutex connect_mutex_; 60 | 61 | // Dynamic reconfigure 62 | std::mutex config_mutex_; 63 | typedef image_proc::ResizeConfig Config; 64 | typedef dynamic_reconfigure::Server ReconfigureServer; 65 | std::shared_ptr reconfigure_server_; 66 | Config config_; 67 | cv_bridge::CvImage scaled_cv_; 68 | 69 | virtual void onInit(); 70 | void connectCb(); 71 | 72 | void imageCb(const sensor_msgs::ImageConstPtr& image_msg); 73 | void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg); 74 | 75 | void configCb(Config& config, uint32_t level); 76 | }; 77 | 78 | void ResizeNodelet::onInit() 79 | { 80 | nh_.reset(new ros::NodeHandle(getNodeHandle())); 81 | pnh_.reset(new ros::NodeHandle(getPrivateNodeHandle())); 82 | it_.reset(new image_transport::ImageTransport(*nh_)); 83 | private_it_.reset(new image_transport::ImageTransport(*pnh_)); 84 | 85 | // Set up dynamic reconfigure 86 | reconfigure_server_.reset(new ReconfigureServer(*pnh_)); 87 | ReconfigureServer::CallbackType f = 88 | std::bind(&ResizeNodelet::configCb, this, std::placeholders::_1, std::placeholders::_2); 89 | reconfigure_server_->setCallback(f); 90 | 91 | // Monitor whether anyone is subscribed to the output 92 | auto connect_cb = std::bind(&ResizeNodelet::connectCb, this); 93 | // Make sure we don't enter connectCb() between advertising and assigning to 94 | // pub_XXX 95 | std::lock_guard lock(connect_mutex_); 96 | pub_image_ = private_it_->advertise("image", 1, connect_cb, connect_cb); 97 | pub_info_ = pnh_->advertise("camera_info", 1, connect_cb, connect_cb); 98 | } 99 | 100 | // Handles (un)subscribing when clients (un)subscribe 101 | void ResizeNodelet::connectCb() 102 | { 103 | std::lock_guard lock(connect_mutex_); 104 | if (pub_image_.getNumSubscribers() == 0) 105 | { 106 | sub_image_.shutdown(); 107 | } 108 | else if (!sub_image_) 109 | { 110 | sub_image_ = it_->subscribe("image", 1, &ResizeNodelet::imageCb, this); 111 | } 112 | if (pub_info_.getNumSubscribers() == 0) 113 | { 114 | sub_info_.shutdown(); 115 | } 116 | else if (!sub_info_) 117 | { 118 | sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this); 119 | } 120 | } 121 | 122 | void ResizeNodelet::configCb(Config& config, uint32_t level) 123 | { 124 | std::lock_guard lock(config_mutex_); 125 | config_ = config; 126 | } 127 | 128 | void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg) 129 | { 130 | Config config; 131 | { 132 | std::lock_guard lock(config_mutex_); 133 | config = config_; 134 | } 135 | 136 | sensor_msgs::CameraInfoPtr dst_info_msg(new sensor_msgs::CameraInfo(*info_msg)); 137 | 138 | double scale_y; 139 | double scale_x; 140 | if (config.use_scale) 141 | { 142 | scale_y = config.scale_height; 143 | scale_x = config.scale_width; 144 | dst_info_msg->height = static_cast(info_msg->height * config.scale_height); 145 | dst_info_msg->width = static_cast(info_msg->width * config.scale_width); 146 | } 147 | else 148 | { 149 | scale_y = static_cast(config.height) / info_msg->height; 150 | scale_x = static_cast(config.width) / info_msg->width; 151 | dst_info_msg->height = config.height; 152 | dst_info_msg->width = config.width; 153 | } 154 | 155 | dst_info_msg->K[0] = dst_info_msg->K[0] * scale_x; // fx 156 | dst_info_msg->K[2] = dst_info_msg->K[2] * scale_x; // cx 157 | dst_info_msg->K[4] = dst_info_msg->K[4] * scale_y; // fy 158 | dst_info_msg->K[5] = dst_info_msg->K[5] * scale_y; // cy 159 | 160 | dst_info_msg->P[0] = dst_info_msg->P[0] * scale_x; // fx 161 | dst_info_msg->P[2] = dst_info_msg->P[2] * scale_x; // cx 162 | dst_info_msg->P[3] = dst_info_msg->P[3] * scale_x; // T 163 | dst_info_msg->P[5] = dst_info_msg->P[5] * scale_y; // fy 164 | dst_info_msg->P[6] = dst_info_msg->P[6] * scale_y; // cy 165 | 166 | dst_info_msg->roi.x_offset = static_cast(dst_info_msg->roi.x_offset * scale_x); 167 | dst_info_msg->roi.y_offset = static_cast(dst_info_msg->roi.y_offset * scale_y); 168 | dst_info_msg->roi.width = static_cast(dst_info_msg->roi.width * scale_x); 169 | dst_info_msg->roi.height = static_cast(dst_info_msg->roi.height * scale_y); 170 | 171 | pub_info_.publish(dst_info_msg); 172 | } 173 | 174 | void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg) 175 | { 176 | Config config; 177 | { 178 | std::lock_guard lock(config_mutex_); 179 | config = config_; 180 | } 181 | 182 | cv_bridge::CvImageConstPtr cv_ptr; 183 | try 184 | { 185 | cv_ptr = cv_bridge::toCvShare(image_msg); 186 | } 187 | catch (cv_bridge::Exception& e) 188 | { 189 | ROS_ERROR("cv_bridge exception: %s", e.what()); 190 | return; 191 | } 192 | 193 | if (config.use_scale) 194 | { 195 | cv::resize(cv_ptr->image, scaled_cv_.image, cv::Size(0, 0), config.scale_width, config.scale_height, 196 | config.interpolation); 197 | } 198 | else 199 | { 200 | int height = config.height == -1 ? image_msg->height : config.height; 201 | int width = config.width == -1 ? image_msg->width : config.width; 202 | cv::resize(cv_ptr->image, scaled_cv_.image, cv::Size(width, height), 0, 0, config.interpolation); 203 | } 204 | 205 | scaled_cv_.header = image_msg->header; 206 | scaled_cv_.encoding = image_msg->encoding; 207 | pub_image_.publish(scaled_cv_.toImageMsg()); 208 | } 209 | 210 | } // namespace image_proc 211 | 212 | #include 213 | PLUGINLIB_EXPORT_CLASS(image_proc::ResizeNodelet, nodelet::Nodelet) 214 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodelets/debayer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include 35 | #include 36 | #if ((BOOST_VERSION / 100) % 1000) >= 53 37 | #include 38 | #endif 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | // Until merged into OpenCV 49 | #include "edge_aware.h" 50 | 51 | #include 52 | 53 | namespace image_proc { 54 | 55 | namespace enc = sensor_msgs::image_encodings; 56 | 57 | class DebayerNodelet : public nodelet::Nodelet 58 | { 59 | // ROS communication 60 | boost::shared_ptr it_; 61 | image_transport::Subscriber sub_raw_; 62 | 63 | boost::mutex connect_mutex_; 64 | image_transport::Publisher pub_mono_; 65 | image_transport::Publisher pub_color_; 66 | 67 | // Dynamic reconfigure 68 | boost::recursive_mutex config_mutex_; 69 | typedef image_proc::DebayerConfig Config; 70 | typedef dynamic_reconfigure::Server ReconfigureServer; 71 | boost::shared_ptr reconfigure_server_; 72 | Config config_; 73 | 74 | virtual void onInit(); 75 | 76 | void connectCb(); 77 | 78 | void imageCb(const sensor_msgs::ImageConstPtr& raw_msg); 79 | 80 | void configCb(Config &config, uint32_t level); 81 | }; 82 | 83 | void DebayerNodelet::onInit() 84 | { 85 | ros::NodeHandle &nh = getNodeHandle(); 86 | ros::NodeHandle &private_nh = getPrivateNodeHandle(); 87 | it_.reset(new image_transport::ImageTransport(nh)); 88 | 89 | // Set up dynamic reconfigure 90 | reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh)); 91 | ReconfigureServer::CallbackType f = boost::bind(&DebayerNodelet::configCb, this, _1, _2); 92 | reconfigure_server_->setCallback(f); 93 | 94 | // Monitor whether anyone is subscribed to the output 95 | typedef image_transport::SubscriberStatusCallback ConnectCB; 96 | ConnectCB connect_cb = boost::bind(&DebayerNodelet::connectCb, this); 97 | // Make sure we don't enter connectCb() between advertising and assigning to pub_XXX 98 | boost::lock_guard lock(connect_mutex_); 99 | pub_mono_ = it_->advertise("image_mono", 1, connect_cb, connect_cb); 100 | pub_color_ = it_->advertise("image_color", 1, connect_cb, connect_cb); 101 | } 102 | 103 | // Handles (un)subscribing when clients (un)subscribe 104 | void DebayerNodelet::connectCb() 105 | { 106 | boost::lock_guard lock(connect_mutex_); 107 | if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) 108 | sub_raw_.shutdown(); 109 | else if (!sub_raw_) 110 | { 111 | image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); 112 | sub_raw_ = it_->subscribe("image_raw", 1, &DebayerNodelet::imageCb, this, hints); 113 | } 114 | } 115 | 116 | void DebayerNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg) 117 | { 118 | int bit_depth = enc::bitDepth(raw_msg->encoding); 119 | //@todo Fix as soon as bitDepth fixes it 120 | if (raw_msg->encoding == enc::YUV422) 121 | bit_depth = 8; 122 | 123 | // First publish to mono if needed 124 | if (pub_mono_.getNumSubscribers()) 125 | { 126 | if (enc::isMono(raw_msg->encoding)) 127 | pub_mono_.publish(raw_msg); 128 | else 129 | { 130 | if ((bit_depth != 8) && (bit_depth != 16)) 131 | { 132 | NODELET_WARN_THROTTLE(30, 133 | "Raw image data from topic '%s' has unsupported depth: %d", 134 | sub_raw_.getTopic().c_str(), bit_depth); 135 | } else { 136 | // Use cv_bridge to convert to Mono. If a type is not supported, 137 | // it will error out there 138 | sensor_msgs::ImagePtr gray_msg; 139 | try 140 | { 141 | if (bit_depth == 8) 142 | gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO8)->toImageMsg(); 143 | else 144 | gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO16)->toImageMsg(); 145 | pub_mono_.publish(gray_msg); 146 | } 147 | catch (cv_bridge::Exception &e) 148 | { 149 | NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what()); 150 | } 151 | } 152 | } 153 | } 154 | 155 | // Next, publish to color 156 | if (!pub_color_.getNumSubscribers()) 157 | return; 158 | 159 | if (enc::isMono(raw_msg->encoding)) 160 | { 161 | // For monochrome, no processing needed! 162 | pub_color_.publish(raw_msg); 163 | 164 | // Warn if the user asked for color 165 | NODELET_WARN_THROTTLE(30, 166 | "Color topic '%s' requested, but raw image data from topic '%s' is grayscale", 167 | pub_color_.getTopic().c_str(), sub_raw_.getTopic().c_str()); 168 | } 169 | else if (enc::isColor(raw_msg->encoding)) 170 | { 171 | pub_color_.publish(raw_msg); 172 | } 173 | else if (enc::isBayer(raw_msg->encoding)) { 174 | int type = bit_depth == 8 ? CV_8U : CV_16U; 175 | const cv::Mat bayer(raw_msg->height, raw_msg->width, CV_MAKETYPE(type, 1), 176 | const_cast(&raw_msg->data[0]), raw_msg->step); 177 | 178 | sensor_msgs::ImagePtr color_msg = boost::make_shared(); 179 | color_msg->header = raw_msg->header; 180 | color_msg->height = raw_msg->height; 181 | color_msg->width = raw_msg->width; 182 | color_msg->encoding = bit_depth == 8? enc::BGR8 : enc::BGR16; 183 | color_msg->step = color_msg->width * 3 * (bit_depth / 8); 184 | color_msg->data.resize(color_msg->height * color_msg->step); 185 | 186 | cv::Mat color(color_msg->height, color_msg->width, CV_MAKETYPE(type, 3), 187 | &color_msg->data[0], color_msg->step); 188 | 189 | int algorithm; 190 | { 191 | boost::lock_guard lock(config_mutex_); 192 | algorithm = config_.debayer; 193 | } 194 | 195 | if (algorithm == Debayer_EdgeAware || 196 | algorithm == Debayer_EdgeAwareWeighted) 197 | { 198 | // These algorithms are not in OpenCV yet 199 | if (raw_msg->encoding != enc::BAYER_GRBG8) 200 | { 201 | NODELET_WARN_THROTTLE(30, "Edge aware algorithms currently only support GRBG8 Bayer. " 202 | "Falling back to bilinear interpolation."); 203 | algorithm = Debayer_Bilinear; 204 | } 205 | else 206 | { 207 | if (algorithm == Debayer_EdgeAware) 208 | debayerEdgeAware(bayer, color); 209 | else 210 | debayerEdgeAwareWeighted(bayer, color); 211 | } 212 | } 213 | if (algorithm == Debayer_Bilinear || 214 | algorithm == Debayer_VNG) 215 | { 216 | int code = -1; 217 | if (raw_msg->encoding == enc::BAYER_RGGB8 || 218 | raw_msg->encoding == enc::BAYER_RGGB16) 219 | code = cv::COLOR_BayerBG2BGR; 220 | else if (raw_msg->encoding == enc::BAYER_BGGR8 || 221 | raw_msg->encoding == enc::BAYER_BGGR16) 222 | code = cv::COLOR_BayerRG2BGR; 223 | else if (raw_msg->encoding == enc::BAYER_GBRG8 || 224 | raw_msg->encoding == enc::BAYER_GBRG16) 225 | code = cv::COLOR_BayerGR2BGR; 226 | else if (raw_msg->encoding == enc::BAYER_GRBG8 || 227 | raw_msg->encoding == enc::BAYER_GRBG16) 228 | code = cv::COLOR_BayerGB2BGR; 229 | 230 | if (algorithm == Debayer_VNG) 231 | code += cv::COLOR_BayerBG2BGR_VNG - cv::COLOR_BayerBG2BGR; 232 | 233 | try 234 | { 235 | cv::cvtColor(bayer, color, code); 236 | } 237 | catch (cv::Exception &e) 238 | { 239 | NODELET_WARN_THROTTLE(30, "cvtColor error: '%s', bayer code: %d, width %d, height %d", 240 | e.what(), code, bayer.cols, bayer.rows); 241 | return; 242 | } 243 | } 244 | 245 | pub_color_.publish(color_msg); 246 | } 247 | else if (raw_msg->encoding == enc::YUV422) 248 | { 249 | // Use cv_bridge to convert to BGR8 250 | sensor_msgs::ImagePtr color_msg; 251 | try 252 | { 253 | color_msg = cv_bridge::toCvCopy(raw_msg, enc::BGR8)->toImageMsg(); 254 | pub_color_.publish(color_msg); 255 | } 256 | catch (cv_bridge::Exception &e) 257 | { 258 | NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what()); 259 | } 260 | } 261 | else if (raw_msg->encoding == enc::TYPE_8UC3) 262 | { 263 | // 8UC3 does not specify a color encoding. Is it BGR, RGB, HSV, XYZ, LUV...? 264 | NODELET_ERROR_THROTTLE(10, 265 | "Raw image topic '%s' has ambiguous encoding '8UC3'. The " 266 | "source should set the encoding to 'bgr8' or 'rgb8'.", 267 | sub_raw_.getTopic().c_str()); 268 | } 269 | else 270 | { 271 | NODELET_ERROR_THROTTLE(10, "Raw image topic '%s' has unsupported encoding '%s'", 272 | sub_raw_.getTopic().c_str(), raw_msg->encoding.c_str()); 273 | } 274 | } 275 | 276 | void DebayerNodelet::configCb(Config &config, uint32_t level) 277 | { 278 | config_ = config; 279 | } 280 | 281 | } // namespace image_proc 282 | 283 | // Register nodelet 284 | #include 285 | PLUGINLIB_EXPORT_CLASS( image_proc::DebayerNodelet, nodelet::Nodelet) 286 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodelets/crop_decimate.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include 35 | #if ((BOOST_VERSION / 100) % 1000) >= 53 36 | #include 37 | #endif 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace image_proc { 49 | 50 | using namespace cv_bridge; // CvImage, toCvShare 51 | 52 | class CropDecimateNodelet : public nodelet::Nodelet 53 | { 54 | // ROS communication 55 | boost::shared_ptr it_in_, it_out_; 56 | image_transport::CameraSubscriber sub_; 57 | int queue_size_; 58 | std::string target_frame_id_; 59 | 60 | boost::mutex connect_mutex_; 61 | image_transport::CameraPublisher pub_; 62 | 63 | // Dynamic reconfigure 64 | boost::recursive_mutex config_mutex_; 65 | typedef image_proc::CropDecimateConfig Config; 66 | typedef dynamic_reconfigure::Server ReconfigureServer; 67 | boost::shared_ptr reconfigure_server_; 68 | Config config_; 69 | 70 | virtual void onInit(); 71 | 72 | void connectCb(); 73 | 74 | void imageCb(const sensor_msgs::ImageConstPtr& image_msg, 75 | const sensor_msgs::CameraInfoConstPtr& info_msg); 76 | 77 | void configCb(Config &config, uint32_t level); 78 | }; 79 | 80 | void CropDecimateNodelet::onInit() 81 | { 82 | ros::NodeHandle& nh = getNodeHandle(); 83 | ros::NodeHandle& private_nh = getPrivateNodeHandle(); 84 | ros::NodeHandle nh_in (nh, "camera"); 85 | ros::NodeHandle nh_out(nh, "camera_out"); 86 | it_in_ .reset(new image_transport::ImageTransport(nh_in)); 87 | it_out_.reset(new image_transport::ImageTransport(nh_out)); 88 | 89 | // Read parameters 90 | private_nh.param("queue_size", queue_size_, 5); 91 | private_nh.param("target_frame_id", target_frame_id_, std::string()); 92 | 93 | // Set up dynamic reconfigure 94 | reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh)); 95 | ReconfigureServer::CallbackType f = boost::bind(&CropDecimateNodelet::configCb, this, _1, _2); 96 | reconfigure_server_->setCallback(f); 97 | 98 | // Monitor whether anyone is subscribed to the output 99 | image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropDecimateNodelet::connectCb, this); 100 | ros::SubscriberStatusCallback connect_cb_info = boost::bind(&CropDecimateNodelet::connectCb, this); 101 | // Make sure we don't enter connectCb() between advertising and assigning to pub_ 102 | boost::lock_guard lock(connect_mutex_); 103 | pub_ = it_out_->advertiseCamera("image_raw", 1, connect_cb, connect_cb, connect_cb_info, connect_cb_info); 104 | } 105 | 106 | // Handles (un)subscribing when clients (un)subscribe 107 | void CropDecimateNodelet::connectCb() 108 | { 109 | boost::lock_guard lock(connect_mutex_); 110 | if (pub_.getNumSubscribers() == 0) 111 | sub_.shutdown(); 112 | else if (!sub_) 113 | { 114 | image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); 115 | sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this, hints); 116 | } 117 | } 118 | 119 | template 120 | void debayer2x2toBGR(const cv::Mat& src, cv::Mat& dst, int R, int G1, int G2, int B) 121 | { 122 | typedef cv::Vec DstPixel; // 8- or 16-bit BGR 123 | dst.create(src.rows / 2, src.cols / 2, cv::DataType::type); 124 | 125 | int src_row_step = src.step1(); 126 | int dst_row_step = dst.step1(); 127 | const T* src_row = src.ptr(); 128 | T* dst_row = dst.ptr(); 129 | 130 | // 2x2 downsample and debayer at once 131 | for (int y = 0; y < dst.rows; ++y) 132 | { 133 | for (int x = 0; x < dst.cols; ++x) 134 | { 135 | dst_row[x*3 + 0] = src_row[x*2 + B]; 136 | dst_row[x*3 + 1] = (src_row[x*2 + G1] + src_row[x*2 + G2]) / 2; 137 | dst_row[x*3 + 2] = src_row[x*2 + R]; 138 | } 139 | src_row += src_row_step * 2; 140 | dst_row += dst_row_step; 141 | } 142 | } 143 | 144 | // Templated on pixel size, in bytes (MONO8 = 1, BGR8 = 3, RGBA16 = 8, ...) 145 | template 146 | void decimate(const cv::Mat& src, cv::Mat& dst, int decimation_x, int decimation_y) 147 | { 148 | dst.create(src.rows / decimation_y, src.cols / decimation_x, src.type()); 149 | 150 | int src_row_step = src.step[0] * decimation_y; 151 | int src_pixel_step = N * decimation_x; 152 | int dst_row_step = dst.step[0]; 153 | 154 | const uint8_t* src_row = src.ptr(); 155 | uint8_t* dst_row = dst.ptr(); 156 | 157 | for (int y = 0; y < dst.rows; ++y) 158 | { 159 | const uint8_t* src_pixel = src_row; 160 | uint8_t* dst_pixel = dst_row; 161 | for (int x = 0; x < dst.cols; ++x) 162 | { 163 | memcpy(dst_pixel, src_pixel, N); // Should inline with small, fixed N 164 | src_pixel += src_pixel_step; 165 | dst_pixel += N; 166 | } 167 | src_row += src_row_step; 168 | dst_row += dst_row_step; 169 | } 170 | } 171 | 172 | void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg, 173 | const sensor_msgs::CameraInfoConstPtr& info_msg) 174 | { 175 | /// @todo Check image dimensions match info_msg 176 | /// @todo Publish tweaks to config_ so they appear in reconfigure_gui 177 | 178 | Config config; 179 | { 180 | boost::lock_guard lock(config_mutex_); 181 | config = config_; 182 | } 183 | int decimation_x = config.decimation_x; 184 | int decimation_y = config.decimation_y; 185 | 186 | // Compute the ROI we'll actually use 187 | bool is_bayer = sensor_msgs::image_encodings::isBayer(image_msg->encoding); 188 | if (is_bayer) 189 | { 190 | // Odd offsets for Bayer images basically change the Bayer pattern, but that's 191 | // unnecessarily complicated to support 192 | config.x_offset &= ~0x1; 193 | config.y_offset &= ~0x1; 194 | config.width &= ~0x1; 195 | config.height &= ~0x1; 196 | } 197 | 198 | int max_width = image_msg->width - config.x_offset; 199 | if (max_width <= 0) 200 | { 201 | ROS_WARN_THROTTLE(30., "x offset is outside the input image width: " 202 | "%i, x offset: %i.", image_msg->width, config.x_offset); 203 | return; 204 | } 205 | int max_height = image_msg->height - config.y_offset; 206 | if (max_height <= 0) 207 | { 208 | ROS_WARN_THROTTLE(30., "y offset is outside the input image height: " 209 | "%i, y offset: %i", image_msg->height, config.y_offset); 210 | return; 211 | } 212 | int width = config.width; 213 | int height = config.height; 214 | if (width == 0 || width > max_width) 215 | width = max_width; 216 | if (height == 0 || height > max_height) 217 | height = max_height; 218 | 219 | // On no-op, just pass the messages along 220 | if (decimation_x == 1 && 221 | decimation_y == 1 && 222 | config.x_offset == 0 && 223 | config.y_offset == 0 && 224 | width == (int)image_msg->width && 225 | height == (int)image_msg->height) 226 | { 227 | pub_.publish(image_msg, info_msg); 228 | return; 229 | } 230 | 231 | // Get a cv::Mat view of the source data 232 | CvImageConstPtr source = toCvShare(image_msg); 233 | 234 | // Except in Bayer downsampling case, output has same encoding as the input 235 | CvImage output(source->header, source->encoding); 236 | // Apply ROI (no copy, still a view of the image_msg data) 237 | output.image = source->image(cv::Rect(config.x_offset, config.y_offset, width, height)); 238 | 239 | // Special case: when decimating Bayer images, we first do a 2x2 decimation to BGR 240 | if (is_bayer && (decimation_x > 1 || decimation_y > 1)) 241 | { 242 | if (decimation_x % 2 != 0 || decimation_y % 2 != 0) 243 | { 244 | NODELET_ERROR_THROTTLE(2, "Odd decimation not supported for Bayer images"); 245 | return; 246 | } 247 | 248 | cv::Mat bgr; 249 | int step = output.image.step1(); 250 | if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8) 251 | debayer2x2toBGR(output.image, bgr, 0, 1, step, step + 1); 252 | else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8) 253 | debayer2x2toBGR(output.image, bgr, step + 1, 1, step, 0); 254 | else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8) 255 | debayer2x2toBGR(output.image, bgr, step, 0, step + 1, 1); 256 | else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8) 257 | debayer2x2toBGR(output.image, bgr, 1, 0, step + 1, step); 258 | else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB16) 259 | debayer2x2toBGR(output.image, bgr, 0, 1, step, step + 1); 260 | else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR16) 261 | debayer2x2toBGR(output.image, bgr, step + 1, 1, step, 0); 262 | else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG16) 263 | debayer2x2toBGR(output.image, bgr, step, 0, step + 1, 1); 264 | else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16) 265 | debayer2x2toBGR(output.image, bgr, 1, 0, step + 1, step); 266 | else 267 | { 268 | NODELET_ERROR_THROTTLE(2, "Unrecognized Bayer encoding '%s'", image_msg->encoding.c_str()); 269 | return; 270 | } 271 | 272 | output.image = bgr; 273 | output.encoding = (bgr.depth() == CV_8U) ? sensor_msgs::image_encodings::BGR8 274 | : sensor_msgs::image_encodings::BGR16; 275 | decimation_x /= 2; 276 | decimation_y /= 2; 277 | } 278 | 279 | // Apply further downsampling, if necessary 280 | if (decimation_x > 1 || decimation_y > 1) 281 | { 282 | cv::Mat decimated; 283 | 284 | if (config.interpolation == image_proc::CropDecimate_NN) 285 | { 286 | // Use optimized method instead of OpenCV's more general NN resize 287 | int pixel_size = output.image.elemSize(); 288 | switch (pixel_size) 289 | { 290 | // Currently support up through 4-channel float 291 | case 1: 292 | decimate<1>(output.image, decimated, decimation_x, decimation_y); 293 | break; 294 | case 2: 295 | decimate<2>(output.image, decimated, decimation_x, decimation_y); 296 | break; 297 | case 3: 298 | decimate<3>(output.image, decimated, decimation_x, decimation_y); 299 | break; 300 | case 4: 301 | decimate<4>(output.image, decimated, decimation_x, decimation_y); 302 | break; 303 | case 6: 304 | decimate<6>(output.image, decimated, decimation_x, decimation_y); 305 | break; 306 | case 8: 307 | decimate<8>(output.image, decimated, decimation_x, decimation_y); 308 | break; 309 | case 12: 310 | decimate<12>(output.image, decimated, decimation_x, decimation_y); 311 | break; 312 | case 16: 313 | decimate<16>(output.image, decimated, decimation_x, decimation_y); 314 | break; 315 | default: 316 | NODELET_ERROR_THROTTLE(2, "Unsupported pixel size, %d bytes", pixel_size); 317 | return; 318 | } 319 | } 320 | else 321 | { 322 | // Linear, cubic, area, ... 323 | cv::Size size(output.image.cols / decimation_x, output.image.rows / decimation_y); 324 | cv::resize(output.image, decimated, size, 0.0, 0.0, config.interpolation); 325 | } 326 | 327 | output.image = decimated; 328 | } 329 | 330 | // Create output Image message 331 | /// @todo Could save copies by allocating this above and having output.image alias it 332 | sensor_msgs::ImagePtr out_image = output.toImageMsg(); 333 | 334 | // Create updated CameraInfo message 335 | sensor_msgs::CameraInfoPtr out_info = boost::make_shared(*info_msg); 336 | int binning_x = std::max((int)info_msg->binning_x, 1); 337 | int binning_y = std::max((int)info_msg->binning_y, 1); 338 | out_info->binning_x = binning_x * config.decimation_x; 339 | out_info->binning_y = binning_y * config.decimation_y; 340 | out_info->roi.x_offset += config.x_offset * binning_x; 341 | out_info->roi.y_offset += config.y_offset * binning_y; 342 | out_info->roi.height = height * binning_y; 343 | out_info->roi.width = width * binning_x; 344 | // If no ROI specified, leave do_rectify as-is. If ROI specified, set do_rectify = true. 345 | if (width != (int)image_msg->width || height != (int)image_msg->height) 346 | out_info->roi.do_rectify = true; 347 | 348 | if (!target_frame_id_.empty()) { 349 | out_image->header.frame_id = target_frame_id_; 350 | out_info->header.frame_id = target_frame_id_; 351 | } 352 | 353 | pub_.publish(out_image, out_info); 354 | } 355 | 356 | void CropDecimateNodelet::configCb(Config &config, uint32_t level) 357 | { 358 | config_ = config; 359 | } 360 | 361 | } // namespace image_proc 362 | 363 | // Register nodelet 364 | #include 365 | PLUGINLIB_EXPORT_CLASS( image_proc::CropDecimateNodelet, nodelet::Nodelet) 366 | -------------------------------------------------------------------------------- /thirdparty/image_proc/src/nodelets/edge_aware.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #include "edge_aware.h" 35 | 36 | #define AVG(a,b) (((int)(a) + (int)(b)) >> 1) 37 | #define AVG3(a,b,c) (((int)(a) + (int)(b) + (int)(c)) / 3) 38 | #define AVG4(a,b,c,d) (((int)(a) + (int)(b) + (int)(c) + (int)(d)) >> 2) 39 | #define WAVG4(a,b,c,d,x,y) ( ( ((int)(a) + (int)(b)) * (int)(x) + ((int)(c) + (int)(d)) * (int)(y) ) / ( 2 * ((int)(x) + (int(y))) ) ) 40 | using namespace std; 41 | 42 | namespace image_proc { 43 | 44 | void debayerEdgeAware(const cv::Mat& bayer, cv::Mat& color) 45 | { 46 | unsigned width = bayer.cols; 47 | unsigned height = bayer.rows; 48 | unsigned rgb_line_step = color.step[0]; 49 | unsigned rgb_line_skip = rgb_line_step - width * 3; 50 | int bayer_line_step = bayer.step[0]; 51 | int bayer_line_step2 = bayer_line_step * 2; 52 | 53 | unsigned char* rgb_buffer = color.data; 54 | unsigned char* bayer_pixel = bayer.data; 55 | unsigned yIdx, xIdx; 56 | 57 | int dh, dv; 58 | 59 | // first two pixel values for first two lines 60 | // Bayer 0 1 2 61 | // 0 G r g 62 | // line_step b g b 63 | // line_step2 g r g 64 | 65 | rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel 66 | rgb_buffer[1] = bayer_pixel[0]; // green pixel 67 | rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue; 68 | 69 | // Bayer 0 1 2 70 | // 0 g R g 71 | // line_step b g b 72 | // line_step2 g r g 73 | //rgb_pixel[3] = bayer_pixel[1]; 74 | rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]); 75 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 76 | 77 | // BGBG line 78 | // Bayer 0 1 2 79 | // 0 g r g 80 | // line_step B g b 81 | // line_step2 g r g 82 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 83 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]); 84 | //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step]; 85 | 86 | // pixel (1, 1) 0 1 2 87 | // 0 g r g 88 | // line_step b G b 89 | // line_step2 g r g 90 | //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] ); 91 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 92 | //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] ); 93 | 94 | rgb_buffer += 6; 95 | bayer_pixel += 2; 96 | // rest of the first two lines 97 | for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2) 98 | { 99 | // GRGR line 100 | // Bayer -1 0 1 2 101 | // 0 r G r g 102 | // line_step g b g b 103 | // line_step2 r g r g 104 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 105 | rgb_buffer[1] = bayer_pixel[0]; 106 | rgb_buffer[2] = bayer_pixel[bayer_line_step + 1]; 107 | 108 | // Bayer -1 0 1 2 109 | // 0 r g R g 110 | // line_step g b g b 111 | // line_step2 r g r g 112 | rgb_buffer[3] = bayer_pixel[1]; 113 | rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]); 114 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 115 | 116 | // BGBG line 117 | // Bayer -1 0 1 2 118 | // 0 r g r g 119 | // line_step g B g b 120 | // line_step2 r g r g 121 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 122 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 123 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 124 | 125 | // Bayer -1 0 1 2 126 | // 0 r g r g 127 | // line_step g b G b 128 | // line_step2 r g r g 129 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 130 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 131 | //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] ); 132 | } 133 | 134 | // last two pixel values for first two lines 135 | // GRGR line 136 | // Bayer -1 0 1 137 | // 0 r G r 138 | // line_step g b g 139 | // line_step2 r g r 140 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 141 | rgb_buffer[1] = bayer_pixel[0]; 142 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; 143 | 144 | // Bayer -1 0 1 145 | // 0 r g R 146 | // line_step g b g 147 | // line_step2 r g r 148 | rgb_buffer[3] = bayer_pixel[1]; 149 | rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]); 150 | //rgb_pixel[5] = bayer_pixel[line_step]; 151 | 152 | // BGBG line 153 | // Bayer -1 0 1 154 | // 0 r g r 155 | // line_step g B g 156 | // line_step2 r g r 157 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 158 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 159 | //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step]; 160 | 161 | // Bayer -1 0 1 162 | // 0 r g r 163 | // line_step g b G 164 | // line_step2 r g r 165 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 166 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 167 | //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step]; 168 | 169 | bayer_pixel += bayer_line_step + 2; 170 | rgb_buffer += rgb_line_step + 6 + rgb_line_skip; 171 | // main processing 172 | for (yIdx = 2; yIdx < height - 2; yIdx += 2) 173 | { 174 | // first two pixel values 175 | // Bayer 0 1 2 176 | // -1 b g b 177 | // 0 G r g 178 | // line_step b g b 179 | // line_step2 g r g 180 | 181 | rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel 182 | rgb_buffer[1] = bayer_pixel[0]; // green pixel 183 | rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); // blue; 184 | 185 | // Bayer 0 1 2 186 | // -1 b g b 187 | // 0 g R g 188 | // line_step b g b 189 | // line_step2 g r g 190 | //rgb_pixel[3] = bayer_pixel[1]; 191 | rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]); 192 | rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]); 193 | 194 | // BGBG line 195 | // Bayer 0 1 2 196 | // 0 g r g 197 | // line_step B g b 198 | // line_step2 g r g 199 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 200 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]); 201 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 202 | 203 | // pixel (1, 1) 0 1 2 204 | // 0 g r g 205 | // line_step b G b 206 | // line_step2 g r g 207 | //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] ); 208 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 209 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 210 | 211 | rgb_buffer += 6; 212 | bayer_pixel += 2; 213 | // continue with rest of the line 214 | for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2) 215 | { 216 | // GRGR line 217 | // Bayer -1 0 1 2 218 | // -1 g b g b 219 | // 0 r G r g 220 | // line_step g b g b 221 | // line_step2 r g r g 222 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 223 | rgb_buffer[1] = bayer_pixel[0]; 224 | rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); 225 | 226 | // Bayer -1 0 1 2 227 | // -1 g b g b 228 | // 0 r g R g 229 | // line_step g b g b 230 | // line_step2 r g r g 231 | 232 | dh = abs (bayer_pixel[0] - bayer_pixel[2]); 233 | dv = abs (bayer_pixel[-bayer_line_step + 1] - bayer_pixel[bayer_line_step + 1]); 234 | 235 | if (dh > dv) 236 | rgb_buffer[4] = AVG (bayer_pixel[-bayer_line_step + 1], bayer_pixel[bayer_line_step + 1]); 237 | else if (dv > dh) 238 | rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[2]); 239 | else 240 | rgb_buffer[4] = AVG4 (bayer_pixel[-bayer_line_step + 1], bayer_pixel[bayer_line_step + 1], bayer_pixel[0], bayer_pixel[2]); 241 | 242 | rgb_buffer[3] = bayer_pixel[1]; 243 | rgb_buffer[5] = AVG4 (bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step], bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 244 | 245 | // BGBG line 246 | // Bayer -1 0 1 2 247 | // -1 g b g b 248 | // 0 r g r g 249 | // line_step g B g b 250 | // line_step2 r g r g 251 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 252 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 253 | 254 | dv = abs (bayer_pixel[0] - bayer_pixel[bayer_line_step2]); 255 | dh = abs (bayer_pixel[bayer_line_step - 1] - bayer_pixel[bayer_line_step + 1]); 256 | 257 | if (dv > dh) 258 | rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 259 | else if (dh > dv) 260 | rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step2]); 261 | else 262 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 263 | 264 | // Bayer -1 0 1 2 265 | // -1 g b g b 266 | // 0 r g r g 267 | // line_step g b G b 268 | // line_step2 r g r g 269 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 270 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 271 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 272 | } 273 | 274 | // last two pixels of the line 275 | // last two pixel values for first two lines 276 | // GRGR line 277 | // Bayer -1 0 1 278 | // 0 r G r 279 | // line_step g b g 280 | // line_step2 r g r 281 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 282 | rgb_buffer[1] = bayer_pixel[0]; 283 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; 284 | 285 | // Bayer -1 0 1 286 | // 0 r g R 287 | // line_step g b g 288 | // line_step2 r g r 289 | rgb_buffer[3] = bayer_pixel[1]; 290 | rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]); 291 | //rgb_pixel[5] = bayer_pixel[line_step]; 292 | 293 | // BGBG line 294 | // Bayer -1 0 1 295 | // 0 r g r 296 | // line_step g B g 297 | // line_step2 r g r 298 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 299 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 300 | //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step]; 301 | 302 | // Bayer -1 0 1 303 | // 0 r g r 304 | // line_step g b G 305 | // line_step2 r g r 306 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 307 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 308 | //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step]; 309 | 310 | bayer_pixel += bayer_line_step + 2; 311 | rgb_buffer += rgb_line_step + 6 + rgb_line_skip; 312 | } 313 | 314 | //last two lines 315 | // Bayer 0 1 2 316 | // -1 b g b 317 | // 0 G r g 318 | // line_step b g b 319 | 320 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel 321 | rgb_buffer[1] = bayer_pixel[0]; // green pixel 322 | rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue; 323 | 324 | // Bayer 0 1 2 325 | // -1 b g b 326 | // 0 g R g 327 | // line_step b g b 328 | //rgb_pixel[3] = bayer_pixel[1]; 329 | rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]); 330 | rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]); 331 | 332 | // BGBG line 333 | // Bayer 0 1 2 334 | // -1 b g b 335 | // 0 g r g 336 | // line_step B g b 337 | //rgb_pixel[rgb_line_step ] = bayer_pixel[1]; 338 | rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]); 339 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 340 | 341 | // Bayer 0 1 2 342 | // -1 b g b 343 | // 0 g r g 344 | // line_step b G b 345 | //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] ); 346 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 347 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 348 | 349 | rgb_buffer += 6; 350 | bayer_pixel += 2; 351 | // rest of the last two lines 352 | for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2) 353 | { 354 | // GRGR line 355 | // Bayer -1 0 1 2 356 | // -1 g b g b 357 | // 0 r G r g 358 | // line_step g b g b 359 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 360 | rgb_buffer[1] = bayer_pixel[0]; 361 | rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); 362 | 363 | // Bayer -1 0 1 2 364 | // -1 g b g b 365 | // 0 r g R g 366 | // line_step g b g b 367 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1]; 368 | rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]); 369 | rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[-bayer_line_step + 2]); 370 | 371 | // BGBG line 372 | // Bayer -1 0 1 2 373 | // -1 g b g b 374 | // 0 r g r g 375 | // line_step g B g b 376 | rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[-1], bayer_pixel[1]); 377 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 378 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 379 | 380 | 381 | // Bayer -1 0 1 2 382 | // -1 g b g b 383 | // 0 r g r g 384 | // line_step g b G b 385 | //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1]; 386 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 387 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 388 | } 389 | 390 | // last two pixel values for first two lines 391 | // GRGR line 392 | // Bayer -1 0 1 393 | // -1 g b g 394 | // 0 r G r 395 | // line_step g b g 396 | rgb_buffer[rgb_line_step ] = rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 397 | rgb_buffer[1] = bayer_pixel[0]; 398 | rgb_buffer[5] = rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); 399 | 400 | // Bayer -1 0 1 401 | // -1 g b g 402 | // 0 r g R 403 | // line_step g b g 404 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1]; 405 | rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[-bayer_line_step + 1]); 406 | //rgb_pixel[5] = AVG( bayer_pixel[line_step], bayer_pixel[-line_step] ); 407 | 408 | // BGBG line 409 | // Bayer -1 0 1 410 | // -1 g b g 411 | // 0 r g r 412 | // line_step g B g 413 | //rgb_pixel[rgb_line_step ] = AVG2( bayer_pixel[-1], bayer_pixel[1] ); 414 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 415 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 416 | 417 | // Bayer -1 0 1 418 | // -1 g b g 419 | // 0 r g r 420 | // line_step g b G 421 | //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1]; 422 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 423 | //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step]; 424 | } 425 | 426 | void debayerEdgeAwareWeighted(const cv::Mat& bayer, cv::Mat& color) 427 | { 428 | unsigned width = bayer.cols; 429 | unsigned height = bayer.rows; 430 | unsigned rgb_line_step = color.step[0]; 431 | unsigned rgb_line_skip = rgb_line_step - width * 3; 432 | int bayer_line_step = bayer.step[0]; 433 | int bayer_line_step2 = bayer_line_step * 2; 434 | 435 | unsigned char* rgb_buffer = color.data; 436 | unsigned char* bayer_pixel = bayer.data; 437 | unsigned yIdx, xIdx; 438 | 439 | int dh, dv; 440 | 441 | // first two pixel values for first two lines 442 | // Bayer 0 1 2 443 | // 0 G r g 444 | // line_step b g b 445 | // line_step2 g r g 446 | 447 | rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel 448 | rgb_buffer[1] = bayer_pixel[0]; // green pixel 449 | rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue; 450 | 451 | // Bayer 0 1 2 452 | // 0 g R g 453 | // line_step b g b 454 | // line_step2 g r g 455 | //rgb_pixel[3] = bayer_pixel[1]; 456 | rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]); 457 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 458 | 459 | // BGBG line 460 | // Bayer 0 1 2 461 | // 0 g r g 462 | // line_step B g b 463 | // line_step2 g r g 464 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 465 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]); 466 | //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step]; 467 | 468 | // pixel (1, 1) 0 1 2 469 | // 0 g r g 470 | // line_step b G b 471 | // line_step2 g r g 472 | //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] ); 473 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 474 | //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] ); 475 | 476 | rgb_buffer += 6; 477 | bayer_pixel += 2; 478 | // rest of the first two lines 479 | for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2) 480 | { 481 | // GRGR line 482 | // Bayer -1 0 1 2 483 | // 0 r G r g 484 | // line_step g b g b 485 | // line_step2 r g r g 486 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 487 | rgb_buffer[1] = bayer_pixel[0]; 488 | rgb_buffer[2] = bayer_pixel[bayer_line_step + 1]; 489 | 490 | // Bayer -1 0 1 2 491 | // 0 r g R g 492 | // line_step g b g b 493 | // line_step2 r g r g 494 | rgb_buffer[3] = bayer_pixel[1]; 495 | rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]); 496 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 497 | 498 | // BGBG line 499 | // Bayer -1 0 1 2 500 | // 0 r g r g 501 | // line_step g B g b 502 | // line_step2 r g r g 503 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 504 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 505 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 506 | 507 | // Bayer -1 0 1 2 508 | // 0 r g r g 509 | // line_step g b G b 510 | // line_step2 r g r g 511 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 512 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 513 | //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] ); 514 | } 515 | 516 | // last two pixel values for first two lines 517 | // GRGR line 518 | // Bayer -1 0 1 519 | // 0 r G r 520 | // line_step g b g 521 | // line_step2 r g r 522 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 523 | rgb_buffer[1] = bayer_pixel[0]; 524 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; 525 | 526 | // Bayer -1 0 1 527 | // 0 r g R 528 | // line_step g b g 529 | // line_step2 r g r 530 | rgb_buffer[3] = bayer_pixel[1]; 531 | rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]); 532 | //rgb_pixel[5] = bayer_pixel[line_step]; 533 | 534 | // BGBG line 535 | // Bayer -1 0 1 536 | // 0 r g r 537 | // line_step g B g 538 | // line_step2 r g r 539 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 540 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 541 | //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step]; 542 | 543 | // Bayer -1 0 1 544 | // 0 r g r 545 | // line_step g b G 546 | // line_step2 r g r 547 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 548 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 549 | //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step]; 550 | 551 | bayer_pixel += bayer_line_step + 2; 552 | rgb_buffer += rgb_line_step + 6 + rgb_line_skip; 553 | // main processing 554 | for (yIdx = 2; yIdx < height - 2; yIdx += 2) 555 | { 556 | // first two pixel values 557 | // Bayer 0 1 2 558 | // -1 b g b 559 | // 0 G r g 560 | // line_step b g b 561 | // line_step2 g r g 562 | 563 | rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel 564 | rgb_buffer[1] = bayer_pixel[0]; // green pixel 565 | rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); // blue; 566 | 567 | // Bayer 0 1 2 568 | // -1 b g b 569 | // 0 g R g 570 | // line_step b g b 571 | // line_step2 g r g 572 | //rgb_pixel[3] = bayer_pixel[1]; 573 | rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]); 574 | rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]); 575 | 576 | // BGBG line 577 | // Bayer 0 1 2 578 | // 0 g r g 579 | // line_step B g b 580 | // line_step2 g r g 581 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 582 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]); 583 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 584 | 585 | // pixel (1, 1) 0 1 2 586 | // 0 g r g 587 | // line_step b G b 588 | // line_step2 g r g 589 | //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] ); 590 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 591 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 592 | 593 | rgb_buffer += 6; 594 | bayer_pixel += 2; 595 | // continue with rest of the line 596 | for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2) 597 | { 598 | // GRGR line 599 | // Bayer -1 0 1 2 600 | // -1 g b g b 601 | // 0 r G r g 602 | // line_step g b g b 603 | // line_step2 r g r g 604 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 605 | rgb_buffer[1] = bayer_pixel[0]; 606 | rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); 607 | 608 | // Bayer -1 0 1 2 609 | // -1 g b g b 610 | // 0 r g R g 611 | // line_step g b g b 612 | // line_step2 r g r g 613 | 614 | dh = abs (bayer_pixel[0] - bayer_pixel[2]); 615 | dv = abs (bayer_pixel[-bayer_line_step + 1] - bayer_pixel[bayer_line_step + 1]); 616 | 617 | if (dv == 0 && dh == 0) 618 | rgb_buffer[4] = AVG4 (bayer_pixel[1 - bayer_line_step], bayer_pixel[1 + bayer_line_step], bayer_pixel[0], bayer_pixel[2]); 619 | else 620 | rgb_buffer[4] = WAVG4 (bayer_pixel[1 - bayer_line_step], bayer_pixel[1 + bayer_line_step], bayer_pixel[0], bayer_pixel[2], dh, dv); 621 | rgb_buffer[3] = bayer_pixel[1]; 622 | rgb_buffer[5] = AVG4 (bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step], bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 623 | 624 | // BGBG line 625 | // Bayer -1 0 1 2 626 | // -1 g b g b 627 | // 0 r g r g 628 | // line_step g B g b 629 | // line_step2 r g r g 630 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 631 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 632 | 633 | dv = abs (bayer_pixel[0] - bayer_pixel[bayer_line_step2]); 634 | dh = abs (bayer_pixel[bayer_line_step - 1] - bayer_pixel[bayer_line_step + 1]); 635 | 636 | if (dv == 0 && dh == 0) 637 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 638 | else 639 | rgb_buffer[rgb_line_step + 1] = WAVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1], dh, dv); 640 | 641 | // Bayer -1 0 1 2 642 | // -1 g b g b 643 | // 0 r g r g 644 | // line_step g b G b 645 | // line_step2 r g r g 646 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 647 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 648 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 649 | } 650 | 651 | // last two pixels of the line 652 | // last two pixel values for first two lines 653 | // GRGR line 654 | // Bayer -1 0 1 655 | // 0 r G r 656 | // line_step g b g 657 | // line_step2 r g r 658 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 659 | rgb_buffer[1] = bayer_pixel[0]; 660 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; 661 | 662 | // Bayer -1 0 1 663 | // 0 r g R 664 | // line_step g b g 665 | // line_step2 r g r 666 | rgb_buffer[3] = bayer_pixel[1]; 667 | rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]); 668 | //rgb_pixel[5] = bayer_pixel[line_step]; 669 | 670 | // BGBG line 671 | // Bayer -1 0 1 672 | // 0 r g r 673 | // line_step g B g 674 | // line_step2 r g r 675 | rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]); 676 | rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 677 | //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step]; 678 | 679 | // Bayer -1 0 1 680 | // 0 r g r 681 | // line_step g b G 682 | // line_step2 r g r 683 | rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]); 684 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 685 | //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step]; 686 | 687 | bayer_pixel += bayer_line_step + 2; 688 | rgb_buffer += rgb_line_step + 6 + rgb_line_skip; 689 | } 690 | 691 | //last two lines 692 | // Bayer 0 1 2 693 | // -1 b g b 694 | // 0 G r g 695 | // line_step b g b 696 | 697 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel 698 | rgb_buffer[1] = bayer_pixel[0]; // green pixel 699 | rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue; 700 | 701 | // Bayer 0 1 2 702 | // -1 b g b 703 | // 0 g R g 704 | // line_step b g b 705 | //rgb_pixel[3] = bayer_pixel[1]; 706 | rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]); 707 | rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]); 708 | 709 | // BGBG line 710 | // Bayer 0 1 2 711 | // -1 b g b 712 | // 0 g r g 713 | // line_step B g b 714 | //rgb_pixel[rgb_line_step ] = bayer_pixel[1]; 715 | rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]); 716 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 717 | 718 | // Bayer 0 1 2 719 | // -1 b g b 720 | // 0 g r g 721 | // line_step b G b 722 | //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] ); 723 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 724 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 725 | 726 | rgb_buffer += 6; 727 | bayer_pixel += 2; 728 | // rest of the last two lines 729 | for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2) 730 | { 731 | // GRGR line 732 | // Bayer -1 0 1 2 733 | // -1 g b g b 734 | // 0 r G r g 735 | // line_step g b g b 736 | rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 737 | rgb_buffer[1] = bayer_pixel[0]; 738 | rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); 739 | 740 | // Bayer -1 0 1 2 741 | // -1 g b g b 742 | // 0 r g R g 743 | // line_step g b g b 744 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1]; 745 | rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]); 746 | rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[-bayer_line_step + 2]); 747 | 748 | // BGBG line 749 | // Bayer -1 0 1 2 750 | // -1 g b g b 751 | // 0 r g r g 752 | // line_step g B g b 753 | rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[-1], bayer_pixel[1]); 754 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 755 | rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 756 | 757 | 758 | // Bayer -1 0 1 2 759 | // -1 g b g b 760 | // 0 r g r g 761 | // line_step g b G b 762 | //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1]; 763 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 764 | rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]); 765 | } 766 | 767 | // last two pixel values for first two lines 768 | // GRGR line 769 | // Bayer -1 0 1 770 | // -1 g b g 771 | // 0 r G r 772 | // line_step g b g 773 | rgb_buffer[rgb_line_step ] = rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]); 774 | rgb_buffer[1] = bayer_pixel[0]; 775 | rgb_buffer[5] = rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); 776 | 777 | // Bayer -1 0 1 778 | // -1 g b g 779 | // 0 r g R 780 | // line_step g b g 781 | rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1]; 782 | rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[-bayer_line_step + 1]); 783 | //rgb_pixel[5] = AVG( bayer_pixel[line_step], bayer_pixel[-line_step] ); 784 | 785 | // BGBG line 786 | // Bayer -1 0 1 787 | // -1 g b g 788 | // 0 r g r 789 | // line_step g B g 790 | //rgb_pixel[rgb_line_step ] = AVG2( bayer_pixel[-1], bayer_pixel[1] ); 791 | rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]); 792 | rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step]; 793 | 794 | // Bayer -1 0 1 795 | // -1 g b g 796 | // 0 r g r 797 | // line_step g b G 798 | //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1]; 799 | rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1]; 800 | //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step]; 801 | 802 | } 803 | 804 | } // namespace image_proc 805 | --------------------------------------------------------------------------------