455 lines
17 KiB
C++
455 lines
17 KiB
C++
/*
|
|
----
|
|
This file is part of SECONDO.
|
|
|
|
Copyright (C) 2019,
|
|
Faculty of Mathematics and Computer Science,
|
|
Database Systems for New Applications.
|
|
|
|
SECONDO is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
SECONDO is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with SECONDO; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
----
|
|
|
|
|
|
//[<] [\ensuremath{<}]
|
|
//[>] [\ensuremath{>}]
|
|
|
|
\setcounter{tocdepth}{3}
|
|
\tableofcontents
|
|
|
|
|
|
|
|
1 Analyze Geometries Operator
|
|
|
|
*/
|
|
#include "opAnalyzeGeom.h"
|
|
|
|
#include "../utility/DbScan.h"
|
|
#include "../utility/Timer.h"
|
|
#include "../utility/MathUtils.h"
|
|
|
|
#include "GpPlane.h"
|
|
#include "GpSphere.h"
|
|
#include "GpCylinder.h"
|
|
#include "GpCone.h"
|
|
|
|
using namespace pointcloud2;
|
|
|
|
extern NestedList *nl;
|
|
|
|
bool op_analyzeGeom::REPORT_TO_CONSOLE = true;
|
|
|
|
/*
|
|
1.1 Type Mapping
|
|
|
|
*/
|
|
ListExpr op_analyzeGeom::analyzeGeomTM(ListExpr args) {
|
|
if (!(nl->HasLength(args, 1) && Pointcloud2::TypeCheck(nl->First(args)))) {
|
|
return listutils::typeError("wrong input");
|
|
}
|
|
|
|
// get the attribute indices of ObjID and CatID, if existing.
|
|
// Ensure these attributes are of type CcInt
|
|
ListExpr cloudType = nl->First(args);
|
|
int attrIndexObjID = -1;
|
|
int attrIndexCatID = -1;
|
|
bool hasTuples = Pointcloud2::isTupleCloud(cloudType);
|
|
ListExpr attrList = nl->TheEmptyList();
|
|
if (hasTuples) {
|
|
attrList = nl->Second(Pointcloud2::getTupleType(cloudType));
|
|
ListExpr type;
|
|
attrIndexObjID = listutils::findAttribute(attrList, OBJ_ID, type) - 1;
|
|
if (attrIndexObjID >= 0 &&
|
|
!listutils::isSymbol(type, CcInt::BasicType())) {
|
|
return listutils::typeError("wrong type of attribute ObjID");
|
|
}
|
|
attrIndexCatID = listutils::findAttribute(attrList, CAT_ID, type) - 1;
|
|
if (attrIndexCatID >= 0 &&
|
|
!listutils::isSymbol(type, CcInt::BasicType())) {
|
|
return listutils::typeError("wrong type of attribute CatID");
|
|
}
|
|
// ensure that either both attributes are present or both are missing
|
|
if ((attrIndexObjID >= 0) != (attrIndexCatID >= 0)) {
|
|
return listutils::typeError("one of the attributes "
|
|
"ObjID and CatID is defined, the other is missing.");
|
|
}
|
|
}
|
|
|
|
// remember whether the source cloud has ObjIDs from a previous analysis.
|
|
// If so, we will have to read through the tuples in Value Mapping to
|
|
// exclude points which have already been assigned to an object.
|
|
bool hasOldObjIDs = (attrIndexObjID >= 0);
|
|
|
|
// append the attributes ObjID and CatID to the result type, if missing
|
|
ListExpr newCloudType;
|
|
if (!hasOldObjIDs) {
|
|
// calculate the appendage for the missing attributes
|
|
ListExpr attrObjID = nl->TwoElemList(
|
|
nl->SymbolAtom(OBJ_ID), listutils::basicSymbol<CcInt>());
|
|
ListExpr attrCatID = nl->TwoElemList(
|
|
nl->SymbolAtom(CAT_ID), listutils::basicSymbol<CcInt>());
|
|
ListExpr appendage = nl->TwoElemList(attrObjID, attrCatID);
|
|
|
|
// determine the new type with the attributes
|
|
newCloudType = Pointcloud2::appendAttributesToCloud(cloudType,
|
|
appendage);
|
|
|
|
// determine the attribute indices in the new cloud type
|
|
attrIndexObjID = hasTuples ? nl->ListLength(attrList) : 0;
|
|
attrIndexCatID = attrIndexObjID + 1;
|
|
} else {
|
|
newCloudType = cloudType;
|
|
// attrIndexObj/CatId will be identical in the new cloud
|
|
}
|
|
|
|
// use the append mechanism (see GuideAlgebra.pdf, p.48, section 11.1)
|
|
// to inform Value Mapping about hasOldObjIDs and attrIndexes
|
|
return nl->ThreeElemList(
|
|
nl->SymbolAtom(Symbols::APPEND()),
|
|
nl->ThreeElemList(
|
|
nl->BoolAtom(hasOldObjIDs),
|
|
nl->IntAtom(attrIndexObjID),
|
|
nl->IntAtom(attrIndexCatID)),
|
|
newCloudType);
|
|
}
|
|
|
|
/*
|
|
1.2 Value Mapping
|
|
|
|
*/
|
|
int op_analyzeGeom::analyzeGeomVM(Word* args, Word& result, int message,
|
|
Word& local, Supplier s) {
|
|
|
|
if (REPORT_TO_CONSOLE)
|
|
cout << endl << "starting analyzeGeom" << endl;
|
|
result = qp->ResultStorage(s);
|
|
|
|
// get source and result Pointcloud2. These variables must NOT be wrapped
|
|
// in a shared_ptr, otherwise both clouds will be deleted when leaving
|
|
// this method!
|
|
Pointcloud2* sourceCloud = static_cast<Pointcloud2*>(args[0].addr);
|
|
Pointcloud2* resultCloud = static_cast<Pointcloud2*>(result.addr);
|
|
|
|
// get further parameters passed to Value Mapping by Type Mapping
|
|
bool hasOldObjIDs = ((CcBool*)args[1].addr)->GetValue();
|
|
int attrIndexObjID = ((CcInt*)args[2].addr)->GetValue();
|
|
int attrIndexCatID = ((CcInt*)args[3].addr)->GetValue();
|
|
|
|
// create a Timer instance for performance measurement
|
|
Timer timer { Timer::UNIT::millis };
|
|
size_t pointCount = sourceCloud->getPointCount();
|
|
timer.getReportStream() << endl << "Timer for analyzeGeom, pointCount = "
|
|
<< formatInt(pointCount) << ":" << endl;
|
|
|
|
// first we'll clone our cloud, adding "ObjID" and "CatID" attributes
|
|
// if they were missing from the sourceCloud
|
|
timer.startTask("copy cloud");
|
|
if (REPORT_TO_CONSOLE) {
|
|
cout << "creating copy of source cloud (" << formatInt(pointCount)
|
|
<< " points) with attributes " << OBJ_ID << " and " << CAT_ID
|
|
<< endl;
|
|
}
|
|
std::vector<std::unique_ptr<Attribute>> attributes;
|
|
if (!hasOldObjIDs) {
|
|
// add the new attributes
|
|
attributes.push_back(std::unique_ptr<Attribute>(new CcInt(true, 0)));
|
|
attributes.push_back(std::unique_ptr<Attribute>(new CcInt(true, 0)));
|
|
} // otherwise, attributes remains empty which allows for simpler copying
|
|
resultCloud->copyAllWithAdditionalAttributes(sourceCloud,
|
|
attributes);
|
|
|
|
// ensure there are points to work with
|
|
if (!resultCloud->isDefined()) {
|
|
cout << "Pointcloud2 is undefined." << endl;
|
|
return 0;
|
|
} else if (resultCloud->getPointCount() == 0) {
|
|
cout << "Pointcloud2 is empty." << endl;
|
|
return 0;
|
|
}
|
|
|
|
// the mmrCloud constructs an MMRTree which stores the respective first
|
|
// point of a point sequence. The size of a sequence is
|
|
// determined by the number of points stored in each RTree node,
|
|
// (theoretically, max. 76 points, usually about 70% of that).
|
|
// Depending on params._maxMMRCloudSequenceLength, these sequences are
|
|
// further refined into several sequences */
|
|
timer.startTask("create MMRCloud");
|
|
if (REPORT_TO_CONSOLE) {
|
|
cout << "reading all " << formatInt(pointCount) << " points "
|
|
"to main memory" << endl;
|
|
}
|
|
std::shared_ptr<MMRCloud> mmrCloud = std::make_shared<MMRCloud>(resultCloud,
|
|
ParamsAnalyzeGeom::_maxMMRCloudSequenceLength);
|
|
if (REPORT_TO_CONSOLE) {
|
|
mmrCloud->_tree->printStats(pointCount,
|
|
sizeof(DbScanPoint<DIMENSIONS>));
|
|
}
|
|
|
|
// "conceptual parameters" that relate to how the
|
|
// analysis is done
|
|
double typicalPointDist = 0.0; // = Pointcloud2::TYPICAL_POINT_DISTANCE;
|
|
if (typicalPointDist <= 0.0) {
|
|
size_t sampleSize = pointCount / 40;
|
|
sampleSize = std::max(sampleSize, size_t(1000));
|
|
typicalPointDist = mmrCloud->estimateTypicalPointDistance(
|
|
sampleSize);
|
|
}
|
|
const ParamsAnalyzeGeom params = ParamsAnalyzeGeom(
|
|
resultCloud->getPointCount(), typicalPointDist);
|
|
|
|
// if the sourceCloud already contained ObjIDs, the points that were
|
|
// assigned to ObjIDs are excluded by setting their bit to "false"
|
|
// in the bitMask; maxObjID is set to the previous maximum ObjID:
|
|
size_t totalPointCount = resultCloud->getPointCount();
|
|
size_t unassignedPointCount = totalPointCount;
|
|
int maxObjID = 0;
|
|
if (hasOldObjIDs) {
|
|
timer.startTask("get previous maximum ObjID");
|
|
maxObjID = resultCloud->getMaxValueOfIntAttr(attrIndexObjID, 0,
|
|
mmrCloud->_bitMask);
|
|
unassignedPointCount = mmrCloud->_bitMask->getTrueCount();
|
|
}
|
|
|
|
// ensure there are points to work with
|
|
bool overwriteObjIDs = false;
|
|
if (unassignedPointCount == 0) {
|
|
cout << "All " << formatInt(totalPointCount) << " points in this "
|
|
"cloud have been assigned to an ObjID in a "
|
|
"previous analysis.";
|
|
cout << endl << "ObjIDs are reset for a new analysis." << endl;
|
|
unassignedPointCount = totalPointCount;
|
|
maxObjID = 0;
|
|
mmrCloud->_bitMask->initializeAll(true);
|
|
overwriteObjIDs = true;
|
|
} else if (hasOldObjIDs) {
|
|
cout << formatInt(totalPointCount - unassignedPointCount) << " point "
|
|
"nave been assigned to " << maxObjID << " objects in a "
|
|
"previous analysis. " << endl;
|
|
cout << "Restricting analysis to the remaining " <<
|
|
formatInt(unassignedPointCount) << " points." << endl;
|
|
}
|
|
|
|
// the ObjectIdManager will provide us with new object ids as needed,
|
|
// and temporarily store the assignment of each point to an object id
|
|
timer.startTask("create ObjectIdManager");
|
|
std::shared_ptr<ObjectIdManager> objectManager
|
|
= std::make_shared<ObjectIdManager>(
|
|
resultCloud->getPointCount(), maxObjID, overwriteObjIDs,
|
|
attrIndexObjID, attrIndexCatID);
|
|
|
|
// we are doing a randomized Hough-transformation, so this sample
|
|
// is random - the _bitMask excludes points already assigned to objects
|
|
// in a previous analysis
|
|
std::stringstream task;
|
|
task << "take sample of " << formatInt(params._sampleSize )<< " points";
|
|
if (REPORT_TO_CONSOLE) {
|
|
cout << "taking sample of " << formatInt(params._sampleSize)
|
|
<< " points" << endl;
|
|
}
|
|
timer.startTask(task.str());
|
|
std::vector<SamplePoint> sample = op_analyzeGeom::takeSample(
|
|
mmrCloud->_points, params._sampleSize, mmrCloud->_bitMask);
|
|
|
|
// now we'll get the primitives in the order in which they shall be
|
|
// identified (e.g. 1. Planes, 2. Spheres, 3. Cylinders, 4. Scones)
|
|
// and iterate over them
|
|
if (REPORT_TO_CONSOLE) {
|
|
cout << endl << "starting analysis with parameters:" << endl
|
|
<< "- minimum object size : "
|
|
<< params._minimumObjectExtent << endl
|
|
<< "- typical point distance in cloud : "
|
|
<< params._typicalPointDistance << endl
|
|
<< "- typical point distance in sample: "
|
|
<< params._typicalSampleDistance << endl
|
|
<< "- neighborhood diameter : "
|
|
<< params._neighborhoodDiameter << endl;
|
|
}
|
|
|
|
// GpSphere will be analyzed first as currently it is the only finite
|
|
// shape and it is less likely for points to get wrongly assigned to a
|
|
// finite sphere than to an infinite plane, cylinder or cone)
|
|
std::vector<std::shared_ptr<AbstractGeomPrimitive>> primitives =
|
|
op_analyzeGeom::getAllPrimitives();
|
|
|
|
|
|
for (std::shared_ptr<AbstractGeomPrimitive> primitive : primitives) {
|
|
|
|
// find primitives of the current type in the sample and match
|
|
// the _points to the identified shapes by setting their
|
|
// _idOfBiggestObject in the objectManager to a new object id
|
|
// unless they were already assigned to a bigger object
|
|
primitive->projectClusterSetObjID(sample, mmrCloud, params,
|
|
objectManager, timer);
|
|
|
|
// tell the objectManager this type of primitives is done with
|
|
objectManager->categoryFinished();
|
|
}
|
|
|
|
// make results (ObjID and CatID) persistent
|
|
timer.startTask("save results");
|
|
if (REPORT_TO_CONSOLE) {
|
|
cout << endl << "saving results to cloud" << endl;
|
|
}
|
|
objectManager->appointFinalObjIDs(params._requiredObjSize);
|
|
op_analyzeGeom::saveResultsToPointcloud(resultCloud,
|
|
mmrCloud,
|
|
objectManager);
|
|
|
|
// print summary
|
|
cout << endl << "analyzeGeom summary:" << endl;
|
|
size_t categoryId = 0;
|
|
size_t unassignedLeft = unassignedPointCount;
|
|
for (std::shared_ptr<AbstractGeomPrimitive> primitive : primitives) {
|
|
++categoryId;
|
|
size_t assigned = objectManager->getAssignedPointCount(categoryId);
|
|
cout << " * " << objectManager->getObjectCount(categoryId) << " "
|
|
<< primitive->getCaption(true) << " with "
|
|
<< formatInt(assigned) << " points found" << endl;
|
|
unassignedLeft -= assigned;
|
|
}
|
|
if (unassignedLeft == 0) {
|
|
cout << "All " << formatInt(unassignedPointCount) << " points were "
|
|
"assigned." << endl;
|
|
} else {
|
|
cout << formatInt(unassignedLeft) << " of "
|
|
<< formatInt(unassignedPointCount) << " points unassigned."
|
|
<< endl;
|
|
}
|
|
// print runtime summary
|
|
std::cout << timer.getReportForAllTasks();
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
1.3 Operator specification
|
|
|
|
*/
|
|
std::string op_analyzeGeom::getOperatorSpec(){
|
|
return OperatorSpec(
|
|
" pointcloud2 -> pointcloud2(...,ObjID,CatID)",
|
|
" pc2 analyzeGeom",
|
|
" Returns classified pc2 ",
|
|
" query pc2 analyzeGeom"
|
|
).getStr();
|
|
}
|
|
|
|
/*
|
|
1.4 Operator methods
|
|
|
|
*/
|
|
std::shared_ptr<Operator> op_analyzeGeom::getOperator(){
|
|
return std::make_shared<Operator>("analyzeGeom",
|
|
getOperatorSpec(),
|
|
&op_analyzeGeom::analyzeGeomVM,
|
|
Operator::SimpleSelect,
|
|
&op_analyzeGeom::analyzeGeomTM);
|
|
}
|
|
|
|
/*
|
|
Does what the signature suggests: returns a random
|
|
selection of parameter points of size size\_of\_sample,
|
|
but none that are flipped in the corresponding bit-mask.
|
|
|
|
*/
|
|
std::vector<PointBase<DIMENSIONS>> op_analyzeGeom::takeSample(
|
|
const std::shared_ptr<std::vector<DbScanPoint<DIMENSIONS>>>& points,
|
|
const size_t size_of_sample,
|
|
const std::shared_ptr<BitArray> bit_mask) {
|
|
|
|
std::vector<PointBase<DIMENSIONS>> result;
|
|
result.reserve(size_of_sample);
|
|
|
|
// get an iterator that returns a random selection of (size_of_sample)
|
|
// ids of the points that are still "true" in the bitArray (i.e. a
|
|
// random selection of the points that have not yet been assigned to a
|
|
// geometrical object by opAnalyzeGeom)
|
|
auto iter = bit_mask->getIterator(size_of_sample);
|
|
int id;
|
|
while ((id = iter->next()) >= 0) {
|
|
PointBase<DIMENSIONS> point = points->at(id).toPointBase();
|
|
result.push_back(point);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
|
|
/*
|
|
Makes the ObjIDs and CatIDs which were calculated in the analysis
|
|
persistent by storing them in the tuples associated with the points of
|
|
the Pointcloud2.
|
|
|
|
*/
|
|
void op_analyzeGeom::saveResultsToPointcloud (
|
|
Pointcloud2* _cloud,
|
|
std::shared_ptr<MMRCloud> mmr_cloud,
|
|
std::shared_ptr<ObjectIdManager> objManager) {
|
|
|
|
bool writeAll = objManager->mustOverwriteOldObjIDs();
|
|
if (!writeAll && !objManager->wereObjectIdsCreated())
|
|
return;
|
|
|
|
const int attrIndexObjID = objManager->_attrIndexObjID;
|
|
const int attrIndexCatID = objManager->_attrIndexCatID;
|
|
std::vector<int> attrIndices;
|
|
attrIndices.push_back(attrIndexObjID);
|
|
attrIndices.push_back(attrIndexCatID);
|
|
for (size_t smiId = 1; smiId < mmr_cloud->_indexOfSmiId.size(); ++smiId) {
|
|
size_t index = mmr_cloud->_indexOfSmiId[smiId];
|
|
if (index == 0)
|
|
continue;
|
|
|
|
// get the object id and the corresponding category id
|
|
// (1 = planes, 2 = spheres etc)
|
|
int objID = objManager->getFinalObjID(index);
|
|
if (!writeAll && objID <= 0)
|
|
continue;
|
|
|
|
// continue even if objID == 0 in case all points had been assigned
|
|
// previously and are reassigned now
|
|
int catID = objManager->getCategoryID(index);
|
|
|
|
PcPoint point;
|
|
_cloud->getPoint(smiId, &point);
|
|
if (point._tupleId == 0) {
|
|
assert (false); // we expect the operator to create the tuples
|
|
// before calling this method.
|
|
// create a tuple to store properties
|
|
Tuple* tuple = _cloud->createEmptyTuple();
|
|
tuple->PutAttribute(attrIndexObjID, new CcInt(objID));
|
|
tuple->PutAttribute(attrIndexCatID, new CcInt(catID));
|
|
_cloud->insertTupleForExistingPoint(tuple, point, smiId);
|
|
// do NOT call tuple->DeleteIfAllowed(); here!
|
|
} else {
|
|
// change an existing tuple
|
|
Tuple* tuple = _cloud->getTuple(point._tupleId);
|
|
std::vector<Attribute*> attrs;
|
|
attrs.push_back(new CcInt(true, objID));
|
|
attrs.push_back(new CcInt(true, catID));
|
|
_cloud->updateTuple(tuple, attrIndices, attrs);
|
|
tuple->DeleteIfAllowed();
|
|
}
|
|
}
|
|
}
|
|
|
|
std::vector<std::shared_ptr<AbstractGeomPrimitive>>
|
|
op_analyzeGeom::getAllPrimitives(){
|
|
std::vector<std::shared_ptr<AbstractGeomPrimitive>> result;
|
|
result.push_back(std::make_shared<GpPlane>());
|
|
result.push_back(std::make_shared<GpSphere>());
|
|
result.push_back(std::make_shared<GpCylinder>());
|
|
result.push_back(std::make_shared<GpCone>());
|
|
return result;
|
|
}
|