コード例 #1
0
ファイル: poly.c プロジェクト: LZChuan/minimal_scene
/* Use Newton's method to find the root of a polynomial.  The
 * polynomial is represented by n coefficients.  The coefficients are
 * in order from low power to high power */
double poly_find_root(poly_t *p, double x0, double thres) {
    /* Evaluate the polynomial at the current x */
    int trials = 0;
    double x_curr = x0;
    double p_x;
    // double roots[10];

    poly_t *p_deriv = poly_deriv(p);

    p_x = poly_eval(p, x_curr);

#define MAX_TRIALS 1024
    /* Iterate until convergence */
    while (fabs(p_x) > thres && trials < MAX_TRIALS) {
	double dp_dx = poly_eval(p_deriv, x_curr);
	
	x_curr = x_curr - p_x / dp_dx;
	p_x = poly_eval(p, x_curr);

	trials++;
    }

#if 0
    if (trials >= MAX_TRIALS) {
	printf("[poly_find_root] Maximum number of trials exceeded.\n");
    }
#endif

    poly_free(p_deriv);

    // find_roots(p->n, p->coeffs, roots);

    return x_curr;
}
コード例 #2
0
ファイル: decode.cpp プロジェクト: nzaghen/qrlocate
static quirc_decode_error_t correct_format(uint16_t *f_ret)
{
	uint16_t u = *f_ret;
	int i;
	uint8_t s[MAX_POLY];
	uint8_t sigma[MAX_POLY];

	/* Evaluate U (received codeword) at each of alpha_1 .. alpha_6
	 * to get S_1 .. S_6 (but we index them from 0).
	 */
	if (!format_syndromes(u, s))
		return QUIRC_SUCCESS;

	berlekamp_massey(s, FORMAT_SYNDROMES, &gf16, sigma);

	/* Now, find the roots of the polynomial */
	for (i = 0; i < 15; i++)
		if (!poly_eval(sigma, gf16_exp[15 - i], &gf16))
			u ^= (1 << i);

	if (format_syndromes(u, s))
		return QUIRC_ERROR_FORMAT_ECC;

	*f_ret = u;
	return QUIRC_SUCCESS;
}
コード例 #3
0
ファイル: quartic.c プロジェクト: ethaneade/MathBits
int main(int argc, char* argv[])
{
    double coef[5];
    assert(argc == 6);
    
    int i;
    for (i=1; i<=5; ++i)
        coef[i-1] = atof(argv[i]);

    double invA = 1/coef[4];
    double roots[4];
    int nr = real_quartic_roots(invA * coef[3],
                                invA * coef[2],
                                invA * coef[1],
                                invA * coef[0],
                                roots);

    for (i=0; i<nr; ++i) {
        double x = roots[i];
        double y = poly_eval(coef, 4, x);
        printf("p(%.16g) = %.16g\n", x, y);
    }

    return 0;
}
コード例 #4
0
ファイル: keypair.c プロジェクト: JacobBarthelmeh/supercop
MAT key_genmat(gf_t *L, poly_t g)
{
  //L- Support
  //t- Number of errors, i.e.=30.
  //n- Length of the Goppa code, i.e.=2^11
  //m- The extension degree of the GF, i.e. =11
  //g- The generator polynomial.
  gf_t x,y;
  MAT H,R; 
  int i,j,k,r,n;
  int * perm, Laux[LENGTH];

  n=LENGTH;//2^11=2048
  r=NB_ERRORS*gf_extd();//32 x 11=352

  H=mat_ini(r,n);//initialize matrix with actual no. of bits.
  mat_set_to_zero(H); //set the matrix with all 0's.

  for(i=0;i< n;i++)
    {
      x = poly_eval(g,L[i]);//evaluate the polynomial at the point L[i].
      x = gf_inv(x);
      y = x;
      for(j=0;j<NB_ERRORS;j++)
	{
	  for(k=0;k<gf_extd();k++)
	    {
	      if(y & (1<<k))//if((y>>k) & 1)
		mat_set_coeff_to_one(H,j*gf_extd()+k,i);//the co-eff. are set in 2^0,...,2^11 ; 2^0,...,2^11 format along the rows/cols?
	    }
	  y = gf_mul(y,L[i]);
	}
    }//The H matrix is fed.
  
  perm = mat_rref(H);
  if (perm == NULL) {
    mat_free(H);
    return NULL;
  }
  
  R = mat_ini(n-r,r);
  mat_set_to_zero(R); //set the matrix with all 0's.
  for (i = 0; i < R->rown; ++i)
    for (j = 0; j < R->coln; ++j)
      if (mat_coeff(H,j,perm[i]))
	mat_change_coeff(R,i,j);

  for (i = 0; i < LENGTH; ++i)
    Laux[i] = L[perm[i]];
  for (i = 0; i < LENGTH; ++i)
    L[i] = Laux[i];

  mat_free(H);
  free(perm);

  return (R);
}
コード例 #5
0
ファイル: decode.cpp プロジェクト: nzaghen/qrlocate
static quirc_decode_error_t correct_block(uint8_t *data, const struct quirc_rs_params *ecc)
{
	int npar = ecc->ce;
	uint8_t s[MAX_POLY];
	uint8_t sigma[MAX_POLY];
	uint8_t sigma_deriv[MAX_POLY];
	uint8_t omega[MAX_POLY];
	int i;

	/* Compute syndrome vector */
	if (!block_syndromes(data, ecc->bs, npar, s))
		return QUIRC_SUCCESS;

	berlekamp_massey(s, npar, &gf256, sigma);

	/* Compute derivative of sigma */
	memset(sigma_deriv, 0, MAX_POLY);
	for (i = 0; i + 1 < MAX_POLY; i += 2)
		sigma_deriv[i] = sigma[i + 1];

	/* Compute error evaluator polynomial */
	poly_mult(omega, sigma, s, &gf256);
	memset(omega + npar, 0, MAX_POLY - npar);

	/* Find error locations and magnitudes */
	for (i = 0; i < ecc->bs; i++) {
		uint8_t xinv = gf256_exp[255 - i];

		if (!poly_eval(sigma, xinv, &gf256)) {
			uint8_t sd_x = poly_eval(sigma_deriv, xinv, &gf256);
			uint8_t omega_x = poly_eval(omega, xinv, &gf256);
			uint8_t error = gf256_exp[(255 - gf256_log[sd_x] +
						   gf256_log[omega_x]) % 255];

			data[ecc->bs - i - 1] ^= error;
		}
	}

	if (block_syndromes(data, ecc->bs, npar, s))
		return QUIRC_ERROR_DATA_ECC;

	return QUIRC_SUCCESS;
}
コード例 #6
0
ファイル: poly.c プロジェクト: compbrain/Athena-SCG-Bot
double
poly_maximum_point(poly *p, double left, double right) {
    double max;
    double max_point;
    double temp;

    assert(p);
    assert(left < right);

    max = -INFINITY;
    max_point = left;

    /* Brute force check. */
    for (; left < right; left += 0.0001) {
        if ((temp = poly_eval(p, left)) > max) {
            max = temp;
            max_point = left;
        }
    }

    /* Check the right-bound */
    return (poly_eval(p, right) > max) ? right : max_point;
}
コード例 #7
0
ファイル: roots.c プロジェクト: FairSky/ggnfs
/*-----------------------------------------------------------------------*/
static int
find_roots_core(int deg, double *poly, double d0, double d1, double *dr)
{
	double p0, p1, db, de, pm;
	volatile double dm;	/* important for some compilers */

	p0 = poly_eval(deg, poly, d0);
	p1 = poly_eval(deg, poly, d1);
	if (p1 == 0.) {
		*dr = d1;
		return 1;
	}
	if (p0 == 0.)
		return 0;
	if (p0 * p1 > 0.)
		return 1;
	db = d0;
	de = d1;
	while (de > db) {
		dm = (db + de) / 2.;
		if ((dm == db) || (dm == de))
			break;
		pm = poly_eval(deg, poly, dm);
		if (pm == 0.)
			break;
		if (p0 * pm > 0.) {
			db = dm;
			p0 = pm;
		}
		else {
			de = dm;
			p1 = pm;
		}
	}
	*dr = dm;
	return 1;
}
コード例 #8
0
ファイル: poly_lab.c プロジェクト: abocz/school-c
/* Functions */
int main(void) {
    /* Variables */
	int i;
    int j;   
    double intervalStart, intervalEnd;
    double x[M];
    double y[M];  
	double coefficients[N] = {4.5, -3, 6, -6.5, 5, 1.5, 2};  
    printf("The polynomial has the follow coefficients: \n");
    for (i=0; i < N; i++)
       printf("%f ", coefficients[i]);
    printf(" \n");
    /* Estimated interval */  
    intervalStart = -4.0;
    intervalEnd = 4.0;
	/* Calculate a vector */
    linspace(x, intervalStart, intervalEnd, M);
    poly_eval(coefficients, x, y);
    printf("\nPlotting Points \n");
    for(j=0; j < M; j++)
        printf("%+.18f %+.18f \n", x[j], y[j]);	   
    return 0;
}
コード例 #9
0
ファイル: poly.c プロジェクト: sageh/libad
int root_laguerre(double tol, int maxit, const polyr *p, double x0, double *res) {
	polyr pd, pdd;
	int deg = poly_deg(p);
	double x = x0;
	double y = poly_eval(p, x);
	int n = 0;

	poly_diff(p, &pd);
	poly_diff(&pd, &pdd);

#ifdef DEBUG
		fprintf(stderr, "tol: %g maxit: %d x0: %g\n", tol, maxit, x0);
		poly_print(stderr, p);
		poly_print(stderr, &pd);
		poly_print(stderr, &pdd);
#endif

	while (fabs(y) > tol && n < maxit) {
		double g = poly_eval(&pd, x) / poly_eval(p, x);
		double h = g*g - poly_eval(&pdd, x) / poly_eval(p, x);
		double sqrtarg = (deg-1)*(deg*h - g*g);
		double sq = sqrt(sqrtarg);
		double denom = (g > 0 ? g+sq : g-sq);

		x = x - deg/denom;
		y = poly_eval(p, x);
		n++;
#ifdef DEBUG
		fprintf(stderr, "g: %g h: %g sqa: %g sq: %g x: %g y: %g\n",
				g, h, sqrtarg, sq, x, y);
#endif

		/* complex roots => EDOM, divide by zero => NAN etc */
		if (sqrtarg < 0 || isinf(x) || isnan(x)
				|| errno == EDOM || errno == ERANGE)
			return 0;
	}

	if (fabs(y) > tol)
		return 0;

	*res = x;
	return 1;
}
コード例 #10
0
KNDdeTorusCollocation::KNDdeTorusCollocation(KNExprSystem& sys_, size_t ndeg1_, size_t ndeg2_, size_t nint1_, size_t nint2_) :
    sys(&sys_),
    ndim(sys_.ndim()), ntau(sys_.ntau()), npar(sys_.npar()),
    ndeg1(ndeg1_), ndeg2(ndeg2_), nint1(nint1_), nint2(nint2_),
    col1(ndeg1_),    col2(ndeg2_),
    mesh1(ndeg1_ + 1), mesh2(ndeg2_ + 1),
    lgr1(ndeg1_ + 1, ndeg1_ + 1), lgr2(ndeg2_ + 1, ndeg2_ + 1),
    dlg1(ndeg1_ + 1, ndeg1_ + 1), dlg2(ndeg2_ + 1, ndeg2_ + 1),
    I1((ndeg1_ + 1), (ndeg1_ + 1)),
    ID1((ndeg1_ + 1), (ndeg1_ + 1)),
    I2((ndeg2_ + 1), (ndeg2_ + 1)),
    ID2((ndeg2_ + 1), (ndeg2_ + 1)),
    mlg1((ndeg1_ + 1)*(ndeg1_ + 1)),
    mlg2((ndeg2_ + 1)*(ndeg2_ + 1)),
    mlgd1((ndeg1_ + 1)*(ndeg1_ + 1)),
    mlgd2((ndeg2_ + 1)*(ndeg2_ + 1)),
    ilg1((ndeg1_ + 1)*(ndeg1_ + 1) + 1),
    ilg2((ndeg2_ + 1)*(ndeg2_ + 1) + 1),
    ilgd1((ndeg1_ + 1)*(ndeg1_ + 1) + 1),
    ilgd2((ndeg2_ + 1)*(ndeg2_ + 1) + 1),
    time1(ndeg1*ndeg2*nint1*nint2), time2(ndeg1*ndeg2*nint1*nint2),
    kk((ntau+1)*(ndeg1+1)*(ndeg2+1), ndeg1*ndeg2*nint1*nint2),
    ee((ntau+1)*(ndeg1+1)*(ndeg2+1), ndeg1*ndeg2*nint1*nint2),
    rr((ntau+1)*(ndeg1+1)*(ndeg2+1), ndeg1*ndeg2*nint1*nint2),
    p_tau(ntau, ndeg1*ndeg2*nint1*nint2), p_dtau(ntau, ndeg1*ndeg2*nint1*nint2),
    p_xx(ndim, ntau+2*(ntau+1), ndeg1*ndeg2*nint1*nint2),
    p_fx(ndim, ndeg1*ndeg2*nint1*nint2),
    p_dfp(ndim, 1, ndeg1*ndeg2*nint1*nint2),
    p_dfx(ndim, ndim, ndeg1*ndeg2*nint1*nint2),
    p_dummy(0, 0, ndeg1*ndeg2*nint1*nint2)
{
  lobatto(mesh1);
  lobatto(mesh2);
  gauss(col1);
  gauss(col2);
  for (size_t i = 0; i < mesh1.size(); i++)
  {
    poly_coeff_lgr(lgr1(i), mesh1, i);
    poly_coeff_diff(dlg1(i), lgr1(i));
  }
  for (size_t i = 0; i < mesh2.size(); i++)
  {
    poly_coeff_lgr(lgr2(i), mesh2, i);
    poly_coeff_diff(dlg2(i), lgr2(i));
  }

  // here comes the phase condition for par(OMEGA0) and par(OMEGA1)
  // the integration in the bottom border
  // construct the diffint matrix
  for (size_t i = 0; i < ndeg1 + 1; i++)
  {
    for (size_t k = 0; k < ndeg1 + 1; k++)
    {
      mlg1.clear();
      mlgd1.clear();
      ilg1.clear();
      ilgd1.clear();
      poly_coeff_mul(mlg1,  lgr1(i), lgr1(k));
      poly_coeff_mul(mlgd1, dlg1(i), lgr1(k));
      poly_coeff_int(ilg1,  mlg1);
      poly_coeff_int(ilgd1,  mlgd1);
      I1(i, k) = (poly_eval(ilg1, 1.0)-poly_eval(ilg1, 0.0)) / nint1;
      ID1(i, k) = poly_eval(ilgd1, 1.0);
    }
  }
  for (size_t j = 0; j < ndeg2 + 1; j++)
  {
    for (size_t l = 0; l < ndeg2 + 1; l++)
    {
      mlg2.clear();
      mlgd2.clear();
      ilg2.clear();
      ilgd2.clear();
      poly_coeff_mul(mlg2,  lgr2(j), lgr2(l));
      poly_coeff_mul(mlgd2, dlg2(j), lgr2(l));
      poly_coeff_int(ilg2,  mlg2);
      poly_coeff_int(ilgd2,  mlgd2);
      I2(j, l) = (poly_eval(ilg2, 1.0) - poly_eval(ilg2, 0.0))/ nint2;
      ID2(j, l) = poly_eval(ilgd2, 1.0);
    }
  }
}
コード例 #11
0
ファイル: structure_from_motion.hpp プロジェクト: matt-42/vpp
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points,
                                              Eigen::MatrixXf end_points,
                                              Eigen::MatrixXf directions,
                                              Eigen::MatrixXf points,
                                              Eigen::MatrixXf &rot_cw,
                                              Eigen::VectorXf &pos_cw)
{


    int n = start_points.cols();
    if(n != directions.cols())
    {
        return;
    }

    if(n<4)
    {
        return;
    }


    float condition_err_threshold = 1e-3;
    Eigen::VectorXf cosAngleThreshold(3);
    cosAngleThreshold << 1.1, 0.9659, 0.8660;
    Eigen::MatrixXf optimumrot_cw(3,3);
    Eigen::VectorXf optimumpos_cw(3);
    std::vector<float> lineLenVec(n,1);

    vfloat3 l1;
    vfloat3 l2;
    vfloat3 nc1;
    vfloat3 Vw1;
    vfloat3 Xm;
    vfloat3 Ym;
    vfloat3 Zm;
    Eigen::MatrixXf Rot(3,3);
    std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0));
    std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0));
    std::vector<vfloat3> n_c(n,vfloat3(0,0,0));
    Eigen::MatrixXf Rx(3,3);
    int line_id;

    for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++)
    {

        if(HowToChooseFixedTwoLines==1)
        {
#pragma omp parallel for
            for(int i = 0; i < n ; i++ )
            {
                // to correct
                float lineLen = 10;
                lineLenVec[i] = lineLen;
            }
            std::vector<float>::iterator result;
            result = std::max_element(lineLenVec.begin(), lineLenVec.end());
            line_id = std::distance(lineLenVec.begin(), result);
            vfloat3 temp;
            temp = start_points.col(0);
            start_points.col(0) = start_points.col(line_id);
            start_points.col(line_id) = temp;

            temp = end_points.col(0);
            end_points.col(0) = end_points.col(line_id);
            end_points.col(line_id) = temp;

            temp = directions.col(line_id);
            directions.col(0) = directions.col(line_id);
            directions.col(line_id) = temp;

            temp = points.col(0);
            points.col(0) = points.col(line_id);
            points.col(line_id) = temp;

            lineLenVec[line_id] = lineLenVec[1];
            lineLenVec[1] = 0;
            l1 = start_points.col(0) - end_points.col(0);
            l1 = l1/l1.norm();
        }


        for(int i = 1; i < n; i++)
        {
            std::vector<float>::iterator result;
            result = std::max_element(lineLenVec.begin(), lineLenVec.end());
            line_id = std::distance(lineLenVec.begin(), result);
            l2 = start_points.col(line_id) - end_points.col(line_id);
            l2 = l2/l2.norm();
            lineLenVec[line_id] = 0;
            MatrixXf cosAngle(3,3);
            cosAngle = (l1.transpose()*l2).cwiseAbs();
            if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines])
            {
                break;
            }
        }



        vfloat3 temp;
        temp = start_points.col(1);
        start_points.col(1) = start_points.col(line_id);
        start_points.col(line_id) = temp;

        temp = end_points.col(1);
        end_points.col(1) = end_points.col(line_id);
        end_points.col(line_id) = temp;

        temp = directions.col(1);
        directions.col(1) = directions.col(line_id);
        directions.col(line_id) = temp;

        temp = points.col(1);
        points.col(1) = points.col(line_id);
        points.col(line_id) = temp;

        lineLenVec[line_id] = lineLenVec[1];
        lineLenVec[1] = 0;

        // The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper,
        // but the framework is the same.
        // R_wc = (Rot') * R * Rot =  (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot
        nc1 = x_cross(start_points.col(1),end_points.col(1));
        nc1 = nc1/nc1.norm();

        Vw1 = directions.col(1);
        Vw1 = Vw1/Vw1.norm();

        //the X axis of Model frame
        Xm = x_cross(nc1,Vw1);
        Xm = Xm/Xm.norm();

        //the Y axis of Model frame
        Ym = nc1;

        //the Z axis of Model frame
        Zm = x_cross(Xm,Zm);
        Zm = Zm/Zm.norm();

        //Rot * [Xm, Ym, Zm] = I.
        Rot.col(0) = Xm;
        Rot.col(1) = Ym;
        Rot.col(2) = Zm;

        Rot = Rot.transpose();


        //rotate all the vector by Rot.
        //nc_bar(:,i) = Rot * nc(:,i)
        //Vw_bar(:,i) = Rot * Vw(:,i)
