Add 'darknet_vendor/' from commit '4d609295280bf9b50cde69bb9a483a8db839ba0b'

git-subtree-dir: darknet_vendor
git-subtree-mainline: a6a4482dd123151500d3e4fbd0a4f36738d23fa3
git-subtree-split: 4d609295280bf9b50cde69bb9a483a8db839ba0b

darknet_vendor/CMakeLists.txt

@@ -0,0 +1,357 @@
+cmake_minimum_required(VERSION 3.10)
+project(darknet_vendor VERSION 0.1.0)
+
+####################
+# Build options
+####################
+
+option(DARKNET_CUDA "Build darknet with CUDA support" ON)
+option(DARKNET_OPENCV "Build darknet with OpenCV support" ON)
+
+####################
+# Download Darknet
+####################
+
+if(CMAKE_VERSION VERSION_LESS "3.11")
+  include("cmake/FetchContent.cmake")
+else()
+  include(FetchContent)
+endif()
+
+# Same version as Debian Buster
+set(darknet_git_tag "61c9d02ec461e30d55762ec7669d6a1d3c356fb2")
+
+FetchContent_Declare(darknet-download
+  GIT_REPOSITORY
+    "https://github.com/pjreddie/darknet.git"
+  GIT_TAG
+    ${darknet_git_tag}
+)
+
+FetchContent_GetProperties(darknet-download)
+if(NOT darknet-download_POPULATED)
+  FetchContent_Populate(darknet-download)
+endif()
+
+set(darknet_src_dir "${darknet-download_SOURCE_DIR}")
+
+####################
+# Classify files
+####################
+set(darknet_public_headers
+  "${CMAKE_CURRENT_BINARY_DIR}/darknet-src/include/darknet.h"
+)
+
+set(darknet_cuda_sources
+  "${darknet_src_dir}/src/activation_kernels.cu"
+  "${darknet_src_dir}/src/avgpool_layer_kernels.cu"
+  "${darknet_src_dir}/src/blas_kernels.cu"
+  "${darknet_src_dir}/src/col2im_kernels.cu"
+  "${darknet_src_dir}/src/convolutional_kernels.cu"
+  "${darknet_src_dir}/src/crop_layer_kernels.cu"
+  "${darknet_src_dir}/src/deconvolutional_kernels.cu"
+  "${darknet_src_dir}/src/dropout_layer_kernels.cu"
+  "${darknet_src_dir}/src/im2col_kernels.cu"
+  "${darknet_src_dir}/src/maxpool_layer_kernels.cu"
+)
+
+set(darknet_exec_c_sources
+  "${darknet_src_dir}/examples/art.c"
+  "${darknet_src_dir}/examples/captcha.c"
+  "${darknet_src_dir}/examples/cifar.c"
+  "${darknet_src_dir}/examples/classifier.c"
+  "${darknet_src_dir}/examples/coco.c"
+  "${darknet_src_dir}/examples/darknet.c"
+  "${darknet_src_dir}/examples/detector.c"
+  "${darknet_src_dir}/examples/go.c"
+  "${darknet_src_dir}/examples/instance-segmenter.c"
+  "${darknet_src_dir}/examples/lsd.c"
+  "${darknet_src_dir}/examples/nightmare.c"
+  "${darknet_src_dir}/examples/super.c"
+  "${darknet_src_dir}/examples/rnn.c"
+  "${darknet_src_dir}/examples/regressor.c"
+  "${darknet_src_dir}/examples/segmenter.c"
+  "${darknet_src_dir}/examples/tag.c"
+  "${darknet_src_dir}/examples/yolo.c"
+)
+
+set(darknet_lib_cpp_sources
+  "${darknet_src_dir}/src/image_opencv.cpp"
+)
+
+set(darknet_lib_c_sources
+  "${darknet_src_dir}/src/activation_layer.c"
+  "${darknet_src_dir}/src/activations.c"
+  "${darknet_src_dir}/src/avgpool_layer.c"
+  "${darknet_src_dir}/src/batchnorm_layer.c"
+  "${darknet_src_dir}/src/blas.c"
+  "${darknet_src_dir}/src/box.c"
+  "${darknet_src_dir}/src/col2im.c"
+  # "${darknet_src_dir}/src/compare.c"
+  "${darknet_src_dir}/src/connected_layer.c"
+  "${darknet_src_dir}/src/convolutional_layer.c"
+  "${darknet_src_dir}/src/cost_layer.c"
+  "${darknet_src_dir}/src/crnn_layer.c"
+  "${darknet_src_dir}/src/crop_layer.c"
+  "${darknet_src_dir}/src/cuda.c"
+  "${darknet_src_dir}/src/data.c"
+  "${darknet_src_dir}/src/deconvolutional_layer.c"
+  "${darknet_src_dir}/src/demo.c"
+  "${darknet_src_dir}/src/detection_layer.c"
+  "${darknet_src_dir}/src/dropout_layer.c"
+  "${darknet_src_dir}/src/gemm.c"
+  "${darknet_src_dir}/src/gru_layer.c"
+  "${darknet_src_dir}/src/im2col.c"
+  "${darknet_src_dir}/src/image.c"
+  "${darknet_src_dir}/src/iseg_layer.c"
+  "${darknet_src_dir}/src/l2norm_layer.c"
+  "${darknet_src_dir}/src/layer.c"
+  "${darknet_src_dir}/src/list.c"
+  "${darknet_src_dir}/src/local_layer.c"
+  "${darknet_src_dir}/src/logistic_layer.c"
+  "${darknet_src_dir}/src/lstm_layer.c"
+  "${darknet_src_dir}/src/matrix.c"
+  "${darknet_src_dir}/src/maxpool_layer.c"
+  "${darknet_src_dir}/src/network.c"
+  "${darknet_src_dir}/src/normalization_layer.c"
+  "${darknet_src_dir}/src/option_list.c"
+  "${darknet_src_dir}/src/parser.c"
+  "${darknet_src_dir}/src/region_layer.c"
+  "${darknet_src_dir}/src/reorg_layer.c"
+  "${darknet_src_dir}/src/rnn_layer.c"
+  "${darknet_src_dir}/src/route_layer.c"
+  "${darknet_src_dir}/src/shortcut_layer.c"
+  "${darknet_src_dir}/src/softmax_layer.c"
+  "${darknet_src_dir}/src/tree.c"
+  "${darknet_src_dir}/src/upsample_layer.c"
+  "${darknet_src_dir}/src/utils.c"
+  "${darknet_src_dir}/src/yolo_layer.c"
+)
+
+####################
+# Darknet Library
+####################
+# set(THREADS_PREFER_PTHREAD_FLAG True)
+find_package(Threads REQUIRED)
+
+if(DARKNET_CUDA)
+  enable_language(CUDA)
+  find_package(CUDA REQUIRED)
+
+  # Flags taken from https://arnon.dk/matching-sm-architectures-arch-and-gencode-for-various-nvidia-cards/
+  if(NOT DEFINED DARKNET_VENDOR_CUDA_FLAGS)
+    if(CUDA_VERSION VERSION_GREATER 10)
+      set(DARKNET_VENDOR_CUDA_FLAGS
+        "-arch=sm_50"
+        "-gencode=arch=compute_50,code=sm_50"
+        "-gencode=arch=compute_52,code=sm_52"
+        "-gencode=arch=compute_60,code=sm_60"
+        "-gencode=arch=compute_61,code=sm_61"
+        "-gencode=arch=compute_70,code=sm_70"
+        "-gencode=arch=compute_75,code=sm_75"
+        "-gencode=arch=compute_75,code=compute_75"
+      )
+    elseif(CUDA_VERSION VERSION_GREATER 9)
+      set(DARKNET_VENDOR_CUDA_FLAGS
+        "-arch=sm_50"
+        "-gencode=arch=compute_50,code=sm_50"
+        "-gencode=arch=compute_52,code=sm_52"
+        "-gencode=arch=compute_60,code=sm_60"
+        "-gencode=arch=compute_61,code=sm_61"
+        "-gencode=arch=compute_70,code=sm_70"
+        "-gencode=arch=compute_70,code=compute_70"
+      )
+    endif()
+  endif()
+
+  add_library(darknet SHARED ${darknet_lib_c_sources} ${darknet_lib_cpp_sources} ${darknet_cuda_sources})
+  target_compile_options(darknet PRIVATE
+    $<$<COMPILE_LANGUAGE:CUDA>:${DARKNET_VENDOR_CUDA_FLAGS}>
+  )
+  target_compile_definitions(darknet PUBLIC GPU=1)
+  target_link_libraries(darknet PUBLIC
+    cuda cudart cublas curand
+  )
+else()  # not using CUDA
+  add_library(darknet SHARED ${darknet_lib_c_sources} ${darknet_lib_cpp_sources})
+endif()
+
+set_target_properties(darknet PROPERTIES LINKER_LANGUAGE C)
+target_include_directories(darknet PUBLIC
+  $<BUILD_INTERFACE:${darknet_src_dir}/include>
+  $<BUILD_INTERFACE:${darknet_src_dir}/src>
+  $<INSTALL_INTERFACE:include>
+)
+
+if(DARKNET_OPENCV)
+  find_package(OpenCV REQUIRED)
+  target_include_directories(darknet PRIVATE ${OpenCV_INCLUDE_DIRS})
+  target_link_libraries(darknet PUBLIC ${OpenCV_LIBRARIES})
+  target_compile_definitions(darknet PUBLIC OPENCV=1)
+endif()
+
+# TODO(sloretz) pthread replacement on Windows?
+target_link_libraries(darknet PUBLIC Threads::Threads)
+if(UNIX)
+  # Need to link C math library on some platforms
+  target_link_libraries(darknet PUBLIC m)
+endif()
+
+####################
+# Darknet_vendor target
+####################
+
+# Add interface library for darknet_vendor headers
+add_library(darknet_vendor INTERFACE)
+target_include_directories(darknet_vendor INTERFACE
+  $<BUILD_INTERFACE:include>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(darknet_vendor INTERFACE darknet)
+
+configure_file("include/darknet_vendor/version.h.in" "version.h" @ONLY)
+
+####################
+# Darknet Executable
+####################
+add_executable(darknet_exec ${darknet_exec_c_sources})
+target_link_libraries(darknet_exec PUBLIC darknet)
+set_target_properties(darknet_exec PROPERTIES OUTPUT_NAME "darknet")
+
+####################
+# ProjectConfig.cmake
+####################
+
+set(INCLUDE_INSTALL_DIR "include")
+set(SHARE_CMAKE_INSTALL_DIR "share/darknet_vendor/cmake")
+set(EXEC_INSTALL_DIR "bin")
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+  "cmake/darknet_vendor-config.cmake.in"
+  "darknet_vendor-config.cmake"
+  INSTALL_DESTINATION "${SHARE_CMAKE_INSTALL_DIR}"
+  PATH_VARS "INCLUDE_INSTALL_DIR" "SHARE_CMAKE_INSTALL_DIR" "EXEC_INSTALL_DIR"
+)
+
+write_basic_package_version_file(
+  "darknet_vendor-version.cmake"
+  COMPATIBILITY SameMajorVersion)
+
+####################
+# Installation
+####################
+
+# Install darknet public headers
+install(
+  DIRECTORY
+    "${darknet_src_dir}/include/"
+  DESTINATION
+    "include"
+)
+
+# Install darknet_vendor public headers
+install(
+  DIRECTORY
+    "include/"
+  DESTINATION
+    "include"
+  FILES_MATCHING PATTERN
+    "*.h"
+)
+install(
+  FILES
+  "${CMAKE_CURRENT_BINARY_DIR}/version.h"
+  DESTINATION
+    "include/darknet_vendor"
+)
+
+# Install darknet library
+install(
+  TARGETS darknet
+  EXPORT "darknet_vendor-targets"
+  ARCHIVE DESTINATION "lib"
+  LIBRARY DESTINATION "lib"
+  RUNTIME DESTINATION "bin"
+)
+
+# Install darknet_vendor interface library
+install(
+  TARGETS darknet_vendor
+  EXPORT "darknet_vendor-targets"
+)
+
+# Install darknet demo executable
+install(TARGETS darknet_exec DESTINATION "${EXEC_INSTALL_DIR}")
+
+# Install config files and helper scripts
+install(
+  DIRECTORY
+    "${darknet_src_dir}/cfg"
+  DESTINATION
+    "share/darknet"
+)
+install(
+  DIRECTORY
+    "${darknet_src_dir}/data"
+  DESTINATION
+    "share/darknet"
+)
+install(
+  DIRECTORY
+    "${darknet_src_dir}/scripts"
+  DESTINATION
+    "share/darknet"
+)
+install(
+  DIRECTORY
+    "${darknet_src_dir}/python"
+  DESTINATION
+    "share/darknet"
+)
+
+# Add an entry to the ament index
+install(
+  FILES
+    resource/darknet_vendor
+  DESTINATION
+    "share/ament_index/resource_index/packages/"
+)
+
+# Install license files
+install(
+  FILES
+    "${darknet_src_dir}/LICENSE"
+    "${darknet_src_dir}/LICENSE.fuck"
+    "${darknet_src_dir}/LICENSE.gen"
+    "${darknet_src_dir}/LICENSE.gpl"
+    "${darknet_src_dir}/LICENSE.meta"
+    "${darknet_src_dir}/LICENSE.mit"
+    "${darknet_src_dir}/LICENSE.v1"
+  DESTINATION
+    "share/darknet"
+)
+
+# Install project-config.cmake and project-version.cmake
+install(
+  FILES
+    "${CMAKE_CURRENT_BINARY_DIR}/darknet_vendor-config.cmake"
+  DESTINATION
+    share/darknet_vendor/cmake
+)
+install(
+  FILES
+    "${CMAKE_CURRENT_BINARY_DIR}/darknet_vendor-version.cmake"
+  DESTINATION
+    share/darknet_vendor/cmake
+)
+
+# install exported targets
+install(
+  EXPORT
+    "darknet_vendor-targets"
+  NAMESPACE
+    "darknet_vendor::"
+  DESTINATION
+    "share/darknet_vendor/cmake"
+)
+

