/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <sstream>
#include <random>

#include <boost/math/constants/constants.hpp>

// include before <Eigen/*> !
#include "test_tomographer.h"

#include <tomographer/densedm/tspacellhwalker.h>

#include <unsupported/Eigen/MatrixFunctions>

#include <tomographer/densedm/param_herm_x.h>
#include <tomographer/densedm/densellh.h>
#include <tomographer/densedm/indepmeasllh.h>

#include <tomographer/tools/boost_test_logger.h>

// -----------------------------------------------------------------------------
// fixture(s)

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_densedm_tspacellhwalker)
// =============================================================================

TOMO_STATIC_ASSERT_EXPR(Tomographer::DenseDM::TSpace::LLHMHWalker<
                        Tomographer::DenseDM::IndepMeasLLH<Tomographer::DenseDM::DMTypes<2> >,
                        std::mt19937, Tomographer::Logger::VacuumLogger
                        >::UseFnSyntaxType == (int)Tomographer::MHUseFnLogValue) ;

BOOST_AUTO_TEST_CASE(tspacellhmhwalker)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> DenseLLH;
  DenseLLH llh(dmt);

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

  DenseLLH::VectorParamListType Exn(6, dmt.dim2());
  Exn <<
    0.5, 0.5,  SQRT22,  0,
    0.5, 0.5, -SQRT22,  0,
    0.5, 0.5,  0,       SQRT22,
    0.5, 0.5,  0,      -SQRT22,
    1,   0,    0,       0,
    0,   1,    0,       0
    ;
  DenseLLH::FreqListType Nx(6);
  Nx << 1500, 800, 300, 300, 10, 30;

  llh.setMeas(Exn, Nx, false);

  typedef Tomographer::Logger::BoostTestLogger LoggerType;
  LoggerType logger(Tomographer::Logger::DEBUG);

  std::mt19937 rng(46570); // seeded rng, deterministic results

  Tomographer::DenseDM::TSpace::LLHMHWalker<DenseLLH, std::mt19937, LoggerType>
    dmmhrw(DMTypes::MatrixType::Zero(), llh, rng, logger);

  DMTypes::MatrixType rho(dmt.initMatrixType());
  rho << 0.8, dmt.cplx(0,0.1),
    dmt.cplx(0,-0.1), 0.2;

  DMTypes::VectorParamType x(dmt.initVectorParamType());
  x = Tomographer::DenseDM::ParamX<DMTypes>(dmt).HermToX(rho);
  DMTypes::MatrixType T(dmt.initMatrixType());
  T = rho.sqrt();

  dmmhrw.init();
  dmmhrw.thermalizingDone();

  const DMTypes::MatrixType Tconst(T); // make sure that fnlogval() accepts const argument
  BOOST_CHECK_CLOSE(dmmhrw.fnLogVal(Tconst), llh.logLikelihoodX(x), tol_percent);

  {
    // Check that the jump distribution is symmetric.  Idea: draw many samples, and
    // average them in linear space (in which T is a hypersphere): normalize that, and you
    // should get T again.
    DMTypes::MatrixType sumT(dmt.initMatrixType());
    const int N_SAMPLES = 10000;
    int num_samples;
    for (num_samples = 0; num_samples < N_SAMPLES; ++num_samples) {
      DMTypes::MatrixType newT = dmmhrw.jumpFn(T, 0.2);
      BOOST_CHECK_CLOSE(newT.norm(), 1.0, tol_percent);
      sumT += newT;
    }
    //sumT /= num_samples; // ### unnecessary, already included in next step:
    sumT /= sumT.norm();
    MY_BOOST_CHECK_EIGEN_EQUAL(sumT, T, 1.0/std::sqrt((double)num_samples));
  }

  dmmhrw.done();
}

BOOST_AUTO_TEST_CASE(tspacellhmhwalkerlight)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> DenseLLH;
  DenseLLH llh(dmt);

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

  DenseLLH::VectorParamListType Exn(6, dmt.dim2());
  Exn <<
    0.5, 0.5,  SQRT22,  0,
    0.5, 0.5, -SQRT22,  0,
    0.5, 0.5,  0,       SQRT22,
    0.5, 0.5,  0,      -SQRT22,
    1,   0,    0,       0,
    0,   1,    0,       0
    ;
  DenseLLH::FreqListType Nx(6);
  Nx << 1500, 800, 300, 300, 10, 30;

  llh.setMeas(Exn, Nx, false);

  typedef Tomographer::Logger::BoostTestLogger LoggerType;
  LoggerType logger(Tomographer::Logger::DEBUG);

  std::mt19937 rng(46570); // seeded rng, deterministic results

  Tomographer::DenseDM::TSpace::LLHMHWalkerLight<DenseLLH, std::mt19937, LoggerType>
    dmmhrw(DMTypes::MatrixType::Zero(), llh, rng, logger);

  DMTypes::MatrixType rho(dmt.initMatrixType());
  rho << 0.8, dmt.cplx(0,0.1),
    dmt.cplx(0,-0.1), 0.2;

  DMTypes::VectorParamType x(dmt.initVectorParamType());
  x = Tomographer::DenseDM::ParamX<DMTypes>(dmt).HermToX(rho);
  DMTypes::MatrixType T(dmt.initMatrixType());
  T = rho.sqrt();

  dmmhrw.init();
  dmmhrw.thermalizingDone();

  const DMTypes::MatrixType Tconst(T); // make sure that fnlogval() accepts const argument
  BOOST_CHECK_CLOSE(dmmhrw.fnLogVal(Tconst), llh.logLikelihoodX(x), tol_percent);

  {
    // Check that the jump distribution is symmetric.  Idea: draw many samples, and
    // average them in linear space (in which T is a hypersphere): normalize that, and you
    // should get T again.
    DMTypes::MatrixType sumT(dmt.initMatrixType());
    const int N_SAMPLES = 10000;
    int num_samples;
    for (num_samples = 0; num_samples < N_SAMPLES; ++num_samples) {
      DMTypes::MatrixType newT = dmmhrw.jumpFn(T, 0.2);
      BOOST_CHECK_CLOSE(newT.norm(), 1.0, tol_percent);
      sumT += newT;
    }
    //sumT /= num_samples; // ### unnecessary, already included in next step:
    sumT /= sumT.norm();
    MY_BOOST_CHECK_EIGEN_EQUAL(sumT, T, 1.0/std::sqrt((double)num_samples));
  }

  dmmhrw.done();
}

// =============================================================================
BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright 2021 Ray Glover
 * Licensed under the Apache License, Version 2.0
 */

#include <cmath>
#include <napi.h>
#include <dlib/matrix.h>
#include <dlib/global_optimization.h>

Napi::Value JSThrow(Napi::Env& env, const std::string& err)
{
    Napi::Error::New(env, err).ThrowAsJavaScriptException();
    return env.Undefined();
}

Napi::Value find_global(const Napi::CallbackInfo& info)
{
    Napi::Env env = info.Env();

    if (info.Length() < 8) {
       return JSThrow(env, "Expected 8 arguments");
    }

    const auto objective_fn = info[0].As<Napi::Function>();
    const auto x_lower      = info[1].As<Napi::Float64Array>();
    const auto x_upper      = info[2].As<Napi::Float64Array>();
    const auto x_is_int     = info[3].As<Napi::Uint8Array>();
    const auto max_iter     = dlib::max_function_calls(info[4].As<Napi::Number>().Int32Value());
    const auto eps          = info[5].As<Napi::Number>().DoubleValue();
    const auto duration     = !info[6].IsUndefined()
        ? std::chrono::milliseconds(info[6].As<Napi::Number>().Int32Value())
        : dlib::FOREVER;

    // Minimize or maximize the objective function?
    auto y_mult = info[7].As<Napi::Number>().Int32Value();

    const size_t N = x_lower.ElementLength();
    if (N != x_upper.ElementLength() || N != x_is_int.ElementLength()) {
        return JSThrow(env, "Expected bounds arrays of length " + std::to_string(N));
    }

    // Wrap the objective function so it can be called by dlib
    auto x_js = Napi::Float64Array::New(env, N);

    auto objective_wrapper = [&, N](const dlib::matrix<double, 0, 1>& x) {
        for (size_t n = 0; n < N; n++) { x_js[n] = x(n); }

        const double value { objective_fn.Call(env.Global(), { x_js }).As<Napi::Number>() };
        if (std::isnan(value)) {
            // dlib::find_max_global will segfault on encountering nan's.
            throw dlib::error("Objective function returned a NaN.");
        }
        return value;
    };

    // Define the bounds
    const dlib::matrix<double, 0, 1> lower_bounds = dlib::mat(x_lower.Data(), N);
    const dlib::matrix<double, 0, 1> upper_bounds = dlib::mat(x_upper.Data(), N);

    // Flag integer variables in the domain
    std::vector<bool> is_int(N, false);
    for (size_t n = 0; n < N; n++) { is_int[n] = x_is_int[n] == 1; }

    // Optimize
    dlib::function_evaluation result;

    try {
        result = y_mult == 1
            ? dlib::find_max_global(objective_wrapper, lower_bounds, upper_bounds, is_int, max_iter, duration, eps)
            : dlib::find_min_global(objective_wrapper, lower_bounds, upper_bounds, is_int, max_iter, duration, eps);
    } catch (dlib::error& e) {
        return JSThrow(env, "(Solver error) " + std::string(e.what()));
    }

    // Copy the result x vector
    for (size_t n = 0; n < N; n++) { x_js[n] = result.x(n); }

    // Output as { x: number[], y: number }
    Napi::Object resultJs = Napi::Object::New(env);
    resultJs["y"] = result.y;
    resultJs["x"] = x_js;

    return resultJs;
}

Napi::Object Init(Napi::Env env, Napi::Object exports)
{
    exports.Set(Napi::String::New(env, "find_global"), Napi::Function::New(env, find_global));
    return exports;
}

NODE_API_MODULE(NODE_GYP_MODULE_NAME, Init)


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <boost/python.hpp>

#include <ndhist/stats/mean.hpp>

namespace bp = boost::python;

namespace ndhist {

namespace stats {

void register_mean()
{
    bp::def("mean"
      , &py::mean
      , ( bp::arg("hist")
        , bp::arg("axis")=bp::object()
        )
      , "Calculates the mean value along the given axis of the given ndhist  \n"
        "object.                                                             \n"
        "Since the mean is equal to the first order expectation, this        \n"
        "function just calls the ``ndhist.stats.expectation`` function to    \n"
        "calculate the first order expectation value.                        \n"
        "If ``None`` is given as axis argument (the default), the mean value \n"
        "for all individual axes of the ndhist object is calculated and      \n"
        "returned as a tuple. But if the dimensionality of the histogram is  \n"
        "1, a scalar value is returned.                                      \n"
        "                                                                    \n"
        ".. note:: This function is only defined for ndhist objects with POD \n"
        "          type axis values AND POD type weight values.              \n"
    );
}

}// namespace stats
}// namespace ndhist


//
// This file is part of an OMNeT++/OMNEST simulation example.
//
// Copyright (C) 1992-2015 Andras Varga
//
// This file is distributed WITHOUT ANY WARRANTY. See the file
// `license' for details on this and other legal matters.
//

#ifdef _MSC_VER
#pragma warning(disable : 4786)
#endif

#include <vector>
#include <omnetpp.h>
#include <ctime>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/discrete_distribution.hpp>

#include "Packet_m.h"
#include "../builder/control_m.h"
#include "../utils/average_queue.hpp"
#include "../utils/average_diff_queue.hpp"
using namespace omnetpp;

enum
{
    INIT = 0,
    WAITING_FOR_TRAFFIC_MATRIX = FSM_Transient(1),
    SENDING_PACKAGE = FSM_Steady(1)
};

/**
 * Generates traffic for the network.
 */
class App : public cSimpleModule
{
private:
    // configuration
    int myAddress;
    simtime_t measureInterval;
    simtime_t lastMeasureCycle;
    std::vector<double> trafficDensity;

