예제 #1
0
    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;
    }
  }
}
예제 #3
0
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));
  }
}
예제 #4
0
 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));
     }
 }
예제 #5
0
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;
}
예제 #6
0
    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;
    }
예제 #7
0
 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_);
 }
예제 #8
0
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;
}
예제 #9
0
파일: filter.cpp 프로젝트: CedricDC/vicon
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;
}
예제 #10
0
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);	
}
예제 #11
0
 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;
 }
예제 #12
0
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());
}
예제 #13
0
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);
}
예제 #14
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;
 }
예제 #15
0
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_;
        }
    }
}
예제 #16
0
void GreenStrain_LIMSolver2D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
  // Compute deformation gradients
  int numTets = mesh->Triangles->rows();
  Ms.resize(3,2*numTets);
  MMTs.resize(3,3*numTets);

  Eigen::Matrix<double,3,2> SelectorM;
  SelectorM.block<2,2>(0,0) = Eigen::Matrix<double,2,2>::Identity();
  SelectorM.row(2) = Eigen::Vector2d::Ones()*-1;
  
  for(int t=0;t<numTets;t++)
  {
    Eigen::Vector2d A,B,C;
    if(mesh->IsCorotatedTriangles)
    {
      A = mesh->CorotatedTriangles->row(t).block<1,2>(0,0).cast<double>();
      B = mesh->CorotatedTriangles->row(t).block<1,2>(0,2).cast<double>();
      C = mesh->CorotatedTriangles->row(t).block<1,2>(0,4).cast<double>();	
    }
    else
    {
      A = mesh->InitalVertices->row(mesh->Triangles->coeff(t,0)).block<1,2>(0,0).cast<double>();
      B = mesh->InitalVertices->row(mesh->Triangles->coeff(t,1)).block<1,2>(0,0).cast<double>();
      C = mesh->InitalVertices->row(mesh->Triangles->coeff(t,2)).block<1,2>(0,0).cast<double>();
    }

    Eigen::Matrix2d V;
    V << A-C,B-C;
    
    Eigen::Matrix<double,3,2> Mtemp = SelectorM*V.inverse().cast<double>();
    Ms.block<3,2>(0,2*t) = Mtemp;
    MMTs.block<3,3>(0,3*t) = Mtemp*Mtemp.transpose();
  }
}
예제 #17
0
inline void igl::PlanarizerShapeUp<DerivedV, DerivedF>::assembleP()
{
  P.setZero(3*ni*numF);
  for (int fi = 0; fi< numF; fi++)
  {
    // todo: this can be made faster by omitting the selector matrix
    Eigen::SparseMatrix<typename DerivedV::Scalar > Sfi;
    assembleSelector(fi, Sfi);
    Eigen::SparseMatrix<typename DerivedV::Scalar > NSi = Ni*Sfi;
    
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> Vi = NSi*Vv;
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> CC(3,ni);
    for (int i = 0; i <ni; ++i)
      CC.col(i) = Vi.segment(3*i, 3);
    Eigen::Matrix<typename DerivedV::Scalar, 3, 3> C = CC*CC.transpose();
    
    // Alec: Doesn't compile
    Eigen::EigenSolver<Eigen::Matrix<typename DerivedV::Scalar, 3, 3>> es(C);
    // the real() is for compilation purposes
    Eigen::Matrix<typename DerivedV::Scalar, 3, 1> lambda = es.eigenvalues().real();
    Eigen::Matrix<typename DerivedV::Scalar, 3, 3> U = es.eigenvectors().real();
    int min_i;
    lambda.cwiseAbs().minCoeff(&min_i);
    U.col(min_i).setZero();
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> PP = U*U.transpose()*CC;
    for (int i = 0; i <ni; ++i)
     P.segment(3*ni*fi+3*i, 3) =  weightsSqrt[fi]*PP.col(i);
    
  }
}
예제 #18
0
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimation2D<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);
  
  float angle = atan2 ((H (0, 1) - H (1, 0)), (H(0, 0) + H (1, 1)));
  
  Eigen::Matrix<Scalar, 3, 3> R (Eigen::Matrix<Scalar, 3, 3>::Identity ());
  R (0, 0) = R (1, 1) = cos (angle);
  R (0, 1) = -sin (angle);
  R (1, 0) = sin (angle);

  // Return the correct transformation
  transformation_matrix.topLeftCorner (3, 3).matrix () = R;
  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3).matrix ());
  transformation_matrix.block (0, 3, 3, 1).matrix () = centroid_tgt.head (3) - Rc;
}
예제 #19
0
void EKF::predict(float* inputArray, float _dt, float *currentEstimate) {

//	__android_log_print(ANDROID_LOG_VERBOSE, "AEKF",
//					"Gyro predict: %.3f %.3f %.3f | %.3f", inputArray[0], inputArray[1], inputArray[2], _dt);

	// We should do predict or correct?
//	if (!correctTime)
//	{
		this->x_apriori = this->statePrediction(inputArray, _dt);
		Eigen::Matrix<double, 7, 7> F = this->jacobian(inputArray, _dt);
		this->P_apriori = F * this->P_aposteriori * F.transpose() + this->Q;
		correctTime = true;
//	}

	// Normalize
	double norm = this->x_apriori.block<4, 1>(0, 0).norm();
	this->x_apriori.block<4, 1>(0, 0) = this->x_apriori.block<4, 1>(0, 0) / norm;

	// Update current estimate
	for (int i=0;i<4;i++)
		currentEstimate[i] = this->x_apriori(i);

//	__android_log_print(ANDROID_LOG_VERBOSE, "AEKF",
//			"Estimate predict: %.3f %.3f %.3f %.3f", currentEstimate[0],
//			currentEstimate[1], currentEstimate[2], currentEstimate[3]);
}
예제 #20
0
 double GaussianDiagCov<ScalarType>::calculateValueWithoutWeights(const Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>& dataVector)
 {
     assert(dataVector.size() == mean.size());
     Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> dist = dataVector - mean;
     //Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> dist2 = dist;
     return preFactorWithoutWeights * std::exp(-0.5 * (dist.transpose() * ldlt.solve(dist))(0));
 }
