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
Unified 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. 67
      modules/3d/src/rgbd/odometry.cpp
  4. 169
      modules/3d/src/rgbd/odometry_functions.cpp
  5. 152
      modules/3d/src/rgbd/odometry_functions.hpp
  6. 53
      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 /** These constants are used to set a type of data which odometry will use
* @param DEPTH only depth data * @param DEPTH only depth data
* @param RGB only rgb image * @param RGB only rgb image
* @param RGB_DEPTH only depth and rgb data simultaneously * @param RGB_DEPTH depth and rgb data simultaneously
*/ */
enum OdometryType enum OdometryType
{ {
@ -26,8 +26,8 @@ enum OdometryType
}; };
/** These constants are used to set the speed and accuracy of odometry /** These constants are used to set the speed and accuracy of odometry
* @param COMMON only accurate but not so fast * @param COMMON accurate but not so fast
* @param FAST only less accurate but faster * @param FAST less accurate but faster
*/ */
enum class OdometryAlgoType enum class OdometryAlgoType
{ {
@ -62,9 +62,9 @@ public:
*/ */
void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame); 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 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: * @param Rt Rigid transformation, which will be calculated, in form:
* { R_11 R_12 R_13 t_1 * { R_11 R_12 R_13 t_1
* R_21 R_22 R_23 t_2 * R_21 R_22 R_23 t_2
@ -73,7 +73,21 @@ 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;

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.assertLessEqual(res, eps)
self.assertTrue(isCorrect) 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__': if __name__ == '__main__':
NewOpenCVTests.bootstrap() NewOpenCVTests.bootstrap()

67
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) 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) const = 0; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) 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) 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) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) 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) const bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) 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, Mat(), srcFrame, dstFrame, cameraMatrix, bool isCorrect = RGBDICPOdometryImpl(Rt, scale, 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) const bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt, float& scale) const
{ {
OdometryFrame srcFrame = this->createOdometryFrame(); OdometryFrame srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = 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); prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype);
bool isCorrect = compute(srcFrame, dstFrame, Rt); bool isCorrect = compute(srcFrame, dstFrame, Rt, scale);
return isCorrect; return isCorrect;
} }
bool OdometryICP::compute(InputArray, InputArray, InputArray, InputArray, OutputArray) const 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 class OdometryRGB : public Odometry::Impl
@ -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) 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) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) 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) const bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const
{ {
Matx33f cameraMatrix; Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix); settings.getCameraMatrix(cameraMatrix);
@ -164,14 +164,15 @@ 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, Mat(), srcFrame, dstFrame, cameraMatrix, bool isCorrect = RGBDICPOdometryImpl(Rt, scale, 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(),
OdometryType::RGB, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); OdometryType::RGB, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype);
return isCorrect; 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 srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = this->createOdometryFrame(); OdometryFrame dstFrame = this->createOdometryFrame();
@ -180,7 +181,7 @@ 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); bool isCorrect = compute(srcFrame, dstFrame, Rt, scale);
return isCorrect; return isCorrect;
} }
@ -202,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) 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) const override; virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) 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;
}; };
@ -233,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) const bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const
{ {
Matx33f cameraMatrix; Matx33f cameraMatrix;
settings.getCameraMatrix(cameraMatrix); settings.getCameraMatrix(cameraMatrix);
@ -242,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, Mat(), srcFrame, dstFrame, cameraMatrix, bool isCorrect = RGBDICPOdometryImpl(Rt, scale, 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(),
@ -250,7 +251,7 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d
return isCorrect; 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"); CV_Error(cv::Error::StsBadFunc, "This volume needs depth and rgb data simultaneously");
} }
@ -266,8 +267,8 @@ 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;
} }
@ -345,15 +346,31 @@ 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)
{ {
//this->prepareFrames(srcFrame, dstFrame); 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, 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, bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const
{ {

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

@ -648,39 +648,26 @@ 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,
float maxDepthDiff, float angleThreshold, const std::vector<int>& iterCounts, float maxDepthDiff, float angleThreshold, const std::vector<int>& iterCounts,
double maxTranslation, double maxRotation, double sobelScale, double maxTranslation, double maxRotation, double sobelScale,
OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype) OdometryType method, OdometryTransformType transformType, OdometryAlgoType algtype)
{ {
int transformDim = -1; if ((transformType == OdometryTransformType::RIGID_TRANSFORMATION) &&
CalcRgbdEquationCoeffsPtr rgbdEquationFuncPtr = 0; (std::abs(scale) > std::numeric_limits<float>::epsilon()))
CalcICPEquationCoeffsPtr icpEquationFuncPtr = 0; {
switch(transfromType) transformType = OdometryTransformType::SIM_TRANSFORMATION;
{ }
case OdometryTransformType::RIGID_TRANSFORMATION: else
transformDim = 6; {
rgbdEquationFuncPtr = calcRgbdEquationCoeffs; scale = 1.0f;
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");
} }
int transformDim = getTransformDim(transformType);
const int minOverdetermScale = 20; const int minOverdetermScale = 20;
const int minCorrespsCount = minOverdetermScale * transformDim; 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 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;
@ -763,7 +751,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
dstFrame.getPyramidAt(dstPyrIdy, OdometryFramePyramidType::PYR_DIY, level); dstFrame.getPyramidAt(dstPyrIdy, OdometryFramePyramidType::PYR_DIY, level);
calcRgbdLsmMatrices(srcPyrCloud, resultRt, dstPyrIdx, dstPyrIdy, calcRgbdLsmMatrices(srcPyrCloud, resultRt, dstPyrIdx, dstPyrIdy,
corresps_rgbd, diffs_rgbd, sigma_rgbd, fx, fy, sobelScale, corresps_rgbd, diffs_rgbd, sigma_rgbd, fx, fy, sobelScale,
AtA_rgbd, AtB_rgbd, rgbdEquationFuncPtr, transformDim); AtA_rgbd, AtB_rgbd, transformType);
AtA += AtA_rgbd; AtA += AtA_rgbd;
AtB += AtB_rgbd; AtB += AtB_rgbd;
} }
@ -776,7 +764,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, 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, icpEquationFuncPtr, transformDim); corresps_icp, AtA_icp, AtB_icp, currScale, transformType);
} }
else else
{ {
@ -792,33 +780,63 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, 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)
{ {
break; break;
} }
if(transfromType == OdometryTransformType::ROTATION)
Mat tmp61(6, 1, CV_64FC1, Scalar(0));
double newScale = 1.0;
if(transformType == OdometryTransformType::ROTATION)
{ {
Mat tmp(6, 1, CV_64FC1, Scalar(0)); ksi.copyTo(tmp61.rowRange(0,3));
ksi.copyTo(tmp.rowRange(0,3)); ksi = tmp61;
ksi = tmp;
} }
else if(transfromType == OdometryTransformType::TRANSLATION) else if(transformType == OdometryTransformType::TRANSLATION)
{ {
Mat tmp(6, 1, CV_64FC1, Scalar(0)); ksi.copyTo(tmp61.rowRange(3,6));
ksi.copyTo(tmp.rowRange(3,6)); ksi = tmp61;
ksi = tmp; }
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
Vec6f x(ksi); Vec6f x(ksi);
Affine3f tinc(Vec3f(x.val), Vec3f(x.val + 3)); Affine3f tinc(Vec3f(x.val), Vec3f(x.val + 3));
transform = tinc * transform; transform = tinc * transform;
isOk = true; isOk = true;
} }
} }
@ -826,7 +844,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, 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;
@ -841,43 +859,44 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
return isOk; return isOk;
} }
// Rotate dst by Rt (which is inverted in fact) 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 Matx33f& _K_inv, const Mat& Rt,
const Mat& image0, const Mat& depth0, const Mat& validMask0, const Mat& imageSrc, const Mat& depthSrc, const Mat& validMaskSrc,
const Mat& image1, const Mat& depth1, const Mat& selectMask1, 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); CV_Assert(Rt.type() == CV_64FC1);
Mat corresps(depth1.size(), CV_16SC2, Scalar::all(-1)); Mat corresps(depthDst.size(), CV_16SC2, Scalar::all(-1));
Mat diffs(depth1.size(), CV_32F, Scalar::all(-1)); Mat diffs(depthDst.size(), CV_32F, Scalar::all(-1));
Matx33d K(_K), K_inv(_K_inv); 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(); Mat Kt = Rt(Rect(3, 0, 1, 3)).clone();
Kt = K * Kt; Kt = K * Kt;
const double* Kt_ptr = Kt.ptr<const double>(); 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_inv0_u1 = buf.data();
float* KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_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 + depth1.rows; float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depthDst.rows;
float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depth1.cols; float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depthDst.cols;
float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depth1.rows; float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depthDst.rows;
float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depth1.cols; float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depthDst.cols;
{ {
Mat R = Rt(Rect(0, 0, 3, 3)).clone(); Mat R = Rt(Rect(0, 0, 3, 3)).clone();
Mat KRK_inv = K * R * K_inv; Mat KRK_inv = K * R * K_inv;
const double* KRK_inv_ptr = KRK_inv.ptr<const double>(); 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_inv0_u1[u1] = (float)(KRK_inv_ptr[0] * u1);
KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1); KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1);
KRK_inv6_u1[u1] = (float)(KRK_inv_ptr[6] * 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_inv1_v1_plus_KRK_inv2[v1] = (float)(KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]);
KRK_inv4_v1_plus_KRK_inv5[v1] = (float)(KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]); KRK_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; double sigma = 0;
int correspCount = 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 float* depth1_row = depthDst.ptr<float>(v1);
const uchar* mask1_row = selectMask1.ptr<uchar>(v1); const uchar* mask1_row = selectMaskDst.ptr<uchar>(v1);
for (int u1 = 0; u1 < depth1.cols; u1++) for (int u1 = 0; u1 < depthDst.cols; u1++)
{ {
float d1 = depth1_row[u1]; float d1 = depth1_row[u1];
if (mask1_row[u1] && !cvIsNaN(d1)) 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])); Kt_ptr[1]));
if (r.contains(Point(u0, v0))) if (r.contains(Point(u0, v0)))
{ {
float d0 = depth0.at<float>(v0, u0); float d0 = depthSrc.at<float>(v0, u0);
if (validMask0.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff) if (validMaskSrc.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff)
{ {
CV_DbgAssert(!cvIsNaN(d0)); CV_DbgAssert(!cvIsNaN(d0));
Vec2s& c = corresps.at<Vec2s>(v0, u0); 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; diff = 0;
int exist_u1 = c[0], exist_v1 = c[1]; int exist_u1 = c[0], exist_v1 = c[1];
float exist_d1 = (float)(depth1.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]) + Kt_ptr[2]);
if (transformed_d1 > exist_d1) if (transformed_d1 > exist_d1)
continue; continue;
if (method == OdometryType::RGB) if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(image0.at<uchar>(v0, u0)) - diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(v0, u0)) -
static_cast<int>(image1.at<uchar>(v1, u1))); static_cast<int>(imageDst.at<uchar>(v1, u1)));
} }
else else
{ {
if (method == OdometryType::RGB) if (method == OdometryType::RGB)
diff = static_cast<float>(static_cast<int>(image0.at<uchar>(v0, u0)) - diff = static_cast<float>(static_cast<int>(imageSrc.at<uchar>(v0, u0)) -
static_cast<int>(image1.at<uchar>(v1, u1))); static_cast<int>(imageDst.at<uchar>(v1, u1)));
correspCount++; correspCount++;
} }
c = Vec2s((short)u1, (short)v1); c = Vec2s((short)u1, (short)v1);
@ -976,13 +995,13 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& dI_dx1, const Mat& dI_dy1, const Mat& dI_dx1, const Mat& dI_dy1,
const Mat& corresps, const Mat& _diffs, const double _sigma, const Mat& corresps, const Mat& _diffs, const double _sigma,
double fx, double fy, double sobelScaleIn, double fx, double fy, double sobelScaleIn,
Mat& AtA, Mat& AtB, CalcRgbdEquationCoeffsPtr func, int transformDim) Mat& AtA, Mat& AtB, OdometryTransformType transformType)
{ {
int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0)); AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0)); AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
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>(); const double* Rt_ptr = Rt.ptr<const double>();
@ -1010,7 +1029,8 @@ 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.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.z = (float)(p0[0] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]);
func(A_ptr, rgbdCoeffsFunc(transformType,
A_ptr,
w_sobelScale * dI_dx1.at<short int>(v1, u1), w_sobelScale * dI_dx1.at<short int>(v1, u1),
w_sobelScale * dI_dy1.at<short int>(v1, u1), w_sobelScale * dI_dy1.at<short int>(v1, u1),
tp0, fx, fy); tp0, fx, fy);
@ -1031,11 +1051,13 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
AtA.at<double>(x, y) = AtA.at<double>(y, x); AtA.at<double>(x, y) = AtA.at<double>(y, x);
} }
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, CalcICPEquationCoeffsPtr func, int transformDim) Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType)
{ {
int transformDim = getTransformDim(transformType);
AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0)); AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0)); AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
double* AtB_ptr = AtB.ptr<double>(); 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); const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
Point3f tp0; Point3f tp0;
tp0.x = (float)(p0[0] * Rt_ptr[0] + p0[1] * Rt_ptr[1] + p0[2] * Rt_ptr[2] + Rt_ptr[3]); 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] * Rt_ptr[4] + p0[1] * Rt_ptr[5] + p0[2] * Rt_ptr[6] + Rt_ptr[7]); 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] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]); 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 n1 = normals1.at<Vec4f>(v1, u1);
Vec4f _v = cloud1.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]; const Vec4i& c = corresps_ptr[correspIndex];
int u1 = c[2], v1 = c[3]; 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.; w = w > DBL_EPSILON ? 1. / w : 1.;
Vec4f n4 = normals1.at<Vec4f>(v1, u1); 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++) for (int y = 0; y < transformDim; y++)
{ {
double* AtA_ptr = AtA.ptr<double>(y); double* AtA_ptr = AtA.ptr<double>(y);
for (int x = y; x < transformDim; x++) for (int x = y; x < transformDim; x++)
{
AtA_ptr[x] += A_ptr[y] * A_ptr[x]; AtA_ptr[x] += A_ptr[y] * A_ptr[x];
}
AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex]; AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
} }
} }

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

