diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..01e00f3 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +CMakeLists.txt.user diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index b0488c2..999664f 100644 --- a/plane_seg/CMakeLists.txt +++ b/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}) + diff --git a/plane_seg/config/rooster_elevation_mapping.yaml b/plane_seg/config/rooster_elevation_mapping.yaml new file mode 100644 index 0000000..ca41a49 --- /dev/null +++ b/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 diff --git a/plane_seg/include/plane_seg/ImageProcessor.hpp b/plane_seg/include/plane_seg/ImageProcessor.hpp new file mode 100644 index 0000000..34c971e --- /dev/null +++ b/plane_seg/include/plane_seg/ImageProcessor.hpp @@ -0,0 +1,83 @@ +#pragma once +#include + +namespace planeseg { +/* +struct colour{ + double r; + double g; + double b; +}; +*/ +struct contours{ + std::vector> contours_; + std::vector> 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 contour_); + void print(std::vector property); + std::vector elongations_; + std::vector convexities_; + std::vector rectangularities_; + +/* + void setColors(); + void assignColors(); + void assignIDs(); + double getR(int id); + double getG(int id); + double getB(int id); + std::vector contour_colours_; + std::vector ids; + std::vector 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 ip_elongations_; + std::vector ip_convexities_; + std::vector ip_rectangularities_; + +private: + +}; + +} //namespace planeseg + diff --git a/plane_seg/include/plane_seg/StepCreator.hpp b/plane_seg/include/plane_seg/StepCreator.hpp new file mode 100644 index 0000000..914822a --- /dev/null +++ b/plane_seg/include/plane_seg/StepCreator.hpp @@ -0,0 +1,38 @@ +#pragma once +#include + +namespace planeseg { + +struct contour{ + std::vector 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 rectangles_; + std::vector> pnts_; + +private: + +}; + +} // namespace planeseg diff --git a/plane_seg/include/plane_seg/Tracker.hpp b/plane_seg/include/plane_seg/Tracker.hpp new file mode 100644 index 0000000..71ac21a --- /dev/null +++ b/plane_seg/include/plane_seg/Tracker.hpp @@ -0,0 +1,40 @@ +#pragma once +#include +#include +#include "plane_seg/BlockFitter.hpp" + +namespace planeseg { + +struct plane { + pcl::PointCloud 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 cloud ); + void reset(); + void convertResult(planeseg::BlockFitter::Result result); + void test(planeseg::BlockFitter::Result result); + void printIds(); + std::vector newStairs; + +private: + + std::vector oldStairs; + std::vector::Ptr> cloud_ptrs; + int totalIds; +}; + +} // namespace planeseg diff --git a/plane_seg/include/plane_seg/Tracker2D.hpp b/plane_seg/include/plane_seg/Tracker2D.hpp new file mode 100644 index 0000000..a080e86 --- /dev/null +++ b/plane_seg/include/plane_seg/Tracker2D.hpp @@ -0,0 +1,28 @@ +#pragma once +#include +#include +#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 cloud ); + void reset(); + void assignIDs(std::vector contours); + void printIds(); + std::vector newRects_; + +private: + + std::vector oldRects_; + std::vector::Ptr> cloud_ptrs; + int totalIds; +}; + +} // namespace planeseg diff --git a/plane_seg/include/plane_seg/Types.hpp b/plane_seg/include/plane_seg/Types.hpp index 02078ad..da32837 100644 --- a/plane_seg/include/plane_seg/Types.hpp +++ b/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 NormalCloud; diff --git a/plane_seg/src/BlockFitter.cpp b/plane_seg/src/BlockFitter.cpp index 5583561..f8fc2ac 100644 --- a/plane_seg/src/BlockFitter.cpp +++ b/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) { diff --git a/plane_seg/src/ImageProcessor.cpp b/plane_seg/src/ImageProcessor.cpp new file mode 100644 index 0000000..12b7753 --- /dev/null +++ b/plane_seg/src/ImageProcessor.cpp @@ -0,0 +1,433 @@ +#include "plane_seg/ImageProcessor.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planeseg { + +void contours::filterMinConvexity(int min_convexity){ + + std::vector> temp; + temp = contours_; + contours_.clear(); + convexities_.clear(); + + for (size_t p = 0; p < temp.size(); ++p){ + std::vector hull; + double hull_perimeter; + double contour_perimeter; + double convexity; + + cv::convexHull(temp[p], hull); + hull_perimeter = cv::arcLength(hull, true); + contour_perimeter = cv::arcLength(temp[p], true); + convexity = hull_perimeter / contour_perimeter; + convexities_.push_back(convexity); + + if (convexity >= min_convexity){ + contours_.push_back(temp[p]); + } + } + +// print(convexities_); +} + +void contours::filterMinElongation(int min_elongation){ + + std::vector> temp; + temp = contours_; + contours_.clear(); + elongations_.clear(); + + for (size_t r = 0; r < temp.size(); ++r){ + double elongation; + cv::RotatedRect minarea_rect; + minarea_rect = cv::minAreaRect(temp[r]); + double major_axis = std::max(minarea_rect.size.height, minarea_rect.size.width); + double minor_axis = std::min(minarea_rect.size.height, minarea_rect.size.width); + elongation = major_axis / minor_axis; + + elongations_.push_back(elongation); + + if (elongation >= min_elongation){ + contours_.push_back(temp[r]); + } + } + +// print(elongations_); +} + +void contours::filterMinRectangularity(int min_rectangularity){ + + std::vector> temp; + temp = contours_; + contours_.clear(); + rectangularities_.clear(); + + for (size_t r = 0; r < temp.size(); ++r){ + double rectangularity, contour_area; + cv::RotatedRect minarea_rect; + minarea_rect = cv::minAreaRect(temp[r]); + contour_area = cv::contourArea(temp[r]); + rectangularity = contour_area / minarea_rect.size.area(); + +// rectangularities_.push_back(rectangularity); + + if (rectangularity >= min_rectangularity){ + contours_.push_back(temp[r]); + } + } +// print(rectangularities_); +} + +void contours::approxAsPoly(){ + + std::vector> temp; + temp = contours_; + contours_rect_.clear(); + + for(size_t l = 0; l < temp.size(); ++l){ + double epsilon; + epsilon = 3; + std::vector approx; + cv::approxPolyDP(temp[l], approx, epsilon, true); + contours_rect_.push_back(approx); + } +} + +void contours::fitMinAreaRect(){ + + std::vector> temp; + temp = contours_; + contours_rect_.clear(); + + for (size_t r = 0; r < temp.size(); ++r){ + cv::RotatedRect minarea_rect; + cv::Point2f point2f_box[4]; + minarea_rect = cv::minAreaRect(temp[r]); + minarea_rect.points(point2f_box); + std::vector point_box; + for (int i = 0; i < 4; ++i){ + cv::Point pnt; + pnt.x = point2f_box[i].x; + pnt.y = point2f_box[i].y; + point_box.push_back(pnt); + } + + contours_rect_.push_back(point_box); + + } +} + +bool contours::isSquare(std::vector contour_){ + double elongation, rectangularity, contour_area; + cv::RotatedRect minarea_rect; + minarea_rect = cv::minAreaRect(contour_); + double major_axis = std::max(minarea_rect.size.height, minarea_rect.size.width); + double minor_axis = std::min(minarea_rect.size.height, minarea_rect.size.width); + elongation = major_axis / minor_axis; + + contour_area = cv::contourArea(contour_); + rectangularity = contour_area / minarea_rect.size.area(); + + if(elongation > 0.5 && elongation < 1.3 && rectangularity > 0.8){ + return true; + } else { + return false; + } +} + +void contours::fitSquare(){ + + contours_rect_.clear(); + for(size_t i = 0; i < contours_.size(); ++i){ + if (isSquare(contours_[i])){ + fitMinAreaRect(); + } else { + contours_rect_.push_back(contours_[i]); + } + } +} + + +void contours::print(std::vector property){ + + for(size_t i = 0; i < property.size(); ++i){ + std::cout << property[i] << ", "; + } + std::cout << std::endl; +} + + +ImageProcessor::ImageProcessor(){ +} + +ImageProcessor::~ImageProcessor(){} + +void ImageProcessor::process(){ + + + ip_convexities_.clear(); + ip_elongations_.clear(); + ip_rectangularities_.clear(); + +// displayImage("original", original_img_.image, 0); +// saveImage(original_img_.image); +// histogram(original_img_); + thresholdImage(0.3); + + processed_img_.image.convertTo(processed_img_.image, CV_8U); +// displayImage("threshold", processed_img_.image, 1); + + erodeImage(1); + dilateImage(2); + erodeImage(1); +// displayImage("morphological", processed_img_.image, 2); + + extractContours(); + splitContours(); + +// drawContoursIP(med_contours_, "med_contours", 3); +// drawContoursIP(large_contours_, "large_contours", 4); + std::cout << "Convexities: " << std::endl; + med_contours_.filterMinConvexity(0.9); // have another look at the threshold for convexity +// ip_convexities_ = med_contours_.convexities_; +// drawContoursIP(med_contours_, "filtered by convexity", 6); + std::cout << "Elongations: " << std::endl; + med_contours_.filterMinElongation(3); +// ip_elongations_ = med_contours_.elongations_; +// drawContoursIP(med_contours_, "filtered by elongation", 7); + std::cout << "Rectangularities: " << std::endl; + med_contours_.filterMinRectangularity(0.6); +// ip_rectangularities_ = med_contours_.rectangularities_; +// drawContoursIP(med_contours_, "filtered by rectangularity", 8); + med_contours_.fitMinAreaRect(); + large_contours_.approxAsPoly(); +// large_contours_.filterMinRectangularity(0.6); +// large_contours_.fitSquare(); +// drawContoursIP(med_contours_, "med_filtered", 5); +// drawContoursIP(large_contours_, "large_approx", 6); + mergeContours(); +// all_contours_.assignIDs(); +// all_contours_.assignColors(); + + displayResult(); + + +// int p = cv::waitKey(0); +// if (p == 's'){ +// saveImage(original_img_.image); +// } + +} + +void ImageProcessor::copyOrigToProc(){ + processed_img_ = original_img_; +} + +void ImageProcessor::displayImage(std::string process, cv::Mat img, int n) { + + cv::namedWindow(process, cv::WINDOW_AUTOSIZE); + cv::moveWindow(process, 100 + img.cols * n, 50); + cv::imshow(process, img); +// saveImage(img); +} + +void ImageProcessor::displayResult(){ + + colour_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8UC3); + rect_img_.image = cv::Mat::zeros(original_img_.image.size(), CV_8UC1); + + for(size_t i = 0; i < all_contours_.contours_.size(); ++i){ + cv::RNG rng; + cv::Scalar color(rand()&255, rand()&255, rand()&255); + cv::drawContours(colour_img_.image, all_contours_.contours_, i, color, cv::FILLED); +// cv::drawContours(processed_img_.image, all_contours_.contours_, i, cv::Scalar(255), cv::FILLED); + } + for(size_t j = 0; j < all_contours_.contours_rect_.size(); ++j){ +// cv::drawContours(colour_img_.image, all_contours_.contours_rect_, j, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + cv::drawContours(rect_img_.image, all_contours_.contours_rect_, j, cv::Scalar(255), cv::FILLED); + } + + final_img_ = rect_img_; +// displayImage("final", rect_img_.image, 7); +} + +void ImageProcessor::saveImage(cv::Mat image_){ + cv::Mat CV_8U_img; + image_.convertTo(CV_8U_img, CV_8U); + + std::string imagename; + std::cout << "Enter filename to save image (don't forget .png!): " << std::endl; + std::cin >> imagename; + std::string homedir = getenv("HOME"); + if(!homedir.empty()){ + boost::filesystem::path output_path(homedir + "/git/4YP/Figures/"); + if(!boost::filesystem::is_directory(output_path)){ + if(boost::filesystem::create_directory(output_path)){ + std::cout << "Creating directory " << output_path.string() << " ... " << std::endl; + } else { + std::cerr << "ERROR: directory " << output_path.string() << " does not exists and couldn't be created." << std::endl; + return; + } + } + cv::imwrite(output_path.string() + imagename, CV_8U_img); + } else { + std::cerr << "ERROR: the $HOME variable is empty!" << std::endl; + return; + } +} + +void ImageProcessor::erodeImage(int erode_size){ + + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, + cv::Size(2*erode_size + 1, 2*erode_size + 1), + cv::Point(erode_size, erode_size) + ); + + cv::erode(processed_img_.image, processed_img_.image, element); +} + +void ImageProcessor::dilateImage(int dilate_size){ + + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, + cv::Size(2*dilate_size+1, 2*dilate_size +1), + cv::Point(dilate_size, dilate_size) + ); + cv::dilate(processed_img_.image, processed_img_.image, element); +} + +void ImageProcessor::thresholdImage(float threshold_value){ + + int threshold_type = cv::ThresholdTypes::THRESH_BINARY_INV; + double maxval = 255; + + cv::threshold(original_img_.image, processed_img_.image, threshold_value, maxval, threshold_type); + +} + +void ImageProcessor::fillContours(){ + drawContoursIP(all_contours_, "fill", 3); +} + +void ImageProcessor::extractContours(){ + cv::findContours(processed_img_.image, all_contours_.contours_, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); +} + +void ImageProcessor::splitContours(){ + + double area; + for(size_t k = 0; k < all_contours_.contours_.size(); ++k){ + area = fabs(cv::contourArea(all_contours_.contours_[k])); + + if (area >= 1000){ + large_contours_.contours_.push_back(all_contours_.contours_[k]); + } else if (area >=200 && area <= 1000){ + med_contours_.contours_.push_back(all_contours_.contours_[k]); + } + } +} + +void ImageProcessor::mergeContours(){ + all_contours_.contours_.clear(); + all_contours_.contours_rect_.clear(); + + // populate all_contours.contours_ + for (size_t v = 0; v < large_contours_.contours_.size(); ++v){ + all_contours_.contours_.push_back(large_contours_.contours_[v]); + } + for (size_t w = 0; w < med_contours_.contours_.size(); ++w){ + all_contours_.contours_.push_back(med_contours_.contours_[w]); + } + + // populate all_contours.contours_rect_ + for (size_t v = 0; v < large_contours_.contours_rect_.size(); ++v){ + all_contours_.contours_rect_.push_back(large_contours_.contours_rect_[v]); + } + for (size_t w = 0; w < med_contours_.contours_rect_.size(); ++w){ + all_contours_.contours_rect_.push_back(med_contours_.contours_rect_[w]); + } +} + +void ImageProcessor::drawContoursIP(contours contour, std::string process, int n){ + cv_bridge::CvImage temp; + temp.image = cv::Mat::zeros(original_img_.image.size(), CV_8U); + + for(size_t i = 0; i < contour.contours_.size(); ++i){ + cv::drawContours(temp.image, contour.contours_, i, cv::Scalar(255), cv::FILLED); + } + + displayImage(process, temp.image, n); +} + +void ImageProcessor::histogram(cv_bridge::CvImage img){ + int bins = 30; + int histSize = bins; + float range[] = {0, 2}; + const float* histRange = { range }; + bool uniform = true, accumulate = false; + cv::Mat hist, mask; + mask = createMask(img); + mask.convertTo(mask, CV_8U); + std::cout << "Mask type: " << mask.type() << std::endl; + cv::calcHist(&img.image, 1, 0, mask, hist, 1, &histSize, &histRange, uniform, accumulate); + + std::cout << "Start Histogram: " << std::endl; + for (int m = 0; m < histSize ; ++m){ + std::cout << hist.at(m) << ", "; + } + std::cout << "END Histogram" << std::endl; + + int hist_w = 512, hist_h = 400; + int bin_w = cvRound( (double) hist_w/bins ); + + cv::Mat histImage( hist_h, hist_w, CV_8UC3, cv::Scalar(0,0,0) ); + cv::normalize(hist, hist, 0, histImage.rows, cv::NORM_MINMAX, -1, cv::Mat() ); + + for (int i = 1; i < histSize; i++){ + cv::line(histImage, cv::Point(bin_w*(i-1), hist_h - cvRound(hist.at(i-1))), cv::Point(bin_w*(i), hist_h - cvRound(hist.at(i))), cv::Scalar(255, 0, 0), 2, 8, 0); + } + +// cv::imshow("original_img_ histogram", histImage); +// cv::waitKey(0); +} + + +void ImageProcessor::reset(){ + original_img_.image = cv::Mat(); + processed_img_.image = cv::Mat(); + final_img_.image = cv::Mat(); + all_contours_.contours_.clear(); + large_contours_.contours_.clear(); + med_contours_.contours_.clear(); +} + + +cv::Mat ImageProcessor::createMask(cv_bridge::CvImage img){ + + cv::Mat mask_; + mask_ = cv::Mat::ones(img.image.size(), CV_32F); + + for(int r = 0; r < img.image.rows; r++) + { + for(int c = 0; c < img.image.cols; c++) + { + if (img.image.at(r,c) == 1) + { + mask_.at(r,c) = 0; + } + } + } + +// displayImage("mask", mask_, 8); + std::cout << "Exited displayImage()" << std::endl; + + return mask_; +} + +} //namespace planeseg diff --git a/plane_seg/src/PlaneSegmenter.cpp b/plane_seg/src/PlaneSegmenter.cpp index 3c2310c..6bebdcc 100644 --- a/plane_seg/src/PlaneSegmenter.cpp +++ b/plane_seg/src/PlaneSegmenter.cpp @@ -9,7 +9,7 @@ using namespace planeseg; PlaneSegmenter:: PlaneSegmenter() { - setMaxError(0.02); + setMaxError(0.02); //0.02 setMaxAngle(30); setSearchRadius(0.03); setMinPoints(500); diff --git a/plane_seg/src/StepCreator.cpp b/plane_seg/src/StepCreator.cpp new file mode 100644 index 0000000..f2be940 --- /dev/null +++ b/plane_seg/src/StepCreator.cpp @@ -0,0 +1,118 @@ +#include "plane_seg/StepCreator.hpp" +#include +#include +#include +#include +#include "plane_seg/ImageProcessor.hpp" + +namespace planeseg { + +StepCreator::StepCreator(){ +} + +StepCreator::~StepCreator(){} + +void StepCreator::go(){ + std::cout << "Entered go" << std::endl; +// displayImage("processed", processed_.image, 0); + extractContours(); + + + for (size_t i = 0; i < rectangles_.size(); ++i){ + elevation_masked_.image = cv::Mat(); + cv::Mat mask; + mask = createMask(rectangles_[i]); +// rectangles_[i].elevation_ = cv::mean(elevation_.image, mask); + maskElevation(mask); + rectangles_[i].elevation_ = medianMat(elevation_masked_.image); + +// std::cout << medianMat(elevation_masked_.image) << std::endl; + } + + std::vector> v; + std::vector elev; + reconstructed_.image = cv::Mat::zeros(processed_.image.size(), CV_32F); + + for (size_t j = 0; j < rectangles_.size(); ++j){ + v.push_back(rectangles_[j].points_); + elev.push_back(rectangles_[j].elevation_); + } + for (size_t k = 0; k < v.size(); ++k){ + cv::drawContours(reconstructed_.image, v, k, elev[k], cv::FILLED); + } +// displayImage("elevation masked", reconstructed_.image, 4); + +} + +void StepCreator::setImages(cv_bridge::CvImage img1, cv_bridge::CvImage img2){ + processed_ = img1; + elevation_ = img2; +} + +void StepCreator::extractContours(){ + +// cv::findContours(processed_.image, pnts, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); +// pnts_ = + + for (size_t j = 0; j < pnts_.size(); ++j){ + contour temp; + temp.points_ = pnts_[j]; + rectangles_.push_back(temp); + } +} + +cv::Mat StepCreator::createMask(contour cntr){ + cv_bridge::CvImage contour_img_; + std::vector> c; + c.push_back(cntr.points_); + contour_img_.image = cv::Mat::zeros(processed_.image.size(), CV_8U); + for(size_t k = 0; k < c.size(); ++k){ + cv::drawContours(contour_img_.image, c, k, cv::Scalar(255), cv::FILLED); + } + return contour_img_.image; +} + +void StepCreator::displayImage(std::string process, cv::Mat img, int n) { + std::cout << "Entered displayImage" << std::endl; + + cv::namedWindow(process, cv::WINDOW_AUTOSIZE); + cv::moveWindow(process, 100 + img.cols * n, 50); + cv::imshow(process, img); + cv::waitKey(0); +} + +void StepCreator::maskElevation(cv::Mat mask_){ + elevation_.image.copyTo(elevation_masked_.image, mask_); +} + +void StepCreator::reset(){ + processed_.image = cv::Mat(); + elevation_.image = cv::Mat(); + elevation_masked_.image = cv::Mat(); + reconstructed_.image = cv::Mat(); + rectangles_.clear(); +} + +double StepCreator::medianMat(cv::Mat Input){ + Input = Input.reshape(0,1); // spread Input Mat to single row + std::vector vecFromMat, vecNoZeros; + Input.copyTo(vecFromMat); // Copy Input Mat to vector vecFromMat + for (size_t i = 0; i < vecFromMat.size(); ++i){ + if (!vecFromMat[i] == 0){ + vecNoZeros.push_back(vecFromMat[i]); + } + } + std::nth_element(vecNoZeros.begin(), vecNoZeros.begin() + vecNoZeros.size() / 2, vecNoZeros.end()); + return vecNoZeros[vecNoZeros.size() / 2]; +} +/* +double StepCreator::medianMat(cv::Mat Input){ + Input = Input.reshape(0,1); // spread Input Mat to single row + std::vector vecFromMat; + Input.copyTo(vecFromMat); // Copy Input Mat to vector vecFromMat + std::sort( vecFromMat.begin(), vecFromMat.end() ); // sort vecFromMat + if (vecFromMat.size()%2==0) {return (vecFromMat[vecFromMat.size()/2-1]+vecFromMat[vecFromMat.size()/2])/2;} // in case of even-numbered matrix + return vecFromMat[(vecFromMat.size()-1)/2]; // odd-number of elements in matrix +} +*/ +} // namespace planeseg diff --git a/plane_seg/src/Tracker.cpp b/plane_seg/src/Tracker.cpp new file mode 100644 index 0000000..a8e67ed --- /dev/null +++ b/plane_seg/src/Tracker.cpp @@ -0,0 +1,123 @@ +#include "plane_seg/Tracker.hpp" +#include +#include +#include +#include +#include "plane_seg/BlockFitter.hpp" +#include "plane_seg/Types.hpp" +#include "pcl/common/impl/centroid.hpp" +#include +#include + +namespace planeseg { + +Tracker3D::Tracker3D(){ + totalIds = 0; +} + +Tracker3D::~Tracker3D(){} + +pcl::PointXYZ Tracker3D::find_centroid(pcl::PointCloud cloud ){ +// std::cout << "Computing centroid" << std::endl; + Eigen::Vector4f centroid_eigen; + pcl::compute3DCentroid(cloud, centroid_eigen); + pcl::PointXYZ centroid; + // NOTE: assuming the first three values of the centroid are the Eucledian + // coordinates of the centroid. The fourth value is discarded. + centroid.x = centroid_eigen(0); + centroid.y = centroid_eigen(1); + centroid.z = centroid_eigen(2); + return centroid; +} + +// converts the result from the BlockFitter to a vector of planes complete with pointclouds, centroids and ids, and saves it as newStairs +void Tracker3D::convertResult(planeseg::BlockFitter::Result result_){ + std::cout << "entered convertResult" << std::endl; + for (size_t i=0; i cloud; + const auto& block = result_.mBlocks[i]; + for (size_t j =0; j < block.mHull.size(); ++j){ + pcl::PointXYZ pt; + pt.x =block.mHull[j](0); + pt.y =block.mHull[j](1); + pt.z =block.mHull[j](2); + cloud.points.push_back(pt); + } + cloud.height = cloud.points.size(); + cloud.width = 1; + + pcl::PointXYZ cloud_centroid; + cloud_centroid = find_centroid(cloud); + + planeseg::plane newPlane; + newPlane.cloud = cloud; + newPlane.centroid = cloud_centroid; + newPlane.id = get_plane_id(newPlane); + + newStairs.push_back(newPlane); + } + +} + +int Tracker3D::get_plane_id(planeseg::plane plane){ + +// std::cout << "entered get_plane_id" << std::endl; + int id; + + if(oldStairs.empty()){ + id = totalIds; + ++totalIds; + } + + else{ + pcl::PointXYZ oldCentroid; + double threshold = 0.1; + double distz; + int closest = -1; + // start with closestDist being far larger than anything that would ever happen + double closestDist = 1000000000; + + // go through each plane in oldStairs and compare it to the new plane + + for (size_t i = 0; i < oldStairs.size(); ++i){ + + distz = fabs(oldStairs[i].centroid.z - plane.centroid.z); + + // is distz small enough to suggest that this point cloud represents the next iteration of an existing frame? + if(distz < threshold){ + // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); + + if (distz < closestDist){ + closest = i; + closestDist = distz; + } + } + } + + if (closest != -1){ + id = oldStairs[closest].id; + } + else{ + id = totalIds; + ++totalIds; + } + } + + return id; +} + +void Tracker3D::reset(){ + std::cout << "entered Tracker reset" << std::endl; + oldStairs = newStairs; + newStairs.clear(); + } + +void Tracker3D::printIds(){ +// std::cout << "entered printIds" << std::endl; + std::cout << "Total number of ids assigned: " << newStairs.size() << std::endl; + for (size_t i = 0; i < newStairs.size(); ++i){ + std::cout << newStairs[i].id << std::endl; + } +} + +} // namespace plane_seg diff --git a/plane_seg/src/Tracker2D.cpp b/plane_seg/src/Tracker2D.cpp new file mode 100644 index 0000000..e3eba4d --- /dev/null +++ b/plane_seg/src/Tracker2D.cpp @@ -0,0 +1,100 @@ +#include "plane_seg/Tracker2D.hpp" +#include +#include +#include +#include +#include "plane_seg/BlockFitter.hpp" +#include "plane_seg/Types.hpp" +#include "pcl/common/impl/centroid.hpp" +#include +#include + +namespace planeseg { + +Tracker2D::Tracker2D(){ + totalIds = 0; +} + +Tracker2D::~Tracker2D(){} + + +// converts the result from the BlockFitter to a vector of planes complete with pointclouds, centroids and ids, and saves it as newStairs +void Tracker2D::assignIDs(std::vector contours){ + std::cout << "entered assignIDs" << std::endl; + for (size_t i = 0; i < contours.size(); ++i){ + planeseg::contour newContour; + newContour.points_ = contours[i].points_; + newContour.elevation_ = contours[i].elevation_; + newContour.id_ = get_contour_id(newContour); + + newRects_.push_back(newContour); + } +} + +int Tracker2D::get_contour_id(planeseg::contour contour){ + +// std::cout << "entered get_plane_id" << std::endl; + int id; + + if(oldRects_.empty()){ +// std::cout << "oldRects_ is empty" << std::endl; + id = totalIds; + ++totalIds; + } + + else{ +// std::cout << "oldRects_ has size " << oldRects_.size() << std::endl; + double threshold = 0.1; + double dist; + int closest = -1; + // start with closestDist being far larger than anything that would ever happen + double closestDist = 1000000; + + // go through each plane in oldStairs and compare it to the new plane + + for (size_t i = 0; i < oldRects_.size(); ++i){ + + dist = fabs(oldRects_[i].elevation_ - contour.elevation_); +// std::cout << "oldRects_[" << i << "].elevation_ = " << oldRects_[i].elevation_; +// std::cout << "contour.elevation_ = " << contour.elevation_ << std::endl; +// std::cout << "dist = " << dist << std::endl; + // is distz small enough to suggest that this point cloud represents the next iteration of an existing frame? + if(dist < threshold){ + // distance = pcl::euclideanDistance(oldStairs[i].centroid, plane.centroid); + + if (dist < closestDist){ + closest = i; + closestDist = dist; + } + } + } + + if (closest != -1){ + std::cout << "Here" << std::endl; + id = oldRects_[closest].id_; + } + else{ + std::cout << "Here2" << std::endl; + id = totalIds; + ++totalIds; + } + } +// std::cout << "id = " << id << std::endl; + return id; +} + +void Tracker2D::reset(){ + std::cout << "entered Tracker2D reset" << std::endl; + oldRects_ = newRects_; + newRects_.clear(); + } + +void Tracker2D::printIds(){ +// std::cout << "entered printIds" << std::endl; + std::cout << "Total number of ids assigned: " << newRects_.size() << std::endl; + for (size_t i = 0; i < newRects_.size(); ++i){ + std::cout << newRects_[i].id_ << std::endl; + } +} + +} // namespace plane_seg diff --git a/plane_seg/src/tracker.cpp b/plane_seg/src/tracker.cpp new file mode 100644 index 0000000..e69de29 diff --git a/plane_seg_ros/CMakeLists.txt b/plane_seg_ros/CMakeLists.txt index d152b27..e1b2a55 100644 --- a/plane_seg_ros/CMakeLists.txt +++ b/plane_seg_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.6.0) +cmake_minimum_required(VERSION 3.0.2) project(plane_seg_ros) @@ -16,21 +16,57 @@ find_package(catkin REQUIRED COMPONENTS plane_seg ) -find_package(OpenCV 3.0 QUIET) -catkin_package( - INCLUDE_DIRS - #include - CATKIN_DEPENDS eigen_conversions pcl_conversions tf_conversions pcl_ros plane_seg -) +find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs) + +catkin_package(INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS eigen_conversions + pcl_conversions + tf_conversions + pcl_ros + plane_seg + grid_map_msgs) + + include_directories( -# include + include ${catkin_INCLUDE_DIRS} ) +add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp + src/geometry_utils.cpp + src/Visualizer.cpp + ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) -set(APP_NAME plane_seg_ros) + +set(APP_NAME ${PROJECT_NAME}_node) add_executable(${APP_NAME} src/${APP_NAME}.cpp) -target_link_libraries(${APP_NAME} boost_system ${catkin_LIBRARIES}) +target_link_libraries(${APP_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${APP_NAME} ${PROJECT_NAME} ${OpenCV_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(TARGETS ${APP_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) + +install(DIRECTORY data/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data) + +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) diff --git a/plane_seg_ros/config/default.yaml b/plane_seg_ros/config/default.yaml new file mode 100644 index 0000000..322358e --- /dev/null +++ b/plane_seg_ros/config/default.yaml @@ -0,0 +1,80 @@ +run_test_program: false +run_sequential_test: false +run_nth_cloud: false + +input_topic: /rooster_elevation_mapping/elevation_map +erode_radius: 0.11 +traversability_threshold: 0.7 +verbose_timer: true +grid_map_sub_topic: /rooster_elevation_mapping/elevation_map +point_cloud_sub_topic: /plane_seg/point_cloud_in +pose_sub_topic: /state_estimator/pose_in_odom + +grid_map_filters: + + - name: buffer_normalizer + type: gridMapFilters/BufferNormalizerFilter + +# # Duplicate layer. +# - name: duplicate +# type: gridMapFilters/DuplicationFilter +# params: +# input_layer: ... +# output_layer: ... + +# Delete color layer. + - name: delete_original_layers + type: gridMapFilters/DeletionFilter + params: + layers: [color] # List of layers. + +# Fill holes in the map with inpainting. +# - name: inpaint +# type: gridMapCv/InpaintFilter +# params: +# input_layer: elevation +# output_layer: elevation_inpainted +# radius: 0.05 + +# Reduce noise with a radial blurring filter. +# - name: mean_in_radius +# type: gridMapFilters/MeanInRadiusFilter +# params: +# input_layer: elevation_inpainted +# output_layer: elevation_smooth +# radius: 0.06 + +# Boxblur as an alternative to the inpaint and radial blurring filter above. +# - name: boxblur +# type: gridMapFilters/SlidingWindowMathExpressionFilter +# params: +# input_layer: elevation +# output_layer: elevation_smooth +# expression: meanOfFinites(elevation) +# compute_empty_cells: true +# edge_handling: crop # options: inside, crop, empty, mean +# window_size: 5 # optional + +# Compute surface normals. + - name: surface_normals + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation + output_layers_prefix: normal_vectors_ + radius: 0.05 + normal_vector_positive_axis: z + +# Add a color layer for visualization based on the surface normal. +# - name: normal_color_map +# type: gridMapFilters/NormalColorMapFilter +# params: +# input_layers_prefix: normal_vectors_ +# output_layer: normal_color + +# Compute slope from surface normal. + - name: slope + type: gridMapFilters/MathExpressionFilter + params: + output_layer: slope + expression: acos(normal_vectors_z) + diff --git a/plane_seg_ros/config/elevation_map_filters_config.rviz b/plane_seg_ros/config/elevation_map_filters_config.rviz new file mode 100644 index 0000000..9eb000f --- /dev/null +++ b/plane_seg_ros/config/elevation_map_filters_config.rviz @@ -0,0 +1,336 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Marker1 + - /GridMap1 + - /PoseWithCovariance1 + - /GridMap2 + - /MarkerArray4 + Splitter Ratio: 0.5058823823928833 + Tree Height: 726 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /plane_seg/hull_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.20000000298023224 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /vilens/pose + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/id_strings + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/centroids + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/linestrips + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: product + Color Transformer: GridMapLayer + Enabled: true + Height Layer: product + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/filtered_map + Unreliable: false + Use Rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /opencv/rectangles + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /opencv/test + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1824 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /os_cloud_node/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /vilens/pose_optimized + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + 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 + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 9.770825386047363 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.317430019378662 + Y: 0.7584229111671448 + Z: 0.11780852824449539 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.739797830581665 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.3391360640525818 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000019500000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a60000003efc0100000002fb0000000800540069006d00650100000000000005a6000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000040b0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1446 + X: 67 + Y: 27 diff --git a/plane_seg_ros/config/elevation_map_plane_seg_config.rviz b/plane_seg_ros/config/elevation_map_plane_seg_config.rviz new file mode 100644 index 0000000..0d5f14b --- /dev/null +++ b/plane_seg_ros/config/elevation_map_plane_seg_config.rviz @@ -0,0 +1,397 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud22 + - /GridMap1 + - /Image1 + - /Marker1 + - /PointCloud23 + - /PointCloud23/Status1 + Splitter Ratio: 0.31725889444351196 + Tree Height: 573 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bottom_screws_center: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + ground_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ground_camera_shim: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + os_imu: + Alpha: 1 + Show Axes: false + Show Trail: false + os_lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + os_sensor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + realsense_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1786 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /os_cloud_node/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/elevation_map_raw + Unreliable: false + Use Rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/infra1/image_rect_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera_ground/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /vilens/pose_optimized + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + 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 + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.113917350769043 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.6538326740264893 + Y: -0.9280670285224915 + Z: 0.3764760196208954 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.319797158241272 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.9804503321647644 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001b300000363fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000030b000000950000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005840000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/plane_seg_ros/config/plane_seg_sequential_config.rviz b/plane_seg_ros/config/plane_seg_sequential_config.rviz new file mode 100644 index 0000000..2c8a4a8 --- /dev/null +++ b/plane_seg_ros/config/plane_seg_sequential_config.rviz @@ -0,0 +1,355 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Marker1 + - /GridMap1 + - /PoseWithCovariance1 + - /GridMap2 + - /PointCloud22 + - /Image1 + Splitter Ratio: 0.5058823823928833 + Tree Height: 405 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /plane_seg/hull_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + hull lines: true + Queue Size: 100 + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: /rooster_elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.20000000298023224 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /vilens/pose + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/id_strings + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/centroids + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/linestrips + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: product + Color Transformer: GridMapLayer + Enabled: true + Height Layer: product + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: /rooster_elevation_mapping/filtered_map + Unreliable: false + Use Rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /opencv/rectangles + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /vilens/pose_optimized + Unreliable: false + Value: true + - Alpha: 0.009999999776482582 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 100 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2601 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /os_cloud_node/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /camera_ground/infra1/image_rect_raw/compressed + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera_ground/infra1/image_rect_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + 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 + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.6142683029174805 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.212801933288574 + Y: 0.8613958358764648 + Z: 0.33506956696510315 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6597974300384521 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.950908660888672 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001be00000363fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000220000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002d8000000c80000001600fffffffb0000000a0049006d00610067006501000002630000013d0000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005790000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/plane_seg_ros/include/plane_seg_ros/Pass.hpp b/plane_seg_ros/include/plane_seg_ros/Pass.hpp new file mode 100644 index 0000000..4d34a47 --- /dev/null +++ b/plane_seg_ros/include/plane_seg_ros/Pass.hpp @@ -0,0 +1,103 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planeseg { + +class Pass{ + public: + Pass(ros::NodeHandle& node); + + ~Pass(){ + } + + bool loadParameters(); + void elevationMapCallback(const grid_map_msgs::GridMap& msg); + void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); + void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); + void imageProcessing(grid_map::GridMap &gridmap); + + void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); + void processFromFile(int test_example); + void stepThroughFile(std::string filename); + void publishHullsAsCloud(std::vector::Ptr> cloud_ptrs, + int secs, int nsecs); + + void publishHullsAsMarkersOLD(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, + int secs, int nsecs); + void printResultAsJson(); + void publishResult(); + void publishIdsAsStrings(); + void publishCentroidsAsSpheres(); + void publishHullsAsMarkers(); + void publishLineStrips(); + void publishRectangles(); + void extractNthCloud(std::string filename, int n); + void tic(); + std::chrono::duration toc(); + void gridMapFilterChain(grid_map::GridMap& input_map); + void stepCreation(grid_map::GridMap &gridmap); + void reset(); + void saveGridMapMsgAsPCD(const grid_map_msgs::GridMap& msg, int frame); + void replaceNan(grid_map::GridMap::Matrix& m, const double newValue); + void replaceZeroToNan(grid_map::GridMap::Matrix& m); + void multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap::Matrix& factor2, grid_map::GridMap::Matrix& result); + bool convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage, bool negative); + bool toImageWithNegatives(const grid_map::GridMap& gridMap, const std::string& layer, const int encoding, const float lowerValue, const float upperValue, cv::Mat& image); + void setupSubscribers(); + void getContourData(); + std::vector all_elongations_; + std::vector all_convexities_; + std::vector all_rectangularities_; + + private: + ros::NodeHandle& node_; + std::vector colors_; + std::vector colors_2; + std::vector colors_3; + + ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; + ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_, elev_map_pub_, pose_pub_, id_strings_pub_, centroids_pub_, hulls_pub_, linestrips_pub_, filtered_map_pub_, rectangles_pub_, test_pub_; + + Eigen::Isometry3d last_robot_pose_; + planeseg::BlockFitter::Result result_; + planeseg::Tracker3D tracking_; + planeseg::Visualizer visualizer_; + planeseg::ImageProcessor imgprocessor_; + planeseg::StepCreator stepcreator_; + planeseg::Tracker2D tracking2D_; + filters::FilterChain filter_chain_; +// filters::FilterChain mask_filter_; + tf::TransformListener listener_; + std::string elevation_map_topic_; + std::string filter_chain_parameters_name_; + std::string filtered_map_topic_; + std::string grid_map_sub_topic_; + std::string point_cloud_sub_topic_; + std::string pose_sub_topic_; + std::string algorithm_; + double erode_radius_; + double traversability_threshold_; + bool verbose_timer_; + float gm_resolution_; + std::vector gm_position_; + std::chrono::high_resolution_clock::time_point last_time_; +}; +} diff --git a/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp new file mode 100644 index 0000000..f424526 --- /dev/null +++ b/plane_seg_ros/include/plane_seg_ros/Visualizer.hpp @@ -0,0 +1,37 @@ +#pragma once +#include +#include +#include "plane_seg/Tracker.hpp" +#include "plane_seg/StepCreator.hpp" +#include + +namespace planeseg{ + +struct line_strip { + visualization_msgs::Marker centroidsMarker; + int id; +}; + +class Visualizer { +public: + Visualizer(); + ~Visualizer(); + +// sensor_msgs::PointCloud2 displayCentroids(std::vector &planes); + visualization_msgs::Marker displayCentroid(planeseg::plane plane); + visualization_msgs::Marker displayString(planeseg::plane plane); + visualization_msgs::Marker displayLineStrip(planeseg::plane plane); + visualization_msgs::Marker displayHull(planeseg::plane plane); +// geometry_msgs::PolygonStamped displayRectangle(planeseg::contour contour); + double getR(int id); + double getG(int id); + double getB(int id); + +private: + + std::vector colors_; + std::vector lineStrips; + +}; + +} // namespace planeseg diff --git a/plane_seg_ros/include/plane_seg_ros/geometry_utils.hpp b/plane_seg_ros/include/plane_seg_ros/geometry_utils.hpp new file mode 100644 index 0000000..18fafd5 --- /dev/null +++ b/plane_seg_ros/include/plane_seg_ros/geometry_utils.hpp @@ -0,0 +1,12 @@ +#pragma once +#include +#include + +namespace planeseg{ + +std::string vecToStr(const Eigen::Vector3f& iVec); +std::string rotToStr(const Eigen::Matrix3f& iRot); +void quat_to_euler(const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw); +Eigen::Vector3f convertRobotPoseToSensorLookDir(Eigen::Isometry3d robot_pose); + +} diff --git a/plane_seg_ros/launch/default.launch b/plane_seg_ros/launch/default.launch new file mode 100644 index 0000000..7c7ebf6 --- /dev/null +++ b/plane_seg_ros/launch/default.launch @@ -0,0 +1,14 @@ + + + + + + + + + + +> + + + diff --git a/plane_seg_ros/launch/elevation_map_with_plane_seg.launch b/plane_seg_ros/launch/elevation_map_with_plane_seg.launch new file mode 100644 index 0000000..39309b8 --- /dev/null +++ b/plane_seg_ros/launch/elevation_map_with_plane_seg.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/plane_seg_ros/launch/nth_cloud.launch b/plane_seg_ros/launch/nth_cloud.launch new file mode 100644 index 0000000..739d4b4 --- /dev/null +++ b/plane_seg_ros/launch/nth_cloud.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/plane_seg_ros/launch/plane_seg_sequential_config.rviz b/plane_seg_ros/launch/plane_seg_sequential_config.rviz new file mode 100644 index 0000000..869b08c --- /dev/null +++ b/plane_seg_ros/launch/plane_seg_sequential_config.rviz @@ -0,0 +1,237 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Marker1 + - /GridMap1 + - /PoseWithCovariance1 + Splitter Ratio: 0.5058823823928833 + Tree Height: 726 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /plane_seg/hull_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /plane_seg/hull_markers + Name: Marker + Namespaces: + hull lines: true + Queue Size: 100 + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /rooster_elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.20000000298023224 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /vilens/pose + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/id_strings + Name: MarkerArray + Namespaces: + strings: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /plane_seg/centroids + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /plane_seg/linestrips + Name: MarkerArray + Namespaces: + linestrips: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + 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 + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6.1330952644348145 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.9186757802963257 + Y: 0.04618240147829056 + Z: -0.26492729783058167 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8402029275894165 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.375433921813965 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004400000003efc0100000002fb0000000800540069006d0065010000000000000440000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1088 + X: 67 + Y: 27 diff --git a/plane_seg_ros/launch/rosbag_sequential.launch b/plane_seg_ros/launch/rosbag_sequential.launch new file mode 100644 index 0000000..63d5e93 --- /dev/null +++ b/plane_seg_ros/launch/rosbag_sequential.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + +> + + + diff --git a/plane_seg_ros/src/Visualizer.cpp b/plane_seg_ros/src/Visualizer.cpp new file mode 100644 index 0000000..8980d68 --- /dev/null +++ b/plane_seg_ros/src/Visualizer.cpp @@ -0,0 +1,269 @@ +#include +#include +#include "plane_seg_ros/Visualizer.hpp" +//#include "plane_seg/StepCreator.hpp" +#include +#include +#include +#include +#include "plane_seg/Tracker.hpp" +#include + +#include +#include +#include + +namespace planeseg { + +Visualizer::Visualizer(){ + colors_ = { + 1, 1, 1, // 42 + 255, 255, 120, + 1, 120, 1, + 1, 225, 1, + 120, 255, 1, + 1, 255, 255, + 120, 1, 1, + 255, 120, 255, + 120, 1, 255, + 1, 1, 120, + 255, 255, 255, + 120, 120, 1, + 120, 120, 120, + 1, 1, 255, + 255, 1, 255, + 120, 120, 255, + 120, 255, 120, + 1, 120, 120, + 1, 1, 255, + 255, 1, 1, + 155, 1, 120, + 120, 1, 120, + 255, 120, 1, + 1, 120, 255, + 255, 120, 120, + 1, 255, 120, + 255, 255, 1}; + +} + +Visualizer::~Visualizer(){} + + +visualization_msgs::Marker Visualizer::displayCentroid(planeseg::plane plane){ + + int id = plane.id; + pcl::PointXYZ centroid = plane.centroid; + + // convert centroid to geometry_msgs/Point + geometry_msgs::Point centroidGM; + centroidGM.x = centroid.x; + centroidGM.y = centroid.y; + centroidGM.z = centroid.z; + + visualization_msgs::Marker centroidMarker; + centroidMarker.type = visualization_msgs::Marker::SPHERE; + centroidMarker.header.frame_id = "odom"; + centroidMarker.header.stamp = ros::Time(); + centroidMarker.ns = "centroids"; + centroidMarker.id = id; + centroidMarker.scale.x = 0.05; + centroidMarker.scale.y = 0.05; + centroidMarker.scale.z = 0.05; + centroidMarker.color.r = getR(id); + centroidMarker.color.g = getG(id); + centroidMarker.color.b = getB(id); + centroidMarker.color.a = 1; + centroidMarker.pose.orientation.w = 1.0; + centroidMarker.pose.position.x = centroidGM.x; + centroidMarker.pose.position.y = centroidGM.y; + centroidMarker.pose.position.z = centroidGM.z; + + return centroidMarker; +} + +visualization_msgs::Marker Visualizer::displayLineStrip(planeseg::plane plane){ + + int id = plane.id; + pcl::PointXYZ newCentroid = plane.centroid; + + bool point_added; + point_added = false; + + // convert newCentroid to geometry_msgs/Point + geometry_msgs::Point newCentroidgm; + newCentroidgm.x = newCentroid.x; + newCentroidgm.y = newCentroid.y; + newCentroidgm.z = newCentroid.z; + + // create visualization markers + visualization_msgs::Marker centroidMarker; + visualization_msgs::Marker lineStripMarker; + visualization_msgs::Marker newPlaneMarker; + + // create string of centroid_ID + std::ostringstream s; + s << "centroid_" << id; + std::string centroid_id; + centroid_id = s.str(); + + // first we must find out if the newCentroid should be added to an already existing linestrip + for (unsigned i = 0; i < lineStrips.size() && point_added == false; i++){ + if (id == lineStrips[i].id){ + lineStrips[i].centroidsMarker.points.push_back(newCentroidgm); + centroidMarker = lineStrips[i].centroidsMarker; + centroidMarker.header.frame_id = "odom"; + centroidMarker.header.stamp = ros::Time::now(); + centroidMarker.ns = "linestrips"; + centroidMarker.id = id; + + // declare outgoing marker as the linestrip with matching ID + lineStripMarker = centroidMarker; + point_added = true; + } + } + // if it is the centroid of a new plane, we must create a new linestrip for it + if (point_added == false){ + + // publish a point for the first marker of the new ID + newPlaneMarker.type = visualization_msgs::Marker::POINTS; + newPlaneMarker.header.frame_id = "odom"; + newPlaneMarker.header.stamp = ros::Time::now(); + newPlaneMarker.ns = "linestrips"; + newPlaneMarker.id = id; + newPlaneMarker.scale.x = 0.01; + newPlaneMarker.scale.y = 0.01; + newPlaneMarker.color.r = getR(id); + newPlaneMarker.color.g = getG(id); + newPlaneMarker.color.b = getB(id); + newPlaneMarker.color.a = 1; + newPlaneMarker.pose.orientation.w = 1.0; + newPlaneMarker.points.push_back(newCentroidgm); + + // create a new linestrip for the new ID + centroidMarker.type = visualization_msgs::Marker::LINE_STRIP; + centroidMarker.scale.x = 0.01; + centroidMarker.color.r = getR(id); + centroidMarker.color.g = getG(id); + centroidMarker.color.b = getB(id); + centroidMarker.color.a = 1; + centroidMarker.pose.orientation.w = 1.0; + centroidMarker.points.push_back(newCentroidgm); + line_strip newLineStrip; + newLineStrip.centroidsMarker = centroidMarker; + newLineStrip.id = id; + lineStrips.push_back(newLineStrip); + + // declare the outgoing marker as the new marker + lineStripMarker = newPlaneMarker; + } + + return lineStripMarker; +} + +visualization_msgs::Marker Visualizer::displayString(planeseg::plane plane){ + + int id = plane.id; + pcl::PointXYZ point = plane.centroid; + + // convert pcl::PointXYZ into geometry_msgs::Point + geometry_msgs::Point pointGM; + pointGM.x = point.x; + pointGM.y = point.y; + pointGM.z = point.z; + + visualization_msgs::Marker stringMarker; + stringMarker.header.frame_id = "odom"; + stringMarker.header.stamp = ros::Time(); + stringMarker.ns = "strings"; + stringMarker.id = id; + stringMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + stringMarker.action = visualization_msgs::Marker::ADD; + stringMarker.pose.position.x = pointGM.x; + stringMarker.pose.position.y = pointGM.y; + stringMarker.pose.position.z = pointGM.z; + stringMarker.scale.x = 0.2; + stringMarker.scale.y = 0.2; + stringMarker.scale.z = 0.2; + stringMarker.color.a = 1; + stringMarker.color.r = 1; + stringMarker.color.g = 1; + stringMarker.color.b = 1; + + std::string id_string = std::to_string(id); + stringMarker.text = id_string; + + return stringMarker; +} + +visualization_msgs::Marker Visualizer::displayHull(planeseg::plane plane){ + + geometry_msgs::Point pointGM; + std_msgs::ColorRGBA point_color; + visualization_msgs::Marker hullMarker; + std::string FrameID; + + hullMarker.header.frame_id = "odom"; + hullMarker.header.stamp = ros::Time(); + hullMarker.ns = "hull lines"; + hullMarker.id = plane.id; + hullMarker.type = visualization_msgs::Marker::LINE_STRIP; + hullMarker.action = visualization_msgs::Marker::ADD; + hullMarker.pose.orientation.w = 1.0; + hullMarker.scale.x = 0.03; + hullMarker.color.r = getR(plane.id); + hullMarker.color.g = getG(plane.id); + hullMarker.color.b = getB(plane.id); + hullMarker.color.a = 1; + + for (size_t i = 0; i < plane.cloud.size(); ++i){ + pointGM.x = plane.cloud[i].x; + pointGM.y = plane.cloud[i].y; + pointGM.z = plane.cloud[i].z; + hullMarker.points.push_back(pointGM); + } + + return hullMarker; +} +/* +geometry_msgs::PolygonStamped Visualizer::displayRectangle(planeseg::contour contour){ + + geometry_msgs::PolygonStamped rectMarker; + std::vector pointsGM(4); + for (size_t r = 0; r < contour.points_.size(); ++r){ + + float x, y, z; + x = static_cast(contour.points_[r].x); + y = static_cast(contour.points_[r].y); + z = static_cast(contour.elevation_[0]); + + pointsGM[r].x = x; + pointsGM[r].y = y; + pointsGM[r].z = z; + } + + rectMarker.polygon.points = pointsGM; + rectMarker.header.frame_id = "odom"; + rectMarker.header.stamp = ros::Time(); + + return rectMarker; +} +*/ +double Visualizer::getR(int id){ + double j; + j = id % (colors_.size()/3); + return colors_[3*j]; +} + +double Visualizer::getG(int id){ + unsigned j; + j = id % (colors_.size()/3); + return colors_[3*j+1]; +} + +double Visualizer::getB(int id){ + unsigned j; + j = id % (colors_.size()/3); + return colors_[3*j+2]; +} +} // namespace planeseg diff --git a/plane_seg_ros/src/geometry_utils.cpp b/plane_seg_ros/src/geometry_utils.cpp new file mode 100644 index 0000000..09c4da7 --- /dev/null +++ b/plane_seg_ros/src/geometry_utils.cpp @@ -0,0 +1,41 @@ +#include "plane_seg_ros/geometry_utils.hpp" +namespace planeseg { +// convenience methods +std::string vecToStr(const Eigen::Vector3f& iVec) { + std::ostringstream oss; + oss << iVec[0] << ", " << iVec[1] << ", " << iVec[2]; + return oss.str(); +} + +std::string rotToStr(const Eigen::Matrix3f& iRot) { + std::ostringstream oss; + Eigen::Quaternionf q(iRot); + oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z(); + return oss.str(); +} + +void quat_to_euler(const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw) { + const double q0 = q.w(); + const double q1 = q.x(); + const double q2 = q.y(); + const double q3 = q.z(); + roll = atan2(2.0*(q0*q1+q2*q3), 1.0-2.0*(q1*q1+q2*q2)); + pitch = asin(2.0*(q0*q2-q3*q1)); + yaw = atan2(2.0*(q0*q3+q1*q2), 1.0-2.0*(q2*q2+q3*q3)); +} + +Eigen::Vector3f convertRobotPoseToSensorLookDir(Eigen::Isometry3d robot_pose){ + + Eigen::Quaterniond quat = Eigen::Quaterniond( robot_pose.rotation() ); + double r,p,y; + quat_to_euler(quat, r, p, y); + //std::cout << r*180/M_PI << ", " << p*180/M_PI << ", " << y*180/M_PI << " rpy in Degrees\n"; + + double yaw = y; + double pitch = -p; + double xDir = cos(yaw)*cos(pitch); + double yDir = sin(yaw)*cos(pitch); + double zDir = sin(pitch); + return Eigen::Vector3f(xDir, yDir, zDir); +} +} diff --git a/plane_seg_ros/src/plane_seg_ros.cpp b/plane_seg_ros/src/plane_seg_ros.cpp index 8890974..0224c82 100644 --- a/plane_seg_ros/src/plane_seg_ros.cpp +++ b/plane_seg_ros/src/plane_seg_ros.cpp @@ -2,172 +2,211 @@ #include #include #include +#include +#include +#include +#include +#include "plane_seg/ImageProcessor.hpp" +#include "plane_seg/BlockFitter.hpp" +#include "plane_seg/Tracker.hpp" +//#include "plane_seg/StepCreator.hpp" + +#include +#include #include #include #include #include +#include -#include -#include #include -#include +#include -#include #include #include +#include +#include +#include +#include +#include -#include "plane_seg/BlockFitter.hpp" - - -// convenience methods -auto vecToStr = [](const Eigen::Vector3f& iVec) { - std::ostringstream oss; - oss << iVec[0] << ", " << iVec[1] << ", " << iVec[2]; - return oss.str(); -}; -auto rotToStr = [](const Eigen::Matrix3f& iRot) { - std::ostringstream oss; - Eigen::Quaternionf q(iRot); - oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z(); - return oss.str(); -}; - - -class Pass{ - public: - Pass(ros::NodeHandle node_); - - ~Pass(){ - } - - void elevationMapCallback(const grid_map_msgs::GridMap& msg); - void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); - void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); +#include - void processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir); - void processFromFile(int test_example); +#include +#include - void publishHullsAsCloud(std::vector::Ptr> cloud_ptrs, - int secs, int nsecs); +#include "plane_seg_ros/Pass.hpp" +#include "plane_seg_ros/geometry_utils.hpp" - void publishHullsAsMarkers(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, - int secs, int nsecs); - void printResultAsJson(); - void publishResult(); - private: - ros::NodeHandle node_; - std::vector colors_; +#define foreach BOOST_FOREACH - ros::Subscriber point_cloud_sub_, grid_map_sub_, pose_sub_; - ros::Publisher received_cloud_pub_, hull_cloud_pub_, hull_markers_pub_, look_pose_pub_; - Eigen::Isometry3d last_robot_pose_; - planeseg::BlockFitter::Result result_; -}; +using namespace planeseg; -Pass::Pass(ros::NodeHandle node_): - node_(node_){ +Pass::Pass(ros::NodeHandle& node): + node_(node), + filter_chain_("grid_map::GridMap") +{ std::string input_body_pose_topic; - node_.getParam("input_body_pose_topic", input_body_pose_topic); - - grid_map_sub_ = node_.subscribe("/elevation_mapping/elevation_map", 100, - &Pass::elevationMapCallback, this); - point_cloud_sub_ = node_.subscribe("/plane_seg/point_cloud_in", 100, - &Pass::pointCloudCallback, this); - pose_sub_ = node_.subscribe("/state_estimator/pose_in_odom", 100, - &Pass::robotPoseCallBack, this); + if(!node_.getParam("input_body_pose_topic", input_body_pose_topic)){ + ROS_WARN_STREAM("Couldn't get parameter: input_body_pose_topic"); + } received_cloud_pub_ = node_.advertise("/plane_seg/received_cloud", 10); hull_cloud_pub_ = node_.advertise("/plane_seg/hull_cloud", 10); hull_markers_pub_ = node_.advertise("/plane_seg/hull_markers", 10); look_pose_pub_ = node_.advertise("/plane_seg/look_pose", 10); + elev_map_pub_ = node_.advertise("/rooster_elevation_mapping/elevation_map", 1); + pose_pub_ = node_.advertise("/vilens/pose", 1); + id_strings_pub_ = node_.advertise("/plane_seg/id_strings", 10); + centroids_pub_ = node_.advertise("/plane_seg/centroids", 10); + hulls_pub_ = node_.advertise("/plane_seg/hulls", 10); + linestrips_pub_ = node_.advertise("/plane_seg/linestrips", 10); + filtered_map_pub_ = node_.advertise("/rooster_elevation_mapping/filtered_map", 1, true); + rectangles_pub_ = node_.advertise("/opencv/rectangles", 100); + test_pub_ = node_.advertise("/opencv/test", 10); last_robot_pose_ = Eigen::Isometry3d::Identity(); - colors_ = { - 51/255.0, 160/255.0, 44/255.0, //0 - 166/255.0, 206/255.0, 227/255.0, - 178/255.0, 223/255.0, 138/255.0,//6 - 31/255.0, 120/255.0, 180/255.0, - 251/255.0, 154/255.0, 153/255.0,// 12 - 227/255.0, 26/255.0, 28/255.0, - 253/255.0, 191/255.0, 111/255.0,// 18 - 106/255.0, 61/255.0, 154/255.0, - 255/255.0, 127/255.0, 0/255.0, // 24 - 202/255.0, 178/255.0, 214/255.0, - 1.0, 0.0, 0.0, // red // 30 - 0.0, 1.0, 0.0, // green - 0.0, 0.0, 1.0, // blue// 36 - 1.0, 1.0, 0.0, - 1.0, 0.0, 1.0, // 42 - 0.0, 1.0, 1.0, - 0.5, 1.0, 0.0, - 1.0, 0.5, 0.0, - 0.5, 0.0, 1.0, - 1.0, 0.0, 0.5, - 0.0, 0.5, 1.0, - 0.0, 1.0, 0.5, - 1.0, 0.5, 0.5, - 0.5, 1.0, 0.5, - 0.5, 0.5, 1.0, - 0.5, 0.5, 1.0, - 0.5, 1.0, 0.5, - 0.5, 0.5, 1.0}; + tracking_ = planeseg::Tracker3D(); + tracking2D_ = planeseg::Tracker2D(); + visualizer_ = planeseg::Visualizer(); + imgprocessor_ = planeseg::ImageProcessor(); + stepcreator_ = planeseg::StepCreator(); -} + if(!node_.param("algorithm", algorithm_, std::string("A"))){ + ROS_WARN_STREAM("Couldn't get parameter: algorithm"); + } + ROS_INFO("ALGORITHM %s", algorithm_.c_str()); -void Pass::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg){ - //std::cout << "got pose\n"; - tf::poseMsgToEigen(msg->pose.pose, last_robot_pose_); -} + if(!node_.getParam("input_topic", elevation_map_topic_)){ + ROS_WARN_STREAM("Couldn't get parameter: input_topic"); + } + if(!node_.getParam("erode_radius", erode_radius_)){ + ROS_WARN_STREAM("Couldn't get parameter: erode_radius"); + } + ROS_INFO("Erode Radius [%f]", erode_radius_); + if(!node_.getParam("traversability_threshold", traversability_threshold_)){ + ROS_WARN_STREAM("Couldn't get parameter: traversability_threshold"); + } + ROS_INFO("traversability_threshold [%f]", traversability_threshold_); + if(!node_.getParam("verbose_timer", verbose_timer_)){ + ROS_WARN_STREAM("Couldn't get parameter: verbose_timer"); + } + if(!node_.getParam("verbose_timer", verbose_timer_)){ + ROS_WARN_STREAM("Couldn't get parameter: verbose_timer"); + } + if(!node_.getParam("grid_map_sub_topic", grid_map_sub_topic_)){ + ROS_WARN_STREAM("Couldn't get parameter: grid_map_sub_topic"); + } + if(!node_.getParam("point_cloud_sub_topic", point_cloud_sub_topic_)){ + ROS_WARN_STREAM("Couldn't get parameter: point_cloud_sub_topic"); + } + if(!node_.getParam("pose_sub_topic", pose_sub_topic_)){ + ROS_WARN_STREAM("Couldn't get parameter: pose_sub_topic"); + } + std::string param_name = "grid_map_filters"; + XmlRpc::XmlRpcValue config; + if(!node_.getParam(param_name, config)) { + ROS_ERROR("Could not load the filter chain configuration from parameter %s, are you sure it was pushed to the parameter server? Assuming that you meant to leave it empty.", param_name.c_str()); + return; + } -void quat_to_euler(const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw) { - const double q0 = q.w(); - const double q1 = q.x(); - const double q2 = q.y(); - const double q3 = q.z(); - roll = atan2(2.0*(q0*q1+q2*q3), 1.0-2.0*(q1*q1+q2*q2)); - pitch = asin(2.0*(q0*q2-q3*q1)); - yaw = atan2(2.0*(q0*q3+q1*q2), 1.0-2.0*(q2*q2+q3*q3)); -} + // Setup filter chain + if (!filter_chain_.configure(param_name, node_)){ + std::cout << "couldn't configure filter chain" << std::endl; + return; + } -Eigen::Vector3f convertRobotPoseToSensorLookDir(Eigen::Isometry3d robot_pose){ + colors_ = { + 1, 1, 1, // 42 + 255, 255, 120, + 1, 120, 1, + 225, 120, 1, + 1, 255, 1, + 1, 255, 255, + 120, 1, 1, + 255, 120, 255, + 120, 1, 255, + 1, 1, 120, + 255, 255, 255, + 120, 120, 1, + 120, 120, 120, + 1, 1, 255, + 255, 1, 255, + 120, 120, 255, + 120, 255, 120, + 1, 120, 120, + 120, 255, 255, + 255, 1, 1, + 155, 1, 120, + 120, 1, 120, + 255, 120, 1, + 1, 120, 255, + 255, 120, 120, + 1, 255, 120, + 255, 255, 1}; - Eigen::Quaterniond quat = Eigen::Quaterniond( robot_pose.rotation() ); - double r,p,y; - quat_to_euler(quat, r, p, y); - //std::cout << r*180/M_PI << ", " << p*180/M_PI << ", " << y*180/M_PI << " rpy in Degrees\n"; +} - double yaw = y; - double pitch = -p; - double xDir = cos(yaw)*cos(pitch); - double yDir = sin(yaw)*cos(pitch); - double zDir = sin(pitch); - return Eigen::Vector3f(xDir, yDir, zDir); +void Pass::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg){ + //std::cout << "got pose\n"; + tf::poseMsgToEigen(msg->pose.pose, last_robot_pose_); } void Pass::elevationMapCallback(const grid_map_msgs::GridMap& msg){ - //std::cout << "got grid map / ev map\n"; + std::cout << "got grid map / ev map\n"; // convert message to GridMap, to PointCloud to LabeledCloud grid_map::GridMap map; grid_map::GridMapRosConverter::fromMessage(msg, map); + std::vector layers; + layers = map.getLayers(); + for (size_t k = 0; k < layers.size(); ++k){ + std::cout << layers[k] << std::endl; + }; + + if (algorithm_ == "C"){ + std::cout << "C" << std::endl; + gridMapFilterChain(map); + imageProcessing(map); + stepCreation(map); + reset(); + return; + } + + if (algorithm_ == "B"){ + std::cout << "B" << std::endl; + gridMapFilterChain(map); + imageProcessing(map); + reset(); + } + sensor_msgs::PointCloud2 pointCloud; - grid_map::GridMapRosConverter::toPointCloud(map, "elevation", pointCloud); + + if (algorithm_ == "A"){ + std::cout << "A end" << std::endl; + grid_map::GridMapRosConverter::toPointCloud(map, "elevation", pointCloud); // takes "elevation" for legacy algorithm + } else { + std::cout << "B end" << std::endl; + grid_map::GridMapRosConverter::toPointCloud(map, "product", pointCloud); // takes "product" for masking algorithm + } + planeseg::LabeledCloud::Ptr inCloud(new planeseg::LabeledCloud()); - pcl::fromROSMsg(pointCloud,*inCloud); + pcl::fromROSMsg(pointCloud, *inCloud); Eigen::Vector3f origin, lookDir; origin << last_robot_pose_.translation().cast(); lookDir = convertRobotPoseToSensorLookDir(last_robot_pose_); processCloud(inCloud, origin, lookDir); + } @@ -243,9 +282,71 @@ void Pass::processFromFile(int test_example){ processCloud(inCloud, origin, lookDir); } +void Pass::stepThroughFile(std::string filename){ + + std::cout << filename < topics; + topics.push_back(std::string("/rooster_elevation_mapping/elevation_map")); + topics.push_back(std::string("/vilens/pose")); + + rosbag::View view(bag, rosbag::TopicQuery(topics)); + std::vector timing_vector; + + foreach(rosbag::MessageInstance const m, view){ + grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); + + if (s != NULL){ + std::cin.get(); + tic(); + + ++frame; + + std::cout << "frames = " << frame << std::endl; +// std::cout << "received gridmap at time " << m.getTime().toNSec() << " with resolution:" << s->info.resolution << "and time: " << s->info.header.stamp << std::endl; + std::cout << "rosbag time: " << s->info.header.stamp << std::endl; + elevationMapCallback(*s); + + std::cout << "Press [Enter] to continue to next gridmap message" << std::endl; + double frame_time; + frame_time = toc().count(); + std::cout << frame_time << " ms: frame_" << frame << std::endl; + timing_vector.push_back(frame_time); + elev_map_pub_.publish(*s); + } + + geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); + if (i !=NULL){ + // std::cout << "position (x, y, z): " << i->pose.pose.position.x << ", " << i->pose.pose.position.y << ", " << i->pose.pose.position.z << std::endl; + robotPoseCallBack(i); + pose_pub_.publish(*i); + } + } + + std::cout << "Timings: " << std::endl; + for (size_t k = 0; k < timing_vector.size(); ++k){ + std::cout << timing_vector[k] << ", "; + } + std::cout << std::endl; + bag.close(); + + + double average = 0.0; + double sum = 0.0; + for (size_t k = 0; k < timing_vector.size(); ++k){ + sum = sum + timing_vector[k]; + } + average = sum / timing_vector.size(); +// average = std::accumulate(timing_vector.begin(), timing_vector.end(), 0.0)/timing_vector.size(); + std::cout << "The average time per frame is" << average << std::endl; +} + void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f origin, Eigen::Vector3f lookDir){ + std::cout << "entered processCloud" << std::endl; planeseg::BlockFitter fitter; fitter.setSensorPose(origin, lookDir); fitter.setCloud(inCloud); @@ -257,7 +358,9 @@ void Pass::processCloud(planeseg::LabeledCloud::Ptr& inCloud, Eigen::Vector3f or fitter.setMaxAngleOfPlaneSegmenter(10); result_ = fitter.go(); - + tracking_.reset(); + tracking_.convertResult(result_); +// tracking_.printIds(); Eigen::Vector3f rz = lookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); @@ -339,8 +442,11 @@ void Pass::publishResult(){ cloud_ptrs.push_back(cloud_ptr); } + publishIdsAsStrings(); + publishCentroidsAsSpheres(); + publishHullsAsMarkers(); + publishLineStrips(); publishHullsAsCloud(cloud_ptrs, 0, 0); - publishHullsAsMarkers(cloud_ptrs, 0, 0); //pcl::PCDWriter pcd_writer_; //pcd_writer_.write ("/home/mfallon/out.pcd", cloud, false); @@ -348,7 +454,6 @@ void Pass::publishResult(){ //std::cout << "cloud: " << cloud.points.size() << " pts\n"; } - // combine the individual clouds into one, with a different each void Pass::publishHullsAsCloud(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, int secs, int nsecs){ @@ -381,123 +486,468 @@ void Pass::publishHullsAsCloud(std::vector< pcl::PointCloud::Ptr } -void Pass::publishHullsAsMarkers(std::vector< pcl::PointCloud::Ptr > cloud_ptrs, - int secs, int nsecs){ + + +void Pass::publishIdsAsStrings(){ + visualization_msgs::MarkerArray strings_array; + for (size_t i = 0; i < tracking_.newStairs.size(); ++i){ + visualization_msgs::Marker id_marker; + id_marker = visualizer_.displayString(tracking_.newStairs[i]); + id_marker.frame_locked = true; + strings_array.markers.push_back(id_marker); + } + id_strings_pub_.publish(strings_array); +} + +void Pass::publishCentroidsAsSpheres(){ + visualization_msgs::MarkerArray centroids_array; + for (size_t i = 0; i < tracking_.newStairs.size(); ++i){ + visualization_msgs::Marker centroid_marker; + centroid_marker = visualizer_.displayCentroid(tracking_.newStairs[i]); + centroid_marker.frame_locked = true; + centroids_array.markers.push_back(centroid_marker); + } + centroids_pub_.publish(centroids_array); +} + +void Pass::publishHullsAsMarkers(){ + + std::vector> clouds; + for (size_t k = 0; k < tracking_.newStairs.size(); ++k){ + clouds.push_back(tracking_.newStairs[k].cloud); + } + + std::vector ids; + for (size_t m = 0; m < tracking_.newStairs.size(); ++m){ + ids.push_back(tracking_.newStairs[m].id); + } + geometry_msgs::Point point; std_msgs::ColorRGBA point_color; - visualization_msgs::Marker marker; + visualization_msgs::Marker hullMarker; std::string frameID; // define markers - marker.header.frame_id = "odom"; - marker.header.stamp = ros::Time(secs, nsecs); - marker.ns = "hull lines"; - marker.id = 0; - marker.type = visualization_msgs::Marker::LINE_LIST; //visualization_msgs::Marker::POINTS; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = 0; - marker.pose.position.y = 0; - marker.pose.position.z = 0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.03; - marker.scale.y = 0.03; - marker.scale.z = 0.03; - marker.color.a = 1.0; - - for (size_t i = 0; i < cloud_ptrs.size (); i++){ - - int nColor = i % (colors_.size()/3); - double r = colors_[nColor*3]*255.0; - double g = colors_[nColor*3+1]*255.0; - double b = colors_[nColor*3+2]*255.0; - - for (size_t j = 1; j < cloud_ptrs[i]->points.size (); j++){ - point.x = cloud_ptrs[i]->points[j-1].x; - point.y = cloud_ptrs[i]->points[j-1].y; - point.z = cloud_ptrs[i]->points[j-1].z; + hullMarker.header.frame_id = "odom"; + hullMarker.header.stamp = ros::Time(0, 0); + hullMarker.ns = "hull lines"; + hullMarker.id = 0; + hullMarker.type = visualization_msgs::Marker::LINE_LIST; //visualization_msgs::Marker::POINTS; + hullMarker.action = visualization_msgs::Marker::ADD; + hullMarker.pose.position.x = 0; + hullMarker.pose.position.y = 0; + hullMarker.pose.position.z = 0; + hullMarker.pose.orientation.x = 0.0; + hullMarker.pose.orientation.y = 0.0; + hullMarker.pose.orientation.z = 0.0; + hullMarker.pose.orientation.w = 1.0; + hullMarker.scale.x = 0.03; + hullMarker.scale.y = 0.03; + hullMarker.scale.z = 0.03; + hullMarker.color.a = 1.0; + + for (size_t i = 0; i < clouds.size (); i++){ + + double r = visualizer_.getR(ids[i]); + double g = visualizer_.getG(ids[i]); + double b = visualizer_.getB(ids[i]); + + for (size_t j = 1; j < clouds[i].points.size (); j++){ + point.x = clouds[i].points[j-1].x; + point.y = clouds[i].points[j-1].y; + point.z = clouds[i].points[j-1].z; point_color.r = r; point_color.g = g; point_color.b = b; point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); // - point.x = cloud_ptrs[i]->points[j].x; - point.y = cloud_ptrs[i]->points[j].y; - point.z = cloud_ptrs[i]->points[j].z; + point.x = clouds[i].points[j].x; + point.y = clouds[i].points[j].y; + point.z = clouds[i].points[j].z; point_color.r = r; point_color.g = g; point_color.b = b; point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); } // start to end line: - point.x = cloud_ptrs[i]->points[0].x; - point.y = cloud_ptrs[i]->points[0].y; - point.z = cloud_ptrs[i]->points[0].z; + point.x = clouds[i].points[0].x; + point.y = clouds[i].points[0].y; + point.z = clouds[i].points[0].z; point_color.r = r; point_color.g = g; point_color.b = b; point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); - point.x = cloud_ptrs[i]->points[ cloud_ptrs[i]->points.size()-1 ].x; - point.y = cloud_ptrs[i]->points[ cloud_ptrs[i]->points.size()-1 ].y; - point.z = cloud_ptrs[i]->points[ cloud_ptrs[i]->points.size()-1 ].z; + point.x = clouds[i].points[ clouds[i].points.size()-1 ].x; + point.y = clouds[i].points[ clouds[i].points.size()-1 ].y; + point.z = clouds[i].points[ clouds[i].points.size()-1 ].z; point_color.r = r; point_color.g = g; point_color.b = b; point_color.a = 1.0; - marker.colors.push_back(point_color); - marker.points.push_back(point); + hullMarker.colors.push_back(point_color); + hullMarker.points.push_back(point); + } + hullMarker.frame_locked = true; + hull_markers_pub_.publish(hullMarker); +} + + +void Pass::publishLineStrips(){ + visualization_msgs::MarkerArray linestrips_array; + for (size_t i = 0; i < tracking_.newStairs.size(); ++i){ + visualization_msgs::Marker linestrip_marker; + linestrip_marker = visualizer_.displayLineStrip(tracking_.newStairs[i]); + linestrip_marker.frame_locked = true; + linestrips_array.markers.push_back(linestrip_marker); + } + linestrips_pub_.publish(linestrips_array); +} + +void Pass::publishRectangles(){ + std::cout << "Entered publishRectangles" << std::endl; + visualization_msgs::MarkerArray rectangles_array; + + std::vector ids; + for (size_t m = 0; m < tracking2D_.newRects_.size(); ++m){ + ids.push_back(tracking2D_.newRects_[m].id_); + } + + std::cout << "Elevation values: " << std::endl; + for (size_t i = 0; i < stepcreator_.rectangles_.size(); ++i){ + + double r = visualizer_.getR(ids[i]); + double g = visualizer_.getG(ids[i]); + double b = visualizer_.getB(ids[i]); + +// std::cout << stepcreator_.rectangles_[i].elevation_ << std::endl; + + planeseg::contour contour = stepcreator_.rectangles_[i]; + + visualization_msgs::Marker rectMarker; + rectMarker.header.frame_id = "odom"; + rectMarker.header.stamp = ros::Time::now(); + rectMarker.ns = "rectangles"; + rectMarker.id = i; + rectMarker.type = visualization_msgs::Marker::LINE_STRIP; + rectMarker.action = visualization_msgs::Marker::ADD; + rectMarker.pose.orientation.w = 0.0; + rectMarker.scale.x = 0.05; + rectMarker.color.r = r; + rectMarker.color.g = g; + rectMarker.color.b = b; + rectMarker.color.a = 1; + std::vector pointsGM(contour.points_.size()+1); + int count; + + for (size_t r = 0; r < contour.points_.size(); ++r){ + + float x, y, z; + x = (static_cast(contour.points_[r].x) * gm_resolution_); //contour.points_[0].x + y = (static_cast(contour.points_[r].y) * gm_resolution_); //contour.points_[0].y + z = static_cast(contour.elevation_); + + pointsGM[r].x = -y + ((imgprocessor_.final_img_.image.rows / 2) * gm_resolution_) + gm_position_[0]; + pointsGM[r].y = -x + ((imgprocessor_.final_img_.image.cols / 2) * gm_resolution_) + gm_position_[1]; + pointsGM[r].z = z; + count = r; + } + + // re-add first point to close the loop + float x, y, z; + x = (static_cast(contour.points_[0].x) * gm_resolution_); + y = (static_cast(contour.points_[0].y) * gm_resolution_); + z = static_cast(contour.elevation_); + + pointsGM[count+1].x = -y + ((imgprocessor_.final_img_.image.rows / 2) * gm_resolution_) + gm_position_[0]; + pointsGM[count+1].y = -x + ((imgprocessor_.final_img_.image.cols / 2) * gm_resolution_) + gm_position_[1]; + pointsGM[count+1].z = z; + + rectMarker.points = pointsGM; + rectMarker.frame_locked = true; + rectangles_array.markers.push_back(rectMarker); + } + + std::cout << "3" << std::endl; + rectangles_pub_.publish(rectangles_array); + +} + +void Pass::extractNthCloud(std::string filename, int n){ + + std::cout << filename < topics; + topics.push_back(std::string("/rooster_elevation_mapping/elevation_map")); + topics.push_back(std::string("/vilens/pose")); + + rosbag::View view(bag, rosbag::TopicQuery(topics)); + + foreach(rosbag::MessageInstance const m, view){ + grid_map_msgs::GridMap::ConstPtr s = m.instantiate(); + + if (s != NULL){ + ++frame; + if (frame == n){ + std::cin.get(); + tic(); + elevationMapCallback(*s); + std::cout << toc().count() << " ms: frame_" << frame << std::endl; + } + } + + geometry_msgs::PoseWithCovarianceStamped::ConstPtr i = m.instantiate(); + if (i !=NULL){ + if (frame == n){ + robotPoseCallBack(i); + pose_pub_.publish(*i); + } + } + } + + +bag.close(); +} + +void Pass::imageProcessing(grid_map::GridMap &gridmap){ + + const float nanValue = 1; + replaceNan(gridmap.get("slope"), nanValue); + + convertGridmapToFloatImage(gridmap, "slope", imgprocessor_.original_img_, true); + + imgprocessor_.process();; +// getContourData(); + + std::cout << "got to here" << std::endl; + + if (algorithm_ == "B"){ + + grid_map::GridMapCvConverter::addLayerFromImage(imgprocessor_.final_img_.image, "mask", gridmap); + + gridmap.add("product"); + multiplyLayers(gridmap.get("elevation"), gridmap.get("mask"), gridmap.get("product")); + replaceZeroToNan(gridmap.get("product")); + } + + + // Publish updated grid map. + grid_map_msgs::GridMap output_msg; + grid_map::GridMapRosConverter::toMessage(gridmap, output_msg); + filtered_map_pub_.publish(output_msg); +} + +void Pass::stepCreation(grid_map::GridMap &gridmap){ + + std::cout << "Gridmap resolution = " << gridmap.getResolution() << std::endl; + std::cout << "Gridmap position = " << gridmap.getPosition()[0] << ", " << gridmap.getPosition()[1] << std::endl; + gm_resolution_ = gridmap.getResolution(); + gm_position_.push_back(gridmap.getPosition()[0]); + gm_position_.push_back(gridmap.getPosition()[1]); + + convertGridmapToFloatImage(gridmap, "elevation", stepcreator_.elevation_, true); + stepcreator_.pnts_ = imgprocessor_.all_contours_.contours_rect_; + stepcreator_.processed_ = imgprocessor_.final_img_; + stepcreator_.go(); + tracking2D_.reset(); + tracking2D_.assignIDs(stepcreator_.rectangles_); +// tracking2D_.printIds(); +// grid_map::GridMapCvConverter::addLayerFromImage(stepcreator_.elevation_masked_.image, "reconstructed", gridmap); + publishRectangles(); +// tracking2D_.reset(); +} + +void Pass::getContourData(){ + // Retrieve information about contours from image processor + for (size_t i = 0; i < imgprocessor_.ip_elongations_.size(); ++i){ + all_elongations_.push_back(imgprocessor_.ip_elongations_[i]); + } + for (size_t j = 0; j < imgprocessor_.ip_convexities_.size(); ++j){ + all_convexities_.push_back(imgprocessor_.ip_convexities_[j]); + } + for (size_t k = 0; k < imgprocessor_.ip_rectangularities_.size(); ++k){ + all_rectangularities_.push_back(imgprocessor_.ip_rectangularities_[k]); + } + + std::cout << "All elongations:" << std::endl; + for(size_t i = 0; i < all_elongations_.size(); ++i){ + std::cout << all_elongations_[i] << ", "; + } + std::cout << std::endl; + + std::cout << "All convexities:" << std::endl; + for(size_t i = 0; i < all_convexities_.size(); ++i){ + std::cout << all_convexities_[i] << ", "; + } + std::cout << std::endl; + + std::cout << "All rectangularities:" << std::endl; + for(size_t i = 0; i < all_rectangularities_.size(); ++i){ + std::cout << all_rectangularities_[i] << ", "; + } + std::cout << std::endl; +} + + +void Pass::reset(){ + gm_position_.clear(); + imgprocessor_.reset(); + stepcreator_.reset(); +// tracking_.reset(); +// tracking2D_.reset(); +} + +void Pass::tic(){ + last_time_ = std::chrono::high_resolution_clock::now(); +} + +std::chrono::duration Pass::toc(){ + auto now_time = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed_time = std::chrono::duration_cast(now_time - last_time_); + last_time_ = now_time; + // std::cout << elapsedTime.count() << "ms elapsed" << std::endl; + return elapsed_time; +} + +void Pass::gridMapFilterChain(grid_map::GridMap& input_map){ +// tic(); + + // Apply filter chain. + grid_map::GridMap output_map; + if (!filter_chain_.update(input_map, output_map)) { + std::cout << "couldn't update the grid map filter chain" << std::endl; + grid_map_msgs::GridMap failmessage; + grid_map::GridMapRosConverter::toMessage(input_map, failmessage); + return; + } +/* + if (verbose_timer_) { + std::cout << toc().count() << " ms: filter chain\n"; } - marker.frame_locked = true; - hull_markers_pub_.publish(marker); + + tic(); +*/ + // Publish filtered output grid map. + grid_map_msgs::GridMap output_msg; + grid_map::GridMapRosConverter::toMessage(output_map, output_msg); + filtered_map_pub_.publish(output_msg); + + input_map = output_map; } +void Pass::saveGridMapMsgAsPCD(const grid_map_msgs::GridMap& msg, int frame){ + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromMessage(msg, grid_map); + sensor_msgs::PointCloud2 pointCloud_sensor_msg; + grid_map::GridMapRosConverter::toPointCloud(grid_map, "elevation", pointCloud_sensor_msg); + pcl::PointCloud point_cloud; + pcl::fromROSMsg(pointCloud_sensor_msg, point_cloud); -int main( int argc, char** argv ){ - // Turn off warning message about labels - // TODO: look into how labels are used - pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + std::string pcd_filename; + pcd_filename = "/home/christos/rosbags/pcd_by_frame/pcd_frame_" + std::to_string(frame) + ".pcd"; + pcl::io::savePCDFile(pcd_filename, point_cloud); + std::cout << "Saved " << point_cloud.size () << " data points to " << pcd_filename << std::endl; +} - ros::init(argc, argv, "plane_seg"); - ros::NodeHandle nh("~"); - std::unique_ptr app = std::make_unique(nh); +void Pass::replaceNan(grid_map::GridMap::Matrix& m, const double newValue){ - ROS_INFO_STREAM("plane_seg ros ready"); - ROS_INFO_STREAM("============================="); + for(int r = 0; r < m.rows(); r++) + { + for(int c = 0; c < m.cols(); c++) + { + if (std::isnan(m(r,c))) + { + m(r,c) = newValue; + } + } + } +} - bool run_test_program = false; - nh.param("/plane_seg/run_test_program", run_test_program, false); - std::cout << "run_test_program: " << run_test_program << "\n"; +void Pass::replaceZeroToNan(grid_map::GridMap::Matrix& m){ + for(int r = 0; r < m.rows(); r++) + { + for(int c = 0; c < m.cols(); c++) + { + if (m(r,c) == 0) + { + m(r,c) = NAN; + } + } + } +} - // Enable this to run the test programs - if (run_test_program){ - std::cout << "Running test examples\n"; - app->processFromFile(0); - app->processFromFile(1); - app->processFromFile(2); - app->processFromFile(3); - // RACE examples don't work well - //app->processFromFile(4); - //app->processFromFile(5); +void Pass::multiplyLayers(grid_map::GridMap::Matrix& factor1, grid_map::GridMap::Matrix& factor2, grid_map::GridMap::Matrix& result){ - std::cout << "Finished!\n"; - exit(-1); + for(int r = 0; r < result.rows(); r++) + { + for(int c = 0; c < result.cols(); c++) + { + result(r,c) = factor1(r,c) * factor2(r,c); + } } +} - ROS_INFO_STREAM("Waiting for ROS messages"); - ros::spin(); +bool Pass::convertGridmapToFloatImage(const grid_map::GridMap& gridMap, const std::string& layer, cv_bridge::CvImage& cvImage, bool negative){ + std::cout << "Entered convertGridmapToFloatImage" << std::endl; + cvImage.header.stamp.fromNSec(gridMap.getTimestamp()); + cvImage.header.frame_id = gridMap.getFrameId(); + cvImage.encoding = CV_32F; + + if (negative == false) { + return grid_map::GridMapCvConverter::toImage(gridMap, layer, CV_32F, 0, 1, cvImage.image); + } else { + return toImageWithNegatives(gridMap, layer, CV_32F, 0, 1, cvImage.image); + } +} + +bool Pass::toImageWithNegatives(const grid_map::GridMap& gridMap, const std::string& layer, const int encoding, const float lowerValue, const float upperValue, cv::Mat& image){ + + // Initialize image. + if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) { + image = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding); + } else { + std::cerr << "Invalid grid map?" << std::endl; + return false; + } + + // Get max image value. + float imageMax; + imageMax = 1; + + grid_map::GridMap map = gridMap; + const grid_map::Matrix& data = map[layer]; + + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index index(*iterator); + const float& value = data(index(0), index(1)); + if (std::isfinite(value)) { + const float imageValue = (float)(((value - lowerValue) / (upperValue - lowerValue)) * (float)imageMax); + const grid_map::Index imageIndex(iterator.getUnwrappedIndex()); + unsigned int channel = 0; + image.at>(imageIndex(0), imageIndex(1))[channel] = imageValue; + } + } + + return true; +} - return 1; +void Pass::setupSubscribers(){ + grid_map_sub_ = node_.subscribe(grid_map_sub_topic_, 100, + &Pass::elevationMapCallback, this); + point_cloud_sub_ = node_.subscribe(point_cloud_sub_topic_, 100, + &Pass::pointCloudCallback, this); + pose_sub_ = node_.subscribe(pose_sub_topic_, 100, + &Pass::robotPoseCallBack, this); } diff --git a/plane_seg_ros/src/plane_seg_ros_node.cpp b/plane_seg_ros/src/plane_seg_ros_node.cpp new file mode 100644 index 0000000..45df8c3 --- /dev/null +++ b/plane_seg_ros/src/plane_seg_ros_node.cpp @@ -0,0 +1,80 @@ +#include +#include "plane_seg_ros/Pass.hpp" +#include +#include +#include + +using namespace planeseg; + +int main( int argc, char** argv ){ + // Turn off warning message about labels + // TODO: look into how labels are used + pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + + + ros::init(argc, argv, "plane_seg"); + ros::NodeHandle nh("~"); + + std::unique_ptr app = std::make_unique(nh); + + ROS_INFO_STREAM("plane_seg ros ready"); + ROS_INFO_STREAM("============================="); + + bool run_test_program = false; + nh.param("run_test_program", run_test_program, false); + std::cout << "run_test_program: " << std::boolalpha << run_test_program << "\n"; + + // Enable this to run the test programs + if (run_test_program){ + std::cout << "Running test examples\n"; + app->processFromFile(0); + app->processFromFile(1); + app->processFromFile(2); + app->processFromFile(3); + // RACE examples don't work well + //app->processFromFile(4); + //app->processFromFile(5); + + std::cout << "Finished!\n"; + return 0; + } + + bool run_sequential_test = false; + nh.param("run_sequential_test", run_sequential_test, false); + std::cout << "run_sequential_test: " << std::boolalpha << run_sequential_test << "\n"; + + //Enable this to run the sequential test + if (run_sequential_test){ + std::cout << "Running sequential test\n"; + + std::string filename_; + nh.getParam("rosbag_path", filename_); + + app->stepThroughFile(filename_); + std::cout << "Finished!\n"; + return 0; + } + + bool run_nth_cloud = false; + nh.param("run_nth_cloud", run_nth_cloud, false); + std::cout << "run_nth_cloud: " << std::boolalpha << run_nth_cloud << "\n"; + + // Enable this to run nth coud + if (run_nth_cloud){ + int n_ = 1; + nh.getParam("n", n_); + std::cout << "Running " << n_ << "-th cloud\n"; + std::string filename_; + nh.getParam("rosbag_path", filename_); + + app->extractNthCloud(filename_, n_); + std::cout << "Finshed!\n"; + return 0; + } + + app->setupSubscribers(); + ROS_INFO_STREAM("Waiting for ROS messages"); + ros::spin(); + + return 0; +}