Chiebot-Mirror
/
opencv
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

moving code from ICP+Scale

Browse Source
pull/22150/head
Saratovtsev 3 years ago committed by Rostislav Vasilikhin
parent b06544bd54
commit df490c6399
7 changed files with 432 additions and 203 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 24
      modules/3d/include/opencv2/3d/odometry.hpp
  2. 31
      modules/3d/misc/python/test/test_odometry.py
  3. 93
      modules/3d/src/rgbd/odometry.cpp
  4. 205
      modules/3d/src/rgbd/odometry_functions.cpp
  5. 210
      modules/3d/src/rgbd/odometry_functions.hpp
  6. 55
      modules/3d/test/test_odometry.cpp
  7. 17
      samples/python/odometry.py

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

@ -16,7 +16,7 @@ namespace cv
/** These constants are used to set a type of data which odometry will use
* @param DEPTH only depth data
* @param RGB only rgb image
* @param RGB_DEPTH only depth and rgb data simultaneously
* @param RGB_DEPTH depth and rgb data simultaneously
*/
enum OdometryType
{
@ -26,8 +26,8 @@ enum OdometryType
};
/** These constants are used to set the speed and accuracy of odometry
* @param COMMON only accurate but not so fast
* @param FAST only less accurate but faster
* @param COMMON accurate but not so fast
* @param FAST less accurate but faster
*/
enum class OdometryAlgoType
{
@ -62,9 +62,9 @@ public:
*/
void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame);
/** Prepare frame for odometry calculation
/** Compute Rigid Transformation between two frames so that Rt * src = dst
* @param srcFrame src frame ("original" image)
* @param dstFrame dsr frame ("rotated" 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
@ -73,7 +73,21 @@ public:
*/
bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt);
/** Compute Rigid Transformation and scale between two frames so that Rt * (src * scale) = dst
* Works only on OdometryType::DEPTH and OdometryAlgoType::COMMON
* @param srcFrame src frame ("original" image)
* @param dstFrame dst frame ("rotated" image)
* @param Rt Rigid transformation, which will be calculated, in form:
* { R_11 R_12 R_13 t_1
* R_21 R_22 R_23 t_2
* R_31 R_32 R_33 t_3
* 0 0 0 1 }
* @param scale scale between srcFrame and dstFrame (use scale = 1 for input)
*/
bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale);
CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const;
CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, OutputArray scale) const;
CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const;
class Impl;

31
modules/3d/misc/python/test/test_odometry.py
Unescape View File

@ -108,5 +108,36 @@ class odometry_test(NewOpenCVTests):
self.assertLessEqual(res, eps)
self.assertTrue(isCorrect)
def test_OdometryScale(self):
depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32)
radian = np.radians(1)
Rt_warp = np.array(
[[np.cos(radian), -np.sin(radian), 0],
[np.sin(radian), np.cos(radian), 0],
[0, 0, 1]], dtype=np.float32
)
Rt_curr = np.array(
[[np.cos(radian), -np.sin(radian), 0, 0],
[np.sin(radian), np.cos(radian), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]], dtype=np.float32
)
Rt_res = np.zeros((4, 4))
scale = 1.01
scale_res = np.zeros((1, 1))
odometry = cv.Odometry()
warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480))
isCorrect = odometry.compute(depth, warped_depth*scale, Rt_res, scale_res)
Rt_diff = np.absolute(Rt_curr - Rt_res).sum()
scale_diff = np.absolute(scale - scale_res[0][0])
Rt_eps = 0.2
scale_eps = 0.1
self.assertLessEqual(Rt_diff, Rt_eps)
self.assertLessEqual(scale_diff, scale_eps)
self.assertTrue(isCorrect)
if __name__ == '__main__':
NewOpenCVTests.bootstrap()

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

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

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