    double totalTrafficDensity;
    cPar *packetLengthBytes;

    // state
    cMessage *generatePacket;
    long pkCounter;

    // boost-based random generation
    int seed;
    boost::random::mt19937 rng;
    boost::random::exponential_distribution<double> randomWaitTime;
    boost::random::discrete_distribution<int, double> randomTarget;
    cFSM fsm;

    // statistics
    AverageQueue<int, double> endToEndLatency;
    AverageDiffQueue<int, double> endToEndJitter;

public:
    App();
    virtual ~App();

protected:
    virtual void initialize() override;
    virtual void finish() override;
    virtual void handleMessage(cMessage *msg) override;
    double getWaitTime();
    int getTarget();
};

Define_Module(App);

App::App()
{
    generatePacket = nullptr;
}

App::~App()
{
    cancelAndDelete(generatePacket);
}

double App::getWaitTime()
{
    return this->randomWaitTime(this->rng);
}

int App::getTarget()
{
    return this->randomTarget(this->rng) + 1;
}

void App::initialize()
{
    myAddress = par("address");
    measureInterval = par("measureInterval");
    packetLengthBytes = &par("packetLength");

    this->seed = static_cast<int>(par("seed").doubleValue());

    pkCounter = 0;

    lastMeasureCycle = 0;
}

void App::finish()
{
    auto result = this->endToEndLatency.fetch();

    for (int i = 0; i < result.size(); i++)
    {
        for (auto &pair : result[i])
        {
            std::stringstream ss;
            ss << "end_to_end_latency_" << i << "_" << pair.first << "_" << this->myAddress;
            recordScalar(ss.str().c_str(), pair.second);
        }
    }

    auto jitterResult = this->endToEndJitter.fetch();

    for (int i = 0; i < result.size(); i++)
    {
        for (auto& pair : result[i])
        {
            std::stringstream ss;
            ss << "end_to_end_jitter_" << i << "_" << pair.first << "_" << this->myAddress;
            recordScalar(ss.str().c_str(), pair.second);
        }
    }
}

void App::handleMessage(cMessage *msg)
{
    FSM_Switch(fsm)
    {
    case FSM_Exit(INIT):
        FSM_Goto(fsm, WAITING_FOR_TRAFFIC_MATRIX);
        break;
    case FSM_Exit(WAITING_FOR_TRAFFIC_MATRIX):
        if (true)
        {
            auto controlPacket = check_and_cast<Control *>(msg);
            EV << "node " << this->myAddress << " receiving command " << controlPacket->getCommand() << std::endl;
            std::vector<double> tokens = cStringTokenizer(controlPacket->getCommand()).asDoubleVector();

            this->totalTrafficDensity = 0;
            int tokensNumber = 0;
            for (auto trafficDensity : tokens)
            {
                this->trafficDensity.push_back(trafficDensity);
                this->totalTrafficDensity += trafficDensity;
                tokensNumber += 1;
            }
            EV << "node " << this->myAddress << " total sending traffic density " << this->totalTrafficDensity << " " << std::endl;
            EV << "node " << this->myAddress << " seed is " << this->seed << std::endl;
            EV << "node " << this->myAddress << " number of total receiving tokens are " << tokensNumber << std::endl;
            this->rng.seed(this->seed);

            this->randomWaitTime = boost::random::exponential_distribution<double>((1e9 / 1.2e4) * (this->totalTrafficDensity / (this->trafficDensity.size() - 1)));
            std::for_each(this->trafficDensity.begin(), this->trafficDensity.end(), [this](double &n) { n /= this->totalTrafficDensity; });
            this->randomTarget = boost::random::discrete_distribution<int, double>(this->trafficDensity);
            delete controlPacket;

            generatePacket = new cMessage("nextPacket");
            scheduleAt(simTime() + this->getWaitTime(), generatePacket);

            std::vector<int> l(this->trafficDensity.size());
            std::iota(l.begin(), l.end(), 1);

            this->endToEndLatency = AverageQueue<int, double>(l, 0.0);
            this->endToEndJitter = AverageDiffQueue<int, double>(l, 0.0);

            FSM_Goto(fsm, SENDING_PACKAGE);
            break;
        }
    case FSM_Enter(SENDING_PACKAGE):
        break;
    case FSM_Exit(SENDING_PACKAGE):
        if (msg == generatePacket)
        {
            // Sending packet
            int destAddress = this->getTarget();

            char pkname[40];
            sprintf(pkname, "pk-%d-to-%d-#%ld", myAddress, destAddress, pkCounter++);

            Packet *pk = new Packet(pkname);
            pk->setByteLength(this->packetLengthBytes->intValue());
            pk->setKind(intuniform(0, 7));
            pk->setSrcAddr(myAddress);
            pk->setDestAddr(destAddress);
            send(pk, "out");

            scheduleAt(simTime() + this->getWaitTime(), generatePacket);
        }
        else
        {
            // Handle incoming packet
            Packet *pk = check_and_cast<Packet *>(msg);

            if (simTime() > this->lastMeasureCycle + this->measureInterval)
            {
                this->endToEndLatency.fresh();
                this->endToEndJitter.fresh();
                this->lastMeasureCycle = this->lastMeasureCycle + this->measureInterval;
                this->endToEndLatency.push(pk->getSrcAddr(), static_cast<const double>(SIMTIME_DBL(simTime() - pk->getCreationTime())));
                this->endToEndJitter.push(pk->getSrcAddr(), static_cast<const double>(SIMTIME_DBL(simTime() - pk->getCreationTime())));
            }
            else
            {
                this->endToEndLatency.push(pk->getSrcAddr(), static_cast<const double>(SIMTIME_DBL(simTime() - pk->getCreationTime())));
                this->endToEndJitter.push(pk->getSrcAddr(), static_cast<const double>(SIMTIME_DBL(simTime() - pk->getCreationTime())));
            }

            delete pk;
        }
        break;
    }
}


#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <ctime>
#include <algorithm>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <ros/ros.h>
#include <ros/package.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"

#include <object_slam/Object_landmark.h>
#include <object_slam/Frame.h>
#include <object_slam/g2o_Object.h>

#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/detect_3d_cuboid.h"

#include "line_lbd/line_lbd_allclass.h"

using namespace std;
using namespace Eigen;

typedef pcl::PointCloud<pcl::PointXYZRGB> CloudXYZRGB;

// global variable
std::string base_folder;
bool online_detect_mode;
bool save_results_to_txt;
cv::Mat_<float> matx_to3d_, maty_to3d_;

// BRIEF 深度图转化为点云图参数.
void set_up_calibration(const Eigen::Matrix3f& calibration_mat,const int im_height,const int im_width)
{
    matx_to3d_.create(im_height, im_width);
    maty_to3d_.create(im_height, im_width);
    float center_x=calibration_mat(0,2);  //cx
    float center_y=calibration_mat(1,2);  //cy
    float fx_inv=1.0/calibration_mat(0,0);  // 1/fx
    float fy_inv=1.0/calibration_mat(1,1);  // 1/fy
    for (int v = 0; v < im_height; v++) {
	for (int u = 0; u < im_width; u++) {
	  matx_to3d_(v,u) = (u - center_x) * fx_inv;
	  maty_to3d_(v,u) = (v - center_y) * fy_inv;
	}
    }
}

// BRIEF 下采样点云图，深度图已经以 m 为单位.
// 输入：原始rgb图 rgb_img		深度图 depth_img 		相机真实位姿 transToWorld		采样之后的点云 point_cloud
void depth_to_cloud(const cv::Mat& rgb_img, const cv::Mat& depth_img,const Eigen::Matrix4f transToWorld, CloudXYZRGB::Ptr& point_cloud,bool downsample=false)
{
    pcl::PointXYZRGB pt;
    pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> vox_grid_;
    float close_depth_thre = 0.1;
    float far_depth_thre = 3.0;
      far_depth_thre = 3;
    int im_width = rgb_img.cols; int im_height= rgb_img.rows;
    for (int32_t i=0; i<im_width*im_height; i++)
	{      // row by row
		int ux=i % im_width; int uy=i / im_width;
		float pix_depth= depth_img.at<float>(uy,ux);
		if (pix_depth>close_depth_thre && pix_depth<far_depth_thre)
		{
			pt.z=pix_depth; pt.x=matx_to3d_(uy,ux)*pix_depth; pt.y=maty_to3d_(uy,ux)*pix_depth;
			Eigen::VectorXf global_pt=homo_to_real_coord_vec<float>(transToWorld*Eigen::Vector4f(pt.x,pt.y,pt.z,1));  // change to global position
			pt.x=global_pt(0); pt.y=global_pt(1); pt.z=global_pt(2);
			pt.r = rgb_img.at<cv::Vec3b>(uy,ux)[2]; pt.g = rgb_img.at<cv::Vec3b>(uy,ux)[1]; pt.b = rgb_img.at<cv::Vec3b>(uy,ux)[0];
			point_cloud->points.push_back(pt);
		}
    }
    if (downsample)
    {
		vox_grid_.setLeafSize(0.02,0.02,0.02);
		vox_grid_.setDownsampleAllData(true);
		vox_grid_.setInputCloud(point_cloud);
		vox_grid_.filter(*point_cloud);
    }
}

// BRIEF 前后标记立方体.
// one cuboid need front and back markers...
// 输入参数： 8个顶点		marker		id
void cuboid_corner_to_marker(const Matrix38d& cube_corners,visualization_msgs::Marker& marker, int bodyOrfront)
{
    Eigen::VectorXd edge_pt_ids;
	// NOTE 整体的边缘.
    if (bodyOrfront==0)
	{
		edge_pt_ids.resize(16);
		// NOTE 按顺序连接各个点，连成一个立方体
		edge_pt_ids << 1,2,3,4,1,5,6,7,8,5,6,2,3,7,8,4;
		edge_pt_ids.array()-=1;
    }
	// NOTE 正前方的边缘.
	else
	{
		edge_pt_ids.resize(5);
		// NOTE 按顺序连接各个点，平面
		edge_pt_ids<<1,2,6,5,1;
		edge_pt_ids.array()-=1;
    }

    marker.points.resize(edge_pt_ids.rows());
	// 确定各个点的位置，连成线.
    for (int pt_id = 0; pt_id < edge_pt_ids.rows(); pt_id++)
    {
		marker.points[pt_id].x = cube_corners(0, edge_pt_ids(pt_id));
		marker.points[pt_id].y = cube_corners(1, edge_pt_ids(pt_id));
		marker.points[pt_id].z = cube_corners(2, edge_pt_ids(pt_id));
    }
}

// BRIEF 立方体需要有前后标记，立方体模型的表达.
// 输入参数：立方体提案  颜色.
// one cuboid need front and back markers...  rgbcolor is 0-1 based
visualization_msgs::MarkerArray cuboids_to_marker(object_landmark* obj_landmark, Vector3d rgbcolor)
{
    visualization_msgs::MarkerArray plane_markers;
	visualization_msgs::Marker marker;

    if (obj_landmark==nullptr)
		return plane_markers;

	// 设置 marker.
    marker.header.frame_id="/world";
	marker.header.stamp=ros::Time::now();
    marker.id = 0; //0
    marker.type = visualization_msgs::Marker::LINE_STRIP;
	marker.action = visualization_msgs::Marker::ADD;
    marker.color.r = rgbcolor(0);
	marker.color.g = rgbcolor(1);
	marker.color.b = rgbcolor(2);
	marker.color.a = 1.0;
    marker.scale.x = 0.02;

	// 显示的立方体对象 cube_opti.
    g2o::cuboid cube_opti = obj_landmark->cube_vertex->estimate();
	// 立方体 8 个顶点的坐标.
    Eigen::MatrixXd cube_corners = cube_opti.compute3D_BoxCorner();
    // std::cout << "cube_corners:   \n" << cube_corners << std::endl;

	// 每个立方体需要两个 marker，一个用于所有的边缘，一个用于前面的边缘，可以具有不同的额颜色.
    for (int ii=0;ii<2;ii++) // each cuboid needs two markers!!! one for all edges, one for front facing edge, could with different color.
    {
		marker.id++;
		cuboid_corner_to_marker(cube_corners,marker, ii);
		plane_markers.markers.push_back(marker);
    }
    return plane_markers;
}

