// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// TRENTO3D: Three-dimensional extension of TRENTO by Weiyao Ke
// MIT License

#include "output.h"

#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/program_options/variables_map.hpp>

#include "event.h"
#include "hdf5_utils.h"

namespace trento {

namespace {

// These output functions are invoked by the Output class.

void write_stream(std::ostream& os, int width,
    int num, double impact_param, const Event& event) {
  using std::fixed;
  using std::setprecision;
  using std::setw;
  using std::scientific;

  // Write a nicely-formatted line of event properties.
  os << setprecision(10)
     << setw(width)            << num
     << setw(15) << fixed      << impact_param
     << setw(5)                << event.npart()
     << setw(18) << scientific << event.multiplicity()
     << fixed;

  for (const auto& ecc : event.eccentricity())
    os << setw(14)             << ecc.second;

  os << '\n';
}

void write_text_file(const fs::path& output_dir, int width,
    int num, double impact_param, const Event& event, bool header) {
  // Open a numbered file in the output directory.
  // Pad the filename with zeros.
  std::ostringstream padded_fname{};
  padded_fname << std::setw(width) << std::setfill('0') << num << ".dat";
  fs::ofstream ofs{output_dir / padded_fname.str()};

  if (header) {
    // Write a commented header of event properties as key = value pairs.
    ofs << std::setprecision(10)
        << "# event "   << num                  << '\n'
        << "# b     = " << impact_param         << '\n'
        << "# npart = " << event.npart()        << '\n'
        << "# mult  = " << event.multiplicity() << '\n';

    for (const auto& ecc : event.eccentricity())
      ofs << "# e" << ecc.first << "    = " << ecc.second << '\n';
  }

  // Write IC profile as a block grid.  Use C++ default float format (not
  // fixed-width) so that trailing zeros are omitted.  This significantly
  // increases output speed and saves disk space since many grid elements are
  // zero.
  bool is3d;
  if (event.density_grid().shape()[2] == 1) is3d = false;
  else is3d = true;

  for (const auto& slice : event.density_grid()) {
    for (const auto& row : slice) {
      for (const auto& item : row) {
		 ofs << item << " ";
	  }
	  if (is3d) ofs << std::endl;
    }
	if (!is3d) ofs << std::endl;
  }
}

#ifdef TRENTO_HDF5

/// Simple functor to write many events to an HDF5 file.
class HDF5Writer {
 public:
  /// Prepare an HDF5 file for writing.
  HDF5Writer(const fs::path& filename);

  /// Write an event.
  void operator()(int num, double impact_param, const Event& event) const;

 private:
  /// Internal storage of the file object.
  H5::H5File file_;
};

// Add a simple scalar attribute to an HDF5 dataset.
template <typename T>
void hdf5_add_scalar_attr(
    const H5::DataSet& dataset, const std::string& name, const T& value) {
  const auto& datatype = hdf5::type<T>();
  auto attr = dataset.createAttribute(name, datatype, H5::DataSpace{});
  attr.write(datatype, &value);
}

HDF5Writer::HDF5Writer(const fs::path& filename)
    : file_(filename.string(), H5F_ACC_TRUNC)
{}

void HDF5Writer::operator()(
    int num, double impact_param, const Event& event) const {
  // Prepare arguments for new HDF5 dataset.

  // The dataset name is a prefix plus the event number.
  const std::string name{"event_" + std::to_string(num)};

  // Cache a reference to the event grid -- will need it several times.
  const auto& grid = event.density_grid();

  // Define HDF5 datatype and dataspace to match the grid.
  const auto& datatype = hdf5::type<Event::Grid3D::element>();
  std::array<hsize_t, Event::Grid3D::dimensionality> shape;
  std::copy(grid.shape(), grid.shape() + shape.size(), shape.begin());
  auto dataspace = hdf5::make_dataspace(shape);

  // Set dataset storage properties.
  H5::DSetCreatPropList proplist{};
  // Set chunk size to the entire grid.  For typical grid sizes (~100x100), this
  // works out to ~80 KiB, which is pretty optimal.  Anyway, it makes logical
  // sense to chunk this way, since chunks must be read contiguously and there's
  // no reason to read a partial grid.
  proplist.setChunk(shape.size(), shape.data());
  // Set gzip compression level.  4 is the default in h5py.
  proplist.setDeflate(4);

  // Create the new dataset and write the grid.
  auto dataset = file_.createDataSet(name, datatype, dataspace, proplist);
  dataset.write(grid.data(), datatype);

  // Write event attributes.
  hdf5_add_scalar_attr(dataset, "b", impact_param);
  hdf5_add_scalar_attr(dataset, "npart", event.npart());
  hdf5_add_scalar_attr(dataset, "mult", event.multiplicity());
  hdf5_add_scalar_attr(dataset, "dxy", event.dxy());
  hdf5_add_scalar_attr(dataset, "deta", event.deta());
  for (const auto& ecc : event.eccentricity())
    hdf5_add_scalar_attr(dataset, "e" + std::to_string(ecc.first), ecc.second);
}

#endif  // TRENTO_HDF5

}  // unnamed namespace

Output::Output(const VarMap& var_map) {
  // Determine the required width (padding) of the event number.  For example if
  // there are 10 events, the numbers are 0-9 and so no padding is necessary.
  // However given 11 events, the numbers are 00-10 with padded 00, 01, ...
  auto nevents = var_map["number-events"].as<int>();
  auto width = static_cast<int>(std::ceil(std::log10(nevents)));

  // Write to stdout unless the quiet option was specified.
  if (!var_map["quiet"].as<bool>()) {
    writers_.emplace_back(
      [width](int num, double impact_param, const Event& event) {
        write_stream(std::cout, width, num, impact_param, event);
      }
    );
  }

  // Possibly write to text or HDF5 files.
  if (var_map.count("output")) {
    const auto& output_path = var_map["output"].as<fs::path>();
    if (hdf5::filename_is_hdf5(output_path)) {
#ifdef TRENTO_HDF5
      if (fs::exists(output_path) && !fs::is_empty(output_path))
        throw std::runtime_error{"file '" + output_path.string() +
                                 "' exists, will not overwrite"};
      writers_.emplace_back(HDF5Writer{output_path});
#else
      throw std::runtime_error{"HDF5 output was not compiled"};
#endif  // TRENTO_HDF5
    } else {
      // Text files are all written into a single directory.  Require the
      // directory to be initially empty to avoid accidental overwriting and/or
      // spewing files into an already-used location.  If the directory does not
      // exist, create it.
      if (fs::exists(output_path)) {
        if (!fs::is_empty(output_path)) {
          throw std::runtime_error{"output directory '" + output_path.string() +
                                   "' must be empty"};
        }
      } else {
        fs::create_directories(output_path);
      }
      auto header = !var_map["no-header"].as<bool>();
      writers_.emplace_back(
        [output_path, width, header](
            int num, double impact_param, const Event& event) {
          write_text_file(output_path, width, num, impact_param, event, header);
        }
      );
    }
  }
}

}  // namespace trento


/*

Copyright 2016 Emanuele Vespa, Imperial College London

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

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

2. 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.

3. 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.

*/

#ifndef PLY_IO_HPP
#define PLY_IO_HPP
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <supereight/node_iterator.hpp>
#include <supereight/octree.hpp>

/*! \file */
namespace se {
template<typename T>
/*! \brief Write the octree structure to file in ply format.
 * Aggregated voxel blocks are written as a single octant of size block_side^3.
 * \param filename output filename
 * \param map octree map
 */
void print_octree(const char* filename, const se::Octree<T>& map) {
    std::stringstream ss_vertex;
    std::stringstream ss_faces;
    se::node_iterator<T> it(map);
    se::Node<T>* n  = it.next();
    const int depth = std::log2(map.size());
    int vertex_num  = 0;
    int faces_num   = 0;
    while (n) {
        const Eigen::Vector3i corner = se::keyops::decode(n->code_);
        const int side = 1 << (depth - se::keyops::level(n->code_));

        Eigen::Vector3f vertices[8];
        vertices[0] = corner.cast<float>();
        vertices[1] = (corner + Eigen::Vector3i(side, 0, 0)).cast<float>();
        vertices[2] = (corner + Eigen::Vector3i(0, side, 0)).cast<float>();
        vertices[3] = (corner + Eigen::Vector3i(side, side, 0)).cast<float>();
        vertices[4] = (corner + Eigen::Vector3i(0, 0, side)).cast<float>();
        vertices[5] = (corner + Eigen::Vector3i(side, 0, side)).cast<float>();
        vertices[6] = (corner + Eigen::Vector3i(0, side, side)).cast<float>();
        vertices[7] =
            (corner + Eigen::Vector3i(side, side, side)).cast<float>();

        for (int i = 0; i < 8; ++i) {
            ss_vertex << vertices[i].x() << " " << vertices[i].y() << " "
                      << vertices[i].z() << std::endl;
        }

        ss_faces << "4 " << vertex_num << " " << vertex_num + 1 << " "
                 << vertex_num + 3 << " " << vertex_num + 2 << std::endl;

        ss_faces << "4 " << vertex_num + 1 << " " << vertex_num + 5 << " "
                 << vertex_num + 7 << " " << vertex_num + 3 << std::endl;

        ss_faces << "4 " << vertex_num + 5 << " " << vertex_num + 7 << " "
                 << vertex_num + 6 << " " << vertex_num + 4 << std::endl;

        ss_faces << "4 " << vertex_num << " " << vertex_num + 2 << " "
                 << vertex_num + 6 << " " << vertex_num + 4 << std::endl;

        ss_faces << "4 " << vertex_num << " " << vertex_num + 1 << " "
                 << vertex_num + 5 << " " << vertex_num + 4 << std::endl;

        ss_faces << "4 " << vertex_num + 2 << " " << vertex_num + 3 << " "
                 << vertex_num + 7 << " " << vertex_num + 6 << std::endl;

        vertex_num += 8;
        faces_num += 6;
        n = it.next();
    }

    {
        std::ofstream f;
        f.open(std::string(filename).c_str());

        f << "ply" << std::endl;
        f << "format ascii 1.0" << std::endl;
        f << "comment octree structure" << std::endl;
        f << "element vertex " << vertex_num << std::endl;
        f << "property float x" << std::endl;
        f << "property float y" << std::endl;
        f << "property float z" << std::endl;
        f << "element face " << faces_num << std::endl;
        f << "property list uchar int vertex_index" << std::endl;
        f << "end_header" << std::endl;
        f << ss_vertex.str();
        f << ss_faces.str();
    }
}
} // namespace se
#endif


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, Locus Robotics
 *  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 <fuse_core/serialization.h>
#include <fuse_core/autodiff_local_parameterization.h>
#include <fuse_core/eigen.h>
#include <fuse_variables/orientation_3d_stamped.h>
#include <fuse_variables/stamped.h>
#include <ros/time.h>

#include <ceres/autodiff_cost_function.h>
#include <ceres/cost_function_to_functor.h>
#include <ceres/problem.h>
#include <ceres/rotation.h>
#include <ceres/solver.h>
#include <Eigen/Core>
#include <gtest/gtest.h>

#include <sstream>
#include <vector>

using fuse_variables::Orientation3DStamped;

TEST(Orientation3DStamped, Type)
{
  Orientation3DStamped variable(ros::Time(12345678, 910111213));
  EXPECT_EQ("fuse_variables::Orientation3DStamped", variable.type());
}

