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; }