//
// Created by june on 19. 12. 5..
//

// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <urdf/model.h>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include "utils.h"
#include "dualarm_controller/TaskCurrentState.h"

//manipulability
#include <ellipsoid.h>
#include <similarity.h>

// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>                // inverse dynamics
#include <kdl/chainjnttojacsolver.hpp>          // jacobian
#include <kdl/chainjnttojacdotsolver.hpp>       // jcobian dot
#include <kdl/chainfksolverpos_recursive.hpp>   // forward kinematics

#include <boost/scoped_ptr.hpp>

#define _USE_MATH_DEFINES
#include <cmath>

#include <SerialManipulator.h>
#include <Controller.h>

#define D2R M_PI/180.0
#define R2D 180.0/M_PI

#define A 0.10
#define b1 0.45
#define b2 -0.30
#define b3 0.45
#define f 0.2

#define l_p1 0.40
#define l_p2 0.30
#define l_p3 0.39

#define Deg_A 70
#define Deg_f 0.5

namespace dualarm_controller
{
    class Taskspace_Control : public controller_interface::Controller<hardware_interface::EffortJointInterface>
    {
    public:
        bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
        {
            // ********* 1. Get joint name / gain from the parameter server *********
            // 1.0 Target Position objective
            if (!n.getParam("target_obj", target_obj))
            {
                ROS_ERROR("Could not find control objective");
                return false;
            }

            if (!n.getParam("ctrl_type", ctrl_type))
            {
                if(ctrl_type != 1 && ctrl_type != 2)
                {
                    ROS_ERROR("Could not find controller mode");
                    return false;
                }
            }

            // 1.1 Joint Name
            if (!n.getParam("joints", joint_names_))
            {
                ROS_ERROR("Could not find joint name");
                return false;
            }
            n_joints_ = joint_names_.size();

            if (n_joints_ == 0)
            {
                ROS_ERROR("List of joint names is empty.");
                return false;
            }
            else
            {
                ROS_INFO("Found %d joint names", n_joints_);
                for (int i = 0; i < n_joints_; i++)
                {
                    ROS_INFO("%s", joint_names_[i].c_str());
                }
            }

            // 1.2 Gain
            // 1.2.1 Task-space Controller

            if (!n.getParam("/dualarm/taskspace_control/gains/Regulation/pid/KpT", Kp_trans_R))
            {
                ROS_ERROR("Cannot find Right-arm pid/Kp Translation gain");
                return false;
            }

            if (!n.getParam("/dualarm/taskspace_control/gains/Regulation/pid/KpR", Kp_rot_R))
            {
                ROS_ERROR("Cannot find Right-arm pid/Kp Rotation gain");
                return false;
            }

            if (!n.getParam("/dualarm/taskspace_control/gains/Regulation/pid/KdT", Kd_trans_R))
            {
                ROS_ERROR("Cannot find Right-arm pid/Kd Translation gain");
                return false;
            }

            if (!n.getParam("/dualarm/taskspace_control/gains/Regulation/pid/KdR", Kd_rot_R))
            {
                ROS_ERROR("Cannot find Right-arm pid/Kd Rotation gain");
                return false;
            }

            if (!n.getParam("/dualarm/taskspace_control/gains/Tracking/pid/KpT", Kp_trans_L))
            {
                ROS_ERROR("Cannot find Left-arm pid/Kp Translation gain");
                return false;
            }

            if (!n.getParam("/dualarm/taskspace_control/gains/Tracking/pid/KpR", Kp_rot_L))
            {
                ROS_ERROR("Cannot find Left-arm pid/Kp Rotation gain");
                return false;
            }

            if (!n.getParam("/dualarm/taskspace_control/gains/Tracking/pid/KdT", Kd_trans_L))
            {
                ROS_ERROR("Cannot find Left-arm pid/Kd Translation gain");
                return false;
            }

            if (!n.getParam("/dualarm/taskspace_control/gains/Tracking/pid/KdR", Kd_rot_L))
            {
                ROS_ERROR("Cannot find Left-arm pid/Kd Rotation gain");
                return false;
            }

            // 2. ********* urdf *********
            urdf::Model urdf;
            if (!urdf.initParam("robot_description"))
            {
                ROS_ERROR("Failed to parse urdf file");
                return false;
            }
            else
            {
                ROS_INFO_STREAM("Found robot_description");
            }

            // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
            for (int i = 0; i < n_joints_; i++)
            {
                try
                {
                    joints_.push_back(hw->getHandle(joint_names_[i]));
                }
                catch (const hardware_interface::HardwareInterfaceException &e)
                {
                    ROS_ERROR_STREAM("Exception thrown: " << e.what());
                    return false;
                }

                urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
                if (!joint_urdf)
                {
                    ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                    return false;
                }
                joint_urdfs_.push_back(joint_urdf);
            }

            // 4. ********* KDL *********
            // 4.1 kdl parser
            if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
            {
                ROS_ERROR("Failed to construct kdl tree");
                return false;
            }

            // 4.2 kdl chain
            std::string root_name, tip_name1, tip_name2;
            if (!n.getParam("root_link", root_name))
            {
                ROS_ERROR("Could not find root link name");
                return false;
            }
            if (!n.getParam("tip_link1", tip_name1))
            {
                ROS_ERROR("Could not find tip link name");
                return false;
            }
            if (!n.getParam("tip_link2", tip_name2))
            {
                ROS_ERROR("Could not find tip link name");
                return false;
            }

            if (!kdl_tree_.getChain(root_name, tip_name1, kdl_chain_))
            {
                ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name1);
                ROS_ERROR_STREAM("  Chain has " << kdl_chain_.getNrOfJoints() << " joints");
                ROS_ERROR_STREAM("  Chain has " << kdl_chain_.getNrOfSegments() << " segments");
                ROS_ERROR_STREAM("  The segments are:");

                KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                KDL::SegmentMap::iterator it;

                for (it = segment_map.begin(); it != segment_map.end(); it++)
                    ROS_ERROR_STREAM("    " << (*it).first);

                return false;
            }
            else
            {
                ROS_INFO_STREAM("Got kdl first chain");
                ROS_INFO_STREAM("  " << root_name << " --> " << tip_name1);
                ROS_INFO_STREAM("  Chain has " << kdl_chain_.getNrOfJoints() << " joints");
                ROS_INFO_STREAM("  Chain has " << kdl_chain_.getNrOfSegments() << " segments");
            }

            if(!kdl_tree_.getChain(root_name, tip_name2, kdl_chain2_))
            {
                ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name2);
                ROS_ERROR_STREAM("  Chain has " << kdl_chain2_.getNrOfJoints() << " joints");
                ROS_ERROR_STREAM("  Chain has " << kdl_chain2_.getNrOfSegments() << " segments");
                ROS_ERROR_STREAM("  The segments are:");

                KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                KDL::SegmentMap::iterator it;

                for (it = segment_map.begin(); it != segment_map.end(); it++)
                    ROS_ERROR_STREAM("    " << (*it).first);

                return false;
            }
            else
            {
                ROS_INFO_STREAM("Got kdl second chain");
                ROS_INFO_STREAM("  " << root_name << " --> " << tip_name2);
                ROS_INFO_STREAM("  Chain has " << kdl_chain2_.getNrOfJoints() << " joints");
                ROS_INFO_STREAM("  Chain has " << kdl_chain2_.getNrOfSegments() << " segments");
            }

            // 4.3 inverse dynamics solver 초기화
            g_kdl_ = KDL::Vector::Zero();
            g_kdl_(2) = -9.81; // 0: x-axis 1: y-axis 2: z-axis

