void color_intersections(
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const Eigen::MatrixXd & U,
const Eigen::MatrixXi & G,
Eigen::MatrixXd & C,
Eigen::MatrixXd & D)
{
using namespace igl;
using namespace igl::cgal;
using namespace Eigen;
MatrixXi IF;
const bool first_only = false;
intersect_other(V,F,U,G,first_only,IF);
C.resize(F.rows(),3);
C.col(0).setConstant(0.4);
C.col(1).setConstant(0.8);
C.col(2).setConstant(0.3);
D.resize(G.rows(),3);
D.col(0).setConstant(0.4);
D.col(1).setConstant(0.3);
D.col(2).setConstant(0.8);
for(int f = 0;f<IF.rows();f++)
{
C.row(IF(f,0)) = RowVector3d(1,0.4,0.4);
D.row(IF(f,1)) = RowVector3d(0.8,0.7,0.3);
}
}
/**
* Generates an artificial topographyGrid of size numRows x numCols if no
* topographic data is available. Results are dumped into topographyGrid.
* @param topographyGrid A pointer to a zero-initialized Grid of size
* numRows x numCols.
* @param numRows The desired number of non-border rows in the resulting matrix.
* @param numCols The desired number of non-border cols in the resulting matrix.
*/
void simulatetopographyGrid(Grid* topographyGrid, int numRows, int numCols) {
Eigen::VectorXd refx = refx.LinSpaced(numCols, -2*M_PI, 2*M_PI);
Eigen::VectorXd refy = refx.LinSpaced(numRows, -2*M_PI, 2*M_PI);
Eigen::MatrixXd X = refx.replicate(1, numRows);
X.transposeInPlace();
Eigen::MatrixXd Y = refy.replicate(1, numCols);
// Eigen can only deal with two matrices at a time,
// so split the computation:
// topographyGrid = sin(X) * sin(Y) * abs(X) * abs(Y) -pi
Eigen::MatrixXd absXY = X.cwiseAbs().cwiseProduct(Y.cwiseAbs());
Eigen::MatrixXd sins = X.array().sin().cwiseProduct(Y.array().sin());
Eigen::MatrixXd temp;
temp.resize(numRows, numCols);
temp = absXY.cwiseProduct(sins);
// All this work to create a matrix of pi...
Eigen::MatrixXd pi;
pi.resize(numRows, numCols);
pi.setConstant(M_PI);
temp = temp - pi;
// And the final result.
topographyGrid->data.block(border, border, numRows, numCols) =
temp.block(0, 0, numRows, numCols);
// Ignore positive values.
topographyGrid->data =
topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
topographyGrid->clearNA();
}
int main(int argc, char *argv[])
{
using namespace Eigen;
using namespace std;
// Create the boundary of a square
V.resize(8,2);
E.resize(8,2);
H.resize(1,2);
V << -1,-1, 1,-1, 1,1, -1, 1,
-2,-2, 2,-2, 2,2, -2, 2;
E << 0,1, 1,2, 2,3, 3,0,
4,5, 5,6, 6,7, 7,4;
H << 0,0;
// Triangulate the interior
igl::triangle::triangulate(V,E,H,"a0.005q",V2,F2);
// Plot the generated mesh
igl::viewer::Viewer viewer;
viewer.data.set_mesh(V2,F2);
viewer.launch();
}
//! Function to filter tidal corrections based on RSS amplitude criteria.
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > filterRawDataForAmplitudes(
const Eigen::MatrixXd rawAmplitudes,
const Eigen::MatrixXd rawFundamentalArgumentMultipliers,
const double minimumAmplitude )
{
// Get number of amplitudes per entry.
int numberOfAmplitudes = rawAmplitudes.cols( );
// Initialize return matrices
int numberOfAcceptedAmplitudes = 0;
Eigen::MatrixXd filteredAmplitudes;
filteredAmplitudes.resize( numberOfAcceptedAmplitudes, numberOfAmplitudes );
Eigen::MatrixXd filteredFundamentalArgumentMultipliers;
filteredFundamentalArgumentMultipliers.resize( numberOfAcceptedAmplitudes, 6 );
// Iterate over all entries, calculate RSS amplitude and accept or reject entry.
double rssAmplitude = 0.0;
for( int i = 0; i < rawAmplitudes.rows( ); i++ )
{
// Calculate RSS of amplitude.
rssAmplitude = 0.0;
for( int j =0; j < numberOfAmplitudes; j++ )
{
rssAmplitude += rawAmplitudes( i, j ) * rawAmplitudes( i, j );
}
rssAmplitude = std::sqrt( rssAmplitude );
// If RSS amplitude is sufficiently large, accept value.
if( rssAmplitude > minimumAmplitude )
{
if( numberOfAcceptedAmplitudes == 0 )
{
numberOfAcceptedAmplitudes++;
filteredAmplitudes.resize( 1, numberOfAmplitudes );
filteredAmplitudes = rawAmplitudes.block( i, 0, 1, numberOfAmplitudes );
filteredFundamentalArgumentMultipliers.resize( 1, 6 );
filteredFundamentalArgumentMultipliers = rawFundamentalArgumentMultipliers.block( i, 0, 1, 6 );
}
else
{
numberOfAcceptedAmplitudes++;
filteredAmplitudes.conservativeResize( numberOfAcceptedAmplitudes, numberOfAmplitudes );
filteredAmplitudes.block( numberOfAcceptedAmplitudes - 1, 0, 1, numberOfAmplitudes ) =
rawAmplitudes.block( i, 0, 1, numberOfAmplitudes );
filteredFundamentalArgumentMultipliers.conservativeResize( numberOfAcceptedAmplitudes, 6 );
filteredFundamentalArgumentMultipliers.block( numberOfAcceptedAmplitudes - 1, 0, 1, 6 ) =
rawFundamentalArgumentMultipliers.block( i, 0, 1, 6 );
}
}
}
return std::pair< Eigen::MatrixXd, Eigen::MatrixXd >( filteredAmplitudes, filteredFundamentalArgumentMultipliers );
}
int MNIST_Parser::read_MNIST_format(
std::ifstream &file,
Eigen::MatrixXd &storage,
unsigned int header_size)
{
if (!file.is_open())
return -1;
unsigned char *buffer = new unsigned char [header_size];
int *header = new int [header_size];
for (unsigned int i = 0; i < header_size; ++i) {
file.read((char*)buffer, 4);
//convert big-endian to little-endian
header[i] = buffer[0] << 24 | buffer[1] << 16 | buffer[2] << 8 | buffer[3] << 0;
//for (int i = 0; i < 4; ++i) {
//std::cout << std::hex << (int)buffer[i] << std::endl;
//}
//if (i==0) std::cout << std::hex;
//else std::cout << std::dec;
//std::cout << header[i] << std::endl;
}
if (header_size == 4)
{
int img_size = header[2]*header[3];
storage.resize(img_size, header[1]);
unsigned char *img = new unsigned char [img_size];
for (int i = 0; i < header[1]; ++i) {
file.read((char*)img, img_size);
for (int j = 0; j < img_size; ++j) {
storage.col(i).row(j) << (double)(img[j]);
}
}
storage /= 255.0;
delete[] img;
}
else if (header_size == 2)
{
int column_size = 10;
storage.resize(column_size, header[1]);
storage = MatrixXd::Zero(column_size, header[1]);
unsigned char *digit = new unsigned char [header[1]];
//std::cout << storage.cols() << std::endl;
//std::cout << storage.rows() << std::endl;
file.read((char*)digit, header[1]);
for (int i = 0; i < header[1]; ++i) {
//std::cout << (int)digit[i] << std::endl;
storage.col(i).row(digit[i]) << 1.0;
}
delete[] digit;
}
delete[] buffer;
delete[] header;
return 0;
}
EReturn getMatrix(const tinyxml2::XMLElement & xml_matrix,
Eigen::MatrixXd & eigen_matrix)
{
int dimension = 0;
if (xml_matrix.QueryIntAttribute("dim", &dimension)
!= tinyxml2::XML_NO_ERROR)
{
INDICATE_FAILURE
;
eigen_matrix = Eigen::MatrixXd(); //!< Null matrix again
return PAR_ERR;
}
if (dimension < 1)
{
INDICATE_FAILURE
;
eigen_matrix = Eigen::MatrixXd(); //!< Null matrix again
return PAR_ERR;
}
eigen_matrix.resize(dimension, dimension);
if (!xml_matrix.GetText())
{
INDICATE_FAILURE
;
eigen_matrix = Eigen::MatrixXd(); //!< Null matrix again
return PAR_ERR;
}
std::istringstream text_parser(xml_matrix.GetText());
double temp_entry;
for (int i = 0; i < dimension; i++) //!< Note that this does not handle comments!
{
for (int j = 0; j < dimension; j++)
{
text_parser >> temp_entry;
if (text_parser.fail() || text_parser.bad()) //!< If a failure other than end of file
{
INDICATE_FAILURE
;
eigen_matrix.resize(0, 0);
return PAR_ERR;
}
else
{
eigen_matrix(i, j) = temp_entry;
}
}
}
return SUCCESS;
}
void parse_rhs(
const int nrhs,
const mxArray *prhs[],
Eigen::MatrixXd & V,
Eigen::MatrixXi & F,
Eigen::MatrixXd & P,
Eigen::MatrixXd & N,
int & num_samples)
{
using namespace std;
if(nrhs < 5)
{
mexErrMsgTxt("nrhs < 5");
}
const int dim = mxGetN(prhs[0]);
if(dim != 3)
{
mexErrMsgTxt("Mesh vertex list must be #V by 3 list of vertex positions");
}
if(dim != (int)mxGetN(prhs[1]))
{
mexErrMsgTxt("Mesh facet size must be 3");
}
if(mxGetN(prhs[2]) != dim)
{
mexErrMsgTxt("Point list must be #P by 3 list of origin locations");
}
if(mxGetN(prhs[3]) != dim)
{
mexErrMsgTxt("Normal list must be #P by 3 list of origin normals");
}
if(mxGetN(prhs[4]) != 1 || mxGetM(prhs[4]) != 1)
{
mexErrMsgTxt("Number of samples must be scalar.");
}
V.resize(mxGetM(prhs[0]),mxGetN(prhs[0]));
copy(mxGetPr(prhs[0]),mxGetPr(prhs[0])+V.size(),V.data());
F.resize(mxGetM(prhs[1]),mxGetN(prhs[1]));
copy(mxGetPr(prhs[1]),mxGetPr(prhs[1])+F.size(),F.data());
F.array() -= 1;
P.resize(mxGetM(prhs[2]),mxGetN(prhs[2]));
copy(mxGetPr(prhs[2]),mxGetPr(prhs[2])+P.size(),P.data());
N.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
copy(mxGetPr(prhs[3]),mxGetPr(prhs[3])+N.size(),N.data());
if(*mxGetPr(prhs[4]) != (int)*mxGetPr(prhs[4]))
{
mexErrMsgTxt("Number of samples should be non negative integer.");
}
num_samples = (int) *mxGetPr(prhs[4]);
}
void parse_rhs(
const int nrhs,
const mxArray *prhs[],
Eigen::MatrixXd & V,
Eigen::MatrixXi & Ele,
Eigen::MatrixXd & Q,
Eigen::MatrixXd & bb_mins,
Eigen::MatrixXd & bb_maxs,
Eigen::VectorXi & elements)
{
using namespace std;
using namespace igl;
using namespace igl::matlab;
mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");
const int dim = mxGetN(prhs[0]);
mexErrMsgTxt(dim == 3 || dim == 2,
"Mesh vertex list must be #V by 2 or 3 list of vertex positions");
mexErrMsgTxt(dim+1 == mxGetN(prhs[1]),
"Mesh \"face\" simplex size must equal dimension+1");
parse_rhs_double(prhs,V);
parse_rhs_index(prhs+1,Ele);
parse_rhs_double(prhs+2,Q);
mexErrMsgTxt(Q.cols() == dim,"Dimension of Q should match V");
if(nrhs > 3)
{
mexErrMsgTxt(nrhs >= 6, "The number of input arguments must be 3 or >=6.");
parse_rhs_double(prhs+3,bb_mins);
if(bb_mins.size()>0)
{
mexErrMsgTxt(bb_mins.cols() == dim,"Dimension of bb_mins should match V");
mexErrMsgTxt(bb_mins.rows() >= Ele.rows(),"|bb_mins| should be > |Ele|");
}
parse_rhs_double(prhs+4,bb_maxs);
mexErrMsgTxt(bb_maxs.cols() == bb_mins.cols(),
"|bb_maxs| should match |bb_mins|");
mexErrMsgTxt(bb_mins.rows() == bb_maxs.rows(),
"|bb_mins| should match |bb_maxs|");
parse_rhs_index(prhs+5,elements);
mexErrMsgTxt(elements.cols() == 1,"Elements should be column vector");
mexErrMsgTxt(bb_mins.rows() == elements.rows(),
"|bb_mins| should match |elements|");
}else
{
// Defaults
bb_mins.resize(0,dim);
bb_maxs.resize(0,dim);
elements.resize(0,1);
}
}
IGL_INLINE void igl::signed_distance_pseudonormal(
const Eigen::MatrixXd & P,
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const AABB<Eigen::MatrixXd,3> & tree,
const Eigen::MatrixXd & FN,
const Eigen::MatrixXd & VN,
const Eigen::MatrixXd & EN,
const Eigen::VectorXi & EMAP,
Eigen::VectorXd & S,
Eigen::VectorXi & I,
Eigen::MatrixXd & C,
Eigen::MatrixXd & N)
{
using namespace Eigen;
const size_t np = P.rows();
S.resize(np,1);
I.resize(np,1);
N.resize(np,3);
C.resize(np,3);
# pragma omp parallel for if(np>1000)
for(size_t p = 0;p<np;p++)
{
double s,sqrd;
RowVector3d n,c;
int i = -1;
RowVector3d q = P.row(p);
signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,q,s,sqrd,i,c,n);
S(p) = s*sqrt(sqrd);
I(p) = i;
N.row(p) = n;
C.row(p) = c;
}
// igl::AABB<MatrixXd,3> tree_P;
// MatrixXi J = VectorXi::LinSpaced(P.rows(),0,P.rows()-1);
// tree_P.init(P,J);
// tree.squared_distance(V,F,tree_P,P,J,S,I,C);
//# pragma omp parallel for if(np>1000)
// for(size_t p = 0;p<np;p++)
// {
// RowVector3d c = C.row(p);
// RowVector3d q = P.row(p);
// const int f = I(p);
// double s;
// RowVector3d n;
// pseudonormal_test(V,F,FN,VN,EN,EMAP,q,f,c,s,n);
// N.row(p) = n;
// S(p) = s*sqrt(S(p));
// }
}
// fvec case, only support row decomposition
void create_matrix(const paracel::list_type<paracel::str_type> & linelst,
Eigen::MatrixXd & blk_dense_mtx,
paracel::dict_type<size_t, paracel::str_type> & rm) {
int csz = 0;
size_t indx = 0;
bool flag = true;
paracel::list_type<Eigen::VectorXd> mtx_llst;
for(auto & line : linelst) {
auto stf = parserfunc(line);
if(flag) { csz = stf.size() - 1; flag = true; }
rm[indx] = stf[0];
indx += 1;
Eigen::VectorXd tmp(csz);
for(int i = 0; i < csz; ++i) {
tmp[i] = std::stod(stf[i + 1]);
}
mtx_llst.push_back(tmp);
}
// create dense block matrix
blk_dense_mtx.resize(rm.size(), csz);
for(size_t i = 0; i < rm.size(); ++i) {
blk_dense_mtx.row(i) = mtx_llst[i];
}
}
EReturn OMPLsolver::convertPath(const ompl::geometric::PathGeometric &pg,
Eigen::MatrixXd & traj)
{
traj.resize(pg.getStateCount(), state_space_->getDimension());
Eigen::VectorXd tmp(state_space_->getDimension());
for (int i = 0; i < (int) pg.getStateCount(); ++i)
{
if (!ok(
compound_ ?
boost::static_pointer_cast<OMPLSE3RNCompoundStateSpace>(
state_space_)->OMPLStateToEigen(pg.getState(i), tmp) :
boost::static_pointer_cast<OMPLStateSpace>(state_space_)->copyFromOMPLState(
pg.getState(i), tmp)))
{
ERROR("Can't copy state "<<i);
return FAILURE;
}
else
{
traj.row(i) = tmp;
}
}
return SUCCESS;
}
//==============================================================================
TEST(Lemke, Lemke_4D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;
f = new Eigen::VectorXd(4);
A.resize(4,4);
A <<
3.999,0.9985, 1.001, -2,
0.9985, 3.998, -2,0.9995,
1.001, -2, 4.002, 1.001,
-2,0.9995, 1.001, 4.001;
b.resize(4);
b <<
-0.01008,
-0.009494,
-0.07234,
-0.07177;
err = dart::lcpsolver::Lemke(A,b,f);
EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}
//==============================================================================
TEST(Lemke, Lemke_6D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;
f = new Eigen::VectorXd(6);
A.resize(6,6);
A <<
3.1360, -2.0370, 0.9723, 0.1096, -2.0370, 0.9723,
-2.0370, 3.7820, 0.8302, -0.0257, 2.4730, 0.0105,
0.9723, 0.8302, 5.1250, -2.2390, -1.9120, 3.4080,
0.1096, -0.0257, -2.2390, 3.1010, -0.0257, -2.2390,
-2.0370, 2.4730, -1.9120, -0.0257, 5.4870, -0.0242,
0.9723, 0.0105, 3.4080, -2.2390, -0.0242, 3.3860;
b.resize(6);
b <<
0.1649,
-0.0025,
-0.0904,
-0.0093,
-0.0000,
-0.0889;
err = dart::lcpsolver::Lemke(A,b,f);
EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}
void CTRCalibration::ComputeErrorJacobian(::Eigen::MatrixXd& error_jacobian)
{
error_jacobian.resize(m_params.size(),1);
::Eigen::VectorXd params_original(m_params);
double epsilon = 0.00001;
double invEpsilon = 1.0/epsilon;
double error_perturbed = 0;
double error_perturbed2 = 0;
double dummy = 0;
for(int i = 0; i < m_params.size(); ++i)
{
double cur_epsilon = epsilon/m_scaleFactor[i];
m_params[i] += cur_epsilon;
//*(robot->GetFreeParameters()[i]) = params[i];
UpdateParamsInAllCTR();
error_perturbed = ComputeErrorOnTrainingSet(dummy);
//error_perturbed = ComputeErrorOnDatasetShape(robot, kinematics, dataset, dummy);
m_params[i] = params_original[i];
m_params[i] -= cur_epsilon;
//*(robot->GetFreeParameters()[i]) = params[i];
UpdateParamsInAllCTR();
error_perturbed2 = ComputeErrorOnTrainingSet(dummy);
//error_perturbed2 = ComputeErrorOnDatasetShape(robot, kinematics, dataset, dummy);
error_jacobian(i) = (error_perturbed - error_perturbed2) / (2*cur_epsilon);
//jacobian(i) = (error_perturbed - error_original) / cur_epsilon;
m_params[i] = params_original[i];
//*(robot->GetFreeParameters()[i]) = m_params[i];
}
}
void FixedJoint::qdot2v(const Eigen::Ref<const Eigen::VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
qdot_to_v.resize(getNumVelocities(), getNumPositions());
if (dqdot_to_v) {
dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
}
}
void FunctionApproximatorGMR::predictVariance(const Eigen::Ref<const Eigen::MatrixXd>& inputs, Eigen::MatrixXd& variances)
{
ENTERING_REAL_TIME_CRITICAL_CODE
variances.resize(inputs.rows(),getExpectedOutputDim());
predict(inputs,empty_prealloc_,variances);
EXITING_REAL_TIME_CRITICAL_CODE
}
void FixedJoint::v2qdot(const Eigen::Ref<const Eigen::VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
v_to_qdot.resize(getNumPositions(), getNumVelocities());
if (dv_to_qdot) {
dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
}
}
void
io_utils::file2matrix(const std::string &file, Eigen::MatrixXd &mat, int cols )
{
std::ifstream in;
open_in_file( in, file );
if(!in){
std::cout << "[io_utils] Cannot open file: " << file << std::endl;
throw std::runtime_error("Runtime error...\n");
}
std::vector<double> data;
double val;
while ( in >> val ){
data.push_back( val );
}
// copy the vector to an eigen matrix
int rows = data.size() / cols;
mat.resize( rows, cols );
std::copy( data.data(), data.data()+data.size(), mat.data() );
for(int r = 0; r < rows; ++r)
for(int c = 0; c < cols; ++c)
{
mat(r,c) = data[cols*r + c];
}
in.close();
}
IGL_INLINE bool igl::harmonic(
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const Eigen::VectorXi & b,
const Eigen::MatrixXd & bc,
const int k,
Eigen::MatrixXd & W)
{
using namespace igl;
using namespace Eigen;
SparseMatrix<double> L,M,Mi;
cotmatrix(V,F,L);
massmatrix(V,F,MASSMATRIX_VORONOI,M);
invert_diag(M,Mi);
SparseMatrix<double> Q = -L;
for(int p = 1;p<k;p++)
{
Q = (Q*Mi*-L).eval();
}
const VectorXd B = VectorXd::Zero(V.rows(),1);
min_quad_with_fixed_data<double> data;
min_quad_with_fixed_precompute(Q,b,SparseMatrix<double>(),true,data);
W.resize(V.rows(),bc.cols());
for(int w = 0;w<bc.cols();w++)
{
const VectorXd bcw = bc.col(w);
VectorXd Ww;
if(!min_quad_with_fixed_solve(data,B,bcw,VectorXd(),Ww))
{
return false;
}
W.col(w) = Ww;
}
return true;
}
/******************************************************************
* Get direction jacobian
*
* @ J : return geo jacobian (6 X dof)
******************************************************************/
void sKinematics::getJacobian(Eigen::MatrixXd& J)
{
int i, j, ai;
double v1[3], v2[3], v3[3];
J.resize(6,dof);
for(ai=0; ai<dof; ai++){
i=active_index[ai];
if(i==0){
for( j=0; j<3; j++){
v1[j] = T0[j][2];
v2[j] = H0F[j][3] - T0[j][3];
}
J(3,ai) = T0[0][2];
J(4,ai) = T0[1][2];
J(5,ai) = T0[2][2];
}
else{
for( j=0; j<3; j++){
v1[j] = sDH[i-1].H0i[j][2];
v2[j] = H0F[j][3] - sDH[i-1].H0i[j][3];
}
J(3,ai) = sDH[i-1].H0i[0][2];
J(4,ai) = sDH[i-1].H0i[1][2];
J(5,ai) = sDH[i-1].H0i[2][2];
}
CrossProduct(v1, v2, v3);
J(0,ai) = v3[0];
J(1,ai) = v3[1];
J(2,ai) = v3[2];
}
}
void sKinematics::getJacobianDirection(int axis, Eigen::MatrixXd &J)
{
int i, j, ai;
double v1[3], v2[3], v3[3];
J.resize(3,dof);
for(ai=0; ai<dof; ai++){
i=active_index[ai];
if( i==0 ){
for( j=0; j<3; j++){
v1[j] = T0[j][2];
v2[j] = H0F[j][axis];
}
}
else{
for( j=0; j<3; j++){
v1[j] = sDH[i-1].H0i[j][2];
v2[j] = H0F[j][axis];
}
}
CrossProduct(v1, v2, v3);
J(0,ai) = v3[0];
J(1,ai) = v3[1];
J(2,ai) = v3[2];
}
}
void file2matrix( const std::string filename, Eigen::MatrixXd & mat )
{
std::ifstream in;
open_file( in, filename );
if(!in)
throw std::runtime_error("Cannot open view point positions file...\n");
std::vector<double> data;
double val;
while ( in >> val ){
data.push_back( val );
}
// copy the vector to an eigen matrix
int cols = 3;
int rows = data.size() / 3;
mat.resize( rows, cols );
std::copy( data.data(), data.data()+data.size(), mat.data() );
for(int r = 0; r < rows; ++r)
for(int c = 0; c < cols; ++c)
{
mat(r,c) = data[cols*r + c];
}
in.close();
}
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 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;
}
void color_selfintersections(
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
Eigen::MatrixXd & C)
{
using namespace igl;
using namespace igl::cgal;
using namespace Eigen;
using namespace std;
MatrixXd SV;
MatrixXi SF,IF;
VectorXi J,IM;
RemeshSelfIntersectionsParam params;
params.detect_only = false;
remesh_self_intersections(V,F,params,SV,SF,IF,J,IM);
writeOBJ("F**K.obj",SV,SF);
C.resize(F.rows(),3);
C.col(0).setConstant(0.4);
C.col(1).setConstant(0.8);
C.col(2).setConstant(0.3);
for(int f = 0;f<IF.rows();f++)
{
C.row(IF(f,0)) = RowVector3d(1,0.4,0.4);
C.row(IF(f,1)) = RowVector3d(1,0.4,0.4);
}
}
TestState() :
x(0.0),
v(0.0)
{
Covariance.resize(ndim(), ndim());
Covariance.setIdentity();
};
void DiagonalBlockSparseMatrix::Solve(const Eigen::MatrixXd& rhs, Eigen::MatrixXd& sol)
{
int numRows = GetNumRows();
int numCols = GetNumCols();
LOG_IF(FATAL, numRows != numCols) << "DiagonalBlockSparseMatrix is not a square matrix and noninvertible.";
int rhsCols = rhs.cols();
int rhsRows = rhs.rows();
LOG_IF(FATAL, numCols != rhsRows) << "DiagonalBlockSparseMatrix and right hand side matrix are of different dimensions.";
sol.resize(rhsRows, rhsCols);
int numUntilLast = 0;
int numDiagElements = static_cast<int>(mDiagElements.size());
for (int i = 0; i < numDiagElements; ++i)
{
int numColsElement = mDiagElements[i].GetNumCols();
Eigen::MatrixXd partSol(numColsElement, rhsCols);
Eigen::MatrixXd partRhs = rhs.block(numUntilLast, 0, numColsElement, rhsCols);
#if LLT_SOLVE
mDiagElements[i].LltSolve(partRhs, partSol);
#else
mDiagElements[i].ConjugateGradientSolve(partRhs, partSol);
#endif
sol.block(numUntilLast, 0, numColsElement, rhsCols) = partSol;
numUntilLast += numColsElement;
}
}
// Converts a representative vector per face in the full set of vectors that describe
// an N-RoSy field
void representative_to_nrosy(
const Eigen::MatrixXd& V,
const Eigen::MatrixXi& F,
const Eigen::MatrixXd& R,
const int N,
Eigen::MatrixXd& Y)
{
using namespace Eigen;
using namespace std;
MatrixXd B1, B2, B3;
igl::local_basis(V,F,B1,B2,B3);
Y.resize(F.rows()*N,3);
for (unsigned i=0; i<F.rows(); ++i)
{
double x = R.row(i) * B1.row(i).transpose();
double y = R.row(i) * B2.row(i).transpose();
double angle = atan2(y,x);
for (unsigned j=0; j<N; ++j)
{
double anglej = angle + 2*M_PI*double(j)/double(N);
double xj = cos(anglej);
double yj = sin(anglej);
Y.row(i*N+j) = xj * B1.row(i) + yj * B2.row(i);
}
}
}
int main(int argc, char *argv[])
{
using namespace std;
using namespace Eigen;
// Load a mesh in OFF format
igl::readOFF("../shared/bumpy.off", V, F);
// Threshold faces with high anisotropy
b.resize(1);
b << 0;
bc.resize(1,3);
bc << 1,1,1;
igl::Viewer viewer;
// Interpolate the field and plot
key_down(viewer, '4', 0);
// Plot the mesh
viewer.data.set_mesh(V, F);
viewer.callback_key_down = &key_down;
// Disable wireframe
viewer.core.show_lines = false;
// Launch the viewer
viewer.launch();
}
bool computePlaneToPlaneHomography( const Eigen::MatrixXd &points1, const Eigen::MatrixXd &points2, Eigen::MatrixXd &H,
std::vector<bool> &mask, double reprojectionErrorThreshold )
{
if( points1.size() != points2.size() )
{
throw std::runtime_error( "computePlaneToPlaneHomography: Point lists must be of the same length." );
}
if( H.cols() != 3 || H.rows() != 3 )
{
H.resize( 3, 3 );
}
bool result = false;
// If we have exactly 4 points, compute the homography.
if( points1.size() == 4 )
{
result = compute4PointPlaneToPlaneHomography( points1, points2, H );
}
else
{
// Otherwise, use RANSAC to remove the outliers.
Detail::HomographyEstimator estimator(4);
estimator.setThreshold( reprojectionErrorThreshold );
result = estimator( points1, points2, H, mask );
}
return result;
}
