Add Tracking Map, Fix cloth edge visualization

cdcpd/CMakeLists.txt

@@ -67,6 +67,8 @@ add_library(cdcpd SHARED
         src/obs_util.cpp
         src/past_template_matcher.cpp
         src/segmenter.cpp
+        src/stopwatch.cpp
+        src/tracking_map.cpp
         )
 target_include_directories(cdcpd PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

cdcpd/include/cdcpd/cdcpd.h

@@ -29,6 +29,7 @@
 #include "cdcpd/obs_util.h"
 #include "cdcpd/optimizer.h"
 #include "cdcpd/past_template_matcher.h"
+#include "cdcpd/stopwatch.h"
 
 typedef pcl::PointXYZRGB PointRGB;
 typedef pcl::PointXYZHSV PointHSV;
@@ -98,49 +99,56 @@ class CDCPD {
     OutputStatus status;
   };
 
-  CDCPD(PointCloud::ConstPtr template_cloud, const Eigen::Matrix2Xi &_template_edges, float objective_value_threshold,
-        bool use_recovery = false, double alpha = 0.5, double beta = 1.0, double lambda = 1.0, double k = 100.0,
-        float zeta = 10.0, float obstacle_cost_weight = 1.0, float fixed_points_weight = 10.0);
+  CDCPD(PointCloud::ConstPtr template_cloud, const Eigen::Matrix2Xi &_template_edges,
+      float objective_value_threshold, bool use_recovery = false, double alpha = 0.5,
+      double beta = 1.0, double lambda = 1.0, double k = 100.0, float zeta = 10.0,
+      float obstacle_cost_weight = 1.0, float fixed_points_weight = 10.0);
 
   CDCPD(ros::NodeHandle nh, ros::NodeHandle ph, PointCloud::ConstPtr template_cloud,
-        const Eigen::Matrix2Xi &_template_edges, float objective_value_threshold, bool use_recovery = false,
-        double alpha = 0.5, double beta = 1.0, double lambda = 1.0, double k = 100.0, float zeta = 10.0,
-        float obstacle_cost_weight = 1.0, float fixed_points_weight = 10.0);
-
-  // If you have want gripper constraints to be added & removed automatically based on is_grasped & distance
-  Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cv::Matx33d &intrinsics,
-                    const PointCloud::Ptr template_cloud, ObstacleConstraints points_normals, double max_segment_length,
-                    const smmap::AllGrippersSinglePoseDelta &q_dot = {},
-                    const smmap::AllGrippersSinglePose &q_config = {}, const std::vector<bool> &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<bool> &is_grasped = {},
+      int pred_choice = 0);
 
   // If you want to used a known correspondence between grippers and node indices (gripper_idx)
-  Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cv::Matx33d &intrinsics,
-                    const PointCloud::Ptr template_cloud, ObstacleConstraints points_normals, double max_segment_length,
-                    const smmap::AllGrippersSinglePoseDelta &q_dot = {},
-                    const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {},
-                    int pred_choice = 0);
+  Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask,
+      const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud,
+      ObstacleConstraints points_normals, Eigen::RowVectorXd const max_segment_length,
+      const smmap::AllGrippersSinglePoseDelta &q_dot = {},
+      const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {},
+      int pred_choice = 0);
 
   Output operator()(const PointCloudRGB::Ptr &points, const PointCloud::Ptr template_cloud,
-                    ObstacleConstraints points_normals, double max_segment_length,
-                    const smmap::AllGrippersSinglePoseDelta &q_dot = {},
-                    const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {},
-                    int pred_choice = 0);
+      ObstacleConstraints points_normals, Eigen::RowVectorXd const max_segment_length,
+      const smmap::AllGrippersSinglePoseDelta &q_dot = {},
+      const smmap::AllGrippersSinglePose &q_config = {}, const Eigen::MatrixXi &gripper_idx = {},
+      int pred_choice = 0);
 
   // The common implementation that the above overloads call
-  Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cv::Matx33d &intrinsics,
-                    const PointCloud::Ptr template_cloud, ObstacleConstraints points_normals, double max_segment_length,
-                    const smmap::AllGrippersSinglePoseDelta &q_dot = {},  // TODO: this should be one data structure
-                    const smmap::AllGrippersSinglePose &q_config = {}, int pred_choice = 0);
+  Output operator()(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask,
+      const cv::Matx33d &intrinsics, const PointCloud::Ptr template_cloud,
+      ObstacleConstraints points_normals, Eigen::RowVectorXd const max_segment_length,
+      const smmap::AllGrippersSinglePoseDelta &q_dot = {},  // TODO: this should be one data structure
+      const smmap::AllGrippersSinglePose &q_config = {}, int pred_choice = 0);
 
