text:
/*
Copyright Barrett Adair 2016-2017

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

*/

#include <boost/callable_traits/class_of.hpp>
#include "test.hpp"

struct foo;

template<typename T>
struct is_substitution_failure_class_of {

    template<typename>
    static auto test(...) -> std::true_type;

    template<typename A,
        typename std::remove_reference<
            TRAIT(class_of, A)>::type* = nullptr>
    static auto test(int) -> std::false_type;

    static constexpr bool value = decltype(test<T>(0))::value;
};

int main() {

    auto lambda = [](){};

    CT_ASSERT(is_substitution_failure_class_of<decltype(lambda)>::value);
    CT_ASSERT(is_substitution_failure_class_of<decltype(lambda)&>::value);
    CT_ASSERT(is_substitution_failure_class_of<void>::value);
    CT_ASSERT(is_substitution_failure_class_of<void*>::value);
    CT_ASSERT(is_substitution_failure_class_of<int>::value);
    CT_ASSERT(is_substitution_failure_class_of<int &>::value);
    CT_ASSERT(is_substitution_failure_class_of<int()>::value);
    CT_ASSERT(is_substitution_failure_class_of<int(*)()>::value);
    CT_ASSERT(is_substitution_failure_class_of<int(**)()>::value);
    CT_ASSERT(is_substitution_failure_class_of<int(&)()>::value);
    CT_ASSERT(is_substitution_failure_class_of<int (foo::** const)()>::value);
}


text:
/*
Copyright (c) 2015, Wator Vapor
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of wator nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "wator.hpp"
using namespace Wator;

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
using namespace boost::property_tree;

#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
namespace fs = boost::filesystem;

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#include <algorithm>
using namespace std;

/**
 * Constructor
 **/
ImageLayer::ImageLayer()
:extension_({".jpg",".JPG",".png",".PNG",".tif",".TIF",".tiff",".TIFF"})
{
    INFO_VAR(this);
}

/**
 * load
 * @return None.
 **/
void ImageLayer::load(bool train)
{
    INFO_VAR(param_.root_);
    const fs::path path(param_.root_);
    BOOST_FOREACH(const fs::path& p, std::make_pair(fs::recursive_directory_iterator(path),fs::recursive_directory_iterator())){
        if (!fs::is_directory(p)){
            auto extension = p.extension().string();
        	INFO_VAR(extension);
            auto itExt = std::find(extension_.begin(),extension_.end(),extension);
            if(itExt != extension_.end()){
                INFO_VAR(p);
                mat_ = cv::imread(p.string());
                pump();
                for(auto &top:top_)
                {
                    if (train) {
                        top->round();
                    } else {
                        top->forward();
                    }
                }
            }
        }
    }
    INFO_VAR("finnish load data");
}

/*
 x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12
 y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11,y12
 z1,z2,z3,z4,z5,z6,z7,z8,z9,z10,z11,z12
 u1,u2,u3,u4,u5,u6,u7,u8,u9,u10,u11,u12
 v1,v2,v3,v4,v5,v6,v7,v8,v9,v10,v11,v12
 w1,w2,w3,w4,w5,w6,w7,w8,w9,w10,w11,w12

 ------------------
 x1,x2,x3,y1,y2,y3,z1,z2,z3,
 x4,x5,x6,y4,y5,y6,z4,z5,z6,
 x7,x8,x9,y7,y8,y9,z7,z8,z9,
 x10,x11,x12,y10,y11,y12,z10,z11,z12,
 u1,u2,u3,v1,v2,v3,w1,w2,w3,
 u4,u5,u6,v4,v5,v6,w4,w5,w6,
 u7,u8,u9,v7,v8,v9,w7,w8,w9,
 u10,u11,u12,v10,v11,v12,w10,w11,w12
 */
#if 0
void ImageLayer::pump(void)
{
    blobs_.clear();
    std::vector<cv::Mat> planes;
    cv::split(mat_, planes);
    for(int i = 0;i < top_.size();i++){
        int outH = 0;
        int outW = 0;
        auto clm = dynamic_pointer_cast<CoulombLayer>(top_[i]);
        if(clm) {
            outH = clm->h_;
            outW = clm->w_;
        }
        auto newton = dynamic_pointer_cast<NewtonLayer>(top_[i]);
        if(newton) {
            outH = newton->h_;
            outW = newton->w_;
        }
        TRACE_VAR(outW);
        TRACE_VAR(outH);
        const int groundW = (mat_.cols/outW)*outW;
        const int groundH = (mat_.rows/outH)*outH;
        auto blob = shared_ptr<Blob<uint8_t>>(new Blob<uint8_t>(groundW,groundH,mat_.channels()));
        for(int channel = 0 ; channel < mat_.channels();channel++){
            auto &mat = planes[channel];
            for(int y = 0;y < mat.rows;y++){
                for(int x = 0;x < mat.cols;x++){
                    auto byte = mat.at<uint8_t>(y, x);
                    TRACE_VAR(x);
                    TRACE_VAR(y);
                    const int grid = ((y/outH) * (mat_.cols/outW))+ (x/outW) ;
                    TRACE_VAR(grid);
                    TRACE_VAR(channel);
                    if(groundW < x || groundH < y) {
                        // out side of last grid.
                        continue;
                    }
                    TRACE_VAR(groundW);
                    TRACE_VAR(groundH);
                    int index = channel * groundW * groundH;
                    index += grid * outW * outH;
                    index += (y%outH)*outW  + x%outW ;
                    TRACE_VAR(index);
                    if(index > blob->size_) {
                        INFO_VAR(index);
                        INFO_VAR(blob->size_);
                        continue;
                    }
                    blob->data_[index] = byte;
                }
            }
        }
        blobs_.push_back(blob);
    }
    this->dump(planes);
    INFO_VAR(mat_.cols*mat_.rows*mat_.channels());
}
#endif

void ImageLayer::pump(void)
{
    blobs_.clear();
    std::vector<cv::Mat> planes;
    cv::split(mat_, planes);
    for(int i = 0;i < top_.size();i++){
        auto blob = shared_ptr<Blob<uint8_t>>(new Blob<uint8_t>(mat_.cols,mat_.rows,mat_.channels()));
        for(int channel = 0 ; channel < mat_.channels();channel++){
            auto &mat = planes[channel];
            for(int y = 0;y < mat.rows;y++){
                for(int x = 0;x < mat.cols;x++){
                    auto byte = mat.at<uint8_t>(y, x);
                    TRACE_VAR(x);
                    TRACE_VAR(y);
                    int index = channel * mat.cols * mat.rows;
                    index += y * mat.cols + x;
                    blob->data_[index] = byte;
                }
            }
        }
        blobs_.push_back(blob);
    }
    this->dump(planes);
    INFO_VAR(mat_.cols*mat_.rows*mat_.channels());
}

/**
 * dump to png
 * @return None.
 **/
void ImageLayer::dump(const std::vector<cv::Mat> &planes){
    static int counter = 0;
    for(int ch = 0 ; ch < mat_.channels();ch++){
        string path = "dump.image.";
        path += typeid(this).name();
        path += ".";
        path += std::to_string(counter);

        cv::Mat zero(planes[ch].rows,planes[ch].cols,planes[ch].depth(),cv::Scalar::all(0));
        std::vector<cv::Mat> array_to_merge;
        cv::Mat color;
        if(0 == ch){
            path += ".B.";
            array_to_merge.push_back(planes[ch]);
            array_to_merge.push_back(zero);
            array_to_merge.push_back(zero);
        }
        if(1 == ch){
            path += ".G.";
            array_to_merge.push_back(zero);
            array_to_merge.push_back(planes[ch]);
            array_to_merge.push_back(zero);
        }
        if(2 == ch){
            path += ".R.";
            array_to_merge.push_back(zero);
            array_to_merge.push_back(zero);
            array_to_merge.push_back(planes[ch]);
        }
        cv::merge(array_to_merge, color);
        path += ".png";
        cv::imwrite(path ,color);
    }
    ++counter;
}

ext: cpp


text:
#ifndef OPTIMIZER_CALLBACK_HPP
#define OPTIMIZER_CALLBACK_HPP

#include <boost/make_shared.hpp>

namespace aslam {
namespace backend {
namespace callback {

/**
 * \brief The callback event specifies the callback injection point in the optimizer.
 */

struct Event {
  Event(
      double currentCost_ = std::numeric_limits<double>::signaling_NaN(),
      double previousLowestCost_ = std::numeric_limits<double>::signaling_NaN()
      )
      : currentCost(currentCost_), previousLowestCost(previousLowestCost_)
  {
  }
  virtual ~Event() = default;

  /**
   * \brief the most recently evaluated effective cost (J = sum of squared errors after the m-estimators being applied).
   * Its relation to the optimization phase depends on the callback event:
   *   OPTIMIZATION_INITIALIZED:      the initial value (before any optimization)
   *   DESIGN_VARIABLES_UPDATED, RESIDUALS_UPDATED:  undefined
   *   COST_UPDATED:                  the new cost after an update of the design variables
   */
  double currentCost;
  /**
   * \brief the most recently evaluated effective cost (J = sum of squared errors after the m-estimators being applied).
   * Its relation to the optimization phase depends on the callback event:
   *   OPTIMIZATION_INITIALIZED, DESIGN_VARIABLES_UPDATED, RESIDUALS_UPDATED:  undefined
   *   COST_UPDATED:                  the lowest previous cost so far (or -1 if this is the initial update)
   */
  double previousLowestCost;
};

namespace event {

/// \brief Right after the the initial cost has been computed.
struct OPTIMIZATION_INITIALIZED : Event {
  using Event::Event;
};

/// \brief At the start of an iteration before any work has been done, same state as in PER_ITERATION_END in previous iteration
struct ITERATION_START : Event {
  using Event::Event;
};

/// \brief At the end of an iteration after all work has been done, same state as in PER_ITERATION_START in next iteration
struct ITERATION_END : Event {
  using Event::Event;
};

/// \brief Right after the linear system was solved
struct LINEAR_SYSTEM_SOLVED : Event {
  using Event::Event;
};

/// \brief Right after the design variables (X) have been updated.
struct DESIGN_VARIABLES_UPDATED : Event {
  using Event::Event;
};

/// \brief After the raw squared error for each error term has been updated but before the m-estimators are applied to compute the effective cost. This is the right event to update m-estimators based on residuals.
struct RESIDUALS_UPDATED : Event {
  using Event::Event;
};

/// \brief Right after the m-estimators are applied to compute the effective cost.
struct COST_UPDATED : Event {
  using Event::Event;
};

}

enum class ProceedInstruction {
  CONTINUE,
  SUCCEED,
  FAIL
};

class OptimizerCallbackInterface {
public:
  typedef boost::shared_ptr<OptimizerCallbackInterface> Ptr;
  virtual ~OptimizerCallbackInterface() {}
  virtual ProceedInstruction operator() (const Event & arg) = 0;
};

template <typename Funct, typename Event_>
class CallbackFunctor : public OptimizerCallbackInterface {
public:
  CallbackFunctor(Funct f) : f(f) {}
  ProceedInstruction operator() (const Event & arg) override {
    return call(arg);
  }
private:
  template<typename F = Funct, std::is_same<ProceedInstruction, decltype((*static_cast<F*>(nullptr))(*static_cast<Event_*>(nullptr)))>* returnsProceedInstruction = nullptr>
  ProceedInstruction call(const Event & arg) {
    return withArg(arg, returnsProceedInstruction);
  }
  template<typename F = Funct, std::is_same<ProceedInstruction, decltype((*static_cast<F*>(nullptr))())>* returnsProceedInstruction = nullptr>
  ProceedInstruction call(const Event &) {
    return withoutArg(returnsProceedInstruction);
  }

  ProceedInstruction withoutArg(std::true_type*) {
    return f();
  }
  ProceedInstruction withoutArg(std::false_type*) {
    f();
    return ProceedInstruction::CONTINUE;
  }
  ProceedInstruction withArg(const Event & arg, std::true_type*) {
    return f(static_cast<const Event_ &>(arg));
  }
  ProceedInstruction withArg(const Event & arg, std::false_type*) {
    f(static_cast<const Event_ &>(arg));
    return ProceedInstruction::CONTINUE;
  }
  Funct f;
};

template <typename Event_ = Event, typename Funct>
OptimizerCallbackInterface::Ptr toOptimizerCallback(Funct f){
  return boost::make_shared<CallbackFunctor<Funct, Event_>>(f);
}

class OptimizerCallback {
public:
  OptimizerCallback(const OptimizerCallbackInterface::Ptr & impl) : impl_(impl) {}

  template <typename Funct>
  OptimizerCallback(Funct funct) : OptimizerCallback(toOptimizerCallback(funct)) {}

  template <typename Event_, typename Funct>
  OptimizerCallback(Event_*, Funct funct) : OptimizerCallback(toOptimizerCallback<Event_>(funct)) {}

