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;
}
