text:
// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#ifndef heom_matrix_trace_observer_hpp
#define heom_matrix_trace_observer_hpp

#include "heom/observer.hpp"

#include <boost/numeric/ublas/matrix.hpp>

#include "heom/common.hpp"

namespace heom {

namespace blas = boost::numeric::ublas;
using matrix_t = blas::matrix<complex_t>;

class matrix_trace_observation_view : public observation_view
{
public:
	matrix_trace_observation_view() {}

	matrix_trace_observation_view(const real_t time, const complex_t& trace)
		: time_(time), trace_(trace)
	{}

	virtual ~matrix_trace_observation_view() = default;

	virtual void print(std::ostream& os) const
	{
		auto format = real_format;
		os << format % time_;
		os << default_delimiter;
		os << format % trace_.real();
		os << default_delimiter;
		os << format % trace_.imag();
	}

	// non observation_view interface
	const real_t& time() const { return time_; }
	const complex_t& trace() const { return trace_; }

	// check for negative time to see whether the value was already set or not
	bool unset() const { return time_ < 0.0; }

	// for accumulating
	matrix_trace_observation_view& operator+=(const matrix_trace_observation_view& other)
	{
		if (unset()) {
			time_ = other.time_;
		} else {
			bool time_equal = time_ == other.time_;
			if (!time_equal)
				std::cerr << "time mismatch: " << time_ << " vs. " << other.time_ << std::endl;
			assert(time_equal); // NOTE: intended floating point equality check
		}
		trace_ += other.trace_;
		return *this;
	}

	// for averaging after accumulating
	matrix_trace_observation_view& avg(int_t count) {
		trace_ = complex_t { trace_.real() / count, trace_.imag() / count };
		return *this;
	}

private:
	real_t time_ = -1.0; // negative means unset
	complex_t trace_ = 0.0;
};

template<typename SOLVER_T>
class matrix_trace_observer : public observer
{
public:
	/**
	 * Ctor without specifying a product matrix to multiply before computing
	 * the trace. Uses identity matrix.
	 */
	matrix_trace_observer(const hierarchy_graph& hierarchy_graph, SOLVER_T& solver)
		: dim_(solver.heom_instance().states()),
		  id_(true),
		  solver_(solver)
	{
		top_buffer_ = new complex_t[solver_.heom_instance().size_hierarchy_byte() / sizeof(complex_t)];
	}

	/**
	 * Ctor which specifies an additional product matrix to be multiplied from the left
	 * before computing the trace.
	 */
	matrix_trace_observer(const hierarchy_graph& hierarchy_graph, SOLVER_T& solver, const complex_t* prod_matrix_data)
		: dim_(solver.heom_instance().states()), prod_matrix_(new matrix_t(dim_, dim_)), solver_(solver)
	{
		for (size_t i = 0; i < dim_; ++i)
			for (size_t j = 0; j < dim_; ++j)
				(*prod_matrix_)(i,j) = prod_matrix_data[index(i,j)];

		top_buffer_ = new complex_t[solver_.heom_instance().size_hierarchy_byte() / sizeof(complex_t)];
	}

	virtual ~matrix_trace_observer()
	{
		delete[] top_buffer_;
	}

	virtual void write_header(std::ostream& os, bool newline) const
	{
		os << "time" << default_delimiter << "trace_real" << default_delimiter << "trace_imag";
		if (newline)
			os << '\n';
	}

	virtual std::unique_ptr<observation_view> observe(const real_t time)
	{
		solver_.hierarchy_top(top_buffer_);
		return std::unique_ptr<observation_view> { new matrix_trace_observation_view(time, compute_trace(reinterpret_cast<real_t*>(top_buffer_))) }; // create a streamable view
	}

	/**
	 * Return actual, not the interface type.
	 */
	matrix_trace_observation_view observe_trace(const real_t time)
	{
		solver_.hierarchy_top(top_buffer_);
		return matrix_trace_observation_view(time, compute_trace(reinterpret_cast<real_t*>(top_buffer_))); // directly return an object here
	}

private:
	size_t index(size_t i, size_t j) const
	{
		return i * dim_ + j;
	}

