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

Merge pull request #20755 from DumDereDum:new_odometry

Browse Source 
New odometry Pipeline

* first intergation

* tests run, but not pass

* add previous version of sigma calc

* add minor comment

* strange fixes

* fix fast ICP

* test changes; fast icp still not work correctly

* finaly, it works

* algtype fix

* change affine comparison

* boolean return

* fix bug with angle and cos

* test pass correctly

* fix for kinfu pipeline

* add compute points normals

* update for new odometry

* change odometry_evaluation

* odometry_evaluation works

* change debug logs

* minor changes

* change depth setting in odometryFrame

* fastICP works with 4num points

* all odometries work with 4mun points

* odometry full works on 4num points and normals

* replace ICP with DEPTH; comments replacements

* create prepareFrame; add docs for Odometry

* change getPyramids()

* delete extra code

* add intrinsics; but dont works

* bugfix with nan checking

* add gpu impl

* change createOdometryFrame func

* remove old fastICP code

* comments fix

* add comments

* minor fixes

* other minor fixes

* add channels assert

* add impl for odometry settings

* add pimpl to odometry

* linux warning fix

* linux warning fix 1

* linux warning fix 2

* linux error fix

* linux warning fix 3

* linux warning fix 4

* linux error fix 2

* fix test warnings

* python build fix

* doxygen fix

* docs fix

* change normal tests for 4channel point

* all Normal tests pass

* plane works

* add warp frame body

* minor fix

* warning fixes

* try to fix

* try to fix 1

* review fix

* lvls fix

* createOdometryFrame fix

* add comment

* const reference

* OPENCV_3D_ prefix

* const methods

* –OdometryFramePyramidType ifx

* add assert

* precomp moved upper

* delete types_c

* add assert for get and set functions

* minor fixes

* remove core.hpp from header

* ocl_run add

* warning fix

* delete extra comment

* minor fix

* setDepth fix

* delete underscore

* odometry settings fix

* show debug image fix

* build error fix

* other minor fix

* add const to signatures

* fix

* conflict fix

* getter fix
This commit is contained in:
Artem Saratovtsev
2021-12-02 20:53:44 +03:00
committed by GitHub
parent 0cf0a5e9d4
commit 6ab4659840
21 changed files with 3229 additions and 3578 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/3d/test/test_normal.cpp
+17 -15
View File
@@ -17,12 +17,12 @@ rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const M
}
#endif
Vec3f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec3d& normal, const Matx33d& Kinv)
Vec4f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec4d& normal, const Matx33d& Kinv)
{
Matx31d L = Kinv * uv1; //a ray passing through camera optical center
//and uv.
L = L * (1.0 / cv::norm(L));
double LdotNormal = L.dot(normal);
double LdotNormal = L.dot(Vec3d(normal[0], normal[1], normal[2]));
double d;
if (std::fabs(LdotNormal) > 1e-9)
{
@@ -34,7 +34,7 @@ Vec3f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const V
std::cout << "warning, LdotNormal nearly 0! " << LdotNormal << std::endl;
std::cout << "contents of L, Normal: " << Mat(L) << ", " << Mat(normal) << std::endl;
}
Vec3f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)));
Vec4f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)), 0);
return xyz;
}
@@ -48,9 +48,9 @@ float cy = H / 2.f + 0.5f;
Mat K = (Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1);
Mat Kinv = K.inv();
void points3dToDepth16U(const Mat_<Vec3f>& points3d, Mat& depthMap);
void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap);
void points3dToDepth16U(const Mat_<Vec3f>& points3d, Mat& depthMap)
void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap)
{
std::vector<Point3f> points3dvec;
for (int i = 0; i < H; i++)
@@ -80,13 +80,14 @@ void points3dToDepth16U(const Mat_<Vec3f>& points3d, Mat& depthMap)
static RNG rng;
struct Plane
{
Vec3d n, p;
Vec4d n, p;
double p_dot_n;
Plane()
{
n[0] = rng.uniform(-0.5, 0.5);
n[1] = rng.uniform(-0.5, 0.5);
n[2] = -0.3; //rng.uniform(-1.f, 0.5f);
n[3] = 0.;
n = n / cv::norm(n);
set_d((float)rng.uniform(-2.0, 0.6));
}
@@ -94,11 +95,11 @@ struct Plane
void
set_d(float d)
{
p = Vec3d(0, 0, d / n[2]);
p = Vec4d(0, 0, d / n[2], 0);
p_dot_n = p.dot(n);
}
Vec3f
Vec4f
intersection(float u, float v, const Matx33f& Kinv_in) const
{
return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in);
@@ -118,8 +119,8 @@ void gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_ma
planes.push_back(px);
}
}
Mat_ < Vec3f > outp(H, W);
Mat_ < Vec3f > outn(H, W);
Mat_ < Vec4f > outp(H, W);
Mat_ < Vec4f > outn(H, W);
plane_mask.create(H, W);
// n ( r - r_0) = 0
@@ -282,15 +283,15 @@ public:
normals_computer->apply(points3d, in_normals);
tm.stop();
Mat_<Vec3f> normals, ground_normals;
in_normals.convertTo(normals, CV_32FC3);
in_ground_normals.convertTo(ground_normals, CV_32FC3);
Mat_<Vec4f> normals, ground_normals;
in_normals.convertTo(normals, CV_32FC4);
in_ground_normals.convertTo(ground_normals, CV_32FC4);
float err = 0;
for (int y = 0; y < normals.rows; ++y)
for (int x = 0; x < normals.cols; ++x)
{
Vec3f vec1 = normals(y, x), vec2 = ground_normals(y, x);
Vec4f vec1 = normals(y, x), vec2 = ground_normals(y, x);
vec1 = vec1 / cv::norm(vec1);
vec2 = vec2 / cv::norm(vec2);
@@ -381,7 +382,8 @@ public:
ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001);
// Compare the normals
Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]);
ASSERT_GE(std::abs(gt_planes[j].n.dot(normal)), 0.95);
Vec4d n = gt_planes[j].n;
ASSERT_GE(std::abs(Vec3d(n[0], n[1], n[2]).dot(normal)), 0.95);
}
CV_LOG_INFO(NULL, "Speed: ");