text:
#include <filesystem>
#include <fstream>
#include <iostream>
#include <string>
#include <utility>
#include <vector>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <boost/property_tree/json_parser.hpp>
#include <openssl/md5.h>
#include <fmt/printf.h>

#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/gzip_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>

#include <TString.h>
#include <TSystem.h>
#include <TBufferFile.h>

#include "zlib.h"
#include "DQMServices/Core/interface/DQMStore.h"
#include "DQMServices/Core/interface/ROOTFilePB.pb.h"
#include "FWCore/Framework/interface/LuminosityBlock.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"
#include "FWCore/ServiceRegistry/interface/Service.h"

#include "DQMFileSaverPB.h"

using namespace dqm;

DQMFileSaverPB::DQMFileSaverPB(const edm::ParameterSet& ps) : DQMFileSaverBase(ps) {
  fakeFilterUnitMode_ = ps.getUntrackedParameter<bool>("fakeFilterUnitMode", false);
  streamLabel_ = ps.getUntrackedParameter<std::string>("streamLabel", "streamDQMHistograms");
  tag_ = ps.getUntrackedParameter<std::string>("tag", "UNKNOWN");

  transferDestination_ = "";
  mergeType_ = "";

  // If tag is set we're running in a DQM Live mode.
  // Snapshot files will be saved for every client, then they will be merged and uploaded to the new DQM GUI.
  if (tag_ != "UNKNOWN") {
    streamLabel_ = "DQMLive";
  }
}

DQMFileSaverPB::~DQMFileSaverPB() = default;

void DQMFileSaverPB::initRun() const {
  if (!fakeFilterUnitMode_) {
    transferDestination_ = edm::Service<evf::EvFDaqDirector>()->getStreamDestinations(streamLabel_);
    mergeType_ = edm::Service<evf::EvFDaqDirector>()->getStreamMergeType(streamLabel_, evf::MergeTypePB);
  }

  if (!fakeFilterUnitMode_) {
    evf::EvFDaqDirector* daqDirector = (evf::EvFDaqDirector*)(edm::Service<evf::EvFDaqDirector>().operator->());
    const std::string initFileName = daqDirector->getInitFilePath(streamLabel_);
    std::ofstream file(initFileName);
    file.close();
  }
}

void DQMFileSaverPB::saveLumi(const FileParameters& fp) const {
  // get from DAQ2 services where to store the files according to their format
  namespace bpt = boost::property_tree;

  std::string openJsonFilePathName;
  std::string jsonFilePathName;
  std::string openHistoFilePathName;
  std::string histoFilePathName;

  evf::FastMonitoringService* fms = nullptr;
  edm::Service<DQMStore> store;

  // create the files names
  if (fakeFilterUnitMode_) {
    std::string runDir = fmt::sprintf("%s/run%06d", fp.path_, fp.run_);
    std::string baseName = "";
    std::filesystem::create_directories(runDir);
    // If tag is configured, append it to the name of the resulting file.
    // This differentiates files saved by different clients.
    // If tag is not configured, we don't add it at all to keep the old behaviour unchanged.
    if (tag_ == "UNKNOWN") {
      baseName = fmt::sprintf("%s/run%06d_ls%04d_%s", runDir, fp.run_, fp.lumi_, streamLabel_);
    } else {
      baseName = fmt::sprintf("%s/run%06d_%s_%s", runDir, fp.run_, tag_, streamLabel_);
    }

    jsonFilePathName = baseName + ".jsn";
    openJsonFilePathName = jsonFilePathName + ".open";

    histoFilePathName = baseName + ".pb";
    openHistoFilePathName = histoFilePathName + ".open";
  } else {
    openJsonFilePathName = edm::Service<evf::EvFDaqDirector>()->getOpenOutputJsonFilePath(fp.lumi_, streamLabel_);
    jsonFilePathName = edm::Service<evf::EvFDaqDirector>()->getOutputJsonFilePath(fp.lumi_, streamLabel_);

    openHistoFilePathName =
        edm::Service<evf::EvFDaqDirector>()->getOpenProtocolBufferHistogramFilePath(fp.lumi_, streamLabel_);
    histoFilePathName = edm::Service<evf::EvFDaqDirector>()->getProtocolBufferHistogramFilePath(fp.lumi_, streamLabel_);

    fms = (evf::FastMonitoringService*)(edm::Service<evf::MicroStateService>().operator->());
  }

  if (fms ? fms->getEventsProcessedForLumi(fp.lumi_) : true) {
    // Save the file in the open directory.
    this->savePB(&*store, openHistoFilePathName, fp.run_, fp.lumi_);

    // Now move the the data and json files into the output directory.
    ::rename(openHistoFilePathName.c_str(), histoFilePathName.c_str());
  }

  // Write the json file in the open directory.
  bpt::ptree pt = fillJson(fp.run_, fp.lumi_, histoFilePathName, transferDestination_, mergeType_, fms);
  write_json(openJsonFilePathName, pt);
  ::rename(openJsonFilePathName.c_str(), jsonFilePathName.c_str());
}

void DQMFileSaverPB::saveRun(const FileParameters& fp) const {
  // no saving for the run
}

boost::property_tree::ptree DQMFileSaverPB::fillJson(int run,
                                                     int lumi,
                                                     const std::string& dataFilePathName,
                                                     const std::string& transferDestinationStr,
                                                     const std::string& mergeTypeStr,
                                                     evf::FastMonitoringService* fms) {
  namespace bpt = boost::property_tree;
  namespace bfs = std::filesystem;

  bpt::ptree pt;

  int hostnameReturn;
  char host[32];
  hostnameReturn = gethostname(host, sizeof(host));
  if (hostnameReturn == -1)
    throw cms::Exception("fillJson") << "Internal error, cannot get host name";

  int pid = getpid();
  std::ostringstream oss_pid;
  oss_pid << pid;

  int nProcessed = fms ? (fms->getEventsProcessedForLumi(lumi)) : -1;

  // Stat the data file: if not there, throw
  std::string dataFileName;
  struct stat dataFileStat;
  dataFileStat.st_size = 0;
  if (nProcessed) {
    if (stat(dataFilePathName.c_str(), &dataFileStat) != 0)
      throw cms::Exception("fillJson") << "Internal error, cannot get data file: " << dataFilePathName;
    // Extract only the data file name from the full path
    dataFileName = bfs::path(dataFilePathName).filename().string();
  }
  // The availability test of the FastMonitoringService was done in the ctor.
  bpt::ptree data;
  bpt::ptree processedEvents, acceptedEvents, errorEvents, bitmask, fileList, fileSize, inputFiles, fileAdler32,
      transferDestination, mergeType, hltErrorEvents;

  processedEvents.put("", nProcessed);  // Processed events
  acceptedEvents.put("", nProcessed);   // Accepted events, same as processed for our purposes

  errorEvents.put("", 0);                               // Error events
  bitmask.put("", 0);                                   // Bitmask of abs of CMSSW return code
  fileList.put("", dataFileName);                       // Data file the information refers to
  fileSize.put("", dataFileStat.st_size);               // Size in bytes of the data file
  inputFiles.put("", "");                               // We do not care about input files!
  fileAdler32.put("", -1);                              // placeholder to match output json definition
  transferDestination.put("", transferDestinationStr);  // SM Transfer destination field
  mergeType.put("", mergeTypeStr);                      // SM Transfer destination field
  hltErrorEvents.put("", 0);                            // Error events

  data.push_back(std::make_pair("", processedEvents));
  data.push_back(std::make_pair("", acceptedEvents));
  data.push_back(std::make_pair("", errorEvents));
  data.push_back(std::make_pair("", bitmask));
  data.push_back(std::make_pair("", fileList));
  data.push_back(std::make_pair("", fileSize));
  data.push_back(std::make_pair("", inputFiles));
  data.push_back(std::make_pair("", fileAdler32));
  data.push_back(std::make_pair("", transferDestination));
  data.push_back(std::make_pair("", mergeType));
  data.push_back(std::make_pair("", hltErrorEvents));

  pt.add_child("data", data);

  if (fms == nullptr) {
    pt.put("definition", "/fakeDefinition.jsn");
  } else {
    // The availability test of the EvFDaqDirector Service was done in the ctor.
    bfs::path outJsonDefName{
        edm::Service<evf::EvFDaqDirector>()->baseRunDir()};  //we assume this file is written bu the EvF Output module
    outJsonDefName /= (std::string("output_") + oss_pid.str() + std::string(".jsd"));
    pt.put("definition", outJsonDefName.string());
  }

  char sourceInfo[64];  //host and pid information
  sprintf(sourceInfo, "%s_%d", host, pid);
  pt.put("source", sourceInfo);

  return pt;
}

void DQMFileSaverPB::fillDescriptions(edm::ConfigurationDescriptions& descriptions) {
  edm::ParameterSetDescription desc;
  desc.setComment("Saves histograms from DQM store, HLT->pb workflow.");

  desc.addUntracked<bool>("fakeFilterUnitMode", false)->setComment("If set, EvFDaqDirector is emulated and not used.");

  desc.addUntracked<std::string>("streamLabel", "streamDQMHistograms")->setComment("Label of the stream.");

  DQMFileSaverBase::fillDescription(desc);

  // Changed to use addDefault instead of add here because previously
  // DQMFileSaverOnline and DQMFileSaverPB both used the module label
  // "saver" which caused conflicting cfi filenames to be generated.
  // add could be used if unique module labels were given.
  descriptions.addDefault(desc);
}