#pragma omp parallel for
        for(int i = 0 ; i < n ; i++)
        {
            vfloat3 nc = x_cross(start_points.col(1),end_points.col(1));
            nc = nc/nc.norm();
            n_c[i] = nc;
            nc_bar[i] = Rot * nc;
            Vw_bar[i] = Rot * directions.col(i);
        }

        //Determine the angle psi, it is the angle between z axis and Vw_bar(:,1).
        //The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis
        float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);.
        float sinpsi= sqrt(1 - cospsi*cospsi);
        Rx.row(0) = vfloat3(1,0,0);
        Rx.row(1) = vfloat3(0,cospsi,-sinpsi);
        Rx.row(2) = vfloat3(0,sinpsi,cospsi);
        vfloat3 Zaxis = Rx * Vw_bar[1];
        if(1-fabs(Zaxis[3]) > 1e-5)
        {
            Rx = Rx.transpose();
        }

        //estimate the rotation angle phi by least square residual.
        //i.e the rotation matrix Rz(phi)
        vfloat3 Vm2 = Rx * Vw_bar[1];
        float A2 = Vm2[0];
        float B2 = Vm2[1];
        float C2 = Vm2[2];
        float x2 = (nc_bar[1])[0];
        float y2 = (nc_bar[1])[1];
        float z2 = (nc_bar[1])[2];
        Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
        Eigen::VectorXf coeff(9);

        std::vector<float> coef(9,0); //coefficients of equation (7)
        Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16);

        //construct the  polynomial F'
