C++ Matrix::dot方法代码示例

 // returns the 90 deg rotation of a (around n) most similar to target b
 /// a and b should be in the same plane orthogonal to N
 static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_new(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
                                                                const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b,
                                                                const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& n)
 {
   Eigen::Matrix<typename DerivedV::Scalar, 3, 1> c = (a.cross(n)).normalized();
   typename DerivedV::Scalar scorea = a.dot(b);
   typename DerivedV::Scalar scorec = c.dot(b);
   if (fabs(scorea)>=fabs(scorec))
     return a*Sign(scorea);
   else
     return c*Sign(scorec);
 }

inline Real evaluate_point<1>(const Triangle<3>& t, const Point& point, const Eigen::Matrix<Real,3,1>& coefficients)
{
	Eigen::Matrix<Real,3,1> bary_coeff = t.getBaryCoordinates(point);
	//std::cout<< "B-coord: "<<bary_coeff<<std::endl;

	return(coefficients.dot(bary_coeff));
}

///
/// @par Detailed description 
/// ... 
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
signed_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{

    /*
    This method calcultes the sign of the angle which is used in the calculation of a dihedral angle.
    As such, this method is not validated for other uses.  It will fail if the basis atoms for the
    dihedral (atom 2 and atom 3) overlap.
    */

    float sa = 180.0 ;
    float angle ; 
    float pi = acos(-1.0) ;

    float ada = coor1.dot(coor1) ;    
    float bdb = coor2.dot(coor2) ;    

    if(ada*bdb <= 0.0)
    {
        std::cout << "; " ;
        return sa ;
    }
    else
    {
        float adb = coor1.dot(coor2) ;    
        float argument = adb/sqrt(ada*bdb) ;
        angle = (180.0/pi) * acos(argument) ;
    }

    Eigen::Matrix<float,3,1> cp ; cp = coor1.cross(coor2) ;
    float dp = coor3.dot(cp) ;
    
    float sign = 0.0 ;
    if(dp < 0.0) 
    {
        sign = -1.0 ;
    }
    else if(dp > 0.0)
    {
        sign = 1.0 ;
    }

    sa = sign*angle ;

    return sa ;
}

 inline double dot_product(const Eigen::Matrix<double, R1, C1>& v1, 
                           const Eigen::Matrix<double, R2, C2>& v2) {
   stan::math::check_vector("dot_product(%1%)",v1,"v1",(double*)0);
   stan::math::check_vector("dot_product(%1%)",v2,"v2",(double*)0);
   stan::math::check_matching_sizes("dot_product(%1%)",v1,"v1",
                                    v2,"v2",(double*)0);
   return v1.dot(v2);
 }

Scalar costFunc(
        const Eigen::Matrix<Scalar, -1,  1> &beta,
        const Eigen::Matrix<Scalar, -1, -1> &X,
        const Eigen::Matrix<Scalar, -1,  1> &y)
{
    Eigen::Matrix<Scalar, -1, 1> tmp = (X * beta - y);
    return tmp.dot(tmp);
}

 inline double multiply(const Eigen::Matrix<double, 1, C1>& rv,
                        const Eigen::Matrix<double, R2, 1>& v) {
   stan::math::check_matching_sizes("multiply",
                                              "rv", rv,
                                              "v", v);
   if (rv.size() != v.size())
     throw std::domain_error("rv.size() != v.size()");
   return rv.dot(v);
 }

 // returns the 180 deg rotation of a (around n) most similar to target b
 // a and b should be in the same plane orthogonal to N
 static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_line(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
                                                                        const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b)
 {
     typename DerivedV::Scalar scorea = a.dot(b);
     if (scorea<0)
         return -a;
     else
         return a;
 }