void DQMFileSaverPB::savePB(DQMStore* store, std::string const& filename, int run, int lumi) const {
  using google::protobuf::io::FileOutputStream;
  using google::protobuf::io::GzipOutputStream;
  using google::protobuf::io::StringOutputStream;

  unsigned int nme = 0;

  dqmstorepb::ROOTFilePB dqmstore_message;

  // We save all histograms, indifferent of the lumi flag: even tough we save per lumi, this is a *snapshot*.
  auto mes = store->getAllContents("");
  for (auto const me : mes) {
    TBufferFile buffer(TBufferFile::kWrite);
    if (me->kind() < MonitorElement::Kind::TH1F) {
      TObjString object(me->tagString().c_str());
      buffer.WriteObject(&object);
    } else {
      buffer.WriteObject(me->getRootObject());
    }
    dqmstorepb::ROOTFilePB::Histo& histo = *dqmstore_message.add_histo();
    histo.set_full_pathname(me->getFullname());
    uint32_t flags = 0;
    flags |= (uint32_t)me->kind();
    if (me->getLumiFlag())
      flags |= DQMNet::DQM_PROP_LUMI;
    if (me->getEfficiencyFlag())
      flags |= DQMNet::DQM_PROP_EFFICIENCY_PLOT;
    histo.set_flags(flags);
    histo.set_size(buffer.Length());

    if (tag_ == "UNKNOWN") {
      histo.set_streamed_histo((void const*)buffer.Buffer(), buffer.Length());
    } else {
      // Compress ME blob with zlib
      int maxOutputSize = this->getMaxCompressedSize(buffer.Length());
      char compression_output[maxOutputSize];
      uLong total_out = this->compressME(buffer, maxOutputSize, compression_output);
      histo.set_streamed_histo(compression_output, total_out);
    }

    // Save quality reports
    for (const auto& qr : me->getQReports()) {
      std::string result;
      // TODO: 64 is likely too short; memory corruption in the old code?
      char buf[64];
      std::snprintf(buf, sizeof(buf), "qr=st:%d:%.*g:", qr->getStatus(), DBL_DIG + 2, qr->getQTresult());
      result = '<' + me->getName() + '.' + qr->getQRName() + '>';
      result += buf;
      result += qr->getAlgorithm() + ':' + qr->getMessage();
      result += "</" + me->getName() + '.' + qr->getQRName() + '>';
      TObjString str(result.c_str());

      dqmstorepb::ROOTFilePB::Histo& qr_histo = *dqmstore_message.add_histo();
      TBufferFile qr_buffer(TBufferFile::kWrite);
      qr_buffer.WriteObject(&str);
      qr_histo.set_full_pathname(me->getFullname() + '.' + qr->getQRName());
      qr_histo.set_flags(static_cast<uint32_t>(MonitorElement::Kind::STRING));
      qr_histo.set_size(qr_buffer.Length());
      // qr_histo.set_streamed_histo((void const*)qr_buffer.Buffer(), qr_buffer.Length());

      if (tag_ == "UNKNOWN") {
        qr_histo.set_streamed_histo((void const*)qr_buffer.Buffer(), qr_buffer.Length());
      } else {
        // Compress ME blob with zlib
        int maxOutputSize = this->getMaxCompressedSize(qr_buffer.Length());
        char compression_output[maxOutputSize];
        uLong total_out = this->compressME(qr_buffer, maxOutputSize, compression_output);
        qr_histo.set_streamed_histo(compression_output, total_out);
      }
    }

    // Save efficiency tag, if any.
    // XXX not supported by protobuf files.

    // Save tag if any.
    // XXX not supported by protobuf files.

    // Count saved histograms
    ++nme;
  }

  int filedescriptor =
      ::open(filename.c_str(), O_WRONLY | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH);
  FileOutputStream file_stream(filedescriptor);
  if (tag_ == "UNKNOWN") {
    GzipOutputStream::Options options;
    options.format = GzipOutputStream::GZIP;
    options.compression_level = 1;
    GzipOutputStream gzip_stream(&file_stream, options);
    dqmstore_message.SerializeToZeroCopyStream(&gzip_stream);

    // Flush the internal streams
    gzip_stream.Close();
    file_stream.Close();
  } else {
    // We zlib compressed individual MEs so no need to compress the entire file again.
    dqmstore_message.SerializeToZeroCopyStream(&file_stream);

    // Flush the internal stream
    file_stream.Close();
  }

  // Close the file descriptor
  ::close(filedescriptor);

  // Maybe make some noise.
  edm::LogInfo("DQMFileSaverPB") << "savePB: successfully wrote " << nme << " objects  "
                                 << "into DQM file '" << filename << "'\n";
}

int DQMFileSaverPB::getMaxCompressedSize(int bufferSize) const {
  // When input data is very badly compressable, zlib will add overhead instead of reducing the size.
  // There is a minor amount of overhead (6 bytes overall and 5 bytes per 16K block) that is taken
  // into consideration here to find out potential absolute maximum size of the output.
  int n16kBlocks = (bufferSize + 16383) / 16384;  // round up any fraction of a block
  int maxOutputSize = bufferSize + 6 + (n16kBlocks * 5);
  return maxOutputSize;
}

ulong DQMFileSaverPB::compressME(const TBufferFile& buffer, int maxOutputSize, char* compression_output) const {
  z_stream deflateStream;
  deflateStream.zalloc = Z_NULL;
  deflateStream.zfree = Z_NULL;
  deflateStream.opaque = Z_NULL;
  deflateStream.avail_in = (uInt)buffer.Length() + 1;   // size of input, string + terminator
  deflateStream.next_in = (Bytef*)buffer.Buffer();      // input array
  deflateStream.avail_out = (uInt)maxOutputSize;        // size of output
  deflateStream.next_out = (Bytef*)compression_output;  // output array, result will be placed here

  // The actual compression
  deflateInit(&deflateStream, Z_BEST_COMPRESSION);
  deflate(&deflateStream, Z_FINISH);
  deflateEnd(&deflateStream);

  return deflateStream.total_out;
}

#include "FWCore/Framework/interface/MakerMacros.h"
DEFINE_FWK_MODULE(DQMFileSaverPB);

max_stars_repo_name: Purva-Chaudhari/cmssw

max_issues_repo_name: Purva-Chaudhari/cmssw

max_forks_repo_name: Purva-Chaudhari/cmssw


text:
/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Aug 22, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file Measurements.hpp
 * @brief This file contains the templated measurement structs, structs encapsulating
 *        Sensor data and related typedefs.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_MEASUREMENTS_HPP_
#define INCLUDE_OKVIS_MEASUREMENTS_HPP_

#include <deque>
#include <vector>
#include <memory>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Woverloaded-virtual"
#include <opencv2/opencv.hpp>
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#include <Eigen/Dense>
#include <okvis/Time.hpp>
#include <okvis/kinematics/Transformation.hpp>

/// \brief okvis Main namespace of this package.
namespace okvis {
/**
 * \brief Generic measurements
 *
 * They always come with a timestamp such that we can perform
 * any kind of asynchronous operation.
 * \tparam MEASUREMENT_T Measurement data type.
 */
template<class MEASUREMENT_T>
struct Measurement {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  okvis::Time timeStamp;      ///< Measurement timestamp
  MEASUREMENT_T measurement;  ///< Actual measurement.
  int sensorId = -1;          ///< Sensor ID. E.g. camera index in a multicamera setup

  /// \brief Default constructor.
  Measurement()
      : timeStamp(0.0) {
  }
  /**
   * @brief Constructor
   * @param timeStamp_ Measurement timestamp.
   * @param measurement_ Actual measurement.
   * @param sensorId Sensor ID (optional).
   */
  Measurement(const okvis::Time& timeStamp_, const MEASUREMENT_T& measurement_,
              int sensorId = -1)
      : timeStamp(timeStamp_),
        measurement(measurement_),
        sensorId(sensorId) {
  }
};

/// \brief IMU measurements. For now assume they are synchronized:
struct ImuSensorReadings {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /// \brief Default constructor.
  ImuSensorReadings()
      : gyroscopes(),
        accelerometers() {
  }
  /**
   * @brief Constructor.
   * @param gyroscopes_ Gyroscope measurement.
   * @param accelerometers_ Accelerometer measurement.
   */
  ImuSensorReadings(Eigen::Vector3d gyroscopes_,
                    Eigen::Vector3d accelerometers_)
      : gyroscopes(gyroscopes_),
        accelerometers(accelerometers_) {
  }
  Eigen::Vector3d gyroscopes;     ///< Gyroscope measurement.
  Eigen::Vector3d accelerometers; ///< Accelerometer measurement.
};

/// \brief Depth camera measurements. For now assume they are synchronized:
struct DepthCameraData {
  cv::Mat image;  ///< Grayscale/RGB image.
  cv::Mat depthImage; ///< Depth image.
  std::vector<cv::KeyPoint> keypoints;  ///< Keypoints if available.
  bool deliversKeypoints; ///< Are keypoints already delievered in measurement?
};

/// \brief Position measurement.
struct PositionReading {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Vector3d position;           ///< Position measurement.
  Eigen::Vector3d positionOffset;     ///< Position offset.
  Eigen::Matrix3d positionCovariance; ///< Measurement covariance.
};

/// \brief GPS position measurement.
struct GpsPositionReading {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double lat_wgs84;   ///< Latitude in WGS84 coordinate system.
  double lon_wgs84;   ///< Longitude in WGS84 coordiante system.
  double alt_wgs84;   ///< Altitude in WGS84 coordinate system.
  double geoidSeparation; ///< Separation between geoid (MSL) and WGS-84 ellipsoid. [m]
  Eigen::Matrix3d positionCovarianceENU; ///< Measurement covariance. East/North/Up.
};

/// \brief Magnetometer measurement.
struct MagnetometerReading {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  ///< The magnetic flux density measurement. [uT]
  Eigen::Vector3d fluxDensity;
};

/// \brief Barometer measurement.
struct BarometerReading {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double pressure;    ///< Pressure measurement. [Pa]
  double temperature; ///< Temperature. [K]
};

/// \brief Differential pressure sensor measurement.
struct DifferentialPressureReading {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double pressure;  ///< Pressure measurement. [Pa]
  Eigen::Vector3d acceleration_B;  ///< Acceleration in B-frame.
};

// this is how we store raw measurements before more advanced filling into
// data structures happens:
typedef Measurement<ImuSensorReadings> ImuMeasurement;
typedef std::deque<ImuMeasurement, Eigen::aligned_allocator<ImuMeasurement> > ImuMeasurementDeque;
/// \brief Camera measurement.
struct CameraData {
  cv::Mat image;  ///< Image.
  std::vector<cv::KeyPoint> keypoints; ///< Keypoints if available.
  bool deliversKeypoints; ///< Are the keypoints delivered too?
};
/// \brief Keypoint measurement.
struct KeypointData {
  std::vector<cv::KeyPoint> keypoints;  ///< Keypoints.
  std::vector<long unsigned int> landmarkIds; ///< Associated landmark IDs.
  cv::Mat descriptors;  ///< Keypoint descriptors.
};
/// \brief Frame measurement.
struct FrameData {
  typedef std::shared_ptr<okvis::FrameData> Ptr;
  CameraData image; ///< Camera measurement, i.e., image.
  KeypointData keypoints; ///< Keypoints.
};
typedef Measurement<CameraData> CameraMeasurement;
typedef Measurement<FrameData> FrameMeasurement;
typedef Measurement<DepthCameraData> DepthCameraMeasurement;

typedef Measurement<PositionReading> PositionMeasurement;
typedef std::deque<PositionMeasurement,
    Eigen::aligned_allocator<PositionMeasurement> > PositionMeasurementDeque;

typedef Measurement<GpsPositionReading> GpsPositionMeasurement;
typedef std::deque<GpsPositionMeasurement,
    Eigen::aligned_allocator<GpsPositionMeasurement> > GpsPositionMeasurementDeque;

typedef Measurement<MagnetometerReading> MagnetometerMeasurement;
typedef std::deque<MagnetometerMeasurement,
    Eigen::aligned_allocator<MagnetometerMeasurement> > MagnetometerMeasurementDeque;

typedef Measurement<BarometerReading> BarometerMeasurement;

typedef Measurement<DifferentialPressureReading> DifferentialPressureMeasurement;

typedef Eigen::Matrix<double, 9, 1> SpeedAndBias;

}  // namespace okvis