TEST(Orientation3DStamped, UUID)
{
  // Verify two orientations at the same timestamp produce the same UUID
  {
    Orientation3DStamped variable1(ros::Time(12345678, 910111213));
    Orientation3DStamped variable2(ros::Time(12345678, 910111213));
    EXPECT_EQ(variable1.uuid(), variable2.uuid());
  }

  auto uuid_1 = fuse_core::uuid::generate("test_hardware1");
  auto uuid_2 = fuse_core::uuid::generate("test_hardware2");

  // Verify two orientations at the same timestamp and same hardware ID produce the same UUID
  {
    Orientation3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
    Orientation3DStamped variable2(ros::Time(12345678, 910111213), uuid_1);
    EXPECT_EQ(variable1.uuid(), variable2.uuid());
  }

  // Verify two orientations with the same timestamp but different hardware IDs generate different UUIDs
  {
    Orientation3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
    Orientation3DStamped variable2(ros::Time(12345678, 910111213), uuid_2);
    EXPECT_NE(variable1.uuid(), variable2.uuid());
  }

  // Verify two orientations with the same hardware ID and different timestamps produce different UUIDs
  {
    Orientation3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
    Orientation3DStamped variable2(ros::Time(12345678, 910111214), uuid_1);
    EXPECT_NE(variable1.uuid(), variable2.uuid());

    Orientation3DStamped variable3(ros::Time(12345678, 910111213), uuid_1);
    Orientation3DStamped variable4(ros::Time(12345679, 910111213), uuid_1);
    EXPECT_NE(variable3.uuid(), variable4.uuid());
  }

  // Verify two orientations with different hardware IDs and different timestamps produce different UUIDs
  {
    Orientation3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
    Orientation3DStamped variable2(ros::Time(12345678, 910111214), uuid_2);
    EXPECT_NE(variable1.uuid(), variable2.uuid());

    Orientation3DStamped variable3(ros::Time(12345678, 910111213), uuid_1);
    Orientation3DStamped variable4(ros::Time(12345679, 910111213), uuid_2);
    EXPECT_NE(variable3.uuid(), variable4.uuid());
  }
}

struct Orientation3DPlus
{
  template<typename T>
  bool operator()(const T* x, const T* delta, T* x_plus_delta) const
  {
    T q_delta[4];
    ceres::AngleAxisToQuaternion(delta, q_delta);
    ceres::QuaternionProduct(x, q_delta, x_plus_delta);
    return true;
  }
};

struct Orientation3DMinus
{
  template<typename T>
  bool operator()(const T* q1, const T* q2, T* delta) const
  {
    T q1_inverse[4];
    q1_inverse[0] = q1[0];
    q1_inverse[1] = -q1[1];
    q1_inverse[2] = -q1[2];
    q1_inverse[3] = -q1[3];
    T q_delta[4];
    ceres::QuaternionProduct(q1_inverse, q2, q_delta);
    ceres::QuaternionToAngleAxis(q_delta, delta);
    return true;
  }
};

using Orientation3DLocalParameterization =
    fuse_core::AutoDiffLocalParameterization<Orientation3DPlus, Orientation3DMinus, 4, 3>;

TEST(Orientation3DStamped, Plus)
{
  auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization();

  double x[4] = {0.842614977, 0.2, 0.3, 0.4};
  double delta[3] = {0.15, -0.2, 0.433012702};
  double result[4] = {0.0, 0.0, 0.0, 0.0};
  bool success = parameterization->Plus(x, delta, result);

  EXPECT_TRUE(success);
  EXPECT_NEAR(0.745561, result[0], 1.0e-5);
  EXPECT_NEAR(0.360184, result[1], 1.0e-5);
  EXPECT_NEAR(0.194124, result[2], 1.0e-5);
  EXPECT_NEAR(0.526043, result[3], 1.0e-5);

  delete parameterization;
}

TEST(Orientation3DStamped, Minus)
{
  auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization();

  double x1[4] = {0.842614977, 0.2, 0.3, 0.4};
  double x2[4] = {0.745561, 0.360184, 0.194124, 0.526043};
  double result[3] = {0.0, 0.0, 0.0};
  bool success = parameterization->Minus(x1, x2, result);

  EXPECT_TRUE(success);
  EXPECT_NEAR(0.15, result[0], 1.0e-5);
  EXPECT_NEAR(-0.2, result[1], 1.0e-5);
  EXPECT_NEAR(0.433012702, result[2], 1.0e-5);

  delete parameterization;
}

TEST(Orientation3DStamped, PlusJacobian)
{
  auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization();
  auto reference = Orientation3DLocalParameterization();

  for (double qx = -0.5; qx < 0.5; qx += 0.1)
  {
    for (double qy = -0.5; qy < 0.5; qy += 0.1)
    {
      for (double qz = -0.5; qz < 0.5; qz += 0.1)
      {
        double qw = std::sqrt(1.0 - qx*qx - qy*qy - qz*qz);

        double x[4] = {qw, qx, qy, qz};
        fuse_core::MatrixXd actual(4, 3);
        actual << 0.0, 0.0, 0.0,
                  0.0, 0.0, 0.0,
                  0.0, 0.0, 0.0,
                  0.0, 0.0, 0.0;
        bool success = parameterization->ComputeJacobian(x, actual.data());

        fuse_core::MatrixXd expected(4, 3);
        expected << 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0;
        reference.ComputeJacobian(x, expected.data());

        EXPECT_TRUE(success);
        Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]");
        EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n"
                                                       << "Actual is:\n" << actual.format(clean) << "\n"
                                                       << "Difference is:\n" << (expected - actual).format(clean)
                                                       << "\n";
      }
    }
  }

  delete parameterization;
}

TEST(Orientation3DStamped, MinusJacobian)
{
  auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization();
  auto reference = Orientation3DLocalParameterization();

  for (double qx = -0.5; qx < 0.5; qx += 0.1)
  {
    for (double qy = -0.5; qy < 0.5; qy += 0.1)
    {
      for (double qz = -0.5; qz < 0.5; qz += 0.1)
      {
        double qw = std::sqrt(1.0 - qx*qx - qy*qy - qz*qz);

        double x[4] = {qw, qx, qy, qz};
        fuse_core::MatrixXd actual(3, 4);
        actual << 0.0, 0.0, 0.0, 0.0,
                  0.0, 0.0, 0.0, 0.0,
                  0.0, 0.0, 0.0, 0.0;
        bool success = parameterization->ComputeMinusJacobian(x, actual.data());

        fuse_core::MatrixXd expected(3, 4);
        expected << 0.0, 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0;
        reference.ComputeMinusJacobian(x, expected.data());

        EXPECT_TRUE(success);
        Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]");
        EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n"
                                                       << "Actual is:\n" << actual.format(clean) << "\n"
                                                       << "Difference is:\n" << (expected - actual).format(clean)
                                                       << "\n";
      }
    }
  }

  delete parameterization;
}

TEST(Orientation3DStamped, Stamped)
{
  fuse_core::Variable::SharedPtr base = Orientation3DStamped::make_shared(ros::Time(12345678, 910111213),
                                                                          fuse_core::uuid::generate("mo"));
  auto derived = std::dynamic_pointer_cast<Orientation3DStamped>(base);
  ASSERT_TRUE(static_cast<bool>(derived));
  EXPECT_EQ(ros::Time(12345678, 910111213), derived->stamp());
  EXPECT_EQ(fuse_core::uuid::generate("mo"), derived->deviceId());

  auto stamped = std::dynamic_pointer_cast<fuse_variables::Stamped>(base);
  ASSERT_TRUE(static_cast<bool>(stamped));
  EXPECT_EQ(ros::Time(12345678, 910111213), stamped->stamp());
  EXPECT_EQ(fuse_core::uuid::generate("mo"), stamped->deviceId());
}

struct QuaternionCostFunction
{
  explicit QuaternionCostFunction(double *observation)
  {
    observation_[0] = observation[0];
    observation_[1] = observation[1];
    observation_[2] = observation[2];
    observation_[3] = observation[3];
  }

  template <typename T>
  bool operator()(const T* quaternion, T* residual) const
  {
    T inverse_quaternion[4] =
    {
      quaternion[0],
      -quaternion[1],
      -quaternion[2],
      -quaternion[3]
    };

    T observation[4] =
    {
      T(observation_[0]),
      T(observation_[1]),
      T(observation_[2]),
      T(observation_[3])
    };

    T output[4];

    ceres::QuaternionProduct(observation, inverse_quaternion, output);

    // Residual can just be the imaginary components
    residual[0] = output[1];
    residual[1] = output[2];
    residual[2] = output[3];

    return true;
  }

  double observation_[4];
};

TEST(Orientation3DStamped, Optimization)
{
  // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees
  Orientation3DStamped orientation(ros::Time(12345678, 910111213));
  orientation.w() = 0.952;
  orientation.x() = 0.038;
  orientation.y() = -0.189;
  orientation.z() = 0.239;

  // Create a simple a constraint with an identity quaternion
  double target_quat[4] = {1.0, 0.0, 0.0, 0.0};
  ceres::CostFunction* cost_function =
    new ceres::AutoDiffCostFunction<QuaternionCostFunction, 3, 4>(new QuaternionCostFunction(target_quat));

  // Build the problem.
  ceres::Problem problem;
  problem.AddParameterBlock(
    orientation.data(),
    orientation.size(),
    orientation.localParameterization());
  std::vector<double*> parameter_blocks;
  parameter_blocks.push_back(orientation.data());
  problem.AddResidualBlock(
    cost_function,
    nullptr,
    parameter_blocks);

  // Run the solver
  ceres::Solver::Options options;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  // Check
  EXPECT_NEAR(target_quat[0], orientation.w(), 1.0e-3);
  EXPECT_NEAR(target_quat[1], orientation.x(), 1.0e-3);
  EXPECT_NEAR(target_quat[2], orientation.y(), 1.0e-3);
  EXPECT_NEAR(target_quat[3], orientation.z(), 1.0e-3);
}

TEST(Orientation3DStamped, Euler)
{
  const double RAD_TO_DEG = 180.0 / M_PI;

  // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees
  Orientation3DStamped orientation_r(ros::Time(12345678, 910111213));
  orientation_r.w() = 0.9961947;
  orientation_r.x() = 0.0871557;
  orientation_r.y() = 0.0;
  orientation_r.z() = 0.0;

  EXPECT_NEAR(10.0, RAD_TO_DEG * orientation_r.roll(), 1e-5);

  Orientation3DStamped orientation_p(ros::Time(12345678, 910111213));
  orientation_p.w() = 0.9848078;
  orientation_p.x() = 0.0;
  orientation_p.y() = -0.1736482;
  orientation_p.z() = 0.0;

  EXPECT_NEAR(-20.0, RAD_TO_DEG * orientation_p.pitch(), 1e-5);

  Orientation3DStamped orientation_y(ros::Time(12345678, 910111213));
  orientation_y.w() = 0.9659258;
  orientation_y.x() = 0.0;
  orientation_y.y() = 0.0;
  orientation_y.z() = 0.258819;

  EXPECT_NEAR(30.0, RAD_TO_DEG * orientation_y.yaw(), 1e-5);
}

