diff --git a/robosherlock/descriptors/annotators/tracker/TrackingAnnotator.yaml b/robosherlock/descriptors/annotators/tracker/TrackingAnnotator.yaml new file mode 100644 index 00000000..ccb13b48 --- /dev/null +++ b/robosherlock/descriptors/annotators/tracker/TrackingAnnotator.yaml @@ -0,0 +1,8 @@ +annotator: + implementation: rs_TrackingAnnotator + name: TrackingAnnotator +parameters: + test_param: 0.01 +capabilities: + inputs: {} + outputs: {} diff --git a/robosherlock/src/tracking/CMakeLists.txt b/robosherlock/src/tracking/CMakeLists.txt index c51c200b..72dacdb8 100644 --- a/robosherlock/src/tracking/CMakeLists.txt +++ b/robosherlock/src/tracking/CMakeLists.txt @@ -14,10 +14,13 @@ endif() rs_add_library(rs_PCLParticleTrackingAnnotator src/PCLParticleTrackingAnnotator.cpp) target_link_libraries(rs_PCLParticleTrackingAnnotator ${catkin_LIBRARIES}) +rs_add_library(rs_TrackingAnnotator src/TrackingAnnotator.cpp) +target_link_libraries(rs_TrackingAnnotator ${catkin_LIBRARIES}) install( TARGETS rs_ObjectIdentityResolution rs_ClusterTracker rs_PCLParticleTrackingAnnotator + rs_TrackingAnnotator DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) diff --git a/robosherlock/src/tracking/src/TrackingAnnotator.cpp b/robosherlock/src/tracking/src/TrackingAnnotator.cpp new file mode 100644 index 00000000..d1d1586e --- /dev/null +++ b/robosherlock/src/tracking/src/TrackingAnnotator.cpp @@ -0,0 +1,171 @@ +#include + +#include + +//RS +#include +#include +#include +#include +#include + +//OCV +#include +#include +#include + +using namespace uima; +using namespace cv; +using namespace std; + +// Convert to string +#define SSTR( x ) static_cast< std::ostringstream & >( \ +( std::ostringstream() << std::dec << x ) ).str() + +class TrackingAnnotator : public DrawingAnnotator +{ +private: + float test_param; + cv::Mat annotatorView; + cv::Mat color; + bool first_pass = true; + Ptr tracker; + string trackerType; + Rect2d bbox; + +public: + + TrackingAnnotator() : DrawingAnnotator(__func__) { + } + + TyErrorId initialize(AnnotatorContext &ctx) + { + outInfo("initialize"); + ctx.extractValue("test_param", test_param); + + // List of tracker types in OpenCV 3.4.1 + string trackerTypes[8] = {"BOOSTING", "MIL", "KCF", "TLD","MEDIANFLOW", "GOTURN", "MOSSE", "CSRT"}; + // vector trackerTypes(types, std::end(types)); + + // Create a tracker + trackerType = trackerTypes[2]; + + #if (CV_MINOR_VERSION < 3) + { + tracker = Tracker::create(trackerType); + } + #else + { + if (trackerType == "BOOSTING") + tracker = TrackerBoosting::create(); + if (trackerType == "MIL") + tracker = TrackerMIL::create(); + if (trackerType == "KCF") + tracker = TrackerKCF::create(); + if (trackerType == "TLD") + tracker = TrackerTLD::create(); + if (trackerType == "MEDIANFLOW") + tracker = TrackerMedianFlow::create(); + if (trackerType == "GOTURN") + tracker = TrackerGOTURN::create(); + if (trackerType == "MOSSE") + tracker = TrackerMOSSE::create(); + //if (trackerType == "CSRT") + // tracker = TrackerCSRT::create(); + } + #endif + + return UIMA_ERR_NONE; + } + + TyErrorId destroy() + { + outInfo("destroy"); + return UIMA_ERR_NONE; + } + + TyErrorId processWithLock(CAS &tcas, ResultSpecification const &res_spec) + { + outInfo("process start"); + rs::StopWatch clock; + rs::SceneCas cas(tcas); + rs::Scene scene = cas.getScene(); + + std::vector clusters; + scene.identifiables.filter(clusters); + + pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud); + + cas.get(VIEW_CLOUD,*cloud_ptr); + cas.get(VIEW_COLOR_IMAGE, color); + cas.get(VIEW_COLOR_IMAGE, annotatorView); + outInfo(clusters.size()); + + for(int a = 0; a < 1 && a < clusters.size(); a++) + { + rs::ImageROI imageRoi(clusters[0].rois()); + cv::Rect rect; + rs::conversion::from(imageRoi.roi(), rect); + + // Read frame + Mat frame = color.clone(); + + if(first_pass) + { + // Define initial bounding box + bbox = Rect2d(rect.x, rect.y, rect.width, rect.height); + + // Display bounding box. + rectangle(frame, bbox, Scalar( 255, 0, 0 ), 2, 1); + rectangle(annotatorView, bbox, Scalar( 255, 0, 0 ), 2, 1); + + tracker->init(frame, bbox); + first_pass = false; + } + else + { + // Start timer + double timer = (double)getTickCount(); + + // Update the tracking result + bool ok = tracker->update(frame, bbox); + + // Calculate Frames per second (FPS) + float fps = getTickFrequency() / ((double)getTickCount() - timer); + + if (ok) + { + // Tracking success : Draw the tracked object + outInfo("Tracking successful"); + rectangle(annotatorView, bbox, Scalar( 255, 0, 0 ), 2, 1); + } + else + { + // Tracking failure detected. + putText(annotatorView, "Tracking failure detected", Point(100,80), FONT_HERSHEY_SIMPLEX, 0.75, Scalar(0,0,255),2); + } + + // Display tracker type on frame + putText(annotatorView, trackerType + " Tracker", Point(100,20), FONT_HERSHEY_SIMPLEX, 0.75, Scalar(50,170,50),2); + + // Display FPS on frame + putText(annotatorView, "FPS : " + SSTR(int(fps)), Point(100,50), FONT_HERSHEY_SIMPLEX, 0.75, Scalar(50,170,50), 2); + } + outInfo("Tracking End"); + } + + + outInfo("Cloud size: " << cloud_ptr->points.size()); + outInfo("took: " << clock.getTime() << " ms."); + return UIMA_ERR_NONE; + } + + void drawImageWithLock(cv::Mat &disp) override + { + disp = annotatorView.clone(); + } + +}; + +// This macro exports an entry point that is used to create the annotator. +MAKE_AE(TrackingAnnotator) \ No newline at end of file