void MeshConversion<SensorT>::fromPointCloud(
const typename pcl::PointCloud<PointT>::ConstPtr& pc, MeshT& mesh)
{
typedef typename MeshT::VertexHandle VertexHandle;
int rows = pc->height - 1; // last row
int cols = pc->width - 1; // last column
int row_offset;
// [h]orizontal, [v]ertical, [l]eft, [r]ight edge check
std::vector<std::vector<bool> > h(rows+1, std::vector<bool>(cols,true));
std::vector<std::vector<bool> > v(rows, std::vector<bool>(cols+1,true));
std::vector<std::vector<bool> > l(rows, std::vector<bool>(cols,true));
std::vector<std::vector<bool> > r(rows, std::vector<bool>(cols,true));
std::vector<std::vector<VertexHandle> >
vh(rows+1, std::vector<VertexHandle>(cols+1)); // vertex handles
/*
* +--+--+ p00 h00 p01 h01 p02
* | | | v00 lr00 v01 lr01 v02
* +--+--+ p10 h10 p11 h11 p12
* | | | v10 lr10 v11 lr11 v12
* +--+--+ p20 h20 p21 h21 p22
*/
// corners
h.front().front() = v.front().front() = r.front().front() = false;
h.front().back() = v.front().back() = l.front().back() = false;
h.back().front() = v.back().front() = l.back().front() = false;
h.back().back() = v.back().back() = r.back().back() = false;
// first and last row
for(int x = 1; x<cols; ++x)
{
h.front()[x-1] = false;
h.front()[x ] = false;
v.front()[x ] = false;
l.front()[x-1] = false;
r.front()[x ] = false;
h.back ()[x-1] = false;
h.back ()[x ] = false;
v.back ()[x ] = false;
r.back ()[x-1] = false;
l.back ()[x ] = false;
}
for(int y = 1; y<rows; ++y)
{
// left column and right column
h[y ].front() = false;
v[y-1].front() = false;
v[y ].front() = false;
l[y-1].front() = false;
r[y ].front() = false;
h[y ].back() = false;
v[y-1].back() = false;
v[y ].back() = false;
r[y-1].back() = false;
l[y ].back() = false;
row_offset = y*(cols+1);
// iterate remaining
for(int x=1; x<cols; ++x)
{
const PointT* p = &(*pc)[row_offset+x];
if( p->z != p->z )
{
v[y-1][x ] = false;
v[y ][x ] = false;
h[y ][x-1] = false;
h[y ][x ] = false;
l[y-1][x ] = false;
l[y ][x-1] = false;
r[y-1][x-1] = false;
r[y ][x ] = false;
}
else
{
vh[y][x] = mesh.addVertex(y*pc->width+x, *p);
}
}
}
// iterate h and v to check if edge is valid
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pii = pc->points.begin();
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pij = pii + 1; // right
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pji = pii + 1 + cols; // below
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pjj = pji + 1; // below right
for(int y=0; y<rows; ++y)
{
for(int x=0; x<cols; ++x)
{
// check horizontal and vertical
if (h[y][x])
h[y][x] = isNeighbor(pii->getVector3fMap(), pij->getVector3fMap());
if (v[y][x])
v[y][x] = isNeighbor(pii->getVector3fMap(), pji->getVector3fMap());
// check diagonal
unsigned char status = (l[y][x] << 1) | r[y][x];
switch(status)
{
case 0b00:
break;
case 0b01:
r[y][x] = isNeighbor(pii->getVector3fMap(), pjj->getVector3fMap());
break;
case 0b10:
l[y][x] = isNeighbor(pij->getVector3fMap(), pji->getVector3fMap());
break;
case 0b11:
if( (pij->z - pji->z) > (pii->z - pjj->z) )
{
r[y][x] = false;
l[y][x] = isNeighbor(pij->getVector3fMap(), pji->getVector3fMap());
}
else
{
l[y][x] = false;
r[y][x] = isNeighbor(pii->getVector3fMap(), pjj->getVector3fMap());
}
break;
}
++pii; ++pij; ++pji; ++pjj;
}
// skip the last column
// note that in the very last iteration, pjj points beyond end()
++pii; ++pij; ++pji; ++pjj;
}
for(int y=0; y<rows; ++y)
{
for(int x=0; x<cols; ++x)
{
/* ii-ji-ij | ij-ji-jj | ii-jj-ij | ii-ji-jj
* +-+ | + | +-+ | +
* |/ | /| | \| | |\
* + | +-+ | + | +-+ */
if(l[y][x])
{
if (h[y ][x] && v[y][x ])
mesh.addFace(vh[y][x ], vh[y+1][x], vh[y ][x+1]);
if (h[y+1][x] && v[y][x+1])
mesh.addFace(vh[y][x+1], vh[y+1][x], vh[y+1][x+1]);
}
else if (r[y][x])
{
if (h[y][x] && v[y][x+1])
mesh.addFace(vh[y][x], vh[y+1][x+1], vh[y][x+1]);
if (v[y][x] && h[y+1][x])
mesh.addFace(vh[y][x], vh[y+1][x], vh[y+1][x+1]);
}
}
}
}