// BRIEF 将李代数表达的相机位姿 pose_Twc 转换成 geometry_msgs/Pose 消息.
geometry_msgs::Pose posenode_to_geomsgs(const g2o::SE3Quat &pose_Twc)
{
    geometry_msgs::Pose pose_msg;
    Eigen::Vector3d pose_trans = pose_Twc.translation();
    pose_msg.position.x=pose_trans(0);
    pose_msg.position.y=pose_trans(1);
    pose_msg.position.z=pose_trans(2);
    Eigen::Quaterniond pose_quat = pose_Twc.rotation();
    pose_msg.orientation.x = pose_quat.x();
    pose_msg.orientation.y = pose_quat.y();
    pose_msg.orientation.z = pose_quat.z();
    pose_msg.orientation.w = pose_quat.w();
    return pose_msg;
}

// BRIEF 将李代数表达的相机位姿 pose_Twc 加上标签 img_header 之后转换成 nav_msgs/Odometry 消息.
nav_msgs::Odometry posenode_to_odommsgs(const g2o::SE3Quat &pose_Twc,const std_msgs::Header &img_header)
{
    nav_msgs::Odometry odom_msg;
    odom_msg.pose.pose=posenode_to_geomsgs(pose_Twc);
    odom_msg.header=img_header;
    return odom_msg;
}

// BRIEF 发布每帧的原始和优化结果.
void publish_all_poses(std::vector<tracking_frame*> all_frames,std::vector<object_landmark*> cube_landmarks_history,
		       std::vector<object_landmark*> all_frame_rawcubes, Eigen::MatrixXd& truth_frame_poses)
{
	// STEP 1.定义 ROS 消息发布器和相关变量.
    ros::NodeHandle n;
	// 估计的和真实的相机【运动轨迹】
	ros::Publisher pub_slam_path = n.advertise<nav_msgs::Path>( "/slam_pose_paths", 10 );
    ros::Publisher pub_truth_path = n.advertise<nav_msgs::Path>( "/truth_pose_paths", 10 );
	// 估计和真实的【相机位姿】（一次性显示）.
    ros::Publisher pub_slam_all_poses = n.advertise<geometry_msgs::PoseArray>("/slam_pose_array", 10);
    ros::Publisher pub_truth_all_poses = n.advertise<geometry_msgs::PoseArray>("/truth_pose_array", 10);
	// 估计的和真实【相机位姿】(逐帧显示).
    ros::Publisher pub_slam_odompose = n.advertise<nav_msgs::Odometry>("/slam_odom_pose", 10);
    ros::Publisher pub_truth_odompose = n.advertise<nav_msgs::Odometry>("/truth_odom_pose", 10);
	// 【立方体模型】消息，分别是：最终路标，优化后模型，优化前模型.
    ros::Publisher pub_final_opti_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_opti", 10); //最终估计出的立方体提案.
    ros::Publisher pub_history_opti_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_opti_hist", 10); // 每次优化后的立方体路标
    ros::Publisher pub_frame_raw_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_raw_frame", 10);
    // 带有提案的【原始图像】.
	ros::Publisher pub_2d_cuboid_project = n.advertise<sensor_msgs::Image>("/cuboid_project_img", 10);
	// 【点云】消息.
    ros::Publisher raw_cloud_pub = n.advertise<CloudXYZRGB> ("/raw_point_cloud", 50);

    int total_frame_number = all_frames.size();

    // 估计的【相机位姿】.
    geometry_msgs::PoseArray all_pred_pose_array;    		// 估计位姿，存为数组，一次行发布所有.
	std::vector<nav_msgs::Odometry> all_pred_pose_odoms;	// 估计位姿，存为向量，逐帧发布.

	// 真实的【相机位姿】.
    geometry_msgs::PoseArray all_truth_pose_array;    		// 真实位姿，存为数组，一次性发布.
	std::vector<nav_msgs::Odometry> all_truth_pose_odoms;	// 真实位姿，存为向量，逐帧发布.

	// pose_header：指明在世界坐标系和时间戳.
    std_msgs::Header pose_header;    				// std_msgs/Header：通常用于在特定坐标系中传送带时间戳的数据.
	pose_header.frame_id = "/world";
	pose_header.stamp = ros::Time::now();

	// 【运动轨迹】，包括位姿和时间戳：path_preds，path_truths.
    nav_msgs::Path path_truths,path_preds;			// nav_msgs/Path：姿态数组，表示运动轨迹.
    path_preds.header = pose_header;
	path_truths.header = pose_header;

	// STEP 2.读取估计的优化之后的相机位姿.
    for (int i = 0; i < total_frame_number; i++)
    {
		// cam_pose_Twc：优化之后的相机位姿.
		// 保存优化之后的相机位姿和时间戳.
		// 分别保存为数组（一次性发布）和向量形式（逐帧发布）.
		all_pred_pose_array.poses.push_back(posenode_to_geomsgs(all_frames[i]->cam_pose_Twc));
		all_pred_pose_odoms.push_back(posenode_to_odommsgs(all_frames[i]->cam_pose_Twc,pose_header) );

		/*NOTE 作者原始代码在这里将估计的相机位姿存放在 path_preds 中，并在后面一次性发布，我给他放到后面逐帧发布了.
		// 位姿时间戳 postamp.
		geometry_msgs::PoseStamped postamp;
		postamp.pose = posenode_to_geomsgs(all_frames[i]->cam_pose_Twc);
		postamp.header = pose_header;
		// NOTE 最终将估计的位姿信息存储到 path_preds.
		path_preds.poses.push_back(postamp);
		*/
    }

	// STEP 3.读取真实的相机位姿..
    if (truth_frame_poses.rows()>0)
    {
		for (int i=0; i < total_frame_number;i++)
		{
			// 在自由空间中的位姿表示，包括位置和方向.
			geometry_msgs::Pose pose_msg;
			pose_msg.position.x=truth_frame_poses(i,1);
			pose_msg.position.y=truth_frame_poses(i,2);
			pose_msg.position.z=truth_frame_poses(i,3);

			pose_msg.orientation.x = truth_frame_poses(i,4);
			pose_msg.orientation.y = truth_frame_poses(i,5);
			pose_msg.orientation.z = truth_frame_poses(i,6);
			pose_msg.orientation.w = truth_frame_poses(i,7);

			// 真实位姿矩阵.
			all_truth_pose_array.poses.push_back(pose_msg);

			// 真实位姿的里程计信息（向量）.
			nav_msgs::Odometry odom_msg;
			odom_msg.pose.pose=pose_msg;
			odom_msg.header = pose_header;
			all_truth_pose_odoms.push_back(odom_msg);

			/*NOTE 作者原始代码在这里将真实的相机位姿存放在 path_truths 中，并在后面一次性发布，我给他放到后面逐帧发布了.
			geometry_msgs::PoseStamped postamp;
			postamp.pose = pose_msg;
			postamp.header = pose_header;
			// NOTE 最终将参考系下真实的位姿信息存储到 path_truths.
			path_truths.poses.push_back(postamp);
			*/
		}
    }

	// 指定坐标系和时间戳.
    all_pred_pose_array.header.stamp=ros::Time::now();
	all_pred_pose_array.header.frame_id="/world";
    all_truth_pose_array.header.stamp=ros::Time::now();
	all_truth_pose_array.header.frame_id="/world";

    // STEP 4.保存相机和物体的位姿.
    if (save_results_to_txt)  // record cam pose and object pose
    {
		// 写入相机位姿：时间戳、位置、方向.
		ofstream resultsFile;
		string resultsPath = base_folder + "output_cam_poses.txt";
		cout << "resultsPath  " << resultsPath << endl;
		resultsFile.open(resultsPath.c_str());
		resultsFile << "# timestamp tx ty tz qx qy qz qw"<<"\n";
		for (int i=0; i<total_frame_number; i++)
		{
			// 时间戳读取真实的相机的时间戳.
			double time_string=truth_frame_poses(i,0);
			ros::Time time_img(time_string);
			resultsFile << time_img<<"  ";
			// 读取位置信息，转换为向量形式.
			resultsFile << all_frames[i]->cam_pose_Twc.toVector().transpose()<<"\n";
		}
		resultsFile.close();

		// 写入物体位姿.
		ofstream objresultsFile;
		string objresultsPath = base_folder + "output_obj_poses.txt";
		objresultsFile.open(objresultsPath.c_str());
		for (size_t j=0;j<cube_landmarks_history.size();j++)
		{
			// 优化之后的相机位姿 cube_landmarks_history.
			g2o::cuboid cube_opti = cube_landmarks_history[j]->cube_vertex->estimate();
			// transform it to local ground plane.... suitable for matlab processing.
			objresultsFile << cube_opti.toMinimalVector().transpose()<<" "<<"\n";
		}
		objresultsFile.close();
    }

	// 数据集的相机参数.
    // sensor parameter for TUM cabinet data!
    Eigen::Matrix3f calib;
    float depth_map_scaling = 5000;
    calib<<535.4,      0, 320.1,
	           0,  539.2, 247.6,
	           0,      0,     1;
    set_up_calibration(calib,480,640);

	// STEP 5. 最终的立方体路标 finalcube_markers.
	// 传入参数：优化后提案 cube_landmarks_history 向量的最后一个元素，颜色绿色.
	// TODO finalcube_markers
    visualization_msgs::MarkerArray finalcube_markers = cuboids_to_marker(cube_landmarks_history.back(),Vector3d(0,1,0));

	// 是否显示点云信息，用于可视化.
    bool show_truth_cloud = true;  // show point cloud using camera pose. for visualization purpose

    pcl::PCLPointCloud2 pcd_cloud2;		// 好像没用到.

    ros::Rate loop_rate(5);  //5
    int frame_number = -1;

	// STEP 6. 发布每一帧对应的消息.
    while ( n.ok() )
    {
		frame_number++;

		// 是否直接一次性显示所有结果.
		if (0) // directly show final results
		{
			// 发布估计和真实的相机位姿.
			pub_slam_all_poses.publish(all_pred_pose_array);
			pub_truth_all_poses.publish(all_truth_pose_array);
			// 发布估计和真实的运动轨迹.
			pub_slam_path.publish(path_preds);
			pub_truth_path.publish(path_truths);
		}

		// STEP 6.1 发布最终确定的立方体模型.
		pub_final_opti_cube.publish(finalcube_markers);

		if (frame_number < total_frame_number)
		{
			// STEP 6.2 发布立方体位姿（优化前后）信息.
			// NOTE 在每帧经过G20优化之后发布其立方体路标，用红色表示 pub_history_opti_cube.
			if (cube_landmarks_history[frame_number]!=nullptr)
				pub_history_opti_cube.publish(cuboids_to_marker(cube_landmarks_history[frame_number],Vector3d(1, 0, 0)));
			// NOTE 发布优化之前每帧中原始的立方体，用蓝色表示 pub_frame_raw_cube.
			if (all_frame_rawcubes.size()>0 && all_frame_rawcubes[frame_number]!=nullptr)
				pub_frame_raw_cube.publish(cuboids_to_marker(all_frame_rawcubes[frame_number],Vector3d(0,0,1)));

			// STEP 6.3 发布相机位姿（估计和真实）信息.
			// NOTE 发布该帧对应的位姿信息.
			// 估计的位姿：pub_slam_odompose.
			// 真实的位姿：pub_truth_odompose.
			pub_slam_odompose.publish(all_pred_pose_odoms[frame_number]);
			pub_truth_odompose.publish(all_truth_pose_odoms[frame_number]);
			// 输出相机在世界中的坐标.
		    std::cout<<"Frame position x/y/z:  "<< frame_number <<"        "
			                                    << all_pred_pose_odoms[frame_number].pose.pose.position.x << "  "
											    << all_pred_pose_odoms[frame_number].pose.pose.position.y << "  "
											    << all_pred_pose_odoms[frame_number].pose.pose.position.z << std::endl;

			// 将 frame_number 化为4位字符 frame_index_c
			char frame_index_c[256];
			sprintf(frame_index_c,"%04d",frame_number);  // format into 4 digit

			// 读取该帧带有立方体提案的图像.
			cv::Mat cuboid_2d_proj_img = all_frames[frame_number]->cuboids_2d_img;

			// 读取原始 rgb 图像 raw_rgb_img.
			std::string raw_rgb_img_name = base_folder+"raw_imgs/" + std::string(frame_index_c) + "_rgb_raw.jpg";
			cv::Mat raw_rgb_img = cv::imread(raw_rgb_img_name, 1);

			// STEP 6.4 发布点云信息.
			// 隔帧显示点云图.
			if (show_truth_cloud && (truth_frame_poses.rows()>0))
			{
				if (frame_number%4==0) // show point cloud every N frames
				{
					// 读取点云图 raw_depth_img 并转换格式.
					std::string raw_depth_img_name = base_folder+"depth_imgs/" + std::string(frame_index_c) + "_depth_raw.png";
					cv::Mat raw_depth_img = cv::imread(raw_depth_img_name, CV_LOAD_IMAGE_ANYDEPTH);
					raw_depth_img.convertTo(raw_depth_img, CV_32FC1, 1.0/depth_map_scaling,0);

					// 读取点云信息 point_cloud.
					CloudXYZRGB::Ptr point_cloud(new CloudXYZRGB());

					// 读取真实的相机位姿，李代数表示
					Eigen::Matrix4f truth_pose_matrix=g2o::SE3Quat(truth_frame_poses.row(frame_number).segment<7>(1)).to_homogeneous_matrix().cast<float>();
					// std::cout<<"truth_pose_matrix:   \n"<< truth_pose_matrix << std::endl;

					// 下采样点云，否则太多.
					depth_to_cloud(raw_rgb_img, raw_depth_img, truth_pose_matrix, point_cloud, true); // need to downsample cloud, otherwise too many
					ros::Time curr_time=ros::Time::now();

					// 发布点云图消息.
					point_cloud->header.frame_id = "/world";
					point_cloud->header.stamp = (curr_time.toNSec() / 1000ull);
					raw_cloud_pub.publish(point_cloud);
				}
			}

			// STEP 6.5 发布原始图与提案消息.
			// 发布带有立方体轮廓的图像，通过 pub_2d_cuboid_project 发布.
			cv_bridge::CvImage out_image;
			out_image.header.stamp=ros::Time::now();
			out_image.image=cuboid_2d_proj_img;
			out_image.encoding=sensor_msgs::image_encodings::TYPE_8UC3;
			pub_2d_cuboid_project.publish(out_image.toImageMsg());

			// TODO 逐帧发布估计的轨迹 path_preds.
			geometry_msgs::PoseStamped postamp;
			postamp.pose = posenode_to_geomsgs(all_frames[frame_number]->cam_pose_Twc);
			postamp.header = pose_header;
			path_preds.poses.push_back(postamp);
			// 发布.
			pub_slam_path.publish(path_preds);

			// TODO 逐帧发布真实的轨迹 path_truths.
			geometry_msgs::PoseStamped postamp2;
			postamp2.pose = all_truth_pose_array.poses[frame_number];
			postamp2.header = pose_header;
			path_truths.poses.push_back(postamp2);
			// 发布.
			pub_truth_path.publish(path_truths);
		}

		// 结束可视化阶段.
		if (frame_number==int(all_pred_pose_odoms.size()))
		{
			cout<<"+++++++++++++Finish all visulialization!+++++++++++++"<<endl;
		}

		ros::spinOnce();
		loop_rate.sleep();
    }
}