	complex_t compute_trace(const real_t* data)
	{
		// compute trace
		const complex_t* data_complex = reinterpret_cast<const complex_t*>(data);
		matrix_t rho(dim_, dim_);

		for (size_t i = 0; i < dim_; ++i)
			for (size_t j = 0; j < dim_; ++j)
				rho(i,j) = data_complex[index(i,j)];

		matrix_t tmp = id_ ? rho : blas::prod(*prod_matrix_, rho); // either use rho, or compute product
		//	blas::matrix_vector_range<matrix_t> diag(tmp, blas::range(0, system_sites), blas::range(0, system_sites));
		//	complex_t trace = blas::sum(diag);

		complex_t trace {0.0, 0.0};
		for (size_t i = 0; i < dim_; ++i)
			trace += tmp(i,i);

		return trace;
	}

	const size_t dim_;
	bool id_ = false;
	std::unique_ptr<matrix_t> prod_matrix_;
	SOLVER_T& solver_;
	complex_t* top_buffer_;
};

} // namespace heom

#endif // heom_matrix_trace_observer_hpp

max_stars_repo_name: noma/dm-heom

max_issues_repo_name: noma/dm-heom

max_forks_repo_name: noma/dm-heom


text:
#include <algorithm>
#include <array>
#include <bits/c++config.h>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <exception>
#include <filesystem>
#include <initializer_list>
#include <iostream>
#include <memory>
#include <numeric>
#include <ostream>
#include <random>
#include <set>
#include <sstream>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

namespace fs = std::filesystem;
using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::Vector2i;
using Eigen::aligned_allocator;
using RowMatrixXd =
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using RowMatrixXi =
    Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

auto readData(const std::string& inputPathString) {
  fs::path inputPath(inputPathString);
  std::set<std::string> validExts{{".jpg", ".png", ".jpeg", ".tiff"}};
  std::vector<cv::Mat> X;
  std::vector<int> y;
  for (const auto &dirEntry : fs::directory_iterator(inputPath)) {
    if (!(dirEntry.is_regular_file() &&
          validExts.find(dirEntry.path().extension().string()) != validExts.end()))
      continue;
    fs::path imagePath = dirEntry.path();
    cv::Mat image = cv::imread(imagePath.string());
    if (image.channels() > 1) {
      // Convert to grayscale.
      cv::Mat imageGray;
      cv::cvtColor(image, imageGray, cv::COLOR_BGR2GRAY);
      X.push_back(imageGray);
    }
    else
      X.push_back(image);
    int label = std::stoi(dirEntry.path().stem()) / 100;
    y.push_back(label);
  }
  return std::make_tuple(X, y);
}

template<std::size_t TABLE_SIZE = 0>
auto computeGLCMMatrix(const std::vector<cv::Mat>& Xs, const Eigen::MatrixX2i& dirs,
                       std::array<uchar, TABLE_SIZE> quantizeTable = {}) {
  for (cv::Mat X : Xs)
    if (X.depth() != CV_8U)
      throw std::invalid_argument("Image must be 8-bit grayscale");
  int nGrays;
  uchar invQuant[256];
  if constexpr (quantizeTable.empty()) {
    nGrays = 256;
  } else {
    // Get number of gray levels.
    nGrays = 1;
    for (int i = 0; i < 255; i++)
      if (quantizeTable[i] != quantizeTable[i + 1])
        nGrays++;
    // Create inverse lookup table.
    invQuant[0] = 0;
    for (int i = 1; i < 256; i++)
      if (quantizeTable[i - 1] != quantizeTable[i])
        invQuant[i] = invQuant[i - 1] + 1;
      else
        invQuant[i] = invQuant[i - 1];
  }
  std::vector<std::vector<MatrixXd, aligned_allocator<MatrixXd>>> GLCMs;
  GLCMs.reserve(Xs.size());
  // For each image in the dataset...
  for (cv::Mat X : Xs) {
    std::vector<MatrixXd, aligned_allocator<MatrixXd>> imageGLCMs;
    imageGLCMs.reserve(dirs.rows());
    // ...and for each direction...
    for (int k = 0; k < dirs.rows(); k++) {
      // ...compute the GLCM.
      MatrixXi GLCM = MatrixXi::Zero(nGrays, nGrays);
      int di = dirs(k, 0), dj = dirs(k, 1);
      int si = std::abs(std::min(di, 0)), sj = std::abs(std::min(dj, 0));
      int ei = X.rows - std::max(di, 0), ej = X.cols - std::max(dj, 0);
      for (int i = si; i < ei; i++) {
        uchar* Xi = X.ptr<uchar>(i);
        uchar* Xipd = X.ptr<uchar>(i + di);
        for (int j = sj; j < ej; j++) {
          int row = Xi[j], col = Xipd[j + dj];
          if constexpr (quantizeTable.empty())
            GLCM(row, col)++;
          else
            // Compute matrix values using indices from reverse mapping.
            GLCM(invQuant[row], invQuant[col])++;
        }
      }
      imageGLCMs.push_back(GLCM.cast<double>() / ((ei - si) * (ej - sj)));
    }
    GLCMs.push_back(imageGLCMs);
  }
  return GLCMs;
}

