mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-06-08 01:53:19 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

1. removed (almost all) additional scale parameter mentions

Browse Source 
2. refactored funcptrs to switch/cases & more
This commit is contained in:
Rostislav Vasilikhin 2022-06-23 23:47:08 +02:00
parent c12d4c82df
commit 770c0d1416
5 changed files with 182 additions and 305 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -73,21 +73,7 @@ public:
*/ */
bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt); 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) 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; CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const;
class Impl; class Impl;

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

@ -20,8 +20,8 @@ public:
virtual OdometryFrame createOdometryFrame() const = 0; virtual OdometryFrame createOdometryFrame() const = 0;
virtual void prepareFrame(OdometryFrame& frame) = 0; virtual void prepareFrame(OdometryFrame& frame) = 0;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) = 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(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const = 0;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const = 0; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const = 0;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const = 0; InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const = 0;
}; };
@ -40,8 +40,8 @@ public:
virtual OdometryFrame createOdometryFrame() const override; virtual OdometryFrame createOdometryFrame() const override;
virtual void prepareFrame(OdometryFrame& frame) override; virtual void prepareFrame(OdometryFrame& frame) override;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) 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(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; 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); 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; Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix); settings.getCameraMatrix(cameraMatrix);
@ -84,7 +84,7 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
settings.getIterCounts(miterCounts); settings.getIterCounts(miterCounts);
for (int i = 0; i < miterCounts.size().height; i++) for (int i = 0; i < miterCounts.size().height; i++)
iterCounts.push_back(miterCounts.at<int>(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(), this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
iterCounts, this->settings.getMaxTranslation(), iterCounts, this->settings.getMaxTranslation(),
this->settings.getMaxRotation(), settings.getSobelScale(), this->settings.getMaxRotation(), settings.getSobelScale(),
@ -92,7 +92,7 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
return isCorrect; 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 srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = 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); prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype);
bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); bool isCorrect = compute(srcFrame, dstFrame, Rt);
return isCorrect; return isCorrect;
} }
@ -123,8 +123,8 @@ public:
virtual OdometryFrame createOdometryFrame() const override; virtual OdometryFrame createOdometryFrame() const override;
virtual void prepareFrame(OdometryFrame& frame) override; virtual void prepareFrame(OdometryFrame& frame) override;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) 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(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; 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); 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; Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix); settings.getCameraMatrix(cameraMatrix);
@ -164,7 +164,7 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
CV_CheckTypeEQ(miterCounts.type(), CV_32S, ""); CV_CheckTypeEQ(miterCounts.type(), CV_32S, "");
for (int i = 0; i < miterCounts.size().height; i++) for (int i = 0; i < miterCounts.size().height; i++)
iterCounts.push_back(miterCounts.at<int>(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(), this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
iterCounts, this->settings.getMaxTranslation(), iterCounts, this->settings.getMaxTranslation(),
this->settings.getMaxRotation(), settings.getSobelScale(), this->settings.getMaxRotation(), settings.getSobelScale(),
@ -172,7 +172,7 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
return isCorrect; 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 srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = this->createOdometryFrame(); OdometryFrame dstFrame = this->createOdometryFrame();
@ -181,13 +181,13 @@ bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArra
prepareRGBFrame(srcFrame, dstFrame, this->settings, false); prepareRGBFrame(srcFrame, dstFrame, this->settings, false);
bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); bool isCorrect = compute(srcFrame, dstFrame, Rt);
return isCorrect; return isCorrect;
} }
bool OdometryRGB::compute(InputArray, InputArray, InputArray, InputArray, OutputArray) const 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 class OdometryRGBD : public Odometry::Impl
@ -203,8 +203,8 @@ public:
virtual OdometryFrame createOdometryFrame() const override; virtual OdometryFrame createOdometryFrame() const override;
virtual void prepareFrame(OdometryFrame& frame) override; virtual void prepareFrame(OdometryFrame& frame) override;
virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) 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(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override;
virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; 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); 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; Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix); settings.getCameraMatrix(cameraMatrix);
@ -243,7 +243,7 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d
settings.getIterCounts(miterCounts); settings.getIterCounts(miterCounts);
for (int i = 0; i < miterCounts.size().height; i++) for (int i = 0; i < miterCounts.size().height; i++)
iterCounts.push_back(miterCounts.at<int>(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(), this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
iterCounts, this->settings.getMaxTranslation(), iterCounts, this->settings.getMaxTranslation(),
this->settings.getMaxRotation(), settings.getSobelScale(), this->settings.getMaxRotation(), settings.getSobelScale(),
@ -251,7 +251,7 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d
return isCorrect; 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"); 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); dstFrame.setImage(_dstRGBFrame);
prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype); prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype);
float scale = 0; bool isCorrect = compute(srcFrame, dstFrame, Rt);
bool isCorrect = compute(srcFrame, dstFrame, Rt, scale);
return isCorrect; return isCorrect;
} }
@ -346,30 +345,15 @@ void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame)
bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt)
{ {
float scale = 0.f; return this->impl->compute(srcFrame, dstFrame, Rt);
return this->impl->compute(srcFrame, dstFrame, Rt, scale);
} }
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 bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const
{ {
float scale = 0.f; return this->impl->compute(srcFrame, dstFrame, Rt);
return this->impl->compute(srcFrame, dstFrame, Rt, scale);
} }
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, bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const

