|
|
|
@ -108,36 +108,5 @@ 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() |
|
|
|
|