  bool operator == (const OptimizerCallback & other) const { return impl_ == other.impl_; };
  ProceedInstruction operator() (const Event & arg){
    return (*impl_)(arg);
  }
private:
  OptimizerCallbackInterface::Ptr impl_;
};

} // namespace callback
} // namespace backend
}  // namespace aslam

#endif /* OPTIMIZER_CALLBACK_HPP */

ext: hpp


text:
#include <msgpack.hpp>
#include <sstream>
#include <iterator>
#include <cmath>

#include <gtest/gtest.h>

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#if defined(MSGPACK_USE_BOOST)

#include <boost/fusion/adapted/struct/define_struct.hpp>
#include <boost/fusion/adapted/struct/adapt_struct.hpp>

const double kEPS = 1e-10;

BOOST_FUSION_DEFINE_STRUCT(
    BOOST_PP_EMPTY(),
    mystruct,
    (int, f1)
    (double, f2)
)

TEST(MSGPACK_BOOST, fusion_pack_unpack_convert)
{
    std::stringstream ss;
    mystruct val1;
    val1.f1 = 42;
    val1.f2 = 123.45;
    msgpack::pack(ss, val1);
    msgpack::object_handle oh =
        msgpack::unpack(ss.str().data(), ss.str().size());
    mystruct val2 = oh.get().as<mystruct>();
    EXPECT_TRUE(val1.f1 == val2.f1);
    EXPECT_TRUE(fabs(val2.f2 - val1.f2) <= kEPS);
}

TEST(MSGPACK_BOOST, object_with_zone_convert)
{
    mystruct val1;
    val1.f1 = 42;
    val1.f2 = 123.45;
    msgpack::zone z;
    msgpack::object obj(val1, z);
    mystruct val2 = obj.as<mystruct>();
    EXPECT_TRUE(val1.f1 == val2.f1);
    EXPECT_TRUE(fabs(val2.f2 - val1.f2) <= kEPS);
}

#if !defined(MSGPACK_USE_CPP03)

struct no_def_con1 {
    no_def_con1() = delete;
    no_def_con1(int i):i(i) {}
    int i;
    MSGPACK_DEFINE(i);
};

inline bool operator==(no_def_con1 const& lhs, no_def_con1 const& rhs) {
    return lhs.i == rhs.i;
}

inline bool operator!=(no_def_con1 const& lhs, no_def_con1 const& rhs) {
    return !(lhs == rhs);
}

struct no_def_con2 {
    no_def_con2() = delete;
    no_def_con2(int i):i(i) {}
    int i;
    MSGPACK_DEFINE(i);
};

inline bool operator==(no_def_con2 const& lhs, no_def_con2 const& rhs) {
    return lhs.i == rhs.i;
}

inline bool operator!=(no_def_con2 const& lhs, no_def_con2 const& rhs) {
    return !(lhs == rhs);
}

namespace msgpack {
MSGPACK_API_VERSION_NAMESPACE(MSGPACK_DEFAULT_API_NS) {
    namespace adaptor {

    template <>
    struct as<no_def_con1> {
        no_def_con1 operator()(msgpack::object const& o) const {
            if (o.type != msgpack::type::ARRAY) throw msgpack::type_error();
            if (o.via.array.size != 1) throw msgpack::type_error();
            return no_def_con1(o.via.array.ptr[0].as<int>());
        }
    };

    template <>
    struct as<no_def_con2> {
        no_def_con2 operator()(msgpack::object const& o) const {
            if (o.type != msgpack::type::ARRAY) throw msgpack::type_error();
            if (o.via.array.size != 1) throw msgpack::type_error();
            return no_def_con2(o.via.array.ptr[0].as<int>());
        }
    };

    } // adaptor
} // MSGPACK_API_VERSION_NAMESPACE(MSGPACK_DEFAULT_API_NS)
} // msgpack

struct mystruct_no_def_con {
    mystruct_no_def_con() = delete;
    // Constructor that have parameters corresponding to BOOST_FUSION_ADAPT_STRUCT is mandatory.
    // See *1, *2, and *3
    mystruct_no_def_con(
        no_def_con1 i,
        no_def_con2 j,
        no_def_con1 k):
        f1(std::move(i)),
        f2(std::move(j)),
        f3(std::move(k)) {}

    no_def_con1 f1;
    no_def_con2 f2;
    no_def_con1 f3;
};

inline bool operator==(mystruct_no_def_con const& lhs, mystruct_no_def_con const& rhs) {
    return lhs.f1 == rhs.f1 && lhs.f2 == rhs.f2 && lhs.f3 == rhs.f3;
}

inline bool operator!=(mystruct_no_def_con const& lhs, mystruct_no_def_con const& rhs) {
    return !(lhs == rhs);
}

BOOST_FUSION_ADAPT_STRUCT(
    mystruct_no_def_con,
    f1, // *1
    f2, // *2
    f3  // *3
)

// MSVC2015's std::tuple requires default constructor during 'as' process.
// It doesn't support Expression SFINAE yet, then 'as' is fallbacked to 'convert'.
// After MSVC would support Expression SFINAE, remove this guard.
#if !defined(_MSC_VER)

TEST(MSGPACK_BOOST, pack_convert_no_def_con)
{
    std::stringstream ss;
    mystruct_no_def_con val1(no_def_con1(1), no_def_con2(2), no_def_con1(3));
    msgpack::pack(ss, val1);
    msgpack::object_handle oh =
        msgpack::unpack(ss.str().data(), ss.str().size());
    mystruct_no_def_con val2 = oh.get().as<mystruct_no_def_con>();
    EXPECT_TRUE(val1 == val2);
}

#endif // !defined(_MSC_VER)

#endif // !defined(MSGPACK_USE_CPP03

#endif // defined(MSGPACK_USE_BOOST)

max_stars_repo_name: Mystfit/msgpack-c

max_issues_repo_name: Mystfit/msgpack-c

max_forks_repo_name: Mystfit/msgpack-c


text:
//  Copyright (c) 2001-2008 Hartmut Kaiser
//
//  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)

#if !defined(BOOST_SPIRIT_KARMA_META_GRAMMAR_MAY_01_2007_0313PM)
#define BOOST_SPIRIT_KARMA_META_GRAMMAR_MAY_01_2007_0313PM

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once      // MS compatible compilers support #pragma once
#endif

#include <boost/spirit/home/karma/domain.hpp>
#include <boost/spirit/home/support/meta_grammar.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace spirit { namespace karma
{
    ///////////////////////////////////////////////////////////////////////////
    // forwards
    ///////////////////////////////////////////////////////////////////////////
    template <typename Char>
    struct any_stream;

    template <typename Char>
    struct stream_director;

    struct main_meta_grammar;

    template <typename Expr, typename Enable>
    struct is_valid_expr;

    template <typename Expr, typename Enable>
    struct expr_transform;

    ///////////////////////////////////////////////////////////////////////////
    // get the director for a stream
    ///////////////////////////////////////////////////////////////////////////
    template <typename Tag>
    struct extract_any_stream_director;

    template <>
    struct extract_any_stream_director<tag::stream>
    {
        typedef any_stream<char> type;
    };

    template <>
    struct extract_any_stream_director<tag::wstream>
    {
        typedef any_stream<wchar_t> type;
    };

    ///////////////////////////////////////////////////////////////////////////
    template <typename Tag, typename T>
    struct extract_stream_director;

    template <typename T>
    struct extract_stream_director<tag::stream, T>
    {
        typedef stream_director<char> type;
    };

    template <typename T>
    struct extract_stream_director<tag::wstream, T>
    {
        typedef stream_director<wchar_t> type;
    };

    ///////////////////////////////////////////////////////////////////////////
    // utility meta-grammar
    ///////////////////////////////////////////////////////////////////////////
    struct utility_meta_grammar
     :  proto::or_<
            // stream, wstream
            meta_grammar::compose_empty<
                proto::if_<
                    is_stream_tag<proto::_arg, karma::domain>()
                >,
                karma::domain,
                mpl::identity<extract_any_stream_director<mpl::_> >
            >,
            // stream(T), wstream(T)
            meta_grammar::compose_function1_eval<
                proto::function<
                    proto::if_<
                        is_stream_tag<proto::_arg, karma::domain>()
                    >,
                    proto::_
                >,
                karma::domain,
                mpl::identity<extract_stream_director<mpl::_, mpl::_> >
            >
        >
    {
    };

    ///////////////////////////////////////////////////////////////////////////
    //  These specializations non-intrusively hook into the Karma meta-grammar.
    //  (see karma/meta_grammar.hpp)
    ///////////////////////////////////////////////////////////////////////////
    template <typename Expr>
    struct is_valid_expr<
            Expr,
            typename enable_if<
                proto::matches<Expr, utility_meta_grammar>
            >::type
        >
      : mpl::true_
    {
    };

    template <typename Expr>
    struct expr_transform<
            Expr,
            typename enable_if<
                proto::matches<Expr, utility_meta_grammar>
            >::type
        >
      : mpl::identity<utility_meta_grammar>
    {
    };

}}}

#endif

max_stars_repo_name: jaredhoberock/gotham

max_issues_repo_name: jaredhoberock/gotham

max_forks_repo_name: jaredhoberock/gotham


text:
#include <math.h>
#include <algorithm>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/bind.hpp>
#include <iomanip>
#include <time.h>
#include <socialgraph.h>

void SocialGraph::setGraph(std::ifstream &fin){
	//read file
	facebook::Data data = facebook::readFile(fin, name);

	//create map for name and id
	map = facebook::createMap(data);

	//create graph
	g = facebook::createGraph(data, map);

	for(int i=0; i<num_vertices(g); i++){
		put(vertex_name, g, i, name[i]);
	}

	forces.resize(num_vertices(g));

	//create centrality graph
	cg = g;
}

void SocialGraph::setGraph2(std::ifstream &fin){
	facebook::Name t_name;
	facebook::Data data = facebook::readFile(fin, t_name);

	map2 = facebook::createMap(data);

	g2 = facebook::createGraph(data, map2);
}

void SocialGraph::setWeightedGraph(){
	WeightedGraph twg(num_vertices(g));

	property_map<WeightedGraph, edge_weight_t>::type weightmap = get(edge_weight, twg);
	EdgeIterator e, e_end;
	for(tie(e, e_end) = edges(cg); e != e_end; e++){
		WeightedEdgeDescriptor e1; bool inserted;
		tie(e1, inserted) = add_edge(source(*e, cg), target(*e, cg), twg);
		weightmap[e1] = 1;
	}

	wg = twg;
}

void SocialGraph::setNodes(){
	nodes.resize(num_vertices(g));
	for(int i=0; i<num_vertices(g); i++){
		nodes[i].x = (rand() % 200) -100;
		nodes[i].y = (rand() % 200) -100;
		nodes[i].dx = 0;
		nodes[i].dy = 0;
	}
}

void SocialGraph::setNode(int i, int x, int y){
	nodes[i].x = x;
	nodes[i].y = y;
}

Graph SocialGraph::getGraph(){
	return g;
}

Graph SocialGraph::getGraph2(){
	return g2;
}

WeightedGraph SocialGraph::getWeightedGraph(){
	return wg;
}

facebook::Name SocialGraph::getName(){
	return name;
}

Graph SocialGraph::getClusterGraph(){
	return cg;
}

Graph SocialGraph::getClusterGraph2(){
	return cg2;
}

Nodes SocialGraph::getNodes(){
	return nodes;
}

void SocialGraph::repF(int i){
	for(int j=i+1; j<nodes.size(); j++){
		float nx = nodes[i].x - nodes[j].x;
		float ny = nodes[i].y - nodes[j].y;
		float r = sqrt(pow(nx, 2) + pow(ny, 2));
		if(r > 0){
			float f = 900 / (r * r);
			forces[i].x += f * nx / r;
			forces[i].y += f * ny / r;
			forces[j].x -= f * nx / r;
			forces[j].y -= f * ny / r;
		}
	}
}

void SocialGraph::repulsiveForce(){
	for(int i=0; i<nodes.size(); i++){
		repF(i);
	}
}

void SocialGraph::attractiveForce(Graph temp_g){
	EdgeIterator e, e_end;
	for(tie(e, e_end) = edges(temp_g); e != e_end; e++){
		int s = source(*e, temp_g);
		int t = target(*e, temp_g);
		float nx = nodes[s].x - nodes[t].x;
		float ny = nodes[s].y - nodes[t].y;
		float r = sqrt(pow(nx, 2) + pow(ny, 2));
		if(r > 0){
			float f = -(r * r) / 30;
			forces[s].x += f * nx / r;
			forces[s].y += f * ny / r;
			forces[t].x -= f * nx / r;
			forces[t].y -= f * ny / r;

			/*
			forces[s].x += f * nx / (r * out_degree(s,temp_g));
			forces[s].y += f * ny / (r * out_degree(s,temp_g));
			forces[t].x -= f * nx / (r * out_degree(t,temp_g));
			forces[t].y -= f * ny / (r * out_degree(t,temp_g));
			*/
		}
	}
}

