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

    void infTimeLQR(const Eigen::Matrix<double,xDim,xDim> & A, 
        const Eigen::Matrix<double,xDim,uDim> & B)
    {
      reset();
      
      /*
      for (int i=0; i<infIter; i++) {
#if 0
        std::cout << "K: " << std::endl << _K; 
        << "V: " << std::endl << _V;
        << "===========================================" << std::endl;
#endif
        LQRBackup(A, B);
      }
      
      std::cout << "loop K\n" << _K << std::endl;
      std::cout << "loop V\n" << _V << std::endl;
      */

      dare(A, B, _V);
      _K = -(B.transpose() * _V * B + _R).llt().solve(B.transpose() * _V * A);
      
      //std::cout << "dare K\n" << _K << std::endl;
      //std::cout << "dare V\n" << _V << std::endl;
    }

void GreenStrain_LIMSolver2D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess)
{
  // green strain tensor energy
  Eigen::Matrix<double,2,3> S;
  for(int t=0;t<mesh->Triangles->rows();t++)
  {
    Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
    Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
    Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);

    Eigen::Matrix<double,2,3> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;

    // hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x
    Eigen::Matrix3d VTV = V.transpose()*V;
    Eigen::Matrix3d MMT = MMTs.block<3,3>(0,3*t);
    Eigen::Matrix<double,2,3> VMMT = V*MMT;
    Eigen::Matrix3d MMTVTV = MMT*VTV;

    int numElem = 0;
    for(int r=0;r<6;r++)
    {
      S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(2,3);
      S.coeffRef(r) = 1;

      Eigen::Matrix<double,2,3> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT);
      
      for(int c=r;c<6;c++)
        *denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider;
    }
  }
}

void static calcRotEqs(const Rod& r, const VecXe& rot, const std::vector<Vec3e>& curveBinorm,
                      VecXe& grad, std::vector<Triplet>& triplets) {
  Eigen::Matrix<real, 2, 2> J;
  J << 0.0, -1.0, 1.0, 0.0;
  for (int i=1; i<r.numEdges()-1; i++) {
    Vec3e m1 = cos(rot(i)) * r.next().u[i] + sin(rot(i)) * r.next().v(i);
    Vec3e m2 = -sin(rot(i)) * r.next().u[i] + cos(rot(i)) * r.next().v(i);
    Vec2e curvePrev(curveBinorm[i-1].dot(m2), -curveBinorm[i-1].dot(m1)); // omega ^i _i
    Vec2e curveNext(curveBinorm[i].dot(m2), -curveBinorm[i].dot(m1)); // omega ^i _i+1
    real dWprev = 1.0 / r.restVoronoiLength(i) *
      curvePrev.dot(J * r.getCS()[i].bendMat() * (curvePrev - r.restCurveNext(i)));
    real dWnext = 1.0 / r.restVoronoiLength(i+1) *
      curveNext.dot(J * r.getCS()[i+1].bendMat() * (curveNext - r.restCurvePrev(i+1)));
    real twistPrev = rot(i) - rot(i-1) + r.next().refTwist(i);
    real twistNext = rot(i+1) - rot(i) + r.next().refTwist(i+1);
    grad(i-1) = -(dWprev + dWnext + 2.0 * r.getCS()[i].twistCoeff() *
                  (twistPrev/r.restVoronoiLength(i) - twistNext/r.restVoronoiLength(i+1)));
    
    real hess = 2.0*(r.getCS()[i].twistCoeff()/r.restVoronoiLength(i) +
                     r.getCS()[i+1].twistCoeff()/r.restVoronoiLength(i+1));
    hess += 1.0 / r.restVoronoiLength(i) *
      (curvePrev.dot(J.transpose() * r.getCS()[i].bendMat() * J * curvePrev)
       - curvePrev.dot(r.getCS()[i].bendMat() * (curvePrev - r.restCurveNext(i))));
    hess += 1.0 /r.restVoronoiLength(i+1) *
      (curveNext.dot(J.transpose() * r.getCS()[i+1].bendMat() * J * curveNext)
       - curveNext.dot(r.getCS()[i+1].bendMat() * (curveNext - r.restCurvePrev(i+1))));
    triplets.push_back(Triplet(i-1, i-1, hess));
  }
}

 double Gaussian<ScalarType>::calculateDistance(const Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>& vector1, const Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>& vector2)
 {
     Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> tmp = vector1 - vector2;
     if (pseudoInverse)
     {
         /*DEBUG_OUT("pseudo", 10);
         DEBUG_VAR_OUT(*pseudoInverse, 0);
         DEBUG_VAR_OUT(tmp, 0);
         DEBUG_VAR_OUT(getCovarianceMatrix(), 0);
         DEBUG_VAR_OUT(getCovarianceMatrix().determinant(), 0);
         DEBUG_VAR_OUT((*pseudoInverse) * tmp, 0);
         DEBUG_VAR_OUT(tmp.transpose() * (*pseudoInverse), 0);
         DEBUG_VAR_OUT(tmp.transpose() * (*pseudoInverse) * tmp, 0);*/
         return std::sqrt(tmp.transpose() * (*pseudoInverse) * tmp);
     }
     else
     {
         /*DEBUG_OUT("nonpseudo", 10);
         DEBUG_VAR_OUT(tmp, 0);
         DEBUG_VAR_OUT(getCovarianceMatrix(), 0);
         DEBUG_VAR_OUT(getCovarianceMatrix().determinant(), 0);
         DEBUG_VAR_OUT(tmp.transpose() * llt.solve(tmp), 0);*/
         return std::sqrt(tmp.transpose() * llt.solve(tmp));
     }
 }