darknet_vendor/CONTRIBUTING.md

@@ -0,0 +1,18 @@
+Any contribution that you make to this repository will
+be under the Apache 2 License, as dictated by that
+[license](http://www.apache.org/licenses/LICENSE-2.0.html):
+
+~~~
+5. Submission of Contributions. Unless You explicitly state otherwise,
+   any Contribution intentionally submitted for inclusion in the Work
+   by You to the Licensor shall be under the terms and conditions of
+   this License, without any additional terms or conditions.
+   Notwithstanding the above, nothing herein shall supersede or modify
+   the terms of any separate license agreement you may have executed
+   with Licensor regarding such Contributions.
+~~~
+
+Contributors must sign-off each commit by adding a `Signed-off-by: ...`
+line to commit messages to certify that they have the right to submit
+the code they are contributing to the project according to the
+[Developer Certificate of Origin (DCO)](https://developercertificate.org/).

darknet_vendor/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

darknet_vendor/README.md

@@ -0,0 +1,68 @@
+# darknet_vendor
+
+This is a CMake wrapper around [darknet](https://pjreddie.com/darknet), an open source neural network framework.
+
+# Building
+
+```
+cd darknet_vendor/
+mkdir build/
+cd build/
+cmake -DCMAKE_BUILD_TYPE=Release ..
+make
+```
+
+The package offers two CMake options to control how it is built.
+
+* `DARKNET_CUDA` (default: `On`) - Whether or not to build with CUDA support
+* `DARKNET_OPENCV` (default: `On`) - Whether or not to build with OpenCV support
+
+These can be disabled prior to compiling if you don't need either of those dependencies.
+
+```
+cmake -DDARKNET_CUDA=Off -DDARKNET_OPENCV=Off ..
+```
+
+# ROS 2 and Colcon
+
+The package includes `package.xml` and should build fine using ROS 2 Dashing.
+
+```
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select darknet_vendor
+```
+
+# Using darknet
+
+If you are using `ament_cmake`, use `ament_target_dependencies()` as normal.
+
+```CMake
+
+find_package(darknet_vendor REQUIRED)
+
+#...
+
+ament_target_dependencies(my_library_or_executable_target
+  darknet_vendor)
+```
+
+Otherwise, use the exported target `darknet_vendor::darknet_vendor`
+
+```CMake
+find_package(darknet_vendor REQUIRED)
+
+#...
+
+target_link_libraries(my_library_or_executable_target PUBLIC darknet_vendor::darknet_vendor)
+```
+
+Then include the `darknet.h` header in your project to make darknet functions available.
+
+```C
+#include <darknet.h>
+```
+
+Alternatively, include `darknet_vendor/darknet_vendor.h` to get preprocessor definitions with info about the installed `darknet_vendor` version.
+
+```C
+#include <darknet_vendor/darknet_vendor.h>
+```

darknet_vendor/cmake/FetchContent/CMakeLists.cmake.in

@@ -0,0 +1,24 @@
+# Distributed under the OSI-approved BSD 3-Clause License.  See accompanying
+# file Copyright.txt or https://cmake.org/licensing for details.
+
+cmake_minimum_required(VERSION ${CMAKE_VERSION})
+
+# We name the project and the target for the ExternalProject_Add() call
+# to something that will highlight to the user what we are working on if
+# something goes wrong and an error message is produced.
+
+project(${contentName}-populate NONE)
+
+include(ExternalProject)
+ExternalProject_Add(${contentName}-populate
+                    ${ARG_EXTRA}
+                    SOURCE_DIR          "${ARG_SOURCE_DIR}"
+                    BINARY_DIR          "${ARG_BINARY_DIR}"
+                    CONFIGURE_COMMAND   ""
+                    BUILD_COMMAND       ""
+                    INSTALL_COMMAND     ""
+                    TEST_COMMAND        ""
+                    USES_TERMINAL_DOWNLOAD  YES
+                    USES_TERMINAL_UPDATE    YES
+)
+

darknet_vendor/cmake/LICENSE.FetchContent

@@ -0,0 +1,132 @@
+CMake - Cross Platform Makefile Generator
+Copyright 2000-2019 Kitware, Inc. and Contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+* Redistributions of source code must retain the above copyright
+  notice, this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Kitware, Inc. nor the names of Contributors
+  may be used to endorse or promote products derived from this
+  software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+------------------------------------------------------------------------------
+
+The following individuals and institutions are among the Contributors:
+
+* Aaron C. Meadows <cmake@shadowguarddev.com>
+* Adriaan de Groot <groot@kde.org>
+* Aleksey Avdeev <solo@altlinux.ru>
+* Alexander Neundorf <neundorf@kde.org>
+* Alexander Smorkalov <alexander.smorkalov@itseez.com>
+* Alexey Sokolov <sokolov@google.com>
+* Alex Merry <alex.merry@kde.org>
+* Alex Turbov <i.zaufi@gmail.com>
+* Andreas Pakulat <apaku@gmx.de>
+* Andreas Schneider <asn@cryptomilk.org>
+* André Rigland Brodtkorb <Andre.Brodtkorb@ifi.uio.no>
+* Axel Huebl, Helmholtz-Zentrum Dresden - Rossendorf
+* Benjamin Eikel
+* Bjoern Ricks <bjoern.ricks@gmail.com>
+* Brad Hards <bradh@kde.org>
+* Christopher Harvey
+* Christoph Grüninger <foss@grueninger.de>
+* Clement Creusot <creusot@cs.york.ac.uk>
+* Daniel Blezek <blezek@gmail.com>
+* Daniel Pfeifer <daniel@pfeifer-mail.de>
+* Enrico Scholz <enrico.scholz@informatik.tu-chemnitz.de>
+* Eran Ifrah <eran.ifrah@gmail.com>
+* Esben Mose Hansen, Ange Optimization ApS
+* Geoffrey Viola <geoffrey.viola@asirobots.com>
+* Google Inc
+* Gregor Jasny
+* Helio Chissini de Castro <helio@kde.org>
+* Ilya Lavrenov <ilya.lavrenov@itseez.com>
+* Insight Software Consortium <insightsoftwareconsortium.org>
+* Jan Woetzel
+* Julien Schueller
+* Kelly Thompson <kgt@lanl.gov>
+* Laurent Montel <montel@kde.org>
+* Konstantin Podsvirov <konstantin@podsvirov.pro>
+* Mario Bensi <mbensi@ipsquad.net>
+* Martin Gräßlin <mgraesslin@kde.org>
+* Mathieu Malaterre <mathieu.malaterre@gmail.com>
+* Matthaeus G. Chajdas
+* Matthias Kretz <kretz@kde.org>
+* Matthias Maennich <matthias@maennich.net>
+* Michael Hirsch, Ph.D. <www.scivision.co>
+* Michael Stürmer
+* Miguel A. Figueroa-Villanueva
+* Mike Jackson
+* Mike McQuaid <mike@mikemcquaid.com>
+* Nicolas Bock <nicolasbock@gmail.com>
+* Nicolas Despres <nicolas.despres@gmail.com>
+* Nikita Krupen'ko <krnekit@gmail.com>
+* NVIDIA Corporation <www.nvidia.com>
+* OpenGamma Ltd. <opengamma.com>
+* Patrick Stotko <stotko@cs.uni-bonn.de>
+* Per Øyvind Karlsen <peroyvind@mandriva.org>
+* Peter Collingbourne <peter@pcc.me.uk>
+* Petr Gotthard <gotthard@honeywell.com>
+* Philip Lowman <philip@yhbt.com>
+* Philippe Proulx <pproulx@efficios.com>
+* Raffi Enficiaud, Max Planck Society
+* Raumfeld <raumfeld.com>
+* Roger Leigh <rleigh@codelibre.net>
+* Rolf Eike Beer <eike@sf-mail.de>
+* Roman Donchenko <roman.donchenko@itseez.com>
+* Roman Kharitonov <roman.kharitonov@itseez.com>
+* Ruslan Baratov
+* Sebastian Holtermann <sebholt@xwmw.org>
+* Stephen Kelly <steveire@gmail.com>
+* Sylvain Joubert <joubert.sy@gmail.com>
+* Thomas Sondergaard <ts@medical-insight.com>
+* Tobias Hunger <tobias.hunger@qt.io>
+* Todd Gamblin <tgamblin@llnl.gov>
+* Tristan Carel
+* University of Dundee
+* Vadim Zhukov
+* Will Dicharry <wdicharry@stellarscience.com>
+
+See version control history for details of individual contributions.
+
+The above copyright and license notice applies to distributions of
+CMake in source and binary form.  Third-party software packages supplied
+with CMake under compatible licenses provide their own copyright notices
+documented in corresponding subdirectories or source files.
+
+------------------------------------------------------------------------------
+
+CMake was initially developed by Kitware with the following sponsorship:
+
+ * National Library of Medicine at the National Institutes of Health
+   as part of the Insight Segmentation and Registration Toolkit (ITK).
+
+ * US National Labs (Los Alamos, Livermore, Sandia) ASC Parallel
+   Visualization Initiative.
+
+ * National Alliance for Medical Image Computing (NAMIC) is funded by the
+   National Institutes of Health through the NIH Roadmap for Medical Research,
+   Grant U54 EB005149.
+
+ * Kitware, Inc.
+

darknet_vendor/cmake/darknet_vendor-config.cmake.in

@@ -0,0 +1,20 @@
+@PACKAGE_INIT@
+
+find_package(Threads)
+
+if(@DARKNET_OPENCV@)
+  find_package(OpenCV REQUIRED)
+endif()
+
+# Export targets for modern-style CMake code
+if(NOT TARGET darknet_vendor::darknet_vendor)
+  include("@PACKAGE_SHARE_CMAKE_INSTALL_DIR@/darknet_vendor-targets.cmake")
+endif()
+
+# Set standard variables for old-style CMake code
+set_and_check(darknet_vendor_INCLUDE_DIRS "@PACKAGE_INCLUDE_INSTALL_DIR@")
+set(darknet_vendor_LIBRARIES "darknet_vendor::darknet_vendor")
+set_and_check(darknet_vendor_EXECUTABLE "@PACKAGE_EXEC_INSTALL_DIR@/darknet")
+set(darknet_vendor_VERSION_MAJOR "@PROJECT_VERSION_MAJOR@")
+set(darknet_vendor_VERSION_MINOR "@PROJECT_VERSION_MINOR@")
+set(darknet_vendor_VERSION_PATCH "@PROJECT_VERSION_PATCH@")

darknet_vendor/include/darknet_vendor/darknet_vendor.h

@@ -0,0 +1,22 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DARKNET_VENDOR__DARKNET_VENDOR_H_
+#define DARKNET_VENDOR__DARKNET_VENDOR_H_
+
+#include <darknet.h>
+#include <darknet_vendor/version.h>
+
+#endif  // DARKNET_VENDOR__DARKNET_VENDOR_H_
+

darknet_vendor/include/darknet_vendor/version.h.in

@@ -0,0 +1,24 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DARKNET_VENDOR__DARKNET_VERSION_H_
+#define DARKNET_VENDOR__DARKNET_VERSION_H_
+
+#define DARKNET_VENDOR_MAJOR_VERSION @PROJECT_VERSION_MAJOR@
+#define DARKNET_VENDOR_MINOR_VERSION @PROJECT_VERSION_MINOR@
+#define DARKNET_VENDOR_PATCH_VERSION @PROJECT_VERSION_PATCH@
+
+#define DARKNET_GIT_TAG "@darknet_git_tag@"
+
+#endif  // DARKNET_VENDOR__DARKNET_VERSION_H_

darknet_vendor/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>darknet_vendor</name>
+  <version>0.1.0</version>
+  <description>CMake wrapper around darknet, an open source neural network framework.</description>
+
+  <author email="sloretz@openrobotics.org">Shane Loretz</author>
+  <maintainer email="sloretz@openrobotics.org">Shane Loretz</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>cmake</buildtool_depend>
+  <buildtool_depend>git</buildtool_depend>
+
+  <depend>libopencv-dev</depend>
+
+  <!-- nvidia-cuda-dev is just the libraries to link against -->
+  <exec_depend>nvidia-cuda-dev</exec_depend>
+
+  <!-- nvidia-cuda includes nvidia-cuda-dev plus the nvcc compiler -->
+  <build_depend>nvidia-cuda</build_depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+</package>

ros2_ws/src/convert_2_img/convert_2_img/convert_to_img.py

@@ -10,8 +10,8 @@ import threading
 class ImageSaver(Node):
     def __init__(self):
         super().__init__('image_saver')
-        self.depth_sub = self.create_subscription(Image, '/zed2i/zed_node/depth/depth_registered', self.depth_callback, 10)
-        self.image_sub = self.create_subscription(Image, '/zed2i/zed_node/left_raw/image_raw_color', self.image_callback, 10)
+        self.depth_sub = self.create_subscription(Image, '/camera/aligned_depth_to_color/image_raw', self.depth_callback, 1)
+        self.image_sub = self.create_subscription(Image, '/camera/color/image_raw', self.image_callback, 1)
         self.cv_bridge = CvBridge()
         self.depth_image = None
         self.color_image = None
@@ -20,12 +20,13 @@ class ImageSaver(Node):
 
     def depth_callback(self, data):
         print("depth cb")
-        self.depth_image = self.cv_bridge.imgmsg_to_cv2(data)
+        self.depth_image = self.cv_bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")
         self.save_images('d')
 
 
     def image_callback(self, data):
         print("image cb")
+        
         self.color_image = self.cv_bridge.imgmsg_to_cv2(data)
         self.save_images('s')
 
@@ -51,7 +52,7 @@ class ImageSaver(Node):
 if __name__ == '__main__':
     rclpy.init()
     image_saver = ImageSaver()
-    rate = image_saver.create_rate(1)  # 10 Hz
+    # rate = image_saver.create_rate(1)  # 10 Hz
     rclpy.spin(image_saver)
     # while rclpy.ok():
     #     image_saver.save_images()

ros2_ws/src/find-pose/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.8)
 project(find-pose)
 
-
+add_compile_options(-g)
 # find dependencies
 find_package(ament_cmake REQUIRED)
 
@@ -59,7 +59,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(pcl_msgs REQUIRED)
 
 add_executable(convert_pointcloud_to_depth src/convert_pointcloud_to_depth.cpp)
-ament_target_dependencies(convert_pointcloud_to_depth ${Libraries} rclcpp_components rclcpp sensor_msgs cv_bridge)
+ament_target_dependencies(convert_pointcloud_to_depth ${Libraries} rclcpp_components rclcpp sensor_msgs cv_bridge pcl_conversions)
 target_link_libraries(convert_pointcloud_to_depth ${PCL_LBRARIES} ${Boost_LIBRARIES})
 
 include_directories(

ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h

@@ -53,7 +53,7 @@ public:
 
 // public Q_SLOTS:
 // 	void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
-    void estimate_pose();
+    void estimate_pose(float objecWidth, float objectHeight, cv::Point2f point_1, cv::Point2f point_2, cv::Point2f point_3, cv::Point2f point_4);
 private:
 	cv::Vec3f getDepth(const cv::Mat & depthImage,
 					   int x, int y,
@@ -69,6 +69,10 @@ private:
 	std::string objFramePrefix_;
 	bool usePnP_;
 	std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+	unsigned int num_objects_;
+	std::vector<double> objectHeight_;
+	std::vector<double> objectWidth_;
+
 
 };
 

ros2_ws/src/find-pose/src/FindObjectROS.cpp

@@ -63,20 +63,28 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) :
 	// pubInfo_ = node->create_publisher<find_object_2d::msg::DetectionInfo>("info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
 
 	// this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
+        num_objects_ = 2;
+        objectHeight_.resize(num_objects_);
 }
 
-void FindObjectROS::estimate_pose()
+void FindObjectROS::estimate_pose(float objectWidth, float objectHeight, cv::Point2f point_1, cv::Point2f point_2, cv::Point2f point_3, cv::Point2f point_4)
 {
     // Read RGB Image
-    cv::Mat rgb_image = cv::imread("/media/psf/freelancing/greenhouse/yolov3/runs/detect/exp8/stereo_image952.jpeg");
+    cv::Mat rgb_image = cv::imread("/media/psf/freelancing/greenhouse/stereo/stereo_5.jpg");
     // read depth Image
-    cv::Mat depth = cv::imread("/media/psf/freelancing/greenhouse/depth_try1/depth_img_952.jpeg");
+    cv::Mat depth_img = cv::imread("/media/psf/freelancing/greenhouse/depth/depth_4.jpg", cv::IMREAD_ANYDEPTH);
     // bounding box dimensions
+    // Convert the image to a single-channel 16-bit image
+    cv::Mat depth;
+    depth_img.convertTo(depth, CV_16UC1);
 
     std::vector<geometry_msgs::msg::TransformStamped> transforms;
     // store bounding boxes. (This should come from yolo ideally)
-    float objectWidth = 372; // width of bounding box in pixels
-    float objectHeight = 554; // height of bounding box in pixels
+//     float objectWidth = 160; // width of bounding box in pixels
+//     float objectHeight = 168; // height of bounding box in pixels
+//     float objectWidth = 155; // width of bounding box in pixels
+//     float objectHeight = 116; // height of bounding box in pixels
+
 
     geometry_msgs::msg::TransformStamped transform;
     transform.transform.rotation.x=0;
@@ -90,50 +98,30 @@ void FindObjectROS::estimate_pose()
     transform.header.frame_id =  "camera_link";
     // transform.header.stamp.sec = ros::Time::now();
     tf2::Quaternion q;
-    float depthConstant = 1.0f/641.587158203125;
-    std::vector<cv::Point3f> objectPoints(6);
-    std::vector<cv::Point2f> imagePoints(6);
-    cv::Vec3f point_1 = this->getDepth(depth,
-            246,665,
-            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-            1.0f/depthConstant, 1.0f/depthConstant);
-    cv::Vec3f point_2 = this->getDepth(depth,
-            618, 665,
-            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-            1.0f/depthConstant, 1.0f/depthConstant);
-    cv::Vec3f point_3 = this->getDepth(depth,
-            618, 111,
-            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-            1.0f/depthConstant, 1.0f/depthConstant);
-    cv::Vec3f point_4 = this->getDepth(depth,
-            246, 111,
-            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-            1.0f/depthConstant, 1.0f/depthConstant);
-    cv::Vec3f point_5 = this->getDepth(depth,
-            432, 386,
-            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-            1.0f/depthConstant, 1.0f/depthConstant);
-    cv::Vec3f point_6 = this->getDepth(depth,
-            537,224,
-            float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-            1.0f/depthConstant, 1.0f/depthConstant);
-    objectPoints[0] = cv::Point3f(point_1.val[0], point_1.val[1], point_1.val[2]);
-    objectPoints[1] = cv::Point3f(point_2.val[0], point_2.val[1], point_2.val[2]);
-    objectPoints[2] = cv::Point3f(point_3.val[0], point_3.val[1], point_3.val[2]);
-    objectPoints[3] = cv::Point3f(point_4.val[0], point_4.val[1], point_4.val[2]);
-    objectPoints[4] = cv::Point3f(point_5.val[0], point_5.val[1], point_5.val[2]);
-    objectPoints[5] = cv::Point3f(point_6.val[0], point_6.val[1], point_6.val[2]);
-
+    float depthConstant = 1.0f/640.8887939453125; // 0.00156033 // 1.0f/cameraInfoMsg->K[4];
+    std::cout << "depthconstant: " << depthConstant << std::endl;
+    std::vector<cv::Point3f> objectPoints(4);
+    std::vector<cv::Point2f> imagePoints(4);
+    objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0);
+    objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0);
+    objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0);
+    objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0);
     // QPointF pt = iter->map(QPointF(0, 0));
