├── .gitignore
├── share
    ├── robot.jpg
    ├── robot.png
    └── robot-mid.jpg
├── doc
    └── screenshot.jpg
├── chilitrack.pc.in
├── src
    ├── CMakeLists.txt
    ├── template.cpp
    ├── utils.h
    └── tracker.cpp
├── include
    ├── stats.h
    └── chilitrack.h
├── README.md
├── samples
    ├── minimal.cpp
    └── tracker.cpp
└── CMakeLists.txt


/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | 


--------------------------------------------------------------------------------
/share/robot.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chili-epfl/chilitrack/HEAD/share/robot.jpg


--------------------------------------------------------------------------------
/share/robot.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chili-epfl/chilitrack/HEAD/share/robot.png


--------------------------------------------------------------------------------
/doc/screenshot.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chili-epfl/chilitrack/HEAD/doc/screenshot.jpg


--------------------------------------------------------------------------------
/share/robot-mid.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chili-epfl/chilitrack/HEAD/share/robot-mid.jpg


--------------------------------------------------------------------------------
/chilitrack.pc.in:
--------------------------------------------------------------------------------
 1 | prefix=@CMAKE_INSTALL_PREFIX@
 2 | exec_prefix=${prefix}
 3 | libdir=${exec_prefix}/lib
 4 | includedir=${prefix}/include
 5 | 
 6 | Name: chilitrack
 7 | Description: Marker-less Tracking for Augmented Reality
 8 | Version: @CPACK_PACKAGE_VERSION_MAJOR@.@CPACK_PACKAGE_VERSION_MINOR@.@CPACK_PACKAGE_VERSION_PATCH@
 9 | Libs: -L${libdir} -lchilitrack
10 | Cflags: @COMPILE_FLAGS@ -I${includedir}/chilitrack
11 | 


--------------------------------------------------------------------------------
/src/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | 
 2 | file(GLOB_RECURSE sources *.cpp)
 3 | 
 4 | add_library(
 5 |         chilitrack
 6 |         SHARED
 7 |         ${sources}
 8 | )
 9 | 
