Line data    Source code

       1             : #include "ObjectInterpreter.hpp"
       2             : #include <boost/filesystem.hpp>
       3             : #include <boost/filesystem/fstream.hpp>
       4             : 
       5             : namespace HLR
       6             : {
       7             : namespace PointCloud
       8             : {
       9           1 : std::vector<Object> ObjectInterpreter::get_current_objects(
      10             :     const HLR::Vision::CupDetection& detected_cups,
      11             :     const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
      12             : {
      13           1 :     merge_objects(detected_cups, point_cloud);
      14           1 :     return current_objects;
      15             : }
      16             : 
      17           3 : std::vector<Object> ObjectInterpreter::get_changed_objects(
      18             :     const HLR::Vision::CupDetection& detected_cups,
      19             :     const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
      20             : {
      21           3 :     merge_objects(detected_cups, point_cloud);
      22           3 :     std::vector<Object> changed_objects;
      23             : 
      24           3 :     for (const auto& i : current_objects)
      25             :     {
      26           0 :         add_only_changed_objects(i, changed_objects);
      27             :     }
      28             : 
      29           3 :     return changed_objects;
      30             : }
      31             : 
      32           2 : void ObjectInterpreter::add_only_changed_objects(
      33             :     const Object& object,
      34             :     std::vector<Object>& changed_objects) const
      35             : {
      36           2 :     bool is_new = true;
      37           6 :     for (const auto& i : history.front())
      38             :     {
      39           4 :         if (object.id == i.id)
      40             :         {
      41           2 :             is_new = false;
      42           2 :             if (object != i)
      43             :             {
      44           2 :                 changed_objects.push_back(object);
      45             :             }
      46             :         }
      47             :     }
      48           2 :     if (is_new)
      49             :     {
      50           1 :         changed_objects.push_back(object);
      51             :     }
      52           2 : }
      53             : 
      54           4 : void ObjectInterpreter::merge_objects(
      55             :     const HLR::Vision::CupDetection& detected_cups,
      56             :     const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
      57             : {
      58           4 :     history.push_front(std::move(current_objects));
      59           4 :     current_objects = lidar_interpreter.get_objects(detected_cups, point_cloud);
      60           4 : }
      61             : 
      62             : } // namespace PointCloud
      63           3 : } // namespace HLR