            id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, g_kdl_));
            jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
            jnt_to_jacdot_solver.reset(new KDL::ChainJntToJacDotSolver(kdl_chain_));
            fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
            J1_kdl_.resize(kdl_chain_.getNrOfJoints());
            J1dot.resize(kdl_chain_.getNrOfJoints());
            M_kdl_.resize(kdl_chain_.getNrOfJoints());
            C_kdl_.resize(kdl_chain_.getNrOfJoints());
            G_kdl_.resize(kdl_chain_.getNrOfJoints());

            id_solver1_.reset(new KDL::ChainDynParam(kdl_chain2_, g_kdl_));
            jnt_to_jac_solver1_.reset(new KDL::ChainJntToJacSolver(kdl_chain2_));
            jnt_to_jacdot_solver1.reset(new KDL::ChainJntToJacDotSolver(kdl_chain2_));
            fk_pos_solver1_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain2_));
            J2_kdl_.resize(kdl_chain2_.getNrOfJoints());
            J2dot.resize(kdl_chain2_.getNrOfJoints());
            M1_kdl_.resize(kdl_chain2_.getNrOfJoints());
            C1_kdl_.resize(kdl_chain2_.getNrOfJoints());
            G1_kdl_.resize(kdl_chain2_.getNrOfJoints());

            Jdot.setZero(12,16);

            q1_.resize(9);
            q1dot_.resize(9);
            q2_.resize(9);
            q2dot_.resize(9);

            // ********* 5. 각종 변수 초기화 *********
            // 5.1 KDL Vector 초기화 (사이즈 정의 및 값 0)
            x_cmd_.data = Eigen::VectorXd::Zero(12);
            ex_.setZero(12);
            ex_dot_.setZero(12);
            dx.setZero(12);
            dxdot.setZero(12);
            dxddot.setZero(12);

            KpTask.setZero(12);
            KdTask.setZero(12);

            qd_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);

            q_.data = Eigen::VectorXd::Zero(n_joints_);
            qdot_.data = Eigen::VectorXd::Zero(n_joints_);
            torque.setZero(n_joints_);

            // ********* 6. ROS 명령어 *********
            // 6.1 publisher
            state_pub_.reset(new realtime_tools::RealtimePublisher<dualarm_controller::TaskCurrentState>(n, "states", 10));
            state_pub_->msg_.header.stamp = ros::Time::now();
            //state_pub_->msg_.header.frame_id = "dualarm";
            state_pub_->msg_.header.seq = 0;
            for(int i=0; i<n_joints_; i++)
            {
                state_pub_->msg_.q.push_back(q_.data(i));
                state_pub_->msg_.qdot.push_back(qdot_.data(i));
                state_pub_->msg_.dq.push_back(qd_.data(i));
                state_pub_->msg_.dqdot.push_back(qd_dot_.data(i));
                state_pub_->msg_.torque.push_back(torque(i));
            }
            for(int j=0; j<2; j++) {
                state_pub_->msg_.InverseConditionNum.push_back(InverseConditionNumber[j]);
            }
            state_pub_->msg_.x.resize(2);
            state_pub_->msg_.dx.resize(2);
            state_pub_->msg_.DAMM = DAMM;
            state_pub_->msg_.TODAMM = TODAMM2;

            state_pub_->msg_.Kp_R = Kp_rot_R;
            state_pub_->msg_.Kp_T = Kp_trans_R;
            pub_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));

            // 6.2 subsriber
            const auto joint_state_cb = utils::makeCallback<dualarm_controller::TaskCurrentState>([&](const auto& msg){

            });
            sub_x_cmd_ = n.subscribe<dualarm_controller::TaskCurrentState>( "command", 5, joint_state_cb);

            return true;
        }

        void starting(const ros::Time &time) override
        {
            t = 0.0;
            InitTime=2.0;

            ROS_INFO("Starting Task-space Controller");

            cManipulator = std::make_shared<SerialManipulator>();

            Control = std::make_unique<HYUControl::Controller>(cManipulator);

            cManipulator->UpdateManipulatorParam();

            if(ctrl_type == 1)
            {
                KpTask.segment(0,3).setConstant(Kp_rot_R);
                KpTask.segment(3,3).setConstant(Kp_trans_R);
                KpTask.segment(6,3).setConstant(Kp_rot_R);
                KpTask.segment(9,3).setConstant(Kp_trans_R);

                KdTask.segment(0,3).setConstant(Kd_rot_R);
                KdTask.segment(3,3).setConstant(Kd_trans_R);
                KdTask.segment(6,3).setConstant(Kd_rot_R);
                KdTask.segment(9,3).setConstant(Kd_trans_R);
            }
            else if( ctrl_type == 2)
            {
                KpTask.segment(0,3).setConstant(Kp_rot_L);
                KpTask.segment(3,3).setConstant(Kp_trans_L);
                KpTask.segment(6,3).setConstant(Kp_rot_L);
                KpTask.segment(9,3).setConstant(Kp_trans_L);

                KdTask.segment(0,3).setConstant(Kd_rot_L);
                KdTask.segment(3,3).setConstant(Kd_trans_L);
                KdTask.segment(6,3).setConstant(Kd_rot_L);
                KdTask.segment(9,3).setConstant(Kd_trans_L);
            }

            Control->SetTaskspaceGain(KpTask, KdTask);
        }

        void update(const ros::Time &time, const ros::Duration &period) override
        {
            std::vector<double> &commands = *pub_buffer_.readFromRT();
            // ********* 0. Get states from gazebo *********
            // 0.1 sampling time
            double dt = period.toSec();

            clock_gettime(CLOCK_MONOTONIC, &begin);

            // 0.2 joint state
            for (int i = 0; i < n_joints_; i++)
            {
                q_(i) = joints_[i].getPosition();
                qdot_(i) = joints_[i].getVelocity();
                //torque_(i) = joints_[i].getEffort();
            }

            //----------------------
            // dynamics calculation
            //----------------------
            cManipulator->pKin->PrepareJacobian(q_.data);
            cManipulator->pDyn->PrepareDynamics(q_.data, qdot_.data);

            cManipulator->pKin->GetForwardKinematics(ForwardPos, ForwardOri, NumChain);
            cManipulator->pKin->GetAngleAxis(ForwardAxis, ForwardAngle, NumChain);
            cManipulator->pKin->GetInverseConditionNumber(InverseConditionNumber);

            q1_.data = q_.data.head(9);
            q1dot_.q = q1_;
            q1dot_.qdot.data = qdot_.data.head(9);

            q2_.data.tail(7) = q_.data.tail(7);
            q2_.data.head(2) = q_.data.head(2);
            q2dot_.q = q2_;
            q2dot_.qdot.data.tail(7) = qdot_.data.tail(7);
            q2dot_.qdot.data.head(2) = qdot_.data.head(2);

            fk_pos_solver_->JntToCart(q1_, x_[0]);
            fk_pos_solver1_->JntToCart(q2_, x_[1]);

            jnt_to_jac_solver_->JntToJac(q1_, J1_kdl_);
            jnt_to_jac_solver1_->JntToJac(q2_, J2_kdl_);

            jnt_to_jacdot_solver->JntToJacDot(q1dot_, J1dot);
            jnt_to_jacdot_solver1->JntToJacDot(q2dot_, J2dot);

            MM = cManipulator->pKin->GetManipulabilityMeasure();
            manipulability_data();

            if( t <= InitTime )
            {
                qd_.data(0) = -0.0*D2R;
                qd_.data(1) = -0.0*D2R;

                qd_.data(2) = 0.0*D2R;
                qd_.data(3) = -0.0*D2R;
                qd_.data(4) = -0.0*D2R;
                qd_.data(5) = -0.0*D2R;
                qd_.data(6) = -70.0*D2R;
                qd_.data(7) = 0.0*D2R;
                qd_.data(8) = 0.0*D2R;

                qd_.data(9) = 0.0*D2R;
                qd_.data(10) = 0.0*D2R;
                qd_.data(11) = 0.0*D2R;
                qd_.data(12) = -0.0*D2R;
                qd_.data(13) = 70.0*D2R;
                qd_.data(14) = -0.0*D2R;
                qd_.data(15) = -0.0*D2R;
            }
            else
            {
                if(target_obj == 0)
                {
                    xd_[0].p(0) = A * sin(f * M_PI * (t - InitTime)) + b1;
                    xd_[0].p(1) = b2;
                    xd_[0].p(2) = b3;
                    xd_[0].M = KDL::Rotation(KDL::Rotation::RPY(0, 0, M_PI/2));

                    xd_[1].p(0) = -A * sin(f * M_PI * (t - InitTime)) + l_p1;
                    xd_[1].p(1) = l_p2;
                    xd_[1].p(2) = l_p3;
                    xd_[1].M = KDL::Rotation(KDL::Rotation::RPY(-M_PI/2, 0, M_PI/2));

                    dxdot.setZero();
                    dxdot(3) = (f * M_PI) * A * cos(f * M_PI * (t - InitTime));
                    dxdot(9) = -(f * M_PI) * A * cos(f * M_PI * (t - InitTime));

                    x_[0].p(0) = ForwardPos[0](0);
                    x_[0].p(1) = ForwardPos[0](1);
                    x_[0].p(2) = ForwardPos[0](2);
                    x_[0].M = KDL::Rotation(KDL::Rotation::RPY(ForwardOri[0](0), ForwardOri[0](1), ForwardOri[0](2)));

                    x_[1].p(0) = ForwardPos[1](0);
                    x_[1].p(1) = ForwardPos[1](1);
                    x_[1].p(2) = ForwardPos[1](2);
                    x_[1].M = KDL::Rotation(KDL::Rotation::RPY(ForwardOri[1](0), ForwardOri[1](1), ForwardOri[1](2)));

                    ex_temp_ = diff(x_[0], xd_[0]);
                    ex_(0) = ex_temp_(3);
                    ex_(1) = ex_temp_(4);
                    ex_(2) = ex_temp_(5);
                    ex_(3) = ex_temp_(0);
                    ex_(4) = ex_temp_(1);
                    ex_(5) = ex_temp_(2);

                    ex_temp_ = diff(x_[1], xd_[1]);
                    ex_(6) = ex_temp_(3);
                    ex_(7) = ex_temp_(4);
                    ex_(8) = ex_temp_(5);
                    ex_(9) = ex_temp_(0);
                    ex_(10) = ex_temp_(1);
                    ex_(11) = ex_temp_(2);
                }
                if (target_obj == 1)
                {
                    dx(0) = 0;
                    dx(1) = -M_PI_2;
                    dx(2) = 0;
                    dx(3) = A * sin(f * M_PI * (t - InitTime)) + b1;
                    dx(4) = b2;
                    dx(5) = b3;

                    dx(6) = 0;
                    dx(7) = -M_PI_2;
                    dx(8) = 0;
                    dx(9) = -A * sin(f * M_PI * (t - InitTime)) + l_p1;
                    dx(10) = l_p2;
                    dx(11) = l_p3;

                    dxdot.setZero();
                    dxdot(3) = (f * M_PI) * A * cos(f * M_PI * (t - InitTime));
                    dxdot(9) = -(f * M_PI) * A * cos(f * M_PI * (t - InitTime));

                    dxddot.setZero();
                    dxddot(3) = -(f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - InitTime));
                    dxddot(9) = (f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - InitTime));
                }
                else if (target_obj == 2)
                {
                    dx(0) = 0;
                    dx(1) = -M_PI_2;
                    dx(2) = 0;
                    dx(3) = b1;
                    dx(4) = A * sin(f * M_PI * (t - InitTime)) + b2;
                    dx(5) = b3;

                    dx(6) = 0;
                    dx(7) = -M_PI_2;
                    dx(8) = 0;
                    dx(9) = l_p1;
                    dx(10) = -A * sin(f * M_PI * (t - InitTime)) + l_p2;
                    dx(11) = l_p3;

                    dxdot.setZero();
                    dxdot(4) = (f * M_PI) * A * cos(f * M_PI * (t - InitTime));
                    dxdot(10) = -(f * M_PI) * A * cos(f * M_PI * (t - InitTime));

                    dxddot.setZero();
                    dxddot(4) = -(f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - InitTime));
                    dxddot(10) = (f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - InitTime));
                }
                else if (target_obj == 3)
                {
                    dx(0) = 0;
                    dx(1) = -M_PI_2;
                    dx(2) = 0;
                    dx(3) = b1-0.015;
                    dx(4) = b2;
                    dx(5) = A * sin(f * M_PI * (t - InitTime)) + b3;

                    dx(6) = 0;
                    dx(7) = -M_PI_2;
                    dx(8) = 0;
                    dx(9) = l_p1-0.01;
                    dx(10) = l_p2;
                    dx(11) = -A * sin(f * M_PI * (t - InitTime)) + l_p3;

                    dxdot.setZero();
                    dxdot(5) = (f * M_PI) * A * cos(f * M_PI * (t - InitTime));
                    dxdot(11) = -(f * M_PI) * A * cos(f * M_PI * (t - InitTime));

                    dxddot.setZero();
                    dxddot(5) = -(f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - InitTime));
                    dxddot(11) = (f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - InitTime));
                }
                else if( target_obj == 4 )
                {
                    dx(0) = 0;
                    dx(1) = -M_PI_2;
                    dx(2) = 0;
                    dx(3) = A * sin(f * M_PI * (t - InitTime)) + b1-0.015;
                    dx(4) = b2+0.1;
                    dx(5) = b3;

                    dx(6) = 0;
                    dx(7) = 0;
                    dx(8) = 0;
                    dx(9) = -A * cos(2*f * M_PI * (t - InitTime));
                    dx(10) = -A * sin(2*f * M_PI * (t - InitTime)) + 0.45;
                    dx(11) = 0;

                    dxdot.setZero();
                    dxdot(3) = (f * M_PI) * A * cos(f * M_PI * (t - InitTime));
                    dxdot(9) = (2*f * M_PI) * A * sin(2*f * M_PI * (t - InitTime));
                    dxdot(10) = -(2*f * M_PI) * A * cos(2*f * M_PI * (t - InitTime));

                    dxddot.setZero();
                    dxddot(3) = -(f * M_PI) * (f * M_PI) * A * sin(f * M_PI * (t - InitTime));
                    dxddot(9) = (2*f * M_PI) * (2*f * M_PI) * A * cos(2*f * M_PI * (t - InitTime));
                    dxddot(10) = (2*f * M_PI) * (2*f * M_PI) * A * sin(2*f * M_PI * (t - InitTime));
                }
            }

            if(t <= InitTime)
            {
                Control->InvDynController(q_.data, qdot_.data, qd_.data, qd_dot_.data, qd_ddot_.data, torque, dt);
            }
            else
            {
                Control->TaskInvDynController(dx, dxdot, dxddot, q_.data, qdot_.data, torque, dt, ctrl_type);
                Control->GetControllerStates(qd_.data, qd_dot_.data, ex_);
            }

            for (int i = 0; i < n_joints_; i++)
            {
                joints_[i].setCommand(torque(i));
            }

            clock_gettime(CLOCK_MONOTONIC, &end);
            // ********* 4. data 저장 *********
            publish_data();

            // ********* 5. state 출력 *********
            print_state();

            t = t + dt;
        }

        void stopping(const ros::Time &time) override
        {
            ROS_INFO("Stop Task-space Controller");
        }

        void manipulability_data()
        {
            const auto desired_manipulability =
                    manipulability_metrics::Ellipsoid{ { { (Eigen::Matrix<double, 6, 1>{} << 1, 0, 0, 0, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 1, 0, 0, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 1, 0, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 1, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 1, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 0, 1).finished(), 1.0 } } };

            const auto desired_Singelmanipulability =
                    manipulability_metrics::Ellipsoid{ { { (Eigen::Matrix<double, 6, 1>{} << 1, 0, 0, 0, 0, 0).finished(), 0.2 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 1, 0, 0, 0, 0).finished(), 0.8 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 1, 0, 0, 0).finished(), 0.8 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 1, 0, 0).finished(), 0.9 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 1, 0).finished(), 1.7 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 0, 1).finished(), 1.7 } } };
            auto left_ellipsoid = manipulability_metrics::ellipsoidFromJacobian(J2_kdl_.data);
            auto right_ellipsoid = manipulability_metrics::ellipsoidFromJacobian(J1_kdl_.data);
            SingleMM[0] = sqrt((J1_kdl_.data * J1_kdl_.data.transpose()).determinant());
            SingleMM[1] = sqrt((J2_kdl_.data * J2_kdl_.data.transpose()).determinant());
            TOMM[0] = manipulability_metrics::inverseShapeDiscrepancy(desired_Singelmanipulability, J1_kdl_.data);
            TOMM[1] = manipulability_metrics::inverseShapeDiscrepancy(desired_Singelmanipulability, J2_kdl_.data);
            DAMM = std::max(manipulability_metrics::volumeIntersection(left_ellipsoid, J1_kdl_.data),
                            manipulability_metrics::volumeIntersection(right_ellipsoid, J2_kdl_.data));
            TODAMM2 = manipulability_metrics::dualInverseShapeDiscrepancy(desired_manipulability, J2_kdl_.data, J1_kdl_.data);
        }

        void publish_data()
        {
            static int loop_count_ = 0;
            if(loop_count_ > 2)
            {
                if(state_pub_->trylock())
                {
                    state_pub_->msg_.header.stamp = ros::Time::now();
                    state_pub_->msg_.header.seq++;
                    for(size_t i=0; i<n_joints_; i++)
                    {
                        state_pub_->msg_.q[i] = q_.data(i);
                        state_pub_->msg_.qdot[i] = qdot_.data(i);
                        state_pub_->msg_.dq[i] = qd_.data(i);
                        state_pub_->msg_.dqdot[i] = qd_dot_.data(i);
                        state_pub_->msg_.torque[i] = torque(i);
                    }

                    for(int j=0; j<2; j++)
                    {
                        state_pub_->msg_.dx[j].orientation.x = dx(6*j);
                        state_pub_->msg_.dx[j].orientation.y = dx(6*j+1);
                        state_pub_->msg_.dx[j].orientation.z = dx(6*j+2);
                        state_pub_->msg_.dx[j].position.x = dx(6*j+3);
                        state_pub_->msg_.dx[j].position.y = dx(6*j+4);
                        state_pub_->msg_.dx[j].position.z = dx(6*j+5);

                        state_pub_->msg_.x[j].orientation.x = ForwardOri[j](0);
                        state_pub_->msg_.x[j].orientation.y = ForwardOri[j](1);
                        state_pub_->msg_.x[j].orientation.z = ForwardOri[j](2);
                        state_pub_->msg_.x[j].position.x = ForwardPos[j](0);
                        state_pub_->msg_.x[j].position.y = ForwardPos[j](1);
                        state_pub_->msg_.x[j].position.z = ForwardPos[j](2);

                        state_pub_->msg_.InverseConditionNum[j] = InverseConditionNumber[j];
                    }

                    state_pub_->msg_.DAMM = DAMM;
                    state_pub_->msg_.TODAMM = TODAMM2;
                    state_pub_->unlockAndPublish();
                }
                loop_count_=0;
            }
            loop_count_++;
        }

        void print_state()
        {
            static int count = 0;
            if (count > 499)
            {
                printf("*********************************************************\n\n");
                printf("*** Calcutaion Time (unit: sec)  ***\n");
                printf("t_cal = %0.9lf\n", static_cast<double>(end.tv_sec - begin.tv_sec) + static_cast<double>(end.tv_nsec - begin.tv_nsec) / 1000000000.0);
                printf("*** Simulation Time (unit: sec)  ***\n");
                printf("t = %0.3lf\n", t);
                printf("CTRL_OBJ:%d\n", target_obj);
                printf("\n");

                printf("*** Command from Subscriber in Task Space (unit: m) ***\n");
                printf("*** States in Joint Space (unit: deg) ***\n");
                for(int i=0; i < n_joints_; i++)
                {
                    printf("[%s]:  \t", joint_names_[i].c_str());
                    printf("q: %0.2lf,\t", q_.data(i) * R2D);
                    printf("qdot: %0.2lf,\t", qdot_.data(i) * R2D);
                    printf("tau: %0.2f\n", torque(i));
                }

                printf("\nForward Kinematics:\n");
                for(int j=0; j<NumChain; j++)
                {
                    printf("no.%d, Actual: x:%0.3lf, y:%0.3lf, z:%0.3lf, u:%0.2lf, v:%0.2lf, w:%0.2lf\n", j,
                           ForwardPos[j](0), ForwardPos[j](1),ForwardPos[j](2),
                           ForwardOri[j](0), ForwardOri[j](1), ForwardOri[j](2));
                    printf("no.%d, Desired: x:%0.3lf, y:%0.3lf, z:%0.3lf, u:%0.2lf, v:%0.2lf, w:%0.2lf\n", j,
                           dx(6*j+3), dx(6*j+4),dx(6*j+5), dx(6*j), dx(6*j+1), dx(6*j+2));
                    printf("no.%d, AngleAxis x: %0.2lf, y: %0.2lf, z: %0.2lf, Angle: %0.3lf\n\n",
                           j, ForwardAxis[j](0), ForwardAxis[j](1), ForwardAxis[j](2), ForwardAngle[j]);
                }
                printf("Right e(u):%0.3lf, e(v):%0.3lf, e(w):%0.3lf, e(x):%0.3lf, e(y):%0.3lf, e(z):%0.3lf\n",
                       ex_(0)*RADtoDEG, ex_(1)*RADtoDEG, ex_(2)*RADtoDEG, ex_(3), ex_(4), ex_(5));
                printf("Left e(u):%0.3lf, e(v):%0.3lf, e(w):%0.3lf, e(x):%0.3lf, e(y):%0.3lf, e(z):%0.3lf\n",
                       ex_(6)*RADtoDEG, ex_(7)*RADtoDEG, ex_(8)*RADtoDEG, ex_(9), ex_(10), ex_(11));

                printf("Inverse Condition Number: Right:%0.5lf, Left:%0.5lf \n", InverseConditionNumber[0], InverseConditionNumber[1]);
                printf("SingleMM: Right:%0.5lf, Left:%0.5lf\n", SingleMM[0], SingleMM[1]);
                printf("TOMM: Right:%0.5lf, Left:%0.5lf\n", TOMM[0], TOMM[1]);
                printf("MM: %0.5lf\n", MM);
                printf("DAMM: %0.5lf\n", DAMM);
                printf("TODAMM: %0.5lf\n\n", TODAMM2);
                printf("\n*********************************************************\n");
                count = 0;

                //std::cout << "Weight Damped Jacobian Pseudoinverse:" << std::endl;
                //std::cout << WdampedpInvJac << "\n" << std::endl;
                //std::cout << "lambda:" << std::endl;
                //std::cout << wpInv_lambda[0] << std::endl;
                //std::cout << wpInv_lambda[1] << std::endl;

                //std::cout << "J1:" <<std::endl;
                //std::cout << J1_kdl_.data << "\n"<< std::endl;
                //std::cout << "J2:" <<std::endl;
                //std::cout << J2_kdl_.data << "\n"<< std::endl;
                //std::cout << "Analytic Jacobian:" <<std::endl;
                //std::cout << AJac << "\n"<< std::endl;
                //std::cout << "J1dot:" <<std::endl;
                //std::cout << J1dot.data << "\n"<< std::endl;
                //std::cout << "J2dot:" <<std::endl;
                //std::cout << J2dot.data << "\n"<< std::endl;

                //std::cout << "Body Jacobiandot:" <<std::endl;
                //std::cout << BodyJacDot << "\n"<< std::endl;
                /*
                Jdot.setZero();
                Jdot.block(0,0,3,9) = J1dot.data.block(3,0,3,9);
                Jdot.block(3,0,3,9) = J1dot.data.block(0,0,3,9);
                Jdot.block(6,0,3,2) = J2dot.data.block(3,0,3,2);
                Jdot.block(9,0,3,2) = J2dot.data.block(0,0,3,2);
                Jdot.block(6,9,3,7) = J2dot.data.block(3,2,3,7);
                Jdot.block(9,9,3,7) = J2dot.data.block(0,2,3,7);

                std::cout << "Jdot:" <<std::endl;
                std::cout << Jdot << "\n"<< std::endl;
                std::cout << "Analytic Jacobiandot:" <<std::endl;
                std::cout << AJacDot << "\n"<< std::endl;
                */

            }
            count++;
        }

    private:
        // others
        double t=0.0;
        int target_obj=0;
        int ctrl_type=0;
        double InitTime=0.0;
        struct timespec begin, end;
        //Joint handles
        unsigned int n_joints_;
        std::vector<std::string> joint_names_;
        std::vector<hardware_interface::JointHandle> joints_;
        std::vector<urdf::JointConstSharedPtr> joint_urdfs_;

        // kdl
        KDL::Tree kdl_tree_;
        KDL::Chain kdl_chain_;
        KDL::Chain kdl_chain2_;

        KDL::JntSpaceInertiaMatrix M_kdl_, M1_kdl_;
        KDL::JntArray C_kdl_, C1_kdl_;
        KDL::JntArray G_kdl_, G1_kdl_;
        KDL::Vector g_kdl_;
        Eigen::VectorXd g_vec_collect;
        Eigen::MatrixXd g_mat_collect;
        Eigen::MatrixXd M_mat_collect;

        KDL::Jacobian J1_kdl_, J2_kdl_;
        KDL::Jacobian J1dot, J2dot;

        // kdl solver
        boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_, fk_pos_solver1_; //Solver to compute the forward kinematics (position)
        boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_, jnt_to_jac_solver1_; //Solver to compute the jacobian
        boost::scoped_ptr<KDL::ChainJntToJacDotSolver> jnt_to_jacdot_solver, jnt_to_jacdot_solver1;
        boost::scoped_ptr<KDL::ChainDynParam> id_solver_, id_solver1_;               // Solver To compute the inverse dynamics

        MatrixXd M;
        VectorXd G;
        VectorXd C;

        Vector3d ForwardPos[2];
        Vector3d ForwardOri[2];
        int NumChain=2;
        Vector3d ForwardAxis[2];
        double ForwardAngle[2];
        double InverseConditionNumber[2];
        double SingleMM[2];
        double TOMM[2];
        double MM;
        double DAMM;
        double TODAMM2;

        // kdl and Eigen Jacobian
        Eigen::MatrixXd pInvJac;
        Eigen::MatrixXd AJac, AJacDot, Jdot;
        Eigen::MatrixXd BodyJacDot;

        // Joint Space State
        KDL::JntArray qd_;
        KDL::JntArray qd_dot_;
        KDL::JntArray qd_ddot_;
        KDL::JntArray q_, q1_, q2_;
        KDL::JntArray qdot_;
        KDL::JntArrayVel q1dot_, q2dot_;
        Eigen::VectorXd q0dot;
        Eigen::VectorXd torque;

        // Task Space State
        // ver. 01
        KDL::Frame xd_[2];
        KDL::Frame x_[2];
        KDL::Twist ex_temp_;

        // KDL::Twist xd_dot_, xd_ddot_;
        Eigen::VectorXd ex_;
        Eigen::VectorXd ex_dot_;
        Eigen::VectorXd dx;
        Eigen::VectorXd dxdot;
        Eigen::VectorXd dxddot;

        // Input
        KDL::JntArray x_cmd_;

        // gains
        Eigen::VectorXd KpTask, KdTask;
        Eigen::VectorXd aKp_, aKi_, aKd_, aK_inf_;
        double Kp_trans_R, Kp_rot_R, Kp_trans_L, Kp_rot_L;
        double Kd_trans_R, Kd_rot_R, Kd_trans_L, Kd_rot_L;

        // publisher
        realtime_tools::RealtimeBuffer<std::vector<double>> pub_buffer_;
        boost::scoped_ptr<realtime_tools::RealtimePublisher<dualarm_controller::TaskCurrentState>> state_pub_;

        // subscriber
        ros::Subscriber sub_x_cmd_;

        std::shared_ptr<SerialManipulator> cManipulator;
        std::unique_ptr<HYUControl::Controller> Control;
    };
}