TEST(Orientation3DStamped, Serialization)
{
  // Create an Orientation3DStamped
  Orientation3DStamped expected(ros::Time(12345678, 910111213));
  expected.w() = 0.952;
  expected.x() = 0.038;
  expected.y() = -0.189;
  expected.z() = 0.239;

  // Serialize the variable into an archive
  std::stringstream stream;
  {
    fuse_core::TextOutputArchive archive(stream);
    expected.serialize(archive);
  }

  // Deserialize a new variable from that same stream
  Orientation3DStamped actual;
  {
    fuse_core::TextInputArchive archive(stream);
    actual.deserialize(archive);
  }

  // Compare
  EXPECT_EQ(expected.deviceId(), actual.deviceId());
  EXPECT_EQ(expected.stamp(), actual.stamp());
  EXPECT_EQ(expected.w(), actual.w());
  EXPECT_EQ(expected.x(), actual.x());
  EXPECT_EQ(expected.y(), actual.y());
  EXPECT_EQ(expected.z(), actual.z());
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <ctime>
#include <mutex>
#include <atomic>
#include <memory>
#include <iomanip>
#include <iostream>
#include <unordered_map>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <Eigen/Dense>
#include <pcl/io/pcd_io.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>

#include <ros/ros.h>
#include <geodesy/utm.h>
#include <geodesy/wgs84.h>
#include <pcl_ros/point_cloud.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>

#include <std_msgs/Time.h>
#include <nav_msgs/Odometry.h>
#include <nmea_msgs/Sentence.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/PointCloud2.h>
#include <geographic_msgs/GeoPointStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <hdl_graph_slam/FloorCoeffs.h>

#include <hdl_graph_slam/SaveMap.h>
#include <hdl_graph_slam/DumpGraph.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <hdl_graph_slam/ros_utils.hpp>
#include <hdl_graph_slam/ros_time_hash.hpp>

#include <hdl_graph_slam/graph_slam.hpp>
#include <hdl_graph_slam/keyframe.hpp>
#include <hdl_graph_slam/keyframe_updater.hpp>
#include <hdl_graph_slam/loop_detector.hpp>
#include <hdl_graph_slam/information_matrix_calculator.hpp>
#include <hdl_graph_slam/map_cloud_generator.hpp>
#include <hdl_graph_slam/nmea_sentence_parser.hpp>

#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/edge_se3_plane.hpp>
#include <g2o/edge_se3_priorxy.hpp>
#include <g2o/edge_se3_priorxyz.hpp>
#include <g2o/edge_se3_priorvec.hpp>
#include <g2o/edge_se3_priorquat.hpp>

#include <watrix_vp/GPS_IMU.h>
namespace hdl_graph_slam {

class HdlGraphSlamNodelet : public nodelet::Nodelet {
public:
  typedef pcl::PointXYZI PointT;

  HdlGraphSlamNodelet() {}
  virtual ~HdlGraphSlamNodelet() {}

  virtual void onInit() {
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    // init parameters
    map_frame_id = private_nh.param<std::string>("map_frame_id", "map");
    odom_frame_id = private_nh.param<std::string>("odom_frame_id", "odom");
    map_cloud_resolution = private_nh.param<double>("map_cloud_resolution", 0.05);
    trans_odom2map.setIdentity();

    max_keyframes_per_update = private_nh.param<int>("max_keyframes_per_update", 10);

    //
    anchor_node = nullptr;
    anchor_edge = nullptr;
    floor_plane_node = nullptr;
    graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));
    keyframe_updater.reset(new KeyframeUpdater(private_nh));
    loop_detector.reset(new LoopDetector(private_nh));
    map_cloud_generator.reset(new MapCloudGenerator());
    inf_calclator.reset(new InformationMatrixCalculator(private_nh));
    nmea_parser.reset(new NmeaSentenceParser());

    gps_time_offset = private_nh.param<double>("gps_time_offset", 0.0);
    gps_edge_stddev_xy = private_nh.param<double>("gps_edge_stddev_xy", 0.000001);
    gps_edge_stddev_z = private_nh.param<double>("gps_edge_stddev_z", 0.001);
    floor_edge_stddev = private_nh.param<double>("floor_edge_stddev", 10.0);
    sim_odom_edge_stddev_xy = private_nh.param<double>("real_odom_edge_stddev_xy", 0.01);
    sim_odom_edge_stddev_z = private_nh.param<double>("real_odom_edge_stddev_z", 0.0001);

    imu_time_offset = private_nh.param<double>("imu_time_offset", 0.0);
    enable_imu_orientation = private_nh.param<bool>("enable_imu_orientation", false);
    enable_imu_acceleration = private_nh.param<bool>("enable_imu_acceleration", false);
    imu_orientation_edge_stddev = private_nh.param<double>("imu_orientation_edge_stddev", 0.1);
    imu_acceleration_edge_stddev = private_nh.param<double>("imu_acceleration_edge_stddev", 3.0);

    points_topic = private_nh.param<std::string>("points_topic", "/velodyne_poits");

    // subscribers
    odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, "/odom", 256));
    cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/filtered_points", 32));
    sync.reset(new message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>(*odom_sub, *cloud_sub, 32));
    sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2));
    //imu_sub = nh.subscribe("/gpsimu_driver/imu_data", 1024, &HdlGraphSlamNodelet::imu_callback, this);
     subGps = nh.subscribe<geometry_msgs::Point>("/use_lidar/may_or_not", 1, &HdlGraphSlamNodelet::GpsHandler, this);//---->>>>>
     first_pose_sub = nh.subscribe<geometry_msgs::Pose>("/lidar/first_pose", 1, &HdlGraphSlamNodelet::firstposecallback, this);
     //sim_odom_sub = nh.subscribe<nav_msgs::Odometry>("/simulation/odom", 1, &HdlGraphSlamNodelet::sim_odom_callback, this);

    imu_sub = nh.subscribe("/mavros/imu/data", 1024, &HdlGraphSlamNodelet::imu_callback, this);
    floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 1024, &HdlGraphSlamNodelet::floor_coeffs_callback, this);

    if(private_nh.param<bool>("enable_gps", true)) {
      gps_sub = mt_nh.subscribe("/gps/geopoint", 1024, &HdlGraphSlamNodelet::gps_callback, this);
      nmea_sub = mt_nh.subscribe("/gpsimu_driver/nmea_sentence", 1024, &HdlGraphSlamNodelet::nmea_callback, this);
      navsat_sub = mt_nh.subscribe("/gps/navsat", 1024, &HdlGraphSlamNodelet::navsat_callback, this);
    }

    // publishers
    markers_pub = mt_nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);
    odom2map_pub = mt_nh.advertise<geometry_msgs::TransformStamped>("/hdl_graph_slam/odom2pub", 16);
    map_points_pub = mt_nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points", 1);
    read_until_pub = mt_nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);
    lidar_pose_pub = mt_nh.advertise<geometry_msgs::PoseStamped>("/lidar_XYZ", 32);

    dump_service_server = mt_nh.advertiseService("/hdl_graph_slam/dump", &HdlGraphSlamNodelet::dump_service, this);
    save_map_service_server = mt_nh.advertiseService("/hdl_graph_slam/save_map", &HdlGraphSlamNodelet::save_map_service, this);

    double graph_update_interval = private_nh.param<double>("graph_update_interval", 3.0);
    double map_cloud_update_interval = private_nh.param<double>("map_cloud_update_interval", 10.0);
    optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval), &HdlGraphSlamNodelet::optimization_timer_callback, this);
    map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &HdlGraphSlamNodelet::map_points_publish_timer_callback, this);

    use_lidar = false;

    count_gps = 0;
    count_lidar = 0;
    dt4 = 0;
  }

private:

//************************************************************************************
  /**
   * @brief GpsHandler
   * @param watrix_vp::GPS_IMU gps
   * @param none
   */

void firstposecallback(const geometry_msgs::PoseConstPtr& msg){

    Eigen::Quaterniond q1(msg->orientation.w,msg->orientation.x,msg->orientation.y,msg->orientation.z);

    //T_init(q1);
    Eigen::Isometry3d T(q1);
    T.pretranslate(Eigen::Vector3d(0,0,0));
    T_init = T;
  }

void GpsHandler(const geometry_msgs::Point msg){

    if(msg.x == 1)
		{
       if(false == use_lidar)
       {

          use_lidar = true;
          //std::cout<<"graph_slam=========>use_lidar!!"<<std::endl;
       }

      return;

    }
    else{

          //std::cout<<"graph_slam=========>sys_reset();!!"<<std::endl;
          //count_gps=0;

          if(use_lidar == true){

            use_lidar = false;

            boost::optional<Eigen::Vector3d> zero1;

            zero_utm = zero1;

            sys_reset();

          }

          return;

    }

    // if(gps.NSV1<10 || gps.NSV2<10)
		// {

    //       use_lidar = true;
    //       //std::cout<<"graph_slam=========>use_lidar!!"<<std::endl;

    //      return;
    // }
    // else{

    //       use_lidar = false;

    //       sys_reset();
    //       return;
    // }

}
//**************************************************************************************
  /**
   * @brief reset system
   * @param none
   * @param none
   */
    void sys_reset()
    {
            keyframes.clear();              //std::vector<KeyFrame::Ptr> keyframes;  也是关键帧的集合不过是向量
            keyframe_queue.clear();         //关键帧集合双端队列
            floor_coeffs_queue.clear();
            imu_queue.clear();
            gps_queue.clear();

            graph_slam->graph->vertices().clear();
	          graph_slam->graph->edges().clear();

            new_keyframes.clear();  // 回环检测用的

            keyframes_snapshot.clear();       //   std::vector<KeyFrameSnapshot::Ptr> keyframes_snapshot;  用于产生地图的,会有延迟
            map_cloud_generator.reset();       //std::unique_ptr<MapCloudGenerator> map_cloud_generator;

            keyframe_hash.clear();           //  std::unordered_map<ros::Time, KeyFrame::Ptr, RosTimeHash> keyframe_hash;

            //keyframe = NULL;

            anchor_node = nullptr;
            anchor_edge = nullptr;
            floor_plane_node = nullptr;
            //graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));
            // keyframe_updater.reset(new KeyframeUpdater(private_nh));
            // loop_detector.reset(new LoopDetector(private_nh));
            // map_cloud_generator.reset(new MapCloudGenerator());
            // inf_calclator.reset(new InformationMatrixCalculator(private_nh));

    }