MatrixXd
getFeatureVectors(const std::vector<std::vector<MatrixXd,
                  aligned_allocator<MatrixXd>>>& GLCMs) {
  if (GLCMs.empty() || GLCMs[0].empty())
    throw std::invalid_argument("Empty vector of GLCMs");
  std::size_t rows = GLCMs.size(), cols = GLCMs[0].size();
  MatrixXd features(rows, 7 * cols);
  Eigen::ArrayXd arange =
      Eigen::ArrayXd::LinSpaced(GLCMs[0][0].rows(), 0, GLCMs[0][0].rows() - 1);
  Eigen::MatrixXd pairwiseDiff =
    arange.rowwise().replicate(arange.size()).rowwise() - arange.transpose();
  for (std::size_t i = 0; i < rows; i++) {
    for (std::size_t j = 0; j < cols; j++) {
      MatrixXd GLCM = GLCMs[i][j];
      // Maximum probability
      double maxProb = GLCM.maxCoeff();
      // Correlation
      double expectedRow =
          (GLCM.rowwise().sum().array() * arange).sum();
      double expectedColumn =
        (GLCM.colwise().sum().transpose().array() * arange).sum();
      Eigen::ArrayXd deviationsRow = arange - expectedRow;
      Eigen::ArrayXd deviationsColumn = arange - expectedColumn;
      double varianceRow =
        (GLCM.rowwise().sum().array() * deviationsRow.square()).sum();
      double varianceColumn =
        (GLCM.colwise().sum().transpose().array() * deviationsColumn.square()).sum();
      double correlation =
        (deviationsRow.matrix().transpose() * (GLCM * deviationsColumn.matrix()))[0] /
        std::sqrt(varianceRow * varianceColumn);
      // Contrast
      double contrast = (pairwiseDiff.array().square() * GLCM.array()).sum();
      // Uniformity
      double uniformity = GLCM.array().square().sum();
      // Homogeneity
      double homogeneity = (GLCM.array() / (1 + pairwiseDiff.array().abs())).sum();
      // Entropy
      Eigen::MatrixXd zeroCorrectedGLCM = (GLCM.array() == 0)
        .select(MatrixXd::Ones(GLCM.rows(), GLCM.cols()), GLCM);
      double entropy = -(GLCM.array() * zeroCorrectedGLCM.array().log2()).sum();
      // Dissimilarity
      double dissimilarity = (GLCM.array() * pairwiseDiff.array().abs()).sum();

      features.block<1, 7>(i, 7 * j) << maxProb, correlation, contrast,
                                        uniformity, homogeneity, entropy,
                                        dissimilarity;
    }
  }
  return features;
}

