mirror of
https://github.com/opencv/opencv.git
synced 2024-12-01 06:49:50 +08:00
moving code from ICP+Scale
This commit is contained in:
Saratovtsev
Rostislav Vasilikhin
parent
b06544bd54
commit
df490c6399
@ -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;
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
return this->impl->compute(srcFrame, dstFrame, Rt);
|
||||
float scale = 0.f;
|
||||
return this->impl->compute(srcFrame, dstFrame, Rt, scale);
|
||||
}
|
||||
|
||||
bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, OutputArray _scale) const
|
||||
{
|
||||
_scale.create(Size(1, 1), CV_64FC1);
|
||||
Mat scaleValue = _scale.getMat();
|
||||
float scale = 1.f;
|
||||
bool res = this->impl->compute(srcFrame, dstFrame, Rt, scale);
|
||||
Mat(1, 1, CV_64FC1, Scalar(scale)).copyTo(scaleValue);
|
||||
return res;
|
||||
}
|
||||
|
||||
bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
|
||||
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const
|
||||
{
|
||||
|
||||
@ -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,38 +648,25 @@ 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()))
|
||||
{
|
||||
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");
|
||||
transformType = OdometryTransformType::SIM_TRANSFORMATION;
|
||||
}
|
||||
else
|
||||
{
|
||||
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 tmp(6, 1, CV_64FC1, Scalar(0));
|
||||
ksi.copyTo(tmp.rowRange(0,3));
|
||||
ksi = tmp;
|
||||
|
||||
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(transfromType == OdometryTransformType::TRANSLATION)
|
||||
else if(transformType == OdometryTransformType::TRANSLATION)
|
||||
{
|
||||
Mat tmp(6, 1, CV_64FC1, Scalar(0));
|
||||
ksi.copyTo(tmp.rowRange(3,6));
|
||||
ksi = tmp;
|
||||
ksi.copyTo(tmp61.rowRange(3,6));
|
||||
ksi = tmp61;
|
||||
}
|
||||
else if (transformType == OdometryTransformType::SIM_TRANSFORMATION)
|
||||
{
|
||||
newScale = ksi.at<double>(6, 0);
|
||||
ksi.rowRange(0, 6).copyTo(tmp61);
|
||||
ksi = tmp61;
|
||||
}
|
||||
|
||||
computeProjectiveMatrix(ksi, currRt);
|
||||
resultRt = currRt * resultRt;
|
||||
//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);
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
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] = -p3d.z * v1 + p3d.y * v2;
|
||||
C[1] = p3d.z * v0 - p3d.x * v2;
|
||||
C[2] = -p3d.y * v0 + p3d.x * v1;
|
||||
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 calcICPEquationCoeffs(double* C, const Point3f& p0, const Vec3f& n1)
|
||||
void calcICPScaleEquationCoeffs(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];
|
||||
double diff = n1.dot(p1 - p0);
|
||||
//TODO: fixit
|
||||
if (diff < DBL_EPSILON || abs(diff) > 1000000)
|
||||
C[6] = 0;
|
||||
else
|
||||
//C[6] = n1.dot(p1-p0);
|
||||
C[6] = -n1.dot(p0);
|
||||
//C[6] = n1.dot((p0 - currentTranslation)/currentScale);
|
||||
}
|
||||
|
||||
static inline
|
||||
void calcICPEquationCoeffs(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1)
|
||||
{
|
||||
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);
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
|
||||
@ -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__':
|
||||
|
||||
Loading…
Reference in New Issue
Block a user