-    imagePoints[0] = cv::Point2f(246,665);
+//     imagePoints[0] = cv::Point2f(350,548);
+//     // pt = iter->map(QPointF(objectWidth, 0));
+//     imagePoints[1] = cv::Point2f(510,548);
+//     // pt = iter->map(QPointF(objectWidth, objectHeight));
+//     imagePoints[2] = cv::Point2f(510,716);
+//     // pt = iter->map(QPointF(0, objectHeight));
+//     imagePoints[3] = cv::Point2f(350,716);
+       // QPointF pt = iter->map(QPointF(0, 0));
+    imagePoints[0] = point_1;
     // pt = iter->map(QPointF(objectWidth, 0));
-    imagePoints[1] = cv::Point2f(618, 665);
+    imagePoints[1] = point_2;
     // pt = iter->map(QPointF(objectWidth, objectHeight));
-    imagePoints[2] = cv::Point2f(618,111);
+    imagePoints[2] = point_3;
     // pt = iter->map(QPointF(0, objectHeight));
-    imagePoints[3] = cv::Point2f(246, 111);
-    imagePoints[4] = cv::Point2f(432, 386);
-    imagePoints[5] = cv::Point2f(537, 224);
+    imagePoints[3] = point_4;
 
 
     cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
@@ -142,7 +130,6 @@ void FindObjectROS::estimate_pose()
     cameraMatrix.at<double>(0,2) = float(depth.cols/2)-0.5f;
     cameraMatrix.at<double>(1,2) = float(depth.rows/2)-0.5f;
     cv::Mat distCoeffs;
