diff --git a/cdcpd/CMakeLists.txt b/cdcpd/CMakeLists.txt index a8186e9..e4052f2 100644 --- a/cdcpd/CMakeLists.txt +++ b/cdcpd/CMakeLists.txt @@ -67,6 +67,8 @@ add_library(cdcpd SHARED src/obs_util.cpp src/past_template_matcher.cpp src/segmenter.cpp + src/stopwatch.cpp + src/tracking_map.cpp ) target_include_directories(cdcpd PUBLIC $ diff --git a/cdcpd/include/cdcpd/cdcpd.h b/cdcpd/include/cdcpd/cdcpd.h index ac28b2a..787c935 100644 --- a/cdcpd/include/cdcpd/cdcpd.h +++ b/cdcpd/include/cdcpd/cdcpd.h @@ -29,6 +29,7 @@ #include "cdcpd/obs_util.h" #include "cdcpd/optimizer.h" #include "cdcpd/past_template_matcher.h" +#include "cdcpd/stopwatch.h" typedef pcl::PointXYZRGB PointRGB; typedef pcl::PointXYZHSV PointHSV; @@ -98,49 +99,56 @@ class CDCPD { OutputStatus status; }; - CDCPD(PointCloud::ConstPtr template_cloud, const Eigen::Matrix2Xi &_template_edges, float objective_value_threshold, - bool use_recovery = false, double alpha = 0.5, double beta = 1.0, double lambda = 1.0, double k = 100.0, - float zeta = 10.0, float obstacle_cost_weight = 1.0, float fixed_points_weight = 10.0); + CDCPD(PointCloud::ConstPtr template_cloud, const Eigen::Matrix2Xi &_template_edges, + float objective_value_threshold, bool use_recovery = false, double alpha = 0.5, + double beta = 1.0, double lambda = 1.0, double k = 100.0, float zeta = 10.0, + float obstacle_cost_weight = 1.0, float fixed_points_weight = 10.0); CDCPD(ros::NodeHandle nh, ros::NodeHandle ph, PointCloud::ConstPtr template_cloud, - const Eigen::Matrix2Xi &_template_edges, float objective_value_threshold, bool use_recovery = false, - double alpha = 0.5, double beta = 1.0, double lambda = 1.0, double k = 100.0, float zeta = 10.0, - float obstacle_cost_weight = 1.0, float fixed_points_weight = 10.0); - - // If you have want gripper constraints to be added & removed automatically based on is_grasped & distance - Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cv::Matx33d &intrinsics, - const PointCloud::Ptr template_cloud, ObstacleConstraints points_normals, double max_segment_length, - const smmap::AllGrippersSinglePoseDelta &q_dot = {}, - const smmap::AllGrippersSinglePose &q_config = {}, const std::vector &is_grasped = {}, - int pred_choice = 0); + const Eigen::Matrix2Xi &_template_edges, float objective_value_threshold, + bool use_recovery = false, double alpha = 0.5, double beta = 1.0, double lambda = 1.0, + double k = 100.0, float zeta = 10.0, float obstacle_cost_weight = 1.0, + float fixed_points_weight = 10.0); + + // If you have want gripper constraints to be added and removed automatically based on is_grasped + // and distance + Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, + const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud, + ObstacleConstraints points_normals, Eigen::RowVectorXd const max_segment_length, + const smmap::AllGrippersSinglePoseDelta &q_dot = {}, + const smmap::AllGrippersSinglePose &q_config = {}, const std::vector &is_grasped = {}, + int pred_choice = 0); // If you want to used a known correspondence between grippers and node indices (gripper_idx) - Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cv::Matx33d &intrinsics, - const PointCloud::Ptr template_cloud, ObstacleConstraints points_normals, double max_segment_length, - const smmap::AllGrippersSinglePoseDelta &q_dot = {}, - const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {}, - int pred_choice = 0); + Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, + const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud, + ObstacleConstraints points_normals, Eigen::RowVectorXd const max_segment_length, + const smmap::AllGrippersSinglePoseDelta &q_dot = {}, + const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {}, + int pred_choice = 0); Output operator()(const PointCloudRGB::Ptr &points, const PointCloud::Ptr template_cloud, - ObstacleConstraints points_normals, double max_segment_length, - const smmap::AllGrippersSinglePoseDelta &q_dot = {}, - const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {}, - int pred_choice = 0); + ObstacleConstraints points_normals, Eigen::RowVectorXd const max_segment_length, + const smmap::AllGrippersSinglePoseDelta &q_dot = {}, + const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {}, + int pred_choice = 0); // The common implementation that the above overloads call - Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cv::Matx33d &intrinsics, - const PointCloud::Ptr template_cloud, ObstacleConstraints points_normals, double max_segment_length, - const smmap::AllGrippersSinglePoseDelta &q_dot = {}, // TODO: this should be one data structure - const smmap::AllGrippersSinglePose &q_config = {}, int pred_choice = 0); + Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, + const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud, + ObstacleConstraints points_normals, Eigen::RowVectorXd const max_segment_length, + const smmap::AllGrippersSinglePoseDelta &q_dot = {}, // TODO: this should be one data structure + const smmap::AllGrippersSinglePose &q_config = {}, int pred_choice = 0); - Eigen::VectorXf visibility_prior(const Eigen::Matrix3Xf &vertices, const cv::Mat &depth, const cv::Mat &mask, - const Eigen::Matrix3f &intrinsics, float kvis); + Eigen::VectorXf visibility_prior(const Eigen::Matrix3Xf &vertices, const cv::Mat &depth, + const cv::Mat &mask, const Eigen::Matrix3f &intrinsics, float kvis); - Eigen::Matrix3Xf cpd(const Eigen::Matrix3Xf &X, const Eigen::Matrix3Xf &Y, const Eigen::Matrix3Xf &Y_pred, - const Eigen::VectorXf &Y_emit_prior); + Eigen::Matrix3Xf cpd(const Eigen::Matrix3Xf &X, const Eigen::Matrix3Xf &Y, + const Eigen::Matrix3Xf &Y_pred, const Eigen::VectorXf &Y_emit_prior); - Eigen::Matrix3Xd predict(const Eigen::Matrix3Xd &P, const smmap::AllGrippersSinglePoseDelta &q_dot, - const smmap::AllGrippersSinglePose &q_config, int pred_choice); + Eigen::Matrix3Xd predict(const Eigen::Matrix3Xd &P, + const smmap::AllGrippersSinglePoseDelta &q_dot, + const smmap::AllGrippersSinglePose &q_config, int pred_choice); ros::NodeHandle nh; ros::NodeHandle ph; @@ -158,7 +166,7 @@ class CDCPD { int lle_neighbors; Eigen::MatrixXf m_lle; Eigen::MatrixXf L_lle; - double tolerance; + double tolerance_cpd; double alpha; double beta; double w; diff --git a/cdcpd/include/cdcpd/cdcpd_node.h b/cdcpd/include/cdcpd/cdcpd_node.h index 0415c64..acf79b8 100644 --- a/cdcpd/include/cdcpd/cdcpd_node.h +++ b/cdcpd/include/cdcpd/cdcpd_node.h @@ -29,6 +29,7 @@ #include "cdcpd_ros/camera_sub.h" #include "cdcpd/deformable_object_configuration.h" #include "cdcpd/segmenter.h" +#include "cdcpd/tracking_map.h" typedef pcl::PointCloud PointCloud; namespace gm = geometry_msgs; @@ -139,7 +140,7 @@ struct CDCPD_Moveit_Node { unsigned int gripper_count; Eigen::MatrixXi gripper_indices; - std::unique_ptr deformable_object_configuration_; + TrackingMap deformable_objects; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/cdcpd/include/cdcpd/deformable_object_configuration.h b/cdcpd/include/cdcpd/deformable_object_configuration.h index b3ab0a9..119f966 100644 --- a/cdcpd/include/cdcpd/deformable_object_configuration.h +++ b/cdcpd/include/cdcpd/deformable_object_configuration.h @@ -8,6 +8,8 @@ #include "opencv2/imgproc.hpp" #include +#include + typedef pcl::PointCloud PointCloud; enum DeformableObjectType @@ -16,18 +18,35 @@ enum DeformableObjectType cloth }; -DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_str) +DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_str); + +// TODO(Dylan): Clean this up and actually implement a graph traversal/construction algorithm that +// isn't just dumping all of the nodes into a big list. +class ConnectivityNode { - std::map obj_type_map{ - {"rope", DeformableObjectType::rope}, - {"cloth", DeformableObjectType::cloth} - }; - return obj_type_map[def_obj_type_str]; -} - -struct DeformableObjectTracking +public: + ConnectivityNode(int const node_id); + + void add_neighbor_node(std::shared_ptr const neighbor); + + int const id; + std::map > neighbors; +}; + +// Represents the edges of deformable object templates as a graph for easy traversal. +class ConnectivityGraph +{ +public: + ConnectivityGraph(); + ConnectivityGraph(Eigen::Matrix2Xi const& edge_list); + + std::map > nodes; +}; + +class DeformableObjectTracking { - DeformableObjectTracking(){} +public: + DeformableObjectTracking(); // Turns the passed in Matrix of XYZ points to a point cloud. // TODO(dylan.colli): remove argument. Should just use the vertices member. @@ -39,12 +58,42 @@ struct DeformableObjectTracking // Sets the vertices from input point vector. void setVertices(std::vector const& vertex_points); - // Sets the edges from input edge vector. + // Sets the vertices member equal to given vertices and forms the point cloud from these + // vertices + void setVertices(Eigen::Matrix3Xf const& vertices_in); + + // Sets the edges from input edge vector and updates the connectivity graph describing edges. void setEdges(std::vector> const& edges); - Eigen::Matrix3Xf vertices_{}; - Eigen::Matrix2Xi edges_{}; + // Sets the edges and updates the connectivity graph describing edges. + void setEdges(Eigen::Matrix2Xi const& edges_in); + + // Sets this tracking's point cloud using std::copy and another point cloud's iterators defining + // the range over which to copy the points. + void setPointCloud(PointCloud::const_iterator const& it_in_begin, + PointCloud::const_iterator const& it_in_end); + + Eigen::Matrix3Xf const& getVertices() const { return vertices_; } + + Eigen::Matrix3Xf getVerticesCopy() const { return vertices_; } + + Eigen::Matrix2Xi const& getEdges() const { return edges_; } + + Eigen::Matrix2Xi getEdgesCopy() const { return edges_; } + + PointCloud::ConstPtr const getPointCloud() const { return points_; } + + PointCloud::Ptr getPointCloudCopy() const { return PointCloud::Ptr(new PointCloud(*points_)); } + + ConnectivityGraph const& getConnectivityGraph() const { return connectivity_graph_; } + + ConnectivityGraph getConnectivityGraphCopy() const { return connectivity_graph_; } + +private: + Eigen::Matrix3Xf vertices_; + Eigen::Matrix2Xi edges_; PointCloud::Ptr points_; + ConnectivityGraph connectivity_graph_; }; class DeformableObjectConfiguration @@ -63,8 +112,8 @@ class DeformableObjectConfiguration int num_points_; float max_segment_length_; - DeformableObjectTracking tracked_; DeformableObjectTracking initial_; + DeformableObjectTracking tracked_; }; class RopeConfiguration : public DeformableObjectConfiguration @@ -98,6 +147,15 @@ class ClothConfiguration : public DeformableObjectConfiguration 0, 0, 1, 0, 0, 0, 0, 1); + // Initial length of the template. + float const length_initial_; + + // Initial width of the template. + float const width_initial_; + + // The supplied initial guess for the grid size. + float const grid_size_initial_guess_; + // Describes the number of points the template is initialized with in the length direction. // Length is downwards direction! int const num_points_length_; @@ -107,13 +165,4 @@ class ClothConfiguration : public DeformableObjectConfiguration int const num_points_width_; int const num_edges_; - - // Initial length of the template. - float const length_initial_; - - // Initial width of the template. - float const width_initial_; - - // The supplied initial guess for the grid size. - float const grid_size_initial_guess_; }; \ No newline at end of file diff --git a/cdcpd/include/cdcpd/optimizer.h b/cdcpd/include/cdcpd/optimizer.h index 0ae12a9..cbda984 100644 --- a/cdcpd/include/cdcpd/optimizer.h +++ b/cdcpd/include/cdcpd/optimizer.h @@ -53,40 +53,39 @@ using ObstacleConstraints = std::vector; class Optimizer { public: - Optimizer(const Eigen::Matrix3Xf initial_template, const Eigen::Matrix3Xf last_template, float stretch_lambda, - float obstacle_cost_weight, float fixed_points_weight); + Optimizer(const Eigen::Matrix3Xf initial_template, const Eigen::Matrix3Xf last_template, + float stretch_lambda, float obstacle_cost_weight, float fixed_points_weight); - [[nodiscard]] std::pair operator()(const Eigen::Matrix3Xf &Y, const Eigen::Matrix2Xi &E, - const std::vector &fixed_points, - ObstacleConstraints const &points_normals, - double max_segment_length); + [[nodiscard]] std::pair operator()(const Eigen::Matrix3Xf &Y, + const Eigen::Matrix2Xi &E, const std::vector &fixed_points, + ObstacleConstraints const &points_normals, Eigen::RowVectorXd const max_segment_length); - std::tuple test_box(const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &box, - geometry_msgs::Pose const &pose); + std::tuple test_box(const Eigen::Matrix3Xf &last_template, + shape_msgs::SolidPrimitive const &box, geometry_msgs::Pose const &pose); private: [[nodiscard]] bool gripper_constraints_satisfiable(const std::vector &fixed_points) const; - [[nodiscard]] std::tuple nearest_points_and_normal_box(const Eigen::Matrix3Xf &last_template, - shape_msgs::SolidPrimitive const &box, - geometry_msgs::Pose const &pose); + [[nodiscard]] std::tuple nearest_points_and_normal_box( + const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &box, + geometry_msgs::Pose const &pose); - [[nodiscard]] std::tuple nearest_points_and_normal_sphere(const Eigen::Matrix3Xf &last_template, - shape_msgs::SolidPrimitive const &sphere, - geometry_msgs::Pose const &pose); + [[nodiscard]] std::tuple nearest_points_and_normal_sphere( + const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &sphere, + geometry_msgs::Pose const &pose); - [[nodiscard]] std::tuple nearest_points_and_normal_plane(const Eigen::Matrix3Xf &last_template, - shape_msgs::Plane const &plane); + [[nodiscard]] std::tuple nearest_points_and_normal_plane( + const Eigen::Matrix3Xf &last_template, shape_msgs::Plane const &plane); [[nodiscard]] std::tuple nearest_points_and_normal_cylinder( const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &cylinder, geometry_msgs::Pose const &pose); - [[nodiscard]] std::tuple nearest_points_and_normal_mesh(const Eigen::Matrix3Xf &last_template, - shape_msgs::Mesh const &shapes_mesh); + [[nodiscard]] std::tuple nearest_points_and_normal_mesh( + const Eigen::Matrix3Xf &last_template, shape_msgs::Mesh const &shapes_mesh); - [[nodiscard]] std::tuple nearest_points_and_normal(const Eigen::Matrix3Xf &last_template, - Objects const &objects); + [[nodiscard]] std::tuple nearest_points_and_normal( + const Eigen::Matrix3Xf &last_template, Objects const &objects); Eigen::Matrix3Xf initial_template_; Eigen::Matrix3Xf last_template_; diff --git a/cdcpd/include/cdcpd/stopwatch.h b/cdcpd/include/cdcpd/stopwatch.h new file mode 100644 index 0000000..1fb5e7b --- /dev/null +++ b/cdcpd/include/cdcpd/stopwatch.h @@ -0,0 +1,20 @@ +#pragma once + +#include +#include + +#include + +static double const NANOSECONDS_TO_MILLISECONDS = 1e-6; + +// "Context manager" for timing and printing runtime to ROS debug logger. +class Stopwatch +{ +public: + Stopwatch(std::string const& routine_name); + ~Stopwatch(); + +private: + std::string const routine_name_; + std::chrono::steady_clock::time_point const start_time_; +}; \ No newline at end of file diff --git a/cdcpd/include/cdcpd/tracking_map.h b/cdcpd/include/cdcpd/tracking_map.h new file mode 100644 index 0000000..26420fb --- /dev/null +++ b/cdcpd/include/cdcpd/tracking_map.h @@ -0,0 +1,57 @@ +#pragma once + +#include + +#include +#include +#include + +#include "cdcpd/deformable_object_configuration.h" + +class TrackingMap +{ +public: + TrackingMap(); + + // Returns the total number of tracked points. + int get_total_num_points() const; + + // Returns the total number of tracked edges. + int get_total_num_edges() const; + + // Returns a map that indicates which vertices belong to which template. + // The tuple indicates the start and end vertex indexes that belong to that template. + std::map > get_vertex_assignments() const; + + // Adds the given deformable object configuration to our tracking map. + void add_def_obj_configuration(std::shared_ptr const def_obj_config); + + // Updates the vertices for all deformable objects given new points predicted from a CDCPD run. + void update_def_obj_vertices(pcl::shared_ptr const vertices_new); + + // Forms the vertices matrix for all tracked templates expected by CDCPD + PointCloud::Ptr form_vertices_cloud(bool const use_initial_state=false) const; + + // Forms the edges matrix for all tracked templates expected by CDCPD + Eigen::Matrix2Xi form_edges_matrix(bool const use_initial_state=false) const; + + // Returns the matrix describing the maximum length each edge can have. + Eigen::RowVectorXd form_max_segment_length_matrix() const; + + // The map that holds the deformable objects we're tracking. + std::map > tracking_map; + +protected: + // The next ID we'll assign to an incoming deformable object configuration to track. + int deformable_object_id_next_; + + // Holds the IDs of the tracked objects in a way that preserves order as the map does not. + // This is necessary for formation of the vertex, edge, and max segment length matrices. + std::vector ordered_def_obj_ids_; + + // Return the appropriate DeformableObjectTracking given if we should take the initial or + // tracked states. + std::shared_ptr get_appropriate_tracking( + std::shared_ptr const def_obj_config, + bool take_initial_state) const; +}; \ No newline at end of file diff --git a/cdcpd/launch/azure_2_rope.launch b/cdcpd/launch/azure_2_rope.launch new file mode 100644 index 0000000..1124bfe --- /dev/null +++ b/cdcpd/launch/azure_2_rope.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/cdcpd/rviz/azure_2.rviz b/cdcpd/rviz/azure_2.rviz index 3af3a99..924f36c 100644 --- a/cdcpd/rviz/azure_2.rviz +++ b/cdcpd/rviz/azure_2.rviz @@ -11,9 +11,9 @@ Panels: - /vision1/kinect1/Occlusion Compensation1 - /cdcpd1 - /cdcpd1/BoundingBox1 - - /cdcpd1/output1/Namespaces1 - /cdcpd1/masked1 - /cdcpd1/contacts1/Namespaces1 + - /cdcpd1/MarkerArray1 - /PlanningScene1 Splitter Ratio: 0.40697672963142395 Tree Height: 837 @@ -35,7 +35,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: masked - Class: merrrt_visualization/AnimationController Name: AnimationController auto_play: true @@ -141,14 +141,6 @@ Visualization Manager: line width: 0.004999999888241291 only edge: true show coords: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /cdcpd/order - Name: output - Namespaces: - {} - Queue Size: 100 - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -259,6 +251,14 @@ Visualization Manager: {} Queue Size: 100 Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /cdcpd/order + Name: MarkerArray + Namespaces: + line_order: true + Queue Size: 100 + Value: true Enabled: true Name: cdcpd - Class: moveit_rviz_plugin/PlanningScene @@ -282,11 +282,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - mock_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true @@ -335,9 +330,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -1.5697963237762451 + Pitch: -0.9397965669631958 Target Frame: anim_camera - Yaw: 4.693721294403076 + Yaw: 4.798727035522461 Saved: ~ Window Geometry: AnimationController: diff --git a/cdcpd/rviz/kinect_tripodB.rviz b/cdcpd/rviz/kinect_tripodB.rviz index 2c08ab1..fa2472f 100644 --- a/cdcpd/rviz/kinect_tripodB.rviz +++ b/cdcpd/rviz/kinect_tripodB.rviz @@ -135,10 +135,10 @@ Visualization Manager: line width: 0.004999999888241291 only edge: true show coords: false - - Class: rviz/Marker + - Class: rviz/MarkerArray Enabled: true Marker Topic: /cdcpd/order - Name: output + Name: MarkerArray Namespaces: line_order: true Queue Size: 100 diff --git a/cdcpd/rviz/realsense.rviz b/cdcpd/rviz/realsense.rviz index 6b61054..701d88f 100644 --- a/cdcpd/rviz/realsense.rviz +++ b/cdcpd/rviz/realsense.rviz @@ -129,10 +129,10 @@ Visualization Manager: line width: 0.004999999888241291 only edge: true show coords: false - - Class: rviz/Marker + - Class: rviz/MarkerArray Enabled: true Marker Topic: /cdcpd/order - Name: output + Name: MarkerArray Namespaces: line_order: true Queue Size: 100 diff --git a/cdcpd/src/cdcpd.cpp b/cdcpd/src/cdcpd.cpp index e5b3a72..58785ee 100644 --- a/cdcpd/src/cdcpd.cpp +++ b/cdcpd/src/cdcpd.cpp @@ -43,7 +43,8 @@ using Eigen::VectorXd; using Eigen::VectorXf; using Eigen::VectorXi; -class CompHSV : public pcl::ComparisonBase { +class CompHSV : public pcl::ComparisonBase +{ using ComparisonBase::capable_; using ComparisonBase::op_; @@ -56,7 +57,6 @@ class CompHSV : public pcl::ComparisonBase { CompHSV(const std::string &component_name, pcl::ComparisonOps::CompareOp op, double compare_val) : component_offset_(), compare_val_(compare_val) - { // Verify the component name if (component_name == "h") { @@ -107,7 +107,8 @@ class CompHSV : public pcl::ComparisonBase { CompHSV() : component_offset_(), compare_val_() {} // not allowed }; -static PointCloud::Ptr mat_to_cloud(const Eigen::Matrix3Xf &mat) { +static PointCloud::Ptr mat_to_cloud(const Eigen::Matrix3Xf &mat) +{ PointCloud::Ptr cloud(new PointCloud); cloud->points.reserve(mat.cols()); for (ssize_t i = 0; i < mat.cols(); ++i) { @@ -116,7 +117,8 @@ static PointCloud::Ptr mat_to_cloud(const Eigen::Matrix3Xf &mat) { return cloud; } -static double initial_sigma2(const MatrixXf &X, const MatrixXf &Y) { +static double initial_sigma2(const MatrixXf &X, const MatrixXf &Y) +{ // X: (3, N) matrix, X^t in Algorithm 1 // Y: (3, M) matrix, Y^(t-1) in Algorithm 1 // Implement Line 2 of Algorithm 1 @@ -130,7 +132,8 @@ static double initial_sigma2(const MatrixXf &X, const MatrixXf &Y) { return total_error / (X.cols() * Y.cols() * X.rows()); } -static MatrixXf gaussian_kernel(const MatrixXf &Y, double beta) { +static MatrixXf gaussian_kernel(const MatrixXf &Y, double beta) +{ // Y: (3, M) matrix, corresponding to Y^(t-1) in Eq. 13.5 (Y^t in VI.A) // beta: beta in Eq. 13.5 (between 13 and 14) MatrixXf diff(Y.cols(), Y.cols()); @@ -145,7 +148,9 @@ static MatrixXf gaussian_kernel(const MatrixXf &Y, double beta) { return kernel; } -MatrixXf barycenter_kneighbors_graph(const pcl::KdTreeFLANN &kdtree, int lle_neighbors, double reg) { +MatrixXf barycenter_kneighbors_graph(const pcl::KdTreeFLANN &kdtree, + int lle_neighbors, double reg) +{ // calculate L in Eq. (15) and Eq. (16) // ENHANCE: use tapkee lib to accelarate // kdtree: kdtree from Y^0 @@ -190,7 +195,9 @@ MatrixXf barycenter_kneighbors_graph(const pcl::KdTreeFLANN &kdtr return graph; } -MatrixXf locally_linear_embedding(PointCloud::ConstPtr template_cloud, int lle_neighbors, double reg) { +MatrixXf locally_linear_embedding(PointCloud::ConstPtr template_cloud, int lle_neighbors, + double reg) +{ // calculate H in Eq. (18) // template_cloud: Y^0 in Eq. (15) and (16) // lle_neighbors: parameter for lle calculation @@ -223,7 +230,8 @@ CDCPD_Parameters::CDCPD_Parameters(ros::NodeHandle& ph) * Implement Eq. (7) in the paper */ VectorXf CDCPD::visibility_prior(const Matrix3Xf &vertices, const Mat &depth, const Mat &mask, - const Matrix3f &intrinsics, const float kvis) { + const Matrix3f &intrinsics, const float kvis) +{ // vertices: (3, M) matrix Y^t (Y in IV.A) in the paper // depth: CV_16U depth image // mask: CV_8U mask for segmentation @@ -288,8 +296,10 @@ VectorXf CDCPD::visibility_prior(const Matrix3Xf &vertices, const Mat &depth, co // https://github.com/ros-perception/image_pipeline/blob/melodic/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp // we expect that cx, cy, fx, fy are in the appropriate places in P static std::tuple point_clouds_from_images( - const cv::Mat &depth_image, const cv::Mat &rgb_image, const cv::Mat &mask, const Eigen::Matrix3f &intrinsics, - const Eigen::Vector3f &lower_bounding_box, const Eigen::Vector3f &upper_bounding_box) { + const cv::Mat &depth_image, const cv::Mat &rgb_image, const cv::Mat &mask, + const Eigen::Matrix3f &intrinsics, const Eigen::Vector3f &lower_bounding_box, + const Eigen::Vector3f &upper_bounding_box) +{ // depth_image: CV_16U depth image // rgb_image: CV_8U3C rgb image // mask: CV_8U mask for segmentation @@ -353,7 +363,8 @@ static std::tuple point_clouds_from_images( } Matrix3Xf CDCPD::cpd(const Matrix3Xf &X, const Matrix3Xf &Y, const Matrix3Xf &Y_pred, - const Eigen::VectorXf &Y_emit_prior) { + const Eigen::VectorXf &Y_emit_prior) +{ // downsampled_cloud: PointXYZ pointer to downsampled point clouds // Y: (3, M) matrix Y^t (Y in IV.A) in the paper // depth: CV_16U depth image @@ -361,6 +372,7 @@ Matrix3Xf CDCPD::cpd(const Matrix3Xf &X, const Matrix3Xf &Y, const Matrix3Xf &Y_ // Y_emit_prior: vector with a probability per tracked point /// CPD step + std::string const logname_cpd = LOGNAME + ".cpd"; // G: (M, M) Guassian kernel matrix MatrixXf G = gaussian_kernel(original_template, beta); // Y, beta); @@ -370,12 +382,12 @@ Matrix3Xf CDCPD::cpd(const Matrix3Xf &X, const Matrix3Xf &Y, const Matrix3Xf &Y_ double sigma2 = initial_sigma2(X, TY) * initial_sigma_scale; int iterations = 0; - double error = tolerance + 1; // loop runs the first time + double error = tolerance_cpd + 1; // loop runs the first time - std::chrono::time_point start, end; - start = std::chrono::system_clock::now(); + { + Stopwatch stopwatch(logname_cpd); - while (iterations <= max_iterations && error > tolerance) { + while (iterations <= max_iterations && error > tolerance_cpd) { double qprev = sigma2; // Expectation step int N = X.cols(); @@ -440,20 +452,23 @@ Matrix3Xf CDCPD::cpd(const Matrix3Xf &X, const Matrix3Xf &Y, const Matrix3Xf &Y_ sigma2 = (xPx - 2 * trPXY + yPy) / (Np * static_cast(D)); if (sigma2 <= 0) { - sigma2 = tolerance / 10; + sigma2 = tolerance_cpd / 10; } error = std::abs(sigma2 - qprev); iterations++; } + } - ROS_DEBUG_STREAM_NAMED(LOGNAME + ".cpd", "cpd error: " << error << " itr: " << iterations); + ROS_DEBUG_STREAM_NAMED(logname_cpd, "cpd error: " << error << " itr: " << iterations); + ROS_DEBUG_STREAM_NAMED(logname_cpd, "cpd std dev: " << std::pow(sigma2, 0.5)); return TY; } Matrix3Xd CDCPD::predict(const Matrix3Xd &P, const smmap::AllGrippersSinglePoseDelta &q_dot, - const smmap::AllGrippersSinglePose &q_config, const int pred_choice) { + const smmap::AllGrippersSinglePose &q_config, const int pred_choice) +{ if (pred_choice == 0) { return P; } else { @@ -470,16 +485,19 @@ Matrix3Xd CDCPD::predict(const Matrix3Xd &P, const smmap::AllGrippersSinglePoseD // This is for the case where the gripper indices are unknown (in real experiment) CDCPD::CDCPD(PointCloud::ConstPtr template_cloud, // this needs a different data-type for python - const Matrix2Xi &template_edges, const float objective_value_threshold, const bool use_recovery, - const double alpha, const double beta, const double lambda, const double k, const float zeta, - const float obstacle_cost_weight, const float fixed_points_weight) - : CDCPD(ros::NodeHandle(), ros::NodeHandle("~"), template_cloud, template_edges, objective_value_threshold, - use_recovery, alpha, beta, lambda, k, zeta, obstacle_cost_weight, fixed_points_weight) {} + const Matrix2Xi &template_edges, const float objective_value_threshold, const bool use_recovery, + const double alpha, const double beta, const double lambda, const double k, const float zeta, + const float obstacle_cost_weight, const float fixed_points_weight) + : CDCPD(ros::NodeHandle(), ros::NodeHandle("~"), template_cloud, template_edges, + objective_value_threshold, use_recovery, alpha, beta, lambda, k, zeta, obstacle_cost_weight, + fixed_points_weight) +{} CDCPD::CDCPD(ros::NodeHandle nh, ros::NodeHandle ph, PointCloud::ConstPtr template_cloud, - const Matrix2Xi &_template_edges, const float objective_value_threshold, const bool use_recovery, - const double alpha, const double beta, const double lambda, const double k, const float zeta, - const float obstacle_cost_weight, const float fixed_points_weight) + const Matrix2Xi &_template_edges, const float objective_value_threshold, + const bool use_recovery, const double alpha, const double beta, const double lambda, + const double k, const float zeta, const float obstacle_cost_weight, + const float fixed_points_weight) : nh(nh), ph(ph), original_template(template_cloud->getMatrixXfMap().topRows(3)), @@ -488,7 +506,7 @@ CDCPD::CDCPD(ros::NodeHandle nh, ros::NodeHandle ph, PointCloud::ConstPtr templa last_upper_bounding_box(original_template.rowwise().maxCoeff()), // TODO make configurable? lle_neighbors(8), // TODO make configurable? m_lle(locally_linear_embedding(template_cloud, lle_neighbors, 1e-3)), // TODO make configurable? - tolerance(1e-4), // TODO make configurable? + tolerance_cpd(1e-4), // TODO make configurable? alpha(alpha), // TODO make configurable? beta(beta), // TODO make configurable? w(0.1), // TODO make configurable? @@ -502,7 +520,8 @@ CDCPD::CDCPD(ros::NodeHandle nh, ros::NodeHandle ph, PointCloud::ConstPtr templa fixed_points_weight(fixed_points_weight), use_recovery(use_recovery), last_grasp_status({false, false}), - objective_value_threshold_(objective_value_threshold) { + objective_value_threshold_(objective_value_threshold) +{ last_lower_bounding_box = last_lower_bounding_box - bounding_box_extend; last_upper_bounding_box = last_upper_bounding_box + bounding_box_extend; @@ -512,11 +531,12 @@ CDCPD::CDCPD(ros::NodeHandle nh, ros::NodeHandle ph, PointCloud::ConstPtr templa L_lle = barycenter_kneighbors_graph(kdtree, lle_neighbors, 0.001); } -CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mask, const cv::Matx33d &intrinsics, - const PointCloud::Ptr template_cloud, ObstacleConstraints obstacle_constraints, - const double max_segment_length, const smmap::AllGrippersSinglePoseDelta &q_dot, - const smmap::AllGrippersSinglePose &q_config, const std::vector &is_grasped, - const int pred_choice) { +CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mask, + const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud, + ObstacleConstraints obstacle_constraints, Eigen::RowVectorXd const max_segment_length, + const smmap::AllGrippersSinglePoseDelta &q_dot, const smmap::AllGrippersSinglePose &q_config, + const std::vector &is_grasped, const int pred_choice) +{ std::vector idx_map; for (auto const &[j, is_grasped_j] : enumerate(is_grasped)) { if (j < q_config.size() and j < q_dot.size()) { @@ -576,11 +596,12 @@ CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mas } // NOTE: this is the one I'm current using for rgb + depth -CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mask, const cv::Matx33d &intrinsics, - const PointCloud::Ptr template_cloud, ObstacleConstraints obstacle_constraints, - const double max_segment_length, const smmap::AllGrippersSinglePoseDelta &q_dot, - const smmap::AllGrippersSinglePose &q_config, const Eigen::MatrixXi &gripper_idx, - const int pred_choice) { +CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mask, + const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud, + ObstacleConstraints obstacle_constraints, Eigen::RowVectorXd const max_segment_length, + const smmap::AllGrippersSinglePoseDelta &q_dot, const smmap::AllGrippersSinglePose &q_config, + const Eigen::MatrixXi &gripper_idx, const int pred_choice) +{ this->gripper_idx = gripper_idx; auto const cdcpd_out = operator()(rgb, depth, mask, intrinsics, template_cloud, obstacle_constraints, max_segment_length, q_dot, q_config, pred_choice); @@ -588,11 +609,13 @@ CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mas } // NOTE: for point cloud inputs -CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, const PointCloud::Ptr template_cloud, - ObstacleConstraints obstacle_constraints, const double max_segment_length, - const smmap::AllGrippersSinglePoseDelta &q_dot, - const smmap::AllGrippersSinglePose &q_config, const Eigen::MatrixXi &gripper_idx, - const int pred_choice) { +CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, + const PointCloud::Ptr template_cloud, ObstacleConstraints obstacle_constraints, + Eigen::RowVectorXd const max_segment_length, const smmap::AllGrippersSinglePoseDelta &q_dot, + const smmap::AllGrippersSinglePose &q_config, const Eigen::MatrixXi &gripper_idx, + const int pred_choice) +{ + Stopwatch stopwatch_cdcpd("CDCPD"); // FIXME: this has a lot of duplicate code this->gripper_idx = gripper_idx; @@ -601,31 +624,36 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, const PointClo total_frames_ += 1; // Perform HSV segmentation - auto segmenter = std::make_unique(ph, last_lower_bounding_box, - last_upper_bounding_box); - segmenter->segment(points); - - // Drop color info from the point cloud. - // NOTE: We use a boost pointer here because that's what our version of pcl is expecting in - // the `setInputCloud` function call. - auto cloud = boost::make_shared(); - pcl::copyPointCloud(segmenter->get_segmented_cloud(), *cloud); - - ROS_INFO_STREAM_THROTTLE_NAMED(1, LOGNAME + ".points", - "filtered cloud: (" << cloud->height << " x " << cloud->width << ")"); - - /// Perform VoxelGrid filter downsampling. + boost::shared_ptr cloud_segmented; PointCloud::Ptr cloud_downsampled(new PointCloud); - pcl::VoxelGrid sor; - ROS_DEBUG_STREAM_THROTTLE_NAMED(1, LOGNAME + ".points", "Points in cloud before leaf: " - << cloud->width); - sor.setInputCloud(cloud); - sor.setLeafSize(0.02f, 0.02f, 0.02f); - sor.filter(*cloud_downsampled); - ROS_INFO_STREAM_THROTTLE_NAMED(1, LOGNAME + ".points", - "Points in filtered point cloud: " << cloud_downsampled->width); + { + Stopwatch stopwatch_segmentation("Segmentation"); + auto segmenter = std::make_unique(ph, last_lower_bounding_box, + last_upper_bounding_box); + segmenter->segment(points); + + // Drop color info from the point cloud. + // NOTE: We use a boost pointer here because that's what our version of pcl is expecting in + // the `setInputCloud` function call. + cloud_segmented = boost::make_shared(); + pcl::copyPointCloud(segmenter->get_segmented_cloud(), *cloud_segmented); + + ROS_INFO_STREAM_THROTTLE_NAMED(1, LOGNAME + ".points", + "filtered cloud: (" << cloud_segmented->height << " x " << cloud_segmented->width << ")"); + + /// Perform VoxelGrid filter downsampling. + pcl::VoxelGrid sor; + ROS_DEBUG_STREAM_THROTTLE_NAMED(1, LOGNAME + ".points", "Points in cloud before leaf: " + << cloud_segmented->width); + sor.setInputCloud(cloud_segmented); + sor.setLeafSize(0.02f, 0.02f, 0.02f); + sor.filter(*cloud_downsampled); + ROS_INFO_STREAM_THROTTLE_NAMED(1, LOGNAME + ".points", + "Points in filtered point cloud: " << cloud_downsampled->width); + } const Matrix3Xf Y = template_cloud->getMatrixXfMap().topRows(3); + // TODO: check whether the change is needed here for unit conversion auto const Y_emit_prior = Eigen::VectorXf::Ones(template_cloud->size()); ROS_DEBUG_STREAM_NAMED(LOGNAME, "Y_emit_prior " << Y_emit_prior); @@ -636,8 +664,8 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, const PointClo PointCloud::Ptr cdcpd_cpd = mat_to_cloud(Y); PointCloud::Ptr cdcpd_pred = mat_to_cloud(Y); - return CDCPD::Output{points, cloud, cloud_downsampled, cdcpd_cpd, cdcpd_pred, cdcpd_out, - OutputStatus::NoPointInFilteredCloud}; + return CDCPD::Output{points, cloud_segmented, cloud_downsampled, cdcpd_cpd, cdcpd_pred, + cdcpd_out, OutputStatus::NoPointInFilteredCloud}; } // Add points to X according to the previous template @@ -656,20 +684,29 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, const PointClo } Matrix3Xf TY, TY_pred; - TY_pred = predict(Y.cast(), q_dot, q_config, pred_choice).cast(); - TY = cpd(X, Y, TY_pred, Y_emit_prior); + { + Stopwatch stopwatch_cpd("CPD"); + TY_pred = predict(Y.cast(), q_dot, q_config, pred_choice).cast(); + TY = cpd(X, Y, TY_pred, Y_emit_prior); + } // Next step: optimization. ROS_DEBUG_STREAM_NAMED(LOGNAME, "fixed points" << pred_fixed_points); - // NOTE: seems like this should be a function, not a class? is there state like the gurobi env? - // ???: most likely not 1.0 - Optimizer opt(original_template, Y, start_lambda, obstacle_cost_weight, fixed_points_weight); - auto const opt_out = opt(TY, template_edges, pred_fixed_points, obstacle_constraints, - max_segment_length); - Matrix3Xf Y_opt = opt_out.first; - double objective_value = opt_out.second; + Matrix3Xf Y_opt; + double objective_value; + { + Stopwatch stopwatch_optimization("Optimization"); - ROS_DEBUG_STREAM_NAMED(LOGNAME + ".objective", "objective: " << objective_value); + // NOTE: seems like this should be a function, not a class? is there state like the gurobi env? + // ???: most likely not 1.0 + Optimizer opt(original_template, Y, start_lambda, obstacle_cost_weight, fixed_points_weight); + auto const opt_out = opt(TY, template_edges, pred_fixed_points, obstacle_constraints, + max_segment_length); + Y_opt = opt_out.first; + objective_value = opt_out.second; + + ROS_DEBUG_STREAM_NAMED(LOGNAME + ".objective", "objective: " << objective_value); + } // Set stateful member variables for next iteration of CDCPD last_lower_bounding_box = Y_opt.rowwise().minCoeff(); @@ -685,13 +722,15 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, const PointClo status = OutputStatus::ObjectiveTooHigh; } - return CDCPD::Output{points, cloud, cloud_downsampled, cdcpd_cpd, cdcpd_pred, cdcpd_out, status}; + return CDCPD::Output{points, cloud_segmented, cloud_downsampled, cdcpd_cpd, cdcpd_pred, cdcpd_out, status}; } -CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mask, const cv::Matx33d &intrinsics, - const PointCloud::Ptr template_cloud, ObstacleConstraints obstacle_constraints, - const double max_segment_length, const smmap::AllGrippersSinglePoseDelta &q_dot, - const smmap::AllGrippersSinglePose &q_config, const int pred_choice) { +CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mask, + const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud, + ObstacleConstraints obstacle_constraints, Eigen::RowVectorXd const max_segment_length, + const smmap::AllGrippersSinglePoseDelta &q_dot, const smmap::AllGrippersSinglePose &q_config, + const int pred_choice) +{ // rgb: CV_8U3C rgb image // depth: CV_16U depth image // mask: CV_8U mask for segmentation @@ -768,7 +807,8 @@ CDCPD::Output CDCPD::operator()(const Mat &rgb, const Mat &depth, const Mat &mas // NOTE: seems like this should be a function, not a class? is there state like the gurobi env? // ???: most likely not 1.0 Optimizer opt(original_template, Y, start_lambda, obstacle_cost_weight, fixed_points_weight); - auto const opt_out = opt(TY, template_edges, pred_fixed_points, obstacle_constraints, max_segment_length); + auto const opt_out = + opt(TY, template_edges, pred_fixed_points, obstacle_constraints, max_segment_length); Matrix3Xf Y_opt = opt_out.first; double objective_value = opt_out.second; diff --git a/cdcpd/src/cdcpd_node.cpp b/cdcpd/src/cdcpd_node.cpp index 148b9b0..937b706 100644 --- a/cdcpd/src/cdcpd_node.cpp +++ b/cdcpd/src/cdcpd_node.cpp @@ -63,7 +63,7 @@ CDCPD_Publishers::CDCPD_Publishers(ros::NodeHandle& nh, ros::NodeHandle& ph) template_publisher = nh.advertise("cdcpd/template", 10); pre_template_publisher = nh.advertise("cdcpd/pre_template", 10); output_publisher = nh.advertise("cdcpd/output", 10); - order_pub = nh.advertise("cdcpd/order", 10); + order_pub = nh.advertise("cdcpd/order", 10); contact_marker_pub = ph.advertise("contacts", 10); bbox_pub = ph.advertise("bbox", 10); } @@ -103,6 +103,7 @@ CDCPD_Moveit_Node::CDCPD_Moveit_Node(std::string const& robot_namespace) model_loader_(std::make_shared(robot_description_)), model_(model_loader_->getModel()), visual_tools_("robot_root", "cdcpd_moveit_node", scene_monitor_), + deformable_objects(), tf_listener_(tf_buffer_) { auto const scene_topic = ros::names::append(robot_namespace, @@ -136,26 +137,26 @@ CDCPD_Moveit_Node::CDCPD_Moveit_Node(std::string const& robot_namespace) // initialize start and end points for rope auto [init_q_config, init_q_dot] = get_q_config(); - Eigen::Vector3f start_position{Eigen::Vector3f::Zero()}; - Eigen::Vector3f end_position{Eigen::Vector3f::Zero()}; - end_position[2] += node_params.max_rope_length; + Eigen::Vector3f start_position_1{Eigen::Vector3f::Zero()}; + Eigen::Vector3f end_position_1{Eigen::Vector3f::Zero()}; + end_position_1[2] += node_params.max_rope_length; if (gripper_count == 2u) { - start_position = init_q_config[0].translation().cast(); - end_position = init_q_config[1].translation().cast(); + start_position_1 = init_q_config[0].translation().cast(); + end_position_1 = init_q_config[1].translation().cast(); } else if (gripper_count == 1u) { - start_position = init_q_config[0].translation().cast(); - end_position = start_position; - end_position[1] += node_params.max_rope_length; + start_position_1 = init_q_config[0].translation().cast(); + end_position_1 = start_position_1; + end_position_1[1] += node_params.max_rope_length; } else if (gripper_count == 0u) { - start_position << -node_params.max_rope_length / 2, 0, 1.0; - end_position << node_params.max_rope_length / 2, 0, 1.0; + start_position_1 << -node_params.max_rope_length / 2, 0, 1.0; + end_position_1 << node_params.max_rope_length / 2, 0, 1.0; } - initialize_deformable_object_configuration(start_position, end_position); + initialize_deformable_object_configuration(start_position_1, end_position_1); - cdcpd = std::make_unique(nh, ph, - deformable_object_configuration_->initial_.points_, - deformable_object_configuration_->initial_.edges_, + PointCloud::Ptr vertices = deformable_objects.form_vertices_cloud(); + Eigen::Matrix2Xi edges = deformable_objects.form_edges_matrix(); + cdcpd = std::make_unique(nh, ph, vertices, edges, cdcpd_params.objective_value_threshold, cdcpd_params.use_recovery, cdcpd_params.alpha, cdcpd_params.beta, cdcpd_params.lambda, cdcpd_params.k_spring, cdcpd_params.zeta, cdcpd_params.obstacle_cost_weight, cdcpd_params.fixed_points_weight); @@ -191,19 +192,20 @@ CDCPD_Moveit_Node::CDCPD_Moveit_Node(std::string const& robot_namespace) void CDCPD_Moveit_Node::initialize_deformable_object_configuration( Eigen::Vector3f const& rope_start_position, Eigen::Vector3f const& rope_end_position) { + std::shared_ptr deformable_object_configuration; if (node_params.deformable_object_type == DeformableObjectType::rope) { - auto configuration = std::unique_ptr(new RopeConfiguration( + auto configuration = std::shared_ptr(new RopeConfiguration( node_params.num_points, node_params.max_rope_length, rope_start_position, rope_end_position)); // Have to call initializeTracking() before casting to base class since it relies on virtual // functions. configuration->initializeTracking(); - deformable_object_configuration_ = std::move(configuration); + deformable_object_configuration = configuration; } else if (node_params.deformable_object_type == DeformableObjectType::cloth) { - auto configuration = std::unique_ptr(new ClothConfiguration( + auto configuration = std::shared_ptr(new ClothConfiguration( node_params.length_initial_cloth, node_params.width_initial_cloth, node_params.grid_size_initial_guess_cloth)); @@ -218,8 +220,11 @@ void CDCPD_Moveit_Node::initialize_deformable_object_configuration( // Have to call initializeTracking() before casting to base class since it relies on virtual // functions. configuration->initializeTracking(); - deformable_object_configuration_ = std::move(configuration); + deformable_object_configuration = configuration; } + + // Add the initialized configuration to our tracking map. + deformable_objects.add_def_obj_configuration(deformable_object_configuration); } ObstacleConstraints CDCPD_Moveit_Node::find_nearest_points_and_normals( @@ -445,7 +450,7 @@ ObstacleConstraints CDCPD_Moveit_Node::get_moveit_obstacle_constriants( ROS_DEBUG_NAMED(LOGNAME + ".moveit", "Finding nearest points and normals"); return find_nearest_points_and_normals(planning_scene, cdcpd_to_moveit); - } +} void CDCPD_Moveit_Node::callback(cv::Mat const& rgb, cv::Mat const& depth, cv::Matx33d const& intrinsics) @@ -458,12 +463,12 @@ void CDCPD_Moveit_Node::callback(cv::Mat const& rgb, cv::Mat const& depth, auto obstacle_constraints = get_obstacle_constraints(); auto const hsv_mask = getHsvMask(ph, rgb); - auto const out = (*cdcpd)(rgb, depth, hsv_mask, intrinsics, - deformable_object_configuration_->tracked_.points_, - obstacle_constraints, - deformable_object_configuration_->max_segment_length_, q_dot, - q_config, gripper_indices); - deformable_object_configuration_->tracked_.points_ = out.gurobi_output; + PointCloud::Ptr vertices = deformable_objects.form_vertices_cloud(); + Eigen::RowVectorXd max_segment_lengths = + deformable_objects.form_max_segment_length_matrix(); + auto const out = (*cdcpd)(rgb, depth, hsv_mask, intrinsics, vertices, obstacle_constraints, + max_segment_lengths, q_dot, q_config, gripper_indices); + deformable_objects.update_def_obj_vertices(out.gurobi_output); publish_outputs(t0, out); reset_if_bad(out); }; @@ -483,10 +488,12 @@ void CDCPD_Moveit_Node::points_callback(const sensor_msgs::PointCloud2ConstPtr& pcl::fromPCLPointCloud2(points_v2, *points); ROS_DEBUG_STREAM_NAMED(LOGNAME, "unfiltered points: " << points->size()); - auto const out = (*cdcpd)(points, deformable_object_configuration_->tracked_.points_, - obstacle_constraints, deformable_object_configuration_->max_segment_length_, q_dot, + PointCloud::Ptr vertices = deformable_objects.form_vertices_cloud(); + Eigen::RowVectorXd max_segment_lengths = + deformable_objects.form_max_segment_length_matrix(); + auto const out = (*cdcpd)(points, vertices, obstacle_constraints, max_segment_lengths, q_dot, q_config, gripper_indices); - deformable_object_configuration_->tracked_.points_ = out.gurobi_output; + deformable_objects.update_def_obj_vertices(out.gurobi_output); publish_outputs(t0, out); reset_if_bad(out); } @@ -514,9 +521,13 @@ void CDCPD_Moveit_Node::publish_bbox() const void CDCPD_Moveit_Node::publish_template() const { auto time = ros::Time::now(); - deformable_object_configuration_->tracked_.points_->header.frame_id = node_params.camera_frame; - pcl_conversions::toPCL(time, deformable_object_configuration_->tracked_.points_->header.stamp); - publishers.pre_template_publisher.publish(deformable_object_configuration_->tracked_.points_); + // TODO(Dylan): Make this a message array instead of just a single point cloud?? + // Get the point cloud representing all of our templates. + PointCloud::Ptr templates_cloud = deformable_objects.form_vertices_cloud(); + templates_cloud->header.frame_id = node_params.camera_frame; + pcl_conversions::toPCL(time, templates_cloud->header.stamp); + + publishers.pre_template_publisher.publish(templates_cloud); } ObstacleConstraints CDCPD_Moveit_Node::get_obstacle_constraints() @@ -524,8 +535,8 @@ ObstacleConstraints CDCPD_Moveit_Node::get_obstacle_constraints() ObstacleConstraints obstacle_constraints; if (moveit_ready and node_params.moveit_enabled) { - obstacle_constraints = get_moveit_obstacle_constriants( - deformable_object_configuration_->tracked_.points_); + PointCloud::Ptr vertices = deformable_objects.form_vertices_cloud(); + obstacle_constraints = get_moveit_obstacle_constriants(vertices); ROS_DEBUG_NAMED(LOGNAME + ".moveit", "Got moveit obstacle constraints"); } return obstacle_constraints; @@ -563,50 +574,85 @@ void CDCPD_Moveit_Node::publish_outputs(ros::Time const& t0, CDCPD::Output const // Publish markers indication the order of the points { - auto rope_marker_fn = [&](PointCloud::ConstPtr cloud, std::string const& ns) { - vm::Marker order; - order.header.frame_id = node_params.camera_frame; - order.header.stamp = ros::Time(); - order.ns = ns; - order.type = visualization_msgs::Marker::LINE_STRIP; - order.action = visualization_msgs::Marker::ADD; - order.pose.orientation.w = 1.0; - order.id = 1; - order.scale.x = 0.01; - order.color.r = 0.1; - order.color.g = 0.6; - order.color.b = 0.9; - order.color.a = 1.0; - - for (auto pc_iter : *cloud) + auto form_edge_markers = [&](std::string const& ns) + { + vm::MarkerArray edge_markers; + int i = 1; + int marker_id = 0; + for (auto const& def_obj_pair : deformable_objects.tracking_map) { - geometry_msgs::Point p; - p.x = pc_iter.x; - p.y = pc_iter.y; - p.z = pc_iter.z; - order.points.push_back(p); + int const& def_obj_id = def_obj_pair.first; + auto const& def_obj_config = def_obj_pair.second; + + PointCloud::ConstPtr const cloud = def_obj_config->tracked_.getPointCloud(); + + // Make a new marker for each edge in each deformable object. + auto const& edge_list = def_obj_config->tracked_.getEdges(); + for (int edge_idx = 0; edge_idx < edge_list.cols(); ++edge_idx) + { + auto const& edge = edge_list.col(edge_idx); + int const id_1 = edge(0, 0); + int const id_2 = edge(1, 0); + + auto const& pt_start = cloud->at(id_1); + auto const& pt_end = cloud->at(id_2); + + vm::Marker marker; + marker.header.frame_id = node_params.camera_frame; + marker.header.stamp = ros::Time(); + marker.ns = ns; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.id = marker_id; + marker.scale.x = 0.01; + marker.color.r = 0.1; + marker.color.g = i * 0.5; + marker.color.b = 0.9; + marker.color.a = 1.0; + + geometry_msgs::Point p_start; + p_start.x = pt_start.x; + p_start.y = pt_start.y; + p_start.z = pt_start.z; + marker.points.push_back(p_start); + + geometry_msgs::Point p_end; + p_end.x = pt_end.x; + p_end.y = pt_end.y; + p_end.z = pt_end.z; + marker.points.push_back(p_end); + + edge_markers.markers.push_back(marker); + ++marker_id; + } + + ++i; } - return order; + + return edge_markers; }; - auto const rope_marker = rope_marker_fn(out.gurobi_output, "line_order"); - publishers.order_pub.publish(rope_marker); + auto const rope_marker_array = form_edge_markers("line_order"); + publishers.order_pub.publish(rope_marker_array); } // compute length and print that for debugging purposes - auto output_length{0.0}; - for (auto point_idx{0}; - point_idx < deformable_object_configuration_->tracked_.points_->size() - 1; - ++point_idx) - { - Eigen::Vector3f const p = - deformable_object_configuration_->tracked_.points_->at(point_idx + 1).getVector3fMap(); - Eigen::Vector3f const p_next = - deformable_object_configuration_->tracked_.points_->at(point_idx).getVector3fMap(); - output_length += (p_next - p).norm(); - } - ROS_DEBUG_STREAM_NAMED(LOGNAME + ".length", "length = " << output_length << " max length = " - << node_params.max_rope_length); + // TODO(dylan): This should compute edge length based on the edges matrix, not just off of + // points. + // auto output_length{0.0}; + // for (auto point_idx{0}; + // point_idx < deformable_object_configuration_->tracked_.points_->size() - 1; + // ++point_idx) + // { + // Eigen::Vector3f const p = + // deformable_object_configuration_->tracked_.points_->at(point_idx + 1).getVector3fMap(); + // Eigen::Vector3f const p_next = + // deformable_object_configuration_->tracked_.points_->at(point_idx).getVector3fMap(); + // output_length += (p_next - p).norm(); + // } + // ROS_DEBUG_STREAM_NAMED(LOGNAME + ".length", "length = " << output_length << " max length = " + // << node_params.max_rope_length); auto const t1 = ros::Time::now(); auto const dt = t1 - t0; @@ -619,9 +665,13 @@ void CDCPD_Moveit_Node::reset_if_bad(CDCPD::Output const& out) out.status == OutputStatus::ObjectiveTooHigh) { // Recreate CDCPD from initial tracking. - std::unique_ptr cdcpd_new(new CDCPD(nh, ph, - deformable_object_configuration_->initial_.points_, - deformable_object_configuration_->initial_.edges_, + bool const use_initial_state = true; + PointCloud::Ptr vertices = + deformable_objects.form_vertices_cloud(use_initial_state); + Eigen::Matrix2Xi edges = + deformable_objects.form_edges_matrix(use_initial_state); + + std::unique_ptr cdcpd_new(new CDCPD(nh, ph, vertices, edges, cdcpd_params.objective_value_threshold, cdcpd_params.use_recovery, cdcpd_params.alpha, cdcpd_params.beta, cdcpd_params.lambda, cdcpd_params.k_spring, cdcpd_params.zeta, cdcpd_params.obstacle_cost_weight, cdcpd_params.fixed_points_weight)); diff --git a/cdcpd/src/deformable_object_configuration.cpp b/cdcpd/src/deformable_object_configuration.cpp index 8cb4f78..f5a8f93 100644 --- a/cdcpd/src/deformable_object_configuration.cpp +++ b/cdcpd/src/deformable_object_configuration.cpp @@ -6,19 +6,71 @@ int unravel_indices(int row, int col, int num_points_width) return row * num_points_width + col; } +DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_str) +{ + std::map obj_type_map{ + {"rope", DeformableObjectType::rope}, + {"cloth", DeformableObjectType::cloth} + }; + return obj_type_map[def_obj_type_str]; +} + +ConnectivityNode::ConnectivityNode(int const node_id) + : id(node_id), + neighbors() +{} + +void ConnectivityNode::add_neighbor_node(std::shared_ptr const neighbor) +{ + neighbors.insert({neighbor->id, neighbor}); +} + +ConnectivityGraph::ConnectivityGraph() + : nodes() +{} + +ConnectivityGraph::ConnectivityGraph(Eigen::Matrix2Xi const& edge_list) + : ConnectivityGraph() +{ + for (int edge_idx = 0; edge_idx < edge_list.cols(); ++edge_idx) + { + auto const& edge = edge_list.col(edge_idx); + int const& id_1 = edge(0, 0); + int const& id_2 = edge(1, 0); + + auto node_1 = std::make_shared(id_1); + auto node_2 = std::make_shared(id_2); + + // Using map::insert here as insert respects if the key, value pair is already in the map. + auto insertion_pair_1 = nodes.insert({id_1, node_1}); + auto insertion_pair_2 = nodes.insert({id_2, node_2}); + + // Add the nodes as neighbors. We're overwriting the nodes we defined earlier as the + // map::insert function returns an iterator to the already present node in the map if there + // was already a node with that ID. + node_1 = insertion_pair_1.first->second; + node_2 = insertion_pair_2.first->second; + + node_1->add_neighbor_node(node_2); + node_2->add_neighbor_node(node_1); + } +} + +DeformableObjectTracking::DeformableObjectTracking() + : vertices_(), + edges_(), + points_(), + connectivity_graph_(edges_) +{} + DeformableObjectTracking::DeformableObjectTracking(DeformableObjectTracking const& other) { vertices_ = other.vertices_; edges_ = other.edges_; - // Need to be careful so that the newly tracked points point to different memory than the - // "other" tracked points - points_ = PointCloud::Ptr(new PointCloud(*other.points_)); + points_ = other.getPointCloudCopy(); + connectivity_graph_ = other.connectivity_graph_; } -DeformableObjectConfiguration::DeformableObjectConfiguration(int const num_points) - : num_points_(num_points) -{} - void DeformableObjectTracking::makeCloud(Eigen::Matrix3Xf const& points_mat) { // TODO: Can we do this cleaner via some sort of data mapping? @@ -42,7 +94,13 @@ void DeformableObjectTracking::setVertices(std::vector const& verte ++i; } - vertices_ = vertices_new; + setVertices(vertices_new); +} + +void DeformableObjectTracking::setVertices(Eigen::Matrix3Xf const& vertices_in) +{ + vertices_ = vertices_in; + makeCloud(vertices_); } void DeformableObjectTracking::setEdges(std::vector> const& edges) @@ -55,9 +113,27 @@ void DeformableObjectTracking::setEdges(std::vector> const& edges_new(1, i) = std::get<1>(edge); ++i; } - edges_ = edges_new; + setEdges(edges_new); +} + +void DeformableObjectTracking::setEdges(Eigen::Matrix2Xi const& edges_in) +{ + edges_ = edges_in; + connectivity_graph_ = ConnectivityGraph(edges_); +} + +void DeformableObjectTracking::setPointCloud(PointCloud::const_iterator const& it_in_begin, PointCloud::const_iterator const& it_in_end) +{ + std::copy(it_in_begin, it_in_end, points_->begin()); } +DeformableObjectConfiguration::DeformableObjectConfiguration(int const num_points) + : num_points_(num_points), + max_segment_length_(0), + initial_(), + tracked_() +{} + void DeformableObjectConfiguration::initializeTracking() { DeformableObjectTracking object_tracking = makeTemplate(); @@ -92,9 +168,8 @@ DeformableObjectTracking RopeConfiguration::makeTemplate() template_edges(1, i - 1) = i; } DeformableObjectTracking configuration; - configuration.vertices_ = template_vertices; - configuration.edges_ = template_edges; - configuration.makeCloud(template_vertices); + configuration.setVertices(template_vertices); + configuration.setEdges(template_edges); return configuration; } @@ -105,8 +180,8 @@ ClothConfiguration::ClothConfiguration(float const length_initial, float const w length_initial_{length_initial}, width_initial_{width_initial}, grid_size_initial_guess_{grid_size_initial_guess}, - num_points_length_{std::ceil(length_initial / grid_size_initial_guess) + 1}, - num_points_width_{std::ceil(width_initial / grid_size_initial_guess) + 1}, + num_points_length_{static_cast(std::ceil(length_initial / grid_size_initial_guess)) + 1}, + num_points_width_{static_cast(std::ceil(width_initial / grid_size_initial_guess)) + 1}, num_edges_{(num_points_length_ - 1) * num_points_width_ + (num_points_width_ - 1) * num_points_length_} { @@ -119,6 +194,7 @@ ClothConfiguration::ClothConfiguration(float const length_initial, float const w float grid_size_initial_actual_length = length_initial_ / static_cast(num_segments_length); float grid_size_initial_actual_width = width_initial_ / static_cast(num_segments_width); + // TODO(Dylan): This needs to be updated to include the stretch limit parameter! max_segment_length_ = std::min({grid_size_initial_actual_length, grid_size_initial_actual_width}); } @@ -141,7 +217,7 @@ DeformableObjectTracking ClothConfiguration::makeTemplate() { edges_list.push_back({idx, unravel_indices(i + 1, j, num_points_width_)}); } - if (j + 1 < num_points_length_) + if (j + 1 < num_points_width_) { edges_list.push_back({idx, unravel_indices(i, j + 1, num_points_width_)}); } @@ -160,7 +236,5 @@ DeformableObjectTracking ClothConfiguration::makeTemplate() configuration.setVertices(template_warped_points); configuration.setEdges(edges_list); - configuration.makeCloud(configuration.vertices_); - return configuration; } \ No newline at end of file diff --git a/cdcpd/src/optimizer.cpp b/cdcpd/src/optimizer.cpp index 427c6bf..abb7b31 100644 --- a/cdcpd/src/optimizer.cpp +++ b/cdcpd/src/optimizer.cpp @@ -40,7 +40,9 @@ static Eigen::Vector3f const bounding_box_extend; // This is equivalent to [point_a' point_b'] * Q * [point_a' point_b']' // where Q is [ I, -I // -I, I] -static GRBQuadExpr buildDifferencingQuadraticTerm(GRBVar *point_a, GRBVar *point_b, const size_t num_vars_per_point) { +static GRBQuadExpr buildDifferencingQuadraticTerm(GRBVar *point_a, GRBVar *point_b, + const size_t num_vars_per_point) +{ GRBQuadExpr expr; // Build the main diagonal @@ -55,7 +57,8 @@ static GRBQuadExpr buildDifferencingQuadraticTerm(GRBVar *point_a, GRBVar *point return expr; } -static GRBEnv &getGRBEnv() { +static GRBEnv &getGRBEnv() +{ try { static GRBEnv env; return env; @@ -65,12 +68,19 @@ static GRBEnv &getGRBEnv() { } } -static Vector3f cgalVec2EigenVec(Vector cgal_v) { return Vector3f(cgal_v[0], cgal_v[1], cgal_v[2]); } +static Vector3f cgalVec2EigenVec(Vector cgal_v) +{ + return Vector3f(cgal_v[0], cgal_v[1], cgal_v[2]); +} -static Vector3f Pt3toVec(const Point_3 pt) { return Vector3f(float(pt.x()), float(pt.y()), float(pt.z())); } +static Vector3f Pt3toVec(const Point_3 pt) +{ + return Vector3f(float(pt.x()), float(pt.y()), float(pt.z())); +} std::tuple Optimizer::nearest_points_and_normal(const Matrix3Xf &last_template, - Objects const &objects) { + Objects const &objects) +{ for (auto const &object : objects) { // Meshes if (object.meshes.size() != object.mesh_poses.size()) { @@ -154,8 +164,8 @@ std::tuple Optimizer::nearest_points_and_normal(const Matrix3Xf } std::tuple Optimizer::nearest_points_and_normal_box(const Matrix3Xf &last_template, - shape_msgs::SolidPrimitive const &box, - geometry_msgs::Pose const &pose) { + shape_msgs::SolidPrimitive const &box, geometry_msgs::Pose const &pose) +{ auto const position = ConvertTo(pose.position); auto const orientation = ConvertTo(pose.orientation).toRotationMatrix(); auto const box_x = box.dimensions[shape_msgs::SolidPrimitive::BOX_X]; @@ -225,24 +235,26 @@ std::tuple Optimizer::nearest_points_and_normal_box(const Matri return {nearestPts, normalVecs}; } -std::tuple Optimizer::nearest_points_and_normal_sphere(const Matrix3Xf &last_template, - shape_msgs::SolidPrimitive const &, - geometry_msgs::Pose const &) { +std::tuple Optimizer::nearest_points_and_normal_sphere( + const Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &, geometry_msgs::Pose const &) +{ Matrix3Xf nearestPts(3, last_template.cols()); Matrix3Xf normalVecs(3, last_template.cols()); return {nearestPts, normalVecs}; } -std::tuple Optimizer::nearest_points_and_normal_plane(const Matrix3Xf &last_template, - shape_msgs::Plane const &) { +std::tuple Optimizer::nearest_points_and_normal_plane( + const Matrix3Xf &last_template, shape_msgs::Plane const &) +{ Matrix3Xf nearestPts(3, last_template.cols()); Matrix3Xf normalVecs(3, last_template.cols()); return {nearestPts, normalVecs}; } -std::tuple Optimizer::nearest_points_and_normal_cylinder(const Matrix3Xf &last_template, - shape_msgs::SolidPrimitive const &cylinder, - geometry_msgs::Pose const &pose) { +std::tuple Optimizer::nearest_points_and_normal_cylinder( + const Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &cylinder, + geometry_msgs::Pose const &pose) +{ auto const position = ConvertTo(pose.position); // NOTE: Yixuan, should orientation be roll, pitch, yaw here? // Answer: As what I can recall, the orientation is the unit vector along center axis @@ -299,8 +311,9 @@ std::tuple Optimizer::nearest_points_and_normal_cylinder(const return {nearestPts, normalVecs}; } -std::tuple Optimizer::nearest_points_and_normal_mesh(const Matrix3Xf &last_template, - shape_msgs::Mesh const &shapes_mesh) { +std::tuple Optimizer::nearest_points_and_normal_mesh( + const Matrix3Xf &last_template, shape_msgs::Mesh const &shapes_mesh) +{ auto mesh = shapes_mesh_to_cgal_mesh(shapes_mesh); auto const fnormals = mesh.add_property_map("f:normals", CGAL::NULL_VECTOR).first; auto const vnormals = mesh.add_property_map("v:normals", CGAL::NULL_VECTOR).first; @@ -345,7 +358,9 @@ std::tuple Optimizer::nearest_points_and_normal_mesh(const Matr return {nearestPts, normalVecs}; } -std::tuple nearest_points_line_segments(const Matrix3Xf &last_template, const Matrix2Xi &E) { +std::tuple nearest_points_line_segments(const Matrix3Xf &last_template, + const Matrix2Xi &E) +{ // find the nearest points on the line segments // refer to the website https://math.stackexchange.com/questions/846054/closest-points-on-two-line-segments MatrixXf startPts( @@ -406,23 +421,24 @@ std::tuple nearest_points_line_segments(const Matrix3Xf &las } std::tuple Optimizer::test_box(const Eigen::Matrix3Xf &last_template, - shape_msgs::SolidPrimitive const &box, - geometry_msgs::Pose const &pose) { + shape_msgs::SolidPrimitive const &box, geometry_msgs::Pose const &pose) +{ return nearest_points_and_normal_box(last_template, box, pose); } Optimizer::Optimizer(const Eigen::Matrix3Xf initial_template, const Eigen::Matrix3Xf last_template, - const float stretch_lambda, const float obstacle_cost_weight, const float fixed_points_weight) + const float stretch_lambda, const float obstacle_cost_weight, const float fixed_points_weight) : initial_template_(initial_template), last_template_(last_template), stretch_lambda_(stretch_lambda), obstacle_cost_weight_(obstacle_cost_weight), - fixed_points_weight_(fixed_points_weight) {} + fixed_points_weight_(fixed_points_weight) +{} std::pair Optimizer::operator()(const Matrix3Xf &Y, const Matrix2Xi &E, - const std::vector &fixed_points, - ObstacleConstraints const &obstacle_constraints, - const double max_segment_length) { + const std::vector &fixed_points, ObstacleConstraints const &obstacle_constraints, + Eigen::RowVectorXd const max_segment_length) +{ // Y: Y^t in Eq. (21) // E: E in Eq. (21) Matrix3Xf Y_opt(Y.rows(), Y.cols()); @@ -452,10 +468,13 @@ std::pair Optimizer::operator()(const Matrix3Xf &Y, const Mat // Add the edge constraints ROS_DEBUG_STREAM_THROTTLE_NAMED(1, LOGNAME, "stretch lambda " << stretch_lambda_); { - for (ssize_t i = 0; i < E.cols(); ++i) { - model.addQConstr(buildDifferencingQuadraticTerm(&vars[E(0, i) * 3], &vars[E(1, i) * 3], 3), GRB_LESS_EQUAL, - stretch_lambda_ * stretch_lambda_ * max_segment_length * max_segment_length, - "upper_edge_" + std::to_string(E(0, i)) + "_to_" + std::to_string(E(1, i))); + for (ssize_t i = 0; i < E.cols(); ++i) + { + double const max_segment_length_i = max_segment_length(0, i); + model.addQConstr(buildDifferencingQuadraticTerm(&vars[E(0, i) * 3], &vars[E(1, i) * 3], 3), + GRB_LESS_EQUAL, + stretch_lambda_ * stretch_lambda_ * (max_segment_length_i * max_segment_length_i), + "upper_edge_" + std::to_string(E(0, i)) + "_to_" + std::to_string(E(1, i))); } model.update(); } @@ -569,7 +588,8 @@ std::pair Optimizer::operator()(const Matrix3Xf &Y, const Mat return {Y_opt, final_objective_value}; } -bool Optimizer::gripper_constraints_satisfiable(const std::vector &fixed_points) const { +bool Optimizer::gripper_constraints_satisfiable(const std::vector &fixed_points) const +{ for (auto const &p1 : fixed_points) { for (auto const &p2 : fixed_points) { float const current_distance = (p1.position - p2.position).squaredNorm(); diff --git a/cdcpd/src/stopwatch.cpp b/cdcpd/src/stopwatch.cpp new file mode 100644 index 0000000..1cd9746 --- /dev/null +++ b/cdcpd/src/stopwatch.cpp @@ -0,0 +1,16 @@ +#include "cdcpd/stopwatch.h" + +Stopwatch::Stopwatch(std::string const& routine_name) + : routine_name_(routine_name), + start_time_(std::chrono::steady_clock::now()) +{} + +Stopwatch::~Stopwatch() +{ + auto const end_time = std::chrono::steady_clock::now(); + auto const runtime_duration = + std::chrono::duration_cast(end_time - start_time_); + double const runtime_ns = static_cast(runtime_duration.count()); + double const runtime_ms = runtime_ns * NANOSECONDS_TO_MILLISECONDS; + ROS_DEBUG_STREAM(routine_name_ << " took " << runtime_ms << " milliseconds to run."); +} \ No newline at end of file diff --git a/cdcpd/src/tracking_map.cpp b/cdcpd/src/tracking_map.cpp new file mode 100644 index 0000000..758e727 --- /dev/null +++ b/cdcpd/src/tracking_map.cpp @@ -0,0 +1,177 @@ +#include "cdcpd/tracking_map.h" + +TrackingMap::TrackingMap() + : tracking_map(), + deformable_object_id_next_(0), + ordered_def_obj_ids_() +{} + +int TrackingMap::get_total_num_points() const +{ + int num_points_total = 0; + for (auto const& def_obj_id : ordered_def_obj_ids_) + { + std::shared_ptr def_obj_config = + tracking_map.at(def_obj_id); + num_points_total += def_obj_config->num_points_; + } + return num_points_total; +} + +int TrackingMap::get_total_num_edges() const +{ + int num_edges_total = 0; + for (auto const& tracked_pair : tracking_map) + { + std::shared_ptr const& def_obj_config = tracked_pair.second; + int num_edges = def_obj_config->tracked_.getEdges().cols(); + num_edges_total += num_edges; + } + return num_edges_total; +} + +std::map > TrackingMap::get_vertex_assignments() const +{ + // NOTE: This could be kept track of as a member variable and instead updated any time a + // deformable object is added/removed. + std::map > vertex_assignments; + int idx_begin = 0; + for (auto const& configuration : tracking_map) + { + int idx_end = idx_begin + configuration.second->num_points_; + std::tuple idx_range{idx_begin, idx_end}; + idx_begin = idx_end; + + vertex_assignments.emplace(configuration.first, idx_range); + } + return vertex_assignments; +} + +void TrackingMap::add_def_obj_configuration( + std::shared_ptr const def_obj_config) +{ + tracking_map.emplace(deformable_object_id_next_, def_obj_config); + + // Just insert the deformable object ID at the back or our track ID list since we know this ID + // is the greatest ID (since we just increment the ID each time). + ordered_def_obj_ids_.push_back(deformable_object_id_next_); + + ++deformable_object_id_next_; +} + +void TrackingMap::update_def_obj_vertices(pcl::shared_ptr const vertices_new) +{ + auto vertex_assignments = get_vertex_assignments(); + auto const& cloud_it_begin = vertices_new->begin(); + for (auto const& assignment_range : vertex_assignments) + { + // Get the deformable object configuration we're updating. + int const& def_obj_id = assignment_range.first; + std::shared_ptr& def_obj_config = tracking_map[def_obj_id]; + + // Grab the range of indices where this configuration's points will be stored in the point + // cloud. + std::tuple const& idx_range = assignment_range.second; + int const& idx_start = std::get<0>(idx_range); + int const& idx_end = std::get<1>(idx_range); + + // Use the point cloud iterators and std::copy to efficiently update the tracked points. + auto const& it_begin = cloud_it_begin + idx_start; + auto const& it_end = cloud_it_begin + idx_end; + def_obj_config->tracked_.setPointCloud(it_begin, it_end); + + PointCloud::ConstPtr tracked_cloud = def_obj_config->tracked_.getPointCloud(); + def_obj_config->tracked_.setVertices(tracked_cloud->getMatrixXfMap().topRows(3)); + } +} + +PointCloud::Ptr TrackingMap::form_vertices_cloud(bool const use_initial_state) const +{ + PointCloud::Ptr vertices_cloud(new PointCloud); + // I don't think we actually care about the stamp at this point. + // bool stamp_copied = false; + for (auto const& def_obj_id : ordered_def_obj_ids_) + { + // Grab the deformable object configuration. + std::shared_ptr const def_obj_config = + tracking_map.at(def_obj_id); + + std::shared_ptr tracking = + get_appropriate_tracking(def_obj_config, use_initial_state); + + // Concatenate this deformable object's point cloud with the aggregate point cloud. + (*vertices_cloud) += (*tracking->getPointCloud()); + } + return vertices_cloud; +} + +Eigen::Matrix2Xi TrackingMap::form_edges_matrix(bool const use_initial_state) const +{ + int const num_edges_total = get_total_num_edges(); + + // Initialize an Eigen matrix for keeping track of the edges. + Eigen::Matrix2Xi edges_total(2, num_edges_total); + + // Populate that edge matrix based on all of our edges. + int edge_idx_aggregate = 0; + for (auto const& def_obj_id : ordered_def_obj_ids_) + { + // Grab the deformable object configuration. + std::shared_ptr const def_obj_config = + tracking_map.at(def_obj_id); + + std::shared_ptr tracking = + get_appropriate_tracking(def_obj_config, use_initial_state); + + Eigen::Matrix2Xi const& edges = tracking->getEdges(); + for (int edge_col = 0; edge_col < edges.cols(); ++edge_col) + { + edges_total.col(edge_idx_aggregate) = edges.col(edge_col); + ++edge_idx_aggregate; + } + } + return edges_total; +} + +Eigen::RowVectorXd TrackingMap::form_max_segment_length_matrix() const +{ + // Construct the structure that will hold the edge lengths. + int const num_edges_total = get_total_num_edges(); + Eigen::RowVectorXd max_segment_lengths(num_edges_total); + + // Populate the edge length structure. + // Right now this isn't very interesting but one could envisage dynamically refining tracking + // by placing more Gaussians in areas of uncertainty, changing the maximum segment length based + // on the new edges created. + size_t edge_idx_aggregate = 0; + for (auto const& def_obj_id : ordered_def_obj_ids_) + { + std::shared_ptr const def_obj_config = + tracking_map.at(def_obj_id); + + double const def_obj_max_segment_length = def_obj_config->max_segment_length_; + for (int i = 0; i < def_obj_config->tracked_.getEdges().cols(); ++i) + { + max_segment_lengths(0, edge_idx_aggregate) = def_obj_max_segment_length; + ++edge_idx_aggregate; + } + } + + return max_segment_lengths; +} + +std::shared_ptr TrackingMap::get_appropriate_tracking( + std::shared_ptr const def_obj_config, + bool take_initial_state) const +{ + std::shared_ptr tracking; + if (take_initial_state) + { + tracking = std::make_shared(def_obj_config->initial_); + } + else + { + tracking = std::make_shared(def_obj_config->tracked_); + } + return tracking; +} \ No newline at end of file diff --git a/cdcpd/tests/integration/static_rope.cpp b/cdcpd/tests/integration/static_rope.cpp index ce39230..c0910a5 100644 --- a/cdcpd/tests/integration/static_rope.cpp +++ b/cdcpd/tests/integration/static_rope.cpp @@ -34,7 +34,7 @@ CDCPD initializeCdcpdSimulator(DeformableObjectTracking const& rope_tracking_ini if (PRINT_DEBUG_MESSAGES) { std::stringstream ss; - ss << rope_tracking_initial.edges_; + ss << rope_tracking_initial.getEdges(); ROS_WARN("Initial template edges"); ROS_WARN(ss.str().c_str()); } @@ -50,7 +50,7 @@ CDCPD initializeCdcpdSimulator(DeformableObjectTracking const& rope_tracking_ini float const obstacle_cost_weight = 0.001; float const fixed_points_weight = 10.0; - CDCPD cdcpd = CDCPD(rope_tracking_initial.points_, rope_tracking_initial.edges_, + CDCPD cdcpd = CDCPD(rope_tracking_initial.getPointCloud(), rope_tracking_initial.getEdges(), objective_value_threshold, use_recovery, alpha, beta, lambda, k, zeta, obstacle_cost_weight, fixed_points_weight); @@ -65,8 +65,13 @@ PointCloud::Ptr resimulateCdcpd(CDCPD& cdcpd_sim, { // Do some setup of parameters and gripper configuration. PointCloud::Ptr tracked_points = initial_tracked_points; + ObstacleConstraints obstacle_constraints; // No need to specify anything with this demo. + float const max_segment_length = max_rope_length / static_cast(num_points); + Eigen::RowVectorXd max_segment_lengths = + Eigen::RowVectorXd::Ones(num_points) * max_segment_length; + unsigned int gripper_count = 0U; smmap::AllGrippersSinglePose q_config; const smmap::AllGrippersSinglePoseDelta q_dot{gripper_count, kinematics::Vector6d::Zero()}; @@ -79,7 +84,7 @@ PointCloud::Ptr resimulateCdcpd(CDCPD& cdcpd_sim, // Run a single "iteration" of CDCPD mimicing the points_callback lambda function found in // cdcpd_node.cpp CDCPD::Output out = cdcpd_sim(cloud, tracked_points, obstacle_constraints, - max_segment_length, q_dot, q_config, gripper_indices); + max_segment_lengths, q_dot, q_config, gripper_indices); tracked_points = out.gurobi_output; // Do a health check of CDCPD @@ -112,7 +117,7 @@ TEST(StaticRope, testResimPointEquivalency) auto input_clouds = readCdcpdInputPointClouds(bag); PointCloud::Ptr tracked_points = resimulateCdcpd(cdcpd_sim, input_clouds, - rope_configuration.initial_.points_, max_rope_length, num_points); + rope_configuration.initial_.getPointCloudCopy(), max_rope_length, num_points); expectPointCloudsEqual(*pt_cloud_last, *tracked_points); diff --git a/cdcpd/tests/unit/include/ClothConfigurationTest.h b/cdcpd/tests/unit/include/ClothConfigurationTest.h index 54dbdcd..ae1eb98 100644 --- a/cdcpd/tests/unit/include/ClothConfigurationTest.h +++ b/cdcpd/tests/unit/include/ClothConfigurationTest.h @@ -24,10 +24,10 @@ TEST(ClothConfigurationTest, simpleClothConfigInitializationBagGridSizeGuess) EXPECT_EQ(cloth_config.num_edges_, 12); } -// Tests the tracking initialization of a very simple template, a 2x2 cloth +// Tests the tracking initialization of a very simple template, a 3x3 cloth TEST(ClothConfigurationTest, simpleClothConfigurationTemplateInitializationBadGridSizeGuess) { - // The following values give a 2x2 template + // The following values give a 3x3 template float const cloth_length = 1.0; float const cloth_width = 1.0; float const grid_size_initial_guess = 0.76; @@ -35,8 +35,7 @@ TEST(ClothConfigurationTest, simpleClothConfigurationTemplateInitializationBadGr config_test.initializeTracking(); // Setup the expected points and vertices - DeformableObjectTracking tracking_expected; - std::vector vertices_expected = { + std::vector verts_expected = { {0, 0, 0}, {0, 0.5, 0}, {0, 1, 0}, @@ -47,6 +46,17 @@ TEST(ClothConfigurationTest, simpleClothConfigurationTemplateInitializationBadGr {1, 0.5, 0}, {1, 1, 0} }; + Eigen::Matrix verts_expected_mat; + int i = 0; + for (auto const& vert : verts_expected) + { + verts_expected_mat(0, i) = vert.x; + verts_expected_mat(1, i) = vert.y; + verts_expected_mat(2, i) = vert.z; + ++i; + } + + int const num_edges_expected = 12; std::vector> edges_expected = { {0, 3}, {0, 1}, @@ -61,9 +71,83 @@ TEST(ClothConfigurationTest, simpleClothConfigurationTemplateInitializationBadGr {6, 7}, {7, 8} }; - tracking_expected.setVertices(vertices_expected); - tracking_expected.setEdges(edges_expected); + Eigen::Matrix edges_expected_mat; + i = 0; + for (auto const& edge : edges_expected) + { + edges_expected_mat(0, i) = std::get<0>(edge); + edges_expected_mat(1, i) = std::get<1>(edge); + ++i; + } + + expectEigenMatsEqual(config_test.tracked_.getVertices(), verts_expected_mat); + expectEigenMatsEqual(config_test.tracked_.getEdges(), edges_expected_mat); + EXPECT_TRUE(config_test.num_edges_ == num_edges_expected); +} + +TEST(ClothConfigurationTest, rectangularTemplate) +{ + double const cloth_length = 1.0; + double const cloth_width = 1.5; + double const grid_size_initial_guess = 0.74; + ClothConfiguration config_test{cloth_length, cloth_width, grid_size_initial_guess}; + config_test.initializeTracking(); + + // Setup the expected points and vertices + std::vector verts_expected = { + {0, 0, 0}, + {0, 0.5, 0}, + {0, 1, 0}, + {0, 1.5, 0}, + {0.5, 0, 0}, + {0.5, 0.5, 0}, + {0.5, 1, 0}, + {0.5, 1.5, 0}, + {1.0, 0, 0}, + {1.0, 0.5, 0}, + {1.0, 1, 0}, + {1.0, 1.5, 0}, + }; + Eigen::Matrix verts_expected_mat; + int i = 0; + for (auto const& vert : verts_expected) + { + verts_expected_mat(0, i) = vert.x; + verts_expected_mat(1, i) = vert.y; + verts_expected_mat(2, i) = vert.z; + ++i; + } + + int const num_edges_expected = 17; + std::vector> edges_expected = { + {0, 4}, + {0, 1}, + {1, 5}, + {1, 2}, + {2, 6}, + {2, 3}, + {3, 7}, + {4, 8}, + {4, 5}, + {5, 9}, + {5, 6}, + {6, 10}, + {6, 7}, + {7, 11}, + {8, 9}, + {9, 10}, + {10, 11} + }; + Eigen::Matrix edges_expected_mat; + i = 0; + for (auto const& edge : edges_expected) + { + edges_expected_mat(0, i) = std::get<0>(edge); + edges_expected_mat(1, i) = std::get<1>(edge); + ++i; + } - expectEigenMatsEqual(config_test.tracked_.vertices_, tracking_expected.vertices_); - expectEigenMatsEqual(config_test.tracked_.edges_, tracking_expected.edges_); + expectEigenMatsEqual(config_test.tracked_.getVertices(), verts_expected_mat); + expectEigenMatsEqual(config_test.tracked_.getEdges(), edges_expected_mat); + EXPECT_TRUE(config_test.num_edges_ == num_edges_expected); } \ No newline at end of file diff --git a/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h b/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h index ac30202..f08a4d9 100644 --- a/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h +++ b/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h @@ -4,32 +4,166 @@ #include "cdcpd/deformable_object_configuration.h" +#include + +class DeformableObjectTrackingTest : public ::testing::Test +{ +protected: + DeformableObjectTrackingTest() + { + track_1 = std::make_shared(); + Eigen::Matrix3Xf vertices_1 = Eigen::Matrix{}; + vertices_1 << 0.0F, 1.0F, 2.0F; + Eigen::Matrix2Xi edges_1 = Eigen::Matrix{}; + edges_1 << 0, 1; + + track_1->setVertices(vertices_1); + track_1->setEdges(edges_1); + + // Copy track 1 into track 2. + track_2 = std::make_shared(*track_1); + + // Modify track 1. + Eigen::Matrix3Xf vertices_2 = Eigen::Matrix{}; + vertices_2 << 3.0F, 4.0F, 5.0F; + Eigen::Matrix2Xi edges_2 = Eigen::Matrix{}; + edges_2 << 2, 3; + + track_1->setVertices(vertices_2); + track_1->setEdges(edges_2); + } + + bool areConnectivityNodesEqual(ConnectivityNode const& node_1, ConnectivityNode const& node_2) + { + bool nodes_equal = true; + + nodes_equal = nodes_equal && (node_1.id == node_2.id); + + // Check if the nodes have the same neighbors. + auto const& neighbors_2 = node_2.neighbors; + for (auto const& neighb_1 : node_1.neighbors) + { + int const id_neighbor_1 = neighb_1.first; + // If the node one's neighbor isn't in node two's neighbors, the nodes aren't equal. + if (neighbors_2.count(id_neighbor_1) == 0) + { + return false; + } + } + + return nodes_equal; + } + + bool doNodesMatch(ConnectivityGraph const& graph_1, ConnectivityGraph const& graph_2) + { + // Check if all nodes in graph 1 are in graph 2 + bool nodes_match = true; + for (auto const& pair_1 : graph_1.nodes) + { + int const id_node_1 = pair_1.first; + auto const node_1 = pair_1.second; + + int const num_matching_nodes = graph_2.nodes.count(id_node_1); + if (num_matching_nodes > 1) + { + std::cout << "Found " << num_matching_nodes + << " matching nodes between graphs. This is very unexpected!" << std::endl; + } + + bool const is_node_1_in_graph_2 = num_matching_nodes == 1; + if (!is_node_1_in_graph_2) + { + return false; + } + } + return nodes_match; + } + + bool areConnectivityGraphsEqual(ConnectivityGraph const& graph_1, + ConnectivityGraph const& graph_2) + { + bool graphs_equal = true; + + // Check that all nodes in graph 1 are in graph 2 and vice versa. + graphs_equal = graphs_equal && doNodesMatch(graph_1, graph_2); + graphs_equal = graphs_equal && doNodesMatch(graph_2, graph_1); + if (!graphs_equal) + { + return false; + } + + for (auto const& pair_1 : graph_1.nodes) + { + int const id_node_1 = pair_1.first; + auto const node_1 = pair_1.second; + + // First check if the node from graph 1 is even in graph 2 + auto it_potential_node_2 = graph_2.nodes.find(id_node_1); + bool const is_node_1_in_graph_2 = it_potential_node_2 != graph_2.nodes.end(); + if (is_node_1_in_graph_2) + { + auto const node_2 = it_potential_node_2->second; + bool const are_nodes_equal = areConnectivityNodesEqual(*node_1, *node_2); + graphs_equal = graphs_equal && are_nodes_equal; + } + else // If the node from graph 1 isn't in graph 2, we know the graphs aren't equal. + { + return false; + } + } + + // If we got to this point, everything is equal. + return true; + } + + std::shared_ptr track_1; + std::shared_ptr track_2; +}; + // Tests that the members of DeformableObjectTracking are properly copied in the copy constructor -// and not just the pointers -TEST(DeformableObjectTrackingTest, copyConstructor) +// and not just the pointers. +// NOTE: This also tests getVertices, getEdges, and getPointCloud methods. +TEST_F(DeformableObjectTrackingTest, copyConstructorDifferentTracks) { - DeformableObjectTracking* track_1 = new DeformableObjectTracking(); - track_1->vertices_ = Eigen::Matrix{}; - track_1->vertices_ << 0.0F, 1.0F, 2.0F; - track_1->edges_ = Eigen::Matrix{}; - track_1->edges_ << 0, 1; - track_1->points_ = PointCloud::Ptr(new PointCloud); - track_1->points_->push_back(pcl::PointXYZ{0, 1, 2}); - - DeformableObjectTracking* track_2 = new DeformableObjectTracking(*track_1); - - track_1->vertices_ = Eigen::Matrix{}; - track_1->vertices_ << 3.0F, 4.0F, 5.0F; - track_1->edges_ = Eigen::Matrix{}; - track_1->edges_ << 2, 3; - track_1->points_ = PointCloud::Ptr(new PointCloud); - track_1->points_->push_back(pcl::PointXYZ{3, 4, 5}); - - EXPECT_FALSE(track_1->vertices_.isApprox(track_2->vertices_)); - - EXPECT_FALSE(track_1->edges_.isApprox(track_2->edges_)); - - EXPECT_FALSE(track_1->points_->points[0].x == track_2->points_->points[0].x); - EXPECT_FALSE(track_1->points_->points[0].y == track_2->points_->points[0].y); - EXPECT_FALSE(track_1->points_->points[0].z == track_2->points_->points[0].z); + // Check that modifications to track 1 didn't carry over to track 2. + auto const& track_1_vertices = track_1->getVertices(); + auto const& track_1_edges = track_1->getEdges(); + auto const track_1_point_cloud = track_1->getPointCloud(); + auto const track_2_point_cloud = track_2->getPointCloud(); + + EXPECT_FALSE(track_1_vertices.isApprox(track_2->getVertices())); + + EXPECT_FALSE(track_1_edges.isApprox(track_2->getEdges())); + + EXPECT_FALSE(track_1_point_cloud->points[0].x == track_2_point_cloud->points[0].x); + EXPECT_FALSE(track_1_point_cloud->points[0].y == track_2_point_cloud->points[0].y); + EXPECT_FALSE(track_1_point_cloud->points[0].z == track_2_point_cloud->points[0].z); + + bool are_graphs_equal = areConnectivityGraphsEqual(track_1->getConnectivityGraph(), + track_2->getConnectivityGraph()); + EXPECT_FALSE(are_graphs_equal); +} + +TEST_F(DeformableObjectTrackingTest, copyConstructorSameTracks) +{ + using namespace std; + + // Copy track 1 back into track 2 to make sure everything is the same. + track_2 = std::make_shared(*track_1); + auto const& track_1_vertices = track_1->getVertices(); + auto const& track_1_edges = track_1->getEdges(); + auto const track_1_point_cloud = track_1->getPointCloud(); + auto const track_2_point_cloud = track_2->getPointCloud(); + + EXPECT_TRUE(track_1_vertices.isApprox(track_2->getVertices())); + + EXPECT_TRUE(track_1_edges.isApprox(track_2->getEdges())); + + EXPECT_TRUE(track_1_point_cloud->points[0].x == track_2_point_cloud->points[0].x); + EXPECT_TRUE(track_1_point_cloud->points[0].y == track_2_point_cloud->points[0].y); + EXPECT_TRUE(track_1_point_cloud->points[0].z == track_2_point_cloud->points[0].z); + + bool are_graphs_equal = areConnectivityGraphsEqual(track_1->getConnectivityGraph(), + track_2->getConnectivityGraph()); + EXPECT_TRUE(are_graphs_equal); } \ No newline at end of file diff --git a/cdcpd/tests/unit/include/RopeConfigurationTest.h b/cdcpd/tests/unit/include/RopeConfigurationTest.h index acbaf87..fc8bec7 100644 --- a/cdcpd/tests/unit/include/RopeConfigurationTest.h +++ b/cdcpd/tests/unit/include/RopeConfigurationTest.h @@ -51,6 +51,6 @@ TEST(RopeConfigurationTest, simpleRopeConfigTemplateInitialization) track_expected.setVertices(vertices_expected); track_expected.setEdges(edges_expected); - expectEigenMatsEqual(config.tracked_.vertices_, track_expected.vertices_); - expectEigenMatsEqual(config.tracked_.edges_, track_expected.edges_); + expectEigenMatsEqual(config.tracked_.getVertices(), track_expected.getVertices()); + expectEigenMatsEqual(config.tracked_.getEdges(), track_expected.getEdges()); } \ No newline at end of file