IGL_INLINE void igl::GeneralPolyVectorFieldFinder<DerivedV, DerivedF>::getGeneralCoeffConstraints(const Eigen::VectorXi &isConstrained,
                                                       const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
                                                       int k,
                                                       const Eigen::VectorXi &rootsIndex,
                                                       Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> &Ck)
{
  int numConstrained = isConstrained.sum();
  Ck.resize(numConstrained,1);
  // int n = rootsIndex.cols();

  Eigen::MatrixXi allCombs;
  {
    Eigen::VectorXi V = Eigen::VectorXi::LinSpaced(n,0,n-1);
    igl::nchoosek(V,k+1,allCombs);
  }

  int ind = 0;
  for (int fi = 0; fi <numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = B2.row(fi);
    if(isConstrained[fi])
    {
      std::complex<typename DerivedV::Scalar> ck(0);

      for (int j = 0; j < allCombs.rows(); ++j)
      {
        std::complex<typename DerivedV::Scalar> tk(1.);
        //collect products
        for (int i = 0; i < allCombs.cols(); ++i)
        {
          int index = allCombs(j,i);

          int ri = rootsIndex[index];
          Eigen::Matrix<typename DerivedV::Scalar, 1, 3> w;
          if (ri>0)
            w = cfW.block(fi,3*(ri-1),1,3);
          else
            w = -cfW.block(fi,3*(-ri-1),1,3);
          typename DerivedV::Scalar w0 = w.dot(b1);
          typename DerivedV::Scalar w1 = w.dot(b2);
          std::complex<typename DerivedV::Scalar> u(w0,w1);
          tk*= u;
        }
        //collect sum
        ck += tk;
      }
      Ck(ind) = ck;
      ind ++;
    }
  }


}

Eigen::Matrix<double, 3, 3> getRotationFromA2B(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B) {
	Eigen::Matrix<double, 3, 1> v = A.cross(B);
	double s = v.norm();
	double c = A.dot(B);

	Eigen::Matrix<double, 3, 3> vx;
	vx <<   0, -v[2], v[1],
			v[2], 0, -v[0],
			-v[1], v[0], 0;
	return Eigen::Matrix<double, 3, 3>::Identity() + vx + ((1.0-c)/(s*s))*vx*vx;
}

IGL_INLINE void igl::per_corner_normals(
  const Eigen::PlainObjectBase<DerivedV>& /*V*/,
  const Eigen::PlainObjectBase<DerivedF>& F,
  const Eigen::PlainObjectBase<DerivedV>& FN,
  const std::vector<std::vector<IndexType> >& VF,
  const double corner_threshold,
  Eigen::PlainObjectBase<DerivedV> & CN)
{
  using namespace igl;
  using namespace Eigen;
  using namespace std;

  // number of faces
  const int m = F.rows();
  // valence of faces
  const int n = F.cols();

  // initialize output to ***zero***
  CN.setZero(m*n,3);

  // loop over faces
  for(size_t i = 0;int(i)<m;i++)
  {
    // Normal of this face
    Eigen::Matrix<typename DerivedV::Scalar,3,1> fn = FN.row(i);
    // loop over corners
    for(size_t j = 0;int(j)<n;j++)
    {
      const std::vector<IndexType> &incident_faces = VF[F(i,j)];
      // loop over faces sharing vertex of this corner
      for(int k = 0;k<(int)incident_faces.size();k++)
      {
        Eigen::Matrix<typename DerivedV::Scalar,3,1> ifn = FN.row(incident_faces[k]);
        // dot product between face's normal and other face's normal
        double dp = fn.dot(ifn);
        // if difference in normal is slight then add to average
        if(dp > cos(corner_threshold*PI/180))
        {
          // add to running sum
          CN.row(i*n+j) += ifn;
        // else ignore
        }else
        {
        }
      }
      // normalize to take average
      CN.row(i*n+j).normalize();
    }
  }
}

float KalmanTrace::sq_distance_in_sigmas (const Observation& observation) {
    update_prediction( observation.time );

    Eigen::Matrix<double,Obs,1> expected_observation 
        = observation_matrix * prediction.position;
    Eigen::Matrix<double,Obs,Obs> covar_of_observations
        = observation_matrix * prediction.covariance
            * observation_matrix.transpose();

    Eigen::Matrix<double,Obs,Obs> covar_of_next_measurement =
            covar_of_observations + observation.covariance;

    Eigen::Matrix<double,Obs,1> difference 
        = expected_observation - observation.position;

    return difference.dot( 
        covar_of_next_measurement.inverse() * difference );
}

