text:
// Copyright (c) 2016-present, Facebook, Inc.
// All rights reserved.
//
// This source code is licensed under the BSD-style license found in the
// LICENSE file in the root directory of this source tree. An additional grant
// of patent rights can be found in the PATENTS file in the same directory.

#include <boost/graph/graphviz.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <devmand/devices/cli/translation/ReaderRegistry.h>

namespace devmand {
namespace devices {
namespace cli {

using namespace std;
using namespace folly;

const shared_ptr<Reader> StructuralReader::INSTANCE =
    static_pointer_cast<Reader>(make_shared<StructuralReader>());

Future<dynamic> CompositeReader::read(
    const Path& path,
    bool config,
    const DeviceAccess& device) const {
  if (shouldDelegate(path)) {
    auto childName = path.unkeyed().getSegments()[registeredPath.getDepth()];
    if (children.find(childName) == children.end()) {
      return Future<dynamic>(ReadException(
          device.id(),
          path,
          "No reader registered at: " +
              registeredPath.getChild(childName).str()));
    }
    return children.at(childName)->read(path, config, device);
  }

  // If reading config and this is oper, skip
  if (!isConfig and config) {
    return Future<dynamic>(dynamic::object());
  }

  vector<Future<dynamic>> childrenFutures;
  childrenFutures.emplace_back(clientReader->read(path, device));
  for (auto& child : children) {
    childrenFutures.emplace_back(
        child.second->read(path.getChild(child.first), config, device));
  }

  return collect(childrenFutures.begin(), childrenFutures.end())
      .thenValue([](auto childrenData) {
        dynamic localData = childrenData[0];
        for (const auto& childData : childrenData) {
          if (childData == localData) {
            continue;
          }
          if (!childData.empty() &&
              !childData.items().begin()->second.empty()) {
            // add child data only if it's not empty
            localData.merge_patch(childData);
          }
        }
        return localData;
      })
      .thenValue([registeredPath = registeredPath](auto mergedData) -> dynamic {
        if (registeredPath == "/") {
          return mergedData;
        } else {
          return dynamic::object(registeredPath.getLastSegment(), mergedData);
        }
      });
}

Future<dynamic> CompositeListReader::read(
    const Path& path,
    bool config,
    const DeviceAccess& device) const {
  string registeredSegment = registeredPath.getLastSegment();
  if (!path.getKeysFromSegment(registeredSegment).empty()) {
    // Read single list item since we have the keys already
    return readSingle(path, config, device);
  }

  // Read entire list
  return static_pointer_cast<ListReader>(clientReader)
      ->readKeys(path, device)
      // FIXME passing "this" into lambda ... but a reader should exist
      // basically forever and only go away when program ends .. we don't
      // expect translation code to be registered and unregistered in runtime
      .thenValue([path, config, device, this](auto childrenKeys) {
        vector<Future<dynamic>> keyFutures;
        for (const auto& childKeys : childrenKeys) {
          keyFutures.emplace_back(
              this->readSingle(path.addKeys(childKeys), config, device));
        }

        return collect(keyFutures.begin(), keyFutures.end());
      })
      .thenValue([registeredSegment](auto childrenData) {
        auto listEntries = dynamic::array();
        for (const auto& childData : childrenData) {
          listEntries.push_back(childData.at(registeredSegment)[0]);
        }
        return dynamic::object(registeredSegment, listEntries);
      });
}

Future<dynamic> CompositeListReader::readSingle(
    const Path& path,
    bool config,
    const DeviceAccess& device) const {
  if (shouldDelegate(path)) {
    return CompositeReader::read(path, config, device);
  }

  // Do some encapsulation if reading a list entry
  return CompositeReader::read(path, config, device)
      .thenValue([registeredSegment =
                      registeredPath.getLastSegment()](auto singleEntryData) {
        return dynamic::object(
            registeredSegment,
            dynamic::array(singleEntryData.at(registeredSegment)));
      });
}

ostream& operator<<(ostream& os, const CompositeReader& reader) {
  os << string(reader.registeredPath.getDepth(), ' ') << reader.registeredPath;
  for (auto& child : reader.children) {
    os << endl << *child.second;
  }
  return os;
}

bool CompositeReader::shouldDelegate(const Path& path) const {
  return path.unkeyed() != registeredPath &&
      path.unkeyed().getDepth() > registeredPath.getDepth();
}

CompositeReader::CompositeReader(
    Path _registeredPath,
    shared_ptr<Reader> _clientReader,
    CompositeReader::Children&& _children,
    bool _isConfig)
    : registeredPath(_registeredPath),
      clientReader(_clientReader),
      children(forward<Children>(_children)),
      isConfig(_isConfig) {}

static bool isListReader(const shared_ptr<Reader>& reader) {
  return dynamic_pointer_cast<ListReader>(reader) != NULL;
}

CompositeListReader::CompositeListReader(
    Path _registeredPath,
    shared_ptr<Reader> _clientReader,
    CompositeReader::Children&& _children,
    bool _isConfig)
    : CompositeReader(
          _registeredPath,
          _clientReader,
          forward<Children>(_children),
          _isConfig) {
  if (!isListReader(_clientReader)) {
    throw ReaderRegistryException(
        "Unable to register non list reader for path: " +
        _registeredPath.str());
  }
}

Future<dynamic> ReaderRegistry::readConfiguration(
    const Path& path,
    const DeviceAccess& device) const {
  // TODO validate returned data
  return (rootReader->read(path, true, device)).thenValue([](auto data) {
    dynamic root = dynamic::object;
    root.merge_patch(data);
    return root;
  });
}

Future<dynamic> ReaderRegistry::readState(
    const Path& path,
    const DeviceAccess& device) const {
  // TODO validate returned data
  return (rootReader->read(path, false, device)).thenValue([](auto data) {
    dynamic root = dynamic::object;
    root.merge_patch(data);
    return root;
  });
}

ostream& operator<<(ostream& os, const ReaderRegistry& registry) {
  os << "ReaderRegistry: " << *registry.rootReader;
  return os;
}

unique_ptr<ReaderRegistry> ReaderRegistryBuilder::build() {
  if (allReaders.empty()) {
    ReaderRegistryException(
        "Unable to construct reader registry from 0 readers");
  }

  YangHierarchy pathGraph;

  for (auto& reader : allReaders) {
    Path childPath = reader.first;

    while (childPath.getDepth() > 0) {
      const Path& parentPath = childPath.getParent();

      YangHierarchy::vertex_descriptor parentVertex =
          boost::add_vertex(parentPath, pathGraph);
      pathGraph[parentPath].path = parentPath;

      YangHierarchy::vertex_descriptor childVertex =
          boost::add_vertex(childPath, pathGraph);
      pathGraph[childPath].path = childPath;

      if (!boost::lookup_edge(parentVertex, childVertex, pathGraph).second) {
        boost::add_edge_by_label(parentPath, childPath, pathGraph);
      }
      childPath = parentPath;
    }
  }

  if (VLOG_IS_ON(MDEBUG)) {
    stringstream dotGraph;
    boost::write_graphviz(dotGraph, pathGraph, PathVertexWriter(pathGraph));
    MLOG(MDEBUG) << "Reader hierarchy calculated: " << dotGraph.str();
  }

  PathVertex& rootVertex = pathGraph[Path::ROOT];
  return make_unique<ReaderRegistry>(
      createCompositeReader(pathGraph, rootVertex));
}

void ReaderRegistryBuilder::add(Path path, ReaderFunction lambda) {
  addReader(
      path, static_pointer_cast<Reader>(make_shared<LambdaReader>(lambda)));
}

void ReaderRegistryBuilder::addList(
    Path path,
    ListReaderFunction keysLambda,
    ReaderFunction lambda) {
  // schema validation
  if (schemaContext != SchemaContext::NO_MODELS) {
    if (!schemaContext.isPathValid(path) or !schemaContext.isList(path)) {
      throw ReaderRegistryException(
          "Unable to register list reader for path: " + path.str() +
          ". Path is not valid or does not point to a list node");
    }
  }

  addReader(
      path,
      static_pointer_cast<Reader>(
          make_shared<LambdaListReader>(keysLambda, lambda)));
}

void ReaderRegistryBuilder::addReader(Path path, shared_ptr<Reader> reader) {
  if (schemaContext != SchemaContext::NO_MODELS) {
    if (!schemaContext.isPathValid(path)) {
      throw ReaderRegistryException(
          "Unable to register reader for path: " + path.str() +
          ". Path is not valid");
    }
  }

  allReaders.insert_or_assign(path, reader);
}

unique_ptr<CompositeReader> ReaderRegistryBuilder::createCompositeReader(
    const YangHierarchy& pathGraph,
    const PathVertex& vertex) const {
  auto vertexIdx = pathGraph.vertex(vertex.path);

  // Create children composite nodes recursively
  CompositeReader::Children children;
  YangHierarchy::out_edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = boost::out_edges(vertexIdx, pathGraph);
       ei != ei_end;
       ++ei) {
    YangHierarchy::vertex_descriptor childVertexIdx =
        boost::target(*ei, pathGraph);
    const PathVertex& childVertex = pathGraph.graph()[childVertexIdx];
    children.emplace(
        childVertex.path.getLastSegment(),
        this->createCompositeReader(pathGraph, childVertex));
  }

  // Create composite reader for current path
  auto clientReader = allReaders.find(vertex.path);

  // Determine if this is config or operational node
  bool isConfig = true;
  if (schemaContext != SchemaContext::NO_MODELS) {
    isConfig = schemaContext.isConfig(vertex.path);
  }

  if (clientReader != allReaders.end()) {
    if (isListReader(clientReader->second)) {
      return make_unique<CompositeListReader>(
          vertex.path, clientReader->second, move(children), isConfig);
    } else {
      return make_unique<CompositeReader>(
          vertex.path, clientReader->second, move(children), isConfig);
    }
  } else {
    // schema validation
    if (schemaContext != SchemaContext::NO_MODELS) {
      if (schemaContext.isList(vertex.path)) {
        throw ReaderRegistryException(
            "Missing list reader for path: " + vertex.path.str() +
            ". List data cannot be generated. Reader hierarchy is invalid");
      }
    }
    return make_unique<CompositeReader>(
        vertex.path, StructuralReader::INSTANCE, move(children), isConfig);
  }
}

} // namespace cli
} // namespace devices
} // namespace devmand

max_stars_repo_name: idoshveki/magma

max_issues_repo_name: idoshveki/magma


text:
// test file for HSO3.hpp and HSO4.hpp

//  (C) Copyright Hubert Holin 2001.
//  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)

#include <iostream>

#include <boost/math/quaternion.hpp>

