Hlwy · NicholasLeone · Nov 9, 2020 · Nov 9, 2020 · Nov 11, 2020 · Nov 11, 2020
diff --git a/CMakeLists.txt b/CMakeLists.txt
diff --git a/cmake/RoboCommanderConfig.cmake.in b/cmake/RoboCommanderConfig.cmake.in
@@ -1,42 +1,14 @@
 # Import cmake variables created in main CMakeLists.txt
-get_filename_component(RoboCommander_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
-set(RoboCommander_CONF_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
-set(RoboCommander_INCLUDES "@CONF_INCLUDES@")
-set(RoboCommander_HEADERS "@CONF_HEADERS@")
-set(RoboCommander_LIBRARIES "@CONF_LIBRARIES@")
-set(RoboCommander_ROOT "@CONF_LOCAL_SRC_ROOT@")
-set(RoboCommander_DESTS "@CONF_DEST_DIRS@")
-set(RoboCommander_INSTALLED_CMAKE_DIR "@CONF_CMAKE_DEST_DIR@")
-set(RoboCommander_INSTALLED_LIBRARIES_DIR "@CONF_LIB_DEST_DIR@")
-set(RoboCommander_INCLUDE_DIR "@CONF_INC_DEST_DIR@")
-set(RoboCommander_EXECUTABLES_DIR "@CONF_EXEC_DEST_DIR@")
+get_filename_component(robocommander_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+set(robocommander_CONF_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
+set(robocommander_INCLUDES "@CONF_INCLUDES@")
+set(robocommander_HEADERS "@CONF_HEADERS@")
+set(robocommander_LIBRARIES "@CONF_LIBRARIES@")
+set(robocommander_ROOT "@CONF_LOCAL_SRC_ROOT@")
+set(robocommander_DESTS "@CONF_DEST_DIRS@")
+set(robocommander_INSTALLED_CMAKE_DIR "@CONF_CMAKE_DEST_DIR@")
+set(robocommander_INSTALLED_LIBRARIES_DIR "@CONF_LIB_DEST_DIR@")
+set(robocommander_INCLUDE_DIR "@CONF_INC_DEST_DIR@")
+set(robocommander_EXECUTABLES_DIR "@CONF_EXEC_DEST_DIR@")
 
-include("${RoboCommander_CMAKE_DIR}/RoboCommanderTargets.cmake")
-
-# Debugging
-# --------
-# message(STATUS "RoboCommanderConfig Source Code Root Directory ------ ${RoboCommander_ROOT}")
-# message(STATUS "RoboCommanderConfig Installed CMake Files Directory ------ ${RoboCommander_INSTALLED_CMAKE_DIR}")
-# message(STATUS "RoboCommanderConfig Installed Libraries Directory ------ ${RoboCommander_INSTALLED_LIBRARIES_DIR}")
-# message(STATUS "RoboCommanderConfig Installed Include Directory ------ ${RoboCommander_INSTALLED_INCLUDES_DIR}")
-# message(STATUS "RoboCommanderConfig Installed Executables Directory ------ ${RoboCommander_EXECUTABLES_DIR}")
-
-# message(STATUS "RoboCommanderConfig Install Destinations ------")
-# foreach(dir ${RoboCommander_DESTS})
-#      message(STATUS "         included='${dir}'")
-# endforeach()
-#
-# message(STATUS "RoboCommanderConfig Headers to install ------")
-# foreach(dir ${RoboCommander_HEADERS})
-#      message(STATUS "         included='${dir}'")
-# endforeach()
-#
-# message(STATUS "RoboCommanderConfig Libraries to install ------")
-# foreach(dir ${RoboCommander_LIBRARIES})
-#      message(STATUS "         included='${dir}'")
-# endforeach()
-#
-# message(STATUS "RoboCommanderConfig Include Directories to install ------")
-# foreach(dir ${RoboCommander_INCLUDES})
-#      message(STATUS "         included='${dir}'")
-# endforeach()
+include("${robocommander_CMAKE_DIR}/robocommanderTargets.cmake")
diff --git a/cmake/RoboCommanderConfigVersion.cmake.in b/cmake/RoboCommanderConfigVersion.cmake.in
@@ -1,4 +1,5 @@
-set(PACKAGE_VERSION "@RoboCommander_VERSION@")
+set(PACKAGE_VERSION "@${PROJECT_NAME}_VERSION@")
+# set(PACKAGE_VERSION "@RoboCommander_VERSION@")
 
 # Check whether the requested PACKAGE_FIND_VERSION is compatible
 if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")

diff --git a/cmake/robocommanderConfig.cmake.in b/cmake/robocommanderConfig.cmake.in
@@ -0,0 +1,42 @@
+# Import cmake variables created in main CMakeLists.txt
+get_filename_component(robocommander_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+set(robocommander_CONF_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
+set(robocommander_INCLUDES "@CONF_INCLUDES@")
+set(robocommander_HEADERS "@CONF_HEADERS@")
+set(robocommander_LIBRARIES "@CONF_LIBRARIES@")
+set(robocommander_ROOT "@CONF_LOCAL_SRC_ROOT@")
+set(robocommander_DESTS "@CONF_DEST_DIRS@")
+set(robocommander_INSTALLED_CMAKE_DIR "@CONF_CMAKE_DEST_DIR@")
+set(robocommander_INSTALLED_LIBRARIES_DIR "@CONF_LIB_DEST_DIR@")
+set(robocommander_INCLUDE_DIR "@CONF_INC_DEST_DIR@")
+set(robocommander_EXECUTABLES_DIR "@CONF_EXEC_DEST_DIR@")
+
+include("${robocommander_CMAKE_DIR}/robocommanderTargets.cmake")
+
+# Debugging
+# --------
+# message(STATUS "robocommanderConfig Source Code Root Directory ------ ${robocommander_ROOT}")
+# message(STATUS "robocommanderConfig Installed CMake Files Directory ------ ${robocommander_INSTALLED_CMAKE_DIR}")
+# message(STATUS "robocommanderConfig Installed Libraries Directory ------ ${robocommander_INSTALLED_LIBRARIES_DIR}")
+# message(STATUS "robocommanderConfig Installed Include Directory ------ ${robocommander_INSTALLED_INCLUDES_DIR}")
+# message(STATUS "robocommanderConfig Installed Executables Directory ------ ${robocommander_EXECUTABLES_DIR}")
+
+# message(STATUS "robocommanderConfig Install Destinations ------")
+# foreach(dir ${robocommander_DESTS})
+#      message(STATUS "         included='${dir}'")
+# endforeach()
+#
+# message(STATUS "robocommanderConfig Headers to install ------")
+# foreach(dir ${robocommander_HEADERS})
+#      message(STATUS "         included='${dir}'")
+# endforeach()
+#
+# message(STATUS "robocommanderConfig Libraries to install ------")
+# foreach(dir ${robocommander_LIBRARIES})
+#      message(STATUS "         included='${dir}'")
+# endforeach()
+#
+# message(STATUS "robocommanderConfig Include Directories to install ------")
+# foreach(dir ${robocommander_INCLUDES})
+#      message(STATUS "         included='${dir}'")
+# endforeach()
diff --git a/cmake/robocommanderConfigVersion.cmake.in b/cmake/robocommanderConfigVersion.cmake.in
@@ -0,0 +1,11 @@
+set(PACKAGE_VERSION "@robocommander_VERSION@")
+
+# Check whether the requested PACKAGE_FIND_VERSION is compatible
+if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
+  set(PACKAGE_VERSION_COMPATIBLE FALSE)
+else()
+  set(PACKAGE_VERSION_COMPATIBLE TRUE)
+  if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
+    set(PACKAGE_VERSION_EXACT TRUE)
+  endif()
+endif()
diff --git a/include/algorithms/vboats/obstacle.h b/include/algorithms/vboats/obstacle.h
@@ -0,0 +1,34 @@
+#ifndef VBOATS_OBSTACLE_H_
+#define VBOATS_OBSTACLE_H_
+
+#include <vector>
+#include <opencv2/core/types.hpp>
+
+class Obstacle{
+public:
+    Obstacle(std::vector<cv::Point> pts, std::vector<int> dBounds);
+
+    void update(bool depth_based,
+         float cam_baseline = 0, float cam_dscale = 0,
+         std::vector<float> cam_focal = {},
+         std::vector<float> cam_principal_point = {},
+         float dtype_gain = 0,
+         float aux_dist_factor = 0,
+         bool verbose = true
+    );
+
+    cv::Point3f get_location();
+    std::string toString();
+
+public:
+    int dMin;
+    int dMax;
+    cv::Point maxXY;
+    cv::Point minXY;
+    double _angle;
+    float _distance;
+    cv::Point3f _location;
+
+};
+
+#endif // VBOATS_OBSTACLE_H_
diff --git a/include/algorithms/vboats/umap_processing_params.h b/include/algorithms/vboats/umap_processing_params.h
@@ -0,0 +1,53 @@
+#ifndef VBOATS_UMAP_PROCESSING_PARAMS_H_
+#define VBOATS_UMAP_PROCESSING_PARAMS_H_
+
+#include <vector>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+
+class UmapProcessingParams{
+public:
+     UmapProcessingParams() : contour_filtering_offset(0, 0){}
+
+     // Contour Filtering
+     cv::Point contour_filtering_offset;
+     float contour_filtering_thresh_min = 40.0;
+     float contour_filtering_thresh_max = -1.0;
+
+     // Stripping-Based Filtering Params
+     std::vector<float> stripping_threshs = {0.3, 0.295, 0.275, 0.3};
+     // Sobelized-Based Filtering Params
+     std::vector<int> sobel_kernel_multipliers = {10, 2};
+     int sobel_thresh_pre_sobel         = 15;
+     int sobel_thresh_sobel_preprocess  = 10;
+     int sobel_thresh_sobel_postprocess = 2;
+     int sobel_dilate_size              = 1;
+     int sobel_blur_size                = 3;
+
+     // Setters
+     void set_contour_filtering_threshold_min(float value){ this->contour_filtering_thresh_min = value; }
+     void set_contour_filtering_threshold_max(float value){ this->contour_filtering_thresh_max = value; }
+
+     void set_threshold_before_sobelizing(int value){ this->sobel_thresh_pre_sobel = value; }
+     void set_threshold_sobelize_preprocessing(int value){ this->sobel_thresh_sobel_preprocess = value; }
+     void set_threshold_sobelize_postprocessing(int value){ this->sobel_thresh_sobel_postprocess = value; }
+     void set_sobelize_dilate_size(int value){ this->sobel_dilate_size = value; }
+     void set_sobelize_blur_size(int value){ this->sobel_blur_size = value; }
+
+     void set_stripping_thresholds(std::vector<float> values){
+          this->stripping_threshs.clear();
+          this->stripping_threshs.assign(values.begin(), values.end());
+     }
+     void set_sobelize_kernel_x_multiplier(int value){ this->sobel_kernel_multipliers.at(0) = value; }
+     void set_sobelize_kernel_y_multiplier(int value){ this->sobel_kernel_multipliers.at(1) = value; }
+     void set_sobelize_kernel_multipliers(int x, int y){
+          this->sobel_kernel_multipliers.at(0) = x;
+          this->sobel_kernel_multipliers.at(1) = y;
+     }
+     void set_contour_offset(int x, int y){
+          this->contour_filtering_offset.x = x;
+          this->contour_filtering_offset.y = y;
+     }
+};
+
+#endif // VBOATS_UMAP_PROCESSING_PARAMS_H_
diff --git a/include/algorithms/vboats/vboats_original.h b/include/algorithms/vboats/vboats_original.h
@@ -0,0 +1,94 @@
+#ifndef ROBOCOMMANDER_ALGORITHMS_VBOATS_H_
+#define ROBOCOMMANDER_ALGORITHMS_VBOATS_H_
+
+#include <string>
+#include <chrono>
+#include <vector>
+#include <thread>
+#include <mutex>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/version.hpp> /* For OpenCV backwards compatibility */
+
+#include "utilities/image_utils.h"
+
+using namespace std;
+
+class Obstacle{
+public:
+    Obstacle(vector<cv::Point> pts, vector<int> dBounds);
+    int dMin;
+    int dMax;
+    cv::Point maxXY;
+    cv::Point minXY;
+    double angle;
+    float distance;
+    cv::Point3f location;
+    void update(bool depth_based, float cam_baseline = 0, float cam_dscale = 0,
+         float* cam_focal = nullptr, float* cam_principal_point = nullptr,
+         float dtype_gain = 0, float aux_dist_factor = 0, bool verbose = true
+    );
+};
+
+class VBOATS{
+private:
+     // Counters
+     int nObs = 0;
+     // Operational Flags
+     bool _is_ground_present = true;
+
+     // Debug Flags
+     bool _debug = false;
+     bool _flag_simulation = false;
+     bool _debug_contours = false;
+     bool _debug_windows = false;
+     bool _debug_obstacle_search = false;
+public:
+     /** Constructors */
+     VBOATS();
+     ~VBOATS();
+
+     /** Startup - Shutdown - Initialization Functions */
+     void init();
+     void update();
+
+     void filter_disparity_vmap(const cv::Mat& input, cv::Mat* output, vector<float>* thresholds, bool verbose = false, bool debug = false, bool visualize = false);
+     void filter_disparity_umap(const cv::Mat& input, cv::Mat* output, vector<float>* thresholds, bool verbose = false, bool debug = false, bool visualize = false);
+
+     bool is_ground_present();
+     int find_ground_lines(const cv::Mat& vmap, cv::Mat* rhos, cv::Mat* thetas, int hough_thresh = 100, bool verbose = false);
+     int find_ground_lines(const cv::Mat& vmap, cv::Mat* found_lines, int hough_thresh = 100, bool verbose = false);
+     int find_ground_lines(const cv::Mat& vmap, vector<cv::Vec2f>* found_lines, int hough_thresh = 100, bool verbose = false);
+     void get_hough_line_params(const float& rho, const float& theta, float* slope, int* intercept);
+     int estimate_ground_line(const vector<cv::Vec2f>& lines, float* best_slope, int* best_intercept, float* worst_slope, int* worst_intercept,
+          double gnd_deadzone = 2.0, double minDeg = 26.0, double maxDeg = 89.0, bool verbose = false, bool debug_timing = false
+     );
+     bool find_ground_line(const cv::Mat& vmap, float* best_slope, int* best_intercept,
+          double minDeg = 26.0, double maxDeg = 89.0, int hough_thresh = 100,
+          double gnd_deadzone = 2.0, bool verbose = false, bool debug_timing = false, bool visualize = false
+     );
+
+     void extract_contour_bounds(const vector<cv::Point>& contour, vector<int>* xbounds, vector<int>* dbounds, bool verbose = false);
+     void find_contours(const cv::Mat& umap, vector<vector<cv::Point>>* found_contours,
+          vector<cv::Vec4i>* found_hierarchy = nullptr, int filter_method = 1, float min_threshold = 30.0, int* offsets = nullptr,
+          float max_threshold = -1, bool verbose = false, bool visualize = false,
+          bool debug = false, bool debug_timing = false
+     );
+
+     int obstacle_search_disparity(const cv::Mat& vmap, const vector<int>& xLimits, vector<int>* yLimits,
+          int* pixel_thresholds = nullptr, int* window_size = nullptr,
+          std::vector<float> line_params = {}, vector<cv::Rect>* obs_windows = nullptr,
+          bool verbose = true, bool visualize = false, bool debug = false, bool debug_timing = false
+     );
+     int find_obstacles_disparity(const cv::Mat& vmap, const vector<vector<cv::Point>>& contours,
+           vector<Obstacle>* found_obstacles, std::vector<float> line_params, vector< vector<cv::Rect> >* obstacle_windows = nullptr,
+           bool verbose = false, bool debug_timing = false
+     );
+
+     int pipeline_disparity(const cv::Mat& disparity, const cv::Mat& umap, const cv::Mat& vmap,
+          vector<Obstacle>* obstacles, cv::Mat* uMorphElement = nullptr,
+          bool verbose = false, bool debug_timing = false, bool visualize = false
+     );
+};
+
+#endif // ROBOCOMMANDER_ALGORITHMS_VBOATS_H_