mirror of
https://github.com/opencv/opencv.git
synced 2024-12-14 17:29:17 +08:00
110 lines
3.5 KiB
C++
110 lines
3.5 KiB
C++
|
// This file is part of OpenCV project.
|
||
|
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||
|
// of this distribution and at http://opencv.org/license.html.
|
||
|
//
|
||
|
// Copyright (C) 2021, Wanli Zhong <zhongwl2018@mail.sustech.edu.cn>
|
||
|
|
||
|
#include "test_ptcloud_utils.hpp"
|
||
|
|
||
|
namespace opencv_test {
|
||
|
|
||
|
void generatePlane(OutputArray plane_pts, const vector<float> &model, float thr, int num,
|
||
|
const vector<float> &limit)
|
||
|
{
|
||
|
if (plane_pts.channels() == 3 && plane_pts.isVector())
|
||
|
{
|
||
|
// std::vector<cv::Point3f>
|
||
|
plane_pts.create(1, num, CV_32FC3);
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
// cv::Mat
|
||
|
plane_pts.create(num, 3, CV_32F);
|
||
|
}
|
||
|
|
||
|
cv::RNG rng(0);
|
||
|
auto *plane_pts_ptr = (float *) plane_pts.getMat().data;
|
||
|
|
||
|
// Part of the points are generated for the specific model
|
||
|
// The other part of the points are used to increase the thickness of the plane
|
||
|
int std_num = (int) (num / 2);
|
||
|
// Difference of maximum d between two parallel planes
|
||
|
float d_thr = thr * sqrt(model[0] * model[0] + model[1] * model[1] + model[2] * model[2]);
|
||
|
|
||
|
for (int i = 0; i < num; i++)
|
||
|
{
|
||
|
// Let d change then generate thickness
|
||
|
float d = i < std_num ? model[3] : rng.uniform(model[3] - d_thr, model[3] + d_thr);
|
||
|
float x, y, z;
|
||
|
// c is 0 means the plane is vertical
|
||
|
if (model[2] == 0)
|
||
|
{
|
||
|
z = rng.uniform(limit[4], limit[5]);
|
||
|
if (model[0] == 0)
|
||
|
{
|
||
|
x = rng.uniform(limit[0], limit[1]);
|
||
|
y = -d / model[1];
|
||
|
}
|
||
|
else if (model[1] == 0)
|
||
|
{
|
||
|
x = -d / model[0];
|
||
|
y = rng.uniform(limit[2], limit[3]);
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
x = rng.uniform(limit[0], limit[1]);
|
||
|
y = -(model[0] * x + d) / model[1];
|
||
|
}
|
||
|
}
|
||
|
// c is not 0
|
||
|
else
|
||
|
{
|
||
|
x = rng.uniform(limit[0], limit[1]);
|
||
|
y = rng.uniform(limit[2], limit[3]);
|
||
|
z = -(model[0] * x + model[1] * y + d) / model[2];
|
||
|
}
|
||
|
|
||
|
plane_pts_ptr[3 * i] = x;
|
||
|
plane_pts_ptr[3 * i + 1] = y;
|
||
|
plane_pts_ptr[3 * i + 2] = z;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void generateSphere(OutputArray sphere_pts, const vector<float> &model, float thr, int num,
|
||
|
const vector<float> &limit)
|
||
|
{
|
||
|
if (sphere_pts.channels() == 3 && sphere_pts.isVector())
|
||
|
{
|
||
|
// std::vector<cv::Point3f>
|
||
|
sphere_pts.create(1, num, CV_32FC3);
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
// cv::Mat
|
||
|
sphere_pts.create(num, 3, CV_32F);
|
||
|
}
|
||
|
cv::RNG rng(0);
|
||
|
auto *sphere_pts_ptr = (float *) sphere_pts.getMat().data;
|
||
|
|
||
|
// Part of the points are generated for the specific model
|
||
|
// The other part of the points are used to increase the thickness of the sphere
|
||
|
int sphere_num = (int) (num / 1.5);
|
||
|
for (int i = 0; i < num; i++)
|
||
|
{
|
||
|
// Let r change then generate thickness
|
||
|
float r = i < sphere_num ? model[3] : rng.uniform(model[3] - thr, model[3] + thr);
|
||
|
// Generate a random vector and normalize it.
|
||
|
Vec3f vec(rng.uniform(limit[0], limit[1]), rng.uniform(limit[2], limit[3]),
|
||
|
rng.uniform(limit[4], limit[5]));
|
||
|
float l = sqrt(vec.dot(vec));
|
||
|
// Normalizes it to have a magnitude of r
|
||
|
vec /= l / r;
|
||
|
|
||
|
sphere_pts_ptr[3 * i] = model[0] + vec[0];
|
||
|
sphere_pts_ptr[3 * i + 1] = model[1] + vec[1];
|
||
|
sphere_pts_ptr[3 * i + 2] = model[2] + vec[2];
|
||
|
}
|
||
|
}
|
||
|
|
||
|
} // opencv_test
|