float SocialGraph::limitForce(){
	float dt = .2;
	float damping = 0.01;
	float kinetic = 0.0;

	for(int i=0; i<nodes.size(); i++){
		nodes[i].dx = (nodes[i].dx + dt * forces[i].x) * damping;
		nodes[i].dy = (nodes[i].dy + dt * forces[i].y) * damping;

		forces[i].x = 0;
		forces[i].y = 0;

		if(nodes[i].dx > 1) nodes[i].dx = 1;
		if(nodes[i].dx < -1) nodes[i].dx = -1;
		if(nodes[i].dy > 1) nodes[i].dy = 1;
		if(nodes[i].dy < -1) nodes[i].dy = -1;

/*
		if(nodes[i].dx > 10) nodes[i].dx = 10;
		if(nodes[i].dx < -10) nodes[i].dx = -10;
		if(nodes[i].dy > 10) nodes[i].dy = 10;
		if(nodes[i].dy < -10) nodes[i].dy = -10;
*/

		nodes[i].x = nodes[i].x + dt * nodes[i].dx;
		nodes[i].y = nodes[i].y + dt * nodes[i].dy;

		kinetic += sqrt(pow(nodes[i].dx, 2) + pow(nodes[i].dy, 2));
	}

	return kinetic;
}

float SocialGraph::springModel(Graph temp_g){
	float kinetic = 0.0;

	repulsiveForce();
	attractiveForce(temp_g);

	kinetic = limitForce();

	return kinetic;
}

void changeColor(float &r, float &g, float &b){
	if(r < 1.0){
		r += .5;
	}
	else{
		r = 0.0;
		if(g < 1.0){
			g += .5;
		}
		else{
			g = 0.0;
			if(b < .5){
				b += .5;
			}
			else
				b = 0.0;
		}
	}
}

void SocialGraph::clusterColor(Graph tempg){
	float r = 0.0;
	float g = 0.0;
	float b = 0.0;

	wg = ::createWeightedGraph(tempg);

	for(int i=0; i<num_vertices(wg); i++){
		for(int j=i+1; j<num_vertices(wg); j++){
			if(edge(i,j,wg).second == 0 && edge(j,i,wg).second != 0)
				remove_edge(j,i,wg);
			else if(edge(i,j,wg).second != 0 && edge(j,i,wg).second == 0)
				remove_edge(i,j,wg);
		}
	}

	std::vector<int> visited(num_vertices(wg), 0);
	int cluster = 0;
	for(int i=0; i<num_vertices(wg); i++){
		if(visited[i] == 0){
			cluster++;

			std::vector<WeightedVertexDescriptor> p(num_vertices(wg));
			std::vector<int> d(num_vertices(wg));
			dijkstra_shortest_paths(wg, i, predecessor_map(&p[0]).distance_map(&d[0]));

			changeColor(r,g,b);
			//std::cout << r << " " << g << " " << b << std::endl;
			for(int j=0; j<num_vertices(wg); j++){
				if(d[j] != INT_MAX){
					visited[j] = 1;
					nodes[j].r = r;
					nodes[j].g = g;
					nodes[j].b = b;
					nodes[j].cluster = cluster;
				}
			}
		}
	}
	std::cout << "number of clusters: " << cluster << std::endl;
}

bool ContainsNode(std::vector<int> visited, int node){
	for(int i=0; i<visited.size(); i++){
		if(visited[i] == node)
			return true;
	}
	return false;
}

void SocialGraph::DepthFirst(std::vector<int> &visited, int max, std::vector<int> &hits, int &max_short, int t){
	int back = visited.back();
	graph_traits<WeightedGraph>::out_edge_iterator e, e_end;

	for(tie(e, e_end) = out_edges(back, wg); e != e_end; e++){
		int node = target(*e, wg);

		if(ContainsNode(visited, node)) continue;

		if(node == t){
			visited.push_back(node);
			for(int i=1; i<visited.size()-1; i++){
				hits[visited[i]]++;
			}
			max_short++;

			int n = (int) visited.size() -1;
			visited.erase(visited.begin() + n);

			break;
		}

		visited.push_back(node);

		if(visited.size() <= max)
			DepthFirst(visited, max, hits, max_short, t);

		int n = (int)visited.size() - 1;
		visited.erase(visited.begin() + n);
	}
}

void SocialGraph::ShortestPaths(int s, std::vector<double> &centrality){
	std::vector<WeightedVertexDescriptor> p(num_vertices(wg));
	std::vector<int> d(num_vertices(wg));
	dijkstra_shortest_paths(wg, s, predecessor_map(&p[0]).distance_map(&d[0]));

	for(int i=0; i<num_vertices(wg); i++){
		//std::cout << i << " " << d[i] << std::endl;
		if(d[i] > 1 && d[i] != INT_MAX){
			std::vector<int> hits(num_vertices(wg), 0);
			int max_short = 0;

			std::vector<int> visited;
			visited.push_back(s);
			DepthFirst(visited, d[i], hits, max_short, i);

			for(int j=0; j<num_vertices(wg); j++){
				if(max_short > 0)
					centrality[j] += (double)hits[j]/(double)max_short;
			}
		}
	}
}

void SocialGraph::setMatrix(){
	Matrix matrix(num_vertices(g));

	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> list(num_vertices(g));
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 1)
				list[j] = 1;
			else
				list[j] = 0;
		}
		matrix[i] = list;
	}

	matrixlist.push_back(matrix);
}

void SocialGraph::addMatrix(){
	Matrix matrix1 = matrixlist[0];
	Matrix matrix2 = matrixlist[matrixlist.size() -1];
	Matrix temp(num_vertices(g));

	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> list(num_vertices(g), 0);
		for(int j=0; j<num_vertices(g); j++){
			for(int k=0; k<num_vertices(g); k++){
				list[j] += matrix1[i][k] * matrix2[k][j];
			}
		}
		temp[i] = list;
	}

	matrixlist.push_back(temp);
}

void SocialGraph::printMatrix(){
	Matrix matrix = matrixlist[matrixlist.size() -1];

	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++)
			std::cout << matrix[i][j] << " ";
		std::cout << std::endl;
	}
}

std::vector<double> SocialGraph::Dependency(int s){
	std::vector<WeightedVertexDescriptor> p(num_vertices(wg));
	std::vector<int> d(num_vertices(wg));
	dijkstra_shortest_paths(wg, s, predecessor_map(&p[0]).distance_map(&d[0]));

	std::vector<double> centrality(num_vertices(g), 0.0);

	for(int i=0; i<num_vertices(g); i++){
		//if(d[i] > 1 && d[i] != INT_MAX){
		if(d[i] > 1 && d[i] < 4){
			while(d[i] > matrixlist.size())
				addMatrix();

			//number of shortest paths
			int num = matrixlist[d[i]-1][s][i];

			std::vector<double> temp(num_vertices(g), 0.0);
			for(int j=1; j<d[i]; j++){
				for(int k=0; k<num_vertices(g); k++)
					temp[k] += matrixlist[j-1][s][k] * matrixlist[d[i]-1-j][k][i];
			}
			for(int k=0; k<num_vertices(g); k++)
				centrality[k] += (double)temp[k] / (double)num;
		}
	}

	return centrality;
}

MatrixScore SocialGraph::AllDependency(){
	MatrixScore all_centrality;

	for(int i=0; i<num_vertices(g); i++){
		std::vector<double> centrality = Dependency(i);
		all_centrality.push_back(centrality);
	}

	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 1)
				all_centrality[i][j] = 0;
		}
	}

	return all_centrality;
}

int SocialGraph::convertNode(int s){
	std::string source = map.left.find(s)->second;
	int t = map2.right.find(source)->second;
	return t;
}

Matrix SocialGraph::getDistance(){
	return distance;
}

void SocialGraph::setAvgDegree(){
	float avg=0.0;

	for(int i=0; i<num_vertices(g); i++){
		avg += out_degree(i, g);
	}
	avg = avg / num_vertices(g);

	avgDegree = avg;
}

float SocialGraph::getAvgDegree(){
	return avgDegree;
}

Edges SocialGraph::newEdgesList(){
	Edges edges;
	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 0){
				std::string source = map.left.find(i)->second;
				std::string target = map.left.find(j)->second;
				int s = map2.right.find(source)->second;
				int t = map2.right.find(target)->second;
				if(s > 0 && t > 0 && s < num_vertices(g2) + 1 && t < num_vertices(g2)){
					if(edge(s, t, g2).second == 1){
						Edge e(i, j);
						edges.push_back(e);
					}
				}
			}
		}
	}
	return edges;
}

void SocialGraph::setNewEdges(){
	Edges edges = newEdgesList();

	for(int i=0; i<edges.size(); i++){
		int s = edges[i].first;
		int t = edges[i].second;

		Edge e; e.first = s; e.second = t;
		if(distance[s][t] < num_vertices(g) && distance[s][t] > 0){
			while(newEdges.size() <= distance[s][t]){
				Edges es;
				newEdges.push_back(es);
			}
			newEdges[distance[s][t]].push_back(e);
		}
		else
			newEdges[0].push_back(e);
	}

	int count = 0;
	float allavg=0.0;
	for(int i=0; i<newEdges.size(); i++){
		count += newEdges[i].size();
		for(int j=0; j<newEdges[i].size(); j++){
			int s = newEdges[i][j].first;
			int t = newEdges[i][j].second;
			allavg += out_degree(s, g) + out_degree(t, g);
		}
	}
	std::cout << "new edges(all_new) = " << count << std::endl;
	numNewEdges = count;
	allavg = allavg/(count*2);
	std::cout << "avg degree(all_new) = " << allavg << std::endl;
	std::cout << std::endl;

	for(int i=0; i<newEdges.size(); i++){
		std::cout << "new edges (" << i << ") = " << newEdges[i].size() << std::endl;
		float avgnew = 0.0;
		for(int j=0; j<newEdges[i].size(); j++){
			int s = newEdges[i][j].first;
			int t = newEdges[i][j].second;
			avgnew += out_degree(s, g) + out_degree(t, g);
		}
		avgnew = avgnew/(newEdges[i].size()*2);
		std::cout << "avg degree (" << i << ") = " << avgnew << std::endl;
	}
	std::cout << std::endl;

	count = 0;
	int count2 = 0;
	float avg=0.0;
	float avg2=0.0;
	for(int i=0; i<newEdges[3].size(); i++){
		int flag = 0;
		int s = newEdges[3][i].first;
		int t = newEdges[3][i].second;
		for(int j=0; j<newEdges[2].size(); j++){
			int s2 = newEdges[2][j].first;
			int t2 = newEdges[2][j].second;
			if(s2 == s){
				if(distance[t2][t] == 1){
					flag = 1;
				}
			}
			else if(t2 == s){
				if(distance[s2][t] == 1){
					flag = 1;
				}
			}
			else if(s2 == t){
				if(distance[t2][s] == 1){
					flag = 1;
				}
			}
			else if(t2 == t){
				if(distance[s2][s] == 1){
					flag = 1;
				}
			}
			if(flag == 1){
				count++;
				avg += out_degree(s,g) + out_degree(t, g);
				break;
			}
		}
		if(flag == 0){
			count2++;
			avg2 += out_degree(s, g) + out_degree(t, g);
		}
	}
	std::cout << "new edges(3)<-(2) = " << count << std::endl;
	avg = avg / (count * 2);
	std::cout << "avg degree of (3)<-(2) = " << avg << std::endl;

	std::cout << "new edges(3)!(2) = " << count2 << std::endl;
	avg2 = avg2 / (count2 * 2);
	std::cout << "avg degree of (3)!(2) = " << avg2 << std::endl;
}

std::vector<Edges> SocialGraph::getNewEdges(){
	return newEdges;
}

