Exemplo n.º 1
0
double EW_CHMN::gR_q(const QCD::quark q) const
{
    double c1, c2, c3;
    switch (q) {
        case QCD::UP:
        case QCD::CHARM:
            c1 = -0.15470;
            c2 = -0.13942;
            c3 = -0.67184;
            break;
        case QCD::DOWN:
        case QCD::STRANGE:
            c1 = 0.07734;
            c2 = 0.06971;
            c3 = 0.33590;
            break;
        case QCD::BOTTOM:
            c1 = 0.07742;
            c2 = 0.06971;
            c3 = 0.33590;
            c1 += -0.000042 * x_h() - 0.000025 * pow(x_h(), 4.0);
            break;
        case QCD::TOP:
        default:
            throw std::runtime_error("Error in EW_CHMN::gR_q()");
    }

    double deltaGVq = 0.0, deltaGAq = 0.0;
    if (SM.getModelName().compare("StandardModel") != 0) {
        deltaGVq = (dynamic_cast<const NPbase*> (&SM))->deltaGV_f(SM.getQuarks(q));
        deltaGAq = (dynamic_cast<const NPbase*> (&SM))->deltaGA_f(SM.getQuarks(q));
    }

    return ( c1 + c2 * Delta_gbarZ2() + c3 * Delta_sbar2()
            + (deltaGVq - deltaGAq) / 2.0);
}
Exemplo n.º 2
0
double EW_CHMN::gL_q(const QCD::quark q) const
{
    double c1, c2, c3;
    switch (q) {
        case QCD::UP:
        case QCD::CHARM:
            c1 = 0.34675;
            c2 = 0.31309;
            c3 = -0.66793;
            break;
        case QCD::DOWN:
        case QCD::STRANGE:
            c1 = -0.42434;
            c2 = -0.38279;
            c3 = 0.33166;
            break;
        case QCD::BOTTOM:
            c1 = -0.42116;
            c2 = -0.38279;
            c3 = 0.33166;
            c1 += -0.000058 * x_h() + 0.000128 * x_t();
            break;
        case QCD::TOP:
        default:
            throw std::runtime_error("Error in EW_CHMN::gL_q()");
    }

    double deltaGVq = 0.0, deltaGAq = 0.0;
    if (SM.getModelName().compare("StandardModel") != 0) {
        deltaGVq = (dynamic_cast<const NPbase*> (&SM))->deltaGV_f(SM.getQuarks(q));
        deltaGAq = (dynamic_cast<const NPbase*> (&SM))->deltaGA_f(SM.getQuarks(q));
    }

    return ( c1 + c2 * Delta_gbarZ2() + c3 * Delta_sbar2()
            + (deltaGVq + deltaGAq) / 2.0);
}
Exemplo n.º 3
0
void LipmVarHeightPlanner::backwardPass(const Traj<3,3> &com)
{
  Eigen::Matrix<double,6,6> Qxx;
  Eigen::Matrix<double,3,3> Quu;
  Eigen::Matrix<double,3,3> invQuu;
  Eigen::Matrix<double,3,6> Qux;
  Eigen::Matrix<double,6,3> Qxu;
  Eigen::Matrix<double,6,1> Qx;
  Eigen::Matrix<double,3,1> Qu;

  Eigen::Matrix<double,6,6> A;
  Eigen::Matrix<double,6,3> B;
  Eigen::Matrix<double,6,6> Vxx = _Vxx;
  Eigen::Matrix<double,6,1> Vx = Eigen::Matrix<double,6,1>::Zero();

  Eigen::Matrix<double,6,1> cur_x;
  Eigen::Matrix<double,3,1> cur_u;
  Eigen::Matrix<double,6,1> x_h;
  Eigen::Matrix<double,3,1> u_h;

  //NEW_GSL_MATRIX(tmpXX0, tmpXX0_d, tmpXX0_v, 6, 6);
  //NEW_GSL_MATRIX(tmpUX0, tmpUX0_d, tmpUX0_v, 3, 6);
 
  const double *x0, *u0;

  for (int t = (int)_K.size()-1; t >= 0; t--) {
    //printf("======================================\n");
    // get x u
    x0 = _traj0[t].x;
    u0 = _traj0[t].u;
    
    //printf("%g %g %g %g %g %g\n", x0[0], x0[1], x0[2], x0[3], x0[4], x0[5]);
    //printf("%g %g %g\n", u0[0], u0[1], u0[2]);
    
    for (int i = 0; i < 6; i++) {
      cur_x(i) = x0[i];
      x_h(i) = x0[i] - _zmp_d[t].x[i];
    }
    for (int i = 0; i < 3; i++) {
      cur_u(i) = u0[i];
      u_h(i) = u0[i] - _zmp_d[t].u[i];
    }
    
    // get A B
    getAB(x0, u0, _z0[t], A, B);
    
    //printGSLMatrix("A", A);
    //printGSLMatrix("B", B);
   
    // Qx = Q*x_hat + A'*Vx
    Qx = _Q*x_h + A.transpose()*Vx;

    // Qu = R*u_hat + B'*Vx
    Qu = _R*u_h + B.transpose()*Vx;

    // Qxx = Q + A'*Vxx*A
    Qxx = _Q + A.transpose()*Vxx*A;

    // Qxu = A'*Vxx*B
    Qxu = A.transpose()*Vxx*B;

    // Quu = R + B'*Vxx*B
    Quu = _R + B.transpose()*Vxx*B;

    // Qux = Qxu'
    Qux = B.transpose()*Vxx*A;

    // K = -inv(Quu)*Qux
    _K[t] = -(Quu.llt().solve(Qux));
    
    // du = -inv(Quu)*Qu
    _du[t] = -(Quu.llt().solve(Qu));

    // Vx = Qx + K'*Qu
    Vx = Qx + _K[t].transpose()*Qu;
    
    // Vxx = Qxx + Qxu*K
    Vxx = Qxx + Qxu*_K[t];
    
    //getchar();
    //assert(!hasInfNan(_K[t]));
    //assert(!hasInfNan(_du[t]));
  }
}
Exemplo n.º 4
0
double LipmVarHeightPlanner::forwardPass(const double *x0, Traj<3,3> &traj1) const
{
  Eigen::Matrix<double,6,1> z;
  Eigen::Matrix<double,6,1> z1;
  Eigen::Matrix<double,3,1> u;

  traj1 = _zmp_d;
  //traj1 = Traj<3,3>();
  //for (size_t i = 0; i < _zmp_d.size(); i++)
  //  traj1.append(_zmp_d[i].time, _zmp_d[i].type, NULL, NULL, NULL, NULL);
  
  dvec_copy(traj1[0].x, x0, 6);
  
  double cost = 0;

  Eigen::Matrix<double,6,1> x_h;
  Eigen::Matrix<double,3,1> u_h;

  for (size_t t = 0; t < _zmp_d.size(); t++) {
    // x - xref
    for (int i = 0; i < 6; i++) {
      z(i) = traj1[t].x[i] - _traj0[t].x[i];
      x_h(i) = traj1[t].x[i] - _zmp_d[t].x[i];
    }

    // u = alpha*du + uref
    u = _alpha*_du[t];
    for (int i = 0; i < 3; i++)
      u(i) += _traj0[t].u[i];

    // u += K*z
    u += _K[t]*z;
    for (int i = 0; i < 3; i++)
      traj1[t].u[i] = u(i);

    for (int i = 0; i < 3; i++)
      u_h(i) = traj1[t].u[i] - _zmp_d[t].u[i];

    // compute cost
    cost += 0.5 * x_h.transpose() * _Q * x_h;
    cost += 0.5 * u_h.transpose() * _R * u_h;
     
    // integrate
    if (t < _zmp_d.size()-1)
      integrate(traj1[t].x, traj1[t].u, _z0[t], traj1[t].acc, traj1[1+t].x);
  }
  
  /*
  FILE *out = fopen("tmp/lipmz_traj0", "w"); 
  for (size_t t = 0; t < _traj0.size(); t++) {
    fprintf(out, "%g %g %g %g %g %g\n", _traj0[t].x[0], _traj0[t].x[1], _traj0[t].x[2], _traj0[t].u[0], _traj0[t].u[1], _traj0[t].u[2]);
  }
  fclose(out);
 
  out = fopen("tmp/lipmz_traj1", "w"); 
  for (size_t t = 0; t < traj1.size(); t++) {
    fprintf(out, "%g %g %g %g %g %g\n", traj1[t].x[0], traj1[t].x[1], traj1[t].x[2], traj1[t].u[0], traj1[t].u[1], traj1[t].u[2]);
  }
  fclose(out);
  */ 

  return cost;
}
Exemplo n.º 5
0
double EW_CHMN::DeltaMw_SM() const
{
    return ( -0.137 * x_h() - 0.019 * x_h() * x_h() + 0.018 * x_t()
            - 0.005 * x_alpha() - 0.002 * x_s());
}
Exemplo n.º 6
0
double EW_CHMN::DeltaTz_SM() const
{
    return ( -0.0995 * x_h() - 0.2858 * x_h() * x_h() + 0.1175 * x_h() * x_h() * x_h()
            + 0.0367 * x_t() + 0.00026 * x_t() * x_t() - 0.0017 * x_h() * x_t()
            - 0.0033 * x_s() - 0.0001 * x_t() * x_s());
}
Exemplo n.º 7
0
double EW_CHMN::DeltaSz_SM() const
{
    return ( 0.2217 * x_h() - 0.1188 * x_h() * x_h() + 0.0320 * x_h() * x_h() * x_h()
            - 0.0014 * x_t() + 0.0005 * x_s());
}
void fi::VPDetectionWrapper::validateVanishingPoint(const std::vector<std::vector< Eigen::Vector2f> > &computed_vp_hypothesis, const Eigen::Matrix3f &cam_calib, Eigen::Vector3f &final_robust_vp_x, Eigen::Vector3f &final_robust_vp_y)
{
	Eigen::Matrix3f inv_cam_calib = cam_calib.inverse(); 

	//trans from vps to rays through camera axis, see Z+H Chapter 8, more on single view geometry!
	unsigned int num_vps = computed_vp_hypothesis.size();
	std::vector< Eigen::Vector3f> computed_vp_hypothesis_x;
	std::vector< Eigen::Vector3f> computed_vp_hypothesis_y;
	std::vector< Eigen::Vector3f> computed_vp_hypothesis_z;
	for (unsigned int i = 0; i < num_vps; i++)
	{
		std::vector<Eigen::Vector2f> a_vp = computed_vp_hypothesis.at(i);

		Eigen::Vector2f a_x = a_vp.at(0);
		Eigen::Vector3f x_h, n_x;
		x_h(0) = a_x(0);
		x_h(1) = a_x(1);
		x_h(2) = 1;
		n_x = inv_cam_calib * x_h;
		n_x = n_x.normalized();
		computed_vp_hypothesis_x.push_back(n_x);

		Eigen::Vector2f a_y = a_vp.at(1);
		Eigen::Vector3f y_h, n_y;
		y_h(0) = a_y(0);
		y_h(1) = a_y(1);
		y_h(2) = 1;
		n_y = inv_cam_calib * y_h;
		n_y = n_y.normalized();
		computed_vp_hypothesis_y.push_back(n_y);

		Eigen::Vector2f a_z = a_vp.at(2);
		Eigen::Vector3f z_h, n_z;
		z_h(0) = a_z(0);
		z_h(1) = a_z(1);
		z_h(2) = 1;
		n_z = inv_cam_calib * z_h;
		n_z = n_z.normalized();
		computed_vp_hypothesis_z.push_back(n_z);
	}

	std::vector<Eigen::Vector3f> in_liers_x;
	std::vector<Eigen::Vector3f> in_liers_y;
	std::vector<Eigen::Vector3f> in_liers_z;
	bool found_inliers_x = getRansacInliers(computed_vp_hypothesis_x, in_liers_x);
	bool found_inliers_y = getRansacInliers(computed_vp_hypothesis_y, in_liers_y);
	bool found_inliers_z = getRansacInliers(computed_vp_hypothesis_z, in_liers_z);

	Eigen::VectorXf optimized_vp_x;
	Eigen::VectorXf optimized_vp_y;
	Eigen::VectorXf optimized_vp_z;
	leastQuaresVPFitting(in_liers_x, optimized_vp_x);
	leastQuaresVPFitting(in_liers_y, optimized_vp_y);
	leastQuaresVPFitting(in_liers_z, optimized_vp_z);
        std::cout<<"Vanishing Points Validated"<<std::endl;

	//test the angles and see if OK otherwise check again if truelly orthogonal
	Eigen::Vector3f vp_x (optimized_vp_x[3], optimized_vp_x[4], optimized_vp_x[5]);;
	Eigen::Vector3f vp_y (optimized_vp_y[3], optimized_vp_y[4], optimized_vp_y[5]);
	Eigen::Vector3f vp_z (optimized_vp_z[3], optimized_vp_z[4], optimized_vp_z[5]);

	Eigen::Vector3f vp_x_centroid (optimized_vp_x[0], optimized_vp_x[1], optimized_vp_x[2]);
	Eigen::Vector3f vp_y_centroid (optimized_vp_y[0], optimized_vp_y[1], optimized_vp_y[2]);
	Eigen::Vector3f vp_z_centroid (optimized_vp_z[0], optimized_vp_z[1], optimized_vp_z[2]);

	float angle_value_radiens_cxy = angleBetweenVectors(vp_x_centroid, vp_y_centroid);
	float angle_value_degrees_cxy = pcl::rad2deg(angle_value_radiens_cxy);
	float angle_value_radiens_cxz = angleBetweenVectors(vp_x_centroid, vp_z_centroid);
	float angle_value_degrees_cxz = pcl::rad2deg(angle_value_radiens_cxz);
	float angle_value_radiens_cyz = angleBetweenVectors(vp_y_centroid, vp_z_centroid);
	float angle_value_degrees_cyz = pcl::rad2deg(angle_value_radiens_cyz);

	float angle_value_radiens_xy = angleBetweenVectors(vp_x, vp_y);
	float angle_value_degrees_xy = pcl::rad2deg(angle_value_radiens_xy);
	float angle_value_radiens_xz = angleBetweenVectors(vp_x, vp_z);
	float angle_value_degrees_xz = pcl::rad2deg(angle_value_radiens_xz);
	float angle_value_radiens_yz = angleBetweenVectors(vp_y, vp_z);
	float angle_value_degrees_yz = pcl::rad2deg(angle_value_radiens_yz);

	//collect only the mean vps
	final_robust_vp_x = optimized_vp_x.tail<3> ();
	final_robust_vp_y = optimized_vp_y.tail<3> ();

	//final_robust_vp_x = vp_x_centroid;
	//final_robust_vp_y = vp_y_centroid;
}