Multi tracking

.clang-format

@@ -0,0 +1,87 @@
+Language: Cpp
+# BasedOnStyle: Google
+AccessModifierOffset: -1
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlinesLeft: true
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: true
+BinPackParameters: true
+BraceWrapping:
+  AfterClass: false
+  AfterControlStatement: false
+  AfterEnum: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterObjCDeclaration: false
+  AfterStruct: false
+  AfterUnion: false
+  BeforeCatch: false
+  BeforeElse: false
+  IndentBraces: false
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Attach
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+ColumnLimit: 80
+CommentPragmas: '^ IWYU pragma:'
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
+IncludeCategories:
+  - Regex: '^<.*\.h>'
+    Priority: 1
+  - Regex: '^<.*'
+    Priority: 2
+  - Regex: '.*'
+    Priority: 3
+IndentCaseLabels: true
+IndentWidth: 2
+IndentWrappedFunctionNames: false
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: false
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: true
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+TabWidth: 8
+UseTab: Never

.gitignore

@@ -46,6 +46,9 @@ qtcreator-*
 # Emacs
 .#*
 
+# Vim
+*.swp
+
 # Catkin custom files
 CATKIN_IGNORE
 tags

object_analytics_node/CMakeLists.txt

@@ -64,6 +64,8 @@ find_package(PCL 1.7 REQUIRED COMPONENTS
 
 include_directories(
   include
+  include/object_analytics_node
+  include/object_analytics_node/common
   ${object_analytics_msgs_INCLUDE_DIRS}
   ${message_filters_INCLUDE_DIRS}
   ${pcl_conversions_INCLUDE_DIRS}
@@ -118,6 +120,7 @@ if(${BUILD_TRACKING})
   add_executable(tracker_regression src/tracker/tracking_regression.cpp
     src/dataset/tr_dataset.cpp
     src/dataset/trimg_dataset.cpp
+    src/dataset/trimg_MTdataset.cpp
     src/dataset/trvid_dataset.cpp)
   ament_target_dependencies(tracker_regression
     "object_analytics_msgs"
@@ -133,7 +136,12 @@ if(${BUILD_TRACKING})
   add_library(tracking_component SHARED
     src/tracker/tracking_node.cpp
     src/tracker/tracking.cpp
+    src/tracker/trackerKCF.cpp
     src/tracker/tracking_manager.cpp
+    src/filter/kalman.cpp
+    src/common/frame.cpp
+    src/common/frame_obj.cpp
+    src/util/logger.cpp
   )
   target_compile_definitions(tracking_component
     PRIVATE "OBJECT_ANALYTICS_NODE_BUILDING_DLL"
@@ -159,6 +167,7 @@ add_library(segmenter_component SHARED
   src/segmenter/segmenter.cpp
   src/segmenter/algorithm_provider_impl.cpp
   src/segmenter/organized_multi_plane_segmenter.cpp
+  src/util/logger.cpp
 )
 target_compile_definitions(segmenter_component
   PRIVATE "OBJECT_ANALYTICS_NODE_BUILDING_DLL"

object_analytics_node/src/visualizer/data/frame.hpp → object_analytics_node/include/object_analytics_node/common/frame.hpp

@@ -12,7 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#pragma once
+#ifndef OBJECT_ANALYTICS_NODE__COMMON__FRAME_HPP_
+#define OBJECT_ANALYTICS_NODE__COMMON__FRAME_HPP_
+
+#include <opencv2/opencv.hpp>
 
 #include <atomic>
 #include <chrono>
@@ -28,33 +31,44 @@
 #include <thread>
 #include <vector>
 
-#include <opencv2/opencv.hpp>
-
-#include "utility.hpp"
 #include "object.hpp"
+#include "utility.hpp"
 
-class sFrame {
- public:
-  sFrame(){};
-  sFrame(cv::Mat &cv_frame);
+class sFrame
+{
+public:
+  sFrame() {}
+  explicit sFrame(cv::Mat & cv_frame);
+  sFrame(cv::Mat & cv_frame, struct timespec st);
 
   virtual ~sFrame() = default;
 
-  bool operator==(const sFrame &c) { return (c.stamp == stamp); };
+  bool operator==(const sFrame & c)
+  {
+    return c.stamp.tv_sec == stamp.tv_sec && c.stamp.tv_nsec == stamp.tv_nsec;
+  }
 
-  bool operator!=(const sFrame &c) { return !(c.stamp == stamp); };
+  bool operator!=(const sFrame & c)
+  {
+    return c.stamp.tv_sec != stamp.tv_sec || c.stamp.tv_nsec != stamp.tv_nsec;
+  }
 
   /**
    * @brief Generate sframe from cv::Mat
    */
-  virtual void genFrame(cv::Mat &cv_frame);
+  virtual void genFrame(cv::Mat & cv_frame);
 
   /**
    * @brief Read time stamp for each frame
    */
-  std::time_t getTimeStamp();
+  struct timespec getTimeStamp();
 
- public:
+public:
   cv::Mat frame;
-  std::time_t stamp;
+  struct timespec stamp;
 };
+
+extern bool operator<(const struct timespec & lhs, const struct timespec & rhs);
+extern bool operator==(const struct timespec & lhs, const struct timespec & rhs);
+
+#endif  // OBJECT_ANALYTICS_NODE__COMMON__FRAME_HPP_

object_analytics_node/src/visualizer/data/frame_obj.hpp → object_analytics_node/include/object_analytics_node/common/frame_obj.hpp

@@ -12,7 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#pragma once
+#ifndef OBJECT_ANALYTICS_NODE__COMMON__FRAME_OBJ_HPP_
+#define OBJECT_ANALYTICS_NODE__COMMON__FRAME_OBJ_HPP_
+
+#include <opencv2/opencv.hpp>
 
 #include <atomic>
 #include <chrono>
@@ -28,35 +31,37 @@
 #include <thread>
 #include <vector>
 
-#include <opencv2/opencv.hpp>
 
-#include "utility.hpp"
 #include "frame.hpp"
+#include "utility.hpp"
 
-class FrameObjs : public sFrame{
- public:
-  FrameObjs(){};
+class FrameObjs : public sFrame
+{
+public:
+  FrameObjs() {}
 
   virtual ~FrameObjs() = default;
 
   /**
    * @brief Generate sframe from cv::Mat and frame index
    */
-  virtual void genFrame(cv::Mat &cv_frame, int idx);
+  virtual void genFrame(cv::Mat & cv_frame, int idx);
 
   /**
    * @brief Add detection for the frame
    */
-  void AddDetection(Object& detect);
+  void AddDetection(Object & detect);
 
   /**
    * @brief Add track for the frame
    */
-  void AddTrack(Object& track);
+  void AddTrack(Object & track);
 
- public:
+public:
   int frame_idx = -1;
 
   std::vector<Object> dets;
   std::vector<Object> tracks;
 };
+
+#endif  // OBJECT_ANALYTICS_NODE__COMMON__FRAME_OBJ_HPP_

object_analytics_node/src/visualizer/data/object.hpp → object_analytics_node/include/object_analytics_node/common/object.hpp

@@ -12,7 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#pragma once
+#ifndef OBJECT_ANALYTICS_NODE__COMMON__OBJECT_HPP_
+#define OBJECT_ANALYTICS_NODE__COMMON__OBJECT_HPP_
+
+#include <opencv2/opencv.hpp>
 
 #include <atomic>
 #include <chrono>
@@ -28,18 +31,22 @@
 #include <thread>
 #include <vector>
 
-#include <opencv2/opencv.hpp>
-
-class Object {
+class Object
+{
 public:
-  Object(){};
-  ~Object(){};
+  Object() {}
+  ~Object() {}
 
 public:
   int ObjectIdx_;
+  std::string Category_;
   cv::Rect2d BoundBox_;
-  float Confidence;
+  float Confidence_;
+
+  struct timespec Stamp_;
+
   cv::Mat Mean_;
   cv::Mat Covariance_;
 };
 
+#endif  // OBJECT_ANALYTICS_NODE__COMMON__OBJECT_HPP_

object_analytics_node/src/visualizer/utils/utility.hpp → object_analytics_node/include/object_analytics_node/common/utility.hpp

@@ -12,19 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#pragma once
+#ifndef OBJECT_ANALYTICS_NODE__COMMON__UTILITY_HPP_
+#define OBJECT_ANALYTICS_NODE__COMMON__UTILITY_HPP_
 
-#define __FILENAME__ \
-  (strrchr(__FILE__, '/') ? (strrchr(__FILE__, '/') + 1) : __FILE__)
-
-#ifndef NDEBUG
-#define TRACE_INFO(fmt, args...)                                  \
-  printf("[INFO]: %s(%d)<%s>\t" fmt "\n", __FILENAME__, __LINE__, \
-         __FUNCTION__, ##args)
-#else
-#define TRACE_INFO(fmt, args...)
+#ifndef UNUSED
+#define UNUSED(var) \
+  do { \
+    (void)(var); \
+  } while (0)
 #endif
 
-#define TRACE_ERR(fmt, args...)                                   \
-  printf("[ERR ]: %s(%d)<%s>\t" fmt "\n", __FILENAME__, __LINE__, \
-         __FUNCTION__, ##args)
+#endif  // OBJECT_ANALYTICS_NODE__COMMON__UTILITY_HPP_

object_analytics_node/include/object_analytics_node/const.hpp

@@ -26,13 +26,20 @@ namespace object_analytics_node
 class Const
 {
 public:
-  static const char kTopicRegisteredPC2[];/**< Topic name of splitter node's input message */
-  static const char kTopicPC2[];          /**< Topic name of segmenter node's input message */
-  static const char kTopicRgb[];          /**< Topic name of 2d detection's input message */
-  static const char kTopicSegmentation[]; /**< Topic name of segmenter node's output message*/
-  static const char kTopicDetection[];    /**< Topic name of 2d detection's output message */
-  static const char kTopicLocalization[]; /**< Topic name of merger node's output message */
-  static const char kTopicTracking[];     /**< Topic name of tracker node's output message */
+  static const char
+    kTopicRegisteredPC2[];   /**< Topic name of splitter node's input message */
+  static const char
+    kTopicPC2[];   /**< Topic name of segmenter node's input message */
+  static const char
+    kTopicRgb[];   /**< Topic name of 2d detection's input message */
+  static const char
+    kTopicSegmentation[];   /**< Topic name of segmenter node's output message*/
+  static const char
+    kTopicDetection[];   /**< Topic name of 2d detection's output message */
+  static const char
+    kTopicLocalization[];   /**< Topic name of merger node's output message */
+  static const char
+    kTopicTracking[];   /**< Topic name of tracker node's output message */
 };
 }  // namespace object_analytics_node
 #endif  // OBJECT_ANALYTICS_NODE__CONST_HPP_