Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

1. removed (almost all) additional scale parameter mentions

Browse Source 
2. refactored funcptrs to switch/cases & more
pull/22150/head
Rostislav Vasilikhin 3 years ago
parent c12d4c82df
commit 770c0d1416
5 changed files with 174 additions and 297 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      modules/3d/include/opencv2/3d/odometry.hpp
  2. 62
      modules/3d/src/rgbd/odometry.cpp
  3. 214
      modules/3d/src/rgbd/odometry_functions.cpp
  4. 146
      modules/3d/src/rgbd/odometry_functions.hpp
  5. 29
      modules/3d/test/test_odometry.cpp

14
modules/3d/include/opencv2/3d/odometry.hpp
Unescape View File

@ -73,21 +73,7 @@ public:
*/
bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt);
/** Compute Rigid Transformation and scale between two frames so that Rt * (src * scale) = dst
* Works only on OdometryType::DEPTH and OdometryAlgoType::COMMON
* @param srcFrame src frame ("original" image)
* @param dstFrame dst frame ("rotated" image)
* @param Rt Rigid transformation, which will be calculated, in form:
* { R_11 R_12 R_13 t_1
* R_21 R_22 R_23 t_2
* R_31 R_32 R_33 t_3
* 0 0 0 1 }
* @param scale scale between srcFrame and dstFrame (use scale = 1 for input)
*/
bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale);
CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const;
CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, OutputArray scale) const;
CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const;
class Impl;

62
modules/3d/src/rgbd/odometry.cpp
Unescape View File

