mirror of
https://github.com/opencv/opencv.git
synced 2024-12-12 23:49:36 +08:00
warpFrame() rewritten, interface changed
This commit is contained in:
Rostislav Vasilikhin
parent
492d9dba1e
commit
fd14b959cf
@ -124,20 +124,20 @@ CV_EXPORTS_W void depthTo3d(InputArray depth, InputArray K, OutputArray points3d
|
||||
*/
|
||||
CV_EXPORTS_W void rescaleDepth(InputArray in, int type, OutputArray out, double depth_factor = 1000.0);
|
||||
|
||||
/** Warp the image: compute 3d points from the depth, transform them using given transformation,
|
||||
* then project color point cloud to an image plane.
|
||||
* This function can be used to visualize results of the Odometry algorithm.
|
||||
* @param image The image (of CV_8UC1 or CV_8UC3 type)
|
||||
* @param depth The depth (of type used in depthTo3d fuction)
|
||||
* @param mask The mask of used pixels (of CV_8UC1), it can be empty
|
||||
* @param Rt The transformation that will be applied to the 3d points computed from the depth
|
||||
* @param cameraMatrix Camera matrix
|
||||
* @param warpedImage The warped image.
|
||||
* @param warpedDepth The warped depth.
|
||||
* @param warpedMask The warped mask.
|
||||
/** Warps depth or RGB-D image by reprojecting it in 3d, applying Rt transformation
|
||||
* and then projecting it back onto the image plane.
|
||||
* This function can be used to visualize the results of the Odometry algorithm.
|
||||
* @param depth Depth data, should be 1-channel CV_16U, CV_16S, CV_32F or CV_64F
|
||||
* @param image RGB image (optional), should be 1-, 3- or 4-channel CV_8U
|
||||
* @param mask Mask of used pixels (optional), should be CV_8UC1
|
||||
* @param Rt Rotation+translation matrix (3x4 or 4x4) to be applied to depth points
|
||||
* @param cameraMatrix Camera intrinsics matrix (3x3)
|
||||
* @param warpedDepth The warped depth data (optional)
|
||||
* @param warpedImage The warped RGB image (optional)
|
||||
* @param warpedMask The mask of valid pixels in warped image (optional)
|
||||
*/
|
||||
CV_EXPORTS_W void warpFrame(InputArray image, InputArray depth, InputArray mask, InputArray Rt, InputArray cameraMatrix,
|
||||
OutputArray warpedImage, OutputArray warpedDepth = noArray(), OutputArray warpedMask = noArray());
|
||||
CV_EXPORTS_W void warpFrame(InputArray depth, InputArray image, InputArray mask, InputArray Rt, InputArray cameraMatrix,
|
||||
OutputArray warpedDepth = noArray(), OutputArray warpedImage = noArray(), OutputArray warpedMask = noArray());
|
||||
|
||||
enum RgbdPlaneMethod
|
||||
{
|
||||
|
||||
@ -362,92 +362,219 @@ bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
|
||||
}
|
||||
|
||||
|
||||
template<class ImageElemType>
|
||||
static void
|
||||
warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
|
||||
const Mat& Rt, const Mat& cameraMatrix,
|
||||
OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
|
||||
|
||||
void warpFrame(InputArray depth, InputArray image, InputArray mask,
|
||||
InputArray Rt, InputArray cameraMatrix,
|
||||
OutputArray warpedDepth, OutputArray warpedImage, OutputArray warpedMask)
|
||||
{
|
||||
//TODO: take into account that image can be empty
|
||||
//TODO: mask can also be empty
|
||||
|
||||
CV_Assert(_image.size() == depth.size());
|
||||
|
||||
Mat cloud;
|
||||
depthTo3d(depth, cameraMatrix, cloud);
|
||||
|
||||
//TODO: replace this by channel split/merge after the function is covered by tests
|
||||
Mat cloud3;
|
||||
cloud3.create(cloud.size(), CV_32FC3);
|
||||
for (int y = 0; y < cloud3.rows; y++)
|
||||
CV_Assert(cameraMatrix.size() == Size(3, 3));
|
||||
CV_Assert(cameraMatrix.depth() == CV_32F || cameraMatrix.depth() == CV_64F);
|
||||
Matx33d K, Kinv;
|
||||
cameraMatrix.getMat().convertTo(K, CV_64F);
|
||||
std::vector<bool> camPlaces { /* fx */ true, false, /* cx */ true, false, /* fy */ true, /* cy */ true, false, false, /* 1 */ true};
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
for (int x = 0; x < cloud3.cols; x++)
|
||||
CV_Assert(camPlaces[i] == (K.val[i] > DBL_EPSILON));
|
||||
}
|
||||
Kinv = K.inv();
|
||||
|
||||
CV_Assert((Rt.cols() == 4) && (Rt.rows() == 3 || Rt.rows() == 4));
|
||||
CV_Assert(Rt.depth() == CV_32F || Rt.depth() == CV_64F);
|
||||
Mat rtmat;
|
||||
Rt.getMat().convertTo(rtmat, CV_64F);
|
||||
Affine3d rt(rtmat);
|
||||
|
||||
CV_Assert(!depth.empty());
|
||||
CV_Assert(depth.channels() == 1);
|
||||
double maxDepth = 0;
|
||||
int depthDepth = depth.depth();
|
||||
switch (depthDepth)
|
||||
{
|
||||
Vec4f p = cloud.at<Vec4f>(y, x);
|
||||
cloud3.at<Vec3f>(y, x) = Vec3f(p[0], p[1], p[2]);
|
||||
case CV_16U:
|
||||
maxDepth = std::numeric_limits<unsigned short>::max();
|
||||
break;
|
||||
case CV_32F:
|
||||
maxDepth = std::numeric_limits<float>::max();
|
||||
break;
|
||||
case CV_64F:
|
||||
maxDepth = std::numeric_limits<double>::max();
|
||||
break;
|
||||
default:
|
||||
CV_Error(Error::StsBadArg, "Unsupported depth data type");
|
||||
}
|
||||
Mat_<double> depthDbl;
|
||||
depth.getMat().convertTo(depthDbl, CV_64F);
|
||||
Size sz = depth.size();
|
||||
|
||||
Mat_<uchar> maskMat;
|
||||
if (!mask.empty())
|
||||
{
|
||||
CV_Assert(mask.type() == CV_8UC1);
|
||||
CV_Assert(mask.size() == sz);
|
||||
maskMat = mask.getMat();
|
||||
}
|
||||
|
||||
int imageType = -1;
|
||||
Mat imageMat;
|
||||
if (!image.empty())
|
||||
{
|
||||
imageType = image.type();
|
||||
CV_Assert(imageType == CV_8UC1 || imageType == CV_8UC3 || imageType == CV_8UC4);
|
||||
CV_Assert(image.size() == sz);
|
||||
CV_Assert(warpedImage.needed());
|
||||
imageMat = image.getMat();
|
||||
}
|
||||
|
||||
CV_Assert(warpedDepth.needed() || warpedImage.needed() || warpedMask.needed());
|
||||
|
||||
// Getting new coords for depth point
|
||||
|
||||
// see the explanation in the loop below
|
||||
Matx33d krki = K * rt.rotation() * Kinv;
|
||||
Matx32d krki_cols01 = krki.get_minor<3, 2>(0, 0);
|
||||
Vec3d krki_col2(krki.col(2).val);
|
||||
|
||||
Vec3d ktmat = K * rt.translation();
|
||||
Mat_<Vec3d> reprojBack(depth.size());
|
||||
for (int y = 0; y < sz.height; y++)
|
||||
{
|
||||
const uchar* maskRow = maskMat.empty() ? nullptr : maskMat[y];
|
||||
const double* depthRow = depthDbl[y];
|
||||
Vec3d* reprojRow = reprojBack[y];
|
||||
for (int x = 0; x < sz.width; x++)
|
||||
{
|
||||
double z = depthRow[x];
|
||||
bool badz = cvIsNaN(z) || cvIsInf(z) || z <= 0 || z >= maxDepth || (maskRow && !maskRow[x]);
|
||||
Vec3d v;
|
||||
if (!badz)
|
||||
{
|
||||
// Reproject pixel (x, y) using known z, rotate+translate and project back
|
||||
// getting new pixel in projective coordinates:
|
||||
// v = K * Rt * K^-1 * ([x, y, 1] * z) = [new_x*new_z, new_y*new_z, new_z]
|
||||
// v = K * (R * K^-1 * ([x, y, 1] * z) + t) =
|
||||
// v = krki * [x, y, 1] * z + ktmat =
|
||||
// v = (krki_cols01 * [x, y] + krki_col2) * z + K * t
|
||||
v = (krki_cols01 * Vec2d(x, y) + krki_col2) * z + ktmat;
|
||||
}
|
||||
else
|
||||
{
|
||||
v = Vec3d();
|
||||
}
|
||||
reprojRow[x] = v;
|
||||
}
|
||||
}
|
||||
|
||||
//TODO: do the following instead of the functions: K*R*Kinv*[uv1]*z + K*t
|
||||
//TODO: optimize it using K's structure
|
||||
std::vector<Point2f> points2d;
|
||||
Mat transformedCloud;
|
||||
perspectiveTransform(cloud3, transformedCloud, Rt);
|
||||
projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
|
||||
Mat::zeros(1, 5, CV_64FC1), points2d);
|
||||
// Draw new depth in z-buffer manner
|
||||
|
||||
Mat image = _image.getMat();
|
||||
Size sz = _image.size();
|
||||
Mat mask = _mask.getMat();
|
||||
_warpedImage.create(sz, image.type());
|
||||
Mat warpedImage = _warpedImage.getMat();
|
||||
Mat warpedImageMat;
|
||||
if (warpedImage.needed())
|
||||
{
|
||||
warpedImage.create(sz, imageType);
|
||||
warpedImage.setZero();
|
||||
warpedImageMat = warpedImage.getMat();
|
||||
}
|
||||
|
||||
const double infinity = std::numeric_limits<double>::max();
|
||||
|
||||
Mat zBuffer(sz, CV_32FC1, infinity);
|
||||
|
||||
Mat zBuffer(sz, CV_32FC1, std::numeric_limits<float>::max());
|
||||
const Rect rect = Rect(Point(), sz);
|
||||
|
||||
for (int y = 0; y < sz.height; y++)
|
||||
{
|
||||
//const Point3f* cloud_row = cloud.ptr<Point3f>(y);
|
||||
const Point3f* transformedCloud_row = transformedCloud.ptr<Point3f>(y);
|
||||
const Point2f* points2d_row = &points2d[y * sz.width];
|
||||
const ImageElemType* image_row = image.ptr<ImageElemType>(y);
|
||||
const uchar* mask_row = mask.empty() ? 0 : mask.ptr<uchar>(y);
|
||||
uchar* imageRow1ch = nullptr;
|
||||
Vec3b* imageRow3ch = nullptr;
|
||||
Vec4b* imageRow4ch = nullptr;
|
||||
switch (imageType)
|
||||
{
|
||||
case -1:
|
||||
break;
|
||||
case CV_8UC1:
|
||||
imageRow1ch = imageMat.ptr<uchar>(y);
|
||||
break;
|
||||
case CV_8UC3:
|
||||
imageRow3ch = imageMat.ptr<Vec3b>(y);
|
||||
break;
|
||||
case CV_8UC4:
|
||||
imageRow4ch = imageMat.ptr<Vec4b>(y);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
const Vec3d* reprojRow = reprojBack[y];
|
||||
for (int x = 0; x < sz.width; x++)
|
||||
{
|
||||
const float transformed_z = transformedCloud_row[x].z;
|
||||
const Point2i p2d = points2d_row[x];
|
||||
if ((!mask_row || mask_row[x]) && transformed_z > 0 &&
|
||||
rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at<float>(p2d) > transformed_z)
|
||||
Vec3d v = reprojRow[x];
|
||||
double z = v[2];
|
||||
|
||||
if (z > 0)
|
||||
{
|
||||
warpedImage.at<ImageElemType>(p2d) = image_row[x];
|
||||
zBuffer.at<float>(p2d) = transformed_z;
|
||||
Point uv(cvFloor(v[0] / z), cvFloor(v[1] / z));
|
||||
if (rect.contains(uv))
|
||||
{
|
||||
float oldz = zBuffer.at<float>(uv);
|
||||
|
||||
if (z < oldz)
|
||||
{
|
||||
zBuffer.at<float>(uv) = z;
|
||||
|
||||
switch (imageType)
|
||||
{
|
||||
case -1:
|
||||
break;
|
||||
case CV_8UC1:
|
||||
warpedImageMat.at<uchar>(uv) = imageRow1ch[x];
|
||||
break;
|
||||
case CV_8UC3:
|
||||
warpedImageMat.at<Vec3b>(uv) = imageRow3ch[x];
|
||||
break;
|
||||
case CV_8UC4:
|
||||
warpedImageMat.at<Vec4b>(uv) = imageRow4ch[x];
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (warpedMask.needed())
|
||||
Mat(zBuffer != std::numeric_limits<float>::max()).copyTo(warpedMask);
|
||||
if (warpedDepth.needed() || warpedMask.needed())
|
||||
{
|
||||
Mat goodMask = (zBuffer < infinity);
|
||||
|
||||
if (warpedDepth.needed())
|
||||
{
|
||||
zBuffer.setTo(std::numeric_limits<float>::quiet_NaN(), zBuffer == std::numeric_limits<float>::max());
|
||||
zBuffer.copyTo(warpedDepth);
|
||||
warpedDepth.create(sz, depthDepth);
|
||||
|
||||
double badVal;
|
||||
switch (depthDepth)
|
||||
{
|
||||
case CV_16U:
|
||||
badVal = 0;
|
||||
break;
|
||||
case CV_32F:
|
||||
badVal = std::numeric_limits<float>::quiet_NaN();
|
||||
break;
|
||||
case CV_64F:
|
||||
badVal = std::numeric_limits<double>::quiet_NaN();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
zBuffer.convertTo(warpedDepth, depthDepth);
|
||||
warpedDepth.setTo(badVal, ~goodMask);
|
||||
}
|
||||
|
||||
if (warpedMask.needed())
|
||||
{
|
||||
warpedMask.create(sz, CV_8UC1);
|
||||
goodMask.copyTo(warpedMask);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void warpFrame(InputArray image, InputArray depth, InputArray mask,
|
||||
InputArray Rt, InputArray cameraMatrix,
|
||||
OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
|
||||
{
|
||||
if (image.type() == CV_8UC1)
|
||||
warpFrameImpl<uchar>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(),
|
||||
warpedImage, warpedDepth, warpedMask);
|
||||
else if (image.type() == CV_8UC3)
|
||||
warpFrameImpl<Point3_<uchar>>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(),
|
||||
warpedImage, warpedDepth, warpedMask);
|
||||
else
|
||||
CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user