-  Eigen::VectorXf visibility_prior(const Eigen::Matrix3Xf &vertices, const cv::Mat &depth, const cv::Mat &mask,
-                                   const Eigen::Matrix3f &intrinsics, float kvis);
+  Eigen::VectorXf visibility_prior(const Eigen::Matrix3Xf &vertices, const cv::Mat &depth,
+      const cv::Mat &mask, const Eigen::Matrix3f &intrinsics, float kvis);
 
-  Eigen::Matrix3Xf cpd(const Eigen::Matrix3Xf &X, const Eigen::Matrix3Xf &Y, const Eigen::Matrix3Xf &Y_pred,
-                       const Eigen::VectorXf &Y_emit_prior);
+  Eigen::Matrix3Xf cpd(const Eigen::Matrix3Xf &X, const Eigen::Matrix3Xf &Y,
+      const Eigen::Matrix3Xf &Y_pred, const Eigen::VectorXf &Y_emit_prior);
 
-  Eigen::Matrix3Xd predict(const Eigen::Matrix3Xd &P, const smmap::AllGrippersSinglePoseDelta &q_dot,
-                           const smmap::AllGrippersSinglePose &q_config, int pred_choice);
+  Eigen::Matrix3Xd predict(const Eigen::Matrix3Xd &P,
+      const smmap::AllGrippersSinglePoseDelta &q_dot,
+      const smmap::AllGrippersSinglePose &q_config, int pred_choice);
 
   ros::NodeHandle nh;
   ros::NodeHandle ph;
@@ -158,7 +166,7 @@ class CDCPD {
   int lle_neighbors;
   Eigen::MatrixXf m_lle;
   Eigen::MatrixXf L_lle;
-  double tolerance;
+  double tolerance_cpd;
   double alpha;
   double beta;
   double w;

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<pcl::PointXYZ> PointCloud;
 namespace gm = geometry_msgs;
@@ -139,7 +140,7 @@ struct CDCPD_Moveit_Node {
     unsigned int gripper_count;
     Eigen::MatrixXi gripper_indices;
 
-    std::unique_ptr<DeformableObjectConfiguration> deformable_object_configuration_;
+    TrackingMap deformable_objects;
 
     tf2_ros::Buffer tf_buffer_;
     tf2_ros::TransformListener tf_listener_;

cdcpd/include/cdcpd/deformable_object_configuration.h

@@ -8,6 +8,8 @@
 #include "opencv2/imgproc.hpp"
 #include <opencv2/core/eigen.hpp>
 
+#include <map>
+
 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
 
 enum DeformableObjectType
@@ -16,18 +18,35 @@ enum DeformableObjectType
     cloth
 };
 
-DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_str)
+DeformableObjectType get_deformable_object_type(std::string const& def_obj_type_str);
+
+// TODO(Dylan): Clean this up and actually implement a graph traversal/construction algorithm that
+// isn't just dumping all of the nodes into a big list.
+class ConnectivityNode
 {
-    std::map<std::string, DeformableObjectType> obj_type_map{
-        {"rope", DeformableObjectType::rope},
-        {"cloth", DeformableObjectType::cloth}
-    };
-    return obj_type_map[def_obj_type_str];
-}
-
-struct DeformableObjectTracking
+public:
+    ConnectivityNode(int const node_id);
+
+    void add_neighbor_node(std::shared_ptr<ConnectivityNode> const neighbor);
+
+    int const id;
+    std::map<int, std::shared_ptr<ConnectivityNode> > 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<int, std::shared_ptr<ConnectivityNode> > nodes;
+};
+
+class DeformableObjectTracking
 {
-    DeformableObjectTracking(){}
+public:
+    DeformableObjectTracking();
 
     // Turns the passed in Matrix of XYZ points to a point cloud.
     // TODO(dylan.colli): remove argument. Should just use the vertices member.
@@ -39,12 +58,42 @@ struct DeformableObjectTracking
     // Sets the vertices from input point vector.
     void setVertices(std::vector<cv::Point3f> const& vertex_points);
 
-    // Sets the edges from input edge vector.
+    // Sets the vertices member equal to given vertices and forms the point cloud from these
+    // vertices
+    void setVertices(Eigen::Matrix3Xf const& vertices_in);
+
+    // Sets the edges from input edge vector and updates the connectivity graph describing edges.
     void setEdges(std::vector<std::tuple<int, int>> const& edges);
 
-    Eigen::Matrix3Xf vertices_{};
-    Eigen::Matrix2Xi edges_{};
+    // Sets the edges and updates the connectivity graph describing edges.
+    void setEdges(Eigen::Matrix2Xi const& edges_in);
+
+    // Sets this tracking's point cloud using std::copy and another point cloud's iterators defining
+    // the range over which to copy the points.
+    void setPointCloud(PointCloud::const_iterator const& it_in_begin,
+        PointCloud::const_iterator const& it_in_end);
+
+    Eigen::Matrix3Xf const& getVertices() const { return vertices_; }
+
+    Eigen::Matrix3Xf getVerticesCopy() const { return vertices_; }
+
+    Eigen::Matrix2Xi const& getEdges() const { return edges_; }
+
+    Eigen::Matrix2Xi getEdgesCopy() const { return edges_; }
+
+    PointCloud::ConstPtr const getPointCloud() const { return points_; }
+
+    PointCloud::Ptr getPointCloudCopy() const { return PointCloud::Ptr(new PointCloud(*points_)); }
+
+    ConnectivityGraph const& getConnectivityGraph() const { return connectivity_graph_; }
+
+    ConnectivityGraph getConnectivityGraphCopy() const { return connectivity_graph_; }
+
+private:
+    Eigen::Matrix3Xf vertices_;
+    Eigen::Matrix2Xi edges_;
     PointCloud::Ptr points_;
+    ConnectivityGraph connectivity_graph_;
 };
 
 class DeformableObjectConfiguration