218
modules/3d/src/rgbd/odometry_functions.cpp
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 srcFrame,
const OdometryFrame dstFrame, const OdometryFrame dstFrame,
const Matx33f& cameraMatrix, const Matx33f& cameraMatrix,
@ -656,16 +656,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
double maxTranslation, double maxRotation, double sobelScale, double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transformType, OdometryAlgoType algtype) 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); int transformDim = getTransformDim(transformType);
const int minOverdetermScale = 20; 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 resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
Mat currRt, ksi; Mat currRt, ksi;
double currScale = 1.0;
Affine3f transform = Affine3f::Identity(); Affine3f transform = Affine3f::Identity();
bool isOk = false; bool isOk = false;
for(int level = (int)iterCounts.size() - 1; level >= 0; level--) for(int level = (int)iterCounts.size() - 1; level >= 0; level--)
{ {
const Matx33f& levelCameraMatrix = pyramidCameraMatrix[level]; const Matx33f& levelCameraMatrix = pyramidCameraMatrix[level];
const Matx33f& levelCameraMatrix_inv = levelCameraMatrix.inv(DECOMP_SVD);
const Mat srcLevelDepth, dstLevelDepth; const Mat srcLevelDepth, dstLevelDepth;
const Mat srcLevelImage, dstLevelImage; const Mat srcLevelImage, dstLevelImage;
srcFrame.getPyramidAt(srcLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level); 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 ++) for(int iter = 0; iter < iterCounts[level]; iter ++)
{ {
Mat resultRt_inv = resultRt.inv(DECOMP_SVD); Mat resultRt_inv = resultRt.inv(DECOMP_SVD);
Mat corresps_rgbd, corresps_icp, diffs_rgbd, diffs_icp; Mat corresps_rgbd, corresps_icp, diffs_rgbd;
double sigma_rgbd = 0, sigma_icp = 0; Mat dummy;
double sigma_rgbd = 0, dummyFloat = 0;
const Mat pyramidMask; const Mat pyramidMask;
srcFrame.getPyramidAt(pyramidMask, OdometryFramePyramidType::PYR_MASK, level); srcFrame.getPyramidAt(pyramidMask, OdometryFramePyramidType::PYR_MASK, level);
if(method != OdometryType::DEPTH)// RGB if(method != OdometryType::DEPTH) // RGB
{ {
const Mat pyramidTexturedMask; const Mat pyramidTexturedMask;
dstFrame.getPyramidAt(pyramidTexturedMask, OdometryFramePyramidType::PYR_TEXMASK, level); dstFrame.getPyramidAt(pyramidTexturedMask, OdometryFramePyramidType::PYR_TEXMASK, level);
computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv, computeCorresps(levelCameraMatrix, resultRt,
srcLevelImage, srcLevelDepth, pyramidMask, srcLevelImage, srcLevelDepth, pyramidMask,
dstLevelImage, dstLevelDepth, pyramidTexturedMask, maxDepthDiff, dstLevelImage, dstLevelDepth, pyramidTexturedMask, maxDepthDiff,
corresps_rgbd, diffs_rgbd, sigma_rgbd, OdometryType::RGB); corresps_rgbd, diffs_rgbd, sigma_rgbd, OdometryType::RGB);
} }
if(method != OdometryType::RGB)// ICP if(method != OdometryType::RGB) // ICP
{ {
if (algtype == OdometryAlgoType::COMMON) if (algtype == OdometryAlgoType::COMMON)
{ {
const Mat pyramidNormalsMask; const Mat pyramidNormalsMask;
dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level); dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level);
computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv, computeCorresps(levelCameraMatrix, resultRt,
Mat(), srcLevelDepth, pyramidMask, Mat(), srcLevelDepth, pyramidMask,
Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff, 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) if (algtype == OdometryAlgoType::COMMON)
{ {
calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals, calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals,
corresps_icp, AtA_icp, AtB_icp, currScale, transformType); corresps_icp, AtA_icp, AtB_icp, transformType);
} }
else else
{ {
@ -780,18 +769,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
AtB += AtB_icp; 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); bool solutionExist = solveSystem(AtA, AtB, determinantThreshold, ksi);
if (!solutionExist) if (!solutionExist)
@ -800,7 +777,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
} }
Mat tmp61(6, 1, CV_64FC1, Scalar(0)); Mat tmp61(6, 1, CV_64FC1, Scalar(0));
double newScale = 1.0;
if(transformType == OdometryTransformType::ROTATION) if(transformType == OdometryTransformType::ROTATION)
{ {
ksi.copyTo(tmp61.rowRange(0,3)); 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.copyTo(tmp61.rowRange(3,6));
ksi = tmp61; 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); computeProjectiveMatrix(ksi, currRt);
//resultRt = currRt * resultRt; 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;
//TODO: fixit, transform is used for Fast ICP only //TODO: fixit, transform is used for Fast ICP only
Vec6f x(ksi); Vec6f x(ksi);
@ -844,7 +804,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
_Rt.create(resultRt.size(), resultRt.type()); _Rt.create(resultRt.size(), resultRt.type());
Mat Rt = _Rt.getMat(); Mat Rt = _Rt.getMat();
resultRt.copyTo(Rt); resultRt.copyTo(Rt);
scale =(float)currScale;
if(isOk) if(isOk)
{ {
Mat deltaRt; Mat deltaRt;
@ -859,81 +818,84 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
return isOk; 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 // 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& imageSrc, const Mat& depthSrc, const Mat& validMaskSrc,
const Mat& imageDst, const Mat& depthDst, const Mat& selectMaskDst, float maxDepthDiff, const Mat& imageDst, const Mat& depthDst, const Mat& selectMaskDst, float maxDepthDiff,
Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method) 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 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));
Matx33d K(_K), K_inv(_K_inv); // 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); Rect r(0, 0, depthDst.cols, depthDst.rows);
Mat Kt = Rt(Rect(3, 0, 1, 3)).clone(); Matx31d tinv = rtInv.get_minor<3, 1>(0, 3);
Kt = K * Kt; Matx31d ktinvm = K * tinv;
const double* Kt_ptr = Kt.ptr<const double>(); //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)); AutoBuffer<float> buf(3 * (depthDst.cols + depthDst.rows));
float* KRK_inv0_u1 = buf.data(); float* KRK_inv0_u1 = buf.data();
float* KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_u1 + depthDst.cols; float* KRK_inv1_v1_plus_KRK_inv2 = buf.data() + depthDst.cols;
float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depthDst.rows; float* KRK_inv3_u1 = buf.data() + depthDst.cols + depthDst.rows;
float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depthDst.cols; float* KRK_inv4_v1_plus_KRK_inv5 = buf.data() + depthDst.cols * 2 + depthDst.rows;
float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depthDst.rows; float* KRK_inv6_u1 = buf.data() + depthDst.cols * 2 + depthDst.rows * 2;
float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depthDst.cols; 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; Matx33d kriki = K * rinv * K_inv;
const double* KRK_inv_ptr = KRK_inv.ptr<const double>(); for (int udst = 0; udst < depthDst.cols; udst++)
for (int u1 = 0; u1 < depthDst.cols; u1++)
{ {
KRK_inv0_u1[u1] = (float)(KRK_inv_ptr[0] * u1); KRK_inv0_u1[udst] = (float)(kriki(0, 0) * udst);
KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1); KRK_inv3_u1[udst] = (float)(kriki(1, 0) * udst);
KRK_inv6_u1[u1] = (float)(KRK_inv_ptr[6] * u1); 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_inv1_v1_plus_KRK_inv2[vdst] = (float)(kriki(0, 1) * vdst + kriki(0, 2));
KRK_inv4_v1_plus_KRK_inv5[v1] = (float)(KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]); KRK_inv4_v1_plus_KRK_inv5[vdst] = (float)(kriki(1, 1) * vdst + kriki(1, 2));
KRK_inv7_v1_plus_KRK_inv8[v1] = (float)(KRK_inv_ptr[7] * v1 + KRK_inv_ptr[8]); KRK_inv7_v1_plus_KRK_inv8[vdst] = (float)(kriki(2, 1) * vdst + kriki(2, 2));
} }
} }
double sigma = 0; double sigma = 0;
int correspCount = 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 float* depthDst_row = depthDst.ptr<float>(vdst);
const uchar* mask1_row = selectMaskDst.ptr<uchar>(v1); const uchar* maskDst_row = selectMaskDst.ptr<uchar>(vdst);
for (int u1 = 0; u1 < depthDst.cols; u1++) for (int udst = 0; udst < depthDst.cols; udst++)
{ {
float d1 = depth1_row[u1]; float ddst = depthDst_row[udst];
if (mask1_row[u1] && !cvIsNaN(d1))
{
//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]);
if (transformed_d1 > 0) if (maskDst_row[udst] && !cvIsNaN(ddst))
{
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_d1_inv = 1.f / transformed_d1; float transformed_ddst_inv = 1.f / transformed_ddst;
int u0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv0_u1[u1] + KRK_inv1_v1_plus_KRK_inv2[v1]) + int usrc = cvRound(transformed_ddst_inv * (ddst * (KRK_inv0_u1[udst] + KRK_inv1_v1_plus_KRK_inv2[vdst]) + ktinv.x));
Kt_ptr[0])); int vsrc = cvRound(transformed_ddst_inv * (ddst * (KRK_inv3_u1[udst] + KRK_inv4_v1_plus_KRK_inv5[vdst]) + ktinv.y));
int v0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv3_u1[u1] + KRK_inv4_v1_plus_KRK_inv5[v1]) + if (r.contains(Point(usrc, vsrc)))
Kt_ptr[1]));
if (r.contains(Point(u0, v0)))
{ {
float d0 = depthSrc.at<float>(v0, u0); float dsrc = depthSrc.at<float>(vsrc, usrc);
if (validMaskSrc.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff) if (validMaskSrc.at<uchar>(vsrc, usrc) && std::abs(transformed_ddst - dsrc) <= maxDepthDiff)
{ {
CV_DbgAssert(!cvIsNaN(d0)); CV_DbgAssert(!cvIsNaN(dsrc));
Vec2s& c = corresps.at<Vec2s>(v0, u0); Vec2s& c = corresps.at<Vec2s>(vsrc, usrc);
float& d = diffs.at<float>(v0, u0);
float diff = 0; float diff = 0;
if (c[0] != -1) if (c[0] != -1)
{ {
@ -941,24 +903,27 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
int exist_u1 = c[0], exist_v1 = c[1]; int exist_u1 = c[0], exist_v1 = c[1];
float exist_d1 = (float)(depthDst.at<float>(exist_v1, exist_u1) * 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; continue;
if (method == OdometryType::RGB) if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(v0, u0)) - diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(vsrc, usrc)) -
static_cast<int>(imageDst.at<uchar>(v1, u1))); static_cast<int>(imageDst.at<uchar>(vdst, udst)));
} }
else else
{ {
if (method == OdometryType::RGB) if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(v0, u0)) - diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(vsrc, usrc)) -
static_cast<int>(imageDst.at<uchar>(v1, u1))); static_cast<int>(imageDst.at<uchar>(vdst, udst)));
correspCount++; correspCount++;
} }
c = Vec2s((short)u1, (short)v1); c = Vec2s((short)udst, (short)vdst);
d = diff; if (method == OdometryType::RGB)
sigma += diff * diff; {
diffs.at<float>(vsrc, usrc) = diff;
sigma += diff * diff;
}
} }
} }
} }
@ -969,20 +934,26 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
_sigma = std::sqrt(sigma / double(correspCount)); _sigma = std::sqrt(sigma / double(correspCount));
_corresps.create(correspCount, 1, CV_32SC4); _corresps.create(correspCount, 1, CV_32SC4);
_diffs.create(correspCount, 1, CV_32F);
Vec4i* corresps_ptr = _corresps.ptr<Vec4i>(); Vec4i* corresps_ptr = _corresps.ptr<Vec4i>();
float* diffs_ptr = _diffs.ptr<float>(); float* diffs_ptr;
for (int v0 = 0, i = 0; v0 < corresps.rows; v0++) if (method == OdometryType::RGB)
{ {
const Vec2s* corresps_row = corresps.ptr<Vec2s>(v0); _diffs.create(correspCount, 1, CV_32F);
const float* diffs_row = diffs.ptr<float>(v0); diffs_ptr = _diffs.ptr<float>();
for (int u0 = 0; u0 < corresps.cols; u0++) }
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 Vec2s& c = corresps_row[usrc];
const float& d = diffs_row[u0]; const float& d = diffs_row[usrc];
if (c[0] != -1) 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) if (method == OdometryType::RGB)
diffs_ptr[i] = d; diffs_ptr[i] = d;
i++; i++;
@ -1003,7 +974,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
double* AtB_ptr = AtB.ptr<double>(); double* AtB_ptr = AtB.ptr<double>();
CV_Assert(Rt.type() == CV_64FC1); 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 float* diffs_ptr = _diffs.ptr<float>();
const Vec4i* corresps_ptr = corresps.ptr<Vec4i>(); const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
@ -1024,10 +995,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
double w_sobelScale = w * sobelScaleIn; double w_sobelScale = w * sobelScaleIn;
const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0); const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
Point3f tp0; Point3f tp0 = rtmat * Point3f(p0[0], p0[1], p0[2]);
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]);
rgbdCoeffsFunc(transformType, rgbdCoeffsFunc(transformType,
A_ptr, A_ptr,
@ -1055,7 +1023,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& cloud1, const Mat& normals1, const Mat& cloud1, const Mat& normals1,
const Mat& corresps, const Mat& corresps,
Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType) Mat& AtA, Mat& AtB, OdometryTransformType transformType)
{ {
int transformDim = getTransformDim(transformType); int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0)); 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); const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
Point3f tp0; 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.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] * scale * Rt_ptr[4] + p0[1] * scale * Rt_ptr[5] + p0[2] * scale * Rt_ptr[6] + Rt_ptr[7]); 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] * scale * Rt_ptr[8] + p0[1] * scale * Rt_ptr[9] + p0[2] * scale * Rt_ptr[10] + Rt_ptr[11]); 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 n1 = normals1.at<Vec4f>(v1, u1);
Vec4f _v = cloud1.at<Vec4f>(v1, u1); Vec4f _v = cloud1.at<Vec4f>(v1, u1);

160
modules/3d/src/rgbd/odometry_functions.hpp
View File

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

33
modules/3d/test/test_odometry.cpp
View File

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