Files
opencv-MIRROR/modules/ptcloud/src/odometry_functions.cpp
Alexander Smorkalov fc3803c67b Merge pull request #29224 from asmorkalov:as/ptcloud2
Dedicated pointcloud module #29224

OpenCV contrib: https://github.com/opencv/opencv_contrib/pull/4134

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [ ] The PR is proposed to the proper branch
- [ ] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake
2026-06-04 12:19:02 +03:00

1501 lines
55 KiB
C++

// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#include "odometry_functions.hpp"
#include "precomp.hpp"
#include "utils.hpp"
#include "opencl_kernels_ptcloud.hpp"
#include "opencv2/imgproc.hpp"
#include <opencv2/core/hal/intrin.hpp>
#include <opencv2/core/dualquaternion.hpp>
namespace cv
{
static void randomSubsetOfMask(InputOutputArray _mask, float part)
{
const int minPointsCount = 1000; // minimum point count (we can process them fast)
const int nonzeros = countNonZero(_mask);
const int needCount = std::max(minPointsCount, int(_mask.total() * part));
if (needCount < nonzeros)
{
RNG rng;
Mat mask = _mask.getMat();
Mat subset(mask.size(), CV_8UC1, Scalar(0));
int subsetSize = 0;
while (subsetSize < needCount)
{
int y = rng(mask.rows);
int x = rng(mask.cols);
if (mask.at<uchar>(y, x))
{
subset.at<uchar>(y, x) = 255;
mask.at<uchar>(y, x) = 0;
subsetSize++;
}
}
_mask.assign(subset);
}
}
static UMat prepareScaledDepth(OdometryFrame& frame)
{
UMat depth;
frame.getDepth(depth);
CV_Assert(!depth.empty());
bool isFloat = (depth.type() == CV_32FC1 || depth.type() == CV_64FC1);
// Odometry works well with depth values in range [0, 10)
// If it's bigger, let's scale it down by 5000, a typical depth factor
UMat depthFlt;
if (isFloat)
{
depth.convertTo(depthFlt, CV_32FC1);
patchNaNs(depthFlt, 0);
}
double maxv;
cv::minMaxLoc(isFloat ? depthFlt : depth, nullptr, &maxv);
if (!isFloat || maxv > 10)
{
depth.convertTo(depthFlt, CV_32FC1, maxv > 10 ? (1.f / 5000.f) : 1.f);
patchNaNs(depthFlt, 0);
}
frame.impl->scaledDepth = depthFlt;
return depthFlt;
}
//TODO: this with bilateral, maybe masks
static void preparePyramidDepth(UMat depth, std::vector<UMat>& dpyramids, int maxLevel)
{
//TODO: make resize down instead
buildPyramid(depth, dpyramids, maxLevel);
//resize(depth, resized, dsize, fx, fy, INTER_NEAREST);
}
static void preparePyramidMask(UMat mask, const std::vector<UMat> pyramidDepth, int nLevels, float minDepth, float maxDepth,
std::vector<UMat>& pyramidMask)
{
minDepth = std::max(0.f, minDepth);
buildPyramid(mask, pyramidMask, nLevels - 1);
for (int i = 0; i < nLevels; i++)
{
UMat maski = pyramidMask[i];
threshold(maski, maski, 254, 255, THRESH_TOZERO);
const UMat depthi = pyramidDepth[i];
UMat gtmin, ltmax, tmpMask;
cv::compare(depthi, Scalar(minDepth), gtmin, CMP_GT);
cv::compare(depthi, Scalar(maxDepth), ltmax, CMP_LT);
cv::bitwise_and(gtmin, ltmax, tmpMask);
cv::bitwise_and(maski, tmpMask, maski);
}
}
static void extendPyrMaskByPyrNormals(const std::vector<UMat>& pyramidNormals, std::vector<UMat>& pyramidMask)
{
if (!pyramidNormals.empty())
{
int nLevels = (int)pyramidNormals.size();
for (int i = 0; i < nLevels; i++)
{
UMat maski = pyramidMask[i];
UMat normali = pyramidNormals[i];
UMat validNormalMask;
finiteMask(normali, validNormalMask);
cv::bitwise_and(maski, validNormalMask, maski);
}
}
}
static void buildPyramidCameraMatrix(const Matx33f& cameraMatrix, int levels, std::vector<Matx33f>& pyramidCameraMatrix)
{
pyramidCameraMatrix.resize(levels);
for (int i = 0; i < levels; i++)
{
Matx33f levelCameraMatrix = (i == 0) ? cameraMatrix : 0.5f * pyramidCameraMatrix[i - 1];
levelCameraMatrix(2, 2) = 1.0;
pyramidCameraMatrix[i] = levelCameraMatrix;
}
}
static void preparePyramidCloud(const std::vector<UMat>& pyramidDepth, const Matx33f& cameraMatrix, std::vector<UMat>& pyramidCloud)
{
int nLevels = (int)pyramidDepth.size();
std::vector<Matx33f> pyramidCameraMatrix;
buildPyramidCameraMatrix(cameraMatrix, nLevels, pyramidCameraMatrix);
pyramidCloud.resize(nLevels, UMat());
for (int i = 0; i < nLevels; i++)
{
UMat cloud;
depthTo3d(pyramidDepth[i], pyramidCameraMatrix[i], cloud, noArray());
pyramidCloud[i] = cloud;
}
}
static void preparePyramidTexturedMask(const std::vector<UMat>& pyramid_dI_dx, const std::vector<UMat>& pyramid_dI_dy,
std::vector<float> minGradMagnitudes, const std::vector<UMat>& pyramidMask, double maxPointsPart,
std::vector<UMat>& pyramidTexturedMask, double sobelScale)
{
int nLevels = (int)pyramid_dI_dx.size();
const float sobelScale2_inv = (float)(1. / (sobelScale * sobelScale));
pyramidTexturedMask.resize(nLevels, UMat());
for (int i = 0; i < nLevels; i++)
{
const float minScaledGradMagnitude2 = minGradMagnitudes[i] * minGradMagnitudes[i] * sobelScale2_inv;
const Mat dIdx = pyramid_dI_dx[i].getMat(ACCESS_READ);
const Mat dIdy = pyramid_dI_dy[i].getMat(ACCESS_READ);
Mat texturedMask(dIdx.size(), CV_8UC1, Scalar(0));
for (int y = 0; y < dIdx.rows; y++)
{
const short *dIdx_row = dIdx.ptr<short>(y);
const short *dIdy_row = dIdy.ptr<short>(y);
uchar *texturedMask_row = texturedMask.ptr<uchar>(y);
for (int x = 0; x < dIdx.cols; x++)
{
float magnitude2 = static_cast<float>(dIdx_row[x] * dIdx_row[x] + dIdy_row[x] * dIdy_row[x]);
if (magnitude2 >= minScaledGradMagnitude2)
texturedMask_row[x] = 255;
}
}
Mat texMask = texturedMask & pyramidMask[i].getMat(ACCESS_READ);
randomSubsetOfMask(texMask, (float)maxPointsPart);
texMask.copyTo(pyramidTexturedMask[i]);
}
}
static void preparePyramidNormals(const UMat &normals, const std::vector<UMat> &pyramidDepth, std::vector<UMat> &pyramidNormals)
{
int nLevels = (int)pyramidDepth.size();
buildPyramid(normals, pyramidNormals, nLevels - 1);
// renormalize normals
for (int i = 1; i < nLevels; i++)
{
Mat currNormals = pyramidNormals[i].getMat(ACCESS_RW);
CV_Assert(currNormals.type() == CV_32FC4);
for (int y = 0; y < currNormals.rows; y++)
{
Vec4f *normals_row = currNormals.ptr<Vec4f>(y);
for (int x = 0; x < currNormals.cols; x++)
{
Vec4f n4 = normals_row[x];
Point3f n(n4[0], n4[1], n4[2]);
double nrm = norm(n);
n *= 1.f / nrm;
normals_row[x] = Vec4f(n.x, n.y, n.z, 0);
}
}
}
}
static void preparePyramidNormalsMask(const std::vector<UMat> &pyramidNormals, const std::vector<UMat> &pyramidMask, double maxPointsPart,
std::vector<UMat> &pyramidNormalsMask)
{
int nLevels = (int)pyramidNormals.size();
pyramidNormalsMask.resize(nLevels, UMat());
for (int i = 0; i < nLevels; i++)
{
Mat pyrMask = pyramidMask[i].getMat(ACCESS_READ);
const Mat normals = pyramidNormals[i].getMat(ACCESS_READ);
Mat_<uchar> normalsMask(pyrMask.size(), (uchar)255);
for (int y = 0; y < normalsMask.rows; y++)
{
const Vec4f *normals_row = normals.ptr<Vec4f>(y);
uchar *normalsMask_row = normalsMask.ptr<uchar>(y);
for (int x = 0; x < normalsMask.cols; x++)
{
Vec4f n = normals_row[x];
if (!(std::isfinite(n[0]) && std::isfinite(n[1]) && std::isfinite(n[2])))
{
normalsMask_row[x] = 0;
}
}
}
cv::bitwise_and(pyrMask, normalsMask, normalsMask);
randomSubsetOfMask(normalsMask, (float)maxPointsPart);
normalsMask.copyTo(pyramidNormalsMask[i]);
}
}
static void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings)
{
UMat grayImage;
frame.getGrayImage(grayImage);
if (grayImage.empty())
{
UMat image;
frame.getImage(image);
CV_Assert(!image.empty() && image.depth() == CV_8U);
int ch = image.channels();
if (ch == 3 || ch == 4)
cvtColor(image, grayImage, ch == 3 ? COLOR_BGR2GRAY : COLOR_BGRA2GRAY, 1);
else if (ch == 1)
grayImage = image;
else
CV_Error(Error::StsBadArg, "Image should have 3 or 4 channels (RGB) or 1 channel (grayscale)");
grayImage.convertTo(grayImage, CV_8U);
frame.impl->imageGray = grayImage;
}
//TODO: don't use scaled when scale bug is fixed
UMat scaledDepth;
frame.getProcessedDepth(scaledDepth);
if (scaledDepth.empty())
{
scaledDepth = prepareScaledDepth(frame);
CV_Assert(scaledDepth.size() == grayImage.size());
}
UMat depthMask;
// ignore small, negative, Inf, NaN values
cv::compare(scaledDepth, Scalar(FLT_EPSILON), depthMask, CMP_GT);
UMat mask;
frame.getMask(mask);
if (mask.empty())
{
frame.impl->mask = depthMask;
}
else
{
CV_Assert(mask.type() == CV_8UC1 || mask.type() == CV_8SC1 || mask.type() == CV_BoolC1);
CV_Assert(mask.size() == grayImage.size());
cv::bitwise_and(mask, depthMask, frame.impl->mask);
}
frame.getMask(mask);
std::vector<int> iterCounts;
settings.getIterCounts(iterCounts);
int maxLevel = (int)iterCounts.size() - 1;
std::vector<UMat>& ipyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_IMAGE];
if (ipyramids.empty())
buildPyramid(grayImage, ipyramids, maxLevel);
std::vector<UMat>& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH];
if (dpyramids.empty())
preparePyramidDepth(scaledDepth, dpyramids, maxLevel);
std::vector<UMat>& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK];
if (mpyramids.empty())
preparePyramidMask(mask, dpyramids, maxLevel + 1, settings.getMinDepth(), settings.getMaxDepth(), mpyramids);
}
static void prepareRGBFrameSrc(OdometryFrame& frame, OdometrySettings settings)
{
Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix);
std::vector<UMat>& cpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_CLOUD];
const std::vector<UMat>& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH];
preparePyramidCloud(dpyramids, cameraMatrix, cpyramids);
}
static void prepareRGBFrameDst(OdometryFrame& frame, OdometrySettings settings)
{
const std::vector<UMat>& ipyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_IMAGE];
const std::vector<UMat>& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK];
std::vector<UMat>& dxpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DIX];
std::vector<UMat>& dypyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DIY];
std::vector<float> minGradientMagnitudes;
settings.getMinGradientMagnitudes(minGradientMagnitudes);
int nLevels = (int)ipyramids.size();
dxpyramids.resize(nLevels, UMat());
dypyramids.resize(nLevels, UMat());
int sobelSize = settings.getSobelSize();
for (int i = 0; i < nLevels; i++)
{
Sobel(ipyramids[i], dxpyramids[i], CV_16S, 1, 0, sobelSize);
Sobel(ipyramids[i], dypyramids[i], CV_16S, 0, 1, sobelSize);
}
std::vector<UMat>& tmpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_TEXMASK];
preparePyramidTexturedMask(dxpyramids, dypyramids, minGradientMagnitudes,
mpyramids, settings.getMaxPointsPart(), tmpyramids, settings.getSobelScale());
}
static void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings)
{
//TODO: don't use scaled when scale bug is fixed
UMat scaledDepth;
frame.getProcessedDepth(scaledDepth);
if (scaledDepth.empty())
{
scaledDepth = prepareScaledDepth(frame);
}
UMat depthMask;
// ignore small, negative, Inf, NaN values
cv::compare(scaledDepth, Scalar(FLT_EPSILON), depthMask, CMP_GT);
UMat mask;
frame.getMask(mask);
if (mask.empty())
{
frame.impl->mask = depthMask;
}
else
{
CV_Assert(mask.type() == CV_8UC1 || mask.type() == CV_8SC1 || mask.type() == CV_BoolC1);
CV_Assert(mask.size() == scaledDepth.size());
cv::bitwise_and(mask, depthMask, frame.impl->mask);
}
frame.getMask(mask);
std::vector<int> iterCounts;
settings.getIterCounts(iterCounts);
int maxLevel = (int)iterCounts.size() - 1;
std::vector<UMat>& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH];
if (dpyramids.empty())
preparePyramidDepth(scaledDepth, dpyramids, maxLevel);
Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix);
std::vector<UMat>& cpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_CLOUD];
if (cpyramids.empty())
preparePyramidCloud(dpyramids, cameraMatrix, cpyramids);
}
static void prepareICPFrameSrc(OdometryFrame& frame, OdometrySettings settings)
{
UMat mask;
frame.getMask(mask);
const std::vector<UMat>& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH];
std::vector<int> iterCounts;
settings.getIterCounts(iterCounts);
std::vector<UMat>& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK];
if (mpyramids.empty())
preparePyramidMask(mask, dpyramids, (int)iterCounts.size(), settings.getMinDepth(), settings.getMaxDepth(), mpyramids);
}
static void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings, Ptr<RgbdNormals>& normalsComputer)
{
Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix);
UMat scaledDepth, mask, normals;
frame.getProcessedDepth(scaledDepth);
frame.getMask(mask);
frame.getNormals(normals);
if (normals.empty())
{
Matx33f K;
if (!normalsComputer.empty())
normalsComputer->getK(K);
if (normalsComputer.empty() ||
normalsComputer->getRows() != scaledDepth.rows ||
normalsComputer->getCols() != scaledDepth.cols ||
norm(K, cameraMatrix) > FLT_EPSILON)
{
int normalWinSize = settings.getNormalWinSize();
float diffThreshold = settings.getNormalDiffThreshold();
RgbdNormals::RgbdNormalsMethod normalMethod = settings.getNormalMethod();
normalsComputer = RgbdNormals::create(scaledDepth.rows,
scaledDepth.cols,
scaledDepth.depth(),
cameraMatrix,
normalWinSize,
diffThreshold,
normalMethod);
}
const UMat& c0 = frame.impl->pyramids[OdometryFramePyramidType::PYR_CLOUD][0];
normalsComputer->apply(c0, normals);
frame.impl->normals = normals;
}
CV_Assert(normals.type() == CV_32FC4);
const std::vector<UMat>& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH];
std::vector<UMat>& npyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_NORM];
if (npyramids.empty())
preparePyramidNormals(normals, dpyramids, npyramids);
std::vector<UMat>& mpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_MASK];
if (mpyramids.empty())
{
std::vector<int> iterCounts;
settings.getIterCounts(iterCounts);
preparePyramidMask(mask, dpyramids, (int)iterCounts.size(), settings.getMinDepth(), settings.getMaxDepth(), mpyramids);
extendPyrMaskByPyrNormals(npyramids, mpyramids);
}
std::vector<UMat>& nmpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_NORMMASK];
if (nmpyramids.empty())
preparePyramidNormalsMask(npyramids, mpyramids, settings.getMaxPointsPart(), nmpyramids);
}
void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings)
{
prepareRGBFrameBase(srcFrame, settings);
prepareRGBFrameBase(dstFrame, settings);
prepareRGBFrameSrc(srcFrame, settings);
prepareRGBFrameDst(dstFrame, settings);
}
void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr<RgbdNormals>& normalsComputer, OdometrySettings settings, OdometryAlgoType algtype)
{
prepareICPFrameBase(srcFrame, settings);
prepareICPFrameBase(dstFrame, settings);
prepareICPFrameSrc(srcFrame, settings);
if (algtype == OdometryAlgoType::FAST)
prepareICPFrameDst(srcFrame, settings, normalsComputer);
prepareICPFrameDst(dstFrame, settings, normalsComputer);
}
void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr<RgbdNormals>& normalsComputer, OdometrySettings settings, OdometryAlgoType algtype)
{
prepareRGBFrame(srcFrame, dstFrame, settings);
prepareICPFrame(srcFrame, dstFrame, normalsComputer, settings, algtype);
}
bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
const OdometryFrame& srcFrame,
const OdometryFrame& dstFrame,
const Matx33f& cameraMatrix,
float maxDepthDiff, float angleThreshold, const std::vector<int>& iterCounts,
double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transformType, OdometryAlgoType algtype)
{
if (srcFrame.getPyramidLevels() != (int)(iterCounts.size()))
CV_Error(Error::StsBadArg, "srcFrame has incorrect number of pyramid levels. Did you forget to call prepareFrame()?");
if (dstFrame.getPyramidLevels() != (int)(iterCounts.size()))
CV_Error(Error::StsBadArg, "dstFrame has incorrect number of pyramid levels. Did you forget to call prepareFrame()?");
int transformDim = getTransformDim(transformType);
const int minOverdetermScale = 20;
const int minCorrespsCount = minOverdetermScale * transformDim;
std::vector<Matx33f> pyramidCameraMatrix;
buildPyramidCameraMatrix(cameraMatrix, (int)iterCounts.size(), pyramidCameraMatrix);
Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
Mat currRt, ksi;
Affine3f transform = Affine3f::Identity();
bool isOk = false;
for(int level = (int)iterCounts.size() - 1; level >= 0; level--)
{
const Matx33f& levelCameraMatrix = pyramidCameraMatrix[level];
const Mat srcLevelDepth, dstLevelDepth;
const Mat srcLevelImage, dstLevelImage;
srcFrame.getPyramidAt(srcLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level);
dstFrame.getPyramidAt(dstLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level);
if (method != OdometryType::DEPTH) // RGB(D)
{
srcFrame.getPyramidAt(srcLevelImage, OdometryFramePyramidType::PYR_IMAGE, level);
dstFrame.getPyramidAt(dstLevelImage, OdometryFramePyramidType::PYR_IMAGE, level);
}
const double fx = levelCameraMatrix(0, 0);
const double fy = levelCameraMatrix(1, 1);
const double determinantThreshold = 1e-6;
Mat AtA_rgbd, AtB_rgbd, AtA_icp, AtB_icp;
// Run transformation search on current level iteratively.
for(int iter = 0; iter < iterCounts[level]; iter ++)
{
Mat resultRt_inv = resultRt.inv(DECOMP_SVD);
Mat corresps_rgbd, corresps_icp, diffs_rgbd;
Mat dummy;
double sigma_rgbd = 0, dummyFloat = 0;
const Mat pyramidMask;
srcFrame.getPyramidAt(pyramidMask, OdometryFramePyramidType::PYR_MASK, level);
if(method != OdometryType::DEPTH) // RGB(D)
{
const Mat pyramidTexturedMask;
dstFrame.getPyramidAt(pyramidTexturedMask, OdometryFramePyramidType::PYR_TEXMASK, level);
computeCorresps(levelCameraMatrix, resultRt,
srcLevelImage, srcLevelDepth, pyramidMask,
dstLevelImage, dstLevelDepth, pyramidTexturedMask, maxDepthDiff,
corresps_rgbd, diffs_rgbd, sigma_rgbd, OdometryType::RGB);
}
if(method != OdometryType::RGB) // ICP, RGBD
{
if (algtype == OdometryAlgoType::COMMON)
{
const Mat pyramidNormalsMask;
dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level);
computeCorresps(levelCameraMatrix, resultRt,
Mat(), srcLevelDepth, pyramidMask,
Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff,
corresps_icp, dummy, dummyFloat, OdometryType::DEPTH);
}
}
if(corresps_rgbd.rows < minCorrespsCount && corresps_icp.rows < minCorrespsCount && algtype != OdometryAlgoType::FAST)
break;
const UMat srcPyrCloud;
srcFrame.getPyramidAt(srcPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level);
Mat AtA(transformDim, transformDim, CV_64FC1, Scalar(0)), AtB(transformDim, 1, CV_64FC1, Scalar(0));
if(corresps_rgbd.rows >= minCorrespsCount)
{
const Mat srcPyrImage, dstPyrImage, dstPyrIdx, dstPyrIdy;
srcFrame.getPyramidAt(srcPyrImage, OdometryFramePyramidType::PYR_IMAGE, level);
dstFrame.getPyramidAt(dstPyrImage, OdometryFramePyramidType::PYR_IMAGE, level);
dstFrame.getPyramidAt(dstPyrIdx, OdometryFramePyramidType::PYR_DIX, level);
dstFrame.getPyramidAt(dstPyrIdy, OdometryFramePyramidType::PYR_DIY, level);
calcRgbdLsmMatrices(srcPyrCloud.getMat(ACCESS_READ), resultRt, dstPyrIdx, dstPyrIdy,
corresps_rgbd, diffs_rgbd, sigma_rgbd, fx, fy, sobelScale,
AtA_rgbd, AtB_rgbd, transformType);
AtA += AtA_rgbd;
AtB += AtB_rgbd;
}
if(corresps_icp.rows >= minCorrespsCount || algtype == OdometryAlgoType::FAST)
{
if (algtype == OdometryAlgoType::COMMON)
{
const Mat dstPyrCloud, dstPyrNormals;
dstFrame.getPyramidAt(dstPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level);
dstFrame.getPyramidAt(dstPyrNormals, OdometryFramePyramidType::PYR_NORM, level);
calcICPLsmMatrices(srcPyrCloud.getMat(ACCESS_READ), resultRt, dstPyrCloud, dstPyrNormals,
corresps_icp, AtA_icp, AtB_icp, transformType);
}
else
{
const UMat dstPyrCloud, dstPyrNormals, srcPyrNormals;
dstFrame.getPyramidAt(dstPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level);
dstFrame.getPyramidAt(dstPyrNormals, OdometryFramePyramidType::PYR_NORM, level);
srcFrame.getPyramidAt(srcPyrNormals, OdometryFramePyramidType::PYR_NORM, level);
cv::Matx66f A;
cv::Vec6f b;
calcICPLsmMatricesFast(cameraMatrix, dstPyrCloud, dstPyrNormals, srcPyrCloud, srcPyrNormals, transform, level, maxDepthDiff, angleThreshold, A, b);
AtA_icp = Mat(A);
AtB_icp = Mat(b);
}
AtA += AtA_icp;
AtB += AtB_icp;
}
bool solutionExist = solveSystem(AtA, AtB, determinantThreshold, ksi);
if (!solutionExist)
{
break;
}
Mat tmp61(6, 1, CV_64FC1, Scalar(0));
if(transformType == OdometryTransformType::ROTATION)
{
ksi.copyTo(tmp61.rowRange(0,3));
ksi = tmp61;
}
else if(transformType == OdometryTransformType::TRANSLATION)
{
ksi.copyTo(tmp61.rowRange(3,6));
ksi = tmp61;
}
computeProjectiveMatrix(ksi, currRt);
resultRt = currRt * resultRt;
//TODO: fixit, transform is used for Fast ICP only
Vec6f x(ksi);
Affine3f tinc(Vec3f(x.val), Vec3f(x.val + 3));
transform = tinc * transform;
isOk = true;
}
}
_Rt.create(resultRt.size(), resultRt.type());
Mat Rt = _Rt.getMat();
resultRt.copyTo(Rt);
if(isOk)
{
Mat deltaRt;
if(initRt.empty())
deltaRt = resultRt;
else
deltaRt = resultRt * initRt.inv(DECOMP_SVD);
isOk = testDeltaTransformation(deltaRt, maxTranslation, maxRotation);
}
return isOk;
}
// Rotate dst by RtInv to get corresponding src pixels
// In RGB case compute sigma and diffs too
void computeCorresps(const Matx33f& _K, const Mat& rt,
const Mat& imageSrc, const Mat& depthSrc, const Mat& validMaskSrc,
const Mat& imageDst, const Mat& depthDst, const Mat& selectMaskDst, float maxDepthDiff,
Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method)
{
Mat mrtInv = rt.inv(DECOMP_SVD);
Matx44d rtInv = mrtInv;
Mat corresps(depthDst.size(), CV_16SC2, Scalar::all(-1));
Mat diffs;
if (method == OdometryType::RGB)
diffs = Mat(depthDst.size(), CV_32F, Scalar::all(-1));
// src_2d = K * src_3d, src_3d = K_inv * [src_2d | z]
//
Matx33d K(_K);
Matx33d K_inv = K.inv(DECOMP_SVD);
Rect r(0, 0, depthDst.cols, depthDst.rows);
Matx31d tinv = rtInv.get_minor<3, 1>(0, 3);
Matx31d ktinvm = K * tinv;
//const double* Kt_ptr = Kt.ptr<const double>();
Point3d ktinv(ktinvm(0, 0), ktinvm(1, 0), ktinvm(2, 0));
AutoBuffer<float> buf(3 * (depthDst.cols + depthDst.rows));
float* KRK_inv0_u1 = buf.data();
float* KRK_inv1_v1_plus_KRK_inv2 = buf.data() + depthDst.cols;
float* KRK_inv3_u1 = buf.data() + depthDst.cols + depthDst.rows;
float* KRK_inv4_v1_plus_KRK_inv5 = buf.data() + depthDst.cols * 2 + depthDst.rows;
float* KRK_inv6_u1 = buf.data() + depthDst.cols * 2 + depthDst.rows * 2;
float* KRK_inv7_v1_plus_KRK_inv8 = buf.data() + depthDst.cols * 3 + depthDst.rows * 2;
{
Matx33d rinv = rtInv.get_minor<3, 3>(0, 0);
Matx33d kriki = K * rinv * K_inv;
for (int udst = 0; udst < depthDst.cols; udst++)
{
KRK_inv0_u1[udst] = (float)(kriki(0, 0) * udst);
KRK_inv3_u1[udst] = (float)(kriki(1, 0) * udst);
KRK_inv6_u1[udst] = (float)(kriki(2, 0) * udst);
}
for (int vdst = 0; vdst < depthDst.rows; vdst++)
{
KRK_inv1_v1_plus_KRK_inv2[vdst] = (float)(kriki(0, 1) * vdst + kriki(0, 2));
KRK_inv4_v1_plus_KRK_inv5[vdst] = (float)(kriki(1, 1) * vdst + kriki(1, 2));
KRK_inv7_v1_plus_KRK_inv8[vdst] = (float)(kriki(2, 1) * vdst + kriki(2, 2));
}
}
double sigma = 0;
int correspCount = 0;
for (int vdst = 0; vdst < depthDst.rows; vdst++)
{
const float* depthDst_row = depthDst.ptr<float>(vdst);
const uchar* maskDst_row = selectMaskDst.ptr<uchar>(vdst);
for (int udst = 0; udst < depthDst.cols; udst++)
{
float ddst = depthDst_row[udst];
if (maskDst_row[udst])
{
float transformed_ddst = static_cast<float>(ddst * (KRK_inv6_u1[udst] + KRK_inv7_v1_plus_KRK_inv8[vdst]) + ktinv.z);
if (transformed_ddst > 0)
{
float transformed_ddst_inv = 1.f / transformed_ddst;
int usrc = cvRound(transformed_ddst_inv * (ddst * (KRK_inv0_u1[udst] + KRK_inv1_v1_plus_KRK_inv2[vdst]) + ktinv.x));
int vsrc = cvRound(transformed_ddst_inv * (ddst * (KRK_inv3_u1[udst] + KRK_inv4_v1_plus_KRK_inv5[vdst]) + ktinv.y));
if (r.contains(Point(usrc, vsrc)))
{
float dsrc = depthSrc.at<float>(vsrc, usrc);
if (validMaskSrc.at<uchar>(vsrc, usrc) && std::abs(transformed_ddst - dsrc) <= maxDepthDiff)
{
CV_DbgAssert(!cvIsNaN(dsrc));
Vec2s& c = corresps.at<Vec2s>(vsrc, usrc);
float diff = 0;
if (c[0] != -1)
{
diff = 0;
int exist_u1 = c[0], exist_v1 = c[1];
float exist_d1 = (float)(depthDst.at<float>(exist_v1, exist_u1) *
(KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + ktinv.z);
if (transformed_ddst > exist_d1)
continue;
if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(vsrc, usrc)) -
static_cast<int>(imageDst.at<uchar>(vdst, udst)));
}
else
{
if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(vsrc, usrc)) -
static_cast<int>(imageDst.at<uchar>(vdst, udst)));
correspCount++;
}
c = Vec2s((short)udst, (short)vdst);
if (method == OdometryType::RGB)
{
diffs.at<float>(vsrc, usrc) = diff;
sigma += diff * diff;
}
}
}
}
}
}
}
_sigma = std::sqrt(sigma / double(correspCount));
_corresps.create(correspCount, 1, CV_32SC4);
Vec4i* corresps_ptr = _corresps.ptr<Vec4i>();
float* diffs_ptr = nullptr;
if (method == OdometryType::RGB)
{
_diffs.create(correspCount, 1, CV_32F);
diffs_ptr = _diffs.ptr<float>();
}
for (int vsrc = 0, i = 0; vsrc < corresps.rows; vsrc++)
{
const Vec2s* corresps_row = corresps.ptr<Vec2s>(vsrc);
const float* diffs_row = nullptr;
if (method == OdometryType::RGB)
diffs_row = diffs.ptr<float>(vsrc);
for (int usrc = 0; usrc < corresps.cols; usrc++)
{
const Vec2s& c = corresps_row[usrc];
const float& d = diffs_row[usrc];
if (c[0] != -1)
{
corresps_ptr[i] = Vec4i(usrc, vsrc, c[0], c[1]);
if (method == OdometryType::RGB)
diffs_ptr[i] = d;
i++;
}
}
}
}
void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& dI_dx1, const Mat& dI_dy1,
const Mat& corresps, const Mat& _diffs, const double _sigma,
double fx, double fy, double sobelScaleIn,
Mat& AtA, Mat& AtB, OdometryTransformType transformType)
{
int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
double* AtB_ptr = AtB.ptr<double>();
CV_Assert(Rt.type() == CV_64FC1);
Affine3d rtmat(Rt);
const float* diffs_ptr = _diffs.ptr<float>();
const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
double sigma = _sigma;
std::vector<double> A_buf(transformDim);
double* A_ptr = &A_buf[0];
for (int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
{
const Vec4i& c = corresps_ptr[correspIndex];
int u0 = c[0], v0 = c[1];
int u1 = c[2], v1 = c[3];
double w = sigma + std::abs(diffs_ptr[correspIndex]);
w = w > DBL_EPSILON ? 1. / w : 1.;
double w_sobelScale = w * sobelScaleIn;
const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
Point3d tp0 = rtmat * Point3d(p0[0], p0[1], p0[2]);
rgbdCoeffsFunc(transformType,
A_ptr,
w_sobelScale * dI_dx1.at<short int>(v1, u1),
w_sobelScale * dI_dy1.at<short int>(v1, u1),
tp0, fx, fy);
for (int y = 0; y < transformDim; y++)
{
double* AtA_ptr = AtA.ptr<double>(y);
for (int x = y; x < transformDim; x++)
{
AtA_ptr[x] += A_ptr[y] * A_ptr[x];
}
AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
}
}
for (int y = 0; y < transformDim; y++)
for (int x = y + 1; x < transformDim; x++)
AtA.at<double>(x, y) = AtA.at<double>(y, x);
}
void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& cloud1, const Mat& normals1,
const Mat& corresps,
Mat& AtA, Mat& AtB, OdometryTransformType transformType)
{
int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
double* AtB_ptr = AtB.ptr<double>();
const int correspsCount = corresps.rows;
CV_Assert(Rt.type() == CV_64FC1);
const double* Rt_ptr = Rt.ptr<const double>();
AutoBuffer<float> diffs(correspsCount);
float* diffs_ptr = diffs.data();
AutoBuffer<Point3f> transformedPoints0(correspsCount);
Point3f* tps0_ptr = transformedPoints0.data();
const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
double sigma = 0;
for (int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
{
const Vec4i& c = corresps_ptr[correspIndex];
int u0 = c[0], v0 = c[1];
int u1 = c[2], v1 = c[3];
const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
Point3f tp0;
tp0.x = (float)(p0[0] * Rt_ptr[0] + p0[1] * Rt_ptr[1] + p0[2] * Rt_ptr[2] + Rt_ptr[3]);
tp0.y = (float)(p0[0] * Rt_ptr[4] + p0[1] * Rt_ptr[5] + p0[2] * Rt_ptr[6] + Rt_ptr[7]);
tp0.z = (float)(p0[0] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]);
Vec4f n1 = normals1.at<Vec4f>(v1, u1);
Vec4f _v = cloud1.at<Vec4f>(v1, u1);
Point3f v = Point3f(_v[0], _v[1], _v[2]) - tp0;
tps0_ptr[correspIndex] = tp0;
diffs_ptr[correspIndex] = n1[0] * v.x + n1[1] * v.y + n1[2] * v.z;
sigma += diffs_ptr[correspIndex] * diffs_ptr[correspIndex];
}
sigma = std::sqrt(sigma / correspsCount);
std::vector<double> A_buf(transformDim);
double* A_ptr = &A_buf[0];
for (int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
{
const Vec4i& c = corresps_ptr[correspIndex];
int u1 = c[2], v1 = c[3];
double w = sigma +std::abs(diffs_ptr[correspIndex]);
w = w > DBL_EPSILON ? 1. / w : 1.;
Vec4f n4 = normals1.at<Vec4f>(v1, u1);
Vec4f p1 = cloud1.at<Vec4f>(v1, u1);
icpCoeffsFunc(transformType,
A_ptr, tps0_ptr[correspIndex], Point3d(p1[0], p1[1], p1[2]), Vec3d(n4[0], n4[1], n4[2]) * w);
for (int y = 0; y < transformDim; y++)
{
double* AtA_ptr = AtA.ptr<double>(y);
for (int x = y; x < transformDim; x++)
{
AtA_ptr[x] += A_ptr[y] * A_ptr[x];
}
AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
}
}
for (int y = 0; y < transformDim; y++)
for (int x = y + 1; x < transformDim; x++)
AtA.at<double>(x, y) = AtA.at<double>(y, x);
}
void computeProjectiveMatrix(const Mat& ksi, Mat& Rt)
{
CV_Assert(ksi.size() == Size(1, 6) && ksi.type() == CV_64FC1);
const double* ksi_ptr = ksi.ptr<const double>();
// 0.5 multiplication is here because (dual) quaternions keep half an angle/twist inside
Matx44d matdq = (DualQuatd(0, ksi_ptr[0], ksi_ptr[1], ksi_ptr[2],
0, ksi_ptr[3], ksi_ptr[4], ksi_ptr[5]) * 0.5).exp().toMat(QUAT_ASSUME_UNIT);
Mat(matdq).copyTo(Rt);
}
bool solveSystem(const Mat& AtA, const Mat& AtB, double detThreshold, Mat& x)
{
double det = determinant(AtA);
if (fabs(det) < detThreshold || cvIsNaN(det) || cvIsInf(det))
return false;
solve(AtA, AtB, x, DECOMP_CHOLESKY);
return true;
}
bool testDeltaTransformation(const Mat& deltaRt, double maxTranslation, double maxRotation)
{
double translation = norm(deltaRt(Rect(3, 0, 1, 3)));
Mat rvec;
Rodrigues(deltaRt(Rect(0, 0, 3, 3)), rvec);
double rotation = norm(rvec) * 180. / CV_PI;
return translation <= maxTranslation && rotation <= maxRotation;
}
// Upper triangle buffer size (for symmetric matrix build)
const size_t UTSIZE = 27;
#if USE_INTRINSICS
static inline bool fastCheck(const v_float32x4& p0, const v_float32x4& p1)
{
float check = v_reduce_sum(p0) + v_reduce_sum(p1);
return !cvIsNaN(check);
}
static inline void getCrossPerm(const v_float32x4& a, v_float32x4& yzx, v_float32x4& zxy)
{
v_uint32x4 aa = v_reinterpret_as_u32(a);
v_uint32x4 yz00 = v_extract<1>(aa, v_setzero_u32());
v_uint32x4 x0y0, tmp;
v_zip(aa, v_setzero_u32(), x0y0, tmp);
v_uint32x4 yzx0 = v_combine_low(yz00, x0y0);
v_uint32x4 y000 = v_extract<2>(x0y0, v_setzero_u32());
v_uint32x4 zx00 = v_extract<1>(yzx0, v_setzero_u32());
zxy = v_reinterpret_as_f32(v_combine_low(zx00, y000));
yzx = v_reinterpret_as_f32(yzx0);
}
static inline v_float32x4 crossProduct(const v_float32x4& a, const v_float32x4& b)
{
v_float32x4 ayzx, azxy, byzx, bzxy;
getCrossPerm(a, ayzx, azxy);
getCrossPerm(b, byzx, bzxy);
return v_sub(v_mul(ayzx, bzxy), v_mul(azxy, byzx));
}
#else
static inline bool fastCheck(const Point3f& p)
{
return !cvIsNaN(p.x);
}
#endif
typedef Matx<float, 6, 7> ABtype;
struct GetAbInvoker : ParallelLoopBody
{
GetAbInvoker(ABtype& _globalAb, Mutex& _mtx,
const Points& _oldPts, const Normals& _oldNrm, const Points& _newPts, const Normals& _newNrm,
Affine3f _pose, Intr::Projector _proj, float _sqDistanceThresh, float _minCos) :
ParallelLoopBody(),
globalSumAb(_globalAb), mtx(_mtx),
oldPts(_oldPts), oldNrm(_oldNrm), newPts(_newPts), newNrm(_newNrm), pose(_pose),
proj(_proj), sqDistanceThresh(_sqDistanceThresh), minCos(_minCos)
{ }
virtual void operator ()(const Range& range) const override
{
#if USE_INTRINSICS
CV_Assert(ptype::channels == 4);
const size_t utBufferSize = 9;
float CV_DECL_ALIGNED(16) upperTriangle[utBufferSize * 4];
for (size_t i = 0; i < utBufferSize * 4; i++)
upperTriangle[i] = 0;
// how values are kept in upperTriangle
const int NA = 0;
const size_t utPos[] =
{
0, 1, 2, 4, 5, 6, 3,
NA, 9, 10, 12, 13, 14, 11,
NA, NA, 18, 20, 21, 22, 19,
NA, NA, NA, 24, 28, 30, 32,
NA, NA, NA, NA, 25, 29, 33,
NA, NA, NA, NA, NA, 26, 34
};
const float(&pm)[16] = pose.matrix.val;
v_float32x4 poseRot0(pm[0], pm[4], pm[8], 0);
v_float32x4 poseRot1(pm[1], pm[5], pm[9], 0);
v_float32x4 poseRot2(pm[2], pm[6], pm[10], 0);
v_float32x4 poseTrans(pm[3], pm[7], pm[11], 0);
v_float32x4 vfxy(proj.fx, proj.fy, 0, 0), vcxy(proj.cx, proj.cy, 0, 0);
v_float32x4 vframe((float)(oldPts.cols - 1), (float)(oldPts.rows - 1), 1.f, 1.f);
float sqThresh = sqDistanceThresh;
float cosThresh = minCos;
for (int y = range.start; y < range.end; y++)
{
const CV_DECL_ALIGNED(16) float* newPtsRow = (const float*)newPts[y];
const CV_DECL_ALIGNED(16) float* newNrmRow = (const float*)newNrm[y];
for (int x = 0; x < newPts.cols; x++)
{
v_float32x4 newP = v_load_aligned(newPtsRow + x * 4);
v_float32x4 newN = v_load_aligned(newNrmRow + x * 4);
if (!fastCheck(newP, newN))
continue;
//transform to old coord system
newP = v_matmuladd(newP, poseRot0, poseRot1, poseRot2, poseTrans);
newN = v_matmuladd(newN, poseRot0, poseRot1, poseRot2, v_setzero_f32());
//find correspondence by projecting the point
v_float32x4 oldCoords;
float pz = v_get0(v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(newP))));
// x, y, 0, 0
oldCoords = v_muladd(v_div(newP, v_setall_f32(pz)), vfxy, vcxy);
if (!v_check_all(v_and(v_ge(oldCoords, v_setzero_f32()), v_lt(oldCoords, vframe))))
continue;
// bilinearly interpolate oldPts and oldNrm under oldCoords point
v_float32x4 oldP;
v_float32x4 oldN;
{
v_int32x4 ixy = v_floor(oldCoords);
v_float32x4 txy = v_sub(oldCoords, v_cvt_f32(ixy));
int xi = v_get0(ixy);
int yi = v_get0(v_rotate_right<1>(ixy));
v_float32x4 tx = v_setall_f32(v_get0(txy));
txy = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(txy)));
v_float32x4 ty = v_setall_f32(v_get0(txy));
const float* prow0 = (const float*)oldPts[yi + 0];
const float* prow1 = (const float*)oldPts[yi + 1];
v_float32x4 p00 = v_load(prow0 + (xi + 0) * 4);
v_float32x4 p01 = v_load(prow0 + (xi + 1) * 4);
v_float32x4 p10 = v_load(prow1 + (xi + 0) * 4);
v_float32x4 p11 = v_load(prow1 + (xi + 1) * 4);
// do not fix missing data
// NaN check is done later
const float* nrow0 = (const float*)oldNrm[yi + 0];
const float* nrow1 = (const float*)oldNrm[yi + 1];
v_float32x4 n00 = v_load(nrow0 + (xi + 0) * 4);
v_float32x4 n01 = v_load(nrow0 + (xi + 1) * 4);
v_float32x4 n10 = v_load(nrow1 + (xi + 0) * 4);
v_float32x4 n11 = v_load(nrow1 + (xi + 1) * 4);
// NaN check is done later
v_float32x4 p0 = v_add(p00, v_mul(tx, v_sub(p01, p00)));
v_float32x4 p1 = v_add(p10, v_mul(tx, v_sub(p11, p10)));
oldP = v_add(p0, v_mul(ty, v_sub(p1, p0)));
v_float32x4 n0 = v_add(n00, v_mul(tx, v_sub(n01, n00)));
v_float32x4 n1 = v_add(n10, v_mul(tx, v_sub(n11, n10)));
oldN = v_add(n0, v_mul(ty, v_sub(n1, n0)));
}
bool oldPNcheck = fastCheck(oldP, oldN);
//filter by distance
v_float32x4 diff = v_sub(newP, oldP);
bool distCheck = !(v_reduce_sum(v_mul(diff, diff)) > sqThresh);
//filter by angle
bool angleCheck = !(abs(v_reduce_sum(v_mul(newN, oldN))) < cosThresh);
if (!(oldPNcheck && distCheck && angleCheck))
continue;
// build point-wise vector ab = [ A | b ]
v_float32x4 VxNv = crossProduct(newP, oldN);
Point3f VxN;
VxN.x = v_get0(VxNv);
VxN.y = v_get0(v_reinterpret_as_f32(v_extract<1>(v_reinterpret_as_u32(VxNv), v_setzero_u32())));
VxN.z = v_get0(v_reinterpret_as_f32(v_extract<2>(v_reinterpret_as_u32(VxNv), v_setzero_u32())));
float dotp = -v_reduce_sum(v_mul(oldN, diff));
// build point-wise upper-triangle matrix [ab^T * ab] w/o last row
// which is [A^T*A | A^T*b]
// and gather sum
v_float32x4 vd = v_or(VxNv, v_float32x4(0, 0, 0, dotp));
v_float32x4 n = oldN;
v_float32x4 nyzx;
{
v_uint32x4 aa = v_reinterpret_as_u32(n);
v_uint32x4 yz00 = v_extract<1>(aa, v_setzero_u32());
v_uint32x4 x0y0, tmp;
v_zip(aa, v_setzero_u32(), x0y0, tmp);
nyzx = v_reinterpret_as_f32(v_combine_low(yz00, x0y0));
}
v_float32x4 vutg[utBufferSize];
for (size_t i = 0; i < utBufferSize; i++)
vutg[i] = v_load_aligned(upperTriangle + i * 4);
int p = 0;
v_float32x4 v;
// vx * vd, vx * n
v = v_setall_f32(VxN.x);
v_store_aligned(upperTriangle + p * 4, v_muladd(v, vd, vutg[p])); p++;
v_store_aligned(upperTriangle + p * 4, v_muladd(v, n, vutg[p])); p++;
// vy * vd, vy * n
v = v_setall_f32(VxN.y);
v_store_aligned(upperTriangle + p * 4, v_muladd(v, vd, vutg[p])); p++;
v_store_aligned(upperTriangle + p * 4, v_muladd(v, n, vutg[p])); p++;
// vz * vd, vz * n
v = v_setall_f32(VxN.z);
v_store_aligned(upperTriangle + p * 4, v_muladd(v, vd, vutg[p])); p++;
v_store_aligned(upperTriangle + p * 4, v_muladd(v, n, vutg[p])); p++;
// nx^2, ny^2, nz^2
v_store_aligned(upperTriangle + p * 4, v_muladd(n, n, vutg[p])); p++;
// nx*ny, ny*nz, nx*nz
v_store_aligned(upperTriangle + p * 4, v_muladd(n, nyzx, vutg[p])); p++;
// nx*d, ny*d, nz*d
v = v_setall_f32(dotp);
v_store_aligned(upperTriangle + p * 4, v_muladd(n, v, vutg[p])); p++;
}
}
ABtype sumAB = ABtype::zeros();
for (int i = 0; i < 6; i++)
{
for (int j = i; j < 7; j++)
{
size_t p = utPos[i * 7 + j];
sumAB(i, j) = upperTriangle[p];
}
}
CV_UNUSED(UTSIZE);
#else
float upperTriangle[UTSIZE];
for (size_t i = 0; i < UTSIZE; i++)
upperTriangle[i] = 0;
for (int y = range.start; y < range.end; y++)
{
const ptype* newPtsRow = newPts[y];
const ptype* newNrmRow = newNrm[y];
for (int x = 0; x < newPts.cols; x++)
{
Point3f newP = fromPtype(newPtsRow[x]);
Point3f newN = fromPtype(newNrmRow[x]);
Point3f oldP(nan3), oldN(nan3);
if (!(fastCheck(newP) && fastCheck(newN)))
continue;
//transform to old coord system
newP = pose * newP;
newN = pose.rotation() * newN;
//find correspondence by projecting the point
Point2f oldCoords = proj(newP);
if (!(oldCoords.x >= 0 && oldCoords.x < oldPts.cols - 1 &&
oldCoords.y >= 0 && oldCoords.y < oldPts.rows - 1))
continue;
// bilinearly interpolate oldPts and oldNrm under oldCoords point
int xi = cvFloor(oldCoords.x), yi = cvFloor(oldCoords.y);
float tx = oldCoords.x - xi, ty = oldCoords.y - yi;
const ptype* prow0 = oldPts[yi + 0];
const ptype* prow1 = oldPts[yi + 1];
Point3f p00 = fromPtype(prow0[xi + 0]);
Point3f p01 = fromPtype(prow0[xi + 1]);
Point3f p10 = fromPtype(prow1[xi + 0]);
Point3f p11 = fromPtype(prow1[xi + 1]);
//do not fix missing data
if (!(fastCheck(p00) && fastCheck(p01) &&
fastCheck(p10) && fastCheck(p11)))
continue;
const ptype* nrow0 = oldNrm[yi + 0];
const ptype* nrow1 = oldNrm[yi + 1];
Point3f n00 = fromPtype(nrow0[xi + 0]);
Point3f n01 = fromPtype(nrow0[xi + 1]);
Point3f n10 = fromPtype(nrow1[xi + 0]);
Point3f n11 = fromPtype(nrow1[xi + 1]);
if (!(fastCheck(n00) && fastCheck(n01) &&
fastCheck(n10) && fastCheck(n11)))
continue;
Point3f p0 = p00 + tx * (p01 - p00);
Point3f p1 = p10 + tx * (p11 - p10);
oldP = p0 + ty * (p1 - p0);
Point3f n0 = n00 + tx * (n01 - n00);
Point3f n1 = n10 + tx * (n11 - n10);
oldN = n0 + ty * (n1 - n0);
if (!(fastCheck(oldP) && fastCheck(oldN)))
continue;
//filter by distance
Point3f diff = newP - oldP;
if (diff.dot(diff) > sqDistanceThresh)
{
continue;
}
//filter by angle
if (abs(newN.dot(oldN)) < minCos)
{
continue;
}
// build point-wise vector ab = [ A | b ]
//try to optimize
Point3f VxN = newP.cross(oldN);
float ab[7] = { VxN.x, VxN.y, VxN.z, oldN.x, oldN.y, oldN.z, oldN.dot(-diff) };
// build point-wise upper-triangle matrix [ab^T * ab] w/o last row
// which is [A^T*A | A^T*b]
// and gather sum
int pos = 0;
for (int i = 0; i < 6; i++)
{
for (int j = i; j < 7; j++)
{
upperTriangle[pos++] += ab[i] * ab[j];
}
}
}
}
ABtype sumAB = ABtype::zeros();
int pos = 0;
for (int i = 0; i < 6; i++)
{
for (int j = i; j < 7; j++)
{
sumAB(i, j) = upperTriangle[pos++];
}
}
#endif
AutoLock al(mtx);
globalSumAb += sumAB;
}
ABtype& globalSumAb;
Mutex& mtx;
const Points& oldPts;
const Normals& oldNrm;
const Points& newPts;
const Normals& newNrm;
Affine3f pose;
const Intr::Projector proj;
float sqDistanceThresh;
float minCos;
};
void calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm,
cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b)
{
CV_Assert(oldPts.size() == oldNrm.size());
CV_Assert(newPts.size() == newNrm.size());
CV_OCL_RUN(ocl::isOpenCLActivated(),
ocl_calcICPLsmMatricesFast(cameraMatrix,
oldPts, oldNrm, newPts, newNrm,
pose, level, maxDepthDiff, angleThreshold,
A, b)
);
ABtype sumAB = ABtype::zeros();
Mutex mutex;
const Points op(oldPts.getMat(AccessFlag::ACCESS_READ)), np(newPts.getMat(AccessFlag::ACCESS_READ));
const Normals on(oldNrm.getMat(AccessFlag::ACCESS_READ)), nn(newNrm.getMat(AccessFlag::ACCESS_READ));
Intr intrinsics(cameraMatrix);
GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose,
intrinsics.scale(level).makeProjector(),
maxDepthDiff * maxDepthDiff, std::cos(angleThreshold));
Range range(0, newPts.rows);
const int nstripes = -1;
parallel_for_(range, invoker, nstripes);
// splitting AB matrix to A and b
for (int i = 0; i < 6; i++)
{
// augment lower triangle of A by symmetry
for (int j = i; j < 6; j++)
{
A(i, j) = A(j, i) = sumAB(i, j);
}
b(i) = sumAB(i, 6);
}
}
#ifdef HAVE_OPENCL
bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm,
cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b)
{
CV_TRACE_FUNCTION();
Size oldSize = oldPts.size(), newSize = newPts.size();
CV_Assert(oldSize == oldNrm.size());
CV_Assert(newSize == newNrm.size());
// calculate 1x7 vector ab to produce b and upper triangle of A:
// [A|b] = ab*(ab^t)
// and then reduce it across work groups
UMat groupedSumBuffer;
cv::String errorStr;
String name = "getAb";
ocl::ProgramSource source = ocl::ptcloud::icp_oclsrc;
cv::String options = "-cl-mad-enable";
ocl::Kernel k;
k.create(name.c_str(), source, options, &errorStr);
if (k.empty())
throw std::runtime_error("Failed to create kernel: " + errorStr);
size_t globalSize[2];
globalSize[0] = (size_t)newPts.cols;
globalSize[1] = (size_t)newPts.rows;
const ocl::Device& device = ocl::Device::getDefault();
size_t wgsLimit = device.maxWorkGroupSize();
size_t memSize = device.localMemSize();
// local memory should keep upperTriangles for all threads in group for reduce
const size_t ltsz = UTSIZE * sizeof(float);
const size_t lcols = 32;
size_t lrows = min(memSize / ltsz, wgsLimit) / lcols;
// round lrows down to 2^n
lrows = roundDownPow2(lrows);
size_t localSize[2] = { lcols, lrows };
Size ngroups((int)divUp(globalSize[0], (unsigned int)localSize[0]),
(int)divUp(globalSize[1], (unsigned int)localSize[1]));
// size of local buffer for group-wide reduce
size_t lsz = localSize[0] * localSize[1] * ltsz;
Intr intrinsics(cameraMatrix);
Intr::Projector proj = intrinsics.scale(level).makeProjector();
Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy);
UMat& groupedSumGpu = groupedSumBuffer;
groupedSumGpu.create(Size(ngroups.width * UTSIZE, ngroups.height),
CV_32F);
groupedSumGpu.setTo(0);
// TODO: optimization possible:
// samplers instead of oldPts/oldNrm (mask needed)
k.args(ocl::KernelArg::ReadOnlyNoSize(oldPts),
ocl::KernelArg::ReadOnlyNoSize(oldNrm),
oldSize,
ocl::KernelArg::ReadOnlyNoSize(newPts),
ocl::KernelArg::ReadOnlyNoSize(newNrm),
newSize,
ocl::KernelArg::Constant(pose.matrix.val,
sizeof(pose.matrix.val)),
fxy.val, cxy.val,
maxDepthDiff * maxDepthDiff,
std::cos(angleThreshold),
ocl::KernelArg::Local(lsz),
ocl::KernelArg::WriteOnlyNoSize(groupedSumGpu)
);
if (!k.run(2, globalSize, localSize, true))
throw std::runtime_error("Failed to run kernel");
float upperTriangle[UTSIZE];
for (size_t i = 0; i < UTSIZE; i++)
upperTriangle[i] = 0;
Mat groupedSumCpu = groupedSumGpu.getMat(ACCESS_READ);
for (int y = 0; y < ngroups.height; y++)
{
const float* rowr = groupedSumCpu.ptr<float>(y);
for (size_t x = 0; x < size_t(ngroups.width); x++)
{
const float* p = rowr + x * UTSIZE;
for (size_t j = 0; j < UTSIZE; j++)
{
upperTriangle[j] += p[j];
}
}
}
groupedSumCpu.release();
ABtype sumAB = ABtype::zeros();
int pos = 0;
for (int i = 0; i < 6; i++)
{
for (int j = i; j < 7; j++)
{
sumAB(i, j) = upperTriangle[pos++];
}
}
// splitting AB matrix to A and b
for (int i = 0; i < 6; i++)
{
// augment lower triangle of A by symmetry
for (int j = i; j < 6; j++)
{
A(i, j) = A(j, i) = sumAB(i, j);
}
b(i) = sumAB(i, 6);
}
return true;
}
#endif
}