template<int QUANTIZE = 256>
MatrixXd extractFeatures(const std::vector<cv::Mat>& Xs, int dirRange) {
  // Compute directions for GLCMs.
  // int r = 0;
  // int dirRows = std::pow(2 * dirRange + 1, 2) - 1;
  // Eigen::MatrixX2i dirs(dirRows, 2);
  // for (int i = -dirRange; i <= dirRange; i++)
  //   for (int j = -dirRange; j <= dirRange; j++)
  //     dirs(r, 0) = i, dirs(r++, 1) = j;
  // Downscale X to 'QUANTIZE' levels.
  Eigen::MatrixX2i dirs(4, 2);
  dirs << 1,  1,
          1, -1,
         -1,  1,
         -1, -1;
  std::vector<cv::Mat> quantizedXs;
  quantizedXs.reserve(Xs.size());
  constexpr std::array<uchar, 256> lookupTable = [] {
    std::array<uchar, 256> table = {};
    for (int i = 0; i < 256; i++)
      table[i] = QUANTIZE * (i / QUANTIZE);
    return table;
  }();
  for (std::size_t i = 0; i < Xs.size(); i++) {
    cv::Mat quantizedX;
    cv::LUT(Xs[i], lookupTable, quantizedX);
    quantizedXs.push_back(quantizedX);
  }
  // Compute GLCM.
  std::vector<std::vector<MatrixXd, aligned_allocator<MatrixXd>>> GLCMs;
  if constexpr (QUANTIZE < 256)
    GLCMs = computeGLCMMatrix(quantizedXs, dirs, lookupTable);
  else
    GLCMs = computeGLCMMatrix(Xs, dirs);
  return getFeatureVectors(GLCMs);
}

template<bool returnPermutations = false>
auto splitTrainTest(const MatrixXd& Xs, std::vector<int> ys,
                    int nClass, int sizeClass, int sizeTrain) {
  // Wrap std::vector data in Eigen object.
  Eigen::VectorXi ysEigen = Eigen::Map<Eigen::VectorXi,
                                       Eigen::Unaligned>(ys.data(), ys.size());
  int sizeTest = sizeClass - sizeTrain;
  MatrixXd trainX(nClass * sizeTrain, Xs.cols());
  Eigen::VectorXi trainY(nClass * sizeTrain);
  MatrixXd testX(nClass * sizeTest, Xs.cols());
  Eigen::VectorXi testY(nClass * sizeTest);
  std::vector<Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic>> permutations;
  if constexpr (returnPermutations)
    permutations.reserve(nClass);
  std::random_device rd;
  std::mt19937 gen(rd());
  Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> perm(sizeClass);
  perm.setIdentity();
  for (int i = 0; i < nClass; i++) {
    std::shuffle(perm.indices().data(),
                 perm.indices().data() + sizeClass,
                 gen);
    if constexpr (returnPermutations)
      permutations.push_back(perm);
    MatrixXd shuffledX = perm * Xs.block(i * sizeClass, 0, sizeClass, Xs.cols());
    MatrixXd trainBlock = shuffledX.block(0, 0, sizeTrain, Xs.cols());
    MatrixXd testBlock = shuffledX.block(sizeTrain, 0, sizeTest, Xs.cols());
    trainX.block(i * sizeTrain, 0,
                 sizeTrain, Xs.cols()) = trainBlock;
    testX.block(i * sizeTest, 0,
                sizeTest, Xs.cols()) = testBlock;
    // Shuffle labels along with data.
    ysEigen.segment(i * sizeClass, sizeClass) =
        perm * ysEigen.segment(i * sizeClass, sizeClass);
    trainY.segment(i * sizeTrain, sizeTrain) =
      ysEigen.segment(i * sizeClass, sizeTrain);
    testY.segment(i * sizeTest, sizeTest) =
      ysEigen.segment(sizeTrain + i * sizeClass, sizeTest);
  }
  if constexpr (returnPermutations)
    return std::make_tuple(trainX, trainY, testX, testY, permutations);
  else
    return std::make_tuple(trainX, trainY, testX, testY);
}

template<int P, bool SELECT = false>
RowMatrixXi findClosest(const MatrixXd& train, const MatrixXd& queries, int k = 5) {
  // Compute distance matrix.
  RowMatrixXi closest(queries.rows(), k);
  RowMatrixXd distances(queries.rows(), train.rows());
  for (int i = 0; i < queries.rows(); i++) {
    distances.row(i) =
      (train.rowwise() - queries.row(i)).rowwise().lpNorm<P>();
  }
  std::vector<int> idxs(distances.cols());
  int i = 0;
  for (const auto& row : distances.rowwise()) {
    auto comparator = [&row](const int &a, const int &b) {
      return row(a) < row(b);
    };
    // Generate indices.
    std::iota(idxs.begin(), idxs.end(), 0);
    if constexpr (SELECT)
      // Select first k indices using distance as comparison key.
      std::nth_element(idxs.begin(), idxs.begin() + (k - 1), idxs.end(),
                       comparator);
    else
      // Sort all indices using distance as comparison key.
      std::sort(idxs.begin(), idxs.end(), comparator);
    std::copy(idxs.begin(), idxs.begin() + k, closest.row(i++).begin());
  }
  return closest;
}