@ -359,8 +359,8 @@ void preparePyramidImage(InputArray image, InputOutputArrayOfArrays pyramidImage
template<typename TMat>
void preparePyramidMask(InputArray mask, InputArrayOfArrays pyramidDepth, float minDepth, float maxDepth,
InputArrayOfArrays pyramidNormal,
InputOutputArrayOfArrays pyramidMask)
InputArrayOfArrays pyramidNormal,
InputOutputArrayOfArrays pyramidMask)
{
minDepth = std::max(0.f, minDepth);
@ -492,8 +492,8 @@ void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputO
}
void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy,
InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart,
InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale)
InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart,
InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale)
{
size_t didxLevels = pyramid_dI_dx.size(-1).width;
size_t texLevels = pyramidTexturedMask.size(-1).width;
@ -606,7 +606,7 @@ void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth,
}
void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart,
InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask)
InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask)
{
size_t maskLevels = pyramidMask.size(-1).width;
size_t norMaskLevels = pyramidNormalsMask.size(-1).width;
@ -648,39 +648,26 @@ void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask
}
}
bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
const OdometryFrame srcFrame,
const OdometryFrame dstFrame,
const Matx33f& cameraMatrix,
float maxDepthDiff, float angleThreshold, const std::vector<int>& iterCounts,
double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype)
OdometryType method, OdometryTransformType transformType, OdometryAlgoType algtype)
{
int transformDim = -1;
CalcRgbdEquationCoeffsPtr rgbdEquationFuncPtr = 0;
CalcICPEquationCoeffsPtr icpEquationFuncPtr = 0;
switch(transfromType)
if ((transformType == OdometryTransformType::RIGID_TRANSFORMATION) &&
(std::abs(scale) > std::numeric_limits<float>::epsilon()))
{
transformType = OdometryTransformType::SIM_TRANSFORMATION;
}
else
{
case OdometryTransformType::RIGID_TRANSFORMATION:
transformDim = 6;
rgbdEquationFuncPtr = calcRgbdEquationCoeffs;
icpEquationFuncPtr = calcICPEquationCoeffs;
break;
case OdometryTransformType::ROTATION:
transformDim = 3;
rgbdEquationFuncPtr = calcRgbdEquationCoeffsRotation;
icpEquationFuncPtr = calcICPEquationCoeffsRotation;
break;
case OdometryTransformType::TRANSLATION:
transformDim = 3;
rgbdEquationFuncPtr = calcRgbdEquationCoeffsTranslation;
icpEquationFuncPtr = calcICPEquationCoeffsTranslation;
break;
default:
CV_Error(Error::StsBadArg, "Incorrect transformation type");
scale = 1.0f;
}
int transformDim = getTransformDim(transformType);
const int minOverdetermScale = 20;
const int minCorrespsCount = minOverdetermScale * transformDim;
@ -689,6 +676,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
Mat currRt, ksi;
double currScale = 1.0;
Affine3f transform = Affine3f::Identity();
bool isOk = false;
@ -740,9 +728,9 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
const Mat pyramidNormalsMask;
dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level);
computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
Mat(), srcLevelDepth, pyramidMask,
Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff,
corresps_icp, diffs_icp, sigma_icp, OdometryType::DEPTH);
Mat(), srcLevelDepth, pyramidMask,
Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff,
corresps_icp, diffs_icp, sigma_icp, OdometryType::DEPTH);
}
}
@ -763,7 +751,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
dstFrame.getPyramidAt(dstPyrIdy, OdometryFramePyramidType::PYR_DIY, level);
calcRgbdLsmMatrices(srcPyrCloud, resultRt, dstPyrIdx, dstPyrIdy,
corresps_rgbd, diffs_rgbd, sigma_rgbd, fx, fy, sobelScale,
AtA_rgbd, AtB_rgbd, rgbdEquationFuncPtr, transformDim);
AtA_rgbd, AtB_rgbd, transformType);
AtA += AtA_rgbd;
AtB += AtB_rgbd;
}
@ -776,7 +764,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
if (algtype == OdometryAlgoType::COMMON)
{
calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals,
corresps_icp, AtA_icp, AtB_icp, icpEquationFuncPtr, transformDim);
corresps_icp, AtA_icp, AtB_icp, currScale, transformType);
}
else
{
@ -792,33 +780,63 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
AtB += AtB_icp;
}
// workaround for bad AtA matrix
if (transformType == OdometryTransformType::SIM_TRANSFORMATION)
if (countNonZero(AtA(Range::all(), Range(6, 7))) == 0)
{
Mat tmp(6, 6, CV_64FC1, Scalar(0));
AtA(Range(0, 6), Range(0, 6)).copyTo(tmp);
AtA = tmp;
transformType = OdometryTransformType::RIGID_TRANSFORMATION;
transformDim = getTransformDim(transformType);
break;
}
bool solutionExist = solveSystem(AtA, AtB, determinantThreshold, ksi);
if (!solutionExist)
{
break;
}
if(transfromType == OdometryTransformType::ROTATION)
Mat tmp61(6, 1, CV_64FC1, Scalar(0));
double newScale = 1.0;
if(transformType == OdometryTransformType::ROTATION)
{
ksi.copyTo(tmp61.rowRange(0,3));
ksi = tmp61;
}
else if(transformType == OdometryTransformType::TRANSLATION)
{
Mat tmp(6, 1, CV_64FC1, Scalar(0));
ksi.copyTo(tmp.rowRange(0,3));
ksi = tmp;
ksi.copyTo(tmp61.rowRange(3,6));
ksi = tmp61;
}
else if(transfromType == OdometryTransformType::TRANSLATION)
else if (transformType == OdometryTransformType::SIM_TRANSFORMATION)
{
Mat tmp(6, 1, CV_64FC1, Scalar(0));
ksi.copyTo(tmp.rowRange(3,6));
ksi = tmp;
newScale = ksi.at<double>(6, 0);
ksi.rowRange(0, 6).copyTo(tmp61);
ksi = tmp61;
}
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
Vec6f x(ksi);
Affine3f tinc(Vec3f(x.val), Vec3f(x.val + 3));
transform = tinc * transform;
isOk = true;
}
}
@ -826,7 +844,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
_Rt.create(resultRt.size(), resultRt.type());
Mat Rt = _Rt.getMat();
resultRt.copyTo(Rt);
scale =(float)currScale;
if(isOk)
{
Mat deltaRt;
@ -841,43 +859,44 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
return isOk;
}
// Rotate dst by Rt (which is inverted in fact) to get corresponding src pixels
// In RGB case compute sigma and diffs too
void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
const Mat& image0, const Mat& depth0, const Mat& validMask0,
const Mat& image1, const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method)
const Mat& imageSrc, const Mat& depthSrc, const Mat& validMaskSrc,
const Mat& imageDst, const Mat& depthDst, const Mat& selectMaskDst, float maxDepthDiff,
Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method)
{
CV_Assert(Rt.type() == CV_64FC1);
Mat corresps(depth1.size(), CV_16SC2, Scalar::all(-1));
Mat diffs(depth1.size(), CV_32F, Scalar::all(-1));
Mat corresps(depthDst.size(), CV_16SC2, Scalar::all(-1));
Mat diffs(depthDst.size(), CV_32F, Scalar::all(-1));
Matx33d K(_K), K_inv(_K_inv);
Rect r(0, 0, depth1.cols, depth1.rows);
Rect r(0, 0, depthDst.cols, depthDst.rows);
Mat Kt = Rt(Rect(3, 0, 1, 3)).clone();
Kt = K * Kt;
const double* Kt_ptr = Kt.ptr<const double>();
AutoBuffer<float> buf(3 * (depth1.cols + depth1.rows));
AutoBuffer<float> buf(3 * (depthDst.cols + depthDst.rows));
float* KRK_inv0_u1 = buf.data();
float* KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_u1 + depth1.cols;
float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depth1.rows;
float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depth1.cols;
float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depth1.rows;
float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depth1.cols;
float* KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_u1 + depthDst.cols;
float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depthDst.rows;
float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depthDst.cols;
float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depthDst.rows;
float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depthDst.cols;
{
Mat R = Rt(Rect(0, 0, 3, 3)).clone();
Mat KRK_inv = K * R * K_inv;
const double* KRK_inv_ptr = KRK_inv.ptr<const double>();
for (int u1 = 0; u1 < depth1.cols; u1++)
for (int u1 = 0; u1 < depthDst.cols; u1++)
{
KRK_inv0_u1[u1] = (float)(KRK_inv_ptr[0] * u1);
KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1);
KRK_inv6_u1[u1] = (float)(KRK_inv_ptr[6] * u1);
}
for (int v1 = 0; v1 < depth1.rows; v1++)
for (int v1 = 0; v1 < depthDst.rows; v1++)
{
KRK_inv1_v1_plus_KRK_inv2[v1] = (float)(KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]);
KRK_inv4_v1_plus_KRK_inv5[v1] = (float)(KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]);
@ -887,11 +906,11 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
double sigma = 0;
int correspCount = 0;
for (int v1 = 0; v1 < depth1.rows; v1++)
for (int v1 = 0; v1 < depthDst.rows; v1++)
{
const float* depth1_row = depth1.ptr<float>(v1);
const uchar* mask1_row = selectMask1.ptr<uchar>(v1);
for (int u1 = 0; u1 < depth1.cols; u1++)
const float* depth1_row = depthDst.ptr<float>(v1);
const uchar* mask1_row = selectMaskDst.ptr<uchar>(v1);
for (int u1 = 0; u1 < depthDst.cols; u1++)
{
float d1 = depth1_row[u1];
if (mask1_row[u1] && !cvIsNaN(d1))
@ -909,8 +928,8 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
Kt_ptr[1]));
if (r.contains(Point(u0, v0)))
{
float d0 = depth0.at<float>(v0, u0);
if (validMask0.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff)
float d0 = depthSrc.at<float>(v0, u0);
if (validMaskSrc.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff)
{
CV_DbgAssert(!cvIsNaN(d0));
Vec2s& c = corresps.at<Vec2s>(v0, u0);
@ -921,20 +940,20 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
diff = 0;
int exist_u1 = c[0], exist_v1 = c[1];
float exist_d1 = (float)(depth1.at<float>(exist_v1, exist_u1) *
(KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + Kt_ptr[2]);
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]);
if (transformed_d1 > exist_d1)
continue;
if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(image0.at<uchar>(v0, u0)) -
static_cast<int>(image1.at<uchar>(v1, u1)));
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(v0, u0)) -
static_cast<int>(imageDst.at<uchar>(v1, u1)));
}
else
{
if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(image0.at<uchar>(v0, u0)) -
static_cast<int>(image1.at<uchar>(v1, u1)));
diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(v0, u0)) -
static_cast<int>(imageDst.at<uchar>(v1, u1)));
correspCount++;
}
c = Vec2s((short)u1, (short)v1);
@ -973,16 +992,16 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
}
void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& dI_dx1, const Mat& dI_dy1,
const Mat& corresps, const Mat& _diffs, const double _sigma,
double fx, double fy, double sobelScaleIn,
Mat& AtA, Mat& AtB, CalcRgbdEquationCoeffsPtr func, int transformDim)
const Mat& dI_dx1, const Mat& dI_dy1,
const Mat& corresps, const Mat& _diffs, const double _sigma,
double fx, double fy, double sobelScaleIn,
Mat& AtA, Mat& AtB, OdometryTransformType transformType)
{
int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
double* AtB_ptr = AtB.ptr<double>();
CV_Assert(Rt.type() == CV_64FC1);
const double* Rt_ptr = Rt.ptr<const double>();
@ -1010,10 +1029,11 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
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]);
func(A_ptr,
w_sobelScale * dI_dx1.at<short int>(v1, u1),
w_sobelScale * dI_dy1.at<short int>(v1, u1),
tp0, fx, fy);
rgbdCoeffsFunc(transformType,
A_ptr,
w_sobelScale * dI_dx1.at<short int>(v1, u1),
w_sobelScale * dI_dy1.at<short int>(v1, u1),
tp0, fx, fy);
for (int y = 0; y < transformDim; y++)
{
@ -1031,11 +1051,13 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
AtA.at<double>(x, y) = AtA.at<double>(y, x);
}
void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& cloud1, const Mat& normals1,
const Mat& corresps,
Mat& AtA, Mat& AtB, CalcICPEquationCoeffsPtr func, int transformDim)
const Mat& cloud1, const Mat& normals1,
const Mat& corresps,
Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType)
{
int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
double* AtB_ptr = AtB.ptr<double>();
@ -1062,9 +1084,9 @@ void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
Point3f tp0;
tp0.x = (float)(p0[0] * Rt_ptr[0] + p0[1] * Rt_ptr[1] + p0[2] * Rt_ptr[2] + Rt_ptr[3]);
tp0.y = (float)(p0[0] * Rt_ptr[4] + p0[1] * Rt_ptr[5] + p0[2] * Rt_ptr[6] + Rt_ptr[7]);
tp0.z = (float)(p0[0] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]);
tp0.x = (float)(p0[0] * scale * Rt_ptr[0] + p0[1] * scale * Rt_ptr[1] + p0[2] * scale * Rt_ptr[2] + Rt_ptr[3]);
tp0.y = (float)(p0[0] * scale * Rt_ptr[4] + p0[1] * scale * Rt_ptr[5] + p0[2] * scale * Rt_ptr[6] + Rt_ptr[7]);
tp0.z = (float)(p0[0] * scale * Rt_ptr[8] + p0[1] * scale * Rt_ptr[9] + p0[2] * scale * Rt_ptr[10] + Rt_ptr[11]);
Vec4f n1 = normals1.at<Vec4f>(v1, u1);
Vec4f _v = cloud1.at<Vec4f>(v1, u1);
@ -1084,18 +1106,21 @@ void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Vec4i& c = corresps_ptr[correspIndex];
int u1 = c[2], v1 = c[3];
double w = sigma + std::abs(diffs_ptr[correspIndex]);
double w = sigma +std::abs(diffs_ptr[correspIndex]);
w = w > DBL_EPSILON ? 1. / w : 1.;
Vec4f n4 = normals1.at<Vec4f>(v1, u1);
func(A_ptr, tps0_ptr[correspIndex], Vec3f(n4[0], n4[1], n4[2]) * w);
Vec4f p1 = cloud1.at<Vec4f>(v1, u1);
icpCoeffsFunc(transformType,
A_ptr, tps0_ptr[correspIndex], Point3f(p1[0], p1[1], p1[2]), Vec3f(n4[0], n4[1], n4[2]) * w);
for (int y = 0; y < transformDim; y++)
{
double* AtA_ptr = AtA.ptr<double>(y);
for (int x = y; x < transformDim; x++)
{
AtA_ptr[x] += A_ptr[y] * A_ptr[x];
}
AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
}
}
@ -1112,7 +1137,7 @@ void computeProjectiveMatrix(const Mat& ksi, Mat& Rt)
const double* ksi_ptr = ksi.ptr<const double>();
// 0.5 multiplication is here because (dual) quaternions keep half an angle/twist inside
Matx44d matdq = (DualQuatd(0, ksi_ptr[0], ksi_ptr[1], ksi_ptr[2],
0, ksi_ptr[3], ksi_ptr[4], ksi_ptr[5]) * 0.5).exp().toMat(QUAT_ASSUME_UNIT);
0, ksi_ptr[3], ksi_ptr[4], ksi_ptr[5]) * 0.5).exp().toMat(QUAT_ASSUME_UNIT);
Mat(matdq).copyTo(Rt);
}

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