#pragma omp parallel for
        for(int i = 3 ; i < n ; i++)
        {
            vfloat3 Vm3 = Rx*Vw_bar[i];
            float A3 = Vm3[0];
            float B3 = Vm3[1];
            float C3 = Vm3[2];
            float x3 = (nc_bar[i])[0];
            float y3 = (nc_bar[i])[1];
            float z3 = (nc_bar[i])[2];
            float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3;
            float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3;
            float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3;
            float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3;
            float u15 =  x2*C2*y3*A3 - y2*A2*x3*C3;
            float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3;
            float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3;
            float u23 =  x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3;
            float u24 =  y2*B2*z3*C3 - z2*C2*y3*B3;
            float u25 =  y2*A2*z3*C3 - z2*C2*y3*A3;
            float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3;
            float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3;
            float u33 =  x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3;
            float u34 =  z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3;
            float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3;
            float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3;
            float a4 =   u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 +   u21*u21 + u22*u22 - u23*u23
                    -2*u21*u22 - u31*u31 - u32*u32 +   u33*u33 + 2*u31*u32;
            float a3 =2*(u11*u14 - u13*u15 - u12*u14 +   u21*u24 -   u23*u25
                         - u22*u24 - u31*u34 + u33*u35 +   u32*u34);
            float a2 =-2*u12*u12 + u13*u13 + u14*u14 -   u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23
                    + u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 -   u33*u33
                    - u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36;
            float a1 =2*(u12*u14 + u13*u15 +  u22*u24 +  u23*u25 -   u32*u34 - u33*u35 - u34*u36);
            float a0 =   u12*u12 + u15*u15+   u22*u22 +  u25*u25 -   u32*u32 - u35*u35 - u36*u36 - 2*u32*u36;
            float b3 =2*(u11*u13 - u12*u13 +  u21*u23 -  u22*u23 -   u31*u33 + u32*u33);
            float b2 =2*(u11*u15 - u12*u15 +  u13*u14 +  u21*u25 -   u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34);
            float b1 =2*(u12*u13 + u14*u15 +  u22*u23 +  u24*u25 -   u32*u33 - u34*u35 - u33*u36);
            float b0 =2*(u12*u15 + u22*u25 -  u32*u35 -  u35*u36);

            float d0 =    a0*a0 -   b0*b0;
            float d1 = 2*(a0*a1 -   b0*b1);
            float d2 =    a1*a1 + 2*a0*a2 +   b0*b0 - b1*b1 - 2*b0*b2;
            float d3 = 2*(a0*a3 +   a1*a2 +   b0*b1 - b1*b2 -   b0*b3);
            float d4 =    a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3;
            float d5 = 2*(a1*a4 +   a2*a3 +   b1*b2 + b0*b3 -   b2*b3);
            float d6 =    a3*a3 + 2*a2*a4 +   b2*b2 - b3*b3 + 2*b1*b3;
            float d7 = 2*(a3*a4 +   b2*b3);
            float d8 =    a4*a4 +   b3*b3;
            std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 };
            Eigen::VectorXf vp;
            vp <<  a4, a3, a2, a1, a0, b3, b2, b1, b0 ;
            //coef = coef + v;
            coeff = coeff + vp;

            polyDF[0] = polyDF[0] + 8*d8*d8;
            polyDF[1] = polyDF[1] + 15* d7*d8;
            polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7;
            polyDF[3] = polyDF[3] + 13*(d5*d8 +  d6*d7);
            polyDF[4] = polyDF[4] + 12*(d4*d8 +  d5*d7) +  6*d6*d6;
            polyDF[5] = polyDF[5] + 11*(d3*d8 +  d4*d7 +  d5*d6);
            polyDF[6] = polyDF[6] + 10*(d2*d8 +  d3*d7 +  d4*d6) + 5*d5*d5;
            polyDF[7] = polyDF[7] + 9 *(d1*d8 +  d2*d7 +  d3*d6  + d4*d5);
            polyDF[8] = polyDF[8] + 8 *(d1*d7 +  d2*d6 +  d3*d5) + 4*d4*d4 + 8*d0*d8;
            polyDF[9] = polyDF[9] + 7 *(d1*d6 +  d2*d5 +  d3*d4) + 7*d0*d7;
            polyDF[10] = polyDF[10] + 6 *(d1*d5 +  d2*d4) + 3*d3*d3 + 6*d0*d6;
            polyDF[11] = polyDF[11] + 5 *(d1*d4 +  d2*d3)+ 5*d0*d5;
            polyDF[12] = polyDF[12] + 4 * d1*d3 +  2*d2*d2 + 4*d0*d4;
            polyDF[13] = polyDF[13] + 3 * d1*d2 +  3*d0*d3;
            polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2;
            polyDF[15] = polyDF[15] + d0*d1;
        }


        Eigen::VectorXd coefficientPoly = VectorXd::Zero(16);

        for(int j =0; j < 16 ; j++)
        {
            coefficientPoly[j] = polyDF[15-j];
        }


        //solve polyDF
        solver.compute(coefficientPoly);
        const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();
        Eigen::VectorXd rs(r.rows());
        Eigen::VectorXd is(r.rows());
        std::vector<float> min_roots;
        for(int j =0;j<r.rows();j++)
        {
            rs[j] = fabs(r[j].real());
            is[j] = fabs(r[j].imag());
        }


        float maxreal = rs.maxCoeff();

        for(int j = 0 ; j < rs.rows() ; j++ )
        {
            if(is[j]/maxreal <= 0.001)
            {
                min_roots.push_back(rs[j]);
            }
        }

        std::vector<float> temp_v(15);
        std::vector<float> poly(15);
        for(int j = 0 ; j < 15 ; j++)
        {
            temp_v[j] = j+1;
        }

        for(int j = 0 ; j < 15 ; j++)
        {
            poly[j] = polyDF[j]*temp_v[j];
        }

        Eigen::Matrix<double,16,1> polynomial;

        Eigen::VectorXd evaluation(min_roots.size());

        for( int j = 0; j < min_roots.size(); j++ )
        {
            evaluation[j] = poly_eval( polynomial, min_roots[j] );
        }


        std::vector<float> minRoots;


        for( int j = 0; j < min_roots.size(); j++ )
        {
            if(!evaluation[j]<=0)
            {
                minRoots.push_back(min_roots[j]);
            }
        }


        if(minRoots.size()==0)
        {
            cout << "No solution" << endl;
            return;
        }

        int numOfRoots = minRoots.size();
        //for each minimum, we try to find a solution of the camera pose, then
        //choose the one with the least reprojection residual as the optimum of the solution.
        float minimalReprojectionError = 100;
        // In general, there are two solutions which yields small re-projection error
        // or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the
        // world scene behind the camera center, the other solution transforms the world
        // scene in front of camera center. While only the latter one is correct.
        // This can easily be checked by verifying their Z coordinates in the camera frame.
        // P_c(Z) must be larger than 0 if it's in front of the camera.



        for(int rootId = 0 ; rootId < numOfRoots ; rootId++)
        {

            float cosphi = minRoots[rootId];
            float sign1 = sign_of_number(coeff[0] * pow(cosphi,4)
                    + coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2)
                    + coeff[3] * cosphi + coeff[4]);
            float  sign2 = sign_of_number(coeff[5] * pow(cosphi,3)
                    + coeff[6] * pow(cosphi,2) + coeff[7] * cosphi   + coeff[8]);
            float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi));
            Eigen::MatrixXf Rz(3,3);
            Rz.row(0) = vfloat3(cosphi,-sinphi,0);
            Rz.row(1) = vfloat3(sinphi,cosphi,0);
            Rz.row(2) = vfloat3(0,0,1);
            //now, according to Sec4.3, we estimate the rotation angle theta
            //and the translation vector at a time.
            Eigen::MatrixXf RzRxRot(3,3);
            RzRxRot = Rz*Rx*Rot;


            //According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we
            //have: scalarproduct(Vi^c, ni^c) = 0  and scalarproduct(Pi^c, ni^c) = 0.
            //where Vi^c = Rwc * Vi^w,  Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos;
            //Using the above two constraints to construct linear equation system Mat about
            //[costheta, sintheta, tx, ty, tz, 1].
            Eigen::MatrixXf Matrice(2*n-1,6);