#endif // INCLUDE_OKVIS_MEASUREMENTS_HPP_

ext: hpp

max_stars_repo_name: acyen/okvis

max_issues_repo_name: dywuha/okvis

max_forks_repo_name: dywuha/okvis


text:
// Copyright 2022 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp"

#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

#include <pcl/filters/crop_hull.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/pcl_search.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace
{
std::optional<geometry_msgs::msg::Transform> getTransform(
  const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
  const std::string & target_frame_id, const rclcpp::Time & time)
{
  try {
    geometry_msgs::msg::TransformStamped self_transform_stamped;
    self_transform_stamped = tf_buffer.lookupTransform(
      /*target*/ target_frame_id, /*src*/ source_frame_id, time,
      rclcpp::Duration::from_seconds(0.5));
    return self_transform_stamped.transform;
  } catch (tf2::TransformException & ex) {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("obstacle_pointcloud_based_validator"), ex.what());
    return std::nullopt;
  }
}

bool transformDetectedObjects(
  const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg,
  const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
  autoware_auto_perception_msgs::msg::DetectedObjects & output_msg)
{
  output_msg = input_msg;

  /* transform to world coordinate */
  if (input_msg.header.frame_id != target_frame_id) {
    output_msg.header.frame_id = target_frame_id;
    tf2::Transform tf_target2objects_world;
    tf2::Transform tf_target2objects;
    tf2::Transform tf_objects_world2objects;
    {
      const auto ros_target2objects_world =
        getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
      if (!ros_target2objects_world) {
        return false;
      }
      tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
    }
    for (size_t i = 0; i < output_msg.objects.size(); ++i) {
      tf2::fromMsg(
        output_msg.objects.at(i).kinematics.pose_with_covariance.pose, tf_objects_world2objects);
      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
      tf2::toMsg(tf_target2objects, output_msg.objects.at(i).kinematics.pose_with_covariance.pose);
      // Note: Covariance is not transformed.
    }
  }
  return true;
}

inline pcl::PointXY toPCL(const double x, const double y)
{
  pcl::PointXY pcl_point;
  pcl_point.x = x;
  pcl_point.y = y;
  return pcl_point;
}

inline pcl::PointXY toPCL(const geometry_msgs::msg::Point & point)
{
  return toPCL(point.x, point.y);
}

inline pcl::PointXYZ toXYZ(const pcl::PointXY & point)
{
  return pcl::PointXYZ(point.x, point.y, 0.0);
}

inline pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(
  const pcl::PointCloud<pcl::PointXY>::Ptr & pointcloud)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
  pointcloud_xyz->reserve(pointcloud->size());
  for (const auto & point : *pointcloud) {
    pointcloud_xyz->push_back(toXYZ(point));
  }
  return pointcloud_xyz;
}

}  // namespace

namespace obstacle_pointcloud_based_validator
{
using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
using Shape = autoware_auto_perception_msgs::msg::Shape;

ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
  const rclcpp::NodeOptions & node_options)
: rclcpp::Node("obstacle_pointcloud_based_validator", node_options),
  objects_sub_(this, "~/input/detected_objects", rclcpp::QoS{1}.get_rmw_qos_profile()),
  obstacle_pointcloud_sub_(
    this, "~/input/obstacle_pointcloud",
    rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()),
  tf_buffer_(get_clock()),
  tf_listener_(tf_buffer_),
  sync_(SyncPolicy(10), objects_sub_, obstacle_pointcloud_sub_)
{
  using std::placeholders::_1;
  using std::placeholders::_2;
  sync_.registerCallback(
    std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2));
  objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
    "~/output/objects", rclcpp::QoS{1});

  min_pointcloud_num_ = declare_parameter<int>("min_pointcloud_num", 10);

  const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
  if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
}

void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
  const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud)
{
  autoware_auto_perception_msgs::msg::DetectedObjects output, removed_objects;
  output.header = input_objects->header;
  removed_objects.header = input_objects->header;

  // Transform to pointcloud frame
  autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
  if (!transformDetectedObjects(
        *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_,
        transformed_objects)) {
    // objects_pub_->publish(*input_objects);
    return;
  }

  // Convert to PCL
  pcl::PointCloud<pcl::PointXY>::Ptr obstacle_pointcloud(new pcl::PointCloud<pcl::PointXY>);
  pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud);
  if (obstacle_pointcloud->empty()) {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receieve pointcloud");
    // objects_pub_->publish(*input_objects);
    return;
  }

  // Create Kd-tree to search neighbor pointcloud to reduce cost.
  pcl::search::Search<pcl::PointXY>::Ptr kdtree =
    boost::make_shared<pcl::search::KdTree<pcl::PointXY>>(false);
  kdtree->setInputCloud(obstacle_pointcloud);

  for (size_t i = 0; i < transformed_objects.objects.size(); ++i) {
    const auto & transformed_object = transformed_objects.objects.at(i);
    const auto & object = input_objects->objects.at(i);
    const auto search_radius = getMaxRadius(transformed_object);
    if (!search_radius) {
      output.objects.push_back(object);
      continue;
    }

    // Search neighbor pointcloud to reduce cost.
    pcl::PointCloud<pcl::PointXY>::Ptr neighbor_pointcloud(new pcl::PointCloud<pcl::PointXY>);
    std::vector<int> indices;
    std::vector<float> distances;
    kdtree->radiusSearch(
      toPCL(transformed_object.kinematics.pose_with_covariance.pose.position),
      search_radius.value(), indices, distances);
    for (const auto & index : indices) {
      neighbor_pointcloud->push_back(obstacle_pointcloud->at(index));
    }
    if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud);

    // Filter object that have few pointcloud in them.
    const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud);
    if (num) {
      if (min_pointcloud_num_ <= num.value())
        output.objects.push_back(object);
      else
        removed_objects.objects.push_back(object);
    } else {
      output.objects.push_back(object);
    }
  }

  objects_pub_->publish(output);
  if (debugger_) {
    debugger_->publishRemovedObjects(removed_objects);
    debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);
    debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header);
  }
}

std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon(
  const autoware_auto_perception_msgs::msg::DetectedObject & object,
  const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud)
{
  pcl::PointCloud<pcl::PointXY>::Ptr polygon(new pcl::PointCloud<pcl::PointXY>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
  std::vector<pcl::Vertices> vertices_array;
  pcl::Vertices vertices;

  toPolygon2d(object, polygon);
  if (polygon->empty()) return std::nullopt;

  for (size_t i = 0; i < polygon->size(); ++i) vertices.vertices.push_back(i);
  vertices_array.push_back(vertices);

  pcl::CropHull<pcl::PointXYZ> cropper;  // don't be implemented PointXY by PCL
  cropper.setInputCloud(toXYZ(pointcloud));
  cropper.setDim(2);
  cropper.setHullIndices(vertices_array);
  cropper.setHullCloud(toXYZ(polygon));
  cropper.setCropOutside(true);
  cropper.filter(*cropped_pointcloud);

  if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud);
  return cropped_pointcloud->size();
}

void ObstaclePointCloudBasedValidator::toPolygon2d(
  const autoware_auto_perception_msgs::msg::DetectedObject & object,
  const pcl::PointCloud<pcl::PointXY>::Ptr & polygon)
{
  if (object.shape.type == Shape::BOUNDING_BOX) {
    const auto & pose = object.kinematics.pose_with_covariance.pose;
    double yaw = tf2::getYaw(pose.orientation);
    Eigen::Matrix2d rotation;
    rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
    Eigen::Vector2d offset0, offset1, offset2, offset3;
    offset0 = rotation *
              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
    offset1 = rotation *
              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
    offset2 = rotation *
              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
    offset3 = rotation *
              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
    polygon->push_back(
      pcl::PointXY(toPCL(pose.position.x + offset0.x(), pose.position.y + offset0.y())));
    polygon->push_back(
      pcl::PointXY(toPCL(pose.position.x + offset1.x(), pose.position.y + offset1.y())));
    polygon->push_back(
      pcl::PointXY(toPCL(pose.position.x + offset2.x(), pose.position.y + offset2.y())));
    polygon->push_back(
      pcl::PointXY(toPCL(pose.position.x + offset3.x(), pose.position.y + offset3.y())));
  } else if (object.shape.type == Shape::CYLINDER) {
    const auto & center = object.kinematics.pose_with_covariance.pose.position;
    const auto & radius = object.shape.dimensions.x * 0.5;
    constexpr int n = 6;
    for (int i = 0; i < n; ++i) {
      Eigen::Vector2d point;
      point.x() = std::cos(
                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
                    M_PI / static_cast<double>(n)) *
                    radius +
                  center.x;
      point.y() = std::sin(
                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
                    M_PI / static_cast<double>(n)) *
                    radius +
                  center.y;
      polygon->push_back(toPCL(point.x(), point.y()));
    }
  } else if (object.shape.type == Shape::POLYGON) {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(), *this->get_clock(), 5000, "POLYGON type is not supported");
  }
}