@ -12,9 +12,26 @@ namespace cv
{
enum class OdometryTransformType
{
ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4
// rotation, translation, rotation+translation, rotation*scale+translation
ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4, SIM_TRANSFORMATION = 8
};
static inline int getTransformDim(OdometryTransformType transformType)
{
switch(transformType)
{
case OdometryTransformType::SIM_TRANSFORMATION:
return 7;
case OdometryTransformType::RIGID_TRANSFORMATION:
return 6;
case OdometryTransformType::ROTATION:
case OdometryTransformType::TRANSLATION:
return 3;
default:
CV_Error(Error::StsBadArg, "Incorrect transformation type");
}
}
static inline
void checkImage(InputArray image)
{
@ -55,18 +72,39 @@ void checkNormals(InputArray normals, const Size& depthSize)
CV_Error(Error::StsBadSize, "Normals type has to be CV_32FC3.");
}
static inline
void calcRgbdScaleEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
double invz = 1. / p3d.z,
v0 = dIdx * fx * invz,
v1 = dIdy * fy * invz,
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
Point3d v(v0, v1, v2);
Point3d pxv = p3d.cross(v);
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
C[3] = v0;
C[4] = v1;
C[5] = v2;
//TODO: fixit
C[6] = 0;
}
static inline
void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
double invz = 1. / p3d.z,
v0 = dIdx * fx * invz,
v1 = dIdy * fy * invz,
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
C[0] = -p3d.z * v1 + p3d.y * v2;
C[1] = p3d.z * v0 - p3d.x * v2;
C[2] = -p3d.y * v0 + p3d.x * v1;
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;
@ -76,58 +114,125 @@ static inline
void calcRgbdEquationCoeffsRotation(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;
C[0] = -p3d.z * v1 + p3d.y * v2;
C[1] = p3d.z * v0 - p3d.x * v2;
C[2] = -p3d.y * v0 + p3d.x * v1;
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;
}
static inline
void calcRgbdEquationCoeffsTranslation(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;
v0 = dIdx * fx * invz,
v1 = dIdy * fy * invz,
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
C[0] = v0;
C[1] = v1;
C[2] = v2;
}
typedef
void (*CalcRgbdEquationCoeffsPtr)(double*, double, double, const Point3f&, double, double);
static inline void rgbdCoeffsFunc(OdometryTransformType transformType,
double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
{
switch(transformType)
{
case OdometryTransformType::SIM_TRANSFORMATION:
calcRgbdScaleEquationCoeffs(C, dIdx, dIdy, p3d, fx, fy);
break;
case OdometryTransformType::RIGID_TRANSFORMATION:
calcRgbdEquationCoeffs(C, dIdx, dIdy, p3d, fx, fy);
break;
case OdometryTransformType::ROTATION:
calcRgbdEquationCoeffsRotation(C, dIdx, dIdy, p3d, fx, fy);
break;
case OdometryTransformType::TRANSLATION:
calcRgbdEquationCoeffsTranslation(C, dIdx, dIdy, p3d, fx, fy);
break;
default:
CV_Error(Error::StsBadArg, "Incorrect transformation type");
}
}
static inline
void calcICPScaleEquationCoeffs(double* C, const Point3f& p0, const Point3f& p1, const Vec3f& n1)
{
Point3d pxv = p0.cross(Point3d(n1));
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
C[3] = n1[0];
C[4] = n1[1];
C[5] = n1[2];
double diff = n1.dot(p1 - p0);
//TODO: fixit
if (diff < DBL_EPSILON || abs(diff) > 1000000)
C[6] = 0;
else
//C[6] = n1.dot(p1-p0);
C[6] = -n1.dot(p0);
//C[6] = n1.dot((p0 - currentTranslation)/currentScale);
}
static inline
void calcICPEquationCoeffs(double* C, const Point3f& p0, const Vec3f& n1)
void calcICPEquationCoeffs(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1)
{
C[0] = -p0.z * n1[1] + p0.y * n1[2];
C[1] = p0.z * n1[0] - p0.x * n1[2];
C[2] = -p0.y * n1[0] + p0.x * n1[1];
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];
}
static inline
void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Vec3f& n1)
void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1)
{
C[0] = -p0.z * n1[1] + p0.y * n1[2];
C[1] = p0.z * n1[0] - p0.x * n1[2];
C[2] = -p0.y * n1[0] + p0.x * n1[1];
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 Vec3f& n1)
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];
}
typedef
void (*CalcICPEquationCoeffsPtr)(double*, const Point3f&, const Vec3f&);
static inline
void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3f& p0, const Point3f& p1, const Vec3f& n1)
{
switch(transformType)
{
case OdometryTransformType::SIM_TRANSFORMATION:
calcICPScaleEquationCoeffs(C, p0, p1, n1);
break;
case OdometryTransformType::RIGID_TRANSFORMATION:
calcICPEquationCoeffs(C, p0, p1, n1);;
break;
case OdometryTransformType::ROTATION:
calcICPEquationCoeffsRotation(C, p0, p1, n1);;
break;
case OdometryTransformType::TRANSLATION:
calcICPEquationCoeffsTranslation(C, p0, p1, n1);;
break;
default:
CV_Error(Error::StsBadArg, "Incorrect transformation type");
}
}
void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype);
void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, bool useDepth);
@ -159,47 +264,48 @@ template<typename TMat>
void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputOutputArrayOfArrays pyramidSobel, int sobelSize);
void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy,
InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart,
InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale);
InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart,
InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale);
void randomSubsetOfMask(InputOutputArray _mask, float part);
void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, InputOutputArrayOfArrays pyramidNormals);
void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart,
InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask);
InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask);
bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
const OdometryFrame srcFrame,
const OdometryFrame dstFrame,
const Matx33f& cameraMatrix,
float maxDepthDiff, float angleThreshold, const std::vector<int>& iterCounts,
double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype);
// scale = 0, if not needs scale; otherwise scale = 1;
bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt,
const OdometryFrame srcFrame,
const OdometryFrame dstFrame,
const Matx33f& cameraMatrix,
float maxDepthDiff, float angleThreshold, const std::vector<int>& iterCounts,
double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype);
void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
const Mat& image0, const Mat& depth0, const Mat& validMask0,
const Mat& image1, const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method);
const Mat& image0, const Mat& depth0, const Mat& validMask0,
const Mat& image1, const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method);
void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& dI_dx1, const Mat& dI_dy1,
const Mat& corresps, const Mat& diffs, const double sigma,
double fx, double fy, double sobelScaleIn,
Mat& AtA, Mat& AtB, CalcRgbdEquationCoeffsPtr func, int transformDim);
const Mat& dI_dx1, const Mat& dI_dy1,
const Mat& corresps, const Mat& diffs, const double sigma,
double fx, double fy, double sobelScaleIn,
Mat& AtA, Mat& AtB, OdometryTransformType transformType);
void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& cloud1, const Mat& normals1,
const Mat& corresps,
Mat& AtA, Mat& AtB, CalcICPEquationCoeffsPtr func, int transformDim);
const Mat& cloud1, const Mat& normals1,
const Mat& corresps,
Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType);
void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm,
cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b);
cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b);
#ifdef HAVE_OPENCL
bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm,
cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b);
cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b);
#endif
void computeProjectiveMatrix(const Mat& ksi, Mat& Rt);

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

