bool operator()(const Matx<_Tp, 2, 2>& a, Matx<_Tp, 2, 2>& b, int) const
    {
        _Tp d = (_Tp)determinant(a);
        if( d == 0 )
=======
示例#2
0
文件: basis.cpp 项目: Paulloz/godot
bool Basis::is_rotation() const {
	return Math::is_equal_approx(determinant(), 1, UNIT_EPSILON) && is_orthogonal();
}
示例#3
0
static CYTHON_INLINE struct ZZ* mat_ZZ_determinant(const mat_ZZ* x, long deterministic)
{
    ZZ* d = new ZZ();
    determinant(*d, *x, deterministic);
    return d;
}
示例#4
0
float2 characteristic_poly(mat2 p)
{
	return float2(determinant(p), -trace(p));
}
示例#5
0
文件: matrix.cpp 项目: asir6/Colt
void WT_Matrix2D::get_inverse (WT_Matrix2D & result) const
{
    get_adjoint(result);
    result *= 1.0 / determinant();
}
示例#6
0
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;
}
示例#7
0
/* 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];
	}
}
示例#8
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");

         }

    }
示例#9
0
    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);

    }
示例#10
0
/*!
  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 );
  }

}
示例#11
0
    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 );
}
示例#13
0
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;
}
示例#14
0
Mat2 Mat2::inverse()
{
	return Mat2( d11,-d10,
				-d01, d00)
				/determinant();
}
示例#15
0
    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);
}
示例#18
0
文件: utils.C 项目: OpenVSP/OpenVSP
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.;

    }

}
示例#19
0
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);
}
示例#20
0
文件: utils.C 项目: OpenVSP/OpenVSP
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.;

    }

}
示例#21
0
文件: Mat4.cpp 项目: fiskercui/test
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;
}
示例#22
0
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;
    }
}
示例#23
0
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));
}
示例#24
0
/* 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;
}
示例#25
0
bool Matrix3::isSingular() const {
    return fuzzyEQ(determinant(), 0);
}
示例#26
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;
}
示例#27
0
文件: basis.cpp 项目: Paulloz/godot
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());
}
示例#28
0
/* ----------------------------- 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;
}
示例#29
0
static inline bool areCollinearPoints(const FloatPoint& p0, const FloatPoint& p1, const FloatPoint& p2)
{
    return !determinant(p1 - p0, p2 - p0);
}
示例#30
0
文件: lab2.c 项目: WxnChen11/CSC190
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;


}