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);
}
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);
}
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]));
}
}
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;
}
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());
}
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());
}
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;
}
