text:
/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    HessianFactor.cpp
 * @author  Richard Roberts
 * @date    Dec 8, 2010
 */

#include <gtsam/linear/HessianFactor.h>

#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <sstream>
#include <limits>

using namespace std;
using namespace boost::assign;
namespace br {
using namespace boost::range;
using namespace boost::adaptors;
}

namespace gtsam {

/* ************************************************************************* */
void HessianFactor::Allocate(const Scatter& scatter) {
  gttic(HessianFactor_Allocate);

  // Allocate with dimensions for each variable plus 1 at the end for the information vector
  const size_t n = scatter.size();
  keys_.resize(n);
  FastVector<DenseIndex> dims(n + 1);
  DenseIndex slot = 0;
  for(const SlotEntry& slotentry: scatter) {
    keys_[slot] = slotentry.key;
    dims[slot] = slotentry.dimension;
    ++slot;
  }
  dims.back() = 1;
  info_ = SymmetricBlockMatrix(dims);

}

//====================== MHHessianFactor::mhAllocate() =================
void MHHessianFactor::mhAllocate(const Scatter& scatter) {
  //gttic(HessianFactor_Allocate);

  // Allocate with dimensions for each variable plus 1 at the end for the information vector
  const size_t n = scatter.size();
  keys_.resize(n);

  //FastVector<DenseIndex> dims(n + 1);

  DenseIndex slot = 0;
  for(const SlotEntry& slotentry: scatter) {
    keys_[slot] = slotentry.key;
    ++slot;
  }

}

//====================== END MHHessianFactor::mhAllocate() =================

/* ************************************************************************* */
HessianFactor::HessianFactor(const Scatter& scatter) {
  Allocate(scatter);
}

/* ************************************************************************* */
HessianFactor::HessianFactor() :
    info_(cref_list_of<1>(1)) {
  assert(info_.rows() == 1);
  constantTerm() = 0.0;
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) :
    GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) {
  if (G.rows() != G.cols() || G.rows() != g.size())
    throw invalid_argument(
        "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
  info_.setDiagonalBlock(0, G);
  linearTerm() = g;
  constantTerm() = f;
}

/* ************************************************************************* */
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
    GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) {
  if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
    throw invalid_argument(
        "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
  info_.setDiagonalBlock(0, Sigma.inverse()); // G
  linearTerm() = info_.diagonalBlock(0) * mu; // g
  constantTerm() = mu.dot(linearTerm().col(0)); // f
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
    const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
    double f) :
    GaussianFactor(cref_list_of<2>(j1)(j2)), info_(
        cref_list_of<3>(G11.cols())(G22.cols())(1)) {
  info_.setDiagonalBlock(0, G11);
  info_.setOffDiagonalBlock(0, 1, G12);
  info_.setDiagonalBlock(1, G22);
  linearTerm() << g1, g2;
  constantTerm() = f;
}

/* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
    const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22,
    const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
    double f) :
    GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_(
        cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) {
  if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
      || G11.rows() != G13.rows() || G11.rows() != g1.size()
      || G22.cols() != G12.cols() || G33.cols() != G13.cols()
      || G22.cols() != g2.size() || G33.cols() != g3.size())
    throw invalid_argument(
        "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
  info_.setDiagonalBlock(0, G11);
  info_.setOffDiagonalBlock(0, 1, G12);
  info_.setOffDiagonalBlock(0, 2, G13);
  info_.setDiagonalBlock(1, G22);
  info_.setOffDiagonalBlock(1, 2, G23);
  info_.setDiagonalBlock(2, G33);
  linearTerm() << g1, g2, g3;
  constantTerm() = f;
}

/* ************************************************************************* */
namespace {
DenseIndex _getSizeHF(const Vector& m) {
  return m.size();
}
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js,
    const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
    GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
  // Get the number of variables
  size_t variable_count = js.size();

  // Verify the provided number of entries in the vectors are consistent
  if (gs.size() != variable_count
      || Gs.size() != (variable_count * (variable_count + 1)) / 2)
    throw invalid_argument(
        "Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
        in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");

  // Verify the dimensions of each provided matrix are consistent
  // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
  for (size_t i = 0; i < variable_count; ++i) {
    DenseIndex block_size = gs[i].size();
    // Check rows
    for (size_t j = 0; j < variable_count - i; ++j) {
      size_t index = i * (2 * variable_count - i + 1) / 2 + j;
      if (Gs[index].rows() != block_size) {
        throw invalid_argument(
            "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
      }
    }
    // Check cols
    for (size_t j = 0; j <= i; ++j) {
      size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j);
      if (Gs[index].cols() != block_size) {
        throw invalid_argument(
            "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
      }
    }
  }

  // Fill in the blocks
  size_t index = 0;
  for (size_t i = 0; i < variable_count; ++i) {
    for (size_t j = i; j < variable_count; ++j) {
      if (i == j) {
        info_.setDiagonalBlock(i, Gs[index]);
      } else {
        info_.setOffDiagonalBlock(i, j, Gs[index]);
      }
      index++;
    }
    info_.setOffDiagonalBlock(i, variable_count, gs[i]);
  }
  constantTerm() = f;
}

/* ************************************************************************* */
namespace {
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) {
  gttic(HessianFactor_fromJacobian);
  const SharedDiagonal& jfModel = jf.get_model();
  auto A = jf.matrixObject().full();
  if (jfModel) {
    if (jf.get_model()->isConstrained())
      throw invalid_argument(
          "Cannot construct HessianFactor from JacobianFactor with constrained noise model");

    auto D = (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal();

    info.setFullMatrix(A.transpose() * D * A);
  } else {
    info.setFullMatrix(A.transpose() * A);
  }
}
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const JacobianFactor& jf) :
    GaussianFactor(jf), info_(
        SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) {
  _FromJacobianHelper(jf, info_);
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) :
    GaussianFactor(gf) {
  // Copy the matrix data depending on what type of factor we're copying from
  if (const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) {
    info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
    _FromJacobianHelper(*jf, info_);
  } else if (const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf)) {
    info_ = hf->info_;
  } else {
    throw std::invalid_argument(
        "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
  }
}

/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
    boost::optional<const Scatter&> scatter) {
  gttic(HessianFactor_MergeConstructor);

  Allocate(scatter ? *scatter : Scatter(factors));

  // Form A' * A
  gttic(update);
  info_.setZero();
  for(const auto& factor: factors) {
    if (factor) {
      factor->updateHessian(keys_, &info_); //mhsiao: link back to JacobianFactor
    }
  }

  gttoc(update);
}

/* ************************************************************************* */
void HessianFactor::print(const std::string& s,
    const KeyFormatter& formatter) const {
  cout << s << "\n";
  cout << " keys: ";
  for (const_iterator key = begin(); key != end(); ++key)
    cout << formatter(*key) << "(" << getDim(key) << ") ";
  cout << "\n";
  gtsam::print(Matrix(info_.selfadjointView()),
      "Augmented information matrix: ");
}

/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
  const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
  if (!rhs || !Factor::equals(lf, tol))
    return false;
  return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(),
      tol);
}