@ -20,8 +20,8 @@ public:
virtual OdometryFrame createOdometryFrame() const = 0;
virtual void prepareFrame(OdometryFrame& frame) = 0;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) = 0;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const = 0;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const = 0;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const = 0;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const = 0;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const = 0;
};
@ -40,8 +40,8 @@ public:
virtual OdometryFrame createOdometryFrame() const override;
virtual void prepareFrame(OdometryFrame& frame) override;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override;
};
@ -75,7 +75,7 @@ void OdometryICP::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame
prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype);
}
bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const
bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
{
Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix);
@ -84,7 +84,7 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
settings.getIterCounts(miterCounts);
for (int i = 0; i < miterCounts.size().height; i++)
iterCounts.push_back(miterCounts.at<int>(i));
bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix,
bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix,
this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
iterCounts, this->settings.getMaxTranslation(),
this->settings.getMaxRotation(), settings.getSobelScale(),
@ -92,7 +92,7 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
return isCorrect;
}
bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt, float& scale) const
bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const
{
OdometryFrame srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = this->createOdometryFrame();
@ -101,7 +101,7 @@ bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArra
prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype);
bool isCorrect = compute(srcFrame, dstFrame, Rt, scale);
bool isCorrect = compute(srcFrame, dstFrame, Rt);
return isCorrect;
}
@ -123,8 +123,8 @@ public:
virtual OdometryFrame createOdometryFrame() const override;
virtual void prepareFrame(OdometryFrame& frame) override;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override;
};
@ -154,7 +154,7 @@ void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame
prepareRGBFrame(srcFrame, dstFrame, this->settings, false);
}
bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const
bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
{
Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix);
@ -164,7 +164,7 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
CV_CheckTypeEQ(miterCounts.type(), CV_32S, "");
for (int i = 0; i < miterCounts.size().height; i++)
iterCounts.push_back(miterCounts.at<int>(i));
bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix,
bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix,
this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
iterCounts, this->settings.getMaxTranslation(),
this->settings.getMaxRotation(), settings.getSobelScale(),
@ -172,7 +172,7 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
return isCorrect;
}
bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt, float& scale) const
bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const
{
OdometryFrame srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = this->createOdometryFrame();
@ -181,13 +181,13 @@ bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArra
prepareRGBFrame(srcFrame, dstFrame, this->settings, false);
bool isCorrect = compute(srcFrame, dstFrame, Rt, scale);
bool isCorrect = compute(srcFrame, dstFrame, Rt);
return isCorrect;
}
bool OdometryRGB::compute(InputArray, InputArray, InputArray, InputArray, OutputArray) const
{
CV_Error(cv::Error::StsBadFunc, "This volume does not work with depth and rgb data simultaneously");
CV_Error(cv::Error::StsBadFunc, "This odometry does not work with depth and rgb data simultaneously");
}
class OdometryRGBD : public Odometry::Impl
@ -203,8 +203,8 @@ public:
virtual OdometryFrame createOdometryFrame() const override;
virtual void prepareFrame(OdometryFrame& frame) override;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override;
virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override;
};
@ -234,7 +234,7 @@ void OdometryRGBD::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFram
prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype);
}
bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const
bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
{
Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix);
@ -243,7 +243,7 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d
settings.getIterCounts(miterCounts);
for (int i = 0; i < miterCounts.size().height; i++)
iterCounts.push_back(miterCounts.at<int>(i));
bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix,
bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix,
this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
iterCounts, this->settings.getMaxTranslation(),
this->settings.getMaxRotation(), settings.getSobelScale(),
@ -251,7 +251,7 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d
return isCorrect;
}
bool OdometryRGBD::compute(InputArray, InputArray, OutputArray, float&) const
bool OdometryRGBD::compute(InputArray, InputArray, OutputArray) const
{
CV_Error(cv::Error::StsBadFunc, "This volume needs depth and rgb data simultaneously");
}
@ -267,8 +267,7 @@ bool OdometryRGBD::compute(InputArray _srcDepthFrame, InputArray _srcRGBFrame,
dstFrame.setImage(_dstRGBFrame);
prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype);
float scale = 0;
bool isCorrect = compute(srcFrame, dstFrame, Rt, scale);
bool isCorrect = compute(srcFrame, dstFrame, Rt);
return isCorrect;
}
@ -346,30 +345,15 @@ void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame)
bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt)
{
float scale = 0.f;
return this->impl->compute(srcFrame, dstFrame, Rt, scale);
return this->impl->compute(srcFrame, dstFrame, Rt);
}
bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale)
{
return this->impl->compute(srcFrame, dstFrame, Rt, scale);
}
bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const
{
float scale = 0.f;
return this->impl->compute(srcFrame, dstFrame, Rt, scale);
return this->impl->compute(srcFrame, dstFrame, Rt);
}
bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, OutputArray _scale) const
{
_scale.create(Size(1, 1), CV_64FC1);
Mat scaleValue = _scale.getMat();
float scale = 1.f;
bool res = this->impl->compute(srcFrame, dstFrame, Rt, scale);
Mat(1, 1, CV_64FC1, Scalar(scale)).copyTo(scaleValue);
return res;
}
bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const

214
modules/3d/src/rgbd/odometry_functions.cpp
Unescape View File