10 | target_link_libraries(chilitrack ${OpenCV_LIBS})
11 | 
12 | install (TARGETS chilitrack LIBRARY DESTINATION lib)
13 | 
14 | file(GLOB_RECURSE chilitrack_headers ../include/*)
15 | 
16 | install(FILES
17 |     ${chilitrack_headers}
18 |     DESTINATION include/chilitrack
19 |     )
20 | 
21 | 


--------------------------------------------------------------------------------
/include/stats.h:
--------------------------------------------------------------------------------
 1 | #ifndef STATS_H
 2 | #define STATS_H
 3 | 
 4 | struct Stats
 5 | {
 6 |     int matches;
 7 |     int inliers;
 8 |     double ratio;
 9 |     int keypoints;
10 |     int duration; //ms
11 | 
12 |     Stats() : matches(0),
13 |         inliers(0),
14 |         ratio(0),
15 |         keypoints(0),
16 |         duration(0)
17 |     {}
18 | 
19 |     Stats& operator+=(const Stats& op) {
20 |         matches += op.matches;
21 |         inliers += op.inliers;
22 |         ratio += op.ratio;
23 |         keypoints += op.keypoints;
24 |         duration += op.duration;
25 |         return *this;
26 |     }
27 |     Stats& operator/=(int num)
28 |     {
29 |         matches /= num;
30 |         inliers /= num;
31 |         ratio /= num;
32 |         keypoints /= num;
33 |         duration /= num;
34 |         return *this;
35 |     }
36 | };
37 | 
38 | #endif // STATS_H
39 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | chilitrack
 2 | ==========
 3 | 
 4 | A marker-less tracking solution for augmented reality.
 5 | 
 6 | ![chilitrack screenshot](doc/screenshot.jpg)
 7 | 
 8 | Currently, a demo app that tracks a given template in the camera
 9 | feed, and show the corresponding 3D transformation.
10 | 
11 | It relies on ORB (or AKAZE) features to find the template in the target image (closely following
12 | [this
13 | approach](http://docs.opencv.org/trunk/doc/tutorials/features2d/akaze_tracking/akaze_tracking.html)), then track the object using the [video optical flow](http://docs.opencv.org/trunk/modules/video/doc/motion_analysis_and_object_tracking.html#calcopticalflowpyrlk) and finally estimate a [3D transformation](http://docs.opencv.org/trunk/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp).
14 | 
15 | Prerequisites
16 | -------------
17 | 
18 | `chilitrack` depends on `OpenCV 3`. You likely need to install `OpenCV 3` by
19 | hand since it is not yet packaged.
20 | 
21 | 
22 | Installation
23 | ------------
24 | 
25 | ```
26 | > mkdir build
27 | > cmake ..
28 | > make
29 | ```
30 | 
31 | Usage
32 | -----
33 | 
34 | ```
35 | > tracker <template image to track>
36 | ```
37 | 
38 | 


--------------------------------------------------------------------------------
/samples/minimal.cpp:
--------------------------------------------------------------------------------
 1 | #include <opencv2/videoio.hpp>
 2 | #include <iostream>
 3 | 
 4 | #include <chilitrack.h>
 5 | 
 6 | using namespace std;
 7 | using namespace cv;
 8 | using namespace chilitrack;
 9 | 
10 | int main(int argc, char **argv)
11 | {
12 |     Mat frame;
13 |     namedWindow("tracking");
14 | 
15 |     if(argc < 2) {
16 |         cerr << "Usage: " << endl <<
17 |                 "tracker template_path" << endl;
18 |         return 1;
19 |     }
20 | 
21 |     // Configure the video capture
22 |     // ===========================
23 | 
24 |     VideoCapture video_in(1);
25 | 
26 |     if(!video_in.isOpened()) {
27 |         cerr << "Couldn't open camera" << endl;
28 |         return 1;
29 |     }
30 | 
31 |     // Read the template
32 |     // =================
33 | 
34 |     Mat image_object = imread(argv[1], IMREAD_GRAYSCALE);
35 | 
36 |     // the size parameter is the *physical* size of the template bounding box,
37 |     // typically in millimeters.
38 |     auto tpl = makePtr<Template>(Template(image_object,
39 |                                           Size(200, 297)));
40 | 
41 |     // Create the tracker
42 |     // ==================
43 | 
44 |     Tracker tracker;
45 |     tracker.add_template("tpl1", tpl);
46 | 
47 |     // Main loop
48 |     // =========
49 | 
50 |     while(true)
51 |     {
52 |         video_in >> frame;
53 | 
54 |         cout << "Found " << tracker.estimate(frame).size() << " templates\n";
55 |         imshow("tracking", tracker._debug);
56 |         if (waitKey(10) >= 0) break;
57 |     }
58 | 
59 |     return 0;
60 | }
61 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | 
 3 | project(chilitrack)
 4 | 
 5 | set(CPACK_PACKAGE_VERSION_MAJOR "0")
 6 | set(CPACK_PACKAGE_VERSION_MINOR "1")
 7 | set(CPACK_PACKAGE_VERSION_PATCH "0")
 8 | 
 9 | 
10 | add_definitions(-std=c++11)
11 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
12 | find_package(OpenCV COMPONENTS video highgui videoio features2d calib3d REQUIRED )
13 | include_directories(${OpenCV_INCLUDE_DIRS})
14 | message(STATUS "OpenCV version: ${OpenCV_VERSION}")
15 | 
16 | if(CMAKE_BUILD_TYPE MATCHES Debug)
17 |     add_definitions(-DCHILITRACK_DEBUG)
18 | endif()
19 | 
20 | include_directories(include)
21 | 
22 | add_subdirectory(src)
23 | 
24 | add_executable(tracker samples/tracker.cpp)
25 | target_link_libraries(tracker chilitrack ${OpenCV_LIBRARIES})
26 | 
27 | add_executable(minimal samples/minimal.cpp)
28 | target_link_libraries(minimal chilitrack ${OpenCV_LIBRARIES})
29 | 
30 | ##########################################
31 | ##       PKGCONFIG file                ###
32 | ##########################################
33 | 
34 | if(EXISTS "${CMAKE_SOURCE_DIR}/chilitrack.pc.in")
35 |     message(STATUS "configuring ${CMAKE_SOURCE_DIR}/chilitrack.pc.in --> ${CMAKE_CURRENT_BINARY_DIR}/chilitrack.pc")
36 | 
37 |     # build a list of -D<...> macros to store in pkgconfig CFlags
38 |     get_directory_property(INTERNAL_COMPILE_FLAGS COMPILE_DEFINITIONS)
39 |     foreach(DEF ${INTERNAL_COMPILE_FLAGS})
40 |         set(COMPILE_FLAGS "${COMPILE_FLAGS} -D${DEF}")
41 |     endforeach()
42 | 
43 |     configure_file(${CMAKE_SOURCE_DIR}/chilitrack.pc.in 
44 |       ${CMAKE_CURRENT_BINARY_DIR}/chilitrack.pc
45 |       @ONLY )
46 | 
47 |     install(FILES
48 |         ${CMAKE_CURRENT_BINARY_DIR}/chilitrack.pc
49 |         DESTINATION lib/pkgconfig
50 |     )
51 | else()
52 |     message(STATUS "${CMAKE_SOURCE_DIR}/chilitrack.pc.in not found!")
53 | 
54 | endif()
55 | 
56 | 
57 | 


--------------------------------------------------------------------------------
/samples/tracker.cpp:
--------------------------------------------------------------------------------
  1 | #include <opencv2/videoio.hpp>
  2 | #include <iostream>
  3 | #include <iomanip>
  4 | 
  5 | #include <chilitrack.h>
  6 | 
  7 | using namespace std;
  8 | using namespace cv;
  9 | using namespace chilitrack;
 10 | 
 11 | const int stats_update_period = 10; // On-screen statistics are updated every 10 frames
 12 | 
 13 | void drawStatistics(Mat image, const Stats& stats)
 14 | {
 15 |     static const int font = FONT_HERSHEY_PLAIN;
 16 |     stringstream str0, str1, str2, str3, str4;
 17 | 
 18 |     str0 << "Keypoints: " << stats.keypoints;
 19 |     str1 << "Matches: " << stats.matches;
 20 |     str2 << "Inliers: " << stats.inliers;
 21 |     str3 << "Inlier ratio: " << setprecision(2) << stats.ratio;
 22 |     str4 << "Process. time: " << stats.duration;
 23 | 
 24 |     putText(image, str0.str(), Point(0, image.rows - 120), font, 2, Scalar::all(255), 3);
 25 |     putText(image, str1.str(), Point(0, image.rows - 90), font, 2, Scalar::all(255), 3);
 26 |     putText(image, str2.str(), Point(0, image.rows - 60), font, 2, Scalar::all(255), 3);
 27 |     putText(image, str3.str(), Point(0, image.rows - 30), font, 2, Scalar::all(255), 3);
 28 |     putText(image, str4.str(), Point(0, image.rows - 0), font, 2, Scalar::all(255), 3);
 29 | }
 30 | 
 31 | 
 32 | int main(int argc, char **argv)
 33 | {
 34 |     Mat frame;
 35 | 
 36 |     namedWindow("tracking");
 37 | 
 38 |     if(argc < 2) {
 39 |         cerr << "Usage: " << endl <<
 40 |                 "tracker template_path [calibration file]" << endl;
 41 |         return 1;
 42 |     }
 43 |     VideoCapture video_in(1);
 44 | 
 45 |     if(!video_in.isOpened()) {
 46 |         cerr << "Couldn't open camera" << endl;
 47 |         return 1;
 48 |     }
 49 | 
 50 |     // Read one frame to initialize the capture
 51 |     video_in >> frame;
 52 | 
 53 |     Mat image_object = imread(argv[1], IMREAD_GRAYSCALE);
 54 |     if( !image_object.data )
 55 |           { cerr << " --(!) Error reading object image " << std::endl; return -1; }
 56 | 
 57 | 
 58 |     //Ptr<Feature2D> detector = Feature2D::create("AKAZE");
 59 |     Ptr<Feature2D> detector = Feature2D::create("ORB");
 60 |     detector->set("nFeatures", 700); // default: 500
 61 | 
 62 |     Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
 63 | 
 64 |     Tracker tracker(detector, matcher);
 65 | 
 66 |     if(argc == 3) {
 67 |         tracker.readCalibration(argv[2]);
 68 |     }
 69 | 
 70 |     auto tpl = makePtr<Template>(Template(image_object,
 71 |                                           Size(125, 266), 
 72 |                                           detector));
 73 | 
 74 |     tracker.add_template("tpl1", tpl);
 75 | 
 76 |     auto stats = makePtr<Stats>();
 77 |     Stats draw_stats, global_stats;
 78 |     int frame_count = 0;
 79 | 
 80 |     for(; ; frame_count++) {
 81 |         bool update_stats = (frame_count % stats_update_period == 0);
 82 |         video_in >> frame;
 83 | 
 84 |         tracker.estimate(frame, stats);
 85 |         global_stats += *stats;
 86 |         if(update_stats) {
 87 |             draw_stats = *stats;
 88 |         }
 89 | 
 90 |         drawStatistics(tracker._debug, draw_stats);
 91 | 
 92 |         imshow("tracking", tracker._debug);
 93 |         if (waitKey(10) >= 0) break;
 94 |     }
 95 |     global_stats /= frame_count;
 96 |     
 97 |     cout << "----------" << endl;
 98 | 
 99 |     cout << "Matches " << stats->matches << endl;
100 |     cout << "Inliers " << stats->inliers << endl;
101 |     cout << "Inlier ratio " << setprecision(2) << stats->ratio << endl;
102 |     cout << "Keypoints " << stats->keypoints << endl;
103 |     cout << endl;
104 | 
105 |     return 0;
106 | }
107 | 


--------------------------------------------------------------------------------
/src/template.cpp:
--------------------------------------------------------------------------------
  1 | #include "opencv2/imgproc.hpp"
  2 | 
  3 | #include "chilitrack.h"
  4 | 
  5 | using namespace std;
  6 | using namespace cv;
  7 | using namespace chilitrack;
  8 | 
  9 | void erosion_mask(Mat src, OutputArray out, int erosion_size )
 10 | {
 11 |     Mat bw;
 12 | 
 13 |     if (src.channels() == 1) {// greyscale
 14 |         // ** assumes the background is white! **
 15 |         threshold(src, bw, 250, 255, THRESH_BINARY );
 16 |         bitwise_not(bw,bw);
 17 |     }
 18 |     else if (src.channels() == 3) { // BGR
 19 |         // ** assumes the background is white! **
 20 |         cvtColor(src, bw, COLOR_BGR2GRAY);
 21 |         threshold(bw, bw, 250, 255, THRESH_BINARY);
 22 |         bitwise_not(bw,bw);
 23 |     }
 24 |     else { // BGRA
 25 |         Mat channels[4];
 26 |         split(src, channels);
 27 |         bw = channels[3];
 28 |     }
 29 | 
 30 | 
 31 |     Mat element = getStructuringElement( MORPH_ELLIPSE,
 32 |             Size( 2*erosion_size + 1, 2*erosion_size+1 ),
 33 |             Point( erosion_size, erosion_size ) );
 34 | 
 35 |     /// Apply the erosion operation
 36 |     erode( bw, out, element );
 37 | 
 38 | }
 39 | 
 40 | Template::Template(Mat tpl, 
 41 |                    Size size, 
 42 |                    Ptr<Feature2D> detector) :
 43 |         tracked(false)
 44 |     {
 45 | 
 46 |     _tpl = tpl.clone();
 47 | 
 48 |     if (!detector) {
 49 |         detector = Feature2D::create("ORB");
 50 |         detector->set("nFeatures", 700);
 51 |     }
 52 | 
 53 |     // Extract features and descriptors from the tpl image
 54 |     // -------------------------------
 55 |     (*detector)(tpl, noArray(), kpts, desc);
 56 | 
 57 |     cout << kpts.size() << " keypoints on the template" << endl;
 58 | 
 59 |     // Store the template bounding box
 60 |     // -------------------------------
 61 | 
 62 |     bb = Rect(-size.width / 2, -size.height / 2, size.width, size.height);
 63 |  
 64 |     // Find good features for tracking
 65 |     // -------------------------------
 66 | 
 67 |     // Compute an eroded mask to obtain features only *inside*
 68 |     // the template
 69 |     Mat mask;
 70 |     erosion_mask(_tpl, mask, 4);
 71 | 
 72 |     // cf http://docs.opencv.org/trunk/modules/imgproc/doc/feature_detection.html#goodfeaturestotrack
 73 |     double quality = 0.01;
 74 |     // TODO: adapt this euclidian distance to the actual tpl size
 75 |     double min_distance = 10;
 76 |     tracking_kpts.reserve(NB_FEATURES);
 77 |     goodFeaturesToTrack(tpl, tracking_kpts, NB_FEATURES, quality, min_distance, mask);
 78 |     
 79 |     if (tracking_kpts.size() < NB_FEATURES) {
 80 |         cerr << "Attention: found " << tracking_kpts.size() << " good features to track" 
 81 |         << " while " << NB_FEATURES << " were desired... Check template.cpp" << endl;
 82 |     }
 83 | 
 84 |     vector<KeyPoint> kpts_viz;
 85 |     KeyPoint::convert(tracking_kpts, kpts_viz);
 86 |     drawKeypoints(_tpl, kpts_viz, _tpl_debug);
 87 | 
 88 |     // Store the physical location of the tracking features 
 89 |     // ----------------------------------------------------
 90 | 
 91 |     // assume (0,0) at the center of the template
 92 |     tpl_points.reserve(NB_FEATURES);
 93 | 
 94 |     int physical_width = size.width; int physical_height = size.height;
 95 |     int width = tpl.cols; int height = tpl.rows;
 96 | 
 97 |     float xratio = (float) physical_width/width;
 98 |     float yratio = (float) physical_height/height;
 99 | 
100 |     for (auto pt : tracking_kpts) {
101 |         tpl_points.push_back(
102 |                     Point3f(pt.x * xratio - physical_width / 2,
103 |                             pt.y * yratio - physical_height / 2,
104 |                             0.f)
105 |                     );
106 |     }
107 | }
108 | 
109 | 


--------------------------------------------------------------------------------
/src/utils.h:
--------------------------------------------------------------------------------
  1 | #ifndef UTILS_H
  2 | #define UTILS_H
  3 | 
  4 | #include <opencv2/core.hpp>
  5 | #include <vector>
  6 | #include <iomanip>
  7 | #include <iostream>
  8 | 
  9 | #include "stats.h"
 10 | 
 11 | using namespace std;
 12 | using namespace cv;
 13 | 
 14 | void drawBoundingBox(Mat image, vector<Point2f> bb);
 15 | void drawStatistics(Mat image, const Stats& stats);
 16 | vector<Point2f> Points(vector<KeyPoint> keypoints);
 17 | 
 18 | void drawBoundingBox(Mat image, vector<Point2f> bb)
 19 | {
 20 |     for(unsigned i = 0; i < bb.size() - 1; i++) {
 21 |         line(image, bb[i], bb[i + 1], Scalar(0, 0, 255), 2);
 22 |     }
 23 |     line(image, bb[bb.size() - 1], bb[0], Scalar(0, 0, 255), 2);
 24 | }
 25 | 
 26 | // by Quentin Bonnard
 27 | void draw3DAxis(Mat image, Matx44d transformation, Mat cameraMatrix)
 28 | {
 29 | 
 30 |     static const float DEFAULT_SIZE = 20.f;
 31 |     static const cv::Vec4d UNITS[4] {
 32 |         {0.f, 0.f, 0.f, 1.f},
 33 |             {DEFAULT_SIZE, 0.f, 0.f, 1.f},
 34 |             {0.f, DEFAULT_SIZE, 0.f, 1.f},
 35 |             {0.f, 0.f, DEFAULT_SIZE, 1.f},
 36 |     };
 37 | 
 38 |     Mat projectionMat = Mat::zeros(4,4,CV_64F);
 39 |     cameraMatrix.copyTo(projectionMat(cv::Rect(0,0,3,3)));
 40 |     Matx44d projection = projectionMat;
 41 |     projection(3,2) = 1;
 42 | 
 43 |     cv::Vec4f referential[4] = {
 44 |         projection*transformation*UNITS[0],
 45 |         projection*transformation*UNITS[1],
 46 |         projection*transformation*UNITS[2],
 47 |         projection*transformation*UNITS[3],
 48 |     };
 49 | 
 50 |     std::vector<cv::Point2f> t2DPoints;
 51 |     for (auto homogenousPoint : referential)
 52 |         t2DPoints.push_back(cv::Point2f(
 53 |                     homogenousPoint[0]/homogenousPoint[3],
 54 |                     homogenousPoint[1]/homogenousPoint[3]));
 55 | 
 56 |     static const int SHIFT = 16;
 57 |     static const float PRECISION = 1<<SHIFT;
 58 |     static const std::string AXIS_NAMES[3] = { "x", "y", "z" };
 59 |     static const cv::Scalar AXIS_COLORS[3] = {
 60 |         {0,0,255},{0,255,0},{255,0,0},
 61 |     };
 62 |     for (int i : {1,2,3}) {
 63 |         cv::line(
 64 |                 image,
 65 |                 PRECISION*t2DPoints[0],
 66 |                 PRECISION*t2DPoints[i],
 67 |                 AXIS_COLORS[i-1],
 68 |                 1, LINE_AA, SHIFT);
 69 |         cv::putText(image, AXIS_NAMES[i-1], t2DPoints[i],
 70 |                 cv::FONT_HERSHEY_SIMPLEX, 0.5, AXIS_COLORS[i-1]);
 71 |     }
 72 | }
 73 | 
 74 | void draw3DRect(Mat image, Rect rect, Matx44d transformation, Mat cameraMatrix)
 75 | {
 76 | 
 77 |     static const cv::Vec4d RECT[4] {
 78 |             {(float) rect.tl().x, (float) rect.tl().y, 0.f, 1.f},
 79 |             {(float) rect.tl().x, (float) rect.br().y, 0.f, 1.f},
 80 |             {(float) rect.br().x, (float) rect.br().y, 0.f, 1.f},
 81 |             {(float) rect.br().x, (float) rect.tl().y, 0.f, 1.f},
 82 |     };
 83 | 
 84 |     Mat projectionMat = Mat::zeros(4,4,CV_64F);
 85 |     cameraMatrix.copyTo(projectionMat(cv::Rect(0,0,3,3)));
 86 |     Matx44d projection = projectionMat;
 87 |     projection(3,2) = 1;
 88 | 
 89 |     cv::Vec4f referential[4] = {
 90 |         projection*transformation*RECT[0],
 91 |         projection*transformation*RECT[1],
 92 |         projection*transformation*RECT[2],
 93 |         projection*transformation*RECT[3],
 94 |     };
 95 | 
 96 |     std::vector<cv::Point2f> t2DPoints;
 97 |     for (auto homogenousPoint : referential)
 98 |         t2DPoints.push_back(cv::Point2f(
 99 |                     homogenousPoint[0]/homogenousPoint[3],
100 |                     homogenousPoint[1]/homogenousPoint[3]));
101 | 
102 |     static const int SHIFT = 16;
103 |     static const float PRECISION = 1<<SHIFT;
104 |     for (int i : {0,1,2,3}) {
105 |         cv::line(
106 |                 image,
107 |                 PRECISION*t2DPoints[i],
108 |                 PRECISION*t2DPoints[(i + 1) % 4],
109 |                 {255, 128, 0},
110 |                 1, LINE_AA, SHIFT);
111 |     }
112 | }
113 | 
114 | Mat combineImages(Mat tpl, Mat frame) {
115 | 
116 |     Mat res;
117 | 
118 |     Size img1size = tpl.size(), img2size = frame.size();
119 |     Size size( img1size.width + img2size.width, MAX(img1size.height, img2size.height)   );
120 |     res.create( size, CV_MAKETYPE(tpl.depth(), 3) );
121 |     res = Scalar::all(0);
122 |     Mat outImg1 = res( Rect(0, 0, img1size.width, img1size.height) );
123 |     Mat outImg2 = res( Rect(img1size.width, 0, img2size.width, img2size.height) );
124 |     tpl.copyTo(outImg1);
125 |     frame.copyTo(outImg2);
126 | 
127 |     return res;
128 | }
129 | 
130 | vector<Point2f> Points(vector<KeyPoint> keypoints)
131 | {
132 |     vector<Point2f> res;
133 |     for(unsigned i = 0; i < keypoints.size(); i++) {
134 |         res.push_back(keypoints[i].pt);
135 |     }
136 |     return res;
137 | }
138 | 
139 | 
140 | #endif // UTILS_H
141 | 


--------------------------------------------------------------------------------
/include/chilitrack.h:
--------------------------------------------------------------------------------
  1 | #ifndef _CHILITRACK_H
  2 | #define _CHILITRACK_H
  3 | 
  4 | #include <opencv2/opencv.hpp>
  5 | #include <vector>
  6 | 
  7 | #include "stats.h" // Stats structure definition
  8 | 
  9 | namespace chilitrack {
 10 | 
 11 | class Template
 12 | {
 13 | 
 14 | public:
 15 |     // Nb of features used for tracking the template
 16 |     static const int NB_FEATURES = 50;
 17 | 
 18 |     /**
 19 |      *
 20 |      * Size is the *physical size*, in mm, of the template image
 21 |      */
 22 |     Template(cv::Mat tpl, 
 23 |              cv::Size size, 
 24 |              cv::Ptr<cv::Feature2D> detector = cv::Ptr<cv::Feature2D>());
 25 | 
 26 |     cv::Mat image() const {return _tpl;}
 27 |     cv::Mat debug() const {return _tpl_debug;}
 28 | 
 29 |     std::vector<cv::KeyPoint> kpts;
 30 |     cv::Mat desc;
 31 |     std::vector<cv::Point2f> tracking_kpts;
 32 | 
 33 |     std::vector<cv::Point3f> tpl_points;
 34 |     cv::Rect bb;
 35 | 
 36 |     bool tracked;
 37 | 
 38 | protected:
 39 |     cv::Mat _tpl;
 40 |     cv::Mat _tpl_debug;
 41 | };
 42 | 
 43 | class Tracker
 44 | {
 45 | public:
 46 |     Tracker(cv::Ptr<cv::Feature2D> _detector = cv::Ptr<cv::Feature2D>(),
 47 |             cv::Ptr<cv::DescriptorMatcher> _matcher = cv::Ptr<cv::DescriptorMatcher>());
 48 | 
 49 |     /** Main tracking method: takes an image and returns all the 4x4 transformations     matrices for the templates found in the image.
 50 |      */
 51 |     std::map<std::string, cv::Matx44d> estimate(
 52 |                                 const cv::Mat frame, 
 53 |                                 cv::Ptr<Stats> stats = cv::Ptr<Stats>());
 54 | 
 55 | 
 56 |     void add_template(const std::string& id, cv::Ptr<Template> tpl) {
 57 |         _templates[id] = tpl;
 58 |     }
 59 | 
 60 |     cv::Ptr<cv::Feature2D> getDetector() {return detector;}
 61 | 
 62 |     /**
 63 |       For accurate results, chilitrack can be provided the calibration data of
 64 |       the camera detecting the chilitags.  See
 65 |       https://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
 66 |       for background on this topic.
 67 | 
 68 |       Note that this method can be called as often as needed with a new calibration
 69 |       configuration (for instance if the user switched to another camera).
 70 | 
 71 |       \param newCameraMatrix the 3x3 matrix of the camera intrinsics (see
 72 |         https://en.wikipedia.org/wiki/Camera_resectioning#Intrinsic_parameters).
 73 |       \param newDistCoeffs a vector containing the distortion coefficients.
 74 |     */
 75 |     void setCalibration(cv::InputArray newCameraMatrix,
 76 |             cv::InputArray newDistCoeffs);
 77 | 
 78 |     /**
 79 |       For accurate results, Chilitags3D can be provided the calibration data of
 80 |       the camera detecting the chilitags.  See
 81 |       http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
 82 |       for background on this topic.
 83 | 
 84 |       Note that this method can be called as often as needed with a new calibration
 85 |       configuration (for instance if the user switched to another camera).
 86 | 
 87 |       This method is similar to setCalibration, but reads the camera calibration
 88 |       information directly from a file, as generated by OpenCV's 'calibration'
 89 |       sample.
 90 | 
 91 |       \param filename the path to a file containing the calibration data
 92 |       */
 93 |     void readCalibration(const std::string &filename);
 94 | 
 95 |     /**
 96 |       Returns the vertical camera field of view in degrees, based on the known
 97 |       camera calibration.
 98 | 
 99 |       Useful to later re-build projection matrices, for instance in OpenGL.
100 |       */
101 |     float getFOV() const;
102 | 
103 | 
104 |     cv::Mat _debug;
105 | 
106 | protected:
107 |     void match(const cv::Mat frame, cv::Ptr<Template> tpl, cv::Ptr<Stats> stats);
108 |     cv::Matx44d track(const cv::Mat frame, cv::Ptr<Template> tpl, cv::Ptr<Stats> stats);
109 | 
110 |     cv::Ptr<cv::Feature2D> detector;
111 |     cv::Ptr<cv::DescriptorMatcher> matcher;
112 | 
113 |     std::vector<cv::Point3f> tpl_points;
114 |     std::vector<cv::Point2f> features;
115 | 
116 |     // when tracking, store the previous frame for optical flow computation
117 |     cv::Mat prev_frame;
118 | 
119 |     cv::Size frameSize;
120 |     cv::Mat cameraMatrix;
121 |     cv::Mat distCoeffs;
122 | 
123 |     std::map<std::string, cv::Ptr<Template>> _templates;
124 | 
125 | private:
126 |     cv::Matx44d computeTransformation(cv::Ptr<Template> tpl) const;
127 | 
128 |     void pruneFeatures(std::vector<cv::Point2f>& features, 
129 |                        std::vector<cv::Point3f>& tpl_points );
130 |     cv::Point2f _centroid;
131 |     double _variance;
132 | 
133 |     // Rotation & translation vectors, computed by cv::solvePnP
134 |     cv::Mat_<double> rotation, translation;
135 | 
136 | };
137 | 
138 | };
139 | 
140 | #endif // _CHILITRACK_H
141 | 