//new functions
void SocialGraph::initialize(int argc, char *argv[]){
	std::ifstream fin;
	fin.open(argv[1]);
	facebook::Data data = facebook::readFile(fin, name);
	map = facebook::createMap(data);
	g = facebook::createGraph(data, map);
	for(int i=0; i<num_vertices(g); i++){
		put(vertex_name, g, i, name[i]);
	}
	forces.resize(num_vertices(g));
	fin.close();

	distance = ::createAllPairsShortestPaths(::createWeightedGraph(g));
	setAvgDegree();

	//initialize graph for t+1
	fin.open(argv[2]);
	Graph temp_g = ::createGraph(fin);
	g2 = g;
	fin.close();

	new_edges = ::newEdgeList(g, temp_g);
	new_matrix = ::newList2Matrix(g, new_edges);
	countd2=0;
	for(int i=0; i<new_edges.size(); i++){
		if(distance[new_edges[i].first][new_edges[i].second] < num_vertices(g)){
			if(distance[new_edges[i].first][new_edges[i].second] == 2){
				countd2++;
			}
			add_edge(new_edges[i].first, new_edges[i].second, g2);
			add_edge(new_edges[i].second, new_edges[i].first, g2);
		}
	}

	distance2 = ::createAllPairsShortestPaths(::createWeightedGraph(g2));

	setNodes();
	setAvgDegree();

	std::cout << "number of vertices: " << num_vertices(g) << std::endl;
	std::cout << "number of edges: " << num_edges(g)/2 << std::endl;
	std::cout << "number of new edges: " << new_edges.size() << std::endl;
	std::cout << "number of new edges at disance 2: " << countd2 << std::endl;

	#pragma omp parallel for
	for(int i=0; i<2; i++){
		if(i == 0)
			cg = ::ClusterEdgeCentrality(g, num_vertices(g));
		if(i == 1)
			cg2 = ::ClusterEdgeCentrality(g2, num_vertices(g2));
	}

	sg = g;
}

void SocialGraph::test(){
	Scores q;

	probabilityAA = ::AdamicAdar(g);
	q = ::SortScores(probabilityAA);

	//use this to count how many were the same as
	//betweenness centrality based method
	Edges daa;
	Edges dbc;

	int a=0;
	for(int i=0; i<new_edges.size(); i++){
	//for(int i=0; i<countd2; i++){
		int s = q[i].second.first;
		int t = q[i].second.second;
		if(edge(s, t, g2).second == 1){
			a++;
			daa.push_back(Edge(s,t));
		}
	}
	std::cout << "adamic-adar:" << a << std::endl;

	std::cout << "new edges of distance > 2: " << a << std::endl;
	std::cout << "average node degree: " << avgDegree << std::endl;

	std::vector<double> vbc(num_vertices(g), 0.0);
	std::vector<double> ebc(num_edges(g), 0.0);
	::VertexBetweennessCentrality(g, vbc, ebc);

	float beta = 0;
	probability = ::BCBasedLP(g, distance, vbc, beta);
	Scores sbc = ::SortScores(probability);
	a=0;
	int b=0;
	for(int i=0; i<new_edges.size(); i++){
	//for(int i=0; i<countd2; i++){
		int s = sbc[i].second.first;
		int t = sbc[i].second.second;
		if(edge(s, t, g2).second == 1 && (probability[s][t] > 0 || probability[t][s] > 0)){
			detected_edges.push_back(Edge(s,t));
			a++;
			dbc.push_back(Edge(s,t));
			if(distance[s][t] > 2)
				b++;
		}
	}
	std::cout << a << ":" << b << std::endl;

	//check how many were the same
	int overlap=0;
	for(int i=0; i<dbc.size(); i++){
		int s = dbc[i].first;
		int t = dbc[i].second;
		for(int j=0; j<daa.size(); j++){
			int s1 = daa[j].first;
			int t1 = daa[j].second;
			if((s1 == s && t1 == t) || (s1 == t && t1 == s))
				overlap++;
		}
	}
	std::cout << "overlap: " << overlap << "/" << dbc.size() << " = " << (float)overlap/dbc.size() << std::endl;

	vertex_bc = vbc;

	//calculate number of paths at what distance
	std::vector<int> count;
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
			if(distance[x][y] < num_vertices(g)){
				while(count.size() <= distance[x][y]){
					count.push_back(0);
				}
				count[distance[x][y]]++;
			}
		}
	}
	for(int i=0; i<count.size(); i++){
		std::cout << i << ":" << count[i] << std::endl;
	}

	::NormalizePr(probability, distance);
	::NormalizePr(probabilityAA, distance);

	edge_bc = ::EdgeCentrality(g, ebc);

	//finds probability of correct new edges detected
	//based on how many new edges detected of one individual
	std::vector<int> nEdges(num_vertices(g),0);
	for(int i=0; i<new_edges.size(); i++){
		int s = new_edges[i].first;
		int t = new_edges[i].second;
		if(distance[s][t] == 2){
			nEdges[s]++;
			nEdges[t]++;
		}
	}
	int necount=0; int necorrect=0;
	for(int i=0; i<num_vertices(g); i++){
		necount += nEdges[i];
		if(nEdges[i] > 0){
			Scores nscores = SortIndividualScores(probability, i);

			for(int j=0; j<nEdges[i]; j++){
				int s = nscores[j].second.first;
				int t = nscores[j].second.second;
				if(edge(s,t,g2).second == 1)
					necorrect++;
			}
		}
	}
	std::cout << necorrect << "/" << necount << std::endl;

	//change size of vertex and edge betweenness centrality
	//for visualization
	vbc = sortDouble(vertex_bc);
	double minbc = vbc[0.05*vbc.size()];
	double maxbc = vbc[0.95*vbc.size()];
	double diff = maxbc - minbc;
	for(int i=0; i<num_vertices(g); i++){
		if(vertex_bc[i] < minbc)
			vertex_bc[i] = 10;
		else if(vertex_bc[i] > maxbc)
			vertex_bc[i] = 25;
		else
			vertex_bc[i] = 10 + 15 * (vertex_bc[i] - minbc)/diff;
	}

	ebc = sortDouble(ebc);
	minbc = ebc[0];
	maxbc = ebc[ebc.size()*.5];
	diff = maxbc - minbc;

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i,j,g).second == 1){
			if(edge_bc[i][j] < minbc){
				edge_bc[i][j] = 5;
				edge_bc[j][i] = 5;
			}
			else if(edge_bc[i][j] > maxbc){
				edge_bc[i][j] = 1;
				edge_bc[j][i] = 1;
			}
			else{
				edge_bc[i][j] = 5.0 - 4.0 * (edge_bc[i][j] - minbc)/diff;
				edge_bc[j][i] = edge_bc[i][j];
			}
			}
		}
	}

	//copy of above for future graph
	std::vector<double> vbc2(num_vertices(g2), 0.0);
	std::vector<double> ebc2(num_edges(g2), 0.0);
	::VertexBetweennessCentrality(g2, vbc2, ebc2);
	vertex_bc2 = vbc2;
	edge_bc2 = ::EdgeCentrality(g2, ebc2);

	vbc2 = sortDouble(vertex_bc2);
	minbc = vbc2[0.05*vbc2.size()];
	maxbc = vbc2[0.95*vbc2.size()];
	diff = maxbc - minbc;
	for(int i=0; i<num_vertices(g); i++){
		if(vertex_bc2[i] < minbc)
			vertex_bc2[i] = 10;
		else if(vertex_bc2[i] > maxbc)
			vertex_bc2[i] = 25;
		else
			vertex_bc2[i] = 10 + 15 * (vertex_bc2[i] - minbc)/diff;
	}

	ebc2 = sortDouble(ebc2);
	minbc = ebc2[0];
	maxbc = ebc2[ebc2.size()*.5];
	diff = maxbc - minbc;

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i,j,g2).second == 1){
			if(edge_bc2[i][j] < minbc){
				edge_bc2[i][j] = 5;
				edge_bc2[j][i] = 5;
			}
			else if(edge_bc2[i][j] > maxbc){
				edge_bc2[i][j] = 1;
				edge_bc2[j][i] = 1;
			}
			else{
				edge_bc2[i][j] = 5.0 - 4.0 * (edge_bc2[i][j] - minbc)/diff;
				edge_bc2[j][i] = edge_bc2[i][j];
			}
			}
		}
	}

}

void SocialGraph::showDetected(){
	static int k=0;

	for(int i=0; i<num_vertices(g); i++){
		nodes[i].r = 0; nodes[i].g = 0; nodes[i].b = 0;
	}
	vizN.clear();

	k++;
	if(k == detected_edges.size())
		k = 0;

	int s = detected_edges[k].first;
	int t = detected_edges[k].second;
	nodes[s].b = 1.0; nodes[t].b = 1.0;

	vizN.push_back(s); vizN.push_back(t);

	for(int i=0; i<num_vertices(g); i++){
		if(distance[s][i] + distance[i][t] == distance[s][t]
		&& s != i && t != i){
			nodes[i].g = 1.0;
			vizN.push_back(i);
		}
	}

	std::cout << "changed " << k << ", normalized bc score = " << probability[s][t] << std::endl;
}

Graph SocialGraph::getSG(){
	return sg;
}

Graph SocialGraph::getSG2(){
	return sg2;
}

void SocialGraph::selectSG(int s){
	if(s != -1){

	Graph tempg(num_vertices(g));

	std::vector<int> nhbr;
	nhbr.push_back(s);
	for(int t=0; t<num_vertices(g); t++){
		if(distance[s][t] == 2 || distance[s][t] == 1){
			nhbr.push_back(t);

			//make nodes closer to selected node
			//for visualization
			/*
			nodes[t].x = nodes[s].x + (20.0*rand()/RAND_MAX - 10.0);
			nodes[t].y = nodes[s].y + (20.0*rand()/RAND_MAX - 10.0);
			*/
		}
	}

	for(int x=0; x<nhbr.size()-1; x++){
		for(int y=x+1; y<nhbr.size(); y++){
			int i = nhbr[x];
			int j = nhbr[y];
			if(edge(i,j,g).second == 1 && edge(i,j,tempg).second != 1){
				add_edge(i,j,tempg);
				add_edge(j,i,tempg);
			}
		}
	}

	sg = tempg;

	/*
	for(int i=0; i<num_vertices(g); i++){
		if(i == s){
			nodes[i].r = 1; nodes[i].g = 0; nodes[i].b = 0;
		}
		else{
			nodes[i].r = 0; nodes[i].g = 0; nodes[i].b = 0;
		}
	}
	*/

	}
}

void SocialGraph::selectFutureSG(int s){
	sg2 = sg;
	sgEdges.clear();

	int count=0;
	for(int i=0; i<num_vertices(g); i++){
		if(distance[s][i] == 2 && edge(s,i,g2).second == 1){
			add_edge(s,i,sg2);
			add_edge(i,s,sg2);
			count++;
			sgEdges.push_back(Edge(s,i));
		}
	}
	std::cout << count << " new edges" << std::endl;
}

float SocialGraph::SpringSG(int s, char flag){
	float kinetic = 0.0;

	if(s !=  -1){

	repulsiveForce();
	if(flag == 0)
		attractiveForce(sg);
	else if(flag == 1 || flag == 2){
		attractiveForce(sg);
		for(int i=0; i<num_vertices(g); i++){
			if(distance[s][i] == 2){
				float nx = nodes[s].x - nodes[i].x;
				float ny = nodes[s].y - nodes[i].y;
				float r = sqrt(pow(nx,2) + pow(ny,2));
				nx = nx / r;
				ny = ny / r;
				if(r>0 &&
				((flag==1)?(probability[s][i]):(probabilityAA[s][i])) >= 1){
				//if(r > 0 && (probability[s][i] >= 1)){
					float f = -2 * (r*r)/30;
					forces[s].x += f * nx;
					forces[s].y += f * ny;
					forces[i].x -= f * nx;
					forces[i].y -= f * ny;
				}
			}
		}
	}
	else if(flag == 3)
		attractiveForce(sg2);

	float dt = 1.0;
	float damping = 0.01;

	for(int i=0; i<nodes.size(); i++){
		nodes[i].dx = (nodes[i].dx + dt * forces[i].x) * damping;
		nodes[i].dy = (nodes[i].dy + dt * forces[i].y) * damping;

		forces[i].x = 0;
		forces[i].y = 0;

		if(nodes[i].dx > 1) nodes[i].dx = 1;
		if(nodes[i].dx < -1) nodes[i].dx = -1;
		if(nodes[i].dy > 1) nodes[i].dy = 1;
		if(nodes[i].dy < -1) nodes[i].dy = -1;

		if(i != s){
			nodes[i].x = nodes[i].x + dt * nodes[i].dx;
			nodes[i].y = nodes[i].y + dt * nodes[i].dy;
		}

		kinetic += sqrt(pow(nodes[i].dx, 2) + pow(nodes[i].dy, 2));
	}

	}

	return kinetic;

}

Edges SocialGraph::getSGEdges(){
	return sgEdges;
}

std::vector<double> SocialGraph::getVBC(){
	return vertex_bc;
}

std::vector<double> SocialGraph::getVBC2(){
	return vertex_bc2;
}

MatrixScore SocialGraph::getEBC(){
	return edge_bc;
}

MatrixScore SocialGraph::getEBC2(){
	return edge_bc2;
}

MatrixScore SocialGraph::getProbability(){
	return probability;
}

MatrixScore SocialGraph::getProbabilityAA(){
	return probabilityAA;
}

max_stars_repo_name: yishihara/Social-Network

max_issues_repo_name: yishihara/Social-Network