@ -648,7 +648,7 @@ void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask
}
}
bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
const OdometryFrame srcFrame,
const OdometryFrame dstFrame,
const Matx33f& cameraMatrix,
@ -656,16 +656,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transformType, OdometryAlgoType algtype)
{
if ((transformType == OdometryTransformType::RIGID_TRANSFORMATION) &&
(std::abs(scale) > std::numeric_limits<float>::epsilon()))
{
transformType = OdometryTransformType::SIM_TRANSFORMATION;
}
else
{
scale = 1.0f;
}
int transformDim = getTransformDim(transformType);
const int minOverdetermScale = 20;
@ -676,14 +666,12 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
Mat currRt, ksi;
double currScale = 1.0;
Affine3f transform = Affine3f::Identity();
bool isOk = false;
for(int level = (int)iterCounts.size() - 1; level >= 0; level--)
{
const Matx33f& levelCameraMatrix = pyramidCameraMatrix[level];
const Matx33f& levelCameraMatrix_inv = levelCameraMatrix.inv(DECOMP_SVD);
const Mat srcLevelDepth, dstLevelDepth;
const Mat srcLevelImage, dstLevelImage;
srcFrame.getPyramidAt(srcLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level);
@ -705,32 +693,33 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
for(int iter = 0; iter < iterCounts[level]; iter ++)
{
Mat resultRt_inv = resultRt.inv(DECOMP_SVD);
Mat corresps_rgbd, corresps_icp, diffs_rgbd, diffs_icp;
double sigma_rgbd = 0, sigma_icp = 0;
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
if(method != OdometryType::DEPTH) // RGB
{
const Mat pyramidTexturedMask;
dstFrame.getPyramidAt(pyramidTexturedMask, OdometryFramePyramidType::PYR_TEXMASK, level);
computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
computeCorresps(levelCameraMatrix, resultRt,
srcLevelImage, srcLevelDepth, pyramidMask,
dstLevelImage, dstLevelDepth, pyramidTexturedMask, maxDepthDiff,
corresps_rgbd, diffs_rgbd, sigma_rgbd, OdometryType::RGB);
}
if(method != OdometryType::RGB)// ICP
if(method != OdometryType::RGB) // ICP
{
if (algtype == OdometryAlgoType::COMMON)
{
const Mat pyramidNormalsMask;
dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level);
computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
computeCorresps(levelCameraMatrix, resultRt,
Mat(), srcLevelDepth, pyramidMask,
Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff,
corresps_icp, diffs_icp, sigma_icp, OdometryType::DEPTH);
corresps_icp, dummy, dummyFloat, OdometryType::DEPTH);
}
}
@ -764,7 +753,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
if (algtype == OdometryAlgoType::COMMON)
{
calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals,
corresps_icp, AtA_icp, AtB_icp, currScale, transformType);
corresps_icp, AtA_icp, AtB_icp, transformType);
}
else
{
@ -780,18 +769,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
AtB += AtB_icp;
}
// workaround for bad AtA matrix
if (transformType == OdometryTransformType::SIM_TRANSFORMATION)
if (countNonZero(AtA(Range::all(), Range(6, 7))) == 0)
{
Mat tmp(6, 6, CV_64FC1, Scalar(0));
AtA(Range(0, 6), Range(0, 6)).copyTo(tmp);
AtA = tmp;
transformType = OdometryTransformType::RIGID_TRANSFORMATION;
transformDim = getTransformDim(transformType);
break;
}
bool solutionExist = solveSystem(AtA, AtB, determinantThreshold, ksi);
if (!solutionExist)
@ -800,7 +777,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
}
Mat tmp61(6, 1, CV_64FC1, Scalar(0));
double newScale = 1.0;
if(transformType == OdometryTransformType::ROTATION)
{
ksi.copyTo(tmp61.rowRange(0,3));
@ -811,25 +787,9 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
ksi.copyTo(tmp61.rowRange(3,6));
ksi = tmp61;
}
else if (transformType == OdometryTransformType::SIM_TRANSFORMATION)
{
newScale = ksi.at<double>(6, 0);
ksi.rowRange(0, 6).copyTo(tmp61);
ksi = tmp61;
}
computeProjectiveMatrix(ksi, currRt);
//resultRt = currRt * resultRt;
cv::Matx33d cr = currRt(cv::Rect(0, 0, 3, 3)), rr = resultRt(cv::Rect(0, 0, 3, 3));
cv::Vec3d ct = currRt(cv::Rect(0, 3, 3, 1)), rt = resultRt(cv::Rect(0, 3, 3, 1));
cv::Matx33d nr = cr * rr;
cv::Vec3f nt = ct + cr * rt * newScale;
Matx44d nrt = Matx44d::eye();
nrt.get_minor<3, 3>(0, 0) = nr;
nrt.get_minor<3, 1>(0, 3) = nt;
nrt.copyTo(resultRt);
currScale *= newScale;
resultRt = currRt * resultRt;
//TODO: fixit, transform is used for Fast ICP only
Vec6f x(ksi);
@ -844,7 +804,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
_Rt.create(resultRt.size(), resultRt.type());
Mat Rt = _Rt.getMat();
resultRt.copyTo(Rt);
scale =(float)currScale;
if(isOk)
{
Mat deltaRt;
@ -859,81 +818,84 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
return isOk;
}
// Rotate dst by Rt (which is inverted in fact) to get corresponding src pixels
// Rotate dst by RtInv to get corresponding src pixels
// In RGB case compute sigma and diffs too
void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
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)
{
CV_Assert(Rt.type() == CV_64FC1);
Mat mrtInv = rt.inv(DECOMP_SVD);
Matx44d rtInv = mrtInv;
Mat corresps(depthDst.size(), CV_16SC2, Scalar::all(-1));
Mat diffs(depthDst.size(), CV_32F, 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), K_inv(_K_inv);
Matx33d K(_K);
Matx33d K_inv = K.inv(DECOMP_SVD);
Rect r(0, 0, depthDst.cols, depthDst.rows);
Mat Kt = Rt(Rect(3, 0, 1, 3)).clone();
Kt = K * Kt;
const double* Kt_ptr = Kt.ptr<const double>();
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 = KRK_inv0_u1 + depthDst.cols;
float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depthDst.rows;
float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depthDst.cols;
float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depthDst.rows;
float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depthDst.cols;
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;
{
Mat R = Rt(Rect(0, 0, 3, 3)).clone();
Matx33d rinv = rtInv.get_minor<3, 3>(0, 0);
Mat KRK_inv = K * R * K_inv;
const double* KRK_inv_ptr = KRK_inv.ptr<const double>();
for (int u1 = 0; u1 < depthDst.cols; u1++)
Matx33d kriki = K * rinv * K_inv;
for (int udst = 0; udst < depthDst.cols; udst++)
{
KRK_inv0_u1[u1] = (float)(KRK_inv_ptr[0] * u1);
KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1);
KRK_inv6_u1[u1] = (float)(KRK_inv_ptr[6] * u1);
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 v1 = 0; v1 < depthDst.rows; v1++)
for (int vdst = 0; vdst < depthDst.rows; vdst++)
{
KRK_inv1_v1_plus_KRK_inv2[v1] = (float)(KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]);
KRK_inv4_v1_plus_KRK_inv5[v1] = (float)(KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]);
KRK_inv7_v1_plus_KRK_inv8[v1] = (float)(KRK_inv_ptr[7] * v1 + KRK_inv_ptr[8]);
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 v1 = 0; v1 < depthDst.rows; v1++)
for (int vdst = 0; vdst < depthDst.rows; vdst++)
{
const float* depth1_row = depthDst.ptr<float>(v1);
const uchar* mask1_row = selectMaskDst.ptr<uchar>(v1);
for (int u1 = 0; u1 < depthDst.cols; u1++)
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 d1 = depth1_row[u1];
if (mask1_row[u1] && !cvIsNaN(d1))
float ddst = depthDst_row[udst];
if (maskDst_row[udst] && !cvIsNaN(ddst))
{
//CV_DbgAssert(!cvIsNaN(d1));
float transformed_d1 = static_cast<float>(d1 * (KRK_inv6_u1[u1] + KRK_inv7_v1_plus_KRK_inv8[v1]) +
Kt_ptr[2]);
float transformed_ddst = static_cast<float>(ddst * (KRK_inv6_u1[udst] + KRK_inv7_v1_plus_KRK_inv8[vdst]) + ktinv.z);
if (transformed_d1 > 0)
if (transformed_ddst > 0)
{
float transformed_d1_inv = 1.f / transformed_d1;
int u0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv0_u1[u1] + KRK_inv1_v1_plus_KRK_inv2[v1]) +
Kt_ptr[0]));
int v0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv3_u1[u1] + KRK_inv4_v1_plus_KRK_inv5[v1]) +
Kt_ptr[1]));
if (r.contains(Point(u0, v0)))
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 d0 = depthSrc.at<float>(v0, u0);
if (validMaskSrc.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff)
float dsrc = depthSrc.at<float>(vsrc, usrc);
if (validMaskSrc.at<uchar>(vsrc, usrc) && std::abs(transformed_ddst - dsrc) <= maxDepthDiff)
{
CV_DbgAssert(!cvIsNaN(d0));
Vec2s& c = corresps.at<Vec2s>(v0, u0);
float& d = diffs.at<float>(v0, u0);
CV_DbgAssert(!cvIsNaN(dsrc));
Vec2s& c = corresps.at<Vec2s>(vsrc, usrc);
float diff = 0;
if (c[0] != -1)
{
@ -941,23 +903,25 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
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]) + Kt_ptr[2]);
(KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + ktinv.z);
if (transformed_d1 > exist_d1)
if (transformed_ddst > exist_d1)
continue;
if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(v0, u0)) -
static_cast<int>(imageDst.at<uchar>(v1, u1)));
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>(v0, u0)) -
static_cast<int>(imageDst.at<uchar>(v1, u1)));
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(vsrc, usrc)) -
static_cast<int>(imageDst.at<uchar>(vdst, udst)));
correspCount++;
}
c = Vec2s((short)u1, (short)v1);
d = diff;
c = Vec2s((short)udst, (short)vdst);
if (method == OdometryType::RGB)
{
diffs.at<float>(vsrc, usrc) = diff;
sigma += diff * diff;
}
}
@ -965,24 +929,31 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
}
}
}
}
_sigma = std::sqrt(sigma / double(correspCount));
_corresps.create(correspCount, 1, CV_32SC4);
_diffs.create(correspCount, 1, CV_32F);
Vec4i* corresps_ptr = _corresps.ptr<Vec4i>();
float* diffs_ptr = _diffs.ptr<float>();
for (int v0 = 0, i = 0; v0 < corresps.rows; v0++)
float* diffs_ptr;
if (method == OdometryType::RGB)
{
const Vec2s* corresps_row = corresps.ptr<Vec2s>(v0);
const float* diffs_row = diffs.ptr<float>(v0);
for (int u0 = 0; u0 < corresps.cols; u0++)
_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;
if (method == OdometryType::RGB)
diffs_row = diffs.ptr<float>(vsrc);
for (int usrc = 0; usrc < corresps.cols; usrc++)
{
const Vec2s& c = corresps_row[u0];
const float& d = diffs_row[u0];
const Vec2s& c = corresps_row[usrc];
const float& d = diffs_row[usrc];
if (c[0] != -1)
{
corresps_ptr[i] = Vec4i(u0, v0, c[0], c[1]);
corresps_ptr[i] = Vec4i(usrc, vsrc, c[0], c[1]);
if (method == OdometryType::RGB)
diffs_ptr[i] = d;
i++;
@ -1003,7 +974,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
double* AtB_ptr = AtB.ptr<double>();
CV_Assert(Rt.type() == CV_64FC1);
const double* Rt_ptr = Rt.ptr<const double>();
Affine3d rtmat(Rt);
const float* diffs_ptr = _diffs.ptr<float>();
const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
@ -1024,10 +995,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
double w_sobelScale = w * sobelScaleIn;
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]);
Point3f tp0 = rtmat * Point3f(p0[0], p0[1], p0[2]);
rgbdCoeffsFunc(transformType,
A_ptr,
@ -1055,7 +1023,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& cloud1, const Mat& normals1,
const Mat& corresps,
Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType)
Mat& AtA, Mat& AtB, OdometryTransformType transformType)
{
int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
@ -1084,9 +1052,9 @@ void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
Point3f tp0;
tp0.x = (float)(p0[0] * scale * Rt_ptr[0] + p0[1] * scale * Rt_ptr[1] + p0[2] * scale * Rt_ptr[2] + Rt_ptr[3]);
tp0.y = (float)(p0[0] * scale * Rt_ptr[4] + p0[1] * scale * Rt_ptr[5] + p0[2] * scale * Rt_ptr[6] + Rt_ptr[7]);
tp0.z = (float)(p0[0] * scale * Rt_ptr[8] + p0[1] * scale * Rt_ptr[9] + p0[2] * scale * Rt_ptr[10] + Rt_ptr[11]);
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);

