IGL_INLINE bool igl::GeneralPolyVectorFieldFinder<DerivedV, DerivedF>::
solve(const Eigen::VectorXi &isConstrained,
const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
const Eigen::VectorXi &rootsIndex,
Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &output)
{
// polynomial is of the form:
// z^(2n) +
// -c[0]z^(2n-1) +
// c[1]z^(2n-2) +
// -c[2]z^(2n-3) +
// ... +
// (-1)^n c[n-1]
std::vector<Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1>> coeffs(n,Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1>::Zero(numF, 1));
for (int i =0; i<n; ++i)
{
int degree = i+1;
Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> Ck;
getGeneralCoeffConstraints(isConstrained,
cfW,
i,
rootsIndex,
Ck);
Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > DD;
computeCoefficientLaplacian(degree, DD);
Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > f; f.resize(numF,1);
if (isConstrained.sum() == numF)
coeffs[i] = Ck;
else
minQuadWithKnownMini(DD, f, isConstrained, Ck, coeffs[i]);
}
std::vector<Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> > pv;
setFieldFromGeneralCoefficients(coeffs, pv);
output.setZero(numF,3*n);
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);
for (int i=0; i<n; ++i)
output.block(fi,3*i, 1, 3) = pv[i](fi,0)*b1 + pv[i](fi,1)*b2;
}
return true;
}
void dare(const Eigen::Matrix<double,xDim,xDim> &A, const Eigen::Matrix<double,xDim,uDim> &B, Eigen::Matrix<double,xDim,xDim> &P)
{
Eigen::Matrix<double,xDim,xDim> Ainv = A.inverse();
Eigen::Matrix<double,xDim,xDim> ABRB = Ainv * B * _R.llt().solve(B.transpose());
Eigen::Matrix<double,2*xDim,2*xDim> Z;
Z.block(0,0,xDim,xDim) = Ainv;
Z.block(0,xDim,xDim,xDim) = ABRB;
Z.block(xDim,0,xDim,xDim) = _Q * Ainv;
Z.block(xDim,xDim,xDim,xDim) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*xDim,2*xDim> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*xDim,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*xDim,2*xDim> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*xDim,xDim> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*xDim; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > xDim)
break;
}
}
Eigen::Matrix<std::complex<double>,xDim,xDim> U21inv = unstableEigVec.block(0,0,xDim,xDim).inverse();
Eigen::Matrix<std::complex<double>,xDim,xDim> PP = unstableEigVec.block(xDim,0,xDim,xDim) * U21inv;
for (int i = 0; i < xDim; i++) {
for (int j = 0; j < xDim; j++) {
P(i,j) = PP(i,j).real();
}
}
}
IGL_INLINE void igl::repdiag(
const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & A,
const int d,
Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & B)
{
int m = A.rows();
int n = A.cols();
B.resize(m*d,n*d);
B.array() *= 0;
for(int i = 0;i<d;i++)
{
B.block(i*m,i*n,m,n) = A;
}
}
IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::getGeneralCoeffConstraints(const Eigen::VectorXi &isConstrained,
const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
int k,
Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> &Ck)
{
int numConstrained = isConstrained.sum();
Ck.resize(numConstrained,1);
int n = cfW.cols()/3;
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);
const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &w = cfW.block(fi,3*index,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*u;
}
//collect sum
ck += tk;
}
Ck(ind) = ck;
ind ++;
}
}
}
void SFMViewer::update(std::vector<cv::Point3d> pcld,
std::vector<cv::Vec3b> pcldrgb,
std::vector<cv::Point3d> pcld_alternate,
std::vector<cv::Vec3b> pcldrgb_alternate,
std::vector<cv::Matx34d> cameras) {
m_pcld = pcld;
m_pcldrgb = pcldrgb;
m_cameras = cameras;
//get the scale of the result cloud using PCA
{
cv::Mat_<double> cldm(pcld.size(), 3);
for (unsigned int i = 0; i < pcld.size(); i++) {
cldm.row(i)(0) = pcld[i].x;
cldm.row(i)(1) = pcld[i].y;
cldm.row(i)(2) = pcld[i].z;
}
cv::Mat_<double> mean; //cv::reduce(cldm,mean,0,CV_REDUCE_AVG);
cv::PCA pca(cldm, mean, CV_PCA_DATA_AS_ROW);
scale_cameras_down = 1.0 / (3.0 * sqrt(pca.eigenvalues.at<double> (0)));
// std::cout << "emean " << mean << std::endl;
// m_global_transform = Eigen::Translation<double,3>(-Eigen::Map<Eigen::Vector3d>(mean[0]));
}
//compute transformation to place cameras in world
m_cameras_transforms.resize(m_cameras.size());
Eigen::Vector3d c_sum(0,0,0);
for (int i = 0; i < m_cameras.size(); ++i) {
Eigen::Matrix<double, 3, 4> P = Eigen::Map<Eigen::Matrix<double, 3, 4,
Eigen::RowMajor> >(m_cameras[i].val);
Eigen::Matrix3d R = P.block(0, 0, 3, 3);
Eigen::Vector3d t = P.block(0, 3, 3, 1);
Eigen::Vector3d c = -R.transpose() * t;
c_sum += c;
m_cameras_transforms[i] =
Eigen::Translation<double, 3>(c) *
Eigen::Quaterniond(R) *
Eigen::UniformScaling<double>(scale_cameras_down)
;
}
m_global_transform = Eigen::Translation<double,3>(-c_sum / (double)(m_cameras.size()));
// m_global_transform = m_cameras_transforms[0].inverse();
updateGL();
}
void ResultElementIpData::CalculateValues(const StructureBase& rStructure,
Eigen::Matrix<double, 1, Eigen::Dynamic>& rValues) const
{
const ElementBase* element(rStructure.ElementGetElementPtr(mElementId));
std::map<Element::eOutput, std::shared_ptr<ElementOutputBase>> elementOutput;
elementOutput[Element::eOutput::IP_DATA] = std::make_shared<ElementOutputIpData>(mIpDataType);
const_cast<ElementBase*>(element)->Evaluate(elementOutput);
const auto& ipDataResult = elementOutput.at(Element::eOutput::IP_DATA)->GetIpData().GetIpDataMap()[mIpDataType];
// iterate over all ips
assert(ipDataResult.cols() == element->GetNumIntegrationPoints());
unsigned int numComponents = ipDataResult.rows();
for (int iCol = 0; iCol < ipDataResult.cols(); ++iCol)
{
rValues.block(0, iCol * numComponents, 1, ipDataResult.rows()) = ipDataResult.col(iCol).transpose();
}
}
geometry_msgs::Pose C_HunoLimbKinematics::ForwardKinematics(const double *thetas_C)
{
geometry_msgs::Pose outLimbPose;
Eigen::Vector3d rot_axis = Eigen::Vector3d::Zero();
double rot_angle;
Eigen::Matrix4d limbTF = Eigen::Matrix4d::Identity();
Eigen::Matrix4d expXihatTheta = Eigen::Matrix4d::Identity();
Eigen::Matrix<double, 6, 5> tempJacobian;
tempJacobian.setZero(); // initialize temporary variable to zeros
int limbCtr = 0;
/* Lock jacobian matrix while being formed */
m_isJacobianLocked = true;
/* Calculate joint transformation matrices sequentially and populate jacobian */
for (int jointIdx = 0; jointIdx < m_numJoints; jointIdx++)
{
/* use previous expXihatTheta to generate adjoint matrix
and fill next column of jacobian */
tempJacobian.col(limbCtr) = AdjointMatrix(expXihatTheta) * m_zetas_M.col(jointIdx);
/* calculate for next joint */
expXihatTheta = ExpXihatTheta(jointIdx, (*(thetas_C+jointIdx))*DEG2RAD);
limbTF *= expXihatTheta;
/* increment counter */
limbCtr++;
}
if (limbCtr == m_numJoints)
{
limbTF *= m_g0Mat_M; // apply configuration matrix
m_jacobian_M = tempJacobian.block(0,0, m_numJoints, m_numJoints); // save jacobian
m_isJacobianLocked = false; // unlock jacobian matrix
}
else
{ // reset matrices since something didn't add up.
limbTF.setZero();
m_jacobian_M.setZero();
}
/* save limb transformation matrix into pose message */
outLimbPose.position.x = limbTF(0,3);
outLimbPose.position.y = limbTF(1,3);
outLimbPose.position.z = limbTF(2,3);
//back out rotation unit vector and angle from rotation matrix
rot_angle = acos( ( ((limbTF.block<3,3>(0,0)).trace())-1 ) /2 );
if (sin(rot_angle) != 0)
{
rot_axis(0) = (limbTF(2,1)-limbTF(1,2)) / (2*sin(rot_angle));
rot_axis(1) = (limbTF(0,2)-limbTF(2,0)) / (2*sin(rot_angle));
rot_axis(2) = (limbTF(1,0)-limbTF(0,1)) / (2*sin(rot_angle));
}
// else rot_axis is zeroes
outLimbPose.orientation.x = rot_axis(0)*sin(rot_angle/2);
outLimbPose.orientation.y = rot_axis(1)*sin(rot_angle/2);
outLimbPose.orientation.z = rot_axis(2)*sin(rot_angle/2);
outLimbPose.orientation.w = cos(rot_angle/2);
return outLimbPose;
} // end ForwardKinematics()
//! Function for updating common blocks of partial to current state.
void EmpiricalAccelerationPartial::update( const double currentTime )
{
if( !( currentTime_ == currentTime ) )
{
using namespace tudat::basic_mathematics;
using namespace tudat::linear_algebra;
// Get current state and associated data.
Eigen::Vector6d currentState = empiricalAcceleration_->getCurrentState( );
Eigen::Vector3d angularMomentumVector = Eigen::Vector3d( currentState.segment( 0, 3 ) ).cross(
Eigen::Vector3d( currentState.segment( 3, 3 ) ) );
Eigen::Vector3d crossVector = angularMomentumVector.cross( Eigen::Vector3d( currentState.segment( 0, 3 ) ) );
// Compute constituent geometric partials.
Eigen::Matrix3d normPositionWrtPosition = calculatePartialOfNormalizedVector( Eigen::Matrix3d::Identity( ), currentState.segment( 0, 3 ) );
Eigen::Matrix3d angularMomentumWrtPosition = -getCrossProductMatrix( currentState.segment( 3, 3 ) );
Eigen::Matrix3d angularMomentumWrtVelocity = getCrossProductMatrix( currentState.segment( 0, 3 ) );
Eigen::Matrix3d normAngularMomentumWrtPosition = calculatePartialOfNormalizedVector( angularMomentumWrtPosition, angularMomentumVector );
Eigen::Matrix3d normAngularMomentumWrtVelocity = calculatePartialOfNormalizedVector( angularMomentumWrtVelocity, angularMomentumVector );
Eigen::Matrix3d crossVectorWrtPosition = getCrossProductMatrix( angularMomentumVector ) -
angularMomentumWrtVelocity * angularMomentumWrtPosition;
Eigen::Matrix3d crossVectorWrtVelocity = -getCrossProductMatrix( currentState.segment( 0, 3 ) ) *
getCrossProductMatrix( currentState.segment( 0, 3 ) );
Eigen::Matrix3d normCrossVectorWrtPosition = calculatePartialOfNormalizedVector( crossVectorWrtPosition, crossVector );
Eigen::Matrix3d normCrossVectorWrtVelocity = calculatePartialOfNormalizedVector( crossVectorWrtVelocity, crossVector );
// Retrieve current empirical acceleration
Eigen::Vector3d localAcceleration = empiricalAcceleration_->getCurrentLocalAcceleration( );
// Compute partial derivative contribution of derivative of rotation matrix from RSW to inertial frame
currentPositionPartial_ = localAcceleration.x( ) * normPositionWrtPosition + localAcceleration.y( ) * normCrossVectorWrtPosition +
localAcceleration.z( ) * normAngularMomentumWrtPosition;
currentVelocityPartial_ = localAcceleration.y( ) * normCrossVectorWrtVelocity + localAcceleration.z( ) * normAngularMomentumWrtVelocity;
// Compute partial derivative contribution of derivative of true anomaly
Eigen::Matrix< double, 1, 6 > localTrueAnomalyPartial = calculateNumericalPartialOfTrueAnomalyWrtState(
empiricalAcceleration_->getCurrentState( ),
empiricalAcceleration_->getCurrentGravitationalParameter( ), cartesianStateElementPerturbations );
Eigen::Matrix< double, 1, 6 > trueAnomalyPartial = localTrueAnomalyPartial;
currentPositionPartial_ += empiricalAcceleration_->getCurrentToInertialFrame( ) * (
empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::sine_empirical ) *
std::cos( empiricalAcceleration_->getCurrentTrueAnomaly( ) ) -
empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::cosine_empirical ) *
std::sin( empiricalAcceleration_->getCurrentTrueAnomaly( ) )
) * trueAnomalyPartial.block( 0, 0, 1, 3 );
currentVelocityPartial_ +=
( empiricalAcceleration_->getCurrentToInertialFrame( ) * (
empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::sine_empirical ) *
std::cos( empiricalAcceleration_->getCurrentTrueAnomaly( ) ) -
empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::cosine_empirical ) *
std::sin( empiricalAcceleration_->getCurrentTrueAnomaly( ) ) ) ) * trueAnomalyPartial.block( 0, 3, 1, 3 );
currentTime_ = currentTime;
// Check output.
if( currentPositionPartial_ != currentPositionPartial_ )
{
throw std::runtime_error( "Error, empirical acceleration position partials are NaN" );
}
if( currentVelocityPartial_ != currentVelocityPartial_ )
{
throw std::runtime_error( "Error, empirical acceleration velocity partials are NaN" );
}
}
}
void SetUp() override
{
using SurgSim::Math::getSubVector;
using SurgSim::Math::getSubMatrix;
using SurgSim::Math::addSubMatrix;
m_nodeIds[0] = 3;
m_nodeIds[1] = 1;
m_nodeIds[2] = 14;
m_nodeIds[3] = 9;
std::vector<size_t> m_nodeIdsVectorForm; // Useful for assembly helper function
m_nodeIdsVectorForm.push_back(m_nodeIds[0]);
m_nodeIdsVectorForm.push_back(m_nodeIds[1]);
m_nodeIdsVectorForm.push_back(m_nodeIds[2]);
m_nodeIdsVectorForm.push_back(m_nodeIds[3]);
m_restState.setNumDof(3, 15);
Vector& x0 = m_restState.getPositions();
// Tet is aligned with the axis (X,Y,Z), centered on (0.1, 1.2, 2.3), embedded in a cube of size 1
getSubVector(m_expectedX0, 0, 3) = getSubVector(x0, m_nodeIds[0], 3) = Vector3d(0.1, 1.2, 2.3);
getSubVector(m_expectedX0, 1, 3) = getSubVector(x0, m_nodeIds[1], 3) = Vector3d(1.1, 1.2, 2.3);
getSubVector(m_expectedX0, 2, 3) = getSubVector(x0, m_nodeIds[2], 3) = Vector3d(0.1, 2.2, 2.3);
getSubVector(m_expectedX0, 3, 3) = getSubVector(x0, m_nodeIds[3], 3) = Vector3d(0.1, 1.2, 3.3);
// The tet is part of a cube of size 1x1x1 (it occupies 1/6 of the cube's volume)
m_expectedVolume = 1.0 / 6.0;
m_rho = 1000.0;
m_E = 1e6;
m_nu = 0.45;
m_expectedMassMatrix.setZero(3*15, 3*15);
m_expectedDampingMatrix.setZero(3*15, 3*15);
m_expectedStiffnessMatrix.setZero(3*15, 3*15);
m_expectedStiffnessMatrix2.setZero(3*15, 3*15);
m_vectorOnes.setOnes(3*15);
Eigen::Matrix<double, 12, 12> M = Eigen::Matrix<double, 12, 12>::Zero();
{
M.diagonal().setConstant(2.0);
M.block(0, 3, 9, 9).diagonal().setConstant(1.0);
M.block(0, 6, 6, 6).diagonal().setConstant(1.0);
M.block(0, 9, 3, 3).diagonal().setConstant(1.0);
M.block(3, 0, 9, 9).diagonal().setConstant(1.0);
M.block(6, 0, 6, 6).diagonal().setConstant(1.0);
M.block(9, 0, 3, 3).diagonal().setConstant(1.0);
}
M *= m_rho * m_expectedVolume / 20.0;
addSubMatrix(M, m_nodeIdsVectorForm, 3 , &m_expectedMassMatrix);
Eigen::Matrix<double, 12, 12> K = Eigen::Matrix<double, 12, 12>::Zero();
{
// Calculation done by hand from
// http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
// ai = {}
// bi = {-1 1 0 0}
// ci = {-1 0 1 0}
// di = {-1 0 0 1}
Eigen::Matrix<double, 6, 12> B = Eigen::Matrix<double, 6, 12>::Zero();
Eigen::Matrix<double, 6, 6> E = Eigen::Matrix<double, 6, 6>::Zero();
B(0, 0) = -1; B(0, 3) = 1;
B(1, 1) = -1; B(1, 7) = 1;
B(2, 2) = -1; B(2, 11) = 1;
B(3, 0) = -1; B(3, 1) = -1; B(3, 4) = 1; B(3, 6) = 1;
B(4, 1) = -1; B(4, 2) = -1; B(4, 8) = 1; B(4, 10) = 1;
B(5, 0) = -1; B(5, 2) = -1; B(5, 5) = 1; B(5, 9) = 1;
B *= 1.0 / (6.0 * m_expectedVolume);
E.block(0, 0, 3, 3).setConstant(m_nu);
E.block(0, 0, 3, 3).diagonal().setConstant(1.0 - m_nu);
E.block(3, 3, 3, 3).diagonal().setConstant(0.5 - m_nu);
E *= m_E / (( 1.0 + m_nu) * (1.0 - 2.0 * m_nu));
K = m_expectedVolume * B.transpose() * E * B;
}
addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix);
// Expecte stiffness matrix given for our case in:
// http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
double E = m_E / (12.0*(1.0 - 2.0*m_nu)*(1.0 + m_nu));
double n0 = 1.0 - 2.0 * m_nu;
double n1 = 1.0 - m_nu;
K.setZero();
// Fill up the upper triangle part first (without diagonal elements)
K(0, 1) = K(0, 2) = K(1, 2) = 1.0;
K(0, 3) = -2.0 * n1; K(0, 4) = -n0; K(0, 5) = -n0;
K(1, 3) = -2.0 * m_nu; K(1, 4) = -n0;
K(2, 3) = -2.0 * m_nu; K(2, 5) = -n0;
K(0, 6) = - n0; K(0, 7) = -2.0 * m_nu;
K(1, 6) = - n0; K(1, 7) = -2.0 * n1; K(1, 8) = - n0;
K(2, 7) = - 2.0 * m_nu; K(2, 8) = -n0;
K(0, 9) = - n0; K(0, 11) = -2.0 * m_nu;
K(1, 10) = - n0; K(1, 11) = -2.0 * m_nu;
K(2, 9) = - n0; K(2, 10) = - n0; K(2, 11) = -2.0 * n1;
K(3, 7) = K(3, 11) = 2.0 * m_nu;
K(4, 6) = n0;
K(5, 9) = n0;
K(7, 11) = 2.0 * m_nu;
K(8, 10) = n0;
K += K.transpose().eval(); // symmetric part (do not forget the .eval() !)
K.block(0,0,3,3).diagonal().setConstant(4.0 - 6.0 * m_nu); // diagonal elements
K.block(3,3,9,9).diagonal().setConstant(n0); // diagonal elements
K(3, 3) = K(7, 7) = K(11, 11) = 2.0 * n1; // diagonal elements
K *= E;
addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix2);
}
void compute_weights() {
assert(m_samples.size() == m_values.size());
using namespace std;
using namespace cgra;
// L = [ K S ] X = [ W ] Y = [ V ]
// [ S^t 0 ], [ A ], [ 0 ]
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> L(m_samples.size() + 3, m_samples.size() + 3);
Eigen::Matrix<T, Eigen::Dynamic, 1> x(m_samples.size() + 3);
Eigen::Matrix<T, Eigen::Dynamic, 1> Y(m_samples.size() + 3);
// build L and Y
// zero out L and Y
L.setConstant(0);
Y.setConstant(0);
for (int i = 0; i < m_samples.size(); i++) {
// K = [ k(s0,s0) k(s0,s1) .. k(s0,sn) ]
// [ k(s1,s0) k(s1,s1) .. k(s1,sn) ]
// [ .. .. .. .. ]
// [ k(sn,s0) k(sn,s1) .. k(sn,sn) ]
for (int j = i + 1; j < m_samples.size(); j++) {
T v = thin_plate_kernal(vector2<T>(m_samples[i]), vector2<T>(m_samples[j]));
L(i, j) = v;
L(j, i) = v;
}
// S = [ 1 x0 y0 ]
// [ 1 x1 y1 ]
// [ .. .. .. ]
// [ 1 xn yn ]
L(i, m_samples.size() + 0) = 1;
L(i, m_samples.size() + 1) = m_samples[i].x;
L(i, m_samples.size() + 2) = m_samples[i].y;
// S^t
L(m_samples.size() + 0, i) = 1;
L(m_samples.size() + 1, i) = m_samples[i].x;
L(m_samples.size() + 2, i) = m_samples[i].y;
// V = [ v0 ]
// [ v1 ]
// [ .. ]
// [ vn ]
Y(i) = m_values[i];
}
// solve for X
// Useing LU decomposition because L is symmetric
Eigen::PartialPivLU<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>> solver(L);
Eigen::Matrix<T, Eigen::Dynamic, 1> X = solver.solve(Y);
// W = [ w0 ]
// [ w1 ]
// [ .. ]
// [ wn ]
m_weights.clear();
m_weights.reserve(m_samples.size() + 3);
for (int i = 0; i < m_samples.size(); i++) {
m_weights.push_back(X(i));
}
// A = [ a0 ]
// [ a1 ]
// [ a2 ]
m_a0 = X(m_samples.size() + 0);
m_a1 = X(m_samples.size() + 1);
m_a2 = X(m_samples.size() + 2);
// caculate the energy I = W^t K W
Eigen::Matrix<T, Eigen::Dynamic, 1> W(X.block(0, 0, m_samples.size(), 1));
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> K(L.block(0, 0, m_samples.size(), m_samples.size()));
m_energy = W.transpose() * K * W;
std::cout << "W" << W << std::endl;
std::cout << "K" << K << std::endl;
std::cout << "m_energy" << m_energy << std::endl << std::endl;
}
Result iterate() {
setupVariables();
// for now we use a iteration counter to protect us from infinite loops
// TODO< implement loop detection scheme, basic idea
// * we manage a fixed sized array as a sliding window or the hashes of the operations
// * if we detect a loop we apply bland's rule to resolve it (is the rule detection realy neccesary?)
//
//
// * every operation (pivot column, pivot row, number of pivot element itself) gets an hash
// * at each iteration we subtract the previous hash and add the current hash
// * if the hash doesn't change in n iteration(where n == 2 for the simple loop) we are looping
// >
// TODO< implement https://en.wikipedia.org/wiki/Bland's_rule >
unsigned iterationCounter = 0;
const unsigned MaximalIterationCounter = 128;
for(;;) {
iterationCounter++;
if( iterationCounter > MaximalIterationCounter ) {
return Result(Result::EnumSolverState::TOOMANYITERATIONS);
}
bool foundMaximumColumn;
const int pivotColumnIndex = searchMinimumColumn(foundMaximumColumn);
// all values in the target value row are < 0.0, done
if( !foundMaximumColumn ) {
return Result(Result::EnumSolverState::FOUNDSOLUTION);
}
//std::cout << "min column " << pivotColumnIndex << std::endl;
if( areAllEntriesOfPivotColumnNegativeOrZero(pivotColumnIndex) ) {
// solution is unbounded
return Result(Result::EnumSolverState::UNBOUNDEDSOLUTION);
}
// divide numbers of pivot column with right side and store in temporary vector
Eigen::Matrix<Type, Eigen::Dynamic, 1> minRatioVector = divideRightSideWithPivotColumnWhereAboveZero(pivotColumnIndex);
//std::cout << "temporary vector" << std::endl;
//std::cout << minRatioVector << std::endl;
const int minIndexOfTargetFunctionCoefficient = getMinIndexOfMinRatioVector(minRatioVector);
const bool positiveMinRatioExists = doesPositiveMinRatioExist(minRatioVector);
if( !positiveMinRatioExists ) {
// solution is unbounded
return Result(Result::EnumSolverState::UNBOUNDEDSOLUTION);
}
const int pivotRowIndex = minIndexOfTargetFunctionCoefficient;
//std::cout << "pivotRowIndex " << pivotRowIndex << std::endl;
Type pivotElement = matrix(pivotRowIndex, pivotColumnIndex);
// divide the pivot row with the pivot element
matrix.block(pivotRowIndex,0, 1, matrix.cols()) /= pivotElement;
// TODO< assert that pivot elemnt is roughtly 1.0 >
// do pivot operation
for(int pivotRowIteration = 0; pivotRowIteration < matrix.rows(); pivotRowIteration++ ) {
if( pivotRowIteration == pivotRowIndex ) {
continue;
}
Type iterationElementAtPivotColumn = matrix(pivotRowIteration, pivotColumnIndex);
matrix.block(pivotRowIteration, 0, 1, matrix.cols()) -= (matrix.block(pivotRowIndex, 0, 1, matrix.cols()) * iterationElementAtPivotColumn);
}
// set the variable identifier that we know which row of the matrix is for which variable
variables[pivotRowIndex] = Variable(Variable::EnumType::NAMEME, pivotColumnIndex);
//std::cout << matrix << std::endl;
}
}
Eigen::Matrix<double, 4, Eigen::Dynamic> bb_cluster_confidence(Eigen::Matrix<
double, 4, 20> const & iBB, Eigen::Matrix<double, 20, 1> const & iConf,
int nD) {
double SPACE_THR = 0.5;
Eigen::VectorXd T;
Eigen::VectorXd Tbak;
unsigned int iBBcols = nD;
Eigen::MatrixXd bdist;
//Calculates the index of the bb that fits the best
switch (iBBcols) {
//0 cols, set indices to 1
case 0:
T = Eigen::VectorXd::Zero(1);
break;
//1 col, set index to 0;
case 1:
T.resize(1);
T(0) = 0;
break;
//2 cols, set indices to zero; if above treshhold to 1
case 2:
T = Eigen::VectorXd::Zero(2);
bdist = bb_distance(iBB);
if (bdist(0, 0) > SPACE_THR) {
T(1) = 1;
}
break;
//workaround for clustering.
default:
Eigen::Vector4d meanBB = iBB.rowwise().mean();
int maxIndex = 0;
double maxDist = 10;
for (int penis = 0; penis < nD; penis++) {
Eigen::MatrixXd bd = bb_distance(iBB.col(penis), meanBB);
//save the shortest distance
if (bd(0, 0) < maxDist) {
maxIndex = penis;
maxDist = bd(0, 0);
}
}
//set the indices to the index of the bounding box with the
//shortest distance
T = Eigen::VectorXd::Constant(iBBcols, maxIndex);
break;
}
Eigen::VectorXd idx_cluster;
idx_cluster.resize(0);
bool breaker;
//filter indices that occur twice
for (int p = 0; p < T.size(); p++) {
breaker = false;
for (int j = 0; j < idx_cluster.size(); j++)
if (idx_cluster(j) == T(p)) {
breaker = true;
break;
}
if (breaker)
continue;
Eigen::VectorXd unibak(idx_cluster.size());
unibak = idx_cluster;
idx_cluster.resize(unibak.size() + 1);
idx_cluster << unibak, T(p);
}
int num_clusters = idx_cluster.size();
Eigen::MatrixXd oBB = Eigen::MatrixXd::Constant(4, num_clusters,
std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd oConf = Eigen::MatrixXd::Constant(4, num_clusters,
std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd oSize = Eigen::MatrixXd::Constant(4, num_clusters,
std::numeric_limits<double>::quiet_NaN());
for (int k = 0; k < num_clusters; k++) {
std::vector<int> idx;
for (int u = 0; u < T.size(); u++)
if (T(u) == idx_cluster(k))
idx.push_back(u);
Eigen::MatrixXd iBBidx(4, idx.size());
for (unsigned int f = 0; f < idx.size(); f++) {
iBBidx.col(f) = iBB.block(0, idx[f], 4, 1);
}
oBB.col(k) = iBBidx.rowwise().mean();
Eigen::VectorXd iConfidx(idx.size());
for (unsigned int f = 0; f < idx.size(); f++)
iConfidx(f) = iConf(idx[f]);
//save information how valid a certain bounding box is
oConf(0, k) = iConfidx.mean();
oSize(0, k) = idx.size();
}
Eigen::MatrixXd ret(4, 3 * num_clusters);
ret << oBB, oConf, oSize;
return ret;
}
int main()
{
float width = 640.0f, height=480.0f;
char buf[255];
int n, pose;
AsfMatrix data;
Eigen::Matrix<float,40*6,-1> Shapes; //240x116
float mean_size = 0;
for(n=0;n<40;n++)
{
for(pose=0;pose<6;pose++)
{
sprintf(buf,"../../../CIL/data/imm_face_db/%.2d-%dm.asf",n+1,pose+1);
if(readAsf2Eigen(buf, data) != 0)
{
sprintf(buf,"../../../CIL/data/imm_face_db/%.2d-%df.asf",n+1,pose+1);
if (readAsf2Eigen(buf, data) != 0)
continue;
}
//Initialize The Shapes Container
if(Shapes.cols() == 0)
Shapes.resize(40*6,data.cols()*2);
//Copy the found data
Shapes.block(n*6+pose,0,1,data.cols()) = data.row(2) * width;
Shapes.block(n*6+pose,data.cols(),1,data.cols()) = data.row(3) * height;
//Compute MeanShape
auto mean_x = Shapes.block(n*6+pose,0,1,data.cols()).mean();
auto mean_y = Shapes.block(n*6+pose,data.cols(),1,data.cols()).mean();
auto mshape_x = (Shapes.block(n*6+pose,0,1,data.cols()).array()-mean_x).pow(2) ;
auto mshape_y = (Shapes.block(n*6+pose,data.cols(),1,data.cols()).array()-mean_y).pow(2) ;
mean_size += sqrt((mshape_x+mshape_y).sum());
//std::cout << Shapes.block(n*pose+pose,0,1,data.cols()) << std::endl;
// std::cout << Shapes.block(0,0,1,5) << std::endl ;
//std::cout << Shapes.block(1,0,1,5) << std::endl ;
//std::cout << Shapes.block(2,0,1,5) << std::endl ;
//std::cout << Shapes.block(3,0,1,5) << std::endl << std::endl;
}
}
mean_size /= 40*6;
int number_of_landmarks = data.cols();
int number_of_shapes = Shapes.rows();
//Complex notation and Substracting Mean.
Eigen::MatrixXcf X(number_of_shapes, number_of_landmarks);
X.real() = Shapes.leftCols(number_of_landmarks);
X.imag() = Shapes.rightCols(number_of_landmarks);
X.array().colwise() -= X.rowwise().mean().array();
//Eigen::MatrixXcd XX(10,10);
//double test[10] = {0};
//Eigen::Map<Eigen::MatrixXd> mat(test, 10, 1);
Eigen::MatrixXcf C;
Eigen::MatrixXcf Mean;
cil::alg::gpa(X,C,Mean);
std::cout << X.rows() << " , " << X.cols() << std::endl<< std::endl;
std::cout << C.rows() << " , " << C.cols() << std::endl<< std::endl;
std::cout << Mean.rows() << " , " << Mean.cols() << std::endl<< std::endl;
std::cout << C.row(1).transpose() << std::endl<< std::endl;
return 0;
X.array().colwise() -= X.rowwise().mean().array();
//Eigen::MatrixXcf X = Shapes.block(0,0,Shapes.rows(),data.cols()) * std::complex<float>(0,1) +
// Shapes.block(0,data.cols(),Shapes.rows(),data.cols())*std::complex<float>(1,0);
//Eigen::VectorXcf Mean = X.rowwise().mean();
//std::complex<float> *m_ptr = Mean.data();
//for(n=0;n<Mean.rows();++n)
// X.row(n) = X.row(n).array() - *m_ptr++;
//Solve Eigen Problem
Eigen::MatrixXcf A = X.transpose().conjugate() * X;
Eigen::ComplexEigenSolver<Eigen::MatrixXcf> solver;
solver.compute(A);
// std::cout << "The Eigenvales of A are:" << std::endl << solver.eigenvalues() <<std::endl<<std::endl;
// std::complex<float> lambda = solver.eigenvalues()[57];
// std::cout << "Consider the first eigenvalue, lambda = " << lambda << std::endl;
// std::cout << "EigenVec for largest EigenVals of A are:" << std::endl << solver.eigenvectors().col(57) <<std::endl<<std::endl;
auto eigvec_mean = solver.eigenvectors().col(solver.eigenvectors().cols()-1);
// Full Procrusters fits
Eigen::MatrixXcf f = (X * eigvec_mean).array() / (X * X.transpose().conjugate()).diagonal().array();
//Transform
auto f_conj = f.conjugate().array();
for(n=0;n<X.cols();++n)
X.col(n) = X.col(n).array() * f_conj;
auto mf = f.mean();
std::cout << mf << std::endl<< std::endl;
mf = mf / sqrt(mf.real()*mf.real()+mf.imag()*mf.imag());
std::cout << mf << std::endl<< std::endl;
auto m = eigvec_mean * mf;
X = X*mf;
std::cout << X.row(0).transpose() << std::endl<< std::endl;
return 0;
}
//! Function to numerical compute the partial derivative of an acceleration w.r.t. a vector parameter
Eigen::Matrix< double, 3, Eigen::Dynamic > calculateAccelerationWrtParameterPartials(
boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter,
boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel,
Eigen::VectorXd parameterPerturbation,
boost::function< void( ) > updateDependentVariables,
const double currentTime,
boost::function< void( const double ) > timeDependentUpdateDependentVariables )
{
// Store uperturbed value.
Eigen::VectorXd unperturbedParameterValue = parameter->getParameterValue( );
if( unperturbedParameterValue.size( ) != parameterPerturbation.size( ) )
{
throw std::runtime_error( "Error when calculating numerical parameter partial of acceleration, parameter and perturbations are not the same size" );
}
Eigen::Matrix< double, 3, Eigen::Dynamic > partialMatrix = Eigen::MatrixXd::Zero( 3, unperturbedParameterValue.size( ) );
Eigen::VectorXd perturbedParameterValue;
for( int i = 0; i < unperturbedParameterValue.size( ); i++ )
{
perturbedParameterValue = unperturbedParameterValue;
perturbedParameterValue( i ) += parameterPerturbation( i );
// Calculate up-perturbation
parameter->setParameterValue( perturbedParameterValue );
updateDependentVariables( );
timeDependentUpdateDependentVariables( currentTime );
Eigen::Vector3d upPerturbedAcceleration = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
accelerationModel, currentTime );
accelerationModel->resetTime( TUDAT_NAN );
// Calculate down-perturbation.
perturbedParameterValue = unperturbedParameterValue;
perturbedParameterValue( i ) -= parameterPerturbation( i );
parameter->setParameterValue( perturbedParameterValue );
updateDependentVariables( );
timeDependentUpdateDependentVariables( currentTime );
Eigen::Vector3d downPerturbedAcceleration = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
accelerationModel, currentTime );
accelerationModel->resetTime( TUDAT_NAN );
// Compute partial entry.
partialMatrix.block( 0, i, 3, 1 ) =
( upPerturbedAcceleration - downPerturbedAcceleration ) / ( 2.0 * parameterPerturbation( i ) );
}
// Reset to original value.
parameter->setParameterValue(
unperturbedParameterValue ) ;
updateDependentVariables( );
timeDependentUpdateDependentVariables( currentTime );
accelerationModel->resetTime( TUDAT_NAN );
basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
accelerationModel, currentTime );
// Calculate partial using central difference.
return partialMatrix;
}
