void
TerrainCoverDrawableObject::Render(ModelData & md)
{
md.points.reserve(points_.size());
md.points = points_;
boost::transform(md.points, md.points.begin(),
[](const Point3d & p)
{
return p + Point3d(0.0f, 0.0f, COVER_Z_SHIFT);
});
PointMap ids;
boost::transform(md.points | boost::adaptors::indexed(0), std::inserter(ids, ids.begin()),
[](const boost::indexed_range<PointVector>::const_iterator::value_type & val)
{
return PointMap::value_type(Point2d::Cast(val.value()), val.index());
});
md.indexes = GetTriangulationIndexes(ids);
NormaleMap normaleMap = GetNormales(md.points, md.indexes);
md.normales.reserve(points_.size());
boost::copy(normaleMap | boost::adaptors::map_values, std::back_inserter(md.normales));
md.textures.reserve(md.points.size());
boost::transform(md.points, std::back_inserter(md.textures),
[](const Point3d & p)
{
return Point2d::Cast(p);
});
md.textureId = TextureId::TransparentGray;
md.type = ModelData::Mode::Triangle;
}
vtkSmartPointer<vtkPolyData> DataDensityFilter::generateOutputData(
PointMap centralPoints, vtkPointSet* input) {
vtkSmartPointer<vtkPolyData> output = vtkSmartPointer<vtkPolyData>::New();
// Create the content of the output poly data object
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
vertices->Allocate(vertices->EstimateSize(1, centralPoints.size()));
vertices->InsertNextCell(centralPoints.size());
// Create all arrays from the input data
QList<vtkSmartPointer<vtkAbstractArray>> inputArrays;
QList<vtkSmartPointer<vtkAbstractArray>> outputArrays;
for (int h = 0; h < input->GetPointData()->GetNumberOfArrays(); ++h) {
vtkSmartPointer<vtkAbstractArray> inputArray = input->GetPointData()->GetAbstractArray(h);
if (!inputArray) {
vtkErrorMacro( << "An input array could not be read.");
return 0;
}
vtkSmartPointer<vtkAbstractArray> outputArray = vtkAbstractArray::CreateArray(
inputArray->GetDataType());
outputArray->SetNumberOfComponents(inputArray->GetNumberOfComponents());
outputArray->SetNumberOfTuples(centralPoints.size());
outputArray->SetName(inputArray->GetName());
inputArrays.append(inputArray);
outputArrays.append(outputArray);
}
void MeaPointTool::SetPosition(const PointMap& points)
{
// Read the position information from the points
// map, and set the corresponding point.
//
PointMap::const_iterator iter = points.find(_T("1"));
if (iter != points.end()) {
m_center = (*iter).second;
}
// Reposition the tool and update the data display.
//
SetPosition();
Update(NormalUpdate);
}
std::vector<PixelRefPair> pixelateMergeLines(const std::vector<Line>& mergeLines, PointMap& currentMap)
{
std::vector<PixelRefPair> mergePixelPairs;
std::vector<Line>::const_iterator iter = mergeLines.begin(), end =
mergeLines.end();
for ( ; iter != end; ++iter )
{
const Line & mergeLine = *iter;
const PixelRef & a = currentMap.pixelate(mergeLine.start(), false);
const PixelRef & b = currentMap.pixelate(mergeLine.end(), false);
mergePixelPairs.push_back(PixelRefPair(a, b));
}
return mergePixelPairs;
}
void integrateScan(PointMap& pmap, LocalizeMap& lmap, DPose2 offset, double maxrange, double usableRange, const RobotLaser& scan){
DTransformation2 tp=DTransformation2(offset)*DTransformation2(scan.laserPose)*DTransformation2(scan.laserParams.laserPose);
//robot pose;
DVector2 rp=tp.translation();
IVector2 start=lmap.world2map(rp);
for (int i=0; i<(int)scan.ranges.size(); i++){
double r=scan.ranges[i];
if (r>=maxrange)
continue;
bool cropped=false;
if (r>usableRange){
r=usableRange;
cropped=true;
}
static GridLineTraversalLine line;
DVector2 bp(r,0);
bp=scan.laserParams.beams[i]*bp;
bp=tp*bp;;
IVector2 end=lmap.world2map(bp);
GridLineTraversal::gridLine(start, end, &line);
for (int i=0; i<line.num_points; i++){
lmap.cell(line.points[i]).distance+=1.;
}
if (! cropped){
lmap.cell(end).occupancy+=1.;
pmap.cell(end)+=bp;
}
}
}
std::vector<SimpleLine> getMergedPixelsAsLines(PointMap& currentMap)
{
std::vector<SimpleLine> mergedPixelsAsLines;
std::vector<std::pair<PixelRef, PixelRef>> mergedPixelPairs = currentMap.getMergedPixelPairs();
std::vector<std::pair<PixelRef, PixelRef>>::const_iterator iter = mergedPixelPairs.begin(), end =
mergedPixelPairs.end();
for ( ; iter != end; ++iter )
{
std::pair<PixelRef, PixelRef> pixelPair = *iter;
mergedPixelsAsLines.push_back(SimpleLine(
currentMap.depixelate(pixelPair.first),
currentMap.depixelate(pixelPair.second)
));
}
return mergedPixelsAsLines;
}
void mergePixelPairs(const std::vector<PixelRefPair>& links, PointMap& currentMap) {
// check if any link pixel already exists on the map
std::vector<PixelRefPair>::const_iterator iter = links.begin(), end =
links.end();
for ( ; iter != end; ++iter )
{
const PixelRefPair link = *iter;
// check in limits:
if (!currentMap.includes(link.a) || !currentMap.getPoint(link.a).filled()
|| !currentMap.includes(link.b) || !currentMap.getPoint(link.b).filled())
{
std::stringstream message;
message << "Line ends not both on painted analysis space (index: "
<< (iter - links.begin() + 1)
<< ")" << std::flush;
throw depthmapX::InvalidLinkException(message.str().c_str());
}
// check if we were given coordinates that fall on a previously
// merged cell, in which case the newest given will replace the
// oldest and effectively delete the whole link
if(currentMap.isPixelMerged(link.a)
|| currentMap.isPixelMerged(link.b))
{
// one of the cells is already on the map
std::stringstream message;
message << "Link pixel found that is already linked on the map (index: "
<< (iter - links.begin() + 1)
<< ")"
<< std::flush;
throw depthmapX::InvalidLinkException(message.str().c_str());
}
// also check if given links have overlapping pixels:
std::vector<PixelRefPair>::const_iterator prevIter = links.begin();
for ( ; prevIter != iter; ++prevIter )
{
const PixelRefPair prevLink = *prevIter;
// PixelRefPair internal == operator only checks a with a and b with b
// but we also need to check the inverse
if(link.a == prevLink.a
|| link.b == prevLink.b
|| link.a == prevLink.b
|| link.b == prevLink.a)
{
// one of the cells has already been seen.
std::stringstream message;
message << "Overlapping link found (index: "
<< (iter - links.begin() + 1)
<< ")" << std::flush;
throw depthmapX::InvalidLinkException(message.str().c_str());
}
}
}
std::for_each(links.begin(), links.end(),
[&](const PixelRefPair &pair)->void{ currentMap.mergePixels(pair.a,pair.b); });
}
void MeaAngleTool::SetPosition(const PointMap& points)
{
PointMap::const_iterator iter;
// Read the position information from the points
// map, and set the corresponding point.
//
iter = points.find(_T("1"));
if (iter != points.end()) {
m_point1 = (*iter).second;
}
iter = points.find(_T("2"));
if (iter != points.end()) {
m_point2 = (*iter).second;
}
iter = points.find(_T("v"));
if (iter != points.end()) {
m_vertex = (*iter).second;
}
// Reposition the tool and update the data display.
//
SetPosition();
Update(NormalUpdate);
}
unsigned long ObjectImpl::addPointNorm(const CTiglPoint& p, const CTiglPoint& n)
{
using namespace std;
//check if point is already in pointlist
unsigned int index = static_cast<unsigned int>(pointlist.size());
std::pair<PointMap::iterator,bool> ret;
ret = pointlist.insert(std::pair<PointImpl, int>(PointImpl(p,n),index));
index = ret.first->second;
/*#ifndef NDEBUG
if (ret.second == false) {
double dist = ret.first->first.dist2(p);
assert(pointlist.size() == 0 || dist < 1e-9);
}
#endif*/
if (ret.second == true) {
// a new point was inserted, we have to keep track
PointImpl * ptmp = (PointImpl*)&ret.first->first;
pPoints.push_back(ptmp);
}
return index;
}
Ref<DetectorResult> Detector::detect() {
Ref<WhiteRectangleDetector> rectangleDetector_(new WhiteRectangleDetector(image_));
std::vector<Ref<ResultPoint> > ResultPoints = rectangleDetector_->detect();
Ref<ResultPoint> pointA = ResultPoints[0];
Ref<ResultPoint> pointB = ResultPoints[1];
Ref<ResultPoint> pointC = ResultPoints[2];
Ref<ResultPoint> pointD = ResultPoints[3];
// Point A and D are across the diagonal from one another,
// as are B and C. Figure out which are the solid black lines
// by counting transitions
std::vector<Ref<ResultPointsAndTransitions> > transitions(4);
transitions[0].reset(transitionsBetween(pointA, pointB));
transitions[1].reset(transitionsBetween(pointA, pointC));
transitions[2].reset(transitionsBetween(pointB, pointD));
transitions[3].reset(transitionsBetween(pointC, pointD));
insertionSort(transitions);
// Sort by number of transitions. First two will be the two solid sides; last two
// will be the two alternating black/white sides
Ref<ResultPointsAndTransitions> lSideOne(transitions[0]);
Ref<ResultPointsAndTransitions> lSideTwo(transitions[1]);
// Figure out which point is their intersection by tallying up the number of times we see the
// endpoints in the four endpoints. One will show up twice.
typedef std::map<Ref<ResultPoint>, int> PointMap;
PointMap pointCount;
increment(pointCount, lSideOne->getFrom());
increment(pointCount, lSideOne->getTo());
increment(pointCount, lSideTwo->getFrom());
increment(pointCount, lSideTwo->getTo());
// Figure out which point is their intersection by tallying up the number of times we see the
// endpoints in the four endpoints. One will show up twice.
Ref<ResultPoint> maybeTopLeft;
Ref<ResultPoint> bottomLeft;
Ref<ResultPoint> maybeBottomRight;
for (PointMap::const_iterator entry = pointCount.begin(), end = pointCount.end(); entry != end; ++entry) {
Ref<ResultPoint> const& point = entry->first;
int value = entry->second;
if (value == 2) {
bottomLeft = point; // this is definitely the bottom left, then -- end of two L sides
} else {
// Otherwise it's either top left or bottom right -- just assign the two arbitrarily now
if (maybeTopLeft == 0) {
maybeTopLeft = point;
} else {
maybeBottomRight = point;
}
}
}
if (maybeTopLeft == 0 || bottomLeft == 0 || maybeBottomRight == 0) {
throw NotFoundException();
}
// Bottom left is correct but top left and bottom right might be switched
std::vector<Ref<ResultPoint> > corners(3);
corners[0].reset(maybeTopLeft);
corners[1].reset(bottomLeft);
corners[2].reset(maybeBottomRight);
// Use the dot product trick to sort them out
ResultPoint::orderBestPatterns(corners);
// Now we know which is which:
Ref<ResultPoint> bottomRight(corners[0]);
bottomLeft = corners[1];
Ref<ResultPoint> topLeft(corners[2]);
// Which point didn't we find in relation to the "L" sides? that's the top right corner
Ref<ResultPoint> topRight;
if (!(pointA->equals(bottomRight) || pointA->equals(bottomLeft) || pointA->equals(topLeft))) {
topRight = pointA;
} else if (!(pointB->equals(bottomRight) || pointB->equals(bottomLeft)
|| pointB->equals(topLeft))) {
topRight = pointB;
} else if (!(pointC->equals(bottomRight) || pointC->equals(bottomLeft)
|| pointC->equals(topLeft))) {
topRight = pointC;
} else {
topRight = pointD;
}
// Next determine the dimension by tracing along the top or right side and counting black/white
// transitions. Since we start inside a black module, we should see a number of transitions
// equal to 1 less than the code dimension. Well, actually 2 less, because we are going to
// end on a black module:
// The top right point is actually the corner of a module, which is one of the two black modules
// adjacent to the white module at the top right. Tracing to that corner from either the top left
// or bottom right should work here.
int dimensionTop = transitionsBetween(topLeft, topRight)->getTransitions();
int dimensionRight = transitionsBetween(bottomRight, topRight)->getTransitions();
//dimensionTop++;
if ((dimensionTop & 0x01) == 1) {
// it can't be odd, so, round... up?
dimensionTop++;
}
dimensionTop += 2;
//dimensionRight++;
if ((dimensionRight & 0x01) == 1) {
// it can't be odd, so, round... up?
dimensionRight++;
}
dimensionRight += 2;
Ref<BitMatrix> bits;
Ref<PerspectiveTransform> transform;
Ref<ResultPoint> correctedTopRight;
// Rectanguar symbols are 6x16, 6x28, 10x24, 10x32, 14x32, or 14x44. If one dimension is more
// than twice the other, it's certainly rectangular, but to cut a bit more slack we accept it as
// rectangular if the bigger side is at least 7/4 times the other:
if (4 * dimensionTop >= 7 * dimensionRight || 4 * dimensionRight >= 7 * dimensionTop) {
// The matrix is rectangular
correctedTopRight = correctTopRightRectangular(bottomLeft, bottomRight, topLeft, topRight,
dimensionTop, dimensionRight);
if (correctedTopRight == NULL) {
correctedTopRight = topRight;
}
dimensionTop = transitionsBetween(topLeft, correctedTopRight)->getTransitions();
dimensionRight = transitionsBetween(bottomRight, correctedTopRight)->getTransitions();
if ((dimensionTop & 0x01) == 1) {
// it can't be odd, so, round... up?
dimensionTop++;
}
if ((dimensionRight & 0x01) == 1) {
// it can't be odd, so, round... up?
dimensionRight++;
}
transform = createTransform(topLeft, correctedTopRight, bottomLeft, bottomRight, dimensionTop,
dimensionRight);
bits = sampleGrid(image_, dimensionTop, dimensionRight, transform);
} else {
// The matrix is square
int dimension = min(dimensionRight, dimensionTop);
// correct top right point to match the white module
correctedTopRight = correctTopRight(bottomLeft, bottomRight, topLeft, topRight, dimension);
if (correctedTopRight == NULL) {
correctedTopRight = topRight;
}
// Redetermine the dimension using the corrected top right point
int dimensionCorrected = std::max(transitionsBetween(topLeft, correctedTopRight)->getTransitions(),
transitionsBetween(bottomRight, correctedTopRight)->getTransitions());
dimensionCorrected++;
if ((dimensionCorrected & 0x01) == 1) {
dimensionCorrected++;
}
transform = createTransform(topLeft, correctedTopRight, bottomLeft, bottomRight,
dimensionCorrected, dimensionCorrected);
bits = sampleGrid(image_, dimensionCorrected, dimensionCorrected, transform);
}
ArrayRef< Ref<ResultPoint> > points (new Array< Ref<ResultPoint> >(4));
points[0].reset(topLeft);
points[1].reset(bottomLeft);
points[2].reset(correctedTopRight);
points[3].reset(bottomRight);
Ref<DetectorResult> detectorResult(new DetectorResult(bits, points));
return detectorResult;
}
DataDensityFilter::PointMap DataDensityFilter::reducePointsKMeans(vtkPointSet* input) {
// Get the points into the format needed for KMeans
vtkSmartPointer<vtkTable> inputData = vtkSmartPointer<vtkTable>::New();
// Create the table needed for the KMeans statistics filter
for (int c = 0; c < 3; ++c) {
std::stringstream colName;
colName << "coord " << c;
vtkSmartPointer<vtkDoubleArray> doubleArray =
vtkSmartPointer<vtkDoubleArray>::New();
doubleArray->SetNumberOfComponents(1);
doubleArray->SetName(colName.str().c_str());
doubleArray->SetNumberOfTuples(input->GetNumberOfPoints());
for (int r = 0; r < input->GetNumberOfPoints(); ++r) {
double p[3];
input->GetPoint(r, p);
doubleArray->SetValue(r, p[c]);
}
inputData->AddColumn(doubleArray);
}
// Actually do the KMeans to calculate the centroids for the clusters
vtkSmartPointer<vtkKMeansStatistics> kMeansStatistics =
vtkSmartPointer<vtkKMeansStatistics>::New();
kMeansStatistics->SetInputData(vtkStatisticsAlgorithm::INPUT_DATA, inputData);
kMeansStatistics->SetColumnStatus(inputData->GetColumnName(0), 1);
kMeansStatistics->SetColumnStatus(inputData->GetColumnName(1), 1);
kMeansStatistics->SetColumnStatus(inputData->GetColumnName(2), 1);
kMeansStatistics->RequestSelectedColumns();
// Set the default number of clusters to the number of points that should be kept
kMeansStatistics->SetDefaultNumberOfClusters(std::max(int((1 - this->dataPercentage) *
input->GetNumberOfPoints()), 1));
// Make the filter only do one iteration for a better performance since we do not need a precise kMeans approximation
kMeansStatistics->SetMaxNumIterations(1);
kMeansStatistics->SetAssessOption(true);
kMeansStatistics->Update();
// Extract the calculated cluster centers from the filter
vtkSmartPointer<vtkPoints> centroids =
vtkSmartPointer<vtkPoints>::New();
vtkMultiBlockDataSet* outputMetaDS = vtkMultiBlockDataSet::SafeDownCast(
kMeansStatistics->GetOutputDataObject(vtkStatisticsAlgorithm::OUTPUT_MODEL));
vtkSmartPointer<vtkTable> outputMeta = vtkTable::SafeDownCast(outputMetaDS->GetBlock(0));
vtkDoubleArray* coord0 = vtkDoubleArray::SafeDownCast(outputMeta->GetColumnByName("coord 0"));
vtkDoubleArray* coord1 = vtkDoubleArray::SafeDownCast(outputMeta->GetColumnByName("coord 1"));
vtkDoubleArray* coord2 = vtkDoubleArray::SafeDownCast(outputMeta->GetColumnByName("coord 2"));
PointMap outputMap;
// Put the centroids into the map
for (int i = 0; i < coord0->GetNumberOfTuples(); ++i) {
PointCoordinates centroidCoordinates(coord0->GetValue(i), coord1->GetValue(i), coord2->GetValue(i));
QList<int> subordinatePoints;
// Extract the point IDs in the centroid's bucket
for (int r = 0; r < kMeansStatistics->GetOutput()->GetNumberOfRows(); ++r) {
vtkVariant v = kMeansStatistics->GetOutput()->GetValue(r,
kMeansStatistics->GetOutput()->GetNumberOfColumns() - 1);
if (v.ToInt() == i) {
subordinatePoints.append(r);
}
}
outputMap.insert(std::make_pair(centroidCoordinates, subordinatePoints));
}
return outputMap;
}
DataDensityFilter::PointMap DataDensityFilter::reducePointsSimple(vtkPointSet* input) {
int numberOfPoints = input->GetNumberOfPoints();
PointMap outputMap;
// Get the width, height and depth of the data in order to set the absoprtion distance
double bounds[6];
input->GetBounds(bounds);
double minX = bounds[0];
double maxX = bounds[1];
double minY = bounds[2];
double maxY = bounds[3];
double minZ = bounds[4];
double maxZ = bounds[5];
double width = maxX - minX;
double height = maxY - minY;
double depth = maxZ - minZ;
double absorptionDistance = this->dataPercentage * (width + height + depth);
// This data structure maps a point index to a boolean flag denoting whether it has been absorped by a central point or has become a central point
std::map<int, bool> pointAbsorptionStatus;
// In the beginning all points are unabsorbed
for (int i = 0; i < numberOfPoints; ++i) {
pointAbsorptionStatus[i] = false;
}
QMap<PointCoordinates, int> indices;
// Generate central points and assign close points to them
for (int i = 0; i < numberOfPoints; ++i) {
if (pointAbsorptionStatus[i] == false) {
double centralPointCoordinatesArray[3];
input->GetPoint(i, centralPointCoordinatesArray);
PointCoordinates centralPointCoordinates(centralPointCoordinatesArray[0],
centralPointCoordinatesArray[1], centralPointCoordinatesArray[2]);
QList<int> subordinatePointIndices;
pointAbsorptionStatus[i] = true;
indices[centralPointCoordinates] = i;
for (int j = 0; j < numberOfPoints; ++j) {
if (pointAbsorptionStatus[j] == false) {
double subordinatePointCoordinatesArray[3];
input->GetPoint(j, subordinatePointCoordinatesArray);
PointCoordinates subordinatePointCoordinates(subordinatePointCoordinatesArray[0],
subordinatePointCoordinatesArray[1], subordinatePointCoordinatesArray[2]);
if (centralPointCoordinates.getDistanceTo(subordinatePointCoordinates) < absorptionDistance) {
subordinatePointIndices.append(j);
pointAbsorptionStatus[j] = true;
}
}
}
outputMap[centralPointCoordinates] = subordinatePointIndices;
}
}
typedef PointMap::iterator it_type;
for (it_type iterator = outputMap.begin(); iterator != outputMap.end(); iterator++) {
this->centroidIndices.append(indices[iterator->first]);
}
return outputMap;
}