146
modules/3d/src/rgbd/odometry_functions.hpp
Unescape View File

@ -12,16 +12,14 @@ namespace cv
{
enum class OdometryTransformType
{
// rotation, translation, rotation+translation, rotation*scale+translation
ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4, SIM_TRANSFORMATION = 8
// rotation, translation, rotation+translation
ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4
};
static inline int getTransformDim(OdometryTransformType transformType)
{
switch(transformType)
{
case OdometryTransformType::SIM_TRANSFORMATION:
return 7;
case OdometryTransformType::RIGID_TRANSFORMATION:
return 6;
case OdometryTransformType::ROTATION:
@ -40,6 +38,7 @@ void checkImage(InputArray image)
if (image.type() != CV_8UC1)
CV_Error(Error::StsBadSize, "Image type has to be CV_8UC1.");
}
static inline
void checkDepth(InputArray depth, const Size& imageSize)
{
@ -72,28 +71,9 @@ void checkNormals(InputArray normals, const Size& depthSize)
CV_Error(Error::StsBadSize, "Normals type has to be CV_32FC3.");
}
static inline
void calcRgbdScaleEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
double invz = 1. / p3d.z,
v0 = dIdx * fx * invz,
v1 = dIdy * fy * invz,
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
Point3d v(v0, v1, v2);
Point3d pxv = p3d.cross(v);
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
C[3] = v0;
C[4] = v1;
C[5] = v2;
//TODO: fixit
C[6] = 0;
}
static inline
void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
Vec6d calcRgbdEquationCoeffs(double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
double invz = 1. / p3d.z,
v0 = dIdx * fx * invz,
@ -102,16 +82,11 @@ void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f&
Point3d v(v0, v1, v2);
Point3d pxv = p3d.cross(v);
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
C[3] = v0;
C[4] = v1;
C[5] = v2;
return Vec6d(pxv.x, pxv.y, pxv.z, v0, v1, v2);
}
static inline
void calcRgbdEquationCoeffsRotation(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
Vec3d calcRgbdEquationCoeffsRotation(double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
double invz = 1. / p3d.z,
v0 = dIdx * fx * invz,
@ -121,117 +96,103 @@ void calcRgbdEquationCoeffsRotation(double* C, double dIdx, double dIdy, const P
Point3d v(v0, v1, v2);
Point3d pxv = p3d.cross(v);
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
return Vec3d(pxv);
}
static inline
void calcRgbdEquationCoeffsTranslation(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
Vec3d calcRgbdEquationCoeffsTranslation(double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
double invz = 1. / p3d.z,
v0 = dIdx * fx * invz,
v1 = dIdy * fy * invz,
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
C[0] = v0;
C[1] = v1;
C[2] = v2;
return Vec3d(v0, v1, v2);
}
static inline void rgbdCoeffsFunc(OdometryTransformType transformType,
double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
int dim = getTransformDim(transformType);
Vec6d ret;
switch(transformType)
{
case OdometryTransformType::SIM_TRANSFORMATION:
calcRgbdScaleEquationCoeffs(C, dIdx, dIdy, p3d, fx, fy);
break;
case OdometryTransformType::RIGID_TRANSFORMATION:
calcRgbdEquationCoeffs(C, dIdx, dIdy, p3d, fx, fy);
{
ret = calcRgbdEquationCoeffs(dIdx, dIdy, p3d, fx, fy);
break;
}
case OdometryTransformType::ROTATION:
calcRgbdEquationCoeffsRotation(C, dIdx, dIdy, p3d, fx, fy);
{
Vec3d r = calcRgbdEquationCoeffsRotation(dIdx, dIdy, p3d, fx, fy);
ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
break;
}
case OdometryTransformType::TRANSLATION:
calcRgbdEquationCoeffsTranslation(C, dIdx, dIdy, p3d, fx, fy);
{
Vec3d r = calcRgbdEquationCoeffsTranslation(dIdx, dIdy, p3d, fx, fy);
ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
break;
}
default:
CV_Error(Error::StsBadArg, "Incorrect transformation type");
}
for (int i = 0; i < dim; i++)
C[i] = ret[i];
}
static inline
void calcICPScaleEquationCoeffs(double* C, const Point3f& p0, const Point3f& p1, const Vec3f& n1)
{
Point3d pxv = p0.cross(Point3d(n1));
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
C[3] = n1[0];
C[4] = n1[1];
C[5] = n1[2];
double diff = n1.dot(p1 - p0);
//TODO: fixit
if (diff < DBL_EPSILON || abs(diff) > 1000000)
C[6] = 0;
else
//C[6] = n1.dot(p1-p0);
C[6] = -n1.dot(p0);
//C[6] = n1.dot((p0 - currentTranslation)/currentScale);
}
static inline
void calcICPEquationCoeffs(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1)
Vec6d calcICPEquationCoeffs(const Point3f& psrc, const Vec3f& ndst)
{
Point3d pxv = p0.cross(Point3d(n1));
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
C[3] = n1[0];
C[4] = n1[1];
C[5] = n1[2];
Point3d pxv = psrc.cross(Point3d(ndst));
return Vec6d(pxv.x, pxv.y, pxv.z, ndst[0], ndst[1], ndst[2]);
}
static inline
void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1)
Vec3d calcICPEquationCoeffsRotation(const Point3f& psrc, const Vec3f& ndst)
{
Point3d pxv = p0.cross(Point3d(n1));
Point3d pxv = psrc.cross(Point3d(ndst));
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
return Vec3d(pxv);
}
static inline
void calcICPEquationCoeffsTranslation(double* C, const Point3f& /*p0*/, const Point3f& /*p1*/, const Vec3f& n1)
Vec3d calcICPEquationCoeffsTranslation( const Point3f& /*p0*/, const Vec3f& ndst)
{
C[0] = n1[0];
C[1] = n1[1];
C[2] = n1[2];
return Vec3d(ndst);
}
static inline
void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3f& p0, const Point3f& p1, const Vec3f& n1)
void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1)
{
int dim = getTransformDim(transformType);
Vec6d ret;
switch(transformType)
{
case OdometryTransformType::SIM_TRANSFORMATION:
calcICPScaleEquationCoeffs(C, p0, p1, n1);
break;
case OdometryTransformType::RIGID_TRANSFORMATION:
calcICPEquationCoeffs(C, p0, p1, n1);;
{
ret = calcICPEquationCoeffs(p0, n1);
break;
}
case OdometryTransformType::ROTATION:
calcICPEquationCoeffsRotation(C, p0, p1, n1);;
{
Vec3d r = calcICPEquationCoeffsRotation(p0, n1);
ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
break;
}
case OdometryTransformType::TRANSLATION:
calcICPEquationCoeffsTranslation(C, p0, p1, n1);;
{
Vec3d r = calcICPEquationCoeffsTranslation(p0, n1);
ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
break;
}
default:
CV_Error(Error::StsBadArg, "Incorrect transformation type");
}
for (int i = 0; i < dim; i++)
C[i] = ret[i];
}
void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype);
@ -275,8 +236,7 @@ void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask
InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask);
// scale = 0, if not needs scale; otherwise scale = 1;
bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
const OdometryFrame srcFrame,
const OdometryFrame dstFrame,
const Matx33f& cameraMatrix,
@ -284,7 +244,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype);
void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
void computeCorresps(const Matx33f& _K, const Mat& Rt,
const Mat& image0, const Mat& depth0, const Mat& validMask0,
const Mat& image1, const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method);
@ -298,7 +258,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& cloud1, const Mat& normals1,
const Mat& corresps,
Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType);
Mat& AtA, Mat& AtB, OdometryTransformType transformType);
void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm,
cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b);