#pragma omp parallel for
            for(int i = 0 ; i < n ; i++)
            {
                float nxi = (nc_bar[i])[0];
                float nyi = (nc_bar[i])[1];
                float nzi = (nc_bar[i])[2];
                Eigen::VectorXf Vm(3);
                Vm = RzRxRot * directions.col(i);
                float Vxi = Vm[0];
                float Vyi = Vm[1];
                float Vzi = Vm[2];
                Eigen::VectorXf Pm(3);
                Pm = RzRxRot * points.col(i);
                float Pxi = Pm(1);
                float Pyi = Pm(2);
                float Pzi = Pm(3);
                //apply the constraint scalarproduct(Vi^c, ni^c) = 0
                //if i=1, then scalarproduct(Vi^c, ni^c) always be 0
                if(i>1)
                {
                    Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;
                    Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;
                    Matrice(2*i-3, 5) = nyi * Vyi;
                }
                //apply the constraint scalarproduct(Pi^c, ni^c) = 0
                Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;
                Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;
                Matrice(2*i-2, 2) = -nxi;
                Matrice(2*i-2, 3) = -nyi;
                Matrice(2*i-2, 4) = -nzi;
                Matrice(2*i-2, 5) = nyi * Pyi;
            }

            //solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0  using SVD,
            JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);
            Eigen::MatrixXf VMat = svd.matrixV();
            Eigen::VectorXf vec(2*n-1);
            //the last column of Vmat;
            vec = VMat.col(5);
            //the condition that the last element of vec should be 1.
            vec = vec/vec[5];
            //the condition costheta^2+sintheta^2 = 1;
            float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);
            float costheta = vec[0]*normalizeTheta;
            float sintheta = vec[1]*normalizeTheta;
            Eigen::MatrixXf Ry(3,3);
            Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;

            //now, we get the rotation matrix rot_wc and translation pos_wc
            Eigen::MatrixXf rot_wc(3,3);
            rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;
            Eigen::VectorXf pos_wc(3);
            pos_wc = - Rot.transpose() * vec.segment(2,4);

            //now normalize the camera pose by 3D alignment. We first translate the points
            //on line in the world frame Pw to points in the camera frame Pc. Then we project
            //Pc onto the line interpretation plane as Pc_new. So we could call the point
            //alignment algorithm to normalize the camera by aligning Pc_new and Pw.
            //In order to improve the accuracy of the aligment step, we choose two points for each
            //lines. The first point is Pwi, the second point is  the closest point on line i to camera center.
            //(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)
            Eigen::MatrixXf Pw2(3,n);
            Pw2.setZero();
            Eigen::MatrixXf Pc_new(3,n);
            Pc_new.setZero();
            Eigen::MatrixXf Pc2_new(3,n);
            Pc2_new.setZero();

            for(int i = 0 ; i < n ; i++)
            {
                vfloat3 nci = n_c[i];
                vfloat3 Pwi = points.col(i);
                vfloat3 Vwi = directions.col(i);
                //first point on line i
                vfloat3 Pci;
                Pci = rot_wc * Pwi + pos_wc;
                Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci;
                //second point is the closest point on line i to camera center.
                vfloat3 Pw2i;
                Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi;
                Pw2.col(i) = Pw2i;
                vfloat3 Pc2i;
                Pc2i = rot_wc * Pw2i + pos_wc;
                Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci;
            }

            MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols());
            XXc << Pc_new, Pc2_new;
            MatrixXf XXw(points.rows(), points.cols()+Pw2.cols());
            XXw << points, Pw2;
            int nm = points.cols()+Pw2.cols();
            cal_campose(XXc,XXw,nm,rot_wc,pos_wc);
            pos_cw = -rot_wc.transpose() * pos_wc;

            //check the condition n_c^T * rot_wc * V_w = 0;
            float conditionErr = 0;
            for(int i =0 ; i < n ; i++)
            {
                float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) );
                conditionErr = conditionErr + val*val;
            }

            if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3)
            {
                //check whether the world scene is in front of the camera.
                int numLineInFrontofCamera = 0;
                if(HowToChooseFixedTwoLines<3)
                {
                    for(int i = 0 ; i < n ; i++)
                    {
                        vfloat3 P_c = rot_wc * (points.col(i) - pos_cw);
                        if(P_c[2]>0)
                        {
                            numLineInFrontofCamera++;
                        }
                    }
                }
                else
                {
                    numLineInFrontofCamera = n;
                }

                if(numLineInFrontofCamera > 0.5*n)
                {
                    //most of the lines are in front of camera, then check the reprojection error.
                    int reprojectionError = 0;
                    for(int i =0; i < n ; i++)
                    {
                        //line projection function
                        vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i));
                        float h1 = nc.transpose() * start_points.col(i);
                        float h2 = nc.transpose() * end_points.col(i);
                        float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3;
                        reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]);
                    }
                    if(reprojectionError < minimalReprojectionError)
                    {
                        optimumrot_cw = rot_wc.transpose();
                        optimumpos_cw = pos_cw;
                        minimalReprojectionError = reprojectionError;
                    }
                }
            }
        }
        if(optimumrot_cw.rows()>0)
        {
            break;
        }
    }
    pos_cw = optimumpos_cw;
    rot_cw = optimumrot_cw;
}
コード例 #12
0
ファイル: poly.c プロジェクト: compbrain/Athena-SCG-Bot
double
poly_maxima(poly *p, double left, double right) {
    assert(p);
    assert(left < right);
    return poly_eval(p, poly_maximum_point(p, left, right));
}
コード例 #13
0
ファイル: tdist.c プロジェクト: Ayato-Harashima/CMVS-PMVS
static double
cornish_fisher (double t, double n)
{
  const double coeffs6[10] = {
    0.265974025974025974026,
    5.449696969696969696970,
    122.20294372294372294372,
    2354.7298701298701298701,
    37625.00902597402597403,
    486996.1392857142857143,
    4960870.65,
    37978595.55,
    201505390.875,
    622437908.625
  };
  const double coeffs5[8] = {
    0.2742857142857142857142,
    4.499047619047619047619,
    78.45142857142857142857,
    1118.710714285714285714,
    12387.6,
    101024.55,
    559494.0,
    1764959.625
  };
  const double coeffs4[6] = {
    0.3047619047619047619048,
    3.752380952380952380952,
    46.67142857142857142857,
    427.5,
    2587.5,
    8518.5
  };
  const double coeffs3[4] = {
    0.4,
    3.3,
    24.0,
    85.5
  };

  double a = n - 0.5;
  double b = 48.0 * a * a;

  double z2 = a * log1p (t * t / n);
  double z = sqrt (z2);

  double p5 = z * poly_eval (coeffs6, 9, z2);
  double p4 = -z * poly_eval (coeffs5, 7, z2);
  double p3 = z * poly_eval (coeffs4, 5, z2);
  double p2 = -z * poly_eval (coeffs3, 3, z2);
  double p1 = z * (z2 + 3.0);
  double p0 = z;

  double y = p5;
  y = (y / b) + p4;
  y = (y / b) + p3;
  y = (y / b) + p2;
  y = (y / b) + p1;
  y = (y / b) + p0;

  if (t < 0)
    y *= -1;

  return y;
}