inline void BinMD::binMDBox(MDBox<MDE, nd> *box, const size_t *const chunkMin,
const size_t *const chunkMax) {
// An array to hold the rotated/transformed coordinates
coord_t *outCenter = new coord_t[m_outD];
// Evaluate whether the entire box is in the same bin
if (box->getNPoints() > (1 << nd) * 2) {
// There is a check that the number of events is enough for it to make sense
// to do all this processing.
size_t numVertexes = 0;
coord_t *vertexes = box->getVertexesArray(numVertexes);
// All vertexes have to be within THE SAME BIN = have the same linear index.
size_t lastLinearIndex = 0;
bool badOne = false;
for (size_t i = 0; i < numVertexes; i++) {
// Cache the center of the event (again for speed)
const coord_t *inCenter = vertexes + i * nd;
// Now transform to the output dimensions
m_transform->apply(inCenter, outCenter);
// std::cout << "Input coord " << VMD(nd,inCenter) << " transformed to "
// << VMD(nd,outCenter) << std::endl;
// To build up the linear index
size_t linearIndex = 0;
// To mark VERTEXES outside range
badOne = false;
/// Loop through the dimensions on which we bin
for (size_t bd = 0; bd < m_outD; bd++) {
// What is the bin index in that dimension
coord_t x = outCenter[bd];
size_t ix = size_t(x);
// Within range (for this chunk)?
if ((x >= 0) && (ix >= chunkMin[bd]) && (ix < chunkMax[bd])) {
// Build up the linear index
linearIndex += indexMultiplier[bd] * ix;
} else {
// Outside the range
badOne = true;
break;
}
} // (for each dim in MDHisto)
// Is the vertex at the same place as the last one?
if (!badOne) {
if ((i > 0) && (linearIndex != lastLinearIndex)) {
// Change of index
badOne = true;
break;
}
lastLinearIndex = linearIndex;
}
// Was the vertex completely outside the range?
if (badOne)
break;
} // (for each vertex)
delete[] vertexes;
if (!badOne) {
// Yes, the entire box is within a single bin
// std::cout << "Box at " << box->getExtentsStr() << " is within a
// single bin.\n";
// Add the CACHED signal from the entire box
signals[lastLinearIndex] += box->getSignal();
errors[lastLinearIndex] += box->getErrorSquared();
// TODO: If MDEvents get a weight, this would need to get the summed
// weight.
numEvents[lastLinearIndex] += static_cast<signal_t>(box->getNPoints());
// And don't bother looking at each event. This may save lots of time
// loading from disk.
delete[] outCenter;
return;
}
}
// If you get here, you could not determine that the entire box was in the
// same bin.
// So you need to iterate through events.
const std::vector<MDE> &events = box->getConstEvents();
typename std::vector<MDE>::const_iterator it = events.begin();
typename std::vector<MDE>::const_iterator it_end = events.end();
for (; it != it_end; it++) {
// Cache the center of the event (again for speed)
const coord_t *inCenter = it->getCenter();
// Now transform to the output dimensions
m_transform->apply(inCenter, outCenter);
// To build up the linear index
size_t linearIndex = 0;
// To mark events outside range
bool badOne = false;
/// Loop through the dimensions on which we bin
for (size_t bd = 0; bd < m_outD; bd++) {
// What is the bin index in that dimension
coord_t x = outCenter[bd];
size_t ix = size_t(x);
// Within range (for this chunk)?
if ((x >= 0) && (ix >= chunkMin[bd]) && (ix < chunkMax[bd])) {
// Build up the linear index
linearIndex += indexMultiplier[bd] * ix;
} else {
// Outside the range
badOne = true;
break;
}
} // (for each dim in MDHisto)
if (!badOne) {
// Sum the signals as doubles to preserve precision
signals[linearIndex] += static_cast<signal_t>(it->getSignal());
errors[linearIndex] += static_cast<signal_t>(it->getErrorSquared());
// TODO: If MDEvents get a weight, this would need to get the summed
// weight.
numEvents[linearIndex] += 1.0;
}
}
// Done with the events list
box->releaseEvents();
delete[] outCenter;
}
void SliceMD::slice(typename MDEventWorkspace<MDE, nd>::sptr ws) {
// Create the ouput workspace
typename MDEventWorkspace<OMDE, ond>::sptr outWS(
new MDEventWorkspace<OMDE, ond>());
for (size_t od = 0; od < m_binDimensions.size(); od++) {
outWS->addDimension(m_binDimensions[od]);
}
outWS->setCoordinateSystem(ws->getSpecialCoordinateSystem());
outWS->initialize();
// Copy settings from the original box controller
BoxController_sptr bc = ws->getBoxController();
// store wrute buffer size for the future
// uint64_t writeBufSize =
// bc->getFileIO()getDiskBuffer().getWriteBufferSize();
// and disable write buffer (if any) for input MD Events for this algorithm
// purposes;
// bc->setCacheParameters(1,0);
BoxController_sptr obc = outWS->getBoxController();
// Use the "number of bins" as the "split into" parameter
for (size_t od = 0; od < m_binDimensions.size(); od++)
obc->setSplitInto(od, m_binDimensions[od]->getNBins());
obc->setSplitThreshold(bc->getSplitThreshold());
bool bTakeDepthFromInputWorkspace =
getProperty("TakeMaxRecursionDepthFromInput");
int tempDepth = getProperty("MaxRecursionDepth");
size_t maxDepth =
bTakeDepthFromInputWorkspace ? bc->getMaxDepth() : size_t(tempDepth);
obc->setMaxDepth(maxDepth);
// size_t outputSize = writeBufSize;
// obc->setCacheParameters(sizeof(OMDE),outputSize);
obc->resetNumBoxes();
// Perform the first box splitting
outWS->splitBox();
size_t lastNumBoxes = obc->getTotalNumMDBoxes();
// --- File back end ? ----------------
std::string filename = getProperty("OutputFilename");
if (!filename.empty()) {
// First save to the NXS file
g_log.notice() << "Running SaveMD to create file back-end" << std::endl;
IAlgorithm_sptr alg = createChildAlgorithm("SaveMD");
alg->setPropertyValue("Filename", filename);
alg->setProperty("InputWorkspace", outWS);
alg->setProperty("MakeFileBacked", true);
alg->executeAsChildAlg();
if (!obc->isFileBacked())
throw std::runtime_error("SliceMD with file-backed output: Can not set "
"up file-backed output workspace ");
auto IOptr = obc->getFileIO();
size_t outBufSize = IOptr->getWriteBufferSize();
// the buffer size for resulting workspace; reasonable size is at least 10
// data chunk sizes (nice to verify)
if (outBufSize < 10 * IOptr->getDataChunk()) {
outBufSize = 10 * IOptr->getDataChunk();
IOptr->setWriteBufferSize(outBufSize);
}
}
// Function defining which events (in the input dimensions) to place in the
// output
MDImplicitFunction *function = this->getImplicitFunctionForChunk(NULL, NULL);
std::vector<API::IMDNode *> boxes;
// Leaf-only; no depth limit; with the implicit function passed to it.
ws->getBox()->getBoxes(boxes, 1000, true, function);
// Sort boxes by file position IF file backed. This reduces seeking time,
// hopefully.
bool fileBackedWS = bc->isFileBacked();
if (fileBackedWS)
API::IMDNode::sortObjByID(boxes);
Progress *prog = new Progress(this, 0.0, 1.0, boxes.size());
// The root of the output workspace
MDBoxBase<OMDE, ond> *outRootBox = outWS->getBox();
// if target workspace has events, we should count them as added
uint64_t totalAdded = outWS->getNEvents();
uint64_t numSinceSplit = 0;
// Go through every box for this chunk.
// PARALLEL_FOR_IF( !bc->isFileBacked() )
for (int i = 0; i < int(boxes.size()); i++) {
MDBox<MDE, nd> *box = dynamic_cast<MDBox<MDE, nd> *>(boxes[i]);
// Perform the binning in this separate method.
if (box) {
// An array to hold the rotated/transformed coordinates
coord_t outCenter[ond];
const std::vector<MDE> &events = box->getConstEvents();
typename std::vector<MDE>::const_iterator it = events.begin();
typename std::vector<MDE>::const_iterator it_end = events.end();
for (; it != it_end; it++) {
// Cache the center of the event (again for speed)
const coord_t *inCenter = it->getCenter();
if (function->isPointContained(inCenter)) {
// Now transform to the output dimensions
m_transformFromOriginal->apply(inCenter, outCenter);
// Create the event
OMDE newEvent(it->getSignal(), it->getErrorSquared(), outCenter);
// Copy extra data, if any
copyEvent(*it, newEvent);
// Add it to the workspace
outRootBox->addEvent(newEvent);
numSinceSplit++;
}
}
box->releaseEvents();
// Ask BC if one needs to split boxes
if (obc->shouldSplitBoxes(totalAdded, numSinceSplit, lastNumBoxes))
// if (numSinceSplit > 20000000 || (i == int(boxes.size()-1)))
{
// This splits up all the boxes according to split thresholds and sizes.
Kernel::ThreadScheduler *ts = new ThreadSchedulerFIFO();
ThreadPool tp(ts);
outWS->splitAllIfNeeded(ts);
tp.joinAll();
// Accumulate stats
totalAdded += numSinceSplit;
numSinceSplit = 0;
lastNumBoxes = obc->getTotalNumMDBoxes();
// Progress reporting
if (!fileBackedWS)
prog->report(i);
}
if (fileBackedWS) {
if (!(i % 10))
prog->report(i);
}
} // is box
} // for each box in the vector
prog->report();
outWS->splitAllIfNeeded(NULL);
// Refresh all cache.
outWS->refreshCache();
g_log.notice() << totalAdded << " " << OMDE::getTypeName()
<< "'s added to the output workspace." << std::endl;
if (outWS->isFileBacked()) {
// Update the file-back-end
g_log.notice() << "Running SaveMD" << std::endl;
IAlgorithm_sptr alg = createChildAlgorithm("SaveMD");
alg->setProperty("UpdateFileBackEnd", true);
alg->setProperty("InputWorkspace", outWS);
alg->executeAsChildAlg();
}
// return the size of the input workspace write buffer to its initial value
// bc->setCacheParameters(sizeof(MDE),writeBufSize);
this->setProperty("OutputWorkspace",
boost::dynamic_pointer_cast<IMDEventWorkspace>(outWS));
delete prog;
}
template<typename PointT> void PersonClusterVisualizer<PointT>::visualize(const std::string& nameSpace, std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
// Save us some computation time if there are no subscribers.
if(_markerArrayPublisher.getNumSubscribers() == 0) return;
// Look up values for this particular namespace
std::set<unsigned int>& oldMarkerIds = _oldMarkerIds[nameSpace];
unsigned int& lastMarkerId = _lastMarkerIds[nameSpace];
visualization_msgs::MarkerArray markerArray;
// Remove old clusters
for(std::set<unsigned int>::const_iterator it = oldMarkerIds.begin(); it != oldMarkerIds.end(); ++it) {
visualization_msgs::Marker oldClusterMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame);
oldClusterMarker.id = *it;
oldClusterMarker.action = visualization_msgs::Marker::DELETE;
markerArray.markers.push_back(oldClusterMarker);
}
oldMarkerIds.clear();
double lifetime = 1.0 / 15;
unsigned int k = lastMarkerId + 1;
for (typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) {
//
// 3D bounding box
//
visualization_msgs::Marker clusterMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame);
clusterMarker.id = k;
clusterMarker.type = visualization_msgs::Marker::CUBE;
clusterMarker.color = MarkerUtils::getPaletteColor(_visualizationCounter);
clusterMarker.color.a = 0.35;
clusterMarker.scale.x = it->getMax().x() - it->getMin().x();
clusterMarker.scale.y = it->getHeight(); // note that in the detection frame, y is up
clusterMarker.scale.z = it->getMax().z() - it->getMin().z();
clusterMarker.lifetime.fromSec(lifetime); // just to be safe
Eigen::Vector3f tcenter = it->getTCenter();
tf::poseEigenToMsg(Eigen::Translation3d(tcenter.cast<double>()) * Eigen::Affine3d::Identity(), clusterMarker.pose);
markerArray.markers.push_back(clusterMarker);
oldMarkerIds.insert(clusterMarker.id);
k++;
//
// Center of gravity
//
visualization_msgs::Marker cogMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame);
cogMarker.id = k;
cogMarker.type = visualization_msgs::Marker::SPHERE;
cogMarker.color = MarkerUtils::getPaletteColor(_visualizationCounter);
cogMarker.color.a = 1.0;
cogMarker.scale.x = cogMarker.scale.y = cogMarker.scale.z = 0.1; // 10 cm
cogMarker.lifetime.fromSec(lifetime); // just to be safe
Eigen::Vector3f center = it->getCenter();
tf::poseEigenToMsg(Eigen::Translation3d(center.cast<double>()) * Eigen::Affine3d::Identity(), cogMarker.pose);
markerArray.markers.push_back(cogMarker);
oldMarkerIds.insert(cogMarker.id);
k++;
//
// Person confidence (if set)
//
if(it->getPersonConfidence() == it->getPersonConfidence()) { // must not be NaN
std::stringstream confidenceStr; confidenceStr << std::fixed << std::setprecision(1) << it->getPersonConfidence();
visualization_msgs::Marker confidenceMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame);
confidenceMarker.id = k;
confidenceMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
//confidenceMarker.color = MarkerUtils::getPaletteColor(_visualizationCounter);
confidenceMarker.color.r = confidenceMarker.color.g = confidenceMarker.color.b = 1.0;
confidenceMarker.color.a = 1.0;
confidenceMarker.scale.z = 0.2; // height of letters in m(?)
confidenceMarker.lifetime.fromSec(lifetime); // just to be safe
confidenceMarker.text = confidenceStr.str();
Eigen::Vector3f top = it->getTop();
tf::poseEigenToMsg(Eigen::Translation3d(top.cast<double>()) * Eigen::Affine3d::Identity(), confidenceMarker.pose);
markerArray.markers.push_back(confidenceMarker);
oldMarkerIds.insert(confidenceMarker.id);
k++;
}
}
lastMarkerId = k;
_markerArrayPublisher.publish(markerArray);
_visualizationCounter++;
}