void CDifodo::getPointsCoord(MatrixXf &x, MatrixXf &y, MatrixXf &z)
{
x.resize(rows,cols);
y.resize(rows,cols);
z.resize(rows,cols);
z = depth_inter[0];
x = xx_inter[0];
y = yy_inter[0];
}
void CDifodo::getDepthDerivatives(MatrixXf &cur_du, MatrixXf &cur_dv, MatrixXf &cur_dt)
{
cur_du.resize(rows,cols);
cur_dv.resize(rows,cols);
cur_dt.resize(rows,cols);
cur_du = du;
cur_dv = dv;
cur_dt = dt;
}
void computeRangeData( osg::Camera* camera , osg::ref_ptr<osg::Image> depthbufferimg, MatrixXf& Ximg, MatrixXf& Yimg, MatrixXf& Zimg) {
// from https://svn.lcsr.jhu.edu/cisst/trunk/saw/components/sawOpenSceneGraph/code/osaOSGCamera.cpp
// This is used by glutUnProject
const osg::Viewport* viewport = camera->getViewport();
size_t width = viewport->width();
size_t height = viewport->height();
GLint view[4];
view[0] = (int)viewport->x();
view[1] = (int)viewport->y();
view[2] = width;
view[3] = height;
// Create a 3xN range data destination matrix.
// [ x1 ... xN ]
// [ y1 ... yN ]
// [ z1 ... zN ]
// VCT_COL_MAJOR is used because we will write in the following order
// x1, y1, z1, x2, y2, z2, ..., xN, yN, zZ
Ximg.resize(height,width);
Yimg.resize(height,width);
Zimg.resize(height,width);
// get the intrinsic parameters of the camera
double fovy, aspectRatio, Zn, Zf;
camera->getProjectionMatrixAsPerspective( fovy, aspectRatio, Zn, Zf );
osg::Matrixd modelm = camera->getViewMatrix();
osg::Matrixd projm = camera->getProjectionMatrix();
//for( int y=0; y<height; y++ ){
for( int y=height-1; 0<=y; y-- ){
for( size_t x=0; x<width; x++ ){
GLdouble X, Y, Z;
float* d = (float*)depthbufferimg->data( x, y );
gluUnProject( x, y, *d, modelm.ptr(), projm.ptr(), view, &X, &Y, &Z );
//gluUnProject( x, y, *d, &model[0][0], &proj[0][0], view, &X, &Y, &Z );
// rangedata is 4xN column major
Ximg(y,x) = X;
Yimg(y,x) = Y;
Zimg(y,x) = Z;
}
}
}
void detectSiftMatchWithOpenCV(const char* img1_path, const char* img2_path, MatrixXf &match) {
Mat img1 = imread(img1_path);
Mat img2 = imread(img2_path);
SiftFeatureDetector detector;
SiftDescriptorExtractor extractor;
vector<KeyPoint> key1;
vector<KeyPoint> key2;
Mat desc1, desc2;
detector.detect(img1, key1);
detector.detect(img2, key2);
extractor.compute(img1, key1, desc1);
extractor.compute(img2, key2, desc2);
FlannBasedMatcher matcher;
vector<DMatch> matches;
matcher.match(desc1, desc2, matches);
match.resize(matches.size(), 6);
cout << "match count: " << matches.size() << endl;
for (int i = 0; i < matches.size(); i++) {
match(i, 0) = key1[matches[i].queryIdx].pt.x;
match(i, 1) = key1[matches[i].queryIdx].pt.y;
match(i, 2) = 1;
match(i, 3) = key2[matches[i].trainIdx].pt.x;
match(i, 4) = key2[matches[i].trainIdx].pt.y;
match(i, 5) = 1;
}
}
void sumAndNormalize( MatrixXf & out, const MatrixXf & in, const MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
VectorXf b = in.col(i);
VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
void toHomogeneous(MatrixXf &mat) {
MatrixXf temp;
if (mat.cols() == 2) {
temp.resize(mat.rows(), 3);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
mat = temp;
} else if (mat.cols() == 4) {
temp.resize(mat.rows(), 6);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
temp.block(0, 3, mat.rows(), 2) = temp.block(0, 2, mat.rows(), 2);
temp.col(5).setConstant(1);
mat = temp;
} else
cout << "toHomogeneous with wrong dimension" << endl;
}
static inline MatrixXf toMatrix(const std::vector<Vector3f> &data) {
MatrixXf result;
result.resize(3, data.size());
for (size_t i = 0; i < data.size(); ++i) {
result.col(i) = data[i];
}
return std::move(result);
}
///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( MatrixXf & out, const MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
bool Surface::read(const QString &p_sFileName, Surface &p_Surface)
{
p_Surface.clear();
printf("Reading surface...\n");
QFile t_File(p_sFileName);
if (!t_File.open(QIODevice::ReadOnly))
{
printf("\tError: Couldn't open the file\n");
return false;
}
QDataStream t_DataStream(&t_File);
t_DataStream.setByteOrder(QDataStream::BigEndian);
//
// Magic numbers to identify QUAD and TRIANGLE files
//
// QUAD_FILE_MAGIC_NUMBER = (-1 & 0x00ffffff) ;
// NEW_QUAD_FILE_MAGIC_NUMBER = (-3 & 0x00ffffff) ;
//
qint32 NEW_QUAD_FILE_MAGIC_NUMBER = 16777213;
qint32 TRIANGLE_FILE_MAGIC_NUMBER = 16777214;
qint32 QUAD_FILE_MAGIC_NUMBER = 16777215;
qint32 magic = IOUtils::fread3(t_DataStream);
qint32 nvert, nface;
MatrixXf verts;
MatrixXi faces;
if(magic == QUAD_FILE_MAGIC_NUMBER || magic == NEW_QUAD_FILE_MAGIC_NUMBER)
{
qint32 nvert = IOUtils::fread3(t_DataStream);
qint32 nquad = IOUtils::fread3(t_DataStream);
if(magic == QUAD_FILE_MAGIC_NUMBER)
printf("\t%s is a quad file (nvert = %d nquad = %d)\n", p_sFileName.toLatin1().constData(),nvert,nquad);
else
printf("\t%s is a new quad file (nvert = %d nquad = %d)\n", p_sFileName.toLatin1().constData(),nvert,nquad);
//vertices
verts.resize(nvert, 3);
if(magic == QUAD_FILE_MAGIC_NUMBER)
{
qint16 iVal;
for(qint32 i = 0; i < nvert; ++i)
{
for(qint32 j = 0; j < 3; ++j)
{
t_DataStream >> iVal;
IOUtils::swap_short(iVal);
verts(i,j) = ((float)iVal) / 100;
}
}
}
void noHomogeneous(MatrixXf &mat) {
MatrixXf temp;
if (mat.cols() == 3) {
temp.resize(mat.rows(), 2);
temp.col(0).array() = mat.col(0).array()/mat.col(2).array();
temp.col(1).array() = mat.col(1).array()/mat.col(2).array();
mat = temp;
} else
cout << "toHomogeneous with wrong dimension" << endl;
}
bool singleModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int maxInlier = -1;
MatrixXf bestResidue;
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount ++;
RandomSampling(psize, dataSize, sample);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
Matrix3f local_H;
MatrixXf local_A;
fitHomography(pts1, pts2, local_H, local_A);
MatrixXf residue;
computeHomographyResidue(x1, x2, local_H, residue);
int inlierCount = (residue.array() < THRESHOLD).count();
if (inlierCount > maxInlier) {
maxInlier = inlierCount;
bestResidue = residue;
}
}
inlier.resize(maxInlier, data.cols());
int transferCounter = 0;
for (int i = 0; i < dataSize; i++) {
if (bestResidue(i) < THRESHOLD) {
inlier.row(transferCounter) = data.row(i);
transferCounter++;
}
}
if (transferCounter != maxInlier) {
cout << "RANSAC result size does not match!!!!" << endl;
return false;
}
return true;
}
virtual VectorXf parameters() const {
if (ktype_ == CONST_KERNEL)
return VectorXf();
else if (ktype_ == DIAG_KERNEL)
return parameters_;
else {
MatrixXf p = parameters_;
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
MatrixXf SphereHelpers::toMatrix_3xN(std::vector<Vector3f> points)
{
MatrixXf m;
m.resize(3, points.size());
for (size_t i = 0; i<points.size(); i++)
{
m.block<3, 1>(0, i) = points.at(i);
}
return m;
}
virtual void setParameters( const VectorXf & p ) {
if (ktype_ == DIAG_KERNEL) {
parameters_ = p;
initLattice( p.asDiagonal() * f_ );
}
else if (ktype_ == FULL_KERNEL) {
MatrixXf tmp = p;
tmp.resize( parameters_.rows(), parameters_.cols() );
parameters_ = tmp;
initLattice( tmp * f_ );
}
}
virtual VectorXf gradient( const MatrixXf & a, const MatrixXf & b ) const {
if (ktype_ == CONST_KERNEL)
return VectorXf();
MatrixXf fg = featureGradient( a, b );
if (ktype_ == DIAG_KERNEL)
return (f_.array()*fg.array()).rowwise().sum();
else {
MatrixXf p = fg*f_.transpose();
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
void computeStats(PointWithNormalStatistcsGenerator & generator, const Matrix3f& cameraMatrix){
zBuffer.resize(depthImage.rows(), depthImage.cols());
gaussians.fromDepthImage(depthImage,cameraMatrix);
gaussians.toPointWithNormalVector(points);
indexImage.resize(depthImage.rows(), depthImage.cols());
gaussians.toIndexImage(indexImage, zBuffer, cameraMatrix, Eigen::Isometry3f::Identity(), 10);
cerr << "points: " << points.size() << endl;
svds.resize(points.size());
double tNormalStart = get_time();
generator.computeNormalsAndSVD(points, svds, indexImage, cameraMatrix);
double tNormalEnd = get_time();
cerr << "Normal Extraction took " << tNormalEnd - tNormalStart << " sec." << endl;
}
void DenseCRF::stepInference( MatrixXf & Q, MatrixXf & tmp1, MatrixXf & tmp2 ) const{
tmp1.resize( Q.rows(), Q.cols() );
tmp1.fill(0);
if( unary_ )
tmp1 -= unary_->get();
// Add up all pairwise potentials
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp2, Q );
tmp1 -= tmp2;
}
// Exponentiate and normalize
expAndNormalize( Q, tmp1 );
}
void fitHomography(MatrixXf pts1, MatrixXf pts2, Matrix3f &H, MatrixXf &A) {
int psize = pts1.rows();
A.resize(psize*2, 9);
for (auto i = 0; i < psize; i++) {
Vector3f p1 = pts1.row(i);
Vector3f p2 = pts2.row(i);
A.row(i*2) << 0, 0, 0, -p1[0], -p1[1], -p1[2], p2[1]*p1[0], p2[1]*p1[1], p2[1]*p1[2];
A.row(i*2+1) << p1[0], p1[1], p1[2], 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1], -p2[0]*p1[2];
}
JacobiSVD<MatrixXf, HouseholderQRPreconditioner> svd(A, ComputeFullV);
MatrixXf V = svd.matrixV();
VectorXf h = V.col(V.cols()-1);
H = rollVector9f(h);
}
void computeHomographyResidue(MatrixXf pts1, MatrixXf pts2, const Matrix3f &H, MatrixXf &residue){
// cross residue
filterPointAtInfinity(pts1, pts2);
residue.resize(pts1.rows(), 1);
MatrixXf Hx1 = (H*pts1.transpose()).transpose();
MatrixXf invHx2 = (H.inverse()*pts2.transpose()).transpose();
noHomogeneous(Hx1);
noHomogeneous(invHx2);
noHomogeneous(pts1);
noHomogeneous(pts2);
MatrixXf diffHx1pts2 = Hx1 - pts2;
MatrixXf diffinvHx2pts1 = invHx2 - pts1;
residue = diffHx1pts2.rowwise().squaredNorm() + diffinvHx2pts1.rowwise().squaredNorm();
}
bool TrivalLayouter::compute( const QList<SymbolNode::Ptr>& childList, MatrixXf& pos2D, VectorXf& radiusVec, float& radius )
{
if (childList.size() == 0)
return false;
VectorXf& rVec = radiusVec;
LayoutSetting::getChildrenRadius(childList, rVec);
pos2D.resize(childList.size(), 2);
if (childList.size() == 1)
{
pos2D(0,0) = pos2D(0,1) = 0;
radius = rVec[0];
return true;
}
const float maxChildR = rVec.maxCoeff();
const float twoPi = 2.f * 3.14159265f;
const float c = rVec.sum() * 2.f;
const float r = c / twoPi + 1e-3;
const float f = 1.3f;
float angle = 0;
for (int ithChild = 0; ithChild < childList.size(); ++ithChild)
{
const SymbolNode::Ptr& node = childList[ithChild];
float l = rVec[ithChild] * 2.f;
float halfAng = 0.5 * l / r;
angle += halfAng;
pos2D(ithChild, 0) = r*f * cos(angle);
pos2D(ithChild, 1) = r*f * sin(angle);
angle += halfAng;
}
radius = (r * f + maxChildR);
return true;
}
void CDifodo::getWeights(MatrixXf &w)
{
w.resize(rows,cols);
w = weights;
}
int EMclustering::EM(int k, int *IDX, bool spatial, bool att)
{
clusternum = k;
MatrixXf x;
/*if(spatial)
{
if(att)
{
x.resize(4,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataPos[i][0];
x(1,i) = dataPos[i][1];
x(2,i) = dataPos[i][2];
x(3,i) = dataDen[i];
}
}
else
{
x.resize(6,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataPos[i][0];
x(1,i) = dataPos[i][1];
x(2,i) = dataPos[i][2];
x(3,i) = dataVel[i][0];
x(4,i) = dataVel[i][1];
x(5,i) = dataVel[i][2];
}
}
}
else
{*/
if(att)
{
x.resize(1,dataDen.size());
for(int i=0;i<dataDen.size();i++)
{
x(0,i) = dataDen[i];
}
//cerr<<x;
//cerr<<endl;
if(k>dataDen.size())
return -1;
}
else
{
x.resize(3,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataVel[i][0];//fabs(cos(-PI/4)*dataVel[i][0] - sin(-PI/4)*dataVel[i][1]);
x(1,i) = dataVel[i][1];//fabs(sin(-PI/4)*dataVel[i][0] + cos(-PI/4)*dataVel[i][1]);
x(2,i) = dataVel[i][2];
}
if(k>dataSize)
return -1;
}
//}
//cout<<"EM for Gaussian mixture: running ... "<<endl;
//cerr<<x<<endl;
MatrixXf r =initialization(x,k);// kmeans(x,k);//
//cerr<<"Initialization is Done"<<endl;//cerr<<r<<endl;
VectorXi label(r.rows());
for(int i=0;i<r.rows();i++)
{
int index;
float tmp1 = r.row(i).maxCoeff(&index);
label(i) = index;
}//cerr<<label<<endl;
VectorXi tmpp(label.size());
VectorXi tmp2 = unique(label,tmpp);
int tmpd = tmp2.size(); //cerr<<tmpd<<endl;
MatrixXf tmpr(r.rows(),tmpd);
for(int i=0;i<tmpd;i++)
{
tmpr.col(i) = r.col(tmp2(i));
}//cerr<<"done1"<<endl;
r.resize(r.rows(),tmpd);
r = tmpr;//cerr<<r.cols()<<endl;
float tol = 1e-10;
int max = 300;
double llh = -9e+9;
bool converged = false;
int t = 1;
//cerr<<"done1"<<endl;
//gaussian_model model;
int clusternum_error;
MatrixXf tmpmodel;
while(!converged&&t<max)
{
t = t + 1;
gaussian_model model = maximization(x,r);//cerr<<t<<" "<<"max"<<endl;
float tmpllh = llh;
r = expectation(x,model,llh);//cerr<<t<<" "<<"exp"<<endl;
for(int i=0;i<r.rows();i++)
{
int index;
float tmp1 = r.row(i).maxCoeff(&index);
label(i) = index;
}
VectorXi u = unique(label,tmpp);//cerr<<t<<" "<<u.size()<<" "<<r.cols()<<" "<<r.rows()<<endl;
clusternum_error = clusternum - u.size();
if(r.cols()!=u.size())
{
/* tmpr.resize(r.rows(),u.size());
for(int i=0;i<u.size();i++)
{
tmpr.col(i) = r.col(u(i));
}
r.resize(r.rows(),u.size());
r = tmpr;//cerr<<"r"<<endl;*/
}
else
{
if((llh - tmpllh)<tol*abs(llh))
converged = true;
else
converged = false;
}
//cerr<<"t"<<t<<endl;
//return_model = model;
tmpmodel.resize(model.mu.rows(),model.mu.cols());
//return_model = model.mu;
tmpmodel = model.mu;
u.resize(0);
//cerr<<tmpmodel<<endl;
}
/*ofstream off2("rr");
off2<<r.row(0)<<endl;
for(int i=1;i<r.rows();i++)
if(r.row(i)!=r.row(i-1))
{off2<<x.col(i)<<" ";
off2<<r.row(i)<<endl;}
off2.close();*/
cerr<<clusternum_error<<endl;
return_model = tmpmodel;
//cerr<<label<<endl;
if (converged)
cerr<<"Converged in "<<t-1<<endl;
else
cerr<<max<<endl;
//cerr<<t-1<<endl;
for(int i=0;i<label.size();i++)
{
IDX[i] = label(i);
//cerr<<IDX[i]<<" ";
}//cerr<<endl;
//cerr<<label.size()<<endl;
x.resize(0,0);
r.resize(0,0);
tmpr.resize(0,0);
tmpmodel.resize(0,0);
label.resize(0);
tmpp.resize(0);
tmp2.resize(0);
return clusternum_error;
}
bool ComponentLayouter::compute( const SparseMatrix* vtxEdgeMatrix,
const VectorXd* edgeWeight,
const QList<SymbolNode::Ptr>& childList,
MatrixXf& childPos,
VectorXf& childRadius,
float& totalRadius)
{
CHECK_ERRORS_RETURN_BOOL(m_status);
// check edge data
int nVtx1 = childList.size();
bool hasEdgeData = (vtxEdgeMatrix != NULL && edgeWeight != NULL );
if (hasEdgeData)
{
int nVtx0 = vtxEdgeMatrix->rows();
int nEdge0= vtxEdgeMatrix->cols();
int nEdge1 = edgeWeight->size();
if (!(nVtx0 == nVtx1 && nEdge0 == nEdge1 && nVtx0 > 0 && nEdge0 > 0))
m_status |= WARNING_INVALID_EDGE_DATA;
}
else
m_status |= WARNING_NO_EDGE;
bool isVtxEdgeMatValid = (m_status & (WARNING_NO_EDGE | WARNING_INVALID_EDGE_DATA)) == 0;
// fill child radius
LayoutSetting::getChildrenRadius(childList, childRadius);
QVector<Component> compInfo;
VectorXi vtxCompIdx, vtxIdx, edgeCompIdx, edgeIdx;
QList<SparseMatrix> compVEMat;
int nComp = 0;
if (isVtxEdgeMatValid)
{
GraphUtility::splitConnectedComponents(*vtxEdgeMatrix, vtxCompIdx, vtxIdx, edgeCompIdx, edgeIdx, compVEMat);
nComp = compVEMat.size();
}
else
{
vtxCompIdx.resize(nVtx1);
vtxIdx.resize(nVtx1);
for (int ithVtx = 0; ithVtx < nVtx1; ++ithVtx)
{
vtxCompIdx[ithVtx] = ithVtx;
vtxIdx[ithVtx] = 0;
compVEMat.push_back(SparseMatrix());
}
nComp = nVtx1;
}
// fill component information array
compInfo.resize(nComp);
for (int ithComp = 0; ithComp < nComp; ++ithComp)
{
Component& comp = compInfo[ithComp];
comp.m_vtxRadius.resize(childRadius.size());
comp.m_hashID.resize(childRadius.size());
comp.m_pVEIncidenceMat = &compVEMat[ithComp];
comp.m_edgeWeight.resize(compVEMat[ithComp].cols());
}
for (int ithVtx = 0; ithVtx < vtxCompIdx.size(); ++ithVtx)
{
Component& comp = compInfo[vtxCompIdx[ithVtx]];
comp.m_vtxRadius[vtxIdx[ithVtx]] = childRadius[ithVtx];
unsigned vtxHash = childList[ithVtx]->getSymInfo().hash();
comp.m_hashID[vtxIdx[ithVtx]] = vtxHash;
comp.m_compHash = comp.m_compHash ^ vtxHash;
}
if (edgeWeight)
{
for (int ithEdge = 0; ithEdge < edgeCompIdx.size(); ++ithEdge)
{
compInfo[edgeCompIdx[ithEdge]].m_edgeWeight[edgeIdx[ithEdge]] = (*edgeWeight)[ithEdge];
}
}
// layout within each components
if (isVtxEdgeMatValid)
layoutByGraph(compInfo);
else
{
// set component result directly
for (int ithComp = 0; ithComp < nComp; ++ithComp)
{
Component& comp = compInfo[ithComp];
float r = childRadius[ithComp];
comp.m_vtxRadius.setConstant(1, r);
comp.m_radius = r;
comp.m_localPos2D.setZero(1,2);
}
}
// layout by word, first fill word attributes
for (int ithChild = 0; ithChild < childList.size(); ++ithChild)
{
const SymbolNode::Ptr& child = childList[ithChild];
if (SymbolWordAttr::Ptr wordAttr = child->getAttr<SymbolWordAttr>())
{
int ithComp = vtxCompIdx[ithChild];
Component& comp = compInfo[ithComp];
comp.m_wordAttr.unite(*wordAttr);
}
}
float finalRadius = 1.f;
layoutByWord(compInfo, finalRadius);
// set children's location
childPos.resize(childList.size(), 2);
childRadius.resize(childList.size());
for (int ithChild = 0; ithChild < childList.size(); ++ithChild)
{
const SymbolNode::Ptr& child = childList[ithChild];
int compID = vtxCompIdx[ithChild];
int newID = vtxIdx[ithChild];
Component& comp = compInfo[compID];
childPos(ithChild, 0) = comp.m_compPos2D[0] + comp.m_localPos2D(newID,0);
childPos(ithChild, 1) = comp.m_compPos2D[1] + comp.m_localPos2D(newID,1);
}
totalRadius = finalRadius;
return true;
}
void load_ply(const std::string &filename, MatrixXu &F, MatrixXf &V, bool load_faces,
const ProgressCallback &progress) {
auto message_cb = [](p_ply ply, const char *msg) {
cerr << "rply: " << msg << endl;
};
Timer<> timer;
cout << "Loading \"" << filename << "\" .. ";
cout.flush();
p_ply ply = ply_open(filename.c_str(), message_cb, 0, nullptr);
if (!ply)
throw std::runtime_error("Unable to open PLY file \"" + filename + "\"!");
if (!ply_read_header(ply)) {
ply_close(ply);
throw std::runtime_error("Unable to open PLY header of \"" + filename + "\"!");
}
p_ply_element element = nullptr;
uint32_t vertexCount = 0, faceCount = 0;
/* Inspect the structure of the PLY file */
while ((element = ply_get_next_element(ply, element)) != nullptr) {
const char *name;
long nInstances;
ply_get_element_info(element, &name, &nInstances);
if (!strcmp(name, "vertex"))
vertexCount = (uint32_t) nInstances;
else if (!strcmp(name, "face"))
faceCount = (uint32_t) nInstances;
}
if (vertexCount == 0 && faceCount == 0)
throw std::runtime_error("PLY file \"" + filename + "\" is invalid! No face/vertex/elements found!");
if (load_faces)
F.resize(3, faceCount);
V.resize(3, vertexCount);
struct VertexCallbackData {
MatrixXf &V;
const ProgressCallback &progress;
VertexCallbackData(MatrixXf &V, const ProgressCallback &progress)
: V(V), progress(progress) {}
};
struct FaceCallbackData {
MatrixXu &F;
const ProgressCallback &progress;
FaceCallbackData(MatrixXu &F, const ProgressCallback &progress)
: F(F), progress(progress) {}
};
auto rply_vertex_cb = [](p_ply_argument argument) -> int {
VertexCallbackData *data;
long index, coord;
ply_get_argument_user_data(argument, (void **) &data, &coord);
ply_get_argument_element(argument, nullptr, &index);
data->V(coord, index) = (Float) ply_get_argument_value(argument);
if (data->progress && coord == 0 && index % 500000 == 0)
data->progress("Loading vertex data", index / (Float) data->V.cols());
return 1;
};
auto rply_index_cb = [](p_ply_argument argument) -> int {
FaceCallbackData *data;
long length, value_index, index;
ply_get_argument_property(argument, nullptr, &length, &value_index);
if (length != 3)
throw std::runtime_error("Only triangle faces are supported!");
ply_get_argument_user_data(argument, (void **) &data, nullptr);
ply_get_argument_element(argument, nullptr, &index);
if (value_index >= 0)
data->F(value_index, index) = (uint32_t) ply_get_argument_value(argument);
if (data->progress && value_index == 0 && index % 500000 == 0)
data->progress("Loading face data", index / (Float) data->F.cols());
return 1;
};
VertexCallbackData vcbData(V, progress);
FaceCallbackData fcbData(F, progress);
if (!ply_set_read_cb(ply, "vertex", "x", rply_vertex_cb, &vcbData, 0) ||
!ply_set_read_cb(ply, "vertex", "y", rply_vertex_cb, &vcbData, 1) ||
!ply_set_read_cb(ply, "vertex", "z", rply_vertex_cb, &vcbData, 2)) {
ply_close(ply);
throw std::runtime_error("PLY file \"" + filename + "\" does not contain vertex position data!");
}
if (load_faces) {
if (!ply_set_read_cb(ply, "face", "vertex_indices", rply_index_cb, &fcbData, 0)) {
ply_close(ply);
throw std::runtime_error("PLY file \"" + filename + "\" does not contain vertex indices!");
}
}
if (!ply_read(ply)) {
ply_close(ply);
throw std::runtime_error("Error while loading PLY data from \"" + filename + "\"!");
}
ply_close(ply);
cout << "done. (V=" << vertexCount;
if (load_faces)
cout << ", F=" << faceCount;
cout << ", took " << timeString(timer.value()) << ")" << endl;
}
bool multiModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
int blockSize = 10;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int h = 0;
MatrixXf Hs(M, 9);
MatrixXf inx(M, psize);
MatrixXf res(dataSize, M);
MatrixXi resIndex(dataSize, M);
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount++;
if (m < blockSize)
RandomSampling(psize, dataSize, sample);
else
WeightedSampling(psize, dataSize, resIndex, sample, h);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
for (int i = 0; i < psize; i++)
inx(m, i) = sample[i];
Matrix3f temp_H;
MatrixXf temp_A, localResidue;
fitHomography(pts1, pts2, temp_H, temp_A);
computeHomographyResidue(x1, x2, temp_H, localResidue);
Hs.row(m) = unrollMatrix3f(temp_H);
res.col(m) = localResidue;
if (m >= (blockSize-1) && (m+1)%blockSize == 0) {
h = round(0.1f*m);
sortResidueForIndex(res, (m/blockSize)*blockSize, ((m+1)/blockSize)*blockSize, resIndex);
}
}
VectorXf bestModel(M);
bestModel.setZero();
int bestIndex = 0;
int bestCount = -1;
for (int i = 0; i < M; i++) {
for (int j = 0; j < dataSize; j++)
if (res(j, i) < THRESHOLD)
bestModel(i) += 1;
if (bestModel(i) > bestCount) {
bestIndex = i;
bestCount = bestModel(i);
}
}
VectorXf bestModelRes = res.col(bestIndex);
int inlierCount = (bestModelRes.array() < THRESHOLD).count();
inlier.resize(inlierCount, data.cols());
int runningIdx = 0;
for (int i = 0; i < dataSize; i++)
if (bestModelRes(i) < THRESHOLD) {
inlier.row(runningIdx) = data.row(i);
runningIdx ++;
}
return true;
}