PLUGINLIB_EXPORT_CLASS(dualarm_controller::Taskspace_Control,controller_interface::ControllerBase)


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/core/IncrementalEstimator.h"

#include <algorithm>
#include <utility>
#include <vector>
#include <ostream>

#include <aslam-tsvd-solver/aslam-tsvd-solver.h>
#include <aslam/backend/GaussNewtonTrustRegionPolicy.hpp>
#include <aslam/backend/Optimizer2.hpp>
#include <boost/make_shared.hpp>
#include <sm/PropertyTree.hpp>

#include "aslam/calibration/core/IncrementalOptimizationProblem.h"
#include "aslam/calibration/base/Timestamp.h"
#include "aslam/calibration/exceptions/InvalidOperationException.h"

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    IncrementalEstimator::IncrementalEstimator(size_t margGroupId,
        const Options& options, const LinearSolverOptions&
        linearSolverOptions, const OptimizerOptions& optimizerOptions) :
        _options(options),
        _margGroupId(margGroupId),
        _optimizer(boost::make_shared<Optimizer>(optimizerOptions)),
        _problem(boost::make_shared<IncrementalOptimizationProblem>()),
        _informationGain(0.0),
        _svLog2Sum(0.0),
        _svdTolerance(0.0),
        _qrTolerance(-1.0),
        _rankTheta(-1),
        _rankThetaDeficiency(-1),
        _rankPsi(-1),
        _rankPsiDeficiency(-1),
        _peakMemoryUsage(0),
        _memoryUsage(0),
        _numFlops(0.0),
        _initialCost(0.0),
        _finalCost(0.0) {
      // create linear solver and trust region policy for the optimizer
      OptimizerOptions& optOptions = _optimizer->options();
      optOptions.linearSystemSolver =
        boost::make_shared<LinearSolver>(linearSolverOptions);
      optOptions.trustRegionPolicy = boost::make_shared<TrustRegionPolicy>();
      _optimizer->initializeLinearSolver();
      _optimizer->initializeTrustRegionPolicy();

      // attach the problem to the optimizer
      _optimizer->setProblem(_problem);
    }

    IncrementalEstimator::IncrementalEstimator(const sm::PropertyTree& config) :
        _informationGain(0.0),
        _svLog2Sum(0.0),
        _svdTolerance(0.0),
        _qrTolerance(-1.0),
        _rankTheta(-1),
        _rankThetaDeficiency(-1),
        _rankPsi(-1),
        _rankPsiDeficiency(-1),
        _peakMemoryUsage(0),
        _memoryUsage(0),
        _numFlops(0.0),
        _initialCost(0.0),
        _finalCost(0.0) {
      // create the optimizer, linear solver, and trust region policy
      boost::shared_ptr<LinearSolver> linearSolver = boost::make_shared<LinearSolver>(sm::PropertyTree(config, "optimizer/linearSolver"));
      _optimizer = boost::make_shared<Optimizer>(sm::PropertyTree(config, "optimizer"), linearSolver, boost::make_shared<TrustRegionPolicy>());

      // create the problem and attach it to the optimizer
      _problem = boost::make_shared<IncrementalOptimizationProblem>();
      _optimizer->setProblem(_problem);

      // parse the options and set them
      _options.infoGainDelta = config.getDouble("infoGainDelta",
        _options.infoGainDelta);
      _options.checkValidity = config.getBool("checkValidity",
        _options.checkValidity);
      _options.verbose = config.getBool("verbose", _options.verbose);
      _margGroupId = config.getInt("groupId");
    }

    IncrementalEstimator::~IncrementalEstimator() {}

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    const IncrementalOptimizationProblem*
        IncrementalEstimator::getProblem() const {
      return _problem.get();
    }

    const IncrementalEstimator::Options&
        IncrementalEstimator::getOptions() const {
      return _options;
    }

    IncrementalEstimator::Options& IncrementalEstimator::getOptions() {
      return _options;
    }

    const LinearSolverOptions& IncrementalEstimator::getLinearSolverOptions()
        const {
      return _optimizer->getSolver<LinearSolver>()->getOptions();
    }

    LinearSolverOptions& IncrementalEstimator::getLinearSolverOptions() {
      return _optimizer->getSolver<LinearSolver>()->getOptions();
    }

    const IncrementalEstimator::OptimizerOptions&
        IncrementalEstimator::getOptimizerOptions() const {
      return _optimizer->options();
    }

    IncrementalEstimator::OptimizerOptions&
        IncrementalEstimator::getOptimizerOptions() {
      return _optimizer->options();
    }

    size_t IncrementalEstimator::getMargGroupId() const {
      return _margGroupId;
    }

    double IncrementalEstimator::getInformationGain() const {
      return _informationGain;
    }

    const aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>&
        IncrementalEstimator::getJacobianTranspose() const {
      return _optimizer->getSolver<LinearSolver>()->getJacobianTranspose();
    }

    std::ptrdiff_t IncrementalEstimator::getRankPsi() const {
      return _rankPsi;
    }

    std::ptrdiff_t IncrementalEstimator::getRankPsiDeficiency() const {
      return _rankPsiDeficiency;
    }

    std::ptrdiff_t IncrementalEstimator::getRankTheta() const {
      return _rankTheta;
    }

    std::ptrdiff_t IncrementalEstimator::getRankThetaDeficiency() const {
      return _rankThetaDeficiency;
    }

    double IncrementalEstimator::getSVDTolerance() const {
      return _svdTolerance;
    }

    double IncrementalEstimator::getQRTolerance() const {
      return _qrTolerance;
    }

    size_t IncrementalEstimator::getPeakMemoryUsage() const {
      return _peakMemoryUsage;
    }

    size_t IncrementalEstimator::getMemoryUsage() const {
      return _memoryUsage;
    }

    double IncrementalEstimator::getNumFlops() const {
      return _numFlops;
    }

    const Eigen::MatrixXd& IncrementalEstimator::getNobsBasis(bool scaled)
        const {
      if (scaled)
        return _nobsBasisScaled;
      else
        return _nobsBasis;
    }

    const Eigen::MatrixXd& IncrementalEstimator::getObsBasis(bool scaled)
        const {
      if (scaled)
        return _obsBasisScaled;
      else
        return _obsBasis;
    }

    const Eigen::MatrixXd& IncrementalEstimator::getSigma2Theta(bool scaled)
        const {
      if (scaled)
        return _sigma2ThetaScaled;
      else
        return _sigma2Theta;
    }

    const Eigen::MatrixXd& IncrementalEstimator::getSigma2ThetaObs(bool scaled)
        const {
      if (scaled)
        return _sigma2ThetaObsScaled;
      else
        return _sigma2ThetaObs;
    }

    const Eigen::VectorXd& IncrementalEstimator::getSingularValues(bool scaled)
        const {
      if (scaled)
        return _singularValuesScaled;
      else
        return _singularValues;
    }

    double IncrementalEstimator::getInitialCost() const {
      return _initialCost;
    }

    double IncrementalEstimator::getFinalCost() const {
      return _finalCost;
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    IncrementalEstimator::ReturnValue IncrementalEstimator::reoptimize() {
      // query the time
      const double timeStart = Timestamp::now();

      // ensure marginalized design variables are well located
      orderMarginalizedDesignVariables();

      // set the marginalization index of the linear solver
      size_t JCols = 0;
      for (auto it = _problem->getGroupsOrdering().cbegin();
          it != _problem->getGroupsOrdering().cend(); ++it)
        JCols += _problem->getGroupDim(*it);
      const size_t dim = _problem->getGroupDim(_margGroupId);
      auto linearSolver = _optimizer->getSolver<LinearSolver>();
      linearSolver->setMargStartIndex(static_cast<std::ptrdiff_t>(JCols - dim));

      // optimize
      aslam::backend::SolutionReturnValue srv = _optimizer->optimize();

      // grep the scaled linear system informations
      if (linearSolver->getOptions().columnScaling) {
        _singularValuesScaled = linearSolver->getSingularValues();
        _nobsBasisScaled = linearSolver->getNullSpace();
        _obsBasisScaled = linearSolver->getRowSpace();
        _sigma2ThetaScaled = linearSolver->getCovariance();
        _sigma2ThetaObsScaled = linearSolver->getRowSpaceCovariance();
      }
      else {
        _singularValuesScaled.resize(0);
        _nobsBasisScaled.resize(0, 0);
        _obsBasisScaled.resize(0, 0);
        _sigma2ThetaScaled.resize(0, 0);
        _sigma2ThetaObsScaled.resize(0, 0);
      }

      // analyze the unscaled marginal system
      linearSolver->analyzeMarginal();

      // retrieve informations from the linear solver
      _informationGain = 0.0;
      _svLog2Sum = linearSolver->getSingularValuesLog2Sum();
      _nobsBasis = linearSolver->getNullSpace();
      _obsBasis = linearSolver->getRowSpace();
      _sigma2Theta = linearSolver->getCovariance();
      _sigma2ThetaObs = linearSolver->getRowSpaceCovariance();
      _singularValues = linearSolver->getSingularValues();
      _svdTolerance = linearSolver->getSVDTolerance();
      _qrTolerance = linearSolver->getQRTolerance();
      _rankTheta = linearSolver->getSVDRank();
      _rankThetaDeficiency = linearSolver->getSVDRankDeficiency();
      _rankPsi = linearSolver->getQRRank();
      _rankPsiDeficiency = linearSolver->getQRRankDeficiency();
      _peakMemoryUsage = linearSolver->getPeakMemoryUsage();
      _memoryUsage = linearSolver->getMemoryUsage();
      _numFlops = linearSolver->getNumFlops();
      _initialCost = srv.JStart;
      _finalCost = srv.JFinal;

      // update output structure
      ReturnValue ret;
      ret.batchAccepted = true;
      ret.informationGain = 0.0;
      ret.rankPsi = _rankPsi;
      ret.rankPsiDeficiency = _rankPsiDeficiency;
      ret.rankTheta = _rankTheta;
      ret.rankThetaDeficiency = _rankThetaDeficiency;
      ret.svdTolerance = _svdTolerance;
      ret.qrTolerance = _qrTolerance;
      ret.nobsBasis = _nobsBasis;
      ret.nobsBasisScaled = _nobsBasisScaled;
      ret.obsBasis = _obsBasis;
      ret.obsBasisScaled = _obsBasisScaled;
      ret.sigma2Theta = _sigma2Theta;
      ret.sigma2ThetaScaled = _sigma2ThetaScaled;
      ret.sigma2ThetaObs = _sigma2ThetaObs;
      ret.sigma2ThetaObsScaled = _sigma2ThetaObsScaled;
      ret.singularValues = _singularValues;
      ret.singularValuesScaled = _singularValuesScaled;
      ret.numIterations = srv.iterations;
      ret.JStart = _initialCost;
      ret.JFinal = _finalCost;
      ret.elapsedTime = Timestamp::now() - timeStart;
      return ret;
    }

    IncrementalEstimator::ReturnValue
        IncrementalEstimator::addBatch(const BatchSP& problem, bool force) {
      // query the time
      const double timeStart = Timestamp::now();

      // insert new batch in the problem
      _problem->add(problem);

      // ensure marginalized design variables are well located
      orderMarginalizedDesignVariables();

      // save design variables in case the batch is rejected
      if (!force)
        _problem->saveDesignVariables();

      // set the marginalization index of the linear solver
      size_t JCols = 0;
      for (auto it = _problem->getGroupsOrdering().cbegin();
          it != _problem->getGroupsOrdering().cend(); ++it)
        JCols += _problem->getGroupDim(*it);
      const size_t dim = _problem->getGroupDim(_margGroupId);
      auto linearSolver = _optimizer->getSolver<LinearSolver>();
      linearSolver->setMargStartIndex(static_cast<std::ptrdiff_t>(JCols - dim));

      // optimize
      aslam::backend::SolutionReturnValue srv = _optimizer->optimize();

      // return value
      ReturnValue ret;

      // fill statistics from optimizer
      ret.numIterations = srv.iterations;
      ret.JStart = srv.JStart;
      ret.JFinal = srv.JFinal;

      // grep the scaled singular values if scaling enabled
      if (linearSolver->getOptions().columnScaling) {
        ret.singularValuesScaled = linearSolver->getSingularValues();
        ret.nobsBasisScaled = linearSolver->getNullSpace();
        ret.obsBasisScaled = linearSolver->getRowSpace();
        ret.sigma2ThetaScaled = linearSolver->getCovariance();
        ret.sigma2ThetaObsScaled = linearSolver->getRowSpaceCovariance();
      }
      else {
        ret.singularValuesScaled.resize(0);
        ret.nobsBasisScaled.resize(0, 0);
        ret.obsBasisScaled.resize(0, 0);
        ret.sigma2ThetaScaled.resize(0, 0);
        ret.sigma2ThetaObsScaled.resize(0, 0);
      }

      // analyze marginal system (unscaled system)
      linearSolver->analyzeMarginal();

      // fill statistics from the linear solver
      ret.rankPsi = linearSolver->getQRRank();
      ret.rankPsiDeficiency = linearSolver->getQRRankDeficiency();
      ret.rankTheta = linearSolver->getSVDRank();
      ret.rankThetaDeficiency = linearSolver->getSVDRankDeficiency();
      ret.svdTolerance = linearSolver->getSVDTolerance();
      ret.qrTolerance = linearSolver->getQRTolerance();
      ret.nobsBasis = linearSolver->getNullSpace();
      ret.obsBasis = linearSolver->getRowSpace();
      ret.sigma2Theta = linearSolver->getCovariance();
      ret.sigma2ThetaObs = linearSolver->getRowSpaceCovariance();
      ret.singularValues = linearSolver->getSingularValues();

      // check if the solution is valid
      bool solutionValid = true;
      if (_options.checkValidity && (srv.iterations ==
          _optimizer->options().maxIterations || srv.JFinal >= srv.JStart))
        solutionValid = false;

      // compute the information gain
      const double svLog2Sum = linearSolver->getSingularValuesLog2Sum();
      ret.informationGain = 0.5 * (svLog2Sum - _svLog2Sum);

      // batch is kept? information gain improvement or rank goes up or force
      bool keepBatch = false;
      if (((ret.informationGain > _options.infoGainDelta ||
          ret.rankTheta > _rankTheta) && solutionValid) || force) {
        // warning for rank going down
        if (ret.rankTheta < _rankTheta && _options.verbose)
          std::cerr << "IncrementalEstimator::addBatch(): "
            "WARNING: RANK GOING DOWN!" << std::endl;

        keepBatch = true;

        // update internal variables
        _informationGain = ret.informationGain;
        _svLog2Sum = svLog2Sum;
        _nobsBasis = ret.nobsBasis;
        _nobsBasisScaled = ret.nobsBasisScaled;
        _obsBasis = ret.obsBasis;
        _obsBasisScaled = ret.obsBasisScaled;
        _sigma2Theta = ret.sigma2Theta;
        _sigma2ThetaScaled = ret.sigma2ThetaScaled;
        _sigma2ThetaObs = ret.sigma2ThetaObs;
        _sigma2ThetaObsScaled = ret.sigma2ThetaObsScaled;
        _singularValues = ret.singularValues;
        _singularValuesScaled = ret.singularValuesScaled;
        _svdTolerance = ret.svdTolerance;
        _qrTolerance = ret.qrTolerance;
        _rankTheta = ret.rankTheta;
        _rankThetaDeficiency = ret.rankThetaDeficiency;
        _rankPsi = ret.rankPsi;
        _rankPsiDeficiency = ret.rankPsiDeficiency;
        _peakMemoryUsage = linearSolver->getPeakMemoryUsage();
        _memoryUsage = linearSolver->getMemoryUsage();
        _numFlops = linearSolver->getNumFlops();
        _initialCost = srv.JStart;
        _finalCost = srv.JFinal;
      }
      ret.batchAccepted = keepBatch;

      // remove batch if necessary
      if (!keepBatch) {
        // restore variables
        _problem->restoreDesignVariables();

        // kick out the problem from the container
        _problem->remove(problem);

        // restore the linear solver
        if (_problem->getNumOptimizationProblems() > 0)
          restoreLinearSolver();
      }

      // insert elapsed time
      ret.elapsedTime = Timestamp::now() - timeStart;

      // output informations
      return ret;
    }

    void IncrementalEstimator::removeBatch(size_t idx) {
      // remove the batch
      _problem->remove(idx);

      // reoptimize
      reoptimize();
    }

    void IncrementalEstimator::removeBatch(const BatchSP& batch) {
      auto it = _problem->getOptimizationProblem(batch);
      if (it != _problem->getOptimizationProblemEnd())
        removeBatch(std::distance(_problem->getOptimizationProblemBegin(), it));
    }

    size_t IncrementalEstimator::getNumBatches() const {
      return _problem->getNumOptimizationProblems();
    }

    void IncrementalEstimator::orderMarginalizedDesignVariables() {
      auto groupsOrdering = _problem->getGroupsOrdering();
      auto margGroupIt = std::find(groupsOrdering.begin(),
        groupsOrdering.end(), _margGroupId);
      if (margGroupIt == groupsOrdering.end())
        throw InvalidOperationException(
          "IncrementalEstimator::orderMarginalizedDesignVariables(): "
          "marginalized group ID should appear in the problem", __FILE__,
          __LINE__);
      else {
        if (*margGroupIt != groupsOrdering.back()) {
          std::swap(*margGroupIt, groupsOrdering.back());
          _problem->setGroupsOrdering(groupsOrdering);
        }
      }
    }

    void IncrementalEstimator::restoreLinearSolver() {
      // init the matrix structure
      std::vector<aslam::backend::DesignVariable*> dvs;
      const size_t numDVS = _problem->numDesignVariables();
      dvs.reserve(numDVS);
      size_t columnBase = 0;
      for (size_t i = 0; i < numDVS; ++i) {
        aslam::backend::DesignVariable* dv = _problem->designVariable(i);
        if (dv->isActive()) {
          dvs.push_back(dv);
          dv->setBlockIndex(dvs.size() - 1);
          dv->setColumnBase(columnBase);
          columnBase += dv->minimalDimensions();
        }
      }
      std::vector<aslam::backend::ErrorTerm*> ets;
      const size_t numETS = _problem->numErrorTerms();
      ets.reserve(numETS);
      size_t dim = 0;
      for (size_t i = 0; i < numETS; ++i) {
        aslam::backend::ErrorTerm* et = _problem->errorTerm(i);
        et->setRowBase(dim);
        dim += et->dimension();
        ets.push_back(et);
      }
      auto linearSolver = _optimizer->getSolver<LinearSolver>();
      linearSolver->initMatrixStructure(dvs, ets, false);

      // build the system
      linearSolver->buildSystem(_optimizer->options().numThreadsJacobian, true);
    }

  }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_WITHIN_UTIL_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_WITHIN_UTIL_HPP

