bool ScanMeshing::updateAll()
{
// this implementation can handle only one input at the moment
if( env->getInputs(this).size() != 1 || env->getOutputs(this).size() != 1 )
throw std::runtime_error("Scanmeshing needs to have exactly 1 input and 1 output for now.");
TriMesh* meshPtr = static_cast<envire::TriMesh*>(*env->getOutputs(this).begin());
LaserScan* scanPtr = static_cast<envire::LaserScan*>(*env->getInputs(this).begin());
std::vector<Eigen::Vector3d>& points(meshPtr->vertices);
std::vector<Eigen::Vector3d>& colors(meshPtr->getVertexData<Eigen::Vector3d>(TriMesh::VERTEX_COLOR));
std::vector<TriMesh::vertex_attr>& point_attrs(meshPtr->getVertexData<TriMesh::vertex_attr>(TriMesh::VERTEX_ATTRIBUTES));
std::vector<double>& uncertainty(meshPtr->getVertexData<double>(Pointcloud::VERTEX_VARIANCE));
typedef TriMesh::triangle_t triangle_t;
std::vector< triangle_t >& faces(meshPtr->faces);
std::vector<Marker> markers;
// our "update" strategy is to clear everything and redo
points.clear();
colors.clear();
point_attrs.clear();
faces.clear();
uncertainty.clear();
envire::LaserScan& scan(*scanPtr);
int line1[scan.points_per_line], line2[scan.points_per_line];
int *idx_line=line1, *prev_idx_line=line2;
for(size_t line_num=0;line_num<scan.lines.size();line_num++) {
const LaserScan::scanline_t& line(scan.lines[line_num]);
float phi = scan.origin_phi + line.delta_phi;
float psi = scan.origin_psi;
bool has_rem = (line.ranges.size() == line.remissions.size());
float prev_edgeAngle = M_PI*0.5;
for(size_t point_num=0;point_num<line.ranges.size();point_num++) {
// check distance derivative over angle to filter ghost
// readings on edges
float range = line.ranges[point_num] / 1000.0;
float edgeAngle = M_PI*0.5;
if( point_num < (line.ranges.size() - 1) )
{
float next_range = line.ranges[point_num+1] / 1000.0;
float c = sqrt( pow(range,2) + pow(next_range,2) - 2*range*next_range*cos( 2*scan.delta_psi ) );
edgeAngle = acos( (pow(range,2) - pow(next_range,2) + pow(c,2) ) / ( 2*range*c ) );
}
bool pass = fabs(edgeAngle-M_PI*0.5) < maxEdgeAngle &&
fabs(prev_edgeAngle-M_PI*0.5) < maxEdgeAngle;
prev_edgeAngle = edgeAngle;
if( range > minRange && range < maxRange && pass )
{
float xx = std::cos( psi ) * range;
Eigen::Vector3d point;
if( scan.x_forward )
{
// x-forward
point = Eigen::Vector3d(
std::cos( phi ) * xx,
std::sin( psi ) * range,
std::sin( phi ) * xx );
}
else
{
// y-forward
point = Eigen::Vector3d(
-std::sin( psi ) * range,
std::cos( phi ) * xx,
std::sin( phi ) * xx );
}
// perform center offset compensation
Eigen::Vector3d offset = Eigen::AngleAxisd(phi, Eigen::Vector3d::UnitX()) * scan.center_offset;
Eigen::Vector3d opoint = point + offset;
points.push_back(opoint);
// calculate uncertainty of the point
// TODO remove numbers and put into parameters
double sigma = std::min(0.01 * range, 0.02);
uncertainty.push_back( sigma * sigma );
if( _useRemission && has_rem )
{
// convert the remission value into a color value
// for now we do that with a simple linear conversion and a cutoff
// TODO really we should store the reflectivity and not convert into a color
// this is up to the visualisation
float cval = std::min( 1.0, std::max( 0.0, line.remissions[point_num] / (double)remissionScaleFactor ) );
colors.push_back( Eigen::Vector3d::Ones() * cval );
if( extractMarkers )
{
// TODO maybe move into separate function
// see if remission value is above threshold for marker
if( line.remissions[point_num] > remissionMarkerThreshold )
{
bool newMarker = true;
for(size_t i=0;i<markers.size();i++)
{
// TODO make max distance configurable
if( markers[i].dist( opoint ) < 0.05 )
{
newMarker = false;
markers[i].addPoint( opoint );
}
}
if( newMarker )
{
Marker m;
m.addPoint( opoint );
markers.push_back( m );
}
}
}
}
// see if the the scanpoint is actually on the edge of the scan
bool edge =
point_num == 0
|| point_num == (line.ranges.size()-1)
|| line_num == 0
|| line_num == (scan.lines.size()-1);
point_attrs.push_back( edge << TriMesh::SCAN_EDGE );
idx_line[point_num] = points.size() - 1;
} else {
idx_line[point_num] = -1;
}
if( line_num > 0 && point_num > 0) {
// prev_line is valid now, we can start looking for polygons
int poly[4] = { prev_idx_line[point_num-1],
prev_idx_line[point_num],
idx_line[point_num],
idx_line[point_num-1] };
int first_poly = -1;
for(int n=0;n<4;n++) {
int idx[] = {0+(n<=0),1+(n<=1),2+(n<=2)};
// check polygon for existance and then see if all
// three edges are within the threshold limit
if( (poly[idx[0]] > 0) && (poly[idx[1]] > 0) && (poly[idx[2]] > 0) &&
(first_poly==-1 || (first_poly+n)%2==0 ) ) {
float dist_a = (points[poly[idx[0]]] - points[poly[idx[1]]]).norm();
float dist_b = (points[poly[idx[0]]] - points[poly[idx[2]]]).norm();
float dist_c = (points[poly[idx[1]]] - points[poly[idx[2]]]).norm();
if( dist_a < maxEdgeLength && dist_b < maxEdgeLength && dist_c < maxEdgeLength ) {
// found a polygon save idx to prevent the
// overlapping part to be selected, but still look
// for a second one
first_poly = n;
// observe the triangle order for faces on the
// other half of the sphere
//
// TODO, this is not the right check for a reverse
// triangle, because of the offset
if( fabs(psi) > (M_PI/2) ) {
triangle_t tri( poly[idx[0]], poly[idx[2]], poly[idx[1]] );
faces.push_back( tri );
} else {
triangle_t tri( poly[idx[0]], poly[idx[1]], poly[idx[2]] );
faces.push_back( tri );
}
}
}
}
}
psi += scan.delta_psi;
}
// swap line and previous line
int *tmp = idx_line;
idx_line = prev_idx_line;
prev_idx_line = tmp;
}
for(size_t i=0;i<markers.size();i++)
{
std::cout << "marker " << i << " center: " << markers[i].center.transpose() << " pixel: " << markers[i].points.size() << std::endl;
}
// calculate vertex normals
meshPtr->calcVertexNormals();
// remove colors if empty
assert( colors.empty() || colors.size() == points.size() );
if( colors.empty() )
meshPtr->removeData( TriMesh::VERTEX_COLOR );
// mark item as modified
env->itemModified( meshPtr );
return true;
}
