void mrpt::vision::pnp::rpnp::calcampose(Eigen::MatrixXd& XXc, Eigen::MatrixXd& XXw, Eigen::Matrix3d& R2, Eigen::Vector3d& t2)
{
Eigen::MatrixXd X = XXc;
Eigen::MatrixXd Y = XXw;
Eigen::MatrixXd K = Eigen::MatrixXd::Identity(n, n) - Eigen::MatrixXd::Ones(n, n) * 1 / n;
Eigen::VectorXd ux, uy;
uy = X.rowwise().mean();
ux = Y.rowwise().mean();
// Need to verify sigmax2
double sigmax2 = (((X*K).array() * (X*K).array()).colwise().sum()).mean();
Eigen::MatrixXd SXY = Y*K*(X.transpose()) / n;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(SXY, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Matrix3d S = Eigen::MatrixXd::Identity(3, 3);
if (SXY.determinant() <0)
S(2, 2) = -1;
R2 = svd.matrixV()*S*svd.matrixU().transpose();
double c2 = (svd.singularValues().asDiagonal()*S).trace() / sigmax2;
t2 = uy - c2*R2*ux;
Eigen::Vector3d x, y, z;
x = R2.col(0);
y = R2.col(1);
z = R2.col(2);
if ((x.cross(y) - z).norm()>0.02)
R2.col(2) = -R2.col(2);
}
// Original code from the ROS vslam package pe3d.cpp
// uses the SVD procedure for aligning point clouds
// SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
{
using namespace Eigen;
SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");
Vector3d c0 = p0.rowwise().mean();
Vector3d c1 = p1.rowwise().mean();
Matrix3d H(Matrix3d::Zero());
// subtract out
// p0a -= c0;
// p0b -= c0;
// p0c -= c0;
// p1a -= c1;
// p1b -= c1;
// p1c -= c1;
// Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
// p1c*p0c.transpose();
for(int i = 0; i < p0.cols(); ++i)
{
H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
}
// do the SVD thang
JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
if (det < 0.0)
{
V.col(2) = V.col(2) * -1.0;
R = V * svd.matrixU().transpose();
}
Vector3d tr = c0-R.transpose()*c1; // translation
// transformation matrix, 3x4
Matrix4d tfm(Matrix4d::Identity());
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.topLeftCorner<3,3>() = R.transpose();
tfm.topRightCorner<3,1>() = tr;
return tfm;
}
bool CGEOM::generate_scene_trans
( const SceneGeneratorOptions& sc_opts,
Eigen::MatrixXd& mP3D,
Eigen::MatrixXd& mMeasT,
Eigen::MatrixXd& mMeasN,
Eigen::Matrix3d& mR,
Eigen::Vector3d& mt
) {
if( !generate_scene( sc_opts,
mP3D, mMeasT, mMeasN
) ) {
return false;
}
// Create random transform
mt = mP3D.rowwise().mean();
const double drotx = rand_range_d( -45., 45. )*3.14159/180.;
const double droty = rand_range_d( -45., 45. )*3.14159/180.;
const double drotz = rand_range_d( -45., 45. )*3.14159/180.;
#if 1
mR =
( CEIGEN::skew_rot<Eigen::Matrix3d,double>( drotx, 0., 0. ) +
CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., droty , 0.) +
CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., 0., drotz ) ).exp();
#else
mR = Eigen::Matrix3d::Identity();
#endif
mP3D.colwise() -= mt;
mP3D = mR.transpose() * mP3D;
return true;
}
Eigen::Vector3d PlaneRegistration::estimate_trj_unit_vector( Eigen::Vector3d& start_point,
Eigen::Vector3d& curr_end_point,
Eigen::MatrixXd& unit_vector_history ){
Eigen::Vector3d curr_vector = ( curr_end_point - start_point );
Eigen::Vector3d curr_unit_vector = curr_vector / curr_vector.norm();
int num_rows = unit_vector_history.rows();
int num_cols = unit_vector_history.cols();
// add the current vector into history
unit_vector_history.conservativeResize( num_rows, num_cols + 1 );
unit_vector_history.col( num_cols ) = curr_unit_vector;
// add current point into history
curr_point_history.conservativeResize( num_rows, num_cols + 1 );
curr_point_history.col( num_cols ) = curr_end_point;
if ( unit_vector_history.cols() > 25 ){
remove_matrix_column( unit_vector_history, 0 );
remove_matrix_column( curr_point_history, 0 );
}
Eigen::Vector3d avg_vector = unit_vector_history.rowwise().mean();
return avg_vector / avg_vector.norm();;
}
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 StompOptimizationTask::computeCosts(const Eigen::MatrixXd& features, Eigen::VectorXd& costs, Eigen::MatrixXd& weighted_feature_values) const
{
weighted_feature_values = Eigen::MatrixXd::Zero(num_time_steps_, num_split_features_);
for (int t=0; t<num_time_steps_; ++t)
{
weighted_feature_values.row(t) = (((features.row(t+stomp::TRAJECTORY_PADDING) - feature_means_.transpose()).array() /
feature_variances_.array().transpose()) * feature_weights_.array().transpose()).matrix();
}
costs = weighted_feature_values.rowwise().sum();
}
void drawField(igl::viewer::Viewer &viewer,
const Eigen::MatrixXd &field,
const Eigen::RowVector3d &color)
{
for (int n=0; n<2; ++n)
{
Eigen::MatrixXd VF = field.block(0,n*3,F.rows(),3);
Eigen::VectorXd c = VF.rowwise().norm();
viewer.data.add_edges(B - global_scale*VF, B + global_scale*VF , color);
}
}
int main(int argc, char *argv[])
{
using namespace Eigen;
using namespace std;
igl::readOBJ(TUTORIAL_SHARED_PATH "/bump-domain.obj",V,F);
U=V;
// Find boundary vertices outside annulus
typedef Matrix<bool,Dynamic,1> VectorXb;
VectorXb is_outer = (V.rowwise().norm().array()-1.0)>-1e-15;
VectorXb is_inner = (V.rowwise().norm().array()-0.15)<1e-15;
VectorXb in_b = is_outer.array() || is_inner.array();
igl::colon<int>(0,V.rows()-1,b);
b.conservativeResize(stable_partition( b.data(), b.data()+b.size(),
[&in_b](int i)->bool{return in_b(i);})-b.data());
bc.resize(b.size(),1);
for(int bi = 0;bi<b.size();bi++)
{
bc(bi) = (is_outer(b(bi))?0.0:1.0);
}
// Pseudo-color based on selection
MatrixXd C(F.rows(),3);
RowVector3d purple(80.0/255.0,64.0/255.0,255.0/255.0);
RowVector3d gold(255.0/255.0,228.0/255.0,58.0/255.0);
for(int f = 0;f<F.rows();f++)
{
if( in_b(F(f,0)) && in_b(F(f,1)) && in_b(F(f,2)))
{
C.row(f) = purple;
}else
{
C.row(f) = gold;
}
}
// Plot the mesh with pseudocolors
igl::viewer::Viewer viewer;
viewer.data.set_mesh(U, F);
viewer.core.show_lines = false;
viewer.data.set_colors(C);
viewer.core.trackball_angle = Eigen::Quaternionf(0.81,-0.58,-0.03,-0.03);
viewer.core.trackball_angle.normalize();
viewer.callback_pre_draw = &pre_draw;
viewer.callback_key_down = &key_down;
viewer.core.is_animating = true;
viewer.core.animation_max_fps = 30.;
cout<<
"Press [space] to toggle animation."<<endl<<
"Press '.' to increase k."<<endl<<
"Press ',' to decrease k."<<endl;
viewer.launch();
}
void RigidObject::computeBoundingBox() {
_bounding_box.resize(8 * 3);
Eigen::Map<const Eigen::MatrixXf> vertices_f(getPositions().data(), 3,
getNPositions());
Eigen::MatrixXd vertices;
vertices = vertices_f.cast<double>();
// subtract vertices mean
Eigen::Vector3d mean_vertices = vertices.rowwise().mean();
vertices = vertices - mean_vertices.replicate(1, getNPositions());
// compute eigenvector covariance matrix
Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver(vertices *
vertices.transpose());
Eigen::MatrixXd real_eigenvectors = eigen_solver.eigenvectors().real();
// rotate centered vertices with inverse eigenvector matrix
vertices = real_eigenvectors.transpose() * vertices;
// compute simple bounding box
auto mn = vertices.rowwise().minCoeff();
auto mx = vertices.rowwise().maxCoeff();
Eigen::Matrix<double, 3, 8> bounding_box;
bounding_box << mn(0), mn(0), mn(0), mn(0), mx(0), mx(0), mx(0), mx(0), mn(1),
mn(1), mx(1), mx(1), mn(1), mn(1), mx(1), mx(1), mn(2), mx(2), mn(2),
mx(2), mn(2), mx(2), mn(2), mx(2);
// rotate and translate bounding box back to original position
Eigen::Matrix3d rot_back = real_eigenvectors;
Eigen::Translation<double, 3> tra_back(mean_vertices);
Eigen::Transform<double, 3, Eigen::Affine> t = tra_back * rot_back;
bounding_box = t * bounding_box;
// convert to float
Eigen::Map<Eigen::MatrixXf> bounding_box_f(_bounding_box.data(), 3, 8);
bounding_box_f = bounding_box.cast<float>();
}
Eigen::MatrixXd getMaxDiffForeachEdge(const Eigen::MatrixXd& m)
{
if(m.cols() == 1){
return m.cwiseAbs();
}
else
{
Eigen::MatrixXd maxValueForEachRow = m.rowwise().maxCoeff();
Eigen::MatrixXd minValueForEachRow = m.rowwise().minCoeff();
Eigen::MatrixXd maxDiffForEachRow = maxValueForEachRow - minValueForEachRow;
Eigen::MatrixXd maxAbsDiffForEachRow = maxDiffForEachRow.cwiseAbs();
return maxAbsDiffForEachRow;
}
}
TEST(slice_into,density_reverse)
{
{
Eigen::MatrixXd A = Eigen::MatrixXd::Random(10,9);
Eigen::VectorXi I = Eigen::VectorXi::LinSpaced(A.rows(),A.rows()-1,0);
Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(A.cols(),0,A.cols()-1);
Eigen::MatrixXd B(I.maxCoeff()+1,J.maxCoeff()+1);
igl::slice_into(A,I,J,B);
// reverse rows (i.e., reverse each column vector)
Eigen::MatrixXd C = A.colwise().reverse().eval();
test_common::assert_eq(B,C);
}
{
Eigen::MatrixXd A = Eigen::MatrixXd::Random(10,9);
Eigen::VectorXi I = Eigen::VectorXi::LinSpaced(A.rows(),0,A.rows()-1);
Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(A.cols(),A.cols()-1,0);
Eigen::MatrixXd B(I.maxCoeff()+1,J.maxCoeff()+1);
igl::slice_into(A,I,J,B);
// reverse cols (i.e., reverse each row vector)
Eigen::MatrixXd C = A.rowwise().reverse().eval();
test_common::assert_eq(B,C);
}
}
void object::test<6>()
{
for (int i = 0;i<10;i++)
{
int dim = rand()%1000+2;
int samplenum1 = rand()%1000+100;
int samplenum2 = rand()%1000+100;
Eigen::MatrixXd ha = Eigen::MatrixXd::Random(dim,samplenum1);
Eigen::MatrixXd hb = Eigen::MatrixXd::Random(dim,samplenum2);
Eigen::VectorXd haa = (ha.array()*ha.array()).colwise().sum();
Eigen::VectorXd hbb = (hb.array()*hb.array()).colwise().sum();
Eigen::MatrixXd hdist = -2*ha.transpose()*hb;
hdist.colwise() += haa;
hdist.rowwise() += hbb.transpose();
Matrix<double> ga(ha),gb(hb);
Matrix<double> gdist = -2*ga.transpose()*gb;
Vector<double> gaa = (ga.array()*ga.array()).colwise().sum();
Vector<double> gbb = (gb.array()*gb.array()).colwise().sum();
gdist.colwise() += gaa;
gdist.rowwise() += gbb;
ensure(check_diff(hdist,gdist));
}
}
int main(int argc, char *argv[])
{
using namespace Eigen;
using namespace std;
// Load a mesh in OFF format
igl::readOFF("../shared/cow.off", V, F);
// Compute Laplace-Beltrami operator: #V by #V
igl::cotmatrix(V,F,L);
// Alternative construction of same Laplacian
SparseMatrix<double> G,K;
// Gradient/Divergence
igl::grad(V,F,G);
// Diagonal per-triangle "mass matrix"
VectorXd dblA;
igl::doublearea(V,F,dblA);
// Place areas along diagonal #dim times
const auto & T = 1.*(dblA.replicate(3,1)*0.5).asDiagonal();
// Laplacian K built as discrete divergence of gradient or equivalently
// discrete Dirichelet energy Hessian
K = -G.transpose() * T * G;
cout<<"|K-L|: "<<(K-L).norm()<<endl;
const auto &key_down = [](igl::Viewer &viewer,unsigned char key,int mod)->bool
{
switch(key)
{
case 'r':
case 'R':
U = V;
break;
case ' ':
{
// Recompute just mass matrix on each step
SparseMatrix<double> M;
igl::massmatrix(U,F,igl::MASSMATRIX_TYPE_BARYCENTRIC,M);
// Solve (M-delta*L) U = M*U
const auto & S = (M - 0.001*L);
Eigen::SimplicialLLT<Eigen::SparseMatrix<double > > solver(S);
assert(solver.info() == Eigen::Success);
U = solver.solve(M*U).eval();
// Compute centroid and subtract (also important for numerics)
VectorXd dblA;
igl::doublearea(U,F,dblA);
double area = 0.5*dblA.sum();
MatrixXd BC;
igl::barycenter(U,F,BC);
RowVector3d centroid(0,0,0);
for(int i = 0;i<BC.rows();i++)
{
centroid += 0.5*dblA(i)/area*BC.row(i);
}
U.rowwise() -= centroid;
// Normalize to unit surface area (important for numerics)
U.array() /= sqrt(area);
break;
}
default:
return false;
}
// Send new positions, update normals, recenter
viewer.data.set_vertices(U);
viewer.data.compute_normals();
viewer.core.align_camera_center(U,F);
return true;
};
// Use original normals as pseudo-colors
MatrixXd N;
igl::per_vertex_normals(V,F,N);
MatrixXd C = N.rowwise().normalized().array()*0.5+0.5;
// Initialize smoothing with base mesh
U = V;
viewer.data.set_mesh(U, F);
viewer.data.set_colors(C);
viewer.callback_key_down = key_down;
cout<<"Press [space] to smooth."<<endl;;
cout<<"Press [r] to reset."<<endl;;
return viewer.launch();
}
void BayesNet::createFullJoint(cspace b_Xprior[2]) {
std::random_device rd;
std::normal_distribution<double> dist(0, 1);
cspace tmpConfig;
for (int i = 0; i < numParticles; i ++) {
// Root
for (int j = 0; j < cdim; j++) {
fullJointPrev[i][j] = b_Xprior[0][j] + b_Xprior[1][j] * (dist(rd));
tmpConfig[j] = fullJointPrev[i][j];
}
// Front Plane
cspace relativeConfig, baseConfig, transformedConfig, edgeConfig;
cspace frontPlaneConfig, sidePlaneConfig, otherSidePlaneConfig;
relativeConfig[0] = 1.22;
relativeConfig[1] = -0.025;
relativeConfig[2] = 0;
relativeConfig[3] = 0;
relativeConfig[4] = 0;
relativeConfig[5] = Pi;
baseConfig = tmpConfig;
transFrameConfig(baseConfig, relativeConfig, frontPlaneConfig);
//TEMP:
if (frontPlaneConfig[5] < 0) frontPlaneConfig[5] += 2 * Pi;
copyParticles(frontPlaneConfig, fullJointPrev[i], cdim);
// Bottom Edge
cspace prior1[2] = {{0,0,0,1.22,0,0},{0,0,0,0.0005,0.0005,0.0005}};
for (int j = 0; j < cdim; j++) {
relativeConfig[j] = prior1[0][j] + prior1[1][j] * (dist(rd));
}
baseConfig = tmpConfig;
transPointConfig(baseConfig, relativeConfig, edgeConfig);
copyParticles(edgeConfig, fullJointPrev[i], 2 * cdim);
// Side Edge
cspace prior2[2] = {{0,-0.025,0,0,-0.025,0.23},{0,0,0,0.0005,0.0005,0.0005}};
for (int j = 0; j < cdim; j++) {
relativeConfig[j] = prior2[0][j] + prior2[1][j] * (dist(rd));
}
baseConfig = tmpConfig;
transPointConfig(baseConfig, relativeConfig, transformedConfig);
copyParticles(transformedConfig, fullJointPrev[i], 3 * cdim);
// Top edge
double edgeTol = 0.001;
double edgeOffSet = 0.23;
Eigen::Vector3d pa, pb;
pa << edgeConfig[0], edgeConfig[1], edgeConfig[2];
pb << edgeConfig[3], edgeConfig[4], edgeConfig[5];
Eigen::Vector3d pa_prime, pb_prime;
inverseTransform(pa, frontPlaneConfig, pa_prime);
inverseTransform(pb, frontPlaneConfig, pb_prime);
double td = dist(rd) * edgeTol;
// pa_prime(1) = 0;
// pb_prime(1) = 0;
Eigen::Vector3d normVec;
normVec << (pb_prime(2) - pa_prime(2)), 0, (pa_prime(0) - pb_prime(0));
normVec.normalize();
normVec *= (edgeOffSet + td);
pa_prime(0) += normVec(0);
pb_prime(0) += normVec(0);
pa_prime(2) += normVec(2);
pb_prime(2) += normVec(2);
Transform(pa_prime, frontPlaneConfig, pa);
Transform(pb_prime, frontPlaneConfig, pb);
fullJointPrev[i][24] = pa(0);
fullJointPrev[i][25] = pa(1);
fullJointPrev[i][26] = pa(2);
fullJointPrev[i][27] = pb(0);
fullJointPrev[i][28] = pb(1);
fullJointPrev[i][29] = pb(2);
// Side Plane
relativeConfig[0] = 0;
relativeConfig[1] = 0;
relativeConfig[2] = 0;
relativeConfig[3] = 0;
relativeConfig[4] = 0;
relativeConfig[5] = -Pi / 2.0;
baseConfig = tmpConfig;
transFrameConfig(baseConfig, relativeConfig, sidePlaneConfig);
copyParticles(sidePlaneConfig, fullJointPrev[i], 5 * cdim);
// Other Side Plane
relativeConfig[0] = 1.24 + dist(rd) * 0.003;
// relativeConfig[0] = 1.2192;
relativeConfig[1] = 0;
relativeConfig[2] = 0;
relativeConfig[3] = 0;
relativeConfig[4] = 0;
relativeConfig[5] = Pi / 2.0;
baseConfig = tmpConfig;
transFrameConfig(baseConfig, relativeConfig, otherSidePlaneConfig);
copyParticles(otherSidePlaneConfig, fullJointPrev[i], 6 * cdim);
// Hole
}
fullJoint = fullJointPrev;
Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>((double *)fullJoint.data(), fulldim, numParticles);
Eigen::MatrixXd mat_centered = mat.colwise() - mat.rowwise().mean();
cov_mat = (mat_centered * mat_centered.adjoint()) / double(max2(mat.cols() - 1, 1));
}
bool BayesNet::updateFullJoint(double cur_M[2][3], double Xstd_ob, double R, int nodeidx) {
cout << "Start updating!" << endl;
std::unordered_set<string> bins;
std::random_device rd;
std::normal_distribution<double> dist(0, 1);
std::uniform_real_distribution<double> distribution(0, numParticles);
int i = 0;
int count = 0;
int count2 = 0;
int count3 = 0;
bool iffar = false;
FullJoint b_X = fullJointPrev;
int idx = 0;
jointCspace tempFullState;
cspace tempState;
double D;
double cur_inv_M[2][3];
double unsigned_dist_check = R + 1 * Xstd_ob;
double signed_dist_check = 1 * Xstd_ob;
//Eigen::Vector3d gradient;
Eigen::Vector3d touch_dir;
int num_bins = 0;
int count_bar = 0;
double coeff = pow(4.0 / ((fulldim + 2.0) * numParticles), 2.0/(fulldim + 4.0)) /1.2155/1.2155;
// cout << "Coeff: " << coeff << endl;
Eigen::MatrixXd H_cov = coeff * cov_mat;
// cout << "full_cov_mat: " << full_cov_mat << endl;
// cout << "cov_mat: " << cov_mat << endl;
// cout << "H_cov: " << H_cov << endl;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(H_cov);
Eigen::MatrixXd rot = eigenSolver.eigenvectors();
Eigen::VectorXd scl = eigenSolver.eigenvalues();
for (int j = 0; j < fulldim; j++) {
scl(j, 0) = sqrt(max2(scl(j, 0),0));
}
Eigen::VectorXd samples(fulldim, 1);
Eigen::VectorXd rot_sample(fulldim, 1);
if (nodeidx == 1 || nodeidx == 5 || nodeidx == 6) { // Plane
cout << "Start updating Plane!" << endl;
while (i < numParticles && i < maxNumParticles) {
idx = int(floor(distribution(rd)));
for (int j = 0; j < fulldim; j++) {
samples(j, 0) = scl(j, 0) * dist(rd);
}
rot_sample = rot*samples;
for (int j = 0; j < fulldim; j++) {
/* TODO: use quaternions instead of euler angles */
tempFullState[j] = b_X[idx][j] + rot_sample(j, 0);
}
for (int j = 0; j < cdim; j++) {
/* TODO: use quaternions instead of euler angles */
tempState[j] = tempFullState[j + nodeidx * cdim];
}
inverseTransform(cur_M, tempState, cur_inv_M);
touch_dir << cur_inv_M[1][0], cur_inv_M[1][1], cur_inv_M[1][2];
D = abs(cur_inv_M[0][1] - R);
// cout << "D: " << D << endl;
// if (xind >= (dist_transform->num_voxels[0] - 1) || yind >= (dist_transform->num_voxels[1] - 1) || zind >= (dist_transform->num_voxels[2] - 1))
// continue;
count += 1;
// if (sqrt(count) == floor(sqrt(count))) cout << "DDDD " << D << endl;
if (D <= signed_dist_check) {
// if (sqrt(count) == floor(sqrt(count))) cout << "D " << D << endl;
count2 ++;
for (int j = 0; j < fulldim; j++) {
fullJoint[i][j] = tempFullState[j];
}
if (checkEmptyBin(&bins, tempState) == 1) {
num_bins++;
// if (i >= N_MIN) {
//int numBins = bins.size();
numParticles = min2(maxNumParticles, max2(((num_bins - 1) * 2), N_MIN));
// }
}
//double d = testResult(mesh, particles[i], cur_M, R);
//if (d > 0.01)
// cout << cur_inv_M[0][0] << " " << cur_inv_M[0][1] << " " << cur_inv_M[0][2] << " " << d << " " << D << //" " << gradient << " " << gradient.dot(touch_dir) <<
// " " << dist_adjacent[0] << " " << dist_adjacent[1] << " " << dist_adjacent[2] << " " << particles[i][2] << endl;
i += 1;
}
}
cout << "End updating Plane!" << endl;
} else { // Edge
// for (int i = 0; i < numParticles; i ++) {
// // for (int j = 0; j < cdim; j ++) {
// // cout << particles[i][j] << " ,";
// // }
// cout << particles[i][0] << " ,";
// }
// cout << endl;
// for (int i = 0; i < numParticles; i ++) {
// // for (int j = 0; j < 18; j ++) {
// // cout << fullParticles[i][j] << " ,";
// // }
// cout << fullParticles[i][0] << " ,";
// }
// cout << endl;
while (i < numParticles && i < maxNumParticles) {
idx = int(floor(distribution(rd)));
for (int j = 0; j < fulldim; j++) {
samples(j, 0) = scl(j, 0) * dist(rd);
}
rot_sample = rot*samples;
for (int j = 0; j < fulldim; j++) {
/* TODO: use quaternions instead of euler angles */
tempFullState[j] = b_X[idx][j] + rot_sample(j, 0);
}
for (int j = 0; j < cdim; j++) {
/* TODO: use quaternions instead of euler angles */
tempState[j] = tempFullState[j + nodeidx * cdim];
}
Eigen::Vector3d x1, x2, x0, tmp1, tmp2;
x1 << tempState[0], tempState[1], tempState[2];
x2 << tempState[3], tempState[4], tempState[5];
x0 << cur_M[0][0], cur_M[0][1], cur_M[0][2];
tmp1 = x1 - x0;
tmp2 = x2 - x1;
D = (tmp1.squaredNorm() * tmp2.squaredNorm() - pow(tmp1.dot(tmp2),2)) / tmp2.squaredNorm();
D = abs(sqrt(D)- R);
// cout << "Cur distance: " << D << endl;
// cout << "D: " << D << endl;
// if (xind >= (dist_transform->num_voxels[0] - 1) || yind >= (dist_transform->num_voxels[1] - 1) || zind >= (dist_transform->num_voxels[2] - 1))
// continue;
count += 1;
// if (sqrt(count) == floor(sqrt(count))) cout << "DDDD " << D << endl;
if (D <= signed_dist_check) {
// if (sqrt(count) == floor(sqrt(count))) cout << "D " << D << endl;
count2 ++;
for (int j = 0; j < fulldim; j++) {
fullJoint[i][j] = tempFullState[j];
}
if (checkEmptyBin(&bins, tempState) == 1) {
num_bins++;
// if (i >= N_MIN) {
//int numBins = bins.size();
numParticles = min2(maxNumParticles, max2(((num_bins - 1) * 2), N_MIN));
// }
}
//double d = testResult(mesh, particles[i], cur_M, R);
//if (d > 0.01)
// cout << cur_inv_M[0][0] << " " << cur_inv_M[0][1] << " " << cur_inv_M[0][2] << " " << d << " " << D << //" " << gradient << " " << gradient.dot(touch_dir) <<
// " " << dist_adjacent[0] << " " << dist_adjacent[1] << " " << dist_adjacent[2] << " " << particles[i][2] << endl;
i += 1;
}
}
cout << "End updating Edge!" << endl;
}
cout << "Number of total iterations: " << count << endl;
cout << "Number of iterations after unsigned_dist_check: " << count2 << endl;
cout << "Number of iterations before safepoint check: " << count3 << endl;
cout << "Number of occupied bins: " << num_bins << endl;
cout << "Number of particles: " << numParticles << endl;
fullJointPrev = fullJoint;
// double* tmpParticles = new double[numParticles * fulldim];
// for(int i = 0; i < numParticles; ++i) {
// for (int j = 0; j < fulldim; j ++) {
// tmpParticles[i * fulldim + j] = fullParticlesPrev[i][j];
// }
// }
Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>((double *)fullJoint.data(), fulldim, numParticles);
Eigen::MatrixXd mat_centered = mat.colwise() - mat.rowwise().mean();
cov_mat = (mat_centered * mat_centered.adjoint()) / double(max2(mat.cols() - 1, 1));
cout << "End updating!" << endl;
return iffar;
}
void FunctionApproximatorIRFRLS::proj(const MatrixXd& vecs, const MatrixXd& periods, const VectorXd& phases, Eigen::MatrixXd& projected)
{
projected = vecs * periods.transpose();
projected.rowwise() += phases.transpose();
projected = projected.unaryExpr(ptr_fun(double_cosine));
}
bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
using namespace std;
using namespace Eigen;
if (key <'1' || key >'6')
return false;
viewer.data.clear();
viewer.core.show_lines = false;
viewer.core.show_texture = false;
if (key == '1')
{
// Frame field constraints
viewer.data.set_mesh(V, F);
MatrixXd F1_t = MatrixXd::Zero(FF1.rows(),FF1.cols());
MatrixXd F2_t = MatrixXd::Zero(FF2.rows(),FF2.cols());
// Highlight in red the constrained faces
MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
for (unsigned i=0; i<b.size();++i)
{
C.row(b(i)) << 1, 0, 0;
F1_t.row(b(i)) = bc1.row(i);
F2_t.row(b(i)) = bc2.row(i);
}
viewer.data.set_colors(C);
MatrixXd C1,C2;
VectorXd K1 = F1_t.rowwise().norm();
VectorXd K2 = F2_t.rowwise().norm();
igl::jet(K1,true,C1);
igl::jet(K2,true,C2);
viewer.data.add_edges(B - global_scale*F1_t, B + global_scale*F1_t ,C1);
viewer.data.add_edges(B - global_scale*F2_t, B + global_scale*F2_t ,C2);
}
if (key == '2')
{
// Frame field
viewer.data.set_mesh(V, F);
MatrixXd C1,C2;
VectorXd K1 = FF1.rowwise().norm();
VectorXd K2 = FF2.rowwise().norm();
igl::jet(K1,true,C1);
igl::jet(K2,true,C2);
viewer.data.add_edges(B - global_scale*FF1, B + global_scale*FF1 ,C1);
viewer.data.add_edges(B - global_scale*FF2, B + global_scale*FF2 ,C2);
// Highlight in red the constrained faces
MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
for (unsigned i=0; i<b.size();++i)
C.row(b(i)) << 1, 0, 0;
viewer.data.set_colors(C);
}
if (key == '3')
{
// Deformed with frame field
viewer.data.set_mesh(V_deformed, F);
viewer.data.add_edges(B_deformed - global_scale*FF1_deformed, B_deformed + global_scale*FF1_deformed ,Eigen::RowVector3d(1,0,0));
viewer.data.add_edges(B_deformed - global_scale*FF2_deformed, B_deformed + global_scale*FF2_deformed ,Eigen::RowVector3d(0,0,1));
viewer.data.set_colors(RowVector3d(1,1,1));
}
if (key == '4')
{
// Deformed with cross field
viewer.data.set_mesh(V_deformed, F);
viewer.data.add_edges(B_deformed - global_scale*X1_deformed, B_deformed + global_scale*X1_deformed ,Eigen::RowVector3d(0,0,1));
viewer.data.add_edges(B_deformed - global_scale*X2_deformed, B_deformed + global_scale*X2_deformed ,Eigen::RowVector3d(0,0,1));
viewer.data.set_colors(RowVector3d(1,1,1));
}
if (key == '5')
{
// Deformed with quad texture
viewer.data.set_mesh(V_deformed, F);
viewer.data.set_uv(V_uv,F_uv);
viewer.data.set_colors(RowVector3d(1,1,1));
viewer.core.show_texture = true;
}
if (key == '6')
{
// Deformed with quad texture
viewer.data.set_mesh(V, F);
viewer.data.set_uv(V_uv,F_uv);
viewer.data.set_colors(RowVector3d(1,1,1));
viewer.core.show_texture = true;
}
// Replace the standard texture with an integer shift invariant texture
Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> texture_R, texture_G, texture_B;
line_texture(texture_R, texture_G, texture_B);
viewer.data.set_texture(texture_R, texture_B, texture_G);
viewer.core.align_camera_center(viewer.data.V,viewer.data.F);
return false;
}
// Squared Euclidean distance: Taking the square root is not necessary
// because EEMS uses the distances to find closest points. For example,
// pairwise_distance(X,Y).col(0).minCoeff( &closest )
// finds the row/point in X that is closest to the first row/point in Y
Eigen::MatrixXd euclidean_dist(const Eigen::MatrixXd &X, const Eigen::MatrixXd &Y) {
return( X.rowwise().squaredNorm().eval().replicate(1,Y.rows())
+ Y.rowwise().squaredNorm().eval().transpose().replicate(X.rows(),1)
- 2.0*X*Y.transpose() );
}