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