Marker Tracking

Abstract 概要

The Marker Tracking algorithm is for tracking the markers (black dots) on the surface of FingerVision in order to estimate the external force distribution.

Table of contents 目次

• Algorithm Overview
  • Preprocess
  • Calibration
  • Tracking
  • Convert Marker Movement to Force Estimation
• Implementation: TBlobTracker2 Class
  • Parameters
    • WriteToYAML and ReadFromYAML
  • Marker Tracking Result
  • Member Functions
    • Set and Get Parameters
    • Init
    • Calibrate
    • Save and Load Calibration Result
    • Step
    • Draw
    • Data
  • Step-by-Step Usage
    • Initialize
    • Progress Marker Tracking
    • Calibration
• Standalone Marker Tracking Program
  • Test with Dummy Data

Algorithm Overview †

We use a blob tracking method. It consists of two processes: calibration to detect initial marker positions, and tracking the marker displacements. In the both processes, the camera image is rectified for compensating the distortion caused by fisheye lens, and then converted to a black and white image by thresholding black color as the markers are black. In the calibration, we cover the sensor with white sheet in order to avoid detecting noise from background. A blob detection method implemented in OpenCV (cv::SimpleBlobDetector) is used. The calibration takes less than 1 second. Marker tracking is done independently per marker. We assume a small region around a marker at its previous position, and apply cv::SimpleBlobDetector. If the marker movement is unexpectedly large, we reject the result since it would be noise. We also compare the size of blob to distinguish the noise.

A benefit to use cv::SimpleBlobDetector is that it provides the detected blob position as floating-point values. The obtained marker position movement is fairly smooth.

Preprocess †

For each incoming image, we rectify the distortion caused by the fisheye lens, and threshold to extract black colors as the current markers are black. We also apply a dilation and an erosion to remove noise.

Calibration †

The sensor is covered with a white sheet to remove the background effect.

We apply cv::SimpleBlobDetector to an entire image. Then we apply the tracking method to several frames (e.g. 10); if some markers are moving, they are removed from the marker candidates as they are noisy points. The remaining blobs are considered as initial markers.

Tracking †

Starting from the initial marker positions, we track each marker frame by frame. We consider a small (e.g. 30x30) region of interest (ROI) around the previous marker position. First we count the non-zero pixels in the ROI and compare it with the non-zero points of the initial marker. If there is large difference, we do not perform marker tracking (i.e. a detection failure). Otherwise we apply the blob detection method to the ROI. Only one blob is expected; otherwise it is considered as a failure. We compare the previous and current blob positions and sizes, and if their difference is large, it is considered as a failure. Otherwise the blob is considered as the new marker location.

Convert Marker Movement to Force Estimation †

From the marker movement, we estimate an array of forces. The blob detection provides a position and a size of each blob. The position change is caused by a horizontal (surface) force, while the size change is caused by a normal force. However, since the size change is subtle compared to the position change as reported in [Yamaguchi,Atkeson,2016,Humanoids(B)], the normal force estimate based on the size change is noisy and unreliable. An alternative approach approximates the normal force at each marker with a norm of marker position change. This approximation is useful especially when taking an average of all the forces. When a normal force is applied to the center of the skin surface, the markers around the point move radially. An average of the horizontal forces in such a case will be close to zero, while an average of the approximated normal forces will have a useful value.

Let \((( d_x, d_y \))) denote the horizontal marker movement from the initial position. The force estimate at each marker is given by:

\$$$ [f_x,f_y,f_z] = [c_x d_x, c_y \sqrt{d_x^2+d_y^2}, c_z d_y] \$$$

where \((( c_x, c_y, c_z \))) denote constant coefficients. We also define an average force and a torque estimate as:

\$$$ \mathbf{f} = \frac{1}{N}\sum{[f_x,f_y,f_z]} \$$$ \$$$ \mathbf{\tau} = \frac{1}{N}\sum{\mathbf{r} \times [f_x,f_y,f_z]} \$$$