//**************************************************************************************
  /**
   * @brief received point clouds are pushed to #keyframe_queue
   * @param odom_msg
   * @param cloud_msg
   */
  void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {

    if(use_lidar == true)
    {
        const ros::Time& stamp = odom_msg->header.stamp;
        Eigen::Isometry3d odom = odom2isometry(odom_msg);

        pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
        pcl::fromROSMsg(*cloud_msg, *cloud);
        if(base_frame_id.empty()) {
          base_frame_id = cloud_msg->header.frame_id;
        }

        if(!keyframe_updater->update(odom)) {
          std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
          if(keyframe_queue.empty()) {
            std_msgs::Header read_until;
            read_until.stamp = stamp + ros::Duration(10, 0);
            read_until.frame_id = points_topic;
            read_until_pub.publish(read_until);
            read_until.frame_id = "/filtered_points";
            read_until_pub.publish(read_until);
          }

          return;
        }

        double accum_d = keyframe_updater->get_accum_distance();
        KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, cloud));

        std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
        keyframe_queue.push_back(keyframe);

    }

    ///*  重复清理了
    else{

      //keyframe_queue.clear();

      sys_reset();
	    return;
    }

    //*/

  }

     //里程计模拟值回调
  /*
  void sim_odom_callback(const nav_msgs::OdometryConstPtr& sim_msg){
        std::lock_guard<std::mutex> lock1(sim_odom_queue_mutex);
        //sim_msg->header.stamp = real_odom_msg->header.stamp + ros::Duration(real_odom_time_offset);
        sim_odom_queue.push_back(sim_msg);
    }

  */

  /**
   * @brief this method adds all the keyframes in #keyframe_queue to the pose graph (odometry edges)
   * @return if true, at least one keyframe was added to the pose graph
   */

  bool flush_keyframe_queue() {
    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);

    if(keyframe_queue.empty()) {   //判断sim_odom_queue是否为空加在下面sim_odom作为边的图优化中
      return false;
    }

    trans_odom2map_mutex.lock();
    Eigen::Isometry3d odom2map(trans_odom2map.cast<double>());
    trans_odom2map_mutex.unlock();

    int num_processed = 0;
    for(int i=0; i<std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) {
      num_processed = i;

      const auto& keyframe = keyframe_queue[i];
      // new_keyframes will be tested later for loop closure
      new_keyframes.push_back(keyframe);

      // add pose node
      Eigen::Isometry3d odom = odom2map * keyframe->odom;    //初始化odom2map是单位阵
      keyframe->node = graph_slam->add_se3_node(odom);      //将odom设为节点并且赋值给keyframe->node,当做节点
      keyframe_hash[keyframe->stamp] = keyframe;            //设置了一个哈希表,是一个map容器

      // fix the first node
      if(keyframes.empty() && new_keyframes.size() == 1) {
        if(private_nh.param<bool>("fix_first_node", false)) {
          anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());
          anchor_node->setFixed(true);
          Eigen::Isometry3d relative_pose = keyframe->odom.inverse();
          //首次位置的优化
          anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, relative_pose, Eigen::MatrixXd::Identity(6, 6));

        }
      }
      //std::cout << "第一个点" << anchor_edge->vertices()[0] << std::endl;
      //std::cout << graph_slam->vertices()[0];

      if(i==0 && keyframes.empty()) {
        continue;
      }

      // add edge between consecutive keyframes  :: ndt匹配的结果
      const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];

      Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
      Eigen::MatrixXd information = inf_calclator->calc_information_matrix(prev_keyframe->cloud, keyframe->cloud, relative_pose);
      auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("odometry_edge_robust_kernel", "NONE"), private_nh.param<double>("odometry_edge_robust_kernel_size", 1.0));

    }

    std_msgs::Header read_until;
    read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(10, 0);
    read_until.frame_id = points_topic;
    read_until_pub.publish(read_until);
    read_until.frame_id = "/filtered_points";
    read_until_pub.publish(read_until);

    keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);
    return true;
  }

  //gps 约束;
  void nmea_callback(const nmea_msgs::SentenceConstPtr& nmea_msg) {
    GPRMC grmc = nmea_parser->parse(nmea_msg->sentence);

    if(grmc.status != 'A') {
      return;
    }

    geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
    gps_msg->header = nmea_msg->header;
    gps_msg->position.latitude = grmc.latitude;
    gps_msg->position.longitude = grmc.longitude;
    gps_msg->position.altitude = NAN;

    gps_callback(gps_msg);
  }

  void navsat_callback(const sensor_msgs::NavSatFixConstPtr& navsat_msg) {

    geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
    gps_msg->header = navsat_msg->header;
    gps_msg->position.latitude = navsat_msg->latitude;
    gps_msg->position.longitude = navsat_msg->longitude;
    gps_msg->position.altitude = navsat_msg->altitude;
    gps_callback(gps_msg);

  }

  /**
   * @brief received gps data is added to #gps_queue
   * @param gps_msg
   */
  void gps_callback(const geographic_msgs::GeoPointStampedPtr& gps_msg) {
    std::lock_guard<std::mutex> lock(gps_queue_mutex);
    gps_msg->header.stamp += ros::Duration(gps_time_offset);
    gps_queue.push_back(gps_msg);
  }

  /**
   * @brief
   * @return
   */
  bool flush_gps_queue() {
    std::lock_guard<std::mutex> lock(gps_queue_mutex);

    if(keyframes.empty() || gps_queue.empty()) {
      return false;
    }

    bool updated = false;
    auto gps_cursor = gps_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > gps_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*gps_cursor)->header.stamp || keyframe->utm_coord) {
        continue;
      }

      // find the gps data which is closest to the keyframe
      auto closest_gps = gps_cursor;
      for(auto gps = gps_cursor; gps != gps_queue.end(); gps++) {
        auto dt = ((*closest_gps)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*gps)->header.stamp - keyframe->stamp).toSec();
        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_gps = gps;
      }

      // if the time residual between the gps and keyframe is too large, skip it
      gps_cursor = closest_gps;
      if(0.2 < std::abs(((*closest_gps)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }

      // convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM coordinate
      geodesy::UTMPoint utm;
      geodesy::fromMsg((*closest_gps)->position, utm);
      Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude);

      // the first gps data position will be the origin of the map    也是把相对位置偏差作为边来优化,只是没有加入角度
      if(!zero_utm) {
        zero_utm = xyz;
      }
      xyz -= (*zero_utm);       //x,y,z是与原点的差值

      std::cout << "纯惯导推算值" << xyz << std::endl;

      keyframe->utm_coord = xyz + Eigen::Vector3d(50,0,0);

      g2o::OptimizableGraph::Edge* edge;
      if(std::isnan(xyz.z())){
        Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / gps_edge_stddev_xy;
        edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);
      } else {
        Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
        information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy;
        information_matrix(2, 2) /= gps_edge_stddev_z;
        edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);
      }
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("gps_edge_robust_kernel", "NONE"), private_nh.param<double>("gps_edge_robust_kernel_size", 1.0));

      updated = true;
    }

    auto remove_loc = std::upper_bound(gps_queue.begin(), gps_queue.end(), keyframes.back()->stamp,
      [=](const ros::Time& stamp, const geographic_msgs::GeoPointStampedConstPtr& geopoint) {
        return stamp < geopoint->header.stamp;
      }
    );
    gps_queue.erase(gps_queue.begin(), remove_loc);
    return updated;
  }

//我们根据机器人位姿来作为图的顶点，这个位姿可以来自机器人的编码器，也可以是ICP匹配得到的，图的边就是位姿之间的关系。
//现在里程计当边还是当顶点.
/***************************************************************
  bool flush_odom_queue() {
    std::lock_guard<std::mutex> lock(sim_odom_queue_mutex);

    if(keyframes.empty() || sim_odom_queue.empty()) {
      return false;
    }

    bool updated = false;
    auto sim_odom_cursor = sim_odom_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > sim_odom_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*sim_odom_cursor)->header.stamp) {
        continue;
      }

      // find odom data which is closest to the keyframe
      auto closest_sim_odom = sim_odom_cursor;
      for(auto sim_odom = sim_odom_cursor; sim_odom != sim_odom_queue.end(); sim_odom++) {

        auto dt = ((*closest_sim_odom)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*sim_odom)->header.stamp - keyframe->stamp).toSec();

        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_sim_odom = sim_odom;
      }

      sim_odom_cursor = closest_sim_odom;
      if(0.2 < std::abs(((*closest_sim_odom)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }

      //里程计的速度是相对于baselink的,位置是相对于轮子中心的;
      //?转换到base坐标系下?   只需要一个平移   不需要

      Eigen::Vector3d odom_xyz((*closest_sim_odom)->pose.pose.position.x, (*closest_sim_odom)->pose.pose.position.y, (*closest_sim_odom)->pose.pose.position.z);
      //将里程计作为一个边来约束
      keyframe->odom_coord = odom_xyz;

      g2o::OptimizableGraph::Edge* edge;

      //信息矩阵;权重矩阵
      Eigen::Matrix3d odom_information_matrix = Eigen::Matrix3d::Identity();
      odom_information_matrix.block<2, 2>(0, 0) /= sim_odom_edge_stddev_xy;
      odom_information_matrix(2, 2) /= sim_odom_edge_stddev_z;

      /***************************************************************
      找到对应的前一帧;
      int num_processed = 0;
      for(int i=0; i<std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) {
          num_processed = i;

          //const auto& keyframe = keyframe_queue[i];

          if(i==0 && keyframes.empty()) {
            continue;
          }

      // add edge between consecutive keyframes
      const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];
      //
      //添加边
      edge = graph_slam->add_se3_point_xyz_edge(keyframe->node,keyframes.back()->node, odom_xyz, odom_information_matrix);

      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("sim_odom_edge_robust_kernel", "NONE"), private_nh.param<double>("sim_odom_edge_robust_kernel_size", 1.0));

      updated = true;
    }

    //将已经优化的odom从队列中移除;
    auto remove_loc = std::upper_bound(sim_odom_queue.begin(), sim_odom_queue.end(), keyframes.back()->stamp,
      [=](const ros::Time& stamp, const nav_msgs::OdometryConstPtr& sim_odom) {
        return stamp < sim_odom->header.stamp;
      }
    );
    sim_odom_queue.erase(sim_odom_queue.begin(), remove_loc);
    return updated;

  }

*************************************************************/