@@ -63,8 +112,8 @@ class DeformableObjectConfiguration
 
     int num_points_;
     float max_segment_length_;
-    DeformableObjectTracking tracked_;
     DeformableObjectTracking initial_;
+    DeformableObjectTracking tracked_;
 };
 
 class RopeConfiguration : public DeformableObjectConfiguration
@@ -98,6 +147,15 @@ class ClothConfiguration : public DeformableObjectConfiguration
         0, 0, 1, 0,
         0, 0, 0, 1);
 
+    // Initial length of the template.
+    float const length_initial_;
+
+    // Initial width of the template.
+    float const width_initial_;
+
+    // The supplied initial guess for the grid size.
+    float const grid_size_initial_guess_;
+
     // Describes the number of points the template is initialized with in the length direction.
     // Length is downwards direction!
     int const num_points_length_;
@@ -107,13 +165,4 @@ class ClothConfiguration : public DeformableObjectConfiguration
     int const num_points_width_;
 
     int const num_edges_;
-
-    // Initial length of the template.
-    float const length_initial_;
-
-    // Initial width of the template.
-    float const width_initial_;
-
-    // The supplied initial guess for the grid size.
-    float const grid_size_initial_guess_;
 };

cdcpd/include/cdcpd/optimizer.h

@@ -53,40 +53,39 @@ using ObstacleConstraints = std::vector<ObstacleConstraint>;
 
 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<Eigen::Matrix3Xf, double> operator()(const Eigen::Matrix3Xf &Y, const Eigen::Matrix2Xi &E,
-                                                               const std::vector<FixedPoint> &fixed_points,
-                                                               ObstacleConstraints const &points_normals,
-                                                               double max_segment_length);
+  [[nodiscard]] std::pair<Eigen::Matrix3Xf, double> operator()(const Eigen::Matrix3Xf &Y,
+      const Eigen::Matrix2Xi &E, const std::vector<FixedPoint> &fixed_points,
+      ObstacleConstraints const &points_normals, Eigen::RowVectorXd const max_segment_length);
 
-  std::tuple<Points, Normals> test_box(const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &box,
-                                       geometry_msgs::Pose const &pose);
+  std::tuple<Points, Normals> 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<FixedPoint> &fixed_points) const;
 
-  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_box(const Eigen::Matrix3Xf &last_template,
-                                                                          shape_msgs::SolidPrimitive const &box,
-                                                                          geometry_msgs::Pose const &pose);
+  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_box(
+      const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &box,
+      geometry_msgs::Pose const &pose);
 
-  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_sphere(const Eigen::Matrix3Xf &last_template,
-                                                                             shape_msgs::SolidPrimitive const &sphere,
-                                                                             geometry_msgs::Pose const &pose);
+  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_sphere(
+      const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &sphere,
+      geometry_msgs::Pose const &pose);
 
-  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_plane(const Eigen::Matrix3Xf &last_template,
-                                                                            shape_msgs::Plane const &plane);
+  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_plane(
+      const Eigen::Matrix3Xf &last_template, shape_msgs::Plane const &plane);
 
   [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_cylinder(
       const Eigen::Matrix3Xf &last_template, shape_msgs::SolidPrimitive const &cylinder,
       geometry_msgs::Pose const &pose);
 
-  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_mesh(const Eigen::Matrix3Xf &last_template,
-                                                                           shape_msgs::Mesh const &shapes_mesh);
+  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal_mesh(
+      const Eigen::Matrix3Xf &last_template, shape_msgs::Mesh const &shapes_mesh);
 
-  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal(const Eigen::Matrix3Xf &last_template,
-                                                                      Objects const &objects);
+  [[nodiscard]] std::tuple<Points, Normals> nearest_points_and_normal(
+      const Eigen::Matrix3Xf &last_template, Objects const &objects);
 
   Eigen::Matrix3Xf initial_template_;
   Eigen::Matrix3Xf last_template_;

cdcpd/include/cdcpd/stopwatch.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <chrono>
+#include <string>
+
+#include <ros/ros.h>
+
+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_;
+};