/* ************************************************************************* */
Matrix HessianFactor::augmentedInformation() const {
  return info_.selfadjointView();
}

/* ************************************************************************* */
Eigen::SelfAdjointView<SymmetricBlockMatrix::constBlock, Eigen::Upper>
HessianFactor::informationView() const {
  return info_.selfadjointView(0, size());
}

/* ************************************************************************* */
Matrix HessianFactor::information() const {
  return informationView();
}

/* ************************************************************************* */
VectorValues HessianFactor::hessianDiagonal() const {
  VectorValues d;
  for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
    d.insert(keys_[j], info_.diagonal(j));
  }
  return d;
}

/* ************************************************************************* */
// Raw memory access version should be called in Regular Factors only currently
void HessianFactor::hessianDiagonal(double* d) const {
  throw std::runtime_error(
      "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
}

/* ************************************************************************* */
map<Key, Matrix> HessianFactor::hessianBlockDiagonal() const {
  map<Key, Matrix> blocks;
  // Loop over all variables
  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
    // Get the diagonal block, and insert it
    blocks.emplace(keys_[j], info_.diagonalBlock(j));
  }
  return blocks;
}

/* ************************************************************************* */
Matrix HessianFactor::augmentedJacobian() const {
  return JacobianFactor(*this).augmentedJacobian();
}

/* ************************************************************************* */
std::pair<Matrix, Vector> HessianFactor::jacobian() const {
  return JacobianFactor(*this).jacobian();
}

/* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const {
  // error 0.5*(f - 2*x'*g + x'*G*x)
  const double f = constantTerm();
  if (empty()) {
    return 0.5 * f;
  }
  double xtg = 0, xGx = 0;
  // extract the relevant subset of the VectorValues
  // NOTE may not be as efficient
  const Vector x = c.vector(keys());
  xtg = x.dot(linearTerm().col(0));
  auto AtA = informationView();
  xGx = x.transpose() * AtA * x;
  return 0.5 * (f - 2.0 * xtg + xGx);
}

/* ************************************************************************* */
void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
                                  SymmetricBlockMatrix* info) const {
  gttic(updateHessian_HessianFactor);
  assert(info);
  // Apply updates to the upper triangle
  DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1;
  vector<DenseIndex> slots(nrVariablesInThisFactor + 1);
  // Loop over this factor's blocks with indices (i,j)
  // For every block (i,j), we determine the block (I,J) in info.
  for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) {
    const bool rhs = (j == nrVariablesInThisFactor);
    const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]);
    slots[j] = J;
    for (DenseIndex i = 0; i <= j; ++i) {
      const DenseIndex I = slots[i];  // because i<=j, slots[i] is valid.

      if (i == j) {
        assert(I == J);
        info->updateDiagonalBlock(I, info_.diagonalBlock(i));
      } else {
        assert(i < j);
        assert(I != J);
        info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j));
      }
    }
  }

}
//============================== MHHF::mhUpdateHessian() ===================
//[MH-A]:
void MHHessianFactor::mhUpdateHessian(const FastVector<Key>& keys, HessList& hessian_list, const int& max_layer_idx) const {

  // max_layer_idx decides the recursive iter...
  const int this_layer_idx = resulting_layer_->getLayerIdx();
  const int layer_diff = max_layer_idx - this_layer_idx;

  std::vector<int> descendant_num_arr(hypoSize());

  size_t dna_idx = 0;
  const HypoList& hypo_list = getHypoList();
  for (HypoListCstIter it = hypo_list.begin(); it != hypo_list.end(); ++it) {
    descendant_num_arr[dna_idx] = (*it)->findDescendantNum(layer_diff);
    ++dna_idx;
  }

  HessListIter hit = hessian_list.begin();
  int dna_count = 0;
  for (HessListCstIter ihit = hessian_list_.begin(); ihit != hessian_list_.end(); ++ihit) { //should NOT ++hit here

    auto& num = descendant_num_arr[dna_count];
    for (int k = 0; k < num; ++k) {
      (*ihit).updateHessian(keys, &((*hit).info()));

      ++hit;
    }
    dna_count++;
  }

}
//============================== END MHHF::mhUpdateHessian() ===================

/* ************************************************************************* */
GaussianFactor::shared_ptr HessianFactor::negate() const {
  shared_ptr result = boost::make_shared<This>(*this);
  // Negate the information matrix of the result
  result->info_.negate();
  return result;
}

/* ************************************************************************* */
void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
    VectorValues& yvalues) const {

  // Create a vector of temporary y values, corresponding to rows i
  vector<Vector> y;
  y.reserve(size());
  for (const_iterator it = begin(); it != end(); it++)
    y.push_back(Vector::Zero(getDim(it)));

  // Accessing the VectorValues one by one is expensive
  // So we will loop over columns to access x only once per column
  // And fill the above temporary y values, to be added into yvalues after
  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
    // xj is the input vector
    Vector xj = x.at(keys_[j]);
    DenseIndex i = 0;
    for (; i < j; ++i)
      y[i] += info_.aboveDiagonalBlock(i, j) * xj;

    // blocks on the diagonal are only half
    y[i] += info_.diagonalBlock(j) * xj;
    // for below diagonal, we take transpose block from upper triangular part
    for (i = j + 1; i < (DenseIndex) size(); ++i)
      y[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj;
  }

  // copy to yvalues
  for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
    bool didNotExist;
    VectorValues::iterator it;
    boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
    if (didNotExist)
      it->second = alpha * y[i]; // init
    else
      it->second += alpha * y[i]; // add
  }
}

/* ************************************************************************* */
VectorValues HessianFactor::gradientAtZero() const {
  VectorValues g;
  size_t n = size();
  for (size_t j = 0; j < n; ++j)
    g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n));
  return g;
}

/* ************************************************************************* */
// Raw memory access version should be called in Regular Factors only currently
void HessianFactor::gradientAtZero(double* d) const {
  throw std::runtime_error(
      "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
}

/* ************************************************************************* */
Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
  const Factor::const_iterator it_i = find(key);
  const DenseIndex I = std::distance(begin(), it_i);

  // Sum over G_ij*xj for all xj connecting to xi
  Vector b = Vector::Zero(x.at(key).size());
  for (Factor::const_iterator it_j = begin(); it_j != end(); ++it_j) {
    const DenseIndex J = std::distance(begin(), it_j);

    // Obtain Gij from the Hessian factor
    // Hessian factor only stores an upper triangular matrix, so be careful when i>j
    const Matrix Gij = info_.block(I, J);
    // Accumulate Gij*xj to gradf
    b += Gij * x.at(*it_j);
  }
  // Subtract the linear term gi
  b += -linearTerm(it_i);
  return b;
}