@ -12,9 +12,26 @@ namespace cv
{ {
enum class OdometryTransformType 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 static inline
void checkImage(InputArray image) void checkImage(InputArray image)
{ {
@ -55,6 +72,25 @@ 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) void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy)
@ -63,10 +99,12 @@ void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f&
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;
Point3d v(v0, v1, v2);
Point3d pxv = p3d.cross(v);
C[0] = -p3d.z * v1 + p3d.y * v2; C[0] = pxv.x;
C[1] = p3d.z * v0 - p3d.x * v2; C[1] = pxv.y;
C[2] = -p3d.y * v0 + p3d.x * v1; C[2] = pxv.z;
C[3] = v0; C[3] = v0;
C[4] = v1; C[4] = v1;
C[5] = v2; C[5] = v2;
@ -79,9 +117,13 @@ void calcRgbdEquationCoeffsRotation(double* C, double dIdx, double dIdy, const P
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] = -p3d.z * v1 + p3d.y * v2;
C[1] = p3d.z * v0 - p3d.x * v2; Point3d v(v0, v1, v2);
C[2] = -p3d.y * v0 + p3d.x * v1; Point3d pxv = p3d.cross(v);
C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
} }
static inline static inline
@ -96,38 +138,101 @@ void calcRgbdEquationCoeffsTranslation(double* C, double dIdx, double dIdy, cons
C[2] = v2; C[2] = v2;
} }
typedef static inline void rgbdCoeffsFunc(OdometryTransformType transformType,
void (*CalcRgbdEquationCoeffsPtr)(double*, double, double, const Point3f&, double, double); 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 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]; Point3d pxv = p0.cross(Point3d(n1));
C[1] = p0.z * n1[0] - p0.x * n1[2];
C[2] = -p0.y * n1[0] + p0.x * n1[1]; C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
C[3] = n1[0]; C[3] = n1[0];
C[4] = n1[1]; C[4] = n1[1];
C[5] = n1[2]; C[5] = n1[2];
} }
static inline 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]; Point3d pxv = p0.cross(Point3d(n1));
C[1] = p0.z * n1[0] - p0.x * n1[2];
C[2] = -p0.y * n1[0] + p0.x * n1[1]; C[0] = pxv.x;
C[1] = pxv.y;
C[2] = pxv.z;
} }
static inline 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[0] = n1[0];
C[1] = n1[1]; C[1] = n1[1];
C[2] = n1[2]; C[2] = n1[2];
} }
typedef static inline
void (*CalcICPEquationCoeffsPtr)(double*, const Point3f&, const Vec3f&); 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 prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype);
void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, bool useDepth); void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, bool useDepth);
@ -170,7 +275,8 @@ void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask
InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask); InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask);
bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, // scale = 0, if not needs scale; otherwise scale = 1;
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,
@ -187,12 +293,12 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
const Mat& dI_dx1, const Mat& dI_dy1, const Mat& dI_dx1, const Mat& dI_dy1,
const Mat& corresps, const Mat& diffs, const double sigma, const Mat& corresps, const Mat& diffs, const double sigma,
double fx, double fy, double sobelScaleIn, double fx, double fy, double sobelScaleIn,
Mat& AtA, Mat& AtB, CalcRgbdEquationCoeffsPtr func, int transformDim); Mat& AtA, Mat& AtB, OdometryTransformType transformType);
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, CalcICPEquationCoeffsPtr func, int transformDim); Mat& AtA, Mat& AtB, double& scale, 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);

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