max_forks_repo_name: yishihara/Social-Network


text:
// All content Copyright (C) 2018 Genomics plc
#include <vector>
#include <set>

#include <boost/algorithm/string/join.hpp>
#include <boost/filesystem.hpp>
#include <boost/regex.hpp>
#include <boost/lexical_cast.hpp>

#include "utils/partition.hpp"
#include "utils/identity.hpp"
#include "caller/region.hpp"
#include "vcf/reader.hpp"
#include "caller/regionUtils.hpp"
#include "io/fastaFile.hpp"
#include "io/bedFile.hpp"

namespace wecall
{
namespace caller
{
    partitionedRegions_t DataRegionsBuilderBase::build()
    {
        const auto inputRegions = this->getRegions();

        auto regions = this->getValidRegions( inputRegions );

        if ( not std::is_sorted( regions.begin(), regions.end(), RegionComp( m_fastaIndexFile ) ) )
        {
            WECALL_LOG( WARNING, "Regions specified are not in order. Sorting them by contig:start-end." );

            std::sort( regions.begin(), regions.end(), RegionComp( m_fastaIndexFile ) );
            WECALL_LOG( SUPER_DEBUG,
                         "Regions sorted to: " + boost::algorithm::join( getRegionStrings( regions ), "," ) );
        }

        auto sameContig = []( const Region & a, const Region & b ) -> bool
        {
            return a.contig() == b.contig();
        };
        auto partitionedRegionsUnMerged = utils::functional::partition( regions, sameContig );

        partitionedRegions_t partitionedRegions;

        for ( const auto & partitionUnMerged : partitionedRegionsUnMerged )
        {
            partitionedRegions.emplace_back( mergeRegions( partitionUnMerged ) );
        }

        return partitionedRegions;
    }

    regions_t DataRegionsBuilderBase::getValidRegions( const regions_t & regions ) const
    {
        // Check the regions
        const auto allowedContigs = m_fastaIndexFile.contigs();

        regions_t goodRegions;
        std::set< std::string > badContigs;

        for ( const auto & region : regions )
        {
            const auto it = allowedContigs.find( region.contig() );

            if ( it == allowedContigs.cend() )
            {
                badContigs.insert( region.contig() );
            }
            else
            {
                goodRegions.emplace_back( region );
            }
        }

        if ( not badContigs.empty() )
        {
            WECALL_LOG( WARNING, "Contig(s) " + boost::algorithm::join( badContigs, "," ) +
                                      " are not contained in reference." );
        }

        return goodRegions;
    }

    regions_t DataRegionsBuilder::getRegions() const
    {
        regions_t regions;
        if ( m_inputRegionStrings.empty() )
        {
            regions = this->getDefaultRegions();
        }
        else
        {
            std::vector< bool > areStringsBedFiles;
            std::transform( m_inputRegionStrings.begin(), m_inputRegionStrings.end(),
                            std::back_inserter( areStringsBedFiles ), this->isBedFile );

            using wecall::utils::functional::identity;

            if ( std::all_of(areStringsBedFiles.begin(), areStringsBedFiles.end(), identity< bool >))
            {
                regions = this->getRegionsFromBedFiles();
            }
            else if ( std::none_of(areStringsBedFiles.begin(), areStringsBedFiles.end(), identity< bool >))
            {
                regions = this->getRegionsFromStrings();
            }
            else
            {
                throw utils::wecall_exception( "Can not have mixture of BED files and region strings" );
            }
        }
        return regions;
    }

    regions_t DataRegionsBuilder::getDefaultRegions() const
    {
        regions_t regions;
        const auto contigs = m_fastaIndexFile.contigs();

        for ( const auto & contigName : m_fastaIndexFile.standardContigs() )
        {
            regions.emplace_back( contigName, contigs.at( contigName ) );
        }

        WECALL_ASSERT( std::is_sorted( regions.begin(), regions.end(), RegionComp( m_fastaIndexFile ) ),
                        "standard contigs in FastaFile not sorted" );

        return regions;
    }

    regions_t DataRegionsBuilder::getRegionsFromBedFiles() const
    {
        regions_t regions;
        for ( const auto & inputRegionString : m_inputRegionStrings )
        {
            auto newRegions = io::BedFile( inputRegionString ).getRegions();
            regions.insert( regions.end(), newRegions.begin(), newRegions.end() );
        }
        return regions;
    }

    regions_t DataRegionsBuilder::getRegionsFromStrings() const
    {
        regions_t regions;
        const auto contigs = m_fastaIndexFile.contigs();
        for ( auto regionString : m_inputRegionStrings )
        {
            regions.push_back( parseRegionString( regionString, contigs ) );
        }

        return regions;
    }

    bool DataRegionsBuilder::isBedFile( std::string inputString )
    {
        auto possPath = boost::filesystem::path( inputString );
        bool isBed = possPath.extension().string() == ".bed";
        bool isZippedBed = possPath.extension().string() == ".gz" and possPath.stem().extension().string() == ".bed";
        return isBed or isZippedBed;
    }

    std::vector< std::string > getRegionStrings( const std::vector< Region > & regions )
    {
        std::vector< std::string > regionStrings;
        for ( const auto & region : regions )
        {
            regionStrings.emplace_back( region.toString() );
        }
        return regionStrings;
    }

    std::vector< Region > getContigsFromRegions(
        const std::vector< Region > & regions,
        const std::map< std::string, utils::Interval > & contigsFromFastaIndexFile )
    {
        std::vector< Region > contigs;
        std::set< std::string > contigStrings;
        for ( const auto & region : regions )
        {
            if ( contigStrings.find( region.contig() ) == contigStrings.end() )
            {
                contigStrings.insert( region.contig() );
                contigs.emplace_back( region.contig(), contigsFromFastaIndexFile.at( region.contig() ) );
            }
        }
        return contigs;
    }

    boost::regex chromOnly( "^" + chromMatch + "$" );
    boost::regex fullRegion( "^" + chromMatch + ":" + posMatch + "-" + posMatch + "$" );
    Region parseRegionString( std::string regionString, const std::map< std::string, utils::Interval > & contigs )
    {
        boost::cmatch what;
        if ( boost::regex_match( regionString.c_str(), what, chromOnly ) )
        {
            std::string contig = what[1].str();

            // No interval provided, so find it in the reference
            auto it = contigs.find( contig );
            if ( it != contigs.end() )
            {
                return Region( what[1].str(), it->second );
            }
            else
            {
                return Region( what[1].str(), 0, 0 );
            }
        }
        else if ( boost::regex_match( regionString.c_str(), what, fullRegion ) )
        {
            return Region( what[1].str(), utils::Interval( boost::lexical_cast< int64_t >( what[2] ),
                                                           boost::lexical_cast< int64_t >( what[3] ) ) );
        }
        else
        {
            throw utils::wecall_exception( "Invalid region specification format: " + regionString +
                                            ". Expecting: chrom or chrom:start-end" );
        }
    }

    std::vector< Region > mergeRegions( const std::vector< Region > & regions )
    {
        if ( regions.empty() )
        {
            return {};
        }

        const auto contig = regions.front().contig();
        const auto sameContig = [contig]( const Region & region )
        {
            return region.contig() == contig;
        };

        WECALL_ASSERT( std::all_of( regions.begin(), regions.end(), sameContig ),
                        "Can not merge regions on different contigs" );

        WECALL_ASSERT( std::is_sorted( regions.begin(), regions.end(), RegionIntervalComp() ),
                        "Merge Regions requires sorted regions" );

        std::vector< Region > output = {regions.front()};
        for ( const auto & region : regions )
        {
            if ( output.back().overlaps( region ) )
            {
                output.back().combine( region );
            }
            else if ( output.back().end() == region.start() )
            {
                output.back().combine( region );
            }
            else
            {
                output.push_back( region );
            }
        }

        return output;
    }

    regions_t padRegions( const regions_t & regions, const int64_t paddingAmount )
    {
        if ( regions.empty() )
        {
            return {};
        }
        regions_t paddedRegions;
        paddedRegions.reserve( regions.size() );
        const auto pad = [paddingAmount]( const Region & unpadded )
        {
            return unpadded.getPadded( paddingAmount );
        };

        std::transform( regions.begin(), regions.end(), std::back_inserter( paddedRegions ), pad );
        return mergeRegions( paddedRegions );
    }

    partitionedRegions_t padPartitionedRegions( const partitionedRegions_t & partitionedRegions,
                                                const int64_t paddingAmount )
    {
        if ( partitionedRegions.empty() )
        {
            return {};
        }
        partitionedRegions_t paddedPartitionedRegions;
        paddedPartitionedRegions.reserve( partitionedRegions.size() );

        const auto pad = [paddingAmount]( const regions_t & input )
        {
            return padRegions( input, paddingAmount );
        };

        std::transform( partitionedRegions.begin(), partitionedRegions.end(),
                        std::back_inserter( paddedPartitionedRegions ), pad );
        return paddedPartitionedRegions;
    }

    bool RegionComp::operator()( const Region & a, const Region & b )
    {
        if ( m_fastaIndexFile.contigStart( a.contig() ) == m_fastaIndexFile.contigStart( b.contig() ) )
        {
            return RegionIntervalComp()( a, b );
        }

        return m_fastaIndexFile.contigStart( a.contig() ) < m_fastaIndexFile.contigStart( b.contig() );
    }
}
}

max_stars_repo_name: dylex/wecall

max_issues_repo_name: dylex/wecall

max_forks_repo_name: dylex/wecall


text:
//------------------------------------------------------------------------------
// @file   : main.cpp
// @brief  :
// @author : Kazuto Nakashima
//------------------------------------------------------------------------------
#include "main.h"

#include "opencv_lib.h"
#include "vector_encoding.h"
#include "libsvm.h"

#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/predicate.hpp>

namespace fs = boost::filesystem;

int main(int argc, const char * argv[]) {
    //----------------------------------------------------------------------------------
    // init
    //----------------------------------------------------------------------------------
    std::ofstream log("C:\\Users\\kazut_000\\Desktop\\data\\log\\classification.log");

    std::vector<Video> video_list;

    int num_data = 1;
    cv::Mat hog_mat = cv::Mat_<float>(num_data, 72);
    cv::Mat hof_mat = cv::Mat_<float>(num_data, 90);
    cv::Mat hoghof_mat = cv::Mat_<float>(num_data, 162);

    //-----------------------------------------------------------------------------------
    // STIP
    //-----------------------------------------------------------------------------------
    fs::path input_path, output_path;
    fs::path video("test.avi");

    input_path = TEST_DIR / video;
    output_path = input_path;
    output_path.replace_extension("txt");

    std::cout << "Test video:  " << input_path << std::endl;
    std::cout << "STIP output: " << output_path << std::endl;

#if 1
    // STIP detector and deescriptor
    std::string stip_det = STIPDET;
    stip_det = stip_det + " -f " + input_path.string();
    stip_det = stip_det + " -o " + output_path.string();
    stip_det += VIS;
    system(stip_det.c_str());
#endif

    // File stream
    std::ifstream ifs(output_path.c_str());
    if (!ifs) { return -1; }

    std::string buf;
    int count[2];
    float tmp;
    size_t p;

    while (ifs && getline(ifs, buf)) {
        if (buf.substr(0, 1) != "#") {
            count[0] = 7;  // info of the features
            count[1] = 72; // the dimension of hog

            // point-type x y t sigma2 tau2 detector-confidence
            while (((p = buf.find('\t')) != buf.npos) && count[0]) {
                buf = buf.substr(p + 1);
                count[0]--;
            }

            // dscr-hog(72)
            count[0] = 0;
            while (((p = buf.find('\t')) != buf.npos) && count[1]) {
                tmp = std::stof(buf.substr(0, p + 1));
                hog_mat.at<float>(num_data - 1, count[0]) = tmp;
                hoghof_mat.at<float>(num_data - 1, count[0]++) = tmp;
                buf = buf.substr(p + 1);
                count[1]--;
            }

            // dscr-hof(90)
            count[1] = count[0];
            count[0] = 0;
            while ((p = buf.find('\t')) != buf.npos) {
                tmp = std::stof(buf.substr(0, p + 1));
                hof_mat.at<float>(num_data - 1, count[0]++) = tmp;
                hoghof_mat.at<float>(num_data - 1, count[1]++) = tmp;
                buf = buf.substr(p + 1);
            }
            hog_mat.resize(++num_data);
            hof_mat.resize(num_data);
            hoghof_mat.resize(num_data);
        }
    }
    hog_mat.resize(--num_data);
    hof_mat.resize(num_data);
    hoghof_mat.resize(num_data);

    video_list.push_back(Video(num_data - 1, -1));

    std::cout << "<< STIP output >>" << std::endl;
    std::cout << "num_data: " << num_data << std::endl;
    std::cout << "hog_mat.cols: " << hog_mat.cols << std::endl;
    std::cout << "hof_mat.cols: " << hof_mat.cols << std::endl;
    std::cout << "hoghof_mat.cols: " << hoghof_mat.cols << std::endl;

    //-----------------------------------------------------------------------------------
    // PCA (Principal component analysis)
    //-----------------------------------------------------------------------------------
    cv::PCA pca;

    // Load the parameters for the principal components
    LoadPCA(pca.eigenvectors, pca.eigenvalues, pca.mean, PCA_DIR);

    // demension-compressed data
    cv::Mat comp_mat = cv::Mat_<float>(num_data, pca.eigenvalues.rows);
    pca.project(FEATURES, comp_mat);

    std::cout << "<< PCA >>" << std::endl;
    std::cout << "comp_mat.rows: " << comp_mat.rows << std::endl;
    std::cout << "comp_mat.cols: " << comp_mat.cols << std::endl;

    for (auto itr = video_list.begin(); itr != video_list.end(); itr++)
        std::cout << "index: " << itr->GetIdx() << "\tlabel: " << itr->GetLabel() << std::endl;

#if 1
    //----------------------------------------------------------------------------------
    // Fisher Vector encoding
    //----------------------------------------------------------------------------------
    FisherVector fisher(GMM_DIR);

    std::cout << "<< Fisher Vector >>" << std::endl;
    cv::Mat fisher_mat = GetFisherMat(fisher, comp_mat, video_list);

    cv::Mat svm_mat = fisher_mat;
#else
    //----------------------------------------------------------------------------------
    // VLAD encoding
    //----------------------------------------------------------------------------------
    VLAD vlad(KMEANS_DIR);

    std::cout << "<< VLAD >>" << std::endl;
    cv::Mat vlad_mat = GetVladMat(vlad, comp_mat, video_list);

    cv::Mat svm_mat = vlad_mat;
#endif

    //----------------------------------------------------------------------------------
    // SVM prediction
    //----------------------------------------------------------------------------------
    struct svm_model* model;    // The model for SVM

    model = svm_load_model(SVM_DIR);

    // store the test data
    svm_node* x_node = new svm_node[svm_mat.cols + 1]; // "+1" means the malloc for the end
    int idx = 0;
    for (int i = 0; i < svm_mat.cols; i++) {
        x_node[idx].index = i + 1;
        x_node[idx].value = svm_mat.at<float>(0, i); // from the histogram
        idx++;
    }
    x_node[idx].index = -1; // the end of a vector

    // probability estimation
    int num_class = svm_get_nr_class(model);
    double *pre_prob = new double[num_class];
    double predict_label = svm_predict_probability(model, x_node, pre_prob);

    std::cout << "The predicted result: " << RestoreLabel((int)predict_label) << std::endl;
    for (int i = 0; i < num_class; i++) {
        std::cout << "Class " << i << ": " << pre_prob[i] << std::endl;
    }

    svm_free_and_destroy_model(&model);
    delete[] x_node;

    std::cout << "hit any key" << std::endl;
    getchar();
    return 0;
}