/* ************************************************************************* */
boost::shared_ptr<GaussianConditional> HessianFactor::eliminateCholesky(const Ordering& keys) {
  gttic(HessianFactor_eliminateCholesky);

  GaussianConditional::shared_ptr conditional;

  try {
    // Do dense elimination
    size_t nFrontals = keys.size();
    assert(nFrontals <= size());

    info_.choleskyPartial(nFrontals);

    // TODO(frank): pre-allocate GaussianConditional and write into it
    const VerticalBlockMatrix Ab = info_.split(nFrontals);
    conditional = boost::make_shared<GaussianConditional>(keys_, nFrontals, Ab);

    // Erase the eliminated keys in this factor
    keys_.erase(begin(), begin() + nFrontals);
  } catch (const CholeskyFailed& e) {
    throw IndeterminantLinearSystemException(keys.front());
  }

  // Return result
  return conditional;
}

/* ************************************************************************* */
VectorValues HessianFactor::solve() {
  gttic(HessianFactor_solve);

  // Do Cholesky Factorization
  const size_t n = size();
  assert(info_.nBlocks() == n + 1);
  info_.choleskyPartial(n);
  auto R = info_.triangularView(0, n);
  auto eta = linearTerm();

  // Solve
  const Vector solution = R.solve(eta);

  // Parse into VectorValues
  VectorValues delta;
  std::size_t offset = 0;
  for (DenseIndex j = 0; j < (DenseIndex)n; ++j) {
    const DenseIndex dim = info_.getDim(j);
    delta.insert(keys_[j], solution.segment(offset, dim));
    offset += dim;
  }

  return delta;
}

/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) {
  gttic(EliminateCholesky);

  // Build joint factor
  HessianFactor::shared_ptr jointFactor;
  try {
    Scatter scatter(factors, keys);
    jointFactor = boost::make_shared<HessianFactor>(factors, scatter);
  } catch (std::invalid_argument&) {
    throw InvalidDenseElimination(
        "EliminateCholesky was called with a request to eliminate variables that are not\n"
        "involved in the provided factors.");
  }

  // Do dense elimination
  auto conditional = jointFactor->eliminateCholesky(keys);

  // Return result
  return make_pair(conditional, jointFactor);
}

/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>,
    boost::shared_ptr<GaussianFactor> > EliminatePreferCholesky(
    const GaussianFactorGraph& factors, const Ordering& keys) {
  gttic(EliminatePreferCholesky);

  // If any JacobianFactors have constrained noise models, we have to convert
  // all factors to JacobianFactors.  Otherwise, we can convert all factors
  // to HessianFactors.  This is because QR can handle constrained noise
  // models but Cholesky cannot.
  if (hasConstraints(factors))
    return EliminateQR(factors, keys);
  else
    return EliminateCholesky(factors, keys);
}

//================================== MHEliminateCholesky() ===========================
//[MH-A]: convert JF/MHJF to HF/MHHF here... returns GaussianConditional and MHHessianFactor
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<MHHessianFactor> >
MHEliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) {

  //gttic(EliminateCholesky);

  // Build joint factor
  MHHessianFactor::shared_ptr jointFactor;
  try {

    Scatter scatter(factors, true, keys);

    //[MH-A]: [REFINE] constructor(): combine several MHJacobianFactor into one MHHessianFactor
    jointFactor = boost::make_shared<MHHessianFactor>(factors, scatter);

  } catch (std::invalid_argument&) {
    throw InvalidDenseElimination(
        "MHEliminateCholesky was called with a request to eliminate variables that are not involved in the provided factors.");
  }

  // Do dense elimination
  //[MH-A]: careful "auto"
  auto conditional = jointFactor->mhEliminateCholesky(keys); //MHHessianFactor::mhEliminateCholesky() returns GaussianConditional

  // Return result
  return make_pair(conditional, jointFactor);
  //
}

//================================== END MHEliminateCholesky() ============================