// NOTE 在线检测模式不使用 pred_frame_objects 和 init_frame_poses.
//     truth_frame_poses 仅使用第一帧.
void incremental_build_graph(Eigen::MatrixXd& offline_pred_frame_objects, Eigen::MatrixXd& init_frame_poses, Eigen::MatrixXd& truth_frame_poses)
{
	// STEP 【1. 变量定义】
    // 设置 TUM 数据集相机内参calib.
    Eigen::Matrix3d calib;
    calib<<535.4,  0,  320.1,
	    0,  539.2, 247.6,
	    0,      0,     1;

    // 帧数，也即truth_frame_poses中的列数.
    int total_frame_number = truth_frame_poses.rows();

	// STEP 【1.2 定义立方体检测对象】
    // 检测所有帧中的立方体，定义为一个【detect_3d_cuboid】类的detect_cuboid_obj对象.
    detect_3d_cuboid detect_cuboid_obj;
    detect_cuboid_obj.whether_plot_detail_images = true;	// 不绘制检测细节图像.
    detect_cuboid_obj.whether_plot_final_images = false;	// 不绘制检测结果图.
    detect_cuboid_obj.print_details = false;  				// 不输出检测细节.
    detect_cuboid_obj.set_calibration(calib);				// 设置内参.
    detect_cuboid_obj.whether_sample_bbox_height = false;	// TODO
    detect_cuboid_obj.nominal_skew_ratio = 2;				// TODO
    detect_cuboid_obj.whether_save_final_images = true;		// 保存检测结果图.
	// STEP 【1.3 定义线检测对象】
    // 定义线检测【line_lbd_detect】类的line_lbd_obj对象.
    line_lbd_detect line_lbd_obj;
    line_lbd_obj.use_LSD = false;		// 使用 LSD 或 detector 线检测.
    line_lbd_obj.line_length_thres = 15;  	// 去除较短的边线.

    // STEP 【2.G2O图优化 graph optimization】
    //in this example, there is only one object!!! perfect association  假设只有一个对象！！！  TODO
    // STEP 【2.1 构造一个名为 graph 的求解器】
    g2o::SparseOptimizer graph;
    // STEP 【2.2 使用Cholmod中的线性方程求解器得到 linearSolver】
    g2o::BlockSolverX::LinearSolverType* linearSolver;
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
    // STEP 【2.3 再用稀疏矩阵块求解器 solver_ptr】
    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
    // STEP 【2.4使用梯度下降算法求解上面的 solver_ptr 得到 solver】
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

    graph.setAlgorithm(solver);		// 设置求解器.
    graph.setVerbose(false);		// 是否打开调试输出.

    // only first truth pose is used. to directly visually compare with truth pose. also provide good roll/pitch
    // 这里仅使用了第一帧的相机真实位姿，为了直接与真实位姿对比，也提供良好的 roll/pitch 角.
	// 构造一个四元数形式的 fixed_init_cam_pose_Twc 读取第一帧的位姿.
    g2o::SE3Quat fixed_init_cam_pose_Twc(truth_frame_poses.row(0).tail<7>());

    // 保存每帧的优化结果.
	// 每帧优化后的路标的位姿.
    std::vector<object_landmark*> cube_pose_opti_history(total_frame_number, nullptr);  //landmark pose after each frame's optimization
    // 优化之前每帧检测到的立方体帧.
	std::vector<object_landmark*> cube_pose_raw_detected_history(total_frame_number, nullptr); //raw detected cuboid frame each frame. before optimization

    int offline_cube_obs_row_id = 0;

	// @PARAM all_frames 存储每一帧的信息.
    std::vector<tracking_frame*> all_frames(total_frame_number);    // 一个 tracking_frame 向量.
    g2o::VertexCuboid* vCube;		// TODO 这个顶点将物体的位姿存储到世界.

    // 在线逐帧处理每一帧图像.
    for (int frame_index=0; frame_index<total_frame_number; frame_index++)
    {
		// @PARAM 李代数位姿表示.
		g2o::SE3Quat curr_cam_pose_Twc;		// 当前帧的位姿.
		g2o::SE3Quat odom_val; 				// 从上一帧到当前帧的旋转.

		// STEP 1. 计算每一帧的位姿.
		if (frame_index==0)
			curr_cam_pose_Twc = fixed_init_cam_pose_Twc;		// 读取第一帧的真实位姿.
		else
		{
			// @PARAM 上一帧的位姿：all_frames[frame_index-1] 的 Tcw.
			g2o::SE3Quat prev_pose_Tcw = all_frames[frame_index-1]->cam_pose_Tcw;
			// 从第三帧开始，使用恒定运动模型来初始化相机.
			// TODO 第 0 帧来自于真实位姿，第 2 帧通过里程计计算出来，第 1 帧呢？？
			if (frame_index > 1)  // from third frame, use constant motion model to initialize camera.
			{
				// 上上一帧的位姿 prev_prev_pose_Tcw.
				g2o::SE3Quat prev_prev_pose_Tcw = all_frames[frame_index-2]->cam_pose_Tcw;
				// 从上一帧到本帧的变换
				odom_val = prev_pose_Tcw*prev_prev_pose_Tcw.inverse();
			}
			// NOTE 计算当前帧的位姿（相机->相机） CT_wc =(T_odom)^-1
			curr_cam_pose_Twc = (odom_val*prev_pose_Tcw).inverse();
		}
		// std::cout << "第 " << frame_index << "帧的位姿：\n" << curr_cam_pose_Twc << std::endl;
		// std::cout << "odom_val：\n" << odom_val << std::endl;

	  // STEP 2. 信息存储.
	  // 将当前帧 currframe 的信息存放在 all_frames 序列中.
	  tracking_frame* currframe = new tracking_frame();
	  currframe->frame_seq_id = frame_index;
	  all_frames[frame_index] = currframe;

	  bool has_detected_cuboid = false;
	  // @PARAM 定义一个立方体类的对象 cube_local_meas
	  g2o::cuboid cube_local_meas;
	  double proposal_error;
	  char frame_index_c[256];
	  // 将当前帧的编号写入到 frame_index_c 中. TODO 这里又将frame_index写入到frame_index_c做什么？仅仅格式化位4位数？
	  sprintf(frame_index_c,"%04d",frame_index);  // 格式化为4位.

	  // read or detect cuboid
	  // STEP 3. 读取（离线）或【检测（在线）立方体物体】.
	  if (online_detect_mode)
	  {
	      // STEP 3.1 开始检测，读取rgb图像到 raw_rgb_img.
	      cv::Mat raw_rgb_img = cv::imread(base_folder+"raw_imgs/"+frame_index_c+"_rgb_raw.jpg", 1);

	      // STEP 3.2 【边缘线检测】.
		  // @PARAM all_lines_raw 边缘线存储的矩阵.
	      cv::Mat all_lines_mat;	// 检测到的线段信息，cv::Mat格式.
	      line_lbd_obj.detect_filter_lines(raw_rgb_img, all_lines_mat);

		  // 将 all_lines_mat 存储到 all_lines_raw 中.
	      Eigen::MatrixXd all_lines_raw(all_lines_mat.rows,4);		// TODO：4，线段的两个端点，每个点的xy坐标.
	      for (int rr=0;rr<all_lines_mat.rows;rr++)
				for (int cc=0;cc<4;cc++)
		  			all_lines_raw(rr,cc) = all_lines_mat.at<float>(rr,cc);
					/*
					std::cout << "all_lines_raw \n" << all_lines_raw << std::endl;
					518.164  179.13     533      46
					453.637 371.208   516.2 180.066
					285.62 322.261 451.626 372.243
					290.387 120.984 285.264 319.981
					384.164 22.1538 291.708 120.726
					514.869 171.607 290.926 123.344
					380.963  167.12 398.815 172.601
					381.094 202.619 395.935 206.264
					397.868 176.387 379.907 170.274
					*/

		  // STEP 3.3 读取 yolo 2D 目标检测.
	      //read cleaned yolo 2d object detection
	      Eigen::MatrixXd raw_2d_objs(10,5);  // 每帧 5 个参数：2D检测框[x1 y1 width height], 和概率. TODO
	      if (!read_all_number_txt(base_folder+"/filter_2d_obj_txts/"+frame_index_c+"_yolo2_0.15.txt", raw_2d_objs))
		  		return;
		  else
		  		std::cout << "yolo:" << "\n" << raw_2d_objs << std::endl;		// 输出该帧的 yolo 检测框信息.
	      raw_2d_objs.leftCols<2>().array() -=1;   // 将matlab的坐标换成c++，x y各减1；change matlab coordinate to c++, minus 1

		  // STEP 3.4 将当前帧的李代数表示的位姿转换成Eigen的变换矩阵.
	      Matrix4d transToWolrd;
	      detect_cuboid_obj.whether_sample_cam_roll_pitch = (frame_index!=0); // 第一帧可以不用采样相机位姿，也可以采样，不重要；first frame doesn't need to sample cam pose. could also sample. doesn't matter much
	      if (detect_cuboid_obj.whether_sample_cam_roll_pitch) //sample around first frame's pose
		  		transToWolrd = fixed_init_cam_pose_Twc.to_homogeneous_matrix();		// 将李代数表示的变换转换成Eigen矩阵表示transToWolrd.
	      else
		  transToWolrd = curr_cam_pose_Twc.to_homogeneous_matrix();

		  // STEP 3.5 立方体检测
	      std::vector<ObjectSet> frames_cuboids; // 立方体提案的排序向量. ObjectSet 是一个matlab的立方体对象.
		  // NOTE detect_cuboid()函数检测立方体.
	      detect_cuboid_obj.detect_cuboid(raw_rgb_img,transToWolrd,raw_2d_objs,all_lines_raw, frames_cuboids);
	      // cuboids_2d_img：带有立方体提案的 2D 图像.
		  currframe->cuboids_2d_img = detect_cuboid_obj.cuboids_2d_img;

		  cvNamedWindow("currframe.cuboids_2d_img");
		  cvMoveWindow("currframe.cuboids_2d_img", 20, 300);
		  cv::imshow("currframe.cuboids_2d_img",currframe->cuboids_2d_img);
		  cv::waitKey(0);

		  // STEP 3.6 检测到了物体，准备物体测量.
	      has_detected_cuboid = frames_cuboids.size()>0 && frames_cuboids[0].size()>0;		// frames_cuboids[0] 第0个对象.
	      if (has_detected_cuboid)  // prepare object measurement
	      {
			  	// 第 0 个物体的第 0 个提案.
				cuboid* detected_cube = frames_cuboids[0][0];  // NOTE this is a simple dataset, only one landmark

				g2o::cuboid cube_ground_value; // local ground frame 中的立方体对象.
				// 立方体 9 自由度的位姿.
				Vector9d cube_pose;
				cube_pose  << 	detected_cube->pos(0),
								detected_cube->pos(1),
								detected_cube->pos(2),
								0,
								0,
								detected_cube->rotY,
								detected_cube->scale(0),
								detected_cube->scale(1),
								detected_cube->scale(2);  // xyz roll pitch yaw scale

				// 转换到相机坐标系下的测量.
				cube_ground_value.fromMinimalVector(cube_pose);
				cube_local_meas = cube_ground_value.transform_to(curr_cam_pose_Twc); // measurement is in local camera frame

				// 如果对roll/pitch采样，则将其转换到正确的相机帧.
				if (detect_cuboid_obj.whether_sample_cam_roll_pitch)  //if camera roll/pitch is sampled, transform to the correct camera frame.
				{
					Vector3d new_camera_eulers =  detect_cuboid_obj.cam_pose_raw.euler_angle;
					new_camera_eulers(0) += detected_cube->camera_roll_delta; new_camera_eulers(1) += detected_cube->camera_pitch_delta;
					Matrix3d rotation_new = euler_zyx_to_rot<double>(new_camera_eulers(0),new_camera_eulers(1),new_camera_eulers(2));
					Vector3d trans = transToWolrd.col(3).head<3>();
					g2o::SE3Quat curr_cam_pose_Twc_new(rotation_new,trans);
					cube_local_meas = cube_ground_value.transform_to(curr_cam_pose_Twc_new);
				}

				// NOTE 提案的误差，误差来自于  detected_cube 对象，其又来自于 frames_cuboids 序列，由前面的【立方体检测函数】得到
				proposal_error = detected_cube->normalized_error;
	      }
	  }
	  // 离线模式.
	  else
	  {
		  // @PARAM 离线已知的立方体位姿矩阵 offline_pred_frame_objects
		  // NOTE 读取 detect_cuboids_saved.txt 中的立方体的位姿.
	      int cube_obs_frame_id = offline_pred_frame_objects(offline_cube_obs_row_id,0);	// 读取第一列编号.
	      has_detected_cuboid = cube_obs_frame_id==frame_index;		// 每一帧都检测到了.
	      if (has_detected_cuboid)  // prepare object measurement   not all frame has observation!!
	      {
			    // 读取立方体的 9 自由度的位姿.
				VectorXd measure_data = offline_pred_frame_objects.row(offline_cube_obs_row_id);
				g2o::cuboid cube_ground_value;
				Vector9d cube_pose;
				cube_pose<< measure_data(1),
							measure_data(2),
							measure_data(3),
							0,
							0,
							measure_data(4),
							measure_data(5),
							measure_data(6),
							measure_data(7);  // xyz roll pitch yaw scale

				// 将 9 自由度的位姿转换成 g2o::cuboid 中的形式.（世界坐标系）
				cube_ground_value.fromMinimalVector(cube_pose);

				// 读取每一帧相机的位姿.
				Eigen::VectorXd cam_pose_vec = init_frame_poses.row(frame_index);
				g2o::SE3Quat cam_val_Twc(cam_pose_vec.segment<7>(1)); // time x y z qx qy qz qw
				// NOTE 将立方体的位姿转换到相机坐标系
				cube_local_meas = cube_ground_value.transform_to(cam_val_Twc); // measurement is in local camera frame

				// NOTE 立方体提案的误差.
				proposal_error = measure_data(8);

				// 读取离线保存的带有提案的 2D 图像，并保存到 currframe->cuboids_2d_img 中.
				std::string detected_cube_2d_img_name = base_folder+"pred_3d_obj_overview/" + std::string(frame_index_c) + "_best_objects.jpg";
				currframe->cuboids_2d_img = cv::imread(detected_cube_2d_img_name, 1);

				offline_cube_obs_row_id++; // switch to next row  NOTE at most one object one frame in this data
	      }
	  }

	  // STEP 4 立方体路标.
	  if (has_detected_cuboid)
	  {
		  // @PARAM 创建立方体路标.
	      object_landmark* localcuboid = new object_landmark();

		  // 读取立方体的位姿，评估质量.
	      localcuboid->cube_meas = cube_local_meas;
	      localcuboid->meas_quality = (1-proposal_error+0.5)/2;  // initial error 0-1, higher worse,  now change to [0.5,1] higher, better
	    //   std::cout << "proposal_error:  " << proposal_error <<std::endl;
		//   std::cout << "meas_quality:  " << localcuboid->meas_quality <<std::endl;
		  currframe->observed_cuboids.push_back(localcuboid);
	  }

	  // STEP 5 G2O 优化.
	  // STEP 5.1 第一帧时，设置 g2o 中的立方体顶点，本数据集中只有一个.
	  // set up g2o cube vertex. only one in this dataset
	  if (frame_index == 0)
	  {
		  // cuboid的初始位姿（世界坐标系）.
	      g2o::cuboid init_cuboid_global_pose = cube_local_meas.transform_from(curr_cam_pose_Twc);
	      vCube = new g2o::VertexCuboid();
	      vCube->setEstimate(init_cuboid_global_pose);
	      vCube->setId(0);
	      vCube->setFixed(false);
	      graph.addVertex(vCube);			// NOTE 添加顶点（立方体）
	  }

	  // STEP 5.2 设置相机的顶点.
	  // set up g2o camera vertex
	  g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
	  currframe->pose_vertex = vSE3;
	  vSE3->setId(frame_index+1);
	  graph.addVertex(vSE3);				// NOTE 添加顶点（相机）
	  // G2O 一般将顶点存储为从世界到相机的位姿.
	  vSE3->setEstimate(curr_cam_pose_Twc.inverse()); //g2o vertex usually stores world to camera pose.
	  vSE3->setFixed(frame_index==0);

	  // STEP 5.3 添加 G2O的边：相机-物体测量.
	  // add g2o camera-object measurement edges, if there is
	  if (currframe->observed_cuboids.size()>0)
	  {
			object_landmark* cube_landmark_meas = all_frames[frame_index]->observed_cuboids[0];

			g2o::EdgeSE3Cuboid* e = new g2o::EdgeSE3Cuboid();
			e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>( vSE3 ));
			e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>( vCube ));
			e->setMeasurement(cube_landmark_meas->cube_meas);
			e->setId(frame_index);

			// TODO inv_sigma 是啥？
			Vector9d inv_sigma;
			inv_sigma << 1,1,1,1,1,1,1,1,1;
			inv_sigma = inv_sigma * 2.0 * cube_landmark_meas->meas_quality;
			// 格式转换	Vector9d ——> Matrix9d
			Matrix9d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
			e->setInformation(info);
			graph.addEdge(e);				// NOTE 添加边
	  }

	  // STEP 5.4 相机——顶点，相机-相机帧间旋转——边
	  // camera vertex, add cam-cam odometry edges
	  if (frame_index>0)
	  {
		g2o::EdgeSE3Expmap* e = new g2o::EdgeSE3Expmap();
		e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>( all_frames[frame_index-1]->pose_vertex ));
		e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>( all_frames[frame_index]->pose_vertex ));
		e->setMeasurement(odom_val);

		e->setId(total_frame_number+frame_index);
		Vector6d inv_sigma;
		inv_sigma << 1,1,1,1,1,1;
		inv_sigma = inv_sigma * 1.0;
		Matrix6d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
		e->setInformation(info);
		graph.addEdge(e);
	  }
	  // STEP 5.4 开始优化！！
	  graph.initializeOptimization();
	  graph.optimize(5); // do optimization!

	  // 优化之后相机的位姿，存储到当前帧序列 all_frames 中
	  // retrieve the optimization result, for debug visualization
	  for (int j=0;j<=frame_index;j++)
	  {
		  	// 优化之后的位姿（世界——相机）.
			all_frames[j]->cam_pose_Tcw = all_frames[j]->pose_vertex->estimate();
			// 相机——世界.
			all_frames[j]->cam_pose_Twc = all_frames[j]->cam_pose_Tcw.inverse();
	  }

	  // NOTE 优化之后的物体位姿 vCube->estimate() —— cube_pose_opti_history
	  object_landmark* current_landmark = new object_landmark();
	  current_landmark->cube_vertex = new g2o::VertexCuboid();
	  current_landmark->cube_vertex->setEstimate(vCube->estimate());
	  cube_pose_opti_history[frame_index] = current_landmark;

	  // NOTE 原始检测到的物体位姿 global_cube —— cube_pose_raw_detected_history
	  if (all_frames[frame_index]->observed_cuboids.size()>0)
	  {
		  // @PARAM cube_landmark_meas-当前帧的立方体路标的测量
	      object_landmark* cube_landmark_meas = all_frames[frame_index]->observed_cuboids[0];
	      g2o::cuboid local_cube = cube_landmark_meas->cube_meas;
	      g2o::cuboid global_cube = local_cube.transform_from(all_frames[frame_index]->cam_pose_Twc);

	      object_landmark* tempcuboids2 = new object_landmark();
		  tempcuboids2->cube_vertex = new g2o::VertexCuboid();
	      tempcuboids2->cube_vertex->setEstimate(global_cube);
	      cube_pose_raw_detected_history[frame_index] = tempcuboids2;
	  }
	  else
	      cube_pose_raw_detected_history[frame_index] = nullptr;
    }

	// STEP 6. 结束优化，开始可视化.
    cout<<"+++++++++++++Finish all optimization! Begin visualization.++++++++++++"<<endl;
    publish_all_poses(all_frames, cube_pose_opti_history,cube_pose_raw_detected_history,truth_frame_poses);
}