//----------------------------------------------------------------------------------
// time measurement
//----------------------------------------------------------------------------------
std::chrono::system_clock::time_point GetTime() {
    return std::chrono::system_clock::now();
}

long long GetTime(std::chrono::system_clock::time_point start,
    std::chrono::system_clock::time_point end) {
    return std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
}

//----------------------------------------------------------------------------------
// Support Vector Machine
//----------------------------------------------------------------------------------
namespace SVM {
    void SetParams(svm_parameter *param) {
        param->svm_type = C_SVC;
        param->kernel_type = LINEAR;
        param->gamma = 2;
        param->C = 1000;
        param->degree = 2;
        param->coef0 = 0.8;
        param->nu = 1000;
        param->cache_size = 10;
        param->eps = 1e-3;
        param->p = 0.1;
        param->shrinking = 1;
        param->probability = 1;
        param->nr_weight = 0;
        param->weight_label = NULL;
        param->weight = NULL;
    }

    void StoreTrainData(svm_problem *prob, cv::Mat& train_data,
        std::vector<Video> video_list) {
        int num_dimension = train_data.cols;
        int idx;

        svm_node **x_space = new svm_node*[prob->l];
        for (int i = 0; i < prob->l; i++) {
            x_space[i] = new svm_node[num_dimension + 1];
            idx = 0;
            prob->y[i] = video_list[i].GetLabel();
            for (int j = 0; j < num_dimension; j++) {
                x_space[i][idx].index = j + 1;
                x_space[i][idx].value = train_data.at<float>(i, j);
                idx++;
            }
            x_space[i][idx].index = -1; // the end of a vector
            prob->x[i] = x_space[i];
        }
    }
}

//------------------------------------------------------------------------------
// 0: eat a meal
// 1: gaze at a robot
// 2: gaze at a tree
// 3: look around
// 4: read a book
//------------------------------------------------------------------------------
std::string AssignLabel(std::string str) {
    if (!str.compare("eat_a_meal")) return "0";
    else if (!str.compare("gaze_at_a_robot")) return "1";
    else if (!str.compare("gaze_at_a_tree")) return "2";
    else if (!str.compare("look_around")) return "3";
    else if (!str.compare("read_a_book")) return "4";
    else return "-1";
}

std::string RestoreLabel(int label) {
    switch (label) {
    case 0: return "eat_a_meal";
    case 1: return "gaze_at_a_robot";
    case 2: return "gaze_at_a_tree";
    case 3: return "look_around";
    case 4: return "read_a_book";
    default: return "-1";
    }
}

cv::Mat GetFisherMat(FisherVector& fisherVec, cv::Mat& comp_mat, std::vector<Video> video_list) {
    cv::Mat fisherMat = cv::Mat_<float>(video_list.size(), fisherVec.GetDimension());

    int idx = 0, j = 0;
    for (auto itr = video_list.begin(); itr != video_list.end(); itr++) {
        cv::Mat dataToEncode = comp_mat.rowRange(cv::Range(idx, itr->GetIdx() + 1));
        fisherVec.FvEncode(dataToEncode).copyTo(fisherMat.row(j++));
        idx = itr->GetIdx() + 1;
    }

    return fisherMat;
}

cv::Mat GetVladMat(VLAD& vlad, cv::Mat& comp_mat, std::vector<Video> video_list) {
    cv::Mat vladMat = cv::Mat_<float>(video_list.size(), vlad.GetDimension());

    int idx = 0, j = 0;
    for (auto itr = video_list.begin(); itr != video_list.end(); itr++) {
        cv::Mat dataToEncode = comp_mat.rowRange(cv::Range(idx, itr->GetIdx() + 1));
        vlad.KmeansQuantize(dataToEncode);
        vlad.VladEncode(dataToEncode).copyTo(vladMat.row(j++));
        idx = itr->GetIdx() + 1;
    }

    return vladMat;
}

void SavePCA(cv::PCA& pca, const char* file_dir) {
    cv::Mat eigenvectors = pca.eigenvectors;
    cv::Mat eigenvalues = pca.eigenvalues;
    cv::Mat mean = pca.mean;

    std::ofstream ofs(file_dir, std::ios_base::out | std::ios_base::binary);

    int type = eigenvectors.type();
    ofs.write((char*)&eigenvectors.rows, sizeof(int));
    ofs.write((char*)&eigenvectors.cols, sizeof(int));
    ofs.write((char*)&type, sizeof(int));
    ofs.write((char*)eigenvectors.data, eigenvectors.elemSize() * eigenvectors.total());

    type = eigenvalues.type();
    ofs.write((char*)&eigenvalues.rows, sizeof(int));
    ofs.write((char*)&eigenvalues.cols, sizeof(int));
    ofs.write((char*)&type, sizeof(int));
    ofs.write((char*)eigenvalues.data, eigenvalues.elemSize() * eigenvalues.total());

    type = mean.type();
    ofs.write((char*)&mean.rows, sizeof(int));
    ofs.write((char*)&mean.cols, sizeof(int));
    ofs.write((char*)&type, sizeof(int));
    ofs.write((char*)mean.data, mean.elemSize() * mean.total());

    ofs.close();
}

void LoadPCA(cv::Mat& eigenvectors, cv::Mat& eigenvalues, cv::Mat& mean, const char* file_dir) {
    std::ifstream ifs(file_dir, std::ios_base::in | std::ios_base::binary);

    int rows, cols, type;
    ifs.read((char*)&rows, sizeof(int));
    ifs.read((char*)&cols, sizeof(int));
    ifs.read((char*)&type, sizeof(int));
    eigenvectors.release();
    eigenvectors.create(rows, cols, type);
    ifs.read((char*)eigenvectors.data, eigenvectors.elemSize() * eigenvectors.total());

    ifs.read((char*)&rows, sizeof(int));
    ifs.read((char*)&cols, sizeof(int));
    ifs.read((char*)&type, sizeof(int));
    eigenvalues.release();
    eigenvalues.create(rows, cols, type);
    ifs.read((char*)eigenvalues.data, eigenvalues.elemSize() * eigenvalues.total());

    ifs.read((char*)&rows, sizeof(int));
    ifs.read((char*)&cols, sizeof(int));
    ifs.read((char*)&type, sizeof(int));
    mean.release();
    mean.create(rows, cols, type);
    ifs.read((char*)mean.data, mean.elemSize() * mean.total());

    ifs.close();
}


text:
//============================================================================
// Name        : kinova_arm.cpp
// Author      : WPI, Clearpath Robotics
// Version     : 0.5
// Copyright   : BSD
// Description : A ROS driver for controlling the Kinova Kinova robotic manipulator arm
//============================================================================

#include "kinova_driver/kinova_arm.h"
#include <string>
#include <boost/lexical_cast.hpp>
#include <kinova_driver/kinova_ros_types.h>

namespace
{
    /// \brief Convert Kinova-specific angle degree variations (0..180, 360-181) to
    ///        a more regular representation (0..180, -180..0).
    inline void convertKinDeg(double& qd)
    {
        static const double PI_180 = (M_PI / 180.0);
        // Angle velocities from the API are 0..180 for positive values,
        // and 360..181 for negative ones, in a kind of 2-complement setup.
        if (qd > 180.0) {
            qd -= 360.0;
        }
        qd *= PI_180;
    }

    inline void convertKinDeg(std::vector<double>& qds)
    {
        for (int i = 0; i < qds.size(); ++i) {
            double& qd = qds[i];
            convertKinDeg(qd);
        }
    }

    inline void convertKinDeg(geometry_msgs::Vector3& qds)
    {
        convertKinDeg(qds.x);
        convertKinDeg(qds.y);
        convertKinDeg(qds.z);
    }
}

