Esempio n. 1
0
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;
}
Esempio n. 2
0
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++;
}