diff --git a/ed_sensor_integration/CMakeLists.txt b/ed_sensor_integration/CMakeLists.txt index 67934c20..5f414bbf 100644 --- a/ed_sensor_integration/CMakeLists.txt +++ b/ed_sensor_integration/CMakeLists.txt @@ -13,14 +13,19 @@ find_package(catkin REQUIRED COMPONENTS geolib2 geometry_msgs image_geometry + pcl_ros rgbd rgbd_image_buffer rosconsole_bridge roscpp sensor_msgs + tf2 + tf2_ros tue_config tue_filesystem visualization_msgs + image_transport + cv_bridge ) # ------------------------------------------------------------------------------------------------ @@ -30,7 +35,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES ed_kinect - CATKIN_DEPENDS code_profiler ed ${PROJECT_NAME}_msgs geolib2 image_geometry rgbd rgbd_image_buffer roscpp tue_config visualization_msgs + CATKIN_DEPENDS code_profiler ed ${PROJECT_NAME}_msgs geolib2 image_geometry rgbd rgbd_image_buffer roscpp tue_config visualization_msgs image_transport cv_bridge DEPENDS OpenCV PCL ) @@ -66,6 +71,8 @@ add_library(ed_kinect include/ed/kinect/fitter.h include/ed/kinect/mesh_tools.h include/ed/kinect/renderer.h + src/kinect/place_area_finder.cpp + include/ed/kinect/place_area_finder.h include/ed/kinect/segmenter.h include/ed/kinect/updater.h @@ -171,6 +178,13 @@ install( DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +add_executable(ed_empty_spot_designator tools/empty_spot_designator.cpp) +target_link_libraries(ed_empty_spot_designator ed_kinect ${catkin_LIBRARIES}) + +catkin_install_python(PROGRAMS yolo_python/yolo_table_segmentor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) # ------------------------------------------------------------------------------------------------ # TESTS # ------------------------------------------------------------------------------------------------ diff --git a/ed_sensor_integration/include/ed/kinect/place_area_finder.h b/ed_sensor_integration/include/ed/kinect/place_area_finder.h new file mode 100644 index 00000000..3ae369f7 --- /dev/null +++ b/ed_sensor_integration/include/ed/kinect/place_area_finder.h @@ -0,0 +1,141 @@ +#ifndef ED_KINECT_PLACE_AREA_FINDER_H_ +#define ED_KINECT_PLACE_AREA_FINDER_H_ + + +#include +#include + + +#include +#include +#include + +#include +#include +#include + +/** + * @brief Tool for finding placement options using rgbd sensor information. + * + */ +class PlaceAreaFinder +{ + +public: + + PlaceAreaFinder(); + + ~PlaceAreaFinder(); + + + /** + * @brief find a horizontal surface suitable to place an object + * + * @param image image in which to find a place position + * @param sensor_pose pose of the sensor with respect to a horizontal plane 'base_link' recommended + * @param[out] place_pose expressed in the same frame as sensor pose. One of the possible poses where an object may be placed, currently returns the pose furthest on the table + * @param mask input mask of table obtained from yolov8 segmentation of the rgb image + * @param donal bool used to run either max or donals version of the solution to speed up comparisons + * @return whether or not a suitable place was found + */ + bool findArea(const rgbd::ImageConstPtr& image, geo::Pose3D sensor_pose, geo::Pose3D& place_pose,const cv::Mat &mask,bool donal); + + /** + * @brief Get an image of the analysed space, used for introspection + * + * @param[out] image canvas to write the image to. + */ + void getCanvas(cv::Mat& image){ canvas.copyTo(image);} + + /** + * @brief Dilate the image, used for introspection + * + * @param[out] image canvas to write the image to. + */ + void getDilatedCanvas(cv::Mat& image){ dilated_canvas.copyTo(image);} + + /** + * @brief Shows the available placement points, used for introspection + * + * @param[out] image canvas to write the image to. + */ + void getPlacementCanvas(cv::Mat& image){ placement_canvas.copyTo(image);} + + /** + * @brief Shows the annotated image, used for introspection + * + * @param[out] image canvas to write the image to. + */ + void getAnnotatedImage(cv::Mat& image){ annotated_image.copyTo(image);} + + +private: + // internal occupancy representation + double resolution = 0.005; + cv::Point2d canvas_center; + cv::Mat canvas; + cv::Mat dilated_canvas; + cv::Mat placement_canvas; + + cv::Mat annotated_image; + + /** + * @brief transform a point in meters to a pixel on the canvas + * + * @param x in meters + * @param y in meters + * @return cv::Point2d + */ + cv::Point2d worldToCanvas(double x, double y); + + /** + * @brief inverse of WorldToCanvas + * + * @param point point on the canvas + * @return geo::Vec2d matching point in 2D space + */ + geo::Vec2d canvasToWorld(cv::Point2d point); + + //-------------------------------------------------------------------------------------------------------------------------------------- + cv::Point2d canvasToWorld2(double u, double v); + void CreateAndVisConvexHull(pcl::PointCloud::Ptr cloud,float height,pcl::PointCloud::Ptr& world_points); + void extractMaskPoints(pcl::PointCloud::Ptr inputCloud); + //-------------------------------------------------------------------------------------------------------------------------------------- + + /** + * @brief Draw the points in a pointcloud on the canvas. will ignore the z direction of the points + * + * @param cloud pointcloud to be rendered + * @param color color to render the points + */ + void createCostmap(pcl::PointCloud::Ptr cloud, cv::Scalar color); + + /** + * @brief morphological dilation + * + * @param canvas image to dilate + * @param dilated_canvas image after dilation + * @param placement_margin radius for dilation in meters! will be converted internally. #TODO remove + */ + void dilateCostmap(cv::Mat& canvas, cv::Mat& dilated_canvas, float placement_margin); + + /** + * @brief Draw a Circle on the canvas indicating the prefered distance to the robot. This distance is currently hardcoded. + * + * @param canvas canvas to draw on + * @param color color to draw with + * @param placement_margin + */ + void createRadiusCostmap(cv::Mat& canvas, cv::Scalar color, float placement_margin); + + /** + * @brief fill the selected pixels with a color for annotation + * + * @param indeces + * @param color + */ + void annotateImage(const rgbd::Image& image, const pcl::Indices index, cv::Scalar color); + +}; + +#endif diff --git a/ed_sensor_integration/include/ed_sensor_integration/sac_model_circle.h b/ed_sensor_integration/include/ed_sensor_integration/sac_model_circle.h new file mode 100644 index 00000000..c895fb47 --- /dev/null +++ b/ed_sensor_integration/include/ed_sensor_integration/sac_model_circle.h @@ -0,0 +1,295 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ +/** \brief @b SampleConsensusModelCircle defines a model for 2D circle from 3D samples (ignoring the third dimension). + * The model coefficients are defined as: + * - \b point_on_axis.x : the X coordinate of the circle's center point + * - \b point_on_axis.y : the Y coordinate of the circle's center point + * - \b radius : the circle's radius + * + * \author Thijs Beurskens + * \ingroup sample_consensus + */ +template +class SampleConsensusModelCircle : public SampleConsensusModel +{ +public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelCylinder. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCircle (const PointCloudConstPtr &cloud, bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelCircle"; + sample_size_ = 3; + model_size_ = 3; + } + + /** \brief Constructor for base SampleConsensusModelCylinder. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCircle (const PointCloudConstPtr &cloud, + const Indices &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelCircle"; + sample_size_ = 3; + model_size_ = 3; + } + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + SampleConsensusModelCircle (const SampleConsensusModelCircle &source) : + SampleConsensusModel () + { + *this = source; + model_name_ = "SampleConsensusModelCircle"; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelCircle () {} + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + inline SampleConsensusModelCircle& + operator = (const SampleConsensusModelCircle &source) + { + SampleConsensusModel::operator=(source); + return (*this); + } + + /** \brief Check whether the given index samples can form a valid cylinder model, compute the model coefficients + * from these samples and store them in model_coefficients. The cylinder coefficients are: point_on_axis, + * axis_direction, cylinder_radius_R + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const Indices &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given cylinder model. + * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + Indices &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the cylinder model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const Indices &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + + /** \brief Create a new point cloud with inliers projected onto the cylinder model. + * \param[in] inliers the data inliers that we want to project on the cylinder model + * \param[in] model_coefficients the coefficients of a cylinder model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const Indices &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients. + * \param[in] indices the data indices that need to be tested against the cylinder model + * \param[in] model_coefficients the cylinder model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_CYLINDER). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_CYLINDER); } // #TODO cannot add a unique identifier + +protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Get the distance from a point to a line (represented by a point and a direction) + * \param[in] pt a point + * \param[in] model_coefficients the line coefficients (a point on the line, line direction) + */ + double + pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const; + + /** \brief Project a point onto a line given by a point and a direction vector + * \param[in] pt the input point to project + * \param[in] line_pt the point on the line (make sure that line_pt[3] = 0 as there are no internal checks!) + * \param[in] line_dir the direction of the line (make sure that line_dir[3] = 0 as there are no internal checks!) + * \param[out] pt_proj the resultant projected point + */ + inline void + projectPointToLine (const Eigen::Vector4f &pt, + const Eigen::Vector4f &line_pt, + const Eigen::Vector4f &line_dir, + Eigen::Vector4f &pt_proj) const + { + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); + // Calculate the projection of the point on the line + pt_proj = line_pt + k * line_dir; + } + + /** \brief Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction, + * cylinder_radius_R) + * \param[in] pt the input point to project + * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R) + * \param[out] pt_proj the resultant projected point + */ + void + projectPointToRectangle (const Eigen::Vector4f &pt, + const Eigen::VectorXf &model_coefficients, + Eigen::Vector4f &pt_proj) const; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const Indices &samples) const override; + +private: + /** \brief Functor for the optimization function */ + struct OptimizationFunctor : pcl::Functor + { + /** Functor constructor + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelCircle *model, const Indices& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} + + /** Cost function to be minimized + * \param[in] x variables array + * \param[out] fvec resultant functions evaluations + * \return 0 + */ + int + operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const + { + Eigen::Vector4f line_pt (x[0], x[1], x[2], 0); + Eigen::Vector4f line_dir (x[3], x[4], x[5], 0); + + for (int i = 0; i < values (); ++i) + { + // dist = f - r + Eigen::Vector4f pt = (*model_->input_)[indices_[i]].getVector4fMap(); + pt[3] = 0; + + fvec[i] = static_cast (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]); + } + return (0); + } + + const pcl::SampleConsensusModelCircle *model_; + const Indices &indices_; + }; +}; +} + +//#ifdef PCL_NO_PRECOMPILE +#include "ed_sensor_integration/sac_model_circle.hpp" +//#endif diff --git a/ed_sensor_integration/include/ed_sensor_integration/sac_model_circle.hpp b/ed_sensor_integration/include/ed_sensor_integration/sac_model_circle.hpp new file mode 100644 index 00000000..8c395a25 --- /dev/null +++ b/ed_sensor_integration/include/ed_sensor_integration/sac_model_circle.hpp @@ -0,0 +1,357 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ + +#include // for LevenbergMarquardt +#include "ed_sensor_integration/sac_model_circle.h" +#include // for getAngle3D +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle::isSampleGood (const Indices &samples) const +{ + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle::computeModelCoefficients ( + const Indices &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 3 samples + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + if (std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits::epsilon () || + std::abs ((*input_)[samples[1]].x - (*input_)[samples[2]].x) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[1]].y - (*input_)[samples[2]].y) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[1]].z - (*input_)[samples[2]].z) <= std::numeric_limits::epsilon () || + std::abs ((*input_)[samples[0]].x - (*input_)[samples[2]].x) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].y - (*input_)[samples[2]].y) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].z - (*input_)[samples[2]].z) <= std::numeric_limits::epsilon ()) + { + return (false); + } + Eigen::Vector2f p1 ((*input_)[samples[0]].x, (*input_)[samples[0]].y); + Eigen::Vector2f p2 ((*input_)[samples[1]].x, (*input_)[samples[1]].y); + Eigen::Vector2f p3 ((*input_)[samples[2]].x, (*input_)[samples[2]].y); + + float x, y, l, r; + + float denominator = 2*(p1(0)*(p2(1)-p3(1)) - p1(1)*(p2(0)-p3(0)) + p2(0)*p3(1) - p3(0)*p2(1)); + x = (p1.dot(p1)*(p2(1)-p3(1)) + p2.dot(p2)*(p3(1)-p1(1)) + p3.dot(p3)*(p1(1)-p2(1))) / denominator; + y = (p1.dot(p1)*(p2(0)-p3(0)) + p2.dot(p2)*(p3(0)-p1(0)) + p3.dot(p3)*(p1(0)-p2(0))) / denominator; + + Eigen::Vector2f d; + d(0) = x-p1(0); + d(1) = y-p1(1); + r = sqrt(d.dot(d)); + + //save model coefficients + model_coefficients.resize (model_size_); + model_coefficients[0] = x; + model_coefficients[1] = y; + model_coefficients[2] = r; + + PCL_DEBUG ("[pcl::SampleConsensusModelCircle::computeModelCoefficients] Model is (%g,%g,%g).\n", + model_coefficients[0], model_coefficients[1], model_coefficients[2]); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + distances.resize (indices_->size ()); + + float x = model_coefficients[0], y = model_coefficients[1], r = model_coefficients[2]; + + Eigen::Vector2f p1, c1; + p1(0) = x; + p1(1) = y; + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector2f in ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y); + c1 = p1 - in; + distances[i] = std::abs(sqrt(c1.dot(c1))-r); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) +{ + std::vector distances; + getDistancesToModel (model_coefficients, distances); + for (int i = 0; i < distances.size(); i++) + { + if (distances[i] < threshold) {inliers.push_back ((*indices_)[i]);} + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelCircle::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + std::vector distances; + getDistancesToModel (model_coefficients, distances); + int count = 0; + for (int i = 0; i < distances.size(); i++) + { + if (distances[i] <= threshold) {count++;} + } + return (count); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle::optimizeModelCoefficients ( + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + optimized_coefficients = model_coefficients; + + // Needs a set of valid model coefficients + if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle::optimizeModelCoefficients] Given model is invalid!\n"); + return; + } + + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); + return; + } + + OptimizationFunctor functor (this, inliers); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, float> lm (num_diff); + int info = lm.minimize (optimized_coefficients); + + // Compute the L2 norm of the residuals + PCL_DEBUG ("[pcl::SampleConsensusModelCircle::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n", + info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], + model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]); + + Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]); + line_dir.normalize (); + optimized_coefficients[3] = line_dir[0]; + optimized_coefficients[4] = line_dir[1]; + optimized_coefficients[5] = line_dir[2]; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle::projectPoints ( + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle::projectPoints] Given model is invalid!\n"); + return; + } + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); + float ptdotdir = line_pt.dot (line_dir); + float dirdotdir = 1.0f / line_dir.dot (line_dir); + + // Copy all the data fields from the input cloud to the projected one? + if (copy_data_fields) + { + // Allocate enough space and copy the basics + projected_points.resize (input_->size ()); + projected_points.width = input_->width; + projected_points.height = input_->height; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor ((*input_)[i], projected_points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cylinder + for (const auto &inlier : inliers) + { + Eigen::Vector4f p ((*input_)[inlier].x, + (*input_)[inlier].y, + (*input_)[inlier].z, + 1); + + float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; + + pcl::Vector4fMap pp = projected_points[inlier].getVector4fMap (); + pp.matrix () = line_pt + k * line_dir; + + Eigen::Vector4f dir = p - pp; + dir[3] = 0.0f; + dir.normalize (); + + // Calculate the projection of the point onto the cylinder + pp += dir * model_coefficients[6]; + } + } + else + { + // Allocate enough space and copy the basics + projected_points.resize (inliers.size ()); + projected_points.width = inliers.size (); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor ((*input_)[inliers[i]], projected_points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cylinder + for (std::size_t i = 0; i < inliers.size (); ++i) + { + pcl::Vector4fMap pp = projected_points[i].getVector4fMap (); + pcl::Vector4fMapConst p = (*input_)[inliers[i]].getVector4fMap (); + + float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; + // Calculate the projection of the point on the line + pp.matrix () = line_pt + k * line_dir; + + Eigen::Vector4f dir = p - pp; + dir[3] = 0.0f; + dir.normalize (); + + // Calculate the projection of the point onto the cylinder + pp += dir * model_coefficients[6]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid model coefficients + if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle::doSamplesVerifyModel] Given model is invalid!\n"); + return (false); + } + + for (const auto &index : indices) + { + // Approximate the distance from the point to the cylinder as the difference between + // dist(point,cylinder_axis) and cylinder radius + // @note need to revise this. + Eigen::Vector4f pt ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z, 0.0f); + if (std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) + return (false); + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::SampleConsensusModelCircle::pointToLineDistance ( + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const +{ + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); + return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle::projectPointToRectangle ( + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const +{ + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); + + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir); + pt_proj = line_pt + k * line_dir; + + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the projection of the point onto the cylinder + pt_proj += dir * model_coefficients[6]; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// TODO do not overwrite we dont change it. +template bool +pcl::SampleConsensusModelCircle::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelCircle(PointT) template class PCL_EXPORTS pcl::SampleConsensusModelCircle; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ diff --git a/ed_sensor_integration/include/ed_sensor_integration/sac_model_double_line.h b/ed_sensor_integration/include/ed_sensor_integration/sac_model_double_line.h new file mode 100644 index 00000000..6a60559b --- /dev/null +++ b/ed_sensor_integration/include/ed_sensor_integration/sac_model_double_line.h @@ -0,0 +1,296 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ +/** \brief @b SampleConsensusModelDoubleLine defines a model for two parallel lines segmentation. + * The model coefficients are defined as: + * - \b point_on_axis.x : the X coordinate of a point located on one of the lines + * - \b point_on_axis.y : the Y coordinate of a point located on one of the lines + * - \b distance : distance between the two lines, signed + * - \b axis_direction.r : the angle between the lines and the x axis + * + * \author Thijs Beurskens + * \ingroup sample_consensus + */ +template +class SampleConsensusModelDoubleLine : public SampleConsensusModel +{ +public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelCylinder. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelDoubleLine (const PointCloudConstPtr &cloud, bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelDoubleLine"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelCylinder. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelDoubleLine (const PointCloudConstPtr &cloud, + const Indices &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelDoubleLine"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + SampleConsensusModelDoubleLine (const SampleConsensusModelDoubleLine &source) : + SampleConsensusModel () + { + *this = source; + model_name_ = "SampleConsensusModelDoubleLine"; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelDoubleLine () {} + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + inline SampleConsensusModelDoubleLine& + operator = (const SampleConsensusModelDoubleLine &source) + { + SampleConsensusModel::operator=(source); + return (*this); + } + + /** \brief Check whether the given index samples can form a valid cylinder model, compute the model coefficients + * from these samples and store them in model_coefficients. The cylinder coefficients are: point_on_axis, + * axis_direction, cylinder_radius_R + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const Indices &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given cylinder model. + * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + Indices &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the cylinder model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const Indices &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + + /** \brief Create a new point cloud with inliers projected onto the cylinder model. + * \param[in] inliers the data inliers that we want to project on the cylinder model + * \param[in] model_coefficients the coefficients of a cylinder model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const Indices &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients. + * \param[in] indices the data indices that need to be tested against the cylinder model + * \param[in] model_coefficients the cylinder model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_CYLINDER). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_CYLINDER); } // #TODO cannot add a unique identifier for double line + +protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Get the distance from a point to a line (represented by a point and a direction) + * \param[in] pt a point + * \param[in] model_coefficients the line coefficients (a point on the line, line direction) + */ + double + pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const; + + /** \brief Project a point onto a line given by a point and a direction vector + * \param[in] pt the input point to project + * \param[in] line_pt the point on the line (make sure that line_pt[3] = 0 as there are no internal checks!) + * \param[in] line_dir the direction of the line (make sure that line_dir[3] = 0 as there are no internal checks!) + * \param[out] pt_proj the resultant projected point + */ + inline void + projectPointToLine (const Eigen::Vector4f &pt, + const Eigen::Vector4f &line_pt, + const Eigen::Vector4f &line_dir, + Eigen::Vector4f &pt_proj) const + { + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); + // Calculate the projection of the point on the line + pt_proj = line_pt + k * line_dir; + } + + /** \brief Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction, + * cylinder_radius_R) + * \param[in] pt the input point to project + * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R) + * \param[out] pt_proj the resultant projected point + */ + void + projectPointToRectangle (const Eigen::Vector4f &pt, + const Eigen::VectorXf &model_coefficients, + Eigen::Vector4f &pt_proj) const; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const Indices &samples) const override; + +private: + /** \brief Functor for the optimization function */ + struct OptimizationFunctor : pcl::Functor + { + /** Functor constructor + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelDoubleLine *model, const Indices& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} + + /** Cost function to be minimized + * \param[in] x variables array + * \param[out] fvec resultant functions evaluations + * \return 0 + */ + int + operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const + { + Eigen::Vector4f line_pt (x[0], x[1], x[2], 0); + Eigen::Vector4f line_dir (x[3], x[4], x[5], 0); + + for (int i = 0; i < values (); ++i) + { + // dist = f - r + Eigen::Vector4f pt = (*model_->input_)[indices_[i]].getVector4fMap(); + pt[3] = 0; + + fvec[i] = static_cast (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]); + } + return (0); + } + + const pcl::SampleConsensusModelDoubleLine *model_; + const Indices &indices_; + }; +}; +} + +//#ifdef PCL_NO_PRECOMPILE +#include "ed_sensor_integration/sac_model_double_line.hpp" +//#endif diff --git a/ed_sensor_integration/include/ed_sensor_integration/sac_model_double_line.hpp b/ed_sensor_integration/include/ed_sensor_integration/sac_model_double_line.hpp new file mode 100644 index 00000000..e7b64bac --- /dev/null +++ b/ed_sensor_integration/include/ed_sensor_integration/sac_model_double_line.hpp @@ -0,0 +1,385 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_DOUBLE_LINE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_DOUBLE_LINE_H_ + +#include // for LevenbergMarquardt +#include "ed_sensor_integration/sac_model_double_line.h" +#include // for getAngle3D +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelDoubleLine::isSampleGood (const Indices &samples) const +{ + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelDoubleLine::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelDoubleLine::computeModelCoefficients ( + const Indices &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 3 samples + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelDoubleLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + if (std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits::epsilon () || + std::abs ((*input_)[samples[1]].x - (*input_)[samples[2]].x) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[1]].y - (*input_)[samples[2]].y) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[1]].z - (*input_)[samples[2]].z) <= std::numeric_limits::epsilon () || + std::abs ((*input_)[samples[0]].x - (*input_)[samples[2]].x) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].y - (*input_)[samples[2]].y) <= std::numeric_limits::epsilon () && + std::abs ((*input_)[samples[0]].z - (*input_)[samples[2]].z) <= std::numeric_limits::epsilon ()) + { + return (false); + } + Eigen::Vector2f p1 ((*input_)[samples[0]].x, (*input_)[samples[0]].y); + Eigen::Vector2f p2 ((*input_)[samples[1]].x, (*input_)[samples[1]].y); + Eigen::Vector2f p3 ((*input_)[samples[2]].x, (*input_)[samples[2]].y); + + float x, y, l, w, r; + + x = p1(0), y = p1(1); //output: x and y values + + Eigen::Vector2f c1, c2; + + c1 = p2 - p1; //vector between point 1 and 2 + l = sqrt(c1.dot(c1)); //define length as distance between point 1 and 2 + // determine rotation: + r = std::atan2(c1(1), c1(0)); //output: orientation of the double_line + + c2 = p1 - p3; //vector between point 3 and 1 + w = (c1(0)*c2(1)-c2(0)*c1(1))/l; //output: width defined as distance between c1 and point 3, signed + + //save model coefficients + model_coefficients.resize (model_size_); + model_coefficients[0] = x; + model_coefficients[1] = y; + model_coefficients[2] = w; + model_coefficients[3] = r; + + //std::cout << w << std::endl; + + PCL_DEBUG ("[pcl::SampleConsensusModelDoubleLine::computeModelCoefficients] Model is (%g,%g,%g,%g,%g).\n", + model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]); + //std::cout << "P1: " << std::endl << p1 << std::endl << "P2: " << std::endl << p2 << std::endl << "P3: " << std::endl << p3 << std::endl << model_coefficients << std::endl; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelDoubleLine::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + distances.resize (indices_->size ()); + + float x = model_coefficients[0]; + float y = model_coefficients[1]; + float w = model_coefficients[2]; + float r = model_coefficients[3]; + float d1, d2; + + Eigen::Vector2f p1, p2, p3, p4, c1, c2; + + // #TODO Calculation of distances without needing to define points + //define edge vectors + c1(0)=std::cos(r); + c1(1)=std::sin(r); + + c2(0)= std::sin(r)*w; + c2(1)=-std::cos(r)*w; + + //define corner vectors + p1(0)=x; + p1(1)=y; + p2 = p1 + c1; + p3 = p2 + c2; + p4 = p1 + c2; + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector2f in ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y); + + //compute distances to lines + d1 = std::abs(c1(0)*(p1(1)-in(1))-(p1(0)-in(0))*c1(1)); //Distance between line 1 and the point + d2 = std::abs(c1(0)*(p3(1)-in(1))-(p3(0)-in(0))*c1(1)); //Distance between line 2 and the point + + distances[i] = std::min(d1, d2); //smallest of d1 and d2 + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelDoubleLine::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) +{ + std::vector distances; + getDistancesToModel (model_coefficients, distances); + for (int i = 0; i < distances.size(); i++) + { + if (distances[i] < threshold) {inliers.push_back ((*indices_)[i]);} + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelDoubleLine::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + std::vector distances; + getDistancesToModel (model_coefficients, distances); + int count = 0; + for (int i = 0; i < distances.size(); i++) + { + if (distances[i] <= threshold) {count++;} + } + return (count); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelDoubleLine::optimizeModelCoefficients ( + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + optimized_coefficients = model_coefficients; + + // Needs a set of valid model coefficients + if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelDoubleLine::optimizeModelCoefficients] Given model is invalid!\n"); + return; + } + + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelDoubleLine:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); + return; + } + + OptimizationFunctor functor (this, inliers); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, float> lm (num_diff); + int info = lm.minimize (optimized_coefficients); + + // Compute the L2 norm of the residuals + PCL_DEBUG ("[pcl::SampleConsensusModelDoubleLine::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n", + info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], + model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]); + + Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]); + line_dir.normalize (); + optimized_coefficients[3] = line_dir[0]; + optimized_coefficients[4] = line_dir[1]; + optimized_coefficients[5] = line_dir[2]; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelDoubleLine::projectPoints ( + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelDoubleLine::projectPoints] Given model is invalid!\n"); + return; + } + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); + float ptdotdir = line_pt.dot (line_dir); + float dirdotdir = 1.0f / line_dir.dot (line_dir); + + // Copy all the data fields from the input cloud to the projected one? + if (copy_data_fields) + { + // Allocate enough space and copy the basics + projected_points.resize (input_->size ()); + projected_points.width = input_->width; + projected_points.height = input_->height; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor ((*input_)[i], projected_points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cylinder + for (const auto &inlier : inliers) + { + Eigen::Vector4f p ((*input_)[inlier].x, + (*input_)[inlier].y, + (*input_)[inlier].z, + 1); + + float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; + + pcl::Vector4fMap pp = projected_points[inlier].getVector4fMap (); + pp.matrix () = line_pt + k * line_dir; + + Eigen::Vector4f dir = p - pp; + dir[3] = 0.0f; + dir.normalize (); + + // Calculate the projection of the point onto the cylinder + pp += dir * model_coefficients[6]; + } + } + else + { + // Allocate enough space and copy the basics + projected_points.resize (inliers.size ()); + projected_points.width = inliers.size (); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor ((*input_)[inliers[i]], projected_points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cylinder + for (std::size_t i = 0; i < inliers.size (); ++i) + { + pcl::Vector4fMap pp = projected_points[i].getVector4fMap (); + pcl::Vector4fMapConst p = (*input_)[inliers[i]].getVector4fMap (); + + float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; + // Calculate the projection of the point on the line + pp.matrix () = line_pt + k * line_dir; + + Eigen::Vector4f dir = p - pp; + dir[3] = 0.0f; + dir.normalize (); + + // Calculate the projection of the point onto the cylinder + pp += dir * model_coefficients[6]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelDoubleLine::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid model coefficients + if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelDoubleLine::doSamplesVerifyModel] Given model is invalid!\n"); + return (false); + } + + for (const auto &index : indices) + { + // Approximate the distance from the point to the cylinder as the difference between + // dist(point,cylinder_axis) and cylinder radius + // @note need to revise this. + Eigen::Vector4f pt ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z, 0.0f); + if (std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::SampleConsensusModelDoubleLine::pointToLineDistance ( + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const +{ + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); + return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelDoubleLine::projectPointToRectangle ( + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const +{ + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); + + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir); + pt_proj = line_pt + k * line_dir; + + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the projection of the point onto the cylinder + pt_proj += dir * model_coefficients[6]; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelDoubleLine::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelDoubleLine(PointT) template class PCL_EXPORTS pcl::SampleConsensusModelDoubleLine; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_DOUBLE_LINE_H_ diff --git a/ed_sensor_integration/include/ed_sensor_integration/sac_model_horizontal_plane.h b/ed_sensor_integration/include/ed_sensor_integration/sac_model_horizontal_plane.h new file mode 100644 index 00000000..b59e34df --- /dev/null +++ b/ed_sensor_integration/include/ed_sensor_integration/sac_model_horizontal_plane.h @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ +/** \brief @b SampleConsensusModelParallelPlane defines a model for 3D plane segmentation using additional + * angular constraints. The plane must be parallel to a user-specified axis + * (\ref setAxis) within a user-specified angle threshold (\ref setEpsAngle). + * In other words, the plane normal must be (nearly) perpendicular to the specified axis. + * + * Code example for a plane model, parallel (within a 15 degrees tolerance) with the Z axis: + * \code + * SampleConsensusModelParallelPlane model (cloud); + * model.setAxis (Eigen::Vector3f (0.0, 0.0, 1.0)); + * model.setEpsAngle (pcl::deg2rad (15)); + * \endcode + * + * \note Please remember that you need to specify an angle > 0 in order to activate the axis-angle constraint! + * + * \author Radu B. Rusu, Nico Blodow + * \ingroup sample_consensus + */ +template +class SampleConsensusModelHorizontalPlane : public SampleConsensusModelPlane +{ +public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::input_; + + using PointCloud = typename SampleConsensusModelPlane::PointCloud; + using PointCloudPtr = typename SampleConsensusModelPlane::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModelPlane::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelHorizontalPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelHorizontalPlane (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModelPlane (cloud, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + , sin_angle_ (-1.0) + { + model_name_ = "SampleConsensusModelHorizontalPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelHorizontalPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelHorizontalPlane (const PointCloudConstPtr &cloud, + const Indices &indices, + bool random = false) + : SampleConsensusModelPlane (cloud, indices, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + , sin_angle_ (-1.0) + { + model_name_ = "SampleConsensusModelHorizontalPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelHorizontalPlane () {} + + /** \brief Set the axis along which we need to search for a plane perpendicular to. + * \param[in] ax the axis along which we need to search for a plane perpendicular to + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } + + /** \brief Get the axis along which we need to search for a plane perpendicular to. */ + inline Eigen::Vector3f + getAxis () const { return (axis_); } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed difference between the plane normal and the given axis. + * \note You need to specify an angle > 0 in order to activate the axis-angle constraint! + */ + inline void + setEpsAngle (const double ea) { eps_angle_ = ea; sin_angle_ = std::abs (sin (ea));} + + /** \brief Get the angle epsilon (delta) threshold. */ + inline double + getEpsAngle () const { return (eps_angle_); } + + bool + computeModelCoefficients (const Indices &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + Indices &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Compute all distances from the cloud data to a given plane model. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Return a unique id for this model (SACMODEL_HORIZONTAL_PLANE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_PARALLEL_PLANE); }//cannot add unique identifier + +protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief The axis along which we need to search for a plane perpendicular to. */ + Eigen::Vector3f axis_; + + /** \brief The maximum allowed difference between the plane and the given axis. */ + double eps_angle_; + + /** \brief The sine of the angle*/ + double sin_angle_; +}; +} + +#include "ed_sensor_integration/sac_model_horizontal_plane.hpp" diff --git a/ed_sensor_integration/include/ed_sensor_integration/sac_model_horizontal_plane.hpp b/ed_sensor_integration/include/ed_sensor_integration/sac_model_horizontal_plane.hpp new file mode 100644 index 00000000..989e5bb2 --- /dev/null +++ b/ed_sensor_integration/include/ed_sensor_integration/sac_model_horizontal_plane.hpp @@ -0,0 +1,135 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + a + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_HORIZONTAL_PLANE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_HORIZONTAL_PLANE_H_ + +#include "ed_sensor_integration/sac_model_horizontal_plane.h" + +////////////////////////////////////////////////////////////////////////// + +template bool +pcl::SampleConsensusModelHorizontalPlane::computeModelCoefficients ( + const Indices &samples, Eigen::VectorXf &model_coefficients) const +{ + model_coefficients.resize (model_size_); + model_coefficients[0] = 0; + model_coefficients[1] = 0; + model_coefficients[2] = -1; + model_coefficients[3] = (*input_)[samples[0]].z; + //std::cout < void +pcl::SampleConsensusModelHorizontalPlane::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + SampleConsensusModelPlane::selectWithinDistance (model_coefficients, threshold, inliers); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelHorizontalPlane::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + return (0); + } + //std::cout << model_coefficients << std::endl; + //std::cout << SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold) << std::endl; + return (SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold)); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelHorizontalPlane::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + SampleConsensusModelPlane::getDistancesToModel (model_coefficients, distances); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelHorizontalPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + { + return (false); + } + /* + // Check against template, if given + if (eps_angle_ > 0.0) + { + // Obtain the plane normal + Eigen::Vector4f coeff = model_coefficients; + coeff[3] = 0.0f; + coeff.normalize (); + + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f); + if (std::abs (axis.dot (coeff)) > sin_angle_) + { + PCL_DEBUG ("[pcl::SampleConsensusModelHorizontalPlane::isModelValid] Angle between plane normal and given axis is too large.\n"); + return (false); + } + } + */ + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelHorizontalPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelHorizontalPlane; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_HORIZONTAL_PLANE_H_ diff --git a/ed_sensor_integration/package.xml b/ed_sensor_integration/package.xml index 7cddbf8c..ffd8c420 100644 --- a/ed_sensor_integration/package.xml +++ b/ed_sensor_integration/package.xml @@ -18,6 +18,8 @@ ed_sensor_integration_msgs geolib2 image_geometry + pcl_ros + tf2_ros libopencv-dev libpcl-common libpcl-kdtree @@ -26,6 +28,9 @@ roscpp tue_config visualization_msgs + cv_bridge + image_transport + geometry_msgs geometry_msgs diff --git a/ed_sensor_integration/scripts/yolov8n-seg.pt b/ed_sensor_integration/scripts/yolov8n-seg.pt new file mode 100644 index 00000000..6d8d8b57 Binary files /dev/null and b/ed_sensor_integration/scripts/yolov8n-seg.pt differ diff --git a/ed_sensor_integration/src/kinect/kinect_plugin.cpp b/ed_sensor_integration/src/kinect/kinect_plugin.cpp index 5188ac26..a6579c14 100644 --- a/ed_sensor_integration/src/kinect/kinect_plugin.cpp +++ b/ed_sensor_integration/src/kinect/kinect_plugin.cpp @@ -54,6 +54,7 @@ void KinectPlugin::initialize(ed::InitData& init) { ROS_INFO_STREAM("[ED KINECT PLUGIN] Initializing kinect client with topic '" << topic << "'."); image_buffer_.initialize(topic, "map"); + image_buffer_bl_.initialize(topic, "base_link"); } // - - - - - - - - - - - - - - - - - - @@ -65,8 +66,11 @@ void KinectPlugin::initialize(ed::InitData& init) srv_get_image_ = nh.advertiseService("kinect/get_image", &KinectPlugin::srvGetImage, this); srv_update_ = nh.advertiseService("kinect/update", &KinectPlugin::srvUpdate, this); srv_ray_trace_ = nh.advertiseService("ray_trace", &KinectPlugin::srvRayTrace, this); + srv_place_area_ = nh.advertiseService("place_area", &KinectPlugin::srvPlaceArea, this); ray_trace_visualization_publisher_ = nh.advertise("ray_trace_visualization", 10); + place_area_publisher_ = nh.advertise("place_area", 10); + place_marker_publisher_ = nh.advertise("place_pose", 10); } // ---------------------------------------------------------------------------------------------------- @@ -281,6 +285,61 @@ bool KinectPlugin::srvRayTrace(ed_sensor_integration_msgs::RayTrace::Request& re return true; } +bool KinectPlugin::srvPlaceArea(__attribute__((unused)) ed_sensor_integration_msgs::PlaceArea::Request& req, ed_sensor_integration_msgs::PlaceArea::Response& res) +{ + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Get new image + cv::Mat mask; + bool donal = true; + rgbd::ImageConstPtr image; + geo::Pose3D sensor_pose; // in base link frame + + if (!image_buffer_bl_.waitForRecentImage(image, sensor_pose, 2.0)) + { + res.error_msg = "Could not get image"; + return true; + } + + // Determine place area + geo::Pose3D place_pose; // w.r.t base link + if (!place_area_finder_.findArea(image, sensor_pose, place_pose,mask,donal)) + { + res.error_msg = "No valid place area found"; + return true; + } + geo::convert(place_pose, res.pose.pose); + + // Publish marker + visualization_msgs::Marker marker_msg; + marker_msg.header.frame_id = "base_link"; + marker_msg.header.stamp = ros::Time::now(); + marker_msg.action = visualization_msgs::Marker::ADD; + marker_msg.color.a = 0.5; + marker_msg.lifetime = ros::Duration(10.0); + double r = 0.05; + marker_msg.scale.x = r; + marker_msg.scale.y = r; + marker_msg.scale.z = r; + + static int iter = 0; + if (++iter % 2 == 0) + { + marker_msg.color.b = marker_msg.color.r = 1; + } + else + { + marker_msg.color.b = marker_msg.color.g = 1; + } + + marker_msg.type = visualization_msgs::Marker::SPHERE; + + marker_msg.pose = res.pose.pose; + + place_marker_publisher_.publish(marker_msg); + + return true; +} + // ---------------------------------------------------------------------------------------------------- ED_REGISTER_PLUGIN(KinectPlugin) diff --git a/ed_sensor_integration/src/kinect/kinect_plugin.h b/ed_sensor_integration/src/kinect/kinect_plugin.h index 5f35fc12..4de26947 100644 --- a/ed_sensor_integration/src/kinect/kinect_plugin.h +++ b/ed_sensor_integration/src/kinect/kinect_plugin.h @@ -5,6 +5,7 @@ #include #include "ed/kinect/updater.h" +#include "ed/kinect/place_area_finder.h" #include @@ -13,6 +14,7 @@ #include #include #include +#include // Visualization #include @@ -38,6 +40,7 @@ class KinectPlugin : public ed::Plugin // Image retrieval rgbd::ImageBuffer image_buffer_; + rgbd::ImageBuffer image_buffer_bl_; // rgbd::ImageConstPtr last_image_; @@ -45,7 +48,7 @@ class KinectPlugin : public ed::Plugin Updater updater_; - + PlaceAreaFinder place_area_finder_; // Communication @@ -73,7 +76,11 @@ class KinectPlugin : public ed::Plugin ros::Publisher ray_trace_visualization_publisher_; + bool srvPlaceArea(ed_sensor_integration_msgs::PlaceArea::Request& req, ed_sensor_integration_msgs::PlaceArea::Response& res); + ros::ServiceServer srv_place_area_; + ros::Publisher place_area_publisher_; + ros::Publisher place_marker_publisher_; }; diff --git a/ed_sensor_integration/src/kinect/place_area_finder.cpp b/ed_sensor_integration/src/kinect/place_area_finder.cpp new file mode 100644 index 00000000..115d312a --- /dev/null +++ b/ed_sensor_integration/src/kinect/place_area_finder.cpp @@ -0,0 +1,695 @@ +#include "ed/kinect/place_area_finder.h" + +#include +#include + +#include + +#include + +// donal +#include +#include + +// +// pcl library # TODO remove the unused ones #TODO find out which ones are unused +#include +#include +#include + + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "ed_sensor_integration/sac_model_horizontal_plane.h" + +/** + * @brief transform an rgbd image to a pointcloud + * + * @param image rgbd image + * @param[out] cloud pcl pointcloud, points are expressed in the frame of the camera according to pcl conventions + * @return double to be removed + */ +double imageToCloud(const rgbd::Image &image, pcl::PointCloud::Ptr cloud) +{ + // Fill in the cloud data + cloud->width = image.getDepthImage().cols; + cloud->height = image.getDepthImage().rows; + cloud->is_dense = false; + cloud->resize(cloud->width * cloud->height); + + double fx = image.getCameraModel().fx(); + double fy = image.getCameraModel().fy(); + + double half_height = 0.5 * cloud->height; + double half_width = 0.5 * cloud->width; + + for (uint i = 0; i < cloud->height; ++i) + { + for (uint j = 0; j < cloud->width; ++j) + { + cv::Vec3b bgr = image.getRGBImage().at(i, j); + double d = image.getDepthImage().at(i, j); + + cloud->at(j, i).x = (-half_width + j) * d / fx; + cloud->at(j, i).y = (-half_height + i) * d / fy; + cloud->at(j, i).z = d; + cloud->at(j, i).r = bgr[2]; + cloud->at(j, i).g = bgr[1]; + cloud->at(j, i).b = bgr[0]; + + } + } + + return image.getCameraModel().fx(); +} + +double imageToMaskFilteredCloud(const rgbd::Image &image, pcl::PointCloud::Ptr cloud) +{ + // Fill in the cloud data + cloud->width = image.getDepthImage().cols; + cloud->height = image.getDepthImage().rows; + cloud->is_dense = false; + cloud->resize(cloud->width * cloud->height); + + double fx = image.getCameraModel().fx(); + double fy = image.getCameraModel().fy(); + + double half_height = 0.5 * cloud->height; + double half_width = 0.5 * cloud->width; + + for (uint i = 0; i < cloud->height; ++i) + { + for (uint j = 0; j < cloud->width; ++j) + { + cv::Vec3b bgr = image.getRGBImage().at(i, j); + if (bgr[0]==255 && bgr[2]==225) + { + double d = image.getDepthImage().at(i, j); + + cloud->at(j, i).x = (-half_width + j) * d / fx; + cloud->at(j, i).y = (-half_height + i) * d / fy; + cloud->at(j, i).z = d; + cloud->at(j, i).r = bgr[2]; + cloud->at(j, i).g = bgr[1]; + cloud->at(j, i).b = bgr[0]; + } + } + } + + return image.getCameraModel().fx(); +} + +/** + * @brief transform a geolib pose into a transformation matrix from the Eigen library + * + * @param pose geolib pose + * @return Eigen::Matrix4f + */ +Eigen::Matrix4f geolibToEigen(geo::Pose3D pose) +{ + // convert from geolib coordinates to ros coordinates #TODO remove geolib coordinates for camera pose + pose.R = pose.R * geo::Mat3(1, 0, 0, 0, -1, 0, 0, 0, -1); + + float x = pose.t.x; + float y = pose.t.y; + float z = pose.t.z; + float xx = pose.R.xx; + float xy = pose.R.xy; + float xz = pose.R.xz; + float yx = pose.R.yx; + float yy = pose.R.yy; + float yz = pose.R.yz; + float zx = pose.R.zx; + float zy = pose.R.zy; + float zz = pose.R.zz; + + Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity(); + + Transform(0, 0) = xx; + Transform(0, 1) = xy; + Transform(0, 2) = xz; + Transform(0, 3) = x; + Transform(1, 0) = yx; + Transform(1, 1) = yy; + Transform(1, 2) = yz; + Transform(1, 3) = y; + Transform(2, 0) = zx; + Transform(2, 1) = zy; + Transform(2, 2) = zz; + Transform(2, 3) = z; + + // std::cout << Transform << std::endl; + return Transform; +} + +/** + * @brief segment the larges horizontal plane in the pointcloud. Horizontal is assumed to be normal to the z-axis. The plane is also assumed to be above z=0.0 + * @param cloud_in pointcloud to be segmented + * @param plane_coefficients the 4 coefficients that define a parallel plane + * @param cloud_out A pointcloud containing all points within the found plane + * @param cloud_no_plane A pointcloud containing all points not within the found plane + * @return height of the segmented plane OR -1.0 if no plane could be found + */ +bool SegmentPlane(const pcl::PointCloud::Ptr cloud_in, pcl::ModelCoefficients::Ptr plane_coefficients, + pcl::PointCloud::Ptr cloud_out, pcl::Indices &plane_index, + pcl::PointCloud::Ptr cloud_no_plane, pcl::Indices &planeless_index) +{ + pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); + // Create the segmentation object + pcl::SACSegmentation seg; + // Optional + seg.setOptimizeCoefficients(true); + // Mandatory + seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(1000); + seg.setDistanceThreshold(0.01); + seg.setAxis(Eigen::Vector3f(1, 0, 0)); + seg.setEpsAngle(5 * 0.0174532925); //*0.0174532925 to radians + + // Create the filtering object + pcl::ExtractIndices extract; + pcl::ExtractIndices extract2; + + // Segment the largest planar component from the remaining cloud + seg.setInputCloud(cloud_in); + seg.segment(*inliers, *plane_coefficients); + + if (inliers->indices.size() == 0) + { + std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; + return false; + } + + // Extract the inliers to a cloud with just the plane + extract.setInputCloud(cloud_in); + extract.setIndices(inliers); + extract.setNegative(false); + extract.filter(*cloud_out); + + plane_index = inliers->indices; + + // std::cout << "PointCloud representing the planar component: " << inliers->indices.size() << " data points." + // << "Plane with coefficients: " << *coefficients << std::endl; + + // Extract outliers to the main cloud without the table plane + extract2.setInputCloud(cloud_in); + extract2.setIndices(inliers); + extract2.setNegative(true); + extract2.filter(*cloud_no_plane); + + // handcraft the planeless index + planeless_index.resize(cloud_in->size() - plane_index.size()); + int plane_i = 0; // most recent index of plane_index + int planeless_i = 0; + for (int i = 0; i < int(cloud_in->size()); i++) + { + if (i == plane_index[plane_i]) + { + plane_i++; + } + else + { + planeless_index[planeless_i] = i; + planeless_i++; + } + } + + return true; +} + +/** + * @brief extract all pixels of a certain color from a canvas + * + * @param canvas original image + * @param[out] placement_canvas image to draw the extracted pixels on must be the same size as canvas + * @param targetColor color to be extracted + */ +void ExtractPlacementOptions(cv::Mat &canvas, cv::Mat &placement_canvas, cv::Scalar targetColor) +{ + cv::Point2d canvas_center(canvas.rows / 2, canvas.cols); + + std::vector identicalPoints; + + for (int row = canvas.rows; row > 0; --row) + { + for (int col = canvas.cols; col > 0; --col) + { + cv::Vec3b currPixel = canvas.at(row, col); + if (currPixel.val[0] == targetColor.val[0] && + currPixel.val[1] == targetColor.val[1] && + currPixel.val[2] == targetColor.val[2]) + { + cv::Point2d p = cv::Point(col, row); + if (p.x >= 0 && p.y >= 0 && p.x < placement_canvas.cols && p.y < placement_canvas.rows) + placement_canvas.at(p) = cv::Vec3b(targetColor[0], targetColor[1], targetColor[2]); + } + } + } +} + +/** + * @brief Get the coordinates of a point in an image which matches the target color + * + * @param canvas image to check + * @param targetColor color to be found + * @param point coordinates of one point which has the targetColor + * @return whether or not a pixel was found + */ +bool GetPlacementOption(cv::Mat &canvas, cv::Scalar targetColor, cv::Point2d &point) +{ + for (int row = 0; row < canvas.rows; ++row) + { + for (int col = canvas.cols; col > 0; --col) + { + cv::Vec3b currPixel = canvas.at(row, col); + if (currPixel.val[0] == targetColor.val[0] && + currPixel.val[1] == targetColor.val[1] && + currPixel.val[2] == targetColor.val[2]) + { + point = cv::Point(col, row); + return true; + } + } + } + return false; +} + +/** + * @brief remove the floor from a pointcloud cloud_in and return the cloud_out with index. It is assumed the floor is aligned with the plane z=0 + * + * @param cloud_in + * @param cloud_out + * @param index + */ +void removeFloor(const pcl::PointCloud::Ptr cloud_in, pcl::PointCloud::Ptr cloud_out, pcl::Indices &index) +{ + float buffer = 0.1; + pcl::ConditionAnd::Ptr range_cond(new pcl::ConditionAnd()); + range_cond->addComparison(pcl::FieldComparison::ConstPtr(new pcl::FieldComparison("z", pcl::ComparisonOps::GT, buffer))); + // build the filter + pcl::ConditionalRemoval condrem; + condrem.setCondition(range_cond); + condrem.setInputCloud(cloud_in); + condrem.setKeepOrganized(true); + // apply filter + condrem.filter(*cloud_out); + (*cloud_out).is_dense = false; + pcl::removeNaNFromPointCloud(*cloud_out, *cloud_out, index); +} + +void filterHeight(const pcl::PointCloud::Ptr cloud_in, float height_min, float height_max, pcl::PointCloud::Ptr cloud_out, pcl::Indices &index) +{ + // Filter out objects above the table plane and put them in seperate cloud + pcl::ConditionAnd::Ptr range_cond(new pcl::ConditionAnd()); + range_cond->addComparison(pcl::FieldComparison::ConstPtr(new pcl::FieldComparison("z", pcl::ComparisonOps::GT, height_min))); + range_cond->addComparison(pcl::FieldComparison::ConstPtr(new pcl::FieldComparison("z", pcl::ComparisonOps::LT, height_max))); + // build the filter + pcl::ConditionalRemoval condrem2; + condrem2.setCondition(range_cond); + condrem2.setInputCloud(cloud_in); + condrem2.setKeepOrganized(true); + // apply filter + condrem2.filter(*cloud_out); + pcl::removeNaNFromPointCloud(*cloud_out, *cloud_out, index); +} + +/** + * @brief Project a plane onto a a plane along the beams of the camera + * + * @param cloud_in cloud with points to be projected + * @param transform transform between the camera and the origin of the cloud in + * @param plane_height height of the plane to be projected onto. + * @param cloud_out cloud containing points with z=height + */ +void projectToPlane(const pcl::PointCloud::Ptr cloud_in, Eigen::Matrix4f transform, float plane_height, pcl::PointCloud::Ptr cloud_out) +{ + cloud_out->width = cloud_in->width; + cloud_out->height = cloud_in->height; + cloud_out->is_dense = cloud_in->is_dense; + cloud_out->points.resize(cloud_out->width * cloud_out->height); + float x = transform(0, 3); + float y = transform(1, 3); + float z = transform(2, 3); + for (uint nIndex = 0; nIndex < cloud_in->points.size(); nIndex++) + { + float lower = cloud_in->points[nIndex].z - z; // height between camera and point. + float upper = plane_height - cloud_in->points[nIndex].z; // height between the point and the table + float lambda = upper / lower; // ratio between camera-point distance and point-projection distance + float dx = cloud_in->points[nIndex].x - x; // difference between point x and camera x + float dy = cloud_in->points[nIndex].y - y; // difference between point y and camera y + + cloud_out->points[nIndex].z = plane_height; + cloud_out->points[nIndex].x = cloud_in->points[nIndex].x + lambda * dx; + cloud_out->points[nIndex].y = cloud_in->points[nIndex].y + lambda * dy; + } +} + +/** + * @brief returns indexes of cloud C to cloud A given the index of cloud B to A and cloud C to B) + * + * @param index1 + * @param index2 + * @return pcl::Indices + */ +pcl::Indices multiplyIndex(pcl::Indices indexBA, pcl::Indices indexCB) +{ + for (uint i = 0; i < indexCB.size(); i++) + { + indexCB[i] = indexBA[indexCB[i]]; + } + return indexCB; +} + + +PlaceAreaFinder::PlaceAreaFinder() +{ +} + +PlaceAreaFinder::~PlaceAreaFinder() +{ +} + +bool PlaceAreaFinder::findArea(const rgbd::ImageConstPtr &image, geo::Pose3D sensor_pose, geo::Pose3D &place_pose,const cv::Mat &mask,bool donal) +{ + bool visualize = true; + // std::cout << "converting image to cloud" << std::endl; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + imageToCloud(*image, cloud); + + // convert image to cloud and filter using cnn segmentation mask + // if(donal){ + // imageToMaskFilteredCloud(*image,cloud); + // } + + // transform to base link frame + Eigen::Matrix4f transform = geolibToEigen(sensor_pose); + pcl::transformPointCloud(*cloud, *cloud, transform); + + + + // keep track of the indices in the original image + std::vector indices; + pcl::Indices floorless_index; + // Filter out floor + pcl::PointCloud::Ptr floorless_cloud(new pcl::PointCloud); + removeFloor(cloud, floorless_cloud, floorless_index); + + // Segment the table plane and return a cloud with the plane and a cloud where the plane is removed + std::cout << "SegmentPlane" << std::endl; + pcl::PointCloud::Ptr plane_cloud(new pcl::PointCloud); + pcl::Indices plane_index; + pcl::PointCloud::Ptr planeless_cloud(new pcl::PointCloud); + pcl::Indices planeless_index; + pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients()); + if (!SegmentPlane(floorless_cloud, plane_coefficients, plane_cloud, plane_index, planeless_cloud, planeless_index)) + { + std::cout << "Could not find plane" << std::endl; + return false; + } + float height = abs(plane_coefficients->values[3]); // take the absolute value in case the plane is fitted upside down + std::cout << "Found plane height " << height << std::endl; + + // Filter out objects above the table plane and put them in seperate cloud + pcl::PointCloud::Ptr object_cloud(new pcl::PointCloud); + pcl::Indices object_index; + filterHeight(planeless_cloud, height, height + 0.30, object_cloud, object_index); + + // Create pointcloud with occluded space based on the object cloud + pcl::PointCloud::Ptr occluded_cloud(new pcl::PointCloud); + projectToPlane(object_cloud, transform, height, occluded_cloud); + + // Filter out objects below the table plane and put them in seperate cloud + pcl::PointCloud::Ptr below_table_cloud(new pcl::PointCloud); + pcl::Indices below_index; + filterHeight(planeless_cloud, 0.0, height - 0.02, below_table_cloud, below_index); + + // Create pointcloud with unoccluded space based on the bewow table cloud + pcl::PointCloud::Ptr not_table_cloud(new pcl::PointCloud); + projectToPlane(below_table_cloud, transform, height, not_table_cloud); + + // std::cout << "creating costmap" << std::endl; + canvas = cv::Mat(500, 500, CV_8UC3, cv::Scalar(0, 0, 0)); + dilated_canvas = cv::Mat(500, 500, CV_8UC3, cv::Scalar(0, 0, 0)); + placement_canvas = cv::Mat(500, 500, CV_8UC3, cv::Scalar(0, 0, 0)); + cv::Scalar table_color(0, 255, 0); + cv::Scalar occupied_color(0, 0, 255); + cv::Scalar occluded_color(255, 0, 0); + cv::Scalar radius_color(100, 0, 100); + cv::Scalar placement_color(255, 255, 255); + cv::Scalar point_color(255, 0, 0); + + // Object placement margins #TODO hardcoded parameters + float object_diameter = 0.10;// removeOutliers(plane_cloud,filtered_plane_cloud); + float error_margin = 0.02; + float length = object_diameter + error_margin; + float placement_margin = 2 * 0.02 + length; + + if ((visualize && !mask.empty())| !donal) + { + std::cout << "annotating image" << std::endl; + annotated_image = image->getRGBImage().clone(); + // pcl::Indices plane_cloud_index = multiplyIndex(floorless_index, plane_index); + // annotateImage(*image, plane_cloud_index, table_color); + // pcl::Indices object_cloud_index = multiplyIndex(floorless_index, multiplyIndex(planeless_index, object_index)); + // annotateImage(*image, object_cloud_index, occupied_color); + // pcl::Indices below_table_cloud_index = multiplyIndex(floorless_index, multiplyIndex(planeless_index, below_index)); + // annotateImage(*image, below_table_cloud_index, occluded_color); + } + + std::cout << "creating costmap" << std::endl; + + // draw clouds to costmap + createCostmap(plane_cloud, table_color); + createCostmap(object_cloud, occupied_color); + createCostmap(occluded_cloud, occluded_color); + createCostmap(not_table_cloud, occupied_color); + + // donal changes----------------------------------------------------------------------------------------------------------------------------- + if (donal){ + cv::Scalar cyan(255, 255, 0); + cv::Scalar white(255, 255, 255); + cv::Scalar yellow(0, 255, 255); + cv::Scalar purple(255, 0, 255); + pcl::PointCloud::Ptr convex_hull_cloud(new pcl::PointCloud); + extractMaskPoints(plane_cloud); + createCostmap(plane_cloud,yellow); + CreateAndVisConvexHull(plane_cloud,height,convex_hull_cloud); + } + // donal changes-------------------------------------------------------------------------------------------------------------------------- + + // HERO preferred radius + createRadiusCostmap(canvas, radius_color, placement_margin); + + // // Dilate the costmap and create a new canvas + dilateCostmap(canvas, dilated_canvas, placement_margin); + + ExtractPlacementOptions(dilated_canvas, placement_canvas, table_color); + + // Extract the placement options and choose a placement solution + cv::Point2d place_point_canvas; + if (!GetPlacementOption(dilated_canvas, table_color, place_point_canvas)) + { + std::cout << "No valid place options" << std::endl; + return false; + } + + // // cv::circle(canvas, place_point_canvas, 3, placement_color, -1); + geo::Vec2d place_point; + place_point = canvasToWorld(place_point_canvas); + + // // fill result + place_pose = geo::Pose3D(place_point.x, place_point.y, height + 0.02); + return true; +} + +cv::Point2d PlaceAreaFinder::worldToCanvas(double x, double y) +{ + return cv::Point2d(round(-y / resolution), round(-x / resolution)) + canvas_center; +} + +cv::Point2d PlaceAreaFinder::canvasToWorld2(double u, double v) +{ + double x = -(v - canvas_center.y) * resolution; + double y = -(u - canvas_center.x) * resolution; + + return cv::Point2d(y, x); +} + +geo::Vec2d PlaceAreaFinder::canvasToWorld(cv::Point2d point) +{ + double y = (point.x - canvas_center.x) * -resolution; + double x = (point.y - canvas_center.y) * -resolution; + return geo::Vec2d(x, y); +} +// donal changes------------------------------------------------------------------------------------------------------------------- +void PlaceAreaFinder::CreateAndVisConvexHull(pcl::PointCloud::Ptr cloud,float height,pcl::PointCloud::Ptr& world_points) +{ + canvas_center = cv::Point2d(canvas.rows / 2, canvas.cols); + + // Create a binary mask + cv::Mat mask = cv::Mat::zeros(canvas.rows, canvas.cols, CV_8UC1); + + std::vector points; + + for (uint nIndex = 0; nIndex < cloud->points.size(); nIndex++) + { + double x = cloud->points[nIndex].x; + double y = cloud->points[nIndex].y; + + cv::Point2d p = worldToCanvas(x, y); + + if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows) + { + mask.at(p) = 255; + points.push_back(p); + } + } + + // Check if there are enough points to compute convex hull + if (points.size() >= 3) + { + // Find convex hull of the points + std::vector hull; + cv::convexHull(points, hull); + cv::fillConvexPoly(mask, hull, cv::Scalar(255, 255, 255)); + + // Transform the hull points to the world frame + for (const auto& point : hull) + { + cv::Point2d worldPoint = canvasToWorld2(point.x, point.y); + + pcl::PointXYZRGB pclPoint; + pclPoint.x = worldPoint.x; + pclPoint.y = worldPoint.y; + pclPoint.z = height; + + // Set the RGB color for visualization + pclPoint.r = 0; + pclPoint.g = 0; + pclPoint.b = 255; + + world_points->push_back(pclPoint); + } + + // Draw the convex hull on the canvas + std::vector> contours = { hull }; + // cv::drawContours(canvas, contours, -1, cv::Scalar(255, 255, 0), cv::FILLED); // Use CV_FILLED to fill the contour + cv::drawContours(canvas, contours, -1, cv::Scalar(255, 255, 255), 2);// Draws white border contour + } + else + { + std::cerr << "Not enough points for convex hull." << std::endl; + } +} + +void PlaceAreaFinder::extractMaskPoints(pcl::PointCloud::Ptr inputCloud) { + pcl::PointIndices::Ptr FilteredByMaskIndices(new pcl::PointIndices); + + for (size_t i = 0; i < inputCloud->points.size(); ++i) { + // Check for mask color (this case mask is blue) + uint8_t b = inputCloud->points[i].b; + uint8_t r = inputCloud->points[i].r; + if (b == 255 && r == 255) { + FilteredByMaskIndices->indices.push_back(static_cast(i)); + } + } + + pcl::ExtractIndices extract; + extract.setInputCloud(inputCloud); + extract.setIndices(FilteredByMaskIndices); + extract.setNegative(false); // Keep points not in the indices list + extract.filter(*inputCloud); +} + + +// donal changes---------------------------------------------------------------------------------------------------------------------------------- +void PlaceAreaFinder::createCostmap(pcl::PointCloud::Ptr cloud, cv::Scalar color) +{ + canvas_center = cv::Point2d(canvas.rows / 2, canvas.cols); + + for (uint nIndex = 0; nIndex < cloud->points.size(); nIndex++) + { + double x = cloud->points[nIndex].x; + double y = cloud->points[nIndex].y; + + cv::Point2d p = worldToCanvas(x, y); + if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows) + canvas.at(p) = cv::Vec3b(color[0], color[1], color[2]); + } +} + +void PlaceAreaFinder::createRadiusCostmap(cv::Mat &canvas, cv::Scalar color, float placement_margin) +{ + canvas_center = cv::Point2d(canvas.rows / 2, canvas.cols); + float upper_radius = 0.75 + placement_margin / 2; + float lower_radius = 0.60 - placement_margin / 2; + for (float phi = 0; phi < 360; phi++) + { + for (int i = 0; i < 100; i++) + { + double y = upper_radius * sin(phi / (180 / M_PI)) + 0.03 * i * sin(phi / (180 / M_PI)); + double x = upper_radius * cos(phi / (180 / M_PI)) + 0.03 * i * cos(phi / (180 / M_PI)); + + cv::Point2d p = worldToCanvas(x, y); + if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows) + canvas.at(p) = cv::Vec3b(color[0], color[1], color[2]); + } + + for (int i = 0; i < 100; i++) + { + double y = lower_radius * sin(phi / (180 / M_PI)) - lower_radius / 100 * i * sin(phi / (180 / M_PI)); + double x = lower_radius * cos(phi / (180 / M_PI)) - lower_radius / 100 * i * cos(phi / (180 / M_PI)); + + cv::Point2d p = worldToCanvas(x, y); + if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows) + canvas.at(p) = cv::Vec3b(color[0], color[1], color[2]); + } + } +} + +void PlaceAreaFinder::dilateCostmap(cv::Mat &canvas, cv::Mat &dilated_canvas, float placement_margin) +{ + float Pixelsize = placement_margin / resolution; + cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(Pixelsize, Pixelsize), + cv::Point(-1, -1)); + cv::dilate(canvas, dilated_canvas, element); +} + +void PlaceAreaFinder::annotateImage(const rgbd::Image &image, const pcl::Indices index, cv::Scalar color) +{ + int width = image.getDepthImage().cols; + + for (uint i = 0; i < index.size(); i++) + { + int col = index[i] % width; + int row = index[i] / width; + + cv::Point2d p = cv::Point2d(col, row); + if (p.x >= 0 && p.y >= 0 && p.x < annotated_image.cols && p.y < annotated_image.rows) + annotated_image.at(p) = cv::Vec3b(color[0], color[1], color[2]); + } +} + + diff --git a/ed_sensor_integration/tools/empty_spot_designator.cpp b/ed_sensor_integration/tools/empty_spot_designator.cpp new file mode 100644 index 00000000..22efab95 --- /dev/null +++ b/ed_sensor_integration/tools/empty_spot_designator.cpp @@ -0,0 +1,330 @@ +#include +#include + +#include + +#include +#include +#include +#include + +// +#include +#include +// +#include +#include + +//pcl library # TODO remove the unused ones #TODO find out which ones are unused +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "ed_sensor_integration/sac_model_horizontal_plane.h" + +#include + + +double resolution = 0.005; +cv::Point2d canvas_center; +cv::Mat mask; // global variable for a\passing the image mask +std::mutex mutex; + +geo::Vector3 simpleRayTrace(geo::Vector3 origin, geo::Vector3 direction) +{ + if (direction.z > 0) + { + std::cout << "Raytrace went terribly wrong" << std::endl; + } + double ratio = -origin.z / direction.z; + + return geo::Vector3(origin.x + ratio * direction.x, origin.y + ratio * direction.y, 0); +} + +void drawFoVMacro(geo::Vector3 direction, cv::Mat& canvas, geo::Pose3D sensor_pose) +{ + // convert vectors to world frame + geo::Vector3 direction_world = sensor_pose.R * direction; + + if (direction_world.z > 0.0) + { + std::cout << "above plane" << std::endl; + return; + } + + // project vectors on place + geo::Vector3 projected_point = simpleRayTrace(sensor_pose.t, direction_world); + + // draw projected points + cv::Point2d p1_canvas = cv::Point2d(-projected_point.y / resolution, -projected_point.x / resolution) + canvas_center; + cv::Scalar fovcolor(0, 255, 255); // Red + cv::circle(canvas, p1_canvas, 5, fovcolor, -1); + + std::cout << "direction in camera frame: " << direction << std::endl; + std::cout << "direction in world frame: " << direction_world << std::endl; + std::cout << "projection in world frame: " << projected_point << std::endl; + std::cout << "cvpoint in canvas frame: x: " << p1_canvas.x << ", y: " << p1_canvas.y << std::endl; +} +/** + * @brief Draw the fiel of view of the camera on a plane. + * + * @param canvas image representing a plane + * @param sensor_pose pose of the camera with respect to the plane. It is assumed that the origin corresponds to the canvas_center point on the plane. + * @param caminfo Used to get focal lengths of the camera + */ +void drawFieldOfView(cv::Mat& canvas, geo::Pose3D sensor_pose, const rgbd::ImageConstPtr& caminfo) +{ + // Get camera info + int width = caminfo->getDepthImage().cols; + int height = caminfo->getDepthImage().rows; + double half_height = 0.5*height; + double half_width = 0.5*width; + + double fx = caminfo->getCameraModel().fx(); + double fy = caminfo->getCameraModel().fy(); + + // determine vectors pointing to corners of FoV + geo::Vector3 c0(0, 0, -1.0); // upper left of image + geo::Vector3 c1(half_width / fx, half_height / fy, -1.0); // upper left of image + geo::Vector3 c2(-half_width / fx, half_height / fy, -1.0); // upper right of image + geo::Vector3 c3(-half_width / fx, -half_height / fy, -1.0); // lower right of image + geo::Vector3 c4(half_width / fx, -half_height / fy, -1.0); // lower left of image + + // draw + std::cout << "center" << std::endl; + drawFoVMacro(c0, canvas, sensor_pose); + std::cout << "upper left" << std::endl; + drawFoVMacro(c1, canvas, sensor_pose); + std::cout << "upper right" << std::endl; + drawFoVMacro(c2, canvas, sensor_pose); + std::cout << "lower right" << std::endl; + drawFoVMacro(c3, canvas, sensor_pose); + std::cout << "lower left" << std::endl; + drawFoVMacro(c4, canvas, sensor_pose); +} + +void imageCallback(const sensor_msgs::ImageConstPtr& msg) +{ + std::cout << "Mask received!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl; + try + { + if(mutex.try_lock()) + { + mask = cv_bridge::toCvCopy(msg, "8UC3")->image; + std::cout << "Mask received" << std::endl; + mutex.unlock(); + } + else + { + std::cout << "Mutex not locked!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl; + } + //cv::waitKey(30); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } +} + +/** + * @brief usage, print how the executable should be used and explain the input + */ +void usage() +{ + std::cout << "Usage: ed_empty_spot_designator RGBD_TOPIC BOOL" << std::endl + << "RGBD_TOPIC topic on which the rgbd image is published, example /hero/head_rgbd_sensor/rgbd" << std::endl + << "Set second argument to donal or max to run that version of the code" << std::endl; +} + +void drawMaskContour(cv::Mat& image, const cv::Mat& mask) +{ + // Convert the mask to grayscale if it's not already + cv::Mat grayMask; + if (mask.channels() > 1) + { + cv::cvtColor(mask, grayMask, cv::COLOR_BGR2GRAY); + } + else + { + grayMask = mask.clone(); // If it's already single-channel, create a copy + } + + // Find contours in the binary mask + std::vector> contours; + cv::findContours(grayMask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + // Draw the contours on the image in white + cv::Scalar contourColor(255, 255, 255); // White color + cv::drawContours(image, contours, -1, contourColor, 2); // -1 means draw all contours, 2 is the thickness +} + +rgbd::ImageConstPtr createModifiedImage(const rgbd::ImageConstPtr& originalImagePtr, const cv::Mat& newRGBValues) { + rgbd::Image modifiedImage = originalImagePtr->clone(); + modifiedImage.setRGBImage(newRGBValues); + rgbd::ImageConstPtr modifiedImagePtr = std::make_shared(modifiedImage); + + return modifiedImagePtr; +} + +/** + * @brief main executable to visualise the empty spot finder of live images. + * @param argc: + * @return + */ +int main (int argc, char **argv) +{ + if (argc != 3) + { + usage(); + return 0; + } + + ros::init(argc, argv, "empty_spot_visualizer"); + + std::string topic = argv[1]; + std::cout << "Using topic: " << topic << std::endl; + + std::string arg = argv[2]; + bool donal = (arg == "donal"); + if (donal){ + std::cout <<"Running Donal's version!"<getRGBImage(); + // cv::imshow("RGB remapped to mask", rgbcanvas); + std::cout << "Replaced RGB with mask!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl; + + if (!place_area_finder.findArea(new_image_ptr, sensor_pose, place_pose,mask,donal)) + { + std::cout << "no place area found" << std::endl; + } + mutex.unlock(); + } + + } + else{ + if (!place_area_finder.findArea(image, sensor_pose, place_pose,mask,donal)) + { + std::cout << "no place area found" << std::endl; + } + } + + + std::cout << place_pose << std::endl; + + cv::Mat canvas; + cv::Mat dilated_canvas; + cv::Mat placement_canvas; + place_area_finder.getCanvas(canvas); + // place_area_finder.getDilatedCanvas(dilated_canvas); + // place_area_finder.getPlacementCanvas(placement_canvas); + /* + canvas_center = cv::Point2d(canvas.rows / 2, canvas.cols); + geo::Pose3D sensor_pose_canvas = sensor_pose; + sensor_pose_canvas.t.z = sensor_pose.t.z - place_pose.t.z; + drawFieldOfView(canvas, sensor_pose_canvas, image); + */ + // Show the different canvasses + + // std::cout << "showing costmap" << std::endl; + if(!canvas.empty()){ + cv::imshow("Costmap topview", canvas); + } + + // std::cout << "showing dilated costmap" << std::endl; + if(!dilated_canvas.empty()){ + cv::imshow("Dilated costmap topview", dilated_canvas); + } + // std::cout << "showing placement costmap" << std::endl; + if(!placement_canvas.empty()){ + cv::imshow("Placement options costmap topview", placement_canvas); + } + // if (mutex.try_lock()){ + + // if (!mask.empty()) { + // cv::imshow("Mask", mask); + // } + // mutex.unlock(); + // } + if (!mask.empty()) + { + cv::Mat annotated_image; + cv::Mat annotated_image_with_mask; + place_area_finder.getAnnotatedImage(annotated_image); + annotated_image_with_mask = annotated_image.clone(); + drawMaskContour(annotated_image_with_mask, mask); + cv::imshow("Annotated_image", annotated_image_with_mask); + } + // Show RGB snapshot + cv::Mat rgbcanvas = image->getRGBImage(); + cv::imshow("Original RGB", rgbcanvas); + drawMaskContour(rgbcanvas, mask); + cv::imshow("Original RGB with Table Mask", rgbcanvas); + + char key = cv::waitKey(30); + + if (key == 'q') + { + break; + } + ros::spinOnce(); + } + cv::destroyAllWindows(); + return 0; +} diff --git a/ed_sensor_integration/tools/table_modeler.cpp b/ed_sensor_integration/tools/table_modeler.cpp new file mode 100644 index 00000000..a6fbe275 --- /dev/null +++ b/ed_sensor_integration/tools/table_modeler.cpp @@ -0,0 +1,511 @@ +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include +#include + +#include + +#include "ed_sensor_integration/sac_model_double_line.h" +#include "ed_sensor_integration/sac_model_circle.h" +#include "ed_sensor_integration/sac_model_horizontal_plane.h" + +/** + * @brief pairAlign use iterative closest point to align two pointclouds with one another + * @param cloud_src: source cloud, will be transformed to be expressed in the same frame as the target cloud. + * @param cloud_tgt: target cloud to match to. + * @param final_transform: transform from target to source + */ +void pairAlign (const pcl::PointCloud::Ptr cloud_src, const pcl::PointCloud::Ptr cloud_tgt, Eigen::Matrix4f &final_transform) +{ + pcl::PointCloud::Ptr src (new pcl::PointCloud); + pcl::PointCloud::Ptr tgt (new pcl::PointCloud); + + *src = *cloud_src; + *tgt = *cloud_tgt; + + pcl::IterativeClosestPointNonLinear reg; + reg.setTransformationEpsilon (1e-6); + // Set the maximum distance between two correspondences (src<->tgt) to 10cm + // Note: adjust this based on the size of your datasets + reg.setMaxCorrespondenceDistance (1); // TODO Magic number + + reg.setInputSource (src); + reg.setInputTarget (tgt); + + // Run the optimization + Eigen::Matrix4f targetToSource; + reg.setMaximumIterations (60); + + reg.align (*src); + + // Get the transformation from target to source + targetToSource = reg.getFinalTransformation();//.inverse(); + + final_transform = targetToSource; +} + +/** + * @brief FilterPlane fit a plane through a pointcloud, filter the points which lie in this plane and return the height of the plane #TODO separation of concerns. + * @param cloud: pointcloud to be filtered. + * @param out: pointcloud with all points that lie within the plane + * @return height (z coordinate) of the fitted plane. + */ +float FilterPlane (pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr out) +{ + std::vector indices; + float threshold = 0.03; + + std::cout << "starting ransac" << std::endl; + // Create SAC model + pcl::SampleConsensusModelHorizontalPlane::Ptr plane (new pcl::SampleConsensusModelHorizontalPlane(cloud)); + std::cout << "created plane object" << std::endl; + // Create SAC method + pcl::RandomSampleConsensus::Ptr sac (new pcl::RandomSampleConsensus (plane, threshold)); + std::cout << "created ransac object" << std::endl; + sac->setMaxIterations(10000); + sac->setProbability(0.99); + + // Fit model + sac->computeModel(); + + // Get inliers + std::vector inliers; + sac->getInliers(inliers); + + // Get the model coefficients + Eigen::VectorXf coeff; + sac->getModelCoefficients (coeff); + std::cout << "ransac complete" << std::endl; + + pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); + range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::GT, (coeff[3]-0.01)))); + *out = *cloud; + //filter out everything below plane + pcl::ConditionalRemoval condrem; + condrem.setCondition (range_cond); + condrem.setInputCloud (out); + condrem.setKeepOrganized(true); + + condrem.filter (*out); + (*out).is_dense = false; + pcl::removeNaNFromPointCloud(*out, *out, indices); + + return(coeff[3]); +} + +/** + * @brief Segment segment the pointcloud and return the cluster closest to the camera + * @param cloud: pointcloud to be segmented, this function will change the pointcloud to only include the segmented cluster. + * @param x: coordinate of the camera + * @param y: coordinate of the camera + * @param z: coordinate of the camera + * @param [out] tableHeight[m]: estimated height of the table based on the cluster. + */ +void Segment (pcl::PointCloud::Ptr cloud, float x, float y, float z, std::vector &tableHeight) +{ + std::cout << "starting segmentation" << std::endl; + std::cout << "x = " << x << ", y = " << y << ", z = " << z << std::endl; + + // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; //using euclidian cluster extraction + ec.setClusterTolerance (0.1); + ec.setMinClusterSize ((*cloud).size()/100); + ec.setMaxClusterSize ((*cloud).size()); + ec.setSearchMethod (tree); + ec.setInputCloud (cloud); + ec.extract (cluster_indices); + + std::cout << "obtained cluster indices" <::Ptr cloud_out; + float mindist = INFINITY; + for (std::vector::const_iterator it = cluster_indices.begin (); it != + cluster_indices.end (); ++it) //iterate through all clusters + { + //construct cluster + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (const auto& idx : it->indices) + cloud_cluster->push_back ((*cloud)[idx]); //* + cloud_cluster->width = cloud_cluster->size (); + cloud_cluster->height = 1; + cloud_cluster->is_dense = true; + float sumx = 0, sumy = 0, sumz = 0, dist = 0; + for (uint j=0; j < (*cloud_cluster).width; ++j) + { + //sum up all points + sumx += (*cloud_cluster)[j].x; + sumy += (*cloud_cluster)[j].y; + sumz += (*cloud_cluster)[j].z; + } + //find distance from camera to the middle of the cluster + dist = pow((sumx/(*cloud_cluster).width-x),2) + pow((sumy/(*cloud_cluster).width-y),2) + pow((sumz/(*cloud_cluster).width-z),2); + std::cout << "distance is " << sqrt(dist) << std::endl; + if (dist < mindist) //check if closest so far + { + std::cout << "updating closest cluster" << std::endl; + mindist = dist; + cloud_out = cloud_cluster; + } + } + + float height = FilterPlane(cloud_out, cloud_out); + if (height != -1) + { + tableHeight.push_back(height); + } + + std::cout << "writing closest cluster" << std::endl; + *cloud = *cloud_out; +} + +/** + * @brief ReadJson: Extract the pose of the camera corresponding to a pcd file. + * Pose is expressed in map frame. (json filename is assumed to be the same as + * the pcd file with a different extension.) #TODO strange interface + * @param pcd_filename: Filename of the pcd file + * @param xout: X Coordinate of the pose of the camera + * @param yout: Y Coordinate of the pose of the camera + * @param zout: Z Coordinate of the pose of the camera + * @return Eigen rotationmatrix + */ +Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, float *zout) { + + std::string json_filename = boost::filesystem::change_extension(pcd_filename, ".json").string(); + // read json metadata + tue::config::DataPointer meta_data; + + try + { + meta_data = tue::config::fromFile(json_filename); + } + catch (tue::config::ParseException& e) + { + std::cerr << "Could not open '" << json_filename << "'.\n\n" << e.what() << std::endl; + //return 0; #TODO proper behaviour for not finding files. + } + + tue::config::Reader r(meta_data); + // Read sensor pose + geo::Pose3D sensor_pose; + if (!ed::deserialize(r, "sensor_pose", sensor_pose)) + { + std::cerr << "No field 'sensor_pose' specified." << std::endl; + //return 0; #TODO proper behaviour for not finding data. + } + // convert from geolib coordinates to ros coordinates #TODO remove geolib coordinates for camera pose + sensor_pose.R = sensor_pose.R * geo::Mat3(1, 0, 0, 0, -1, 0, 0, 0, -1); + + float x = sensor_pose.t.x; + float y = sensor_pose.t.y; + float z = sensor_pose.t.z; + float xx = sensor_pose.R.xx; + float xy = sensor_pose.R.xy; + float xz = sensor_pose.R.xz; + float yx = sensor_pose.R.yx; + float yy = sensor_pose.R.yy; + float yz = sensor_pose.R.yz; + float zx = sensor_pose.R.zx; + float zy = sensor_pose.R.zy; + float zz = sensor_pose.R.zz; + + *xout = x; + *yout = y; + *zout = z; + + Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity(); + + Transform(0,0) = xx; + Transform(0,1) = xy; + Transform(0,2) = xz; + Transform(0,3) = x; + Transform(1,0) = yx; + Transform(1,1) = yy; + Transform(1,2) = yz; + Transform(1,3) = y; + Transform(2,0) = zx; + Transform(2,1) = zy; + Transform(2,2) = zz; + Transform(2,3) = z; + + std::cout << Transform << std::endl; + return Transform; +} + +/** + * @brief Flatten: Remove z dimension from pointcloud. + * Also centers the pointcloud around x,y = (0,0) + * Also creates the convex hull of the flattened pointcloud. #TODO separation of concerns + * @param cloud: Input cloud + * @return Flattened point cloud. + */ +pcl::PointCloud Flatten(pcl::PointCloud cloud) { + float totx = 0, toty = 0, avgx, avgy; + + pcl::PointCloud::Ptr flat(new pcl::PointCloud); + Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity (); + std::cout << "Cloud width = " << (cloud).width << std::endl; + (*flat).width = (cloud).width; + (*flat).resize((*flat).width); + for (uint i=0; i < (cloud).width; ++i) + { + (*flat)[i].x = cloud[i].x; + (*flat)[i].y = cloud[i].y; + (*flat)[i].z = 0; + + totx += (*flat)[i].x; + toty += (*flat)[i].y; + } + Transform(0,3) = -totx/(*flat).width; + Transform(1,3) = -toty/(*flat).width; + + pcl::transformPointCloud (*flat, *flat, Transform); + + pcl::ConcaveHull CHull; + CHull.setInputCloud(flat); + CHull.setAlpha (0.1); + CHull.setDimension(2); + CHull.reconstruct(*flat); + return *flat; +} + +/** + * @brief Fit: Estimate a geometric representation of the shape underlying the data in the pointcloud + * Will attempt a parallelogram model and a cirle model. #TODO add support for concave hull model. + * @param cloud: Point cloud specifiying a concave hull in 2D. + * @return Coefficients of the best model. + */ +Eigen::VectorXf Fit(pcl::PointCloud cloud) { + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + *cloud_ptr = cloud; + float threshold = 0.03; + // Create SAC model + pcl::SampleConsensusModelDoubleLine::Ptr line1 (new pcl::SampleConsensusModelDoubleLine(cloud_ptr)); + + // Create SAC method + pcl::RandomSampleConsensus::Ptr sac1 (new pcl::RandomSampleConsensus (line1, threshold)); + sac1->setMaxIterations(10000); + sac1->setProbability(1); + + // Fit model + sac1->computeModel(); + + // Get inliers + std::vector inliers1; + sac1->getInliers(inliers1); + + // Get the model coefficients + Eigen::VectorXf coeff1; + sac1->getModelCoefficients (coeff1); + + //extract the inliers and fit second set of lines perpendicular + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + pcl::PointIndices::Ptr line1_inliers (new pcl::PointIndices); + line1_inliers->indices = inliers1; + pcl::ExtractIndices extract; + extract.setInputCloud (cloud_ptr); + extract.setIndices (line1_inliers); + extract.setNegative (true); + extract.filter (*cloud2); + // Create second SAC model + pcl::SampleConsensusModelDoubleLine::Ptr line2 (new pcl::SampleConsensusModelDoubleLine(cloud2)); + // Create SAC method + pcl::RandomSampleConsensus::Ptr sac2 (new pcl::RandomSampleConsensus (line2, threshold)); + sac2->setMaxIterations(10000); + sac2->setProbability(1); + // Fit model + sac2->computeModel(); + // Get inliers + std::vector inliers2; + sac2->getInliers(inliers2); + // Get the model coefficients + Eigen::VectorXf coeff2; + sac2->getModelCoefficients (coeff2); + + pcl::PointIndices::Ptr line2_inliers (new pcl::PointIndices); + line2_inliers->indices = inliers2; + extract.setInputCloud (cloud2); + extract.setIndices (line2_inliers); + extract.filter (*cloud2); + + //repeat above steps for a circle + pcl::SampleConsensusModelCircle::Ptr circle (new pcl::SampleConsensusModelCircle(cloud_ptr)); + + // Create SAC method + pcl::RandomSampleConsensus::Ptr saccirc (new pcl::RandomSampleConsensus (circle, threshold)); + saccirc->setMaxIterations(10000); + saccirc->setProbability(1); + + // Fit model + saccirc->computeModel(); + + // Get inliers + std::vector inliers3; + saccirc->getInliers(inliers3); + + // Get the model coefficients + Eigen::VectorXf coeff3; + saccirc->getModelCoefficients (coeff3); + + float min_inliers = 0.0; + Eigen::VectorXf output; + + std::cout << "Parallelogram model, " << static_cast(inliers1.size()+inliers2.size())/static_cast(cloud.size()) << std::endl; + std::cout << "Circle model, " << static_cast(inliers3.size())/static_cast(cloud.size()) << std::endl; + + if ((inliers1.size()+inliers2.size() >= inliers3.size()) && (static_cast(inliers1.size()+inliers2.size())/static_cast(cloud.size()) > min_inliers)) + { + float w = coeff1[2]; + float l = coeff2[2]; + float r = coeff1[3] - coeff2[3];//difference in angle: if not 90 degrees, model is a parallellogram + + output.resize(3); + output[0] = l; + output[1] = w; + output[2] = r; + } + else if ((inliers1.size()+inliers2.size() < inliers3.size()) && (static_cast(inliers3.size())/static_cast(cloud.size()) > min_inliers)) + { + output.resize(1); + output[0] = coeff3[2]; + } + return (output); +} + +int main(int argc, char **argv) +{ + if (argc < 2) + { + std::cout << "Usage:\n\n table_modeler FILENAME1 FILENAME2 ...\n\n"; + return 1; + } + + std::vector ::Ptr, Eigen::aligned_allocator ::Ptr > > inputs; + + std::vector indices; + + std::cout << "starting to open" << std::endl; + + for (int i = 1; i < argc; ++i) + { + std::string name = std::string(argv[i]); + pcl::PointCloud::Ptr m(new pcl::PointCloud); + std::cout << "attempting to open " << name << std::endl; + if (pcl::io::loadPCDFile (name, *m) == -1) //* load the file + { + std::cout << "Couldn't read file" << name << std::endl; + return -1; + } + std::cout << "opened " << name << std::endl; + pcl::removeNaNFromPointCloud(*m, *m, indices); + std::cout << "removed NAN from " << name << std::endl; + inputs.push_back(m); + } + + pcl::PointCloud::Ptr result (new pcl::PointCloud), source, target; + Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform; + + std::vector tableHeight;//used to keep track of the table height + + float x, y, z;//used to store camera position + pcl::transformPointCloud (*inputs[0], *inputs[0], ReadJson(argv[1], &x, &y, &z)); + + // Filter out floor + pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); + range_cond->addComparison (pcl::FieldComparison::ConstPtr (new pcl::FieldComparison ("z", pcl::ComparisonOps::GT, 0.1))); + // build the filter + pcl::ConditionalRemoval condrem; + condrem.setCondition (range_cond); + condrem.setInputCloud (inputs[0]); + condrem.setKeepOrganized(true); + // apply filter + condrem.filter (*inputs[0]); + (*inputs[0]).is_dense = false; + pcl::removeNaNFromPointCloud(*inputs[0], *inputs[0], indices); + + Segment(inputs[0], x, y, z, tableHeight); + + *result = *inputs[0]; + + pcl::io::savePCDFileASCII ("first.pcd", *inputs[0]); + + for (int i = 1; i < argc-1; ++i) + { + std::cout << "iteration " << i << std::endl; + + // align to world model coordinates + pcl::transformPointCloud (*inputs[i], *inputs[i], ReadJson(argv[i+1], &x, &y, &z)); + + //filter out floor + condrem.setInputCloud (inputs[i]); + // apply filter + condrem.filter (*inputs[i]); + (*inputs[i]).is_dense = false; + pcl::removeNaNFromPointCloud(*inputs[i], *inputs[i], indices); + + Segment(inputs[i], x, y, z, tableHeight); + + // align to previous file + + source = inputs[i]; + pcl::PointCloud::Ptr temp (new pcl::PointCloud); + + std::cout << "aligning cloud " << i << " to cloud " << i - 1 << std::endl; + pairAlign (source, result, pairTransform); + std::cout << "transformation matrix =" << std::endl << pairTransform << std::endl; + + GlobalTransform *= pairTransform; + + pcl::transformPointCloud (*source, *temp, pairTransform); + + *result += *temp; + } + + pcl::PointCloud flat; + flat = Flatten(*result); + std::cout << flat.width << std::endl; + + Eigen::VectorXf model = Fit(flat); + + std::cout << "model coefficients: " << std::endl << model << std::endl; + + std::cout << "Writing clouds" << std::endl; + pcl::io::savePCDFileASCII ("combined.pcd", *result); + pcl::io::savePCDFileASCII ("flat.pcd", flat); + std::cout << "The table is " << std::accumulate(tableHeight.begin(), tableHeight.end(), 0.0) / tableHeight.size() << "m tall" << std::endl; + + return 0; +} diff --git a/ed_sensor_integration/yolo_python/previous_iterations/YOLOv8_single_image.py b/ed_sensor_integration/yolo_python/previous_iterations/YOLOv8_single_image.py new file mode 100644 index 00000000..89fa6717 --- /dev/null +++ b/ed_sensor_integration/yolo_python/previous_iterations/YOLOv8_single_image.py @@ -0,0 +1,165 @@ +import cv2 +from matplotlib import image as img +from ultralytics import YOLO +import numpy as np +import time +def detect(model,img): + + results = model.predict(source=img.copy(),save=False,save_txt=False) + result=results[0] + segmentation_contours_idx = [] + for seg in result.masks.xy: + segment = np.array(seg,dtype=np.int32) + segmentation_contours_idx.append(segment) + bboxes = np.array(result.boxes.xyxy.cpu(),dtype="int") + class_ids = np.array(result.boxes.cls.cpu(),dtype="int") + #scores = np.array(result.boxes.conf.cpu(),dtype="float").round(2) + return bboxes, class_ids, segmentation_contours_idx#, scores + +# def brightness(image,brightness_factor): #positive value -> brighter, negative -> darker +# # Convert the image to 32-bit float +# image_float = image.astype(np.float32) +# # Apply the darkness factor to each pixel +# darkened_image_float = image_float + brightness_factor +# # Clip the values to ensure they stay within the valid range [0, 255] +# darkened_image_float = np.clip(darkened_image_float, 0, 255) +# # Convert the result back to 8-bit unsigned integer +# darkened_image = darkened_image_float.astype(np.uint8) +# return darkened_image + +# def change_saturation(image, saturation_factor): +# # Convert the image from BGR to HSV color space +# hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + +# # Split the HSV image into its components +# h, s, v = cv2.split(hsv_image) + +# # Apply the saturation factor +# s = np.clip(s * saturation_factor, 0, 255).astype(np.uint8) + +# # Merge the modified components back into an HSV image +# modified_hsv = cv2.merge((h, s, v)) + +# # Convert the modified HSV image back to BGR color space +# modified_image = cv2.cvtColor(modified_hsv, cv2.COLOR_HSV2BGR) + +# return modified_image +#Colours dict +colours = { + 'red': (0, 0, 255), + 'green': (0, 255, 0), + 'blue': (255, 0, 0), + 'yellow': (0, 255, 255), + 'purple': (128, 0, 128), + # Add more colors as needed +} + +#Inputs +#image_path = r"C:\Users\Dónal\Desktop\segment_any\images\corner2.jpg" +model_path = "/home/donal/ros/noetic/system/src/ed_sensor_integration/scripts/yolov8n-seg.pt" +device = "cuda" + +#Loads image, converts to BGR colour channel, darkens image and loads model +#image = img.imread(image_path) +#image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) +# darkness_factor = -25 # Decrease the brightness by 50 units +# image = brightness(image,darkness_factor) +# saturation_factor = 0.8 +# image = change_saturation(image,saturation_factor) + +class_id_to_extract = 0 +colours = { + 'yellow': (0, 255, 255), + 'blue': (255, 0, 0), +} + +cap = cv2.VideoCapture(0) # Open the webcam + +if not cap.isOpened(): + print("Error: Could not open the camera.") + exit() + +while True: + ret, frame = cap.read() + + if not ret: + print("Error: Could not read a frame.") + break + model = YOLO(model_path) + bboxes, classes, segmentations = detect(model, frame) #, scores = detect(model, frame) + + # Extract the segment of class id 60 (table) + table_indices = [i for i, class_id in enumerate(classes) if class_id == class_id_to_extract] + + for i in table_indices: + x, y, x2, y2 = bboxes[i] + seg = segmentations[i] + + cv2.rectangle(frame, (x, y), (x2, y2), colours['yellow'], 2) + cv2.polylines(frame, [seg], True, colours['blue'], 2) + cv2.putText(frame, "Table", (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, colours['yellow'], 2) + + cv2.imshow("Video Feed", frame) + + if cv2.waitKey(1) & 0xFF == ord('q'): + break + +cap.release() +cv2.destroyAllWindows() +# cap = cv2.VideoCapture(0) + +# while cap.isOpened(): +# success, frame = cap.read() + +# if success: +# start = time.perf_counter() + +# model = YOLO(model_path) + +# end = time.perf_counter() +# total_time = end -start +# dps = 1/ total_time + +# #extracts data from model +# bboxes, classes, segmentations, scores = detect(model,frame) + +# #Extracting only table mask +# for bbox, class_id, seg, score in zip(bboxes, classes, segmentations, scores): +# (x, y, x2, y2) = bbox +# # Table id from trained dataset is 60 +# if class_id == 0: +# cv2.rectangle(frame, (x, y), (x2, y2), colours['yellow'], 2) #Colour is Blue Green Red BGR +# cv2.polylines(frame, [seg], True, colours['blue'], 2) +# # cv2.fillPoly(image, [seg], colours['purple']) +# cv2.putText(frame, "Table", (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, colours['yellow'], 2) +# cv2.imshow("Video Feed",frame) +# if cv2.waitKey(1) & 0xFF == ord('q'): +# break + +# # After the loop release the cap object +# cap.release() +# # Destroy all the windows +# cv2.destroyAllWindows() + + + + +# model = YOLO(model_path) + +# #extracts data from model +# bboxes, classes, segmentations, scores = detect(model,image) + +# #Extracting only table mask +# for bbox, class_id, seg, score in zip(bboxes, classes, segmentations, scores): +# (x, y, x2, y2) = bbox +# # Table id from trained dataset is 60 +# if class_id == 60: +# cv2.rectangle(image, (x, y), (x2, y2), colours['yellow'], 2) #Colour is Blue Green Red BGR +# cv2.polylines(image, [seg], True, colours['blue'], 2) +# # cv2.fillPoly(image, [seg], colours['purple']) +# cv2.putText(image, "Table", (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, colours['yellow'], 2) + +# #Visualization +# cv2.imshow('Result Image', image) +# cv2.waitKey(0) +# cv2.destroyAllWindows() \ No newline at end of file diff --git a/ed_sensor_integration/yolo_python/previous_iterations/YOLOv8_video_optimized.py b/ed_sensor_integration/yolo_python/previous_iterations/YOLOv8_video_optimized.py new file mode 100644 index 00000000..8950266d --- /dev/null +++ b/ed_sensor_integration/yolo_python/previous_iterations/YOLOv8_video_optimized.py @@ -0,0 +1,61 @@ +import cv2 +from ultralytics import YOLO +import numpy as np +import time + +def detect(model, frame): + results = model(frame) + result = results[0] + segmentation_contours_idx = [np.array(seg, dtype=np.int32) for seg in result.masks.xy] + class_ids = np.array(result.boxes.cls.cpu(), dtype="int") + return class_ids, segmentation_contours_idx + +colours = { + 'yellow': (0, 255, 255), + 'blue': (255, 0, 0), +} + +model_path = "/home/donal/ros/noetic/system/src/ed_sensor_integration/scripts/yolov8n-seg.pt" +device = "cpu" +model = YOLO(model_path).to(device) +table_class = 0 #table class defined with index 60 (person = 0) + +# Detection Loop with webcam +cap = cv2.VideoCapture(0) + +if not cap.isOpened(): + print("Error: Could not open the camera.") + exit() + +# Initialize refresh rate calc +start_time = time.time() +frame_count = 0 + +while True: + ret, frame = cap.read() + + if not ret: + print("Error: Could not read a frame.") + break + #Get classes and segments + classes, segmentations = detect(model, frame) + #extract table segment and add to frame + for class_id, seg in zip(classes, segmentations): + if class_id == table_class: + cv2.polylines(frame, [seg], True, colours['blue'], 2) + # Calculate the refresh rate for segmentation + frame_count += 1 + end_time = time.time() + elapsed_time = end_time - start_time + segmentation_frame_rate = int(frame_count / elapsed_time) + start_time = end_time + frame_count = 0 + # Display segmentation refresh rate + cv2.putText(frame, f"Seg FPS: {segmentation_frame_rate}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2) + cv2.imshow("Video Feed", frame) + #press q to quit window + if cv2.waitKey(1) & 0xFF == ord('q'): + break + +cap.release() +cv2.destroyAllWindows() \ No newline at end of file diff --git a/ed_sensor_integration/yolo_python/previous_iterations/mask_mapper.cpp b/ed_sensor_integration/yolo_python/previous_iterations/mask_mapper.cpp new file mode 100644 index 00000000..f5f01b02 --- /dev/null +++ b/ed_sensor_integration/yolo_python/previous_iterations/mask_mapper.cpp @@ -0,0 +1,30 @@ +#include +#include +#include +#include + +void imageCallback(const sensor_msgs::ImageConstPtr& msg) +{ + try + { + cv::imshow("view", cv_bridge::toCvShare(msg, "8UC3")->image); + cv::waitKey(30); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } +} + +int main(int argc, char **argv) +{ + std::string mask_topic = "/hero/segmented_image"; + ros::init(argc, argv, "image_listener"); + ros::NodeHandle nh; + cv::namedWindow("view"); + + image_transport::ImageTransport it(nh); + image_transport::Subscriber sub = it.subscribe(mask_topic, 1, imageCallback); + ros::spin(); + cv::destroyWindow("view"); +} \ No newline at end of file diff --git a/ed_sensor_integration/yolo_python/previous_iterations/talker.py b/ed_sensor_integration/yolo_python/previous_iterations/talker.py new file mode 100755 index 00000000..033dc449 --- /dev/null +++ b/ed_sensor_integration/yolo_python/previous_iterations/talker.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Revision $Id$ + +## Simple talker demo that published std_msgs/Strings messages +## to the 'chatter' topic + +import rospy +from std_msgs.msg import String + +def talker(): + pub = rospy.Publisher('chatter', String, queue_size=10) + rospy.init_node('talker', anonymous=True) + rate = rospy.Rate(10) # 10hz + while not rospy.is_shutdown(): + hello_str = "hello world %s" % rospy.get_time() + rospy.loginfo(hello_str) + pub.publish(hello_str) + rate.sleep() + +if __name__ == '__main__': + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/ed_sensor_integration/yolo_python/yolo_table_segmentor.py b/ed_sensor_integration/yolo_python/yolo_table_segmentor.py new file mode 100755 index 00000000..57780a28 --- /dev/null +++ b/ed_sensor_integration/yolo_python/yolo_table_segmentor.py @@ -0,0 +1,57 @@ + +import rospy +import cv2 +import numpy as np +from ultralytics import YOLO +from cv_bridge import CvBridge +from sensor_msgs.msg import Image + +class table_segmentor: + def __init__(self) -> None: + model_path = "~/MEGA/developers/Donal/yolov8x-seg.pt" + device = "cuda" + self.model = YOLO(model_path).to(device) + self.table_class = 60 #table class defined with index 60 (person = 0) + + rospy.init_node('listener', anonymous=True) + self.publisher = rospy.Publisher('/hero/segmented_image',Image,queue_size=10) + self.subscriber = rospy.Subscriber('/hero/head_rgbd_sensor/rgb/image_raw',Image , self.callback) + + @staticmethod + def detect(model, frame): + results = model(frame) + result = results[0] + segmentation_contours_idx = [np.array(seg, dtype=np.int32) for seg in result.masks.xy] + class_ids = np.array(result.boxes.cls.cpu(), dtype="int") + return class_ids, segmentation_contours_idx + + def extract_table_segment(self, image, class_ids, segmentations): + table_mask = np.zeros_like(image, dtype=np.uint8) + purple_colour = (255, 0, 255) + for class_id, seg in zip(class_ids, segmentations): + if class_id == self.table_class: + cv2.fillPoly(table_mask, [seg], purple_colour) + return table_mask + + def callback(self, data): + rospy.loginfo("got message") + bridge = CvBridge() + cv_image = bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') + cv_image = cv2.GaussianBlur(cv_image,(5,5),0) + cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) + rospy.loginfo("converted message") + + classes, segmentations = self.detect(self.model, cv_image) + table_segment = self.extract_table_segment(cv_image, classes, segmentations) + + # Publish the table segment as a binary mask + table_message = bridge.cv2_to_imgmsg(table_segment, encoding="passthrough") + self.publisher.publish(table_message) + + def listener(): + rospy.Subscriber('/hero/head_rgbd_sensor/rgb/image_raw',Image , callback) + rospy.spin() + +if __name__ == '__main__': + ts = table_segmentor() + rospy.spin() diff --git a/ed_sensor_integration_msgs/CMakeLists.txt b/ed_sensor_integration_msgs/CMakeLists.txt index 47ce1060..269bddb5 100644 --- a/ed_sensor_integration_msgs/CMakeLists.txt +++ b/ed_sensor_integration_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ add_service_files( GetImage.srv Update.srv RayTrace.srv + PlaceArea.srv ) generate_messages( diff --git a/ed_sensor_integration_msgs/srv/PlaceArea.srv b/ed_sensor_integration_msgs/srv/PlaceArea.srv new file mode 100644 index 00000000..f575de18 --- /dev/null +++ b/ed_sensor_integration_msgs/srv/PlaceArea.srv @@ -0,0 +1,4 @@ +# request info +--- +string error_msg # empty if no error +geometry_msgs/PoseStamped pose # pose of a possible place location