namespace kinova
{

KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const std::string &kinova_robotType, const std::string &kinova_robotName)
    : kinova_comm_(arm), node_handle_(nodeHandle), kinova_robotType_(kinova_robotType), kinova_robotName_(kinova_robotName)
{
    for (int i=0;i<COMMAND_SIZE;i++)
    {
      l_joint_torque_[i] = 0;
      l_force_cmd_[i] = 0;
    }

    //multiple arms
    if (node_handle_.hasParam("/kinova_robots"))
    {
        XmlRpc::XmlRpcValue robot_list;
        node_handle_.getParam("/kinova_robots", robot_list);
        if (robot_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
        {
          ROS_ERROR("Parameter controller_list should be specified as an array");
          return;
        }
        robots_.resize(robot_list.size());
        for (int i = 0; i < robot_list.size(); ++i)
        {
          if (!robot_list[i].hasMember("name") || !robot_list[i].hasMember("serial"))
          {
            ROS_ERROR_STREAM("Name and serial must be specifed for each robot");
            continue;
          }

          robots_[i].name = std::string(robot_list[i]["name"]);
          robots_[i].name = std::string(robot_list[i]["type"]);
          robots_[i].name = std::string(robot_list[i]["serial"]);
        }
    }

    /* Set up parameters for different robot type */
    // example for a kinova_robotType: j2n6s300

    if (valid_kinovaRobotType(kinova_robotType_) == false)
    {
        ROS_WARN("Invalid kinova_robotType error! Obtained: %s.", kinova_robotType_.c_str());
        return;
    }

//    tf_prefix_ = kinova_robotType_ + "_" + boost::lexical_cast<string>(same_type_index); // in case of multiple same_type robots
    tf_prefix_ = kinova_robotName_ + "_";

    // Maximum number of joints on Kinova-like robots:
    robot_category_ = kinova_robotType_[0];
    robot_category_version_ = kinova_robotType_[1]-'0';
    wrist_type_ = kinova_robotType_[2];
    arm_joint_number_ = kinova_robotType_[3]-'0';
    robot_mode_ = kinova_robotType_[4];
    finger_number_ = kinova_robotType_[5]-'0';

    if (kinova_robotType_ == "j2n6s300")
      finger_number_ = 0;

    joint_total_number_ = arm_joint_number_ + finger_number_;

    if (robot_category_=='j') // jaco robot
    {
        // special parameters for jaco
        if (arm_joint_number_ == 6)
        {
            if (wrist_type_ == 'n')
                robot_type_ = JACOV2_6DOF_SERVICE;
            else
                robot_type_ = SPHERICAL_6DOF_SERVICE;
        }
        else if (arm_joint_number_ == 4)
        {
            robot_type_ = JACOV2_4DOF_SERVICE;
        }
        else if (arm_joint_number_ == 7)
        {
            robot_type_ = SPHERICAL_7DOF_SERVICE;
        }
    }
    else if (robot_category_ == 'm') // mico robot
    {
        // special parameters for mico
        if (arm_joint_number_ == 6)
            robot_type_ = MICO_6DOF_SERVICE;
        else if (arm_joint_number_ == 4)
            robot_type_ = MICO_4DOF_SERVICE;
    }
    else if (robot_category_ == 'r') // roco robot
    {
        // special parameters for roco
    }
    else
    {
        // special parameters for custom robot or other cases
    }

    if (finger_number_==3)
    {
        // finger_angle_conv_ratio used to display finger position properly in Rviz
        // Approximative conversion ratio from finger position (0..6400) to joint angle
        // in radians (0.. 1.4) for 3 finger hand
        finger_conv_ratio_= 1.4 / 6400.0;
    }
    else if(finger_number_==2)
    {
        // the two finger hand may different
        finger_conv_ratio_= 1.4 / 6400.0;
    }
    else
    {
        // no fingers
    }

    for (int i = 0; i<arm_joint_number_; i++)
    {
        joint_names_.resize(joint_total_number_);
        joint_names_[i] = tf_prefix_ + "joint_" + boost::lexical_cast<std::string>(i+1);
    }
    for (int i = 0; i<finger_number_; i++)
    {
        joint_names_[arm_joint_number_+i] = tf_prefix_ + "joint_finger_" + boost::lexical_cast<std::string>(i+1);
    }

    /* Set up Services */
    stop_service_ = node_handle_.advertiseService("in/stop", &KinovaArm::stopServiceCallback, this);
    start_service_ = node_handle_.advertiseService("in/start", &KinovaArm::startServiceCallback, this);
    homing_service_ = node_handle_.advertiseService("in/home_arm", &KinovaArm::homeArmServiceCallback, this);
    add_trajectory_ = node_handle_.advertiseService("in/add_pose_to_Cartesian_trajectory",
                      &KinovaArm::addCartesianPoseToTrajectory, this);
    clear_trajectories_ = node_handle_.advertiseService("in/clear_trajectories",
                          &KinovaArm::clearTrajectoriesServiceCallback, this);

    set_force_control_params_service_ = node_handle_.advertiseService("in/set_force_control_params", &KinovaArm::setForceControlParamsCallback, this);
    start_force_control_service_ = node_handle_.advertiseService("in/start_force_control", &KinovaArm::startForceControlCallback, this);
    stop_force_control_service_ = node_handle_.advertiseService("in/stop_force_control", &KinovaArm::stopForceControlCallback, this);
    set_actuator_torques_to_zero_ = node_handle_.advertiseService(
                "in/set_zero_torques", &KinovaArm::setJointTorquesToZeroService, this);
    run_COM_parameter_estimation_service_ = node_handle_.advertiseService(
                "in/run_COM_parameters_estimation",
                &KinovaArm::runCOMParameterEstimationService,this);

    set_end_effector_offset_service_ = node_handle_.advertiseService("in/set_end_effector_offset",
        &KinovaArm::setEndEffectorOffsetCallback, this);

    start_null_space_service_ = node_handle_.advertiseService("in/set_null_space_mode_state", &KinovaArm::ActivateNullSpaceModeCallback, this);
    set_torque_control_mode_service_ = node_handle_.advertiseService("in/set_torque_control_mode", &KinovaArm::setTorqueControlModeService, this);
    set_torque_control_parameters_service_ = node_handle_.advertiseService
            ("in/set_torque_control_parameters",
             &KinovaArm::setTorqueControlParametersService,this);

    /* Set up Publishers */
    joint_angles_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>
            ("out/joint_angles", 2);
    joint_torque_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>
            ("out/joint_torques", 2);
    joint_state_publisher_ = node_handle_.advertise<sensor_msgs::JointState>
            ("out/joint_state", 2);
    tool_position_publisher_ = node_handle_.advertise<geometry_msgs::PoseStamped>
            ("out/tool_pose", 2);
    tool_wrench_publisher_ = node_handle_.advertise<geometry_msgs::WrenchStamped>
            ("out/tool_wrench", 2);
    finger_position_publisher_ = node_handle_.advertise<kinova_msgs::FingerPosition>
            ("out/finger_position", 2);

    // Publish last command for relative motion (read current position cause arm drop)
    joint_command_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>("out/joint_command", 2);
    cartesian_command_publisher_ = node_handle_.advertise<kinova_msgs::KinovaPose>("out/cartesian_command", 2);

    /* Set up Subscribers*/
    joint_velocity_subscriber_ = node_handle_.subscribe("in/joint_velocity", 1,
                                 &KinovaArm::jointVelocityCallback, this);
    cartesian_velocity_subscriber_ = node_handle_.subscribe("in/cartesian_velocity", 1,
                                     &KinovaArm::cartesianVelocityCallback, this);
    joint_torque_subscriber_ = node_handle_.subscribe("in/joint_torque", 1,
                               &KinovaArm::jointTorqueSubscriberCallback, this);
    cartesian_force_subscriber_ = node_handle_.subscribe("in/cartesian_force", 1,
                                  &KinovaArm::forceSubscriberCallback, this);

    node_handle_.param<double>("status_interval_seconds", status_interval_seconds_, 0.1);

    // Depending on the API version, the arm might return velocities in the
    // 0..360 range (0..180 for positive values, 181..360 for negative ones).
    // This indicates that the ROS node should convert them first before
    // updating the joint_state topic.
    node_handle_.param("convert_joint_velocities", convert_joint_velocities_, true);

    status_timer_ = node_handle_.createTimer(ros::Duration(status_interval_seconds_),
                                           &KinovaArm::statusTimer, this);

    ROS_INFO("The arm is ready to use.");
}

KinovaArm::~KinovaArm()
{
}

bool KinovaArm::homeArmServiceCallback(kinova_msgs::HomeArm::Request &req, kinova_msgs::HomeArm::Response &res)
{
    kinova_comm_.homeArm();
    kinova_comm_.initFingers();
    res.homearm_result = "KINOVA ARM HAS BEEN RETURNED HOME";
    return true;
}

bool KinovaArm::ActivateNullSpaceModeCallback(kinova_msgs::SetNullSpaceModeState::Request &req, kinova_msgs::SetNullSpaceModeState::Response &res)
{
    kinova_comm_.SetRedundantJointNullSpaceMotion(req.state);
}

bool KinovaArm::setTorqueControlModeService(kinova_msgs::SetTorqueControlMode::Request &req, kinova_msgs::SetTorqueControlMode::Response &res)
{
    kinova_comm_.SetTorqueControlState(req.state);
}

bool KinovaArm::setTorqueControlParametersService(kinova_msgs::SetTorqueControlParameters::Request &req, kinova_msgs::SetTorqueControlParameters::Response &res)
{
    float safetyFactor;
    node_handle_.param<float>("torque_parameters/safety_factor", safetyFactor,1.0);
    kinova_comm_.setToquesControlSafetyFactor(safetyFactor);

    std::vector<float> payload;
    if (node_handle_.getParam("payload", payload))
    {
        kinova_comm_.setPayload(payload);
    }

    std::vector<float> min_torque, max_torque;
    if (node_handle_.getParam("torque_parameters/torque_min", min_torque)
          && node_handle_.getParam("torque_parameters/torque_max", max_torque))
    {
        AngularInfo min_torque_info,max_torque_info;

        //since fist 7 members of the struct are float we assume no padding
        //and use float pointer to access struct elements
        float *min_torque_actuator = &(min_torque_info.Actuator1);
        float *max_torque_actuator = &(max_torque_info.Actuator1);
        for (int i = 0; i<min_torque.size(); i++)
        {
            min_torque_actuator[i] = min_torque.at(i);
            max_torque_actuator[i] = max_torque.at(i);
        }
        kinova_comm_.setJointTorqueMinMax(min_torque_info,max_torque_info);
    }

    std::vector<float> com_parameters;
    if (node_handle_.getParam("torque_parameters/com_parameters", com_parameters))
    {
        bool use_estimated_COM;
        node_handle_.param("torque_parameters/use_estimated_COM_parameters",
                              use_estimated_COM,true);
        if (use_estimated_COM == true)
            kinova_comm_.setRobotCOMParam(OPTIMAL,com_parameters);
        else
            kinova_comm_.setRobotCOMParam(MANUAL_INPUT,com_parameters);

    }
}

void KinovaArm::jointVelocityCallback(const kinova_msgs::JointVelocityConstPtr& joint_vel)
{
    if (!kinova_comm_.isStopped())
    {
        joint_velocities_.Actuator1 = joint_vel->joint1;
        joint_velocities_.Actuator2 = joint_vel->joint2;
        joint_velocities_.Actuator3 = joint_vel->joint3;
        joint_velocities_.Actuator4 = joint_vel->joint4;
        joint_velocities_.Actuator5 = joint_vel->joint5;
        joint_velocities_.Actuator6 = joint_vel->joint6;
        joint_velocities_.Actuator7 = joint_vel->joint7;

        kinova_comm_.setJointVelocities(joint_velocities_);
    }
}

void KinovaArm::jointTorqueSubscriberCallback(const kinova_msgs::JointTorqueConstPtr& joint_torque)
{
    if (!kinova_comm_.isStopped())
    {
        l_joint_torque_[0] = joint_torque->joint1;
        l_joint_torque_[1] = joint_torque->joint2;
        l_joint_torque_[2] = joint_torque->joint3;
        l_joint_torque_[3] = joint_torque->joint4;
        l_joint_torque_[4] = joint_torque->joint5;
        l_joint_torque_[5] = joint_torque->joint6;
        l_joint_torque_[6] = joint_torque->joint7;

        kinova_comm_.setJointTorques(l_joint_torque_);

    }
}

void KinovaArm::forceSubscriberCallback(const kinova_msgs::CartesianForceConstPtr& force)
{
    if (!kinova_comm_.isStopped())
    {
        l_force_cmd_[0] = force->force_x;
        l_force_cmd_[1] = force->force_y;
        l_force_cmd_[2] = force->force_z;
        l_force_cmd_[3] = force->torque_x;
        l_force_cmd_[4] = force->torque_y;
        l_force_cmd_[5] = force->torque_z;

        kinova_comm_.sendCartesianForceCommand(l_force_cmd_);
    }
}

/*!
 * \brief Handler for "stop" service.
 *
 * Instantly stops the arm and prevents further movement until start service is
 * invoked.
 */
bool KinovaArm::stopServiceCallback(kinova_msgs::Stop::Request &req, kinova_msgs::Stop::Response &res)
{
    kinova_comm_.stopAPI();
    res.stop_result = "Arm stopped";
    ROS_DEBUG("Arm stop requested");
    return true;
}

/*!
 * \brief Handler for "start" service.
 *
 * Re-enables control of the arm after a stop.
 */
bool KinovaArm::startServiceCallback(kinova_msgs::Start::Request &req, kinova_msgs::Start::Response &res)
{
    kinova_comm_.startAPI();
    res.start_result = "Arm started";
    ROS_DEBUG("Arm start requested");
    return true;
}

bool KinovaArm::addCartesianPoseToTrajectory(kinova_msgs::AddPoseToCartesianTrajectory::Request &req,
                            kinova_msgs::AddPoseToCartesianTrajectory::Response &res)
{
    KinovaPose pose;
    pose.X = req.X;
    pose.Y = req.Y;
    pose.Z = req.Z;
    pose.ThetaX = req.ThetaX;
    pose.ThetaY = req.ThetaY;
    pose.ThetaZ = req.ThetaZ;
    kinova_comm_.setCartesianPosition(pose,false);
}

bool KinovaArm::clearTrajectoriesServiceCallback(
        kinova_msgs::ClearTrajectories::Request &req,
        kinova_msgs::ClearTrajectories::Response &res)
{
    kinova_comm_.eraseAllTrajectories();
}

bool KinovaArm::setForceControlParamsCallback(
        kinova_msgs::SetForceControlParams::Request &req,
        kinova_msgs::SetForceControlParams::Response &res)
{
    CartesianInfo inertia, damping, force_min, force_max;
    inertia.X      = req.inertia_linear.x;
    inertia.Y      = req.inertia_linear.y;
    inertia.Z      = req.inertia_linear.z;
    inertia.ThetaX = req.inertia_angular.x;
    inertia.ThetaY = req.inertia_angular.y;
    inertia.ThetaZ = req.inertia_angular.z;
    damping.X      = req.damping_linear.x;
    damping.Y      = req.damping_linear.y;
    damping.Z      = req.damping_linear.z;
    damping.ThetaX = req.damping_angular.x;
    damping.ThetaY = req.damping_angular.y;
    damping.ThetaZ = req.damping_angular.z;

    kinova_comm_.setCartesianInertiaDamping(inertia, damping);

    force_min.X      = req.force_min_linear.x;
    force_min.Y      = req.force_min_linear.y;
    force_min.Z      = req.force_min_linear.z;
    force_min.ThetaX = req.force_min_angular.x;
    force_min.ThetaY = req.force_min_angular.y;
    force_min.ThetaZ = req.force_min_angular.z;
    force_max.X      = req.force_max_linear.x;
    force_max.Y      = req.force_max_linear.y;
    force_max.Z      = req.force_max_linear.z;
    force_max.ThetaX = req.force_max_angular.x;
    force_max.ThetaY = req.force_max_angular.y;
    force_max.ThetaZ = req.force_max_angular.z;

    kinova_comm_.setCartesianForceMinMax(force_min, force_max);

    return true;
}

bool KinovaArm::startForceControlCallback(kinova_msgs::Start::Request &req, kinova_msgs::Start::Response &res)
{
    kinova_comm_.startForceControl();
    res.start_result = "Start force control requested.";
    return true;
}

bool KinovaArm::stopForceControlCallback(kinova_msgs::Stop::Request &req, kinova_msgs::Stop::Response &res)
{
    kinova_comm_.stopForceControl();
    res.stop_result = "Stop force control requested.";
    return true;
}

bool KinovaArm::setJointTorquesToZeroService(kinova_msgs::ZeroTorques::Request &req,
                                             kinova_msgs::ZeroTorques::Response &res)
{
    kinova_comm_.setZeroTorque();
    return true;
}

bool KinovaArm::runCOMParameterEstimationService(
        kinova_msgs::RunCOMParametersEstimation::Request &req,
        kinova_msgs::RunCOMParametersEstimation::Response &res)
{
    kinova_comm_.runCOMParameterEstimation(robot_type_);
}

bool KinovaArm::setEndEffectorOffsetCallback(kinova_msgs::SetEndEffectorOffset::Request &req, kinova_msgs::SetEndEffectorOffset::Response &res)
{
    kinova_comm_.setEndEffectorOffset(req.status, req.offset.x, req.offset.y, req.offset.z);

    return true;
}

void KinovaArm::cartesianVelocityCallback(const kinova_msgs::PoseVelocityConstPtr& cartesian_vel)
{
    if (!kinova_comm_.isStopped())
    {
        cartesian_velocities_.X = cartesian_vel->twist_linear_x;
        cartesian_velocities_.Y = cartesian_vel->twist_linear_y;
        cartesian_velocities_.Z = cartesian_vel->twist_linear_z;
        cartesian_velocities_.ThetaX = cartesian_vel->twist_angular_x;
        cartesian_velocities_.ThetaY = cartesian_vel->twist_angular_y;
        cartesian_velocities_.ThetaZ = cartesian_vel->twist_angular_z;

        // orientation velocity of cartesian_velocities_ is based on twist.angular
        kinova_comm_.setCartesianVelocities(cartesian_velocities_);
    }
}

/*!
 * \brief Publishes the current joint angles.
 *
 * Joint angles are published in both their raw state as obtained from the arm
 * (JointAngles), and transformed & converted to radians (joint_state) as per
 * the Kinova Kinematics PDF.
 *
 * Velocities and torques (effort) are only published in the JointStates
 * message, only for the first 6 joints as these values are not available for
 * the fingers.
 */
void KinovaArm::publishJointAngles(void)
{

    FingerAngles fingers;
    kinova_comm_.getFingerPositions(fingers);

    if (arm_joint_number_ != 4 && arm_joint_number_ != 6 && arm_joint_number_ != 7)
    {
         ROS_WARN_ONCE("The joint_state publisher only supports 4, 6 and 7 DOF for now.: %d", arm_joint_number_);
    }

    // Query arm for current joint angles
    KinovaAngles current_angles;
    kinova_comm_.getJointAngles(current_angles);
    kinova_msgs::JointAngles kinova_angles = current_angles.constructAnglesMsg();

    AngularPosition joint_command;
    kinova_comm_.getAngularCommand(joint_command);
    kinova_msgs::JointAngles joint_command_msg = KinovaAngles(joint_command.Actuators).constructAnglesMsg();

    sensor_msgs::JointState joint_state;
    joint_state.name = joint_names_;
    joint_state.header.stamp = ros::Time::now();

    // Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
    joint_state.position.resize(joint_total_number_);
    joint_state.position[0] = kinova_angles.joint1 * M_PI/180;
    joint_state.position[1] = kinova_angles.joint2 * M_PI/180;
    joint_state.position[2] = kinova_angles.joint3 * M_PI/180;
    joint_state.position[3] = kinova_angles.joint4 * M_PI/180;
    if (arm_joint_number_ >= 6)
    {
        joint_state.position[4] = kinova_angles.joint5 * M_PI/180;
        joint_state.position[5] = kinova_angles.joint6 * M_PI/180;
    }
    if (arm_joint_number_ == 7)
    {
         joint_state.position[6] = kinova_angles.joint7 * M_PI/180;
    }

    if(finger_number_==2)
    {
        joint_state.position[joint_total_number_-2] = fingers.Finger1/6800*80*M_PI/180;
        joint_state.position[joint_total_number_-1] = fingers.Finger2/6800*80*M_PI/180;
    }
    else if(finger_number_==3)
    {
        joint_state.position[joint_total_number_-3] = fingers.Finger1/6800*80*M_PI/180;
        joint_state.position[joint_total_number_-2] = fingers.Finger2/6800*80*M_PI/180;
        joint_state.position[joint_total_number_-1] = fingers.Finger3/6800*80*M_PI/180;
    }

    // Joint velocities
    KinovaAngles current_vels;
    kinova_comm_.getJointVelocities(current_vels);
    joint_state.velocity.resize(joint_total_number_);
    joint_state.velocity[0] = current_vels.Actuator1;
    joint_state.velocity[1] = current_vels.Actuator2;
    joint_state.velocity[2] = current_vels.Actuator3;
    joint_state.velocity[3] = current_vels.Actuator4;
    // no velocity info for fingers
    if(finger_number_==2)
    {
        joint_state.velocity[joint_total_number_-2] = 0;
        joint_state.velocity[joint_total_number_-1] = 0;
    }
    else if(finger_number_==3)
    {
        joint_state.velocity[joint_total_number_-3] = 0;
        joint_state.velocity[joint_total_number_-2] = 0;
        joint_state.velocity[joint_total_number_-1] = 0;
    }

    if (arm_joint_number_ >= 6)
    {
        joint_state.velocity[4] = current_vels.Actuator5;
        joint_state.velocity[5] = current_vels.Actuator6;
    }
    if (arm_joint_number_ == 7)
    {
        joint_state.velocity[6] = current_vels.Actuator7;
    }

//    ROS_DEBUG_THROTTLE(0.1,
//                       "Raw joint velocities: %f %f %f %f %f %f",
//                       current_vels.Actuator1,
//                       current_vels.Actuator2,
//                       current_vels.Actuator3,
//                       current_vels.Actuator4,
//                       current_vels.Actuator5,
//                       current_vels.Actuator6);

    if (convert_joint_velocities_) {
        convertKinDeg(joint_state.velocity);
    }

    // Joint torques (effort)
    KinovaAngles joint_tqs;
    bool gravity_comp;
    node_handle_.param("torque_parameters/publish_torque_with_gravity_compensation", gravity_comp, false);
    if (gravity_comp==true)
      kinova_comm_.getGravityCompensatedTorques(joint_tqs);
    else
      kinova_comm_.getJointTorques(joint_tqs);
    joint_torque_publisher_.publish(joint_tqs.constructAnglesMsg());

    joint_state.effort.resize(joint_total_number_);
    joint_state.effort[0] = joint_tqs.Actuator1;
    joint_state.effort[1] = joint_tqs.Actuator2;
    joint_state.effort[2] = joint_tqs.Actuator3;
    joint_state.effort[3] = joint_tqs.Actuator4;
    // no effort info for fingers
    if(finger_number_==2)
    {
        joint_state.effort[joint_total_number_-2] = 0;
        joint_state.effort[joint_total_number_-1] = 0;
    }
    else if(finger_number_==3)
    {
        joint_state.effort[joint_total_number_-3] = 0;
        joint_state.effort[joint_total_number_-2] = 0;
        joint_state.effort[joint_total_number_-1] = 0;
    }
    if (arm_joint_number_ >= 6)
    {
        joint_state.effort[4] = joint_tqs.Actuator5;
        joint_state.effort[5] = joint_tqs.Actuator6;
    }
    if (arm_joint_number_ == 7)
    {
        joint_state.effort[6] = joint_tqs.Actuator7;
    }

    joint_angles_publisher_.publish(kinova_angles);
    joint_command_publisher_.publish(joint_command_msg);
    joint_state_publisher_.publish(joint_state);

}

/*!
 * \brief Publishes the current cartesian coordinates
 */
void KinovaArm::publishToolPosition(void)
{
    KinovaPose pose;
    geometry_msgs::PoseStamped current_position;
    kinova_comm_.getCartesianPosition(pose);

    CartesianPosition cartesian_command;
    kinova_comm_.getCartesianCommand(cartesian_command);
    kinova_msgs::KinovaPose cartesian_command_msg = KinovaPose(cartesian_command.Coordinates).constructKinovaPoseMsg();

    current_position.pose            = pose.constructPoseMsg();
    current_position.header.stamp    = ros::Time::now();
    current_position.header.frame_id = tf_prefix_ + "link_base";

    tool_position_publisher_.publish(current_position);
    cartesian_command_publisher_.publish(cartesian_command_msg);
}

/*!
 * \brief Publishes the current cartesian forces at the end effector.
 */
void KinovaArm::publishToolWrench(void)
{
    KinovaPose wrench;
    geometry_msgs::WrenchStamped current_wrench;

    kinova_comm_.getCartesianForce(wrench);

    current_wrench.wrench          = wrench.constructWrenchMsg();
    current_wrench.header.stamp    = ros::Time::now();
    // TODO: Rotate wrench to fit the end effector frame.
    // Right now, the orientation of the wrench is in the API's (base) frame.
    current_wrench.header.frame_id = tf_prefix_ + "link_base";

    // Same conversion issue as with velocities:
    if (convert_joint_velocities_) {
        convertKinDeg(current_wrench.wrench.torque);
    }

    tool_wrench_publisher_.publish(current_wrench);
}

/*!
 * \brief Publishes the current finger positions.
 */
void KinovaArm::publishFingerPosition(void)
{
    FingerAngles fingers;
    kinova_comm_.getFingerPositions(fingers);
    finger_position_publisher_.publish(fingers.constructFingersMsg());
}

void KinovaArm::statusTimer(const ros::TimerEvent&)
{
    publishJointAngles();
    publishToolPosition();
    publishToolWrench();
    publishFingerPosition();
}

}  // namespace kinova