@ -124,11 +124,13 @@ public:
OdometryAlgoType _algtype,
double _maxError1,
double _maxError5,
bool _testScale = false,
double _idError = DBL_EPSILON) :
otype(_otype),
algtype(_algtype),
maxError1(_maxError1),
maxError5(_maxError5),
testScale(_testScale),
idError(_idError)
{ }
@ -154,6 +156,7 @@ public:
OdometryAlgoType algtype;
double maxError1;
double maxError5;
bool testScale;
double idError;
};
@ -248,21 +251,27 @@ void OdometryTest::run()
odf.setImage(image);
odf.setDepth(depth);
Mat calcRt;
float scale = 1.0f;
// 1. Try to find Rt between the same frame (try masks also).
Mat mask(image.size(), CV_8UC1, Scalar(255));
odometry.prepareFrame(odf);
bool isComputed = odometry.compute(odf, odf, calcRt);
bool isComputed;
if (testScale)
isComputed = odometry.compute(odf, odf, calcRt, scale);
else
isComputed = odometry.compute(odf, odf, calcRt);
if(!isComputed)
{
FAIL() << "Can not find Rt between the same frame" << std::endl;
}
double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1));
if(ndiff > idError)
float sdiff = abs(scale - 1.f);
if (ndiff > idError && abs(scale - 1.f) < FLT_EPSILON)
{
FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << std::endl;
FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << " sdiff = " << sdiff << std::endl;
}
// 2. Generate random rigid body motion in some ranges several times (iterCount).
@ -279,23 +288,36 @@ void OdometryTest::run()
Mat rvec, tvec;
generateRandomTransformation(rvec, tvec);
Mat warpedImage, warpedDepth;
warpFrame(image, depth, rvec, tvec, K, warpedImage, warpedDepth);
Mat warpedImage, warpedDepth, scaledDepth;
float test_scale = 1.03f;
scaledDepth = testScale ? depth * test_scale : depth;
warpFrame(image, scaledDepth, rvec, tvec, K, warpedImage, warpedDepth);
dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
OdometryFrame odfSrc = odometry.createOdometryFrame();
OdometryFrame odfDst = odometry.createOdometryFrame();
float scale_error = 0.05f;
odfSrc.setImage(image);
odfSrc.setDepth(depth);
odfDst.setImage(warpedImage);
odfDst.setDepth(warpedDepth);
odometry.prepareFrames(odfSrc, odfDst);
isComputed = odometry.compute(odfSrc, odfDst, calcRt);
if (testScale)
isComputed = odometry.compute(odfSrc, odfDst, calcRt, scale);
else
isComputed = odometry.compute(odfSrc, odfDst, calcRt);
if (!isComputed)
{
CV_LOG_INFO(NULL, "Iter " << iter << "; Odometry compute returned false");
continue;
Mat calcR = calcRt(Rect(0,0,3,3)), calcRvec;
}
Mat calcR = calcRt(Rect(0, 0, 3, 3)), calcRvec;
cv::Rodrigues(calcR, calcRvec);
calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows);
Mat calcTvec = calcRt(Rect(3,0,1,3));
@ -312,6 +334,8 @@ void OdometryTest::run()
// compare rotation
double possibleError = algtype == OdometryAlgoType::COMMON ? 0.11f : 0.015f;
if (testScale)
possibleError = 0.2f;
Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec));
Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec));
@ -320,16 +344,15 @@ void OdometryTest::run()
double rdiffnorm = cv::norm(diff.rvec());
double tdiffnorm = cv::norm(diff.translation());
if (rdiffnorm < possibleError && tdiffnorm < possibleError)
{
if (rdiffnorm < possibleError && tdiffnorm < possibleError && abs(scale - test_scale) < scale_error)
better_1time_count++;
}
if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError)
if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError && abs(scale - test_scale) < scale_error)
better_5times_count++;
CV_LOG_INFO(NULL, "Iter " << iter);
CV_LOG_INFO(NULL, "rdiff: " << Vec3f(diff.rvec()) << "; rdiffnorm: " << rdiffnorm);
CV_LOG_INFO(NULL, "tdiff: " << Vec3f(diff.translation()) << "; tdiffnorm: " << tdiffnorm);
CV_LOG_INFO(NULL, "test_scale: " << test_scale << "; scale: " << scale);
CV_LOG_INFO(NULL, "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count);
}
@ -400,6 +423,12 @@ TEST(RGBD_Odometry_ICP, algorithmic)
test.run();
}
TEST(RGBD_Odometry_ICP_Scale, algorithmic)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.65, 0.0, true);
test.run();
}
TEST(RGBD_Odometry_RgbdICP, algorithmic)
{
OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
@ -408,7 +437,7 @@ TEST(RGBD_Odometry_RgbdICP, algorithmic)
TEST(RGBD_Odometry_FastICP, algorithmic)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON);
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON);
test.run();
}
@ -433,7 +462,7 @@ TEST(RGBD_Odometry_RgbdICP, UMats)
TEST(RGBD_Odometry_FastICP, UMats)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON);
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON);
test.checkUMats();
}