#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/strategies/agnostic/point_in_poly_winding.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace overlay
{

template<typename Tag, typename Point, typename Geometry>
struct within_code
{};

template<typename Point, typename Box>
struct within_code<box_tag, Point, Box>
{
    static inline int apply(Point const& point, Box const& box)
    {
        // 1. Check outside
        if (get<0>(point) < get<min_corner, 0>(box)
            || get<0>(point) > get<max_corner, 0>(box)
            || get<1>(point) < get<min_corner, 1>(box)
            || get<1>(point) > get<max_corner, 1>(box))
        {
            return -1;
        }
        // 2. Check border
        if (geometry::math::equals(get<0>(point), get<min_corner, 0>(box))
            || geometry::math::equals(get<0>(point), get<max_corner, 0>(box))
            || geometry::math::equals(get<1>(point), get<min_corner, 1>(box))
            || geometry::math::equals(get<1>(point), get<max_corner, 1>(box)))
        {
            return 0;
        }
        return 1;
    }
};
template<typename Point, typename Ring>
struct within_code<ring_tag, Point, Ring>
{
    static inline int apply(Point const& point, Ring const& ring)
    {
        // Same as point_in_ring but here ALWAYS with winding.
        typedef strategy::within::winding<Point> strategy_type;

        return detail::within::point_in_ring
            <
                Point,
                Ring,
                order_as_direction<geometry::point_order<Ring>::value>::value,
                geometry::closure<Ring>::value,
                strategy_type
            >::apply(point, ring, strategy_type());
    }
};

template<typename Point, typename Geometry>
inline int point_in_ring(Point const& point, Geometry const& geometry)
{
    return within_code<typename geometry::tag<Geometry>::type, Point, Geometry>
        ::apply(point, geometry);
}

template<typename Point, typename Geometry>
inline bool within_or_touch(Point const& point, Geometry const& geometry)
{
    return within_code<typename geometry::tag<Geometry>::type, Point, Geometry>
        ::apply(point, geometry) >= 0;
}

}} // namespace detail::overlay
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_WITHIN_UTIL_HPP


