From 98006fb8580d50a7388011f7529edda6a0ccde85 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:08:51 +0530 Subject: [PATCH 01/12] removed irrelevant files --- Doxyfile | 22 -- cite.bib | 38 ---- cmake-configs/FindTBB.cmake | 283 ------------------------ cmake-configs/FindWhyCon.cmake | 13 -- cmake-configs/HandleLibraryTypes.cmake | 48 ----- config.h.cmake | 14 -- establish_error.rb | 28 --- include/transformer.h | 36 ---- launch/custom/vrep-set-axis.launch | 8 - launch/custom/vrep.launch | 13 -- launch/robot_pose_publisher.launch | 17 -- launch/set_axis.launch | 10 - launch/whycon.launch | 16 -- launch/whycon.rviz | 150 ------------- msg/Projection.msg | 2 - nodelets.xml | 7 - src/camera_calibrator.cpp | 111 ---------- src/main.cpp | 288 ------------------------- src/ros/robot_pose_publisher.cpp | 65 ------ src/ros/robot_pose_publisher.h | 23 -- src/ros/robot_pose_publisher_node.cpp | 12 -- src/ros/set_axis.cpp | 224 ------------------- src/ros/set_axis.h | 37 ---- src/ros/set_axis_node.cpp | 12 -- src/ros/transformer.cpp | 75 ------- src/ros/transformer_node.cpp | 11 - src/ros/triangulator.cpp | 244 --------------------- src/ros/triangulator.h | 35 --- src/ros/triangulator_node.cpp | 11 - src/ros/whycon_nodelet.cpp | 20 -- 30 files changed, 1873 deletions(-) delete mode 100644 Doxyfile delete mode 100644 cite.bib delete mode 100644 cmake-configs/FindTBB.cmake delete mode 100644 cmake-configs/FindWhyCon.cmake delete mode 100644 cmake-configs/HandleLibraryTypes.cmake delete mode 100644 config.h.cmake delete mode 100644 establish_error.rb delete mode 100644 include/transformer.h delete mode 100644 launch/custom/vrep-set-axis.launch delete mode 100644 launch/custom/vrep.launch delete mode 100644 launch/robot_pose_publisher.launch delete mode 100644 launch/set_axis.launch delete mode 100644 launch/whycon.launch delete mode 100644 launch/whycon.rviz delete mode 100644 msg/Projection.msg delete mode 100644 nodelets.xml delete mode 100644 src/camera_calibrator.cpp delete mode 100644 src/main.cpp delete mode 100644 src/ros/robot_pose_publisher.cpp delete mode 100644 src/ros/robot_pose_publisher.h delete mode 100644 src/ros/robot_pose_publisher_node.cpp delete mode 100644 src/ros/set_axis.cpp delete mode 100644 src/ros/set_axis.h delete mode 100644 src/ros/set_axis_node.cpp delete mode 100644 src/ros/transformer.cpp delete mode 100644 src/ros/transformer_node.cpp delete mode 100644 src/ros/triangulator.cpp delete mode 100644 src/ros/triangulator.h delete mode 100644 src/ros/triangulator_node.cpp delete mode 100644 src/ros/whycon_nodelet.cpp diff --git a/Doxyfile b/Doxyfile deleted file mode 100644 index 9e79ba3..0000000 --- a/Doxyfile +++ /dev/null @@ -1,22 +0,0 @@ -PROJECT_NAME=WhyCon -PROJECT_BRIEF="WhyCon localization system" -#PROJECT_LOGO= logo lrse -BUILTIN_STL_SUPPORT=YES -GENERATE_TODOLIST=YES - -#requerido por el usuario -#RECURSIVE=NO -INPUT=. -FILE_PATTERNS=*.cpp *.h README.md -USE_MDFILE_AS_MAINPAGE=README.md - -# default -EXTRACT_ALL=YES -SOURCE_BROWSER=YES -INLINE_SOURCES=YES -OUTPUT_DIRECTORY=doc # poner directorio del server (repo/doc) -GENERATE_LATEX=NO - -# opcional del usuario -MACRO_EXPANSION=YES -PREDEFINED=ENABLE_MAVCONN diff --git a/cite.bib b/cite.bib deleted file mode 100644 index 073eae9..0000000 --- a/cite.bib +++ /dev/null @@ -1,38 +0,0 @@ -#this journal article provides a thorough description of the localization method and detailed performance evaluations -@article{whycon_jint, - title={A Practical Multirobot Localization System}, - author={Krajn{\' i}k, Tom{\' a}{\v s} and Nitsche, Mat{\' i}as and Faigl, Jan and Van{\v e}k, Petr and Saska, Martin and P{\v r}eu{\v c}il, Libor and Duckett, Tom and Mejail, Marta}, - journal={Journal of Intelligent \& Robotic Systems}, - url={http://dx.doi.org/10.1007/s10846-014-0041-x}, - publisher={Springer Netherlands}, - doi={10.1007/s10846-014-0041-x}, - year={2014}, - issn={0921-0296}, -} - -#this conference article provides a brief description of the localization algorithm and describes some basic experiments -@inproceedings{whycon_icar, - title={External localization system for mobile robotics}, - author = {Krajn{\' i}k, T. and Nitsche, M. and Faigl, J. and Duckett, T. and Mejail, M. and P{\v r}eu{\v c}il, L.}, - booktitle={16th International Conference on Advanced Robotics (ICAR)}, - year={2013}, - month={Nov}, - doi={10.1109/ICAR.2013.6766520} -} - -#this extended abstract gives an overview of the WhyCon system applications -@inproceedings{whycon_workshop, - Author = {Matias Nitsche and Tom{\'a}{\v s} Krajn{\'\i}k and Petr {\v C}{\' i}{\v z}ek and Marta Mejail and Tom Duckett}, - Booktitle = {IROS Workshop on Open Source Aerial Robotics}, - Title = {WhyCon: An Efficent, Marker-based Localization System}, - Year = {2015} -} - -#this is a reference to the WhyCon localization software release on GitHub -@article{whycon_github, - author = "Nitsche, Matias and Krajn{\' i}k, Tom{\' a}{\v s}", - title = "{WhyCon: Stable release, GitHub}", - month = "May", - year = "2014", - doi = {10.5281/zenodo.10049} -} diff --git a/cmake-configs/FindTBB.cmake b/cmake-configs/FindTBB.cmake deleted file mode 100644 index f9e3e0f..0000000 --- a/cmake-configs/FindTBB.cmake +++ /dev/null @@ -1,283 +0,0 @@ -# Locate Intel Threading Building Blocks include paths and libraries -# FindTBB.cmake can be found at https://code.google.com/p/findtbb/ -# Written by Hannes Hofmann -# Improvements by Gino van den Bergen , -# Florian Uhlig , -# Jiri Marsik - -# The MIT License -# -# Copyright (c) 2011 Hannes Hofmann -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler. -# e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21" -# TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found -# in the TBB installation directory (TBB_INSTALL_DIR). -# -# GvdB: Mac OS X distribution places libraries directly in lib directory. -# -# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER. -# TBB_ARCHITECTURE [ ia32 | em64t | itanium ] -# which architecture to use -# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9 -# which compiler to use (detected automatically on Windows) - -# This module respects -# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR} - -# This module defines -# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc. -# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc -# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug -# TBB_INSTALL_DIR, the base TBB install directory -# TBB_LIBRARIES, the libraries to link against to use TBB. -# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols. -# TBB_FOUND, If false, don't try to use TBB. -# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h - - -if (WIN32) - # has em64t/vc8 em64t/vc9 - # has ia32/vc7.1 ia32/vc8 ia32/vc9 - set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB" "C:/Program Files (x86)/Intel/TBB") - set(_TBB_LIB_NAME "tbb") - set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") - set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") - set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") - if (MSVC71) - set (_TBB_COMPILER "vc7.1") - endif(MSVC71) - if (MSVC80) - set(_TBB_COMPILER "vc8") - endif(MSVC80) - if (MSVC90) - set(_TBB_COMPILER "vc9") - endif(MSVC90) - if(MSVC10) - set(_TBB_COMPILER "vc10") - endif(MSVC10) - # Todo: add other Windows compilers such as ICL. - set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) -endif (WIN32) - -if (UNIX) - if (APPLE) - # MAC - set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions") - # libs: libtbb.dylib, libtbbmalloc.dylib, *_debug - set(_TBB_LIB_NAME "tbb") - set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") - set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") - set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") - # default flavor on apple: ia32/cc4.0.1_os10.4.9 - # Jiri: There is no reason to presume there is only one flavor and - # that user's setting of variables should be ignored. - if(NOT TBB_COMPILER) - set(_TBB_COMPILER "cc4.0.1_os10.4.9") - elseif (NOT TBB_COMPILER) - set(_TBB_COMPILER ${TBB_COMPILER}) - endif(NOT TBB_COMPILER) - if(NOT TBB_ARCHITECTURE) - set(_TBB_ARCHITECTURE "ia32") - elseif(NOT TBB_ARCHITECTURE) - set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) - endif(NOT TBB_ARCHITECTURE) - else (APPLE) - # LINUX - set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include") - set(_TBB_LIB_NAME "tbb") - set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") - set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") - set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") - # has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21 - # has ia32/* - # has itanium/* - set(_TBB_COMPILER ${TBB_COMPILER}) - set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) - endif (APPLE) -endif (UNIX) - -if (CMAKE_SYSTEM MATCHES "SunOS.*") -# SUN -# not yet supported -# has em64t/cc3.4.3_kernel5.10 -# has ia32/* -endif (CMAKE_SYSTEM MATCHES "SunOS.*") - - -#-- Clear the public variables -set (TBB_FOUND "NO") - - -#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR} -# first: use CMake variable TBB_INSTALL_DIR -if (TBB_INSTALL_DIR) - set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR}) -endif (TBB_INSTALL_DIR) -# second: use environment variable -if (NOT _TBB_INSTALL_DIR) - if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "") - set (_TBB_INSTALL_DIR $ENV{TBB_INSTALL_DIR}) - endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "") - # Intel recommends setting TBB21_INSTALL_DIR - if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "") - set (_TBB_INSTALL_DIR $ENV{TBB21_INSTALL_DIR}) - endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "") - if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "") - set (_TBB_INSTALL_DIR $ENV{TBB22_INSTALL_DIR}) - endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "") - if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "") - set (_TBB_INSTALL_DIR $ENV{TBB30_INSTALL_DIR}) - endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "") -endif (NOT _TBB_INSTALL_DIR) -# third: try to find path automatically -if (NOT _TBB_INSTALL_DIR) - if (_TBB_DEFAULT_INSTALL_DIR) - set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR}) - endif (_TBB_DEFAULT_INSTALL_DIR) -endif (NOT _TBB_INSTALL_DIR) -# sanity check -if (NOT _TBB_INSTALL_DIR) - message ("ERROR: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}") -else (NOT _TBB_INSTALL_DIR) -# finally: set the cached CMake variable TBB_INSTALL_DIR -if (NOT TBB_INSTALL_DIR) - set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory") - mark_as_advanced(TBB_INSTALL_DIR) -endif (NOT TBB_INSTALL_DIR) - - -#-- A macro to rewrite the paths of the library. This is necessary, because -# find_library() always found the em64t/vc9 version of the TBB libs -macro(TBB_CORRECT_LIB_DIR var_name) -# if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t") - string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}}) -# endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t") - string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}}) - string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) - string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) - string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) - string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) -endmacro(TBB_CORRECT_LIB_DIR var_content) - - -#-- Look for include directory and set ${TBB_INCLUDE_DIR} -set (TBB_INC_SEARCH_DIR ${_TBB_INSTALL_DIR}/include) -# Jiri: tbbvars now sets the CPATH environment variable to the directory -# containing the headers. -find_path(TBB_INCLUDE_DIR - tbb/task_scheduler_init.h - PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH -) -mark_as_advanced(TBB_INCLUDE_DIR) - - -#-- Look for libraries -# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh] -if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "") - set (_TBB_LIBRARY_DIR - ${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM} - ${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib - ) -endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "") -# Jiri: This block isn't mutually exclusive with the previous one -# (hence no else), instead I test if the user really specified -# the variables in question. -if ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL "")) - # HH: deprecated - message(STATUS "[Warning] FindTBB.cmake: The use of TBB_ARCHITECTURE and TBB_COMPILER is deprecated and may not be supported in future versions. Please set \$ENV{TBB_ARCH_PLATFORM} (using tbbvars.[bat|csh|sh]).") - # Jiri: It doesn't hurt to look in more places, so I store the hints from - # ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER - # variables and search them both. - set (_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR}) -endif ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL "")) - -# GvdB: Mac OS X distribution places libraries directly in lib directory. -list(APPEND _TBB_LIBRARY_DIR ${_TBB_INSTALL_DIR}/lib) - -# Jiri: No reason not to check the default paths. From recent versions, -# tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH -# variables, which now point to the directories of the lib files. -# It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS -# argument instead of the implicit PATHS as it isn't hard-coded -# but computed by system introspection. Searching the LIBRARY_PATH -# and LD_LIBRARY_PATH environment variables is now even more important -# that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates -# the use of TBB built from sources. -find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR} - PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) -find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR} - PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) - -#Extract path from TBB_LIBRARY name -get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH) - -#TBB_CORRECT_LIB_DIR(TBB_LIBRARY) -#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY) -mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY) - -#-- Look for debug libraries -# Jiri: Changed the same way as for the release libraries. -find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR} - PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) -find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR} - PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) - -# Jiri: Self-built TBB stores the debug libraries in a separate directory. -# Extract path from TBB_LIBRARY_DEBUG name -get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH) - -#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG) -#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG) -mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG) - - -if (TBB_INCLUDE_DIR) - if (TBB_LIBRARY) - set (TBB_FOUND "YES") - set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES}) - set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES}) - set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE) - set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE) - # Jiri: Self-built TBB stores the debug libraries in a separate directory. - set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE) - mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES) - message(STATUS "Found Intel TBB") - endif (TBB_LIBRARY) -endif (TBB_INCLUDE_DIR) - -if (NOT TBB_FOUND) - message("ERROR: Intel TBB NOT found!") - message(STATUS "Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}") - # do only throw fatal, if this pkg is REQUIRED - if (TBB_FIND_REQUIRED) - message(FATAL_ERROR "Could NOT find TBB library.") - endif (TBB_FIND_REQUIRED) -endif (NOT TBB_FOUND) - -endif (NOT _TBB_INSTALL_DIR) - -if (TBB_FOUND) - set(TBB_INTERFACE_VERSION 0) - FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS) - STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}") - set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}") -endif (TBB_FOUND) diff --git a/cmake-configs/FindWhyCon.cmake b/cmake-configs/FindWhyCon.cmake deleted file mode 100644 index 75107fe..0000000 --- a/cmake-configs/FindWhyCon.cmake +++ /dev/null @@ -1,13 +0,0 @@ -find_path(WhyCon_INCLUDE_DIRS whycon/localization_system.h PATH_SUFFIXES whycon HINTS ${CMAKE_CURRENT_LIST_DIR}/../../../include) -find_library(WhyCon_LIBRARIES NAMES whycon HINTS ${CMAKE_CURRENT_LIST_DIR}/../../../lib) - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(whycon DEFAULT_MSG WhyCon_LIBRARIES WhyCon_INCLUDE_DIRS) - -find_package(OpenCV REQUIRED) -find_package(Boost COMPONENTS program_options thread system REQUIRED) - -set(WhyCon_LIBRARIES ${WHYCON_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES}) -set(WhyCon_INCLUDE_DIRS ${WHYCON_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - -mark_as_advanced(WhyCon_LIBRARIES WhyCon_INCLUDE_DIRS) diff --git a/cmake-configs/HandleLibraryTypes.cmake b/cmake-configs/HandleLibraryTypes.cmake deleted file mode 100644 index 7dd0bc2..0000000 --- a/cmake-configs/HandleLibraryTypes.cmake +++ /dev/null @@ -1,48 +0,0 @@ -##====================================================================== -# -# PIXHAWK Micro Air Vehicle Flying Robotics Toolkit -# Please see our website at -# -# -# Original Authors: -# @author Reto Grieder -# @author Fabian Landau -# Contributing Authors (in alphabetical order): -# -# Todo: -# -# -# (c) 2009 PIXHAWK PROJECT -# -# This file is part of the PIXHAWK project -# -# PIXHAWK is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# PIXHAWK is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with PIXHAWK. If not, see . -# -##======================================================================== - -FUNCTION(HANDLE_LIBRARY_TYPES _name) - # Additional libraries can be added as additional arguments - IF(${_name}_LIBRARY_DEBUG AND ${_name}_LIBRARY_OPTIMIZED) - SET(${_name}_LIBRARY - optimized ${${_name}_LIBRARY_OPTIMIZED} ${ARGN} - debug ${${_name}_LIBRARY_DEBUG} ${ARGN} - PARENT_SCOPE - ) - ELSE() - SET(${_name}_LIBRARY - ${${_name}_LIBRARY_OPTIMIZED} ${ARGN} - PARENT_SCOPE - ) - ENDIF() -ENDFUNCTION(HANDLE_LIBRARY_TYPES) diff --git a/config.h.cmake b/config.h.cmake deleted file mode 100644 index 6f4e2d3..0000000 --- a/config.h.cmake +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef __CONFIG_H__ -#define __CONFIG_H__ - -#cmakedefine ENABLE_FULL_UNDISTORT -#cmakedefine ENABLE_RANDOMIZED_THRESHOLD -#cmakedefine ENABLE_VERBOSE - -#if defined(ENABLE_VERBOSE) -#define WHYCON_DEBUG(x) cout << x << endl -#else -#define WHYCON_DEBUG(x) -#endif - -#endif diff --git a/establish_error.rb b/establish_error.rb deleted file mode 100644 index 48cec10..0000000 --- a/establish_error.rb +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/ruby - -file=ARGV.first -lines=[] -File.open(file,'r') do |f| - lines=f.readlines -end - -tilex=0.625 -tiley=1.25 -xtiles=9 -ytiles=4 -errors=[] -error_sum=0 -lines.each do |l| - line=l.split - inx,iny = line[5].to_f, line[6].to_f - trx,try = inx * xtiles, iny * ytiles - idx,idy = trx.round, try.round - diff = Math.sqrt(((trx - idx) * tilex) ** 2 + ((try - idy) * tiley) ** 2) - errors << diff - error_sum += diff - puts "in #{inx} #{iny} out #{idx} #{idy} tr #{trx} #{try} diff #{diff}" -end - -max_error=errors.max * 100 -avg_error=error_sum/errors.size * 100 -STDERR.puts "max: #{max_error} avg: #{avg_error} [cm]" diff --git a/include/transformer.h b/include/transformer.h deleted file mode 100644 index 605cea6..0000000 --- a/include/transformer.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef WHYCON_TRANSFORMER_H -#define WHYCON_TRANSFORMER_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace whycon -{ - class Transformer { - public: - Transformer(ros::NodeHandle& n); - - ros::Publisher poses_pub, poses_2d_pub; - - message_filters::Subscriber poses_sub; - message_filters::Subscriber projection_sub; - boost::shared_ptr> synchronizer; - - std::string world_frame_id; - - cv::Matx33d projection; - bool has_projection; - - void on_poses(const geometry_msgs::PoseArrayConstPtr& poses_msg, const ProjectionConstPtr& projection_msg); - - boost::shared_ptr transform_listener; - }; -} - -#endif diff --git a/launch/custom/vrep-set-axis.launch b/launch/custom/vrep-set-axis.launch deleted file mode 100644 index 6cbda43..0000000 --- a/launch/custom/vrep-set-axis.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/launch/custom/vrep.launch b/launch/custom/vrep.launch deleted file mode 100644 index 14626fa..0000000 --- a/launch/custom/vrep.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/launch/robot_pose_publisher.launch b/launch/robot_pose_publisher.launch deleted file mode 100644 index ac53a9e..0000000 --- a/launch/robot_pose_publisher.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/launch/set_axis.launch b/launch/set_axis.launch deleted file mode 100644 index f80f363..0000000 --- a/launch/set_axis.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/launch/whycon.launch b/launch/whycon.launch deleted file mode 100644 index 0f69e31..0000000 --- a/launch/whycon.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/launch/whycon.rviz b/launch/whycon.rviz deleted file mode 100644 index f7eccb4..0000000 --- a/launch/whycon.rviz +++ /dev/null @@ -1,150 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Grid1 - - /Marker1 - - /Marker1/Namespaces1 - - /Marker2 - - /Marker2/Status1 - - /Marker2/Namespaces1 - - /Image1 - - /PoseArray1 - Splitter Ratio: 0.5 - Tree Height: 464 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: whycon - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /whycon/visualization_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /triangulator/visualization_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /whycon/image_out - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Value: true - - Arrow Length: 0.3 - Class: rviz/PoseArray - Color: 255; 25; 0 - Enabled: true - Name: PoseArray - Topic: /whycon/poses - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: whycon - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Angle: 0 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Name: Current View - Near Clip Distance: 0.01 - Scale: 48.8711 - Target Frame: - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 743 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001ba0000023bfc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006c01000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000218000000de01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000025a000000220000002201000003fb0000000a0049006d0061006700650000000235000000220000000000000000fb0000000a0049006d006100670065010000012d000001390000000000000000fb0000000a0049006d00610067006501000001ad000000b90000000000000000fb0000000a0049006d006100670065010000013a000000760000000000000000fb0000000a0049006d00610067006501000001b6000000b0000000000000000000000001000001100000023bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000410000023b000000bc01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055600000048fc0100000002fb0000000800540069006d00650100000000000005560000020a01000003fb0000000800540069006d006501000000000000045000000000000000000000028a0000023b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1366 - X: 0 - Y: 1080 diff --git a/msg/Projection.msg b/msg/Projection.msg deleted file mode 100644 index faca8b2..0000000 --- a/msg/Projection.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -float64[9] projection diff --git a/nodelets.xml b/nodelets.xml deleted file mode 100644 index 87fa898..0000000 --- a/nodelets.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - WhyCon nodelet - - - diff --git a/src/camera_calibrator.cpp b/src/camera_calibrator.cpp deleted file mode 100644 index c2dfd72..0000000 --- a/src/camera_calibrator.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include -#include -#include -#include -using namespace std; - -bool stop = false; -void interrupt(int s) { - stop = true; -} - -bool clicked = false, rclicked = false; -void mouse_callback(int event, int x, int y, int flags, void* param) { - if (event == CV_EVENT_LBUTTONDOWN) clicked = true; - if (event == CV_EVENT_RBUTTONDOWN) rclicked = true; -} - -int main(int argc, char** argv) { - if (argc != 9) { - cout << "usage: camera_calibrator [-cam | -img ]" << endl; - cout << "X,Y corresponds to width,height in image" << endl; - return 1; - } - - /* setup camera */ - int width = atoi(argv[1]); - int height = atoi(argv[2]); - - bool is_camera = (string(argv[7]) == "-cam"); - cv::VideoCapture capture; - if (is_camera) { - capture.open(atoi(argv[7])); - capture.set(CV_CAP_PROP_FRAME_WIDTH, width); - capture.set(CV_CAP_PROP_FRAME_HEIGHT, height); - capture.set(CV_CAP_PROP_FPS, 20); - } - else { - capture.open(argv[8]); - } - if (!capture.isOpened()) { cout << "error opening input source" << endl; return 1; } - - /* load calibration and setup system */ - cv::Mat frame; - cv::Mat K, dist_coeff; - - /* setup gui and start capturing / processing */ - cvStartWindowThread(); - cv::namedWindow("input"); - cv::setMouseCallback("input", mouse_callback); - - int x_squares = atoi(argv[3]); - int y_squares = atoi(argv[4]); - float x_size = atof(argv[5]); - float y_size = atof(argv[6]); - cv::Size pattern_size(x_squares - 1, y_squares - 1); - vector< vector > all_corners; - - vector grid3d; - for(int i = 0; i < (x_squares - 1) * (y_squares - 1); i++) - grid3d.push_back(cv::Point3f((i / (x_squares - 1)) * x_size, (i % (x_squares - 1)) * y_size, 0.0f)); // TODO: set units here - - while (true) { - bool last_frame = capture.grab(); - capture.retrieve(frame); - - vector corners; - int result = cv::findChessboardCorners(frame, pattern_size, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE); - if (result) { - cv::Mat gray; - cv::cvtColor(frame, gray, CV_BGR2GRAY); - cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.05)); - cv::drawChessboardCorners(frame, pattern_size, cv::Mat(corners), result); - - if (!is_camera || clicked) { - clicked = false; - all_corners.push_back(corners); - } - } - - if (!all_corners.empty() && ((is_camera && rclicked) || (!is_camera && last_frame))) { - rclicked = false; - vector< vector > grid3d_all(all_corners.size(), grid3d); - vector rotations, translations; - int flags = 0; - for (int i = 0; i < 5; i++) { - cout << "iteration " << i << endl; - double error = cv::calibrateCamera(grid3d_all, all_corners, frame.size(), K, dist_coeff, rotations, translations, flags); - cout << "K: " << K << endl; - cout << "dist: " << dist_coeff << endl; - cout << "reprojection error: " << error << endl; - flags = CV_CALIB_USE_INTRINSIC_GUESS; - } - - cv::FileStorage file("calibration.xml", cv::FileStorage::WRITE); - file << "K" << K; - file << "dist" << dist_coeff; - return 0; - } - - ostringstream ostr; - ostr << "frames: " << all_corners.size(); - cv::putText(frame, ostr.str(), cv::Point(5, 15), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255,0,255), 1.5, CV_AA); - - if (!frame.empty()) cv::imshow("input", frame); - } - return 0; -} - - - - diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 27688a5..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,288 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -using namespace std; -namespace po = boost::program_options; - -bool stop = false; -void interrupt(int s) { - stop = true; -} - -bool clicked = false; -void mouse_callback(int event, int x, int y, int flags, void* param) { - if (event == CV_EVENT_LBUTTONDOWN) { cout << "clicked window" << endl; clicked = true; } -} - -bool do_tracking; -bool use_gui; -int number_of_targets; -bool load_axis; -bool custom_diameter = false; - -po::variables_map process_commandline(int argc, char** argv) -{ - po::options_description options_description("WhyCon options"); - options_description.add_options() - ("help,h", "display this help") - ; - - po::options_description mode_options("Operating mode"); - mode_options.add_options() - ("set-axis,s", po::value(), "perform axis detection and save results to specified XML file") - ("track,t", po::value(), "perform tracking of the specified ammount of targets") - ; - - po::options_description input_options("Input source"); - input_options.add_options() - ("cam,c", po::value(), "use camera as input (expects id of camera)") - ("video,v", po::value(), "use video as input (expects path to video file)") - ("img,i", po::value(), "use sequence of images as input (expects pattern describing sequence). " - "Use a pattern such as 'directory/%03d.png' for files named 000.png to " - "999.png inside said directory") - ; - - po::options_description tracking_options("Tracking options"); - tracking_options.add_options() - ("axis,a", po::value(), "use specified axis definition XML file for coordinate transformation during tracking") - ("no-axis,n", "do not transform 3D coordinates during tracking") - ("output,o", po::value(), "name to be used for all tracking output files") - ; - - po::options_description parameter_options("Other options"); - parameter_options.add_options() - ("inner-diameter", po::value(), "use specified inner diameter (in meters) of circles") - ("outer-diameter", po::value(), "use specified outer diameter (in meters) of circles") - ("mat,m", po::value(), "use specified matlab (.m) calibration toolbox file for camera calibration parameters") - ("xml,x", po::value(), "use specified 'camera_calibrator' file (.xml) for camera calibration parameters") - ("service", "run as a mavconn service, outputting pose information through bus") - ("no-gui", "disable opening of GUI") - ("width", po::value(), "input camera resolution width") - ("height", po::value(), "input camera resolution height") - ; - - options_description.add(mode_options).add(input_options).add(tracking_options).add(parameter_options); - - po::variables_map config_vars; - try { - po::store(po::parse_command_line(argc, argv, options_description), config_vars); - if (config_vars.count("help")) { cerr << options_description << endl; exit(1); } - - po::notify(config_vars); - - if (config_vars.count("track")) do_tracking = true; - else if (config_vars.count("set-axis")) do_tracking = false; - else throw std::runtime_error("Select either tracking or axis setting mode"); - - if (!config_vars.count("mat") && !config_vars.count("xml")) - throw std::runtime_error("Please specify one source for calibration parameters"); - - if (!config_vars.count("cam") && !config_vars.count("video") && !config_vars.count("img")) - throw std::runtime_error("Please specify one input source"); - - if (config_vars.count("width") != config_vars.count("height")) - throw std::runtime_error("Please specify both width and height for camera resolution"); - - if (!config_vars.count("output")) - throw std::runtime_error("Specify prefix name for output files"); - - use_gui = !config_vars.count("no-gui"); - - if (do_tracking) { - if (config_vars["track"].as() < 0) throw std::runtime_error("Number of circles to track should be greater than 0"); - if (config_vars.count("no-axis")) load_axis = false; - else { - if (!config_vars.count("axis")) throw std::runtime_error("Axis definition file is missing, if you dont need to perform coordinate transformation use --no-axis"); - load_axis = true; - } - if (config_vars.count("inner-diameter") && config_vars.count("outer-diameter")) - custom_diameter = true; - else if (config_vars.count("inner-diameter") || config_vars.count("outer-diameter")) - throw std::runtime_error("please specify both outer and inner diameters"); - - number_of_targets = config_vars["track"].as(); - } - else { - if (config_vars.count("video")) throw std::runtime_error("Video input is not supported for axis definition"); - if (!use_gui && config_vars.count("cam")) throw std::runtime_error("Camera input is not supported for axis setting when GUI is disabled"); - if (config_vars.count("service")) throw std::runtime_error("Running as service is only for tracking mode"); - } - } - catch(const std::runtime_error& e) { - cerr << options_description << endl << endl; - cerr << "ERROR: " << e.what() << endl; - exit(1); - } - catch(po::error& e) { - cerr << options_description << endl << endl; - cerr << endl << "ERROR: " << e.what() << endl; - exit(1); - } - return config_vars; -} - - -int main(int argc, char** argv) -{ - signal(SIGINT, interrupt); - - /* process command line */ - po::variables_map config_vars = process_commandline(argc, argv); - - /* setup input */ - bool is_camera = config_vars.count("cam"); - cv::VideoCapture capture; - if (is_camera) { - int cam_id = config_vars["cam"].as(); - capture.open(cam_id); - if (config_vars.count("width")) { - capture.set(CV_CAP_PROP_FRAME_WIDTH, config_vars["width"].as()); - capture.set(CV_CAP_PROP_FRAME_HEIGHT, config_vars["height"].as()); - } - } - else { - std::string video_name(config_vars.count("img") ? config_vars["img"].as() : config_vars["video"].as()); - capture.open(video_name); - } - if (!capture.isOpened()) { cout << "error opening camera/video" << endl; return 1; } - - /* load calibration */ - cv::Mat K, dist_coeff; - if (config_vars.count("xml")) - cv::LocalizationSystem::load_opencv_calibration(config_vars["xml"].as(), K, dist_coeff); - else - cv::LocalizationSystem::load_matlab_calibration(config_vars["mat"].as(), K, dist_coeff); - - /* init system */ - - cv::Size frame_size(capture.get(CV_CAP_PROP_FRAME_WIDTH), capture.get(CV_CAP_PROP_FRAME_HEIGHT)); - cout << "frame size: " << frame_size << endl; - float inner_diameter = (custom_diameter ? config_vars["inner-diameter"].as() : WHYCON_DEFAULT_INNER_DIAMETER); - float outer_diameter = (custom_diameter ? config_vars["outer-diameter"].as() : WHYCON_DEFAULT_OUTER_DIAMETER); - cv::LocalizationSystem system(number_of_targets, frame_size.width, frame_size.height, K, dist_coeff, - outer_diameter, inner_diameter); - cout << "using diameters (outer/inner): " << outer_diameter << " " << inner_diameter << endl; - - #ifdef ENABLE_MAVCONN - bool run_service = config_vars.count("service"); - cv::LocalizationService service(system); - if (run_service) service.start(); - #endif - - /* setup gui */ - if (use_gui) { - cvStartWindowThread(); - cv::namedWindow("output", CV_WINDOW_NORMAL); - cv::setMouseCallback("output", mouse_callback); - } - - /* set tracking output */ - std::string output_name = config_vars["output"].as(); - cv::VideoWriter video_writer; - ofstream data_file; - if (do_tracking) { - video_writer.open(output_name + ".avi", CV_FOURCC('M','J','P','G'), 15, frame_size); - if (!video_writer.isOpened()) throw std::runtime_error("error opening output video"); - data_file.open((output_name + ".log").c_str(), ios_base::out | ios_base::trunc); - if (!data_file) throw std::runtime_error(string("error opening '") + output_name + ".log' output data file"); - } - - /* setup gui and start capturing / processing */ - bool is_tracking = false; - if (!is_camera) clicked = true; // when not using camera, emulate user click so that tracking starts immediately - cv::Mat original_frame, frame; - long frame_idx = 0; - - /* read axis from file when in tracking mode */ - if (do_tracking) { - if (load_axis) { - cout << "loading axis definition file" << endl; - system.read_axis(config_vars["axis"].as()); - } - else cout << "coordinate transform disabled" << endl; - } - - int max_attempts = is_camera ? 1 : 5; - int refine_steps = is_camera ? 1 : 15; - - while (!stop) - { - if (!capture.read(original_frame)) { cout << "no more frames left to read" << endl; break; } - - #if defined(ENABLE_FULL_UNDISTORT) - cv::Mat undistorted; - cv::undistort(original_frame, undistorted, K, dist_coeff, K); - undistorted.copyTo(original_frame); - #endif - - original_frame.copyTo(frame); - - if (!do_tracking) { - if (!is_camera || clicked) { - bool axis_was_set = system.set_axis(original_frame, max_attempts, refine_steps, config_vars["set-axis"].as()); - if (!axis_was_set) throw std::runtime_error("Error setting axis!"); - system.draw_axis(frame); - cv::imwrite(output_name + "_axis_detected.png", frame); - stop = true; - } - if (use_gui) cv::imshow("output", frame); - } - else { - if (!use_gui || !is_camera || clicked) { - if (!is_tracking) cout << "resetting targets" << endl; - - int64_t ticks = cv::getTickCount(); - is_tracking = system.localize(original_frame, !is_tracking, max_attempts, refine_steps); - double delta_ticks = (double)(cv::getTickCount() - ticks) / cv::getTickFrequency(); - - cout << "localized all? " << is_tracking << " t: " << delta_ticks << " " << " fps: " << 1/delta_ticks << endl; - - for (int i = 0; i < number_of_targets; i++) { - const cv::CircleDetector::Circle& circle = system.get_circle(i); - if (!circle.valid) continue; - cv::Vec3f coord = system.get_pose(circle).pos; - cv::Vec3f coord_trans = coord; - if (load_axis) { - coord_trans = system.get_transformed_pose(circle).pos; - } - - if (use_gui) { - ostringstream ostr; - ostr << fixed << setprecision(2); - ostr << coord_trans << " " << i; - circle.draw(frame, ostr.str(), cv::Vec3b(255,255,0)); - } - - data_file << setprecision(15) << "frame " << frame_idx << " circle " << i - << " transformed: " << coord_trans(0) << " " << coord_trans(1) << " " << coord_trans(2) - << " original: " << coord(0) << " " << coord(1) << " " << coord(2) << endl; - - #ifdef ENABLE_MAVCONN - if (run_service) service.publish(); - #endif - } - - video_writer << frame; - } - if (use_gui) cv::imshow("output", frame); - } - - frame_idx++; - } - - /*#ifdef ENABLE_VIEWER - if (!stop) viewer.wait(); - #endif*/ - return 0; -} - diff --git a/src/ros/robot_pose_publisher.cpp b/src/ros/robot_pose_publisher.cpp deleted file mode 100644 index 16badcf..0000000 --- a/src/ros/robot_pose_publisher.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include "robot_pose_publisher.h" -#include -#include - -whycon::RobotPosePublisher::RobotPosePublisher(ros::NodeHandle& n) -{ - n.param("axis_length_tolerance", axis_length_tolerance, 0.05); - n.param("world_frame", world_frame, std::string("world")); - n.param("target_frame", target_frame, std::string("target")); - - broadcaster = boost::make_shared(); - - pose_sub = n.subscribe("/whycon/trans_poses", 1, &whycon::RobotPosePublisher::on_poses, this); -} - -/* this assumes an L-shaped pattern, defining the two axis of the robot on the plane (forward and left are positive) */ -void whycon::RobotPosePublisher::on_poses(const geometry_msgs::PoseArrayConstPtr& pose_array) -{ - ROS_INFO_STREAM("receiving poses"); - tf::Transform T; - - if (pose_array->poses.size() != 3) { ROS_WARN_STREAM("More/less than three circles detected, will not compute pose"); return; } - - const std::vector& ps = pose_array->poses; - float dists[3]; - ROS_INFO_STREAM("poses: " << ps[0].position << " " << ps[1].position << " " << ps[2].position); - - dists[0] = sqrt(pow(ps[0].position.x - ps[1].position.x, 2) + pow(ps[0].position.y - ps[1].position.y, 2)); // d(0,1) - dists[1] = sqrt(pow(ps[1].position.x - ps[2].position.x, 2) + pow(ps[1].position.y - ps[2].position.y, 2)); // d(1,2) - dists[2] = sqrt(pow(ps[2].position.x - ps[0].position.x, 2) + pow(ps[2].position.y - ps[0].position.y, 2)); // d(2,0) - ROS_INFO_STREAM("distances: " << dists[0] << " " << dists[1] << " " << dists[2]); - size_t max_idx = (std::max_element(dists, dists + 3) - dists); - std::swap(dists[max_idx], dists[0]); // dists[0] is now the longer distance (hypothenuse) - - - /* the two smaller distances correspond to the two cathetus, which should be equal in length (up to a certain tolerance) */ - if (fabsf(dists[1] - dists[2]) > axis_length_tolerance) { ROS_WARN_STREAM("Axis size differ: " << dists[1] << " , " << dists[2]); return ; } - if (fabsf(dists[0] - sqrt(pow(dists[1],2) + pow(dists[2],2))) > axis_length_tolerance) { ROS_WARN_STREAM("Pythagoras check not passed"); return ; } - - /* compute robot position as center of coordinate frame */ - const geometry_msgs::Pose& center = ps[(max_idx + 2) % 3]; - T.setOrigin(tf::Vector3(center.position.x, center.position.y, 0.0)); - - /* compute robot orientation as orientation of coordinate frame */ - const geometry_msgs::Pose& p1 = ps[(max_idx + 0) % 3]; - const geometry_msgs::Pose& p2 = ps[(max_idx + 1) % 3]; - - float v1[2], v2[2]; - v1[0] = p1.position.x - center.position.x; v1[1] = p1.position.y - center.position.y; - v2[0] = p2.position.x - center.position.x; v2[1] = p2.position.y - center.position.y; - - float n1 = sqrt(v1[0] * v1[0] + v1[1] * v1[1]); - float n2 = sqrt(v2[0] * v2[0] + v2[1] * v2[1]); - - v1[0] /= n1; v1[1] /= n1; - v2[0] /= n2; v2[1] /= n2; - - if (angles::normalize_angle(atan2(v1[1], v1[0]) - atan2(v2[1], v2[0])) > 0) - T.setRotation(tf::createQuaternionFromYaw(atan2(v2[1], v2[0]))); - else - T.setRotation(tf::createQuaternionFromYaw(atan2(v1[1], v1[0]))); - - broadcaster->sendTransform(tf::StampedTransform(T, pose_array->header.stamp, world_frame, target_frame)); -} - diff --git a/src/ros/robot_pose_publisher.h b/src/ros/robot_pose_publisher.h deleted file mode 100644 index be9b61e..0000000 --- a/src/ros/robot_pose_publisher.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef ROBOT_POSE_PUBLISHER_H -#define ROBOT_POSE_PUBLISHER_H - -#include -#include -#include -#include - -namespace whycon { - class RobotPosePublisher { - public: - RobotPosePublisher(ros::NodeHandle& n); - - ros::Subscriber pose_sub; - boost::shared_ptr broadcaster; - - double axis_length_tolerance; - std::string world_frame, target_frame, axis_file; - void on_poses(const geometry_msgs::PoseArrayConstPtr& pose_array); - }; -} - -#endif // ROBOT_POSE_PUBLISHER_H diff --git a/src/ros/robot_pose_publisher_node.cpp b/src/ros/robot_pose_publisher_node.cpp deleted file mode 100644 index c67d323..0000000 --- a/src/ros/robot_pose_publisher_node.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include -#include "robot_pose_publisher.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "robot_pose_publisher"); - ros::NodeHandle n("~"); - - whycon::RobotPosePublisher robot_pose_publisher(n); - ros::spin(); -} - diff --git a/src/ros/set_axis.cpp b/src/ros/set_axis.cpp deleted file mode 100644 index 8bc3d8c..0000000 --- a/src/ros/set_axis.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include -#include -#include -#include "set_axis.h" - -namespace std { - std::ostream& operator<<(std::ostream& str, const tf::Matrix3x3& m) - { - for (int i = 0; i < 3; i++) { - str << std::endl; - for (int j = 0; j < 3; j++) - str << m[i][j] << " "; - } - - return str; - } - - std::ostream& operator<<(std::ostream& str, const tf::Vector3& v) - { - str << v.getX() << " " << v.getY() << " " << v.getZ(); - - return str; - } -} - - -whycon::AxisSetter::AxisSetter(ros::NodeHandle &n) -{ - transforms_set = false; - - if (!n.getParam("xscale", xscale) || !n.getParam("yscale", yscale)) throw std::runtime_error("Please specify xscale and yscale"); - - // axis should be specified in same order as detect_square - if (n.getParam("axis_order", axis_order)) { - if (axis_order.size() != 4) throw std::runtime_error("Exactly four indices are needed for specifying axis"); - } - - poses_sub = n.subscribe("/whycon/poses", 1, &AxisSetter::on_poses, this); - image_sub = n.subscribe("/camera/image_rect_color", 1, &AxisSetter::on_image, this); - image_pub = n.advertise("image", 1); -} - -/** - * Return four tf::Point's ordered as (0,0),(0,1),(1,0),(1,1) - */ -void whycon::AxisSetter::detect_square(std::vector& points) -{ - tf::Point vectors[3]; - for (int i = 0; i < 3; i++) { - vectors[i] = points[i + 1] - points[0]; - } - - int min_prod_i = 0; - float min_prod = std::numeric_limits::max(); - for (int i = 0; i < 3; i++) - { - float prod = fabsf(vectors[(i + 2) % 3].dot(vectors[i])); - if (prod < min_prod) { min_prod = prod; min_prod_i = i; } - } - - int axis1_i = (((min_prod_i + 2) % 3) + 1); - int axis2_i = (min_prod_i + 1); - if (fabsf(points[axis1_i].getX()) < fabsf(points[axis2_i].getX())) std::swap(axis1_i, axis2_i); - int xy_i = 0; - for (int i = 1; i <= 3; i++) if (i != axis1_i && i != axis2_i) { xy_i = i; break; } - - std::vector points_original(points); - points.resize(4); - points[0] = points_original[0]; - points[1] = points_original[axis1_i]; - points[2] = points_original[axis2_i]; - points[3] = points_original[xy_i]; - - ROS_INFO_STREAM("Axis: (0,0) -> 0, (1,0) -> " << axis1_i << ", (0,1) -> " << axis2_i << ", (1,1) -> " << xy_i); -} - -void whycon::AxisSetter::build_square(std::vector& points) -{ - std::vector points_original(points); - points.resize(4); - for (int i = 0; i < 4; i++) - points[i] = points_original[axis_order[i]]; - - ROS_INFO_STREAM("Axis: (0,0) -> " << axis_order[0] << ", (1,0) -> " << axis_order[1] << ", (0,1) -> " << axis_order[2] << ", (1,1) -> " << axis_order[3]); -} - -tf::Matrix3x3 whycon::AxisSetter::compute_projection(const std::vector& points, float xscale, float yscale) -{ - /* TODO: use only ROS/Eigen for this */ - std::vector src(4); - for (int i = 0; i < 4; i++) src[i] = cv::Vec2d(points[i].getX(), points[i].getY()) / points[i].getZ(); - std::vector dest = { cv::Vec2d(0,0), cv::Vec2d(xscale, 0), cv::Vec2d(0, yscale), cv::Vec2d(xscale, yscale) }; - - cv::Matx33d projection = cv::findHomography(src, dest, CV_LMEDS); - - tf::Matrix3x3 m; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - m[i][j] = projection(i, j); - - return m; -} - -tf::Transform whycon::AxisSetter::compute_similarity(const std::vector& points) -{ - - tf::Point vectors[3]; - vectors[0] = points[1] - points[0]; - vectors[1] = points[2] - points[0]; - vectors[2] = vectors[0].cross(vectors[1]); - - for (int i = 0; i < 3; i++) vectors[i].normalize(); - - tf::Matrix3x3 rotation; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - rotation[j][i] = vectors[i][j]; - - tf::Quaternion Q; - rotation.getRotation(Q); - - tf::Transform rot_transform; - rot_transform.setRotation(Q); - - tf::Transform similarity; - similarity.setOrigin(rot_transform.inverse() * -points[0]); - similarity.setRotation(Q.inverse()); - - ROS_INFO_STREAM("transformed origin -> " << similarity * points[0]); - ROS_INFO_STREAM("transformed circle along X -> " << similarity * points[1]); - ROS_INFO_STREAM("transformed circle along Y -> " << similarity * points[2]); - ROS_INFO_STREAM("transformed circle along XY -> " << similarity * points[3]); - - /*float original_x = (points[1] - points[0]).length(); - float original_y = (points[2] - points[0]).length(); - float x = (similarity * points[1] - similarity * points[0]).length(); - float y = (similarity * points[2] - similarity * points[0]).length(); - float scaling_x = xscale / x; - float scaling_y = yscale / y; - - float xy = (scaling_x * (similarity * points[3]) - scaling_y * (similarity * points[0])).length(); - ROS_INFO_STREAM("original X " << original_x << " original Y " << original_y << " norm X " << x << " norm Y " << y); - ROS_INFO_STREAM("obtained XY scale: " << xy << " expected: " << sqrt(xscale * xscale + yscale * yscale) << " scaling: " << scaling_x << " " << scaling_y);*/ - - return similarity; -} - -void whycon::AxisSetter::write_projection(YAML::Emitter& yaml, const tf::Matrix3x3& projection) -{ - yaml << YAML::Key << "projection"; - yaml << YAML::Value << YAML::BeginSeq; - - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - yaml << projection[i][j]; - - yaml << YAML::EndSeq; -} - -void whycon::AxisSetter::write_similarity(YAML::Emitter& yaml, const tf::Transform& similarity) -{ - yaml << YAML::Key << "similarity"; - yaml << YAML::Value << YAML::BeginMap; - - yaml << YAML::Key << "origin"; - yaml << YAML::Value; - - yaml << YAML::BeginSeq; - yaml << similarity.getOrigin().getX() << similarity.getOrigin().getY() << similarity.getOrigin().getZ(); - yaml << YAML::EndSeq; - - yaml << YAML::Key << "rotation"; - yaml << YAML::Value; - yaml << YAML::BeginSeq; - yaml << similarity.getRotation().getX() << similarity.getRotation().getY() << similarity.getRotation().getZ() << similarity.getRotation().getW(); - yaml << YAML::EndSeq; - yaml << YAML::EndMap; -} - -void whycon::AxisSetter::on_poses(const geometry_msgs::PoseArrayConstPtr& poses_msg) -{ - if (transforms_set) return; - - if (poses_msg->poses.size() < 4) { - ROS_WARN_STREAM("Not computing, only " << poses_msg->poses.size() << " targets detected, need four."); - return; - } - - std::vector points(4); - for (int i = 0; i < 4; i++) - tf::pointMsgToTF(poses_msg->poses[i].position, points[i]); - - if (axis_order.empty()) - detect_square(points); - else - build_square(points); - - tf::Matrix3x3 projection = compute_projection(points, xscale, yscale); - tf::Transform similarity = compute_similarity(points); - - ROS_INFO_STREAM("Computed Transformations for \"" << poses_msg->header.frame_id << "\" localizer"); - ROS_INFO_STREAM("Projection:" << projection); - ROS_INFO_STREAM("Similarity Translation:" << similarity.getOrigin()); - ROS_INFO_STREAM("Similarity Rotation:" << similarity.getBasis()); - - YAML::Emitter yaml; - yaml << YAML::BeginMap; - write_projection(yaml, projection); - write_similarity(yaml, similarity); - yaml << YAML::EndMap; - - std::string filename = poses_msg->header.frame_id + "_transforms.yml"; - std::ofstream config_file(filename); - config_file << yaml.c_str(); - - ROS_INFO_STREAM("Wrote transformations to " << filename); - - transforms_set = true; -} - -void whycon::AxisSetter::on_image(const sensor_msgs::ImageConstPtr& image_msg) -{ - cv_bridge::CvImagePtr image_ptr = cv_bridge::toCvCopy(image_msg); -} diff --git a/src/ros/set_axis.h b/src/ros/set_axis.h deleted file mode 100644 index 0e68575..0000000 --- a/src/ros/set_axis.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef SET_AXIS_NODE_H -#define SET_AXIS_NODE_H - -#include -#include -#include -#include -#include - -namespace whycon { - class AxisSetter { - public: - AxisSetter(ros::NodeHandle& n); - - ros::Subscriber poses_sub, image_sub; - ros::Publisher image_pub; - - void on_poses(const geometry_msgs::PoseArrayConstPtr& poses_msg); - void on_image(const sensor_msgs::ImageConstPtr& image_msg); - - double xscale, yscale; - bool transforms_set; - - std::vector axis_order; - - private: - void detect_square(std::vector& points); - tf::Matrix3x3 compute_projection(const std::vector& points, float xscale, float yscale); - tf::Transform compute_similarity(const std::vector& points); - - void write_projection(YAML::Emitter& yaml, const tf::Matrix3x3& projection); - void write_similarity(YAML::Emitter& yaml, const tf::Transform& similarity); - void build_square(std::vector& points); - }; -} - -#endif // SET_AXIS_NODE_H diff --git a/src/ros/set_axis_node.cpp b/src/ros/set_axis_node.cpp deleted file mode 100644 index fbea9a2..0000000 --- a/src/ros/set_axis_node.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include -#include "set_axis.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "set_axis"); - ros::NodeHandle n("~"); - - whycon::AxisSetter axis_setter(n); - ros::spin(); -} - diff --git a/src/ros/transformer.cpp b/src/ros/transformer.cpp deleted file mode 100644 index 12828d2..0000000 --- a/src/ros/transformer.cpp +++ /dev/null @@ -1,75 +0,0 @@ - -#include "transformer.h" - -whycon::Transformer::Transformer(ros::NodeHandle &n) -{ - has_projection = false; - - n.param("world_frame", world_frame_id, std::string("world")); - - poses_pub = n.advertise("poses", 1); - poses_2d_pub = n.advertise("poses_2d", 1); - - poses_sub.subscribe(n, "/whycon/poses", 1); - projection_sub.subscribe(n, "/whycon/projection", 1); - synchronizer = boost::make_shared>(poses_sub, projection_sub, 10); - synchronizer->registerCallback(boost::bind(&Transformer::on_poses, this, _1, _2)); - - transform_listener = boost::make_shared(); -} - -void whycon::Transformer::on_poses(const geometry_msgs::PoseArrayConstPtr& poses_msg, const whycon::ProjectionConstPtr& projection_msg) -{ - if (poses_pub.getNumSubscribers() > 0) - { - if (!transform_listener->canTransform(world_frame_id, poses_msg->header.frame_id, ros::Time(0))) - return; - - tf::StampedTransform t; - transform_listener->lookupTransform(world_frame_id, poses_msg->header.frame_id, ros::Time(0), t); - - geometry_msgs::PoseArray transformed_poses; - transformed_poses.header = poses_msg->header; - transformed_poses.header.frame_id = world_frame_id; - - transformed_poses.poses.resize(poses_msg->poses.size()); - for (size_t i = 0; i < poses_msg->poses.size(); i++) { - tf::Point point; - tf::pointMsgToTF(poses_msg->poses[i].position, point); - tf::Point transformed_point = t * point; - tf::pointTFToMsg(transformed_point, transformed_poses.poses[i].position); - } - - poses_pub.publish(transformed_poses); - } - - if (poses_2d_pub.getNumSubscribers() > 0) - { - if (!has_projection) - { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - projection(i,j) = projection_msg->projection[3 * i + j]; - - has_projection = true; - } - - geometry_msgs::PoseArray transformed_poses; - transformed_poses.header = poses_msg->header; - transformed_poses.header.frame_id = world_frame_id; - - transformed_poses.poses.resize(poses_msg->poses.size()); - for (size_t i = 0; i < poses_msg->poses.size(); i++) { - tf::Point point; - tf::pointMsgToTF(poses_msg->poses[i].position, point); - - cv::Vec3d vec(point.getX(), point.getY(), point.getZ()); - vec = projection * vec; - - tf::pointTFToMsg(tf::Point(vec(0) / vec(2), vec(1) / vec(2), 0), transformed_poses.poses[i].position); - } - - poses_2d_pub.publish(transformed_poses); - } -} - diff --git a/src/ros/transformer_node.cpp b/src/ros/transformer_node.cpp deleted file mode 100644 index b17698b..0000000 --- a/src/ros/transformer_node.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include "transformer.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "transformer"); - ros::NodeHandle n("~"); - - whycon::Transformer t(n); - ros::spin(); -} diff --git a/src/ros/triangulator.cpp b/src/ros/triangulator.cpp deleted file mode 100644 index d792c51..0000000 --- a/src/ros/triangulator.cpp +++ /dev/null @@ -1,244 +0,0 @@ -#include -#include -#include -#include -#include "triangulator.h" - -whycon::Triangulator::Triangulator(ros::NodeHandle& n) -{ - points_left_sub.subscribe(n, "/whycon_left/points", 10); - points_right_sub.subscribe(n, "/whycon_right/points", 10); - - poses_left_sub.subscribe(n, "/whycon_left/poses", 10); - poses_right_sub.subscribe(n, "/whycon_right/poses", 10); - - /*typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - message_filters::Synchronizer sync(MySyncPolicy(10), points_left_sub, points_right_sub);*/ - sync = new message_filters::TimeSynchronizer - (points_left_sub, points_right_sub, poses_left_sub, poses_right_sub, 20); - - sync->registerCallback(boost::bind(&Triangulator::on_points, this, _1, _2, _3, _4)); - - viz_pub = n.advertise("visualization_marker", 1); - - std::string camera_left_name, camera_right_name; - if (!n.getParam("camera_left_name", camera_left_name)) throw std::runtime_error("Private parameter \"camera_left_name\" not provided"); - if (!n.getParam("camera_right_name", camera_right_name)) throw std::runtime_error("Private parameter \"camera_right_name\" not provided"); - - boost::shared_ptr cam_mgr; - sensor_msgs::CameraInfo camera_info; - - cam_mgr = boost::make_shared(n, camera_left_name); - camera_info = cam_mgr->getCameraInfo(); - dist_coeffs_left = cv::Mat(camera_info.D.size(), 1, CV_64FC1, &camera_info.D[0]).clone(); - K_left = cv::Mat(3, 3, CV_64FC1, &camera_info.K[0]).clone(); - R_left = cv::Mat(3, 3, CV_64FC1, &camera_info.R[0]).clone(); - P_left = cv::Mat(3, 4, CV_64FC1, &camera_info.P[0]).clone(); - ROS_INFO_STREAM("dist coeffs left " << dist_coeffs_left); - ROS_INFO_STREAM("K/P/R left " << K_left << " " << P_left << " " << R_left); - - cam_mgr = boost::make_shared(n, camera_right_name); - camera_info = cam_mgr->getCameraInfo(); - dist_coeffs_right = cv::Mat(camera_info.D.size(), 1, CV_64FC1, &camera_info.D[0]).clone(); - K_right = cv::Mat(3, 3, CV_64FC1, &camera_info.K[0]).clone(); - R_right = cv::Mat(3, 3, CV_64FC1, &camera_info.R[0]).clone(); - P_right = cv::Mat(3, 4, CV_64FC1, &camera_info.P[0]).clone(); - ROS_INFO_STREAM("dist coeffs right " << dist_coeffs_right); - ROS_INFO_STREAM("K/P/R right " << K_right << " " << P_right << " " << R_right); - - //R = (cv::Mat_(3, 3) << 0.9998201107700715, 0.01481867853984462, 0.011838617573635324, -0.01492480437321419, 0.9998487689673721, 0.008926892452136617, -0.011704542457666821, -0.009101975651663522, 0.9998900728205542); -} - -whycon::Triangulator::~Triangulator(void) -{ - delete sync; -} - -/** - From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997 - */ -cv::Mat_ LinearLSTriangulation(cv::Point3f u, //homogenous image point (u,v,1) - cv::Matx34f P, //camera 1 matrix - cv::Point3f u1, //homogenous image point in 2nd camera - cv::Matx34f P1 //camera 2 matrix - ) -{ - //build matrix A for homogenous equation system Ax = 0 - //assume X = (x,y,z,1), for Linear-LS method - //which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1 - cv::Matx43f A(u.x*P(2,0)-P(0,0), u.x*P(2,1)-P(0,1), u.x*P(2,2)-P(0,2), - u.y*P(2,0)-P(1,0), u.y*P(2,1)-P(1,1), u.y*P(2,2)-P(1,2), - u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1), u1.x*P1(2,2)-P1(0,2), - u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1), u1.y*P1(2,2)-P1(1,2) - ); - cv::Mat_ B = (cv::Mat_(4,1) << -(u.x*P(2,3) -P(0,3)), - -(u.y*P(2,3) -P(1,3)), - -(u1.x*P1(2,3) -P1(0,3)), - -(u1.y*P1(2,3) -P1(1,3))); - - cv::Mat_ X; - cv::solve(A,B,X,cv::DECOMP_SVD); - return X; -} - -#define EPSILON 0.001 - -cv::Mat_ IterativeLinearLSTriangulation(cv::Point3d u, //homogenous image point (u,v,1) - cv::Matx34d P, //camera 1 matrix - cv::Point3d u1, //homogenous image point in 2nd camera - cv::Matx34d P1 //camera 2 matrix - ) { - float wi = 1, wi1 = 1; - cv::Mat_ X(4,1); - cv::Mat_ X_; - - /*cv::Mat_ X_ = LinearLSTriangulation(u,P,u1,P1); - X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X(3) = 1.0;*/ - - for (int i=0; i<10; i++) { //Hartley suggests 10 iterations at most - //reweight equations and solve - cv::Matx43d A((u.x*P(2,0)-P(0,0))/wi, (u.x*P(2,1)-P(0,1))/wi, (u.x*P(2,2)-P(0,2))/wi, - (u.y*P(2,0)-P(1,0))/wi, (u.y*P(2,1)-P(1,1))/wi, (u.y*P(2,2)-P(1,2))/wi, - (u1.x*P1(2,0)-P1(0,0))/wi1, (u1.x*P1(2,1)-P1(0,1))/wi1, (u1.x*P1(2,2)-P1(0,2))/wi1, - (u1.y*P1(2,0)-P1(1,0))/wi1, (u1.y*P1(2,1)-P1(1,1))/wi1, (u1.y*P1(2,2)-P1(1,2))/wi1 - ); - cv::Mat_ B = (cv::Mat_(4,1) << -(u.x*P(2,3) -P(0,3))/wi, - -(u.y*P(2,3) -P(1,3))/wi, - -(u1.x*P1(2,3) -P1(0,3))/wi1, - -(u1.y*P1(2,3) -P1(1,3))/wi1 - ); - - cv::solve(A,B,X_,cv::DECOMP_SVD); - X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X(3) = 1.0f; - //ROS_INFO_STREAM("A " << A << " B "<< B << " X_ " << X_); - - //recalculate weights - double p2x = cv::Mat_(cv::Mat_(P).row(2)*X)(0); - double p2x1 = cv::Mat_(cv::Mat_(P1).row(2)*X)(0); - - //breaking point - double e1 = fabsf(wi - p2x); - double e2 = fabsf(wi1 - p2x1); - //ROS_INFO_STREAM("Xin " << X << " " << P << " " << P1); - //ROS_INFO_STREAM("e " << e1 << " " << e2 << " " << p2x << " " << p2x1); - ROS_INFO_STREAM("X " << X << " e " << e1 << " " << e2); - if(e1 <= EPSILON && e2 <= EPSILON) { break; } - - wi = p2x; - wi1 = p2x1; - } - return X; -} - -void triangulatePoints2(const cv::Mat& P1, const cv::Mat& P2, const std::vector& points1, const std::vector& points2, cv::Mat& out) -{ - for (size_t i = 0; i < points1.size(); i++) { - cv::Vec3d point1(points1[i].x, points1[i].y, 1); - cv::Vec3d point2(points2[i].x, points2[i].y, 1); - out.row(i) = IterativeLinearLSTriangulation(point1, P1, point2, P2).t(); - } -} - -void whycon::Triangulator::on_points(const whycon::PointArray::ConstPtr& points_left, const whycon::PointArray::ConstPtr& points_right, - const geometry_msgs::PoseArray::ConstPtr& poses_left, const geometry_msgs::PoseArray::ConstPtr& poses_right) -{ - if (points_left->points.size() != points_right->points.size()) - ROS_WARN_STREAM("skipped since not equal number of points"); - - ROS_INFO_STREAM("seq " << points_left->header.stamp << " " << points_right->header.stamp); - - size_t points_n = points_left->points.size(); - std::vector points_left_vec, points_right_vec; - points_left_vec.reserve(points_n); - points_right_vec.reserve(points_n); - - for (size_t i = 0; i < points_n; i++) { - points_left_vec.push_back(cv::Point2f(points_left->points[i].x, points_left->points[i].y)); - points_right_vec.push_back(cv::Point2f(points_right->points[i].x, points_right->points[i].y)); - } - - std::vector points_left_vec_u, points_right_vec_u; - - cv::Mat realR, realT, realK; - cv::decomposeProjectionMatrix(P_right, realK, realR, realT); - ROS_INFO_STREAM("rot: " << realR << " " << realT << " tx " << (realT.at(0) / realT.at(3))); - - cv::Mat P1, P2; - /*P_left.convertTo(P1, CV_32FC1); - P_right.convertTo(P2, CV_32FC1); */ - P1 = cv::Mat::eye(3, 4, CV_64FC1); - P2 = cv::Mat::eye(3, 4, CV_64FC1); - P2.at(0, 3) = P_right.at(0, 3) / P_right.at(0, 0); - //P2.at(0, 3) = -(realT.at(0) / realT.at(3)); - - ROS_INFO_STREAM("P1 " << P1); - ROS_INFO_STREAM("P2 " << P2); - - cv::undistortPoints(points_left_vec, points_left_vec_u, K_left, dist_coeffs_left, R_left/*, P1*/); - cv::undistortPoints(points_right_vec, points_right_vec_u, K_right, dist_coeffs_right, R_right/*, P2*/); - - for (size_t i = 0; i < points_n; i++) { - ROS_INFO_STREAM("pixl " << points_left_vec[i].x << " " << points_left_vec[i].y); - ROS_INFO_STREAM("pixr " << points_right_vec[i].x << " " << points_right_vec[i].y); - ROS_INFO_STREAM("l " << points_left_vec_u[i].x << " " << points_left_vec_u[i].y); - ROS_INFO_STREAM("r " << points_right_vec_u[i].x << " " << points_right_vec_u[i].y); - ROS_INFO_STREAM("lp " << poses_left->poses[i].position.x << " " << poses_left->poses[i].position.y << " " << poses_left->poses[i].position.z); - ROS_INFO_STREAM("rp " << poses_right->poses[i].position.x << " " << poses_right->poses[i].position.y << " " << poses_right->poses[i].position.z); - - points_right_vec[i].y = points_left_vec[i].y; - } - - /*cv::Mat P1 = eye: - cv::Mat P2 = R teye*/ - cv::Mat points4d(points_n, 4, CV_64FC1); - /*cv::Mat P1p = cv::Mat::eye(3, 4, CV_32FC1); - cv::Mat P2p = cv::Mat::eye(3, 4, CV_32FC1); - P2p.at(0, 3) = (P2.at(0, 3) - P2.at(0, 2)) / P2.at(0, 0); - ROS_INFO_STREAM("P1P " << P1p << " " << P2p);*/ - //cv::triangulatePoints(P1, P2, points_left_vec_u, points_right_vec_u, points4d); - triangulatePoints2(P1, P2, points_left_vec_u, points_right_vec_u, points4d); - - - visualization_msgs::Marker marker; - marker.header = points_left->header; - marker.ns = "points"; - marker.id = 0; - - marker.type = visualization_msgs::Marker::POINTS; - marker.action = visualization_msgs::Marker::ADD; - marker.scale.x = 0.1; - marker.scale.y = 0.1; - marker.color.a = 1; - marker.color.r = 0; - marker.color.g = 1; - marker.color.b = 0; - - //points4d = ((realR * cv::Mat::eye(3, 4, CV_32FC1)) * points4d.t()).t(); - - for (size_t i = 0; i < points_n; i++) { - cv::Vec4d hom_point(points4d.at(i, 0), points4d.at(i, 1), points4d.at(i, 2), points4d.at(i, 3)); - cv::Vec3d inhom_point(hom_point(0) / hom_point(3), hom_point(1) / hom_point(3), hom_point(2) / hom_point(3)); - - geometry_msgs::Point marker_point; - cv::Vec3d rotated_point = cv::Matx33d(R_left) * inhom_point; - marker_point.x = rotated_point(0); - marker_point.y = rotated_point(1); - marker_point.z = rotated_point(2); - marker.points.push_back(marker_point); - ROS_INFO_STREAM("inhom: " << inhom_point); - ROS_INFO_STREAM("hom: " << hom_point); - ROS_INFO_STREAM("rot point: " << rotated_point); - - /* reprojection error */ - cv::Vec3d rep1 = cv::Matx34d(P1) * hom_point; - cv::Vec3d rep2 = cv::Matx34d(P2) * hom_point; - rep1 = rep1 * 1/rep1(2); - rep2 = rep2 * 1/rep2(2); - ROS_INFO_STREAM("rep: " << rep1 << " " << rep2); - ROS_INFO_STREAM("err: " << cv::norm(cv::Vec2d(rep1(0), rep1(1)), cv::Vec2d(points_left_vec_u[i])) << " " << cv::norm(cv::Vec2d(rep2(0), rep2(1)), cv::Vec2d(points_right_vec_u[i]))); - } - - viz_pub.publish(marker); -} - - diff --git a/src/ros/triangulator.h b/src/ros/triangulator.h deleted file mode 100644 index 428c2eb..0000000 --- a/src/ros/triangulator.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef __TRIANGULATOR_H__ -#define __TRIANGULATOR_H__ - -#include -#include -#include -#include -#include -#include -#include - -namespace whycon { - class Triangulator { - public: - Triangulator(ros::NodeHandle& n); - ~Triangulator(void); - - void on_points(const whycon::PointArray::ConstPtr& points_left, const whycon::PointArray::ConstPtr& points_right, - const geometry_msgs::PoseArray::ConstPtr& poses_left, const geometry_msgs::PoseArray::ConstPtr& poses_right); - - private: - ros::Publisher viz_pub; - cv::Mat P_left, P_right; - cv::Mat K_left, K_right; - cv::Mat dist_coeffs_left, dist_coeffs_right; - cv::Mat R_left, R_right; - cv::Mat R; - - message_filters::Subscriber points_left_sub, points_right_sub; - message_filters::Subscriber poses_left_sub, poses_right_sub; - message_filters::TimeSynchronizer* sync; - }; -} - -#endif diff --git a/src/ros/triangulator_node.cpp b/src/ros/triangulator_node.cpp deleted file mode 100644 index f80bb44..0000000 --- a/src/ros/triangulator_node.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include "triangulator.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "triangulator"); - ros::NodeHandle n("~"); - - whycon::Triangulator t(n); - ros::spin(); -} diff --git a/src/ros/whycon_nodelet.cpp b/src/ros/whycon_nodelet.cpp deleted file mode 100644 index 88ce1f2..0000000 --- a/src/ros/whycon_nodelet.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include -#include -#include -#include "whycon_ros.h" - -namespace whycon { - class WhyconNodelet : public nodelet::Nodelet - { - public: - void onInit(void) { - t = boost::make_shared(getPrivateNodeHandle()); - } - - boost::shared_ptr t; - }; -} - -PLUGINLIB_EXPORT_CLASS(whycon::WhyconNodelet, nodelet::Nodelet) - - From f9921c582878b437a7ee35405309a073db4df31a Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:11:03 +0530 Subject: [PATCH 02/12] chore: files renamed --- include/whycon/{circle_detector.h => circle_detector.hpp} | 0 include/whycon/{localization_system.h => localization_system.hpp} | 0 .../whycon/{many_circle_detector.h => many_circle_detector.hpp} | 0 src/{ros => ros2}/whycon_node.cpp | 0 src/{ros => ros2}/whycon_ros.cpp | 0 src/{ros/whycon_ros.h => ros2/whycon_ros.hpp} | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename include/whycon/{circle_detector.h => circle_detector.hpp} (100%) rename include/whycon/{localization_system.h => localization_system.hpp} (100%) rename include/whycon/{many_circle_detector.h => many_circle_detector.hpp} (100%) rename src/{ros => ros2}/whycon_node.cpp (100%) rename src/{ros => ros2}/whycon_ros.cpp (100%) rename src/{ros/whycon_ros.h => ros2/whycon_ros.hpp} (100%) diff --git a/include/whycon/circle_detector.h b/include/whycon/circle_detector.hpp similarity index 100% rename from include/whycon/circle_detector.h rename to include/whycon/circle_detector.hpp diff --git a/include/whycon/localization_system.h b/include/whycon/localization_system.hpp similarity index 100% rename from include/whycon/localization_system.h rename to include/whycon/localization_system.hpp diff --git a/include/whycon/many_circle_detector.h b/include/whycon/many_circle_detector.hpp similarity index 100% rename from include/whycon/many_circle_detector.h rename to include/whycon/many_circle_detector.hpp diff --git a/src/ros/whycon_node.cpp b/src/ros2/whycon_node.cpp similarity index 100% rename from src/ros/whycon_node.cpp rename to src/ros2/whycon_node.cpp diff --git a/src/ros/whycon_ros.cpp b/src/ros2/whycon_ros.cpp similarity index 100% rename from src/ros/whycon_ros.cpp rename to src/ros2/whycon_ros.cpp diff --git a/src/ros/whycon_ros.h b/src/ros2/whycon_ros.hpp similarity index 100% rename from src/ros/whycon_ros.h rename to src/ros2/whycon_ros.hpp From 1aad6cf6876e038b8ba15e19c141e7d1e05f5fd6 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:17:02 +0530 Subject: [PATCH 03/12] feat: converted from ROS->ROS 2 --- src/ros2/whycon_node.cpp | 21 ++--- src/ros2/whycon_ros.cpp | 168 +++++++++++++++++---------------------- src/ros2/whycon_ros.hpp | 65 +++++++++------ 3 files changed, 123 insertions(+), 131 deletions(-) diff --git a/src/ros2/whycon_node.cpp b/src/ros2/whycon_node.cpp index 427342d..44a2531 100644 --- a/src/ros2/whycon_node.cpp +++ b/src/ros2/whycon_node.cpp @@ -1,11 +1,14 @@ -#include -#include "whycon_ros.h" +#include "rclcpp/rclcpp.hpp" +#include "whycon_ros.hpp" -int main(int argc, char** argv) -{ - ros::init(argc, argv, "whycon"); - ros::NodeHandle n("~"); +/*Instead of passing the node’s name to the library initialization call, + we do the initialization, then pass the node name to the creation of the node object */ - whycon::WhyConROS whycon_ros(n); - ros::spin(); -} +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node_handle = std::make_shared(); + rclcpp::spin(node_handle); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/ros2/whycon_ros.cpp b/src/ros2/whycon_ros.cpp index 55e32bf..9b9b055 100644 --- a/src/ros2/whycon_ros.cpp +++ b/src/ros2/whycon_ros.cpp @@ -1,62 +1,71 @@ -#include +#include "camera_info_manager/camera_info_manager.hpp" //recheck #include -#include -#include +#include "sensor_msgs/msg/camera_info.hpp" +#include +#include #include -#include -#include -#include -#include "whycon_ros.h" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2/LinearMath/Quaternion.h" +#include "whycon_ros.hpp" + +whycon::WhyConROS::WhyConROS() : Node("whycon"), is_tracking(false), should_reset(true),node_handle(std::shared_ptr(this, [](auto *) {})), it(node_handle) -whycon::WhyConROS::WhyConROS(ros::NodeHandle& n) : is_tracking(false), should_reset(true), it(n) { - transformation_loaded = false; - similarity.setIdentity(); - - if (!n.getParam("targets", targets)) throw std::runtime_error("Private parameter \"targets\" is missing"); - - n.param("name", frame_id, std::string("whycon")); - n.param("world_frame", world_frame_id, std::string("world")); - n.param("max_attempts", max_attempts, 1); - n.param("max_refine", max_refine, 1); - - n.getParam("outer_diameter", parameters.outer_diameter); - n.getParam("inner_diameter", parameters.inner_diameter); - n.getParam("center_distance_tolerance_abs", parameters.center_distance_tolerance_abs); - n.getParam("center_distance_tolerance_ratio", parameters.center_distance_tolerance_ratio); - n.getParam("roundness_tolerance", parameters.roundness_tolerance); - n.getParam("circularity_tolerance", parameters.circularity_tolerance); - n.getParam("max_size", parameters.max_size); - n.getParam("min_size", parameters.min_size); - n.getParam("ratio_tolerance", parameters.ratio_tolerance); - n.getParam("max_eccentricity", parameters.max_eccentricity); - - load_transforms(); - transform_broadcaster = boost::make_shared(); + + + this->declare_parameter("targets", 1); + this->declare_parameter("name", "default"); + this->declare_parameter("world_frame", "default"); + this->declare_parameter("outer_diameter", outer_diameter); + this->declare_parameter("inner_diameter", inner_diameter); + + + if (!this->get_parameter("targets", targets)) throw std::runtime_error("Private parameter \"targets\" is missing"); + + this->get_parameter_or("name", frame_id, std::string("whycon")); + this->get_parameter_or("world_frame", world_frame_id, std::string("world")); + this->get_parameter_or("max_attempts", max_attempts, 1); + this->get_parameter_or("max_refine", max_refine, 1); + + this->get_parameter("outer_diameter", parameters.outer_diameter); + this->get_parameter("inner_diameter", parameters.inner_diameter); + this->get_parameter("center_distance_tolerance_abs", parameters.center_distance_tolerance_abs); + this->get_parameter("center_distance_tolerance_ratio", parameters.center_distance_tolerance_ratio); + this->get_parameter("roundness_tolerance", parameters.roundness_tolerance); + this->get_parameter("circularity_tolerance", parameters.circularity_tolerance); + this->get_parameter("max_size", parameters.max_size); + this->get_parameter("min_size", parameters.min_size); + this->get_parameter("ratio_tolerance", parameters.ratio_tolerance); + this->get_parameter("max_eccentricity", parameters.max_eccentricity); + /* initialize ros */ int input_queue_size = 1; - n.param("input_queue_size", input_queue_size, input_queue_size); - cam_sub = it.subscribeCamera("/camera/image_rect_color", input_queue_size, boost::bind(&WhyConROS::on_image, this, _1, _2)); + this->get_parameter_or("input_queue_size", input_queue_size, input_queue_size); + //this->declare_parameter("image_transport", "compressed"); + cam_sub = it.subscribeCamera("/image_raw", input_queue_size, std::bind(&WhyConROS::on_image, this, std::placeholders::_1, std::placeholders::_2)); //check again - image_pub = n.advertise("image_out", 1); - poses_pub = n.advertise("poses", 1); - context_pub = n.advertise("context", 1); - projection_pub = n.advertise("projection", 1); - reset_service = n.advertiseService("reset", &WhyConROS::reset, this); + image_pub = it.advertise("/whycon/image_out", 1); + poses_pub = this->create_publisher("/whycon/poses", 1); + context_pub = it.advertise("/whycon/context", 1); + + + reset_service = this->create_service("reset", std::bind(&WhyConROS::reset, this, std::placeholders::_1, std::placeholders::_2)); + } -void whycon::WhyConROS::on_image(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg) +void whycon::WhyConROS::on_image(const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &info_msg) { - camera_model.fromCameraInfo(info_msg); - if (camera_model.fullResolution().width == 0) { ROS_ERROR_STREAM("camera is not calibrated!"); return; } + camera_model.fromCameraInfo(info_msg); + if (camera_model.fullResolution().width == 0) { RCLCPP_ERROR_STREAM(this->get_logger(), "camera is not calibrated!"); return; } cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "rgb8"); const cv::Mat& image = cv_ptr->image; if (!system) - system = boost::make_shared(targets, image.size().width, image.size().height, cv::Mat(camera_model.fullIntrinsicMatrix()), cv::Mat(camera_model.distortionCoeffs()), parameters); + system = std::make_shared(targets, image.size().width, image.size().height, cv::Mat(camera_model.fullIntrinsicMatrix()), cv::Mat(camera_model.distortionCoeffs()), parameters); is_tracking = system->localize(image, should_reset/*!is_tracking*/, max_attempts, max_refine); @@ -64,28 +73,28 @@ void whycon::WhyConROS::on_image(const sensor_msgs::ImageConstPtr& image_msg, co publish_results(image_msg->header, cv_ptr); should_reset = false; } + else if (image_pub.getNumSubscribers() != 0) - image_pub.publish(cv_ptr); + image_pub.publish(image_msg); if (context_pub.getNumSubscribers() != 0) { cv_bridge::CvImage cv_img_context; cv_img_context.encoding = cv_ptr->encoding; cv_img_context.header.stamp = cv_ptr->header.stamp; system->detector.context.debug_buffer(cv_ptr->image, cv_img_context.image); - context_pub.publish(cv_img_context.toImageMsg()); + context_pub.publish((cv_img_context.toImageMsg())); } } -bool whycon::WhyConROS::reset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) +void whycon::WhyConROS::reset(const std::shared_ptr request, std::shared_ptr response) { should_reset = true; - return true; } -void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv_bridge::CvImageConstPtr& cv_ptr) +void whycon::WhyConROS::publish_results(const std_msgs::msg::Header& header, const cv_bridge::CvImageConstPtr& cv_ptr) { bool publish_images = (image_pub.getNumSubscribers() != 0); - bool publish_poses = (poses_pub.getNumSubscribers() != 0); + bool publish_poses = (poses_pub->get_subscription_count() != 0); if (!publish_images && !publish_poses) return; @@ -94,7 +103,7 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv if (publish_images) output_image = cv_ptr->image.clone(); - geometry_msgs::PoseArray pose_array; + geometry_msgs::msg::PoseArray pose_array; // go through detected targets for (int i = 0; i < system->targets; i++) { @@ -110,15 +119,23 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,255)); /*whycon::CircleDetector::Circle new_circle = circle.improveEllipse(cv_ptr->image); new_circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,0));*/ - cv::circle(output_image, camera_model.project3dToPixel(cv::Point3d(coord)), 1, cv::Scalar(255,0,255), 1, CV_AA); + //cv::circle(output_image, camera_model.project3dToPixel(cv::Point3d(coord)), 1, cv::Scalar(255,0,255), 1, CV_AA); + // The error you're encountering in ROS 2 when using CV_AA is because CV_AA is not defined in OpenCV for ROS 2. In OpenCV for ROS 2, anti-aliased drawing is typically achieved by using the LINE_AA flag. + //You should replace CV_AA with LINE_AA like this: + cv::circle(output_image, camera_model.project3dToPixel(cv::Point3d(coord)), 1, cv::Scalar(255, 0, 255), 1, cv::LINE_AA); + } if (publish_poses) { - geometry_msgs::Pose p; + geometry_msgs::msg::Pose p; p.position.x = pose.pos(0); p.position.y = pose.pos(1); p.position.z = pose.pos(2); - p.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, pose.rot(0), pose.rot(1)); + + tf2::Quaternion tf2_quat; + tf2_quat.setRPY(0, pose.rot(0), pose.rot(1)); + geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat); + p.orientation = msg_quat; pose_array.poses.push_back(p); } } @@ -126,58 +143,15 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv if (publish_images) { cv_bridge::CvImage output_image_bridge = *cv_ptr; output_image_bridge.image = output_image; - image_pub.publish(output_image_bridge); + image_pub.publish(output_image_bridge.toImageMsg()); } if (publish_poses) { pose_array.header = header; pose_array.header.frame_id = frame_id; - poses_pub.publish(pose_array); + poses_pub->publish(pose_array); } - if (transformation_loaded) - { - transform_broadcaster->sendTransform(tf::StampedTransform(similarity, header.stamp, world_frame_id, frame_id)); - whycon::Projection projection_msg; - projection_msg.header = header; - for (size_t i = 0; i < projection.size(); i++) projection_msg.projection[i] = projection[i]; - projection_pub.publish(projection_msg); - } } -void whycon::WhyConROS::load_transforms(void) -{ - std::string filename = frame_id + "_transforms.yml"; - ROS_INFO_STREAM("Loading file " << filename); - - std::ifstream file(filename); - if (!file) { - ROS_WARN_STREAM("Could not load \"" << filename << "\""); - return; - } - - YAML::Node node = YAML::Load(file); - - projection.resize(9); - for (int i = 0; i < 9; i++) - projection[i] = node["projection"][i].as(); - - std::vector similarity_origin(3); - for (int i = 0; i < 3; i++) similarity_origin[i] = node["similarity"]["origin"][i].as(); - - std::vector similarity_rotation(4); - for (int i = 0; i < 4; i++) similarity_rotation[i] = node["similarity"]["rotation"][i].as(); - - tf::Vector3 origin(similarity_origin[0], similarity_origin[1], similarity_origin[2]); - tf::Quaternion Q(similarity_rotation[0], similarity_rotation[1], similarity_rotation[2], similarity_rotation[3]); - - similarity.setOrigin(origin); - similarity.setRotation(Q); - - transformation_loaded = true; - - ROS_INFO_STREAM("Loaded transformation from \"" << filename << "\""); -} - - diff --git a/src/ros2/whycon_ros.hpp b/src/ros2/whycon_ros.hpp index 6c1f2be..cba6aee 100644 --- a/src/ros2/whycon_ros.hpp +++ b/src/ros2/whycon_ros.hpp @@ -1,46 +1,61 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "tf2/transform_datatypes.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" //added +#include "rclcpp/node_options.hpp" //added +#include +#include "whycon/localization_system.hpp" //recheck +//#include +/*ROS 2 provides its own smart pointer types for managing resources and objects. +The primary smart pointer used in ROS 2 is std::shared_ptr, which is part of the C++ Standard Library.*/ +#include "sensor_msgs/msg/image.hpp" +#include "cv_bridge/cv_bridge.h" +#include "image_transport/image_transport.hpp" +#include "image_geometry/pinhole_camera_model.h" +#include "std_srvs/srv/empty.hpp" +#include "geometry_msgs/msg/pose_array.hpp" + namespace whycon { - class WhyConROS { + class WhyConROS : public rclcpp::Node + { public: - WhyConROS(ros::NodeHandle& n); + WhyConROS(); + //WhyConROS(const rclcpp::NodeOptions& options);//added + + void on_image(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg); + + void reset(const std::shared_ptr request, std::shared_ptr response); + + + protected: + rclcpp::Node::SharedPtr node_handle; + image_transport::ImageTransport it; + image_transport::CameraSubscriber cam_sub; + - void on_image(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); - bool reset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); private: void load_transforms(void); - void publish_results(const std_msgs::Header& header, const cv_bridge::CvImageConstPtr& cv_ptr); + void publish_results(const std_msgs::msg::Header& header, const cv_bridge::CvImageConstPtr& cv_ptr); //review later whycon::DetectorParameters parameters; - boost::shared_ptr system; + std::shared_ptr system; bool is_tracking, should_reset; int max_attempts, max_refine; std::string world_frame_id, frame_id; int targets; double xscale, yscale; + double inner_diameter, outer_diameter; - std::vector projection; - tf::Transform similarity; - - image_transport::ImageTransport it; - image_transport::CameraSubscriber cam_sub; - ros::ServiceServer reset_service; + + rclcpp::Service::SharedPtr reset_service; //review later - ros::Publisher image_pub, poses_pub, context_pub, projection_pub; - boost::shared_ptr transform_broadcaster; + image_transport::Publisher image_pub; + rclcpp::Publisher::SharedPtr poses_pub; + image_transport::Publisher context_pub; + image_geometry::PinholeCameraModel camera_model; - bool transformation_loaded; }; } From d04fdb909647d4ecd4123f48afb1999f9e47ae55 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:24:16 +0530 Subject: [PATCH 04/12] feat: Updated header files for ROS2 compatibility --- include/whycon/circle_detector.hpp | 2 +- include/whycon/localization_system.hpp | 2 +- include/whycon/many_circle_detector.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/whycon/circle_detector.hpp b/include/whycon/circle_detector.hpp index 03ea58f..9f4a3c0 100644 --- a/include/whycon/circle_detector.hpp +++ b/include/whycon/circle_detector.hpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include //recheck #include namespace whycon { diff --git a/include/whycon/localization_system.hpp b/include/whycon/localization_system.hpp index 2427c3e..666c6ea 100644 --- a/include/whycon/localization_system.hpp +++ b/include/whycon/localization_system.hpp @@ -2,7 +2,7 @@ #define __LOCALIZATION_SYSYEM__ #include -#include +#include namespace whycon { class LocalizationSystem { diff --git a/include/whycon/many_circle_detector.hpp b/include/whycon/many_circle_detector.hpp index b44cad2..01348d3 100644 --- a/include/whycon/many_circle_detector.hpp +++ b/include/whycon/many_circle_detector.hpp @@ -2,7 +2,7 @@ #define __CIRCLE_LOCALIZER_H__ #include -#include +#include namespace whycon { class ManyCircleDetector { From 232206b15085470a186ecae280ecb6933d38aad4 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:24:51 +0530 Subject: [PATCH 05/12] feat: Updated lib files for ROS2 compatibility --- src/lib/circle_detector.cpp | 8 +++++--- src/lib/localization_system.cpp | 4 ++-- src/lib/many_circle_detector.cpp | 2 +- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/lib/circle_detector.cpp b/src/lib/circle_detector.cpp index 68ee81a..2e5edab 100644 --- a/src/lib/circle_detector.cpp +++ b/src/lib/circle_detector.cpp @@ -1,5 +1,7 @@ #include -#include +#include +#include +#include using namespace std; #define min(a,b) ((a) < (b) ? (a) : (b)) @@ -486,7 +488,7 @@ void whycon::CircleDetector::Circle::draw(cv::Mat& image, const std::string& tex float scale = image.size().width / 1800.0f; //float thickness = scale * 3.0; //if (thickness < 1) thickness = 1; - cv::putText(image, text.c_str(), cv::Point(x + 2 * m0, y + 2 * m1), CV_FONT_HERSHEY_SIMPLEX, scale, cv::Scalar(color), thickness, CV_AA); + cv::putText(image, text.c_str(), cv::Point(x + 2 * m0, y + 2 * m1), cv::FONT_HERSHEY_SIMPLEX, scale, cv::Scalar(color), thickness, cv::LINE_AA); cv::line(image, cv::Point(x + v0 * m0 * 2, y + v1 * m0 * 2), cv::Point(x - v0 * m0 * 2, y - v1 * m0 * 2), cv::Scalar(color), 1, 8); cv::line(image, cv::Point(x + v1 * m1 * 2, y - v0 * m1 * 2), cv::Point(x - v1 * m1 * 2, y + v0 * m1 * 2), cv::Scalar(color), 1, 8); } @@ -601,7 +603,7 @@ whycon::CircleDetector::Circle whycon::CircleDetector::Circle::improveEllipse(co cv::waitKey();*/ std::vector< std::vector > contours; - cv::findContours(cannified, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); + cv::findContours(cannified, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); if (contours.empty() || contours[0].size() < 5) return new_circle; cv::Mat contour_img; diff --git a/src/lib/localization_system.cpp b/src/lib/localization_system.cpp index 4f9bc25..97bf18e 100644 --- a/src/lib/localization_system.cpp +++ b/src/lib/localization_system.cpp @@ -6,8 +6,8 @@ #include #include #include -#include -#include +#include +#include using std::cout; using std::endl; using std::numeric_limits; diff --git a/src/lib/many_circle_detector.cpp b/src/lib/many_circle_detector.cpp index 91fc298..0d48a16 100644 --- a/src/lib/many_circle_detector.cpp +++ b/src/lib/many_circle_detector.cpp @@ -1,5 +1,5 @@ #include -#include +#include using namespace std; whycon::ManyCircleDetector::ManyCircleDetector(int _number_of_circles, int _width, int _height, const whycon::DetectorParameters& parameters) : From 2373165277b4326e97e26a2a5f604db45691fb6d Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:25:26 +0530 Subject: [PATCH 06/12] feat: added ROS 2 launch files --- launch/usb_cam_launch.py | 23 ++++++++++++++++ launch/whycon_only_launch.py | 23 ++++++++++++++++ launch/whycon_ros_launch.py | 51 ++++++++++++++++++++++++++++++++++++ 3 files changed, 97 insertions(+) create mode 100644 launch/usb_cam_launch.py create mode 100644 launch/whycon_only_launch.py create mode 100644 launch/whycon_ros_launch.py diff --git a/launch/usb_cam_launch.py b/launch/usb_cam_launch.py new file mode 100644 index 0000000..b771698 --- /dev/null +++ b/launch/usb_cam_launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + + Node( + package='usb_cam', + name='usb_cam', + executable='usb_cam_node_exe', + output='screen', + parameters=[{ + 'video_device': '/dev/video4', + 'image_width': 1920, + 'image_height': 1080, + 'pixel_format': 'mjpeg2rgb', + 'io_method': 'mmap', + 'framerate': 30.0, + 'camera_frame_id': 'usb_cam', + 'av_device_format': 'YUV422P', + }] + ) + ]) \ No newline at end of file diff --git a/launch/whycon_only_launch.py b/launch/whycon_only_launch.py new file mode 100644 index 0000000..cb172cd --- /dev/null +++ b/launch/whycon_only_launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + + Node( + package='whycon', + name='whycon', + namespace='whycon', + executable='whycon', + output='screen', + parameters=[{ + 'targets': 1, + 'name': 'whycon', + 'outer_diameter': 0.038, + 'inner_diameter': 0.014 + }], + remappings=[ + ('image_raw', '/camera/image_raw') + ] + ), + ]) diff --git a/launch/whycon_ros_launch.py b/launch/whycon_ros_launch.py new file mode 100644 index 0000000..635f0cd --- /dev/null +++ b/launch/whycon_ros_launch.py @@ -0,0 +1,51 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + + Node( + package='usb_cam', + name='usb_cam', + executable='usb_cam_node_exe', + output='screen', + parameters=[{ + 'video_device': '/dev/video4', + 'image_width': 1920, + 'image_height': 1080, + 'pixel_format': 'mjpeg2rgb', + 'io_method': 'mmap', + 'framerate': 30.0, + 'camera_frame_id': 'usb_cam', + 'av_device_format': 'YUV422P', + }] + ), + + Node( + package='whycon', + name='whycon', + namespace='whycon', + executable='whycon', + output='screen', + parameters=[{ + 'targets': 1, + 'name': 'whycon', + 'outer_diameter': 0.38, + 'inner_diameter': 0.14, + }], + remappings=[ + ('image_raw', '/camera/image_raw') + ] + ), + + Node( + package='image_view', + executable='image_view', + namespace='whycon_display', + name='image_view', + output='screen', + remappings=[ + ('image', '/whycon/image_out') + ] + ) + ]) From eded0ff00d7d01d76ef1ff315a41f73288d97d37 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:26:23 +0530 Subject: [PATCH 07/12] feat: whycon for ROS 2 --- CMakeLists.txt | 184 +++++++++++++++++++++---------------------------- package.xml | 63 +++++++---------- 2 files changed, 106 insertions(+), 141 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b59481f..0bf30b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,125 +1,101 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.8) project(whycon) -# set some flags -add_definitions(-std=c++11) -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELEASE} -Wall -g -O4 -march=native -Wfatal-errors") -#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELEASE} -Wall -O0 -g -Wfatal-errors") -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-configs") +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() # set some options -option(DISABLE_ROS "Do not build for ROS, but as standalone code" OFF) option(ENABLE_FULL_UNDISTORT "Undistort the whole frame" OFF) option(ENABLE_RANDOMIZED_THRESHOLD "Use rand() instead of binary-like search for threshold" OFF) option(ENABLE_VERBOSE "Enable verbose console messages during detection" OFF) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/config.h.cmake ${CMAKE_CURRENT_SOURCE_DIR}/include/whycon/config.h) -#### ROS CONFIGURATION #### -if(NOT DISABLE_ROS) - find_package(catkin REQUIRED COMPONENTS - cv_bridge - roscpp - sensor_msgs - std_msgs - camera_info_manager - message_generation - geometry_msgs - message_filters - image_geometry - image_transport - angles - visualization_msgs - tf - nodelet - std_srvs - ) - - add_message_files( - FILES - Projection.msg - ) - - generate_messages( - DEPENDENCIES geometry_msgs - ) - - catkin_package( - INCLUDE_DIRS include - LIBRARIES whycon - CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs message_runtime geometry_msgs - ) -endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(camera_info_manager REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) ### SYSTEM DEPENDENCIES ### find_package(OpenCV REQUIRED) message(STATUS "Using OpenCV version ${OpenCV_VERSION}") -find_package(Boost COMPONENTS program_options thread system REQUIRED) - -find_package(PkgConfig) -pkg_check_modules(YAML_CPP yaml-cpp) ### TARGETS ### -include_directories( - ${catkin_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} include -) +include_directories(include) add_library(whycon SHARED src/lib/circle_detector.cpp src/lib/many_circle_detector.cpp src/lib/localization_system.cpp) -target_link_libraries(whycon ${OpenCV_LIBS} ${Boost_LIBRARIES}) - -if(NOT DISABLE_ROS) - add_executable(whycon-node src/ros/whycon_node.cpp src/ros/whycon_ros.cpp) - set_target_properties(whycon-node PROPERTIES OUTPUT_NAME whycon) - - add_library(whycon_nodelet src/ros/whycon_nodelet.cpp src/ros/whycon_ros.cpp) - - add_executable(set_axis src/ros/set_axis_node.cpp src/ros/set_axis.cpp) - #add_executable(triangulator src/ros/triangulator_node.cpp src/ros/triangulator.cpp) - add_executable(robot_pose_publisher src/ros/robot_pose_publisher.cpp src/ros/robot_pose_publisher_node.cpp) - add_executable(transformer src/ros/transformer.cpp src/ros/transformer_node.cpp) - - add_dependencies(whycon-node whycon_generate_messages_cpp) - add_dependencies(whycon_nodelet whycon_generate_messages_cpp) - add_dependencies(set_axis whycon_generate_messages_cpp) - #add_dependencies(triangulator whycon_generate_messages_cpp) - add_dependencies(robot_pose_publisher whycon_generate_messages_cpp) - add_dependencies(transformer whycon_generate_messages_cpp) - - target_link_libraries(whycon-node ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} whycon) - target_link_libraries(whycon_nodelet ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} whycon) - target_link_libraries(set_axis ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} whycon) - target_link_libraries(transformer ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} whycon) - #target_link_libraries(triangulator ${catkin_LIBRARIES} whycon) - target_link_libraries(robot_pose_publisher ${catkin_LIBRARIES} whycon) -else() - add_executable(whycon-main src/main.cpp) - set_target_properties(whycon-main PROPERTIES OUTPUT_NAME whycon) - target_link_libraries(whycon-main whycon) - - add_executable(camera-calibrator src/camera_calibrator.cpp) - target_link_libraries(camera-calibrator ${OpenCV_LIBS}) -endif() +target_link_libraries(whycon ${OpenCV_LIBS}) + +add_executable(whycon-node src/ros2/whycon_node.cpp src/ros2/whycon_ros.cpp) +set_target_properties(whycon-node PROPERTIES OUTPUT_NAME whycon) + +ament_target_dependencies(whycon-node +rclcpp +camera_info_manager +sensor_msgs +geometry_msgs +tf2_geometry_msgs +tf2 +cv_bridge +image_transport +image_geometry +std_srvs +) + +target_link_libraries(whycon-node whycon) ### INSTALL ### ### Mark cpp header files for installation -if(NOT DISABLE_ROS) - install(DIRECTORY include/${PACKAGE_NAME} - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - ) - ## Mark executables and/or libraries for installation - install(TARGETS whycon whycon-node robot_pose_publisher #triangulator - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - - ## Mark executables and/or libraries for installation - install(FILES nodelets.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) -else() - install(TARGETS whycon DESTINATION lib) - install(TARGETS whycon-main camera-calibrator DESTINATION bin) - install(DIRECTORY include/whycon DESTINATION include) - install(FILES cmake-configs/FindWhyCon.cmake DESTINATION share/cmake/Modules) -endif() + +install(DIRECTORY include/ +DESTINATION include/${PROJECT_NAME} +) +## Mark executables and/or libraries for installation +install(TARGETS + whycon + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS whycon-node + DESTINATION lib/${PROJECT_NAME} +) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_dependencies( + cv_bridge + rclcpp + sensor_msgs + std_msgs + geometry_msgs + tf2_geometry_msgs + tf2 +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_export_include_directories( + include +) + +ament_export_libraries( + whycon +) + +ament_package() diff --git a/package.xml b/package.xml index 982b811..70a3bbc 100644 --- a/package.xml +++ b/package.xml @@ -1,53 +1,42 @@ - + + whycon - 1.2.0 - - WhyCon localization system - - Matias Nitsche - Matias Nitsche - https://github.com/lrse/whycon - https://github.com/lrse/whycon + 2.0.0 + WhyCon for ROS 2 + arunser LGPLv3 - catkin + ament_cmake - message_generation cv_bridge - roscpp + rclcpp sensor_msgs - std_msgs camera_info_manager image_geometry - angles std_srvs - tf - visualization_msgs - nodelet - - message_runtime - cv_bridge - roscpp - sensor_msgs - std_msgs - camera_info_manager - image_geometry - angles - std_srvs - visualization_msgs - tf - nodelet - + tf2 + tf2_geometry_msgs geometry_msgs image_transport - message_filters - geometry_msgs - image_transport - message_filters - - + cv_bridge + rclcpp + sensor_msgs + camera_info_manager + image_geometry + std_srvs + tf2_geometry_msgs + tf2 + geometry_msgs + image_transport + ros2launch + + ament_lint_auto + ament_lint_common + + + ament_cmake From 81026cb114e6a4c27faf1494a2287b81aae97bcd Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:26:52 +0530 Subject: [PATCH 08/12] docs: README updated --- README.md | 58 ++++++++++++------------------------------------------- 1 file changed, 12 insertions(+), 46 deletions(-) diff --git a/README.md b/README.md index 322985d..2568351 100644 --- a/README.md +++ b/README.md @@ -46,73 +46,39 @@ Note that this .bib includes not only the references to the scientific works tha ## Installing WhyCon -The code can be compiled either as a ROS package (shared library) or in a standalone version. +The code can be compiled as a ROS 2 package -**NOTE**: while the standalone version includes a demo application, this demo is not actively maintained anymore and will probably be removed soon. The reference application is provided as a series of ROS nodes, which utilize the _WhyCon_ shared library. For an example of how to implement your own standalone application, see the ROS node. - -Stable [releases](https://github.com/lrse/whycon/releases) are available periodically. Latest stable release can be downloaded by clicking [here](https://github.com/lrse/whycon/releases/latest). +Stable [releases](https://github.com/arunser/whycon2/releases) are available periodically. Latest stable release can be downloaded by clicking [here](https://github.com/arunser/whycon2/releases/latest). For the latest development version (which should also work and may contain new experimental features) you can clone the repository directly. -### ROS +### ROS 2 -Only LTS versions are targeted. At the moment, Indigo and Kinetic are targeted. +Only LTS versions are targeted. At the moment, only Humble is targeted. #### Dependencies It is recommended to install required dependencies using - rosdep install -y --from-path - -**NOTE**: it is recommended to use OpenCV 3 since it is actually latest stable version. In ROS Kinetic this is installed by default. -On ROS Indigo, you should install it by doing: - - apt-get install ros-indigo-opencv3 + sudo apt install ros-humble-usb-cam + sudo apt install ros-humble-image-view #### Compilation -The main directory should be placed inside a catkin workspace source-space (e.g.: ~catkin_ws/src). -It can then be compiled simply by: - - catkin_make - -Or, if you are using catkin-tools - - catkin build - -### Standalone - -**NOTE**: as previously mentioned, this version is not actively maintained and cannot provide support for it - -The standalone version requires you to take care of installing the correct dependencies: OpenCV and Boost. If you are on Ubuntu, simply perform the following: - - sudo apt-get install libopencv-dev libboost-all-dev - -The installation process is really straightforward, as with any CMake based project. -Inside the main directory do: - - mkdir build - cd build - cmake -DDISABLE_ROS=ON .. - make - -The code can be installed to the system by doing: - - make install - -Note the default CMake location is `/usr/local`, but you can redefine this by invoking cmake in this way instead: +The main directory should be placed inside a colcon workspace source-space (e.g.: ~whycon_ws/src). +It can then be build using: + + cd ~whycon_ws/ + colcon build - cmake -DDISABLE_ROS=ON -DCMAKE_INSTALL_PREFIX=/usr .. ## Using WhyCon -Please refer to the [wiki](https://github.com/lrse/whycon/wiki). +Please refer to the [wiki](https://github.com/arunser/whycon2/wiki). ---- ### Acknowledgements -The development of this work was supported by EU within its Seventh Framework Programme project ICT-600623 ``STRANDS''. -The Czech Republic and Argentina have given support through projects 7AMB12AR022, ARC/11/11 and 13-18316P. From 6060b633d468913628579dc1fd70968636035115 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:38:54 +0530 Subject: [PATCH 09/12] chore: gitignore updated --- .gitignore | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index ad48910..1d74e21 100644 --- a/.gitignore +++ b/.gitignore @@ -1,14 +1 @@ -build -include/whycon/config.h -papers -Calib_Results* -whycon.config -whycon.creator* -whycon.includes -whycon.files -doc -images -*.m -TODO -videos -wiki +.vscode/ From acdc2a4eec040d09687d98a338a490e555641f56 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 00:40:06 +0530 Subject: [PATCH 10/12] config.h added --- .gitlab-ci.yml | 21 --------------------- include/whycon/config.h | 14 ++++++++++++++ 2 files changed, 14 insertions(+), 21 deletions(-) delete mode 100644 .gitlab-ci.yml create mode 100644 include/whycon/config.h diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml deleted file mode 100644 index c881b9f..0000000 --- a/.gitlab-ci.yml +++ /dev/null @@ -1,21 +0,0 @@ -before_script: - - /prepare.sh - -indigo_build: - tags: - - ros-indigo - script: - - /build.sh - -indigo_opencv3_build: - tags: - - ros-indigo - script: - - apt-get install -y ros-indigo-opencv3 - - /build.sh - -kinetic_build: - tags: - - ros-kinetic - script: - - /build.sh diff --git a/include/whycon/config.h b/include/whycon/config.h new file mode 100644 index 0000000..135f4df --- /dev/null +++ b/include/whycon/config.h @@ -0,0 +1,14 @@ +#ifndef __CONFIG_H__ +#define __CONFIG_H__ + +/* #undef ENABLE_FULL_UNDISTORT */ +/* #undef ENABLE_RANDOMIZED_THRESHOLD */ +/* #undef ENABLE_VERBOSE */ + +#if defined(ENABLE_VERBOSE) +#define WHYCON_DEBUG(x) cout << x << endl +#else +#define WHYCON_DEBUG(x) +#endif + +#endif From b8c23cb38733d8cad681a275580c276cc19b7fb3 Mon Sep 17 00:00:00 2001 From: Arun P Madhu Date: Wed, 4 Sep 2024 02:04:08 +0530 Subject: [PATCH 11/12] cmake-configs added --- cmake-configs/FindTBB.cmake | 283 +++++++++++++++++++++++++ cmake-configs/FindWhyCon.cmake | 13 ++ cmake-configs/HandleLibraryTypes.cmake | 48 +++++ config.h.cmake | 14 ++ 4 files changed, 358 insertions(+) create mode 100644 cmake-configs/FindTBB.cmake create mode 100644 cmake-configs/FindWhyCon.cmake create mode 100644 cmake-configs/HandleLibraryTypes.cmake create mode 100644 config.h.cmake diff --git a/cmake-configs/FindTBB.cmake b/cmake-configs/FindTBB.cmake new file mode 100644 index 0000000..f9e3e0f --- /dev/null +++ b/cmake-configs/FindTBB.cmake @@ -0,0 +1,283 @@ +# Locate Intel Threading Building Blocks include paths and libraries +# FindTBB.cmake can be found at https://code.google.com/p/findtbb/ +# Written by Hannes Hofmann +# Improvements by Gino van den Bergen , +# Florian Uhlig , +# Jiri Marsik + +# The MIT License +# +# Copyright (c) 2011 Hannes Hofmann +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler. +# e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21" +# TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found +# in the TBB installation directory (TBB_INSTALL_DIR). +# +# GvdB: Mac OS X distribution places libraries directly in lib directory. +# +# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER. +# TBB_ARCHITECTURE [ ia32 | em64t | itanium ] +# which architecture to use +# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9 +# which compiler to use (detected automatically on Windows) + +# This module respects +# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR} + +# This module defines +# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc. +# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc +# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug +# TBB_INSTALL_DIR, the base TBB install directory +# TBB_LIBRARIES, the libraries to link against to use TBB. +# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols. +# TBB_FOUND, If false, don't try to use TBB. +# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h + + +if (WIN32) + # has em64t/vc8 em64t/vc9 + # has ia32/vc7.1 ia32/vc8 ia32/vc9 + set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB" "C:/Program Files (x86)/Intel/TBB") + set(_TBB_LIB_NAME "tbb") + set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") + set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") + set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") + if (MSVC71) + set (_TBB_COMPILER "vc7.1") + endif(MSVC71) + if (MSVC80) + set(_TBB_COMPILER "vc8") + endif(MSVC80) + if (MSVC90) + set(_TBB_COMPILER "vc9") + endif(MSVC90) + if(MSVC10) + set(_TBB_COMPILER "vc10") + endif(MSVC10) + # Todo: add other Windows compilers such as ICL. + set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) +endif (WIN32) + +if (UNIX) + if (APPLE) + # MAC + set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions") + # libs: libtbb.dylib, libtbbmalloc.dylib, *_debug + set(_TBB_LIB_NAME "tbb") + set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") + set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") + set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") + # default flavor on apple: ia32/cc4.0.1_os10.4.9 + # Jiri: There is no reason to presume there is only one flavor and + # that user's setting of variables should be ignored. + if(NOT TBB_COMPILER) + set(_TBB_COMPILER "cc4.0.1_os10.4.9") + elseif (NOT TBB_COMPILER) + set(_TBB_COMPILER ${TBB_COMPILER}) + endif(NOT TBB_COMPILER) + if(NOT TBB_ARCHITECTURE) + set(_TBB_ARCHITECTURE "ia32") + elseif(NOT TBB_ARCHITECTURE) + set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) + endif(NOT TBB_ARCHITECTURE) + else (APPLE) + # LINUX + set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include") + set(_TBB_LIB_NAME "tbb") + set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc") + set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug") + set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug") + # has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21 + # has ia32/* + # has itanium/* + set(_TBB_COMPILER ${TBB_COMPILER}) + set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) + endif (APPLE) +endif (UNIX) + +if (CMAKE_SYSTEM MATCHES "SunOS.*") +# SUN +# not yet supported +# has em64t/cc3.4.3_kernel5.10 +# has ia32/* +endif (CMAKE_SYSTEM MATCHES "SunOS.*") + + +#-- Clear the public variables +set (TBB_FOUND "NO") + + +#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR} +# first: use CMake variable TBB_INSTALL_DIR +if (TBB_INSTALL_DIR) + set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR}) +endif (TBB_INSTALL_DIR) +# second: use environment variable +if (NOT _TBB_INSTALL_DIR) + if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB_INSTALL_DIR}) + endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "") + # Intel recommends setting TBB21_INSTALL_DIR + if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB21_INSTALL_DIR}) + endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "") + if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB22_INSTALL_DIR}) + endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "") + if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "") + set (_TBB_INSTALL_DIR $ENV{TBB30_INSTALL_DIR}) + endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "") +endif (NOT _TBB_INSTALL_DIR) +# third: try to find path automatically +if (NOT _TBB_INSTALL_DIR) + if (_TBB_DEFAULT_INSTALL_DIR) + set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR}) + endif (_TBB_DEFAULT_INSTALL_DIR) +endif (NOT _TBB_INSTALL_DIR) +# sanity check +if (NOT _TBB_INSTALL_DIR) + message ("ERROR: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}") +else (NOT _TBB_INSTALL_DIR) +# finally: set the cached CMake variable TBB_INSTALL_DIR +if (NOT TBB_INSTALL_DIR) + set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory") + mark_as_advanced(TBB_INSTALL_DIR) +endif (NOT TBB_INSTALL_DIR) + + +#-- A macro to rewrite the paths of the library. This is necessary, because +# find_library() always found the em64t/vc9 version of the TBB libs +macro(TBB_CORRECT_LIB_DIR var_name) +# if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t") + string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}}) +# endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t") + string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}}) + string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) + string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) + string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) + string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}}) +endmacro(TBB_CORRECT_LIB_DIR var_content) + + +#-- Look for include directory and set ${TBB_INCLUDE_DIR} +set (TBB_INC_SEARCH_DIR ${_TBB_INSTALL_DIR}/include) +# Jiri: tbbvars now sets the CPATH environment variable to the directory +# containing the headers. +find_path(TBB_INCLUDE_DIR + tbb/task_scheduler_init.h + PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH +) +mark_as_advanced(TBB_INCLUDE_DIR) + + +#-- Look for libraries +# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh] +if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "") + set (_TBB_LIBRARY_DIR + ${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM} + ${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib + ) +endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "") +# Jiri: This block isn't mutually exclusive with the previous one +# (hence no else), instead I test if the user really specified +# the variables in question. +if ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL "")) + # HH: deprecated + message(STATUS "[Warning] FindTBB.cmake: The use of TBB_ARCHITECTURE and TBB_COMPILER is deprecated and may not be supported in future versions. Please set \$ENV{TBB_ARCH_PLATFORM} (using tbbvars.[bat|csh|sh]).") + # Jiri: It doesn't hurt to look in more places, so I store the hints from + # ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER + # variables and search them both. + set (_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR}) +endif ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL "")) + +# GvdB: Mac OS X distribution places libraries directly in lib directory. +list(APPEND _TBB_LIBRARY_DIR ${_TBB_INSTALL_DIR}/lib) + +# Jiri: No reason not to check the default paths. From recent versions, +# tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH +# variables, which now point to the directories of the lib files. +# It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS +# argument instead of the implicit PATHS as it isn't hard-coded +# but computed by system introspection. Searching the LIBRARY_PATH +# and LD_LIBRARY_PATH environment variables is now even more important +# that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates +# the use of TBB built from sources. +find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) +find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) + +#Extract path from TBB_LIBRARY name +get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH) + +#TBB_CORRECT_LIB_DIR(TBB_LIBRARY) +#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY) +mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY) + +#-- Look for debug libraries +# Jiri: Changed the same way as for the release libraries. +find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) +find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR} + PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH) + +# Jiri: Self-built TBB stores the debug libraries in a separate directory. +# Extract path from TBB_LIBRARY_DEBUG name +get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH) + +#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG) +#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG) +mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG) + + +if (TBB_INCLUDE_DIR) + if (TBB_LIBRARY) + set (TBB_FOUND "YES") + set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES}) + set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES}) + set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE) + set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE) + # Jiri: Self-built TBB stores the debug libraries in a separate directory. + set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE) + mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES) + message(STATUS "Found Intel TBB") + endif (TBB_LIBRARY) +endif (TBB_INCLUDE_DIR) + +if (NOT TBB_FOUND) + message("ERROR: Intel TBB NOT found!") + message(STATUS "Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}") + # do only throw fatal, if this pkg is REQUIRED + if (TBB_FIND_REQUIRED) + message(FATAL_ERROR "Could NOT find TBB library.") + endif (TBB_FIND_REQUIRED) +endif (NOT TBB_FOUND) + +endif (NOT _TBB_INSTALL_DIR) + +if (TBB_FOUND) + set(TBB_INTERFACE_VERSION 0) + FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS) + STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}") + set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}") +endif (TBB_FOUND) diff --git a/cmake-configs/FindWhyCon.cmake b/cmake-configs/FindWhyCon.cmake new file mode 100644 index 0000000..75107fe --- /dev/null +++ b/cmake-configs/FindWhyCon.cmake @@ -0,0 +1,13 @@ +find_path(WhyCon_INCLUDE_DIRS whycon/localization_system.h PATH_SUFFIXES whycon HINTS ${CMAKE_CURRENT_LIST_DIR}/../../../include) +find_library(WhyCon_LIBRARIES NAMES whycon HINTS ${CMAKE_CURRENT_LIST_DIR}/../../../lib) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(whycon DEFAULT_MSG WhyCon_LIBRARIES WhyCon_INCLUDE_DIRS) + +find_package(OpenCV REQUIRED) +find_package(Boost COMPONENTS program_options thread system REQUIRED) + +set(WhyCon_LIBRARIES ${WHYCON_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES}) +set(WhyCon_INCLUDE_DIRS ${WHYCON_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) + +mark_as_advanced(WhyCon_LIBRARIES WhyCon_INCLUDE_DIRS) diff --git a/cmake-configs/HandleLibraryTypes.cmake b/cmake-configs/HandleLibraryTypes.cmake new file mode 100644 index 0000000..7dd0bc2 --- /dev/null +++ b/cmake-configs/HandleLibraryTypes.cmake @@ -0,0 +1,48 @@ +##====================================================================== +# +# PIXHAWK Micro Air Vehicle Flying Robotics Toolkit +# Please see our website at +# +# +# Original Authors: +# @author Reto Grieder +# @author Fabian Landau +# Contributing Authors (in alphabetical order): +# +# Todo: +# +# +# (c) 2009 PIXHAWK PROJECT +# +# This file is part of the PIXHAWK project +# +# PIXHAWK is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# PIXHAWK is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with PIXHAWK. If not, see . +# +##======================================================================== + +FUNCTION(HANDLE_LIBRARY_TYPES _name) + # Additional libraries can be added as additional arguments + IF(${_name}_LIBRARY_DEBUG AND ${_name}_LIBRARY_OPTIMIZED) + SET(${_name}_LIBRARY + optimized ${${_name}_LIBRARY_OPTIMIZED} ${ARGN} + debug ${${_name}_LIBRARY_DEBUG} ${ARGN} + PARENT_SCOPE + ) + ELSE() + SET(${_name}_LIBRARY + ${${_name}_LIBRARY_OPTIMIZED} ${ARGN} + PARENT_SCOPE + ) + ENDIF() +ENDFUNCTION(HANDLE_LIBRARY_TYPES) diff --git a/config.h.cmake b/config.h.cmake new file mode 100644 index 0000000..6f4e2d3 --- /dev/null +++ b/config.h.cmake @@ -0,0 +1,14 @@ +#ifndef __CONFIG_H__ +#define __CONFIG_H__ + +#cmakedefine ENABLE_FULL_UNDISTORT +#cmakedefine ENABLE_RANDOMIZED_THRESHOLD +#cmakedefine ENABLE_VERBOSE + +#if defined(ENABLE_VERBOSE) +#define WHYCON_DEBUG(x) cout << x << endl +#else +#define WHYCON_DEBUG(x) +#endif + +#endif From 4ccf9bacd0e4b876920ef537d7e148ea03d56aab Mon Sep 17 00:00:00 2001 From: Arun P Madhu <73698669+arunser@users.noreply.github.com> Date: Wed, 27 Nov 2024 14:47:32 +0530 Subject: [PATCH 12/12] Update whycon_ros.cpp image transport type has been changed from 'raw' to 'compressed' to keep the rate of incoming images fixed at 30. --- src/ros2/whycon_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2/whycon_ros.cpp b/src/ros2/whycon_ros.cpp index 9b9b055..f5cc792 100644 --- a/src/ros2/whycon_ros.cpp +++ b/src/ros2/whycon_ros.cpp @@ -43,7 +43,7 @@ whycon::WhyConROS::WhyConROS() : Node("whycon"), is_tracking(false), should_rese /* initialize ros */ int input_queue_size = 1; this->get_parameter_or("input_queue_size", input_queue_size, input_queue_size); - //this->declare_parameter("image_transport", "compressed"); + this->declare_parameter("image_transport", "compressed"); cam_sub = it.subscribeCamera("/image_raw", input_queue_size, std::bind(&WhyConROS::on_image, this, std::placeholders::_1, std::placeholders::_2)); //check again