//
//========================== MHHessianFactor::mhEliminateCholesky() =========================
boost::shared_ptr<GaussianConditional> MHHessianFactor::mhEliminateCholesky(const Ordering& keys) {

  //gttic(HessianFactor_eliminateCholesky);

  GaussianConditional::shared_ptr conditional;

  try {

    // Do dense elimination
    size_t nFrontals = keys.size();
    assert(nFrontals <= size());

    std::list<VerticalBlockMatrix> Ab_list;
    for (HessListIter it = hessian_list_.begin(); it != hessian_list_.end(); ++it) {

      (it->info()).choleskyPartial(nFrontals);

      // TODO(frank): pre-allocate GaussianConditional and write into it
      Ab_list.push_back((it->info()).split(nFrontals));

    }
    //[MH-A]: [DONE] call MHJacobianFactor() as base constructor...

    conditional = boost::make_shared<GaussianConditional>(keys_, nFrontals, Ab_list, resulting_layer_);

    // Erase the eliminated keys in this factor
    keys_.erase(begin(), begin() + nFrontals);
    for (HessListIter it = hessian_list_.begin(); it != hessian_list_.end(); ++it) {
      it->keys().erase(it->begin(), it->begin() + nFrontals);
    }

  } catch (const CholeskyFailed& e) {
    throw IndeterminantLinearSystemException(keys.front());
  }

  // Return result
  return conditional;
}
//================================== END mhEliminateCholesky() ============================

//======================= HessianFactor(GaussianFactorGraph, Sactter) ==================
MHHessianFactor::MHHessianFactor(const GaussianFactorGraph& factors, boost::optional<const Scatter&> scatter) {
  //gttic(HessianFactor_MergeConstructor);

  //[MH-A]: try to copy this...
  mhAllocate(scatter ? *scatter : Scatter(factors, true));

  // Find the max_hypo_num
  int max_layer_idx = -1;
  boost::shared_ptr<GaussianFactor> max_fac_ptr;
  for(const auto& fac: factors) {
    if (fac) {
      const int tmp_layer_idx = fac->getHypoLayer()->getLayerIdx();

      if (tmp_layer_idx >= max_layer_idx) {
        max_layer_idx = tmp_layer_idx;
        max_fac_ptr = fac;
      }
    }
  }

  const HypoList& hypo_list = max_fac_ptr->getHypoList();

  hessian_list_.resize(hypo_list.size());

  for (HessListIter it = hessian_list_.begin(); it != hessian_list_.end(); ++it) {

    (*it).Allocate(scatter ? *scatter : Scatter(factors, true));
  }

  for(const auto& fac: factors) {
    //[MH-A]: find corresponding hypos in each factor and update corresponding hessian info (use recursive)

    if (fac) {
      //[MH-C]:
      if (fac->removeAccumulatedPruned()) { //[MH-opt]: work on cached factors only...
        // Some Factors NOT pruned correctly????
        //TODO: Do nothing for now...
      }

      fac->mhUpdateHessian(keys_, hessian_list_, max_layer_idx);
    }
  }

  resulting_layer_ = max_fac_ptr->getHypoLayer();

  //gttoc(update);

}
//======================= END HessianFactor(GaussianFactorGraph, Sactter) ==================

} // gtsam


text:
//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/CPPAlliance/url
//

#ifndef BOOST_URL_DETAIL_IMPL_PCT_ENCODING_IPP
#define BOOST_URL_DETAIL_IMPL_PCT_ENCODING_IPP

#include <boost/url/detail/pct_encoding.hpp>
#include <boost/assert.hpp>
#include <cstring>

