in visualization/FitAdam/src/meshTrackingProj.cpp [300:452]
void getVirtualImageConstraint(
std::unique_ptr<Renderer>& render,
const double* K,
const CMeshModelInstance& mesh1,
const CMeshModelInstance& mesh2,
const cv::Mat_<float>& depthMap1, // depthmap of mesh1
const cv::Mat_<float>& depthMap2, // depthmap of mesh2
const cv::Mat& sourceImg,
const cv::Mat& targetImg,
cv::Mat& resultImg,
std::vector<cv::Point3i>& target_constraints,
const uint sample_dist)
{
assert(render);
assert(mesh1.m_vertices.size() == mesh2.m_vertices.size());
VisualizedData vis_data;
vis_data.read_rgbfloat_buffer = RGBFLOAT_BUFFER;
CopyMesh(mesh2, vis_data);
for (auto iv = 0u; iv < mesh1.m_vertices.size(); iv++)
{
cv::Point2d target;
const bool bvisibility = ComputePtVisibilityUsingDepthMap(mesh1.m_vertices[iv], K, depthMap1, target);
if (bvisibility)
vis_data.m_meshVerticesColor[iv] = cv::Point3d(target.x, target.y, 0.); // change the color from RGB to our desired value
else
vis_data.m_meshVerticesColor[iv] = cv::Point3d(-10000., -10000., 0.);
}
render->CameraMode(0);
double calibK[9];
std::copy(K, K + 9, calibK);
render->options.K = calibK;
render->RenderProjection(vis_data);
render->RenderAndReadProjection();
cv::Mat XY = cv::Mat(sourceImg.rows, sourceImg.cols, CV_32FC3, RGBFLOAT_BUFFER); // wrapper around buffer
cv::flip(XY, XY, 0);
cv::Mat XYc[3];
cv::split(XY, XYc);
resultImg.create(sourceImg.rows, sourceImg.cols, sourceImg.type());
resultImg.setTo(cv::Scalar(0, 0, 0));
int count_pixel = 0;
for (auto y = 0; y < 1080; y++)
for (auto x = 0; x < 1920; x++)
{
const int x0 = XYc[0].at<float>(y, x);
const int y0 = XYc[1].at<float>(y, x);
if (x0 >= 0 && x0 < 1920 && y0 >= 0 && y0 < 1080)
{
resultImg.at<uchar>(y, x) = sourceImg.at<uchar>(y0, x0);
count_pixel++;
}
else
{
resultImg.at<uchar>(y, x) = targetImg.at<uchar>(y, x);
// resultImg.at<uchar>(y, x) = sourceImg.at<uchar>(y, x);
}
}
std::cout << "Copy from texture pixels: " << count_pixel << std::endl;
#ifdef VISUALIZE_TRACKING
cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
cv::imshow( "Display window", depthMap2 / 999 );
cv::waitKey(0);
cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
cv::imshow( "Display window", XYc[0] / 1920 );
cv::waitKey(0);
cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
cv::imshow( "Display window", resultImg );
cv::waitKey(0);
#endif
// compute optical from from virtual Image to the target Img
cv::gpu::GpuMat frame0(resultImg), 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 warped(1080, 1920, CV_8UC1); warped.setTo(cv::Scalar(0));
cv::Mat tracking_valid(1080, 1920, CV_8UC1, cv::Scalar(0));
uint count_threshold = 0;
for (auto y = 0; y < 1080; y++)
for (auto x = 0; x < 1920; x++)
{
const int dest_x = x + round(fu.at<float>(y, x));
const int dest_y = y + round(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)
{
count_threshold++;
continue;
}
const int x0 = XYc[0].at<float>(y, x);
const int y0 = XYc[1].at<float>(y, x);
// warped.at<uchar>(dest_y, dest_x) = resultImg.at<uchar>(y, x); // with the if statement, show only the pixel from the mesh.
if (x0 >= 0 && x0 < 1920 && y0 >= 0 && y0 < 1080)
warped.at<uchar>(dest_y, dest_x) = resultImg.at<uchar>(y, x);
tracking_valid.at<uchar>(y, x) = 1;
}
std::cout << "Back tracking threshold exceeded: " << count_threshold << " points." << std::endl;
#ifdef VISUALIZE_TRACKING
cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
cv::imshow( "Display window", targetImg );
cv::waitKey(0);
cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
cv::imshow( "Display window", warped );
cv::waitKey(0);
#endif
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));
}
int count_invisible = 0;
for (auto iv = 0u; iv < mesh2.m_vertices.size(); iv++)
{
cv::Point2d pt2d;
const bool bvisibility = ComputePtVisibilityUsingDepthMap(mesh2.m_vertices[iv], K, depthMap2, pt2d);
if (!bvisibility) {count_invisible++; continue;}
const int x = round(pt2d.x);
const int y = round(pt2d.y);
if (!tracking_valid.at<uchar>(y, x)) continue;
if (sample_dist > 0)
if (cover.at<uchar>(int(y / sample_dist), int(x / sample_dist)))
continue;
const int dest_x = x + round(fu.at<float>(y, x));
const int dest_y = y + round(fv.at<float>(y, x));
if (sample_dist > 0)
cover.at<uchar>(int(y / sample_dist), int(x / sample_dist)) = 1;
target_constraints.push_back(cv::Point3i(dest_x, dest_y, iv));
}
std::cout << "Invisible vertices: " << count_invisible << std::endl;
}