/*
 * System.cpp
 *
 *  Created on: May 20, 2019
 *      Author: lorenzo
 */

#include "System.h"

#include <glm/gtx/component_wise.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include <algorithm>
#include <iostream>
#include <fstream>

System::System(double rc) :
				particle_radius(0.5),
				r_cut(rc) {

	particle_radius_sqr = particle_radius * particle_radius;
}

System::~System() {

}

void System::init_from_LAMMPS(char *filename) {
	std::ifstream input(filename);

	if(!input.good()) {
		std::cerr << "Invalid file '" << filename << "'" << std::endl;
		exit(1);
	}

	std::string line;
	std::vector<std::string> split;
	std::getline(input, line);
	std::getline(input, line);
	std::getline(input, line);
	std::getline(input, line);
	std::getline(input, line);

	for(int i = 0; i < 3; i++) {
		std::getline(input, line);
		boost::split(split, line, boost::is_any_of(" "));
		try {
			box[i] = boost::lexical_cast<double>(boost::trim_copy(split[1])) - boost::lexical_cast<double>(boost::trim_copy(split[0]));
		}
		catch(boost::bad_lexical_cast &e) {
			std::cerr << "The box line '" << line << "' found in the oxDNA configuration is not valid" << std::endl;
			exit(1);
		}
	}
	std::getline(input, line);

	bool end = false;
	while(input.good() && !end) {
		if(input.peek() == 't') {
			end = true;
		}
		else {
			getline(input, line);
			if(input.good() && line.size() > 0) {
				boost::split(split, line, boost::is_any_of(" "));
				vec3 position = vec3(boost::lexical_cast<double>(split[2]), boost::lexical_cast<double>(split[3]), boost::lexical_cast<double>(split[4]));
				particles.push_back(position);
			}
		}
	}

	std::cerr << "Number of particles: " << particles.size() << ", box: <" << box[0] << ", " << box[1] << ", " << box[2] << ">" << std::endl;

	input.close();

	_init_cells();
}