void logPerformanceMetrics(const MatrixXi& retrievedLabels,
                           const Eigen::VectorXi& testLabels,
                           int numClasses = 10) {
  Eigen::Index retrievedCols = retrievedLabels.cols();
  Eigen::Index retrievedRows = retrievedLabels.rows();
  std::initializer_list<Eigen::Index> nRetrieved =
    {10, 30, 50, 100, retrievedCols};
  for (Eigen::Index nret : nRetrieved)
    if (nret < retrievedCols) {
      MatrixXi currentRetrievedLabels =
        retrievedLabels.block(0, 0, retrievedRows, nret);
      MatrixXi currentTestLabels =
          testLabels.rowwise().replicate(nret);
      MatrixXi confusionMatrix = MatrixXi::Zero(numClasses, numClasses);
      for (int i = 0; i < currentTestLabels.rows(); i++)
        for (int j = 0; j < currentTestLabels.cols(); j++) {
          int row = currentRetrievedLabels(i, j);
          int col = currentTestLabels(i, j);
          confusionMatrix(row, col)++;
        }
      Eigen::VectorXd diag = confusionMatrix.diagonal().cast<double>();
      Eigen::VectorXd sumCWise =
        confusionMatrix.colwise().sum().cast<double>();
      Eigen::VectorXd sumRWise =
        confusionMatrix.rowwise().sum().cast<double>();
      Eigen::VectorXd recall = diag.array() / sumCWise.array();
      Eigen::VectorXd precision = diag.array() / sumRWise.array();
      std::cout << "------------------------------\n";
      std::cout << "Number retrieved per query: " << nret << " \n";
      for (int i = 0; i < numClasses; i++) {
        std::cout << "Class: " << i << '\n';
        std::cout << "\tPrecision (" << nret <<"): " << precision(i) << '\n';
        std::cout << "\tRecall (" << nret << "): " << recall(i) << '\n';
      }
      std::cout << "Number of retrieved elements: " << confusionMatrix.sum()
                << '\n';
      std::cout << "Confusion matrix:\n";
      std::cout << confusionMatrix << '\n';
      std::cout << "Average precision (" << nret << "): "
                << precision.mean() << '\n';
      std::cout << "Average recall (" << nret << "): "
                << recall.mean() << '\n';
    }
}