where \((( N \))) denotes a number of markers, and \((( \mathbf{r} \))) denotes a position of a marker from the center of the image.

Implementation: TBlobTracker2 Class †

The marker tracker algorithm is implemented as the TBlobTracker2 class in fv_core/blob_tracker2.h which depends on OpenCV and the Boost C++ Libraries.

Note that TBlobTracker2 does not have the function to convert the marker movement to force estimate (cf. Convert Marker Movement to Force Estimation). This is because the conversion is experimental, i.e. there would be other approaches. Thus, we implemented such conversion as an external filter (FV-Filter) so that we can easily replace it by other filters.

Reading the code of the standalone program of marker tracker standalone/blob_tracker2_test.cpp would be a good practice to learn the usage of this class.

Note that TBlobTracker2 itself does not do the image rectification since it is a common process among other image processing. The image rectification method is introduced in Video Capture and Rectification.

Parameters †

The TBlobTracker2Params structure is a set of the parameters of TBlobTracker2. It is defined as follows:

struct TBlobTracker2Params
{
  // For blob detection:
  cv::SimpleBlobDetector::Params SBDParams;

  // For preprocessing:
  int ThreshH;
  int ThreshS;
  int ThreshV;
  int NDilate1;
  int NErode1;
  // For keypoint tracking;
  float SWidth;  // Width of search ROI of each keypoint
  float NonZeroMin; // Minimum ratio of nonzero pixels in ROI over original keypoint size.
  float NonZeroMax; // Maximum ratio of nonzero pixels in ROI over original keypoint size.
  float VPMax;  // Maximum position change (too large one might be noise)
  float VSMax;  // Maximum size change (too large one might be noise)
  int   NReset;  // When number of tracking failure in a row exceeds this, tracking is reset
  // For calibration:
  float DistMaxCalib;
  float DSMaxCalib;

  // For visualization:
  float DSEmp;  // Emphasize (scale) ratio of DS to draw
  float DPEmp;  // Emphasize (scale) ratio of DP to draw
  // For calibration:
  int NCalibPoints;  // Number of points for calibration
};

The preprocessing parameters are for detecting black colors (== marker colors).

The following utility functions are provided for TBlobTracker2Params:

WriteToYAML and ReadFromYAML †

void WriteToYAML(const std::vector<TBlobTracker2Params> &blob_params, const std::string &file_name);
void ReadFromYAML(std::vector<TBlobTracker2Params> &blob_params, const std::string &file_name);

These functions are saving and loading multiple TBlobTracker2Params objects into/from a YAML file. An example of the configuration file is: fingervision/config/fv_2_l.yaml. Note that only the BlobTracker2 section matters. Other sections such as CameraInfo are ignored.

Marker Tracking Result †

The marker tracking result is hold by the TPointMove2 structure. It is defined as follows:

struct TPointMove2
{
  cv::Point2f Po;  // Original position
  float So;        // Original size
  cv::Point2f DP;  // Displacement of position
  float DS;        // Displacement of size
  int NTrackFailed; // Number of traking failures in a raw
};

Member Functions †

Set and Get Parameters †

TBlobTracker2Params& Params();
const TBlobTracker2Params& Params() const;

Params gives writable and access to the parameter object.

Init †

void Init();

Initialization of internal objects. Init should be called after setting parameters.

Calibrate †

void Calibrate(const std::vector<cv::Mat> &images);

Calibrate the marker tracking with a sequence of images. Note that images should be rectified.

Save and Load Calibration Result †

void SaveCalib(const std::string &file_name) const;
void LoadCalib(const std::string &file_name);

Save and load calibration result into a YAML file.

Step †

void Step(const cv::Mat &img);

Progress the marker tracking with a given image img. Note that img should be rectified.

Draw †

void Draw(cv::Mat &img);

Visualize the calibration result on img.

Data †

const std::vector<TPointMove2>& Data() const;

Access to the marker tracking result. About the TPointMove2 structure, refer to Marker Tracking Result.

Step-by-Step Usage †