namespace boost {
namespace urls {
namespace detail {

// returns true if plain_key == decode(encoded)
bool
key_equal_encoded(
    string_view plain_key,
    pct_encoded_str encoded) noexcept
{
    if(plain_key.size() !=
        encoded.decoded_size)
        return false; // trivial reject
    if(encoded.str.size() ==
        encoded.decoded_size)
    {
        // plain comparison
        return std::memcmp(
            plain_key.data(),
            encoded.str.data(),
            plain_key.size()) == 0;
    }
    auto it0 = plain_key.data();
    auto it1 = encoded.str.data();
    auto const end0 = it0 + plain_key.size();
    auto const end1 = it1 + encoded.str.size();
    for(;;)
    {
        if(it1 == end1)
            return it0 == end0;
        if(it0 == end0)
            return false;
        if(*it1 != '%')
        {
            if(*it1++ != *it0++)
                return false;
            continue;
        }
        // VFALCO Was
        // BOOST_ASSERT(end1 - it1 >= 3);
        if(end1 - it1 < 3)
            return false;
        auto const ch = static_cast<char>(
            (static_cast<unsigned char>(
                bnf::hexdig_value(it1[1])) << 4) +
            static_cast<unsigned char>(
                bnf::hexdig_value(it1[2])));
        if(ch != *it0++)
            return false;
        it1 += 3;
    }
}

//------------------------------------------------

// VFALCO This is here for the cases
// where we don't store the decoded size
std::size_t
pct_decode_size_unchecked(
    string_view s) noexcept
{
    auto it = s.data();
    auto const end =
        it + s.size();
    std::size_t n = 0;
    while(it != end)
    {
        if(*it != '%')
        {
            // unescaped
            ++it;
            ++n;
            continue;
        }
        BOOST_ASSERT(
            end - it >= 3);
        it += 3;
        ++n;
    }
    return n;
}

// Preconditions:
// * valid encoding for options
// * [first, last) is exactly big enough
void
pct_decode_unchecked(
    char* dest,
    char const* last,
    string_view s,
    pct_decode_opts const& opt) noexcept
{
    (void)last;
    auto it = s.data();
    auto const end = it + s.size();
    error_code ec;
    if(! opt.plus_to_space)
    {
        while(it != end)
        {
            BOOST_ASSERT(dest < last);
            if(*it == '%')
            {
                // escaped
                ++it;
                *dest++ = decode_hex_octet(
                    it, end, ec);
                BOOST_ASSERT(! ec.failed());
                continue;
            }
            // unescaped
            *dest++ = *it++;
        }
        return;
    }

    // plus decodes to space
    while(it != end)
    {
        BOOST_ASSERT(dest < last);
        if(*it == '+')
        {
            // plus to space
            *dest++ = ' ';
            ++it;
            continue;
        }
        if(*it == '%')
        {
            // escaped
            ++it;
            *dest++ = decode_hex_octet(
                it, end, ec);
            BOOST_ASSERT(! ec.failed());
            continue;
        }
        // unescaped
        *dest++ = *it++;
    }
}

} // detail
} // urls
} // boost

#endif

ext: ipp

max_stars_repo_name: TankerHQ/url

max_issues_repo_name: TankerHQ/url

max_forks_repo_name: TankerHQ/url


text:
/**
 * Copyright 2013 JDSU
 * Author: Andy Lai (Andy.Lai@jdsu.com)
 */
#include <boost/regex.hpp>
#include <boost/algorithm/string/regex.hpp>
#include "xtreme/common/pubsub_filter/eci.h"

namespace xtreme {
namespace filter {
boost::regex regex_eci_standard_("[\\da-fA-F]{7,7}");
// boost::regex regex_eci_star_("([\\da-fA-F]{5,5})[\\*]");
boost::regex regex_eci_range_("([\\da-fA-F]{5,5})[\\*]-([\\da-fA-F]{5,5})[\\*]");
ECI::ECI(const std::string& value) throw(InvalidFieldValueException)
    : value_(0), mask_(0), value_set_(false), range_(false) {
  if (value.empty()) {
    throw InvalidFieldValueException("string is empty");
  }
  boost::match_results<std::string::const_iterator> what;
  boost::match_flag_type flags = boost::match_default;
  if (value.find("-") != std::string::npos) {
    if (boost::regex_match(value, what, regex_eci_range_, flags)) {
      mask_ = 0x0fffff00;
      std::string str_lower = std::string(what[1].first, what[1].second);
      std::string str_upper = std::string(what[2].first, what[2].second);
      str_lower += "??";
      str_upper += "??";
      ExtractValue(str_lower, false, lower_);
      ExtractValue(str_upper, false, upper_);
    } else {
      throw InvalidFieldValueException("The format is incorrect");
    }
    range_ = true;
  } else if (value.find("*") != std::string::npos || value.find("?") != std::string::npos) {
    std::string val_question_symbol("");
    std::vector<std::string> result_star;
    boost::algorithm::split_regex(result_star, value,
                                  boost::regex("\\*"));
    unsigned int result_star_len = result_star.size();
    if (result_star_len > 2) {
      char str[256];
      snprintf(str, sizeof(str), "[%s] contain more than 1 symbol *", value.c_str());
      throw InvalidFieldValueException(str);
    } else if (result_star_len == 2) {  // one *
      uint8_t len_replace = 7 - (value.length() - 1);
      uint8_t value_len = value.length();
      for (uint8_t i = 0; i < value_len; ++i) {
        if (value[i] == '*') {
          for (uint8_t j = 0; j < len_replace; ++j) {
            val_question_symbol += '?';
          }
        } else {
          val_question_symbol += value[i];
        }
      }
    } else {  // without *
      val_question_symbol = value;
    }
    if (val_question_symbol.length() != 7) {
      throw InvalidFieldValueException("length of string is incorrect!");
    }
    ExtractValue(val_question_symbol, true, value_);
  } else {
    if (boost::regex_match(value, what, regex_eci_standard_, flags)) {
      mask_ = 0x0fffffff;
      ExtractValue(value, false, value_);
    } else {
      throw InvalidFieldValueException("The format is incorrect");
    }
  }
}

bool ECI::operator ==(const uint32_t& b) const {
  if (!range_) {
    return (value_ == (b & mask_));
  } else {
    uint32_t tmp = (b & mask_);
    return (tmp >= lower_ && tmp <= upper_);
  }
}

void ECI::ExtractValue(const std::string& value, bool set_mask, uint32_t& output)
throw(InvalidFieldValueException) {
  output = 0;
  if (set_mask)
    mask_ = 0;
  int len = value.length();
  for (int i = 0; i < len; ++i) {
    if (value[i] == '?') {
      value_set_ = true;
    } else {
      if (!(value[i] >= '0' && value[i] <= '9')
          && !(value[i] >= 'a' && value[i] <= 'f')
          && !(value[i] >= 'A' && value[i] <= 'F')) {
        throw InvalidFieldValueException("string contain non hexadecimal symbol");
      } else {
        uint32_t value_base;
        if (value[i] >= '0' && value[i] <= '9')
          value_base = value[i] - '0';
        else if (value[i] >= 'a' && value[i] <= 'f') {
          value_base = value[i] - 'a' + 10;
        } else {
          value_base = value[i] - 'A' + 10;
        }
        output += (value_base << ((6-i)*4));
        if (set_mask) {
          uint32_t mask_base = 0xf;
          mask_ += (mask_base << ((6-i)*4));
        }
      }
    }
  }
}
}
}