///
/// @par Detailed description 
/// ... 
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
calc_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{
    /*
    Method to calculate the angle between three atoms
    */

    float angle ;

    Eigen::Matrix<float,3,1> u ; u = coor1 - coor2 ;
    Eigen::Matrix<float,3,1> v ; v = coor3 - coor2 ;

    float c = (u.dot(v))/(u.norm()*v.norm()) ;
    angle = acos(c) ;

    return angle ;
}

IGL_INLINE void igl::grad(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
  const Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> &F,
  const Eigen::Matrix<T, Eigen::Dynamic, 1>&X,
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &G )
{
  G = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(F.rows(),3);
  for (int i = 0; i<F.rows(); ++i)
  {
    // renaming indices of vertices of triangles for convenience
    int i1 = F(i,0);
    int i2 = F(i,1);
    int i3 = F(i,2);
    
    // #F x 3 matrices of triangle edge vectors, named after opposite vertices
    Eigen::Matrix<T, 1, 3> v32 = V.row(i3) - V.row(i2);
    Eigen::Matrix<T, 1, 3> v13 = V.row(i1) - V.row(i3);
    Eigen::Matrix<T, 1, 3> v21 = V.row(i2) - V.row(i1);
    
    // area of parallelogram is twice area of triangle
    // area of parallelogram is || v1 x v2 || 
    Eigen::Matrix<T, 1, 3> n  = v32.cross(v13); 
    
    // This does correct l2 norm of rows, so that it contains #F list of twice
    // triangle areas
    double dblA = std::sqrt(n.dot(n));
    
    // now normalize normals to get unit normals
    Eigen::Matrix<T, 1, 3> u = n / dblA;
    
    // rotate each vector 90 degrees around normal
    double norm21 = std::sqrt(v21.dot(v21));
    double norm13 = std::sqrt(v13.dot(v13));
    Eigen::Matrix<T, 1, 3> eperp21 = u.cross(v21);
    eperp21 = eperp21 / std::sqrt(eperp21.dot(eperp21));
    eperp21 *= norm21;
    Eigen::Matrix<T, 1, 3> eperp13 = u.cross(v13);
    eperp13 = eperp13 / std::sqrt(eperp13.dot(eperp13));
    eperp13 *= norm13;
    
    G.row(i) = ((X[i2] -X[i1]) *eperp13 + (X[i3] - X[i1]) *eperp21) / dblA;
  };
}

//.........这里部分代码省略.........
        b(1)   = b(1)   + dV2*dW ;

        A(2,2) = A(2,2) + dUV*dUV;
        A(2,3) = A(2,3) + dUV*dU ;
        A(2,4) = A(2,4) + dUV*dV ;
        A(2,5) = A(2,5) + dUV    ;
        b(3)   = b(3)   + dUV*dW ;

        A(3,3) = A(3,3) + dU *dU ;
        A(3,4) = A(3,4) + dU *dV ;
        A(3,5) = A(3,5) + dU     ;
        b(3)   = b(3)   + dU *dW ;

        A(4,4) = A(4,4) + dV *dV ;
        A(4,5) = A(4,5) + dV     ;
        b(5)   = b(5)   + dV *dW ;

        A(5,5) = A(5,5) + 1      ;
        b(5)   = b(5)   + 1  *dW ;
    }

    // Mat is symmetric
    //
    A(1,0) = A(0,1);
    A(2,0) = A(0,2);
    A(3,0) = A(0,3);
    A(4,0) = A(0,4);
    A(5,0) = A(0,5);

    A(2,1) = A(1,2);
    A(3,1) = A(1,3);
    A(4,1) = A(1,4);
    A(5,1) = A(1,5);

    A(3,2) = A(2,3);
    A(4,2) = A(2,4);
    A(5,2) = A(2,5);

    A(4,3) = A(3,4);
    A(5,3) = A(3,5);

    A(5,4) = A(4,5);

    Eigen::HouseholderQR< Eigen::Matrix<double,6,6> > qr(A);
    x = qr.solve(b);

    // FunctionContainer gets an implicit function F(x,y,z) = 0 of this form
    // _fCoeff[0] +
    // _fCoeff[1]*x   + _fCoeff[2]*y   + _fCoeff[3]*z   +
    // _fCoeff[4]*x^2 + _fCoeff[5]*y^2 + _fCoeff[6]*z^2 +
    // _fCoeff[7]*x*y + _fCoeff[8]*x*z + _fCoeff[9]*y*z
    //
    // The bivariate polynomial surface we get here is of the form
    // z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f
    // Writing it as implicit surface F(x,y,z) = 0 gives this form
    // F(x,y,z) = f(x,y) - z = a*x^2 + b*y^2 + c*x*y + d*x + e*y - z + f
    // Thus:
    // _fCoeff[0] = f
    // _fCoeff[1] = d
    // _fCoeff[2] = e
    // _fCoeff[3] = -1
    // _fCoeff[4] = a
    // _fCoeff[5] = b
    // _fCoeff[6] = 0
    // _fCoeff[7] = c
    // _fCoeff[8] = 0
    // _fCoeff[9] = 0

    _fCoeff[0] = x(5);
    _fCoeff[1] = x(3);
    _fCoeff[2] = x(4);
    _fCoeff[3] = -1.0;
    _fCoeff[4] = x(0);
    _fCoeff[5] = x(1);
    _fCoeff[6] =  0.0;
    _fCoeff[7] = x(2);
    _fCoeff[8] =  0.0;
    _fCoeff[9] =  0.0;

    // Get S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2]
    double sigma = 0;
    FunctionContainer clFuncCont(_fCoeff);
    for (std::vector<Base::Vector3d>::const_iterator it = transform.begin(); it != transform.end(); ++it) {
        double u = it->x;
        double v = it->y;
        double z = clFuncCont.F(u, v, 0.0);
        sigma += z*z;
    }

    sigma += dW2 - 2 * x.dot(b);
    // This must be caused by some round-off errors. Theoretically it's impossible
    // that 'sigma' becomes negative.
    if (sigma < 0)
        sigma = 0;
    if (!_vPoints.empty())
        sigma = sqrt(sigma/_vPoints.size());

    _fLastResult = static_cast<float>(sigma);
    return _fLastResult;
}