int main() {
  using ms = std::chrono::milliseconds;
  std::chrono::steady_clock::time_point timeStart;
  std::chrono::steady_clock::time_point timeEnd;
  int k = 100;
  std::cout << "Reading data...\n";
  timeStart = std::chrono::steady_clock::now();
  auto [X, y] = readData("data/lab4");
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  std::cout << "Constructing feature vectors...\n";
  timeStart = std::chrono::steady_clock::now();
  MatrixXd featureMatrix = extractFeatures<256>(X, 3);
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  std::cout << "Splitting into train and test sets...\n";
  timeStart = std::chrono::steady_clock::now();
  auto [trainX, trainY, testX, testY, permutations] =
    splitTrainTest<true>(featureMatrix, y, 10, 100, 70);
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  std::cout << "Finding closest " << k << " points...\n";
  timeStart = std::chrono::steady_clock::now();
  MatrixXi closestIndices = findClosest<1, false>(trainX, testX, k);
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  auto labelFetchLambda = [&trainY](int elem) { return trainY(elem); };
  MatrixXi retrievedY = closestIndices.unaryExpr(labelFetchLambda);
  // for (int i = 0; i < retrievedY.rows(); i++)
  //   std::cout << retrievedY(i, Eigen::all) << " -> " << testY[i] <<
  //   std::endl;
  logPerformanceMetrics(retrievedY, testY, 10);
  // Save some queries along with their top 5 most similar responses.
  // First, permute the images using the random permutations returned
  // by the splitTrainTest function.
  std::vector<int> idxs(X.size());
  std::iota(idxs.begin(), idxs.end(), 0);
  Eigen::VectorXi idxsEigen =
    Eigen::Map<Eigen::VectorXi, Eigen::Unaligned>(idxs.data(), idxs.size());
  for (int i = 0; i < 10; i++) {
    idxsEigen.segment<100>(i * 100) =
      permutations[i].transpose() * idxsEigen.segment<100>(i*100);
  }
  for (int i = 0; i < 3; i++) {
    int queryIdx = idxsEigen(70 + i);
    cv::Mat queryImage = X[queryIdx];
    std::ostringstream ss1;
    ss1 << "output/lab4/images/query" << i << ".jpg";
    std::string outputPathString1 = ss1.str();
    cv::imwrite(outputPathString1, queryImage);
    for (int k = 0; k < 5; k++) {
      int retrievedIdx = idxsEigen(closestIndices(i, k));
      cv::Mat retrievedImage = X[retrievedIdx];
      std::ostringstream ss2;
      ss2 << "output/lab4/images/retrieved" << i << "_" << k << ".jpg";
      std::string outputPathString2 = ss2.str();
      cv::imwrite(outputPathString2, retrievedImage);
    }
  }
  return EXIT_SUCCESS;
}

ext: cpp

max_stars_repo_name: claudiu-ghiga/image-classifier

max_issues_repo_name: claudiu-ghiga/image-classifier

max_forks_repo_name: claudiu-ghiga/image-classifier


text:
//把kitti的groundtru格式类型的pose(时间戳 4X3的转移矩阵[去掉了0 0 0 1]) 转化为 Tum数据集类型的数据( 'timestamp tx ty tz qx qy qz qw' )
//在转化的过程中经过一个对pose进行的一个变换transpose*pose*transposeInv 保存 , 详情见 int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses, Eigen::Matrix4d &transpose);

#include <iostream>
#include <iomanip> //要用到格式控制符
#include <fstream>
#include <boost/program_options.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <boost/format.hpp>  // for formating strings

#include <vector>
#include <string>

#include <sstream>
#include <boost/filesystem.hpp>
#include <algorithm>
#include <iterator>
#include <math.h>

using namespace std;

int loadKittiPoses(const string & file_name, vector<Eigen::Matrix4d> &poses) ;
int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses, Eigen::Matrix4d &transpose);
int loadKittiTimes( const string & file_name,  vector<double> & timestamps);
int convert(  vector<Eigen::Matrix4d> & poses,  vector<double> & timestamps,  const string & filename);

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

    if (argc != 4)
    {
        //example: ./project    kitti类型的pose.txt   kitti时间戳文件夹   要输出的文件名
        cerr << endl << "Usage: ./project    kittiPoseXX.txt     kittiTimeXX.txt    tumOutxx.txt " << endl;
        return 1;
    }

    string kittipath =  std::string(argv[1]);
    string kittTimepath =  std::string(argv[2]);
    string tumFileName = std::string(argv[3]);
    cout << kittipath << endl;
    cout << kittTimepath << endl;
    cout << tumFileName << endl;

    vector<Eigen::Matrix4d> poses;         // 相机位姿
    vector<double>  timestamps;// 时间戳

     Eigen::Matrix4d T_rectC2_to_rectC0;
     T_rectC2_to_rectC0 << 1.0000, 0.0000,   -0.0000,   -0.0595,
             0.0000,    1.0000,    0.0000,    0.0015,
             -0.0000,   -0.0000,    1.0000,   -0.0038,
             0,         0,         0,    1.0000; //00-02

//     T_rectC2_to_rectC0 << 1.0000, 0.0000,   -0.0000,   -0.0596,
//             0.0000,    1.0000,    0.0000,    0.0006,
//             -0.0000,   -0.0000,    1.0000,   -0.0027,
//             0,         0,         0,    1.0000; //03