//// add edge between consecutive keyframes    ::   里程计计算的结果
    //时间戳找的不对吗?

  /*
  bool flush_odom_queue() {

      std::lock_guard<std::mutex> lock1(sim_odom_queue_mutex);
      bool updated = false;
      if(keyframes.empty() || sim_odom_queue.empty()) {
        return false;
    }

      if(!sim_odom_queue.empty()){
          //std::lock_guard<std::mutex> lock1(sim_odom_queue_mutex);

          int i = 0;
          auto sim_odom_cursor = sim_odom_queue.begin();
          //找到所有关键帧对应的里程计数据

          for(auto& keyframe__ : keyframes) {

            if(i == 0){          //不优化第一帧;
              i = 1;
              continue;
            }
            //i++;

            if(keyframe__->stamp > sim_odom_queue.back()->header.stamp) {
              break;
            }

            if(keyframe__->stamp < (*sim_odom_cursor)->header.stamp) {
              i++;
              continue;
            }

            // find odom data which is closest to the keyframe
            auto closest_sim_odom = sim_odom_cursor;
            for(auto sim_odom = sim_odom_cursor; sim_odom != sim_odom_queue.end(); sim_odom++) {

              auto dt = ((*closest_sim_odom)->header.stamp - keyframe__->stamp).toSec();
              auto dt2 = ((*sim_odom)->header.stamp - keyframe__->stamp).toSec();

              if(std::abs(dt) < std::abs(dt2)) {
                break;
              }

              closest_sim_odom = sim_odom;
            }

            sim_odom_cursor = closest_sim_odom;
            if(0.2 < std::abs(((*closest_sim_odom)->header.stamp - keyframe__->stamp).toSec())) {
              continue;
            }

            //里程计的速度是相对于baselink的,位置是相对于轮子中心的;
            //?转换到base坐标系下?   只需要一个平移   不需要

            Eigen::Vector3d odom_xyz_((*closest_sim_odom)->pose.pose.position.x, (*closest_sim_odom)->pose.pose.position.y, (*closest_sim_odom)->pose.pose.position.z);
            odom_xyz = odom_xyz_;
            //将里程计作为一个边来约束
            keyframe__->odom_coord = odom_xyz;

            keyframe_cur = keyframe__;
            keyframe_pre = keyframes[i-1];

            Eigen::Isometry3d T_sim_odom = keyframe_cur->odom;
            T_sim_odom.pretranslate(odom_xyz);

            //Eigen::Isometry3d relative_pose1 = T_sim_odom.inverse() * keyframe_pre->odom;
            //Eigen::MatrixXd information1 = 5* inf_calclator->calc_information_matrix(keyframe_pre->cloud, keyframe_cur->cloud, relative_pose1);
            //auto edge1 = graph_slam->add_se3_edge(keyframe_cur->node, keyframe_pre->node, relative_pose1, information1);
            //graph_slam->add_robust_kernel(edge1, private_nh.param<std::string>("sim_odom_edge_robust_kernel", "NONE"), private_nh.param<double>("sim_odom_edge_robust_kernel_size", 1.0));

            updated = true;

          }//找最近的odom与keyframe循环结束;
      }

          /*找到离着keyframe_cur最近的上一个关键帧;
          for(auto& keyframe1 : keyframes) {
            if(keyframe1->stamp > keyframe_cur->stamp) {
              break;
            }
            auto closest_keyframe_cur = keyframe1;
            auto dt3 = (keyframe_cur->stamp - closest_keyframe_cur->stamp).toSec();

            if(dt4 ==0){
              dt4 = dt3;
            }
            //double dt4;
            //auto dt4 = dt3;    //初始化
            if(std::abs(dt4) >= std::abs(dt3)){
              keyframe_pre = closest_keyframe_cur;
              dt4 = dt3;

            }

            if(std::abs(dt4) < std::abs(dt3)){
              continue;
            }

            if(0.2 < std::abs((keyframe_cur->stamp - keyframe_pre->stamp).toSec())) {
              keyframe_pre = NULL;
              continue;
            }
          }

            //T_sim_odom的旋转量设置为ndt的旋转量
            //定义一个里程计的T
            //Eigen::Quaterniond q(1,0,0,0);

            //g2o::OptimizableGraph::Edge* edge;

            //信息矩阵;权重矩阵
            //Eigen::Matrix3d odom_information_matrix = Eigen::Matrix3d::Identity();
            //odom_information_matrix.block<2, 2>(0, 0) /= sim_odom_edge_stddev_xy;
            //odom_information_matrix(2, 2) /= sim_odom_edge_stddev_z;

            //计算信息矩阵
            //Eigen::Isometry3d relative_pose1(loop->relative_pose.cast<double>());
            //Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);

            //if(keyframe_pre != NULL && keyframe_cur != NULL){

           // }
     // }

    //将已经优化的odom从队列中移除;
    auto remove_loc = std::upper_bound(sim_odom_queue.begin(), sim_odom_queue.end(), keyframes.back()->stamp,
      [=](const ros::Time& stamp, const nav_msgs::OdometryConstPtr& sim_odom) {
        return stamp < sim_odom->header.stamp;
      }
    );
    sim_odom_queue.erase(sim_odom_queue.begin(), remove_loc);
    return updated;

  }

  */

  void imu_callback(const sensor_msgs::ImuPtr& imu_msg) {
    if(!enable_imu_orientation && !enable_imu_acceleration) {
      return;
    }

   {
      ROS_INFO("imu get!! %f,%f,%f" ,(*imu_msg).linear_acceleration.x ,(*imu_msg).linear_acceleration.y,(*imu_msg).linear_acceleration.z);

    //  float temp = (*imu_msg).linear_acceleration.x;
    //  float temp2 = (*imu_msg).angular_velocity.x;
    //  (*imu_msg).linear_acceleration.x = (*imu_msg).linear_acceleration.y;      (*imu_msg).angular_velocity.x = (*imu_msg).angular_velocity.y;
    //  (*imu_msg).linear_acceleration.y = temp;                                  (*imu_msg).angular_velocity.y = temp2;
    //  (*imu_msg).linear_acceleration.z = -1 * (*imu_msg).linear_acceleration.z; (*imu_msg).angular_velocity.z = -1 * (*imu_msg).angular_velocity.z;

    //  (*imu_msg).linear_acceleration.y = -1 * (*imu_msg).linear_acceleration.y;
    //  (*imu_msg).linear_acceleration.z = -1 * (*imu_msg).linear_acceleration.z;

    //  (*imu_msg).angular_velocity.y = -1 * (*imu_msg).angular_velocity.y;
    //  (*imu_msg).angular_velocity.z = -1 * (*imu_msg).angular_velocity.z;

   }

    std::lock_guard<std::mutex> lock(imu_queue_mutex);
    imu_msg->header.stamp += ros::Duration(imu_time_offset);
    imu_queue.push_back(imu_msg);
  }

  bool flush_imu_queue() {
    std::lock_guard<std::mutex> lock(imu_queue_mutex);
    if(keyframes.empty() || imu_queue.empty() || base_frame_id.empty()) {
      return false;
    }

    bool updated = false;
    auto imu_cursor = imu_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > imu_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*imu_cursor)->header.stamp || keyframe->acceleration) {
        continue;
      }

      // find imu data which is closest to the keyframe
      auto closest_imu = imu_cursor;
      for(auto imu = imu_cursor; imu != imu_queue.end(); imu++) {
        auto dt = ((*closest_imu)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*imu)->header.stamp - keyframe->stamp).toSec();
        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_imu = imu;
      }

      imu_cursor = closest_imu;
      if(0.2 < std::abs(((*closest_imu)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }

      const auto& imu_ori = (*closest_imu)->orientation;
      const auto& imu_acc = (*closest_imu)->linear_acceleration;

      geometry_msgs::Vector3Stamped acc_imu;
      geometry_msgs::Vector3Stamped acc_base;
      geometry_msgs::QuaternionStamped quat_imu;
      geometry_msgs::QuaternionStamped quat_base;

      quat_imu.header.frame_id = acc_imu.header.frame_id = (*closest_imu)->header.frame_id;
      quat_imu.header.stamp = acc_imu.header.stamp = ros::Time(0);
      acc_imu.vector = (*closest_imu)->linear_acceleration;
      quat_imu.quaternion = (*closest_imu)->orientation;

      try {
        tf_listener.transformVector(base_frame_id, acc_imu, acc_base);
        tf_listener.transformQuaternion(base_frame_id, quat_imu, quat_base);
      } catch (std::exception& e) {
        std::cerr << "failed to find transform!!" << std::endl;
        return false;
      }

      keyframe->acceleration = Eigen::Vector3d(acc_base.vector.x, acc_base.vector.y, acc_base.vector.z);
      keyframe->orientation = Eigen::Quaterniond(quat_base.quaternion.w, quat_base.quaternion.x, quat_base.quaternion.y, quat_base.quaternion.z);
      keyframe->orientation = keyframe->orientation;
      if(keyframe->orientation->w() < 0.0) {
        keyframe->orientation->coeffs() = -keyframe->orientation->coeffs();
      }

      if(enable_imu_orientation) {
        Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_orientation_edge_stddev;
        auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);
        graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_orientation_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_orientation_edge_robust_kernel_size", 1.0));
      }

      if(enable_imu_acceleration) {
        Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_acceleration_edge_stddev;
        g2o::OptimizableGraph::Edge* edge = graph_slam->add_se3_prior_vec_edge(keyframe->node, Eigen::Vector3d::UnitZ(), *keyframe->acceleration, info);
        graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_acceleration_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_acceleration_edge_robust_kernel_size", 1.0));
      }
      updated = true;
    }

    auto remove_loc = std::upper_bound(imu_queue.begin(), imu_queue.end(), keyframes.back()->stamp,
      [=](const ros::Time& stamp, const sensor_msgs::ImuConstPtr& imu) {
        return stamp < imu->header.stamp;
      }
    );
    imu_queue.erase(imu_queue.begin(), remove_loc);

    return true;
  }

  /**
   * @brief received floor coefficients are added to #floor_coeffs_queue
   * @param floor_coeffs_msg
   */
  void floor_coeffs_callback(const hdl_graph_slam::FloorCoeffsConstPtr& floor_coeffs_msg) {
    if(floor_coeffs_msg->coeffs.empty()) {
      return;
    }

    std::lock_guard<std::mutex> lock(floor_coeffs_queue_mutex);
    floor_coeffs_queue.push_back(floor_coeffs_msg);
  }

  /**
   * @brief this methods associates floor coefficients messages with registered keyframes, and then adds the associated coeffs to the pose graph
   * @return if true, at least one floor plane edge is added to the pose graph
   */
  bool flush_floor_queue() {
    std::lock_guard<std::mutex> lock(floor_coeffs_queue_mutex);

    if(keyframes.empty()) {
      return false;
    }

    const auto& latest_keyframe_stamp = keyframes.back()->stamp;

    bool updated = false;
    for(const auto& floor_coeffs : floor_coeffs_queue) {
      if(floor_coeffs->header.stamp > latest_keyframe_stamp) {
        break;
      }

      auto found = keyframe_hash.find(floor_coeffs->header.stamp);
      if(found == keyframe_hash.end()) {
        continue;
      }

      if(!floor_plane_node) {
        floor_plane_node = graph_slam->add_plane_node(Eigen::Vector4d(0.0, 0.0, 1.0, 0.0));
        floor_plane_node->setFixed(true);
      }

      const auto& keyframe = found->second;

      Eigen::Vector4d coeffs(floor_coeffs->coeffs[0], floor_coeffs->coeffs[1], floor_coeffs->coeffs[2], floor_coeffs->coeffs[3]);
      Eigen::Matrix3d information = Eigen::Matrix3d::Identity() * (1.0 / floor_edge_stddev);
      auto edge = graph_slam->add_se3_plane_edge(keyframe->node, floor_plane_node, coeffs, information);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("floor_edge_robust_kernel", "NONE"), private_nh.param<double>("floor_edge_robust_kernel_size", 1.0));

      keyframe->floor_coeffs = coeffs;

      updated = true;
    }

    auto remove_loc = std::upper_bound(floor_coeffs_queue.begin(), floor_coeffs_queue.end(), latest_keyframe_stamp,
      [=](const ros::Time& stamp, const hdl_graph_slam::FloorCoeffsConstPtr& coeffs) {
        return stamp < coeffs->header.stamp;
      }
    );
    floor_coeffs_queue.erase(floor_coeffs_queue.begin(), remove_loc);

    return updated;
  }

  /**
   * @brief generate map point cloud and publish it
   * @param event
   */
  void map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
    if(!map_points_pub.getNumSubscribers()) {
      return;
    }

    std::vector<KeyFrameSnapshot::Ptr> snapshot;

    keyframes_snapshot_mutex.lock();
    snapshot = keyframes_snapshot;
    keyframes_snapshot_mutex.unlock();

    auto cloud = map_cloud_generator->generate(snapshot, 0.05);
    if(!cloud) {
      return;
    }

    cloud->header.frame_id = map_frame_id;
    cloud->header.stamp = snapshot.back()->cloud->header.stamp;

    sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
    pcl::toROSMsg(*cloud, *cloud_msg);

    map_points_pub.publish(cloud_msg);
  }

  /**
   * @brief this methods adds all the data in the queues to the pose graph, and then optimizes the pose graph
   * @param event
   */
  void optimization_timer_callback(const ros::WallTimerEvent& event) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    // add keyframes and floor coeffs in the queues to the pose graph
    bool keyframe_updated = flush_keyframe_queue();

    if(!keyframe_updated) {
      std_msgs::Header read_until;
      read_until.stamp = ros::Time::now() + ros::Duration(30, 0);
      read_until.frame_id = points_topic;
      read_until_pub.publish(read_until);
      read_until.frame_id = "/filtered_points";
      read_until_pub.publish(read_until);
    }

    if(!keyframe_updated & !flush_floor_queue() & !flush_gps_queue() &!flush_imu_queue()) {
      return;
    }

    /* loop detection
    std::vector<Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
    for(const auto& loop : loops) {
      Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
      Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
      auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("loop_closure_edge_robust_kernel", "NONE"), private_nh.param<double>("loop_closure_edge_robust_kernel_size", 1.0));
    }

*/
    std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));
    new_keyframes.clear();

    // optimize the pose graph
    int num_iterations = private_nh.param<int>("g2o_solver_num_iterations", 1024);
    graph_slam->optimize(num_iterations);

    // publish tf
    const auto& keyframe = keyframes.back();
    //publish 优化后的位姿;
    geometry_msgs::PoseStamped increment_lidar_p;

    increment_lidar_p.header.stamp = ros::Time::now();
    increment_lidar_p.pose.position.x = keyframe->node->estimate().translation().x();
    increment_lidar_p.pose.position.y = keyframe->node->estimate().translation().y();
    increment_lidar_p.pose.position.z = keyframe->node->estimate().translation().z();
    //increment_lidar_p.orientation.x = keyframe->node->estimate().rotation().x();
    Eigen::AngleAxisd rotation_vector(keyframe->node->estimate().rotation());
    Eigen::Quaterniond quaternion(keyframe->node->estimate().rotation());
    ::Eigen::Quaterniond q;
    q = keyframe->node->estimate().rotation();
    //std::cout << "四元数" << q.x() << " he " << quaternion.x() << std::endl;
    increment_lidar_p.pose.orientation.x = q.x();
    increment_lidar_p.pose.orientation.y = q.y();
    increment_lidar_p.pose.orientation.z = q.z();
    increment_lidar_p.pose.orientation.w = q.w();

    Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse();
    trans_odom2map_mutex.lock();
    trans_odom2map = trans.matrix().cast<float>();
    trans_odom2map_mutex.unlock();

    std::vector<KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
    std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(),
      [=](const KeyFrame::Ptr& k) {
        return std::make_shared<KeyFrameSnapshot>(k);
    });

    keyframes_snapshot_mutex.lock();
    keyframes_snapshot.swap(snapshot);
    keyframes_snapshot_mutex.unlock();

    std::cout<<"测试一下～ "<<std::endl;
    //std::cout<<"keyframes.size()="<<keyframes.size()<<std::endl;   keyframes[i]->node->estimate().translation()
   //std::cout<<"keyframes->node->estimate()= "<<keyframes[keyframes.size()-1]->node->estimate().translation().x()<<" Y= "<<keyframes[keyframes.size()-1]->node->estimate().translation().y()<<" Z= "<<keyframes[keyframes.size()-1]->node->estimate().translation().z()<<std::endl;
   ROS_INFO("keyframe!! %f,%f,%f" ,keyframe->node->estimate().translation().x(),keyframe->node->estimate().translation().y(),keyframe->node->estimate().translation().z());