void System::init_from_oxDNA(char *filename) {
	std::ifstream input(filename);

	if(!input.good()) {
		std::cerr << "Invalid file '" << filename << "'" << std::endl;
		exit(1);
	}

	std::string line;
	std::vector<std::string> split;
	std::getline(input, line);
	std::getline(input, line);
	boost::split(split, line, boost::is_any_of(" "));
	try {
		box[0] = boost::lexical_cast<double>(boost::trim_copy(split[2]));
		box[1] = boost::lexical_cast<double>(boost::trim_copy(split[3]));
		box[2] = boost::lexical_cast<double>(boost::trim_copy(split[4]));
	}
	catch(boost::bad_lexical_cast &e) {
		std::cerr << "The box line '" << line << "' found in the oxDNA configuration is not valid" << std::endl;
		exit(1);
	}
	std::getline(input, line);

	bool end = false;
	while(input.good() && !end) {
		if(input.peek() == 't') {
			end = true;
		}
		else {
			getline(input, line);
			if(input.good() && line.size() > 0) {
				boost::split(split, line, boost::is_any_of(" "));
				vec3 position = vec3(boost::lexical_cast<double>(split[0]), boost::lexical_cast<double>(split[1]), boost::lexical_cast<double>(split[2]));
				particles.push_back(position);
			}
		}
	}

	std::cerr << "Number of particles: " << particles.size() << ", box: <" << box[0] << ", " << box[1] << ", " << box[2] << ">" << std::endl;

	input.close();

	_init_cells();
}

int System::get_cell_index(const vec3 &pos) const {
	int res = (int) ((pos[0] / box[0] - floor(pos[0] / box[0])) * (1.0 - DBL_EPSILON) * N_cells_side[0]);
	res += N_cells_side[0] * ((int) ((pos[1] / box[1] - floor(pos[1] / box[1])) * (1.0 - DBL_EPSILON) * N_cells_side[1]));
	res += N_cells_side[0] * N_cells_side[1] * ((int) ((pos[2] / box[2] - floor(pos[2] / box[2])) * (1.0 - DBL_EPSILON) * N_cells_side[2]));
	return res;
}

ivec3 System::get_cell(const vec3 &pos) const {
	int cell_index = get_cell_index(pos);
	return ivec3(cell_index % N_cells_side[0], (cell_index / N_cells_side[0]) % N_cells_side[1], cell_index / (N_cells_side[0] * N_cells_side[1]));
}

void System::_init_cells() {
	N_cells_side = glm::floor(box / r_cut);
	for(int i = 0; i < 3; i++) {
		// everything becomes easier if the number of cells per side is odd
		if((N_cells_side[i] % 2) == 0) {
			N_cells_side[i]--;
		}
		if(N_cells_side[i] < 3) {
			N_cells_side[i] = 3;
		}
	}
	int N_cells = N_cells_side[0] * N_cells_side[1] * N_cells_side[2];
	next.resize(particles.size(), -1);
	heads.resize(N_cells, -1);

	std::cerr << "Number of cells: " << N_cells << ", cells per box side: <" << N_cells_side[0] << ", " << N_cells_side[1] << ", " << N_cells_side[2] << ">" << std::endl;

	for(unsigned int i = 0; i < particles.size(); i++) {
		int cell_idx = get_cell_index(particles[i]);
		next[i] = heads[cell_idx];
		heads[cell_idx] = i;
	}

	// here we generate lists of all possible shifts according to their order (the shell around the central cell they belong to)
	int max_shift = (glm::compMax(N_cells_side) - 1) / 2;
	cell_shifts.resize(max_shift);
	ivec3 shift;
	for(shift[0] = -N_cells_side[0] / 2; shift[0] <= N_cells_side[0] / 2; shift[0]++) {
		for(shift[1] = -N_cells_side[1] / 2; shift[1] <= N_cells_side[1] / 2; shift[1]++) {
			for(shift[2] = -N_cells_side[2] / 2; shift[2] <= N_cells_side[2] / 2; shift[2]++) {
				auto shift_abs = glm::abs(shift);
				int shift_order = glm::compMax(shift_abs) - 1;
				if(shift_order == -1) {
					shift_order = 0;
				}
				cell_shifts[shift_order].push_back(shift);
			}
		}
	}

	std::cerr << "Maximum shift order: " << max_shift << ", number of shifts per order:";
	for(auto &shift : cell_shifts) {
		std::cerr << " " << shift.size();
	}
	std::cerr << std::endl;
}


/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Social Robotics Lab, University of Freiburg
*  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 the copyright holder 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 "features.h"

#include <Eigen/Eigenvalues>
#include <ros/console.h>

#include <sstream>
#include <algorithm>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#define USE_STANDARD_FEATURES
#define USE_EXTENT_FEATURES
//#define USE_HSV_FEATURES
#define USE_YCBCR_FEATURES
//#define USE_RGB_FEATURES

enum Features {
    // NOTE: New features must be added to the end of the list, otherwise
    // existing learned classifier models will break!
#ifdef USE_STANDARD_FEATURES
    F_NUM_POINTS,
    F_SPHERICITY,
    F_FLATNESS,
    F_LINEARITY,
    F_STDDEV,
    F_KURTOSIS,
    F_MAD_MEDIAN,
    F_NORM_RES_PLANARITY,
    F_NUM_POINTS_RATIO,
#endif

    // Added after ICRA'15 paper: Width, depth, height of voxel content
#ifdef USE_EXTENT_FEATURES
    F_WIDTH,
    F_DEPTH,
    F_HEIGHT,
    F_ASPECT_RATIO_XY,
#endif

    // Added after ICRA'15 paper: HSV color features
#ifdef USE_HSV_FEATURES
    F_HSV_MEAN_H,
    F_HSV_MEAN_S,
    F_HSV_MEAN_V,
    F_HSV_STDDEV_H,
    F_HSV_STDDEV_S,
    F_HSV_STDDEV_V,
#endif

    // Added after ICRA'15 paper: YCbCr color features
#ifdef USE_YCBCR_FEATURES
    F_YCBCR_MEAN_Y,
    F_YCBCR_MEAN_CB,
    F_YCBCR_MEAN_CR,
    F_YCBCR_STDDEV_Y,
    F_YCBCR_STDDEV_CB,
    F_YCBCR_STDDEV_CR,
#endif

    // Added after ICRA'15 paper: RGB color features
#ifdef USE_RGB_FEATURES
    F_RGB_MEAN_R,
    F_RGB_MEAN_G,
    F_RGB_MEAN_B,
    F_RGB_STDDEV_R,
    F_RGB_STDDEV_G,
    F_RGB_STDDEV_B,
#endif

    F_COUNT, /* add new features before this element */
};

FeatureCalculator::FeatureCalculator()
{
    std::vector<std::string> enabledFeatureSets;

    m_featureNames.resize(F_COUNT);
    m_allFeaturesMask = std::vector<bool>(F_COUNT, true);

#ifdef USE_STANDARD_FEATURES
    enabledFeatureSets.push_back("Standard geometric features");
    m_featureNames[F_NUM_POINTS]            = "Number of points";
    m_featureNames[F_SPHERICITY]            = "Sphericity";
    m_featureNames[F_FLATNESS]              = "Flatness";
    m_featureNames[F_LINEARITY]             = "Linearity";
    m_featureNames[F_STDDEV]                = "Standard deviation w.r.t. centroid";
    m_featureNames[F_KURTOSIS]              = "Kurtosis w.r.t. centroid";
    m_featureNames[F_MAD_MEDIAN]            = "Average deviation from median";
    m_featureNames[F_NORM_RES_PLANARITY]    = "Normalized residual planarity";
    m_featureNames[F_NUM_POINTS_RATIO]      = "Number of points ratio relative to total point count in cloud";
#endif

#ifdef USE_EXTENT_FEATURES
    enabledFeatureSets.push_back("Geometric extent features");
    m_featureNames[F_WIDTH]                 = "Width of voxel content (extent in y direction)";
    m_featureNames[F_DEPTH]                 = "Depth of voxel content (extent in x direction)";
    m_featureNames[F_HEIGHT]                = "Height of voxel content (extent in z direction)";
    m_featureNames[F_ASPECT_RATIO_XY]       = "Aspect ratio x/y";
#endif

#ifdef USE_HSV_FEATURES
    enabledFeatureSets.push_back("HSV color features");
    m_featureNames[F_HSV_MEAN_H]            = "HSV color mean (hue)";
    m_featureNames[F_HSV_MEAN_S]            = "HSV color mean (saturation)";
    m_featureNames[F_HSV_MEAN_V]            = "HSV color mean (value)";
    m_featureNames[F_HSV_STDDEV_H]          = "HSV color standard deviation (hue)";
    m_featureNames[F_HSV_STDDEV_S]          = "HSV color standard deviation (saturation)";
    m_featureNames[F_HSV_STDDEV_V]          = "HSV color standard deviation (value)";
#endif

#ifdef USE_YCBCR_FEATURES
    enabledFeatureSets.push_back("YCbCr color features");
    m_featureNames[F_YCBCR_MEAN_Y]          = "YCbCr color mean (Y)";
    m_featureNames[F_YCBCR_MEAN_CB]         = "YCbCr color mean (Cb)";
    m_featureNames[F_YCBCR_MEAN_CR]         = "YCbCr color mean (Cr)";
    m_featureNames[F_YCBCR_STDDEV_Y]        = "YCbCr color standard deviation (Y)";
    m_featureNames[F_YCBCR_STDDEV_CB]       = "YCbCr color standard deviation (Cb)";
    m_featureNames[F_YCBCR_STDDEV_CR]       = "YCbCr color standard deviation (Cr)";
#endif

#ifdef USE_RGB_FEATURES
    enabledFeatureSets.push_back("RGB color features");
    m_featureNames[F_RGB_MEAN_R]            = "RGB color mean (R)";
    m_featureNames[F_RGB_MEAN_G]            = "RGB color mean (G)";
    m_featureNames[F_RGB_MEAN_B]            = "RGB color mean (B)";
    m_featureNames[F_RGB_STDDEV_R]          = "RGB color standard deviation (R)";
    m_featureNames[F_RGB_STDDEV_G]          = "RGB color standard deviation (G)";
    m_featureNames[F_RGB_STDDEV_B]          = "RGB color standard deviation (B)";
#endif

    std::stringstream ss;
    for(size_t i = 0; i < enabledFeatureSets.size(); i++) {
        ss << enabledFeatureSets[i];
        if(i < enabledFeatureSets.size() - 1) ss << ", ";
    }
    ROS_INFO_STREAM("Using the following sets of features: " << ss.str());
}