17
samples/python/odometry.py
Unescape View File

@ -13,6 +13,7 @@ def main():
help="""DEPTH - works with depth,
RGB - works with images,
RGB_DEPTH - works with all,
SCALE - works with depth and calculate Rt with scale,
default - runs all algos""",
default="")
parser.add_argument(
@ -34,7 +35,7 @@ def main():
args = parser.parse_args()
if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" or args.algo == "":
if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" or args.algo == "SCALE" or args.algo == "":
source_depth_frame = cv.samples.findFile(args.source_depth_frame)
destination_depth_frame = cv.samples.findFile(args.destination_depth_frame)
depth1 = cv.imread(source_depth_frame, cv.IMREAD_ANYDEPTH).astype(np.float32)
@ -50,18 +51,24 @@ def main():
odometry = cv.Odometry(cv.DEPTH)
Rt = np.zeros((4, 4))
odometry.compute(depth1, depth2, Rt)
print(Rt)
print("Rt:\n {}".format(Rt))
if args.algo == "RGB" or args.algo == "":
odometry = cv.Odometry(cv.RGB)
Rt = np.zeros((4, 4))
odometry.compute(rgb1, rgb2, Rt)
print(Rt)
print("Rt:\n {}".format(Rt))
if args.algo == "RGB_DEPTH" or args.algo == "":
odometry = cv.Odometry(cv.RGB_DEPTH)
Rt = np.zeros((4, 4))
odometry.compute(depth1, rgb1, depth2, rgb2, Rt)
print(Rt)
print("Rt:\n {}".format(Rt))
if args.algo == "SCALE" or args.algo == "":
print(args.algo)
odometry = cv.Odometry()
Rt = np.zeros((4, 4))
scale = np.zeros((1, 1))
odometry.compute(depth1, depth2*1.05, Rt, scale)
print("Rt:\n {}\nScale: {}".format(Rt, scale))
if __name__ == '__main__':

Write Preview
Loading…
Cancel
Save