in visualization/FitAdam/src/meshTrackingProj.cpp [12:187]
void Tracking_MeshVertex_depthMap(
const bool brox,
const cv::Mat& sourceImg,
const cv::Mat& targetImg,
const cv::Mat_<float>& depthMap,
const double* K,
const std::vector<cv::Point3d>& vertices,
std::vector<cv::Point3i>& target_constraints,
const uint sample_dist) // If sample_dist > 0, only return 1 constraint in every (sample_dist, sample_dist) square, else return all valid constraint.
{
if (sourceImg.type() != CV_8UC1 || targetImg.type() != CV_8UC1)
{
std::cerr << "Source image and target image must be grayscale" << std::endl;
exit(0);
}
assert(K);
std::vector<cv::Point2f> pt2d_source;
std::vector<uint> vertices_idx;
for (auto iv = 0u; iv < vertices.size(); iv++)
{
cv::Point2d pt2d;
bool bvisible = ComputePtVisibilityUsingDepthMap(vertices[iv], K, depthMap, pt2d);
if (!bvisible)
continue;
// visible vertices
pt2d_source.push_back(cv::Point2f(pt2d.x, pt2d.y));
vertices_idx.push_back(iv);
}
std::vector<cv::Point3i> new_constraints;
if (brox)
{
// use Brox optical flow
// compute optical from from virtual Image to the target Img
cv::gpu::GpuMat frame0(sourceImg), frame1(targetImg);
frame0.convertTo(frame0, CV_32F, 1.0 / 255.0);
frame1.convertTo(frame1, CV_32F, 1.0 / 255.0);
const double brox_alpha = 0.197;
const double brox_gamma = 50.0;
const double brox_scale = 0.8;
const int brox_inner = 10;
const int brox_outer = 70;
const int brox_solver = 10;
cv::gpu::BroxOpticalFlow brox_flow(brox_alpha, brox_gamma, brox_scale, brox_inner, brox_outer, brox_solver);
cv::gpu::GpuMat cuda_fu, cuda_fv, cuda_bu, cuda_bv;
brox_flow(frame0, frame1, cuda_fu, cuda_fv);
brox_flow(frame1, frame0, cuda_bu, cuda_bv);
cv::Mat fu(cuda_fu), fv(cuda_fv), bu(cuda_bu), bv(cuda_bv);
cv::Mat cover;
if (sample_dist > 0)
{
cover.create(sourceImg.rows / sample_dist + 1, sourceImg.cols / sample_dist + 1, CV_8UC1);
cover.setTo(cv::Scalar(0));
}
for (uint iv2 = 0; iv2 < pt2d_source.size(); iv2++)
{
const int x = round(pt2d_source[iv2].x);
const int y = round(pt2d_source[iv2].y);
const int dest_x = round(x + fu.at<float>(y, x));
const int dest_y = round(y + fv.at<float>(y, x));
if (dest_x < 0 || dest_x >= 1920 || dest_y < 0 || dest_y >= 1080)
continue;
const int back_x = dest_x + round(bu.at<float>(dest_y, dest_x));
const int back_y = dest_y + round(bv.at<float>(dest_y, dest_x));
const float dist = sqrt((x - back_x) * (x - back_x) + (y - back_y) * (y - back_y));
if (dist > backward_check_thresh)
continue;
if (sample_dist > 0)
{
if (cover.at<uchar>(int(dest_y / sample_dist), int(dest_x / sample_dist)))
continue;
cover.at<uchar>(int(dest_y / sample_dist), int(dest_x / sample_dist)) = 1;
}
new_constraints.push_back(cv::Point3i(dest_x, dest_y, vertices_idx[iv2]));
}
#ifdef VISUALIZE_TRACKING
cv::Mat warped(sourceImg.rows, sourceImg.cols, CV_8UC1, cv::Scalar(0));
for (int y = 0; y < sourceImg.rows; y++)
for (int x = 0; x < sourceImg.cols; x++)
{
const int dest_x = round(x + fu.at<float>(y, x));
const int dest_y = round(y + fv.at<float>(y, x));
if (dest_x < 0 || dest_x >= 1920 || dest_y < 0 || dest_y >= 1080)
continue;
const int back_x = dest_x + round(bu.at<float>(dest_y, dest_x));
const int back_y = dest_y + round(bv.at<float>(dest_y, dest_x));
const float dist = sqrt((x - back_x) * (x - back_x) + (y - back_y) * (y - back_y));
if (dist > backward_check_thresh)
continue;
warped.at<uchar>(dest_y, dest_x) = sourceImg.at<uchar>(y, x);
}
cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
cv::imshow( "Display window", warped );
cv::waitKey(0);
#endif
}
else
{
std::vector<cv::Point2f> pt2d_target;
std::vector<cv::Point2f> pt2d_backtracked;
std::vector<uchar> outputStatus;
std::vector<float> outputError;
std::vector<uchar> outputStatus_2;
std::vector<float> outputError_2;
pt2d_target.reserve(pt2d_source.size());
pt2d_backtracked.reserve(pt2d_source.size());
outputStatus.reserve(pt2d_source.size());
outputError.reserve(pt2d_source.size());
outputStatus_2.reserve(pt2d_source.size());
outputError_2.reserve(pt2d_source.size());
// use LK optical flow
const int pyrSize = 25;
//forward tracking
cv::calcOpticalFlowPyrLK(sourceImg, targetImg,
pt2d_source, pt2d_target,
outputStatus, outputError, cvSize(pyrSize, pyrSize), 3);
//backward checking
cv::calcOpticalFlowPyrLK(targetImg, sourceImg,
pt2d_target, pt2d_backtracked,
outputStatus_2, outputError_2, cvSize(pyrSize, pyrSize), 3);
cv::Mat cover;
if (sample_dist > 0)
{
cover.create(sourceImg.rows / sample_dist + 1, sourceImg.cols / sample_dist + 1, CV_8UC1);
cover.setTo(cv::Scalar(0));
}
for (uint iv2 = 0; iv2 < pt2d_source.size(); iv2++)
{
if (!outputStatus[iv2]) continue;
const float backtrackDist = sqrt( (pt2d_backtracked[iv2].x - pt2d_source[iv2].x) * (pt2d_backtracked[iv2].x - pt2d_source[iv2].x) +
(pt2d_backtracked[iv2].y - pt2d_source[iv2].y) * (pt2d_backtracked[iv2].y - pt2d_source[iv2].y) );
if (backtrackDist > backward_check_thresh)
outputStatus[iv2] = 0;
else
{
if (sample_dist > 0)
{
if (!cover.at<uchar>(int(pt2d_target[iv2].y / sample_dist), int(pt2d_target[iv2].x / sample_dist)))
{
new_constraints.push_back(cv::Point3i(pt2d_target[iv2].x, pt2d_target[iv2].y, vertices_idx[iv2]));
cover.at<uchar>(int(pt2d_target[iv2].y / sample_dist), int(pt2d_target[iv2].x / sample_dist)) = 1;
}
}
else
new_constraints.push_back(cv::Point3i(pt2d_target[iv2].x, pt2d_target[iv2].y, vertices_idx[iv2]));
}
}
}
// copy data
target_constraints.insert(target_constraints.end(), new_constraints.begin(), new_constraints.end());
#ifdef VISUALIZE_TRACKING
std::cout << "Number of constraints: " << new_constraints.size() << std::endl;
cv::Mat vis;
cv::hconcat(sourceImg, targetImg, vis);
for (uint iv3 = 0; iv3 < new_constraints.size(); iv3++)
{
cv::line(vis, ProjectPt(vertices[new_constraints[iv3].z], K), cv::Point2f(new_constraints[iv3].x + targetImg.cols, new_constraints[iv3].y), 255);
}
// cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
// cv::imshow( "Display window", depthMap / 1000 );
// cv::waitKey(0);
cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
cv::imshow( "Display window", vis );
cv::waitKey(0);
#endif
}