@ -124,11 +124,13 @@ public:
OdometryAlgoType _algtype, OdometryAlgoType _algtype,
double _maxError1, double _maxError1,
double _maxError5, double _maxError5,
bool _testScale = false,
double _idError = DBL_EPSILON) : double _idError = DBL_EPSILON) :
otype(_otype), otype(_otype),
algtype(_algtype), algtype(_algtype),
maxError1(_maxError1), maxError1(_maxError1),
maxError5(_maxError5), maxError5(_maxError5),
testScale(_testScale),
idError(_idError) idError(_idError)
{ } { }
@ -154,6 +156,7 @@ public:
OdometryAlgoType algtype; OdometryAlgoType algtype;
double maxError1; double maxError1;
double maxError5; double maxError5;
bool testScale;
double idError; double idError;
}; };
@ -248,21 +251,27 @@ 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 = 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) if(!isComputed)
{ {
FAIL() << "Can not find Rt between the same frame" << std::endl; FAIL() << "Can not find Rt between the same frame" << std::endl;
} }
double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1)); 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). // 2. Generate random rigid body motion in some ranges several times (iterCount).
@ -279,23 +288,36 @@ void OdometryTest::run()
Mat rvec, tvec; Mat rvec, tvec;
generateRandomTransformation(rvec, tvec); generateRandomTransformation(rvec, tvec);
Mat warpedImage, warpedDepth; Mat warpedImage, warpedDepth, scaledDepth;
warpFrame(image, depth, rvec, tvec, K, warpedImage, warpedDepth);
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 dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
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, scale);
else
isComputed = odometry.compute(odfSrc, odfDst, calcRt); isComputed = odometry.compute(odfSrc, odfDst, calcRt);
if (!isComputed) if (!isComputed)
{
CV_LOG_INFO(NULL, "Iter " << iter << "; Odometry compute returned false");
continue; continue;
Mat calcR = calcRt(Rect(0,0,3,3)), calcRvec; }
Mat calcR = calcRt(Rect(0, 0, 3, 3)), calcRvec;
cv::Rodrigues(calcR, calcRvec); cv::Rodrigues(calcR, calcRvec);
calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows); calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows);
Mat calcTvec = calcRt(Rect(3,0,1,3)); Mat calcTvec = calcRt(Rect(3,0,1,3));
@ -312,6 +334,8 @@ 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));
@ -320,16 +344,15 @@ 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) if (rdiffnorm < possibleError && tdiffnorm < possibleError && abs(scale - test_scale) < scale_error)
{
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);
} }
@ -400,6 +423,12 @@ 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);
@ -408,7 +437,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, FLT_EPSILON); OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON);
test.run(); test.run();
} }
@ -433,7 +462,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, FLT_EPSILON); OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON);
test.checkUMats(); test.checkUMats();
} }

17
samples/python/odometry.py
Unescape View File

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

Write Preview
Loading…
Cancel
Save