Eigen::MatrixXd cIKSolver::BuildConsThetaWorldJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypeThetaWorld);
int num_joints = static_cast<int>(joint_mat.rows());
int joint_id = static_cast<int>(cons_desc(eConsDescParam0));
int num_dof = cKinTree::GetNumDof(joint_mat);
int cons_dim = GetConsDim(cons_desc);
Eigen::MatrixXd J = Eigen::MatrixXd(cons_dim, num_dof);
J.setZero();
int curr_id = joint_id;
while (true)
{
J(0, gPosDims + curr_id) = 1;
int curr_parent_id = cKinTree::GetParent(joint_mat, curr_id);
if (curr_parent_id == cKinTree::gInvalidJointID)
{
break;
}
curr_id = curr_parent_id;
}
return J;
}
void Model::construct_feedback_matrix(Eigen::MatrixXd &S,
const Eigen::MatrixXd &M,
const Parameter ¶meter) {
S.setZero();
Eigen::Matrix2d T;
T.setConstant(1.0 / sqrt(2));
T(0, 0) = -1 * T(0, 0);
Eigen::DiagonalMatrix<double, 2> W(1, parameter.wgt);
Eigen::MatrixXd DV(n_dimensions, 1);
double s;
for (unsigned int i = 0; i < n_alternatives; i++) {
for (unsigned int j = 0; j < n_alternatives; j++) {
for (unsigned int k = 0; k < n_dimensions; k++) {
DV(k, 0) = M(i, k) - M(j, k);
}
DV = T * DV;
s = (DV.transpose() * W * DV)(0, 0);
S(i, j) = parameter.phi2 * exp(-1 * parameter.phi1 * s * s);
}
}
S = Eigen::MatrixXd::Identity(n_alternatives, n_alternatives) - S;
}
void filter(const Eigen::MatrixXd& x, Eigen::MatrixXd& y, const Eigen::MatrixXd& b, const Eigen::MatrixXd& a)
{
y.setZero();
for(int c = 0; c < x.rows(); c++)
{
for(int t = 0; t < x.cols(); t++)
{
const int maxPQ = std::max(b.rows(), a.rows());
for(int pq = 0; pq < maxPQ; pq++)
{
const double tSource = t - pq;
if(pq < b.rows())
{
if(tSource >= 0)
y(c, t) += b(pq) * x(c, tSource);
else
y(c, t) += b(pq) * x(c, -tSource);
}
if(pq > 0 && pq < a.rows())
{
if(tSource >= 0)
y(c, t) += a(pq) * x(c, tSource);
else
y(c, t) += a(pq) * x(c, -tSource);
}
}
y(c, t) /= a(0);
}
}
}
//------------------------------------------------------------------------------
void
LocalAssembler::assemble(Eigen::MatrixXd& A,
UFC& ufc,
const std::vector<double>& vertex_coordinates,
ufc::cell& ufc_cell,
const Cell& cell,
const MeshFunction<std::size_t>* cell_domains,
const MeshFunction<std::size_t>* exterior_facet_domains,
const MeshFunction<std::size_t>* interior_facet_domains)
{
// Clear tensor
A.setZero();
cell.get_cell_data(ufc_cell);
// Assemble contributions from cell integral
assemble_cell(A, ufc, vertex_coordinates, ufc_cell, cell, cell_domains);
// Assemble contributions from facet integrals
for (FacetIterator facet(cell); !facet.end(); ++facet)
{
ufc_cell.local_facet = facet.pos();
if (facet->num_entities(cell.dim()) == 2)
{
assemble_interior_facet(A, ufc, vertex_coordinates, ufc_cell, cell,
*facet, facet.pos(), interior_facet_domains);
}
else
{
assemble_exterior_facet(A, ufc, vertex_coordinates, ufc_cell, cell,
*facet, facet.pos(), exterior_facet_domains);
}
}
}
void At_mul_A(Eigen::MatrixXd & out, SparseDoubleFeat & A) {
if (out.cols() != A.cols()) {
throw std::runtime_error("At_mul_A(SparseDoubleFeat): out.cols() must equal A.cols()");
}
if (out.cols() != out.rows()) {
throw std::runtime_error("At_mul_A(SparseDoubleFeat): out must be square matrix.)");
}
out.setZero();
const int nfeat = A.M.ncol;
#pragma omp parallel for schedule(dynamic, 8)
for (int f1 = 0; f1 < nfeat; f1++) {
// looping over all non-zero rows of f1
for (int i = A.Mt.row_ptr[f1], end = A.Mt.row_ptr[f1 + 1]; i < end; i++) {
int Mrow = A.Mt.cols[i]; /* row in M */
double val1 = A.Mt.vals[i]; /* value for Mrow */
for (int j = A.M.row_ptr[Mrow], end2 = A.M.row_ptr[Mrow + 1]; j < end2; j++) {
int f2 = A.M.cols[j];
if (f1 <= f2) {
out(f2, f1) += A.M.vals[j] * val1;
}
}
}
}
}
void LiftingLine::solve_for_best_gamma(double cL)
{
int matsize = this->segments.size() + 1;
Eigen::MatrixXd matrix;
Eigen::VectorXd rhs;
Eigen::VectorXd result;
matrix.resize(matsize, matsize);
matrix.setZero();
rhs.resize(matsize);
rhs.setZero();
result.resize(matsize);
result.setZero();
// adding the main min-function
for (int i = 0; i < (matsize - 1); i++)
{
for (int j = 0; j < (matsize - 1); j++)
{
matrix(i, j) += this->segments[i].b() * this->segments[j].ind_influence(this->segments[i]);
matrix(i, j) += this->segments[j].b() * this->segments[i].ind_influence(this->segments[j]);
}
// adding lagrange multiplicator
matrix(i, matsize - 1) += this->segments[i].lift_factor;
}
for (int i = 0; i < (matsize -1); i++)
{
matrix(matsize - 1, i) += this->segments[i].lift_factor;
}
rhs(matsize - 1) += cL;
result = matrix.fullPivHouseholderQr().solve(rhs);
for (int i = 0; i < matsize - 1; i++)
{
this->segments[i].best_gamma = result[i];
}
}
Eigen::MatrixXd cIKSolver::BuildConsPosJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypePos);
int num_joints = static_cast<int>(joint_mat.rows());
int parent_id = static_cast<int>(cons_desc(eConsDescParam0));
tVector attach_pt = tVector(cons_desc(eConsDescParam1), cons_desc(eConsDescParam2), 0.f, 0.f);
tVector end_pos = cKinTree::LocalToWorldPos(joint_mat, pose, parent_id, attach_pt);
const Eigen::Vector3d rot_axis = Eigen::Vector3d(0, 0, 1);
int num_dof = cKinTree::GetNumDof(joint_mat);
Eigen::MatrixXd J = Eigen::MatrixXd(gPosDims, num_dof);
J.setZero();
for (int i = 0; i < gPosDims; ++i)
{
J(i, i) = 1;
}
int curr_id = parent_id;
while (true)
{
tVector joint_pos = cKinTree::CalcJointWorldPos(joint_mat, pose, curr_id);
tVector delta = end_pos - joint_pos;
Eigen::Vector3d tangent = rot_axis.cross(Eigen::Vector3d(delta(0), delta(1), delta(2)));
int curr_parent_id = cKinTree::GetParent(joint_mat, curr_id);
for (int i = 0; i < gPosDims; ++i)
{
J(i, gPosDims + curr_id) = tangent(i);
}
if (curr_parent_id == cKinTree::gInvalidJointID)
{
// no scaling for root
break;
}
else
{
#if !defined(DISABLE_LINK_SCALE)
double attach_x = joint_desc(curr_id, cKinTree::eJointDescAttachX);
double attach_y = joint_desc(curr_id, cKinTree::eJointDescAttachY);
double attach_z = joint_desc(curr_id, cKinTree::eJointDescAttachZ);
double parent_world_theta = cKinTree::CalcJointWorldTheta(joint_desc, curr_parent_id);
double world_attach_x = std::cos(parent_world_theta) * attach_x - std::sin(parent_world_theta) * attach_y;
double world_attach_y = std::sin(parent_world_theta) * attach_x + std::cos(parent_world_theta) * attach_y;
double world_attach_z = attach_z; // hack ignoring z, do this properly
J(0, gPosDims + num_joints + curr_id) = world_attach_x;
J(1, gPosDims + num_joints + curr_id) = world_attach_y;
#endif
curr_id = curr_parent_id;
}
}
return J;
}
void SubSystem::calcJacobi(VEC_pD ¶ms, Eigen::MatrixXd &jacobi)
{
jacobi.setZero(csize, params.size());
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator
pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end())
for (int i=0; i < csize; i++)
jacobi(i,j) = clist[i]->grad(pmapfind->second);
}
}
void Model::construct_contrast_matrix(Eigen::MatrixXd &C) {
if (n_alternatives > 1) {
C.setConstant(-1.0 / (n_alternatives - 1.0));
} else {
C.setZero();
}
for (unsigned int i = 0; i < n_alternatives; i++) {
C(i, i) = 1.0;
}
}
void CompressionMatrixFactory::createCompressionMatrix(Eigen::MatrixXd& cm)
{
if(cm.rows() < paramDim || cm.cols() < inputDim)
cm.resize(paramDim, inputDim);
if(compress)
{
cm.setZero();
fillCompressionMatrix(cm);
}
else
cm = Eigen::MatrixXd::Identity(cm.rows(), cm.cols());
}
segment_info(unsigned int nc) {
E.resize(6, nc);
E_tilde.resize(6, nc);
G.resize(nc);
M.resize(nc, nc);
EZ.resize(nc);
E.setZero();
E_tilde.setZero();
M.setZero();
G.setZero();
EZ.setZero();
};
void dynamicsRegressorLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
// Store the base_world translational transform in world orientation
KDL::Frame world_base_X_world_world = KDL::Frame(-(X_b[traversal.getBaseLink()->getLinkIndex()].p));
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//Base dynamics
// The base dynamics is expressed with the orientation of the world but
// with respect to the base origin
dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(world_base_X_world_world*X_b[i])*netWrenchRegressor_i;
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
#ifndef NDEBUG
//std::cerr << "Calculating regressor columns for link " << link->getName() << " and joint " << child_link->getAdjacentJoint(parent_link)->getName() << std::endl;
#endif
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(6+dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
#ifndef NDEBUG
//std::cout << "Getting parent link of link of index " << child_link->getName() << " " << child_link->getLinkIndex() << std::endl;
//std::cout << "Current base " << traversal.order[0]->getName() << " " << traversal.order[0]->getLinkIndex() << std::endl;
#endif
parent_link = traversal.getParentLink(child_link);
}
}
}
void ALLModel::calculateEuclDistanceMatrix(Eigen::MatrixXd& m1, Eigen::MatrixXd& m2, Eigen::MatrixXd& output)
{
output.resize(m1.rows(), m2.rows());
output.setZero();
Statistics n;
for (int i = 0; i < m1.rows(); i++)
{
for (int j = 0; j < m2.rows(); j++)
{
//get euclidean distances of the two current rows
output(i, j) = n.euclDistance(m1, m2, i, j);
}
}
}
segment_info(unsigned int nc):
D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
{
E.resize(6, nc);
E_tilde.resize(6, nc);
G.resize(nc);
M.resize(nc, nc);
EZ.resize(nc);
E.setZero();
E_tilde.setZero();
M.setZero();
G.setZero();
EZ.setZero();
};
void Model::construct_L_matrix(Eigen::MatrixXd &L, const unsigned int i) {
L.setZero();
for (unsigned int row = 0; row < n_alternatives - 1; row++) {
for (unsigned int col = 0; col < n_alternatives; col++) {
if (col == i) {
L(row, col) = 1;
} else if ((col < i) && (row == col)) {
L(row, col) = -1;
} else if ((col > i) && (row == col - 1)) {
L(row, col) = -1;
}
}
}
}
Eigen::MatrixXd cIKSolver::BuildConsThetaJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypeTheta);
int num_joints = static_cast<int>(joint_mat.rows());
int joint_id = static_cast<int>(cons_desc(eConsDescParam0));
int num_dof = cKinTree::GetNumDof(joint_mat);
int cons_dim = GetConsDim(cons_desc);
Eigen::MatrixXd J = Eigen::MatrixXd(cons_dim, num_dof);
J.setZero();
J(0, gPosDims + joint_id) = 1;
return J;
}
void drawConstraints(igl::viewer::Viewer &viewer)
{
for (int n=0; n<2; ++n)
{
Eigen::MatrixXd Bc = igl::slice(B, b, 1);
Eigen::MatrixXd color;
color.setZero(b.rows(),3);
color.col(2).setOnes();
for (int i =0; i<b.rows(); ++i)
if (blevel[i] ==1 && n>0)
color.row(i)<<0.7,0.7,0.7;
// Eigen::RowVector3d color; color<<0.5,0.5,0.5;
viewer.data.add_edges(Bc - global_scale*bc.block(0,n*3,bc.rows(),3), Bc + global_scale*bc.block(0,n*3,bc.rows(),3) , color);
}
}
void LoadEstimator::getDotMatrix(const Eigen::Vector3d& input_vector, Eigen::MatrixXd& dotted_matrix)
{
dotted_matrix.setZero(3, 6);
dotted_matrix(0,0) = input_vector(0);
dotted_matrix(0,1) = input_vector(1);
dotted_matrix(0,2) = input_vector(2);
dotted_matrix(1,1) = input_vector(0);
dotted_matrix(1,3) = input_vector(1);
dotted_matrix(1,4) = input_vector(2);
dotted_matrix(2,2) = input_vector(0);
dotted_matrix(2,4) = input_vector(1);
dotted_matrix(2,5) = input_vector(2);
}
void ALLModel::calculateXY(Eigen::VectorXd& w, Eigen::MatrixXd& res)
{
res.resize(descriptor_matrix_.cols(), Y_.cols());
res.setZero();
for (int i = 0; i < descriptor_matrix_.cols(); i++)
{
for (int j = 0; j < Y_.cols(); j++)
{
for (int s = 0; s < descriptor_matrix_.rows(); s++)
{
res(i, j) += w(s)*descriptor_matrix_(s, i)*Y_(s, j);
}
}
}
}
void ALLModel::calculateXX(Eigen::VectorXd& w, Eigen::MatrixXd& res)
{
res.resize(descriptor_matrix_.cols(), descriptor_matrix_.cols());
res.setZero();
// for all descriptors, calculate their weighted cross-product
for (int i = 0; i < descriptor_matrix_.cols(); i++)
{
for (int j = i; j < descriptor_matrix_.cols(); j++)
{
for (int s = 0; s < descriptor_matrix_.rows(); s++)
{
res(i, j) += w(s)*descriptor_matrix_(s, i)*descriptor_matrix_(s, j);
}
res(j, i) = res(i, j);
}
}
}
// TODO: eigen ref????
void PoseTask::getJacobian(const Eigen::VectorXd& q,
Eigen::MatrixXd& Jacobian)
{
// TODO: Check that the Jacobian is initially set to zero
if (taskType_ == 0){
//Jacobian.setZero();
Jacobian.resize(3,ndof_);
Jacobian.setZero(); // VERY IMPORTANT FOR THE JACOBIAN!!!
//CalcPointJacobian(*model_, q, linkNum_, localpos_, Jacobian, false);
CalcPointJacobian(*model_, q, linkNum_, localpos_, Jacobian, true);
// std::cout << "qdes" << q.transpose() << std::endl;
// std::cout << "rwrist: " << linkNum_ << std::endl;
// std::cout << "localpos: " << localpos_.transpose() << std::endl;
}
else if (taskType_ == 1)
std::cout << "TODO" << std::endl;
else if (taskType_ == 2)
std::cout << "TODO" << std::endl;
// TODO: Use CalcBodySpatialJacobian for the orientation
}
Eigen::MatrixXd ClassifyMotion::convert_to_matrix( const std::vector< std::vector< std::string > >& matrix )
{
Eigen::MatrixXd result;
if(matrix.empty())
return result;
if(matrix[0].empty())
return result;
result.setZero( matrix.size(), matrix[0].size() );
for( int i=0; i<int(matrix.size()); i++ )
{
for( int j=0; j<int(matrix[i].size()); j++ )
{
std::istringstream convert( matrix[i][j] );
convert >> result(i,j);
}
}
return result;
}
int baseDynamicsRegressor::computeRegressor(const KDL::JntArray &q,
const KDL::JntArray &q_dot,
const KDL::JntArray &q_dotdot,
const std::vector<KDL::Frame> & X_dynamic_base,
const std::vector<KDL::Twist> & v,
const std::vector<KDL::Twist> & a,
const std::vector< KDL::Wrench > & measured_wrenches,
const KDL::JntArray & measured_torques,
Eigen::MatrixXd & regressor_matrix,
Eigen::VectorXd & known_terms)
{
#ifndef NDEBUG
//std::cerr << "Called computeRegressor " << std::endl;
//std::cerr << (*p_ft_list).toString();
#endif
const KDL::CoDyCo::UndirectedTree & undirected_tree = *p_undirected_tree;
if( regressor_matrix.rows() != getNrOfOutputs() ) {
return -1;
}
//all other columns, beside the one relative to the inertial parameters of the links of the subtree, are zero
regressor_matrix.setZero();
for(int link_id =0; link_id < (int)undirected_tree.getNrOfLinks(); link_id++ ) {
if( linkIndeces2regrCols[link_id] != -1 ) {
Eigen::Matrix<double,6,10> netWrenchRegressor_i = netWrenchRegressor(v[link_id],a[link_id]);
regressor_matrix.block(0,(int)(10*linkIndeces2regrCols[link_id]),getNrOfOutputs(),10) = WrenchTransformationMatrix(X_dynamic_base[link_id])*netWrenchRegressor_i;
}
}
return 0;
}
void dynamicsRegressorFixedBaseLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
parent_link = traversal.getParentLink(child_link);
}
}
}
int TreeInertialParameters::dynamicsRegressor( const JntArray &q,
const JntArray &q_dot,
const JntArray &q_dotdot,
const Twist& base_velocity,
const Twist& base_acceleration,
Eigen::MatrixXd & dynamics_regressor)
{
if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || dynamics_regressor.cols()!=10*ns || dynamics_regressor.rows()!=(6+nj))
return -1;
unsigned int i;
unsigned int j=0;
//Kinematic propagation copied by RNE
for(unsigned int l = 0; l < recursion_order.size(); l++ ) {
unsigned int curr_index = recursion_order[l];
const Segment& seg = seg_vector[curr_index]->second.segment;
const Joint& jnt = seg.getJoint();
double q_,qdot_,qdotdot_;
if(jnt.getType() !=Joint::None){
int idx = link2joint[curr_index];
q_=q(idx);
qdot_=q_dot(idx);
qdotdot_=q_dotdot(idx);
}else
q_=qdot_=qdotdot_=0.0;
Frame& eX = X[curr_index];
Twist& eS = S[curr_index];
Twist& ev = v[curr_index];
Twist& ea = a[curr_index];
Wrench& ef = f[curr_index];
Frame& eX_b = X_b[curr_index];
assert(X.size() == ns);
//Calculate segment properties: X,S,vj,cj
X[curr_index] = seg.pose(q_);
//eX=seg.pose(q_);
//Remark this is the inverse of the
//frame for transformations from
//the parent to the current coord frame
//Transform velocity and unit velocity to segment frame
Twist vj=eX.M.Inverse(seg.twist(q_,qdot_));
eS=eX.M.Inverse(seg.twist(q_,1.0));
//We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
//calculate velocity and acceleration of the segment (in segment coordinates)
int parent_index = lambda[curr_index];
if( parent_index == -1 ) {
eX_b = eX;
} else {
eX_b = X_b[parent_index]*eX;
}
Twist parent_a, parent_v;
if( parent_index == -1 ) {
parent_a = base_acceleration;
parent_v = base_velocity;
} else {
parent_a = a[parent_index];
parent_v = v[parent_index];
}
ev=eX.Inverse(parent_v)+vj;
ea=eX.Inverse(parent_a)+eS*qdotdot_+ev*vj;
}
//end kinematic phase
dynamics_regressor.setZero();
//just for design, then the loop can be put together
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
for(i=0;i<ns;i++) {
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
Frame X_j_i;
for(j=0;j<ns;j++) {
X_j_i = X_b[j].Inverse()*X_b[i];
if( seg_vector[j]->second.segment.getJoint().getType() != Joint::None ) {
if( indicator_function[i][link2joint[j]] ) {
dynamics_regressor.block(6+link2joint[j],10*i,1,10) =
toEigen(S[j]).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
}
}
}
return 0;
}
void MaxPosterior::DE_eachIter(IM im, popTree* poptree, Chain coldCh, unsigned int nProcs, unsigned int crr_procID)
{
Eigen::MatrixXd newPara;
newPara.resize(1,nPara);
for(unsigned int i=0; i<nParaVectors; i++)
{
unsigned int recombinationID = runiform_discrete(nPara);
newPara.setZero();
double newPara_each = 0.0;
// select base vectors
unsigned int b1=i;
unsigned int b2=i;
unsigned int b3=i;
if(crr_procID == 0)
{
while(b1==i)
b1 = runiform_discrete(nParaVectors);
while(b2 ==i || b2 == b1)
b2 = runiform_discrete(nParaVectors);
while(b3==i || b3== b2|| b3== b1)
b3 = runiform_discrete(nParaVectors);
}
for(unsigned int j=0; j<nPara; j++)
{
if(crr_procID == 0)
{
newPara_each = para_atPrev(i,j);
if(runiform() < CR || j== recombinationID)
{
newPara_each = para_atPrev(b1,j)+F*(para_atPrev(b2,j)-para_atPrev(b3,j)); // differential mutation
while(newPara_each <0 || newPara_each >priorsMax.at(j))
{
if(newPara_each <0)
newPara_each = para_atPrev(b1,j)-runiform()*(para_atPrev(b1,j));
if(newPara_each > priorsMax.at(j))
newPara_each = para_atPrev(b1,j)+runiform()*(priorsMax.at(j)-para_atPrev(b1,j));
}
if(newPara_each <0 || newPara_each > priorsMax.at(j))
{
std::cout << "\n*** Error *** in map::DE_eachIter()\n";
}
}
}
MPI::COMM_WORLD.Barrier();
MPI::COMM_WORLD.Bcast(&newPara_each, 1, MPI_DOUBLE, 0);
MPI::COMM_WORLD.Barrier();
newPara(0,j) = newPara_each;
} //END of for(unsigned int j=0; j<nPara; j++)
Eigen::MatrixXd paraVector(1,6);
unsigned int nPara_popSizes =1;
if(poptree->get_age() ==0)// single population
{
for(unsigned int ii=0; ii<6; ii++)
{
if(ii==2)
paraVector(0,ii) = newPara(0,0);
else
paraVector(0,ii) = 0;
}
}
else
{
//-- sampling populations --//
paraVector(0,0) = newPara(0,0);
if(im.get_samePopulationSizes() == 1)
paraVector(0,1) = newPara(0,0);
else
{
paraVector(0,1) = newPara(0,1);
nPara_popSizes++;
}
//-- ancestral population --//
if(im.get_ancPop() == 1)
{
if(im.get_samePopulationSizes() == 1)
paraVector(0,2) = newPara(0,0);
else
{
paraVector(0,2) = newPara(0,2);
nPara_popSizes++;
}
}
else // no ancestral population
paraVector(0,2) = 0;
//-- migration rates --//
paraVector(0,3) = newPara(0,nPara_popSizes);
if(im.get_sameMigrationRates() == 1)
paraVector(0,4) = newPara(0,nPara_popSizes);
else
paraVector(0,4) = newPara(0,nPara_popSizes+1);
//-- splitting time --//
if(im.get_ancPop() == 1)
paraVector(0,5) = newPara(0,nPara-1);
else // no ancestral population
paraVector(0,5) =im.get_splittingTimeMax();
}
long double logPosterior_newPara = 0;
unsigned int lociInParallel = im.get_lociInParallel();
if(lociInParallel ==1)
{
if(coldCh.get_multiLocusSpecific_mutationRate() ==1) // variable mutation rate scalars
{
logPosterior_newPara = computeLogJointDensity_mutationScalars_MPI_overSubLoci(paraVector, im, poptree, coldCh, nProcs, crr_procID);
}
else // constant mutation rate scalars
{
logPosterior_newPara = computeLogJointDensity_MPI_overSubLoci(paraVector, im, poptree, coldCh, nProcs, crr_procID);
}
}
else if(lociInParallel ==0)
{
logPosterior_newPara = (long double) log(computeJointDensity_MPI_overSubSample(paraVector, im, poptree, coldCh, nProcs, crr_procID));
}
else
{
std::cout << "lociInParallel should be 1 or 0, but lociInParallel = " << lociInParallel <<"\n";
}
MPI::COMM_WORLD.Barrier();
// YC 2/27/2014
// All the processes share the same posterior densities and parameter values.
if(logPosterior_newPara > posterior_atPrev.at(i))
{
para_atCrr.row(i) = newPara;
posterior_atCrr.at(i) = logPosterior_newPara;
}
else if(isinf(logPosterior_newPara) && isinf(posterior_atPrev.at(i)) )
{
// YC 08/07/2018
// If the current and new posteriors are negative infinity, the new parameter values are taken.
para_atCrr.row(i) = newPara;
posterior_atCrr.at(i) = logPosterior_newPara;
}
else
{
para_atCrr.row(i) = para_atPrev.row(i);
posterior_atCrr.at(i) = posterior_atPrev.at(i);
}
/*
if(crr_procID == 0)
{
marginals.add(newPara, posterior_newPara);
}
*/
}
MPI::COMM_WORLD.Barrier();
para_atPrev = para_atCrr;
posterior_atPrev = posterior_atCrr;
if(crr_procID == 0)
{
std::vector<long double>::const_iterator iter_max, iter_min;
iter_max = max_element(posterior_atCrr.begin(), posterior_atCrr.end());
iter_min = min_element(posterior_atCrr.begin(), posterior_atCrr.end());
logPosteriorMax = *iter_max;
logPosteriorMin = *iter_min;
/*
unsigned int found_max = 0;
ID_max = 0;
while(ID_max < nParaVectors && found_max==0 )
{
if(*iter_max == posterior_atCrr.at(ID_max))
found_max =1;
else
ID_max++;
}
*/
// std::cout <<"logPosteriorMax = " << logPosteriorMax <<" logPosteriorMin = " << logPosteriorMin <<"\n";
}
MPI::COMM_WORLD.Barrier();
MPI::COMM_WORLD.Bcast(&logPosteriorMax, 1, MPI_LONG_DOUBLE, 0);
MPI::COMM_WORLD.Barrier();
MPI::COMM_WORLD.Bcast(&logPosteriorMin, 1, MPI_LONG_DOUBLE, 0);
MPI::COMM_WORLD.Barrier();
return;
}
void CameraDirectLinearTransformation::init(const std::vector<Vector3d> &x, const stlalignedvector4d &X, bool decomposeProjectionMatrix, bool computeOpenGLMatrices, double x0, double y0, int width, int height, double znear, double zfar)
{
this->points2D = x;
this->points3D = X;
if (x.size() != X.size() )
throw std::logic_error("There must be the same number of correspondencies");
// Reset all the matrices used in the class
this->K.setZero();
this->P.setZero();
this->R.setZero();
this->t.setZero();
this->C.setZero();
this->gl_ModelView_Matrix.setIdentity();
this->gl_Projection_Matrix.matrix().setZero();
this->gl_ModelViewProjection_Matrix.matrix().setZero();
this->viewport << 0,0,width,height;
unsigned int n=x.size();
Eigen::MatrixXd A;
A.setZero(2*n,12);
for ( unsigned int i=0; i<n; i++ )
{
Vector3d m = x.at(i);
RowVector4d M = X.at(i).transpose();
A.row(2*i) << 0,0,0,0, -m[2]*M, m[1]*M;
A.row(2*i+1) << m[2]*M,0,0,0,0, -m[0]*M;
}
// http://my.safaribooksonline.com/book/-/9781449341916/4dot4-augmented-reality/id2706803
JacobiSVD<MatrixXd> svd(A, ComputeFullV );
// Copy the data in the last column of matrix V (the eigenvector with the smallest eigenvalue of A^T*A)
// maintaining the correct order
int k=0;
for (int i=0; i<3;i++)
{
for (int j=0; j<4; j++)
{
double t = svd.matrixV().col(11).coeffRef(k);
P(i,j)= t;
++k;
}
}
cerr << "[INFO] Reproduction error" << this->getReprojectionError(P,x,X) << endl;
if (decomposeProjectionMatrix)
{
this->decomposePMatrix(this->P);
this->DecompositionComputed=true;
if (computeOpenGLMatrices)
{
this->computeOpenGLProjectionMatrix(x0,y0,width,height,znear,zfar);
this->computeOpenGLModelViewMatrix(this->R, this->t);
this->gl_ModelViewProjection_Matrix = gl_Projection_Matrix* gl_ModelView_Matrix;
this->ModelViewProjectionInitialized=true;
}
// Compute the projection error
cerr << "[INFO] OpenGL Reproduction error=" << getReproductionErrorOpenGL(gl_Projection_Matrix,gl_ModelView_Matrix,viewport,points2D,points3D) << endl;
}
}
/*! \fn inline void symmetryBlocking(Eigen::MatrixXd & matrix, int cavitySize, int ntsirr, int nr_irrep)
* \param[out] matrix the matrix to be block-diagonalized
* \param[in] cavitySize the size of the cavity (size of the matrix)
* \param[in] ntsirr the size of the irreducible portion of the cavity (size of the blocks)
* \param[in] nr_irrep the number of irreducible representations (number of blocks)
*/
inline void symmetryBlocking(Eigen::MatrixXd & matrix, size_t cavitySize, int ntsirr,
int nr_irrep)
{
// This function implements the simmetry-blocking of the PCM
// matrix due to point group symmetry as reported in:
// L. Frediani, R. Cammi, C. S. Pomelli, J. Tomasi and K. Ruud, J. Comput.Chem. 25, 375 (2003)
// u is the character table for the group (t in the paper)
Eigen::MatrixXd u = Eigen::MatrixXd::Zero(nr_irrep, nr_irrep);
for (int i = 0; i < nr_irrep; ++i) {
for (int j = 0; j < nr_irrep; ++j) {
u(i, j) = parity(i&j);
}
}
// Naming of indices:
// a, b, c, d run over the total size of the cavity (N)
// i, j, k, l run over the number of irreps (n)
// p, q, r, s run over the irreducible size of the cavity (N/n)
// Instead of forming U (T in the paper) and then perform the dense
// multiplication, we multiply block-by-block using just the u matrix.
// matrix = U * matrix * Ut; U * Ut = Ut * U = id
// First half-transformation, i.e. first_half = matrix * Ut
Eigen::MatrixXd first_half = Eigen::MatrixXd::Zero(cavitySize, cavitySize);
for (int i = 0; i < nr_irrep; ++i) {
int ioff = i * ntsirr;
for (int k = 0; k < nr_irrep; ++k) {
int koff = k * ntsirr;
for (int j = 0; j < nr_irrep; ++j) {
int joff = j * ntsirr;
double ujk = u(j, k) / nr_irrep;
for (int p = 0; p < ntsirr; ++p) {
int a = ioff + p;
for (int q = 0; q < ntsirr; ++q) {
int b = joff + q;
int c = koff + q;
first_half(a, c) += matrix(a, b) * ujk;
}
}
}
}
}
// Second half-transformation, i.e. matrix = U * first_half
matrix.setZero(cavitySize, cavitySize);
for (int i = 0; i < nr_irrep; ++i) {
int ioff = i * ntsirr;
for (int k = 0; k < nr_irrep; ++k) {
int koff = k * ntsirr;
for (int j = 0; j < nr_irrep; ++j) {
int joff = j * ntsirr;
double uij = u(i, j);
for (int p = 0; p < ntsirr; ++p) {
int a = ioff + p;
int b = joff + p;
for (int q = 0; q < ntsirr; ++q) {
int c = koff + q;
matrix(a, c) += uij * first_half(b, c);
}
}
}
}
}
// Traverse the matrix and discard numerical zeros
for (size_t a = 0; a < cavitySize; ++a) {
for (size_t b = 0; b < cavitySize; ++b) {
if (numericalZero(matrix(a, b))) {
matrix(a, b) = 0.0;
}
}
}
}
//! Function to read a gravity field file
std::pair< double, double > readGravityFieldFile(
const std::string& fileName, const int maximumDegree, const int maximumOrder,
std::pair< Eigen::MatrixXd, Eigen::MatrixXd >& coefficients,
const int gravitationalParameterIndex, const int referenceRadiusIndex )
{
// Attempt to open gravity file.
std::fstream stream( fileName.c_str( ), std::ios::in );
if( stream.fail( ) )
{
boost::throw_exception(
std::runtime_error( "Pds gravity field data file could not be opened." ) );
}
// Declare variables for reading file.
std::vector< std::string > vectorOfIndividualStrings;
vectorOfIndividualStrings.resize( 4 );
std::string line;
double gravitationalParameter = TUDAT_NAN;
double referenceRadius = TUDAT_NAN;
if( ( gravitationalParameterIndex >= 0 ) &&
( referenceRadiusIndex >= 0 ) )
{
// Get first line of file.
std::getline( stream, line );
// Get reference radius and gravitational parameter from first line of file.
boost::algorithm::trim( line );
boost::algorithm::split( vectorOfIndividualStrings,
line,
boost::algorithm::is_any_of( "\t, " ),
boost::algorithm::token_compress_on );
if( gravitationalParameterIndex >= static_cast< int >( vectorOfIndividualStrings.size( ) ) ||
referenceRadiusIndex >= static_cast< int >( vectorOfIndividualStrings.size( ) ) )
{
throw std::runtime_error( "Error when reading gravity field file, requested header index exceeds file contents" );
}
gravitationalParameter = boost::lexical_cast< double >( vectorOfIndividualStrings[ gravitationalParameterIndex ] );
referenceRadius = boost::lexical_cast< double >( vectorOfIndividualStrings[ referenceRadiusIndex ] );
}
else if( ( !( gravitationalParameterIndex >= 0 ) &&
( referenceRadiusIndex >= 0 ) ) ||
( ( gravitationalParameterIndex >= 0 ) &&
!( referenceRadiusIndex >= 0 ) ) )
{
throw std::runtime_error( "Error when reading gravity field file, must retrieve either both or neither of Re and mu" );
}
// Declare variables for reading in cosine and sine coefficients.
int currentDegree = 0, currentOrder = 0;
Eigen::MatrixXd cosineCoefficients = Eigen::MatrixXd( maximumDegree + 1, maximumOrder + 1 );
cosineCoefficients.setZero( );
Eigen::MatrixXd sineCoefficients = Eigen::MatrixXd( maximumDegree + 1, maximumOrder + 1 );
sineCoefficients.setZero( );
// Read coefficients up to required maximum degree and order.
while ( !stream.fail( ) && !stream.eof( ) &&
( currentDegree <= maximumDegree || currentOrder <= maximumOrder ) )
{
// Read current line
std::getline( stream, line );
// Trim input string (removes all leading and trailing whitespaces).
boost::algorithm::trim( line );
// Split string into multiple strings, each containing one element from a line from the
// data file.
boost::algorithm::split( vectorOfIndividualStrings,
line,
boost::algorithm::is_any_of( ", " ),
boost::algorithm::token_compress_on );
// Check current line for consistency
if( vectorOfIndividualStrings.size( ) < 4 )
{
std::cerr<<"Error when reading pds gravity field file, number of fields is "
<<vectorOfIndividualStrings.size( )<<std::endl;
}
else
{
// Read current degree and orde from line.
currentDegree = boost::lexical_cast< int >( vectorOfIndividualStrings[ 0 ] );
currentOrder = boost::lexical_cast< int >( vectorOfIndividualStrings[ 1 ] );
// Set cosine and sine coefficients for current degree and order.
if( currentDegree <= maximumDegree && currentOrder <= maximumOrder )
{
cosineCoefficients( currentDegree, currentOrder ) =
boost::lexical_cast< double >( vectorOfIndividualStrings[ 2 ] );
sineCoefficients( currentDegree, currentOrder ) =
boost::lexical_cast< double >( vectorOfIndividualStrings[ 3 ] );
}
}
}
// Set cosine coefficient at (0,0) to 1.
cosineCoefficients( 0, 0 ) = 1.0;
coefficients = std::make_pair( cosineCoefficients, sineCoefficients );
return std::make_pair( gravitationalParameter, referenceRadius );
}
double
NDTMatcherFeatureD2D::derivativesNDT(
const std::vector<NDTCell*> &sourceNDT,
const NDTMap &targetNDT,
Eigen::MatrixXd &score_gradient,
Eigen::MatrixXd &Hessian,
bool computeHessian
)
{
struct timeval tv_start, tv_end;
double score_here = 0;
gettimeofday(&tv_start,NULL);
NUMBER_OF_ACTIVE_CELLS = 0;
score_gradient.setZero();
Hessian.setZero();
pcl::PointXYZ point;
Eigen::Vector3d transformed;
Eigen::Vector3d meanMoving, meanFixed;
Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
NDTCell *cell;
bool exists = false;
double det = 0;
for (unsigned int j = 0; j < _corr.size(); j++)
{
if (!_goodCorr[j])
continue;
unsigned int i = _corr[j].second;
if (i >= sourceNDT.size())
{
std::cout << "sourceNDT.size() : " << sourceNDT.size() << ", i: " << i << std::endl;
}
assert(i < sourceNDT.size());
meanMoving = sourceNDT[i]->getMean();
CMoving= sourceNDT[i]->getCov();
this->computeDerivatives(meanMoving, CMoving, computeHessian);
point.x = meanMoving(0);
point.y = meanMoving(1);
point.z = meanMoving(2);
cell = targetNDT.getCellIdx(_corr[j].first);
if(cell == NULL)
{
continue;
}
if(cell->hasGaussian_)
{
transformed = meanMoving - cell->getMean();
CFixed = cell->getCov();
CSum = (CFixed+CMoving);
CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
if(!exists)
{
//delete cell;
continue;
}
double l = (transformed).dot(Cinv*(transformed));
if(l*0 != 0)
{
//delete cell;
continue;
}
double sh = -lfd1*(exp(-lfd2*l/2));
//std::cout<<"m1 = ["<<meanMoving.transpose()<<"]';\n m2 = ["<<cell->getMean().transpose()<<"]';\n";
//std::cout<<"C1 = ["<<CMoving<<"];\n C2 = ["<<CFixed<<"];\n";
//update score gradient
if(!this->update_gradient_hessian(score_gradient,Hessian,transformed, Cinv, sh, computeHessian))
{
continue;
}
score_here += sh;
cell = NULL;
}
}
gettimeofday(&tv_end,NULL);
//double time_load = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
//std::cout<<"time derivatives took is: "<<time_load<<std::endl;
return score_here;
}