--------------------------------------------------------------------------------
/src/tracker.cpp:
--------------------------------------------------------------------------------
  1 | #include <opencv2/features2d.hpp>
  2 | #include <opencv2/video.hpp>
  3 | #include <opencv2/opencv.hpp>
  4 | 
  5 | #include "utils.h" // Drawing and printing functions
  6 | 
  7 | #include "chilitrack.h"
  8 | 
  9 | using namespace std;
 10 | using namespace cv;
 11 | using namespace chilitrack;
 12 | 
 13 | const double ransac_thresh = 2.5f; // RANSAC inlier threshold
 14 | const double nn_match_ratio = 0.8f; // Nearest-neighbour matching ratio
 15 | 
 16 | const int min_inliers = 10; // Minimal number of inliers to compute transform
 17 | const int min_inliers_ratio = 0.5; // Minimal ratio of inliers vs matches to compute transform
 18 | 
 19 | Point2f mean(const vector<Point2f>& vals)
 20 | {
 21 |     size_t nbvals = vals.size();
 22 | 
 23 |     auto sum = vals[0];
 24 |     for(uint i = 1 ; i < nbvals ; ++i) sum += vals[i];
 25 |     return sum * (1.f/nbvals);
 26 | }
 27 | 
 28 | double variance(const vector<Point2f>& vals)
 29 | {
 30 |     size_t nbvals = vals.size();
 31 | 
 32 |     auto current_mean = mean(vals);
 33 | 
 34 |     auto temp = norm(current_mean-vals[0])*norm(current_mean-vals[0]);
 35 |     for(uint i = 1 ; i < vals.size() ; ++i)
 36 |         temp += norm(current_mean-vals[i])*norm(current_mean-vals[i]);
 37 |     return temp/nbvals;
 38 | }
 39 | 
 40 | 
 41 | Tracker::Tracker(Ptr<Feature2D> _detector, 
 42 |                  Ptr<DescriptorMatcher> _matcher) :
 43 |         detector(_detector),
 44 |         matcher(_matcher),
 45 |         cameraMatrix(),
 46 |         distCoeffs(),
 47 |         rotation(3,1),
 48 |         translation(3,1)
 49 | {
 50 |     if (!detector) {
 51 |         detector = Feature2D::create("ORB");
 52 |         detector->set("nFeatures", 700);
 53 |     }
 54 |     if(!matcher) {
 55 |         matcher = DescriptorMatcher::create("BruteForce-Hamming");
 56 |     }
 57 | 
 58 | }
 59 | 
 60 | map<string, Matx44d> Tracker::estimate(const Mat frame, Ptr<Stats> stats)
 61 | {
 62 | 
 63 |     map<string, Matx44d> res;
 64 | 
 65 |     for (auto& kv : _templates) {
 66 |         if (!kv.second->tracked) {
 67 |             match(frame, kv.second, stats);
 68 |         }
 69 |         else {
 70 |             auto mat = track(frame, kv.second, stats);
 71 |             if (!Mat(mat).empty()) res[kv.first] = mat;
 72 |         }
 73 |     }
 74 | 
 75 |     return res;
 76 | }
 77 | 
 78 | void Tracker::match(const Mat frame, Ptr<Template> tpl, Ptr<Stats> stats)
 79 | {
 80 | 
 81 |     if (cameraMatrix.empty()) {
 82 |         frameSize = frame.size();
 83 |         double focalLength = 700.;
 84 |         cameraMatrix = (cv::Mat_<double>(3,3) <<
 85 |             focalLength ,            0 , frameSize.width /2,
 86 |                     0 ,  focalLength , frameSize.height/2,
 87 |                     0,             0 , 1
 88 |         );
 89 |     }
 90 | 
 91 |     auto t1 = getTickCount();
 92 | 
 93 |     vector<KeyPoint> kp;
 94 |     Mat desc;
 95 |     (*detector)(frame, noArray(), kp, desc);
 96 | 
 97 |     if (stats) stats->keypoints = (int)kp.size();
 98 | 
 99 |     vector< vector<DMatch> > matches;
100 |     vector<KeyPoint> matched1, matched2;
101 |     matcher->knnMatch(tpl->desc, desc, matches, 2);
102 |     for(unsigned i = 0; i < matches.size(); i++) {
103 |         if(matches[i][0].distance < nn_match_ratio * matches[i][1].distance) {
104 |             matched1.push_back(tpl->kpts[matches[i][0].queryIdx]);
105 |             matched2.push_back(      kp[matches[i][0].trainIdx]);
106 |         }
107 |     }
108 |     if(stats) stats->matches = (int)matched1.size();
109 | 
110 |     Mat inlier_mask, homography;
111 |     vector<KeyPoint> inliers1, inliers2;
112 |     vector<DMatch> inlier_matches;
113 |     if(matched1.size() >= 4) {
114 |         homography = findHomography(Points(matched1), Points(matched2),
115 |                                     RANSAC, ransac_thresh, inlier_mask);
116 |     }
117 | 
118 |     if(matched1.size() < 4 || homography.empty()) {
119 | #ifdef CHILITRACK_DEBUG
120 |         Mat outImg;
121 |         Size img1size = tpl->image().size(), img2size = frame.size();
122 |         Size size( img1size.width + img2size.width, MAX(img1size.height, img2size.height)   );
123 |         _debug.create( size, CV_MAKETYPE(tpl->image().depth(), 3) );
124 |         _debug = Scalar::all(0);
125 |         Mat outImg1 = _debug( Rect(0, 0, img1size.width, img1size.height) );
126 |         Mat outImg2 = _debug( Rect(img1size.width, 0, img2size.width, img2size.height) );
127 |         tpl->debug().copyTo(outImg1);
128 |         frame.copyTo(outImg2);
129 | #endif
130 | 
131 |         if(stats) {
132 |             stats->inliers = 0;
133 |             stats->ratio = 0;
134 |         }
135 |         return;
136 |     }
137 |     for(unsigned i = 0; i < matched1.size(); i++) {
138 |         if(inlier_mask.at<uchar>(i)) {
139 |             int new_i = static_cast<int>(inliers1.size());
140 |             inliers1.push_back(matched1[i]);
141 |             inliers2.push_back(matched2[i]);
142 |             inlier_matches.push_back(DMatch(new_i, new_i, 0));
143 |         }
144 |     }
145 |     if(stats) {
146 |         stats->inliers = (int)inliers1.size();
147 |         stats->ratio = stats->inliers * 1.0 / stats->matches;
148 | 
149 |         stats->duration = (int)((double)(getTickCount() - t1)/getTickFrequency() * 1000);
150 |     }
151 | 
152 |     // Project the tpl tracking features onto the scene and enable tracking
153 |     if((float)(inliers1.size()) / matched1.size() >= min_inliers_ratio
154 |         && inliers1.size() >= min_inliers) 
155 |     {
156 |         perspectiveTransform(tpl->tracking_kpts, features, homography);
157 |         prev_frame = frame.clone();
158 |         tpl->tracked = true;
159 |     
160 |         // initially, all tpl features are present
161 |         tpl_points = tpl->tpl_points;
162 |    
163 |         // compute the initial centroid and variance of the feature cloud.
164 |         _centroid = mean(features);
165 |         _variance = variance(features);
166 |     }
167 | 
168 | #ifdef CHILITRACK_DEBUG
169 |     drawMatches(tpl->debug(), inliers1, frame, inliers2,
170 |                 inlier_matches, _debug,
171 |                 Scalar(255, 0, 0), Scalar(255, 0, 0));
172 | #endif
173 | }
174 | 
175 | 
176 | Matx44d Tracker::track(const Mat frame, Ptr<Template> tpl, Ptr<Stats> stats)
177 | {
178 |     auto t1 = getTickCount();
179 | 
180 |     vector<Point2f> next_features;
181 |     next_features.reserve(Template::NB_FEATURES);
182 |     vector<unsigned char> status;
183 |     vector<float> err;
184 | 
185 |     calcOpticalFlowPyrLK(prev_frame, frame, 
186 |                          features, next_features, 
187 |                          status, err, 
188 |                          Size(10,10), 3, 
189 |                          TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 20, 0.01));
190 | 
191 |     prev_frame = frame.clone();
192 | 
193 |     vector<Point3f> remaining_tpl_features;
194 |     vector<Point2f> found;
195 |     found.reserve(Template::NB_FEATURES);
196 | 
197 |     for ( size_t i = 0 ; i < features.size() ; i++ ) {
198 |         if (status[i] == 1) {
199 |             remaining_tpl_features.push_back(tpl_points[i]);
200 |             found.push_back(next_features[i]);
201 |         }
202 |         else {
203 |         }
204 |     }
205 | 
206 |     pruneFeatures(found, remaining_tpl_features);
207 | 
208 |     // update the centroid and variance.
209 |     // The centroid may have changed if the shape is rotated for instance
210 |     // The variance will change when scaling the shape
211 |     _centroid = mean(found);
212 |     _variance = variance(found);
213 | 
214 |     tpl_points = remaining_tpl_features;
215 |     features = found;
216 |     
217 |     // If too many features are lost, leave tracking and go back to match
218 |     if (found.size() < Template::NB_FEATURES / 2) {
219 |         tpl->tracked = false;
220 | 
221 | #ifdef CHILITRACK_DEBUG
222 |         Mat outImg;
223 |         vector<KeyPoint> kpts_viz;
224 |         KeyPoint::convert(found, kpts_viz);
225 |         drawKeypoints(frame, kpts_viz, outImg, Scalar::all(255));
226 | 
227 |         _debug = combineImages(tpl->debug(), outImg);
228 | #endif
229 |         return Matx44d();
230 |     }
231 | 
232 |     auto transformation = computeTransformation(tpl);
233 | 
234 |     if (stats) stats->duration = (int)((double)(getTickCount() - t1)/getTickFrequency() * 1000);
235 | 
236 | #ifdef CHILITRACK_DEBUG
237 |     Mat outImg;
238 |     vector<KeyPoint> kpts_viz;
239 |     KeyPoint::convert(found, kpts_viz);
240 |     drawKeypoints(frame, kpts_viz, outImg, Scalar::all(255));
241 | 
242 |     draw3DAxis(outImg, transformation, cameraMatrix);
243 |     draw3DRect(outImg, tpl->bb, transformation, cameraMatrix);
244 | 
245 | 
246 |     _debug = combineImages(tpl->debug(), outImg);
247 | #endif
248 | 
249 |     return transformation;
250 | }
251 | 
252 | Matx44d Tracker::computeTransformation(Ptr<Template> tpl) const
253 | {
254 |     // Find the 3D pose of our template
255 |     cv::solvePnPRansac(tpl_points,
256 |                  features,
257 |                  cameraMatrix, distCoeffs,
258 |                  rotation, translation, false,
259 |                  SOLVEPNP_ITERATIVE);
260 | 
261 |     cv::Matx33d rotMat;
262 |     cv::Rodrigues(rotation, rotMat);
263 | 
264 |     return {
265 |         rotMat(0,0) , rotMat(0,1) , rotMat(0,2) , translation(0) ,
266 |         rotMat(1,0) , rotMat(1,1) , rotMat(1,2) , translation(1) ,
267 |         rotMat(2,0) , rotMat(2,1) , rotMat(2,2) , translation(2) ,
268 |                   0 ,           0 ,           0 ,                         1 ,
269 |     };
270 | }
271 | 
272 | /**
273 |  * Only keep features 'close enough' to the feature cloud centroid.
274 |  * 'close' is dependent on the initial variance of the cloud.
275 |  */
276 | void Tracker::pruneFeatures(vector<Point2f>& features, vector<Point3f>& tpl_points ) {
277 | 
278 |     vector<Point2f> prunedFeatures;
279 |     vector<Point3f> prunedTplPoints;
280 | 
281 |     for ( size_t i = 0 ; i < features.size() ; i++ ) {
282 |         auto p = features[i];
283 |         if (pow(norm(p - _centroid),2) < 3 * _variance) {
284 |             prunedFeatures.push_back(p);
285 |             prunedTplPoints.push_back(tpl_points[i]);
286 |         }
287 |     }
288 | 
289 |     features = prunedFeatures;
290 |     tpl_points = prunedTplPoints;
291 | 
292 | }
293 | 
294 | /** Sets new camera calibration values.
295 |  */
296 | void Tracker::setCalibration(cv::InputArray newCameraMatrix,
297 |                     cv::InputArray newDistCoeffs){
298 |     cameraMatrix = newCameraMatrix.getMat();
299 |     distCoeffs = newDistCoeffs.getMat();
300 | }
301 | 
302 | void Tracker::readCalibration(const std::string &filename) {
303 | 
304 |     cv::Size size;
305 |     cv::FileStorage fs(filename, cv::FileStorage::READ);
306 |     if (!fs.isOpened()) {
307 |         cerr << "Could not read calibration file " << filename << endl;
308 |         cerr << "Using default camera parameter as fallback." << endl;
309 |         return;
310 |     }
311 | 
312 |     fs["image_width"]             >> size.width;
313 |     fs["image_height"]            >> size.height;
314 | 
315 |     if (frameSize.width * frameSize.height != 0
316 |         && size != frameSize) {
317 |         cerr << "The calibration was done for a resolution (" << size 
318 |              << ") that does not match the current one (" << frameSize 
319 |              << ")." <<endl;
320 |         cerr << "Using default camera parameter as fallback." << endl;
321 |         return;
322 |     }
323 | 
324 |     fs["distortion_coefficients"] >> distCoeffs;
325 |     fs["camera_matrix"]           >> cameraMatrix;
326 | 
327 |     if( distCoeffs.type() != CV_64F )
328 |         distCoeffs = cv::Mat_<double>(distCoeffs);
329 |     if( cameraMatrix.type() != CV_64F )
330 |         cameraMatrix = cv::Mat_<double>(cameraMatrix);
331 | 
332 |     cout << "Using camera calibration from file " << filename << "." << endl;
333 | }
334 | 
335 | float Tracker::getFOV() const {
336 |     return 2 * atan(cameraMatrix.at<double>(1,2)/cameraMatrix.at<double>(0,0)) * 180.f/M_PI;
337 | }
338 | 
339 | 


--------------------------------------------------------------------------------