template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
    const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
    const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
    Matrix4 &transformation_matrix) const
{
  transformation_matrix.setIdentity ();

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();

  // Return the correct transformation
  transformation_matrix.topLeftCorner (3, 3) = R;
  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
  transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;
}

    void LQRBackup(const Eigen::Matrix<double,xDim,xDim> & A, 
        const Eigen::Matrix<double,xDim,uDim> & B)
    {
      Eigen::Matrix<double,uDim,xDim> tmpUX = B.transpose()*_V;
      _K = -(tmpUX*B + _R).llt().solve(tmpUX*A);

      Eigen::Matrix<double,xDim,xDim> tmpXX = A + B*_K;
      _V = tmpXX.transpose()*_V*tmpXX + _K.transpose()*_R*_K + _Q;
    }

 void KalmanFilter::updateState(double measurement, double variance, const
     Eigen::Matrix<double, 1, 2>& C, const Eigen::Matrix2d& P_k_km1) {
   Eigen::Matrix<double, 2, 1> K_k = P_k_km1 * C.transpose() *
     (C * P_k_km1 * C.transpose() +
     (Eigen::Matrix<double, 1, 1>() << variance).finished()).inverse();
   P_k_k_ = (Eigen::Matrix2d::Identity() - K_k * C) * P_k_km1;
   x_k_ = x_k_ + K_k *
     ((Eigen::Matrix<double, 1, 1>() << measurement).finished() - C * x_k_);
 }

bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d) {
    Eigen::Matrix<double,3,2> B;
    B <<  qC2C1.rotate(get_nor().getVec()), other.get_nor().getVec();
    const Eigen::Matrix2d BtB = B.transpose() * B;
    if(BtB.determinant() < 0.000001) { // Projection rays almost parallel.
        return false;
    }
    const Eigen::Vector2d dv = - BtB.inverse() * B.transpose() * C2rC2C1;
    d.setParameter(fabs(dv[0]));
    return true;
}

void KalmanFilter::measurementUpdate(const Measurement_t &meas, double dt)
{
  Eigen::Matrix<double, n_meas, n_states> H;
  H.setZero();
  H(0, 0) = 1;
  H(1, 1) = 1;
  H(2, 2) = 1;

  const Eigen::Matrix<double, n_states, n_meas> K = P * H.transpose() *
      (H*P*H.transpose() + R).inverse();
  const Measurement_t inno = meas - H*x;
  x += K*inno;
  P = (ProcessCov_t::Identity() - K*H) * P;
}

void boxFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, Pose pose){		
	//Transform the cloud
	//convert the tranform from our fiducial markers to the Eigen
    Eigen::Matrix<float, 3, 3> R;
    Eigen::Vector3f T;
    cv::cv2eigen(pose.getT(), T);
    cv::cv2eigen(pose.getR(), R);
    //get the inverse transform to bring the point cloud's into the
    //same coordinate frame
    Eigen::Affine3f transform;
    transform = Eigen::AngleAxisf(R.transpose());
    transform *= Eigen::Translation3f(-T);
    //transform the cloud in place
    pcl::transformPointCloud(*cloud, *cloud, transform);
	
	//Define the box
	float box = 200.00;
	pcl::PassThrough<pcl::PointXYZRGB> pass_z, pass_x, pass_y;
	//Filters in x
	pass_x.setFilterFieldName("x");
	pass_x.setFilterLimits(-box, box);
	pass_x.setInputCloud(cloud);
	pass_x.filter(*cloud);
	//Filters in y
	pass_y.setFilterFieldName("y");
	pass_y.setFilterLimits(-box, box);
	pass_y.setInputCloud(cloud);
	pass_y.filter(*cloud);
	//Filters in z
	pass_z.setFilterFieldName("z");
	pass_z.setFilterLimits(0,box);
	pass_z.setInputCloud(cloud);
	pass_z.filter(*cloud);	
}

 Eigen::MatrixXd parse_scheme (const Header& header)
 {
   Eigen::MatrixXd PE;
   const auto it = header.keyval().find ("pe_scheme");
   if (it != header.keyval().end()) {
     try {
       PE = parse_matrix (it->second);
     } catch (Exception& e) {
       throw Exception (e, "malformed PE scheme in image \"" + header.name() + "\"");
     }
     if (ssize_t(PE.rows()) != ((header.ndim() > 3) ? header.size(3) : 1))
       throw Exception ("malformed PE scheme in image \"" + header.name() + "\" - number of rows does not equal number of volumes");
   } else {
     // Header entries are cast to lowercase at some point
     const auto it_dir  = header.keyval().find ("PhaseEncodingDirection");
     const auto it_time = header.keyval().find ("TotalReadoutTime");
     if (it_dir != header.keyval().end() && it_time != header.keyval().end()) {
       Eigen::Matrix<default_type, 4, 1> row;
       row.head<3>() = Axes::id2dir (it_dir->second);
       row[3] = to<default_type>(it_time->second);
       PE.resize ((header.ndim() > 3) ? header.size(3) : 1, 4);
       PE.rowwise() = row.transpose();
     }
   }
   return PE;
 }