//std::cout<<"keyframes->node->estimate()= "<<keyframes[0]->node->estimate().translation().x()<<" Y= "<<keyframes[0]->node->estimate().translation().y()<<" Z= "<<keyframes[0]->node->estimate().translation().z()<<std::endl;

    if(lidar_pose_pub.getNumSubscribers()) {
      lidar_pose_pub.publish(increment_lidar_p);
    }

    if(odom2map_pub.getNumSubscribers()) {
      geometry_msgs::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
      odom2map_pub.publish(ts);

       //std::cout<<"translation X= "<<ts.transform.translation.x<<" Y= "<<ts.transform.translation.y<<" Z= "<<ts.transform.translation.z<<std::endl;
       //std::cout<<"translation X= "<<ts.transform.translation.x<<" Y= "<<ts.transform.translation.y<<" Z= "<<ts.transform.translation.z<<std::endl;
    }

    if(markers_pub.getNumSubscribers()) {
      auto markers = create_marker_array(ros::Time::now());
      markers_pub.publish(markers);
    }
  }

  /**
   * @brief create visualization marker
   * @param stamp
   * @return
   */
  visualization_msgs::MarkerArray create_marker_array(const ros::Time& stamp) const {
    visualization_msgs::MarkerArray markers;
    markers.markers.resize(5);

    // node markers
    visualization_msgs::Marker& traj_marker = markers.markers[0];
    traj_marker.header.frame_id = "map";
    traj_marker.header.stamp = stamp;
    traj_marker.ns = "nodes";
    traj_marker.id = 0;
    traj_marker.type = visualization_msgs::Marker::SPHERE_LIST;

    traj_marker.pose.orientation.w = 1.0;
    traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 0.5;

    visualization_msgs::Marker& imu_marker = markers.markers[4];
    imu_marker.header = traj_marker.header;
    imu_marker.ns = "imu";
    imu_marker.id = 4;
    imu_marker.type = visualization_msgs::Marker::SPHERE_LIST;

    imu_marker.pose.orientation.w = 1.0;
    imu_marker.scale.x = imu_marker.scale.y = imu_marker.scale.z = 0.75;

    traj_marker.points.resize(keyframes.size());
    traj_marker.colors.resize(keyframes.size());
    for(int i=0; i<keyframes.size(); i++) {
      Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
      traj_marker.points[i].x = pos.x();
      traj_marker.points[i].y = pos.y();
      traj_marker.points[i].z = pos.z();

      double p = static_cast<double>(i) / keyframes.size();
      traj_marker.colors[i].r = 1.0 - p;
      traj_marker.colors[i].g = p;
      traj_marker.colors[i].b = 0.0;
      traj_marker.colors[i].a = 1.0;

      if(keyframes[i]->acceleration) {
        Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
        geometry_msgs::Point point;
        point.x = pos.x();
        point.y = pos.y();
        point.z = pos.z();

        std_msgs::ColorRGBA color;
        color.r = 0.0;
        color.g = 0.0;
        color.b = 1.0;
        color.a = 0.1;

        imu_marker.points.push_back(point);
        imu_marker.colors.push_back(color);
      }
    }

    // edge markers
    visualization_msgs::Marker& edge_marker = markers.markers[1];
    edge_marker.header.frame_id = "map";
    edge_marker.header.stamp = stamp;
    edge_marker.ns = "edges";
    edge_marker.id = 1;
    edge_marker.type = visualization_msgs::Marker::LINE_LIST;

    edge_marker.pose.orientation.w = 1.0;
    edge_marker.scale.x = 0.05;

    edge_marker.points.resize(graph_slam->graph->edges().size() * 2);
    edge_marker.colors.resize(graph_slam->graph->edges().size() * 2);

    auto edge_itr = graph_slam->graph->edges().begin();
    for(int i=0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) {
      g2o::HyperGraph::Edge* edge = *edge_itr;
      g2o::EdgeSE3* edge_se3 = dynamic_cast<g2o::EdgeSE3*>(edge);
      if(edge_se3) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[0]);
        g2o::VertexSE3* v2 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[1]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = v2->estimate().translation();

        edge_marker.points[i*2].x = pt1.x();
        edge_marker.points[i*2].y = pt1.y();
        edge_marker.points[i*2].z = pt1.z();
        edge_marker.points[i*2 + 1].x = pt2.x();
        edge_marker.points[i*2 + 1].y = pt2.y();
        edge_marker.points[i*2 + 1].z = pt2.z();

        double p1 = static_cast<double>(v1->id()) / graph_slam->graph->vertices().size();
        double p2 = static_cast<double>(v2->id()) / graph_slam->graph->vertices().size();
        edge_marker.colors[i*2].r = 1.0 - p1;
        edge_marker.colors[i*2].g = p1;
        edge_marker.colors[i*2].a = 1.0;
        edge_marker.colors[i*2 + 1].r = 1.0 - p2;
        edge_marker.colors[i*2 + 1].g = p2;
        edge_marker.colors[i*2 + 1].a = 1.0;

        if(std::abs(v1->id() - v2->id()) > 2) {
          edge_marker.points[i*2].z += 0.5;
          edge_marker.points[i*2 + 1].z += 0.5;
        }

        continue;
      }

      g2o::EdgeSE3Plane* edge_plane = dynamic_cast<g2o::EdgeSE3Plane*>(edge);
      if(edge_plane) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_plane->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0);

        edge_marker.points[i*2].x = pt1.x();
        edge_marker.points[i*2].y = pt1.y();
        edge_marker.points[i*2].z = pt1.z();
        edge_marker.points[i*2 + 1].x = pt2.x();
        edge_marker.points[i*2 + 1].y = pt2.y();
        edge_marker.points[i*2 + 1].z = pt2.z();

        edge_marker.colors[i*2].b = 1.0;
        edge_marker.colors[i*2].a = 1.0;
        edge_marker.colors[i*2 + 1].b = 1.0;
        edge_marker.colors[i*2 + 1].a = 1.0;

        continue;
      }

      g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast<g2o::EdgeSE3PriorXY*>(edge);
      if(edge_priori_xy) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xy->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = Eigen::Vector3d::Zero();
        pt2.head<2>() = edge_priori_xy->measurement();

        edge_marker.points[i*2].x = pt1.x();
        edge_marker.points[i*2].y = pt1.y();
        edge_marker.points[i*2].z = pt1.z() + 0.5;
        edge_marker.points[i*2 + 1].x = pt2.x();
        edge_marker.points[i*2 + 1].y = pt2.y();
        edge_marker.points[i*2 + 1].z = pt2.z() + 0.5;

        edge_marker.colors[i*2].r = 1.0;
        edge_marker.colors[i*2].a = 1.0;
        edge_marker.colors[i*2 + 1].r = 1.0;
        edge_marker.colors[i*2 + 1].a = 1.0;

        continue;
      }

      g2o::EdgeSE3PriorXYZ* edge_priori_xyz = dynamic_cast<g2o::EdgeSE3PriorXYZ*>(edge);
      if(edge_priori_xyz) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xyz->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = edge_priori_xyz->measurement();

        edge_marker.points[i*2].x = pt1.x();
        edge_marker.points[i*2].y = pt1.y();
        edge_marker.points[i*2].z = pt1.z() + 0.5;
        edge_marker.points[i*2 + 1].x = pt2.x();
        edge_marker.points[i*2 + 1].y = pt2.y();
        edge_marker.points[i*2 + 1].z = pt2.z();

        edge_marker.colors[i*2].r = 1.0;
        edge_marker.colors[i*2].a = 1.0;
        edge_marker.colors[i*2 + 1].r = 1.0;
        edge_marker.colors[i*2 + 1].a = 1.0;

        continue;
      }
    }

    // sphere
    visualization_msgs::Marker& sphere_marker = markers.markers[3];
    sphere_marker.header.frame_id = "map";
    sphere_marker.header.stamp = stamp;
    sphere_marker.ns = "loop_close_radius";
    sphere_marker.id = 0;
    sphere_marker.type = visualization_msgs::Marker::SPHERE;

    if(!keyframes.empty()) {
      Eigen::Vector3d pos = keyframes.back()->node->estimate().translation();
      sphere_marker.pose.position.x = pos.x();
      sphere_marker.pose.position.y = pos.y();
      sphere_marker.pose.position.z = pos.z();
    }
    sphere_marker.pose.orientation.w = 1.0;
    sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0;

    sphere_marker.color.r = 1.0;
    sphere_marker.color.a = 0.3;

    return markers;
  }

  /**
   * @brief dump all data to the current directory
   * @param req
   * @param res
   * @return
   */
  bool dump_service(hdl_graph_slam::DumpGraphRequest& req, hdl_graph_slam::DumpGraphResponse& res) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    std::string directory = req.destination;

    if(directory.empty()) {
      std::array<char, 64> buffer;
      buffer.fill(0);
      time_t rawtime;
      time(&rawtime);
      const auto timeinfo = localtime(&rawtime);
      strftime(buffer.data(), sizeof(buffer), "%d-%m-%Y %H:%M:%S", timeinfo);
      std::string directory(buffer.data());
    }

    if(!boost::filesystem::is_directory(directory)) {
      boost::filesystem::create_directory(directory);
    }

    std::cout << "all data dumped to:" << directory << std::endl;

    graph_slam->save(directory + "/graph.g2o");
    for(int i=0; i<keyframes.size(); i++) {
      std::stringstream sst;
      sst << boost::format("%s/%06d") % directory % i;

      keyframes[i]->dump(sst.str());
    }

    if(zero_utm) {
      std::ofstream zero_utm_ofs(directory + "/zero_utm");
      zero_utm_ofs << *zero_utm << std::endl;
    }

    res.success = true;
    return true;
  }

  /**
   * @brief save map data as pcd
   * @param req
   * @param res
   * @return
   */
  bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res) {
    std::vector<KeyFrameSnapshot::Ptr> snapshot;

    keyframes_snapshot_mutex.lock();
    snapshot = keyframes_snapshot;
    keyframes_snapshot_mutex.unlock();

    auto cloud = map_cloud_generator->generate(snapshot, req.resolution);
    if(!cloud) {
      res.success = false;
      return true;
    }

    if(zero_utm && req.utm) {
      for(auto& pt : cloud->points) {
        pt.getVector3fMap() += (*zero_utm).cast<float>();
      }
    }

    cloud->header.frame_id = map_frame_id;
    cloud->header.stamp = snapshot.back()->cloud->header.stamp;

    if(zero_utm) {
      std::ofstream ofs(req.destination + ".utm");
      ofs << (*zero_utm).transpose() << std::endl;
    }

    int ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
    res.success = ret == 0;

    return true;
  }