-    std::cout << "depth img cols: " << depth.cols << " rows: " << depth.rows << std::endl;
     cv::Mat rvec(1,3, CV_64FC1);
     cv::Mat tvec(1,3, CV_64FC1);
     cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
@@ -156,12 +143,15 @@ void FindObjectROS::estimate_pose()
             R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
             R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2));
     rotationMatrix.getRotation(q);
-
-    QPointF center = QPointF(objectWidth/2, objectHeight/2);
+    
+//     QPointF center = QPointF(imagePoints[0].x + objectWidth/2, imagePoints[0].y + objectHeight/2);
+    QPointF center = QPointF(442.252, 609.895);
     cv::Vec3f center3D = this->getDepth(depth,
             center.x()+0.5f, center.y()+0.5f,
             float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
             1.0f/depthConstant, 1.0f/depthConstant);
+    std::cout << "center x: " << center.x() << " y: " << center.y() << std::endl;
+    std::cout << "center 3D x: " << center3D[0] << " y: " << center3D[1] <<" z: " << center3D[1] << std::endl;
     
     transform.transform.translation.x = tvec.at<double>(0)*(center3D.val[2]/tvec.at<double>(2));
     transform.transform.translation.y = tvec.at<double>(1)*(center3D.val[2]/tvec.at<double>(2));