29
modules/3d/test/test_odometry.cpp
Unescape View File

@ -156,7 +156,6 @@ public:
OdometryAlgoType algtype;
double maxError1;
double maxError5;
bool testScale;
double idError;
};
@ -251,16 +250,12 @@ void OdometryTest::run()
odf.setImage(image);
odf.setDepth(depth);
Mat calcRt;
float scale = 1.0f;
// 1. Try to find Rt between the same frame (try masks also).
Mat mask(image.size(), CV_8UC1, Scalar(255));
odometry.prepareFrame(odf);
bool isComputed;
if (testScale)
isComputed = odometry.compute(odf, odf, calcRt, scale);
else
isComputed = odometry.compute(odf, odf, calcRt);
if(!isComputed)
@ -290,8 +285,6 @@ void OdometryTest::run()
Mat warpedImage, warpedDepth, scaledDepth;
float test_scale = 1.03f;
scaledDepth = testScale ? depth * test_scale : depth;
warpFrame(image, scaledDepth, rvec, tvec, K, warpedImage, warpedDepth);
dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
@ -299,17 +292,12 @@ void OdometryTest::run()
OdometryFrame odfSrc = odometry.createOdometryFrame();
OdometryFrame odfDst = odometry.createOdometryFrame();
float scale_error = 0.05f;
odfSrc.setImage(image);
odfSrc.setDepth(depth);
odfDst.setImage(warpedImage);
odfDst.setDepth(warpedDepth);
odometry.prepareFrames(odfSrc, odfDst);
if (testScale)
isComputed = odometry.compute(odfSrc, odfDst, calcRt, scale);
else
isComputed = odometry.compute(odfSrc, odfDst, calcRt);
if (!isComputed)
@ -334,8 +322,6 @@ void OdometryTest::run()
// compare rotation
double possibleError = algtype == OdometryAlgoType::COMMON ? 0.11f : 0.015f;
if (testScale)
possibleError = 0.2f;
Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec));
Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec));
@ -344,15 +330,14 @@ void OdometryTest::run()
double rdiffnorm = cv::norm(diff.rvec());
double tdiffnorm = cv::norm(diff.translation());
if (rdiffnorm < possibleError && tdiffnorm < possibleError && abs(scale - test_scale) < scale_error)
if (rdiffnorm < possibleError && tdiffnorm < possibleError)
better_1time_count++;
if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError && abs(scale - test_scale) < scale_error)
if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError)
better_5times_count++;
CV_LOG_INFO(NULL, "Iter " << iter);
CV_LOG_INFO(NULL, "rdiff: " << Vec3f(diff.rvec()) << "; rdiffnorm: " << rdiffnorm);
CV_LOG_INFO(NULL, "tdiff: " << Vec3f(diff.translation()) << "; tdiffnorm: " << tdiffnorm);
CV_LOG_INFO(NULL, "test_scale: " << test_scale << "; scale: " << scale);
CV_LOG_INFO(NULL, "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count);
}
@ -423,12 +408,6 @@ TEST(RGBD_Odometry_ICP, algorithmic)
test.run();
}
TEST(RGBD_Odometry_ICP_Scale, algorithmic)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.65, 0.0, true);
test.run();
}
TEST(RGBD_Odometry_RgbdICP, algorithmic)
{
OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
@ -437,7 +416,7 @@ TEST(RGBD_Odometry_RgbdICP, algorithmic)
TEST(RGBD_Odometry_FastICP, algorithmic)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON);
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON);
test.run();
}
@ -462,7 +441,7 @@ TEST(RGBD_Odometry_RgbdICP, UMats)
TEST(RGBD_Odometry_FastICP, UMats)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON);
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON);
test.checkUMats();
}

Write Preview
Loading…
Cancel
Save