ext: cc


text:
/*!
 * @file
 * Defines the @ref Monoid typeclass.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_MONOID_HPP
#define BOOST_MPL11_MONOID_HPP

#include <boost/mpl11/fwd/monoid.hpp>

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/core.hpp>
#include <boost/mpl11/detail/left_folds/variadic.hpp>

namespace boost { namespace mpl11 {
    template <typename Left, typename Right, typename>
    struct Monoid : false_ { };

    template <typename x1, typename x2, typename ...xn>
    struct plus
        : detail::left_folds::variadic<plus, x1, x2, xn...>
    { };

    template <typename x, typename y>
    struct plus<x, y> :
        Monoid<
            typename datatype<typename x::type>::type,
            typename datatype<typename y::type>::type
        >::template plus_impl<typename x::type, typename y::type>
    { };

    template <typename Datatype>
    struct zero : Monoid<Datatype>::template zero_impl<> { };
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_MONOID_HPP

ext: hpp


text:
//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2006-2009. Distributed under the Boost
// Software License, Version 1.0. (See accompanying file
// LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/interprocess for documentation.
//
//////////////////////////////////////////////////////////////////////////////
#include <boost/interprocess/detail/config_begin.hpp>
#include <boost/interprocess/detail/workaround.hpp>
//[doc_complex_map
#include <boost/interprocess/managed_shared_memory.hpp>
#include <boost/interprocess/allocators/allocator.hpp>
#include <boost/interprocess/containers/map.hpp>
#include <boost/interprocess/containers/vector.hpp>
#include <boost/interprocess/containers/string.hpp>
//<-
#include "../test/get_process_id_name.hpp"
//->

using namespace boost::interprocess;

//Typedefs of allocators and containers
typedef managed_shared_memory::segment_manager                       segment_manager_t;
typedef allocator<void, segment_manager_t>                           void_allocator;
typedef allocator<int, segment_manager_t>                            int_allocator;
typedef vector<int, int_allocator>                                   int_vector;
typedef allocator<int_vector, segment_manager_t>                     int_vector_allocator;
typedef vector<int_vector, int_vector_allocator>                     int_vector_vector;
typedef allocator<char, segment_manager_t>                           char_allocator;
typedef basic_string<char, std::char_traits<char>, char_allocator>   char_string;

class complex_data
{
   int               id_;
   char_string       char_string_;
   int_vector_vector int_vector_vector_;

   public:
   //Since void_allocator is convertible to any other allocator<T>, we can simplify
   //the initialization taking just one allocator for all inner containers.
   complex_data(int id, const char *name, const void_allocator &void_alloc)
      : id_(id), char_string_(name, void_alloc), int_vector_vector_(void_alloc)
   {}
   //Other members...
};

//Definition of the map holding a string as key and complex_data as mapped type
typedef std::pair<const char_string, complex_data>                      map_value_type;
typedef std::pair<char_string, complex_data>                            movable_to_map_value_type;
typedef allocator<map_value_type, segment_manager_t>                    map_value_type_allocator;
typedef map< char_string, complex_data
           , std::less<char_string>, map_value_type_allocator>          complex_map_type;

int main ()
{
   //Remove shared memory on construction and destruction
   struct shm_remove
   {
      //<-
      #if 1
      shm_remove() { shared_memory_object::remove(test::get_process_id_name()); }
      ~shm_remove(){ shared_memory_object::remove(test::get_process_id_name()); }
      #else
      //->
      shm_remove() { shared_memory_object::remove("MySharedMemory"); }
      ~shm_remove(){ shared_memory_object::remove("MySharedMemory"); }
      //<-
      #endif
      //->
   } remover;

   //Create shared memory
   //<-
   #if 1
   managed_shared_memory segment(create_only,test::get_process_id_name(), 65536);
   #else
   //->
   managed_shared_memory segment(create_only,"MySharedMemory", 65536);
   //<-
   #endif
   //->

   //An allocator convertible to any allocator<T, segment_manager_t> type
   void_allocator alloc_inst (segment.get_segment_manager());

   //Construct the shared memory map and fill it
   complex_map_type *mymap = segment.construct<complex_map_type>
      //(object name), (first ctor parameter, second ctor parameter)
         ("MyMap")(std::less<char_string>(), alloc_inst);

   for(int i = 0; i < 100; ++i){
      //Both key(string) and value(complex_data) need an allocator in their constructors
      char_string  key_object(alloc_inst);
      complex_data mapped_object(i, "default_name", alloc_inst);
      map_value_type value(key_object, mapped_object);
      //Modify values and insert them in the map
      mymap->insert(value);
   }
   return 0;
}
//]
#include <boost/interprocess/detail/config_end.hpp>

ext: cpp

max_issues_repo_name: boost-cmake/vintage

max_forks_repo_name: boost-cmake/vintage


text:
#ifndef BOOST_THREAD_EXPERIMENTAL_TASK_REGION_HPP
#define BOOST_THREAD_EXPERIMENTAL_TASK_REGION_HPP

//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Vicente J. Botet Escriba 2014. Distributed under the Boost
// Software License, Version 1.0. (See accompanying file
// LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/thread for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#include <boost/thread/experimental/parallel/v2/task_region.hpp>

#endif

max_stars_repo_name: rudylee/expo

max_issues_repo_name: rudylee/expo

max_forks_repo_name: rudylee/expo


text:
#include <thread>
#include <memory>
#include <fmt/core.h>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "zmqpp/zmqpp.hpp"

class dechat
{
private:
	zmqpp::context_t& ctx_;
	std::unique_ptr<zmqpp::actor> actor_;

public:
	dechat(zmqpp::context_t& ctx) :
		ctx_{ctx}
	{}

	void start()
	{
		zmqpp::socket_t broadcaster(ctx_, zmqpp::socket_type::publish);
		broadcaster.bind(std::format("tcp://*:9000"));

		actor_ = std::make_unique<zmqpp::actor>([this](zmqpp::socket_t* pipe) -> bool {
			return listener(pipe);
			});

		while (1) {
			std::string line;
			std::cin >> line;
			if (line.empty()) break;
			broadcaster.send(std::format("{}:{}", "Joe", line));
		}
	}

private:
	bool listener(zmqpp::socket_t* pipe)
	{
		pipe->send(zmqpp::signal::ok);

		zmqpp::socket_t lis(ctx_, zmqpp::socket_type::subscribe);

		for (int addr = 1; addr < 255; ++addr) {
			lis.connect(std::format("tcp://192.168.1.{}:9000", addr));
		}

		lis.set(zmqpp::socket_option::subscribe, "");
		while (1) {
			std::string msg;
			lis.receive(msg);
			fmt::print("{}\n", msg);
		}
		return true;
	}
};

int main()
{
	zmqpp::context_t ctx;
	dechat chat(ctx);
	chat.start();
	return 1;
}

max_stars_repo_path: zeromq/8-A Framework for Distributed Computing/dechat.cpp

max_stars_repo_name: pvthuyet/books

max_issues_repo_name: pvthuyet/books

max_forks_repo_name: pvthuyet/books


text:
//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_DIVFIX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_DIVFIX_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, divfix_, boost::dispatch::elementwise_<divfix_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, divfix_);
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::divfix_,divfix);

} }

#endif