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> ¢roid_src, const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean, const Eigen::Matrix<Scalar, 4, 1> ¢roid_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_; } } }
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(); } }
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); } }
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> ¢roid_src, const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean, const Eigen::Matrix<Scalar, 4, 1> ¢roid_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; }
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]); }
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)); }
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(); }
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(); }
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(); } }
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"; }
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(); }
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); }
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; }