The following code shows the step-by-step usage of TBlobTracker2.

Initialize †

A simple way to initialize TBlobTracker2 is:

TBlobTracker2 tracker;
tracker.Params().xxx= yyy;
tracker.Init();

You can omit tracker.Params().xxx= ... if you will use the default parameters.

The following style is loading the parameters from a configuration file in YAML format.

TBlobTracker2 tracker;
std::vector<TBlobTracker2Params> blobtrack_info;
ReadFromYAML(blobtrack_info, config_file);
tracker.Params()= blobtrack_info[0];
tracker.Init();

Note that ReadFromYAML can load multiple TBlobTracker2Params objects from a single file. The above example uses only the first object.

Loading calibration data from a file:

tracker.LoadCalib("blob_calib.yaml");

Progress Marker Tracking †

We assume CAPTURE_AND_RECTIFY() is a function to capture an image and preprocess it. An image may be captured from a device, video stream, and a file. It should be preprocessed with resizing, rotating, and rectifying.

cv::Mat frame= CAPTURE_AND_RECTIFY()

Then the image is fed to the Step method:

tracker.Step(frame);

For visualization of calibration result:

tracker.Draw(frame);

Calibration †

Cover the sensor with white sheet in order to avoid detecting noise from background. Then, run the code:

std::vector<cv::Mat> frames;
for(int i(0); i<tracker.Params().NCalibPoints; ++i)
  frames.push_back(CAPTURE_AND_RECTIFY());
tracker.Calibrate(frames);

Saving the calibration result is useful for future execution:

tracker.SaveCalib(blob_calib_yaml);

Standalone Marker Tracking Program †

The standalone code to test TBlobTracker2 is implemented in standalone/blob_tracker2_test.cpp.

//-------------------------------------------------------------------------------------------
/*! \file    blob_tracker2_test.cpp
    \brief   Marker tracker (blob tracker) of FingerVision.
    \author  Akihiko Yamaguchi, info@akihikoy.net
    \version 0.1
    \date    May.10, 2016
    \version 0.2
    \date    Aug.9, 2018
             Using the same core programs as the ROS version.
             Supporting to load YAML configuration.
             Supporting camera rectification.
    \version 0.3
    \date    May.20, 2019
             Modified the trackbars.

g++ -g -Wall -O2 -o blob_tracker2_test.out blob_tracker2_test.cpp -I../3rdparty -I../fv_core -lopencv_core -lopencv_imgproc -lopencv_features2d -lopencv_calib3d -lopencv_highgui -lopencv_videoio

Run:
  $ ./blob_tracker2_test.out [CAMERA_NUMBER [WIDTH HEIGHT]]
    CAMERA_NUMBER: Camera device number (e.g. 1).
  $ ./blob_tracker2_test.out CONFIG_FILE
    CONFIG_FILE: Configuration file in YAML (e.g. fv_1.yaml).
    The configuration file may include camera configuration (CameraInfo) and
    the configuration of blob tracker (BlobTracker2).
Usage:
  Press 'q' or Esc: Exit the program.
  Press 'c': Calibrate the tracker (detecting markers and storing the initial positions and sizes).
             Tips: Show a white paper or white wall during the calibration.
  Press 'C': Switch the parameter configuration trackbar mode.
  Press 'P': Save the tracker parameter into a file in /tmp.
  Press 'W' (shift+'w'): Start/stop video recording.
  Press 'p': Print the calibration result.
  Press 's': Save the calibration result to file "blob_calib.yaml".
  Press 'l': Load a calibration from file "blob_calib.yaml".
*/
//-------------------------------------------------------------------------------------------
#include "blob_tracker2.h"
#include "ay_vision/vision_util.h"
#include "ay_cpp/cpp_util.h"
#include "ay_cpp/sys_util.h"
//-------------------------------------------------------------------------------------------
#include "blob_tracker2.cpp"
#include "ay_vision/vision_util.cpp"
//-------------------------------------------------------------------------------------------
using namespace std;
using namespace trick;
//-------------------------------------------------------------------------------------------