max_stars_repo_name: wvu-irl/kinova-ros

max_issues_repo_name: wvu-irl/kinova-ros

max_forks_repo_name: wvu-irl/kinova-ros


text:
//&>/dev/null;x="${0%.*}";[ ! "$x" -ot "$0" ]||(rm -f "$x";clang++ -lboost_date_time -o "$x" "$0")&&"$x" $@;rm -f "$x";exit

#include <iostream>
#include <locale>
#include <string>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/posix_time/posix_time_io.hpp>

int main()
{
  const boost::posix_time::ptime time =
      boost::posix_time::time_from_string("1981-08-20 08:05:00");

  // ptime to string.
  const std::string str_time = to_simple_string(time);
  std::cout << str_time << std::endl;

  // ptime to stringstream to string.
  std::stringstream stream;
  stream << time;
  std::cout << stream.str() << std::endl;
  stream.str("");

  // Use a facet to display time in a custom format (only hour and minutes).
  boost::posix_time::time_facet* facet = new boost::posix_time::time_facet();
  facet->format("%H:%M");
  stream.imbue(std::locale(std::locale::classic(), facet));
  stream << time;
  std::cout << stream.str() << std::endl;

  boost::posix_time::ptime discordTime = boost::posix_time::from_iso_extended_string("2017-07-23T06:52:57.784000+00:00");
}

ext: cc

max_stars_repo_name: Aidoboy/Tests

max_issues_repo_name: Aidoboy/Tests

max_forks_repo_name: Aidoboy/Tests