std::optional<float> ObstaclePointCloudBasedValidator::getMaxRadius(
  const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
  if (object.shape.type == Shape::BOUNDING_BOX) {
    return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
  } else if (object.shape.type == Shape::CYLINDER) {
    return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
  } else if (object.shape.type == Shape::POLYGON) {
    float max_dist = 0.0;
    for (const auto & point : object.shape.footprint.points) {
      const float dist = std::hypot(point.x, point.y);
      max_dist = max_dist < dist ? dist : max_dist;
    }
    return max_dist;
  } else {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type");
    return std::nullopt;
  }
}

}  // namespace obstacle_pointcloud_based_validator

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(
  obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator)

ext: cpp


text:
//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/core/utility/config.hpp>
#include <nt2/core/utility/mt19937stream.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

namespace nt2
{
  void mt19937stream_::seed(uint32_t s)
  {
    generator_.seed(s);
  }

  void mt19937stream_::rand(double* data, std::size_t i0, std::size_t i1)
  {
    boost::random::uniform_real_distribution<double> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void mt19937stream_::rand(float*  data, std::size_t i0, std::size_t i1)
  {
    boost::random::uniform_real_distribution<float> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void mt19937stream_::randn(double* data, std::size_t i0, std::size_t i1)
  {
    boost::random::normal_distribution<double> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void mt19937stream_::randn(float*  data, std::size_t i0, std::size_t i1)
  {
    boost::random::normal_distribution<float> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  mt19937stream_::~mt19937stream_()
  {
  }

  nt2::randstream_* mt19937stream()
  {
    static nt2::mt19937stream_ instance;
    return &instance;
  }
}


text:
/* Copyright 2012 Facebook, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "folly/String.h"

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <gflags/gflags.h>

#include <algorithm>
#include <iostream>

#include <boost/algorithm/string.hpp>

#include "folly/Logging.h"
#include "folly/Range.h"
#include "folly/Conv.h"
#include "folly/String.h"
#include "hbase/HbcExtensions.h"
#include "hbase/NativeHbaseClient.h"

const char* usage =
  "Usage: hbc [params] command [command params]\n"
  "\n"
  "Note: all commands rely on either the --zookeeper option or the \n"
  "HBC_ZOOKEEPER environment variable, which contains the ZooKeeper \n"
  "instance where the HBase cluster stores its state.\n"
  "\n"
  "For more details, see:\n"
  "  https://www.intern.facebook.com/intern/wiki/index.php/HbaseClientTool\n";

using namespace facebook;
using namespace std;

using folly::backslashify;
using folly::humanify;
using folly::StringPiece;
using folly::to;

DEFINE_string(zookeeper, getenv("HBC_ZOOKEEPER") ? : "",
              "SMC tier containing the zookeeper hosts, or directly a comma "
              "separated list of host:ports for our zookeeper instance");
DEFINE_string(start_row, "", "Row to begin with in 'scan' mode");
DEFINE_string(end_row, "", "Row to end with in 'scan' mode");
DEFINE_string(column, "", "Column to print (regex)");
DEFINE_int32(num_scan_rows, -1, "Max number of rows to return when scanning; "
             "-1 for all rows");
DEFINE_string(timestamp, "", "Timestamp to restrict returned rows to (uint64)");

DEFINE_bool(print_timestamp, false, "Include timestamps in output");
DEFINE_bool(bypass_special_row_display, false, "always display row results as "
            "key/value rather than using extended output processing");
DEFINE_bool(smart_row_interpretation, true, "if set, hbc will attempt to "
           "guess row key meaning based on prefixes rather than literally; "
            "for instance, keys starting with 0x will be interpreted as hex");
DECLARE_int32(hbase_port);
DEFINE_bool(disable_wal, false,
            "whether Mutation write-ahead-log will be disabled on the server");

namespace facebook { namespace hbase {
const string smartKeyGuess(const string& input) {
  if (!FLAGS_smart_row_interpretation) return input;

  string ret(input);
  if (boost::starts_with(input, "0x")) {
    if (!folly::unhexlify(string(input, 2), ret)) {
      LOG(FATAL) << "Invalid hex string: " << input;
    }
    return ret;
  }
  // TODO(chip): also support backslash encoding, perhaps binary
  // encoding as well?

  return ret;
}

static vector<RowPrinter> row_printers;
bool invokeRowPrinter(const TRowResult* row) {
  if (FLAGS_bypass_special_row_display) return false;

  for (auto& rp : row_printers) {
    if (rp(row)) return true;
  }
  return false;
}

class RegionsCommand : public Subcommand {
public:
  RegionsCommand() : Subcommand("regions") { }

  void Usage(const string& prefix) {
    cout << prefix << "regions [ <table> <table> ... ]" << endl;
  }

  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    vector<Text> tables;

    client->listTables(&tables);
    for (int i = 0; i < tables.size(); ++i) {
      if (args.size() > 0 &&
          find(args.begin(), args.end(), tables[i]) == args.end()) {
        continue;
      }
      cout << "Table: " << tables[i] << endl;

      vector<TRegionInfo> regions;
      client->listTableRegions(&regions, tables[i]);
      for (int j = 0; j < regions.size(); ++j) {
        cout << "  Region " << backslashify(regions[j].name) << endl
             << "    Host : " << regions[j].serverName << ":" << regions[j].port
             << endl
             << "    Start: " << humanify(regions[j].startKey) << endl
             << "    End  : " << humanify(regions[j].endKey) << endl;
      }
      cout << endl;
    }

    return 0;
  }
};

class TablesCommand : public Subcommand {
public:
  TablesCommand() : Subcommand("tables") { }
  void Usage(const string& prefix) {
    cout << prefix << "tables" << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    vector<Text> tables;

    client->listTables(&tables);
    for (int i = 0; i < tables.size(); ++i) {
      cout << "Table: " << tables[i] << endl;
    }

    return 0;
  }
};

class RegionServersCommand : public Subcommand {
public:
  RegionServersCommand() : Subcommand("region_servers") { }
  void Usage(const string& prefix) {
    cout << prefix << "region_servers" << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    vector<pair<string, int>> region_servers;

    client->listRegionServers(&region_servers);
    cout << "Region servers:" << endl;
    for (int i = 0; i < region_servers.size(); ++i) {
      cout << "  " << region_servers[i].first << ":"
           << region_servers[i].second << endl;
    }

    return 0;
  }
};

// A relatively simple routine that attempts to format a cell for
// display.  For printable strings, it just returns the string in
// quotes; for 1, 2, 4, or 8 byte unprintable strings, it turns them
// into the appropriate type of integer, performs an endian swap, and
// returns that.  Otherwise, unprintable strings are hexdumped.
static string FormatCellValue(const TCell& cell) {
  const string& input = cell.value;
  bool has_binary = false;
  for (unsigned char c : input) {
    if (!isprint(c)) {
      has_binary = true;
      break;
    }
  }
  if (!has_binary) {
    return '"' + input + '"';
  }

  string output;
  if (input.size() == 1) {
    int8_t value;
    memcpy(&value, &input[0], 1);
    output = folly::stringPrintf("int8:%d", value);
  } else if (input.size() == 2) {
    int16_t value;
    memcpy(&value, &input[0], 2);
    output =  folly::stringPrintf("int16:%d", htons(value));
  } else if (input.size() == 4) {
    int32_t value;
    memcpy(&value, &input[0], 4);
    output =  folly::stringPrintf("int32_t:%d", htonl(value));
  } else if (input.size() == 8) {
    int64_t value;
    memcpy(&value, &input[0], 8);
    output = folly::stringPrintf("int64_t:%lu", htonll(value));
  } else {
    CHECK(folly::hexlify(cell.value, output));
    output = "0x" + output;
  }

  if (FLAGS_print_timestamp) {
    output += folly::stringPrintf(" (%lu)", cell.timestamp);
  }

  return output;
}

class GetCommand : public Subcommand {
public:
  GetCommand() : Subcommand("get") { }
  void Usage(const string& prefix) {
    cout << prefix << "get <table> <row>" << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() < 2) {
      throw UsageException();
    }

    if (!FLAGS_timestamp.empty() && FLAGS_column.empty()) {
      LOG(ERROR) << "Must include --column when using --timestamp";
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));
    const string row_key(smartKeyGuess(args[1]));

    vector<TRowResult> rows;
    if (FLAGS_column.empty()) {
      client->getRow(&rows, table, row_key);
    } else {
      vector<string> columns;
      columns.push_back(FLAGS_column);
      if (!FLAGS_timestamp.empty()) {
        uint64_t timestamp;
        timestamp = to<uint64_t>(FLAGS_timestamp);
        client->getRowWithColumnsTs(&rows, table, row_key, columns, timestamp);
      } else {
        client->getRowWithColumns(&rows, table, row_key, columns);
      }
    }
    for (TRowResult &row : rows) {
      if (!invokeRowPrinter(&row)) {
        for (auto& kv : row.columns) {
          cout << humanify(kv.first) << ": " << humanify(kv.second.value)
               << endl;
        }
      }
    }

    return 0;
  }
};

class SetCommand : public Subcommand {
public:
  SetCommand() : Subcommand("set") { }
  void Usage(const string& prefix) {
    cout << prefix
         << "set [--disable_wal] <table> <row> column=valuue column=value ..."
         << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() < 3) {
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));
    const string row_key(smartKeyGuess(args[1]));
    vector<BatchMutation> mutations;

    bool write_to_wal = !FLAGS_disable_wal;
    for (int i = 2; i < args.size(); ++i) {
      vector<string> assignment;
      folly::split("=", args[i], assignment);
      if (assignment.size() != 2) {
        LOG(ERROR) << "Invalid assignment: " << args[i];
        throw UsageException();
      }

      mutations.resize(mutations.size() + 1);
      BatchMutation &batch = mutations.back();
      batch.row = row_key;

      Mutation mutation;
      mutation.column = assignment[0];
      mutation.value = assignment[1];
      mutation.isDelete = false;
      mutation.writeToWAL = write_to_wal;
      batch.mutations.push_back(mutation);
    }
    if (!client->mutateRows(table, mutations)) {
      LOG(ERROR) << "One or more mutation failed";
    }

    return 0;
  }
};

class DeleteCommand : public Subcommand {
public:
  DeleteCommand() : Subcommand("delete") { }
  void Usage(const string& prefix) {
    cout << prefix << "delete <table> <row> [column column ...]"
         << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() < 2) {
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));
    const string row_key(smartKeyGuess(args[1]));

    // Use batch mutation just to exercise the more complex codepath.
    vector<BatchMutation> mutations;

    if (args.size() == 2) {
      if (!client->deleteAllRow(table, row_key)) {
        LOG(ERROR) << "Unable to delete entire row: " << row_key;
      }
    } else {
      for (int i = 2; i < args.size(); ++i) {
        if (!client->deleteAll(table, row_key, args[i])) {
          LOG(ERROR) << "Delete failed, row: " << row_key << " cell: "
                     << args[i];
        }
      }
    }

    return 0;
  }
};

class ScanCommand : public Subcommand {
public:
  ScanCommand() : Subcommand("scan") { }
  void Usage(const string& prefix) {
    cout << prefix << "scan [ --start_row R ] [ --end_row R ] "
         << "[ --column C ] [--scan_limit N] <table>" << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() < 1) {
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));
    vector<string> columns;
    if (FLAGS_column.size()) {
      columns.push_back(FLAGS_column);
    }
    std::unique_ptr<Scanner> scanner(
      client->getScanner(table, smartKeyGuess(FLAGS_start_row), columns));

    if (FLAGS_end_row.size()) {
      scanner->setEndRow(smartKeyGuess(FLAGS_end_row));
    }
    scanner->scan();
    TRowResult *result;
    int rows_remaining = FLAGS_num_scan_rows;
    while (scanner->next(&result) && rows_remaining != 0) {
      if (!invokeRowPrinter(result)) {
        cout << "Row " << humanify(result->row) << endl;
        Text column;
        TCell cell;
        for(auto& kv : result->columns) {
          cout << "  " << humanify(kv.first) << " - "
               << FormatCellValue(kv.second) << endl;
        }
      }
      --rows_remaining;
    }

    return 0;
  }
};

class SchemaCommand : public Subcommand {
public:
  SchemaCommand() : Subcommand("schema") { }
  void Usage(const string& prefix) {
    cout << prefix << "schema <table>" << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() != 1) {
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));
    map<string, ColumnDescriptor> columns;
    client->getColumnDescriptors(&columns, table);

    cout << "# Table schema for " << table << endl;
    time_t now = time(NULL);
    char buf[26];
    CHECK(ctime_r(&now, buf));
    cout << "# Dumped on " << buf;
    cout << endl;

#define FIELD_PRINT(f) cout << "  " #f ": " << kv.second.f << endl
    for (auto& kv : columns) {
      cout << "<" << endl;
      FIELD_PRINT(name);
      FIELD_PRINT(maxVersions);
      FIELD_PRINT(compression);
      FIELD_PRINT(inMemory);
      FIELD_PRINT(bloomFilterType);
      FIELD_PRINT(bloomFilterVectorSize);
      FIELD_PRINT(bloomFilterNbHashes);
      FIELD_PRINT(blockCacheEnabled);
      FIELD_PRINT(timeToLive);
      cout << ">" << endl;
    }
#undef FIELD_PRINT

    return 0;
  }
};

class CreateCommand : public Subcommand {
public:
  CreateCommand() : Subcommand("create") { }
  void Usage(const string& prefix) {
    cout << prefix << "create <table> /path/to/schema" << endl;
    cout << prefix << "create <table> (schema read from stdin)" << endl;
  }
  bool parseLine(ColumnDescriptor* column, const string& line) {
    size_t pos = line.find(":");
    if (pos == 0 || pos == string::npos) {
      return false;
    }
    string field(line, 0, pos);
    ++pos;
    while (pos < line.length() && isspace(line[pos])) {
      ++pos;
    }
    string value(line, pos);

    if (field == "name") {
      column->name = value;
    } else if (field == "maxVersions") {
      column->maxVersions = to<int32_t>(value);
    } else if (field == "compression") {
      column->compression = value;
    } else if (field == "inMemory") {
      column->inMemory = to<bool>(value);
    } else if (field == "bloomFilterType") {
      column->bloomFilterType = value;
    } else if (field == "bloomFilterVectorSize") {
      column->bloomFilterVectorSize = to<int32_t>(value);
    } else if (field == "bloomFilterNbHashes") {
      column->bloomFilterNbHashes = to<int32_t>(value);
    } else if (field == "blockCacheEnabled") {
      column->blockCacheEnabled = to<int32_t>(value);
    } else if (field == "timeToLive") {
      column->timeToLive = to<int32_t>(value);
    } else {
      return false;
    }

    return true;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() != 1 && args.size() != 2) {
      throw UsageException();
    }

    const string table(args[0]);

    int fd;
    if (args.size() == 1) {
      fd = STDIN_FILENO;
    }
    else {
      fd = open(args.at(1).c_str(), O_RDONLY);
      if (fd < 0) {
        PLOG(FATAL) << "Unable to open file: " << args[1];
      }
    }

    string contents(65536, '\0');
    size_t size = read(fd, &contents[0], contents.size());
    if (size <= 0 || size == contents.size()) {
      throw FatalException("Unable to read schema definition");
    }
    contents.resize(size);

    vector<StringPiece> lines;
    folly::split("\n", contents, lines);

    vector<ColumnDescriptor> columns;
    bool in_column_definition = false;
    ColumnDescriptor current_column;
    for (auto raw_line : lines) {
      string line = raw_line.toString();
      boost::trim(line);
      if (line.size() == 0) continue;
      if (line[0] == '#') continue;
      if (line == "<") {
        CHECK(!in_column_definition);
        in_column_definition = true;
      } else if (line == ">") {
        CHECK(in_column_definition);
        in_column_definition = false;
        CHECK(!current_column.name.empty());
        columns.push_back(current_column);
      } else {
        CHECK(in_column_definition);
        if (!parseLine(&current_column, line)) {
          return 1;
        }
      }
    }
    CHECK(!in_column_definition);

    client->createTable(table, columns);
    cout << "Create successful!" << endl;

    return 0;
  }
};

class DeleteTableCommand : public Subcommand {
public:
  DeleteTableCommand() : Subcommand("drop") { }
  void Usage(const string& prefix) {
    cout << prefix << "drop <table>" << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() != 1) {
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));

    try {
      client->deleteTable(table);
      cout << "Table deleted." << endl;
      return 0;
    }
    catch (const IOError& e) {
      cout << "Unable to drop table: " << e.message << endl;
    }
    return 1;
  }
};

class TableStatusCommand : public Subcommand {
public:
  TableStatusCommand() : Subcommand("status") { }
  void Usage(const string& prefix) {
    cout << prefix << "status <table>" << endl;
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() != 1) {
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));

    bool result = client->isTableEnabled(table);
    if (result) {
      cout << "Table is enabled." << endl;
    } else {
      cout << "Table is disabled." << endl;
    }

    return !result;
  }
};

class EnableDisableTableCommand : public Subcommand {
public:
  explicit EnableDisableTableCommand(bool enable) :
      Subcommand(enable ? "enable" : "disable"),
      enable_(enable) { }
  void Usage(const string& prefix) {
    if (enable_) {
      cout << prefix << "enable <table>" << endl;
    } else {
      cout << prefix << "disable <table>" << endl;
    }
  }
  int Execute(NativeHbaseClient* client, const vector<string>& args) {
    if (args.size() != 1) {
      throw UsageException();
    }

    const string table(extractTable(client, args, 0));

    if (enable_) {
      client->enableTable(table);
      cout << "Table enabled." << endl;
    } else {
      client->disableTable(table);
      cout << "Table disabled." << endl;
    }

    return 0;
  }
private:
  bool enable_;
};

static vector<CommandGroup*> extension_command_groups;
void RegisterExtensionCommands(CommandGroup* group) {
  extension_command_groups.push_back(group);
}

static ConnectionMaker default_connection_maker =
              [](const string& zk) { return new NativeHbaseClient(zk); };

void RegisterConnectionMaker(ConnectionMaker maker) {
  static bool first_invocation = true;
  CHECK(first_invocation)
    << "Only one call to RegisterConnectionMaker is allowed; are you linking "
    << "incompatible hbc extensions?";
  first_invocation = false;
  default_connection_maker = maker;
}

void RegisterSpecialRowPrinter(RowPrinter printer) {
  row_printers.push_back(printer);
}

} } // namespace facebook::hbase

namespace h = facebook::hbase;

void Usage(const vector<h::CommandGroup*>& command_groups) {
  cout << usage << endl;
  if (!FLAGS_zookeeper.empty()) {
    cout << "Current ZooKeeper instance: " << FLAGS_zookeeper << endl;
  }
  else {
    cout << "No ZooKeeper instance set via command line or environment."
         << endl;
  }
  cout << endl;

  cout << "Subcommands:" << endl;
  for (h::CommandGroup* group : command_groups) {
    cout << "  " << group->name << ": " << endl;
    for (h::Subcommand* command : group->commands) {
      command->Usage("    hbc ");
    }
    cout << endl;
  }
}

// This function is implemented externally; it is different for
// Facebook to tie in our hooks but generally will register new
// subfunctions and connection creation objects.
namespace facebook { namespace hbase {
void initHbcExtensions();
#if NHC_OPEN_SOURCE
void initHbcExtensions() { }
#endif
} }

int main(int argc, char **argv) {
  google::InstallFailureSignalHandler();
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);
  facebook::hbase::initHbcExtensions();

  vector<h::CommandGroup*> command_groups;

  h::CommandGroup* inspection = new h::CommandGroup("Table/Cell inspection");
  inspection->addSubcommand(new h::TablesCommand());
  inspection->addSubcommand(new h::SchemaCommand());
  inspection->addSubcommand(new h::RegionsCommand());
  inspection->addSubcommand(new h::RegionServersCommand());
  command_groups.push_back(inspection);

  h::CommandGroup* data = new h::CommandGroup("Data manipulation");
  data->addSubcommand(new h::ScanCommand());
  data->addSubcommand(new h::GetCommand());
  data->addSubcommand(new h::SetCommand());
  data->addSubcommand(new h::DeleteCommand());
  command_groups.push_back(data);

  h::CommandGroup* schema = new h::CommandGroup("Schema manipulation");
  schema->addSubcommand(new h::CreateCommand());
  schema->addSubcommand(new h::DeleteTableCommand());
  schema->addSubcommand(new h::EnableDisableTableCommand(true));
  schema->addSubcommand(new h::EnableDisableTableCommand(false));
  schema->addSubcommand(new h::TableStatusCommand());
  command_groups.push_back(schema);

  // Extension groups always come after core groups.
  std::copy(h::extension_command_groups.begin(),
            h::extension_command_groups.end(),
            std::back_inserter(command_groups));

  if (argc < 2) {
    Usage(command_groups);
    return 1;
  }

  vector<string> args(argc - 2);
  copy(&(argv[2]), &(argv[argc]), args.begin());

  for (h::CommandGroup* group : command_groups) {
    for (h::Subcommand* command : group->commands) {
      if (command->name == argv[1]) {
        try {
          unique_ptr<h::NativeHbaseClient> client(
            h::default_connection_maker(FLAGS_zookeeper));
          if (!client->connect()) {
            cerr << "Unable to connect to zookeeper: " << FLAGS_zookeeper
                 << endl;
            return 2;
          }
          return command->Execute(client.get(), args);
        } catch (const h::UsageException& e) {
          command->Usage("Error: Usage: hbc ");
          return 1;
        } catch (const h::FatalException& e) {
          cerr << e.what() << endl;
          return 2;
        }
      }
    }
  }

  Usage(command_groups);

  return 1;
}

max_stars_repo_name: hinaria/native-cpp-hbase-client

max_issues_repo_name: hinaria/native-cpp-hbase-client

max_forks_repo_name: hinaria/native-cpp-hbase-client


text:
// Copyright 2017 Emanuele Palazzolo (emanuele.palazzolo@uni-bonn.de)
#include "fastcd/processed_image.h"
#include <algorithm>
#include <string>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Core>
#include "fastcd/camera.h"

namespace fastcd {

ProcessedImage::ProcessedImage(const Image &img,
                               const std::vector<glow::vec4> &depth)
    : Image(img), depth_(depth) {
  inconsistencies_ =
        cv::Mat(raw_image_.rows, raw_image_.cols, CV_32FC1, cv::Scalar(0));
}

Image ProcessedImage::Warp(const ProcessedImage &target_image) const {
  cv::Mat result(raw_image_.rows, raw_image_.cols, raw_image_.type(),
                 cv::Scalar(0, 0, 0));
  cv::Mat zbuffer(raw_image_.rows / 2, raw_image_.cols / 2, CV_64FC1,
                  cv::Scalar(1000));
  Eigen::Vector2i coord;
  #pragma omp parallel for collapse(2)
  for (int i = 0; i < raw_image_.rows; ++i) {
    for (int j = 0; j < raw_image_.cols; j++) {
      int idx = i * raw_image_.cols + j;
      if (depth_[idx].w > 0.5) {
        coord = target_image.GetCamera().Project(
            static_cast<double>(depth_[idx].x),
            static_cast<double>(depth_[idx].y),
            static_cast<double>(depth_[idx].z));
        // Check if the point is seen in both images
        // (taking into account occlusions)
        if (coord(1) > 0 && coord(1) < raw_image_.rows && coord(0) > 0 &&
            coord(0) < raw_image_.cols) {
          double distance =
              sqrt(pow(static_cast<double>(depth_[idx].x) -
                           target_image.GetCamera().GetPosition()(0), 2) +
                   pow(static_cast<double>(depth_[idx].y) -
                           target_image.GetCamera().GetPosition()(1), 2) +
                   pow(static_cast<double>(depth_[idx].z) -
                           target_image.GetCamera().GetPosition()(2), 2));
          if (distance < zbuffer.at<double>(coord(1) / 2, coord(0) / 2)) {
            zbuffer.at<double>(coord(1) / 2, coord(0) / 2) = distance;
            int depth_idx =
                (raw_image_.rows - coord(1) - 1) * target_image.Width() +
                coord(0);
            double dist_target =
                sqrt(pow(target_image.GetCamera().GetPosition()(0) -
                             target_image.GetDepth()[depth_idx].x, 2) +
                     pow(target_image.GetCamera().GetPosition()(1) -
                             target_image.GetDepth()[depth_idx].y, 2) +
                     pow(target_image.GetCamera().GetPosition()(2) -
                             target_image.GetDepth()[depth_idx].z, 2));
            if (dist_target >= distance - 0.5) {
              result.at<cv::Vec3b>(coord(1), coord(0)) =
                  raw_image_.at<cv::Vec3b>(raw_image_.rows - i - 1, j);
            }
          }
        }
      }
    }
  }

  // Interpolation
  cv::Mat interpolation(result.rows, result.cols, result.type());
  int dilation_size = 1;
  cv::Mat dilation_kernel = cv::getStructuringElement(
      cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
      cv::Point(dilation_size, dilation_size));
  cv::dilate(result, interpolation, dilation_kernel);
  for (int i = 0; i < result.rows; i++) {
    for (int j = 0; j < result.cols; j++) {
      if (result.at<cv::Vec3b>(i, j) == cv::Vec3b(0, 0, 0)) {
        result.at<cv::Vec3b>(i, j) = interpolation.at<cv::Vec3b>(i, j);
      }
    }
  }
  return Image(result, target_image.GetCamera());
}

void ProcessedImage::UpdateInconsistencies(const Image &image,
                                           int kernel_size) {
  cv::Mat inconsistency =
      Subtract(raw_image_, image.GetRawImage(), kernel_size);

  cv::normalize(inconsistency, inconsistency, 0.0f, 1.0f, cv::NORM_MINMAX);
  if (num_img_compared_ > 0)
    inconsistencies_ = Minimum(inconsistencies_, inconsistency);
  else
    inconsistencies_ = inconsistency;
  cv::medianBlur(inconsistencies_, inconsistencies_, 3);
  cv::normalize(inconsistencies_, inconsistencies_, 0.0f, 1.0f,
                cv::NORM_MINMAX);

  num_img_compared_++;
}

cv::Mat ProcessedImage::GetInconsistencies() { return inconsistencies_; }

std::vector<int> ProcessedImage::ComputeMeansCovariances(double chi_square) {
  std::vector<int> found_labels;
  for (int i = 0; i <= max_region_label_; i++) {
    std::vector<int> idx;
    for (size_t j = 0; j < regions_.size(); j++) {
      if (regions_[j].label_ == i) {
        idx.push_back(j);
      }
    }
    if (!idx.empty()) {
      found_labels.push_back(i);
      cv::Mat region(Height(), Width(), CV_8UC1, cv::Scalar(0));
      std::vector<std::vector<cv::Point>> contours;
      for (int j : idx) {
        contours.push_back(regions_[j].contour_);
      }
      cv::drawContours(region, contours, -1, cv::Scalar(255), CV_FILLED);
      cv::Mat non_zero = FindNonZero(region);
      cv::Mat mean;
      cv::Mat covariance(2, 2, CV_64F);
      cv::calcCovarMatrix(non_zero, covariance, mean,
                        CV_COVAR_NORMAL | CV_COVAR_ROWS | CV_COVAR_SCALE);
      Eigen::Vector2d emean;
      Eigen::Matrix2d ecovariance;
      emean << mean.at<double>(0, 0), mean.at<double>(0, 1);
      ecovariance << covariance.at<double>(0, 0), covariance.at<double>(0, 1),
      covariance.at<double>(1, 0), covariance.at<double>(1, 1);
      PointCovariance2d mean_covariance(emean, ecovariance, chi_square);
      regions_mean_cov_[i] = mean_covariance;
    }
  }
  return found_labels;
}

void ProcessedImage::ComputeRegions(int threshold_change_area) {
  cv::Mat image(Height(), Width(), CV_8UC1);
  inconsistencies_.convertTo(image, CV_8UC1, 255);
  cv::threshold(image, image, 30, 255.0f, cv::THRESH_TOZERO);

  cv::Mat dilation_kernel =
      cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(11, 11));
  cv::Mat erosion_kernel =
      cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
  cv::erode(image, image, erosion_kernel);
  cv::dilate(image, image, dilation_kernel);
  image = AverageSegmentedRegions(image, threshold_change_area);

  cv::equalizeHist(image, image);
  cv::normalize(image, image, 0, 255, cv::NORM_MINMAX);
  cv::threshold(image, image, 50, 255.0f, cv::THRESH_TOZERO);

  std::vector<std::vector<cv::Point>> contours;
  cv::findContours(image, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
  for (size_t i = 0; i < contours.size(); i++) {
    ImageRegion region(contours[i]);
    regions_.push_back(region);
  }
}

const std::vector<ImageRegion>& ProcessedImage::GetRegions() const {
  return regions_;
}

const std::vector<glow::vec4>& ProcessedImage::GetDepth() const {
  return depth_;
}

void ProcessedImage::UpdateLabels(const std::vector<int> &labels) {
  for (size_t i = 0; i < regions_.size(); i++) {
    if (labels[i] > max_region_label_) max_region_label_ = labels[i];
    regions_[i].label_ = labels[i];
  }
}

cv::Mat ProcessedImage::AverageSegmentedRegions(const cv::Mat &img,
                                                int threshold_change_area) {
  cv::Mat labels(img.rows, img.cols, img.type(), cv::Scalar(0));
  cv::Mat dst(img.rows, img.cols, img.type(), cv::Scalar(0));
  cv::Mat tmp = img.clone();
  std::vector<std::vector<cv::Point>> contours;
  cv::findContours(tmp, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
  std::vector<float> cont_avgs(contours.size(), 0.f);
  for (size_t i = 0; i < contours.size(); ++i) {
    cv::drawContours(labels, contours, i, cv::Scalar(i + 1), CV_FILLED);
    cv::Rect roi = cv::boundingRect(contours[i]);
    cv::Mat mask;
    cv::bitwise_and(labels(roi) == (i + 1), img(roi) != 0, mask);
    cv::Scalar mean = cv::mean(img(roi), mask);
    cont_avgs[i] = mean[0];
  }
  for (size_t i = 0; i < contours.size(); ++i) {
    if (cv::contourArea(contours[i]) > threshold_change_area)
      cv::drawContours(dst, contours, i, cv::Scalar(cont_avgs[i]), CV_FILLED);
  }
  return dst;
}

std::unordered_map<int, PointCovariance2d>
ProcessedImage::GetMeansCovariances() const {
  return regions_mean_cov_;
}

cv::Mat ProcessedImage::Subtract(const cv::Mat &img1, const cv::Mat &img2,
                                 int kernel_size) {
  cv::Mat result(img1.rows, img1.cols, CV_32FC1, 0.0f);

  for (int i = 0; i < img1.rows; ++i) {
    for (int j = 0; j < img1.cols; ++j) {
      Eigen::Vector3f img1_color, img2_color;
      img1_color << static_cast<float>(img1.at<cv::Vec3b>(i, j)[0]),
          static_cast<float>(img1.at<cv::Vec3b>(i, j)[1]),
          static_cast<float>(img1.at<cv::Vec3b>(i, j)[2]);
      if (img1_color.norm() > 0.01) {
        if (j - kernel_size / 2 > 0 &&
            i - kernel_size / 2 > 0 &&
            j + kernel_size / 2 < img2.cols &&
            i + kernel_size / 2 < img2.rows) {
          cv::Rect window(j - kernel_size / 2,
                          i - kernel_size / 2, kernel_size,
                          kernel_size);
          // Loop on the window
          float min = 10000;
          for (int subi = 0; subi < window.height; subi++) {
            for (int subj = 0; subj < window.width; subj++) {
              cv::Mat area = img2(window);
              img2_color << static_cast<float>(
                  area.at<cv::Vec3b>(subi, subj)[0]),
                  static_cast<float>(area.at<cv::Vec3b>(subi, subj)[1]),
                  static_cast<float>(area.at<cv::Vec3b>(subi, subj)[2]);
            // Compare the values here and keep track of the minimum difference
              if (img2_color.norm() > 0.01) {
                float difference = (img1_color - img2_color).norm();
                if (difference < min) min = difference;
              }
            }
          }
          if (min < 1000) result.at<float>(i, j) = min;
        }
      }
    }
  }
  return result;
}

cv::Mat ProcessedImage::Minimum(const cv::Mat &img1, const cv::Mat &img2) {
  cv::Mat result(img1.rows, img1.cols, CV_32FC1, 0.0f);
  #pragma omp parallel for collapse(2)
  for (int i = 0; i < img1.rows; ++i) {
    for (int j = 0; j < img1.cols; ++j) {
      result.at<float>(i, j) =
          std::min(img1.at<float>(i, j), img2.at<float>(i, j));
    }
  }
  return result;
}

cv::Mat ProcessedImage::FindNonZero(const cv::Mat &img) {
  cv::Mat result(img.rows * img.cols, 2, CV_64F);
  int idx = 0;
  for (int i = 0; i < img.rows; i++) {
    for (int j = 0; j < img.cols; j++) {
      if (img.at<uchar>(i, j) > 0) {
        result.at<double>(idx, 0) = j;
        result.at<double>(idx, 1) = i;
        idx++;
      }
    }
  }
  return result(cv::Rect(0, 0, 2, idx));
}

int ProcessedImage::ImageCompared() { return num_img_compared_; }

}  // namespace fastcd


text:
#pragma once

#include <logos/consensus/messages/rejection.hpp>
#include <logos/consensus/message_validator.hpp>
#include <logos/consensus/messages/messages.hpp>
#include <logos/consensus/consensus_state.hpp>
#include <logos/consensus/messages/util.hpp>
#include <logos/lib/utility.hpp>
#include <logos/lib/log.hpp>

#include <boost/asio/deadline_timer.hpp>
#include <boost/asio/io_service.hpp>

#include <unordered_map>
template<ConsensusType CT>
class BackupDelegate;

class PrimaryDelegate : public Self<PrimaryDelegate>
{
    using uint128_t = logos::uint128_t;

    struct Weight
    {
        uint128_t vote_weight  = 0;
        uint128_t stake_weight = 0;
    };

    friend class Archiver;

    template <ConsensusType CT>
    friend class BackupDelegate;

    using Signatures = std::unordered_map<uint8_t, MessageValidator::DelegateSignature>;
    using Timer      = boost::asio::deadline_timer;
    using Error      = boost::system::error_code;
    using Service    = boost::asio::io_service;
    using Seconds    = boost::posix_time::seconds;
    using Minutes    = boost::posix_time::minutes;
    using Store      = logos::block_store;
    using Weights    = std::unordered_map<uint8_t, Weight>;

public:

    enum class ProceedAction : uint8_t
    {
        APPROVED = 0,
        REJECTED,
        DO_NOTHING
    };

    PrimaryDelegate(Service & service,
                    MessageValidator & validator,
                    uint32_t epoch_number);

    static void SetQuorum(uint128_t & max_fault, uint128_t & quorum, const uint128_t & total);

    template<typename M>
    void OnConsensusMessage(const M & message, uint8_t remote_delegate_id)
    {
        ProcessMessage(message, remote_delegate_id);
    }

    template<typename M>
    bool Validate(const M & message, uint8_t remote_delegate_id);

    virtual ~PrimaryDelegate()
    {}

    virtual void OnCurrentEpochSet();

    virtual void Send(const void * data, size_t size) = 0;

    void AdvanceState(ConsensusState new_state);

    template<typename TYPE>
    void Send(const TYPE & data)
    {
        std::vector<uint8_t> buf;
        data.Serialize(buf);
        Send(buf.data(), buf.size());
        SendP2p(buf.data(), buf.size(), data.type, _epoch_number, 0xff);
    }

    uint8_t GetDelegateIndex()
    {
        return _delegate_id;
    }

    /// set previous hash, microblock and epoch block have only one chain
    /// consequently primary has to set all backup's hash to previous in
    /// the overload function
    /// @param hash to set
    virtual void SetPreviousPrePrepareHash(const BlockHash &hash)
    {
        _prev_pre_prepare_hash = hash;
    }

    uint32_t GetEpochNumber()
    {
        return _epoch_number;
    }

protected:

    virtual bool SendP2p(const uint8_t *, uint32_t, MessageType, uint32_t, uint8_t) = 0;
    virtual void EnableP2p(bool enable) = 0;

    virtual void UpdateVotes();

    template<ConsensusType C>
    void OnConsensusInitiated(const PrePrepareMessage<C> & block, bool reproposing);

    bool StateReadyForConsensus();
    void CancelTimer();

    bool ReachedQuorum(uint128_t vote, uint128_t stake);

    // TODO: Revert to std::mutex after
    //       benchmark.
    //
    std::recursive_mutex _mutex;
    BlockHash            _prev_pre_prepare_hash;
    BlockHash            _pre_prepare_hash;
    BlockHash            _post_prepare_hash;
    DelegateSig          _pre_prepare_sig;
    AggSignature         _post_prepare_sig;
    AggSignature         _post_commit_sig;
    std::atomic_bool     _ongoing         = ATOMIC_VAR_INIT(false);
    std::atomic_bool     _state_changing  = ATOMIC_VAR_INIT(false);
    std::mutex           _state_mutex;
    Weights              _weights;
    ApprovedEB           _current_epoch;
    ConsensusState       _state           = ConsensusState::VOID;
    uint128_t            _vote_total      = 0;
    uint128_t            _stake_total     = 0;
    uint128_t            _vote_max_fault  = 0;
    uint128_t            _stake_max_fault = 0;
    uint128_t            _vote_quorum     = 0;
    uint128_t            _stake_quorum    = 0;
    uint128_t            _prepare_vote    = 0;
    uint128_t            _prepare_stake   = 0;
    uint128_t            _my_vote         = 0;
    uint128_t            _my_stake        = 0;
    uint8_t              _delegate_id     = 0;
    uint32_t             _epoch_number    = 0;
    uint64_t             _cur_batch_timestamp = 0;

private:

    template<ConsensusType C>
    void ProcessMessage(const RejectionMessage<C> & message, uint8_t remote_delegate_id);
    template<ConsensusType C>
    void ProcessMessage(const PrepareMessage<C> & message, uint8_t remote_delegate_id);
    template<ConsensusType C>
    void ProcessMessage(const CommitMessage<C> & message, uint8_t remote_delegate_id);

    // The Tally method needs to be called with _state_mutex locked to ensure atomicity
    template<ConsensusType C>
    void Tally(const RejectionMessage<C> & message, uint8_t remote_delegate_id);

    template<ConsensusType C>
    void Tally(const PrepareMessage<C> & message, uint8_t remote_delegate_id);

    template<ConsensusType C>
    void Tally(const CommitMessage<C> & message, uint8_t remote_delegate_id);

    template<typename M>
    void TallyStandardPhaseMessage(const M & message, uint8_t remote_delegate_id);

    virtual void TallyPrepareMessage(const PrepareMessage<ConsensusType::Request> & message,
                                     uint8_t remote_delegate_id);
    virtual void TallyPrepareMessage(const PrepareMessage<ConsensusType::MicroBlock> & message,
                                     uint8_t remote_delegate_id);
    virtual void TallyPrepareMessage(const PrepareMessage<ConsensusType::Epoch> & message,
                                     uint8_t remote_delegate_id);

    template<ConsensusType C>
    BlockHash GetHashSigned(const RejectionMessage<C> & message)
    {
        return message.Hash();
    }
    template<ConsensusType C>
    BlockHash GetHashSigned(const PrepareMessage<C> & message)
    {
        return _pre_prepare_hash;
    }
    template<ConsensusType C>
    BlockHash GetHashSigned(const CommitMessage<C> & message)
    {
        return _post_prepare_hash;
    }

    virtual void OnRejection(const RejectionMessage<ConsensusType::Request> & message, uint8_t remote_delegate_id);
    virtual void OnRejection(const RejectionMessage<ConsensusType::MicroBlock> & message, uint8_t remote_delegate_id);
    virtual void OnRejection(const RejectionMessage<ConsensusType::Epoch> & message, uint8_t remote_delegate_id);

    template<ConsensusType C>
    void OnPrePrepareTimeout(const Error & error);
    template<ConsensusType C>
    void OnPostPrepareTimeout(const Error & error);

    template<ConsensusType C>
    void OnTimeout(const Error & error,
                   const std::string & timeout,
                   ConsensusState expected_state);

    template<ConsensusType C>
    void CycleTimers(bool cancel = false,bool reproposing=false);

    bool ReachedQuorum();
    bool AllDelegatesResponded();

    template<typename M>
    ProceedAction ProceedWithMessage(const M & message, ConsensusState expected_state, uint8_t remote_delegate_id);

    virtual void OnStateAdvanced();
    virtual bool IsPrePrepareRejected();
    virtual void OnPrePrepareRejected();

    virtual void OnConsensusReached() = 0;
    virtual void OnQuorumFailed() = 0;

    Signatures         _signatures;
    Log                _log;
    MessageValidator & _validator;
    Timer              _recall_timer;
    Timer              _primary_timer;
    uint8_t            _delegates_responded = 0;
    bool               _timer_cancelled     = false;
    uint8_t _num_proposals                       = 0;
};

max_stars_repo_name: LogosNetwork/logos-core

max_issues_repo_name: LogosNetwork/logos-core

max_forks_repo_name: LogosNetwork/logos-core


text:
#include <ros/ros.h>
#include <ros/publisher.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>
#include <boost/asio.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/math/constants/constants.hpp>
#include <algorithm>
#include <string>

using namespace std;

/*
  README
  @author Brian Cochran, github: @btdubs
  Node for taking in serial CSV distances from ultrasonic array,
  then outputing a point cloud. Ultrasonic sensors defined in
  urdf and uses that for transforming points.
  Requires a tf tree being published.
*/

//defaults for launch file paramenters
#define NUM_SENSORS_DEFAULT 6
#define DRAW_METHOD_DEFAULT 0 //0 = Single Point, 1 = wall, 2 = circle, 3 = semicircle
#define NUM_POINTS_DEFAULT 12 //# minimum points for each semicircle/circle/wall
#define RADIUS_DEFAULT 0.1f //radius of semicircle/circle in meters
#define LENGTH_DEFAULT 0.1f //length of wall
#define DISTANCE_CLIP_DEFAULT 5.0f //distance beyond which we ignore point in meters
#define BAUD_RATE_DEFAULT 9600 //rate of transfer. Needs to match Arduino

const double pi = boost::math::constants::pi<double>();

string readLine(boost::asio::serial_port &port) {
    string line = "";
    bool inLine = false;
    while (true) {
        char in;
        try {
            boost::asio::read(port, boost::asio::buffer(&in, 1));
            if (in == '\n') {
                return line;
            }
            line += in;
        } catch (
            boost::exception_detail::clone_impl <boost::exception_detail::error_info_injector<boost::system::system_error>> &err) {
            ROS_ERROR("Error reading serial port.");
            ROS_ERROR_STREAM(err.what());
        }

    }
}

vector<double> parseLine(string line) {
  vector<string> strs;
  boost::split(strs, line, boost::is_any_of(","));

  vector<double> dist(strs.size());
  std::transform(strs.begin(), strs.end(), dist.begin(), [](const auto &s){return std::stod(s);}); //convert string to double

  return dist;
}

void drawWall(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointXYZ center, float length, int numPoints) {
  length = (length + 1) / 2.0; //@Note: adds one to ensure we get at least the numPoints desired
  float step = length / (float) numPoints / 2.0;
  float dist = step;
  for (int i = 0; i < numPoints / 2; i++) {
    //draw half the line and mirror
    pcl::PointXYZ point1(center.x, dist, 0); //#TODO: might need to swap x and y
    pcl::PointXYZ point2(center.x, -dist, 0); //#TODO: might need to swap x and y

    cloud.push_back(point1);
    cloud.push_back(point2);
    dist = dist + step;
  }

}

void drawSemiCircle(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointXYZ point, float radius, int numPoints) {
  numPoints = (numPoints + 1) / 2; //@Note: adds one to ensure we get at least the numPoints desired
  float angleStep = (pi / 2.0) / numPoints;

  float angle = 0.0; //#TODO: may need to shift this start angle

  for (int i = 0; i < numPoints ; i++) {
    //draw a quarter of a cicle and mirror
    float x = radius * cos(angle);
    float y = radius * sin(angle);

    cloud.push_back(pcl::PointXYZ(point.x + radius - x, y, 0));
    cloud.push_back(pcl::PointXYZ(point.x + radius - x, -y, 0));

    angle = angle + angleStep;
  }

}

void drawCircle(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointXYZ point, double radius, int numPoints) {
  double angleStep = (2.0 * pi) / numPoints;

  double angle = 0; //start angle

  for (int i = 0; i < numPoints; i++) {
    //draw a circle
    double x = radius * cos(angle);
    double y = radius * sin(angle);

    cloud.push_back(pcl::PointXYZ(point.x + radius + x, y, 0)); //point.x + radius is center of circle
    angle = angle + angleStep;

  }

}

int main(int argc, char** argv) {

  ros::init(argc, argv, "ultrasonic_array");

  ros::NodeHandle nh;
  ros::NodeHandle nhp("~");

  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/ultrasonic_array", 1);

  //Roslaunch file
  string sensor_base_link;
  string sensor_link;
  float update_rate;
  string serial_port_name;
  float distance_clip;
  float length;
  float radius;
  int num_sensors;
  int num_points;
  int draw_method;
  int baud_rate;
  nhp.param(string("serial_port_name"), serial_port_name, string("/dev/ttyUSB0"));
  nhp.param(string("sensor_base_link"), sensor_base_link, string("ultrasonic_array_base"));
  nhp.param(string("sensor_link_prefix"), sensor_link, string("ultrasonic_"));
  nhp.param(string("update_rate"), update_rate, 10.0f);
  nhp.param(string("number_of_sensors"), num_sensors, NUM_SENSORS_DEFAULT);
  nhp.param(string("baud_rate"), baud_rate, BAUD_RATE_DEFAULT);
  nhp.param(string("distance_clip"), distance_clip, DISTANCE_CLIP_DEFAULT);
  nhp.param(string("draw_method"), draw_method, DRAW_METHOD_DEFAULT);
  nhp.param(string("wall_length"), length, LENGTH_DEFAULT);
  nhp.param(string("circle_radius"), radius, RADIUS_DEFAULT);
  nhp.param(string("number_of_points"), num_points, NUM_POINTS_DEFAULT);

  //Connect serial
  ROS_INFO_STREAM("Connecting to serial at port: " + serial_port_name);
  boost::asio::io_service io_service;
  boost::asio::serial_port serial(io_service, serial_port_name);
  serial.set_option(boost::asio::serial_port_base::baud_rate(baud_rate));

  // wait for microcontroller to start
  ros::Duration(2.0).sleep();

  ros::Rate rate(update_rate);

  //Transforms
  tf::TransformListener tf_listener;
  tf::StampedTransform tf_transform;
  pcl::PointCloud<pcl::PointXYZ> compiled_cloud;

  //lookup transforms and store them as they do not change
  vector<tf::StampedTransform> tf_transform_vector;
  for (int i = 0; i < num_sensors; i++) {
    if(tf_listener.waitForTransform(sensor_base_link, sensor_link + to_string(i), ros::Time(0), ros::Duration(5.0))) {
      tf_listener.lookupTransform(sensor_base_link, sensor_link + to_string(i), ros::Time(0), tf_transform);
    }

    tf_transform_vector.push_back(tf_transform);
  }

  function<void(pcl::PointCloud<pcl::PointXYZ> &a, pcl::PointXYZ &b)> draw;
  switch (draw_method) {
      default:
      case 0: draw = [](pcl::PointCloud<pcl::PointXYZ> a, pcl::PointXYZ b){};
              break;
      case 1: draw = bind(drawWall, _1, _2, length, num_points);
              break;
      case 2: draw = bind(drawCircle, _1, _2, radius, num_points);
              break;
      case 3: draw = bind(drawSemiCircle, _1, _2, radius, num_points);
              break;
  }

  while(ros::ok() && serial.is_open()) {
    ros::spinOnce();

    string line = readLine(serial);
    ROS_INFO_STREAM(line);
    vector<double> distances = parseLine(line);

    for (int i = 0; i < num_sensors; i++) {
      pcl::PointCloud<pcl::PointXYZ> cloud;

      if(distances[i] < distance_clip) {
        pcl::PointXYZ point(distances[i], 0.0, 0.0);

        cloud.push_back(point); //add point direct from Arduino sensor
        draw(cloud, point);

        pcl_ros::transformPointCloud(cloud, cloud, tf_transform_vector[i]);

        //add point cloud to the output cloud
        compiled_cloud += cloud;
      }
    }

    sensor_msgs::PointCloud2 outmsg;
    pcl::toROSMsg(compiled_cloud, outmsg);
    outmsg.header.frame_id = sensor_base_link;

    pub.publish(outmsg);
    compiled_cloud.clear(); //remove old data for next round
    rate.sleep();
  }

  ROS_INFO_STREAM("Shutting down Ultrasonic Array");
  serial.close();

  return 0;
}