Initialize implementation of tracking in RapidTesting.cpp

Update after discussion. Fixed issue with cv::Mat T.