cv::Mat Capture(cv::VideoCapture &cap, TCameraInfo &info)
{
  cv::Mat frame;
  while(!cap.read(frame))
  {
    if(CapWaitReopen(info,cap)) continue;
    else  return cv::Mat();
  }
  return frame;
}
//-------------------------------------------------------------------------------------------

struct TMouseEventData
{
  cv::Mat &frame;
  TBlobTracker2 &tracker;
  TMouseEventData(cv::Mat &f, TBlobTracker2 &t) : frame(f), tracker(t) {}
};
void OnMouse(int event, int x, int y, int flags, void *data)
{
  if(event == cv::EVENT_RBUTTONDOWN && (flags & cv::EVENT_FLAG_SHIFTKEY))
  {
    TMouseEventData &d(*reinterpret_cast<TMouseEventData*>(data));
    d.tracker.RemovePointAt(cv::Point2f(x,y));
  }
}
//-------------------------------------------------------------------------------------------

int main(int argc, char**argv)
{
  std::string cam("0"), config_file;
  if(argc>1)  cam= argv[1];

  std::vector<TCameraInfo> cam_info;
  // If cam exists as a file, it is considered as a configuration YAML file.
  if(FileExists(cam))
  {
    ReadFromYAML(cam_info, cam);
    config_file= cam;
  }
  else
  {
    cam_info.push_back(TCameraInfo());
    cam_info[0].DevID= cam;
    cam_info[0].Width= ((argc>2)?atoi(argv[2]):320);
    cam_info[0].Height= ((argc>3)?atoi(argv[3]):240);
  }
  cv::VideoCapture cap;
  if(!CapOpen(cam_info[0], cap))  return -1;

  TCameraRectifier cam_rectifier;
  if(cam_info[0].Rectification)
  {
    cv::Size size_in(cam_info[0].Width,cam_info[0].Height), size_out(cam_info[0].Width,cam_info[0].Height);
    cam_rectifier.Setup(cam_info[0].K, cam_info[0].D, cam_info[0].R, size_in, cam_info[0].Alpha, size_out);
  }

  TBlobTracker2 tracker;
  if(config_file=="")
  {
    tracker.Init();
  }
  else
  {
    std::vector<TBlobTracker2Params> blobtrack_info;
    ReadFromYAML(blobtrack_info, config_file);
    tracker.Params()= blobtrack_info[0];
    tracker.Init();
  }

  bool calib_request(true);
  std::string blob_calib_yaml("blob_calib.yaml");
  if(FileExists(blob_calib_yaml))
  {
    tracker.LoadCalib(blob_calib_yaml);
    std::cerr<<"Loaded calibration data from "<<blob_calib_yaml<<std::endl;
    calib_request= false;
  }

  cv::Mat frame;

  std::string win("camera");
  cv::namedWindow("camera",1);
  TMouseEventData mouse_data(frame,tracker);
  cv::setMouseCallback("camera", OnMouse, &mouse_data);
  int trackbar_mode(0);
  bool init_request(false);

  TEasyVideoOut vout, vout_orig;
  vout.SetfilePrefix("/tmp/blobtr");
  vout_orig.SetfilePrefix("/tmp/blobtr-orig");

  int show_fps(0);
  double dim_levels[]={0.0,0.3,0.7,1.0};  int dim_idx(3);
  for(int f(0);;++f)
  {
    if(init_request)
    {
      tracker.Init();
      init_request= false;
    }

    frame= Capture(cap, cam_info[0]);
    vout_orig.Step(frame);
    Preprocess(frame, cam_info[0], &cam_rectifier);

    tracker.Step(frame);
    frame*= dim_levels[dim_idx];
    tracker.Draw(frame);

    vout.Step(frame);
    vout.VizRec(frame);
    cv::imshow("camera", frame);
    char c(cv::waitKey(1));
    if(c=='\x1b'||c=='q') break;
    else if(char(c)=='W')  {vout.Switch(); vout_orig.Switch();}
    else if(c=='m')
    {
      dim_idx++;
      if(dim_idx>=int(sizeof(dim_levels)/sizeof(dim_levels[0])))  dim_idx=0;
    }
    else if(c=='p')  tracker.SaveCalib("/dev/stdout");
    else if(c=='s')
    {
      tracker.SaveCalib(blob_calib_yaml);
      std::cerr<<"Saved calibration data to "<<blob_calib_yaml<<std::endl;
    }
    else if(c=='l')
    {
      tracker.LoadCalib(blob_calib_yaml);
      std::cerr<<"Loaded calibration data from "<<blob_calib_yaml<<std::endl;
    }
    else if(c=='P')
    {
      std::vector<TBlobTracker2Params> p;
      p.push_back(tracker.Params());
      WriteToYAML(p,"/tmp/blobtr_params.yaml");
      std::cerr<<"Parameters of the tracker are saved into /tmp/blobtr_params.yaml"<<std::endl;
    }
    else if(c=='C')
    {
      // Remove trackbars from window.
      cv::destroyWindow(win);
      cv::namedWindow(win,1);
      ++trackbar_mode;
      CreateTrackbars(win, tracker.Params(), trackbar_mode, init_request);
      cv::setMouseCallback("camera", OnMouse, &mouse_data);
    }
    else if(c=='c' || calib_request)
    {
      std::vector<cv::Mat> frames;
      for(int i(0); i<tracker.Params().NCalibPoints; ++i)
      {
        frames.push_back(Capture(cap, cam_info[0]));
        Preprocess(frames.back(), cam_info[0], &cam_rectifier);
      }
      tracker.Calibrate(frames);
      calib_request= false;
    }
    // usleep(10000);
    if(show_fps==0)
    {
      std::cerr<<"FPS: "<<vout.FPS()<<std::endl;
      show_fps= vout.FPS()*4;
    }
    --show_fps;
  }
  return 0;
}
//-------------------------------------------------------------------------------------------

