bool operator()(const Matx<_Tp, 2, 2>& a, Matx<_Tp, 2, 2>& b, int) const
{
_Tp d = (_Tp)determinant(a);
if( d == 0 )
=======
bool Basis::is_rotation() const {
return Math::is_equal_approx(determinant(), 1, UNIT_EPSILON) && is_orthogonal();
}
static CYTHON_INLINE struct ZZ* mat_ZZ_determinant(const mat_ZZ* x, long deterministic)
{
ZZ* d = new ZZ();
determinant(*d, *x, deterministic);
return d;
}
float2 characteristic_poly(mat2 p)
{
return float2(determinant(p), -trace(p));
}
void WT_Matrix2D::get_inverse (WT_Matrix2D & result) const
{
get_adjoint(result);
result *= 1.0 / determinant();
}
Mat RANSAC::getTransformationMatrix(std::vector<Mat>& X, std::vector<Mat>& Y){
//calculate centroids
Mat u_x(1, 3, CV_32FC1, float(0));
Mat u_y(1, 3, CV_32FC1, float(0));
for (int i = 0; i < X.size(); i++) {
u_x += X[i];
u_y += Y[i];
}
u_x /= (float)X.size();
u_y /= (float)X.size();
//Calculate E_xy covariance matrix
Mat E_xy(3, 3, CV_32FC1, double(0));
for (int i = 0; i < X.size(); i++) {
Mat a = (Y[i] - u_y);
Mat b;
transpose((X[i] - u_x), b);
E_xy += b*a;
}
E_xy /= X.size();
//Calculate SVD(E)
Mat w, U, Vt;
SVD::compute(E_xy, w, U, Vt, SVD::MODIFY_A);
//Find S
Mat S;
double det_U = determinant(U);
double det_Vt = determinant(Vt);
if (det_U*det_Vt > 0){
S = Mat::eye(3, 3, CV_32FC1);
}
else{
S = Mat::eye(3, 3, CV_32FC1);
S.at<float>(2, 2) = -1;
}
//Calculate Rotation and translation
Mat V, Ut, Rt;
transpose(Vt, V);
transpose(U, Ut);
Mat R = V*S*Ut;
//Mat R = U*S*Vt;
//transpose(R, Rt);
transpose(u_x, u_x);
Mat temp = R*u_x;
transpose(temp, temp);
Mat t = u_y - temp;
transpose(t, t);
//printf("E %.2f\t%.2f\t%.2f\n", E_xy.at<float>(0, 0), E_xy.at<float>(0, 1), E_xy.at<float>(0, 2));
//printf("E %.2f\t%.2f\t%.2f\n", E_xy.at<float>(1, 0), E_xy.at<float>(1, 1), E_xy.at<float>(1, 2));
//printf("E %.2f\t%.2f\t%.2f\n", E_xy.at<float>(2, 0), E_xy.at<float>(2, 1), E_xy.at<float>(2, 2));
//printf("R %.2f\t%.2f\t%.2f\n", R.at<float>(0, 0), R.at<float>(0, 1), R.at<float>(0, 2));
//printf("R %.2f\t%.2f\t%.2f\n", R.at<float>(1, 0), R.at<float>(1, 1), R.at<float>(1, 2));
//printf("R %.2f\t%.2f\t%.2f\n", R.at<float>(2, 0), R.at<float>(2, 1), R.at<float>(2, 2));
//printf("\n");
//printf("U %.2f\t%.2f\t%.2f\n", U.at<float>(0, 0), U.at<float>(0, 1), U.at<float>(0, 2));
//printf("U %.2f\t%.2f\t%.2f\n", U.at<float>(1, 0), U.at<float>(1, 1), U.at<float>(1, 2));
//printf("U %.2f\t%.2f\t%.2f\n", U.at<float>(2, 0), U.at<float>(2, 1), U.at<float>(2, 2));
//printf("\n");
//printf("Vt %.2f\t%.2f\t%.2f\n", Vt.at<float>(0, 0), Vt.at<float>(0, 1), Vt.at<float>(0, 2));
//printf("Vt %.2f\t%.2f\t%.2f\n", Vt.at<float>(1, 0), Vt.at<float>(1, 1), Vt.at<float>(1, 2));
//printf("Vt %.2f\t%.2f\t%.2f\n", Vt.at<float>(2, 0), Vt.at<float>(2, 1), Vt.at<float>(2, 2));
//printf("\n");
//printf("t %.2f\n", t.at<float>(0));
//printf("t %.2f\n", t.at<float>(1));
//printf("t %.2f\n", t.at<float>(2));
//printf("\n");
//Get Transformation matrix
Mat m = Mat::eye(4, 4, CV_32FC1);
R.copyTo(m(Rect(0, 0, 3, 3)));
Mat aux_t = m(Rect(3, 0, 1, 3));
t.copyTo(aux_t);
//printf("T %.3f\t%.3f\t%.3f\t%.3f\n", m.at<float>(0, 0), m.at<float>(0, 1), m.at<float>(0, 2), m.at<float>(0, 3));
//printf("T %.3f\t%.3f\t%.3f\t%.3f\n", m.at<float>(1, 0), m.at<float>(1, 1), m.at<float>(1, 2), m.at<float>(1, 3));
//printf("T %.3f\t%.3f\t%.3f\t%.3f\n", m.at<float>(2, 0), m.at<float>(2, 1), m.at<float>(2, 2), m.at<float>(2, 3));
//printf("T %.3f\t%.3f\t%.3f\t%.3f\n", m.at<float>(3, 0), m.at<float>(3, 1), m.at<float>(3, 2), m.at<float>(3, 3));
//printf("\n");
//final movement Y_i = T*X_i
return m;
}
/* It is a function that call getHoography multiple times and measure its accuracy.
* If it runs the function more than the maxLoop value or if the accuracy measure
* starts to increase, the loop stops*/
cv::Mat AntiShake::calcHomographyFeedbackController(Mat &img_1, Mat &img_2, int loops,
double final_pic_size, int featurePoints, int coreSize, double absoluteRelation,
int matches_type) {
// STEP 0: RE-ESCALE, SO THE BIGGEST RESOLUTION IS 590x(something <= 590)
Mat workImage1, workImage2;
double scale = 1.0 / (MAX(img_1.rows,img_1.cols) / final_pic_size);
if (final_pic_size == 0) {
scale = 1.0;
}
workImage1.create(scale * img_1.rows, scale * img_1.cols, img_1.type());
workImage2.create(scale * img_2.rows, scale * img_2.cols, img_2.type());
cv::resize(img_1, workImage1, workImage1.size(), 0, 0, INTER_CUBIC);
cv::resize(img_2, workImage2, workImage2.size(), 0, 0, INTER_CUBIC);
cout << "=== STEP 0 complete: RE-ESCALE" << endl;
// LETS NOW START TO ITERATE IN ORTHER TO get a Homography matrix and refine it
Mat homography;
vector<cv::Mat> Hs, eigenvalues;
vector<double> dets;
int loopCounter = 0;
do {
loopCounter++;
try {
//pixels:
homography = antiShake(workImage1, workImage2, matches_type, featurePoints, coreSize,
absoluteRelation); // exceptions could appear here... //STEPS 1 to 8 there.
double det = determinant(homography);
Mat eigen;
cv::eigen(homography, eigen);
cout << endl << "==== STEP 9: HOMOGRAPHY: " << endl << homography << endl << "determinant: "
<< det << endl << "eigenvalues: " << eigen << endl << endl;
// *** Checks if the determinant is small enough. If not, the transformation could be awful.
if (det != 1) {
Hs.push_back(homography);
eigenvalues.push_back(eigen);
dets.push_back(abs(det - 1));
applyHomography(homography, workImage1, workImage2);
//Checks if the error has decreased in the last iteration:
int size = dets.size();
if (size > 2) {
if (dets[size - 1] > dets[size - 2]) {
/*
* Explaining the above if comparision:
* - dets[size - 1] is the nth (last) calculated determinant
* - dets[size - 2] is the (n-1)th (penultimate) calculated determinant*
* - The biggest is the difference between the two pictures, the highest
* will be the det value. Thing is: once the algorithm is well applicated,
* if applyied again in the now fixed pictures, should generate a matrix similar
* to the Identity I (at least more similar to I than the first matrix).
* - dets receive the value ov abd(det - 1). It is used as the measuring parameter.
* If the value of the det(n) increases from one run to the next,
* means the algorithm may not be being well used (for any picture noise or miscalibration)
*/
cout << "==== POP BACK" << endl;
//Remove last element of the vectors
Hs.pop_back();
eigenvalues.pop_back();
dets.pop_back();
break;
}
}
} else {
// if abs(det-1) is too high, give him some munchies along with the identity matrix:
break;
}
} catch (Exception &e) {
cout
<< "ATTENTION ON THE ANTISHAKE.CPP CLASS: exception found in the function getHomographyFeedbackController. ERROR: "
<< e.err << endl;
homography = (Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
cout << "IDENTITY 1" << endl;
}
} while (loopCounter < loops || homography.empty());
// CALCULATES RESULTANT HOMOGRAPHY:
Mat resultantH = (Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
for (unsigned int position = 0; position < Hs.size(); ++position) {
resultantH = (Hs[position]) * resultantH;
}
if (abs(determinant(resultantH) - 1.0) < maxDetDiff) {
return resultantH;
} else {
return Hs[0];
}
}
void transpose(float num[25][25], float fac[25][25], float r)
{
int i, j;
float b[25][25], inverse[25][25], d;
for (i = 0;i < r; i++)
{
for (j = 0;j < r; j++)
{
b[i][j] = fac[j][i];
}
}
d = determinant(num, r);
for (i = 0;i < r; i++)
{
for (j = 0;j < r; j++)
{
inverse[i][j] = b[i][j] / d;
}
}
printf("\n\n\nThe inverse of matrix is : \n");
for (i = 0;i < r; i++)
{
for (j = 0;j < r; j++)
{
printf("\t%f", inverse[i][j]);
}
printf("\n");
}
}
float determinant(float a[25][25], float k)
{
float s = 1, det = 0, b[25][25];
int i, j, m, n, c;
if (k == 1)
{
return (a[0][0]);
}
else
{
det = 0;
for (c = 0; c < k; c++)
{
m = 0;
n = 0;
for (i = 0;i < k; i++)
{
for (j = 0 ;j < k; j++)
{
b[i][j] = 0;
if (i != 0 && j != c)
{
b[m][n] = a[i][j];
if (n < (k - 2))
n++;
else
{
n = 0;
m++;
}
}
}
}
det = det + s * (a[0][c] * determinant(b, k - 1));
s = -1 * s;
}
}
return (det);
}
/*!
multiple quality measures of a quad
*/
C_FUNC_DEF void v_quad_quality( int num_nodes, VERDICT_REAL coordinates[][3],
unsigned int metrics_request_flag, QuadMetricVals *metric_vals )
{
memset( metric_vals, 0, sizeof(QuadMetricVals) );
// for starts, lets set up some basic and common information
/* node numbers and side numbers used below
2
3 +--------- 2
/ +
/ |
3 / | 1
/ |
+ |
0 -------------+ 1
0
*/
// vectors for each side
VerdictVector edges[4];
make_quad_edges( edges, coordinates );
double areas[4];
signed_corner_areas( areas, coordinates );
double lengths[4];
lengths[0] = edges[0].length();
lengths[1] = edges[1].length();
lengths[2] = edges[2].length();
lengths[3] = edges[3].length();
VerdictBoolean is_collapsed = is_collapsed_quad(coordinates);
// handle collapsed quads metrics here
if(is_collapsed == VERDICT_TRUE && metrics_request_flag &
( V_QUAD_MINIMUM_ANGLE | V_QUAD_MAXIMUM_ANGLE | V_QUAD_JACOBIAN |
V_QUAD_SCALED_JACOBIAN ))
{
if(metrics_request_flag & V_QUAD_MINIMUM_ANGLE)
metric_vals->minimum_angle = v_tri_minimum_angle(3, coordinates);
if(metrics_request_flag & V_QUAD_MAXIMUM_ANGLE)
metric_vals->maximum_angle = v_tri_maximum_angle(3, coordinates);
if(metrics_request_flag & V_QUAD_JACOBIAN)
metric_vals->jacobian = (VERDICT_REAL)(v_tri_area(3, coordinates) * 2.0);
if(metrics_request_flag & V_QUAD_SCALED_JACOBIAN)
metric_vals->jacobian = (VERDICT_REAL)(v_tri_scaled_jacobian(3, coordinates) * 2.0);
}
// calculate both largest and smallest angles
if(metrics_request_flag & (V_QUAD_MINIMUM_ANGLE | V_QUAD_MAXIMUM_ANGLE)
&& is_collapsed == VERDICT_FALSE )
{
// gather the angles
double angles[4];
angles[0] = acos( -(edges[0] % edges[1])/(lengths[0]*lengths[1]) );
angles[1] = acos( -(edges[1] % edges[2])/(lengths[1]*lengths[2]) );
angles[2] = acos( -(edges[2] % edges[3])/(lengths[2]*lengths[3]) );
angles[3] = acos( -(edges[3] % edges[0])/(lengths[3]*lengths[0]) );
if( lengths[0] <= VERDICT_DBL_MIN ||
lengths[1] <= VERDICT_DBL_MIN ||
lengths[2] <= VERDICT_DBL_MIN ||
lengths[3] <= VERDICT_DBL_MIN )
{
metric_vals->minimum_angle = 360.0;
metric_vals->maximum_angle = 0.0;
}
else
{
// if smallest angle, find the smallest angle
if(metrics_request_flag & V_QUAD_MINIMUM_ANGLE)
{
metric_vals->minimum_angle = VERDICT_DBL_MAX;
for(int i = 0; i<4; i++)
metric_vals->minimum_angle = VERDICT_MIN(angles[i], metric_vals->minimum_angle);
metric_vals->minimum_angle *= 180.0 / VERDICT_PI;
}
// if largest angle, find the largest angle
if(metrics_request_flag & V_QUAD_MAXIMUM_ANGLE)
{
metric_vals->maximum_angle = 0.0;
for(int i = 0; i<4; i++)
metric_vals->maximum_angle = VERDICT_MAX(angles[i], metric_vals->maximum_angle);
metric_vals->maximum_angle *= 180.0 / VERDICT_PI;
if( areas[0] < 0 || areas[1] < 0 ||
areas[2] < 0 || areas[3] < 0 )
metric_vals->maximum_angle = 360 - metric_vals->maximum_angle;
}
}
}
// handle aspect, skew, taper, and area together
if( metrics_request_flag & ( V_QUAD_ASPECT | V_QUAD_SKEW | V_QUAD_TAPER ) )
{
//get principle axes
VerdictVector principal_axes[2];
principal_axes[0] = edges[0] - edges[2];
principal_axes[1] = edges[1] - edges[3];
if(metrics_request_flag & (V_QUAD_ASPECT | V_QUAD_SKEW | V_QUAD_TAPER))
{
double len1 = principal_axes[0].length();
double len2 = principal_axes[1].length();
// calculate the aspect ratio
if(metrics_request_flag & V_QUAD_ASPECT)
{
if( len1 < VERDICT_DBL_MIN || len2 < VERDICT_DBL_MIN )
metric_vals->aspect = VERDICT_DBL_MAX;
else
metric_vals->aspect = VERDICT_MAX( len1 / len2, len2 / len1 );
}
// calculate the taper
if(metrics_request_flag & V_QUAD_TAPER)
{
double min_length = VERDICT_MIN( len1, len2 );
VerdictVector cross_derivative = edges[1] + edges[3];
if( min_length < VERDICT_DBL_MIN )
metric_vals->taper = VERDICT_DBL_MAX;
else
metric_vals->taper = cross_derivative.length()/ min_length;
}
// calculate the skew
if(metrics_request_flag & V_QUAD_SKEW)
{
if( principal_axes[0].normalize() < VERDICT_DBL_MIN ||
principal_axes[1].normalize() < VERDICT_DBL_MIN )
metric_vals->skew = 0.0;
else
metric_vals->skew = fabs( principal_axes[0] % principal_axes[1] );
}
}
}
// calculate the area
if(metrics_request_flag & (V_QUAD_AREA | V_QUAD_RELATIVE_SIZE_SQUARED) )
{
metric_vals->area = 0.25 * (areas[0] + areas[1] + areas[2] + areas[3]);
}
// calculate the relative size
if(metrics_request_flag & (V_QUAD_RELATIVE_SIZE_SQUARED | V_QUAD_SHAPE_AND_SIZE |
V_QUAD_SHEAR_AND_SIZE ) )
{
double quad_area = v_quad_area (4, coordinates);
v_set_quad_size( quad_area );
double w11,w21,w12,w22;
get_weight(w11,w21,w12,w22);
double avg_area = determinant(w11,w21,w12,w22);
if( avg_area < VERDICT_DBL_MIN )
metric_vals->relative_size_squared = 0.0;
else
metric_vals->relative_size_squared = pow( VERDICT_MIN(
metric_vals->area/avg_area,
avg_area/metric_vals->area ), 2 );
}
// calculate the jacobian
if(metrics_request_flag & V_QUAD_JACOBIAN)
{
metric_vals->jacobian = VERDICT_MIN(
VERDICT_MIN( areas[0], areas[1] ),
VERDICT_MIN( areas[2], areas[3] ) );
}
if( metrics_request_flag & ( V_QUAD_SCALED_JACOBIAN | V_QUAD_SHEAR | V_QUAD_SHEAR_AND_SIZE ) )
{
double scaled_jac, min_scaled_jac = VERDICT_DBL_MAX;
if( lengths[0] < VERDICT_DBL_MIN ||
lengths[1] < VERDICT_DBL_MIN ||
lengths[2] < VERDICT_DBL_MIN ||
lengths[3] < VERDICT_DBL_MIN )
{
metric_vals->scaled_jacobian = 0.0;
metric_vals->shear = 0.0;
}
else
{
scaled_jac = areas[0] / (lengths[0] * lengths[3]);
min_scaled_jac = VERDICT_MIN( scaled_jac, min_scaled_jac );
scaled_jac = areas[1] / (lengths[1] * lengths[0]);
min_scaled_jac = VERDICT_MIN( scaled_jac, min_scaled_jac );
scaled_jac = areas[2] / (lengths[2] * lengths[1]);
min_scaled_jac = VERDICT_MIN( scaled_jac, min_scaled_jac );
scaled_jac = areas[3] / (lengths[3] * lengths[2]);
min_scaled_jac = VERDICT_MIN( scaled_jac, min_scaled_jac );
metric_vals->scaled_jacobian = min_scaled_jac;
//what the heck...set shear as well
if( min_scaled_jac <= VERDICT_DBL_MIN )
metric_vals->shear = 0.0;
else
metric_vals->shear = min_scaled_jac;
}
}
if( metrics_request_flag & (V_QUAD_WARPAGE | V_QUAD_ODDY) )
{
VerdictVector corner_normals[4];
corner_normals[0] = edges[3] * edges[0];
corner_normals[1] = edges[0] * edges[1];
corner_normals[2] = edges[1] * edges[2];
corner_normals[3] = edges[2] * edges[3];
if( metrics_request_flag & V_QUAD_ODDY )
{
double oddy, max_oddy = 0.0;
double diff, dot_prod;
double length_squared[4];
length_squared[0] = corner_normals[0].length_squared();
length_squared[1] = corner_normals[1].length_squared();
length_squared[2] = corner_normals[2].length_squared();
length_squared[3] = corner_normals[3].length_squared();
if( length_squared[0] < VERDICT_DBL_MIN ||
length_squared[1] < VERDICT_DBL_MIN ||
length_squared[2] < VERDICT_DBL_MIN ||
length_squared[3] < VERDICT_DBL_MIN )
metric_vals->oddy = VERDICT_DBL_MAX;
else
{
diff = (lengths[0]*lengths[0]) - (lengths[1]*lengths[1]);
dot_prod = edges[0]%edges[1];
oddy = ((diff*diff) + 4*dot_prod*dot_prod ) / (2*length_squared[1]);
max_oddy = VERDICT_MAX( oddy, max_oddy );
diff = (lengths[1]*lengths[1]) - (lengths[2]*lengths[2]);
dot_prod = edges[1]%edges[2];
oddy = ((diff*diff) + 4*dot_prod*dot_prod ) / (2*length_squared[2]);
max_oddy = VERDICT_MAX( oddy, max_oddy );
diff = (lengths[2]*lengths[2]) - (lengths[3]*lengths[3]);
dot_prod = edges[2]%edges[3];
oddy = ((diff*diff) + 4*dot_prod*dot_prod ) / (2*length_squared[3]);
max_oddy = VERDICT_MAX( oddy, max_oddy );
diff = (lengths[3]*lengths[3]) - (lengths[0]*lengths[0]);
dot_prod = edges[3]%edges[0];
oddy = ((diff*diff) + 4*dot_prod*dot_prod ) / (2*length_squared[0]);
max_oddy = VERDICT_MAX( oddy, max_oddy );
metric_vals->oddy = max_oddy;
}
}
if( metrics_request_flag & V_QUAD_WARPAGE )
{
if( corner_normals[0].normalize() < VERDICT_DBL_MIN ||
corner_normals[1].normalize() < VERDICT_DBL_MIN ||
corner_normals[2].normalize() < VERDICT_DBL_MIN ||
corner_normals[3].normalize() < VERDICT_DBL_MIN )
metric_vals->warpage = VERDICT_DBL_MAX;
else
{
metric_vals->warpage = pow(
VERDICT_MIN( corner_normals[0]%corner_normals[2],
corner_normals[1]%corner_normals[3]), 3 );
}
}
}
if( metrics_request_flag & V_QUAD_STRETCH )
{
VerdictVector temp;
temp.set( coordinates[2][0] - coordinates[0][0],
coordinates[2][1] - coordinates[0][1],
coordinates[2][2] - coordinates[0][2]);
double diag02 = temp.length_squared();
temp.set( coordinates[3][0] - coordinates[1][0],
coordinates[3][1] - coordinates[1][1],
coordinates[3][2] - coordinates[1][2]);
double diag13 = temp.length_squared();
static const double QUAD_STRETCH_FACTOR = sqrt(2.0);
// 'diag02' is now the max diagonal of the quad
diag02 = VERDICT_MAX( diag02, diag13 );
if( diag02 < VERDICT_DBL_MIN )
metric_vals->stretch = VERDICT_DBL_MAX;
else
metric_vals->stretch = QUAD_STRETCH_FACTOR *
VERDICT_MIN(
VERDICT_MIN( lengths[0], lengths[1] ),
VERDICT_MIN( lengths[2], lengths[3] ) ) /
sqrt(diag02);
}
if(metrics_request_flag & (V_QUAD_CONDITION | V_QUAD_SHAPE | V_QUAD_SHAPE_AND_SIZE ) )
{
double lengths_squared[4];
lengths_squared[0] = edges[0].length_squared();
lengths_squared[1] = edges[1].length_squared();
lengths_squared[2] = edges[2].length_squared();
lengths_squared[3] = edges[3].length_squared();
if( areas[0] < VERDICT_DBL_MIN ||
areas[1] < VERDICT_DBL_MIN ||
areas[2] < VERDICT_DBL_MIN ||
areas[3] < VERDICT_DBL_MIN )
{
metric_vals->condition = VERDICT_DBL_MAX;
metric_vals->shape= VERDICT_DBL_MAX;
}
else
{
double max_condition = 0.0, condition;
condition = (lengths_squared[0] + lengths_squared[3])/areas[0];
max_condition = VERDICT_MAX( max_condition, condition );
condition = (lengths_squared[1] + lengths_squared[0])/areas[1];
max_condition = VERDICT_MAX( max_condition, condition );
condition = (lengths_squared[2] + lengths_squared[1])/areas[2];
max_condition = VERDICT_MAX( max_condition, condition );
condition = (lengths_squared[3] + lengths_squared[2])/areas[3];
max_condition = VERDICT_MAX( max_condition, condition );
metric_vals->condition = 0.5*max_condition;
metric_vals->shape = 2/max_condition;
}
}
if(metrics_request_flag & V_QUAD_AREA )
{
if( metric_vals->area > 0 )
metric_vals->area = (VERDICT_REAL) VERDICT_MIN( metric_vals->area, VERDICT_DBL_MAX );
metric_vals->area = (VERDICT_REAL) VERDICT_MAX( metric_vals->area, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_ASPECT )
{
if( metric_vals->aspect > 0 )
metric_vals->aspect = (VERDICT_REAL) VERDICT_MIN( metric_vals->aspect, VERDICT_DBL_MAX );
metric_vals->aspect = (VERDICT_REAL) VERDICT_MAX( metric_vals->aspect, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_CONDITION )
{
if( metric_vals->condition > 0 )
metric_vals->condition = (VERDICT_REAL) VERDICT_MIN( metric_vals->condition, VERDICT_DBL_MAX );
metric_vals->condition = (VERDICT_REAL) VERDICT_MAX( metric_vals->condition, -VERDICT_DBL_MAX );
}
// calculate distortion
if(metrics_request_flag & V_QUAD_DISTORTION)
{
metric_vals->distortion = v_quad_distortion(num_nodes, coordinates);
if( metric_vals->distortion > 0 )
metric_vals->distortion = (VERDICT_REAL) VERDICT_MIN( metric_vals->distortion, VERDICT_DBL_MAX );
metric_vals->distortion = (VERDICT_REAL) VERDICT_MAX( metric_vals->distortion, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_JACOBIAN )
{
if( metric_vals->jacobian > 0 )
metric_vals->jacobian = (VERDICT_REAL) VERDICT_MIN( metric_vals->jacobian, VERDICT_DBL_MAX );
metric_vals->jacobian = (VERDICT_REAL) VERDICT_MAX( metric_vals->jacobian, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_MAXIMUM_ANGLE )
{
if( metric_vals->maximum_angle > 0 )
metric_vals->maximum_angle = (VERDICT_REAL) VERDICT_MIN( metric_vals->maximum_angle, VERDICT_DBL_MAX );
metric_vals->maximum_angle = (VERDICT_REAL) VERDICT_MAX( metric_vals->maximum_angle, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_MINIMUM_ANGLE )
{
if( metric_vals->minimum_angle > 0 )
metric_vals->minimum_angle = (VERDICT_REAL) VERDICT_MIN( metric_vals->minimum_angle, VERDICT_DBL_MAX );
metric_vals->minimum_angle = (VERDICT_REAL) VERDICT_MAX( metric_vals->minimum_angle, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_ODDY )
{
if( metric_vals->oddy > 0 )
metric_vals->oddy = (VERDICT_REAL) VERDICT_MIN( metric_vals->oddy, VERDICT_DBL_MAX );
metric_vals->oddy = (VERDICT_REAL) VERDICT_MAX( metric_vals->oddy, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_RELATIVE_SIZE_SQUARED )
{
if( metric_vals->relative_size_squared> 0 )
metric_vals->relative_size_squared = (VERDICT_REAL) VERDICT_MIN( metric_vals->relative_size_squared, VERDICT_DBL_MAX );
metric_vals->relative_size_squared = (VERDICT_REAL) VERDICT_MAX( metric_vals->relative_size_squared, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_SCALED_JACOBIAN )
{
if( metric_vals->scaled_jacobian> 0 )
metric_vals->scaled_jacobian = (VERDICT_REAL) VERDICT_MIN( metric_vals->scaled_jacobian, VERDICT_DBL_MAX );
metric_vals->scaled_jacobian = (VERDICT_REAL) VERDICT_MAX( metric_vals->scaled_jacobian, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_SHEAR )
{
if( metric_vals->shear > 0 )
metric_vals->shear = (VERDICT_REAL) VERDICT_MIN( metric_vals->shear, VERDICT_DBL_MAX );
metric_vals->shear = (VERDICT_REAL) VERDICT_MAX( metric_vals->shear, -VERDICT_DBL_MAX );
}
// calculate shear and size
// reuse values from above
if(metrics_request_flag & V_QUAD_SHEAR_AND_SIZE)
{
metric_vals->shear_and_size = metric_vals->shear * metric_vals->relative_size_squared;
if( metric_vals->shear_and_size > 0 )
metric_vals->shear_and_size = (VERDICT_REAL) VERDICT_MIN( metric_vals->shear_and_size, VERDICT_DBL_MAX );
metric_vals->shear_and_size = (VERDICT_REAL) VERDICT_MAX( metric_vals->shear_and_size, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_SHAPE )
{
if( metric_vals->shape > 0 )
metric_vals->shape = (VERDICT_REAL) VERDICT_MIN( metric_vals->shape, VERDICT_DBL_MAX );
metric_vals->shape = (VERDICT_REAL) VERDICT_MAX( metric_vals->shape, -VERDICT_DBL_MAX );
}
// calculate shape and size
// reuse values from above
if(metrics_request_flag & V_QUAD_SHAPE_AND_SIZE)
{
metric_vals->shape_and_size = metric_vals->shape * metric_vals->relative_size_squared;
if( metric_vals->shape_and_size > 0 )
metric_vals->shape_and_size = (VERDICT_REAL) VERDICT_MIN( metric_vals->shape_and_size, VERDICT_DBL_MAX );
metric_vals->shape_and_size = (VERDICT_REAL) VERDICT_MAX( metric_vals->shape_and_size, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_SKEW )
{
if( metric_vals->skew > 0 )
metric_vals->skew = (VERDICT_REAL) VERDICT_MIN( metric_vals->skew, VERDICT_DBL_MAX );
metric_vals->skew = (VERDICT_REAL) VERDICT_MAX( metric_vals->skew, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_STRETCH )
{
if( metric_vals->stretch > 0 )
metric_vals->stretch = (VERDICT_REAL) VERDICT_MIN( metric_vals->stretch, VERDICT_DBL_MAX );
metric_vals->stretch = (VERDICT_REAL) VERDICT_MAX( metric_vals->stretch, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_TAPER )
{
if( metric_vals->taper > 0 )
metric_vals->taper = (VERDICT_REAL) VERDICT_MIN( metric_vals->taper, VERDICT_DBL_MAX );
metric_vals->taper = (VERDICT_REAL) VERDICT_MAX( metric_vals->taper, -VERDICT_DBL_MAX );
}
if(metrics_request_flag & V_QUAD_WARPAGE )
{
if( metric_vals->warpage > 0 )
metric_vals->warpage = (VERDICT_REAL) VERDICT_MIN( metric_vals->warpage, VERDICT_DBL_MAX );
metric_vals->warpage = (VERDICT_REAL) VERDICT_MAX( metric_vals->warpage, -VERDICT_DBL_MAX );
}
}
void cofactor(float num[25][25], float f)
{
float b[25][25], fac[25][25];
int p, q, m, n, i, j;
for (q = 0;q < f; q++)
{
for (p = 0;p < f; p++)
{
m = 0;
n = 0;
for (i = 0;i < f; i++)
{
for (j = 0;j < f; j++)
{
if (i != q && j != p)
{
b[m][n] = num[i][j];
if (n < (f - 2))
n++;
else
{
n = 0;
m++;
}
}
}
}
fac[q][p] = pow(-1, q + p) * determinant(b, f - 1);
}
}
transpose(num, fac, f);//calling the function transpose to transpose the matrix
}
inline typename result_of::adjugate_at_index<Row,Column,Matrix>::type adjugate_at_index( const Matrix& m )
{
matrix_minor<Matrix, Column, Row> mm( m );
return power_c<-1,Row+Column>::value * determinant( mm );
}
Ref speedtest_cmd(ref_list args){
puts("Starting test...");
bool doplot = true; keepgoing = true;
Fun f = (Fun) args->list[0]->inst;
int min = (int) (*(float*) ((Var)args->list[1]->inst)->val);
int max = (int) (*(float*) ((Var)args->list[2]->inst)->val);
int step = (int) (*(float*) ((Var)args->list[3]->inst)->val);
if ( min > max || step == 0) {
set_err(ENOVAL, "infinite tests are not allowed");
return NULL;
}
/* Optional parameter */
if (args->length > 4 && cmptype_ref(FLOAT, args->list[4])) {
int countdown = (int) (*(float*) ((Var)args->list[4]->inst)->val);
signal(SIGALRM, stop_speedtest);
alarm(countdown);
}
/* Points list */
clock_t* y_time = malloc (sizeof(clock_t) * (max-min)/step + 1);
int* x_size = malloc (sizeof(int) * (max-min)/step + 1);
if (y_time == NULL || x_size == NULL){
free(x_size); free(y_time);
return NULL;
}
int i,j;
for ( i = min, j=0; i <= max && keepgoing ; i += step, j++){
/* Generate matrices.
Note: they don't need to be initialized
*/
Matrix m1 = newMatrix(i,i), m2 = newMatrix(i,i), ret = NULL;
float* f1 = malloc(sizeof(float));
if (m1 == NULL || m2 == NULL || f1 == NULL){
perror("");
keepgoing = false; doplot = false;
}
/* Start test */
clock_t start = clock();
if (f->fun == mult_call) {
ret = multiplication(m1,m2);
} else if ( f->fun == sub_call) {
ret = soustraction(m1,m2);
} else if (f->fun == addition_call){
ret = addition(m1,m2);
} else if (f->fun == transpose_call){
ret = transpose(m1);
} else if (f->fun == determinant_call){
determinant(m1, f1);
} else if (f->fun == invert_call){
invert(m1,&ret);
} else if (f->fun == rank_call ){
rank(m1);
} else {
printf("Sorry...cannot test this function\n");
keepgoing = false; doplot = false;
}
/* Finish test */
clock_t end = clock();
/* Store result */
x_size[j] = i; y_time[j] = end-start;
dropMatrix(m1);
dropMatrix(m2);
dropMatrix(ret);
free(f1);
}
alarm(0); // Remove countdown
if (doplot == true) plot(args->list[0]->name, x_size, y_time, j);
free(x_size); free(y_time);
return NO_REF;
}
Mat2 Mat2::inverse()
{
return Mat2( d11,-d10,
-d01, d00)
/determinant();
}
static void compute(Mat A, Mat B, int n, Mat& Rotation, Mat& Translation, bool& RT_flag){
vector<int> newvec;
vector<float> Ratio, inliers_num;
vector<Mat> R_models , t_models;
int rows =A.rows;
float threshold=0.1;
RT_flag = true;
//generate n models
for(int Round = 0; Round<n; Round++){
int inliers_count=0;
//randomly select 4 pairs of points and estimate the model
newvec=shuffle(int(A.rows));
//cout<<"NWVEC "<<newvec[0]<<" "<<newvec[1]<<" " <<newvec[2] << " " <<newvec[3]<<endl;
int select = 4;
// if (rows > 4)
// select = 2*rows /3;
Mat points_curr,points_prev;
for (int j=0; j<select; ++j){
int index = newvec[j];
//cout<<"index" << index<<endl;
points_prev.push_back(B.row(index));
points_curr.push_back(A.row(index));
}
//cout<<"points_prev \n"<<points_prev<<endl;
//cout<<"points_curr \n"<<points_curr<<endl;
//model estimation
Mat R,t;
poseEstimate::poseestimate::compute(points_curr.t(),points_prev.t(),R,t);
//model generation done
//use the generated model to test all the samples to count outliers and inliers
for (int i=0 ; i<rows; ++i){
Mat point_p = B.row(i);
Mat point_c = A.row(i);
float error;
checkModel(point_p.t(),point_c.t(),R,t,error);
//cout<<" ---ERROR--> "<<error;
// if error is less than threshold inliers_count++
if (error < threshold){
inliers_count +=1;
}
}
//save Ratio, R and t
float ratio=inliers_count*1.0/rows;
Ratio.push_back(ratio);
inliers_num.push_back(inliers_count);
R_models.push_back(R);
t_models.push_back(t);
}
//ROS_INFO("%d many models created, select best one!",n);
int index=0;
float max=0.0;
for (int i=0; i<n; i++){
if (max < Ratio[i]){
max = Ratio[i];
index = i;
}
}
cout<<"best fit model has a ratio = "<<max<<endl;
//best model
float t_limit = 6.0;
float R_limit = 0.1;
if (max == 0){
RT_flag = false;
Rotation = R_models[index];
Translation = t_models[index];
cout<< "estimated t_limit "<<Translation.dot(Translation)<<endl;
cout<< "estimated R_limit "<<determinant(Rotation)<<endl;
cout<<"MAX = 0!!!"<<endl;
} else if (inliers_num[index] >= 4) {
//Select inlier points according to the best model and then generate an accurate model using all inlier points
Mat inlierA, inlierB;
for (int i=0 ; i<rows; ++i){
Mat point_p = B.row(i);
Mat point_c = A.row(i);
float error;
checkModel(point_p.t(),point_c.t(),R_models[index],t_models[index],error);
//cout<<" ---ERROR--> "<<error;
// if error is less than threshold inliers_count++
if (error < threshold){
inlierA.push_back(point_c);
inlierB.push_back(point_p);
}
}
poseEstimate::poseestimate::compute(inlierA.t(),inlierB.t(),Rotation,Translation);
} else{
Rotation = R_models[index];
Translation = t_models[index];
}
if(Translation.dot(Translation) > t_limit || abs( determinant(Rotation) ) < R_limit){
RT_flag = false;
cout<< "estimated t_limit "<<Translation.dot(Translation)<<endl;
cout<< "estimated R_limit "<<determinant(Rotation)<<endl;
}
}
/**************************************************
matlab resouce:http://www.robots.ox.ac.uk/~vgg/hzbook/code/
vgg_F_from_P.m
*****************************************************/
void AbstractReconstructor::ProjectionToFundamentalMatrix(cv::Mat& mat1, cv::Mat& mat2, cv::Mat& result)
{
cv::Mat X1,X2,X3,Y1,Y2,Y3;
X1.create(2,4,CV_32F);
X2.create(2,4,CV_32F);
X3.create(2,4,CV_32F);
Y1.create(2,4,CV_32F);
Y2.create(2,4,CV_32F);
Y3.create(2,4,CV_32F);
//mat1(1,2) copy to X1
mat1.row(1).copyTo(X1.row(0));
mat1.row(2).copyTo(X1.row(1));
//mat1(2,0) copy to X2
mat1.row(2).copyTo(X2.row(0));
mat1.row(0).copyTo(X2.row(1));
//mat1(0,1) copy to X2
mat1.row(0).copyTo(X3.row(0));
mat1.row(1).copyTo(X3.row(1));
//mat2(1,2) copy to Y1
mat2.row(1).copyTo(Y1.row(0));
mat2.row(2).copyTo(Y1.row(1));
//mat2(2,0) copy to Y2
mat2.row(2).copyTo(Y2.row(0));
mat2.row(0).copyTo(Y2.row(1));
//mat2(0,1) copy to Y2
mat2.row(0).copyTo(Y3.row(0));
mat2.row(1).copyTo(Y3.row(1));
cv::Mat temp;
temp.create(4,4, CV_32F);
//calculate result[0,0]
temp.row(0) = X1.row(0);
temp.row(1) = X1.row(1);
temp.row(2) = Y1.row(0);
temp.row(3) = Y1.row(1);
result.at<float>(0, 0) = determinant(temp);
//calculate result[0,1]
temp.row(0) = X2.row(0);
temp.row(1) = X2.row(1);
temp.row(2) = Y1.row(0);
temp.row(3) = Y1.row(1);
result.at<float>(0, 1) = determinant(temp);
//calculate result[0,2]
temp.row(0) = X3.row(0);
temp.row(1) = X3.row(1);
temp.row(2) = Y1.row(0);
temp.row(3) = Y1.row(1);
result.at<float>(0, 2) = determinant(temp);
//calculate result[1,0]
temp.row(0) = X1.row(0);
temp.row(1) = X1.row(1);
temp.row(2) = Y2.row(0);
temp.row(3) = Y2.row(1);
result.at<float>(1, 0) = determinant(temp);
//calculate result[1,1]
temp.row(0) = X2.row(0);
temp.row(1) = X2.row(1);
temp.row(2) = Y2.row(0);
temp.row(3) = Y2.row(1);
result.at<float>(1, 1) = determinant(temp);
//calculate result[1,2]
temp.row(0) = X3.row(0);
temp.row(1) = X3.row(1);
temp.row(2) = Y2.row(0);
temp.row(3) = Y2.row(1);
result.at<float>(1, 2) = determinant(temp);
//calculate result[2,0]
temp.row(0) = X1.row(0);
temp.row(1) = X1.row(1);
temp.row(2) = Y3.row(0);
temp.row(3) = Y3.row(1);
result.at<float>(2, 0) = determinant(temp);
//calculate result[2,1]
temp.row(0) = X2.row(0);
temp.row(1) = X2.row(1);
temp.row(2) = Y3.row(0);
temp.row(3) = Y3.row(1);
result.at<float>(2, 1) = determinant(temp);
//calculate result[2,2]
temp.row(0) = X3.row(0);
temp.row(1) = X3.row(1);
temp.row(2) = Y3.row(0);
temp.row(3) = Y3.row(1);
result.at<float>(2, 2) = determinant(temp);
return ;
}
Matrix CramersRuleInverse(const Matrix& mat)
{
return (CofactorMatrix(mat)).transp()/determinant(mat);
}
void in_sphere(float xyz[4][3], float xyz_in[3], float &radius_in)
{
float matrix_1[4][4], matrix_2[4][4], rhs[4], det_matrix_1, sub_det[4];
float norm[4][3];
int i, j, l;
// get outward facing normals for each face of this tet
get_normals(xyz,norm);
// pack the matrix equations
for ( j = 0 ; j <= 2 ; j++ ) {
matrix_1[0][j] = norm[0][j];
matrix_1[1][j] = norm[1][j];
matrix_1[2][j] = norm[2][j];
matrix_1[3][j] = norm[3][j];
}
for ( i = 0 ; i <= 3 ; i++ ) {
matrix_1[i][3] = 1.;
}
// pack the right-handside
rhs[0] = norm[0][0]*xyz[0][0] + norm[0][1]*xyz[0][1] + norm[0][2]*xyz[0][2];
rhs[1] = norm[1][0]*xyz[3][0] + norm[1][1]*xyz[3][1] + norm[1][2]*xyz[3][2];
rhs[2] = norm[2][0]*xyz[3][0] + norm[2][1]*xyz[3][1] + norm[2][2]*xyz[3][2];
rhs[3] = norm[3][0]*xyz[3][0] + norm[3][1]*xyz[3][1] + norm[3][2]*xyz[3][2];
// get determinant of original 4x4 matrix
determinant(matrix_1,&det_matrix_1);
// solve system of equations using Kramer's rule
if ( det_matrix_1 != 0. ) {
for ( l = 0 ; l <= 3 ; l++ ) {
// grab a copy of the original matrix
for ( i = 0 ; i <= 3 ; i++ ) {
for ( j = 0 ; j <= 3 ; j++ ) {
matrix_2[i][j] = matrix_1[i][j];
}
}
// fill the l'th column with right-hand-side vector
for ( i = 0 ; i <= 3 ; i++ ) {
matrix_2[i][l] = rhs[i];
}
// solve for determinant of this system
determinant(matrix_2,&sub_det[l]);
}
// solve for the unknowns xyz_in and radius_in
for ( j = 0 ; j <= 2 ; j++ ) {
xyz_in[j] = sub_det[j]/det_matrix_1;
}
// solve for radius
radius_in = sub_det[3]/det_matrix_1;
}
else {
radius_in = -1.;
}
}
void BestOf2NearestMatcher::match(const ImageFeatures &features1, const ImageFeatures &features2,
MatchesInfo &matches_info)
{
(*impl_)(features1, features2, matches_info);
// Check if it makes sense to find homography
if (matches_info.matches.size() < static_cast<size_t>(num_matches_thresh1_))
return;
// Construct point-point correspondences for homography estimation
Mat src_points(1, static_cast<int>(matches_info.matches.size()), CV_32FC2);
Mat dst_points(1, static_cast<int>(matches_info.matches.size()), CV_32FC2);
for (size_t i = 0; i < matches_info.matches.size(); ++i)
{
const DMatch& m = matches_info.matches[i];
Point2f p = features1.keypoints[m.queryIdx].pt;
p.x -= features1.img_size.width * 0.5f;
p.y -= features1.img_size.height * 0.5f;
src_points.at<Point2f>(0, static_cast<int>(i)) = p;
p = features2.keypoints[m.trainIdx].pt;
p.x -= features2.img_size.width * 0.5f;
p.y -= features2.img_size.height * 0.5f;
dst_points.at<Point2f>(0, static_cast<int>(i)) = p;
}
// Find pair-wise motion
matches_info.H = findHomography(src_points, dst_points, matches_info.inliers_mask, CV_RANSAC);
if (matches_info.H.empty() || std::abs(determinant(matches_info.H)) < std::numeric_limits<double>::epsilon())
return;
// Find number of inliers
matches_info.num_inliers = 0;
for (size_t i = 0; i < matches_info.inliers_mask.size(); ++i)
if (matches_info.inliers_mask[i])
matches_info.num_inliers++;
// These coeffs are from paper M. Brown and D. Lowe. "Automatic Panoramic Image Stitching
// using Invariant Features"
matches_info.confidence = matches_info.num_inliers / (8 + 0.3 * matches_info.matches.size());
// Set zero confidence to remove matches between too close images, as they don't provide
// additional information anyway. The threshold was set experimentally.
matches_info.confidence = matches_info.confidence > 3. ? 0. : matches_info.confidence;
// Check if we should try to refine motion
if (matches_info.num_inliers < num_matches_thresh2_)
return;
// Construct point-point correspondences for inliers only
src_points.create(1, matches_info.num_inliers, CV_32FC2);
dst_points.create(1, matches_info.num_inliers, CV_32FC2);
int inlier_idx = 0;
for (size_t i = 0; i < matches_info.matches.size(); ++i)
{
if (!matches_info.inliers_mask[i])
continue;
const DMatch& m = matches_info.matches[i];
Point2f p = features1.keypoints[m.queryIdx].pt;
p.x -= features1.img_size.width * 0.5f;
p.y -= features1.img_size.height * 0.5f;
src_points.at<Point2f>(0, inlier_idx) = p;
p = features2.keypoints[m.trainIdx].pt;
p.x -= features2.img_size.width * 0.5f;
p.y -= features2.img_size.height * 0.5f;
dst_points.at<Point2f>(0, inlier_idx) = p;
inlier_idx++;
}
// Rerun motion estimation on inliers only
matches_info.H = findHomography(src_points, dst_points, CV_RANSAC);
}
void circ_sphere(float xyz[4][3], float xyz_out[3], float &radius_out)
{
float matrix_1[4][4], matrix_2[4][4], rhs[4], det_matrix_1, sub_det[4];
int i, j, l;
// pack the matrix equations
for ( j = 0 ; j <= 2 ; j++ ) {
matrix_1[0][j] = -2.*xyz[0][j];
matrix_1[1][j] = -2.*xyz[1][j];
matrix_1[2][j] = -2.*xyz[2][j];
matrix_1[3][j] = -2.*xyz[3][j];
}
for ( i = 0 ; i <= 3 ; i++ ) {
matrix_1[i][3] = 1.;
}
// pack the right-handside
rhs[0] = -( SQR(xyz[0][0]) + SQR(xyz[0][1]) + SQR(xyz[0][2]) );
rhs[1] = -( SQR(xyz[1][0]) + SQR(xyz[1][1]) + SQR(xyz[1][2]) );
rhs[2] = -( SQR(xyz[2][0]) + SQR(xyz[2][1]) + SQR(xyz[2][2]) );
rhs[3] = -( SQR(xyz[3][0]) + SQR(xyz[3][1]) + SQR(xyz[3][2]) );
// get determinant of original 4x4 matrix
determinant(matrix_1,&det_matrix_1);
// solve system of equations using Kramer's rule
if ( det_matrix_1 != 0. ) {
for ( l = 0 ; l <= 2 ; l++ ) {
// grab a copy of the original matrix
for ( i = 0 ; i <= 3 ; i++ ) {
for (j = 0 ; j <= 3 ; j++ ) {
matrix_2[i][j] = matrix_1[i][j];
}
}
// fill the l'th column with righthandside vector
for ( i = 0 ; i <= 3 ; i++ ) {
matrix_2[i][l] = rhs[i];
}
// find the determinant of this matrix
determinant(matrix_2,&sub_det[l]);
}
// solve for the unknowns xyz_out[] and the radius squared
radius_out = 0.;
for ( j = 0 ; j <= 2 ; j++ ) {
xyz_out[j] = sub_det[j]/det_matrix_1;
radius_out = radius_out + SQR((xyz[0][j] - xyz_out[j]));
}
// the above radius is radius squared
radius_out = sqrt(radius_out);
}
else {
radius_out = -1.;
}
}
bool Mat4::decompose(Vec3* scale, Quaternion* rotation, Vec3* translation) const
{
if (translation)
{
// Extract the translation.
translation->x = m[12];
translation->y = m[13];
translation->z = m[14];
}
// Nothing left to do.
if (scale == nullptr && rotation == nullptr)
return true;
// Extract the scale.
// This is simply the length of each axis (row/column) in the matrix.
Vec3 xaxis(m[0], m[1], m[2]);
float scaleX = xaxis.length();
Vec3 yaxis(m[4], m[5], m[6]);
float scaleY = yaxis.length();
Vec3 zaxis(m[8], m[9], m[10]);
float scaleZ = zaxis.length();
// Determine if we have a negative scale (true if determinant is less than zero).
// In this case, we simply negate a single axis of the scale.
float det = determinant();
if (det < 0)
scaleZ = -scaleZ;
if (scale)
{
scale->x = scaleX;
scale->y = scaleY;
scale->z = scaleZ;
}
// Nothing left to do.
if (rotation == nullptr)
return true;
// Scale too close to zero, can't decompose rotation.
if (scaleX < MATH_TOLERANCE || scaleY < MATH_TOLERANCE || fabs(scaleZ) < MATH_TOLERANCE)
return false;
float rn;
// Factor the scale out of the matrix axes.
rn = 1.0f / scaleX;
xaxis.x *= rn;
xaxis.y *= rn;
xaxis.z *= rn;
rn = 1.0f / scaleY;
yaxis.x *= rn;
yaxis.y *= rn;
yaxis.z *= rn;
rn = 1.0f / scaleZ;
zaxis.x *= rn;
zaxis.y *= rn;
zaxis.z *= rn;
// Now calculate the rotation from the resulting matrix (axes).
float trace = xaxis.x + yaxis.y + zaxis.z + 1.0f;
if (trace > MATH_EPSILON)
{
float s = 0.5f / sqrt(trace);
rotation->w = 0.25f / s;
rotation->x = (yaxis.z - zaxis.y) * s;
rotation->y = (zaxis.x - xaxis.z) * s;
rotation->z = (xaxis.y - yaxis.x) * s;
}
else
{
// Note: since xaxis, yaxis, and zaxis are normalized,
// we will never divide by zero in the code below.
if (xaxis.x > yaxis.y && xaxis.x > zaxis.z)
{
float s = 0.5f / sqrt(1.0f + xaxis.x - yaxis.y - zaxis.z);
rotation->w = (yaxis.z - zaxis.y) * s;
rotation->x = 0.25f / s;
rotation->y = (yaxis.x + xaxis.y) * s;
rotation->z = (zaxis.x + xaxis.z) * s;
}
else if (yaxis.y > zaxis.z)
{
float s = 0.5f / sqrt(1.0f + yaxis.y - xaxis.x - zaxis.z);
rotation->w = (zaxis.x - xaxis.z) * s;
rotation->x = (yaxis.x + xaxis.y) * s;
rotation->y = 0.25f / s;
rotation->z = (zaxis.y + yaxis.z) * s;
}
else
{
float s = 0.5f / sqrt(1.0f + zaxis.z - xaxis.x - yaxis.y );
rotation->w = (xaxis.y - yaxis.x ) * s;
rotation->x = (zaxis.x + xaxis.z ) * s;
rotation->y = (zaxis.y + yaxis.z ) * s;
rotation->z = 0.25f / s;
}
}
return true;
}
void menu(int m1, int n1, int m2, int n2, int m3, int n3)
{
printf("\n1.Read an Array.\n2.Print an Array.\n3.Addition of 2 Arrays.\n4.Substraction of 2 Arrays.\n5.Multiplication of 2 Arrays.\n6.Multiplication of an Array by a Number.\n7.Transpose of a Matrix.\n8.Determinant of a Matrix.\n9.Exit.\n");
scanf("%d",&f);
switch (f) {
case 1:
read(x, &m1, &n1, y, &m2, &n2, z, &m3, &n3);
menu(m1, n1, m2, n2, m3, n3);
break;
case 2:
printa(x, m1, n1, y, m2, n2, z, m3, n3);
printf("\n1.Menu.\n2.Exit.\n");
scanf("%d",&f);
switch (f) {
case 1:
menu(m1, n1, m2, n2, m3, n3);
break;
case 2:
break;
default:
break;
}
break;
case 3:
Addition(x, m1, n1, y, m2, n2, z, m3, n3, &m1, &n1, &m2, &n2, &m3, &n3);
menu(m1, n1, m2, n2, m3, n3);
break;
case 4:
substraction(x, m1, n1, y, m2, n2, z, m3, n3, &m1, &n1, &m2, &n2, &m3, &n3);
menu(m1, n1, m2, n2, m3, n3);
break;
case 5:
multiplication_array(x, m1, n1, y, m2, n2, z, m3, n3, &m1, &n1, &m2, &n2, &m3, &n3);
menu(m1, n1, m2, n2, m3, n3);
break;
case 6:
multiplaction_number(x, m1, n1, y, m2, n2, z, m3, n3, &m1, &n1, &m2, &n2, &m3, &n3);
menu(m1, n1, m2, n2, m3, n3);
break;
case 7:
transpose(x, m1, n1, y, m2, n2, z, m3, n3, &m1, &n1, &m2, &n2, &m3, &n3);
menu(m1, n1, m2, n2, m3, n3);
break;
case 8:
determinant(x, m1, n1, y, m2, n2, z, m3, n3, &m1, &n1, &m2, &n2, &m3, &n3);
menu(m1, n1, m2, n2, m3, n3);
break;
case 9:
break;
default:
break;
}
}
float3 characteristic_poly(mat3 p)
{
float2 v1 = (p.v[0].xy * p.v[1].yx) + (p.v[0].xz * p.v[2].zx) + (p.v[1].yz * p.v[2].zy);
return float3(-determinant(p), v1.x - v1.y, -trace(p));
}
/* Find eigenvalues of 3x3 symmetric system by solving cubic. */
void
ch_evals3 (
double H[3][3], /* 3x3 sym matrix for lowest eigenvalue */
double *eval1, /* smallest eigenvalue */
double *eval2, /* middle eigenvalue */
double *eval3 /* largest eigenvalue */
)
{
double mat[3][3]; /* scaled version of H */
double a1, a2, a3; /* coefficents of cubic equation */
double q, r; /* intermediate terms */
double q3, r2; /* powers of q and r */
double theta; /* angular parameter */
double root1, root2, root3; /* three roots of cubic */
double tol = 1.0e-6; /* allowed deviation */
double xmax; /* largest matrix element for scaling */
int i, j; /* loop indices */
double determinant();
/* This first requires solving a cubic equation. */
/* Normalize to avoid any numerical problems. */
xmax = 0.0;
for (i = 0; i < 3; i++) {
for (j = i; j < 3; j++) {
if (fabs(H[i][j]) > xmax)
xmax = fabs(H[i][j]);
}
}
if (xmax != 0) {
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++)
mat[i][j] = H[i][j] / xmax;
}
}
a1 = -(mat[0][0] + mat[1][1] + mat[2][2]);
a2 = (mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]) +
(mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]) +
(mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]);
a3 = -determinant(mat, 3);
if (a3 == 0) {
root1 = 0;
/* Solve quadratic. */
q = -.5 * (a1 + sign(a1) * sqrt(a1 * a1 - 4 * a2));
root2 = q;
root3 = a2 / q;
}
else { /* solve cubic */
q = (a1 * a1 - 3 * a2) / 9;
r = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) / 54;
q3 = q * q * q;
r2 = r * r;
/* To avoid missing a root, check for roundoff. */
if ((q3 < r2) && fabs(q3 - r2) < tol * (fabs(q3) + fabs(r2))) {
q3 = r2;
}
if (q3 >= r2) { /* Three real roots. */
if (r == 0)
theta = HALFPI;
else {
q3 = sqrt(q3);
if (q3 < fabs(r))
q3 = fabs(r);
theta = acos(r / q3);
}
q = -2 * sqrt(q);
root1 = q * cos(theta / 3) - a1 / 3;
root2 = q * cos((theta + TWOPI) / 3) - a1 / 3;
root3 = q * cos((theta + 2 * TWOPI) / 3) - a1 / 3;
}
else { /* Only one real root. */
theta = sqrt(r2 - q3) + fabs(r);
theta = pow(theta, 1.0 / 3.0);
root1 = root2 = root3 = -sign(r) * (theta + q / theta) - a1 / 3;
}
}
root1 *= xmax;
root2 *= xmax;
root3 *= xmax;
*eval1 = min(root1, root2);
*eval1 = min(*eval1, root3);
*eval3 = max(root1, root2);
*eval3 = max(*eval3, root3);
if (root1 != *eval1 && root1 != *eval3)
*eval2 = root1;
else if (root2 != *eval1 && root2 != *eval3)
*eval2 = root2;
else
*eval2 = root3;
}
bool Matrix3::isSingular() const {
return fuzzyEQ(determinant(), 0);
}
bool FloatQuad::isCounterclockwise() const
{
// Return if the two first vectors are turning clockwise. If the quad is convex then all following vectors will turn the same way.
return determinant(m_p2 - m_p1, m_p3 - m_p2) < 0;
}
Vector3 Basis::get_scale_local() const {
real_t det_sign = SGN(determinant());
return det_sign * Vector3(elements[0].length(), elements[1].length(), elements[2].length());
}
/* ----------------------------- MNI Header -----------------------------------
@NAME : calculate_smoothness_ind
@INPUT : fwhm_info - pointer to data for calculating smoothness
in GLM model fit
lambda_buffer - pointer to residuals and info about smoothness
@OUTPUT : fwhm_gaussian->fwhm, smoothness, int_smoothness
@RETURNS : nothing
@DESCRIPTION: Calculates the sample covariance matrix at a given voxel
@CREATED : Apr. 20, 1997, J. Taylor
@MODIFIED :
---------------------------------------------------------------------------- */
void calculate_smoothness_ind(Fwhm_Info *fwhm_info, Lambda *lambda_buffer)
{
int ibuff, i, j;
double *data;
double avg, avg_sq;
avg_sq = 0.0;
for(i=0; i<lambda_buffer->num_dim; i++) {
for(j=0; j<=i; j++) {
fwhm_info->lambda->values[i][j] = 0.0;
}
}
for(ibuff=0; ibuff<lambda_buffer->num_buffers; ibuff++) {
data = fwhm_info->data[ibuff];
if(lambda_buffer->num_dim == 2) {
fwhm_info->deriv[0] = 0.5* ((data[lambda_buffer->index[3]] +
data[lambda_buffer->index[2]]) -
(data[lambda_buffer->index[1]] +
data[lambda_buffer->index[0]]));
fwhm_info->deriv[1] = 0.5 * ((data[lambda_buffer->index[3]] +
data[lambda_buffer->index[1]]) -
(data[lambda_buffer->index[2]] +
data[lambda_buffer->index[0]]));
}
else if(lambda_buffer->num_dim == 3) {
fwhm_info->deriv[0] = 0.25 * ((data[lambda_buffer->index[7]] +
data[lambda_buffer->index[6]] +
data[lambda_buffer->index[5]] +
data[lambda_buffer->index[4]]) -
(data[lambda_buffer->index[3]] +
data[lambda_buffer->index[2]] +
data[lambda_buffer->index[1]] +
data[lambda_buffer->index[0]]));
fwhm_info->deriv[1] = 0.25 * ((data[lambda_buffer->index[7]] +
data[lambda_buffer->index[6]] +
data[lambda_buffer->index[3]] +
data[lambda_buffer->index[2]]) -
(data[lambda_buffer->index[5]] +
data[lambda_buffer->index[4]] +
data[lambda_buffer->index[1]] +
data[lambda_buffer->index[0]]));
fwhm_info->deriv[2] = 0.25 * ((data[lambda_buffer->index[7]] +
data[lambda_buffer->index[5]] +
data[lambda_buffer->index[3]] +
data[lambda_buffer->index[1]]) -
(data[lambda_buffer->index[6]] +
data[lambda_buffer->index[4]] +
data[lambda_buffer->index[2]] +
data[lambda_buffer->index[0]]));
}
else if (lambda_buffer->num_dim == 4) {
fwhm_info->deriv[0] = 0.125 * ((data[lambda_buffer->index[15]] +
data[lambda_buffer->index[14]] +
data[lambda_buffer->index[13]] +
data[lambda_buffer->index[12]] +
data[lambda_buffer->index[11]] +
data[lambda_buffer->index[10]] +
data[lambda_buffer->index[9]] +
data[lambda_buffer->index[8]]) -
(data[lambda_buffer->index[7]] +
data[lambda_buffer->index[6]] +
data[lambda_buffer->index[5]] +
data[lambda_buffer->index[4]] +
data[lambda_buffer->index[3]] +
data[lambda_buffer->index[2]] +
data[lambda_buffer->index[1]] +
data[lambda_buffer->index[0]]));
fwhm_info->deriv[1] = 0.125 * ((data[lambda_buffer->index[15]] +
data[lambda_buffer->index[14]] +
data[lambda_buffer->index[13]] +
data[lambda_buffer->index[12]] +
data[lambda_buffer->index[7]] +
data[lambda_buffer->index[6]] +
data[lambda_buffer->index[5]] +
data[lambda_buffer->index[4]]) -
(data[lambda_buffer->index[11]] +
data[lambda_buffer->index[10]] +
data[lambda_buffer->index[9]] +
data[lambda_buffer->index[8]] +
data[lambda_buffer->index[3]] +
data[lambda_buffer->index[2]] +
data[lambda_buffer->index[1]] +
data[lambda_buffer->index[0]]));
fwhm_info->deriv[2] = 0.125 * ((data[lambda_buffer->index[15]] +
data[lambda_buffer->index[14]] +
data[lambda_buffer->index[11]] +
data[lambda_buffer->index[10]] +
data[lambda_buffer->index[7]] +
data[lambda_buffer->index[6]] +
data[lambda_buffer->index[3]] +
data[lambda_buffer->index[2]]) -
(data[lambda_buffer->index[13]] +
data[lambda_buffer->index[12]] +
data[lambda_buffer->index[9]] +
data[lambda_buffer->index[8]] +
data[lambda_buffer->index[5]] +
data[lambda_buffer->index[4]] +
data[lambda_buffer->index[1]] +
data[lambda_buffer->index[0]]));
fwhm_info->deriv[3] = 0.125 * ((data[lambda_buffer->index[15]] +
data[lambda_buffer->index[13]] +
data[lambda_buffer->index[11]] +
data[lambda_buffer->index[9]] +
data[lambda_buffer->index[7]] +
data[lambda_buffer->index[5]] +
data[lambda_buffer->index[3]] +
data[lambda_buffer->index[1]]) -
(data[lambda_buffer->index[14]] +
data[lambda_buffer->index[12]] +
data[lambda_buffer->index[10]] +
data[lambda_buffer->index[8]] +
data[lambda_buffer->index[6]] +
data[lambda_buffer->index[4]] +
data[lambda_buffer->index[2]] +
data[lambda_buffer->index[0]]));
}
if (fwhm_info->is_gaussian == FALSE) {
avg = 0.0;
for(i=0; i<lambda_buffer->num_check; i++) {
avg += data[lambda_buffer->index[i]];
}
avg /= lambda_buffer->num_check;
avg_sq += avg * avg;
}
for(i=0; i<lambda_buffer->num_dim; i++) {
for(j=0; j<=i; j++) {
fwhm_info->lambda->values[i][j] +=\
fwhm_info->deriv[i] * fwhm_info->deriv[j];
if (fwhm_info->lambda_pool != NULL) {
fwhm_info->lambda_pool->values[i][j] +=\
fwhm_info->deriv[i] * fwhm_info->deriv[j];
}
}
}
}
if (fwhm_info->is_gaussian == FALSE) {
for(i=0; i<lambda_buffer->num_dim; i++) {
for(j=0; j<=i; j++) {
fwhm_info->lambda->values[i][j] /= avg_sq;
}
}
}
for(i=0; i<lambda_buffer->num_dim; i++) {
for(j=lambda_buffer->num_dim-1; j>i; j--) {
fwhm_info->lambda->values[i][j] = fwhm_info->lambda->values[j][i];
}
}
fwhm_info->smoothness = sqrt(determinant(fwhm_info->lambda));
if(fwhm_info->smoothness > 0.0)
fwhm_info->int_lambda += fwhm_info->smoothness;
fwhm_info->smoothness *= fwhm_info->constant;
if(fwhm_info->smoothness > 0.0) {
fwhm_info->fwhm = pow(fwhm_info->smoothness, 1.0/lambda_buffer->num_dim);
}
else
fwhm_info->fwhm = INVALID_DATA;
if(fwhm_info->fwhm != INVALID_DATA) {
fwhm_info->fwhm = (lambda_buffer->constant / fwhm_info->fwhm);
}
else
fwhm_info->fwhm = INVALID_DATA;
if (fwhm_info->fwhm != INVALID_DATA)
fwhm_info->num_calc++;
return;
}
static inline bool areCollinearPoints(const FloatPoint& p0, const FloatPoint& p1, const FloatPoint& p2)
{
return !determinant(p1 - p0, p2 - p0);
}
int determinant(int n, int *matrix)
{
if(n==1)
{
return *matrix;
}
if(n==2)
{
//printf("end\n");
return *matrix*(*(matrix+3)) - *(matrix+1)*(*(matrix+2));
}
int x,y,z,sum=0,count=0;
for(x=0; x<n; x++)
{
int a = *(matrix+x);
int *add = matrix;
add += n;
int *p;
int arr[(n-1)*(n-1)];
int m;
for(m=0; m<(n-1)*(n-1); m++){
arr[m]=0;
}
p = &arr[0];
int *e = p;
for(y=1; y<n; y++)
{
for(z=0; z<n; z++)
{
if (z != x)
{
*p = *add;
p+=1;
}
add+=1;
}
}
//printf("a = %d and n= %d \n",a,n);
//printMatrix(n-1,e);
//printf("------------------------\n");
if(count%2==0)
{
sum += a * determinant(n-1,e);
}
if(count%2==1)
{
sum -= a * determinant(n-1,e);
}
count++;
}
return sum;
}