TwoBodyJastrowFactor::TwoBodyJastrowFactor (const Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic> &correlation_matrix)
    : m_correlation_matrix(correlation_matrix)
{
    // ensure the matrix is square and symmetric
    BOOST_ASSERT(correlation_matrix.rows() == correlation_matrix.cols());
    BOOST_ASSERT(correlation_matrix == correlation_matrix.transpose());
}

IGL_INLINE void igl::create_vector_vbo(
  const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & V,
  GLuint & V_vbo_id)
{
  //// Expects that input is list of 3D vectors along rows
  //assert(V.cols() == 3);

  // Generate Buffers
  glGenBuffers(1,&V_vbo_id);
  // Bind Buffers
  glBindBuffer(GL_ARRAY_BUFFER,V_vbo_id);
  // Copy data to buffers
  // We expect a matrix with each vertex position on a row, we then want to
  // pass this data to OpenGL reading across rows (row-major)
  if(V.Options & Eigen::RowMajor)
  {
    glBufferData(
      GL_ARRAY_BUFFER,
      sizeof(T)*V.size(),
      V.data(),
      GL_STATIC_DRAW);
  }else
  {
    // Create temporary copy of transpose
    Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> VT = V.transpose();
    // If its column major then we need to temporarily store a transpose
    glBufferData(
      GL_ARRAY_BUFFER,
      sizeof(T)*V.size(),
      VT.data(),
      GL_STATIC_DRAW);
  }
  // bind with 0, so, switch back to normal pointer operation
  glBindBuffer(GL_ARRAY_BUFFER, 0);
}

     /**
      * p = R*x + T
      * if inverse:
      *  x = R^1*(p - T)
      */
 inline Eigen::Affine3f
 RT2Transform(cv::Mat& R, cv::Mat& T, bool inverse)
 {
   //convert the tranform from our fiducial markers to
   //the Eigen
   Eigen::Matrix<float, 3, 3> eR;
   Eigen::Vector3f eT;
   cv::cv2eigen(R, eR);
   cv::cv2eigen(T, eT);
   // p = R*x + T
   Eigen::Affine3f transform;
   if (inverse)
   {
     //x = R^1*(p - T)
     transform = Eigen::Translation3f(-eT);
     transform.prerotate(eR.transpose());
   }
   else
   {
     //p = R*x + T
     transform = Eigen::AngleAxisf(eR);
     transform.translate(eT);
   }
   return transform;
 }

void PointsKalmanFilterPredictor::predict(double time, const std::vector<Eigen::Vector3d> &u, std::vector<Eigen::Matrix<double, 6, 1> >& mu, std::vector<Eigen::Matrix<double, 6, 6> >& sigma)
{
    if (time == 0.)
    {
        mu = mus_;
        sigma = sigmas_;
    }
    else
    {
        Eigen::Matrix<double, 6, 6> A;
        Eigen::Matrix<double, 6, 3> B;

        A.setIdentity();
        A.block(0, 3, 3, 3) = Eigen::Matrix3d::Identity() * time;

        B.block(0, 0, 3, 3).setIdentity();
        B.block(3, 0, 3, 3) = Eigen::Matrix3d::Identity() * time;

        mu.resize(mus_.size());
        sigma.resize(sigmas_.size());

        for (int i=0; i<mu.size(); i++)
        {
            mu[i] = A * mus_[i] + B * u[i];
            sigma[i] = A * sigmas_[i] * A.transpose() + R_;
        }
    }
}