566 lines
18 KiB
C++
566 lines
18 KiB
C++
/*M///////////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// This is a homography decomposition implementation contributed to OpenCV
|
|
// by Samson Yilma. It implements the homography decomposition algorithm
|
|
// described in the research report:
|
|
// Malis, E and Vargas, M, "Deeper understanding of the homography decomposition
|
|
// for vision-based control", Research Report 6303, INRIA (2007)
|
|
//
|
|
// 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, Samson Yilma (samson_yilma@yahoo.com), all rights reserved.
|
|
// Copyright (C) 2018, Intel Corporation, 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.
|
|
//
|
|
//M*/
|
|
|
|
#include "precomp.hpp"
|
|
#include <memory>
|
|
|
|
namespace cv
|
|
{
|
|
|
|
namespace HomographyDecomposition
|
|
{
|
|
|
|
//struct to hold solutions of homography decomposition
|
|
typedef struct _CameraMotion {
|
|
cv::Matx33d R; //!< rotation matrix
|
|
cv::Vec3d n; //!< normal of the plane the camera is looking at
|
|
cv::Vec3d t; //!< translation vector
|
|
} CameraMotion;
|
|
|
|
inline int signd(const double x)
|
|
{
|
|
return ( x >= 0 ? 1 : -1 );
|
|
}
|
|
|
|
class HomographyDecomp {
|
|
|
|
public:
|
|
HomographyDecomp() {}
|
|
virtual ~HomographyDecomp() {}
|
|
virtual void decomposeHomography(const cv::Matx33d& H, const cv::Matx33d& K,
|
|
std::vector<CameraMotion>& camMotions);
|
|
bool isRotationValid(const cv::Matx33d& R, const double epsilon=0.01);
|
|
|
|
protected:
|
|
bool passesSameSideOfPlaneConstraint(CameraMotion& motion);
|
|
virtual void decompose(std::vector<CameraMotion>& camMotions) = 0;
|
|
const cv::Matx33d& getHnorm() const {
|
|
return _Hnorm;
|
|
}
|
|
|
|
private:
|
|
/**
|
|
* Normalize the homograhpy \f$H\f$.
|
|
*
|
|
* @param H Homography matrix.
|
|
* @param K Intrinsic parameter matrix.
|
|
* @return It returns
|
|
* \f[
|
|
* K^{-1} * H * K
|
|
* \f]
|
|
*/
|
|
cv::Matx33d normalize(const cv::Matx33d& H, const cv::Matx33d& K);
|
|
void removeScale();
|
|
cv::Matx33d _Hnorm;
|
|
};
|
|
|
|
class HomographyDecompZhang CV_FINAL : public HomographyDecomp {
|
|
|
|
public:
|
|
HomographyDecompZhang():HomographyDecomp() {}
|
|
virtual ~HomographyDecompZhang() {}
|
|
|
|
private:
|
|
virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE;
|
|
bool findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion);
|
|
};
|
|
|
|
class HomographyDecompInria CV_FINAL : public HomographyDecomp {
|
|
|
|
public:
|
|
HomographyDecompInria():HomographyDecomp() {}
|
|
virtual ~HomographyDecompInria() {}
|
|
|
|
private:
|
|
virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE;
|
|
double oppositeOfMinor(const cv::Matx33d& M, const int row, const int col);
|
|
void findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R);
|
|
};
|
|
|
|
// normalizes homography with intrinsic camera parameters
|
|
Matx33d HomographyDecomp::normalize(const Matx33d& H, const Matx33d& K)
|
|
{
|
|
return K.inv() * H * K;
|
|
}
|
|
|
|
void HomographyDecomp::removeScale()
|
|
{
|
|
Mat W;
|
|
SVD::compute(_Hnorm, W);
|
|
_Hnorm = _Hnorm * (1.0/W.at<double>(1));
|
|
}
|
|
|
|
/*! This checks that the input is a pure rotation matrix 'm'.
|
|
* The conditions for this are: R' * R = I and det(R) = 1 (proper rotation matrix)
|
|
*/
|
|
bool HomographyDecomp::isRotationValid(const Matx33d& R, const double epsilon)
|
|
{
|
|
Matx33d RtR = R.t() * R;
|
|
Matx33d I(1,0,0, 0,1,0, 0,0,1);
|
|
if (norm(RtR, I, NORM_INF) > epsilon)
|
|
return false;
|
|
return (fabs(determinant(R) - 1.0) < epsilon);
|
|
}
|
|
|
|
bool HomographyDecomp::passesSameSideOfPlaneConstraint(CameraMotion& motion)
|
|
{
|
|
typedef Matx<double, 1, 1> Matx11d;
|
|
Matx31d t = Matx31d(motion.t);
|
|
Matx31d n = Matx31d(motion.n);
|
|
Matx11d proj = n.t() * motion.R.t() * t;
|
|
if ( (1 + proj(0, 0) ) <= 0 )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
//!main routine to decompose homography
|
|
void HomographyDecomp::decomposeHomography(const Matx33d& H, const cv::Matx33d& K,
|
|
std::vector<CameraMotion>& camMotions)
|
|
{
|
|
//normalize homography matrix with intrinsic camera matrix
|
|
_Hnorm = normalize(H, K);
|
|
//remove scale of the normalized homography
|
|
removeScale();
|
|
//apply decomposition
|
|
decompose(camMotions);
|
|
}
|
|
|
|
/* function computes R&t from tstar, and plane normal(n) using
|
|
R = H * inv(I + tstar*transpose(n) );
|
|
t = R * tstar;
|
|
returns true if computed R&t is a valid solution
|
|
*/
|
|
bool HomographyDecompZhang::findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion)
|
|
{
|
|
Matx31d tstar_m = Mat(tstar);
|
|
Matx31d n_m = Mat(n);
|
|
Matx33d temp = tstar_m * n_m.t();
|
|
temp(0, 0) += 1.0;
|
|
temp(1, 1) += 1.0;
|
|
temp(2, 2) += 1.0;
|
|
motion.R = getHnorm() * temp.inv();
|
|
if (cv::determinant(motion.R) < 0)
|
|
{
|
|
motion.R *= -1;
|
|
}
|
|
motion.t = motion.R * tstar;
|
|
motion.n = n;
|
|
return passesSameSideOfPlaneConstraint(motion);
|
|
}
|
|
|
|
void HomographyDecompZhang::decompose(std::vector<CameraMotion>& camMotions)
|
|
{
|
|
Mat W, U, Vt;
|
|
SVD::compute(getHnorm(), W, U, Vt);
|
|
CV_Assert(W.total() > 2 && Vt.total() > 7);
|
|
double lambda1=W.at<double>(0);
|
|
double lambda3=W.at<double>(2);
|
|
double lambda1m3 = (lambda1-lambda3);
|
|
double lambda1m3_2 = lambda1m3*lambda1m3;
|
|
double lambda1t3 = lambda1*lambda3;
|
|
|
|
double t1 = 1.0/(2.0*lambda1t3);
|
|
double t2 = sqrt(1.0+4.0*lambda1t3/lambda1m3_2);
|
|
double t12 = t1*t2;
|
|
|
|
double e1 = -t1 + t12; //t1*(-1.0f + t2 );
|
|
double e3 = -t1 - t12; //t1*(-1.0f - t2);
|
|
double e1_2 = e1*e1;
|
|
double e3_2 = e3*e3;
|
|
|
|
double nv1p = sqrt(e1_2*lambda1m3_2 + 2*e1*(lambda1t3-1) + 1.0);
|
|
double nv3p = sqrt(e3_2*lambda1m3_2 + 2*e3*(lambda1t3-1) + 1.0);
|
|
double v1p[3], v3p[3];
|
|
|
|
v1p[0]=Vt.at<double>(0)*nv1p, v1p[1]=Vt.at<double>(1)*nv1p, v1p[2]=Vt.at<double>(2)*nv1p;
|
|
v3p[0]=Vt.at<double>(6)*nv3p, v3p[1]=Vt.at<double>(7)*nv3p, v3p[2]=Vt.at<double>(8)*nv3p;
|
|
|
|
/*The eight solutions are
|
|
(A): tstar = +- (v1p - v3p)/(e1 -e3), n = +- (e1*v3p - e3*v1p)/(e1-e3)
|
|
(B): tstar = +- (v1p + v3p)/(e1 -e3), n = +- (e1*v3p + e3*v1p)/(e1-e3)
|
|
*/
|
|
double v1pmv3p[3], v1ppv3p[3];
|
|
double e1v3me3v1[3], e1v3pe3v1[3];
|
|
double inv_e1me3 = 1.0/(e1-e3);
|
|
|
|
for(int kk=0;kk<3;++kk){
|
|
v1pmv3p[kk] = v1p[kk]-v3p[kk];
|
|
v1ppv3p[kk] = v1p[kk]+v3p[kk];
|
|
}
|
|
|
|
for(int kk=0; kk<3; ++kk){
|
|
double e1v3 = e1*v3p[kk];
|
|
double e3v1=e3*v1p[kk];
|
|
e1v3me3v1[kk] = e1v3-e3v1;
|
|
e1v3pe3v1[kk] = e1v3+e3v1;
|
|
}
|
|
|
|
Vec3d tstar_p, tstar_n;
|
|
Vec3d n_p, n_n;
|
|
|
|
///Solution group A
|
|
for(int kk=0; kk<3; ++kk) {
|
|
tstar_p[kk] = v1pmv3p[kk]*inv_e1me3;
|
|
tstar_n[kk] = -tstar_p[kk];
|
|
n_p[kk] = e1v3me3v1[kk]*inv_e1me3;
|
|
n_n[kk] = -n_p[kk];
|
|
}
|
|
|
|
CameraMotion cmotion;
|
|
//(A) Four different combinations for solution A
|
|
// (i) (+, +)
|
|
if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
|
|
// (ii) (+, -)
|
|
if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
|
|
// (iii) (-, +)
|
|
if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
|
|
// (iv) (-, -)
|
|
if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
//////////////////////////////////////////////////////////////////
|
|
///Solution group B
|
|
for(int kk=0;kk<3;++kk){
|
|
tstar_p[kk] = v1ppv3p[kk]*inv_e1me3;
|
|
tstar_n[kk] = -tstar_p[kk];
|
|
n_p[kk] = e1v3pe3v1[kk]*inv_e1me3;
|
|
n_n[kk] = -n_p[kk];
|
|
}
|
|
|
|
//(B) Four different combinations for solution B
|
|
// (i) (+, +)
|
|
if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
|
|
// (ii) (+, -)
|
|
if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
|
|
// (iii) (-, +)
|
|
if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
|
|
// (iv) (-, -)
|
|
if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion))
|
|
camMotions.push_back(cmotion);
|
|
}
|
|
|
|
double HomographyDecompInria::oppositeOfMinor(const Matx33d& M, const int row, const int col)
|
|
{
|
|
int x1 = col == 0 ? 1 : 0;
|
|
int x2 = col == 2 ? 1 : 2;
|
|
int y1 = row == 0 ? 1 : 0;
|
|
int y2 = row == 2 ? 1 : 2;
|
|
|
|
return (M(y1, x2) * M(y2, x1) - M(y1, x1) * M(y2, x2));
|
|
}
|
|
|
|
//computes R = H( I - (2/v)*te_star*ne_t )
|
|
void HomographyDecompInria::findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R)
|
|
{
|
|
Matx31d tstar_m = Matx31d(tstar);
|
|
Matx31d n_m = Matx31d(n);
|
|
Matx33d I(1.0, 0.0, 0.0,
|
|
0.0, 1.0, 0.0,
|
|
0.0, 0.0, 1.0);
|
|
|
|
R = getHnorm() * (I - (2/v) * tstar_m * n_m.t() );
|
|
if (cv::determinant(R) < 0)
|
|
{
|
|
R *= -1;
|
|
}
|
|
}
|
|
|
|
void HomographyDecompInria::decompose(std::vector<CameraMotion>& camMotions)
|
|
{
|
|
const double epsilon = 0.001;
|
|
Matx33d S;
|
|
|
|
//S = H'H - I
|
|
S = getHnorm().t() * getHnorm();
|
|
S(0, 0) -= 1.0;
|
|
S(1, 1) -= 1.0;
|
|
S(2, 2) -= 1.0;
|
|
|
|
//check if H is rotation matrix
|
|
if( norm(S, NORM_INF) < epsilon) {
|
|
CameraMotion motion;
|
|
motion.R = Matx33d(getHnorm());
|
|
motion.t = Vec3d(0, 0, 0);
|
|
motion.n = Vec3d(0, 0, 0);
|
|
camMotions.push_back(motion);
|
|
return;
|
|
}
|
|
|
|
//! Compute nvectors
|
|
Vec3d npa, npb;
|
|
|
|
double M00 = oppositeOfMinor(S, 0, 0);
|
|
double M11 = oppositeOfMinor(S, 1, 1);
|
|
double M22 = oppositeOfMinor(S, 2, 2);
|
|
|
|
double rtM00 = sqrt(M00);
|
|
double rtM11 = sqrt(M11);
|
|
double rtM22 = sqrt(M22);
|
|
|
|
double M01 = oppositeOfMinor(S, 0, 1);
|
|
double M12 = oppositeOfMinor(S, 1, 2);
|
|
double M02 = oppositeOfMinor(S, 0, 2);
|
|
|
|
int e12 = signd(M12);
|
|
int e02 = signd(M02);
|
|
int e01 = signd(M01);
|
|
|
|
double nS00 = abs(S(0, 0));
|
|
double nS11 = abs(S(1, 1));
|
|
double nS22 = abs(S(2, 2));
|
|
|
|
//find max( |Sii| ), i=0, 1, 2
|
|
int indx = 0;
|
|
if(nS00 < nS11){
|
|
indx = 1;
|
|
if( nS11 < nS22 )
|
|
indx = 2;
|
|
}
|
|
else {
|
|
if(nS00 < nS22 )
|
|
indx = 2;
|
|
}
|
|
|
|
switch (indx) {
|
|
case 0:
|
|
npa[0] = S(0, 0), npb[0] = S(0, 0);
|
|
npa[1] = S(0, 1) + rtM22, npb[1] = S(0, 1) - rtM22;
|
|
npa[2] = S(0, 2) + e12 * rtM11, npb[2] = S(0, 2) - e12 * rtM11;
|
|
break;
|
|
case 1:
|
|
npa[0] = S(0, 1) + rtM22, npb[0] = S(0, 1) - rtM22;
|
|
npa[1] = S(1, 1), npb[1] = S(1, 1);
|
|
npa[2] = S(1, 2) - e02 * rtM00, npb[2] = S(1, 2) + e02 * rtM00;
|
|
break;
|
|
case 2:
|
|
npa[0] = S(0, 2) + e01 * rtM11, npb[0] = S(0, 2) - e01 * rtM11;
|
|
npa[1] = S(1, 2) + rtM00, npb[1] = S(1, 2) - rtM00;
|
|
npa[2] = S(2, 2), npb[2] = S(2, 2);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
double traceS = S(0, 0) + S(1, 1) + S(2, 2);
|
|
double v = 2.0 * sqrt(1 + traceS - M00 - M11 - M22);
|
|
|
|
double ESii = signd(S(indx, indx)) ;
|
|
double r_2 = 2 + traceS + v;
|
|
double nt_2 = 2 + traceS - v;
|
|
|
|
double r = sqrt(r_2);
|
|
double n_t = sqrt(nt_2);
|
|
|
|
Vec3d na = npa / norm(npa);
|
|
Vec3d nb = npb / norm(npb);
|
|
|
|
double half_nt = 0.5 * n_t;
|
|
double esii_t_r = ESii * r;
|
|
|
|
Vec3d ta_star = half_nt * (esii_t_r * nb - n_t * na);
|
|
Vec3d tb_star = half_nt * (esii_t_r * na - n_t * nb);
|
|
|
|
camMotions.resize(4);
|
|
|
|
Matx33d Ra, Rb;
|
|
Vec3d ta, tb;
|
|
|
|
//Ra, ta, na
|
|
findRmatFrom_tstar_n(ta_star, na, v, Ra);
|
|
ta = Ra * ta_star;
|
|
|
|
camMotions[0].R = Ra;
|
|
camMotions[0].t = ta;
|
|
camMotions[0].n = na;
|
|
|
|
//Ra, -ta, -na
|
|
camMotions[1].R = Ra;
|
|
camMotions[1].t = -ta;
|
|
camMotions[1].n = -na;
|
|
|
|
//Rb, tb, nb
|
|
findRmatFrom_tstar_n(tb_star, nb, v, Rb);
|
|
tb = Rb * tb_star;
|
|
|
|
camMotions[2].R = Rb;
|
|
camMotions[2].t = tb;
|
|
camMotions[2].n = nb;
|
|
|
|
//Rb, -tb, -nb
|
|
camMotions[3].R = Rb;
|
|
camMotions[3].t = -tb;
|
|
camMotions[3].n = -nb;
|
|
}
|
|
|
|
} //namespace HomographyDecomposition
|
|
|
|
// function decomposes image-to-image homography to rotation and translation matrices
|
|
int decomposeHomographyMat(InputArray _H,
|
|
InputArray _K,
|
|
OutputArrayOfArrays _rotations,
|
|
OutputArrayOfArrays _translations,
|
|
OutputArrayOfArrays _normals)
|
|
{
|
|
using namespace std;
|
|
using namespace HomographyDecomposition;
|
|
|
|
Mat H = _H.getMat().reshape(1, 3);
|
|
CV_Assert(H.cols == 3 && H.rows == 3);
|
|
|
|
Mat K = _K.getMat().reshape(1, 3);
|
|
CV_Assert(K.cols == 3 && K.rows == 3);
|
|
|
|
cv::Ptr<HomographyDecomp> hdecomp(new HomographyDecompInria);
|
|
|
|
vector<CameraMotion> motions;
|
|
hdecomp->decomposeHomography(H, K, motions);
|
|
|
|
int nsols = static_cast<int>(motions.size());
|
|
int depth = CV_64F; //double precision matrices used in CameraMotion struct
|
|
|
|
if (_rotations.needed()) {
|
|
_rotations.create(nsols, 1, depth);
|
|
for (int k = 0; k < nsols; ++k ) {
|
|
_rotations.getMatRef(k) = Mat(motions[k].R);
|
|
}
|
|
}
|
|
|
|
if (_translations.needed()) {
|
|
_translations.create(nsols, 1, depth);
|
|
for (int k = 0; k < nsols; ++k ) {
|
|
_translations.getMatRef(k) = Mat(motions[k].t);
|
|
}
|
|
}
|
|
|
|
if (_normals.needed()) {
|
|
_normals.create(nsols, 1, depth);
|
|
for (int k = 0; k < nsols; ++k ) {
|
|
_normals.getMatRef(k) = Mat(motions[k].n);
|
|
}
|
|
}
|
|
|
|
return nsols;
|
|
}
|
|
|
|
void filterHomographyDecompByVisibleRefpoints(InputArrayOfArrays _rotations,
|
|
InputArrayOfArrays _normals,
|
|
InputArray _beforeRectifiedPoints,
|
|
InputArray _afterRectifiedPoints,
|
|
OutputArray _possibleSolutions,
|
|
InputArray _pointsMask)
|
|
{
|
|
CV_Assert(_beforeRectifiedPoints.type() == CV_32FC2 && _afterRectifiedPoints.type() == CV_32FC2);
|
|
CV_Assert(_pointsMask.empty() || _pointsMask.type() == CV_8U);
|
|
|
|
Mat beforeRectifiedPoints = _beforeRectifiedPoints.getMat();
|
|
Mat afterRectifiedPoints = _afterRectifiedPoints.getMat();
|
|
Mat pointsMask = _pointsMask.getMat();
|
|
int nsolutions = (int)_rotations.total();
|
|
int npoints = (int)beforeRectifiedPoints.total();
|
|
CV_Assert(pointsMask.empty() || pointsMask.checkVector(1, CV_8U) == npoints);
|
|
const uchar* pointsMaskPtr = pointsMask.data;
|
|
|
|
std::vector<uchar> solutionMask(nsolutions, (uchar)1);
|
|
std::vector<Mat> normals(nsolutions);
|
|
std::vector<Mat> rotnorm(nsolutions);
|
|
Mat R;
|
|
|
|
for( int i = 0; i < nsolutions; i++ )
|
|
{
|
|
_normals.getMat(i).convertTo(normals[i], CV_64F);
|
|
CV_Assert(normals[i].total() == 3);
|
|
_rotations.getMat(i).convertTo(R, CV_64F);
|
|
rotnorm[i] = R*normals[i];
|
|
CV_Assert(rotnorm[i].total() == 3);
|
|
}
|
|
|
|
for( int j = 0; j < npoints; j++ )
|
|
{
|
|
if( !pointsMaskPtr || pointsMaskPtr[j] )
|
|
{
|
|
Point2f prevPoint = beforeRectifiedPoints.at<Point2f>(j);
|
|
Point2f currPoint = afterRectifiedPoints.at<Point2f>(j);
|
|
|
|
for( int i = 0; i < nsolutions; i++ )
|
|
{
|
|
if( !solutionMask[i] )
|
|
continue;
|
|
|
|
const double* normal_i = normals[i].ptr<double>();
|
|
const double* rotnorm_i = rotnorm[i].ptr<double>();
|
|
double prevNormDot = normal_i[0]*prevPoint.x + normal_i[1]*prevPoint.y + normal_i[2];
|
|
double currNormDot = rotnorm_i[0]*currPoint.x + rotnorm_i[1]*currPoint.y + rotnorm_i[2];
|
|
|
|
if (prevNormDot <= 0 || currNormDot <= 0)
|
|
solutionMask[i] = (uchar)0;
|
|
}
|
|
}
|
|
}
|
|
|
|
std::vector<int> possibleSolutions;
|
|
for( int i = 0; i < nsolutions; i++ )
|
|
if( solutionMask[i] )
|
|
possibleSolutions.push_back(i);
|
|
|
|
Mat(possibleSolutions).copyTo(_possibleSolutions);
|
|
}
|
|
|
|
} //namespace cv
|