void PeopleTracker::estimateGroundPlaneFromUserInput(const ntk::RGBDImage& image)
{
// Estimate ground plane equation asking 3 points to the user.
TrackerGroundMouseData ground_mouse_data;
ground_mouse_data.window_name = "Select 3 points to estimate the ground plane (right click)";
namedWindow(ground_mouse_data.window_name,
CV_WINDOW_NORMAL|CV_WINDOW_KEEPRATIO|CV_GUI_EXPANDED);
ground_mouse_data.image = toMat3b(normalize_toMat1b(image.depth()));
imshow(ground_mouse_data.window_name, ground_mouse_data.image);
setMouseCallback(ground_mouse_data.window_name, on_tracker_ground_mouse, &ground_mouse_data);
while (ground_mouse_data.points.size() != 3)
{
waitKey(30);
QApplication::processEvents();
}
ntk_dbg(0) << "I am here!";
Point3f p0, p1, p2;
p0 = image.calibration()->depth_pose->unprojectFromImage(ground_mouse_data.points[0],
image.depth()(ground_mouse_data.points[0]));
p1 = image.calibration()->depth_pose->unprojectFromImage(ground_mouse_data.points[1],
image.depth()(ground_mouse_data.points[1]));
p2 = image.calibration()->depth_pose->unprojectFromImage(ground_mouse_data.points[2],
image.depth()(ground_mouse_data.points[2]));
destroyWindow(ground_mouse_data.window_name);
Vec3f normal = -(p1-p0).cross(p2-p0);
normalize(normal);
if (normal[2] < 0)
normal = -normal;
m_ground_plane = Plane(normal, p0);
}
void trackHead(ntk::RGBDImage& img, Point3f& hpos) {
Mat1f& depth = img.depthRef();
Mat1b& mask = img.depthMaskRef();
const RGBDCalibration* calib = img.calibration();
if (calib) {
const Pose3D* rgb_pose = calib->rgb_pose;
const Pose3D* depth_pose = calib->depth_pose;
for (int y = 0; y < depth.rows; y+=10)
for (int x = 0; x < depth.cols; x+=10) {
if (mask.at<bool>(y, x)) {
// probably a more opencv-ey way to do this, but eh.
y = min(depth.rows, y + 20);
int minx = x - 9, maxx = x + 40;
for(int c = max(x - 9, 0); c < x + 40 && c < depth.cols; c++) {
if(!mask.at<bool>(y, x)) {
if (c < x) minx = c;
if (c > x) { maxx = c; break; }
}
}
x = (minx + maxx) / 2;
float val = depth.at<float>(y, x);
Point2f screenpos(x, y);
cv::Point3f p = depth_pose->unprojectFromImage(screenpos, val);
hpos.x = p.x; hpos.y = p.y; hpos.z = p.z - 0.05;
return;
}
}
}
}
void trackHands(ntk::RGBDImage& img, Point3f& rpos, Point3f& bpos) {
Mat1f& depth = img.depthRef();
Mat1b& mask = img.depthMaskRef();
const RGBDCalibration* calib = img.calibration();
if (calib) {
const Pose3D* rgb_pose = calib->rgb_pose;
const Pose3D* depth_pose = calib->depth_pose;
Mat rgb = img.rgb().clone();
//blur(rgb, rgb, Size(5, 5));
int wid = 64, ht = 64;
int min_rad = 4;
cv::Point2f fudge(8, 8);
Mat1b result(Size(wid, ht), 0);
Point2f top;
double rrad = min_rad,
brad = min_rad;
vector<Mat> hsvs;
Point3f upleft = depth_pose->projectToImage(rpos) - Point3f(wid / 2, ht / 2, 0);
if (upleft.x == upleft.x) { //!isNaN(upleft)
filterColor(img, hsvs, result, upleft, fudge, 160, 10, 160, 256, 80, 256);
findLargest(result, top, rrad);
top.x += upleft.x; top.y += upleft.y;
top *= STEP_SIZE;
mapPos(depth_pose, depth, top, rpos);
//printf("partial r %f %f %f \n", top.x, top.y, rrad);
hsvs.clear();
}
upleft = depth_pose->projectToImage(bpos) - Point3f(wid / 2, ht / 2, 0);
if (upleft.x == upleft.x) { //!isNaN(upleft)
result = Mat1b(Size(wid, ht), 0);
filterColor(img, hsvs, result, upleft, fudge, 70, 100, 50, 256, 50, 256);
findLargest(result, top, brad);
//displayBoolean(result);
top.x += upleft.x; top.y += upleft.y;
top *= STEP_SIZE;
mapPos(depth_pose, depth, top, bpos);
// printf("partial b %f %f %f \n", top.x, top.y, brad);
hsvs.clear();
}
upleft = Point3f(0, 0, 0);
if (rrad <= min_rad) {
top = Point2f(0, 0);
result = Mat1b(Size(depth.cols / STEP_SIZE, depth.rows / STEP_SIZE), 0);
filterColor(img, hsvs, result, upleft, fudge, 160, 10, 160, 256, 80, 256);
//Mat1b h(hsvs[0]);
//displayVal(h);
findLargest(result, top, rrad);
top *= STEP_SIZE;
mapPos(depth_pose, depth, top, rpos);
//printf("full r %f %f %f \n", top.x, top.y, rrad);
}
if (brad <= min_rad) {
top = Point2f(0, 0);
result = Mat1b(Size(depth.cols / STEP_SIZE, depth.rows / STEP_SIZE), 0);
filterColor(img, hsvs, result, upleft, fudge, 70, 100, 50, 256, 50, 256);
findLargest(result, top, brad);
top *= STEP_SIZE;
mapPos(depth_pose, depth, top, bpos);
//printf("full b %f %f %f \n", top.x, top.y, brad);
}
} else {
bpos.x = bpos.y = bpos.z = 0;
rpos.x = rpos.y = rpos.z = 0;
}
}