#include "HSO3.hpp"
#include "HSO4.hpp"

const int    number_of_intervals = 5;

const float    pi = ::std::atan(1.0f)*4;

void    test_SO3();

void    test_SO4();

int    main()

{
    test_SO3();

    test_SO4();

    ::std::cout << "That's all folks!" << ::std::endl;
}

//
//    Test of quaternion and R^3 rotation relationship
//

void    test_SO3_spherical()
{
    ::std::cout << "Testing spherical:" << ::std::endl;
    ::std::cout << ::std::endl;

    const float    rho = 1.0f;

    float        theta;
    float        phi1;
    float        phi2;

    for    (int idxphi2 = 0; idxphi2 <= number_of_intervals; idxphi2++)
    {
        phi2 = (-pi/2)+(idxphi2*pi)/number_of_intervals;

        for    (int idxphi1 = 0; idxphi1 <= number_of_intervals; idxphi1++)
        {
            phi1 = (-pi/2)+(idxphi1*pi)/number_of_intervals;

            for    (int idxtheta = 0; idxtheta <= number_of_intervals; idxtheta++)
            {
                theta = -pi+(idxtheta*(2*pi))/number_of_intervals;

                ::std::cout << "theta = " << theta << " ; ";
                ::std::cout << "phi1 = " << phi1 << " ; ";
                ::std::cout << "phi2 = " << phi2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::spherical(rho, theta, phi1, phi2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_semipolar()
{
    ::std::cout << "Testing semipolar:" << ::std::endl;
    ::std::cout << ::std::endl;

    const float    rho = 1.0f;

    float        alpha;
    float        theta1;
    float        theta2;

    for    (int idxalpha = 0; idxalpha <= number_of_intervals; idxalpha++)
    {
        alpha = (idxalpha*(pi/2))/number_of_intervals;

        for    (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
        {
            theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;

            for    (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
            {
                theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;

                ::std::cout << "alpha = " << alpha << " ; ";
                ::std::cout << "theta1 = " << theta1 << " ; ";
                ::std::cout << "theta2 = " << theta2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::semipolar(rho, alpha, theta1, theta2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_multipolar()
{
    ::std::cout << "Testing multipolar:" << ::std::endl;
    ::std::cout << ::std::endl;

    float    rho1;
    float    rho2;

    float    theta1;
    float    theta2;

    for    (int idxrho = 0; idxrho <= number_of_intervals; idxrho++)
    {
        rho1 = (idxrho*1.0f)/number_of_intervals;
        rho2 = ::std::sqrt(1.0f-rho1*rho1);

        for    (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
        {
            theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;

            for    (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
            {
                theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;

                ::std::cout << "rho1 = " << rho1 << " ; ";
                ::std::cout << "theta1 = " << theta1 << " ; ";
                ::std::cout << "theta2 = " << theta2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::multipolar(rho1, theta1, rho2, theta2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_cylindrospherical()
{
    ::std::cout << "Testing cylindrospherical:" << ::std::endl;
    ::std::cout << ::std::endl;

    float    t;

    float    radius;
    float    longitude;
    float    latitude;

    for    (int idxt = 0; idxt <= number_of_intervals; idxt++)
    {
        t = -1.0f+(idxt*2.0f)/number_of_intervals;
        radius = ::std::sqrt(1.0f-t*t);

        for    (int idxlatitude = 0; idxlatitude <= number_of_intervals; idxlatitude++)
        {
            latitude = (-pi/2)+(idxlatitude*pi)/number_of_intervals;

            for    (int idxlongitude = 0; idxlongitude <= number_of_intervals; idxlongitude++)
            {
                longitude = -pi+(idxlongitude*(2*pi))/number_of_intervals;

                ::std::cout << "t = " << t << " ; ";
                ::std::cout << "longitude = " << longitude;
                ::std::cout << "latitude = " << latitude;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::cylindrospherical(t, radius, longitude, latitude);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3_cylindrical()
{
    ::std::cout << "Testing cylindrical:" << ::std::endl;
    ::std::cout << ::std::endl;

    float    r;
    float    angle;

    float    h1;
    float    h2;

    for    (int idxh2 = 0; idxh2 <= number_of_intervals; idxh2++)
    {
        h2 = -1.0f+(idxh2*2.0f)/number_of_intervals;

        for    (int idxh1 = 0; idxh1 <= number_of_intervals; idxh1++)
        {
            h1 = ::std::sqrt(1.0f-h2*h2)*(-1.0f+(idxh2*2.0f)/number_of_intervals);
            r = ::std::sqrt(1.0f-h1*h1-h2*h2);

            for    (int idxangle = 0; idxangle <= number_of_intervals; idxangle++)
            {
                angle = -pi+(idxangle*(2*pi))/number_of_intervals;

                ::std::cout << "angle = " << angle << " ; ";
                ::std::cout << "h1 = " << h1;
                ::std::cout << "h2 = " << h2;
                ::std::cout << ::std::endl;

                ::boost::math::quaternion<float>    q = ::boost::math::cylindrical(r, angle, h1, h2);

                ::std::cout << "q = " << q << ::std::endl;

                R3_matrix<float>                    rot = quaternion_to_R3_rotation(q);

                ::std::cout << "rot = ";
                ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
                ::std::cout << "\t";
                ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;

                ::boost::math::quaternion<float>    p = R3_rotation_to_quaternion(rot, &q);

                ::std::cout << "p = " << p << ::std::endl;

                ::std::cout << "round trip discrepancy: " << ::boost::math::abs(q-p) << ::std::endl;

                ::std::cout << ::std::endl;
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO3()
{
    ::std::cout << "Testing SO3:" << ::std::endl;
    ::std::cout << ::std::endl;

    test_SO3_spherical();

    test_SO3_semipolar();

    test_SO3_multipolar();

    test_SO3_cylindrospherical();

    test_SO3_cylindrical();
}

//
//    Test of quaternion and R^4 rotation relationship
//

void    test_SO4_spherical()
{
    ::std::cout << "Testing spherical:" << ::std::endl;
    ::std::cout << ::std::endl;

    const float    rho1 = 1.0f;
    const float    rho2 = 1.0f;

    float        theta1;
    float        phi11;
    float        phi21;

    float        theta2;
    float        phi12;
    float        phi22;

    for    (int idxphi21 = 0; idxphi21 <= number_of_intervals; idxphi21++)
    {
        phi21 = (-pi/2)+(idxphi21*pi)/number_of_intervals;

        for    (int idxphi22 = 0; idxphi22 <= number_of_intervals; idxphi22++)
        {
            phi22 = (-pi/2)+(idxphi22*pi)/number_of_intervals;

            for    (int idxphi11 = 0; idxphi11 <= number_of_intervals; idxphi11++)
            {
                phi11 = (-pi/2)+(idxphi11*pi)/number_of_intervals;

                for    (int idxphi12 = 0; idxphi12 <= number_of_intervals; idxphi12++)
                {
                    phi12 = (-pi/2)+(idxphi12*pi)/number_of_intervals;

                    for    (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
                    {
                        theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;

                        for    (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
                        {
                            theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;

                            ::std::cout << "theta1 = " << theta1 << " ; ";
                            ::std::cout << "phi11 = " << phi11 << " ; ";
                            ::std::cout << "phi21 = " << phi21;
                            ::std::cout << "theta2 = " << theta2 << " ; ";
                            ::std::cout << "phi12 = " << phi12 << " ; ";
                            ::std::cout << "phi22 = " << phi22;
                            ::std::cout << ::std::endl;

                            ::boost::math::quaternion<float>    p1 = ::boost::math::spherical(rho1, theta1, phi11, phi21);

                            ::std::cout << "p1 = " << p1 << ::std::endl;

                            ::boost::math::quaternion<float>    q1 = ::boost::math::spherical(rho2, theta2, phi12, phi22);

                            ::std::cout << "q1 = " << q1 << ::std::endl;

                            ::std::pair< ::boost::math::quaternion<float> , ::boost::math::quaternion<float> >    pq1 =
                                ::std::make_pair(p1,q1);

                            R4_matrix<float>                    rot = quaternions_to_R4_rotation(pq1);

                            ::std::cout << "rot = ";
                            ::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << "\t" << rot.a14 << ::std::endl;
                            ::std::cout << "\t";
                            ::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << "\t" << rot.a24 << ::std::endl;
                            ::std::cout << "\t";
                            ::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << "\t" << rot.a34 << ::std::endl;
                            ::std::cout << "\t";
                            ::std::cout << "\t" << rot.a41 << "\t" << rot.a42 << "\t" << rot.a43 << "\t" << rot.a44 << ::std::endl;

                            ::std::pair< ::boost::math::quaternion<float> , ::boost::math::quaternion<float> >    pq2 =
                                R4_rotation_to_quaternions(rot, &pq1);

                            ::std::cout << "p1 = " << pq2.first << ::std::endl;
                            ::std::cout << "p2 = " << pq2.second << ::std::endl;

                            ::std::cout << "round trip discrepancy: " << ::std::sqrt(::boost::math::norm(pq1.first-pq2.first)+::boost::math::norm(pq1.second-pq2.second)) << ::std::endl;

                            ::std::cout << ::std::endl;
                        }
                    }
                }
            }
        }
    }

    ::std::cout << ::std::endl;
}

void    test_SO4()
{
    ::std::cout << "Testing SO4:" << ::std::endl;
    ::std::cout << ::std::endl;

    test_SO4_spherical();
}

ext: cpp


text:
/*
 * Copyright 2018 Google 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 "health_checker.h"

#include "store.h"

#include <boost/filesystem.hpp>
#include <boost/algorithm/string/join.hpp>
#include <vector>
#include <fstream>

namespace google {

HealthChecker::HealthChecker(const Configuration& config,
                             const MetadataStore& store)
    : config_(config), store_(store) {}

void HealthChecker::SetUnhealthy(const std::string& component) {
  std::lock_guard<std::mutex> lock(mutex_);
  unhealthy_components_.insert(component);
}

std::set<std::string> HealthChecker::UnhealthyComponents() const {
  std::lock_guard<std::mutex> lock(mutex_);
  std::set<std::string> result(unhealthy_components_);

  Timestamp cutoff = std::chrono::system_clock::now()
    - std::chrono::seconds(config_.HealthCheckMaxDataAgeSeconds());
  auto last_collection_times = store_.GetLastCollectionTimes();
  for (const auto& kv : last_collection_times) {
    const std::string& object_type = kv.first;
    const Timestamp& collected_at = kv.second;
    if (collected_at < cutoff) {
      result.insert(object_type);
    }
  }
  return result;
}

}  // namespace google

ext: cc

max_stars_repo_name: StevenYCChou/metadata-agent

max_issues_repo_name: Stackdriver/metadata-agent

max_forks_repo_name: StevenYCChou/metadata-agent


text:
//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk.h>
#include <OpenTissue/core/geometry/geometry_obb.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk);

BOOST_AUTO_TEST_CASE(face_aligned_separated_boxes)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::real_type                            real_type;

  vector3_type p_a;
  vector3_type p_b;
  real_type tol = 0.01;
  OpenTissue::gjk::obsolete::detail::GJK<vector3_type > gjk;          ///< GJK collision detection Algorithm.
  OpenTissue::geometry::OBB<math_types> A;
  OpenTissue::geometry::OBB<math_types> B;

  A.init(1.0,1.0,1.0);
  B.init(1.0,1.0,1.0);

  vector3_type vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector3_type vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector3_type vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector3_type vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector3_type vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector3_type vector_mz = vector3_type( 0.0, 0.0,-1.0);

  coordsys_type  Awcs = coordsys_type( vector_mx, quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector_px, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  real_type distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_px, quaternion_type());
  Bwcs = coordsys_type( vector_mx, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_py, quaternion_type());
  Bwcs = coordsys_type( vector_my, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_my, quaternion_type());
  Bwcs = coordsys_type( vector_py, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_pz, quaternion_type());
  Bwcs = coordsys_type( vector_mz, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_mz, quaternion_type());
  Bwcs = coordsys_type( vector_pz, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );
}

BOOST_AUTO_TEST_CASE(non_aligned_cases)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::real_type                            real_type;

  vector3_type p_a;
  vector3_type p_b;
  real_type tol = 0.01;
  OpenTissue::gjk::obsolete::detail::GJK<vector3_type > gjk;          ///< GJK collision detection Algorithm.
  OpenTissue::geometry::OBB<math_types> A;
  OpenTissue::geometry::OBB<math_types> B;

  A.init(1.0,1.0,1.0);
  B.init(1.0,1.0,1.0);

  real_type sqrt_half = std::sqrt(0.5);
  real_type test_distance = 1.0 - (sqrt_half - 0.5);

  quaternion_type q;
  q.Rz( math_types::value_traits::pi()/4.0 );

  vector3_type vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector3_type vector_mx = vector3_type(-1.0, 0.0, 0.0);
  coordsys_type  Awcs = coordsys_type( vector_mx, quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector_px, q);
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  real_type distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, test_distance, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector_mx, quaternion_type());
  Bwcs = coordsys_type( vector3_type( 1.0,0.1,0.1), q);
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, test_distance, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector3_type(-1.0,0.0,0.5), quaternion_type());
  Bwcs = coordsys_type( vector_px, q);
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, test_distance, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector3_type(-1.0,0.0,-0.5), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 1.0,0.0,0.5), quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );
}

BOOST_AUTO_TEST_CASE(penetrating_cases)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::real_type                            real_type;

  vector3_type p_a;
  vector3_type p_b;

  OpenTissue::gjk::obsolete::detail::GJK<vector3_type > gjk;          ///< GJK collision detection Algorithm.
  OpenTissue::geometry::OBB<math_types> A;
  OpenTissue::geometry::OBB<math_types> B;

  A.init(1.0,1.0,1.0);
  B.init(1.0,1.0,1.0);

  vector3_type vector_px = vector3_type( 1.0, 0.0, 0.0);
  coordsys_type  Awcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector3_type( 0.25,0.25,0.25), quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  real_type distance = length( p_a - p_b);
  BOOST_CHECK(fabs(distance) < 10e-7);
  BOOST_CHECK( gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  B.init(0.9,0.9,0.9);

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);

  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK(fabs(distance) < 10e-7);
  BOOST_CHECK( gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( gjk.get_common_point(A,B,vector_px,p_a,p_b) );
}

BOOST_AUTO_TEST_SUITE_END();

max_stars_repo_name: ricortiz/OpenTissue

max_issues_repo_name: ricortiz/OpenTissue

max_forks_repo_name: ricortiz/OpenTissue


text:
// polyvec
#include <polyvec/geometry/winding_number.hpp>
#include <polyvec/mc/raster_image_connectivity.hpp>
#include <polyvec/mc/get_bounding_box.hpp>
#include <polyvec/mc/bit_vector.hpp>
#include <polyvec/mc/associate_holes.hpp>

// libc++
#include <cstdio>
#include <memory>

// eigen
#include <Eigen/Geometry>

NAMESPACE_BEGIN ( polyfit )
NAMESPACE_BEGIN ( mc )

constexpr int RasterImageConnectivity::infinite_region;

Eigen::Vector2i
RasterImageConnectivity::pixel_vertex_index_to_pos ( const int index ) {
    return Eigen::Vector2i ( index % pixel_vertex_dims().x(), index / pixel_vertex_dims().x() );
}

int
RasterImageConnectivity::pixel_vertex_pos_to_index ( const Eigen::Vector2i& pos ) {
    return pos.x() + pos.y() * pixel_vertex_dims().x();
}

int
RasterImageConnectivity::n_pixel_vertices() {
    return pixel_vertex_dims().x() * pixel_vertex_dims().y();
}

const Eigen::Vector2i&
RasterImageConnectivity::pixel_vertex_dims() const {
    return _pixel_vertex_dims;
}

Mesh_connectivity&
RasterImageConnectivity::connectivity() {
    return _connectivity;
}

int
RasterImageConnectivity::n_polygons() {
    return connectivity().n_active_faces();
}

int
RasterImageConnectivity::n_regions() {
    return _n_regions;
}

int
RasterImageConnectivity::n_region_polygons ( const int region_id ) {
    assert_break ( region_id < n_regions() );
    const int ans= _region_to_polygon_xval[region_id+1]-_region_to_polygon_xval[region_id];
    assert_break ( ans >= 1 );
    return ans;
}

int
RasterImageConnectivity::polygon_to_region ( const int polygon_id ) {
    assert_break ( polygon_id < n_polygons() );
    return _polygon_to_region[ polygon_id ];
}

void
RasterImageConnectivity::region_to_polygon ( const int region_id, int& outer_boundary, std::vector<int>& holes ) {
    outer_boundary = _region_to_polygon_val[ _region_to_polygon_xval[ region_id ] ];
    assert_break ( outer_boundary == region_id );

    holes.resize ( 0 );
    holes.reserve ( std::max ( 0, n_region_polygons ( region_id )-1 ) );

    for ( int j= _region_to_polygon_xval[ region_id ]+1 ; j < _region_to_polygon_xval[ region_id+1 ] ; ++j ) {
        holes.push_back (  _region_to_polygon_val [j] );
    }
}

RasterImageConnectivity::PolygonType
RasterImageConnectivity::get_polygon_type ( const int polygon_id ) {
    if ( polygon_id < n_regions() ) {
        return POLYGON_OUTER_BOUNDARY;
    } else if ( polygon_id < n_polygons() ) {
        return POLGYON_HOLE ;
    } else {
        assert_break ( 0 );
        return POLYGON_INVALID;
    }
}

Eigen::Matrix2Xi
RasterImageConnectivity::get_polygon_points ( const int pid ) {
    // Lambda to extract the points on polygon
    std::vector<int> points;

    Mesh_connectivity::Face_iterator face = connectivity().face_at ( pid );
    Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
    Mesh_connectivity::Half_edge_iterator he = he_end;

    int safe_guard = 0;
    const int safe_gaurd_cap = 10000;

    do {
        Eigen::Vector2i pos =  pixel_vertex_index_to_pos ( he.origin().index() );
        points.push_back ( pos.x() );
        points.push_back ( pos.y() );
        assert_break ( safe_guard < safe_gaurd_cap );
        ++safe_guard;
        he = he.next();
    } while ( !he.is_equal ( he_end ) );

    return Eigen::Matrix2Xi::ConstMapType ( points.data(), 2, points.size() /2 );
}

std::vector<int>
RasterImageConnectivity::regions_sorted_by_area() {
    std::vector<int> by_area(n_regions());
    std::vector<double> areas(n_regions());
    for (int i = 0; i < (int)by_area.size(); ++i) {
      by_area[i] = i;
      bool _1;
      Eigen::Matrix2Xi bdry = get_polygon_points(i);
      polyvec::WindingNumber::compute_orientation(bdry.cast<double>(), _1,
                                                  areas[i]);
    }
    std::sort(by_area.begin(), by_area.end(),
              [areas](int i, int j) { return areas[i] > areas[j]; });

    return by_area;
}

// dump as vtk for debugging
void
RasterImageConnectivity::dump_as_vtk ( const std::string filename ) {

    // Defrag the mesh
    Mesh_connectivity::Defragmentation_maps defrag;
    connectivity().compute_defragmention_maps ( defrag );

    FILE* fl = fopen ( filename.c_str(), "w" );
    assert_break ( fl && "FILE should be open" );

    /*
     * Write the vtk file.
     */

    // write the header
    fprintf ( fl, "# vtk DataFile Version 2.0\n" );
    fprintf ( fl, "Shayan's output mesh\n" );
    fprintf ( fl, "ASCII\n" );
    fprintf ( fl, "DATASET UNSTRUCTURED_GRID\n" );
    fprintf ( fl, "\n" );

    // write the vertices
    fprintf ( fl, "POINTS %d float\n", connectivity().n_active_vertices() );

    for ( int vnidx = 0; vnidx < connectivity().n_active_vertices(); vnidx++ ) {
        int voidx = defrag.new2old_vertices[vnidx];
        Eigen::Vector2i pos = pixel_vertex_index_to_pos ( voidx );
        fprintf ( fl, "%d %d 0 \n",  pos.x(), pos.y() );
    }

    fprintf ( fl, "\n" );

    //
    // write the faces
    //

    // count their total number of vertices.
    int total_vert_duplicated_per_face = 0;

    for ( int fn = 0; fn < connectivity().n_active_faces(); ++fn ) {
        int fo = defrag.new2old_faces[fn];
        Mesh_connectivity::Face_iterator face = connectivity().face_at ( fo );
        Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
        Mesh_connectivity::Half_edge_iterator he = face.half_edge();

        do {
            ++total_vert_duplicated_per_face;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );
    }

    int face_verts_cache[4096]; // this must be more than maximum number of edges per face.
    fprintf ( fl, "CELLS %d %d \n", connectivity().n_active_faces(), connectivity().n_active_faces() + total_vert_duplicated_per_face );

    for ( int fn = 0; fn < connectivity().n_active_faces(); ++fn ) {
        int fo = defrag.new2old_faces[fn];
        Mesh_connectivity::Face_iterator face = connectivity().face_at ( fo );
        int n_verts = 0;
        Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
        Mesh_connectivity::Half_edge_iterator he = face.half_edge();

        do {
            face_verts_cache[n_verts] = defrag.old2new_vertices[he.origin().index()];
            ++n_verts;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );

        fprintf ( fl, "%d ", n_verts );

        for ( int voffset = 0; voffset < n_verts; ++voffset ) {
            //
            fprintf ( fl, "%d ", face_verts_cache[voffset] );
        }

        fprintf ( fl, "\n" );
    }

    fprintf ( fl, "\n" );

    // write the face types
    fprintf ( fl, "CELL_TYPES %d \n", connectivity().n_active_faces() );

    for ( int f = 0; f < connectivity().n_active_faces(); f++ ) {
        fprintf ( fl, "7 \n" ); // VTK POLYGON
    }

    fprintf ( fl, "\n" );

    // Now write the pixel indices
    fprintf ( fl, "POINT_DATA %d \n", connectivity().n_active_vertices() );

//
    fprintf ( fl, "SCALARS %s int 1 \n", "pixel_point_index" );
    fprintf ( fl, "LOOKUP_TABLE default \n" );

    for ( int i = 0; i < connectivity().n_active_vertices(); i++ ) {
        int voidx = defrag.new2old_vertices[i];
        fprintf ( fl, "%d \n", voidx );
    }

// Write region colors?
    fprintf ( fl, "CELL_DATA %d \n", connectivity().n_active_faces() );

//
    fprintf ( fl, "SCALARS %s int 1 \n", "region_index" );
    fprintf ( fl, "LOOKUP_TABLE default \n" );

    for ( int i = 0; i < connectivity().n_active_faces(); i++ ) {
        fprintf ( fl, "%d \n", i );
    }

    fclose ( fl );
}

void
RasterImageConnectivity::enumerate_holes ( Mesh_connectivity& pixconn, std::vector<int>& half_edge_on_hole ) {
    assert_break ( pixconn.n_active_half_edges() == pixconn.n_total_half_edges() );
    assert_break ( pixconn.n_active_faces() == pixconn.n_total_faces() );

    // Start marking all the half-edges
    Bit_vector is_half_edge_marked ( pixconn.n_active_half_edges() );

    // A lambda to traverse a hole and mark all the half edges on it
    auto traverse_bounday = [&] ( const int seed_he ) {
        Mesh_connectivity::Half_edge_iterator he_end = pixconn.half_edge_at ( seed_he );
        Mesh_connectivity::Half_edge_iterator he = he_end;
        assert_break ( he.face().is_equal ( pixconn.hole() ) );

        int safe_guard = 0;
        const int safe_gaurd_cap = 10000;

        do {
            is_half_edge_marked.set ( he.index() );
            assert_break ( safe_guard < safe_gaurd_cap );
            ++safe_guard;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );
    };

    // Loop over all half-edges, if not marked and boundary, it is a hole
    half_edge_on_hole.resize ( 0 );

    for ( int heid = 0 ; heid < pixconn.n_total_half_edges() ; ++heid ) {
        Mesh_connectivity::Half_edge_iterator he = pixconn.half_edge_at ( heid );

        if ( he.face().is_equal ( pixconn.hole() ) && ( !is_half_edge_marked[heid] ) ) {
            half_edge_on_hole.push_back ( heid );
            traverse_bounday ( heid );
        }
    }
}

// constructor
RasterImageConnectivity
RasterImageConnectivity::build ( const std::vector<Eigen::Matrix2Xi>& region_boundaries ) {

    RasterImageConnectivity ans;

    //
    // First find the bounding box of the image
    //
    {
        Eigen::AlignedBox<int, 2> bbox = get_bounding_box ( region_boundaries );
        // Make sure min is 0,0. This must have been done in get_pixel_regions()
        assert_break ( bbox.min() == Eigen::Vector2i ( 0, 0 ) );
        // Set the dims to the max
        ans._pixel_vertex_dims = bbox.max()+Eigen::Vector2i ( 1, 1 );
    }

    //
    // Now bould the connectivity by treating each region as a BIG polygon
    //
    {
        std::vector<int> polygon_verts;
        std::vector<int> polygon_xadj = {0};

        for ( int i = 0 ; i < ( int ) region_boundaries.size() ; ++i ) {
            polygon_xadj.push_back ( polygon_xadj.back() );
            const Eigen::Matrix2Xi& region_boundary = region_boundaries[i];

            for ( int j = 0 ; j < ( int ) region_boundary.cols() ; ++j ) {
                polygon_verts.push_back ( ans.pixel_vertex_pos_to_index ( region_boundary.col ( j ) ) );
                ++polygon_xadj.back();
            }
        }

        ans._connectivity.build_from_polygons ( ans.n_pixel_vertices(), polygon_verts, polygon_xadj );
    }

    //
    // Now get all the holes and add them
    // Even the outer boundary -- it should be fine
    //
    {
        Mesh_connectivity& pixconn = ans.connectivity();

        // Set number of regions
        ans._n_regions = ( int ) region_boundaries.size();

        // A lambda to modify the connectivity
        auto associate_face_and_he = [&] ( const int seed_he, const int parent_face ) {
            Mesh_connectivity::Half_edge_iterator he_end = pixconn.half_edge_at ( seed_he );
            Mesh_connectivity::Half_edge_iterator he = he_end;
            Mesh_connectivity::Face_iterator face = pixconn.face_at ( parent_face );
            assert_break ( he.face().is_equal ( pixconn.hole() ) );

            int safe_guard = 0;
            const int safe_gaurd_cap = 10000;

            // Make the association
            face.data().half_edge = he.index();

            do {
                he.data().face = face.index();
                assert_break ( safe_guard < safe_gaurd_cap );
                ++safe_guard;
                he = he.next();
            } while ( !he.is_equal ( he_end ) );
        };

        // Get all the holes
        std::vector<int> he_on_holes;
        enumerate_holes ( pixconn,  he_on_holes );

        for ( int heid: he_on_holes ) {
            Mesh_connectivity::Face_iterator face =  pixconn.add_face();
            associate_face_and_he ( heid, face.index() );
        }

    } // End of creating the hole polygons

    // Now associate holes and boundary (polygons to regions)
    {
        Mesh_connectivity& pixconn = ans.connectivity();

        // No extract the points
        // Get the holes and polygons as CCW paths
        // And also find the areas
        std::vector<Eigen::Matrix2Xd> outer_boundaries;
        std::vector<double> outer_boundary_areas;
        std::vector<Eigen::Matrix2Xd> holes;
        std::vector<double> hole_areas;

        for ( int i = 0 ; i < ans.n_regions() ; ++i ) {
            outer_boundaries.push_back ( ans.get_polygon_points ( i ).cast<double>() );
            bool is_ccw;
            double area;
            polyvec::WindingNumber::compute_orientation ( outer_boundaries.back(), is_ccw, area );
            assert_break ( is_ccw );
            outer_boundary_areas.push_back ( area );
        }

        for ( int i = ans.n_regions()  ; i < ans.n_polygons() ; ++i ) {
            holes.push_back ( ans.get_polygon_points ( i ).cast<double>() );
            holes.back().rowwise().reverseInPlace(); // make CCW
            bool is_ccw;
            double area;
            polyvec::WindingNumber::compute_orientation ( holes.back(), is_ccw, area );
            assert_break ( is_ccw );
            hole_areas.push_back ( area );
        }

        //
        // Create lambdas that say if a polygon is inside another
        //  assumes that out and in are both pixel boundaries
        // and are ccw
        // Only works if the area of out is bigger than in
        //
        auto is_inside = [&] ( const Eigen::Matrix2Xd &out, const Eigen::Matrix2Xd &in ) {
            const Eigen::Vector2d mid = (in.col ( 0 ) + in.col ( 1 )) /2.;
            const Eigen::Vector2d tang = in.col ( 1 ) -  in.col ( 0 );
            const Eigen::Vector2d normal ( -tang.y(), tang.x() );
            const Eigen::Vector2d definitely_inside_in = normal/2. + mid;

            bool is_trustable;
            double winding;
            ::polyvec::WindingNumber::compute_winding ( out, definitely_inside_in, winding, is_trustable );
            assert_break ( is_trustable );
            return std::abs ( winding ) > 1e-5;
        };

        // Now call the association function
        std::vector<std::vector<int>> boundary_holes;
        associate_holes (
            ( int ) outer_boundaries.size(),
            ( int ) holes.size(),
        [&] ( const int bout, const int hin ) {
            if ( ( int ) outer_boundary_areas[bout] <= ( int ) hole_areas[hin] ) {
                return false;
            } else {
                return  is_inside ( outer_boundaries[bout], holes[hin] );
            }
        },
        [&] ( const int hout, const int hin ) {
            if ( ( int ) hole_areas[hout] <= ( int ) hole_areas[hin] ) {
                return false;
            } else {
                return  is_inside ( holes[hout], holes[hin] );
            }
        },
        boundary_holes );

        // Now build the adj and xadj
        ans._polygon_to_region.resize ( ans.n_polygons(), infinite_region );
        ans._region_to_polygon_xval.resize ( 1, 0 );
        ans._region_to_polygon_val.resize ( 0 );

        for ( int i = 0 ; i < ans.n_regions() ; ++i ) {
            const int region_id = i;
            ans._region_to_polygon_xval.push_back ( ans._region_to_polygon_xval.back() );

            // Don't forget yourself :)
            ans._polygon_to_region[region_id] = region_id;
            ++ans._region_to_polygon_xval.back();
            ans._region_to_polygon_val.push_back ( region_id );

            for ( int  j = 0 ; j < ( int ) boundary_holes[region_id].size() ; ++j ) {
                const int polygon_id = ans.n_regions() + boundary_holes[region_id][j];
                ans._polygon_to_region[polygon_id] = region_id;
                ++ans._region_to_polygon_xval.back();
                ans._region_to_polygon_val.push_back ( polygon_id );
            }
        }
    } // ALL DONE WITH REGION HOLE ASSOCIATION

    return ans;
}

NAMESPACE_END ( mc )
NAMESPACE_END ( polyfit )

max_stars_repo_name: ShnitzelKiller/polyfit

max_issues_repo_name: ShnitzelKiller/polyfit

max_forks_repo_name: ShnitzelKiller/polyfit


text:
#ifndef SRC_UTIL_MODULE_CACHE_DIR_HPP_
#define SRC_UTIL_MODULE_CACHE_DIR_HPP_

#include <cpp-pcp-client/util/thread.hpp>
#include <leatherman/curl/client.hpp>
#include <leatherman/json_container/json_container.hpp>
#include <boost/filesystem/path.hpp>

namespace PXPAgent {

  class ModuleCacheDir {
    public:
      ModuleCacheDir() = delete;
      ModuleCacheDir(const ModuleCacheDir&) = delete;
      ModuleCacheDir& operator=(const ModuleCacheDir&) = delete;
      ModuleCacheDir(const std::string& cache_dir,
                     const std::string& cache_dir_purge_ttl);

      boost::filesystem::path createCacheDir(const std::string& sha256);
      boost::filesystem::path getCachedFile(const std::vector<std::string>& master_uris,
                                      uint32_t connect_timeout,
                                      uint32_t timeout,
                                      leatherman::curl::client& client,
                                      const boost::filesystem::path& cache_dir,
                                      leatherman::json_container::JsonContainer& file);

      boost::filesystem::path downloadFileFromMaster(const std::vector<std::string>& master_uris,
                                                    uint32_t connect_timeout,
                                                    uint32_t timeout,
                                                    leatherman::curl::client& client,
                                                    const boost::filesystem::path& cache_dir,
                                                    const boost::filesystem::path& destination,
                                                    const leatherman::json_container::JsonContainer& file);

      unsigned int purgeCache(const std::string& ttl,
                              std::vector<std::string> ongoing_transactions,
                              std::function<void(const std::string& dir_path)> purge_callback);

      std::string cache_dir_;
      std::string purge_ttl_;

    private:
      std::tuple<bool, std::string> downloadFileWithCurl(const std::vector<std::string>& master_uris,
                                                        uint32_t connect_timeout_s,
                                                        uint32_t timeout_s,
                                                        leatherman::curl::client& client,
                                                        const boost::filesystem::path& file_path,
                                                        const leatherman::json_container::JsonContainer& uri);

      std::string createUrlEndpoint(const leatherman::json_container::JsonContainer& uri);
      std::string calculateSha256(const std::string& path);
      PCPClient::Util::mutex cache_purge_mutex_;
      PCPClient::Util::mutex curl_mutex_;
  };
}
#endif

ext: hpp

max_stars_repo_name: barriserloth/pxp-agent

max_issues_repo_name: barriserloth/pxp-agent

max_forks_repo_name: barriserloth/pxp-agent


text:
#include <cstdlib>
#include <ctime>

#include "core/common/clint.h"
#include "elf_loader.h"
#include "debug_memory.h"
#include "iss.h"
#include "mem.h"
#include "memory.h"
#include "syscall.h"
#include "platform/common/options.h"
#include "fe310_plic.h"

#include "gdb-mc/gdb_server.h"
#include "gdb-mc/gdb_runner.h"

//////////////////////////////////////////////////////////////////////
//-------------------------RRAM header file---------------------------
#include "platform/common/rram.h"
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
//-------------------------NVDLA header file---------------------------
#include "nvdla_vp.h"
//////////////////////////////////////////////////////////////////////
#include "tester.h"

#include <boost/io/ios_state.hpp>
#include <boost/program_options.hpp>
#include <iomanip>
#include <iostream>

using namespace rv32;
namespace po = boost::program_options;

struct TinyOptions : public Options {
public:
	typedef unsigned int addr_t;

	addr_t mem_size = 1024 * 1024 * 32;  // 32 MB ram, to place it before the CLINT and run the base examples (assume
	                                     // memory start at zero) without modifications
	addr_t mem_start_addr = 0x00000000;
	addr_t mem_end_addr = mem_start_addr + mem_size - 1;
	addr_t clint_start_addr = 0x02000000;
	addr_t clint_end_addr = 0x0200ffff;
	addr_t sys_start_addr = 0x02010000;
	addr_t sys_end_addr = 0x020103ff;

//////////////////////////////////////////////////////////////////////
//-------------------RRAM address in the memory map--------------------
	addr_t rram_start_addr = 0x03000000;
	addr_t rram_end_addr = 0x30001FF; // = rram_start_addr + rram_size -1
	// rram_size = 256x16 bits = 512 Bytes = 0x00000200
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
//-------------------nvdla address in the memory map--------------------
	addr_t	nvdla_small = 256 * 1024; // 256KB see nvdla MMIO layout
	addr_t	nvdla_large = 256 * 1024;

	addr_t nvdla_start_addr = 0x04000000;
	addr_t nvdla_small_end_addr = nvdla_start_addr + nvdla_small-1;   //400DFFF
	//addr_t nvdla_large_end_addr = nvdla_start_addr + nvdla_large-1;   //4010FFF
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
//-------------------SRAM address in the memory map--------------------
	    addr_t smem_size = 1024  * 32;  // 32 KB sram,
	    addr_t smem_start_addr = 0x80000000;
		addr_t smem_end_addr = mem_start_addr + mem_size - 1;
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
//-------------------PLIC address in the memory map--------------------
	addr_t plic_start_addr = 0x50000000;
	addr_t plic_end_addr = 0x51000000;
//////////////////////////////////////////////////////////////////////
	bool quiet = false;
	bool use_E_base_isa = false;

	TinyOptions(void) {
		// clang-format off
		add_options()
			("quiet", po::bool_switch(&quiet), "do not output register values on exit")
			("memory-start", po::value<unsigned int>(&mem_start_addr), "set memory start address")
			("memory-size", po::value<unsigned int>(&mem_size), "set memory size")
			("use-E-base-isa", po::bool_switch(&use_E_base_isa), "use the E instead of the I integer base ISA");
        	// clang-format on
        }

	void parse(int argc, char **argv) override {
		Options::parse(argc, argv);
		mem_end_addr = mem_start_addr + mem_size - 1;
	}
};

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

	TinyOptions opt;
	opt.parse(argc, argv);

	std::srand(std::time(nullptr));  // use current time as seed for random generator

	tlm::tlm_global_quantum::instance().set(sc_core::sc_time(opt.tlm_global_quantum, sc_core::SC_NS));

	//ISS core(0, opt.use_E_base_isa);
   // MMU mmu(core);
	//CombinedMemoryInterface core_mem_if("MemoryInterface0", core, &mmu);

	SimpleMemory mem("SimpleMemory", opt.mem_size);
	//adding an sram
	SimpleMemory smem("sram", opt.smem_size);

	ELFLoader loader(opt.input_program.c_str());
	SimpleBus<4, 7> bus("SimpleBus");
	SyscallHandler sys("SyscallHandler");
	CLINT<1> clint("CLINT");

	DebugMemoryInterface dbg_if("DebugMemoryInterface");

//////////////////////////////////////////////////////////////////////
//-------------------------PLIC instantiation-------------------------

	FE310_PLIC<1, 64, 96, 32> plic("PLIC");
	bus.ports[5] = new PortMapping(opt.plic_start_addr, opt.plic_end_addr);
	bus.isocks[5].bind(plic.tsock);

//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
//-------------------------RRAM instantiation-------------------------
	rram rram_mem("rram");
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
//-------------------------NVDLA instantiation-------------------------
	NVDLA_top_level nvdla_vp ("nvdla_vp",10);
	//SC_REPORT_INFO_VERB("nvdla_vp", "mc_socket created", SC_DEBUG);

//////////////////////////////////////////////////////////////////////

	MemoryDMI dmi = MemoryDMI::create_start_size_mapping(mem.data, opt.mem_start_addr, mem.size);
	//InstrMemoryProxy instr_mem(dmi, core);

	std::shared_ptr<BusLock> bus_lock = std::make_shared<BusLock>();
	//core_mem_if.bus_lock = bus_lock;

	//instr_memory_if *instr_mem_if = &core_mem_if;
	//data_memory_if *data_mem_if = &core_mem_if;
	//if (opt.use_instr_dmi)
	//	instr_mem_if = &instr_mem;
	//if (opt.use_data_dmi) {
	//	core_mem_if.dmi_ranges.emplace_back(dmi);
	//}

	loader.load_executable_image(mem.data, mem.size, opt.mem_start_addr);
	//core.init(instr_mem_if, data_mem_if, &clint, loader.get_entrypoint(), rv32_align_address(opt.mem_end_addr));
	sys.init(mem.data, opt.mem_start_addr, loader.get_heap_addr());
	//sys.register_core(&core);

	//if (opt.intercept_syscalls)
	//	core.sys = &sys;

	// setup port mapping
	bus.ports[0] = new PortMapping(opt.mem_start_addr, opt.mem_end_addr);
	bus.ports[1] = new PortMapping(opt.clint_start_addr, opt.clint_end_addr);
	bus.ports[2] = new PortMapping(opt.sys_start_addr, opt.sys_end_addr);

//////////////////////////////////////////////////////////////////////
	//-------------------------RRAM port mapping--------------------------
	bus.ports[3] = new PortMapping(opt.rram_start_addr, opt.rram_end_addr);
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
	//-------------------------NVDLA port mapping--------------------------
	bus.ports[4] = new PortMapping(opt.nvdla_start_addr, opt.nvdla_small_end_addr);
	//bus.ports[4] = new PortMapping(opt.nvdla_start_addr, opt.nvdla_large_end_addr);
//////////////////////////////////////////////////////////////////////
	//bus.ports[5] = new PortMapping(opt.smem_start_addr, opt.smem_end_addr);
	// connect TLM sockets
	//core_mem_if.isock.bind(bus.tsocks[0]); // this will be substituted by the testermaster

	Testmaster master("master_tester",0x04000000+0x5000,0x1001,4);

	master.isock.bind(bus.tsocks[0]);
	dbg_if.isock.bind(bus.tsocks[1]);
	bus.isocks[0].bind(mem.tsock);
	bus.isocks[6].bind(smem.tsock);
	bus.isocks[1].bind(clint.tsock);
	bus.isocks[2].bind(sys.tsock);

//////////////////////////////////////////////////////////////////////
	//--------------------connecting RRAM TLM sockets---------------------
	bus.isocks[3].bind(rram_mem.tsock);

//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
	//--------------------connecting NVDLA TLM sockets---------------------

	    /*
		 * Use this adapter to attach peripherals with write access (e.g. DMA) to the bus.
		 * This ensures that those peripherals do not violate the RISC-V LR/SC atomic semantic.
		 */
	//nvdla_vp.axi_adaptor->standard_axi.bind(nvdla_connector.tsock);
	//nvdla_vp.isock.bind(nvdla_connector.tsock);
	PeripheralWriteConnector nvdla_connector("nvdla_connector");  // to respect ISS bus locking
	nvdla_vp.m_nvdla->nvdla_core2dbb_axi4.bind(nvdla_connector.tsock);
	nvdla_connector.isock.bind(bus.tsocks[2]);
	nvdla_connector.bus_lock = bus_lock;

	//nvdla_vp.m_nvdla->nvdla_core2cvsram_axi4.bind(bus.tsocks[3]);
	PeripheralWriteConnector nvdla_connector_2("nvdla_connector_2");  // to respect ISS bus locking
	nvdla_vp.m_nvdla->nvdla_core2cvsram_axi4.bind(nvdla_connector_2.tsock);
	nvdla_connector_2.isock.bind(bus.tsocks[3]);
	nvdla_connector_2.bus_lock = bus_lock;

	//bus.isocks[4].bind(nvdla_vp.tsock);
	bus.isocks[4].bind(nvdla_vp.m_nvdla->nvdla_host_master_if);

/*	sc_object * obj = dynamic_cast<sc_object*>(socket.get_interface());
	std::cout << obj->name() << std::endl;*/

//////////////////////////////////////////////////////////////////////

	// connect interrupt signals/communication
	//clint.target_harts[0] = &core;
	nvdla_vp.plic = &plic;
	nvdla_vp.m_nvdla->nvdla_intr(nvdla_vp.m_coreInter); //binding the sc_out interrupt signal from nvdla to sc_buffer in top level

	// switch for printing instructions
	//core.trace = opt.trace_mode;

	//std::vector<debug_target_if *> threads;
	//threads.push_back(&core);

	//if (opt.use_debug_runner) {
	//	auto server = new GDBServer("GDBServer", threads, &dbg_if, opt.debug_port);
	//	new GDBServerRunner("GDBRunner", server, &core);
	//} else {
	//	new DirectCoreRunner(core);
	//}

	if (opt.quiet)
		 sc_core::sc_report_handler::set_verbosity_level(sc_core::SC_NONE);

//////////////////////////////////////////////////////////////////////
//----------------Loading values into NVDLA csb master------------------
	#define MEMORY_BASE  0x04000000
	#define CDMA_BASE 0x5000
	#define BDMA_BASE 0x4000
	#define MCIF_BASE 0x2000
    #define CDP_BASE 0xf000

#ifndef DEBUG_LOG
#define DEBUG_LOG 0
#endif
#if DEBUG_LOG
	/*
	 * Setting he verbosity level to debug (highest level)
	 * enables to output inner logs/messages of nvdla core
	 */
sc_report_handler::set_verbosity_level(SC_DEBUG);
cout << "verbosity=" <<dec<< sc_report_handler::get_verbosity_level() << endl;

	/*
	 * output the log messages into log.txt
	 */
sc_report_handler::set_log_file_name("log.txt");
#endif

	sc_core::sc_start();
	/*
	/* master.address=MEMORY_BASE+CDMA_BASE+0x18;
	 * Fatal: cmac_a_reg_model.cpp: 24:Failed to decode NVDLA_CMAC_A register write with w_mask at offset 0x18, data 0x00000011
	 */
	//master.address=MEMORY_BASE+CDMA_BASE+0x14;
	//master.val=0x00011;
	//master.run();

	master.address=MEMORY_BASE+MCIF_BASE+0x0014;
	master.val=0x0001;
	master.run();

	/*
	 * csb master subunit addresses
	 * from http://nvdla.org/hw/v1/hwarch.html
	 *
	 *
#define GLB_BASE 0x0
#define GEC_BASE 0x1000
#define MCIF_BASE 0x2000
#define CVIF_BASE 0x3000
#define BDMA_BASE 0x4000
#define CDMA_BASE 0x5000
#define CSC_BASE 0x6000
#define CMAC_A_BASE 0x7000
#define CMAC_B_BASE 0x8000
#define CACC_BASE 0x9000
#define SDP_RDMA_BASE 0xa000
#define SDP_BASE 0xb000
#define PDP_RDMA_BASE 0xc000
#define PDP_BASE 0xd000
#define CDP_RDMA_BASE 0xe000
#define CDP_BASE 0xf000
#define RBK_BASE 0x10000

	if (!opt.quiet) {
		core.show();
	}
*/
	return 0;
}

max_stars_repo_name: amedoc/riscv-vp

max_issues_repo_name: amedoc/riscv-vp

max_forks_repo_name: amedoc/riscv-vp


text:
//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
#include "pxr/usd/usdSkel/bakeSkinning.h"

#include "pxr/usd/usd/primRange.h"
#include "pxr/usd/usdSkel/cache.h"
#include "pxr/usd/usdSkel/root.h"

#include <boost/python.hpp>

using namespace boost::python;

PXR_NAMESPACE_USING_DIRECTIVE

void wrapUsdSkelBakeSkinning()
{
    def("BakeSkinning", ((bool (*)(const UsdSkelRoot&,
                                   const GfInterval&))&UsdSkelBakeSkinning),
        (arg("root"), arg("interval")=GfInterval::GetFullInterval()));

    def("BakeSkinning", ((bool (*)(const UsdPrimRange&,
                                   const GfInterval&))&UsdSkelBakeSkinning),
        (arg("range"), arg("interval")=GfInterval::GetFullInterval()));

}

max_stars_repo_name: DougRogers-DigitalFish/USD

max_issues_repo_name: DougRogers-DigitalFish/USD

max_forks_repo_name: DougRogers-DigitalFish/USD


text:
#include "temperature.h"
#include <bit>
#include <boost/accumulators/statistics/stats.hpp>
#include <gsl/gsl-lite.hpp>

namespace angonoka::stun {
Temperature::Temperature(
    Beta beta,
    BetaScale beta_scale,
    StunWindow stun_window,
    RestartPeriod restart_period)
    : value{beta}
    , acc{tag::rolling_window::window_size
          = static_cast<std::int_fast32_t>(stun_window)}
    , beta_scale{beta_scale}
    , restart_period_mask{
          static_cast<std::size_t>(restart_period) - 1}
{
    Expects(beta > 0.F);
    Expects(beta_scale > 0.F);
    Expects(static_cast<std::int_fast32_t>(stun_window) > 0);
    Expects(static_cast<std::size_t>(restart_period) > 0);
    Expects(
        std::popcount(static_cast<std::size_t>(restart_period)) == 1);
}

[[nodiscard]] float Temperature::average_stun() const noexcept
{
    using boost::accumulators::rolling_mean;
    return rolling_mean(acc);
}

void Temperature::update(float stun) noexcept
{
    using boost::accumulators::count;
    Expects(stun >= 0.F);

    acc(stun);
    const auto dampening
        = static_cast<float>(count(acc) & restart_period_mask)
        / static_cast<float>(restart_period_mask + 1);
    const auto diff = average_stun() - 0.03F;
    const auto t = 1.F - dampening;
    value *= 1.F + diff * beta_scale * t * t;

    Ensures(value >= 0.F);
}

Temperature& Temperature::operator=(Temperature&& other) noexcept
{
    value = other.value;
    try {
        acc = other.acc;
    } catch (...) {
        // 99% certain that the missing noexcept on
        // accumulator's copy ctor is a bug in
        // a boost lib.
#ifdef __llvm__
        __builtin_unreachable();
#endif // __llvm__
    }
    beta_scale = other.beta_scale;
    restart_period_mask = other.restart_period_mask;
    return *this;
}

Temperature::Temperature(const Temperature& other) = default;
Temperature::Temperature(Temperature&& other) noexcept = default;
Temperature& Temperature::operator=(const Temperature& other)
    = default;
Temperature::~Temperature() noexcept = default;
} // namespace angonoka::stun

max_stars_repo_name: coffee-lord/angonoka

max_issues_repo_name: coffee-lord/angonoka

max_forks_repo_name: coffee-lord/angonoka


text:
// Refer to the paper: "Robust Point Cloud Based Reconstruction of Large-Scale Outdoor Scenes" in CVPR 2019
// Link: https://arxiv.org/abs/1905.09634

// This file is an implememntation of the Gaussian-Uniform mixture model as described in Sec. 5 of the paper.
// The Gaussian-Uniform mixture model degenerates to the approach in Choi et al. "Robust Reconstruction of Indoor Scenes" in CVPR 2017.
// This implementation adopts some techniques used by Choi et al.

#include <ceres/ceres.h>
#include <iostream>
#include <sophus/se3.hpp>

#include <vector>
#include <fstream>
#include <Eigen/Core>

#include <iomanip>
#include <algorithm>

#include "local_parameterization_se3.hpp"

#define NUM_ITERATION_EM 1000

#define EPSILON 6                             // upper bound of feature matching errors, used in Eq. (27)
#define M_HAT EPSILON * EPSILON               // mean error, used in Eq. (27)
#define P_HAT 0.9                             // trust level, used in Eq. (27)
#define THETA P_HAT / ( 1.0 - P_HAT) * M_HAT  // THETA is solved using Eq. (27)

#define THESHOLD_TO_BE_AN_INLIER_LOOP 0.8 * P_HAT    // theshold to prune outlier loop closures, used to generate results in Tab. 1 and 2

typedef Eigen::Matrix< double, 6, 6, Eigen::RowMajor > InformationMatrix;
typedef Eigen::Matrix< double, 6, 1> Vector6d;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

struct FramedTransformation {
  int id1_;
  int id2_;
  int frame_;
  Eigen::Matrix4d transformation_;      // pose in matrix form
  Sophus::SE3d transformation_se3_;     // pose in se3 form

  FramedTransformation( int id1, int id2, int f, Eigen::Matrix4d t )
    : id1_( id1 ), id2_( id2 ), frame_( f ), transformation_( t )
  {
    Eigen::Quaterniond q;
    q = t.block<3,3>(0,0);
    transformation_se3_ = Sophus::SE3d(q, Sophus::SE3d::Point(t(0,3), t(1,3), t(2,3)));
  }

  FramedTransformation(int id1, int id2, int f, Sophus::SE3d t)
    : id1_( id1 ), id2_( id2 ), frame_( f ), transformation_se3_( t )
    {
      transformation_ = t.matrix();
    }
};

struct PCLTrajectory {
  std::vector< FramedTransformation > data_;
  int index_;

  void LoadFromFile(const char* filename ) {
    data_.clear();
    index_ = 0;
    int id1, id2, frame;
    Eigen::Matrix4d trans;
    FILE * f = fopen( filename, "r" );
    if ( f != NULL ) {
      char buffer[1024];
      while ( fgets( buffer, 1024, f ) != NULL ) {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
          sscanf( buffer, "%d %d %d", &id1, &id2, &frame);
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(0,0), &trans(0,1), &trans(0,2), &trans(0,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(1,0), &trans(1,1), &trans(1,2), &trans(1,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(2,0), &trans(2,1), &trans(2,2), &trans(2,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(3,0), &trans(3,1), &trans(3,2), &trans(3,3) );
          data_.push_back( FramedTransformation( id1, id2, frame, trans ) );
        }
      }
      fclose( f );
    }
  }

  void SaveToFile(const char* filename ) {
    FILE * f = fopen( filename, "w" );
    for ( int i = 0; i < ( int )data_.size(); i++ ) {
      Sophus::SE3d trans_se3 = data_[ i ].transformation_se3_;
      Eigen::Matrix4d trans = trans_se3.matrix();
      fprintf( f, "%d\t%d\t%d\n", data_[ i ].id1_, data_[ i ].id2_, data_[ i ].frame_ );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );

    }
    fclose( f );
  }

};

struct FramedInformation {
  int id1_;
  int id2_;
  int frame_;
  InformationMatrix information_;

  FramedInformation( int id1, int id2, int f, InformationMatrix t )
    : id1_( id1 ), id2_( id2 ), frame_( f ), information_( t )
  {}
};

struct PCLInformation {
  std::vector< FramedInformation > data_;

  void LoadFromFile(const char*  filename ) {
    data_.clear();
    int id1, id2, frame;
    InformationMatrix info;
    FILE * f = fopen( filename, "r" );
    if ( f != NULL ) {
      char buffer[1024];
      while ( fgets( buffer, 1024, f ) != NULL ) {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
          sscanf( buffer, "%d %d %d", &id1, &id2, &frame);
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(0,0), &info(0,1), &info(0,2), &info(0,3), &info(0,4), &info(0,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(1,0), &info(1,1), &info(1,2), &info(1,3), &info(1,4), &info(1,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(2,0), &info(2,1), &info(2,2), &info(2,3), &info(2,4), &info(2,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(3,0), &info(3,1), &info(3,2), &info(3,3), &info(3,4), &info(3,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(4,0), &info(4,1), &info(4,2), &info(4,3), &info(4,4), &info(4,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(5,0), &info(5,1), &info(5,2), &info(5,3), &info(5,4), &info(5,5) );
          data_.push_back( FramedInformation( id1, id2, frame, info ) );
        }
      }
      fclose( f );
    }
  }
};

struct GaussianFunctor {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  GaussianFunctor(Sophus::SE3d trans_next_to_current, InformationMatrix info, int id1, int id2)
  : trans_next_to_current_(trans_next_to_current), info_(info), id1_(id1), id2_(id2) {}

  template <typename T>
  bool operator()(T const* const sT_current, T const* const sT_next,
                  T* residual) const {

    Eigen::Map<Sophus::SE3<T> const> const T_current(sT_current);
    Eigen::Map<Sophus::SE3<T> const> const T_next(sT_next);

    Sophus::SE3<T> xi_7T = trans_next_to_current_
                          * T_next.inverse()
                          * T_current;

    // xi: local parameterization as in Choi et al.
    Eigen::Matrix<T,6,1> xi;
    xi << xi_7T.data()[4], xi_7T.data()[5], xi_7T.data()[6], xi_7T.data()[0], xi_7T.data()[1], xi_7T.data()[2];

    Eigen::Matrix<T,1,1> sq_error = xi.transpose() * info_ * xi;
    residual[0] = T( sqrt(sq_error(0,0)) );

    return true;
  }

private:
  const Sophus::SE3d trans_next_to_current_;
  const InformationMatrix info_;
  const int id1_, id2_;
};

class RobustPCLReconstruction_GaussianUniform {
public:

  // Odometry contraints
  PCLTrajectory odometry_log_;
  PCLInformation odometry_info_;

  // Loop closure contraints
  PCLTrajectory loop_log_;
  PCLInformation loop_info_;

  // Fragment poses
  PCLTrajectory fragment_poses_;
  PCLTrajectory last_fragment_poses_;   // used to check for EM convergence

  // P_ij table (Eq. 25)
  std::vector< std::vector<double> > P_;
  double average_P_;
  double last_average_P_;   // used to check for EM convergence

  RobustPCLReconstruction_GaussianUniform()
  {
    average_P_ = -1.;
    last_average_P_ = -1.;
  }

  ~RobustPCLReconstruction_GaussianUniform() {}

  void LoadOdometryLog(const char* filename) {
    odometry_log_.LoadFromFile(filename);
  }

  void LoadOdometryInfo(const char* filename) {
    odometry_info_.LoadFromFile(filename);
  }

  void LoadLoopLog(const char* filename) {
    loop_log_.LoadFromFile(filename);
  }

  void LoadLoopInfo(const char* filename) {
    loop_info_.LoadFromFile(filename);
  }

  void InitPosesFromFile(const char* filename) {
    fragment_poses_.LoadFromFile(filename);

    // Initialize P_ij table
    P_.resize(NumPoses());
    for (int i = 0; i < NumPoses(); i++) {
      P_[i].resize(NumPoses());
    }
  }

  // void InitPoses() {
  //   fragment_poses_.data_.clear();
  //   fragment_poses_.index_ = 0;
  //   Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
  //   fragment_poses_.data_.push_back( FramedTransformation( 0, 0, 1, pose ) );

  //   for( std::vector< FramedTransformation >::iterator it = odometry_log_.data_.begin();
  //     it != odometry_log_.data_.end(); it ++)
  //   {
  //     pose = pose * it->transformation_;
  //     fragment_poses_.data_.push_back( FramedTransformation( it->id2_, it->id2_, it->id2_ + 1, pose ) );
  //   }
  //   std::cout << "total data size: " << odometry_log_.data_.size() << std::endl;

  //   P_.resize(NumPoses());
  //   for (int i = 0; i < NumPoses(); i++) {
  //     P_[i].resize(NumPoses());
  //   }
  // }

  void SavePoses(const char* filename) {
    fragment_poses_.SaveToFile(filename);
  }

  void SaveLinks(const char* filename) {
    FILE * f = fopen( filename, "w" );

    for ( int i = 0; i < NumOdometryConstraints(); i++ ) {
      Sophus::SE3d trans_se3 = odometry_log_.data_[ i ].transformation_se3_;
      Eigen::Matrix4d trans = trans_se3.matrix();
      fprintf( f, "%d\t%d\t%d\n", odometry_log_.data_[ i ].id1_, odometry_log_.data_[ i ].id2_, odometry_log_.data_[ i ].frame_ );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );
    }

    for (int i = 0; i < NumLoopClosureConstraints(); i++ ) {
      Sophus::SE3d trans_se3 = loop_log_.data_[ i ].transformation_se3_;
      int id1 = loop_log_.data_[ i ].id1_;
      int id2 = loop_log_.data_[ i ].id2_;
      assert(id1 < id2);
      if (id1 != id2 - 1 && P_[id1][id2] > THESHOLD_TO_BE_AN_INLIER_LOOP) {
        Eigen::Matrix4d trans = trans_se3.matrix();
        fprintf( f, "%d\t%d\t%d\n", loop_log_.data_[ i ].id1_, loop_log_.data_[ i ].id2_, loop_log_.data_[ i ].frame_ );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );
      }
    }
    fclose( f );
  }

  void Expectation() {
    std::cout << std::endl << "This is the beginning of EM iteration " << fragment_poses_.index_ + 1 << std::endl
              << ">>>> Expectation Step <<<<" << std::endl;

    double sum_P = 0;   // used to compute average_P_ later
    std::vector<int> P_ij_counter(20);
    for (int ii = 0; ii < 20; ii ++)
      P_ij_counter[ii] = 0;

    std::cout << std::setw(5) << "id1"
              << std::setw(5) << "id2"
              << std::setw(10) << "#matches"
              << std::setw(15) << "xiLxi"
              << std::setw(15) << "B_ij"
              << std::setw(15) << "B_ij^2"
              << std::setw(15) << "expterm"
              << std::setw(5) << " --> "
              << std::setw(15) << "P_ij"
              << std::endl;

    // Compute P_ij table
    for (int i = 0; i < NumLoopClosureConstraints(); i ++) {

      FramedTransformation trans = loop_log_.data_[i];
      FramedInformation info = loop_info_.data_[i];

      assert(trans.id1_ == info.id1_ && trans.id2_ == info.id2_ && trans.id1_ < trans.id2_);
      int id1 = info.id1_;
      int id2 = info.id2_;

      // Compute the matrix form of xi
      Sophus::SE3d M_xi = trans.transformation_se3_
                          * fragment_poses_.data_[id2].transformation_se3_.inverse()
                          * fragment_poses_.data_[id1].transformation_se3_;

      // Extract the vector form of xi
      Vector6d xi;
      xi << M_xi.data()[4], M_xi.data()[5], M_xi.data()[6], M_xi.data()[0], M_xi.data()[1], M_xi.data()[2];

      // Compute B_ij (Eq. 23)
      // This approximiation technique is adopted from Choi et al.
      double xiLxi = xi.transpose() * info.information_ * xi;
      double B_ij = xiLxi / info.information_(0,0);

      // Compute P_ij (Eq. 25)
      // The exponential-of-log trick is used to avoid the numerical issues when B_ij is too large.
      double expterm = exp(2 * log(B_ij) - log (THETA));
      P_[id1][id2] = 1./ (1. + expterm);

      // Accumulate sum_P
      sum_P += P_[id1][id2];

      // Visualize intermediate steps
      std::cout << std::setw(5) << id1
                << std::setw(5) << id2
                << std::setw(10) << info.information_(0,0)
                << std::setw(15) << xiLxi
                << std::setw(15) << B_ij
                << std::setw(15) << B_ij * B_ij
                << std::setw(15) << expterm
                << std::setw(5) << " --> "
                << std::setw(15) << P_[id1][id2]
                << std::endl;

      // Use P_ij_counter to visualize the distribution of P_ij
      for (int ii = 0; ii < 20; ii ++)
        if (P_[id1][id2] > 0.05 * ii && P_[id1][id2] <= 0.05 * (ii+1))
          P_ij_counter[ii] ++;

    }

    // Save
    last_average_P_ = average_P_;
    last_fragment_poses_.index_ = fragment_poses_.index_;
    last_fragment_poses_.data_.clear();
    for (int i = 0; i < NumPoses(); i++)
      last_fragment_poses_.data_.push_back( FramedTransformation( fragment_poses_.data_[i].id1_,
                                                                  fragment_poses_.data_[i].id2_,
                                                                  fragment_poses_.data_[i].frame_,
                                                                  fragment_poses_.data_[i].transformation_se3_));
    // Update
    average_P_ = sum_P / NumLoopClosureConstraints();
    fragment_poses_.index_++;

    // Visualize the distribution of P_ij
    std::cout << "P_ij distributed in 20 bins :" << std::endl;
    for (int ii = 0; ii < 20 ; ii++)
      std::cout << "between"
                << std::setw(5) << 0.05 * ii
                << std::setw(5) << "and"
                << std::setw(5) << 0.05 * (ii+1)
                << std::setw(15) << P_ij_counter[ii]
                << std::endl;
  }

  void Maximization() {
    std::cout << std::endl << ">>>> Maximization Step <<<<" << std::endl;

    // Build the problem.
    ceres::Problem problem;

    // Specify local update rule for the parameters
    for (std::vector< FramedTransformation >::iterator it = fragment_poses_.data_.begin();
         it != fragment_poses_.data_.end(); it++ ) {
      problem.AddParameterBlock(it->transformation_se3_.data(), Sophus::SE3d::num_parameters,
                                new Sophus::test::LocalParameterizationSE3);
    }

    // Create and add cost functions. Derivatives will be evaluated via
    // automatic differentiation
    for (int i = 0; i < NumOdometryConstraints(); i++) {

      FramedTransformation trans = odometry_log_.data_[i];
      FramedInformation info = odometry_info_.data_[i];

      assert(trans.id1_ == info.id1_ && trans.id2_ == info.id2_ && trans.id1_ < trans.id2_);
      int id1 = info.id1_;
      int id2 = info.id2_;

      if (info.information_(0,0) <= 1)
        continue;

      ceres::CostFunction* cost_odometry =
          new ceres::AutoDiffCostFunction<GaussianFunctor, 1,
                                          Sophus::SE3d::num_parameters,
                                          Sophus::SE3d::num_parameters>(
              new GaussianFunctor(trans.transformation_se3_, info.information_, id1, id2));
      problem.AddResidualBlock(cost_odometry, NULL,
                               fragment_poses_.data_[id1].transformation_se3_.data(),
                               fragment_poses_.data_[id2].transformation_se3_.data());
    }

    for (int i = 0; i < NumLoopClosureConstraints(); i++) {

      FramedTransformation trans = loop_log_.data_[i];
      FramedInformation info = loop_info_.data_[i];

      assert(trans.id1_ == info.id1_ && trans.id2_ == info.id2_ && trans.id1_ < trans.id2_);
      int id1 = info.id1_;
      int id2 = info.id2_;

      ceres::CostFunction* cost_loop =
          new ceres::AutoDiffCostFunction<GaussianFunctor, 1,
                                          Sophus::SE3d::num_parameters,
                                          Sophus::SE3d::num_parameters>(
              new GaussianFunctor(trans.transformation_se3_, info.information_, id1, id2));
      problem.AddResidualBlock(cost_loop, new ceres::ScaledLoss(NULL, P_[id1][id2], ceres::TAKE_OWNERSHIP),
                               fragment_poses_.data_[id1].transformation_se3_.data(),
                               fragment_poses_.data_[id2].transformation_se3_.data());
    }

    // Set solver options
    ceres::Solver::Options options;
    // options.max_num_iterations = 1000;
    options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

    // Solve and report
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;

    std::cout << "This is the end of EM iteration : " << fragment_poses_.index_ << std::endl;
  }

  int NumPoses() {
    return fragment_poses_.data_.size();
  }

  int NumOdometryConstraints() {
    return odometry_log_.data_.size();
  }

  int NumLoopClosureConstraints() {
    return loop_log_.data_.size();
  }

  bool IsConverged() {
    if (last_average_P_ < 0) {
      return false;         // not started yet
    }

    std::cout << "Checking for the convergence of average_P_ ...\t";
    if (fabs(average_P_ - last_average_P_) > 0.00001) {
      std::cout << "NOT coverged yet, as average_P_ was updated from " << last_average_P_ << " to " << average_P_ << std::endl;
      return false;
    }
    std::cout << "CONVERGED!" << std::endl;

    std::cout << "Checking for the convergence of poses ...\t";
    for (int i = 0; i < NumPoses(); i++)
    {
      Sophus::SE3d last_pose = last_fragment_poses_.data_[i].transformation_se3_;
      Sophus::SE3d current_pose = fragment_poses_.data_[i].transformation_se3_;

      double const mse = (last_pose.inverse() * current_pose).log().squaredNorm();
      bool const converged = mse < 10. * Sophus::Constants<double>::epsilon();

      if (!converged) {
        std::cout << "NOT converged yet." << std::endl;
        return false;
      }
    }
    std::cout << "CONVERGED!" << std::endl;
    return true;
  }
};

int main(int argc, char** argv) {
  if (argc != 8)
  {
    std::cout << "Please provide the following arguments: InputOdometryLog, InputOdometryInfo, InputLoopLog, InputLoopInfo, InputInitPoses, OutputFinalPoses, OutputKeptLoops" << std::endl;
    return 1;
  }
  RobustPCLReconstruction_GaussianUniform RobustPCLReconstruction_g;

  RobustPCLReconstruction_g.LoadOdometryLog(argv[1]);
  RobustPCLReconstruction_g.LoadOdometryInfo(argv[2]);
  RobustPCLReconstruction_g.LoadLoopLog(argv[3]);
  RobustPCLReconstruction_g.LoadLoopInfo(argv[4]);
  RobustPCLReconstruction_g.InitPosesFromFile(argv[5]);

  bool isConverged = false;
  for (int i = 0 ; i < NUM_ITERATION_EM; i ++) {
    RobustPCLReconstruction_g.Expectation();
    RobustPCLReconstruction_g.Maximization();
    if (RobustPCLReconstruction_g.IsConverged()) {
      std::cout << "Optimization ends since EM is converged" << std::endl << std::endl;
      isConverged = true;
      break;
    }
  }

  if (!isConverged)
    std::cout << "Optimization ends after " << NUM_ITERATION_EM << " EM iterations" << std::endl << std::endl;

  RobustPCLReconstruction_g.SavePoses(argv[6]);
  std::cout << "Final poses are saved to " << argv[6] << std::endl;

  RobustPCLReconstruction_g.SaveLinks(argv[7]);
  std::cout << "Good links are saved to " << argv[7] << std::endl;

  std::cout << "There are " << RobustPCLReconstruction_g.NumPoses() << " poses" << std::endl;
  std::cout << "There are " << RobustPCLReconstruction_g.NumOdometryConstraints() << " odometry constraints" << std::endl;
  std::cout << "There are " << RobustPCLReconstruction_g.NumLoopClosureConstraints() << " loops" << std::endl;

  std::cout << std::endl;

  return 0;
}


text:
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <iostream>
#include <sstream>

std::stringstream ss;

class person
{
public:
  person()
  {
  }

  person(int age)
    : age_(age)
  {
  }

  int age() const
  {
    return age_;
  }

private:
  friend class boost::serialization::access;

  template <typename Archive>
  friend void serialize(Archive &ar, person &p, const unsigned int version);

  int age_;
};

template <typename Archive>
void serialize(Archive &ar, person &p, const unsigned int version)
{
  ar & p.age_;
}

void save()
{
  boost::archive::text_oarchive oa(ss);
  person p(31);
  oa << p;
}

void load()
{
  boost::archive::text_iarchive ia(ss);
  person p;
  ia >> p;
  std::cout << p.age() << std::endl;
}

int main()
{
  save();
  load();
}

max_stars_repo_path: Boost/The Boost C++ Libraries/src/11.2.5/main.cpp


text:
//__BEGIN_LICENSE__
//  Copyright (c) 2009-2012, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is 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.
// __END_LICENSE__

#include <iostream>
#include <time.h>
#include <sys/stat.h>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <boost/math/distributions/gamma.hpp>

#include <vw/Core.h>
#include <vw/Image.h>
#include <vw/FileIO.h>
#include <vw/Cartography.h>
#include <vw/Math.h>
using namespace vw;
using namespace vw::cartography;

#include <photk/Reconstruct.h>
#include <photk/Camres.h>
#include <photk/Misc.h>
using namespace photometry;

max_stars_repo_name: NeoGeographyToolkit/PhotometryTK

max_issues_repo_name: NeoGeographyToolkit/PhotometryTK

max_forks_repo_name: NeoGeographyToolkit/PhotometryTK


text:
//
// $Id: SpectrumPeakExtractor.hpp 10348 2017-01-11 18:26:26Z chambm $
//
//
// Original author: Jarrett Egertson <jegertso .@. uw.edu>
//
// 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.
//

#ifndef _SPECTRUMPEAKEXTRACTOR_HPP
#define _SPECTRUMPEAKEXTRACTOR_HPP

#include <vector>
#include "pwiz/data/msdata/MSData.hpp"
#include <boost/smart_ptr/shared_array.hpp>
#include "DemuxSolver.hpp"
#include "DemuxHelpers.hpp"

namespace pwiz{
namespace analysis{

    /// Extracts sets of centroided peaks from spectra using a user-defined list of peaks to extract
    class SpectrumPeakExtractor
    {
    public:

        /// Generates a SpectrumPeakExtractor
        /// \param peakMzList The m/z values around which will be searched to centroid peaks.
        ///                   The size of this list defines how many peak centroids will be output.
        /// \param ppmError The tolerance that defines how far around a nominal peak to search relative to its m/z.
        /// \pre The peakMzList must be sorted from smallest to largest mz values with no duplicates.
        SpectrumPeakExtractor(const std::vector<double>& peakMzList, const pwiz::chemistry::MZTolerance& massError);

        /// Extracts centroided peaks from an input spectrum.
        /// The peaks extracted are chosen from the peakMzList provided during initialization of the SpectrumPeakExtractor.
        /// Peaks are extracted to a user-defined row of a user-provided matrix.
        /// \param[in] spectrum Spectrum from which peaks are searched and binned.
        /// \param[out] matrix The matrix into which peaks will be extracted.
        /// \param[in] rowNum The index of the matrix row to extract peaks to.
        /// \param[in] weight The relative weight to apply to this row (multiplies the output row by the given scalar).
        /// \pre The size of the output matrix row must be the same as the size of the peakMzList given during instantiation.
        void operator()(msdata::Spectrum_const_ptr spectrum, MatrixType& matrix, size_t rowNum, double weight=1.0) const;

        /// Returns the number of peaks extracted
        size_t numPeaks() const;

    private:

        boost::shared_array< std::pair<double, double> > _ranges; ///< defines the set of m/z windows to search, one for each peak in the search list.

        size_t _numPeakBins; ///< the number of peaks given in the search list peakMzList. This is also the number of centroids that will be output.

        double _maxDelta; ///< the m/z half-window size to bin around a peak based on the set ppm error.

        double _minValue; ///< the minimum m/z that will be searched for peaks across the full m/z range.

        double _maxValue; ///< the maximum m/z that will be searched for peaks across the full m/z range.
    };

} // namespace analysis
} // namespace pwiz

#endif // _SPECTRUMPEAKEXTRACTOR_HPP

max_stars_repo_name: edyp-lab/pwiz-mzdb

max_issues_repo_name: shze/pwizard-deb

max_forks_repo_name: shze/pwizard-deb