@@ -173,10 +163,11 @@ void FindObjectROS::estimate_pose()
     q *= q2;
     transform.transform.rotation = tf2::toMsg(q.normalized());
     transforms.push_back(transform);
-    if(transforms.size())
-    {
-        tfBroadcaster_->sendTransform(transforms);
-    }
+//     if(transforms.size())
+//     {
+//         tfBroadcaster_->sendTransform(transforms);
+//     }
+        std::cout << "Done!!!!!!!!!!!!" << std::endl;
 }
 
 cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
@@ -199,24 +190,24 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
 	cv::Vec3f pt;
 
 	// Use correct principal point from calibration
-	float center_x = cx; //cameraInfo.K.at(2)
-	float center_y = cy; //cameraInfo.K.at(5)
+	float center_x = cx; //cameraInfo.K.at(2) // 656.1123046875
+	float center_y = cy; //cameraInfo.K.at(5) // 361.80828857421875
 
-    // std::cout << "cx: " << center_x << " cy: " << center_y << std::endl;
-    // std::cout << "fx: " << fx << " fy: " << fy << std::endl;
+        std::cout << "cx: " << center_x << " cy: " << center_y << std::endl;
+        std::cout << "fx: " << fx << " fy: " << fy << std::endl;
 	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
 
 	// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
 	float unit_scaling = isInMM?0.001f:1.0f;
