vendor: OpenCV 5.0.0 snapshot at 755e50675d97db9b7d449d8bd6b09888646f6c6e
This commit is contained in:
Gitea Mirror Bot
@@ -0,0 +1,337 @@
|
||||
//
|
||||
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
||||
//
|
||||
// By downloading, copying, installing or using the software you agree to this license.
|
||||
// If you do not agree to this license, do not download, install,
|
||||
// copy or use the software.
|
||||
//
|
||||
//
|
||||
// License Agreement
|
||||
// For Open Source Computer Vision Library
|
||||
//
|
||||
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
|
||||
// Third party copyrights are property of their respective owners.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without modification,
|
||||
// are permitted provided that the following conditions are met:
|
||||
//
|
||||
// * Redistribution's of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
//
|
||||
// * Redistribution's in binary form must reproduce the above copyright notice,
|
||||
// this list of conditions and the following disclaimer in the documentation
|
||||
// and/or other materials provided with the distribution.
|
||||
//
|
||||
// * The name of the copyright holders may not be used to endorse or promote products
|
||||
// derived from this software without specific prior written permission.
|
||||
//
|
||||
// This software is provided by the copyright holders and contributors "as is" and
|
||||
// any express or implied warranties, including, but not limited to, the implied
|
||||
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
||||
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
||||
// indirect, incidental, special, exemplary, or consequential damages
|
||||
// (including, but not limited to, procurement of substitute goods or services;
|
||||
// loss of use, data, or profits; or business interruption) however caused
|
||||
// and on any theory of liability, whether in contract, strict liability,
|
||||
// or tort (including negligence or otherwise) arising in any way out of
|
||||
// the use of this software, even if advised of the possibility of such damage.
|
||||
//
|
||||
// Author: Tolga Birdal <tbirdal AT gmail.com>
|
||||
|
||||
#ifndef __OPENCV_SURFACE_MATCHING_UTILS_HPP_
|
||||
#define __OPENCV_SURFACE_MATCHING_UTILS_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace ppf_match_3d
|
||||
{
|
||||
|
||||
const float EPS = 1.192092896e-07F; /* smallest such that 1.0+FLT_EPSILON != 1.0 */
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.1415926535897932384626433832795
|
||||
#endif
|
||||
|
||||
static inline void TNormalize3(Vec3d& v)
|
||||
{
|
||||
double norm = cv::norm(v);
|
||||
if (norm > EPS)
|
||||
{
|
||||
v *= 1.0 / norm;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Calculate angle between two normalized vectors
|
||||
*
|
||||
* \param [in] a normalized vector
|
||||
* \param [in] b normalized vector
|
||||
* \return angle between a and b vectors in radians
|
||||
*/
|
||||
static inline double TAngle3Normalized(const Vec3d& a, const Vec3d& b)
|
||||
{
|
||||
/*
|
||||
angle = atan2(a dot b, |a x b|) # Bertram (accidental mistake)
|
||||
angle = atan2(|a x b|, a dot b) # Tolga Birdal (correction)
|
||||
angle = acos(a dot b) # Hamdi Sahloul (simplification, a & b are normalized)
|
||||
*/
|
||||
|
||||
return acos(a.dot(b));
|
||||
}
|
||||
|
||||
static inline void rtToPose(const Matx33d& R, const Vec3d& t, Matx44d& Pose)
|
||||
{
|
||||
Matx34d P;
|
||||
hconcat(R, t, P);
|
||||
vconcat(P, Matx14d(0, 0, 0, 1), Pose);
|
||||
}
|
||||
|
||||
static inline void poseToR(const Matx44d& Pose, Matx33d& R)
|
||||
{
|
||||
Mat(Pose).rowRange(0, 3).colRange(0, 3).copyTo(R);
|
||||
}
|
||||
|
||||
static inline void poseToRT(const Matx44d& Pose, Matx33d& R, Vec3d& t)
|
||||
{
|
||||
poseToR(Pose, R);
|
||||
Mat(Pose).rowRange(0, 3).colRange(3, 4).copyTo(t);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Axis angle to rotation
|
||||
*/
|
||||
static inline void aaToR(const Vec3d& axis, double angle, Matx33d& R)
|
||||
{
|
||||
const double sinA = sin(angle);
|
||||
const double cosA = cos(angle);
|
||||
const double cos1A = (1 - cosA);
|
||||
uint i, j;
|
||||
|
||||
Mat(cosA * Matx33d::eye()).copyTo(R);
|
||||
|
||||
for (i = 0; i < 3; i++)
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
if (i != j)
|
||||
{
|
||||
// Symmetry skew matrix
|
||||
R(i, j) += (((i + 1) % 3 == j) ? -1 : 1) * sinA * axis[3 - i - j];
|
||||
}
|
||||
R(i, j) += cos1A * axis[i] * axis[j];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Compute a rotation in order to rotate around X direction
|
||||
*/
|
||||
static inline void getUnitXRotation(double angle, Matx33d& Rx)
|
||||
{
|
||||
const double sx = sin(angle);
|
||||
const double cx = cos(angle);
|
||||
|
||||
Mat(Rx.eye()).copyTo(Rx);
|
||||
Rx(1, 1) = cx;
|
||||
Rx(1, 2) = -sx;
|
||||
Rx(2, 1) = sx;
|
||||
Rx(2, 2) = cx;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Compute a rotation in order to rotate around Y direction
|
||||
*/
|
||||
static inline void getUnitYRotation(double angle, Matx33d& Ry)
|
||||
{
|
||||
const double sy = sin(angle);
|
||||
const double cy = cos(angle);
|
||||
|
||||
Mat(Ry.eye()).copyTo(Ry);
|
||||
Ry(0, 0) = cy;
|
||||
Ry(0, 2) = sy;
|
||||
Ry(2, 0) = -sy;
|
||||
Ry(2, 2) = cy;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Compute a rotation in order to rotate around Z direction
|
||||
*/
|
||||
static inline void getUnitZRotation(double angle, Matx33d& Rz)
|
||||
{
|
||||
const double sz = sin(angle);
|
||||
const double cz = cos(angle);
|
||||
|
||||
Mat(Rz.eye()).copyTo(Rz);
|
||||
Rz(0, 0) = cz;
|
||||
Rz(0, 1) = -sz;
|
||||
Rz(1, 0) = sz;
|
||||
Rz(1, 1) = cz;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Convert euler representation to rotation matrix
|
||||
*
|
||||
* \param [in] euler RPY angles
|
||||
* \param [out] R 3x3 Rotation matrix
|
||||
*/
|
||||
static inline void eulerToDCM(const Vec3d& euler, Matx33d& R)
|
||||
{
|
||||
Matx33d Rx, Ry, Rz;
|
||||
|
||||
getUnitXRotation(euler[0], Rx);
|
||||
getUnitYRotation(euler[1], Ry);
|
||||
getUnitZRotation(euler[2], Rz);
|
||||
|
||||
Mat(Rx * (Ry * Rz)).copyTo(R);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* \brief Compute the transformation needed to rotate n1 onto x axis and p1 to origin
|
||||
*/
|
||||
static inline void computeTransformRT(const Vec3d& p1, const Vec3d& n1, Matx33d& R, Vec3d& t)
|
||||
{
|
||||
// dot product with x axis
|
||||
double angle = acos(n1[0]);
|
||||
|
||||
// cross product with x axis
|
||||
Vec3d axis(0, n1[2], -n1[1]);
|
||||
|
||||
// we try to project on the ground plane but it's already parallel
|
||||
if (n1[1] == 0 && n1[2] == 0)
|
||||
{
|
||||
axis[1] = 1;
|
||||
axis[2] = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
TNormalize3(axis);
|
||||
}
|
||||
|
||||
aaToR(axis, angle, R);
|
||||
t = -R * p1;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Flip a normal to the viewing direction
|
||||
*
|
||||
* \param [in] point Scene point
|
||||
* \param [in] vp view direction
|
||||
* \param [in] n normal
|
||||
*/
|
||||
static inline void flipNormalViewpoint(const Vec3f& point, const Vec3f& vp, Vec3f& n)
|
||||
{
|
||||
float cos_theta;
|
||||
|
||||
// See if we need to flip any plane normals
|
||||
Vec3f diff = vp - point;
|
||||
|
||||
// Dot product between the (viewpoint - point) and the plane normal
|
||||
cos_theta = diff.dot(n);
|
||||
|
||||
// Flip the plane normal
|
||||
if (cos_theta < 0)
|
||||
{
|
||||
n *= -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Convert a rotation matrix to axis angle representation
|
||||
*
|
||||
* \param [in] R Rotation matrix
|
||||
* \param [out] axis Axis vector
|
||||
* \param [out] angle Angle in radians
|
||||
*/
|
||||
static inline void dcmToAA(Matx33d& R, Vec3d& axis, double *angle)
|
||||
{
|
||||
Mat(Vec3d(R(2, 1) - R(2, 1),
|
||||
R(0, 2) - R(2, 0),
|
||||
R(1, 0) - R(0, 1))).copyTo(axis);
|
||||
TNormalize3(axis);
|
||||
*angle = acos(0.5 * (cv::trace(R) - 1.0));
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Convert axis angle representation to rotation matrix
|
||||
*
|
||||
* \param [in] axis Axis Vector
|
||||
* \param [in] angle Angle (In radians)
|
||||
* \param [out] R 3x3 Rotation matrix
|
||||
*/
|
||||
static inline void aaToDCM(const Vec3d& axis, double angle, Matx33d& R)
|
||||
{
|
||||
uint i, j;
|
||||
Matx33d n = Matx33d::all(0);
|
||||
|
||||
for (i = 0; i < 3; i++)
|
||||
for (j = 0; j < 3; j++)
|
||||
if (i != j)
|
||||
n(i, j) = (((i + 1) % 3 == j) ? -1 : 1) * axis[3 - i - j];
|
||||
Mat(Matx33d::eye() + sin(angle) * n + cos(angle) * n * n).copyTo(R);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Convert a discrete cosine matrix to quaternion
|
||||
*
|
||||
* \param [in] R Rotation Matrix
|
||||
* \param [in] q Quaternion
|
||||
*/
|
||||
static inline void dcmToQuat(Matx33d& R, Vec4d& q)
|
||||
{
|
||||
double tr = cv::trace(R);
|
||||
Vec3d v(R(0, 0), R(1, 1), R(2, 2));
|
||||
int idx = tr > 0.0 ? 3 : (int)(std::max_element(v.val, v.val + 3) - v.val);
|
||||
double norm4 = q[(idx + 1) % 4] = 1.0 + (tr > 0.0 ? tr : 2 * R(idx, idx) - tr);
|
||||
int i, prev, next, step = idx % 2 ? 1 : -1, curr = 3;
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
curr = (curr + step) % 4;
|
||||
next = (curr + 1) % 3, prev = (curr + 2) % 3;
|
||||
q[(idx + i + 2) % 4] = R(next, prev) + (tr > 0.0 || idx == curr ? -1 : 1) * R(prev, next);
|
||||
}
|
||||
q *= 0.5 / sqrt(norm4);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Convert quaternion to a discrete cosine matrix
|
||||
*
|
||||
* \param [in] q Quaternion (w is at first element)
|
||||
* \param [in] R Rotation Matrix
|
||||
*
|
||||
*/
|
||||
static inline void quatToDCM(Vec4d& q, Matx33d& R)
|
||||
{
|
||||
Vec4d sq = q.mul(q);
|
||||
|
||||
double tmp1, tmp2;
|
||||
|
||||
R(0, 0) = sq[0] + sq[1] - sq[2] - sq[3]; // since norm(q) = 1
|
||||
R(1, 1) = sq[0] - sq[1] + sq[2] - sq[3];
|
||||
R(2, 2) = sq[0] - sq[1] - sq[2] + sq[3];
|
||||
|
||||
tmp1 = q[1] * q[2];
|
||||
tmp2 = q[3] * q[0];
|
||||
|
||||
R(0, 1) = 2.0 * (tmp1 + tmp2);
|
||||
R(1, 0) = 2.0 * (tmp1 - tmp2);
|
||||
|
||||
tmp1 = q[1] * q[3];
|
||||
tmp2 = q[2] * q[0];
|
||||
|
||||
R(0, 2) = 2.0 * (tmp1 - tmp2);
|
||||
R(2, 0) = 2.0 * (tmp1 + tmp2);
|
||||
|
||||
tmp1 = q[2] * q[3];
|
||||
tmp2 = q[1] * q[0];
|
||||
|
||||
R(1, 2) = 2.0 * (tmp1 + tmp2);
|
||||
R(2, 1) = 2.0 * (tmp1 - tmp2);
|
||||
}
|
||||
|
||||
} // namespace ppf_match_3d
|
||||
|
||||
} // namespace cv
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user