// NOTE: This code might be a bit hard to read since it contains optimizations to compute only what is absolutely
// required, given the bit mask of active features for this particular volume.
void FeatureCalculator::calculateFeatures(const Volume& parentVolume, const PointCloud& fullCloud, const std::vector<int>& pointIndices,
    const std::vector<bool>& activeFeatures, std::vector<double>& featureVector) const
{
    double pointCount = pointIndices.size();

    #ifdef USE_STANDARD_FEATURES
    {
        // Feature: Number of points
        if(activeFeatures[F_NUM_POINTS]) featureVector.push_back(pointCount);

        // Determine sample mean for scatter matrix
        bool needEigenvalues = activeFeatures[F_SPHERICITY] || activeFeatures[F_FLATNESS] || activeFeatures[F_LINEARITY] || activeFeatures[F_NORM_RES_PLANARITY];
        bool needMedian = activeFeatures[F_MAD_MEDIAN];
        bool needSampleMean = needEigenvalues || needMedian || activeFeatures[F_STDDEV] || activeFeatures[F_KURTOSIS];

        Eigen::Vector3d sampleMean = Eigen::Vector3d::Zero();
        std::vector<double> xs, ys, zs; // for feature: average deviation from median

        if(needSampleMean && needMedian) {
            xs.reserve(pointIndices.size());
            ys.reserve(pointIndices.size());
            zs.reserve(pointIndices.size());

            foreach(int pointIndex, pointIndices) {
                const PointType& point = fullCloud.points[pointIndex];
                sampleMean += Eigen::Vector3d(point.x, point.y, point.z);

                // for feature: average deviation from median
                xs.push_back(point.x);
                ys.push_back(point.y);
                zs.push_back(point.z);
            }
            sampleMean /= pointCount;
        }
        else if(needSampleMean && !needMedian) {
            foreach(int pointIndex, pointIndices) {
                const PointType& point = fullCloud.points[pointIndex];
                sampleMean += Eigen::Vector3d(point.x, point.y, point.z);
            }
            sampleMean /= pointCount;
        }
        else if(!needSampleMean && needMedian) {
            xs.reserve(pointIndices.size());
            ys.reserve(pointIndices.size());
            zs.reserve(pointIndices.size());

            foreach(int pointIndex, pointIndices) {
                const PointType& point = fullCloud.points[pointIndex];

                // for feature: average deviation from median
                xs.push_back(point.x);
                ys.push_back(point.y);
                zs.push_back(point.z);
            }
        }

        Eigen::Vector3d componentWiseMedian;
        if(needMedian) {
            // for feature: average deviation from median
            // NOTE: Using std::nth_element() (which produces a partial sort) is significantly faster than std::sort()!
            std::nth_element(xs.begin(), xs.begin() + xs.size()/2, xs.end());
            std::nth_element(ys.begin(), ys.begin() + ys.size()/2, ys.end());
            std::nth_element(zs.begin(), zs.begin() + zs.size()/2, zs.end());
            componentWiseMedian = Eigen::Vector3d(xs[xs.size() / 2], ys[ys.size() / 2], zs[zs.size() / 2]);
        }

        // determine scatter matrix etc.
        Eigen::Matrix3d scatterMatrix = Eigen::Matrix3d::Zero();
        double sumOfSquaredDifferences = 0; // for feature: standard deviation
        double sumOfSquaredSquaredDifferences = 0; // for feature: kurtosis
        double sumOfDeviationsFromMedian = 0; // for feature: average deviation from median

        if(needEigenvalues || activeFeatures[F_STDDEV] || activeFeatures[F_KURTOSIS] || activeFeatures[F_MAD_MEDIAN])
        {
            foreach(int pointIndex, pointIndices) {
                const PointType& point = fullCloud.points[pointIndex];
                Eigen::Vector3d x(point.x, point.y, point.z);

                Eigen::Vector3d meanDiff = x - sampleMean;
                if(needEigenvalues) scatterMatrix += meanDiff * meanDiff.transpose();

                double meanDiffDotProduct = meanDiff.dot(meanDiff);
                if(activeFeatures[F_STDDEV])     sumOfSquaredDifferences += meanDiffDotProduct;
                if(activeFeatures[F_KURTOSIS])   sumOfSquaredSquaredDifferences += meanDiffDotProduct * meanDiffDotProduct;
                if(activeFeatures[F_MAD_MEDIAN]) sumOfDeviationsFromMedian += (x - componentWiseMedian).norm();
            }
        }

        Eigen::EigenSolver<Eigen::Matrix3d> eigensolver;
        Eigen::EigenSolver<Eigen::Matrix3d>::EigenvalueType eigenvalues;
        std::vector<double> sortedEigenvalues;

        if(needEigenvalues) {
            eigensolver = Eigen::EigenSolver<Eigen::Matrix3d>(scatterMatrix);
            eigenvalues = eigensolver.eigenvalues();
            sortedEigenvalues.push_back(eigenvalues(0).real());
            sortedEigenvalues.push_back(eigenvalues(1).real());
            sortedEigenvalues.push_back(eigenvalues(2).real());
            std::sort(sortedEigenvalues.begin(), sortedEigenvalues.end(), std::greater<double>());
            double sumOfEigenvalues = eigenvalues.real().sum();

            // Feature: Sphericity
            if(activeFeatures[F_SPHERICITY]) {
                double sphericity = 3 * sortedEigenvalues[2] / sumOfEigenvalues;
                featureVector.push_back(sphericity);
            }

            // Feature: Flatness
            if(activeFeatures[F_FLATNESS]) {
                double flatness = 2 * (sortedEigenvalues[1] - sortedEigenvalues[2]) / sumOfEigenvalues;
                featureVector.push_back(flatness);
            }

            // Feature: Linearity
            if(activeFeatures[F_LINEARITY]) {
                double linearity = (sortedEigenvalues[0] - sortedEigenvalues[1]) / sumOfEigenvalues;
                featureVector.push_back(linearity);
            }
        }

        // Feature: Standard deviation w.r.t. centroid
        double stddev;
        if(activeFeatures[F_STDDEV] || activeFeatures[F_KURTOSIS]) {
            stddev = sqrt(1/(pointCount - 1) * sumOfSquaredDifferences);
            if(activeFeatures[F_STDDEV]) featureVector.push_back(stddev);
        }

        // Feature: Kurtosis w.r.t. centroid
        if(activeFeatures[F_KURTOSIS]) {
            double kurtosis = sumOfSquaredSquaredDifferences / stddev;
            featureVector.push_back(kurtosis);
        }

        // Feature: Average deviation from median
        if(activeFeatures[F_MAD_MEDIAN]) {
            double averageDeviationFromMedian = sumOfDeviationsFromMedian / pointCount;
            featureVector.push_back(averageDeviationFromMedian);
        }

        // Feature: Normalized residual planarity
        // As described here: http://missingbytes.blogspot.de/2012/06/fitting-plane-to-point-cloud.html
        // in implicit form C+n*x = 0
        // the plane's normal vector n is the eigenvector associated with the smallest eigenvalue of the scatter matrix (=sortedEigenvalues(2))
        // and the center vector C is just the sample mean
        // FIXME: Might want to use SVD instead for numerical stability: http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points?rq=1
        if(activeFeatures[F_NORM_RES_PLANARITY]) {
            Eigen::EigenSolver<Eigen::Matrix3d>::EigenvectorsType eigenvectors = eigensolver.eigenvectors();
            Eigen::Vector3d normalVector = Eigen::Vector3d::Zero();
            for(int i = 0; i < eigenvectors.cols(); i++) {
                if(eigenvalues(i) == sortedEigenvalues[2]) {
                    normalVector = eigenvectors.col(i).real();
                    break;
                }
            }

            double distanceToOrigin = -normalVector.dot(sampleMean);

            double sumOfSquaredErrors = 0;
            foreach(int pointIndex, pointIndices) {
                const PointType& point = fullCloud.points[pointIndex];
                Eigen::Vector3d x(point.x, point.y, point.z);

                double residual = normalVector.dot(x) + distanceToOrigin;
                sumOfSquaredErrors += residual * residual;
            }

            double normalizedResidualPlanarity = sumOfSquaredErrors / pointCount;
            featureVector.push_back(normalizedResidualPlanarity);
        }

        // Feature: Number of points ratio
        //pcl::PointXYZ volumeSize = parentVolume.getSize();
        //double numberOfPointsRatio = pointCount / (parentVolume.x * parentVolume.y * parentVolume.z);
        if(activeFeatures[F_NUM_POINTS_RATIO]) {
            double numberOfPointsRatio = pointCount / (double)fullCloud.points.size(); // this assumes that full cloud contains only the person
            featureVector.push_back(numberOfPointsRatio);
        }
    }
    #endif

    //
    // Added after ICRA'15 paper: Width, depth, height of voxel content
    //
    #ifdef USE_EXTENT_FEATURES
    {
        bool needExtents = activeFeatures[F_WIDTH] || activeFeatures[F_DEPTH] || activeFeatures[F_HEIGHT] || activeFeatures[F_ASPECT_RATIO_XY];

        if(needExtents) {
            double xmin = +std::numeric_limits<double>::infinity(), xmax = -std::numeric_limits<double>::infinity();
            double ymin = +std::numeric_limits<double>::infinity(), ymax = -std::numeric_limits<double>::infinity();
            double zmin = +std::numeric_limits<double>::infinity(), zmax = -std::numeric_limits<double>::infinity();

            foreach(int pointIndex, pointIndices) {
                const PointType& point = fullCloud.points[pointIndex];

                if(point.x < xmin) xmin = point.x;
                if(point.y < ymin) ymin = point.y;
                if(point.z < zmin) zmin = point.z;

                if(point.x > xmax) xmax = point.x;
                if(point.y > ymax) ymax = point.y;
                if(point.z > zmax) zmax = point.z;
            }

            if(activeFeatures[F_WIDTH])  featureVector.push_back( ymax - ymin );
            if(activeFeatures[F_DEPTH])  featureVector.push_back( xmax - xmin );
            if(activeFeatures[F_HEIGHT]) featureVector.push_back( zmax - zmin );

            if(activeFeatures[F_ASPECT_RATIO_XY]) featureVector.push_back( (xmax - xmin) / (ymax - ymin) );
        }
    }
    #endif

    //
    // Added after ICRA'15 paper: HSV color features
    //
    #ifdef USE_HSV_FEATURES
    bool needHSVMeans = activeFeatures[F_HSV_MEAN_H] || activeFeatures[F_HSV_MEAN_S] || activeFeatures[F_HSV_MEAN_V];
    bool needHSVStdDevs = activeFeatures[F_HSV_STDDEV_H] || activeFeatures[F_HSV_STDDEV_S] || activeFeatures[F_HSV_STDDEV_V];
    if(needHSVMeans || needHSVStdDevs)
    {
        Eigen::Vector3d hsvMean = Eigen::Vector3d::Zero();
        std::vector<Eigen::Vector3d> hsvPoints; // for feature: HSV std devs

        foreach(int pointIndex, pointIndices) {
            const PointType& point = fullCloud.points[pointIndex];

            // Get RGB components
            unsigned char r = point.r;
            unsigned char g = point.g;
            unsigned char b = point.b;

            // Compute HSV vector
            int min;    // min. value of RGB
            int max;    // max. value of RGB
            int delta;  // delta RGB value in current sample

            if (r > g) { min = g; max = r; }
            else { min = r; max = g; }
            if (b > max) max = b;
            if (b < min) min = b;

            delta = max - min;
            float h = 0, s;
            float v = max;

            if (delta == 0) {
                h = 0; s = 0;
            }
            else {
                s = delta / 255.0f;
                if ( r == max )         h = (      (g - b)/(float)delta) * 60.0;
                else if ( g == max )    h = ( 2 +  (b - r)/(float)delta) * 60.0;
                else if ( b == max )    h = ( 4 +  (r - g)/(float)delta) * 60.0;
            }

            Eigen::Vector3d hsv;
            hsv(0) = h; hsv(1) = s * 100.0; hsv(2) = v * 100.0;

            // for feature: HSV means, HSV std devs
            hsvMean += hsv;

            // for feature: HSV std devs
            hsvPoints.push_back(hsv);
        }

        // for feature: HSV means
        if(activeFeatures[F_HSV_MEAN_H]) featureVector.push_back( hsvMean(0) );
        if(activeFeatures[F_HSV_MEAN_S]) featureVector.push_back( hsvMean(1) );
        if(activeFeatures[F_HSV_MEAN_V]) featureVector.push_back( hsvMean(2) );

        // for feature: HSV std devs
        if(needHSVStdDevs) {
            Eigen::Vector3d sumOfSquaredHSVDifferences = Eigen::Vector3d::Zero();
            foreach(const Eigen::Vector3d& hsv, hsvPoints) {
                Eigen::Vector3d meanDiff = hsv - hsvMean;
                Eigen::Vector3d meanDiffDotProduct = meanDiff.cwiseProduct(meanDiff);
                sumOfSquaredHSVDifferences += meanDiffDotProduct;
            }

            if(activeFeatures[F_HSV_STDDEV_H]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredHSVDifferences(0)) );
            if(activeFeatures[F_HSV_STDDEV_S]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredHSVDifferences(1)) );
            if(activeFeatures[F_HSV_STDDEV_V]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredHSVDifferences(2)) );
        }
    } // end HSV color features
    #endif

    //
    // Added after ICRA'15 paper: YCbCr features
    //
    #ifdef USE_YCBCR_FEATURES
    bool needYCbCrMeans = activeFeatures[F_YCBCR_MEAN_Y] || activeFeatures[F_YCBCR_MEAN_CB] || activeFeatures[F_YCBCR_MEAN_CR];
    bool needYCbCrStdDevs = activeFeatures[F_YCBCR_STDDEV_Y] || activeFeatures[F_YCBCR_STDDEV_CB] || activeFeatures[F_YCBCR_STDDEV_CR];
    if(needYCbCrMeans || needYCbCrStdDevs)
    {
        Eigen::Vector3d ycbcrMean = Eigen::Vector3d::Zero();
        std::vector<Eigen::Vector3d> ycbcrPoints; // for feature: YCbCr std devs

        foreach(int pointIndex, pointIndices) {
            const PointType& point = fullCloud.points[pointIndex];

            // Get RGB components
            unsigned char r = point.r;
            unsigned char g = point.g;
            unsigned char b = point.b;

            // Compute YCbCr vector
            double y  =  0.299   * r + 0.587   * g + 0.114   * b;
            double cb = -0.16874 * r - 0.33126 * g + 0.50000 * b;
            double cr =  0.50000 * r - 0.41869 * g - 0.08131 * b;

            Eigen::Vector3d ycbcr;
            ycbcr(0) = y; ycbcr(1) = cb; ycbcr(2) = cr;

            // for feature: YCbCr means, YCbCr std devs
            ycbcrMean += ycbcr;

            // for feature: YCbCr std devs
            ycbcrPoints.push_back(ycbcr);
        }

        // for feature: YCbCr means
        if(activeFeatures[F_YCBCR_MEAN_Y])  featureVector.push_back( ycbcrMean(0) );
        if(activeFeatures[F_YCBCR_MEAN_CB]) featureVector.push_back( ycbcrMean(1) );
        if(activeFeatures[F_YCBCR_MEAN_CR]) featureVector.push_back( ycbcrMean(2) );

        // for feature: YCbCr std devs
        if(needYCbCrStdDevs) {
            Eigen::Vector3d sumOfSquaredYCbCrDifferences = Eigen::Vector3d::Zero();
            foreach(const Eigen::Vector3d& ycbcr, ycbcrPoints) {
                Eigen::Vector3d meanDiff = ycbcr - ycbcrMean;
                Eigen::Vector3d meanDiffDotProduct = meanDiff.cwiseProduct(meanDiff);
                sumOfSquaredYCbCrDifferences += meanDiffDotProduct;
            }

            if(activeFeatures[F_YCBCR_STDDEV_Y])  featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredYCbCrDifferences(0)) );
            if(activeFeatures[F_YCBCR_STDDEV_CB]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredYCbCrDifferences(1)) );
            if(activeFeatures[F_YCBCR_STDDEV_CR]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredYCbCrDifferences(2)) );
          }
    } // end YCbCr color features
    #endif

    //
    // Added after ICRA'15 paper: RGB color features
    //
    #ifdef USE_RGB_FEATURES
    bool needRGBMeans = activeFeatures[F_RGB_MEAN_R] || activeFeatures[F_RGB_MEAN_G] || activeFeatures[F_RGB_MEAN_B];
    bool needRGBStdDevs = activeFeatures[F_RGB_STDDEV_R] || activeFeatures[F_RGB_STDDEV_G] || activeFeatures[F_RGB_STDDEV_B];
    if(needRGBMeans || needRGBStdDevs)
    {
        Eigen::Vector3d rgbMean = Eigen::Vector3d::Zero();
        std::vector<Eigen::Vector3d> rgbPoints; // for feature: RGB std devs

        foreach(int pointIndex, pointIndices) {
            const PointType& point = fullCloud.points[pointIndex];

            // Get RGB components
            Eigen::Vector3d rgb(point.r, point.g, point.b);

            // for feature: RGB means, RGB std devs
            rgbMean += rgb;

            // for feature: RGB std devs
            rgbPoints.push_back(rgb);
        }

        // for feature: RGB means
        if(activeFeatures[F_RGB_MEAN_R]) featureVector.push_back( rgbMean(0) );
        if(activeFeatures[F_RGB_MEAN_G]) featureVector.push_back( rgbMean(1) );
        if(activeFeatures[F_RGB_MEAN_B]) featureVector.push_back( rgbMean(2) );

        // for feature: RGB std devs
        if(needRGBStdDevs) {
            Eigen::Vector3d sumOfSquaredRGBDifferences = Eigen::Vector3d::Zero();
            foreach(const Eigen::Vector3d& rgb, rgbPoints) {
                Eigen::Vector3d meanDiff = rgb - rgbMean;
                Eigen::Vector3d meanDiffDotProduct = meanDiff.cwiseProduct(meanDiff);
                sumOfSquaredRGBDifferences += meanDiffDotProduct;
            }

            if(activeFeatures[F_RGB_STDDEV_R]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredRGBDifferences(0)) );
            if(activeFeatures[F_RGB_STDDEV_G]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredRGBDifferences(1)) );
            if(activeFeatures[F_RGB_STDDEV_B]) featureVector.push_back( sqrt(1/(pointCount - 1) * sumOfSquaredRGBDifferences(2)) );
        }
    } // end RGB color features
    #endif

    // Safety check that we got the correct number of features
    #ifdef _DEBUG
        size_t expectedFeatureVectorLength = 0;
        for(size_t i = 0; i < activeFeatures.size(); i++) expectedFeatureVectorLength += activeFeatures[i] ? 1 : 0;
        ROS_ASSERT(featureVector.size() == expectedFeatureVectorLength);
    #endif
}

