potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-29 15:23:05 +04:00
Code Packages Releases Wiki Activity

Merge pull request #21439 from DumDereDum:python_odometry

Browse Source 
Odometry python support
This commit is contained in:
Artem Saratovtsev
2022-02-07 19:16:17 +03:00
committed by GitHub
parent 0a0714bd1b
commit 4ba2b05df8
9 changed files with 330 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/3d/include/opencv2/3d/odometry.hpp
+7 -3
View File
@@ -18,7 +18,7 @@ namespace cv
* @param RGB only rgb image
* @param RGB_DEPTH only depth and rgb data simultaneously
*/
enum class OdometryType
enum OdometryType
{
DEPTH = 0,
RGB = 1,
@@ -32,13 +32,14 @@ enum class OdometryType
enum class OdometryAlgoType
{
COMMON = 0,
FAST = 1
FAST = 1
};
class CV_EXPORTS_W Odometry
{
public:
Odometry();
CV_WRAP Odometry();
CV_WRAP Odometry(OdometryType otype);
Odometry(OdometryType otype, const OdometrySettings settings, OdometryAlgoType algtype);
~Odometry();
@@ -72,6 +73,9 @@ public:
*/
bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt);
CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const;
CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const;
class Impl;
private:
Ptr<Impl> impl;
modules/3d/misc/python/test/test_odometry.py
+112
View File
@@ -0,0 +1,112 @@
#!/usr/bin/env python
import numpy as np
import cv2 as cv
from tests_common import NewOpenCVTests
class odometry_test(NewOpenCVTests):
def test_OdometryDefault(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))
odometry = cv.Odometry()
warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480))
isCorrect = odometry.compute(depth, warped_depth, Rt_res)
res = np.absolute(Rt_curr - Rt_res).sum()
eps = 0.15
self.assertLessEqual(res, eps)
self.assertTrue(isCorrect)
def test_OdometryDepth(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))
odometry = cv.Odometry(cv.DEPTH)
warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480))
isCorrect = odometry.compute(depth, warped_depth, Rt_res)
res = np.absolute(Rt_curr - Rt_res).sum()
eps = 0.15
self.assertLessEqual(res, eps)
self.assertTrue(isCorrect)
def test_OdometryRGB(self):
rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR)
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))
odometry = cv.Odometry(cv.RGB)
warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480))
isCorrect = odometry.compute(rgb, warped_rgb, Rt_res)
res = np.absolute(Rt_curr - Rt_res).sum()
eps = 0.15
self.assertLessEqual(res, eps)
self.assertTrue(isCorrect)
def test_OdometryRGB_Depth(self):
depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32)
rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR)
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))
odometry = cv.Odometry(cv.RGB_DEPTH)
warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480))
warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480))
isCorrect = odometry.compute(depth, rgb, warped_depth, warped_rgb, Rt_res)
res = np.absolute(Rt_curr - Rt_res).sum()
eps = 0.15
self.assertLessEqual(res, eps)
self.assertTrue(isCorrect)
if __name__ == '__main__':
NewOpenCVTests.bootstrap()
modules/3d/src/rgbd/odometry.cpp
+99 -2
View File
@@ -21,6 +21,9 @@ public:
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(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const = 0;
};
@@ -38,6 +41,9 @@ public:
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(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override;
};
OdometryICP::OdometryICP(OdometrySettings _settings, OdometryAlgoType _algtype)
@@ -86,6 +92,23 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
return isCorrect;
}
bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const
{
OdometryFrame srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = this->createOdometryFrame();
srcFrame.setDepth(_srcFrame);
dstFrame.setDepth(_dstFrame);
prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype);
bool isCorrect = compute(srcFrame, dstFrame, Rt);
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");
}
class OdometryRGB : public Odometry::Impl
{
@@ -101,6 +124,9 @@ public:
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(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override;
};
OdometryRGB::OdometryRGB(OdometrySettings _settings, OdometryAlgoType _algtype)
@@ -120,12 +146,12 @@ OdometryFrame OdometryRGB::createOdometryFrame() const
void OdometryRGB::prepareFrame(OdometryFrame& frame)
{
prepareRGBFrame(frame, frame, this->settings);
prepareRGBFrame(frame, frame, this->settings, false);
}
void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame)
{
prepareRGBFrame(srcFrame, dstFrame, this->settings);
prepareRGBFrame(srcFrame, dstFrame, this->settings, false);
}
bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
@@ -145,7 +171,23 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds
OdometryType::RGB, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype);
return isCorrect;
}
bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const
{
OdometryFrame srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = this->createOdometryFrame();
srcFrame.setImage(_srcFrame);
dstFrame.setImage(_dstFrame);
prepareRGBFrame(srcFrame, dstFrame, this->settings, false);
bool isCorrect = compute(srcFrame, dstFrame, Rt);
return isCorrect;
}
bool OdometryRGB::compute(InputArray, InputArray, InputArray, InputArray, OutputArray) const
{
CV_Error(cv::Error::StsBadFunc, "This volume does not work with depth and rgb data simultaneously");
}
class OdometryRGBD : public Odometry::Impl
{
@@ -161,6 +203,9 @@ public:
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(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override;
};
OdometryRGBD::OdometryRGBD(OdometrySettings _settings, OdometryAlgoType _algtype)
@@ -205,6 +250,26 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d
return isCorrect;
}
bool OdometryRGBD::compute(InputArray, InputArray, OutputArray) 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
{
OdometryFrame srcFrame = this->createOdometryFrame();
OdometryFrame dstFrame = this->createOdometryFrame();
srcFrame.setDepth(_srcDepthFrame);
srcFrame.setImage(_srcRGBFrame);
dstFrame.setDepth(_dstDepthFrame);
dstFrame.setImage(_dstRGBFrame);
prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype);
bool isCorrect = compute(srcFrame, dstFrame, Rt);
return isCorrect;
}
Odometry::Odometry()
{
@@ -212,6 +277,27 @@ Odometry::Odometry()
this->impl = makePtr<OdometryICP>(settings, OdometryAlgoType::COMMON);
}
Odometry::Odometry(OdometryType otype)
{
OdometrySettings settings;
switch (otype)
{
case OdometryType::DEPTH:
this->impl = makePtr<OdometryICP>(settings, OdometryAlgoType::FAST);
break;
case OdometryType::RGB:
this->impl = makePtr<OdometryRGB>(settings, OdometryAlgoType::COMMON);
break;
case OdometryType::RGB_DEPTH:
this->impl = makePtr<OdometryRGBD>(settings, OdometryAlgoType::COMMON);
break;
default:
CV_Error(Error::StsInternal,
"Incorrect OdometryType, you are able to use only { DEPTH = 0, RGB = 1, RGB_DEPTH = 2 }");
break;
}
}
Odometry::Odometry(OdometryType otype, OdometrySettings settings, OdometryAlgoType algtype)
{
switch (otype)
@@ -263,6 +349,17 @@ bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFr
return this->impl->compute(srcFrame, dstFrame, Rt);
}
bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const
{
return this->impl->compute(srcFrame, dstFrame, Rt);
}
bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const
{
return this->impl->compute(srcDepthFrame, srcRGBFrame, dstDepthFrame, dstRGBFrame, Rt);
}
template<class ImageElemType>
static void
modules/3d/src/rgbd/odometry_frame_impl.cpp
+6 -5
View File
@@ -238,12 +238,13 @@ void OdometryFrameImplTMat<TMat>::getPyramidAt(OutputArray _img, OdometryFramePy
template<typename TMat>
void OdometryFrameImplTMat<TMat>::findMask(InputArray _depth)
{
Mat d = _depth.getMat();
Mat m(d.size(), CV_8UC1, Scalar(255));
for (int y = 0; y < d.rows; y++)
for (int x = 0; x < d.cols; x++)
Mat depth_value = _depth.getMat();
CV_Assert(depth_value.type() == DEPTH_TYPE);
Mat m(depth_value.size(), CV_8UC1, Scalar(255));
for (int y = 0; y < depth_value.rows; y++)
for (int x = 0; x < depth_value.cols; x++)
{
if (cvIsNaN(d.at<float>(y, x)) || d.at<float>(y, x) <= FLT_EPSILON)
if (cvIsNaN(depth_value.at<float>(y, x)) || depth_value.at<float>(y, x) <= FLT_EPSILON)
m.at<uchar>(y, x) = 0;
}
this->setMask(m);
modules/3d/src/rgbd/odometry_functions.cpp
+28 -25
View File
@@ -24,14 +24,14 @@ enum
void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, OdometryAlgoType algtype)
{
prepareRGBFrame(srcFrame, dstFrame, settings);
prepareRGBFrame(srcFrame, dstFrame, settings, true);
prepareICPFrame(srcFrame, dstFrame, settings, algtype);
}
void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings)
void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, bool useDepth)
{
prepareRGBFrameBase(srcFrame, settings);
prepareRGBFrameBase(dstFrame, settings);
prepareRGBFrameBase(srcFrame, settings, useDepth);
prepareRGBFrameBase(dstFrame, settings, useDepth);
prepareRGBFrameSrc(srcFrame, settings);
prepareRGBFrameDst(dstFrame, settings);
@@ -48,7 +48,7 @@ void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometryS
prepareICPFrameDst(dstFrame, settings);
}
void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings)
void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool useDepth)
{
// Can be transformed into template argument in the future
// when this algorithm supports OCL UMats too
@@ -70,29 +70,33 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings)
}
checkImage(image);
TMat depth;
frame.getDepth(depth);
if (depth.empty())
if (useDepth)
{
if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
frame.getDepth(depth);
if (depth.empty())
{
TMat pyr0;
frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0);
frame.setDepth(pyr0);
if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
{
TMat pyr0;
frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0);
frame.setDepth(pyr0);
}
else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0)
{
TMat cloud;
frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0);
std::vector<TMat> xyz;
split(cloud, xyz);
frame.setDepth(xyz[2]);
}
else
CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
}
else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0)
{
TMat cloud;
frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0);
std::vector<TMat> xyz;
split(cloud, xyz);
frame.setDepth(xyz[2]);
}
else
CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
checkDepth(depth, image.size());
}
checkDepth(depth, image.size());
else
depth = TMat(image.size(), CV_32F, 1);
TMat mask;
frame.getMask(mask);
@@ -120,8 +124,7 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings)
std::vector<TMat> mpyramids;
std::vector<TMat> npyramids;
preparePyramidMask<TMat>(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(),
npyramids, mpyramids);
preparePyramidMask<TMat>(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(), npyramids, mpyramids);
setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
}
modules/3d/src/rgbd/odometry_functions.hpp
+2 -2
View File
@@ -130,10 +130,10 @@ typedef
void (*CalcICPEquationCoeffsPtr)(double*, const Point3f&, const Vec3f&);
void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype);
void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings);
void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, bool useDepth);
void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype);
void prepareRGBFrameBase(OdometryFrame& frame, const OdometrySettings settings);
void prepareRGBFrameBase(OdometryFrame& frame, const OdometrySettings settings, bool useDepth);
void prepareRGBFrameSrc (OdometryFrame& frame, const OdometrySettings settings);
void prepareRGBFrameDst (OdometryFrame& frame, const OdometrySettings settings);
modules/3d/src/rgbd/odometry_settings_impl.cpp
+7 -4
View File
@@ -108,10 +108,13 @@ private:
public:
class DefaultSets {
public:
const cv::Matx33f defaultCameraMatrix =
{ /* fx, 0, cx*/ 0, 0, 0,
/* 0, fy, cy*/ 0, 0, 0,
/* 0, 0, 0*/ 0, 0, 0 };
static const int width = 640;
static const int height = 480;
static constexpr float fx = 525.f;
static constexpr float fy = 525.f;
static constexpr float cx = float(width) / 2.f - 0.5f;
static constexpr float cy = float(height) / 2.f - 0.5f;
const cv::Matx33f defaultCameraMatrix = { fx, 0, cx, 0, fy, cy, 0, 0, 1 };
const std::vector<int> defaultIterCounts = { 7, 7, 7, 10 };
static constexpr float defaultMinDepth = 0.f;
modules/3d/test/test_odometry.cpp
+1 -1
View File
@@ -310,7 +310,7 @@ void OdometryTest::run()
}
// compare rotation
double possibleError = algtype == OdometryAlgoType::COMMON ? 0.09f : 0.015f;
double possibleError = algtype == OdometryAlgoType::COMMON ? 0.11f : 0.015f;
Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec));
Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec));
samples/python/odometry.py
+68
View File
@@ -0,0 +1,68 @@
#!/usr/bin/env python
import numpy as np
import cv2 as cv
import argparse
import sys
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
'--algo',
help="""DEPTH - works with depth,
RGB - works with images,
RGB_DEPTH - works with all,
default - runs all algos""",
default="")
parser.add_argument(
'-src_d',
'--source_depth_frame',
default="")
parser.add_argument(
'-dst_d',
'--destination_depth_frame',
default="")
parser.add_argument(
'-src_rgb',
'--source_rgb_frame',
default="")
parser.add_argument(
'-dst_rgb',
'--destination_rgb_frame',
default="")
args = parser.parse_args()
if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" 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)
depth2 = cv.imread(destination_depth_frame, cv.IMREAD_ANYDEPTH).astype(np.float32)
if args.algo == "RGB_DEPTH" or args.algo == "RGB" or args.algo == "":
source_rgb_frame = cv.samples.findFile(args.source_rgb_frame)
destination_rgb_frame = cv.samples.findFile(args.destination_rgb_frame)
rgb1 = cv.imread(source_rgb_frame, cv.IMREAD_COLOR)
rgb2 = cv.imread(destination_rgb_frame, cv.IMREAD_COLOR)
if args.algo == "DEPTH" or args.algo == "":
odometry = cv.Odometry(cv.DEPTH)
Rt = np.zeros((4, 4))
odometry.compute(depth1, depth2, Rt)
print(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)
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)
if __name__ == '__main__':
main()