private:

  //zhen
  KeyFrame::Ptr keyframe_cur;
  KeyFrame::Ptr keyframe_pre;
  double dt4;
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;
  ros::WallTimer optimization_timer;
  ros::WallTimer map_publish_timer;

  std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
  std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> cloud_sub;
  std::unique_ptr<message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>> sync;

  ros::Subscriber gps_sub;
  ros::Subscriber nmea_sub;
  ros::Subscriber navsat_sub;

  ros::Subscriber imu_sub;
  ros::Subscriber floor_sub;

  ros::Publisher markers_pub;

  std::string map_frame_id;
  std::string odom_frame_id;

  std::mutex trans_odom2map_mutex;
  Eigen::Matrix4f trans_odom2map;
  ros::Publisher odom2map_pub;

  std::string points_topic;
  ros::Publisher read_until_pub;
  ros::Publisher map_points_pub;

  ros::Publisher lidar_pose_pub;   //图优化之后的pose发布

  tf::TransformListener tf_listener;

  ros::ServiceServer dump_service_server;
  ros::ServiceServer save_map_service_server;

  // keyframe queue
  std::string base_frame_id;
  std::mutex keyframe_queue_mutex;
  std::deque<KeyFrame::Ptr> keyframe_queue;

  // gps queue
  double gps_time_offset;
  double gps_edge_stddev_xy;
  double gps_edge_stddev_z;
  boost::optional<Eigen::Vector3d> zero_utm;
  std::mutex gps_queue_mutex;
  std::deque<geographic_msgs::GeoPointStampedConstPtr> gps_queue;

  // imu queue
  double imu_time_offset;
  bool enable_imu_orientation;
  double imu_orientation_edge_stddev;
  bool enable_imu_acceleration;
  double imu_acceleration_edge_stddev;
  std::mutex imu_queue_mutex;
  std::deque<sensor_msgs::ImuConstPtr> imu_queue;

  //sim odom queue
  std::mutex sim_odom_queue_mutex;
  std::deque<nav_msgs::OdometryConstPtr> sim_odom_queue;
  double sim_odom_edge_stddev_xy;
  double sim_odom_edge_stddev_z;
  Eigen::Vector3d odom_xyz;

  // floor_coeffs queue
  double floor_edge_stddev;
  std::mutex floor_coeffs_queue_mutex;
  std::deque<hdl_graph_slam::FloorCoeffsConstPtr> floor_coeffs_queue;

  // for map cloud generation
  double map_cloud_resolution;
  std::mutex keyframes_snapshot_mutex;
  std::vector<KeyFrameSnapshot::Ptr> keyframes_snapshot;
  std::unique_ptr<MapCloudGenerator> map_cloud_generator;

  // graph slam
  // all the below members must be accessed after locking main_thread_mutex
  std::mutex main_thread_mutex;

  int max_keyframes_per_update;
  std::deque<KeyFrame::Ptr> new_keyframes;

  g2o::VertexSE3* anchor_node;
  g2o::EdgeSE3* anchor_edge;
  g2o::VertexPlane* floor_plane_node;
  std::vector<KeyFrame::Ptr> keyframes;
  std::unordered_map<ros::Time, KeyFrame::Ptr, RosTimeHash> keyframe_hash;

  std::unique_ptr<GraphSLAM> graph_slam;
  std::unique_ptr<LoopDetector> loop_detector;
  std::unique_ptr<KeyframeUpdater> keyframe_updater;
  std::unique_ptr<NmeaSentenceParser> nmea_parser;

  std::unique_ptr<InformationMatrixCalculator> inf_calclator;

  //*********************************************88
  ros::Subscriber subGps;
  ros::Subscriber first_pose_sub;

  ros::Subscriber sim_odom_sub;

  Eigen::Isometry3d T_init;

  bool use_lidar;

  int count_gps;
  int count_lidar;
};

}

PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::HdlGraphSlamNodelet, nodelet::Nodelet)


#include <openvslam_ros.h>

#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <openvslam/publish/map_publisher.h>
#include <Eigen/Geometry>

namespace openvslam_ros {
system::system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : SLAM_(cfg, vocab_file_path), cfg_(cfg), node_(std::make_shared<rclcpp::Node>("run_slam")), custom_qos_(rmw_qos_profile_default),
      tp_0_(std::chrono::steady_clock::now()), mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
      pose_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("~/camera_pose", 1)) {
    custom_qos_.depth = 1;
    exec_.add_node(node_);
}

void system::publish_pose() {
    // SLAM get the motion matrix publisher
    auto cam_pose_wc = SLAM_.get_map_publisher()->get_current_cam_pose_wc();

    // Extract rotation matrix and translation vector from
    Eigen::Matrix3d rot = cam_pose_wc.block<3, 3>(0, 0);
    Eigen::Vector3d trans = cam_pose_wc.block<3, 1>(0, 3);
    Eigen::Matrix3d cv_to_ros;
    cv_to_ros << 0, 0, 1,
        -1, 0, 0,
        0, -1, 0;

    // Transform from CV coordinate system to ROS coordinate system on camera coordinates
    Eigen::Quaterniond quat(cv_to_ros * rot * cv_to_ros.transpose());
    trans = cv_to_ros * trans;

    // Create odometry message and update it with current camera pose
    nav_msgs::msg::Odometry pose_msg;
    pose_msg.header.stamp = node_->now();
    pose_msg.header.frame_id = "map";
    pose_msg.child_frame_id = "camera_link";
    pose_msg.pose.pose.orientation.x = quat.x();
    pose_msg.pose.pose.orientation.y = quat.y();
    pose_msg.pose.pose.orientation.z = quat.z();
    pose_msg.pose.pose.orientation.w = quat.w();
    pose_msg.pose.pose.position.x = trans(0);
    pose_msg.pose.pose.position.y = trans(1);
    pose_msg.pose.pose.position.z = trans(2);
    pose_pub_->publish(pose_msg);
}

mono::mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : system(cfg, vocab_file_path, mask_img_path) {
    sub_ = image_transport::create_subscription(
        node_.get(), "camera/image_raw", [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { callback(msg); }, "raw", custom_qos_);
}
void mono::callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
    const auto tp_1 = std::chrono::steady_clock::now();
    const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0_).count();

    // input the current frame and estimate the camera pose
    SLAM_.feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_);

    const auto tp_2 = std::chrono::steady_clock::now();

    const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
    track_times_.push_back(track_time);
}
} // namespace openvslam_ros


#include <aslam/backend/TransformationExpressionNode.hpp>
#include <sm/kinematics/transformations.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/EuclideanExpressionNode.hpp>
#include <aslam/backend/ExpressionNodeVisitor.hpp>
#include <aslam/backend/RotationExpression.hpp>
#include <aslam/backend/RotationExpressionNode.hpp>
#include <Eigen/Dense>

namespace aslam {
  namespace backend {

    ////////////////////////////////////////////
    // TransformationExpressionNode: The Super Class
    ////////////////////////////////////////////
    TransformationExpressionNode::TransformationExpressionNode(){}

    TransformationExpressionNode::~TransformationExpressionNode(){}

    void TransformationExpressionNode::evaluateJacobians(JacobianContainer & outJacobians) const
    {
      evaluateJacobiansImplementation(outJacobians);
    }

    void TransformationExpressionNode::getDesignVariables(DesignVariable::set_t & designVariables) const
    {
      getDesignVariablesImplementation(designVariables);
    }

    /////////////////////////////////////////////////
    // TransformationExpressionNodeMultiply: A container for C1 * C2
    /////////////////////////////////////////////////

    TransformationExpressionNodeMultiply::TransformationExpressionNodeMultiply(boost::shared_ptr<TransformationExpressionNode> lhs, boost::shared_ptr<TransformationExpressionNode> rhs):
      _lhs(lhs), _rhs(rhs)
    {
    }

    TransformationExpressionNodeMultiply::~TransformationExpressionNodeMultiply(){}

    void TransformationExpressionNodeMultiply::getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const
    {
      _lhs->getDesignVariables(designVariables);
      _rhs->getDesignVariables(designVariables);
    }

    void TransformationExpressionNode::accept(ExpressionNodeVisitor& visitor) {
      visitor.visit("T", this);
    }

    Eigen::Matrix4d TransformationExpressionNodeMultiply::toTransformationMatrixImplementation()
    {
      _T_lhs = _lhs->toTransformationMatrix();
      _T_rhs = _rhs->toTransformationMatrix();
      return  _T_lhs * _T_rhs;
    }

    void TransformationExpressionNodeMultiply::evaluateJacobiansImplementation(JacobianContainer & outJacobians) const
    {
      _rhs->evaluateJacobians(outJacobians,sm::kinematics::boxTimes(_T_lhs));
      _lhs->evaluateJacobians(outJacobians);
    }

    ////////////////////////////////////////////////////
    /// TransformationExpressionNodeInverse: A container for T^-1
    ////////////////////////////////////////////////////

    TransformationExpressionNodeInverse::TransformationExpressionNodeInverse(boost::shared_ptr<TransformationExpressionNode> dvTransformation) : _dvTransformation(dvTransformation)
    {
    }

    TransformationExpressionNodeInverse::~TransformationExpressionNodeInverse(){}

    Eigen::Matrix4d TransformationExpressionNodeInverse::toTransformationMatrixImplementation()
    {
      _T = _dvTransformation->toTransformationMatrix().inverse();
      return  _T;
    }

    void TransformationExpressionNodeInverse::evaluateJacobiansImplementation(JacobianContainer & outJacobians) const
    {
      _dvTransformation->evaluateJacobians(outJacobians, -sm::kinematics::boxTimes(_T));
    }

    void TransformationExpressionNodeInverse::getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const
    {
      _dvTransformation->getDesignVariables(designVariables);
    }

      TransformationExpressionNodeConstant::TransformationExpressionNodeConstant(const Eigen::Matrix4d & T) : _T(T) {}
      TransformationExpressionNodeConstant::~TransformationExpressionNodeConstant(){}

      Eigen::Matrix4d TransformationExpressionNodeConstant::toTransformationMatrixImplementation(){ return _T; }
  void TransformationExpressionNodeConstant::evaluateJacobiansImplementation(JacobianContainer & /* outJacobians */) const{}
  void TransformationExpressionNodeConstant::getDesignVariablesImplementation(DesignVariable::set_t & /* designVariables */) const{}

  RotationExpression aslam::backend::TransformationExpressionNode::toRotationExpression(const boost::shared_ptr<TransformationExpressionNode>& thisShared) const {
    assert(thisShared.get() == this);
    return boost::shared_ptr< RotationExpressionNode >( new RotationExpressionNodeTransformation( thisShared ) );
  }

  EuclideanExpression aslam::backend::TransformationExpressionNode::toEuclideanExpression(const boost::shared_ptr<TransformationExpressionNode>& thisShared) const {
    assert(thisShared.get() == this);
    return boost::shared_ptr< EuclideanExpressionNode >( new EuclideanExpressionNodeTranslation( thisShared ) );
  }

