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;
}
void BaseOfSupport::buildCi(const Eigen::MatrixXd& Ab, const Eigen::MatrixXd& Cp) {
if (Ab.size() < 1)
OCRA_ERROR("Reference Ab hasn't been resized");
if (Cp.size() < 1)
OCRA_ERROR("Reference Cp hasn't been resized");
_Ci.setZero();
_Ci.block(10,10,4,6) = _Ab*_Cp;
OCRA_INFO("Built Ci");
}
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 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 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 FixedAxisOneDoFJoint::v2qdot(double* q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());
if (dv_to_qdot) {
dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
}
}
void FixedAxisOneDoFJoint::qdot2v(double* q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());
if (dqdot_to_v) {
dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
}
}
void QuaternionFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
v_to_qdot.resize(getNumPositions(), getNumVelocities());
auto quat = q.middleRows<QUAT_SIZE>(SPACE_DIMENSION);
Matrix3d R = quat2rotmat(quat);
Matrix<double, QUAT_SIZE, SPACE_DIMENSION> M;
if (dv_to_qdot) {
auto dR = dquat2rotmat(quat);
Gradient<decltype(M), QUAT_SIZE, 1>::type dM;
angularvel2quatdotMatrix(quat, M, &dM);
dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
setSubMatrixGradient<4>(*dv_to_qdot, dR, intRange<3>(0), intRange<3>(3), v_to_qdot.rows(), 3);
auto dMR = matGradMultMat(M, R, dM, dR);
setSubMatrixGradient<4>(*dv_to_qdot, dMR, intRange<4>(3), intRange<3>(0), v_to_qdot.rows(), 3);
}
else {
angularvel2quatdotMatrix(quat, M, (Gradient<decltype(M), QUAT_SIZE, 1>::type*) nullptr);
}
v_to_qdot.block<3, 3>(0, 0).setZero();
v_to_qdot.block<3, 3>(0, 3) = R;
v_to_qdot.block<4, 3>(3, 0).noalias() = M * R;
v_to_qdot.block<4, 3>(3, 3).setZero();
}
void RollPitchYawFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());
if (dv_to_qdot) {
dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
}
}
void RollPitchYawFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());
if (dqdot_to_v) {
dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
}
}
bool ClassifyMotion::load_model( const std::string& folder )
{
m_nb_classes = 8;
//------------------------------------------------------
cout << "Load Priors" << endl;
m_priors.resize( m_nb_classes );
for( int i=0; i<int(m_priors.size()); i++)
{
ostringstream filename;
filename << "Prior_1_" << i+1 << ".csv";
//cout << folder + filename.str() << endl;
Eigen::MatrixXd mat = load_from_csv( folder + filename.str() );
if( mat.size() == 0 )
return false;
m_priors[i] = mat.transpose();
}
//------------------------------------------------------
cout << "Load Mu" << endl;
m_mu.resize( m_nb_classes );
for( int i=0; i<int(m_mu.size()); i++)
{
ostringstream filename;
filename << "Mu_1_" << i+1 << ".csv";
//cout << folder + filename.str() << endl;
m_mu[i] = load_from_csv( folder + filename.str() );
if( m_mu[i].size() == 0 )
return false;
}
//------------------------------------------------------
cout << "Load Sigma" << endl;
m_sigma.resize( m_nb_classes );
m_nb_states = m_priors[0].size();
for( int i=0; i<int(m_sigma.size()); i++)
{
m_sigma[i].resize( m_nb_states );
for( int j=0; j<int(m_sigma[i].size()); j++)
{
ostringstream filename;
filename << "Sigma_1_" << j+1 << "_"<< i+1 << ".csv";
m_sigma[i][j] = load_from_csv( folder + filename.str() );
if( m_sigma[i][j].size() == 0 )
return false;
}
}
return true;
}
void PseudoInverse(const ::Eigen::MatrixXd& inputMatrix, ::Eigen::MatrixXd& outputMatrix, const ::Eigen::MatrixXd& weight)
{
Eigen::MatrixXd tmp;
if (weight.size() > 0)
tmp = inputMatrix.transpose() * weight.transpose() * weight * inputMatrix;
else
tmp = inputMatrix * inputMatrix.transpose();
tmp = tmp.inverse();
outputMatrix = inputMatrix.transpose() * tmp;
}
void Mesh::relative_to_mesh(Eigen::MatrixXd & C) const
{
if(C.size() == 0)
{
return;
}
C = C * rotation.matrix();
C /= scale;
C.col(0).array() -= shift(0);
C.col(1).array() -= shift(1);
C.col(2).array() -= shift(2);
}
void Mesh::unrelative_to_mesh(Eigen::MatrixXd & C) const
{
if(C.size() == 0)
{
return;
}
C.col(0).array() += shift(0);
C.col(1).array() += shift(1);
C.col(2).array() += shift(2);
C *= scale;
C = C * rotation.conjugate().matrix();
}
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::viewer::ViewerCore::align_camera_center(
const Eigen::MatrixXd& V)
{
if(V.rows() == 0)
return;
get_scale_and_shift_to_fit_mesh(V,model_zoom,model_translation);
// Rather than crash on empty mesh...
if(V.size() > 0)
{
object_scale = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
}
}
IGL_INLINE bool igl::writeDMAT(
const std::string file_name,
const Eigen::MatrixBase<DerivedW> & W,
const bool ascii)
{
FILE * fp = fopen(file_name.c_str(),"wb");
if(fp == NULL)
{
fprintf(stderr,"IOError: writeDMAT() could not open %s...",file_name.c_str());
return false;
}
if(ascii)
{
// first line contains number of rows and number of columns
fprintf(fp,"%d %d\n",(int)W.cols(),(int)W.rows());
// Loop over columns slowly
for(int j = 0;j < W.cols();j++)
{
// loop over rows (down columns) quickly
for(int i = 0;i < W.rows();i++)
{
fprintf(fp,"%0.17lg\n",(double)W(i,j));
}
}
}else
{
// write header for ascii
fprintf(fp,"0 0\n");
// first line contains number of rows and number of columns
fprintf(fp,"%d %d\n",(int)W.cols(),(int)W.rows());
// reader assumes the binary part is double precision
Eigen::MatrixXd Wd = W.template cast<double>();
fwrite(Wd.data(),sizeof(double),Wd.size(),fp);
//// Loop over columns slowly
//for(int j = 0;j < W.cols();j++)
//{
// // loop over rows (down columns) quickly
// for(int i = 0;i < W.rows();i++)
// {
// double d = (double)W(i,j);
// fwrite(&d,sizeof(double),1,fp);
// }
//}
}
fclose(fp);
return true;
}
IGL_INLINE void igl::ViewerData::set_edges(
const Eigen::MatrixXd& P,
const Eigen::MatrixXi& E,
const Eigen::MatrixXd& C)
{
using namespace Eigen;
lines.resize(E.rows(),9);
assert(C.cols() == 3);
for(int e = 0;e<E.rows();e++)
{
RowVector3d color;
if(C.size() == 3)
{
color<<C;
}else if(C.rows() == E.rows())
{
color<<C.row(e);
}
lines.row(e)<< P.row(E(e,0)), P.row(E(e,1)), color;
}
dirty |= DIRTY_OVERLAY_LINES;
}
void QuaternionFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
qdot_to_v.resize(getNumVelocities(), getNumPositions());
auto quat = q.middleRows<QUAT_SIZE>(SPACE_DIMENSION);
Matrix3d R = quat2rotmat(quat);
Vector4d quattilde;
Matrix<double, SPACE_DIMENSION, QUAT_SIZE> M;
Matrix<double, SPACE_DIMENSION, QUAT_SIZE> RTransposeM;
Gradient<Vector4d, QUAT_SIZE, 1>::type dquattildedquat;
if (dqdot_to_v) {
Gradient<Vector4d, QUAT_SIZE, 2>::type ddquattildedquat;
normalizeVec(quat, quattilde, &dquattildedquat, &ddquattildedquat);
auto dR = dquat2rotmat(quat);
Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type dM;
quatdot2angularvelMatrix(quat, M, &dM);
RTransposeM.noalias() = R.transpose() * M;
auto dRTranspose = transposeGrad(dR, R.rows());
auto dRTransposeM = matGradMultMat(R.transpose(), M, dRTranspose, dM);
auto dRTransposeMdquattildedquat = matGradMultMat(RTransposeM, dquattildedquat, dRTransposeM, ddquattildedquat);
dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
setSubMatrixGradient<4>(*dqdot_to_v, dRTranspose, intRange<3>(3), intRange<3>(0), qdot_to_v.rows(), 3);
setSubMatrixGradient<4>(*dqdot_to_v, dRTransposeMdquattildedquat, intRange<3>(0), intRange<4>(3), qdot_to_v.rows(), 3);
}
else {
normalizeVec(quat, quattilde, &dquattildedquat);
quatdot2angularvelMatrix(quat, M);
RTransposeM.noalias() = R.transpose() * M;
}
qdot_to_v.block<3, 3>(0, 0).setZero();
qdot_to_v.block<3, 4>(0, 3).noalias() = RTransposeM * dquattildedquat;
qdot_to_v.block<3, 3>(3, 0) = R.transpose();
qdot_to_v.block<3, 4>(3, 3).setZero();
}
IGL_INLINE void igl::draw_mesh(
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const Eigen::MatrixXd & N,
const Eigen::MatrixXi & NF,
const Eigen::MatrixXd & C,
const Eigen::MatrixXd & TC,
const Eigen::MatrixXi & TF,
const Eigen::MatrixXd & W,
const GLuint W_index,
const Eigen::MatrixXi & WI,
const GLuint WI_index)
{
using namespace std;
using namespace Eigen;
const int rF = F.rows();
const int cF = F.cols();
const int cC = C.cols();
const int rC = C.rows();
const int cW = W.cols();
const int rW = W.rows();
const int rV = V.rows();
const int rTC = TC.rows();
const int rTF = TF.rows();
const int rNF = NF.rows();
const int rN = N.rows();
if(F.size() > 0)
{
assert(F.maxCoeff() < V.rows());
assert(V.cols() == 3);
assert(rC == rV || rC == rF || rC == rF*3 || rC==1 || C.size() == 0);
assert(C.cols() == 3 || C.size() == 0);
assert(N.cols() == 3 || N.size() == 0);
assert(TC.cols() == 2 || TC.size() == 0);
assert(cF == 3 || cF == 4);
assert(TF.size() == 0 || TF.cols() == F.cols());
assert(NF.size() == 0 || NF.cols() == NF.cols());
}
if(W.size()>0)
{
assert(W.rows() == V.rows());
assert(WI.rows() == V.rows());
assert(W.cols() == WI.cols());
}
switch(F.cols())
{
default:
case 3:
glBegin(GL_TRIANGLES);
break;
case 4:
glBegin(GL_QUADS);
break;
}
// loop over faces
for(int i = 0; i<rF;i++)
{
// loop over corners of triangle
for(int j = 0;j<cF;j++)
{
int tc = -1;
if(rTF != 0)
{
tc = TF(i,j);
} else if(rTC == 1)
{
tc = 0;
}else if(rTC == rV)
{
tc = F(i,j);
}else if(rTC == rF*cF)
{
tc = i*cF + j;
}else if(rTC == rF)
{
tc = i;
}else
{
assert(TC.size() == 0);
}
// RGB(A)
Matrix<MatrixXd::Scalar,1,Dynamic> color;
if(rC == 1)
{
color = C.row(0);
}else if(rC == rV)
{
color = C.row(F(i,j));
}else if(rC == rF*cF)
{
color = C.row(i*cF+j);
}else if(rC == rF)
{
color = C.row(i);
}else
{
assert(C.size() == 0);
}
int n = -1;
if(rNF != 0)
{
n = NF(i,j); // indexed normals
} else if(rN == 1)
{
n = 0; // uniform normals
}else if(rN == rF)
{
n = i; // face normals
}else if(rN == rV)
{
n = F(i,j); // vertex normals
}else if(rN == rF*cF)
{
n = i*cF + j; // corner normals
}else
{
assert(N.size() == 0);
}
{
if(rW>0 && W_index !=0 && WI_index != 0)
{
int weights_left = cW;
while(weights_left != 0)
{
int pass_size = std::min(4,weights_left);
int weights_already_passed = cW-weights_left;
// Get attribute location of next 4 weights
int pass_W_index = W_index + weights_already_passed/4;
int pass_WI_index = WI_index + weights_already_passed/4;
switch(pass_size)
{
case 1:
glVertexAttrib1d(
pass_W_index,
W(F(i,j),0+weights_already_passed));
glVertexAttrib1d(
pass_WI_index,
WI(F(i,j),0+weights_already_passed));
break;
case 2:
glVertexAttrib2d(
pass_W_index,
W(F(i,j),0+weights_already_passed),
W(F(i,j),1+weights_already_passed));
glVertexAttrib2d(
pass_WI_index,
WI(F(i,j),0+weights_already_passed),
WI(F(i,j),1+weights_already_passed));
break;
case 3:
glVertexAttrib3d(
pass_W_index,
W(F(i,j),0+weights_already_passed),
W(F(i,j),1+weights_already_passed),
W(F(i,j),2+weights_already_passed));
glVertexAttrib3d(
pass_WI_index,
WI(F(i,j),0+weights_already_passed),
WI(F(i,j),1+weights_already_passed),
WI(F(i,j),2+weights_already_passed));
break;
default:
glVertexAttrib4d(
pass_W_index,
W(F(i,j),0+weights_already_passed),
W(F(i,j),1+weights_already_passed),
W(F(i,j),2+weights_already_passed),
W(F(i,j),3+weights_already_passed));
glVertexAttrib4d(
pass_WI_index,
WI(F(i,j),0+weights_already_passed),
WI(F(i,j),1+weights_already_passed),
WI(F(i,j),2+weights_already_passed),
WI(F(i,j),3+weights_already_passed));
break;
}
weights_left -= pass_size;
}
}
if(tc != -1)
{
glTexCoord2d(TC(tc,0),TC(tc,1));
}
if(rC>0)
{
switch(cC)
{
case 3:
glColor3dv(color.data());
break;
case 4:
glColor4dv(color.data());
break;
default:
break;
}
}
if(n != -1)
{
glNormal3d(N(n,0),N(n,1),N(n,2));
}
glVertex3d(V(F(i,j),0),V(F(i,j),1),V(F(i,j),2));
}
}
}
glEnd();
}
IGL_INLINE bool igl::boundary_conditions(
const Eigen::MatrixXd & V ,
const Eigen::MatrixXi & /*Ele*/,
const Eigen::MatrixXd & C ,
const Eigen::VectorXi & P ,
const Eigen::MatrixXi & BE ,
const Eigen::MatrixXi & CE ,
Eigen::VectorXi & b ,
Eigen::MatrixXd & bc )
{
using namespace Eigen;
using namespace igl;
using namespace std;
if(P.size()+BE.rows() == 0)
{
verbose("^%s: Error: no handles found\n",__FUNCTION__);
return false;
}
vector<int> bci;
vector<int> bcj;
vector<int> bcv;
// loop over points
for(int p = 0;p<P.size();p++)
{
VectorXd pos = C.row(P(p));
// loop over domain vertices
for(int i = 0;i<V.rows();i++)
{
// Find samples just on pos
//Vec3 vi(V(i,0),V(i,1),V(i,2));
// EIGEN GOTCHA:
// double sqrd = (V.row(i)-pos).array().pow(2).sum();
// Must first store in temporary
VectorXd vi = V.row(i);
double sqrd = (vi-pos).squaredNorm();
if(sqrd <= FLOAT_EPS)
{
//cout<<"sum((["<<
// V(i,0)<<" "<<
// V(i,1)<<" "<<
// V(i,2)<<"] - ["<<
// pos(0)<<" "<<
// pos(1)<<" "<<
// pos(2)<<"]).^2) = "<<sqrd<<endl;
bci.push_back(i);
bcj.push_back(p);
bcv.push_back(1.0);
}
}
}
// loop over bone edges
for(int e = 0;e<BE.rows();e++)
{
// loop over domain vertices
for(int i = 0;i<V.rows();i++)
{
// Find samples from tip up to tail
VectorXd tip = C.row(BE(e,0));
VectorXd tail = C.row(BE(e,1));
// Compute parameter along bone and squared distance
double t,sqrd;
project_to_line(
V(i,0),V(i,1),V(i,2),
tip(0),tip(1),tip(2),
tail(0),tail(1),tail(2),
t,sqrd);
if(t>=-FLOAT_EPS && t<=(1.0f+FLOAT_EPS) && sqrd<=FLOAT_EPS)
{
bci.push_back(i);
bcj.push_back(P.size()+e);
bcv.push_back(1.0);
}
}
}
// Cage edges are not considered yet
// loop over cage edges
for(int e = 0;e<CE.rows();e++)
{
// loop over domain vertices
for(int i = 0;i<V.rows();i++)
{
// Find samples from tip up to tail
VectorXd tip = C.row(P(CE(e,0)));
VectorXd tail = C.row(P(CE(e,1)));
// Compute parameter along bone and squared distance
double t,sqrd;
project_to_line(
V(i,0),V(i,1),V(i,2),
tip(0),tip(1),tip(2),
tail(0),tail(1),tail(2),
t,sqrd);
if(t>=-FLOAT_EPS && t<=(1.0f+FLOAT_EPS) && sqrd<=FLOAT_EPS)
{
bci.push_back(i);
bcj.push_back(CE(e,0));
bcv.push_back(1.0-t);
bci.push_back(i);
bcj.push_back(CE(e,1));
bcv.push_back(t);
}
}
}
// find unique boundary indices
vector<int> vb = bci;
sort(vb.begin(),vb.end());
vb.erase(unique(vb.begin(), vb.end()), vb.end());
b.resize(vb.size());
bc = MatrixXd::Zero(vb.size(),P.size()+BE.rows());
// Map from boundary index to index in boundary
map<int,int> bim;
int i = 0;
// Also fill in b
for(vector<int>::iterator bit = vb.begin();bit != vb.end();bit++)
{
b(i) = *bit;
bim[*bit] = i;
i++;
}
// Build BC
for(i = 0;i < (int)bci.size();i++)
{
assert(bim.find(bci[i]) != bim.end());
bc(bim[bci[i]],bcj[i]) = bcv[i];
}
// Normalize accross rows so that conditions sum to one
for(i = 0;i<bc.rows();i++)
{
double sum = bc.row(i).sum();
assert(sum != 0);
bc.row(i).array() /= sum;
}
if(bc.size() == 0)
{
verbose("^%s: Error: boundary conditions are empty.\n",__FUNCTION__);
return false;
}
// If there's only a single boundary condition, the following tests
// are overzealous.
if(bc.rows() == 1)
{
return true;
}
// Check that every Weight function has at least one boundary value of 1 and
// one value of 0
for(i = 0;i<bc.cols();i++)
{
double min_abs_c = bc.col(i).array().abs().minCoeff();
double max_c = bc.col(i).maxCoeff();
if(min_abs_c > FLOAT_EPS)
{
verbose("^%s: Error: handle %d does not receive 0 weight\n",__FUNCTION__,i);
return false;
}
if(max_c< (1-FLOAT_EPS))
{
verbose("^%s: Error: handle %d does not receive 1 weight\n",__FUNCTION__,i);
return false;
}
}
return true;
}
void parse_rhs(
const int nrhs,
const mxArray *prhs[],
Eigen::MatrixXd & IV,
Eigen::MatrixXi & IF,
double & level,
double & angle_bound,
double & radius_bound,
double & distance_bound,
igl::SignedDistanceType & type)
{
using namespace std;
using namespace igl;
using namespace Eigen;
mexErrMsgTxt(nrhs >= 2, "The number of input arguments must be >=2.");
const int dim = mxGetN(prhs[0]);
mexErrMsgTxt(dim == 3,
"Mesh vertex list must be #V by 3 list of vertex positions");
parse_rhs_double(prhs,IV);
parse_rhs_index(prhs+1,IF);
// defaults
level = 0.0;
angle_bound = 28.0;
double bbd = 1.0;
// Radius and distance in terms of fraction of bbd
if(IV.size() > 0)
{
bbd = (IV.colwise().maxCoeff()-IV.colwise().minCoeff()).norm();
}
radius_bound = 0.02*bbd;
distance_bound = 0.02*bbd;
type = SIGNED_DISTANCE_TYPE_DEFAULT;
{
int i = 2;
while(i<nrhs)
{
mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
// Cast to char
const char * name = mxArrayToString(prhs[i]);
const auto requires_arg =
[](const int i, const int nrhs, const char * name)
{
mexErrMsgTxt((i+1)<nrhs,
C_STR("Parameter '"<<name<<"' requires argument"));
};
const auto validate_double =
[](const int i, const mxArray * prhs[], const char * name)
{
mexErrMsgTxt(mxIsDouble(prhs[i]),
C_STR("Parameter '"<<name<<"' requires Double argument"));
};
const auto validate_scalar =
[](const int i, const mxArray * prhs[], const char * name)
{
mexErrMsgTxt(mxGetN(prhs[i])==1 && mxGetM(prhs[i])==1,
C_STR("Parameter '"<<name<<"' requires scalar argument"));
};
const auto validate_char =
[](const int i, const mxArray * prhs[], const char * name)
{
mexErrMsgTxt(mxIsChar(prhs[i]),
C_STR("Parameter '"<<name<<"' requires char argument"));
};
if(strcmp("Level",name) == 0)
{
requires_arg(i,nrhs,name);
i++;
validate_double(i,prhs,name);
validate_scalar(i,prhs,name);
level = (double)*mxGetPr(prhs[i]);
}else if(strcmp("AngleBound",name) == 0)
{
requires_arg(i,nrhs,name);
i++;
validate_double(i,prhs,name);
validate_scalar(i,prhs,name);
angle_bound = (double)*mxGetPr(prhs[i]);
}else if(strcmp("RadiusBound",name) == 0)
{
requires_arg(i,nrhs,name);
i++;
validate_double(i,prhs,name);
validate_scalar(i,prhs,name);
radius_bound = ((double)*mxGetPr(prhs[i])) * bbd;
}else if(strcmp("DistanceBound",name) == 0)
{
requires_arg(i,nrhs,name);
i++;
validate_double(i,prhs,name);
validate_scalar(i,prhs,name);
distance_bound = ((double)*mxGetPr(prhs[i])) * bbd;
}else if(strcmp("SignedDistanceType",name) == 0)
{
requires_arg(i,nrhs,name);
i++;
validate_char(i,prhs,name);
const char * type_name = mxArrayToString(prhs[i]);
if(strcmp("pseudonormal",type_name)==0)
{
type = igl::SIGNED_DISTANCE_TYPE_PSEUDONORMAL;
}else if(strcmp("winding_number",type_name)==0)
{
type = igl::SIGNED_DISTANCE_TYPE_WINDING_NUMBER;
}else if(strcmp("default",type_name)==0)
{
type = igl::SIGNED_DISTANCE_TYPE_DEFAULT;
}else if(strcmp("unsigned",type_name)==0)
{
type = igl::SIGNED_DISTANCE_TYPE_UNSIGNED;
}else
{
mexErrMsgTxt(false,C_STR("Unknown SignedDistanceType: "<<type_name));
}
}else
{
mexErrMsgTxt(false,"Unknown parameter");
}
i++;
}
}
if(type != igl::SIGNED_DISTANCE_TYPE_UNSIGNED)
{
mexErrMsgTxt(dim == mxGetN(prhs[1]),
"Mesh \"face\" simplex size must equal dimension");
}
}
void CloudAnalyzer2D::examineFreeSpaceEvidence() {
freeSpaceEvidence.clear();
Eigen::Vector3f cameraCenter = -1.0 * pointMin;
voxel::DirectVoxel<char> freeSpace(numX, numY, numZ);
for (int k = 0; k < numZ; ++k) {
for (int j = 0; j < numY; ++j) {
for (int i = 0; i < numX; ++i) {
if (!pointInVoxel->at(i, j, k))
continue;
Eigen::Vector3d ray(i, j, k);
ray[0] -= cameraCenter[0] * FLAGS_scale;
ray[1] -= cameraCenter[1] * FLAGS_scale;
ray[2] -= cameraCenter[2] * zScale;
double length = ray.norm();
Eigen::Vector3d unitRay = ray / length;
Eigen::Vector3i voxelHit;
for (int a = 0; a <= ceil(length); ++a) {
voxelHit[0] = floor(cameraCenter[0] * FLAGS_scale + a * unitRay[0]);
voxelHit[1] = floor(cameraCenter[1] * FLAGS_scale + a * unitRay[1]);
voxelHit[2] = floor(cameraCenter[2] * zScale + a * unitRay[2]);
if (voxelHit[0] < 0 || voxelHit[0] >= numX)
continue;
if (voxelHit[1] < 0 || voxelHit[1] >= numY)
continue;
if (voxelHit[2] < 0 || voxelHit[2] >= numZ)
continue;
freeSpace(voxelHit) = 1;
}
}
}
}
for (int r = 0; r < R->size(); ++r) {
Eigen::MatrixXd collapsedCount = Eigen::MatrixXd::Zero(newRows, newCols);
#pragma omp parallel for schedule(static)
for (int i = 0; i < collapsedCount.cols(); ++i) {
for (int j = 0; j < collapsedCount.rows(); ++j) {
for (int k = 0; k < numZ; ++k) {
const Eigen::Vector3d coord(i, j, k);
const Eigen::Vector3i src =
(R->at(r) * (coord - newZZ) + zeroZero)
.unaryExpr([](auto v) { return std::round(v); })
.cast<int>();
if (src[0] < 0 || src[0] >= numX || src[1] < 0 || src[1] >= numY ||
src[2] < 0 || src[2] >= numZ)
continue;
if (freeSpace(src))
++collapsedCount(j, i);
}
}
}
double average, sigma;
const double *vPtr = collapsedCount.data();
std::tie(average, sigma) = place::aveAndStdev(
vPtr, vPtr + collapsedCount.size(), [](double v) { return v; },
[](double v) -> bool { return v; });
cv::Mat heatMap(newRows, newCols, CV_8UC1, cv::Scalar::all(255));
for (int j = 0; j < heatMap.rows; ++j) {
uchar *dst = heatMap.ptr<uchar>(j);
for (int i = 0; i < heatMap.cols; ++i) {
const double count = collapsedCount(j, i);
if (count > 0) {
const int gray = cv::saturate_cast<uchar>(
255.0 * ((count - average) / sigma + 1.0));
dst[i] = 255 - gray;
}
}
}
const double radius = 0.3;
for (int j = -sqrt(radius) * FLAGS_scale; j < sqrt(radius) * FLAGS_scale;
++j) {
uchar *dst = heatMap.ptr<uchar>(j + imageZeroZero[1]);
for (int i = -sqrt(radius * FLAGS_scale * FLAGS_scale - j * j);
i < sqrt(radius * FLAGS_scale * FLAGS_scale - j * j); ++i) {
dst[i + imageZeroZero[0]] = 0;
}
}
if (FLAGS_preview) {
cvNamedWindow("Preview", CV_WINDOW_NORMAL);
cv::imshow("Preview", heatMap);
cv::waitKey(0);
}
freeSpaceEvidence.push_back(heatMap);
}
}