// BRIEF
int main(int argc, char* argv[])
{
	// STEP 【1 ：初始化参数.】
    // 初始化节点与命名空间.
    ros::init(argc, argv, "object_slam");
    ros::NodeHandle nh;
    /** 初始化参数. 格式：nh.param ("name", name, value);
     *  @param	base_folder		【data文件目录】：~/catkin_object/src/cube_slam/object_slam/data
     *  @param	online_detect_mode	【检测模式】，默认为true，【在线检测】，false为使用离线相机位姿
     *  @param	save_results_to_txt	【是否保存结果】，默认为false
     */
    nh.param ("base_folder", base_folder, ros::package::getPath("object_slam")+"/data/");
    nh.param ("online_detect_mode", online_detect_mode, true);
    nh.param ("save_results_to_txt", save_results_to_txt, true);
    // 输出参数信息.
    cout<<""<<endl;
    cout<<"base_folder   "<<base_folder<<endl;
    if (online_detect_mode)
		ROS_WARN_STREAM("Online detect object mode !!\n");
    else
		ROS_WARN_STREAM("Offline read object mode !!\n");
	if (save_results_to_txt)
		ROS_WARN_STREAM("save results to output_cam_poses.txt !!\n");
    else
		ROS_WARN_STREAM("don`t save results to txt !!\n");

	// STEP 【2：读取参数.】
	// NOTE 在线模式下，不使用 pred_objs_txt 和 init_camera_pose ，仅使用 truth_camera_pose 中的第一帧
    /** 读取参数文件
     *  @param	pred_objs_txt		local ground frame中的立方体位姿.
     *  @param	init_camera_pose	离线相机位姿 (x y yaw=0, truth roll/pitch/height).
     *  @param	truth_camera_pose	真实的相机位姿. TODO 这两个相机位姿的区别.
     */
    std::string pred_objs_txt = base_folder+"detect_cuboids_saved.txt";   // saved cuboids in local ground frame.
    std::string init_camera_pose = base_folder+"pop_cam_poses_saved.txt"; // offline camera pose for cuboids detection (x y yaw=0, truth roll/pitch/height)
    std::string truth_camera_pose = base_folder+"truth_cam_poses.txt";

	/** 从各个文件中读取位姿矩阵.
     *  @param	pred_frame_objects	读取的已知的立方体位姿.
     *  @param	init_frame_poses	读取的已知的相机位姿.
     *  @param	truth_frame_poses	相机真实的位姿（仅使用第一帧的信息） NOTE .
     */
    Eigen::MatrixXd pred_frame_objects(100,10);  // 100 is some large row number, each row in txt has 10 numbers
    Eigen::MatrixXd init_frame_poses(100,8);	 // NOTE 需要给定列数，行数会自动读取！！
    Eigen::MatrixXd truth_frame_poses(100,8);
    if (!read_all_number_txt(pred_objs_txt,pred_frame_objects))
		return -1;
    if (!read_all_number_txt(init_camera_pose,init_frame_poses))
		return -1;
    if (!read_all_number_txt(truth_camera_pose,truth_frame_poses))
		return -1;
	//std::cout << truth_frame_poses << std::endl;
    // 输出三个文件的行数：read data size:  51  58  58.
    std::cout<<"read data size:  "<<pred_frame_objects.rows()<<"  "<<init_frame_poses.rows()<<"  "<<truth_frame_poses.rows()<<std::endl;

    // STEP 【3：传入参数开始增量式图优化.】
	std::cout << "+++++++++++++ 参数读取完毕，开始优化！！ ++++++++++++" << std::endl;
    incremental_build_graph(pred_frame_objects,init_frame_poses,truth_frame_poses);

    return 0;
}