template<typename PointSource, typename PointTarget> double
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
                                                                                  double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
                                                                                  PointCloudSource &trans_cloud)
{
  // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
  double phi_0 = -score;
  // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
  double d_phi_0 = -(score_gradient.dot (step_dir));

  Eigen::Matrix<double, 6, 1>  x_t;

  if (d_phi_0 >= 0)
  {
    // Not a decent direction
    if (d_phi_0 == 0)
      return 0;
    else
    {
      // Reverse step direction and calculate optimal step.
      d_phi_0 *= -1;
      step_dir *= -1;

    }
  }

  // The Search Algorithm for T(mu) [More, Thuente 1994]

  int max_step_iterations = 10;
  int step_iterations = 0;

  // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994]
  double mu = 1.e-4;
  // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
  double nu = 0.9;

  // Initial endpoints of Interval I,
  double a_l = 0, a_u = 0;

  // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
  double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
  double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);

  double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
  double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);

  // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
  bool interval_converged = (step_max - step_min) > 0, open_interval = true;

  double a_t = step_init;
  a_t = std::min (a_t, step_max);
  a_t = std::max (a_t, step_min);

  x_t = x + step_dir * a_t;

  final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
                           Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
                           Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
                           Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();

  // New transformed point cloud
  transformPointCloud (*input_, trans_cloud, final_transformation_);

  // Updates score, gradient and hessian.  Hessian calculation is unessisary but testing showed that most step calculations use the
  // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
  score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);

  // Calculate phi(alpha_t)
  double phi_t = -score;
  // Calculate phi'(alpha_t)
  double d_phi_t = -(score_gradient.dot (step_dir));

  // Calculate psi(alpha_t)
  double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
  // Calculate psi'(alpha_t)
  double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);

  // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
  while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
  {
    // Use auxilary function if interval I is not closed
    if (open_interval)
    {
      a_t = trialValueSelectionMT (a_l, f_l, g_l,
                                   a_u, f_u, g_u,
                                   a_t, psi_t, d_psi_t);
    }
    else
    {
      a_t = trialValueSelectionMT (a_l, f_l, g_l,
                                   a_u, f_u, g_u,
                                   a_t, phi_t, d_phi_t);
    }

    a_t = std::min (a_t, step_max);
    a_t = std::max (a_t, step_min);

    x_t = x + step_dir * a_t;

    final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
//.........这里部分代码省略.........