/**
* @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
* (as done with the Microsoft Kinect), otherwise, if given as CV_32F, it is assumed in meters)
* @param K The calibration matrix
* @param points3d the resulting 3d points. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the
* depth of `K` if `depth` is of depth CV_U
* @param mask the mask of the points to consider (can be empty)
*/
void
depthTo3d(InputArray depth_in, InputArray K_in, OutputArray points3d_out, InputArray mask_in)
{
cv::Mat depth = depth_in.getMat();
cv::Mat K = K_in.getMat();
cv::Mat mask = mask_in.getMat();
CV_Assert(K.cols == 3 && K.rows == 3 && (K.depth() == CV_64F || K.depth()==CV_32F));
CV_Assert(
depth.type() == CV_64FC1 || depth.type() == CV_32FC1 || depth.type() == CV_16UC1 || depth.type() == CV_16SC1);
CV_Assert(mask.empty() || mask.channels() == 1);
// TODO figure out what to do when types are different: convert or reject ?
cv::Mat K_new;
if ((depth.depth() == CV_32F || depth.depth() == CV_64F) && depth.depth() != K.depth())
{
K.convertTo(K_new, depth.depth());
}
else
K_new = K;
// Create 3D points in one go.
cv::Mat points3d;
if (!mask.empty())
{
cv::Mat points3d;
depthTo3dMask(depth, K_new, mask, points3d);
points3d_out.create(points3d.size(), CV_MAKETYPE(K_new.depth(), 3));
points3d.copyTo(points3d_out.getMat());
}
else
{
points3d_out.create(depth.size(), CV_MAKETYPE(K_new.depth(), 3));
cv::Mat points3d = points3d_out.getMat();
if (K_new.depth() == CV_64F)
depthTo3dNoMask<double>(depth, K_new, points3d);
else
depthTo3dNoMask<float>(depth, K_new, points3d);
}
}
int
process(const tendrils& in, const tendrils& out)
{
cv::Mat depth = in.get<cv::Mat>("depth");
if (depth.empty())
return ecto::OK;
cv::Mat R, T, K;
in.get<cv::Mat>("R").convertTo(R, CV_64F);
in.get<cv::Mat>("T").convertTo(T, CV_64F);
in.get<cv::Mat>("K").convertTo(K, CV_64F);
if (R.empty() || T.empty() || K.empty())
return ecto::OK;
cv::Mat mask = cv::Mat::zeros(depth.size(), CV_8UC1);
box_mask.create(depth.size());
box_mask.setTo(cv::Scalar(0));
std::vector<cv::Point3f> box(8);
box[0] = cv::Point3f(*x_crop, *y_crop, *z_min);
box[1] = cv::Point3f(-*x_crop, *y_crop, *z_min);
box[2] = cv::Point3f(-*x_crop, -*y_crop, *z_min);
box[3] = cv::Point3f(*x_crop, -*y_crop, *z_min);
box[4] = cv::Point3f(*x_crop, *y_crop, *z_crop);
box[5] = cv::Point3f(-*x_crop, *y_crop, *z_crop);
box[6] = cv::Point3f(-*x_crop, -*y_crop, *z_crop);
box[7] = cv::Point3f(*x_crop, -*y_crop, *z_crop);
std::vector<cv::Point2f> projected, hull;
cv::projectPoints(box, R, T, K, cv::Mat(4, 1, CV_64FC1, cv::Scalar(0)), projected);
cv::convexHull(projected, hull, true);
std::vector<cv::Point> points(hull.size());
std::copy(hull.begin(), hull.end(), points.begin());
cv::fillConvexPoly(box_mask, points.data(), points.size(), cv::Scalar::all(255));
int width = mask.size().width;
int height = mask.size().height;
cv::Mat_<uint8_t>::iterator it = mask.begin<uint8_t>();
cv::Mat_<uint8_t>::iterator mit = box_mask.begin();
cv::Mat_<cv::Vec3f> points3d;
depthTo3dMask(K, depth, box_mask, points3d);
if (points3d.empty())
return ecto::OK;
cv::Matx<double, 3, 1> p, p_r, Tx(T); //Translation
cv::Matx<double, 3, 3> Rx; //inverse Rotation
Rx = cv::Mat(R.t());
cv::Mat_<cv::Vec3f>::iterator point = points3d.begin();
// std::cout << cv::Mat(Rx) << "\n" << cv::Mat(Tx) << std::endl;
// std::cout << fx << " " << fy << " " << cx << " " << cy << " " << std::endl;
double z_min_ = *z_min, z_max_ = *z_crop, x_min_ = -*x_crop, x_max_ = *x_crop, y_min_ = -*y_crop,
y_max_ = *y_crop;
for (int v = 0; v < height; v++)
{
for (int u = 0; u < width; u++, ++it, ++mit)
{
if (*mit == 0)
continue;
//calculate the point based on the depth
p(0) = (*point).val[0];
p(1) = (*point).val[1];
p(2) = (*point).val[2];
++point;
p_r = Rx * (p - Tx);
// std::cout <<"p=" << cv::Mat(p) << ",p_r="<< cv::Mat(p_r) << std::endl;
if (p_r(2) > z_min_ && p_r(2) < z_max_ && p_r(0) > x_min_ && p_r(0) < x_max_ && p_r(1) > y_min_
&& p_r(1) < y_max_)
*it = 255;
}
}
out["mask"] << mask;
return ecto::OK;
}