The code is already explained above about TBlobTracker2 and Video Capture and Rectification.

Compile:

 g++ -g -Wall -O2 -o blob_tracker2_test.out blob_tracker2_test.cpp -I../3rdparty -I../fv_core -lopencv_core -lopencv_imgproc -lopencv_features2d -lopencv_calib3d -lopencv_highgui

Execution:

 $ ./blob_tracker2_test.out [CAMERA_NUMBER [WIDTH HEIGHT]]
   CAMERA_NUMBER: Camera device number (e.g. 1).
 $ ./blob_tracker2_test.out CONFIG_FILE
   CONFIG_FILE: Configuration file in YAML (e.g. fv_1.yaml).
   The configuration file may include camera configuration (CameraInfo) and
   the configuration of blob tracker (BlobTracker2).

Usage:

• Press 'q' or Esc: Exit the program.
• Press 'c': Calibrate the tracker (detecting markers and storing the initial positions and sizes). Tips: Show a white paper or white wall during the calibration.
• Press 'C': Show/hide the parameter configuration trackbars.
• Press 'W' (shift+'w'): Start/stop video recording.
• Press 'p': Print the calibration result.
• Press 's': Save the calibration result to file "blob_calib.yaml".
• Press 'l': Load a calibration from file "blob_calib.yaml".

Test with Dummy Data †

Here we run the standalone program with dummy data (no actual FingerVision is necessary).

Refer to Dummy FingerVision Data for the dummy data and the detailed explanation of streaming the video files over the network.

$ cd fingervision/tools/
$ ./stream_file1.sh

Access http://localhost:8080/stream.html to see the video stream.

Then run the standalone program.

$ cd fingervision/standalone
$ ./blob_tracker2_test.out fv_3_l.yaml

Perhaps the calibration of the marker tracking is not good. You can try to press c to calibrate. Or, copy the calibration data:

$ cp ../fingervision/data_gen/blob_fv_3_l.yaml blob_calib.yaml

And run ./blob_tracker2_test.out again.

---