//     T_rectC2_to_rectC0 << 1.0000, 0.0000,   -0.0000,   -0.0602,
//             0.0000,    1.0000,    0.0000,    0.0021,
//             -0.0000,   -0.0000,    1.0000,   -0.0062,
//             0,         0,         0,    1.0000;//04-10

    //load  poses
    if (loadKittiPoses(kittipath,  poses, T_rectC2_to_rectC0) == 0 )
    {
        cerr << "cannot find pose file" << endl;
        return 0;
    }
    //load  poses
    if (loadKittiTimes(kittTimepath,  timestamps) == 0 )
    {
        cerr << "cannot find time file" << endl;
        return 0;
    }

    if ( convert( poses, timestamps, tumFileName) == 0)
    {
        cerr << "convert failed!!!" << endl;
        return 0;
    }

    cout << "It is done!------------------------------------------------" << endl;

    return 1;
}

int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses)
{
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[3] [4];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   &P[0][0], &P[0][1], &P[0][2], &P[0][3],
                   &P[1][0], &P[1][1], &P[1][2], &P[1][3],
                   &P[2][0], &P[2][1], &P[2][2], &P[2][3] ) == 12) {
            Eigen::Matrix4d T;
            T << P[0][0], P[0][1], P[0][2], P[0][3],
            P[1][0], P[1][1], P[1][2], P[1][3],
            P[2][0], P[2][1], P[2][2], P[2][3],
            0, 0, 0, 1;
            poses.push_back(T);
        }
    }
    fclose(fp);
    return 1;

}

//保存transpose*T*transposeInv到poses
int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses, Eigen::Matrix4d &transpose)
{
 Eigen::Matrix4d transposeInv=transpose.inverse();
	cout<<"transpose: "<<transpose<<endl;
cout<<"transposeInv: "<<transposeInv<<endl;
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[3] [4];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   &P[0][0], &P[0][1], &P[0][2], &P[0][3],
                   &P[1][0], &P[1][1], &P[1][2], &P[1][3],
                   &P[2][0], &P[2][1], &P[2][2], &P[2][3] ) == 12) {
            Eigen::Matrix4d T;
            T << P[0][0], P[0][1], P[0][2], P[0][3],
            P[1][0], P[1][1], P[1][2], P[1][3],
            P[2][0], P[2][1], P[2][2], P[2][3],
            0, 0, 0, 1;
            poses.push_back(transpose*T*transposeInv);
        }
    }
    fclose(fp);
    return 1;

}

int loadKittiTimes( const string & file_name,  vector<double> & timestamps)
{
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double timestamp;
        if (fscanf(fp, "%lf ", &timestamp ) == 1) {
            timestamps.push_back(timestamp);
        }
    }
    fclose(fp);
    return 1;

}

int convert(  vector<Eigen::Matrix4d> & poses,  vector<double> & timestamps,  const string & filename)
{
    cout << endl << "Saving camera pose to " << filename << " ..." << endl;
    ofstream fileout;
    fileout.open(filename.c_str(),std::ios::out);
    fileout << "#Format: timestamp tx ty tz qx qy qz qw" << endl ;

    vector<double>::iterator itTimestamp = timestamps.begin();
    if (poses.size() != timestamps.size())
    {
        cerr << "!!!-----------poses.size() != timestamps.size()----------!!!" << endl;
        //return 0;
    }

//NOTE 这里以pose的size()为准
    for (vector<Eigen::Matrix4d>::iterator it = poses.begin(); it != poses.end(); it++, itTimestamp++)
    {
        Eigen::Matrix4d tmpPose = *it;
        Eigen::Matrix3d tmpRotationMatrix = tmpPose.block(0, 0, 3, 3);
        Eigen::Quaterniond q = Eigen::Quaterniond ( tmpRotationMatrix ); //// 请注意四元数 的顺序是(x,y,z,w), w 为实部，前三者为虚部
        //cout<< q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
        fileout << setprecision(6) << *itTimestamp << " " <<  setprecision(9) <<  tmpPose(0, 3) << " " << tmpPose(1, 3) << " " << tmpPose(2, 3) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;

    }

    fileout.close();
    cout << endl << "trajectory saved!" << endl;
    return 1;

}