예제 #21
0
 inline double
 trace_inv_quad_form_ldlt(const stan::math::LDLT_factor<double,R2,C2> &A,
                          const Eigen::Matrix<double,R3,C3> &B) {
   stan::math::validate_multiplicable(A,B,"trace_inv_quad_form_ldlt");
   
   return (B.transpose()*A._ldltP->solve(B)).trace();
 }
예제 #22
0
 UncertainTransformation::covariance_t UncertainTransformation::UOplus() const
 {
   Eigen::Matrix<double,6,6> S;
   S.setIdentity();
   S.topRightCorner<3,3>() = -sm::kinematics::crossMx(_t_a_b_a);
   return S.inverse().eval()*_U*S.transpose().inverse().eval();
 }
void KalmanFilter::delta_change(){
  
  float dt2_2 = (pow(delta_t_,2))/2;
  
  //Recompute A, state transition
  A_ = Eigen::Matrix<float, 9, 9>::Identity();
  A_ << 1., 0.,   0.,     delta_t_,   0.,     0.,    dt2_2,     0.,     0.,
	0., 1.,   0.,       0.,     delta_t_, 0.,      0.,     dt2_2,   0.,
	0., 0.,   1.,       0.,       0.,   delta_t_,  0.,      0.,    dt2_2,
	0., 0.,   0.,       1.,       0.,     0.,    delta_t_,  0.,     0.,
	0., 0.,   0.,       0.,       1.,     0.,      0.,    delta_t_, 0.,
	0., 0.,   0.,       0.,       0.,     1.,      0.,      0.,     delta_t_,
	0., 0.,   0.,       0.,       0.,     0.,      1.,      0.,     0.,
	0., 0.,   0.,       0.,       0.,     0.,      0.,      1.,     0.,
	0., 0.,   0.,       0.,       0.,     0.,      0.,      0.,     1.;
     
  //Recompute Q, covariance process noise
  Eigen::Matrix<float, 9, 3>  G;
  G.fill(0.);
  //Process noise only in the highest order term
  G(6,0) = delta_t_;
  G(7,1) = delta_t_;
  G(8,2) = delta_t_;
  Q_ = G * sigma_jerk_ * G.transpose();
  
}
예제 #24
0
cfgScalar MaterialQuad2D::getEnergy(Element2D* ele, ElementMesh2D * mesh)
{
  cfgScalar energy = 0;
  if (m_planeStress==false)
  {
    for(int ii = 0; ii<q->x.size();ii++){
      Matrix2S F = ele->defGrad(q->x[ii], mesh->X, mesh->x);
      energy += q->w[ii] * e[ii]->getEnergy(F);
    }
    energy *= ele->getVol(mesh->X);
  }
  else
  {
    MatrixXS K = getStiffness(ele, mesh);

    Eigen::Matrix<cfgScalar, 8, 1> u;

    for(int ii = 0; ii<ele->nV(); ii++)
    {
      int vi = ele->at(ii);
      Vector2S ui = mesh->x[vi] - mesh->X[vi];
      u(2*ii) = ui[0];
      u(2*ii+1) = ui[1];
    }
    energy = 0.5*u.transpose()*K*u;
  }
  return energy;
}
void GreenStrain_LIMSolver3D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
  const int numNodes = mesh->InitalVertices->rows();

  // Compute deformation gradients
  int numTets = mesh->Tetrahedra->rows();
  Ms.resize(4,3*numTets);
  MMTs.resize(4,4*numTets);

  Eigen::Matrix<double,4,3> SelectorM;
  SelectorM.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  SelectorM.row(3) = Eigen::Vector3d::Ones()*-1;
  
  for(int t=0;t<numTets;t++)
  {
    Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);

    Eigen::Vector3d A = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,0)).cast<double>();
    Eigen::Vector3d B = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,1)).cast<double>();
    Eigen::Vector3d C = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,2)).cast<double>();
    Eigen::Vector3d D = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,3)).cast<double>();

    Eigen::Matrix3d V;
    V << A-D,B-D,C-D;
    
    Eigen::Matrix<double,4,3> Mtemp = SelectorM*V.inverse().cast<double>();
    Ms.block<4,3>(0,3*t) = Mtemp;
    MMTs.block<4,4>(0,4*t) = Mtemp*Mtemp.transpose();
  }
}
예제 #26
0
void CKF::update(const SensorValues &sensorValues){

   // Process matrix   
   Eigen::Matrix<float, 4, 4> F = Eigen::Matrix<float, 4, 4>::Identity();
   F(0,2) = cos(state(2))*dt;
   F(1,2) = sin(state(2))*tan(state(1))*dt;
   F(1,3) = dt;
   
   // Process update
   state = F*state;
   var = F*var*F.transpose()+Q;

   // Observation update
   Eigen::Vector4f obs;
    
   // Calculate pitch and roll angle from accelerometers
   float Ax = sensorValues.sensors[Sensors::InertialSensor_AccX];
   float Ay = sensorValues.sensors[Sensors::InertialSensor_AccY];
   float Az = sensorValues.sensors[Sensors::InertialSensor_AccZ];

   obs(0) = atan(Ay/Az); // Roll
   obs(1) = atan(-Ax/Az)*cos(obs(0)); // Pitch
   obs(2) = sensorValues.sensors[Sensors::InertialSensor_GyrX]; // Roll velocity
   obs(3) = sensorValues.sensors[Sensors::InertialSensor_GyrY]; // Pitch velocity

   //std::cout << "Forward Lean / Side Lean : Obs = " << RAD2DEG(obs(1)) << ", " << RAD2DEG(obs(2)) << "\n";

   Eigen::Matrix<float, 4, 4> K = var * (var + R).inverse();
   state = state + K*(obs-state);
   var = var - K*var;

   //std::cout << "Roll / Pitch: " << RAD2DEG(state(0)) << ", " << RAD2DEG(state(1)) << "\n";

}
예제 #27
0
 inline double
 trace_quad_form(const Eigen::Matrix<double,RA,CA> &A,
                 const Eigen::Matrix<double,RB,CB> &B)
 {
   validate_square(A,"trace_quad_form");
   validate_multiplicable(A,B,"trace_quad_form");
   return (B.transpose()*A*B).trace();
 }
