From 4ddbc8ac04fc5c622243102b45b5b5010563c332 Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 13:12:27 -0400 Subject: [PATCH 1/8] Add Multi-Template Tracking Map (#48) # High-Level Description Adds the ability for CDCPD to run on multiple deformable object configurations by storing a map of configurations by unique ID (ID unique to each deformable object configuration). This isn't particularly important now but opens the possibility for tracking multiple templates in the future. This does NOT implement multiple template tracking in the sense of: - Learning separate LLE weights for separate templates given different initial template states. - Separating multiple deformable object configurations in the CPD step, meaning that the masked points that belong to another template still impact the CPD score of another, separate deformable object configuration. - Using masked point association history to disambiguate multi-template (non-instance) segmentation, meaning that if CDCPD receives a mask indicating there are two deformable objects in the segmentation that are interacting, there's no way to tell them apart in the tracking's use of the segmented mask. ## Key Changes - Adds a std::map to keep track of deformable object configurations (how tracked templates are stored) by unique IDs. - Changes max segment length to be per edge so that edges can have different maximum lengths. - Something of this flavor is necessary to specify max edge lengths based on each template's (potentially different) edge lengths compared to any other template's edge lengths. - Though, it's up for debate as to whether this is the best way to handle this. - Updates the RVIZ edge visualization to only connect tracked points that belong to the same template. - Accomplishes this by publishing a MarkerArray message instead of a single Marker message. - TODO: Connect points based on the edge list. This would make the visualization respect the actual edges and thus correctly visualize cloth. ## Miscellaneous Changes - Formats CDCPD and Optimizer function signatures as they could have been considered hard to read. --- cdcpd/CMakeLists.txt | 1 + cdcpd/include/cdcpd/cdcpd.h | 71 ++++--- cdcpd/include/cdcpd/cdcpd_node.h | 3 +- .../cdcpd/deformable_object_configuration.h | 9 +- cdcpd/include/cdcpd/optimizer.h | 39 ++-- cdcpd/include/cdcpd/tracking_map.h | 57 ++++++ cdcpd/launch/azure_2_rope.launch | 40 ++++ cdcpd/rviz/azure_2.rviz | 29 ++- cdcpd/rviz/kinect_tripodB.rviz | 4 +- cdcpd/rviz/realsense.rviz | 4 +- cdcpd/src/cdcpd.cpp | 104 ++++++---- cdcpd/src/cdcpd_node.cpp | 189 +++++++++++------- cdcpd/src/deformable_object_configuration.cpp | 10 + cdcpd/src/optimizer.cpp | 80 +++++--- cdcpd/src/tracking_map.cpp | 176 ++++++++++++++++ cdcpd/tests/integration/static_rope.cpp | 7 +- 16 files changed, 595 insertions(+), 228 deletions(-) create mode 100644 cdcpd/include/cdcpd/tracking_map.h create mode 100644 cdcpd/launch/azure_2_rope.launch create mode 100644 cdcpd/src/tracking_map.cpp diff --git a/cdcpd/CMakeLists.txt b/cdcpd/CMakeLists.txt index a8186e9..b9c1cb5 100644 --- a/cdcpd/CMakeLists.txt +++ b/cdcpd/CMakeLists.txt @@ -67,6 +67,7 @@ add_library(cdcpd SHARED src/obs_util.cpp src/past_template_matcher.cpp src/segmenter.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..2fc0c69 100644 --- a/cdcpd/include/cdcpd/cdcpd.h +++ b/cdcpd/include/cdcpd/cdcpd.h @@ -98,49 +98,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; 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..60d8c6c 100644 --- a/cdcpd/include/cdcpd/deformable_object_configuration.h +++ b/cdcpd/include/cdcpd/deformable_object_configuration.h @@ -16,14 +16,7 @@ enum DeformableObjectType cloth }; -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]; -} +DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_str); struct DeformableObjectTracking { 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/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..5305e98 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 @@ -453,7 +464,8 @@ Matrix3Xf CDCPD::cpd(const Matrix3Xf &X, const Matrix3Xf &Y, const Matrix3Xf &Y_ } 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 +482,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)), @@ -502,7 +517,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 +528,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 +593,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 +606,12 @@ 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) +{ // FIXME: this has a lot of duplicate code this->gripper_idx = gripper_idx; @@ -688,10 +707,12 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, const PointClo return CDCPD::Output{points, cloud, 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 +789,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..9305e30 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,34 @@ 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()}; + // Eigen::Vector3f start_position_2{Eigen::Vector3f::Zero()}; + // Eigen::Vector3f end_position_2{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; + // start_position_1 << 0.5, -node_params.max_rope_length / 2, 1.0; + // end_position_1 << 0.5, node_params.max_rope_length / 2, 1.0; + + // start_position_2 << -0.5, -node_params.max_rope_length / 2, 1.0; + // end_position_2 << -0.5, node_params.max_rope_length / 2, 1.0; } - initialize_deformable_object_configuration(start_position, end_position); + initialize_deformable_object_configuration(start_position_1, end_position_1); + // initialize_deformable_object_configuration(start_position_2, end_position_2); - 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 +200,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 +228,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 +458,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 +471,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 +496,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 +529,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 +543,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 +582,68 @@ 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 rope_marker_fn_multi_templates = [&](std::string const& ns) + { + vm::MarkerArray rope_orders; + int i = 1; + 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; + + 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 = def_obj_id; + order.scale.x = 0.01; + order.color.r = 0.1; + order.color.g = i * 0.5; + order.color.b = 0.9; + order.color.a = 1.0; + + // TODO: This should loop through the edges instead of the points as the edges + // actually indicate which points form edges... + // Though this isn't a huge deal as of right now as the rope points are guaranteed + // to be in edge order if the template was initialized in edge order. + for (auto pc_iter : *def_obj_config->tracked_.points_) + { + geometry_msgs::Point p; + p.x = pc_iter.x; + p.y = pc_iter.y; + p.z = pc_iter.z; + order.points.push_back(p); + } + rope_orders.markers.push_back(order); + ++i; } - return order; + + return rope_orders; }; - auto const rope_marker = rope_marker_fn(out.gurobi_output, "line_order"); - publishers.order_pub.publish(rope_marker); + auto const rope_marker_array = rope_marker_fn_multi_templates("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 +656,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..2d18e85 100644 --- a/cdcpd/src/deformable_object_configuration.cpp +++ b/cdcpd/src/deformable_object_configuration.cpp @@ -6,6 +6,15 @@ 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]; +} + DeformableObjectTracking::DeformableObjectTracking(DeformableObjectTracking const& other) { vertices_ = other.vertices_; @@ -119,6 +128,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}); } 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/tracking_map.cpp b/cdcpd/src/tracking_map.cpp new file mode 100644 index 0000000..7aab3ae --- /dev/null +++ b/cdcpd/src/tracking_map.cpp @@ -0,0 +1,176 @@ +#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_.edges_.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; + std::copy(it_begin, it_end, def_obj_config->tracked_.points_->begin()); + + def_obj_config->tracked_.vertices_ = def_obj_config->tracked_.points_->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->points_); + } + 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->edges_; + 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_.edges_.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..6a846c5 100644 --- a/cdcpd/tests/integration/static_rope.cpp +++ b/cdcpd/tests/integration/static_rope.cpp @@ -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 From d939d27b9f8c781a8c0c8b811c94cfa7252e27cc Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 13:22:00 -0400 Subject: [PATCH 2/8] Rename CPD tolerance to tolerance_cpd - Simply renames the CDCPD class "tolerance" member to "tolerance_cpd" as the former is ambiguous as to what tolerance we're specifying. --- cdcpd/include/cdcpd/cdcpd.h | 2 +- cdcpd/src/cdcpd.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cdcpd/include/cdcpd/cdcpd.h b/cdcpd/include/cdcpd/cdcpd.h index 2fc0c69..15ee3c2 100644 --- a/cdcpd/include/cdcpd/cdcpd.h +++ b/cdcpd/include/cdcpd/cdcpd.h @@ -165,7 +165,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/src/cdcpd.cpp b/cdcpd/src/cdcpd.cpp index 5305e98..e43236b 100644 --- a/cdcpd/src/cdcpd.cpp +++ b/cdcpd/src/cdcpd.cpp @@ -381,12 +381,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(); - while (iterations <= max_iterations && error > tolerance) { + while (iterations <= max_iterations && error > tolerance_cpd) { double qprev = sigma2; // Expectation step int N = X.cols(); @@ -451,7 +451,7 @@ 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); @@ -503,7 +503,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? From 8a3fb9affb64731365e93c030aaba5153bc7bb40 Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 14:06:10 -0400 Subject: [PATCH 3/8] Add class for timing runtime execution - Adds the Stopwatch class which is a super useful context manager that automatically times a code block's runtime duration and outputs to the given logger (by passing in the logger's name). --- cdcpd/CMakeLists.txt | 1 + cdcpd/include/cdcpd/cdcpd.h | 1 + cdcpd/include/cdcpd/stopwatch.h | 20 +++++++ cdcpd/src/cdcpd.cpp | 94 ++++++++++++++++++++------------- cdcpd/src/stopwatch.cpp | 16 ++++++ 5 files changed, 94 insertions(+), 38 deletions(-) create mode 100644 cdcpd/include/cdcpd/stopwatch.h create mode 100644 cdcpd/src/stopwatch.cpp diff --git a/cdcpd/CMakeLists.txt b/cdcpd/CMakeLists.txt index b9c1cb5..e4052f2 100644 --- a/cdcpd/CMakeLists.txt +++ b/cdcpd/CMakeLists.txt @@ -67,6 +67,7 @@ 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 15ee3c2..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; 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/src/cdcpd.cpp b/cdcpd/src/cdcpd.cpp index e43236b..58785ee 100644 --- a/cdcpd/src/cdcpd.cpp +++ b/cdcpd/src/cdcpd.cpp @@ -372,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); @@ -383,8 +384,8 @@ Matrix3Xf CDCPD::cpd(const Matrix3Xf &X, const Matrix3Xf &Y, const Matrix3Xf &Y_ int iterations = 0; 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_cpd) { double qprev = sigma2; @@ -457,8 +458,10 @@ Matrix3Xf CDCPD::cpd(const Matrix3Xf &X, const Matrix3Xf &Y, const Matrix3Xf &Y_ 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; } @@ -612,6 +615,7 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, 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; @@ -620,31 +624,36 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, 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); @@ -655,8 +664,8 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, 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 @@ -675,20 +684,29 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, } 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(); @@ -704,7 +722,7 @@ CDCPD::Output CDCPD::operator()(const PointCloudRGB::Ptr &points, 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, 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 From f3dfdeaf0ff1712a4038a4917273a47b274d8c8d Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 14:07:44 -0400 Subject: [PATCH 4/8] Make tracking vertices, points, and edges private - Makes the vertices, points, and edges of the DeformableObjectTracking class private and provides getters/setters for these. Should prevent people from making changes that will shoot them in the foot later. --- .../cdcpd/deformable_object_configuration.h | 58 +++++++++++- cdcpd/src/cdcpd_node.cpp | 16 +--- cdcpd/src/deformable_object_configuration.cpp | 89 ++++++++++++++++--- cdcpd/src/tracking_map.cpp | 13 +-- cdcpd/tests/integration/static_rope.cpp | 6 +- .../unit/include/ClothConfigurationTest.h | 4 +- .../include/DeformableObjectTrackingTest.h | 74 +++++++++------ .../unit/include/RopeConfigurationTest.h | 4 +- 8 files changed, 196 insertions(+), 68 deletions(-) diff --git a/cdcpd/include/cdcpd/deformable_object_configuration.h b/cdcpd/include/cdcpd/deformable_object_configuration.h index 60d8c6c..bad87b0 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 @@ -18,9 +20,33 @@ enum DeformableObjectType DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_str); -struct DeformableObjectTracking +// 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 +{ +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. @@ -32,12 +58,36 @@ struct DeformableObjectTracking // Sets the vertices from input point vector. void setVertices(std::vector const& vertex_points); + // 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. void setEdges(std::vector> const& edges); - Eigen::Matrix3Xf vertices_{}; - Eigen::Matrix2Xi 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_)); } + +private: + Eigen::Matrix3Xf vertices_; + Eigen::Matrix2Xi edges_; PointCloud::Ptr points_; + ConnectivityGraph connectivity_graph_; }; class DeformableObjectConfiguration diff --git a/cdcpd/src/cdcpd_node.cpp b/cdcpd/src/cdcpd_node.cpp index 9305e30..8373381 100644 --- a/cdcpd/src/cdcpd_node.cpp +++ b/cdcpd/src/cdcpd_node.cpp @@ -139,8 +139,6 @@ CDCPD_Moveit_Node::CDCPD_Moveit_Node(std::string const& robot_namespace) auto [init_q_config, init_q_dot] = get_q_config(); Eigen::Vector3f start_position_1{Eigen::Vector3f::Zero()}; Eigen::Vector3f end_position_1{Eigen::Vector3f::Zero()}; - // Eigen::Vector3f start_position_2{Eigen::Vector3f::Zero()}; - // Eigen::Vector3f end_position_2{Eigen::Vector3f::Zero()}; end_position_1[2] += node_params.max_rope_length; if (gripper_count == 2u) { start_position_1 = init_q_config[0].translation().cast(); @@ -152,15 +150,9 @@ CDCPD_Moveit_Node::CDCPD_Moveit_Node(std::string const& robot_namespace) } else if (gripper_count == 0u) { start_position_1 << -node_params.max_rope_length / 2, 0, 1.0; end_position_1 << node_params.max_rope_length / 2, 0, 1.0; - // start_position_1 << 0.5, -node_params.max_rope_length / 2, 1.0; - // end_position_1 << 0.5, node_params.max_rope_length / 2, 1.0; - - // start_position_2 << -0.5, -node_params.max_rope_length / 2, 1.0; - // end_position_2 << -0.5, node_params.max_rope_length / 2, 1.0; } initialize_deformable_object_configuration(start_position_1, end_position_1); - // initialize_deformable_object_configuration(start_position_2, end_position_2); PointCloud::Ptr vertices = deformable_objects.form_vertices_cloud(); Eigen::Matrix2Xi edges = deformable_objects.form_edges_matrix(); @@ -609,12 +601,12 @@ void CDCPD_Moveit_Node::publish_outputs(ros::Time const& t0, CDCPD::Output const // actually indicate which points form edges... // Though this isn't a huge deal as of right now as the rope points are guaranteed // to be in edge order if the template was initialized in edge order. - for (auto pc_iter : *def_obj_config->tracked_.points_) + for (auto const cloud_point : *def_obj_config->tracked_.getPointCloud()) { geometry_msgs::Point p; - p.x = pc_iter.x; - p.y = pc_iter.y; - p.z = pc_iter.z; + p.x = cloud_point.x; + p.y = cloud_point.y; + p.z = cloud_point.z; order.points.push_back(p); } rope_orders.markers.push_back(order); diff --git a/cdcpd/src/deformable_object_configuration.cpp b/cdcpd/src/deformable_object_configuration.cpp index 2d18e85..07a102f 100644 --- a/cdcpd/src/deformable_object_configuration.cpp +++ b/cdcpd/src/deformable_object_configuration.cpp @@ -15,19 +15,62 @@ DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_ 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? @@ -51,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) @@ -67,6 +116,23 @@ void DeformableObjectTracking::setEdges(std::vector> const& edges_ = edges_new; } +void DeformableObjectTracking::setEdges(Eigen::Matrix2Xi const& edges_in) +{ + edges_ = edges_in; +} + +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(); @@ -101,9 +167,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; } @@ -170,7 +235,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/tracking_map.cpp b/cdcpd/src/tracking_map.cpp index 7aab3ae..758e727 100644 --- a/cdcpd/src/tracking_map.cpp +++ b/cdcpd/src/tracking_map.cpp @@ -24,7 +24,7 @@ int TrackingMap::get_total_num_edges() const for (auto const& tracked_pair : tracking_map) { std::shared_ptr const& def_obj_config = tracked_pair.second; - int num_edges = def_obj_config->tracked_.edges_.cols(); + int num_edges = def_obj_config->tracked_.getEdges().cols(); num_edges_total += num_edges; } return num_edges_total; @@ -78,9 +78,10 @@ void TrackingMap::update_def_obj_vertices(pcl::shared_ptr const vert // 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; - std::copy(it_begin, it_end, def_obj_config->tracked_.points_->begin()); + def_obj_config->tracked_.setPointCloud(it_begin, it_end); - def_obj_config->tracked_.vertices_ = def_obj_config->tracked_.points_->getMatrixXfMap().topRows(3); + PointCloud::ConstPtr tracked_cloud = def_obj_config->tracked_.getPointCloud(); + def_obj_config->tracked_.setVertices(tracked_cloud->getMatrixXfMap().topRows(3)); } } @@ -99,7 +100,7 @@ PointCloud::Ptr TrackingMap::form_vertices_cloud(bool const use_initial_state) c get_appropriate_tracking(def_obj_config, use_initial_state); // Concatenate this deformable object's point cloud with the aggregate point cloud. - (*vertices_cloud) += (*tracking->points_); + (*vertices_cloud) += (*tracking->getPointCloud()); } return vertices_cloud; } @@ -122,7 +123,7 @@ Eigen::Matrix2Xi TrackingMap::form_edges_matrix(bool const use_initial_state) co std::shared_ptr tracking = get_appropriate_tracking(def_obj_config, use_initial_state); - Eigen::Matrix2Xi const& edges = tracking->edges_; + 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); @@ -149,7 +150,7 @@ Eigen::RowVectorXd TrackingMap::form_max_segment_length_matrix() const 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_.edges_.cols(); ++i) + 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; diff --git a/cdcpd/tests/integration/static_rope.cpp b/cdcpd/tests/integration/static_rope.cpp index 6a846c5..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); @@ -117,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..cf4e8bc 100644 --- a/cdcpd/tests/unit/include/ClothConfigurationTest.h +++ b/cdcpd/tests/unit/include/ClothConfigurationTest.h @@ -64,6 +64,6 @@ TEST(ClothConfigurationTest, simpleClothConfigurationTemplateInitializationBadGr tracking_expected.setVertices(vertices_expected); tracking_expected.setEdges(edges_expected); - expectEigenMatsEqual(config_test.tracked_.vertices_, tracking_expected.vertices_); - expectEigenMatsEqual(config_test.tracked_.edges_, tracking_expected.edges_); + expectEigenMatsEqual(config_test.tracked_.getVertices(), tracking_expected.getVertices()); + expectEigenMatsEqual(config_test.tracked_.getEdges(), tracking_expected.getEdges()); } \ No newline at end of file diff --git a/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h b/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h index ac30202..21eb2fe 100644 --- a/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h +++ b/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h @@ -4,32 +4,54 @@ #include "cdcpd/deformable_object_configuration.h" +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); + + } + + 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, copyConstructor) { - 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); } \ 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 From be00408fa39dc1ff340a416f347d8715ce0fa2ec Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 14:10:55 -0400 Subject: [PATCH 5/8] Add testing of ConnectivityGraph - Adds testing of ConnectivityGraph in DeformableObjectTracking's tests. Not ideal but at least it has some tests. --- .../cdcpd/deformable_object_configuration.h | 10 +- cdcpd/src/deformable_object_configuration.cpp | 3 +- .../include/DeformableObjectTrackingTest.h | 114 +++++++++++++++++- 3 files changed, 123 insertions(+), 4 deletions(-) diff --git a/cdcpd/include/cdcpd/deformable_object_configuration.h b/cdcpd/include/cdcpd/deformable_object_configuration.h index bad87b0..c51b135 100644 --- a/cdcpd/include/cdcpd/deformable_object_configuration.h +++ b/cdcpd/include/cdcpd/deformable_object_configuration.h @@ -62,14 +62,16 @@ class DeformableObjectTracking // vertices void setVertices(Eigen::Matrix3Xf const& vertices_in); - // Sets the edges from input edge vector. + // Sets the edges from input edge vector and updates the connectivity graph describing edges. void setEdges(std::vector> const& 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); + void setPointCloud(PointCloud::const_iterator const& it_in_begin, + PointCloud::const_iterator const& it_in_end); Eigen::Matrix3Xf const& getVertices() const { return vertices_; } @@ -83,6 +85,10 @@ class DeformableObjectTracking 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_; diff --git a/cdcpd/src/deformable_object_configuration.cpp b/cdcpd/src/deformable_object_configuration.cpp index 07a102f..ce3a4c5 100644 --- a/cdcpd/src/deformable_object_configuration.cpp +++ b/cdcpd/src/deformable_object_configuration.cpp @@ -113,12 +113,13 @@ 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) diff --git a/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h b/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h index 21eb2fe..f08a4d9 100644 --- a/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h +++ b/cdcpd/tests/unit/include/DeformableObjectTrackingTest.h @@ -4,6 +4,8 @@ #include "cdcpd/deformable_object_configuration.h" +#include + class DeformableObjectTrackingTest : public ::testing::Test { protected: @@ -29,7 +31,89 @@ class DeformableObjectTrackingTest : public ::testing::Test 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; @@ -39,7 +123,7 @@ class DeformableObjectTrackingTest : public ::testing::Test // Tests that the members of DeformableObjectTracking are properly copied in the copy constructor // and not just the pointers. // NOTE: This also tests getVertices, getEdges, and getPointCloud methods. -TEST_F(DeformableObjectTrackingTest, copyConstructor) +TEST_F(DeformableObjectTrackingTest, copyConstructorDifferentTracks) { // Check that modifications to track 1 didn't carry over to track 2. auto const& track_1_vertices = track_1->getVertices(); @@ -54,4 +138,32 @@ TEST_F(DeformableObjectTrackingTest, copyConstructor) 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 From 82001cfb9a084edec489a78be3170cccbaf19ee9 Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 14:12:10 -0400 Subject: [PATCH 6/8] Fix bug in cloth edge initialization - When a non-square cloth was tracked, not all edges were connected. This was due to using the number of points in the length direction for the width direction. --- cdcpd/src/deformable_object_configuration.cpp | 2 +- .../unit/include/ClothConfigurationTest.h | 100 ++++++++++++++++-- 2 files changed, 93 insertions(+), 9 deletions(-) diff --git a/cdcpd/src/deformable_object_configuration.cpp b/cdcpd/src/deformable_object_configuration.cpp index ce3a4c5..0f39943 100644 --- a/cdcpd/src/deformable_object_configuration.cpp +++ b/cdcpd/src/deformable_object_configuration.cpp @@ -217,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_)}); } diff --git a/cdcpd/tests/unit/include/ClothConfigurationTest.h b/cdcpd/tests/unit/include/ClothConfigurationTest.h index cf4e8bc..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_.getVertices(), tracking_expected.getVertices()); - expectEigenMatsEqual(config_test.tracked_.getEdges(), tracking_expected.getEdges()); + 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 From c4b02999b91b36b0fd5f49a80a3e458091aad7b8 Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 14:17:04 -0400 Subject: [PATCH 7/8] Fix edge RVIZ visualization (#47) - Fixes edge visualization in RVIZ to actually respect the edges specfied in the template instead of just going off of point order. --- cdcpd/src/cdcpd_node.cpp | 75 ++++++++++++++++++++++++---------------- 1 file changed, 46 insertions(+), 29 deletions(-) diff --git a/cdcpd/src/cdcpd_node.cpp b/cdcpd/src/cdcpd_node.cpp index 8373381..937b706 100644 --- a/cdcpd/src/cdcpd_node.cpp +++ b/cdcpd/src/cdcpd_node.cpp @@ -574,49 +574,66 @@ 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_multi_templates = [&](std::string const& ns) + auto form_edge_markers = [&](std::string const& ns) { - vm::MarkerArray rope_orders; + vm::MarkerArray edge_markers; int i = 1; + int marker_id = 0; for (auto const& def_obj_pair : deformable_objects.tracking_map) { int const& def_obj_id = def_obj_pair.first; auto const& def_obj_config = def_obj_pair.second; - 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 = def_obj_id; - order.scale.x = 0.01; - order.color.r = 0.1; - order.color.g = i * 0.5; - order.color.b = 0.9; - order.color.a = 1.0; - - // TODO: This should loop through the edges instead of the points as the edges - // actually indicate which points form edges... - // Though this isn't a huge deal as of right now as the rope points are guaranteed - // to be in edge order if the template was initialized in edge order. - for (auto const cloud_point : *def_obj_config->tracked_.getPointCloud()) + 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) { - geometry_msgs::Point p; - p.x = cloud_point.x; - p.y = cloud_point.y; - p.z = cloud_point.z; - order.points.push_back(p); + 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; } - rope_orders.markers.push_back(order); + ++i; } - return rope_orders; + return edge_markers; }; - auto const rope_marker_array = rope_marker_fn_multi_templates("line_order"); + auto const rope_marker_array = form_edge_markers("line_order"); publishers.order_pub.publish(rope_marker_array); } From d6313785343e86b96246837a2ca555819f2b9494 Mon Sep 17 00:00:00 2001 From: Dylan Colli Date: Mon, 27 Mar 2023 14:30:24 -0400 Subject: [PATCH 8/8] Clean up compiler warnings - Cleans up some compiler warnings introduced in earlier commits --- .../cdcpd/deformable_object_configuration.h | 20 +++++++++---------- cdcpd/src/deformable_object_configuration.cpp | 4 ++-- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/cdcpd/include/cdcpd/deformable_object_configuration.h b/cdcpd/include/cdcpd/deformable_object_configuration.h index c51b135..119f966 100644 --- a/cdcpd/include/cdcpd/deformable_object_configuration.h +++ b/cdcpd/include/cdcpd/deformable_object_configuration.h @@ -112,8 +112,8 @@ class DeformableObjectConfiguration int num_points_; float max_segment_length_; - DeformableObjectTracking tracked_; DeformableObjectTracking initial_; + DeformableObjectTracking tracked_; }; class RopeConfiguration : public DeformableObjectConfiguration @@ -147,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_; @@ -156,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/src/deformable_object_configuration.cpp b/cdcpd/src/deformable_object_configuration.cpp index 0f39943..f5a8f93 100644 --- a/cdcpd/src/deformable_object_configuration.cpp +++ b/cdcpd/src/deformable_object_configuration.cpp @@ -180,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_} {