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
Unified 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. 216
      modules/3d/src/rgbd/odometry_functions.cpp
  4. 146
      modules/3d/src/rgbd/odometry_functions.hpp
  5. 33
      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); 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
Unescape 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

216
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 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));
// 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); 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))
if (maskDst_row[udst] && !cvIsNaN(ddst))
{ {
//CV_DbgAssert(!cvIsNaN(d1)); float transformed_ddst = static_cast<float>(ddst * (KRK_inv6_u1[udst] + KRK_inv7_v1_plus_KRK_inv8[vdst]) + ktinv.z);
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 (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);

146
modules/3d/src/rgbd/odometry_functions.hpp
Unescape 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,28 +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
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 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, double invz = 1. / p3d.z,
v0 = dIdx * fx * invz, v0 = dIdx * fx * invz,
@ -102,16 +82,11 @@ void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f&
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;
} }
static inline 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, 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
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 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)); Point3d pxv = psrc.cross(Point3d(ndst));
C[0] = pxv.x; return Vec6d(pxv.x, pxv.y, pxv.z, ndst[0], ndst[1], ndst[2]);
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) 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;
} }
static inline 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]; return Vec3d(ndst);
C[1] = n1[1];
C[2] = n1[2];
} }
static inline 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) 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
Unescape 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();
} }

Write Preview
Loading…
Cancel
Save