예제 #28
0
inline double
squared_distance(const Eigen::Matrix<double, R1, C1>& v1,
                 const Eigen::Matrix<double, R2, C2>& v2) {
    check_vector("squared_distance", "v1", v1);
    check_vector("squared_distance", "v2", v2);
    check_matching_sizes("squared_distance", "v1", v1, "v2", v2);
    return (v1.transpose()-v2).squaredNorm();
}
T my_matrixfun(
       Eigen::Matrix<T,Eigen::Dynamic,1> const &a,
       Eigen::Matrix<T,Eigen::Dynamic,1> const &b){
   Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> m(a.size(),a.size());
   m.setZero();
   m.diagonal() = a;
   return (b.transpose() * m * b)(0,0);
}
예제 #30
0
int main()
{
  typedef Matrix<double,5,1> Vector5d;

  Vector5d roots = Vector5d::Random();
  cout << "Roots: " << roots.transpose() << endl;
  Eigen::Matrix<double,6,1> polynomial;
  roots_to_monicPolynomial( roots, polynomial );

  PolynomialSolver<double,5> psolve( polynomial );
  cout << "Complex roots: " << psolve.roots().transpose() << endl;

  std::vector<double> realRoots;
  psolve.realRoots( realRoots );
  Map<Vector5d> mapRR( &realRoots[0] );
  cout << "Real roots: " << mapRR.transpose() << endl;

  cout << endl;
  cout << "Illustration of the convergence problem with the QR algorithm: " << endl;
  cout << "---------------------------------------------------------------" << endl;
  Eigen::Matrix<float,7,1> hardCase_polynomial;
  hardCase_polynomial <<
  -0.957, 0.9219, 0.3516, 0.9453, -0.4023, -0.5508, -0.03125;
  cout << "Hard case polynomial defined by floats: " << hardCase_polynomial.transpose() << endl;
  PolynomialSolver<float,6> psolvef( hardCase_polynomial );
  cout << "Complex roots: " << psolvef.roots().transpose() << endl;
  Eigen::Matrix<float,6,1> evals;
  for( int i=0; i<6; ++i ){ evals[i] = std::abs( poly_eval( hardCase_polynomial, psolvef.roots()[i] ) ); }
  cout << "Norms of the evaluations of the polynomial at the roots: " << evals.transpose() << endl << endl;

  cout << "Using double's almost always solves the problem for small degrees: " << endl;
  cout << "-------------------------------------------------------------------" << endl;
  PolynomialSolver<double,6> psolve6d( hardCase_polynomial.cast<double>() );
  cout << "Complex roots: " << psolve6d.roots().transpose() << endl;
  for( int i=0; i<6; ++i )
  {
    std::complex<float> castedRoot( psolve6d.roots()[i].real(), psolve6d.roots()[i].imag() );
    evals[i] = std::abs( poly_eval( hardCase_polynomial, castedRoot ) );
  }
  cout << "Norms of the evaluations of the polynomial at the roots: " << evals.transpose() << endl << endl;

  cout.precision(10);
  cout << "The last root in float then in double: " << psolvef.roots()[5] << "\t" << psolve6d.roots()[5] << endl;
  std::complex<float> castedRoot( psolve6d.roots()[5].real(), psolve6d.roots()[5].imag() );
  cout << "Norm of the difference: " << internal::abs( psolvef.roots()[5] - castedRoot ) << endl;
}