size_t FeatureCalculator::getFeatureCount() const {
    return F_COUNT;
}


#include <iostream>
#include <random>

#include <boost/program_options.hpp>

#include <IO/ReadBufferFromFileDescriptor.h>
#include <IO/WriteBufferFromFileDescriptor.h>
#include <IO/WriteBufferFromFile.h>

#include "MarkovModel.h"

using namespace DB;

int main(int argc, char ** argv)
try
{
    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        ("create", "create model")
        ("order", boost::program_options::value<unsigned>(), "order of model to create")
        ("noise", boost::program_options::value<double>(), "relative random noise to apply to created model")
        ("generate", "generate random strings with model")
        ("max-string-size", boost::program_options::value<UInt64>()->default_value(10000), "maximum size of generated string")
        ("limit", boost::program_options::value<UInt64>(), "stop after specified count of generated strings")
        ("seed", boost::program_options::value<UInt64>(), "seed passed to random number generator")
    ;

    boost::program_options::variables_map options;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), options);

    auto show_usage = [&]
    {
        std::cout << "Usage: \n"
            << argv[0] << " --create --order=N < strings.tsv > model\n"
            << argv[0] << " --generate < model > strings.tsv\n\n";
        std::cout << desc << std::endl;
    };

    if (options.count("help"))
    {
        show_usage();
        return 1;
    }

    ReadBufferFromFileDescriptor in(STDIN_FILENO);
    WriteBufferFromFileDescriptor out(STDOUT_FILENO);

    std::mt19937 random;

    if (options.count("seed"))
        random.seed(options["seed"].as<UInt64>());

    if (options.count("create"))
    {
        MarkovModel model(options["order"].as<unsigned>());

        String s;
        while (!in.eof())
        {
            readText(s, in);
            assertChar('\n', in);

            model.consume(s.data(), s.size());
        }

        if (options.count("noise"))
        {
            double noise = options["noise"].as<double>();
            model.modifyCounts([&](UInt32 count)
            {
                double modified = std::normal_distribution<double>(count, count * noise)(random);
                if (modified < 1)
                    modified = 1;

                return std::round(modified);
            });
        }

        model.write(out);
    }
    else if (options.count("generate"))
    {
        MarkovModel model;
        model.read(in);
        String s;

        UInt64 limit = options.count("limit") ? options["limit"].as<UInt64>() : 0;
        UInt64 max_string_size = options["max-string-size"].as<UInt64>();

        for (size_t i = 0; limit == 0 || i < limit; ++i)
        {
            s.resize(max_string_size);
            s.resize(model.generate(&s[0], s.size(), [&]{ return random(); }));

            writeText(s, out);
            writeChar('\n', out);
        }
    }
    else
    {
        show_usage();
        return 1;
    }

    return 0;
}
catch (...)
{
    std::cerr << getCurrentExceptionMessage(true) << '\n';
    throw;
}


#ifndef CANNON_GEOM_GRAPH_H
#define CANNON_GEOM_GRAPH_H

#include <boost/graph/adjacency_list.hpp>

/*!
 * \file cannon/geom/graph.hpp
 * \brief File containing Graph class definition and utility functions.
 */

namespace cannon {
  namespace geom {

    using EdgeProperty = boost::property<boost::edge_weight_t, double>;

    using BGraph =
        boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS,
                              boost::no_property,
                              EdgeProperty>;

    using Vertex = boost::graph_traits<BGraph>::vertex_descriptor;

    /*!
     * \brief Class representing a directed graph composed of nodes and edges,
     * with edge weights. Primarily a wrapper around Boost graphs.
     */
    class Graph {
    public:

      /*!
       * \brief Default constructor.
       */
      Graph() : weights_(get(boost::edge_weight, g_)) {}

      /*!
       * \brief Adds an edge to this graph, implicitly creating the
       * referenced vertices if they don't already exist.
       *
       * \param u "From" vertex
       * \param v "To" vertex
       * \param weight Weight for the edge
       */
      void add_edge(Vertex u, Vertex v, double weight);

      /*!
       * \brief Conduct an A* search on this graph, using the input heuristic.
       *
       * \param start Start vertex for the search.
       * \param goal Goal vertex for the search.
       * \param heur A* heuristic
       *
       * \returns The shortest path from start to goal, as computed by A*
       */
      std::vector<Vertex> astar(Vertex start, Vertex goal,
                                std::function<double(Vertex)> heur);

      /*!
       * \brief Get the number of vertices in this graph.
       *
       * \returns The number of vertices.
       */
      unsigned int num_vertices() {
        return boost::num_vertices(g_);
      }

    private:
      BGraph g_; //!< Internal graph representation
      boost::property_map<BGraph, boost::edge_weight_t>::type
          weights_; //!< Weight property map
    };

  }
}

#endif /* ifndef CANNON_GEOM_GRAPH_H */