//=======================================================================
// Copyright (c) 2013 Alberto Santini
// Author: Alberto Santini <alberto@santini.in>
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/config.hpp>

#ifdef BOOST_MSVC
// Without disabling this we get hard errors about initialialized pointers:
#pragma warning(disable:4703)
#endif

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/r_c_shortest_paths.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <vector>
#include <memory>

using std::vector;
using std::allocator;
using namespace boost;

class VertexProperty {
public:
  int property1;
  int property2;
  int id;
  VertexProperty() {}
  VertexProperty(const int property1, const int property2) : property1(property1), property2(property2) {}
};

class EdgeProperty {
public:
  int cost;
  int id;
  EdgeProperty() {}
  EdgeProperty(const int cost) : cost(cost) {}
};

typedef adjacency_list<listS, listS, bidirectionalS, VertexProperty, EdgeProperty> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

class ResourceCont {
public:
  int res;
  ResourceCont(const int res = 5) : res(res) {}
  bool operator==(const ResourceCont& rc) const { return (res == rc.res); }
  bool operator<(const ResourceCont& rc) const { return (res > rc.res); }
};

class LabelExt {
public:
  bool operator()(const Graph& g, ResourceCont& rc, const ResourceCont& old_rc, Edge e) const {
    rc.res = old_rc.res - g[e].cost;
    return (rc.res > 0);
  }
};

class LabelDom {
public:
  bool operator()(const ResourceCont& rc1, const ResourceCont& rc2) const {
    return (rc1 == rc2 || rc1 < rc2);
  }
};

int main() {
  VertexProperty vp1(1, 1);
  VertexProperty vp2(5, 9);
  VertexProperty vp3(4, 3);
  EdgeProperty e12(1);
  EdgeProperty e23(2);

  Graph g;

  Vertex v1 = add_vertex(g); g[v1] = vp1;
  Vertex v2 = add_vertex(g); g[v2] = vp2;
  Vertex v3 = add_vertex(g); g[v3] = vp3;

  add_edge(v1, v2, e12, g);
  add_edge(v2, v3, e23, g);

  int index = 0;
  BGL_FORALL_VERTICES(v, g, Graph) {
    g[v].id = index++;
  }
  index = 0;
  BGL_FORALL_EDGES(e, g, Graph) {
    g[e].id = index++;
  }

  typedef vector<vector<Edge> > OptPath;
  typedef vector<ResourceCont> ParetoOpt;

  OptPath op;
  ParetoOpt ol;

  r_c_shortest_paths( g,
                      get(&VertexProperty::id, g),
                      get(&EdgeProperty::id, g),
                      v1,
                      v2,
                      op,
                      ol,
                      ResourceCont(5),
                      LabelExt(),
                      LabelDom(),
                      allocator<r_c_shortest_paths_label<Graph, ResourceCont> >(),
                      default_r_c_shortest_paths_visitor());

  return 0;
}


#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <fstream>
#include <opencv2/features2d.hpp>
#include <opencv2/highgui.hpp>
#include <boost/filesystem.hpp>
#include <algorithm>
#include "camera_pose.h"
#include "3d_point.h"
#include "image_data.h"
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "bundle_adjustment.h"

using namespace std;
namespace bfs = boost::filesystem;

// downsample the image to speed up processing
const int DOWNSAMPLE_FACTOR = 4;
// focal length in pixels, after downsampling
const double FOCAL_LENGTH = 4308 / DOWNSAMPLE_FACTOR;
// Path to the directory withh all the image files
const string PATH_TO_FILES = "../Images/";
const bool SHOW_RESULTS = true;

// A helper function to enable code reuse. This function helps in building up the list of 3D points. It needs to be called twice, within each pair of image matches, once to obtain the 3D points needed to estimate scale and a second time to create the actual 3D points list.
void Process3DPoints(vector<uchar> &mask, Mat & cur3D, int i, vector<int> & idx1, vector<int> & idx2, map<int, int> &cur_featureTo3D, vector<pair<int,Point3f>> &seen3D,vector<ThreeDPoint> &all3D, map<int, int> &featureTo3D, bool addToAll3D){
    for (size_t k=0; k < mask.size(); k++) {
        // Keep only inlier matches
        if (mask[k]) {
            // Convert from homogeneous coordinates.
            Point3d tmp3D;
            tmp3D.x = cur3D.at<float>(0, k) / cur3D.at<float>(3, k);
            tmp3D.y = cur3D.at<float>(1, k) / cur3D.at<float>(3, k);
            tmp3D.z = cur3D.at<float>(2, k) / cur3D.at<float>(3, k);
            // If this 3D point was seen before
            if(featureTo3D.count(idx1[k]) > 0){
                // the index of this seen point in all3D
                auto idxToUnq3D = featureTo3D[idx1[k]];
                if(!addToAll3D){
                    seen3D.push_back(pair<int,Point3f>(idxToUnq3D,tmp3D));
                }
                else{
                    //cout << "all3d " << all3D[idxToUnq3D].position << endl;
                    //cout << "tmp3D " << tmp3D << endl;
                    // Update the already seen 3d point to indicate that i+1 th image saw it at feature point number idx2[k]
                    all3D[idxToUnq3D].imgIndex.emplace_back(i+1);
                    all3D[idxToUnq3D].kpIndex.push_back(idx2[k]);
                    // the two 3D locations could be different, so replace it with their average. Could be improved
                    all3D[idxToUnq3D].position = (all3D[idxToUnq3D].position + tmp3D)/2.0f ;
                }
                // Update the 2D to 3D map for next iteration
                cur_featureTo3D[idx2[k]] = idxToUnq3D;

            }
            else{
                if(addToAll3D){
                    // Add the newly seen 3D point to the list of all 3D points
                    ThreeDPoint t3D;
                    // Since this point is new and was seen in both images add that info to the ThreeDPoint object
                    t3D.imgIndex.emplace_back(i);
                    t3D.kpIndex.push_back(idx1[k]);
                    t3D.imgIndex.emplace_back(i+1);
                    t3D.kpIndex.push_back(idx2[k]);

                    t3D.position = tmp3D;
                    all3D.emplace_back(t3D);
                    // Update the 2D to 3D map for next iteration
                    cur_featureTo3D[idx2[k]] = all3D.size() - 1;
                }
            }
        }
    }
}