-	float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
-	float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
+	float constant_x = unit_scaling / fx; //cameraInfo.K.at(0) // 642.437744140625
+	float constant_y = unit_scaling / fy; //cameraInfo.K.at(4) // 640.8887939453125
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();
 
 	float depth;
 	bool isValid;
 	if(isInMM)
 	{
-        // std::cout << "depth is in mm!!" << std::endl;
+        std::cout << "depth is in mm!!" << std::endl;
 		depth = (float)depthImage.at<uint16_t>(y,x);
 		isValid = depth != 0.0f;
 	}
@@ -225,14 +216,14 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
         
 		depth = depthImage.at<float>(y,x);
 		isValid = std::isfinite(depth) && depth > 0.0f;
-        // std::cout << "depth is NOT in mm!! " << depth << std::endl;
+        std::cout << "depth is NOT in mm!! " << depth << std::endl;
 	}
 
 	// Check for invalid measurements
 	if (!isValid)
 	{
 		pt.val[0] = pt.val[1] = pt.val[2] = bad_point;
-                // std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl;
+                std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl;
 	}
 	else
 	{

ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp

@@ -11,19 +11,116 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/search/impl/search.hpp>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/common/transforms.h>
+#include <iostream>
 
 int count = 0;
 class PointCloudToDepthImageNode : public rclcpp::Node {
 public:
     PointCloudToDepthImageNode() : Node("pointcloud_to_depth_image") {
-         sub_rgb_ = create_subscription<sensor_msgs::msg::Image>(
-        "/camera/color/image_raw", 10, std::bind(&PointCloudToDepthImageNode::rgb_callback, this, std::placeholders::_1));
+        //  sub_rgb_ = create_subscription<sensor_msgs::msg::Image>(
+        // "/camera/color/image_raw", 10, std::bind(&PointCloudToDepthImageNode::rgb_callback, this, std::placeholders::_1));
         subscription_ = create_subscription<sensor_msgs::msg::PointCloud2>(
-      "/camera/depth/color/points", 10, std::bind(&PointCloudToDepthImageNode::pc_callback, this, std::placeholders::_1));
+      "/camera/depth/color/points", 10, std::bind(&PointCloudToDepthImageNode::convert_unorganized_to_organized_pc, this, std::placeholders::_1));
         publisher_ = create_publisher<sensor_msgs::msg::Image>("/depth_image_topic", rclcpp::QoS(10));
     }
 
 private:
+    void convert_unorganized_to_organized_pc(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
+    {
+        // Define lidar field of view parameters
+         double h_fov_max = 87.0; // Maximum horizontal angle in radians
+         double h_fov_min = 0; // Minimum horizontal angle in radians
+         double v_fov_max = 58.0; // Maximum vertical angle in radians
+         double v_fov_min = 0; // Minimum vertical angle in radians
+
+        double width = 1280;
+        double height = 720;
+        float hfov_deg = 87.0;
+        float vfov_deg = 58.0;
+        double pi = 3.14159265359;
+        float hfov_rad = hfov_deg * (pi / 180);
+        float vfov_rad = vfov_deg * (pi / 180);
+        float h_angle_per_pixel = hfov_rad / 1280; // calculate the angle per pixel
+        // h_fov_max = (1280 / 2) / tan(h_angle_per_pixel / 2); // calculate the maximum range
+        float v_angle_per_pixel = vfov_rad / 720; // calculate the angle per pixel
+        // v_fov_max = (720 / 2) / tan(v_angle_per_pixel / 2); // calculate the maximum range
+        // Calculate angular resolutions
+        const double h_resolution = (h_fov_max - h_fov_min) / width;
+        const double v_resolution = (v_fov_max - v_fov_min) / height;
+        std::cout << "convert_unorganized_to_organized_pc : " << h_fov_max << " v: " << v_fov_max << std::endl;
+        // Load the unorganized point cloud from a PCD file
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+        pcl::moveFromROSMsg(*msg, *cloud);
+
+        // Remove NaN points
+        // std::vector<int> indices;
+        // pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
+
+        // Initialize the organized point cloud
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr organized_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+        organized_cloud->width = 1280;
+        organized_cloud->height = 720;
+        organized_cloud->is_dense = true;
+        organized_cloud->points.resize(organized_cloud->width * organized_cloud->height);
+        std::cout << "cloud width: " << cloud->width << " height: " << cloud->height << std::endl;
+        // pcl::PassThrough<pcl::PointXYZRGB> pass;
+        // pass.setInputCloud(cloud);
+        // pass.setFilterFieldName("x");
+        // pass.setFilterLimitsNegative(false);
+        // pass.setFilterLimits(-tan(hfov_rad / 2.0), tan(hfov_rad / 2.0));
+        // pass.filter(*cloud);
+        // pass.setInputCloud(cloud);
+        // pass.setFilterFieldName("y");
+        // pass.setFilterLimitsNegative(false);
+        // pass.setFilterLimits(-tan(vfov_rad / 2.0), tan(vfov_rad / 2.0));
+        // pass.filter(*cloud);
+        // Copy the points from the unorganized point cloud to the organized point cloud
+        // for (int i = 0; i < cloud->points.size(); ++i)
+        // {
+        //     int row = i / cloud->width;
+        //     int col = i % cloud->width;
+        //     organized_cloud->at(col, row) = cloud->points[i];
+        // }
+        // for (int i = 0; i < cloud->points.size(); ++i)
+        // {
+        //     pcl::PointXYZRGB point = cloud->points[i];
+            
+        //     // std::cout << point.x << std::endl;
+        //     int x = (point.x + tan(hfov_rad / 2.0)) / (tan(hfov_rad) / organized_cloud->width);
+        //     int y = (point.y + tan(vfov_rad / 2.0)) / (tan(vfov_rad) / organized_cloud->height);
+        //     int index = y * organized_cloud->width + x;
+        //     std::cout << "point x: " << x << std::endl;
+        //     std::cout << "point y: " << y << std::endl;
+        //     std::cout << "point index: " << index << std::endl;
+        //     if (organized_cloud->points[index].z == 0)
+        //     {
+        //         organized_cloud->points[index] = point;
+        //     }
+        //     else if (point.z < organized_cloud->points[index].z)
+        //     {
+        //         organized_cloud->points[index] = point;
+        //     }
+        // }
+        for (int i = 0; i < cloud->size(); i++) {
+            pcl::PointXYZRGB point = cloud->points[i];
+            double h_angle = atan2(point.y, point.x);
+            double v_angle = atan2(point.z, sqrt(point.x * point.x + point.y * point.y));
+            int col = static_cast<int>((h_angle - h_fov_min) / h_resolution);
+            int row = static_cast<int>((v_angle - v_fov_min) / v_resolution);
+            // std::cout << "point col: " << col << " row: " << row << std::endl;
+            if (col >= 0 && col < organized_cloud->width && row >= 0 && row < organized_cloud->height) {
+                organized_cloud->at(col, row) = point;
+        }
+}
+
+        // Save the organized point cloud to a PCD file
+        // pcl::io::savePCDFileASCII("organized_cloud.pcd", *organized_cloud);
+
+    }
     // void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
     //     // Create a point cloud object from the message
   

ros2_ws/src/find-pose/src/find_pose_node.cpp

@@ -35,7 +35,8 @@ class FindPoseNode : public rclcpp::Node {
 		findObjectROS_(0)
 	{
         findObjectROS_ = new FindObjectROS(this);
-        findObjectROS_->estimate_pose();
+        findObjectROS_->estimate_pose(115, 116, cv::Point2f(819,600), cv::Point2f(974,600), cv::Point2f(974,716), cv::Point2f(819,716));
+        findObjectROS_->estimate_pose(160, 168, cv::Point2f(350,548), cv::Point2f(510,548), cv::Point2f(510,716), cv::Point2f(350,716));
     }
 
     private: