void NeighbourJoining::configure(const MatrixXf& D, MatrixXf& currentD, MatrixXi& rowsID, int numOccupiedNodes) { numObservableNodes = rowsID.rows(); numCurrentNodes = numObservableNodes; //allocates memory for the latent nodes rowsID.conservativeResize((2 * numObservableNodes - 2), 1); //gives latent nodes IDs for (int i = numObservableNodes; i < 2 * numObservableNodes - 2; ++i) { rowsID(i) = i + numOccupiedNodes; } sort(rowsID.data(), rowsID.data() + rowsID.size()); //cout << "rowsID after sort:" << rowsID.transpose(); cout << endl; //copies distances of selected Nodes int offseti = 0; int offsetj = 0; for (int i = 0; i < numObservableNodes; ++i) { while (rowsID(i) != i + offseti) ++offseti; offsetj = 0; for (int j = 0; j < numObservableNodes; ++j) { while (rowsID(j) != j + offsetj) ++offsetj; currentD(i, j) = D(i + offseti, j + offsetj); currentD(j, i) = currentD(i, j); } } //cout << "copied matrix: " << endl; printMatrix(currentD); }
// Read a surface mesh from a {.obj|.off|.mesh} files // Inputs: // mesh_filename path to {.obj|.off|.mesh} file // Outputs: // V #V by 3 list of mesh vertex positions // F #F by 3 list of triangle indices // Returns true only if successfuly able to read file bool load_mesh_from_file( const std::string mesh_filename, Eigen::MatrixXd & V, Eigen::MatrixXi & F) { using namespace std; using namespace igl; using namespace Eigen; string dirname, basename, extension, filename; pathinfo(mesh_filename,dirname,basename,extension,filename); transform(extension.begin(), extension.end(), extension.begin(), ::tolower); bool success = false; if(extension == "obj") { success = readOBJ(mesh_filename,V,F); }else if(extension == "off") { success = readOFF(mesh_filename,V,F); }else if(extension == "mesh") { // Unused Tets read from .mesh file MatrixXi Tets; success = readMESH(mesh_filename,V,Tets,F); // We're not going to use any input tets. Only the surface if(Tets.size() > 0 && F.size() == 0) { // If Tets read, but no faces then use surface of tet volume }else { // Rearrange vertices so that faces come first VectorXi IM; faces_first(V,F,IM); // Dont' bother reordering Tets, but this is how one would: //Tets = // Tets.unaryExpr(bind1st(mem_fun( static_cast<VectorXi::Scalar& // (VectorXi::*)(VectorXi::Index)>(&VectorXi::operator())), // &IM)).eval(); // Don't throw away any interior vertices, since user may want weights // there } }else { cerr<<"Error: Unknown shape file format extension: ."<<extension<<endl; return false; } return success; }
MatrixXf CharacterController::GenerateXapv(const std::vector<int> &activeParts) { // pvDim without auto& allClipinfo = m_cpxClipinfo; auto& pvFacade = allClipinfo.PvFacade; int pvDim = pvFacade.GetAllPartDimension(); assert(pvDim > 0); MatrixXf Xabpv(allClipinfo.ClipFrames(), size(activeParts) * pvDim); ArrayXi incX(pvDim); incX.setLinSpaced(0, pvDim - 1); MatrixXi apMask = VectorXi::Map(activeParts.data(), activeParts.size()).replicate(1, pvDim).transpose(); apMask.array() = apMask.array() * pvDim + incX.replicate(1, apMask.cols()); auto maskVec = VectorXi::Map(apMask.data(), apMask.size()); selectCols(pvFacade.GetAllPartsSequence(), maskVec, &Xabpv); Pca<MatrixXf> pcaXabpv(Xabpv); int dXabpv = pcaXabpv.reducedRank(g_CharacterPcaCutoff); Xabpv = pcaXabpv.coordinates(dXabpv); XabpvT = pcaXabpv.components(dXabpv); uXabpv = pcaXabpv.mean(); if (g_EnableDebugLogging) { ofstream fout(g_CharacterAnalyzeDir / (m_pCharacter->Name + "_Xabpv.pd.csv")); fout << Xabpv.format(CSVFormat); fout.close(); } return Xabpv; }
std::vector<PointId> SMRFilter::processGround(PointViewPtr view) { log()->get(LogLevel::Info) << "processGround: Running SMRF...\n"; // The algorithm consists of four conceptually distinct stages. The first is // the creation of the minimum surface (ZImin). The second is the processing // of the minimum surface, in which grid cells from the raster are // identified as either containing bare earth (BE) or objects (OBJ). This // second stage represents the heart of the algorithm. The third step is the // creation of a DEM from these gridded points. The fourth step is the // identification of the original LIDAR points as either BE or OBJ based on // their relationship to the interpolated std::vector<PointId> groundIdx; BOX2D bounds; view->calculateBounds(bounds); SpatialReference srs(view->spatialReference()); // Determine the number of rows and columns at the given cell size. m_numCols = ((bounds.maxx - bounds.minx) / m_cellSize) + 1; m_numRows = ((bounds.maxy - bounds.miny) / m_cellSize) + 1; MatrixXd cx(m_numRows, m_numCols); MatrixXd cy(m_numRows, m_numCols); for (auto c = 0; c < m_numCols; ++c) { for (auto r = 0; r < m_numRows; ++r) { cx(r, c) = bounds.minx + (c + 0.5) * m_cellSize; cy(r, c) = bounds.miny + (r + 0.5) * m_cellSize; } } // STEP 1: // As with many other ground filtering algorithms, the first step is // generation of ZImin from the cell size parameter and the extent of the // data. The two vectors corresponding to [min:cellSize:max] for each // coordinate – xi and yi – may be supplied by the user or may be easily and // automatically calculated from the data. Without supplied ranges, the SMRF // algorithm creates a raster from the ceiling of the minimum to the floor // of the maximum values for each of the (x,y) dimensions. If the supplied // cell size parameter is not an integer, the same general rule applies to // values evenly divisible by the cell size. For example, if cell size is // equal to 0.5 m, and the x values range from 52345.6 to 52545.4, the range // would be [52346 52545]. // The minimum surface grid ZImin defined by vectors (xi,yi) is filled with // the nearest, lowest elevation from the original point cloud (x,y,z) // values, provided that the distance to the nearest point does not exceed // the supplied cell size parameter. This provision means that some grid // points of ZImin will go unfilled. To fill these values, we rely on // computationally inexpensive image inpainting techniques. Image inpainting // involves the replacement of the empty cells in an image (or matrix) with // values calculated from other nearby values. It is a type of interpolation // technique derived from artistic replacement of damaged portions of // photographs and paintings, where preservation of texture is an important // concern (Bertalmio et al., 2000). When empty values are spread through // the image, and the ratio of filled to empty pixels is quite high, most // methods of inpainting will produce satisfactory results. In an evaluation // of inpainting methods on ground identification from the final terrain // model, we found that Laplacian techniques produced error rates nearly // three times higher than either an average of the eight nearest neighbors // or D’Errico’s spring-metaphor inpainting technique (D’Errico, 2004). The // spring-metaphor technique imagines springs connecting each cell with its // eight adjacent neighbors, where the inpainted value corresponds to the // lowest energy state of the set, and where the entire (sparse) set of // linear equations is solved using partial differential equations. Both of // these latter techniques were nearly the same with regards to total error, // with the spring technique performing slightly better than the k-nearest // neighbor (KNN) approach. MatrixXd ZImin = eigen::createMinMatrix(*view.get(), m_numRows, m_numCols, m_cellSize, bounds); // MatrixXd ZImin_painted = inpaintKnn(cx, cy, ZImin); // MatrixXd ZImin_painted = TPS(cx, cy, ZImin); MatrixXd ZImin_painted = expandingTPS(cx, cy, ZImin); if (!m_outDir.empty()) { std::string filename = FileUtils::toAbsolutePath("zimin.tif", m_outDir); eigen::writeMatrix(ZImin, filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("zimin_painted.tif", m_outDir); eigen::writeMatrix(ZImin_painted, filename, "GTiff", m_cellSize, bounds, srs); } ZImin = ZImin_painted; // STEP 2: // The second stage of the ground identification algorithm involves the // application of a progressive morphological filter to the minimum surface // grid (ZImin). At the first iteration, the filter applies an image opening // operation to the minimum surface. An opening operation consists of an // application of an erosion filter followed by a dilation filter. The // erosion acts to snap relative high values to relative lows, where a // supplied window radius and shape (or structuring element) defines the // search neighborhood. The dilation uses the same window radius and // structuring element, acting to outwardly expand relative highs. Fig. 2 // illustrates an opening operation on a cross section of a transect from // Sample 1–1 in the ISPRS LIDAR reference dataset (Sithole and Vosselman, // 2003), following Zhang et al. (2003). // paper has low point happening later, i guess it doesn't matter too much, this is where he does it in matlab code MatrixXi Low = progressiveFilter(-ZImin, m_cellSize, 5.0, 1.0); // matlab code has net cutting occurring here MatrixXd ZInet = ZImin; MatrixXi isNetCell = MatrixXi::Zero(m_numRows, m_numCols); if (m_cutNet > 0.0) { MatrixXd bigOpen = eigen::matrixOpen(ZImin, 2*std::ceil(m_cutNet / m_cellSize)); for (auto c = 0; c < m_numCols; c += std::ceil(m_cutNet/m_cellSize)) { for (auto r = 0; r < m_numRows; ++r) { isNetCell(r, c) = 1; } } for (auto c = 0; c < m_numCols; ++c) { for (auto r = 0; r < m_numRows; r += std::ceil(m_cutNet/m_cellSize)) { isNetCell(r, c) = 1; } } for (auto c = 0; c < m_numCols; ++c) { for (auto r = 0; r < m_numRows; ++r) { if (isNetCell(r, c)==1) ZInet(r, c) = bigOpen(r, c); } } } // and finally object detection MatrixXi Obj = progressiveFilter(ZInet, m_cellSize, m_percentSlope, m_maxWindow); // STEP 3: // The end result of the iteration process described above is a binary grid // where each cell is classified as being either bare earth (BE) or object // (OBJ). The algorithm then applies this mask to the starting minimum // surface to eliminate nonground cells. These cells are then inpainted // according to the same process described previously, producing a // provisional DEM (ZIpro). // we currently aren't checking for net cells or empty cells (haven't i already marked empty cells as NaNs?) MatrixXd ZIpro = ZImin; for (int i = 0; i < Obj.size(); ++i) { if (Obj(i) == 1 || Low(i) == 1 || isNetCell(i) == 1) ZIpro(i) = std::numeric_limits<double>::quiet_NaN(); } // MatrixXd ZIpro_painted = inpaintKnn(cx, cy, ZIpro); // MatrixXd ZIpro_painted = TPS(cx, cy, ZIpro); MatrixXd ZIpro_painted = expandingTPS(cx, cy, ZIpro); if (!m_outDir.empty()) { std::string filename = FileUtils::toAbsolutePath("zilow.tif", m_outDir); eigen::writeMatrix(Low.cast<double>(), filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("zinet.tif", m_outDir); eigen::writeMatrix(ZInet, filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("ziobj.tif", m_outDir); eigen::writeMatrix(Obj.cast<double>(), filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("zipro.tif", m_outDir); eigen::writeMatrix(ZIpro, filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("zipro_painted.tif", m_outDir); eigen::writeMatrix(ZIpro_painted, filename, "GTiff", m_cellSize, bounds, srs); } ZIpro = ZIpro_painted; // STEP 4: // The final step of the algorithm is the identification of ground/object // LIDAR points. This is accomplished by measuring the vertical distance // between each LIDAR point and the provisional DEM, and applying a // threshold calculation. While many authors use a single value for the // elevation threshold, we suggest that a second parameter be used to // increase the threshold on steep slopes, transforming the threshold to a // slope-dependent value. The total permissible distance is then equal to a // fixed elevation threshold plus the scaling value multiplied by the slope // of the DEM at each LIDAR point. The rationale behind this approach is // that small horizontal and vertical displacements yield larger errors on // steep slopes, and as a result the BE/OBJ threshold distance should be // more per- missive at these points. // The calculation requires that both elevation and slope are interpolated // from the provisional DEM. There are any number of interpolation // techniques that might be used, and even nearest neighbor approaches work // quite well, so long as the cell size of the DEM nearly corresponds to the // resolution of the LIDAR data. A comparison of how well these different // methods of interpolation perform is given in the next section. Based on // these results, we find that a splined cubic interpolation provides the // best results. // It is common in LIDAR point clouds to have a small number of outliers // which may be either above or below the terrain surface. While // above-ground outliers (e.g., a random return from a bird in flight) are // filtered during the normal algorithm routine, the below-ground outliers // (e.g., those caused by a reflection) require a separate approach. Early // in the routine and along a separate processing fork, the minimum surface // is checked for low outliers by inverting the point cloud in the z-axis // and applying the filter with parameters (slope = 500%, maxWindowSize = // 1). The resulting mask is used to flag low outlier cells as OBJ before // the inpainting of the provisional DEM. This outlier identification // methodology is functionally the same as that of Zhang et al. (2003). // The provisional DEM (ZIpro), created by removing OBJ cells from the // original minimum surface (ZImin) and then inpainting, tends to be less // smooth than one might wish, especially when the surfaces are to be used // to create visual products like immersive geographic virtual environments. // As a result, it is often worthwhile to reinter- polate a final DEM from // the identified ground points of the original LIDAR data (ZIfin). Surfaces // created from these data tend to be smoother and more visually satisfying // than those derived from the provisional DEM. // Very large (>40m in length) buildings can sometimes prove troublesome to // remove on highly differentiated terrain. To accommodate the removal of // such objects, we implemented a feature in the published SMRF algorithm // which is helpful in removing such features. We accomplish this by // introducing into the initial minimum surface a ‘‘net’’ of minimum values // at a spacing equal to the maximum window diameter, where these minimum // values are found by applying a morphological open operation with a disk // shaped structuring element of radius (2?wkmax). Since only one example in // this dataset had features this large (Sample 4–2, a trainyard) we did not // include this portion of the algorithm in the formal testing procedure, // though we provide a brief analysis of the effect of using this net filter // in the next section. MatrixXd scaled = ZIpro / m_cellSize; MatrixXd gx = eigen::gradX(scaled); MatrixXd gy = eigen::gradY(scaled); MatrixXd gsurfs = (gx.cwiseProduct(gx) + gy.cwiseProduct(gy)).cwiseSqrt(); // MatrixXd gsurfs_painted = inpaintKnn(cx, cy, gsurfs); // MatrixXd gsurfs_painted = TPS(cx, cy, gsurfs); MatrixXd gsurfs_painted = expandingTPS(cx, cy, gsurfs); if (!m_outDir.empty()) { std::string filename = FileUtils::toAbsolutePath("gx.tif", m_outDir); eigen::writeMatrix(gx, filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("gy.tif", m_outDir); eigen::writeMatrix(gy, filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("gsurfs.tif", m_outDir); eigen::writeMatrix(gsurfs, filename, "GTiff", m_cellSize, bounds, srs); filename = FileUtils::toAbsolutePath("gsurfs_painted.tif", m_outDir); eigen::writeMatrix(gsurfs_painted, filename, "GTiff", m_cellSize, bounds, srs); } gsurfs = gsurfs_painted; MatrixXd thresh = (m_threshold + m_scalar * gsurfs.array()).matrix(); if (!m_outDir.empty()) { std::string filename = FileUtils::toAbsolutePath("thresh.tif", m_outDir); eigen::writeMatrix(thresh, filename, "GTiff", m_cellSize, bounds, srs); } for (PointId i = 0; i < view->size(); ++i) { using namespace Dimension; double x = view->getFieldAs<double>(Id::X, i); double y = view->getFieldAs<double>(Id::Y, i); double z = view->getFieldAs<double>(Id::Z, i); int c = Utils::clamp(static_cast<int>(floor(x - bounds.minx) / m_cellSize), 0, m_numCols-1); int r = Utils::clamp(static_cast<int>(floor(y - bounds.miny) / m_cellSize), 0, m_numRows-1); // author uses spline interpolation to get value from ZIpro and gsurfs if (std::isnan(ZIpro(r, c))) continue; // not sure i should just brush this under the rug... if (std::isnan(gsurfs(r, c))) continue; double ez = ZIpro(r, c); // double ez = interp2(r, c, cx, cy, ZIpro); // double si = gsurfs(r, c); // double si = interp2(r, c, cx, cy, gsurfs); // double reqVal = m_threshold + 1.2 * si; if (std::abs(ez - z) > thresh(r, c)) continue; // if (std::abs(ZIpro(r, c) - z) > m_threshold) // continue; groundIdx.push_back(i); } return groundIdx; }
void Mesh::CreateSimpleMesh() { int c = 0; MatrixXi g(n_node_x, n_node_y); for (size_t i=0; i<n_node_x; i++) { for (size_t j=0; j<n_node_y; j++) { g(i,j) = c; c++; } } MatrixXi gg(n_node_p_elem, n_elem); size_t xi = 0; size_t xj = 0; for (size_t i=0; i<n_elem; i++) { size_t xi_min = xi; size_t xj_min = xj; size_t xi_max = xi + n_node_p_dim; size_t xj_max = xj + n_node_p_dim; MatrixXi block = g.block(xi_min, xj_min, n_node_p_dim, n_node_p_dim); Map<VectorXi> gv(block.data(), block.size()); gg.col(i) = gv; if (xi_max == n_node_x) { xi = 0; xj = xj_max - 1; } else { xi = xi_max - 1; } } c = 0; MatrixXi nf(eqn_p_node, n_node); for (size_t i=0; i<n_node; i++) { nf(0,i) = c; nf(1,i) = c+1; c += 2; } imap.resize(n_dof_p_elem, n_elem); for (size_t i=0; i<n_elem; i++) { c = 0; while (c+eqn_p_node <= n_dof_p_elem) { imap.block(c, i, eqn_p_node, 1) = nf.block( 0, gg(c/eqn_p_node, i), eqn_p_node, 1); c += eqn_p_node; } } valen.resize(n_dof_p_elem, n_elem); valen.setZero(); for (size_t i=0; i<n_elem; i++) { for (size_t j=0; j<n_dof_p_elem; j++) { valen(j, i) += 1; } } size_t k = 0; size_t e = 0; size_t n_node_x_vis {n_elem_x + 1}; connec.resize(4*n_elem); for (size_t j=0; j<n_elem_y; j++) { for (size_t i=0; i<n_elem_x; i++) { connec(k+0) = (i+0) + (j+0) * n_node_x_vis; connec(k+1) = (i+1) + (j+0) * n_node_x_vis; connec(k+2) = (i+1) + (j+1) * n_node_x_vis; connec(k+3) = (i+0) + (j+1) * n_node_x_vis; k+=4; e++; } } connec = connec.array() + 1; n_vis_node = connec.maxCoeff(); k = 0; ex.resize(n_vis_node); ey.resize(n_vis_node); m_node_n.resize(n_vis_node); for (size_t j=0; j<=n_elem_y; j++) { for (size_t i=0; i<=n_elem_x; i++) { ex(k) = i * s_elem_x; ey(k) = j * s_elem_y; m_node_n(k) = k; k++; } } m_node_n = m_node_n.array() + 1; ctr_elem_x.resize(n_elem); ctr_elem_y.resize(n_elem); k = 0; for (size_t j=0; j<n_elem_y; j++) { for (size_t i=0; i<n_elem_x; i++) { ctr_elem_x(k) = (1/2.) * (ex(k) + ex(k+1)); ctr_elem_y(k) = (1/2.) * (ey(k) + ey(k+1)); k++; } } }