  void TransformationExpressionNodeMultiply::accept(ExpressionNodeVisitor& visitor) {
    visitor.visit("*", this, _lhs, _rhs);
  }

  void TransformationExpressionNodeInverse::accept(ExpressionNodeVisitor& visitor) {
    visitor.visit("^-1", this, _dvTransformation);
  }

  void TransformationExpressionNodeConstant::accept(ExpressionNodeVisitor& visitor) {
    visitor.visit("#", this);
  }

  } // namespace backend
}  // namespace aslam


#ifndef PARMCB_MPI_SVA_TREES_HPP_
#define PARMCB_MPI_SVA_TREES_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>

#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>
#include <boost/mpi/timer.hpp>

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>
#include <cmath>

#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/detail/fvs.hpp>
#include <parmcb/detail/cycles.hpp>
#include <parmcb/util.hpp>
#include <parmcb/mpi/sptrees.hpp>

namespace parmcb {

    template<class Graph, class WeightMap, class CycleOutputIterator, class CyclesBuilder, bool ParallelUsingTBB>
    typename boost::property_traits<WeightMap>::value_type _mcb_sva_trees_mpi(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out, boost::mpi::communicator &world) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        boost::mpi::timer total_timer;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();
        if (world.rank() == 0) {
            std::cout << "Cycle space dimension: " << csd << std::endl;
        }

        /*
         * Initialize support vectors
         */
        std::vector<SpVecGF2<std::size_t>> support;
        for (std::size_t k = 0; k < csd; k++) {
            support.emplace_back(k);
        }

        /**
         * Scatter candidate cycles
         */
        std::vector<SerializableCandidateCycle<Graph>> candidate_cycles;
        if (world.rank() == 0) {
            /*
             * Compute all vertex-edge candidate pairs
             */
            std::vector<parmcb::SPTree<Graph, WeightMap>> trees;
            std::vector<parmcb::CandidateCycle<Graph, WeightMap>> cycles;
            CyclesBuilder cycles_builder;
            cycles_builder(g, weight_map, trees, cycles);
            std::cout << "Total candidate cycles: " << cycles.size() << std::endl;

            std::vector<SerializableCandidateCycle<Graph>> all_candidate_cycles;
            parmcb::CandidateCycleToSerializableConverter<Graph, WeightMap> converter(trees, forest_index);
            for (const auto &c : cycles) {
                all_candidate_cycles.push_back(converter(c));
            }

            /*
             * Divide them to processes
             */
            std::size_t stride = ceil((double) all_candidate_cycles.size() / world.size());
            std::vector<std::vector<SerializableCandidateCycle<Graph>>> chunks;
            for (int p = 0; p < world.size(); p++) {
                std::size_t istart = p * stride;
                std::size_t iend = istart + stride;
                std::vector<SerializableCandidateCycle<Graph>> chunk;
                std::size_t total = all_candidate_cycles.size();
                for (std::size_t i = istart; i < iend && i < total; i++) {
                    chunk.push_back(all_candidate_cycles[i]);
                }
                chunks.push_back(chunk);
            }
            boost::mpi::scatter(world, chunks, candidate_cycles, 0);
        } else {
            boost::mpi::scatter(world, std::vector<std::vector<SerializableCandidateCycle<Graph>>> { },
                    candidate_cycles, 0);
        }

        std::cout << "Rank " << world.rank() << " received " << candidate_cycles.size() << " candidates" << std::endl;

        /*
         * Group local candidate cycles per vertex
         */
        std::map<Vertex, std::vector<Edge>> perVertexCandidates;
        for (SerializableCandidateCycle<Graph> t : candidate_cycles) {
            Vertex s = t.v;
            Edge e = forest_index(t.e);
            if (perVertexCandidates.find(s) == perVertexCandidates.end()) {
                perVertexCandidates.insert(std::make_pair(s, std::vector<Edge> { }));
            }
            perVertexCandidates[s].push_back(e);
        }

        /*
         * Build trees for local candidate cycles
         */
        std::vector<parmcb::SPTree<Graph, WeightMap>> trees;
        std::vector<parmcb::CandidateCycle<Graph, WeightMap>> cycles;
        for (auto const &p : perVertexCandidates) {
            SPTree<Graph, WeightMap> tree(trees.size(), g, boost::get(boost::vertex_index, g), weight_map, p.first);
            trees.push_back(tree);
            std::vector<CandidateCycle<Graph, WeightMap>> tree_cycles = tree.create_candidate_cycles(p.second.begin(),
                    p.second.end());
            cycles.insert(cycles.end(), tree_cycles.begin(), tree_cycles.end());
        }
        std::cout << "Total candidate cycles: " << cycles.size() << std::endl;
        const bool sorted_cycles = true;
        if (sorted_cycles) {
            // sort
            std::cout << "Sorting cycles" << std::endl;
            std::sort(cycles.begin(), cycles.end(), [](const auto &a, const auto &b) {
                return a.weight() < b.weight();
            });
        }
        ShortestOddCycleLookup<Graph, WeightMap, ParallelUsingTBB> cycle_lookup(g, weight_map, trees, cycles,
                sorted_cycles);

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        for (std::size_t k = 0; k < csd; k++) {
            if (k % 250 == 0) {
                std::cout << "Rank " << world.rank() << " at cycle " << k << std::endl;
            }

            // broadcast support vector
            if (world.rank() == 0) {
                boost::mpi::broadcast(world, support[k], 0);
            } else {
                SpVecGF2<std::size_t> received;
                boost::mpi::broadcast(world, received, 0);
                support[k] = received;
            }

            //std::cout << "Rank " << world.rank() << " has support = " << support[k] << std::endl;

            std::set<Edge> signed_edges;
            convert_edges(support[k], std::inserter(signed_edges, signed_edges.end()), forest_index);

            std::tuple<std::set<Edge>, WeightType, bool> best_local_cycle = cycle_lookup(signed_edges);

            std::vector<typename ForestIndex<Graph>::size_type> best_local_cycle_as_indices;
            convert_edges(std::get<0>(best_local_cycle),
                    std::inserter(best_local_cycle_as_indices, best_local_cycle_as_indices.end()), forest_index);
            SerializableMinOddCycle<Graph, WeightMap> local_min_odd_cycle(best_local_cycle_as_indices,
                    std::get<1>(best_local_cycle), std::get<2>(best_local_cycle));
            SerializableMinOddCycle<Graph, WeightMap> global_min_odd_cycle;

            boost::mpi::reduce(world, local_min_odd_cycle, global_min_odd_cycle,
                    SerializableMinOddCycleMinOp<Graph, WeightMap>(), 0);

            if (world.rank() == 0) {
                std::set<std::size_t> cyclek;
                for (std::size_t e : global_min_odd_cycle.edges) {
                    cyclek.insert(e);
                }
                for (std::size_t l = k + 1; l < csd; l++) {
                    if (support[l] * cyclek == 1) {
                        support[l] += support[k];
                    }
                }

                std::list<Edge> cyclek_edgelist;
                convert_edges(global_min_odd_cycle.edges, std::inserter(cyclek_edgelist, cyclek_edgelist.end()),
                        forest_index);
                mcb_weight += global_min_odd_cycle.weight;
                *out++ = cyclek_edgelist;
            }
        }

        if (world.rank() == 0) {
            std::cout << "Total time: " << total_timer.elapsed() << " (sec)" << std::endl;
        }

        return mcb_weight;

    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_fvs_trees_mpi(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out, boost::mpi::communicator &world) {
        return _mcb_sva_trees_mpi<Graph, WeightMap, CycleOutputIterator,
                parmcb::detail::FVSCyclesBuilder<Graph, WeightMap>, false>(g, weight_map, out, world);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_fvs_trees_tbb_mpi(const Graph &g,
            WeightMap weight_map, CycleOutputIterator out, boost::mpi::communicator &world) {
        return _mcb_sva_trees_mpi<Graph, WeightMap, CycleOutputIterator,
                parmcb::detail::FVSCyclesBuilder<Graph, WeightMap>, true>(g, weight_map, out, world);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_iso_trees_mpi(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out, boost::mpi::communicator &world) {
        return _mcb_sva_trees_mpi<Graph, WeightMap, CycleOutputIterator,
                parmcb::detail::ISOCyclesBuilder<Graph, WeightMap>, false>(g, weight_map, out, world);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_iso_trees_tbb_mpi(const Graph &g,
            WeightMap weight_map, CycleOutputIterator out, boost::mpi::communicator &world) {
        return _mcb_sva_trees_mpi<Graph, WeightMap, CycleOutputIterator,
                parmcb::detail::ISOCyclesBuilder<Graph, WeightMap>, true>(g, weight_map, out, world);
    }

} // namespace mcb

#endif


/*
 * pair_array.hpp
 *
 *  Created on: 2012-10-18
 *      Author: yanghuan
 */

#ifndef PAIR_ARRAY_HPP_
#define PAIR_ARRAY_HPP_

#include <type_traits>
#include <utility>

#include <boost/mpl/front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/string.hpp>

namespace refection {

template<class T,size_t Size>
struct PairArray {
    typedef std::pair<typename PairArray<T,Size - 1>::type,T> type;
};

template<class T>
struct PairArray<T,1> {
    typedef T type;
};

template<class T>
struct PairArray<T,0> {

};

template<class T>
struct PairElementTrait {

};

template<class Sequence, size_t size, class T, class Pair>
constexpr typename std::enable_if<size == 0, Pair>::type
getPairArray(Pair p) {
    return p;
}

template<class Sequence, size_t size, class T, class Pair>
constexpr typename std::enable_if<size != 0, typename PairArray<T, size + sizeof(Pair) / sizeof(T)>::type>::type
getPairArray(Pair p) {
    typedef typename boost::mpl::front<Sequence>::type First;
    typedef typename boost::mpl::pop_front<Sequence>::type Tail;
    typedef PairElementTrait<First> ElementTrait;
    return getPairArray<Tail, boost::mpl::size<Tail>::value, T>(std::make_pair(p, ElementTrait::create()));
}

template<class Sequence,class T>
constexpr typename PairArray<T, boost::mpl::size<Sequence>::value>::type
getPairArray() {
    typedef typename boost::mpl::front<Sequence>::type First;
    typedef typename boost::mpl::pop_front<Sequence>::type Tail;
    typedef PairElementTrait<First> ElementTrait;
    return getPairArray<Tail, boost::mpl::size<Tail>::value, T>(ElementTrait::create());
}

namespace detail {

template<class Sequence, size_t Size>
struct Array {
    typedef typename boost::mpl::front<Sequence>::type First;
    typedef PairElementTrait<First> ElementTrait;
    typedef typename ElementTrait::type element_type;
    typedef typename PairArray<element_type,Size>::type value_type;
    static_assert(sizeof(value_type) == Size * sizeof(element_type),"");
    static const value_type value;
    static constexpr const element_type* array() {
        return reinterpret_cast<const element_type*>(&value);
    }
};

template<class Sequence, size_t Size>
const typename Array<Sequence, Size>::value_type Array<Sequence, Size>::value
    = getPairArray<Sequence,typename Array<Sequence, Size>::element_type>();

template<class Sequence>
struct Array<Sequence, 0> {
    static constexpr const void* array() {
        return nullptr;
    }
};

}  // namespace detail

template<class Sequence>
struct C_Array {
    static const size_t size = boost::mpl::size<Sequence>::value;
    typedef detail::Array<Sequence,size> array_type;
    static constexpr auto array() ->decltype(array_type::array()) {
        return array_type::array();
    }
};

} // namespace refection

#endif /* PAIR_ARRAY_HPP_ */