int main() {

    //m = m.t();
    //float* mp = &m.at<float>(0);

    /**************** Read all files in folder and sort them by filename  ******************/
    bfs::path dirPath(PATH_TO_FILES);
    vector<string> filePaths;
    for (auto i = bfs::directory_iterator(dirPath); i != bfs::directory_iterator(); i++)
    {
        // Only files, no directories
        if (!bfs::is_directory(i->path()))
        {
            filePaths.emplace_back(i->path().filename().string()) ;
        }
        else
            continue;
    }
    //If we found no files
    if(filePaths.empty()){
        cout << "No image files found in the input directory. Exiting the program.\n" ;
        return 0;
    }

    // Sorting by filename
    sort(filePaths.begin(), filePaths.end());

    /**************** Detect features and extract descriptors  ******************/
    Ptr<AKAZE>  feature = AKAZE::create();
    namedWindow("img", WINDOW_NORMAL);

    //Vector to hold all image data
    vector<ImageData> allImgData;

    for (auto &f : filePaths)
    {
        ImageData id;
        // Read in the image
        Mat img = imread(PATH_TO_FILES + f);
        if(img.empty())
            continue;
        // Downsample
        resize(img, img, img.size()/DOWNSAMPLE_FACTOR);
        id.img = img;
        // convert to grayscale
        cvtColor(img, img, COLOR_BGR2GRAY);
        // Detect feature points and compute descriptors for the detected feature points
        feature->detectAndCompute(img, noArray(), id.kp, id.desc);

        allImgData.emplace_back(id);
    }
    //If we found no files
       if(allImgData.empty()){
           cout << "No image files found in the input directory. Exiting the program.\n" ;
           return 0;
       }
    /************************* Match Features, Estimate Pose and Estimate Realtive Scale  ****************************/
    // We are going to assume that all images have the same pricipal point
    float cx = allImgData[0].img.size().width/2;
    float cy = allImgData[0].img.size().height/2;
    Point2d pp(cx, cy);
    // Set the camera pose for the first image
    CameraPose cp(FOCAL_LENGTH,cx,cy);
    allImgData[0].cam = move(cp);
    //Initialize the overall transform
    Mat overallT = Mat::eye(4, 4, CV_64F);
    // Initilaize the map that tells you if a feature point was observed in the prior image
    // Note: this map can be made to map from a std::pair to 3D points, that way we could identify obseved 3D points in all prior images (instead of just the last image)
    map<int, int> featureTo3D;
    // Stores unique 3D points
    vector<ThreeDPoint> all3D;

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    const float ratio_thresh = 0.7f;
    for(size_t i = 0 ; i < allImgData.size() - 1; ++i){
        auto &id1 = allImgData[i];
        auto &id2 = allImgData[i+1];
        // Get the best and second best matches
        vector< vector<DMatch> > knn_matches;
        matcher->knnMatch( id1.desc, id2.desc, knn_matches, 2 );
        // Filter matches using the Lowe's ratio test
        std::vector<DMatch> good_matches;
        vector<Point2f> px1, px2;
        vector<int> idx1, idx2;
        for (size_t k = 0; k < knn_matches.size(); ++k)
        {
            if (knn_matches[k][0].distance < ratio_thresh * knn_matches[k][1].distance)
            {
                good_matches.push_back(knn_matches[k][0]);
                idx1.emplace_back(knn_matches[k][0].queryIdx);
                idx2.emplace_back(knn_matches[k][0].trainIdx);
                px1.push_back(id1.kp[idx1.back()].pt);
                px2.push_back(id2.kp[idx2.back()].pt);
            }
        }
        // Show matches if the flag is set.
        if(SHOW_RESULTS){
            //Draw matches
            Mat img_matches;
            drawMatches( id1.img, id1.kp, id2.img, id2.kp, good_matches, img_matches, Scalar::all(-1),
                        Scalar::all(-1), std::vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
            //Show detected matches
            imshow("Good Matches", img_matches );
            waitKey(500);
        }
        // Estimate the essential matrix
        vector<uchar> mask;
        Mat E = findEssentialMat(px2, px1, FOCAL_LENGTH, pp, RANSAC, 0.999, 1.0, mask);
        // Recover the pose
        Mat incR, inc_t;
        recoverPose(E, px2, px1, incR, inc_t, FOCAL_LENGTH, pp, mask);
        //
        //cout << "IncR: " << incR << endl;
        //cout << "Inc T: " << inc_t << endl;
        CameraPose cp(FOCAL_LENGTH,cx,cy,incR,inc_t,id1.cam.T);
        id2.cam = move(cp);
        // Triangulate to get 3D points
        //cout << "ID1 Cam P: " << id1.cam.P << endl;
        //cout << "ID2 Cam P: " << id2.cam.P << endl;

        Mat cur3D;
        triangulatePoints(id1.cam.P, id2.cam.P, px1, px2, cur3D);
        // count of number of 3D points so far
        //int num3D = all3D.size();
        //Vector to store the new values of 3D points sen before
        vector<pair<int,Point3f>> seen3D;
        map<int, int> cur_featureTo3D;

        if(i == 0)
            Process3DPoints(mask,cur3D,i,idx1,idx2,cur_featureTo3D,seen3D,all3D,featureTo3D,true);
        else
            Process3DPoints(mask,cur3D,i,idx1,idx2,cur_featureTo3D,seen3D,all3D,featureTo3D,false);
        // Compute scale if i > 0
        if(i > 0){
            // Use a few random point pairs to compute scale
            const int t = seen3D.size() - 1;
            int Lmt = 2500, counter =0 ;
            double scale = 0;
            for( int k =0 ; k < Lmt ; ++k){
                auto i1 = rand()%t ;
                auto i2 = rand()%t;
                if( i1 == i2)
                    continue;
                scale += (norm(all3D[seen3D[i1].first].position - all3D[seen3D[i2].first].position) / norm(seen3D[i1].second - seen3D[i2].second));
                counter++;
            }
            scale /= counter;
            cout << "Sacle: " << i << " to " << i + 1 << " : " << scale << endl;
            //Scale the translation vector and Recalculate the projection matrix
            CameraPose cp(FOCAL_LENGTH,cx,cy,incR,scale*inc_t,id1.cam.T);
            id2.cam = move(cp);
            //Recalculate the 3D points
            Mat cur3D;
            triangulatePoints(id1.cam.P, id2.cam.P, px1, px2, cur3D);
            //Vector to store the new values of 3D points seen before
            vector<pair<int,Point3f>> seen3D;
            map<int, int> cur_featureTo3D;
            Process3DPoints(mask,cur3D,i,idx1,idx2,cur_featureTo3D,seen3D,all3D,featureTo3D,true);
        }

        featureTo3D = cur_featureTo3D;
    }
    /*************************  Bundle Adjustemnt  ****************************/
    // For the camerapose of each image set the transposed rotation matrix
    for( auto &id :  allImgData){
        Mat tmp = id.cam.Rot().t();
        double* mp = &tmp.at<double>(0);
        ceres::RotationMatrixToAngleAxis( mp, id.cam.optVal);
        //add translation components too
        id.cam.FillOptValue();
        //cout << "opt: " << id.cam.optVal[3] << " " << id.cam.optVal[4] << " " << id.cam.optVal[5] << endl;
    }

    ceres::Problem problem;
    //For each 3D point
    for( size_t i = 0  ; i < all3D.size() ; ++i){
        //For each image in whcih this 3D point was observed add a residual block
        auto &cur_pnt = all3D[i];
        for(size_t j = 0; j <  cur_pnt.imgIndex.size(); ++j)
        {
            int img_id = cur_pnt.imgIndex[j];
            int kp_id = cur_pnt.kpIndex[j];
            //
            Point2f observed = allImgData[img_id].kp[kp_id].pt;
            ceres::CostFunction* cost_function = ReprojectionError::create(observed.x,observed.y, FOCAL_LENGTH, cx, cy);
            double* camera = (double*)  &allImgData[img_id].cam.optVal[0];
            double * p3D = (double*) &cur_pnt.position;
            problem.AddResidualBlock(cost_function,NULL, camera,p3D);
        }

    }
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 5e2;
    options.function_tolerance = 1e-6;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
    //Write 3D points to file
    ofstream myfile(PATH_TO_FILES + "/3DPoints.txt");
    if (myfile.is_open()){
        for (size_t i = 0; i < all3D.size(); i++){
            myfile << all3D[i].position.x << "\t" << all3D[i].position.y << "\t" << all3D[i].position.z << "\n";
        }
        myfile.close();
    }

    return 0;
}


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

#pragma once

#include "io/parameters.hpp"
#include "particles/base.hpp"
#include "enhance/random.hpp"
#include <memory>

#if __has_include(<eigen3/Eigen/Geometry>)
    #include <eigen3/Eigen/Geometry>
#elif __has_include(<Eigen/Geometry>)
    #include <Eigen/Geometry>
#else
    #pragma error "no eigen include"
#endif

namespace ves {
    enum class PERIODIC : bool
    {
        ON=true,
        OFF=false}
    ;

    template<PERIODIC P> class Box;
}

// a simulation box implementation
// represents a box from 0 to x,x,z
// is ParameterDependentComponent
//
// may eiter be PERIODIC::ON to calculate with periodic boundary conditions
// or may be PERIODIC::OFF to caclulate without periodic boundary conditions
namespace ves{
template<PERIODIC P>
class Box
{
public:
    typedef Eigen::Matrix<REAL,3,1> cartesian;

    // set x by mutableAccess to Parameter base class
    void setLengthX(REAL);

    // set y by mutableAccess to Parameter base class
    void setLengthY(REAL);

    // set z by mutableAccess to Parameter base class
    void setLengthZ(REAL);

    REAL getLengthX() const;
    REAL getLengthY() const;
    REAL getLengthZ() const;
    REAL getVolume() const;

    cartesian getCenter() const;

    // calcaulates the distance vector of two ves::Particle::Bases
    // depending on PERIODIC ON or OFF
    // called from anywhere else
    cartesian distanceVector(const cartesian&, const cartesian&) const;
    // implementation for ves::Particle::Base base class. calls cartesian version
    cartesian distanceVector(const ves::Particle::Base&, const ves::Particle::Base&) const;

    // distance
    // calls squared_distance and calculates std::sqrt
    REAL distance(const cartesian&, const cartesian&) const;
    // implementation for ves::Particle::Base base class. calls cartesian version
    REAL distance(const ves::Particle::Base&, const ves::Particle::Base&) const;

    // squared distance
    REAL squared_distance(const cartesian&, const cartesian&) const;
    // implementation for ves::Particle::Base base class. calls cartesian version
    REAL squared_distance(const ves::Particle::Base&, const ves::Particle::Base&) const;

    // scales down any give coordinates into the simulation box
    cartesian scaleDown(cartesian) const ;
    // implementation for ves::Particle::Base base class. calls cartesian version
    cartesian scaleDown(const ves::Particle::Base&) const;

    // scales down any give coordinates into the simulation box
    // VMD simulation box is from -x/2 to x/2
    // scales accordingly
    cartesian scaleDownForVMD(cartesian) const ;
    // implementation for ves::Particle::Base base class. calls cartesian version
    cartesian scaleDownForVMD(const ves::Particle::Base&) const;

    // checks if bounding_box contains coordinates
    // if PERIODIC::ON calls scaleDown before
    bool contains(const cartesian&) const;
    // implementation for ves::Particle::Base base class. calls cartesian version
    bool contains(const ves::Particle::Base&) const;

    // destroy if derived is destroyed
    virtual ~Box() = default;

    // check if all parameters are set to make bounding_box
    // necessary for contains(const ves::Particle::Base&)
    void check_for_aligned_box_setup();

    cartesian randomPointInside() const;

protected:

private:
    REAL x {0};
    REAL y {0};
    REAL z {0};

    std::unique_ptr<Eigen::AlignedBox<REAL,3>> bounding_box {nullptr};
};

template<PERIODIC P>
void Box<P>::setLengthX(REAL l)
{
    x = l;
    check_for_aligned_box_setup();
}

template<PERIODIC P>
void Box<P>::setLengthY(REAL l)
{
    y = l;
    check_for_aligned_box_setup();
}

template<PERIODIC P>
void Box<P>::setLengthZ(REAL l)
{
    z = l;
    check_for_aligned_box_setup();
}

template<PERIODIC P>
REAL Box<P>::getLengthX() const
{
    return x;
}

template<PERIODIC P>
REAL Box<P>::getLengthY() const
{
    return y;
}

template<PERIODIC P>
REAL Box<P>::getLengthZ() const
{
    return z;
}

template<PERIODIC P>
REAL Box<P>::getVolume() const
{
    return bounding_box->volume();
}

template<PERIODIC P>
typename Box<P>::cartesian Box<P>::getCenter() const
{
    return bounding_box->center();
}

template<PERIODIC P>
void Box<P>::check_for_aligned_box_setup()
{
    bounding_box.reset(nullptr);
    cartesian vec (x,y,z);
    bounding_box = std::make_unique<Eigen::AlignedBox<REAL,3>>( cartesian::Zero(), vec );
}

template<>
EIGEN_STRONG_INLINE Box<PERIODIC::ON>::cartesian Box<PERIODIC::ON>::distanceVector(const cartesian& c1, const cartesian& c2) const
{
    cartesian distance_cartesian = c2-c1;
    distance_cartesian(0) = distance_cartesian(0) - x * std::round(static_cast<REAL>(distance_cartesian(0)/(x)));
    distance_cartesian(1) = distance_cartesian(1) - y * std::round(static_cast<REAL>(distance_cartesian(1)/(y)));
    distance_cartesian(2) = distance_cartesian(2) - z * std::round(static_cast<REAL>(distance_cartesian(2)/(z)));
    return distance_cartesian;
}

template<>
EIGEN_STRONG_INLINE Box<PERIODIC::OFF>::cartesian Box<PERIODIC::OFF>::distanceVector(const cartesian& c1, const cartesian& c2) const
{
    return (c2-c1);
}

template<PERIODIC P>
EIGEN_STRONG_INLINE typename Box<P>::cartesian Box<P>::distanceVector(const ves::Particle::Base& p1, const ves::Particle::Base& p2) const
{
    return distanceVector(p1.getCoordinates(),p2.getCoordinates());
}

template<PERIODIC P>
EIGEN_STRONG_INLINE REAL Box<P>::squared_distance(const cartesian& c1, const cartesian& c2) const
{
    return distanceVector(c1,c2).squaredNorm();
}

template<PERIODIC P>
EIGEN_STRONG_INLINE REAL Box<P>::squared_distance(const ves::Particle::Base& p1, const ves::Particle::Base& p2) const
{
    return squared_distance(p1.getCoordinates(),p2.getCoordinates());
}

template<PERIODIC P>
EIGEN_STRONG_INLINE REAL Box<P>::distance(const cartesian& c1, const cartesian& c2) const
{
    return std::sqrt(squared_distance(c1,c2));
}

template<PERIODIC P>
EIGEN_STRONG_INLINE REAL Box<P>::distance(const ves::Particle::Base& p1, const ves::Particle::Base& p2) const
{
    return distance(p1.getCoordinates(),p2.getCoordinates());
}

template<PERIODIC P>
typename Box<P>::cartesian Box<P>::scaleDown(cartesian c) const
{
    while( c(0) > x ) c(0) -= x;
    while( c(1) > y ) c(1) -= y;
    while( c(2) > z ) c(2) -= z;

    while( c(0) < 0.f ) c(0) += x;
    while( c(1) < 0.f ) c(1) += y;
    while( c(2) < 0.f ) c(2) += z;
    return c;
}

template<PERIODIC P>
typename Box<P>::cartesian Box<P>::scaleDown(const ves::Particle::Base& p) const
{
    return scaleDown(p.getCoordinates());
}

template<PERIODIC P>
typename Box<P>::cartesian Box<P>::scaleDownForVMD(cartesian c) const
{
    c(0) = c(0) - x * std::round(c(0)/(x));
    c(1) = c(1) - y * std::round(c(1)/(y));
    c(2) = c(2) - z * std::round(c(2)/(z));
    return c;
}

template<PERIODIC P>
typename Box<P>::cartesian Box<P>::scaleDownForVMD(const ves::Particle::Base& p) const
{
    return scaleDownForVMD(p.getCoordinates());
}

template<>
inline bool Box<PERIODIC::ON>::contains(const cartesian& c) const
{
    assert(bounding_box);
    return bounding_box->contains(scaleDown(c));
}

template<>
inline bool Box<PERIODIC::OFF>::contains(const cartesian& c) const
{
    assert(bounding_box);
    return bounding_box->contains(c);
}

template<PERIODIC P>
inline bool Box<P>::contains(const ves::Particle::Base& p) const
{
    return contains(p.getCoordinates());
}

template<PERIODIC P>
EIGEN_STRONG_INLINE typename Box<P>::cartesian Box<P>::randomPointInside() const
{
    return cartesian
    (
        enhance::random<cartesian::Scalar>()(0.f,getLengthX()),
        enhance::random<cartesian::Scalar>()(0.f,getLengthY()),
        enhance::random<cartesian::Scalar>()(0.f,getLengthZ())
    );
}

} // namespace ves


// g++ -O3 -DNDEBUG -DMATSIZE=<x> benchmark.cpp -o benchmark && time ./benchmark

#include <iostream>

#include <Eigen/Core>

#ifdef __EMSCRIPTEN__
    #include <GLES2/gl2.h>
    #include <emscripten.h>
    #include <emscripten/threading.h>
    #include <emscripten/html5.h>
#endif

#include <igl/readOFF.h>
#include <igl/readCSV.h>
#include <igl/triangle_triangle_adjacency.h>
#include <igl/opengl/glfw/Viewer.h>

#include "hard_nrosy.h"

#ifndef MATSIZE
#define MATSIZE 3
#endif

using namespace std;
using namespace Eigen;

#ifndef REPEAT
#define REPEAT 4
#endif

#ifndef SCALAR
#define SCALAR double
#endif

#ifdef __EMSCRIPTEN__
static void create_webgl_context()
{
       EmscriptenWebGLContextAttributes attr;
       emscripten_webgl_init_context_attributes(&attr);
       attr.majorVersion = 2; attr.minorVersion = 0;
       EMSCRIPTEN_WEBGL_CONTEXT_HANDLE ctx = emscripten_webgl_create_context(0, &attr);
       emscripten_webgl_make_context_current(ctx);
       printf("created webgl context\n");
}
#endif

Eigen::MatrixXd V;
Eigen::MatrixXi F;

Eigen::MatrixXi TT;
Eigen::MatrixXi TTi;

// constraint points
Eigen::MatrixXd VC;
Eigen::VectorXi IC;

// vector field
Eigen::MatrixXd R;

// Constrained faces id
Eigen::VectorXi b;

// Cosntrained faces representative vector
Eigen::MatrixXd bc;

igl::opengl::glfw::Viewer viewer;

int N;

bool key_down(igl::opengl::glfw::Viewer& viewer, unsigned char key, int modifier)
{
    return true;
}

bool mouse_down(igl::opengl::glfw::Viewer& viewer, int, int)
{
    return false;
}

int main(int argc, char *argv[])
{
    #ifdef __EMSCRIPTEN__
        create_webgl_context();
    #endif
    N = 1;
    std::string shape = "data/hand.off";
    std::string constraints = "data/irr4-cyl2.constraints";

    igl::readOFF(shape, V, F);
    igl::readCSV(constraints, VC);

    igl::triangle_triangle_adjacency(F,TT,TTi);

    b.resize(VC.rows());
    b = VC.transpose().row(0).cast<int>();

    bc.resize(VC.rows(), 3);
    bc.col(0) = VC.col(1);
    bc.col(1) = VC.col(2);
    bc.col(2) = VC.col(3);

    R = hard_nrosy(V,F,TT,b,bc,N);

    viewer.data().set_mesh(V, F);

    viewer.callback_key_down = &key_down;
    viewer.callback_mouse_down = &mouse_down;
    viewer.data().show_lines = true;

    viewer.launch();

    return 0;
}


// Copyright (c) 2015 Egor Tensin <Egor.Tensin@gmail.com>
// This file is part of the "AES tools" project.
// For details, see https://github.com/egor-tensin/aes-tools.
// Distributed under the MIT License.

#include "block_cmd_parser.hpp"
#include "block_dumper.hpp"
#include "block_input.hpp"

#include <aesxx/all.hpp>

#include <boost/program_options.hpp>

#include <exception>
#include <iostream>
#include <stdexcept>
#include <string>

namespace
{
    template <aes::Algorithm algorithm, aes::Mode mode>
    void encrypt_with_mode(
        const Input& input,
        bool verbose = false)
    {
        typename aes::Types<algorithm>::Block iv;

        if (aes::ModeRequiresInitVector<mode>::value)
        {
            aes::from_string<algorithm>(iv, input.iv);
            if (verbose)
                dump_iv<algorithm>(iv);
        }

        typename aes::Types<algorithm>::Key key;
        aes::from_string<algorithm>(key, input.key);
        if (verbose)
            dump_key<algorithm>(key);

        aes::EncryptWrapper<algorithm, mode> encrypt{key, iv};
        if (verbose)
            dump_wrapper<algorithm, mode>(encrypt);

        for (const auto& input_block_string : input.blocks)
        {
            typename aes::Types<algorithm>::Block plaintext, ciphertext;
            aes::from_string<algorithm>(plaintext, input_block_string);

            encrypt.encrypt_block(plaintext, ciphertext);

            if (verbose)
            {
                dump_plaintext<algorithm>(plaintext);
                dump_ciphertext<algorithm>(ciphertext);
                dump_next_iv<algorithm, mode>(encrypt);
            }
            else
            {
                std::cout << aes::to_string<algorithm>(ciphertext) << '\n';
            }
        }
    }

    template <aes::Algorithm algorithm>
    void encrypt_with_algorithm(
        aes::Mode mode,
        const Input& input,
        bool verbose = false)
    {
        switch (mode)
        {
            case AES_ECB:
                encrypt_with_mode<algorithm, AES_ECB>(input, verbose);
                break;

            case AES_CBC:
                encrypt_with_mode<algorithm, AES_CBC>(input, verbose);
                break;

            case AES_CFB:
                encrypt_with_mode<algorithm, AES_CFB>(input, verbose);
                break;

            case AES_OFB:
                encrypt_with_mode<algorithm, AES_OFB>(input, verbose);
                break;

            case AES_CTR:
                encrypt_with_mode<algorithm, AES_CTR>(input, verbose);
                break;

            default:
                throw std::runtime_error("the selected mode of operation is not implemented");
                break;
        }
    }

    void encrypt_using_cxx_api(
        aes::Algorithm algorithm,
        aes::Mode mode,
        const Input& input,
        bool verbose = false)
    {
        switch (algorithm)
        {
            case AES_AES128:
                encrypt_with_algorithm<AES_AES128>(mode, input, verbose);
                break;

            case AES_AES192:
                encrypt_with_algorithm<AES_AES192>(mode, input, verbose);
                break;

            case AES_AES256:
                encrypt_with_algorithm<AES_AES256>(mode, input, verbose);
                break;

            default:
                throw std::runtime_error("the selected algorithm is not implemented");
                break;
        }
    }

    void encrypt_using_particular_box(
        aes::Box& box,
        const std::vector<std::string>& input_block_strings)
    {
        for (const auto& input_block_string : input_block_strings)
        {
            aes::Box::Block plaintext;
            box.parse_block(plaintext, input_block_string);

            aes::Box::Block ciphertext;
            box.encrypt_block(plaintext, ciphertext);
            std::cout << box.format_block(ciphertext) << '\n';
        }
    }

    void encrypt_using_boxes(
        aes::Algorithm algorithm,
        aes::Mode mode,
        const Input& input)
    {
        aes::Box::Key key;
        aes::Box::parse_key(key, algorithm, input.key);

        if (aes::mode_requires_init_vector(mode))
        {
            aes::Box::Block iv;
            aes::Box::parse_block(iv, algorithm, input.iv);
            aes::Box box{algorithm, key, mode, iv};

            encrypt_using_particular_box(box, input.blocks);
        }
        else
        {
            aes::Box box{algorithm, key};
            encrypt_using_particular_box(box, input.blocks);
        }
    }
}

int main(int argc, char** argv)
{
    try
    {
        BlockSettings settings{argv[0]};

        try
        {
            settings.parse(argc, argv);
        }
        catch (const boost::program_options::error& e)
        {
            settings.usage_error(e);
            return 1;
        }

        if (settings.exit_with_usage)
        {
            settings.usage();
            return 0;
        }

        for (const auto& input : settings.inputs)
        {
            if (settings.use_boxes)
            {
                encrypt_using_boxes(
                    settings.algorithm,
                    settings.mode,
                    input);
            }
            else
            {
                encrypt_using_cxx_api(
                    settings.algorithm,
                    settings.mode,
                    input,
                    settings.verbose);
            }
        }
    }
    catch (const aes::Error& e)
    {
        std::cerr << e;
        return 1;
    }
    catch (const std::exception& e)
    {
        std::cerr << e.what() << "\n";
        return 1;
    }
    return 0;
}