added markers per tracker parameter. fixed some bugs.

Author: ju1ce

AprilTagTrackers/AprilTagWrapper.cpp

@@ -24,7 +24,7 @@ AprilTagWrapper::AprilTagWrapper(const Parameters* params)
 
     if (parameters->markerLibrary == ARUCO_4X4)
     {
-        aruco_dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
+        aruco_dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
         aruco_params = parameters->aruco_params;
     }
 
@@ -56,7 +56,8 @@ void AprilTagWrapper::detectMarkers(
     const cv::Mat& image,
     std::vector<std::vector<cv::Point2f> >* corners,
     std::vector<int>* ids,
-    std::vector<cv::Point2f>* centers)
+    std::vector<cv::Point2f>* centers,
+    std::vector<cv::Ptr<cv::aruco::Board>> trackers)
 {
     cv::Mat gray;
     if (image.type() != CV_8U)
@@ -76,8 +77,8 @@ void AprilTagWrapper::detectMarkers(
     {
         std::vector<std::vector<cv::Point2f>> rejectedCorners;
         cv::aruco::detectMarkers(gray, aruco_dictionary, *corners, *ids, aruco_params, rejectedCorners);
-        for(int i = 0; i<parameters->trackers.size();i++)
-            cv::aruco::refineDetectedMarkers(gray, parameters->trackers[i], *corners, *ids, rejectedCorners);
+        //for(int i = 0; i<trackers.size();i++)
+        //    cv::aruco::refineDetectedMarkers(gray, trackers[i], *corners, *ids, rejectedCorners);
 
         return;
     }

AprilTagTrackers/AprilTagWrapper.h

@@ -27,7 +27,8 @@ class AprilTagWrapper
         const cv::Mat& frame,
         std::vector<std::vector<cv::Point2f> >* corners,
         std::vector<int>* ids,
-        std::vector<cv::Point2f>* centers
+        std::vector<cv::Point2f>* centers,
+        std::vector<cv::Ptr<cv::aruco::Board>> trackers
     );
 
     std::vector<std::string> getTimeProfile();

AprilTagTrackers/Connection.cpp

@@ -92,7 +92,7 @@ void Connection::Connect()
         for (int i = 0; i < parameters->trackerNum; i++)
         {
             TrackerConnection temp;
-            temp.TrackerId = i + 1;
+            temp.TrackerId = i;
             temp.DriverId = i;
             temp.Name = "ApriltagTracker" + std::to_string(i + 1);
             temp.Role = "TrackerRole_Waist";

AprilTagTrackers/Parameters.cpp

@@ -13,6 +13,9 @@ void Parameters::Load()
 
     if (!fs["cameraAddr"].empty())			//if file exists, load all parameters from file into variables
     {
+        fs["markersPerTracker"] >> markersPerTracker;
+        if (markersPerTracker <= 0)
+            markersPerTracker = 45;
         aruco_params = cv::aruco::DetectorParameters::create();
         aruco_params->detectInvertedMarker = true;
         aruco_params->cornerRefinementMethod = 2;
@@ -137,6 +140,7 @@ void Parameters::Load()
 void Parameters::Save()
 {
     cv::FileStorage fs("params.yml", cv::FileStorage::WRITE);
+    fs << "markersPerTracker" << markersPerTracker;
     fs << "arucoParams";
     fs << "{";
     fs << "cornerRefinementMethod" << aruco_params->cornerRefinementMethod;

AprilTagTrackers/Parameters.h

@@ -57,6 +57,7 @@ class Parameters
     float depthSmoothing = 0;
     float additionalSmoothing = 0;
     int markerLibrary = 0;
+    int markersPerTracker = 45;
 
     cv::Ptr<cv::aruco::DetectorParameters> aruco_params = cv::aruco::DetectorParameters::create();
 

AprilTagTrackers/Tracker.cpp

@@ -824,7 +824,7 @@ void Tracker::CalibrateTracker()
 
     AprilTagWrapper april{parameters};
 
-    int markersPerTracker = 45;
+    int markersPerTracker = parameters->markersPerTracker;
     int trackerNum = parameters->trackerNum;
 
     std::vector<cv::Vec3d> boardRvec, boardTvec;
@@ -847,6 +847,8 @@ void Tracker::CalibrateTracker()
     std::vector<int> idsList;
     std::vector<std::vector < std::vector<cv::Point3f >>> cornersList;
 
+    trackers.clear();
+
     while (cameraRunning && mainThreadRunning)
     {
         CopyFreshCameraImageTo(image);
@@ -861,7 +863,7 @@ void Tracker::CalibrateTracker()
         std::vector<cv::Point2f> centers;
 
         //cv::aruco::detectMarkers(image, dictionary, corners, ids, params);
-        april.detectMarkers(image, &corners, &ids, &centers);
+        april.detectMarkers(image, &corners, &ids, &centers,trackers);
         if (showTimeProfile)
         {
             april.drawTimeProfile(image, cv::Point(10, 60));
@@ -1003,8 +1005,6 @@ void Tracker::CalibrateTracker()
         cv::imshow("out", drawImg);
         cv::waitKey(1);
     }
-    
-    trackers.clear();
 
     for (int i = 0; i < boardIds.size(); i++)
     {
@@ -1241,11 +1241,11 @@ void Tracker::MainLoop()
         }
 
         //Then define your mask image
-        cv::Mat mask = cv::Mat::zeros(image.size(), image.type());
+        cv::Mat mask = cv::Mat::zeros(gray.size(), gray.type());
 
-        cv::Mat dstImage = cv::Mat::zeros(image.size(), image.type());
+        cv::Mat dstImage = cv::Mat::zeros(gray.size(), gray.type());
 
-        int size = image.rows * parameters->searchWindow;
+        int size = gray.rows * parameters->searchWindow;
 
         bool doMasking = false;
 
@@ -1273,8 +1273,8 @@ void Tracker::MainLoop()
         //Now you can copy your source image to destination image with masking
         if (doMasking)
         {
-            image.copyTo(dstImage, mask);
-            image = dstImage;
+            gray.copyTo(dstImage, mask);
+            gray = dstImage;
         }
 
         //cv::imshow("test", image);
@@ -1423,8 +1423,11 @@ void Tracker::MainLoop()
 
             connection->SendStation(0, a, b, c, stationQ.w, stationQ.x, stationQ.y, stationQ.z);
         }
-
-        april.detectMarkers(gray, &corners, &ids, &centers);
+        else
+        {
+            calibControllerLastPress = clock();
+        }
+        april.detectMarkers(gray, &corners, &ids, &centers, trackers);
         for (int i = 0; i < trackerNum; ++i) {
 
             //estimate the pose of current board