Sequential step noetic

.gitignore

@@ -0,0 +1 @@
+CMakeLists.txt.user

plane_seg/CMakeLists.txt

@@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_ros
 )
 
-find_package(OpenCV REQUIRED)
+find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs features2d)
 
 catkin_package(
   INCLUDE_DIRS
@@ -34,9 +34,14 @@ add_library(${LIB_NAME} SHARED
   src/PlaneSegmenter.cpp
   src/RectangleFitter.cpp
   src/BlockFitter.cpp
+  src/Tracker.cpp
+  src/ImageProcessor.cpp
+  src/StepCreator.cpp
+  src/Tracker2D.cpp
 )
 add_dependencies(plane_seg ${catkin_EXPORTED_TARGETS})
 target_link_libraries(plane_seg ${catkin_LIBRARIES})
+target_link_libraries(plane_seg ${OpenCV_LIBRARIES})
 
 
 # standalone lcm-based block fitter
@@ -63,5 +68,9 @@ install(TARGETS plane_seg
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
 
-# Mark header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS block_fitter plane_seg_test plane_seg_test2
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+

plane_seg/config/rooster_elevation_mapping.yaml

@@ -0,0 +1,35 @@
+point_cloud_topic: "/camera/depth/color/points"
+map_frame_id: "odom"
+sensor_frame_id: "ground_camera_depth_optical_frame"
+robot_base_frame_id: "base"
+robot_pose_with_covariance_topic: "/vilens/pose_optimized"
+robot_pose_cache_size: 500
+track_point_frame_id: "base"
+track_point_x: 0.0
+track_point_y: 0.0
+track_point_z: 0.0
+
+
+# Robot.
+
+min_update_rate: 0.5
+time_tolerance: 0.5
+time_offset_for_point_cloud: 0.0
+sensor_processor/ignore_points_above: 0.4
+robot_motion_map_update/covariance_scale: 0.01
+
+# Map.
+length_in_x: 10.0
+length_in_y: 4.0
+position_x: 0.0
+position_y: 0.0
+resolution: 0.02
+min_variance: 0.0001
+max_variance: 0.05
+mahalanobis_distance_threshold: 2.5
+multi_height_noise: 0.001
+surface_normal_positive_axis: z
+fused_map_publishing_rate: 0.5
+enable_visibility_cleanup: false
+visibility_cleanup_rate: 1.0
+scanning_duration: 1.0

plane_seg/include/plane_seg/ImageProcessor.hpp

@@ -0,0 +1,83 @@
+#pragma once
+#include <cv_bridge/cv_bridge.h>
+
+namespace planeseg {
+/*
+struct colour{
+    double r;
+    double g;
+    double b;
+};
+*/
+struct contours{
+    std::vector<std::vector<cv::Point>> contours_;
+    std::vector<std::vector<cv::Point>> contours_rect_;
+    void filterSmallContours();
+    void filterMinConvexity(int min_convexity);
+    void filterMinElongation(int min_elongation);
+    void filterMinRectangularity(int min_rectangularity);
+    void fitMinAreaRect();
+    void approxAsPoly();
+    void fitSquare();
+    bool isSquare(std::vector<cv::Point> contour_);
+    void print(std::vector<double> property);
+    std::vector<double> elongations_;
+    std::vector<double> convexities_;
+    std::vector<double> rectangularities_;
+
+/*
+    void setColors();
+    void assignColors();
+    void assignIDs();
+    double getR(int id);
+    double getG(int id);
+    double getB(int id);
+    std::vector<colour> contour_colours_;
+    std::vector<int> ids;
+    std::vector<double> colors_;
+*/
+};
+
+class ImageProcessor {
+public:
+    ImageProcessor();
+    ~ImageProcessor();
+
+    void process();
+    void copyOrigToProc();
+    void displayImage(std::string process, cv::Mat img, int n = 0);
+    void saveImage(cv::Mat image);
+    void erodeImage(int erode_size);
+    void thresholdImage(float threshold_value);
+    void dilateImage(int dilate_size);
+    void blobDetector(cv_bridge::CvImage image);
+    void removeBlobs(cv_bridge::CvImage image);
+    void extractContours();
+    void fillContours();
+    void splitContours();
+    void mergeContours();
+    void drawContoursIP(contours contour, std::string process, int n = 0);
+    void displayResult();
+    void reset();
+    void histogram(cv_bridge::CvImage img);
+    cv::Mat createMask(cv_bridge::CvImage img);
+    cv_bridge::CvImage original_img_;
+    cv_bridge::CvImage processed_img_;
+    cv_bridge::CvImage rect_img_;
+    cv_bridge::CvImage final_img_;
+    cv_bridge::CvImage colour_img_;
+
+    contours all_contours_;
+    contours large_contours_;
+    contours med_contours_;
+
+    std::vector<double> ip_elongations_;
+    std::vector<double> ip_convexities_;
+    std::vector<double> ip_rectangularities_;
+
+private:
+
+};
+
+} //namespace planeseg
+

plane_seg/include/plane_seg/StepCreator.hpp

@@ -0,0 +1,38 @@
+#pragma once
+#include <cv_bridge/cv_bridge.h>
+
+namespace planeseg {
+
+struct contour{
+    std::vector<cv::Point> points_;
+    double elevation_;
+    int id_;
+};
+
+
+class StepCreator{
+
+public:
+    StepCreator();
+    ~StepCreator();
+
+    void go();
+    void setImages(cv_bridge::CvImage img1, cv_bridge::CvImage img2);
+    void extractContours();
+    void displayImage(std::string process, cv::Mat img, int n);
+    void maskElevation(cv::Mat mask_);
+    void reset();
+    double medianMat(cv::Mat Input);
+    cv::Mat createMask(contour cntr);
+    cv_bridge::CvImage processed_;
+    cv_bridge::CvImage elevation_;
+    cv_bridge::CvImage elevation_masked_;
+    cv_bridge::CvImage reconstructed_;
+    std::vector<contour> rectangles_;
+    std::vector<std::vector<cv::Point>> pnts_;
+
+private:
+
+};
+
+} // namespace planeseg

plane_seg/include/plane_seg/Tracker.hpp

@@ -0,0 +1,40 @@
+#pragma once
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include "plane_seg/BlockFitter.hpp"
+
+namespace planeseg {
+
+struct plane {
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+    int id;
+    pcl::PointXYZ centroid;
+};
+
+// a vector to hold the ids of oldStairs and a flag for whether the id has been assigned
+struct IdAssigned{
+    bool taken;
+    int id;
+};
+
+class Tracker3D {
+public:
+    Tracker3D();
+    ~Tracker3D();
+
+    int get_plane_id(planeseg::plane plane_);
+    pcl::PointXYZ find_centroid(pcl::PointCloud<pcl::PointXYZ> cloud );
+    void reset();
+    void convertResult(planeseg::BlockFitter::Result result);
+    void test(planeseg::BlockFitter::Result result);
+    void printIds();
+    std::vector<plane> newStairs;
+
+private:
+
+    std::vector<plane> oldStairs;
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_ptrs;
+    int totalIds;
+};
+
+} // namespace planeseg

plane_seg/include/plane_seg/Tracker2D.hpp

@@ -0,0 +1,28 @@
+#pragma once
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include "plane_seg/BlockFitter.hpp"
+#include "plane_seg/StepCreator.hpp"
+
+namespace planeseg {
+
+class Tracker2D {
+public:
+    Tracker2D();
+    ~Tracker2D();
+
+    int get_contour_id(contour contour);
+    pcl::PointXYZ find_centroid(pcl::PointCloud<pcl::PointXYZ> cloud );
+    void reset();
+    void assignIDs(std::vector<contour> contours);
+    void printIds();
+    std::vector<contour> newRects_;
+
+private:
+
+    std::vector<contour> oldRects_;
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_ptrs;
+    int totalIds;
+};
+
+} // namespace planeseg

plane_seg/include/plane_seg/Types.hpp

@@ -8,7 +8,7 @@
 
 namespace planeseg {
 
-  /*
+/*
 struct Point {
   PCL_ADD_POINT4D;
   float delta;
@@ -23,7 +23,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(Point,
                                   (float, delta, delta)
                                   (int, label, label)
 )
-  */
+*/
 typedef pcl::PointXYZL Point;
 
 typedef pcl::PointCloud<pcl::Normal> NormalCloud;

plane_seg/src/BlockFitter.cpp

@@ -338,7 +338,7 @@ go() {
     results.push_back(result);
   }
 
-  if (mDebug) {
+  if (true) { // Used to be mDebug
     std::ofstream ofs("boxes.txt");
     for (int i = 0; i < (int)results.size(); ++i) {
       auto& res = results[i];
@@ -383,7 +383,7 @@ go() {
     block.mPose = res.mPose;
     block.mPose.translation() -=
       block.mPose.rotation().col(2)*mBlockDimensions[2]/2;
-    block.mHull = res.mConvexHull;
+    block.mHull = res.mPolygon; // USED TO BE res.mConvexHull
     result.mBlocks.push_back(block);
   }
   if (mDebug) {