#include <QApplication>

#include <boost/qvm/all.hpp>
#include "qvm_eigen.h"
#include "qvm_osg.h"

//#include "rtcvImage.h"

#include "Roi3DF_osg.h"

#include "QtOsgWidget.h"
#include "OsgGeometryManager.h"
#include "osg_utils.h"

#include "DisplayableAreaLights.h"
#include "DisplayableCameras.h"
#include "DisplayableCoordinateSystem.h"
#include "DisplayableGrid.h"

#include "CoordinateTransform.h"
#include "CoordinateSystem.h"

#include "Camera.h"
#include "Rotation.h"

#include "commonTypes.h"

struct UnrealTag {};

using UnrealCoordinateSystem = CoordinateSystem<3, ScalarType, UnrealTag>;

using UnrealVector = NamedType<UnrealCoordinateSystem::Vector, UnrealCoordinateSystem, VectorTypeSkills>;
using UnrealTransform = NamedType<UnrealCoordinateSystem::AffineTransform, UnrealCoordinateSystem>;

using UnrealToWorld = CoordinateTransform<UnrealCoordinateSystem, WorldCoordinateSystem>;

using UnrealRotation = Rotation<UnrealCoordinateSystem>;

UnrealTransform getCameraTransformation_ue(Degrees yaw, Degrees pitch, Degrees roll)
{
	const Radians angle_rotZ = deg2rad<ScalarType>(yaw);
	const Radians angle_rotY = deg2rad<ScalarType>(pitch);
	const Radians angle_rotX = deg2rad<ScalarType>(roll);

	const float cZ = cos(angle_rotZ.get());
	const float cY = cos(angle_rotY.get());
	const float cX = cos(angle_rotX.get());

	const float sZ = sin(angle_rotZ.get());
	const float sY = sin(angle_rotY.get());
	const float sX = sin(angle_rotX.get());

	Matrix33 mat_rotZ;
	Matrix33 mat_rotX;
	Matrix33 mat_rotY;

	mat_rotZ <<
		cZ , -sZ , 0 ,
		sZ , cZ , 0 ,
		0 , 0 , 1;

	mat_rotY <<
		cY , 0 , -sY ,
		0 , 1 , 0 ,
		sY , 0 , cY;

	mat_rotX <<
		1 , 0 , 0 ,
		0 , cX , sX ,
		0 , -sX , cX;

	Matrix33 mat_rot = mat_rotZ * mat_rotY * mat_rotX;

	return UnrealTransform(Transform(mat_rot));
}

CameraGeometry::Info makeInfo(const Camera<WorldCoordinateSystem> & cam)
{
	using namespace boost::qvm;

	using Size = Camera<WorldCoordinateSystem>::Size;

	const osg::Vec3 origin = convert_to<osg::Vec3>(cam.getOrigin().get());

	const Size imageSize = cam.getImageSize();

	const ImagePoint topLeft = make_named<ImagePoint>(0, 0);
	const ImagePoint topRight = make_named<ImagePoint>(imageSize(0), 0);
	const ImagePoint bottomLeft = make_named<ImagePoint>(0, imageSize(1));
	const ImagePoint bottomRight = make_named<ImagePoint>(imageSize(0), imageSize(1));

	const ViewRay<WorldCoordinateSystem> dir_topLeft = cam.imageToWorld(topLeft);
	const ViewRay<WorldCoordinateSystem> dir_topRight = cam.imageToWorld(topRight);
	const ViewRay<WorldCoordinateSystem> dir_bottomLeft = cam.imageToWorld(bottomLeft);
	const ViewRay<WorldCoordinateSystem> dir_bottomRight = cam.imageToWorld(bottomRight);

	return CameraGeometry::Info
	{
		origin,
		convert_to<osg::Vec3>(dir_topLeft.direction.get()),
		convert_to<osg::Vec3>(dir_topRight.direction.get()),
		convert_to<osg::Vec3>(dir_bottomLeft.direction.get()),
		convert_to<osg::Vec3>(dir_bottomRight.direction.get())
	};
}

class DisplayablePoints : public OsgDisplayable
{
public:
	DisplayablePoints(const std::vector<osg::Vec3> & points, float size) :
		m_points(points),
		m_size(size)
	{
		// empty
	}

	osg::ref_ptr<osg::Group> getGeometry() const override
	{
		osg::ref_ptr<osg::Group> camGroup = new osg::Group();
		for (const auto & p : m_points)
		{
			osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(osg::ref_ptr<osg::Sphere>(new osg::Sphere(p, m_size)));
			sd->setColor(osg::Vec4(1, 0, 0, 1));
			camGroup->addChild(sd);
		}

		return camGroup;
	}

private:
	std::vector<osg::Vec3> m_points;
	float m_size;
};

int main(int argc, char ** argv)
{
	UnrealCoordinateSystem::setDirections({ Vector3(1, 0, 0), Vector3(0, -1, 0), Vector3(0, 0, 1) });
	WorldCoordinateSystem::setDirections({ Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) });

	Camera<WorldCoordinateSystem> camera;

	using Vector = WorldCoordinateSystem::Vector;
	using FocalLength = decltype(camera)::FocalLength;
	using Size = decltype(camera)::Size;

	camera.setBaseDirections(
		{
			make_named<WorldVector>(0.f, -1.f, 0.f),
			make_named<WorldVector>(0.f, 0.f, -1.f),
			make_named<WorldVector>(1.f, 0.f, 0.f)
		}
	);

	const Degrees yaw(223.12f);
	const Degrees pitch(316.78f);
	const Degrees roll(0.f);

	const UnrealVector ue_cameraPos(Vector(31.61f, 28.6f, 23.54f));
	const UnrealRotation ue_cameraRotation = UnrealRotation(yaw, pitch, roll);

	std::cout << ue_cameraPos(0) << ", " << ue_cameraPos(1) << ", " << ue_cameraPos(2) << std::endl;

	WorldVector w_cameraPos = UnrealToWorld::sourceToTarget(ue_cameraPos);
	WorldVector w_cameraPos2 = convertTo<WorldCoordinateSystem>(ue_cameraPos);
	WorldRotation w_cameraRotation(ue_cameraRotation);

	camera.extrinsicTransform(w_cameraRotation);
	camera.extrinsicTransform(make_named<WorldTransform>(Translation(w_cameraPos.get())));

	camera.setFocalLength(FocalLength(713.2f));
	camera.setOpticalCenter(make_named<ImagePoint>(400, 300));
	camera.setImageSize(make_named<Size>(800, 600));

	std::ofstream("C:/TEMP/cam.txt") << camera;
	decltype(camera) loaded;
	std::ifstream("C:/TEMP/cam.txt") >> loaded;

	std::cout << std::boolalpha << (camera == camera) << std::endl;
	std::cout << std::boolalpha << (camera == Camera<WorldCoordinateSystem>()) << std::endl;
	std::cout << std::boolalpha << (camera == loaded) << std::endl;

	std::vector<UnrealVector> ue_points =
	{
		make_named<UnrealVector>(0.f, 0.f, 0.f),
		make_named<UnrealVector>(0.f, -15.f, 0.f),
		make_named<UnrealVector>(-10.f, -10.f, 0.f),
		make_named<UnrealVector>(19.f, 13.f, 0.f),
		make_named<UnrealVector>(-8.f, 16.f, 0.f)
	};

	std::vector<WorldVector> w_points;
	std::transform(ue_points.begin(), ue_points.end(), std::back_inserter(w_points),
	               [](const UnrealVector & ue_v)
	               {
		               return UnrealToWorld::sourceToTarget(ue_v);
	               }
	);

	for (const auto & w_p : w_points)
	{
		std::cout << w_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<decltype(camera)::CameraVector> cam_points;
	std::transform(w_points.begin(), w_points.end(), std::back_inserter(cam_points),
	               [&camera](const WorldVector & w_v)
	               {
		               return camera.worldToCamera(w_v);
	               });

	for (const auto & cam_p : cam_points)
	{
		std::cout << cam_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<ImagePoint> img_points;
	std::transform(w_points.begin(), w_points.end(), std::back_inserter(img_points),
	               [&camera](const WorldVector & w_v)
	               {
		               return camera.worldToImage(w_v);
	               });

	for (const auto & img_p : img_points)
	{
		std::cout << img_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<decltype(camera)::CameraVector> cam_points_reconstructed;
	std::transform(img_points.begin(), img_points.end(), std::back_inserter(cam_points_reconstructed),
		[&camera](const ImagePoint & p)
	{
		return camera.imageToCamera(p);
	});

	for (const auto & cam_p : cam_points_reconstructed)
	{
		std::cout << cam_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<ViewRay<WorldCoordinateSystem>> viewRays;
	std::transform(img_points.begin(), img_points.end(), std::back_inserter(viewRays),
		[&camera](const ImagePoint & p)
	{
		return camera.imageToWorld(p);
	});

	for (int i = 0; i < viewRays.size(); ++i)
	{
		const auto & v = viewRays[i];
		const float dist = (w_points[i].get() - v.origin.get()).norm();
		std::cout << v.origin.get() + dist * v.direction.get() << std::endl << std::endl;
	}

	/*rtcvImageRgba img;
	img.resize(800, 600);
	img.fill(rtcvRgbaValue(20, 20, 20));

	for(const auto & p : img_points)
	{
		img.drawRoi(rtcvRoi(rtcvPoint(p.get()(0), p.get()(1)), 10, 10), rtcvRed);
	}

	img.writeToFile("C:/TEMP/testImg.ppm");*/

	QApplication app(argc, argv);

	Roi3DF area(osg::Vec3(-30, -30, 0), osg::Vec3(30, 30, 20));

	QtOsgWidget widget(nullptr);

	osg::StateSet * rootStateSet = widget.getRoot()->getOrCreateStateSet();
	rootStateSet->setMode(GL_LIGHTING, osg::StateAttribute::ON);
	rootStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);

	//widget.addNodeToScene(createLightsFromUsableArea(area));

	GeometryManager manager(&widget);

	manager.switchGeometry(std::make_unique<DisplayableAreaLight>(area));
	manager.switchGeometry(std::make_unique<DisplayableGrid>(area, osg::Vec3(1, 1, 1)));
	manager.switchGeometry(std::make_unique<DisplayableCoordinateSystem>(osg::Vec3(0, 0, 0), 0.5f, 5.f));

	std::vector<CameraGeometry::Info> camerasToDisplay = { makeInfo(camera) };
	manager.switchGeometry(std::make_unique<DisplayableCameras>(camerasToDisplay, 1.f, 5.f));

	std::vector<osg::Vec3> pointsToDisplay;
	std::transform(w_points.begin(), w_points.end(), std::back_inserter(pointsToDisplay),
		[](const WorldVector & v)
	{
		return boost::qvm::convert_to<osg::Vec3>(v.get());
	}
	);

	manager.switchGeometry(std::make_unique<DisplayablePoints>(pointsToDisplay, 1.f));

	widget.show();

	return app.exec();
}


/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2020-2030. All rights reserved.
 * Description:  Implementation of different 3D Plane Detection Algorithm.
 * Author: Created by huangjingwei 589411
 * Create date: 2021-08-13
 */
// STL includes.
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <iterator>
#include <string>
#include <vector>

// CGAL includes.
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/pca_estimate_normals.h>
#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>
#include <CGAL/Random.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>
#include <CGAL/Timer.h>

#include <Eigen/Core>
#include <Eigen/Dense>

// Type declarations.
using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
using FT = typename Kernel::FT;
using Point_3 = typename Kernel::Point_3;
using Vector_3 = typename Kernel::Vector_3;
using Input_range = CGAL::Point_set_3<Point_3>;
using Point_map = typename Input_range::Point_map;
using Normal_map = typename Input_range::Vector_map;
using Neighbor_query =
    CGAL::Shape_detection::Point_set::K_neighbor_query<Kernel, Input_range,
                                                       Point_map>;
// using Neighbor_query =
// CGAL::Shape_detection::Point_set::Sphere_neighbor_query<Kernel, Input_range,
// Point_map>;
using Region_type =
    CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region<
        Kernel, Input_range, Point_map, Normal_map>;
using Region_growing =
    CGAL::Shape_detection::Region_growing<Input_range, Neighbor_query,
                                          Region_type>;
using Indices = std::vector<std::size_t>;
using Output_range = CGAL::Point_set_3<Point_3>;
using Points_3 = std::vector<Point_3>;

typedef std::pair<Point_3, Vector_3> PointVectorPair;
typedef std::vector<PointVectorPair> PointList;

// Concurrency
typedef CGAL::Parallel_if_available_tag Concurrency_tag;

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    MatrixD;
typedef Eigen::Matrix<FT, 3, 1> Vector3;

struct Pointcloud {
  MatrixD P, N, C;
};

void ComputePointNormals(
    Pointcloud& pc,                         // input points + output normals
    unsigned int nb_neighbors_pca_normals)  // number of neighbors
{
  PointList points(pc.P.rows());
  for (int i = 0; i < points.size(); ++i) {
    auto p = pc.P.row(i);
    points[i].first = Point_3(p[0], p[1], p[2]);
  }
  CGAL::Timer task_timer;
  task_timer.start();

  // Estimates normals direction.
  // Note: pca_estimate_normals() requires an iterator over points
  // as well as property maps to access each point's position and normal.
  CGAL::pca_estimate_normals<Concurrency_tag>(
      points, nb_neighbors_pca_normals,
      CGAL::parameters::point_map(
          CGAL::First_of_pair_property_map<PointVectorPair>())
          .normal_map(CGAL::Second_of_pair_property_map<PointVectorPair>()));

  std::size_t memory = CGAL::Memory_sizer().virtual_size();
  pc.N.conservativeResize(points.size(), 3);
  for (int i = 0; i < points.size(); ++i) {
    pc.P.row(i) =
        Vector3(points[i].first.x(), points[i].first.y(), points[i].first.z());
    pc.N.row(i) = Vector3(points[i].second.x(), points[i].second.y(),
                          points[i].second.z());
    if (pc.N.row(i).dot(Vector3(1, 1, 1)) < 0) pc.N.row(i) = -pc.N.row(i);
  }
}

// Define an insert iterator.
int PlaneDetectRegion(Pointcloud& pc,
                      std::vector<std::pair<Vector3, FT> >& plane_parameters,
                      std::vector<int>& new_instances,
                      double dist_thres, double angle_thres, int min_points,
                      int num_neigbhors) {
  const bool with_normal_map = true;
  Input_range input_range(with_normal_map);
  for (int i = 0; i < pc.P.rows(); ++i) {
    input_range.insert(Kernel::Point_3(pc.P(i, 0), pc.P(i, 1), pc.P(i, 2)));
  }
  auto it = input_range.begin();
  for (int i = 0; i < pc.N.rows(); ++i) {
    input_range.normal(*(it++)) =
        Kernel::Vector_3(pc.N(i, 0), pc.N(i, 1), pc.N(i, 2));
  }
  // Default parameter values for the data file point_set_3.xyz.
  const std::size_t k = num_neigbhors;
  const FT max_distance_to_plane = dist_thres;
  const FT max_accepted_angle = angle_thres;
  const std::size_t min_region_size = min_points;

  // Create instances of the classes Neighbor_query and Region_type.
  Neighbor_query neighbor_query(input_range, k, input_range.point_map());
  Region_type region_type(input_range, max_distance_to_plane,
                          max_accepted_angle, min_region_size,
                          input_range.point_map(), input_range.normal_map());
  // Create an instance of the region growing class.
  Region_growing region_growing(input_range, neighbor_query, region_type);
  // Run the algorithm.
  Output_range output_range;
  std::size_t number_of_regions = 0;

  std::vector<std::vector<std::size_t> > regions;
  region_growing.detect(std::back_inserter(regions));
  plane_parameters.resize(regions.size());
  new_instances.resize(pc.P.rows(), -1);

  for (int i = 0; i < regions.size(); ++i) {
    Vector3 c(0, 0, 0);
    for (auto& idx : regions[i]) {
      new_instances[idx] = i;
      c += pc.P.row(idx);
    }
    c /= (double)regions[i].size();

    MatrixD C = MatrixD::Zero(3, 3);
    for (auto& idx : regions[i]) {
      Vector3 diff = pc.P.row(idx);
      diff -= c;
      C += diff * diff.transpose();
    }
    Eigen::JacobiSVD<MatrixD> svd(C, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Vector3 N = svd.matrixU().col(2);
    plane_parameters[i] = std::make_pair(N, -N.dot(c));
  }
  return regions.size();
}

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

  const int kNeighbors = 16;
  const double kDistThres = 1e-1;
  const double kAngleThres = 20;
  const int kMinPoints = 200;

  // simulate a box
  std::vector<Vector3> points;
  for (int i = 0; i < 100; ++i) {
    for (int j = 0; j < 100; ++j) {
      points.push_back(Vector3(i * 0.01, j * 0.01, 0));
      points.push_back(Vector3(i * 0.01, j * 0.01, 1));
      points.push_back(Vector3(i * 0.01, 0, j * 0.01));
      points.push_back(Vector3(i * 0.01, 1, j * 0.01));
      points.push_back(Vector3(0, i * 0.01, j * 0.01));
      points.push_back(Vector3(1, i * 0.01, j * 0.01));
    }
  }
  for (auto& p : points) {
    double dx = rand() % 256 / 256.0 - 0.5;
    double dy = rand() % 256 / 256.0 - 0.5;
    double dz = rand() % 256 / 256.0 - 0.5;
    p += Vector3(dx * 0.01, dy * 0.01, dz * 0.01);
  }

  Pointcloud pc;
  pc.P.resize(points.size(), 3);
  memcpy(pc.P.data(), points.data(), sizeof(Vector3) * points.size());

  ComputePointNormals(pc, kNeighbors);

  std::vector<std::pair<Vector3, double> > plane_parameters;
  std::vector<int> plane_instances;
  int num_inst = PlaneDetectRegion(pc, plane_parameters, plane_instances,
    kDistThres, kAngleThres, kMinPoints, kNeighbors);

  printf("Num inst: %d\n", num_inst);
  printf("Params:\n");
  for (auto& p : plane_parameters) {
    printf("%f %f %f %f\n", p.first[0], p.first[1], p.first[2], p.second);
  }
  std::vector<Vector3> colors(num_inst);
  for (auto& c : colors) {
    c = Vector3(rand() % 256 / 256.0,
      rand() % 256 / 256.0, rand() % 256 / 256.0);
  }

  std::ofstream os("result.obj");
  for (int i = 0; i < points.size(); ++i) {
    auto p = points[i];
    auto c = Vector3(0, 0, 0);
    if (plane_instances[i] >= 0)
      c = colors[plane_instances[i]];
    os << "v " << p[0] << " " << p[1] << " " << p[2] << " "
      << c[0] << " " << c[1] << " " << c[2] << "\n";
  }
  os.close();
  return 0;
}


#pragma once

#include "geometry/any_rect2d.hpp"
#include "geometry/point2d.hpp"

#include "base/exception.hpp"
#include "base/math.hpp"

#include <vector>

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wcomma"
#include <boost/geometry.hpp>
#pragma clang diagnostic pop
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace geometry
{
double constexpr kPenaltyScore = -1.0;
DECLARE_EXCEPTION(NotAPolygonException, RootException);

namespace impl
{
using PointXY = boost::geometry::model::d2::point_xy<double>;
using Polygon = boost::geometry::model::polygon<PointXY>;
using MultiPolygon = boost::geometry::model::multi_polygon<Polygon>;
}  // namespace impl

template <typename Container>
impl::Polygon PointsToPolygon(Container const & points);
template <typename Container>
impl::MultiPolygon TrianglesToPolygon(Container const & points);

// The return value is a real number from [-1.0, 1.0].
// * Returns positive value when the geometries intersect (returns intersection area divided by union area).
//   In particular, returns 1.0 when the geometries are equal.
// * Returns zero when the geometries do not intersect.
// * Returns kPenaltyScore as penalty. It is possible when any of the geometries is empty or invalid.
/// |lhs| and |rhs| are any areal boost::geometry types.
template <typename LGeometry, typename RGeometry>
double GetIntersectionScore(LGeometry const & lhs, RGeometry const & rhs)
{
  if (!boost::geometry::is_valid(lhs) || !boost::geometry::is_valid(rhs) ||
      boost::geometry::is_empty(lhs) || boost::geometry::is_empty(rhs))
  {
    return kPenaltyScore;
  }

  auto const lhsArea = boost::geometry::area(lhs);
  auto const rhsArea = boost::geometry::area(rhs);
  impl::MultiPolygon result;
  boost::geometry::intersection(lhs, rhs, result);
  auto const intersectionArea = boost::geometry::area(result);
  auto const unionArea = lhsArea + rhsArea - intersectionArea;

  auto const score = intersectionArea / unionArea;

  return score;
}

/// Throws NotAPolygonException exception.
/// For detailed info see comment for
/// double GetIntersectionScore(LPolygon const & lhs, RPolygon const & rhs).
/// |lhs| and |rhs| are any standard container of m2::Point with random access iterator.
/// |toPolygonConverter| is a method which converts |lhs| and |rhs| to boost::geometry areal type.
template <typename Container, typename Converter>
double GetIntersectionScore(Container const & lhs, Container const & rhs,
                            Converter const & toPolygonConverter)
{
  auto const lhsPolygon = toPolygonConverter(lhs);
  if (boost::geometry::is_empty(lhsPolygon))
    return kPenaltyScore;

  auto const rhsPolygon = toPolygonConverter(rhs);
  if (boost::geometry::is_empty(rhsPolygon))
    return kPenaltyScore;

  return GetIntersectionScore(lhsPolygon, rhsPolygon);
}

/// Throws NotAPolygonException exception.
/// For detailed info see comment for
/// double GetIntersectionScore(LPolygon const & lhs, RPolygon const & rhs).
/// |lhs| and |rhs| are any standard containers of m2::Point with random access iterator.
template <typename Container>
double GetIntersectionScoreForPoints(Container const & lhs, Container const & rhs)
{
  return GetIntersectionScore<Container>(lhs, rhs, PointsToPolygon<Container>);
}

/// Throws NotAPolygonException exception.
/// For detailed info see comment for
/// double GetIntersectionScore(LPolygon const & lhs, RPolygon const & rhs).
/// |lhs| and |rhs| are any standard containers of m2::Point with random access iterator.
template <typename Container>
double GetIntersectionScoreForTriangulated(Container const & lhs, Container const & rhs)
{
  return GetIntersectionScore<Container>(lhs, rhs, TrianglesToPolygon<Container>);
}

/// |points| is any standard container of m2::Point with random access iterator.
template <typename Container>
impl::Polygon PointsToPolygon(Container const & points)
{
  impl::Polygon polygon;
  for (auto const & point : points)
    polygon.outer().push_back(impl::PointXY(point.x, point.y));
  boost::geometry::correct(polygon);
  if (!boost::geometry::is_valid(polygon))
    MYTHROW(geometry::NotAPolygonException, ("The points is not valid polygon"));

  return polygon;
}

/// |points| is any standard container of m2::Point with random access iterator.
template <typename Container>
impl::MultiPolygon TrianglesToPolygon(Container const & points)
{
  size_t const kTriangleSize = 3;
  if (points.size() % kTriangleSize != 0)
    MYTHROW(geometry::NotAPolygonException, ("Count of points must be multiple of", kTriangleSize));

  std::vector<impl::MultiPolygon> polygons;
  for (size_t i = 0; i < points.size(); i += kTriangleSize)
  {
    impl::MultiPolygon polygon;
    polygon.resize(1);
    auto & p = polygon[0];
    auto & outer = p.outer();
    for (size_t j = i; j < i + kTriangleSize; ++j)
      outer.push_back(impl::PointXY(points[j].x, points[j].y));
    boost::geometry::correct(p);
    if (!boost::geometry::is_valid(polygon))
      MYTHROW(geometry::NotAPolygonException, ("The triangle is not valid"));
    polygons.push_back(polygon);
  }

  if (polygons.empty())
    return {};

  auto & result = polygons[0];
  for (size_t i = 1; i < polygons.size(); ++i)
  {
    impl::MultiPolygon u;
    boost::geometry::union_(result, polygons[i], u);
    u.swap(result);
  }
  return result;
}
}  // namespace geometry


#include <qualisys/QualisysOdom.h>

#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>

namespace qualisys
{
QualisysOdom::QualisysOdom(ros::NodeHandle &n):
  nh(n) {
  return;
}

bool QualisysOdom::init() {
  double max_accel;
  nh.param("max_accel", max_accel, 5.0);
  //nh.param("publish_tf", publish_tf_, false);
  //if(publish_tf_ && !nh.getParam("child_frame_id", child_frame_id_)) {
  //  throw std::runtime_error("qualisys_odom: child_frame_id required for publishing tf");
  //  return false;
  //}

  // There should only be one vicon_fps, so we read from nh
  double dt, qualisys_fps;
  nh.param("qualisys_fps", qualisys_fps, 100.0);
  ROS_ASSERT(qualisys_fps > 0.0);
  dt = 1 / qualisys_fps;

  // Initialize KalmanFilter
  KalmanFilter::State_t proc_noise_diag;
  proc_noise_diag(0) = 0.5 * max_accel * dt * dt;
  proc_noise_diag(1) = 0.5 * max_accel * dt * dt;
  proc_noise_diag(2) = 0.5 * max_accel * dt * dt;
  proc_noise_diag(3) = max_accel * dt;
  proc_noise_diag(4) = max_accel * dt;
  proc_noise_diag(5) = max_accel * dt;
  proc_noise_diag = proc_noise_diag.array().square();
  KalmanFilter::Measurement_t meas_noise_diag;
  meas_noise_diag(0) = 1e-4;
  meas_noise_diag(1) = 1e-4;
  meas_noise_diag(2) = 1e-4;
  meas_noise_diag = meas_noise_diag.array().square();
  kf_.initialize(KalmanFilter::State_t::Zero(),
                 0.01 * KalmanFilter::ProcessCov_t::Identity(),
                 proc_noise_diag.asDiagonal(), meas_noise_diag.asDiagonal());

  // Initialize publisher and subscriber
  odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 10);
  qualisys_sub_ = nh.subscribe("qualisys_subject", 10, &QualisysOdom::QualisysCallback,
                            this, ros::TransportHints().tcpNoDelay());
  return true;
}

void QualisysOdom::QualisysCallback(const qualisys::Subject::ConstPtr &msg)
{
  static ros::Time t_last_proc = msg->header.stamp;
  double dt = (msg->header.stamp - t_last_proc).toSec();
  t_last_proc = msg->header.stamp;

  // Kalman filter for getting translational velocity from position measurements
  kf_.processUpdate(dt);
  const KalmanFilter::Measurement_t meas(msg->position.x, msg->position.y,
                                         msg->position.z);
  if(!msg->occluded)
  {
    static ros::Time t_last_meas = msg->header.stamp;
    double meas_dt = (msg->header.stamp - t_last_meas).toSec();
    t_last_meas = msg->header.stamp;
    kf_.measurementUpdate(meas, meas_dt);
  }

  const KalmanFilter::State_t state = kf_.getState();
  const KalmanFilter::ProcessCov_t proc_noise = kf_.getProcessNoise();

  nav_msgs::Odometry odom_msg;
  odom_msg.header = msg->header;
  odom_msg.child_frame_id = msg->name;
  odom_msg.pose.pose.position.x = state(0);
  odom_msg.pose.pose.position.y = state(1);
  odom_msg.pose.pose.position.z = state(2);
  odom_msg.twist.twist.linear.x = state(3);
  odom_msg.twist.twist.linear.y = state(4);
  odom_msg.twist.twist.linear.z = state(5);
  for(int i = 0; i < 3; i++)
  {
    for(int j = 0; j < 3; j++)
    {
      odom_msg.pose.covariance[6 * i + j] = proc_noise(i, j);
      odom_msg.twist.covariance[6 * i + j] = proc_noise(3 + i, 3 + j);
    }
  }

  odom_msg.pose.pose.orientation = msg->orientation;

  // Single step differentitation for angular velocity
  static Eigen::Matrix3d R_prev(Eigen::Matrix3d::Identity());
  Eigen::Matrix3d R(Eigen::Quaterniond(msg->orientation.w, msg->orientation.x,
                                       msg->orientation.y, msg->orientation.z));
  if(dt > 1e-6)
  {
    const Eigen::Matrix3d R_dot = (R - R_prev) / dt;
    const Eigen::Matrix3d w_hat = R_dot * R.transpose();

    odom_msg.twist.twist.angular.x = w_hat(2, 1);
    odom_msg.twist.twist.angular.y = w_hat(0, 2);
    odom_msg.twist.twist.angular.z = w_hat(1, 0);
  }
  R_prev = R;

  odom_pub_.publish(odom_msg);
  //if(publish_tf_)
  //{
  //  PublishTransform(odom_msg.pose.pose, odom_msg.header,
  //                   odom_msg.child_frame_id);
  //}
}

//void QualisysOdom::PublishTransform(const geometry_msgs::Pose &pose,
//                                 const std_msgs::Header &header,
//                                 const std::string &child_frame_id)
//{
//  // Publish tf
//  geometry_msgs::Vector3 translation;
//  translation.x = pose.position.x;
//  translation.y = pose.position.y;
//  translation.z = pose.position.z;
//
//  geometry_msgs::TransformStamped transform_stamped;
//  transform_stamped.header = header;
//  transform_stamped.child_frame_id = child_frame_id;
//  transform_stamped.transform.translation = translation;
//  transform_stamped.transform.rotation = pose.orientation;
//
//  tf_broadcaster_.sendTransform(transform_stamped);
//}

} // namespace qualisys


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_IDIVROUND_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_IDIVROUND_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/idivround.hpp>
#include <boost/simd/include/functions/scalar/iround.hpp>
#include <boost/simd/include/functions/scalar/tofloat.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivround_, tag::cpu_
                        , (A0)
                        , (scalar_< arithmetic_<A0> >)
                          (scalar_< arithmetic_<A0> >)
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
      {
      return (!a1) ? a1 :iround(tofloat(a0)/tofloat(a1));
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::idivround_, tag::cpu_,
                       (A0),
                       (scalar_< floating_<A0> > )
                       (scalar_< floating_<A0> > )
                       )
  {
    typedef typename dispatch::meta::as_integer < A0 >::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return iround(a0/a1);
    }
  };
} } }

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

#endif


/* boost random/gamma_distribution.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: gamma_distribution.hpp,v 1.9 2004/07/27 03:43:32 dgregor Exp $
 *
 */

#ifndef BOOST_RANDOM_GAMMA_DISTRIBUTION_HPP
#define BOOST_RANDOM_GAMMA_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/exponential_distribution.hpp>

namespace boost {

// Knuth
template<class RealType = double>
class gamma_distribution
{
public:
  typedef RealType input_type;
  typedef RealType result_type;

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit gamma_distribution(const result_type& alpha = result_type(1))
    : _exp(result_type(1)), _alpha(alpha)
  {
    assert(alpha > result_type(0));
    init();
  }

  // compiler-generated copy ctor and assignment operator are fine

  RealType alpha() const { return _alpha; }

  void reset() { _exp.reset(); }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::tan; using std::sqrt; using std::exp; using std::log;
    using std::pow;
#endif
    if(_alpha == result_type(1)) {
      return _exp(eng);
    } else if(_alpha > result_type(1)) {
      // Can we have a boost::mathconst please?
      const result_type pi = result_type(3.14159265358979323846);
      for(;;) {
        result_type y = tan(pi * eng());
        result_type x = sqrt(result_type(2)*_alpha-result_type(1))*y
          + _alpha-result_type(1);
        if(x <= result_type(0))
          continue;
        if(eng() >
           (result_type(1)+y*y) * exp((_alpha-result_type(1))
                                        *log(x/(_alpha-result_type(1)))
                                        - sqrt(result_type(2)*_alpha
                                               -result_type(1))*y))
          continue;
        return x;
      }
    } else /* alpha < 1.0 */ {
      for(;;) {
        result_type u = eng();
        result_type y = _exp(eng);
        result_type x, q;
        if(u < _p) {
          x = exp(-y/_alpha);
          q = _p*exp(-x);
        } else {
          x = result_type(1)+y;
          q = _p + (result_type(1)-_p) * pow(x, _alpha-result_type(1));
        }
        if(u >= q)
          continue;
        return x;
      }
    }
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const gamma_distribution& gd)
  {
    os << gd._alpha;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, gamma_distribution& gd)
  {
    is >> std::ws >> gd._alpha;
    gd.init();
    return is;
  }
#endif

private:
  void init()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::exp;
#endif
    _p = exp(result_type(1)) / (_alpha + exp(result_type(1)));
  }

  exponential_distribution<RealType> _exp;
  result_type _alpha;
  // some data precomputed from the parameters
  result_type _p;
};

} // namespace boost

#endif // BOOST_RANDOM_GAMMA_DISTRIBUTION_HPP


/*

Project: icp alignment for localization
Date: 20/08/20
@Author: ALL
Detail: align two clouds in the localization,
        so as to get the global pose of the current frame
Scenario: localization

*/
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/common/transforms.h>
#include <vector>

#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/sem.h>

#include <string.h>
#include <dirent.h>
#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#include <fstream>
#include <iostream>

#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>

#include "../include/Lidar.h"
#include "../include/ipc.h"
#include "../include/QuadTree.h" //structure define and the other shits
#include "../include/Localizate.h"
#include "../include/ProcessCom.h"

using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormal;

struct information_of_barrier
{
    float centerx;
    float centery;
};
int num_of_barrier;

struct Posemat
{
    Eigen::Matrix4f global_transform;
};

//uniform sampling func
//data acquired: Cloud's ptr for processing.
void uniform_sampling (PointCloud::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::UniformSampling<pcl::PointXYZ> US;
    US.setInputCloud(cloud);
    US.setRadiusSearch(0.2f);
    US.filter(*cloud_filtered);
    *cloud = *cloud_filtered;
}

//icp point align algorithm
//data acquired: 1 & 2 is the Cloud's ptr for aligning, 3 is the Cloud's ptr for output, 4 is transformation matrix, 5 is unclear.
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f&final_transform)
{
    int iterations = 100;
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_src);
    icp.setInputTarget(cloud_tgt);
    icp.setTransformationEpsilon(1e-15);
    icp.setMaxCorrespondenceDistance(2);
    icp.setEuclideanFitnessEpsilon(0.001);
    icp.setMaximumIterations(iterations);
    icp.align(*output);
    final_transform = icp.getFinalTransformation();
}

// func from raw pose values to the eigen transformation mat
void six2Trans(float roll, float pitch, float yaw, float x, float y, float z, Eigen::Matrix4f& transform)
{
    Eigen::Vector3f eulerAngle(yaw, pitch, roll);
    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(eulerAngle(2),Eigen::Vector3f::UnitX()));
    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(eulerAngle(1),Eigen::Vector3f::UnitY()));
    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(eulerAngle(0),Eigen::Vector3f::UnitZ()));

    Eigen::Matrix3f rotation;
    rotation = yawAngle * pitchAngle * rollAngle;

    Eigen::Translation3f translation(x, y, z);
    transform = (translation * rotation).matrix();
}

void quaterniond2eulerangle(Eigen::Quaternionf& q, float& roll, float& pitch, float& yaw)
{
    double sinr_cosp = +2.0*(q.w()*q.x() + q.y()*q.z());
    double cosr_cosp = +1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
    roll = atan2(sinr_cosp, cosr_cosp);

    double sinp = +2.0*(q.w()*q.y() - q.z()*q.x());
    if(fabs(sinp) >= 1) pitch = copysign(M_PI/2, sinp);
    else pitch = asin(sinp);

    double siny_cosp = +2.0*(q.w()*q.z() + q.x()*q.y());
    double cosy_cosp = +1.0 - 2.0*(q.y()*q.y() + q.z()*q.z());
    yaw = atan2(siny_cosp, cosy_cosp);
}

vector<information_of_barrier> remove_duplicates( vector<vector<float>> &data)
{
    for(int i = 0; i < data.size(); i++)
    {
        data[i][0] = (float)round(data[i][0]*4);
        data[i][1] = (float)round(data[i][1]*4);
    }
    sort(data.begin(),data.end());
    data.erase(unique(data.begin(), data.end()),data.end());
    std::vector<information_of_barrier> Information_of_barrier;
    for(int i = 0; i < data.size(); i++)
    {
        if(data[i][0]<=12&&data[i][0]>=0&&data[i][1]<=8&&data[i][1]>=-8){
        information_of_barrier barrier;
        barrier.centerx = data[i][0]/4;
        barrier.centery = data[i][1]/4;
        Information_of_barrier.push_back(barrier);
        }

    }
    return Information_of_barrier;
}

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

    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;

    struct QuadTreeNode root;
    struct Region root_region;
    struct ElePoint ele;
    struct ElePoint old_ele;
    initRegion(&root_region, -50, 100, -50, 100);
    initNode(&root, 1, root_region);

    std::vector<std::vector<float>> data3d;
    std::vector<std::vector<float>> data_after;
    std::vector<float> data2d;
    std::vector<Posemat> Pose;
    int line = 0;

    ifstream fin("../pose.txt");
    Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity();
    while(!fin.eof())
    {
        fin >> line >> x >> y >> z >> yaw >> pitch >> roll;
        cout << "Checking No. " << line << " Pose." << endl;
        six2Trans(roll, pitch, yaw, x, y, z, GlobalTransform);
        Posemat a;
        a.global_transform = GlobalTransform;
        cout << "global: \n" << GlobalTransform << endl;
        Pose.push_back(a);
        ele.x = x;
        ele.y = y;
        ele.index = line;
        insertEle(&root, ele);
        if (!fin.good()) break;
    }

    pthread_mutex_init(&LidarMutex,NULL);
    lidar.InitLidar();//start get lidar data
    sleep(3);

    InitSem(LidarOccPosSemId, LidarOccPosSemKey, 1, IPC_CREAT|0777);
    MessageQueueInit(LidarOccPosMsgId, LidarOccPosMsgKey, IPC_CREAT|0777);
    InitShareMemory(LidarOccPosShmId, LidarOccPosShmKey,4096, IPC_CREAT|0777);

    V_sem(LidarOccPosSemId, 0);
    V_sem(PosSemId, 0);

    while(1)
    {
        PointCloud::Ptr source(new PointCloud);
        PointCloud::Ptr target(new PointCloud);
        PointCloud::Ptr temp(new PointCloud);
        PointCloud::Ptr Lidarcloud(new PointCloud);

        pthread_mutex_lock(&LidarMutex);
        pcl::copyPointCloud(*safecloud, *Lidarcloud);
        //end updata lidar data

        for(int i = 0; i<Lidarcloud->size(); i++)
        {
            if(Lidarcloud->points[i].x >= 0.0 && Lidarcloud->points[i].x <= 3.0 && Lidarcloud->points[i].y >= -2 && Lidarcloud->points[i].y <= 2)
            {
                data2d.push_back(Lidarcloud->points[i].x);
                data2d.push_back(Lidarcloud->points[i].y);
                data3d.push_back(data2d);
                data2d.clear();
            }
        }
        std::vector<information_of_barrier> Information_of_barrier = remove_duplicates(data3d);
        //std::cout<<"*********"<<Information_of_barrier.size()<<std::endl;

        //链接内存
        void* lidarOccPosMemory;
        lidarOccPosMemory = AttachShareMemory(LidarOccPosShmId);

        if(lidarOccPosMemory == (void*)(-1))
        {
            std::cout << "Get occPosMemoy failed" << std::endl;
            return -1;
        }

        P_sem(LidarOccPosSemId, 0);//使用互斥量
        int occNum = Information_of_barrier.size();
        //std::cout << "occNum" << occNum << std::endl;
        WriteShareMemory(lidarOccPosMemory, &occNum, sizeof(int));
        for(int loop = 0; loop < occNum; loop++)
        {
            WriteShareMemory((void*)(lidarOccPosMemory + sizeof(int) + loop * sizeof(information_of_barrier)), &Information_of_barrier[loop], sizeof(information_of_barrier));
        }
        V_sem(LidarOccPosSemId, 0);

        int ret = DisattachShareMemory(lidarOccPosMemory);
        if(ret != 0)
        {
            std::cout << "Failed detached memory" << std::endl;
        }

        //get the index from quad_tree
        float x_now, y_now, z_now, roll_now, pitch_now, yaw_now, v_now, w_now;
        x_now = y_now = z_now = roll_now = pitch_now = yaw_now = v_now = w_now = 0;
        PriorLocation(x_now, y_now, z_now, roll_now, pitch_now, yaw_now, v_now, w_now);
        //float d = 1;
        //float disturbance[9][2] = {{d, 0}, {d, d}, {0, d}, {0, 0}, {-d, d}, {-d, 0}, {-d, -d}, {0, -d}, {d, -d}};
        float candidate[MAX_ELE_NUM*9 + 1][3] = {0};
        float min_distance = 1000;
        float distance = 0;
        int found_index = -1;

        //find possible index
        //for(int i = 0; i < 9; i++)
        //{
            struct ElePoint test;
            test.x = x_now;// + disturbance[i][0];
            test.y = y_now; // + disturbance[i][1];
            queryEle(root, test, candidate, candidate[MAX_ELE_NUM*9][0]);
        //}

        //find closest index
        for(int k = 0; k < candidate[MAX_ELE_NUM*9][0]; k++)
        {
            distance = hypot(x_now - candidate[k][0], y_now - candidate[k][1]);
            //cout << "candidate_index: " << candidate[k][2] << " , distance: " << distance << endl;
            //1. to eradicate the keyframes on the time-related sequence
            //2. to find the closest point to the determined pose
            if(distance < min_distance)
            {
                min_distance = distance;
                found_index = candidate[k][2];
            }
        }
        cout << "found_index: " << found_index << " , mindistance: " << min_distance << endl;
        //pseudo mat
        Eigen::Matrix4f globalTransform_curr = Eigen::Matrix4f::Identity(); // for the result store
        Eigen::Matrix4f pairTransform        = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f priorTransform       = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f relateTransform      = Eigen::Matrix4f::Identity();
        float min = 0;

        cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" << endl;
        cout << "x: " << x_now << ", y: " <<  y_now  << ", yaw: "<< yaw_now << endl;
        six2Trans(roll_now, pitch_now, yaw_now, x_now, y_now, z_now, priorTransform);

        cout << "prior: \n" << priorTransform << endl;
        cout << "keyframe: \n" << Pose[found_index].global_transform << endl;
        relateTransform = Pose[found_index].global_transform.inverse() * priorTransform;
        cout << "relate: \n" << relateTransform << endl;

        min = hypot(relateTransform(0,3), relateTransform(1,3));
        float x_new, y_new, z_new, roll_new, pitch_new, yaw_new;
        if(min < 0.4)
        {
            char namebuf[30];
            sprintf(namebuf, "../data/%d.pcd", found_index+1);
            pcl::io::loadPCDFile (namebuf, *source);    //get source cloud from keyframe inventory

            uniform_sampling(Lidarcloud);
            uniform_sampling(source);
            cout << "size after filter:" << Lidarcloud->size() << endl;

            pcl::transformPointCloud(*Lidarcloud, *target, relateTransform);
            pairAlign(target, source, temp, pairTransform);
            cout << "pair: \n" << pairTransform << endl;
            cout << "pair inverse: \n" <<  pairTransform.inverse() << endl;
            globalTransform_curr = Pose[found_index].global_transform * pairTransform * relateTransform;
            cout << "ture: \n" << globalTransform_curr<< endl;

            //sent to local
            Eigen::Matrix3f rotation_matrix = globalTransform_curr.block<3,3>(0,0);
            Eigen::Quaternionf quaternion(rotation_matrix);
            quaterniond2eulerangle(quaternion, roll_new, pitch_new, yaw_new);
            x_new = globalTransform_curr(0,3);
            y_new = globalTransform_curr(1,3);
            z_new = globalTransform_curr(2,3);
            PosteriorLocation(x_new, y_new, z_new, roll_new, pitch_new, yaw_new, v_now, w_now);
        }
        else
        {
            PosteriorLocation(x_now, y_now, z_now, roll_now, pitch_now, yaw_now, v_now, w_now);
        }
        pthread_mutex_unlock(&LidarMutex);
    }
    DestoryLocalizate();
    return 0;
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018-2021 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
// Altered from:
// @author Ignacio Vizzo     [ivizzo@uni-bonn.de]
//
// Copyright (c) 2021 Ignacio Vizzo, Cyrill Stachniss, University of Bonn.
// ----------------------------------------------------------------------------

#include "open3d/pipelines/registration/GeneralizedICP.h"

#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

#include "open3d/geometry/KDTreeSearchParam.h"
#include "open3d/geometry/PointCloud.h"
#include "open3d/utility/Eigen.h"
#include "open3d/utility/Logging.h"

namespace open3d {
namespace pipelines {
namespace registration {

namespace {

/// Obtain the Rotation matrix that transform the basis vector e1 onto the
/// input vector x.
inline Eigen::Matrix3d GetRotationFromE1ToX(const Eigen::Vector3d &x) {
    const Eigen::Vector3d e1{1, 0, 0};
    const Eigen::Vector3d v = e1.cross(x);
    const double c = e1.dot(x);
    if (c < -0.99) {
        // Then means that x and e1 are in the same direction
        return Eigen::Matrix3d::Identity();
    }

    const Eigen::Matrix3d sv = utility::SkewMatrix(v);
    const double factor = 1 / (1 + c);
    return Eigen::Matrix3d::Identity() + sv + (sv * sv) * factor;
}

/// Compute the covariance matrix according to the original paper. If the input
/// has already pre-computed covariances returns immediately. If the input has
/// pre-computed normals but no covariances, compute the covariances from those
/// normals. If there is no covariances nor normals, compute each covariance
/// matrix following the original implementation of GICP using 20 NN.
std::shared_ptr<geometry::PointCloud> InitializePointCloudForGeneralizedICP(
        const geometry::PointCloud &pcd, double epsilon) {
    auto output = std::make_shared<geometry::PointCloud>(pcd);
    if (output->HasCovariances()) {
        utility::LogDebug("GeneralizedICP: Using pre-computed covariances.");
        return output;
    }
    if (output->HasNormals()) {
        utility::LogDebug("GeneralizedICP: Computing covariances from normals");
    } else {
        // Compute covariances the same way is done in the original GICP paper.
        utility::LogDebug("GeneralizedICP: Computing covariances from points.");
        output->EstimateNormals(open3d::geometry::KDTreeSearchParamKNN(20));
    }

    output->covariances_.resize(output->points_.size());
    const Eigen::Matrix3d C = Eigen::Vector3d(epsilon, 1, 1).asDiagonal();
#pragma omp parallel for schedule(static)
    for (int i = 0; i < (int)output->normals_.size(); i++) {
        const auto Rx = GetRotationFromE1ToX(output->normals_[i]);
        output->covariances_[i] = Rx * C * Rx.transpose();
    }
    return output;
}
}  // namespace

double TransformationEstimationForGeneralizedICP::ComputeRMSE(
        const geometry::PointCloud &source,
        const geometry::PointCloud &target,
        const CorrespondenceSet &corres) const {
    if (corres.empty()) {
        return 0.0;
    }
    double err = 0.0;
    for (const auto &c : corres) {
        const Eigen::Vector3d &vs = source.points_[c[0]];
        const Eigen::Matrix3d &Cs = source.covariances_[c[0]];
        const Eigen::Vector3d &vt = target.points_[c[1]];
        const Eigen::Matrix3d &Ct = target.covariances_[c[1]];
        const Eigen::Vector3d d = vs - vt;
        const Eigen::Matrix3d M = Ct + Cs;
        const Eigen::Matrix3d W = M.inverse().sqrt();
        err += d.transpose() * W * d;
    }
    return std::sqrt(err / (double)corres.size());
}

Eigen::Matrix4d
TransformationEstimationForGeneralizedICP::ComputeTransformation(
        const geometry::PointCloud &source,
        const geometry::PointCloud &target,
        const CorrespondenceSet &corres) const {
    if (corres.empty() || !target.HasCovariances() ||
        !source.HasCovariances()) {
        return Eigen::Matrix4d::Identity();
    }

    auto compute_jacobian_and_residual =
            [&](int i,
                std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
                std::vector<double> &r, std::vector<double> &w) {
                const Eigen::Vector3d &vs = source.points_[corres[i][0]];
                const Eigen::Matrix3d &Cs = source.covariances_[corres[i][0]];
                const Eigen::Vector3d &vt = target.points_[corres[i][1]];
                const Eigen::Matrix3d &Ct = target.covariances_[corres[i][1]];
                const Eigen::Vector3d d = vs - vt;
                const Eigen::Matrix3d M = Ct + Cs;
                const Eigen::Matrix3d W = M.inverse().sqrt();

                Eigen::Matrix<double, 3, 6> J;
                J.block<3, 3>(0, 0) = -utility::SkewMatrix(vs);
                J.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity();
                J = W * J;

                constexpr int n_rows = 3;
                J_r.resize(n_rows);
                r.resize(n_rows);
                w.resize(n_rows);
                for (size_t i = 0; i < n_rows; ++i) {
                    r[i] = W.row(i).dot(d);
                    w[i] = kernel_->Weight(r[i]);
                    J_r[i] = J.row(i);
                }
            };

    Eigen::Matrix6d JTJ;
    Eigen::Vector6d JTr;
    double r2 = -1.0;
    std::tie(JTJ, JTr, r2) =
            utility::ComputeJTJandJTr<Eigen::Matrix6d, Eigen::Vector6d>(
                    compute_jacobian_and_residual, (int)corres.size());

    bool is_success = false;
    Eigen::Matrix4d extrinsic;
    std::tie(is_success, extrinsic) =
            utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);

    return is_success ? extrinsic : Eigen::Matrix4d::Identity();
}

RegistrationResult RegistrationGeneralizedICP(
        const geometry::PointCloud &source,
        const geometry::PointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
        const TransformationEstimationForGeneralizedICP
                &estimation /* = TransformationEstimationForGeneralizedICP()*/,
        const ICPConvergenceCriteria
                &criteria /* = ICPConvergenceCriteria()*/) {
    return RegistrationICP(
            *InitializePointCloudForGeneralizedICP(source, estimation.epsilon_),
            *InitializePointCloudForGeneralizedICP(target, estimation.epsilon_),
            max_correspondence_distance, init, estimation, criteria);
}

}  // namespace registration
}  // namespace pipelines
}  // namespace open3d


#include <Eigen/Dense>
#include <chrono>
#include <fstream>
#include <ros/ros.h>
#include <sstream>
#include <string>
#include <vector>

#include "arf_graph/graph.h"
#include "arf_graph/util.h"
#include "arf_moveit_wrapper/moveit_wrapper.h"
#include "arf_trajectory/trajectory.h"
#include "ros/package.h"

#include "util.h"

std::string FILE_NAME_SINGLE_RUN = "/data/results_case_1_single_run_l1.csv";
std::string FILE_NAME_ITERATIVE = "/data/results_case_1_l1.csv";

class Demo1
{
  std::vector<std::vector<std::vector<double>>> graph_data_;
  std::vector<TrajectoryPoint> ee_trajectory_;
  std::vector<FreeOrientationPoint> ee_trajectory_2_;
  std::vector<std::vector<double>> shortest_path_;

public:
  double last_path_cost_;
  void createTrajectory();
  void createGraphData(Robot& robot, Rviz& rviz);
  void calculateShortestPath(Robot& robot);
  void showShortestPath(Robot& robot, Rviz& rviz);
  void orientationFreeSampling(Robot& robot, int num_samples);
  void sampleNearSolution(Robot& robot, Rviz& rviz, double dist, int n);
  void showTrajectory(Rviz& rviz);
  void readTaskFromYaml(const std::string filename);
  void readTask1(ros::NodeHandle& nh);
  void reset();
};

void Demo1::reset()
{
  graph_data_.clear();
  shortest_path_.clear();
  last_path_cost_ = 0;
}

std::vector<TrajectoryPoint> createPath();

void showResults(std::vector<std::chrono::duration<double>>& times, std::vector<double> costs)
{
  ROS_INFO_STREAM("========================================");
  ROS_INFO_STREAM("Timing results");
  for (std::size_t i = 0; i < times.size(); ++i)
  {
    ROS_INFO_STREAM("time: " << times[i].count() << " cost: " << costs[i]);
  }
}

void run_case(Demo1& demo1, Robot& robot, Rviz& rviz, int num_samples, std::ofstream& file, int run_index)
{
  ROS_INFO_STREAM("Started running planning case 1");
  // parameters
  double reduction_factor = 2.0;
  double initial_quat_dist = 0.25 * M_PI;  // same as free
  int num_iterations = 4;                  // including first run

  std::vector<std::chrono::duration<double>> times;
  std::vector<double> run_time;
  std::vector<double> costs;

  // first run without reduced tolerance
  auto start = std::chrono::high_resolution_clock::now();
  demo1.orientationFreeSampling(robot, num_samples);
  demo1.calculateShortestPath(robot);
  auto stop = std::chrono::high_resolution_clock::now();

  times.push_back(stop - start);
  costs.push_back(demo1.last_path_cost_);

  double dist = initial_quat_dist;
  for (int i = 0; i < num_iterations - 1; ++i)
  {
    dist = dist / reduction_factor;
    ROS_INFO_STREAM("Sampling iteration " << i << " with dist: " << dist);

    start = std::chrono::high_resolution_clock::now();
    demo1.sampleNearSolution(robot, rviz, dist, num_samples);
    demo1.calculateShortestPath(robot);
    stop = std::chrono::high_resolution_clock::now();

    times.push_back(stop - start);
    costs.push_back(demo1.last_path_cost_);

    // demo1.showShortestPath(robot, rviz);
    // rviz.clear();
  }

  // print results
  for (std::size_t i = 0; i < costs.size(); ++i)
  {
    file << run_index << ",";
    file << i << ",";  // iteration
    file << num_samples << ",";
    file << costs[i] << ",";
    file << times[i].count();
    file << std::endl;
  }
}

void run_multiple_cases(Demo1& demo, Robot& robot, Rviz& rviz, std::vector<int> num_samples, int num_runs)
{
  std::string header = "run,iteration,samples,cost,time";
  std::string filename = ros::package::getPath("arf_demo") + FILE_NAME_ITERATIVE;

  std::ofstream data_file;
  data_file.open(filename);

  if (data_file.is_open())
  {
    data_file << header << std::endl;
  }
  else
  {
    ROS_ERROR_STREAM("Failed to open file for data output.");
  }

  for (int k = 0; k < num_runs; ++k)
  {
    for (auto ns : num_samples)
    {
      demo.reset();
      run_case(demo, robot, rviz, ns, data_file, k);
    }
  }

  data_file.close();
}

void single_run(Demo1& demo1, Robot& robot, Rviz& rviz, int num_samples, std::ofstream& file, int run_index)
{
  ROS_INFO_STREAM("Started running single run, " << num_samples << " samples.");

  std::vector<std::chrono::duration<double>> times;
  std::vector<double> run_time;
  std::vector<double> costs;

  // first run without reduced tolerance
  auto start = std::chrono::high_resolution_clock::now();
  demo1.orientationFreeSampling(robot, num_samples);
  demo1.calculateShortestPath(robot);
  auto stop = std::chrono::high_resolution_clock::now();

  times.push_back(stop - start);
  costs.push_back(demo1.last_path_cost_);

  // print results
  for (std::size_t i = 0; i < costs.size(); ++i)
  {
    file << run_index << ",";
    file << num_samples << ",";
    file << costs[i] << ",";
    file << times[i].count();
    file << std::endl;

    std::cout << "Cost: " << costs[i] << " Time: " << times[i].count() << std::endl;
  }
}

void run_multiple_cases_single_run(Demo1& demo, Robot& robot, Rviz& rviz, std::vector<int> num_samples, int num_runs)
{
  std::string header = "run,samples,cost,time";
  std::string filename = ros::package::getPath("arf_demo") + FILE_NAME_SINGLE_RUN;

  std::ofstream data_file;
  data_file.open(filename);

  if (data_file.is_open())
  {
    data_file << header << std::endl;
  }
  else
  {
    ROS_ERROR_STREAM("Failed to open file for data output.");
  }

  for (int k = 0; k < num_runs; ++k)
  {
    for (auto ns : num_samples)
    {
      demo.reset();
      single_run(demo, robot, rviz, ns, data_file, k);
    }
  }

  data_file.close();
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "demo_table");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  Robot robot;
  Rviz rviz;
  Demo1 demo1;

  rviz.clear();

  // task orientation free
  std::string filename = ros::package::getPath("arf_demo") + "/config/table_task.csv";
  demo1.readTaskFromYaml(filename);
  demo1.showTrajectory(rviz);

  std::vector<double> home = { 0, -1.5, 1.5, 0, 0, 0 };
  robot.plot(rviz.visual_tools_, home);

  std::vector<int> num_samples = { 300, 600, 900 };
  run_multiple_cases(demo1, robot, rviz, num_samples, 10);

  std::vector<int> num_samples_2 = { 300, 600, 900, 1200, 1800, 2500, 3000, 3600 };
  run_multiple_cases_single_run(demo1, robot, rviz, num_samples_2, 10);

  // std::ofstream ftest;
  // ftest.open("test.csv");
  // single_run(demo1, robot, rviz, 1000, ftest, 0);

  demo1.showShortestPath(robot, rviz);
  rviz.clear();
  demo1.showTrajectory(rviz);

  ros::shutdown();

  return 0;
}

TrajectoryPoint createPointFromParameters(ros::NodeHandle& nh, const std::string absolute_path)
{
  std::vector<double> pose, ld, ud;
  std::vector<int> ns;

  // nh.getParam(absolute_path + "/type", type);
  nh.getParam(absolute_path + "/pose", pose);
  nh.getParam(absolute_path + "/num_samples", ns);
  nh.getParam(absolute_path + "/lower_delta", ld);
  nh.getParam(absolute_path + "/upper_delta", ud);

  std::vector<std::shared_ptr<Number>> values;
  for (int i = 0; i < 6; ++i)
  {
    if (ns[i] == 0)
    {
      values.push_back(std::make_shared<Number>(pose[i]));
    }
    else
    {
      values.push_back(std::make_shared<TolerancedNumber>(pose[i], pose[i] - ld[i], pose[i] + ud[i], ns[i]));
    }
  }

  // TolerancedNumber x(pose[0], pose[0] - ld[0], pose[0] + ud[0], ns[0]);
  // Number x(pose[0]);
  // Number y(pose[1]);
  // Number z(pose[2]);
  // Number rx(pose[3]);
  // TolerancedNumber ry(pose[4], pose[4] - ld[4], pose[4] + ud[4], ns[4]);
  // TolerancedNumber rz(pose[5], pose[5] - ld[5], pose[5] + ud[5], ns[5]);

  // TrajectoryPoint tp(x, y, z, rx, ry, rz);

  TrajectoryPoint tp(*values[0], *values[1], *values[2], *values[3], *values[4], *values[5]);
  return tp;
}

void Demo1::readTask1(ros::NodeHandle& nh)
{
  ee_trajectory_.clear();
  std::vector<double> ud, ld;
  std::string task_name = "path";
  if (nh.hasParam(task_name))
  {
    int num_steps = 0;
    nh.getParam(task_name + "/num_steps", num_steps);
    ROS_INFO_STREAM("Found path of length: " << num_steps);

    for (int i = 0; i < num_steps; ++i)
    {
      ee_trajectory_.push_back(createPointFromParameters(nh, task_name + "/point_" + std::to_string(i)));
    }
  }
  else
  {
    ROS_ERROR_STREAM("Failed to read a path from parameter server");
  }

  // for (int i = 0; i < ud.size(); ++i)
  // {
  //   std::cout << "ud: " << ud[i] << "  ld: " << ld[i] << std::endl;
  // }
  // for (int i = 0; i < 10; ++i)
  // {
  //   Number x(0.8);
  //   Number y(-0.2 + static_cast<double>(i) / 20);
  //   Number z(0.2);
  //   Number rx; //, ry(-M_PI);
  //   TolerancedNumber ry(-M_PI, -M_PI - ld[4], -M_PI + ud[4], 5);
  //   TolerancedNumber rz(0, -ld[5], ud[5], 20);
  //   TrajectoryPoint tp(x, y, z, rx, ry, rz);
  //   ee_trajectory_.push_back(tp);
  // }
}

void Demo1::createTrajectory()
{
  for (int i = 0; i < 10; ++i)
  {
    Number x(0.8);
    Number y(-0.2 + static_cast<double>(i) / 20);
    Number z(0.2);
    Number rx;  //, ry(-M_PI);
    TolerancedNumber ry(-M_PI, -M_PI - 1.0, -M_PI + 1.0, 5);
    TolerancedNumber rz(0, -M_PI, M_PI, 20);
    TrajectoryPoint tp(x, y, z, rx, ry, rz);
    ee_trajectory_.push_back(tp);
  }
}

void Demo1::showTrajectory(Rviz& rviz)
{
  if (ee_trajectory_.size() > 0)
  {
    for (auto tp : ee_trajectory_)
      tp.plot(rviz.visual_tools_);
  }
  else
  {
    for (auto tp : ee_trajectory_2_)
      tp.plot(rviz.visual_tools_);
  }
}

void Demo1::readTaskFromYaml(const std::string filename)
{
  ee_trajectory_2_.clear();

  std::ifstream data_file;
  data_file.open(filename);

  std::string line, number;
  std::vector<double> pt;
  int i = -1;
  while (std::getline(data_file, line))
  {
    if (i < 0)
    {
      // skip first line
      ++i;
      continue;
    }
    std::stringstream line_stream(line);
    pt.clear();
    while (std::getline(line_stream, number, ','))
    {
      pt.push_back(std::stod(number));
    }

    Number x(pt[0]), y(pt[1]), z(pt[2]);
    Number rx(pt[3]), ry(pt[4]), rz(pt[5]);
    FreeOrientationPoint tp(x, y, z, rx, ry, rz);
    ee_trajectory_2_.push_back(tp);
    ++i;
  }
}

void Demo1::createGraphData(Robot& robot, Rviz& rviz)
{
  for (auto tp : ee_trajectory_)
  {
    std::vector<std::vector<double>> new_data;
    for (auto pose : tp.getGridSamples())
    {
      rviz.plotPose(pose);
      for (auto q_sol : robot.ik(pose))
      {
        if (!robot.isInCollision(q_sol))
          new_data.push_back(q_sol);
      }
    }
    graph_data_.push_back(new_data);
  }
}

void Demo1::orientationFreeSampling(Robot& robot, int num_samples = 500)
{
  std::cout << "Trajectory length: " << ee_trajectory_2_.size() << std::endl;
  graph_data_.clear();

  for (auto tp : ee_trajectory_2_)
  {
    std::vector<std::vector<double>> new_data;
    for (auto pose : tp.sampleUniform(num_samples))
    {
      for (auto q_sol : robot.ik(pose))
      {
        if (!robot.isInCollision(q_sol))
          new_data.push_back(q_sol);
      }
    }
    graph_data_.push_back(new_data);
  }
}

void Demo1::sampleNearSolution(Robot& robot, Rviz& rviz, double dist, int n = 500)
{
  if (shortest_path_.size() < 1)
  {
    throw std::invalid_argument("There is no solution to sample around.");
  }

  graph_data_.clear();
  for (std::size_t i = 0; i < shortest_path_.size(); ++i)
  {
    auto fk_sol = robot.fk(shortest_path_[i]);
    Transform nom_pose(fk_sol.matrix());
    ee_trajectory_2_[i].setNominalPose(nom_pose);

    std::vector<std::vector<double>> new_data;
    for (auto pose : ee_trajectory_2_[i].sampleUniformNear(dist, n))
    {
      // rviz.plotPose(pose);
      for (auto q_sol : robot.ik(pose))
      {
        if (!robot.isInCollision(q_sol))
          new_data.push_back(q_sol);
      }
    }
    graph_data_.push_back(new_data);
  }
}

void Demo1::calculateShortestPath(Robot& robot)
{
  Graph demo_graph(graph_data_);
  demo_graph.runMultiSourceDijkstra();
  std::vector<Node*> sp = demo_graph.getShortestPath();
  // std::cout << "Shortest path \n";

  shortest_path_.clear();
  for (auto node : sp)
  {
    // std::cout << (*node) << std::endl;
    shortest_path_.push_back(*(*node).jv);
  }

  last_path_cost_ = demo_graph.last_path_cost;
}

void Demo1::showShortestPath(Robot& robot, Rviz& rviz)
{
  for (auto q : shortest_path_)
  {
    robot.plot(rviz.visual_tools_, q);
    ros::Duration(0.5).sleep();
  }
}


/****************************
 * 题目：给定一组世界坐标系下的3D点(p3d.txt)以及它在相机中对应的坐标(p2d.txt)，以及相机的内参矩阵。
 * 使用bundle adjustment 方法（g2o库实现）来估计相机的位姿T。初始位姿T为单位矩阵。
 *
* 本程序学习目标：
 * 熟悉g2o库编写流程，熟悉顶点定义方法。
 *
 * 公众号：计算机视觉life。发布于公众号旗下知识星球：从零开始学习SLAM
 * 时间：2019.02
****************************/

#include <vector>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace Eigen;

using namespace cv;
using namespace std;

string p3d_file = "./../p3d.txt";
string p2d_file = "./../p2d.txt";

void bundleAdjustment (
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        Mat& K );

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

    vector< Point3f > p3d;//保存世界坐标系下的3D点
    vector< Point2f > p2d;//保存相机坐标系下对应的2D点

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    // 导入3D点和对应的2D点

    ifstream fp3d(p3d_file);
    if (!fp3d){
        cout<< "No p3d.text file" << endl;
        return -1;
    }
    else {
        while (!fp3d.eof()){
            double pt3[3] = {0};
            for (auto &p:pt3) {
                fp3d >> p;
            }
            p3d.push_back(Point3f(pt3[0],pt3[1],pt3[2]));
        }
    }
    ifstream fp2d(p2d_file);
    if (!fp2d){
        cout<< "No p2d.text file" << endl;
        return -1;
    }
    else {
        while (!fp2d.eof()){
            double pt2[2] = {0};
            for (auto &p:pt2) {
                fp2d >> p;
            }
            Point2f p2(pt2[0],pt2[1]);
            p2d.push_back(p2);
        }
    }

    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    bundleAdjustment ( p3d, p2d, K );
    return 0;
}

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K   )
{
    // creat g2o
    // new g2o version. Ref:https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html

    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    // 第1步：创建一个线性求解器LinearSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

    // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
    //Block* solver_ptr = new Block (std::unique_ptr<Block::LinearSolverType>(linearSolver));
	    Block* solver_ptr = new Block (linearSolver);
    // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );

    // 第4步：创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

//    // old g2o version
//    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
//    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
//    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
//    g2o::SparseOptimizer optimizer;
//    optimizer.setAlgorithm ( solver );

    // 第5步：定义图的顶点和边。并添加到SparseOptimizer中

	// ----------------------开始你的代码：设置并添加顶点，初始位姿为单位矩阵

	//添加相机的位姿，这里为待优化的变量，直接设置为单位矩阵
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose，g2o内定的类型
    Eigen::Matrix3d R_mat;

    R_mat.setIdentity(3,3);
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( 0, 0, 0) )
                        );
    optimizer.addVertex ( pose );

    //g2o中世界坐标系的3维坐标才是顶点（vertex），而其对应的相机坐标系下的点是评估（estimate）点!!!!!!
    int index=1;
    for(const Point3f p:points_3d)
    {
		g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );//只有观测点
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

	// ----------------------结束你的代码

    // 设置相机内参
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // 设置边
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  //设置观测值
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    // 第6步：设置优化参数，开始执行优化
    optimizer.setVerbose ( false );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    // 输出优化结果
    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/// \file
/// \brief Implementation of \link kwiver::vital::mesh mesh \endlink class

#include <Eigen/Geometry>
#include <vital/logger/logger.h>
#include <vital/types/mesh.h>

#include <map>

namespace kwiver {

namespace vital {

/// compute the vector normal to the plane defined by 3 vertices
vector_3d
mesh_tri_normal( const vector_3d& a,
                 const vector_3d& b,
                 const vector_3d& c )
{
  vector_3d ac( c - a );
  vector_3d ab( b - a );
  return ab.cross( ac );
}

// ----------------------------------------------------------------------------
// Mesh vertices

/// Checks approximate equality
bool
mesh_vertex_array_base
::approx_equal( mesh_vertex_array_base const& other, double epsilon ) const
{
  if( dim() != other.dim() || size() != other.size() )
  {
    return false;
  }

  const std::vector< vector_3d > these_normals = normals();
  const std::vector< vector_3d > other_normals = other.normals();
  if( these_normals.size() != other_normals.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < these_normals.size(); i++ )
  {
    if( abs( these_normals[ i ][ 0 ] - other_normals[ i ][ 0 ] ) > epsilon ||
        abs( these_normals[ i ][ 1 ] - other_normals[ i ][ 1 ] ) > epsilon ||
        abs( these_normals[ i ][ 2 ] - other_normals[ i ][ 2 ] ) > epsilon )
    {
      return false;
    }
  }

  return true;
}

/// Checks approximate equality
template < unsigned int d >
bool
mesh_vertex_array< d >
::approx_equal( mesh_vertex_array_base const& other, double epsilon ) const
{
  if( mesh_vertex_array_base::approx_equal( other, epsilon ) == false )
  {
    return false;
  }

  const mesh_vertex_array< d > other_actual = other;
  for( unsigned int i = 0; i < size(); i++ )
  {
    if( this[ i ] != other_actual[ i ] )
    {
      return false;
    }
  }

  return true;
}

// ----------------------------------------------------------------------------
// Mesh faces

/// Equality operator
template < unsigned s > template < unsigned U >
bool
mesh_regular_face< s >
::operator==( mesh_regular_face< U > const& other ) const
{
  if( num_verts() != other.num_verts() )
  {
    return false;
  }

  for( unsigned int i = 0; i < s; i++ )
  {
    if( verts_[ i ] != other.verts_[ i ] )
    {
      return false;
    }
  }

  return true;
}

/// Checks approximate equality
bool
mesh_face_array_base
::approx_equal( mesh_face_array_base const& other, double epsilon) const
{
  if( size() != other.size() )
  {
    return false;
  }
  if( regularity() != other.regularity() )
  {
    return false;
  }

  const std::vector< vector_3d > these_normals = normals();
  const std::vector< vector_3d > other_normals = other.normals();
  if( these_normals.size() != other_normals.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < these_normals.size(); i++ )
  {
    if( abs( these_normals[ i ][ 0 ] - other_normals[ i ][ 0 ] ) > epsilon ||
        abs( these_normals[ i ][ 1 ] - other_normals[ i ][ 1 ] ) > epsilon ||
        abs( these_normals[ i ][ 2 ] - other_normals[ i ][ 2 ] ) > epsilon )
    {
      return false;
    }
  }

  const std::vector< std::pair< std::string, unsigned int > >
  these_groups = groups();
  const std::vector< std::pair< std::string, unsigned int > >
  other_groups = other.groups();
  if( these_groups.size() != other_groups.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < these_groups.size(); i++ )
  {
    if( these_groups[ i ].first != other_groups[ i ].first ||
        these_groups[ i ].second != other_groups[ i ].second )
    {
      return false;
    }
  }

  return true;
}

/// Return the group name for a given face index
std::string
mesh_face_array_base
::group_name( unsigned int f ) const
{
  if( groups_.empty() )
  {
    return "";
  }

  unsigned int i = 0;
  for(; i < groups_.size() && groups_[ i ].second < f; ++i )
  {
  }

  if( i >= groups_.size() )
  {
    return "";
  }

  return groups_[ i ].first;
}

/// Return a set of all faces in a group
std::set< unsigned int >
mesh_face_array_base
::group_face_set( const std::string& name ) const
{
  std::set< unsigned int > face_set;
  unsigned int start = 0, end;
  for( unsigned int g = 0; g < groups_.size(); ++g )
  {
    end = groups_[ g ].second;
    if( groups_[ g ].first == name )
    {
      for( unsigned int i = start; i < end; ++i )
      {
        face_set.insert( i );
      }
    }
    start = end;
  }
  return face_set;
}

/// Assign a group name to all faces currently unnamed
unsigned int
mesh_face_array_base
::make_group( const std::string& name )
{
  unsigned int start_idx = 0;
  if( !groups_.empty() )
  {
    start_idx = groups_.back().second;
  }

  if( start_idx < this->size() )
  {
    groups_.push_back( std::pair< std::string, unsigned int >( name,
                                                               this->size() ) );
  }

  return this->size() - start_idx;
}

/// Append this array of faces (must be the same type)
void
mesh_face_array_base
::append( const mesh_face_array_base& other, unsigned int )
{
  if( this->has_normals() && other.has_normals() )
  {
    normals_.insert( normals_.end(), other.normals_.begin(),
                     other.normals_.end() );
  }
  else
  {
    normals_.clear();
  }

  if( other.has_groups() )
  {
    // group any ungrouped faces in this array
    this->make_group( "ungrouped" );

    const unsigned int offset = this->size();
    for( unsigned int g = 0; g < other.groups_.size(); ++g )
    {
      groups_.push_back( other.groups_[ g ] );
      groups_.back().second += offset;
    }
  }
}

/// Checks approximate equality
bool
mesh_face_array
::approx_equal( mesh_face_array_base const& other, double epsilon ) const
{
  if( mesh_face_array_base::approx_equal( other, epsilon ) == false )
  {
    return false;
  }

  const mesh_face_array other_actual = *std::make_shared< mesh_face_array >(
    other );
  for( unsigned int i = 0; i < size(); i++ )
  {
    if( this[ i ].size() != other_actual[ i ].size() )
    {
      return false;
    }
    for( unsigned int j = 0; j < this[ i ].size(); j++ )
    {
      if( faces_[ i ][ j ] != other_actual[ i ][ j ] )
      {
        return false;
      }
    }
  }

  return true;
}

/// Append this array of faces
void
mesh_face_array
::append( const mesh_face_array_base& other, unsigned int ind_shift )
{
  mesh_face_array_base::append( other, ind_shift );

  const unsigned int new_begin = static_cast< unsigned int >( faces_.size() );

  if( other.regularity() == 0 )
  {
    const mesh_face_array& fs = static_cast< const mesh_face_array& >( other );
    faces_.insert( faces_.end(), fs.faces_.begin(), fs.faces_.end() );

    if( ind_shift > 0 )
    {
      for( unsigned int i = new_begin; i < faces_.size(); ++i )
      {
        std::vector< unsigned int >& f = faces_[ i ];
        for( unsigned int j = 0; j < f.size(); ++j )
        {
          f[ j ] += ind_shift;
        }
      }
    }
  }
  else
  {
    for( unsigned int i = 0; i < other.size(); ++i )
    {
      std::vector< unsigned int > f( other.num_verts( i ) );
      for( unsigned int j = 0; j < other.num_verts( i ); ++j )
      {
        f[ j ] = other( i, j ) + ind_shift;
      }
      faces_.push_back( f );
    }
  }
}

/// Checks approximate equality
template < unsigned int s >
bool
mesh_regular_face_array< s >
::approx_equal( mesh_face_array_base const& other, double epsilon ) const
{
  if( mesh_face_array_base::approx_equal( other, epsilon ) == false )
  {
    return false;
  }

  const mesh_regular_face_array< s > other_actual = other;
  for( unsigned int i = 0; i < size(); i++ )
  {
    if( this[ i ].size() != other_actual[ i ].size() )
    {
      return false;
    }
    for( unsigned int j = 0; j < this[ i ].size(); j++ )
    {
      if( this[ i ] != other_actual[ i ] )
      {
        return false;
      }
    }
  }

  return true;
}

/// Merge the two face arrays
std::unique_ptr< mesh_face_array_base >
merge_face_arrays( const mesh_face_array_base& f1,
                   const mesh_face_array_base& f2,
                   unsigned int ind_shift )
{
  std::unique_ptr< mesh_face_array_base > f;
  // if both face sets are regular with the same number of vertices per face
  if( f1.regularity() == f2.regularity() || f1.regularity() == 0 )
  {
    f.reset( f1.clone() );
  }
  else
  {
    f.reset( new mesh_face_array( f1 ) );
  }
  f->append( f2, ind_shift );
  return f;
}

// ----------------------------------------------------------------------------
// Mesh edges

/// Construct from a face index list
mesh_half_edge_set
::mesh_half_edge_set(
  const std::vector< std::vector< unsigned int > >& face_list )
{
  build_from_ifs( face_list );
}

/// Build the half edges from an indexed face set
void
mesh_half_edge_set
::build_from_ifs( const std::vector< std::vector< unsigned int > >& face_list )
{
  half_edges_.clear();
  typedef std::pair< unsigned int, unsigned int > vert_pair;

  std::map< vert_pair, unsigned int > edge_map;

  face_to_he_.resize( face_list.size(), mesh_invalid_idx );

  unsigned int max_v = 0;

  const unsigned int num_faces =
    static_cast< unsigned int >( face_list.size() );
  for( unsigned int f = 0; f < num_faces; ++f )
  {
    const std::vector< unsigned int >& verts = face_list[ f ];
    const unsigned int num_verts = static_cast< unsigned int >( verts.size() );
    unsigned int first_e = mesh_invalid_idx; // first edge index
    unsigned int prev_e = mesh_invalid_idx; // previous edge index
    for( unsigned int i = 0; i < num_verts; ++i )
    {
      const unsigned int& v = verts[ i ];
      if( v > max_v )
      {
        max_v = v;
      }

      unsigned int ni = ( i + 1 ) % num_verts;
      const unsigned int& nv = verts[ ni ];

      vert_pair vp( v, nv );
      if( v > nv )
      {
        vp = vert_pair( nv, v );
      }

      std::map< vert_pair, unsigned int >::iterator m = edge_map.find( vp );
      unsigned int curr_e;
      if( m == edge_map.end() )
      {
        curr_e = static_cast< unsigned int >( half_edges_.size() );
        edge_map.insert( std::pair< vert_pair, unsigned int >( vp, curr_e ) );
        half_edges_.push_back( mesh_half_edge( curr_e, mesh_invalid_idx, v,
                                               f ) );
        half_edges_.push_back( mesh_half_edge( curr_e + 1, mesh_invalid_idx,
                                               nv, mesh_invalid_idx ) );
      }
      else
      {
        curr_e = m->second + 1;
        assert( half_edges_[ curr_e ].next_index() == mesh_invalid_idx );
        assert( half_edges_[ curr_e ].vert_index() == v );
        half_edges_[ curr_e ].face_ = f;
      }
      if( first_e == mesh_invalid_idx )
      {
        first_e = curr_e;
      }
      if( prev_e != mesh_invalid_idx )
      {
        half_edges_[ prev_e ].next_ = curr_e;
      }
      prev_e = curr_e;
    }
    if( prev_e != mesh_invalid_idx )
    {
      half_edges_[ prev_e ].next_ = first_e;
    }
    face_to_he_[ f ] = first_e;
  }

  vert_to_he_.resize( max_v + 1, mesh_invalid_idx );

  // create half edges for boundaries
  for( unsigned int i = 0; i < half_edges_.size(); ++i )
  {
    mesh_half_edge& he = half_edges_[ i ];
    if( i < vert_to_he_[ he.vert_index() ] )
    {
      vert_to_he_[ he.vert_index() ] = i;
    }
    if( he.next_index() != mesh_invalid_idx )
    {
      continue;
    }

    unsigned int next_b = half_edges_[ i ].pair_index();
    while( half_edges_[ next_b ].face_index() != mesh_invalid_idx )
    {
      f_iterator fi( next_b, *this );
      while( fi->next_index() != next_b )
      {
        ++fi;
      }
      next_b = fi->pair_index();
    }
    he.next_ = next_b;
  }
}

/// Equality operator
bool
mesh_half_edge_set
::operator==( mesh_half_edge_set const& other ) const
{
  if( half_edges_.size() != other.half_edges_.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < half_edges_.size(); i++ )
  {
    if( half_edges_[ i ] != other.half_edges_[ i ] )
    {
      return false;
    }
  }

  if( vert_to_he_.size() != other.vert_to_he_.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < vert_to_he_.size(); i++ )
  {
    if( vert_to_he_[ i ] != other.vert_to_he_[ i ] )
    {
      return false;
    }
  }

  if( face_to_he_.size() != other.face_to_he_.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < face_to_he_.size(); i++ )
  {
    if( face_to_he_[ i ] != other.face_to_he_[ i ] )
    {
      return false;
    }
  }

  return true;
}

/// Count the number of vertices pointed to by these edges
unsigned int
mesh_half_edge_set
::num_verts() const
{
  unsigned int count = 0;
  for( unsigned int i = 0; i < vert_to_he_.size(); ++i )
  {
    if( vert_to_he_[ i ] != mesh_invalid_idx )
    {
      ++count;
    }
  }
  return count;
}

/// Count the number of faces pointed to by these edges
unsigned int
mesh_half_edge_set
::num_faces() const
{
  unsigned int count = 0;
  for( unsigned int i = 0; i < face_to_he_.size(); ++i )
  {
    if( face_to_he_[ i ] != mesh_invalid_idx )
    {
      ++count;
    }
  }
  return count;
}

// ----------------------------------------------------------------------------
// Mesh

/// Copy Constructor
mesh
::mesh( const mesh& other )
  : verts_( ( other.verts_.get() ) ? other.verts_->clone() : 0 ),
    faces_( ( other.faces_.get() ) ? other.faces_->clone() : 0 ),
    half_edges_( other.half_edges_ ),
    tex_coords_( other.tex_coords_ ),
    tex_source_( other.tex_source_ ),
    valid_tex_faces_( other.valid_tex_faces_ ),
    tex_coord_status_( other.tex_coord_status_ )
{
}

/// Assignment operator
mesh&
mesh
::operator=( mesh const& other )
{
  if( this != &other )
  {
    verts_ = std::unique_ptr< mesh_vertex_array_base >( ( other.verts_.get() )
                                                        ? other.verts_->clone() : 0 );
    faces_ = std::unique_ptr< mesh_face_array_base >( ( other.faces_.get() )
                                                      ? other.faces_->clone() : 0 );
    half_edges_ = other.half_edges_;
    tex_coords_ = other.tex_coords_;
    valid_tex_faces_ = other.valid_tex_faces_;
    tex_coord_status_ = other.tex_coord_status_;
  }
  return *this;
}

/// Checks approximate equality
bool
mesh
::approx_equal( mesh const& other, double epsilon ) const
{
  if( is_init() != other.is_init() )
  {
    return false;
  }
  if( is_init() == false )
  {
    return true;
  }

  if( verts_->approx_equal( *other.verts_, epsilon ) == false )
  {
    return false;
  }
  if( faces_->approx_equal( *other.faces_, epsilon ) == false )
  {
    return false;
  }
  if( half_edges_ != other.half_edges_ )
  {
    return false;
  }

  if( tex_coords_.size() != other.tex_coords_.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < tex_coords_.size(); i++ )
  {
    if( tex_coords_[ i ] != other.tex_coords_[ i ] )
    {
      return false;
    }
  }

  if( tex_source_ != other.tex_source_ )
  {
    return false;
  }

  if( valid_tex_faces_.size() != other.valid_tex_faces_.size() )
  {
    return false;
  }
  for( unsigned int i = 0; i < valid_tex_faces_.size(); i++ )
  {
    if( valid_tex_faces_[ i ] != other.valid_tex_faces_[ i ] )
    {
      return false;
    }
  }

  if( tex_coord_status_ != other.tex_coord_status_ )
  {
    return false;
  }

  return true;
}

/// Merge the data from another mesh into this one
void
mesh
::merge( const mesh& other )
{
  const unsigned num_v = this->num_verts();
  faces_ = merge_face_arrays( *this->faces_, *other.faces_, verts_->size() );
  verts_->append( *other.verts_ );

  if( this->has_tex_coords() == TEX_COORD_NONE )
  {
    std::vector< vector_2d > tex;
    if( other.has_tex_coords() == TEX_COORD_ON_VERT )
    {
      tex = std::vector< vector_2d >( num_v, vector_2d( 0, 0 ) );
    }
    else if( other.has_tex_coords() == TEX_COORD_ON_CORNER )
    {
      unsigned int nb_corners = faces_->regularity() * num_faces();
      if( nb_corners == 0 )
      {
        for( unsigned int f = 0; f < this->num_faces(); ++f )
        {
          nb_corners += this->faces().num_verts( f );
        }
      }
      tex = std::vector< vector_2d >( nb_corners, vector_2d( 0, 0 ) );
    }

    tex.insert( tex.end(), other.tex_coords().begin(),
                other.tex_coords().end() );
    this->set_tex_coords( tex );
  }
  else if( this->has_tex_coords() == other.has_tex_coords() )
  {
    this->tex_coords_.insert( this->tex_coords_.end(),
                              other.tex_coords().begin(),
                              other.tex_coords().end() );
  }

  if( this->has_half_edges() && other.has_half_edges() )
  {
    this->build_edge_graph();
  }
  else
  {
    this->half_edges_.clear();
  }
}

/// Set the texture coordinates
void
mesh
::set_tex_coords( const std::vector< vector_2d >& tc )
{
  unsigned int nb_corners = faces_->regularity() * num_faces();
  if( nb_corners == 0 )
  {
    for( unsigned int f = 0; f < this->num_faces(); ++f )
    {
      nb_corners += this->faces().num_verts( f );
    }
  }

  if( tc.size() == this->num_verts() )
  {
    tex_coord_status_ = TEX_COORD_ON_VERT;
  }
  else if( tc.size() == nb_corners )
  {
    tex_coord_status_ = TEX_COORD_ON_CORNER;
  }
  else
  {
    tex_coord_status_ = TEX_COORD_NONE;
  }

  tex_coords_ = tc;
}

/// Construct the half edges graph structure
void
mesh
::build_edge_graph()
{
  const mesh_face_array_base& faces = this->faces();
  std::vector< std::vector< unsigned int > > face_list( faces.size() );
  for( unsigned int f = 0; f < faces.size(); ++f )
  {
    face_list[ f ].resize( faces.num_verts( f ) );
    for( unsigned int v = 0; v < faces.num_verts( f ); ++v )
    {
      face_list[ f ][ v ] = faces( f, v );
    }
  }

  half_edges_.build_from_ifs( face_list );
}

/// Compute vertex normals
void
mesh
::compute_vertex_normals()
{
  if( !this->has_half_edges() )
  {
    this->build_edge_graph();
  }

  const mesh_half_edge_set& half_edges = this->half_edges();
  mesh_vertex_array< 3 >& verts = this->vertices< 3 >();

  std::vector< vector_3d > normals( this->num_verts(), vector_3d( 0, 0, 0 ) );

  for( unsigned int he = 0; he < half_edges.size(); ++he )
  {
    mesh_half_edge_set::f_const_iterator fi( he, half_edges );
    if( fi->is_boundary() )
    {
      continue;
    }

    const unsigned int vp = fi->vert_index();
    const unsigned int v = ( ++fi )->vert_index();
    const unsigned int vn = ( ++fi )->vert_index();
    normals[ v ] +=
      mesh_tri_normal( verts[ v ], verts[ vn ], verts[ vp ] ).normalized();
  }

  for( unsigned v = 0; v < verts.size(); ++v )
  {
    normals[ v ].normalize();
  }

  verts.set_normals( normals );
}

/// Compute vertex normals using face normals
void
mesh
::compute_vertex_normals_from_faces()
{
  if( !this->has_half_edges() )
  {
    this->build_edge_graph();
  }

  if( !this->faces_->has_normals() )
  {
    this->compute_face_normals();
  }

  const std::vector< vector_3d >& fnormals = faces_->normals();

  const mesh_half_edge_set& half_edges = this->half_edges();
  mesh_vertex_array< 3 >& verts = this->vertices< 3 >();

  std::vector< vector_3d > normals( this->num_verts(), vector_3d( 0, 0, 0 ) );

  for( unsigned int he = 0; he < half_edges.size(); ++he )
  {
    const mesh_half_edge& half_edge = half_edges[ he ];
    if( half_edge.is_boundary() )
    {
      continue;
    }

    const unsigned int v = half_edge.vert_index();
    normals[ v ] += fnormals[ half_edge.face_index() ].normalized();
  }

  for( unsigned v = 0; v < verts.size(); ++v )
  {
    normals[ v ].normalize();
  }

  verts.set_normals( normals );
}

/// Compute face normals
void
mesh
::compute_face_normals( bool norm )
{
  mesh_face_array_base& faces = this->faces();
  const mesh_vertex_array< 3 >& verts = this->vertices< 3 >();

  std::vector< vector_3d > normals( this->num_faces(), vector_3d( 0, 0, 0 ) );

  for( unsigned int i = 0; i < faces.size(); ++i )
  {
    const unsigned int num_v = faces.num_verts( i );
    vector_3d& n = normals[ i ];
    for( unsigned int j = 2; j < num_v; ++j )
    {
      n += mesh_tri_normal( verts[ faces( i, 0 ) ],
                            verts[ faces( i, j - 1 ) ],
                            verts[ faces( i, j ) ] );
    }
    if( norm )
    {
      n.normalize();
    }
  }

  faces.set_normals( normals );
}

/// Map a barycentric coordinate (u,v) on triangle \param tri into texture
/// space
vector_2d
mesh
::texture_map( unsigned int tri, double u, double v ) const
{
  vector_2d tex( 0, 0 );
  if( this->tex_coord_status_ == TEX_COORD_ON_VERT )
  {
    const unsigned int v1 = ( *faces_ )( tri, 0 );
    const unsigned int v2 = ( *faces_ )( tri, 1 );
    const unsigned int v3 = ( *faces_ )( tri, 2 );
    tex += ( 1 - u - v ) * tex_coords_[ v1 ];
    tex += u * tex_coords_[ v2 ];
    tex += v * tex_coords_[ v3 ];
  }
  else if( this->tex_coord_status_ == TEX_COORD_ON_CORNER )
  {
    const unsigned int i1 = 3 * tri + 0;
    const unsigned int i2 = 3 * tri + 1;
    const unsigned int i3 = 3 * tri + 2;
    tex += ( 1 - u - v ) * tex_coords_[ i1 ];
    tex += u * tex_coords_[ i2 ];
    tex += v * tex_coords_[ i3 ];
  }
  return tex;
}

/// Set the vector indicating which faces have texture
void
mesh
::set_valid_tex_faces( const std::vector< bool >& valid )
{
  if( valid.size() == this->num_faces() && has_tex_coords() )
  {
    valid_tex_faces_ = valid;
  }
}

/// Label all faces with positive (counter clockwise orientation) area as valid
void
mesh
::label_ccw_tex_faces_valid()
{
  switch( tex_coord_status_ )
  {
    case TEX_COORD_ON_VERT:
    {
      valid_tex_faces_.resize( this->num_faces() );

      mesh_face_array_base& faces = this->faces();
      for( unsigned int f = 0; f < num_faces(); ++f )
      {
        const unsigned int num_v = faces.num_verts( f );
        // compute (2x) area with Green's theorem
        double area = 0.0;
        for( unsigned int i = 0, j = num_v - 1; i < num_v; j = i++ )
        {
          const vector_2d& tex_i = tex_coords_[ faces( f, i ) ];
          const vector_2d& tex_j = tex_coords_[ faces( f, j ) ];
          area += tex_j.x() * tex_i.y() - tex_i.x() * tex_j.y();
        }
        valid_tex_faces_[ f ] = area > 0;
      }
      break;
    }
    case TEX_COORD_ON_CORNER:
    {
      logger_handle_t logger( get_logger( "vital.mesh" ) );
      LOG_ERROR( logger, "mesh::label_ccw_tex_faces_valid()"
                         " not implemented for TEX_COORD_ON_CORNER" );
      break;
    }
    default:
      break;
  }
}

} // end namespace vital

} // end namespace kwiver


#include <Eigen/Dense>
#include <memory>
#include <utility>
#include <gtest/gtest.h>

#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/snopt_solver.h"
#include "drake/solvers/solve.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "solvers/optimization_utils.h"

namespace dairlib {
namespace solvers {
namespace {

using drake::solvers::MathematicalProgram;
using drake::CompareMatrices;
using std::cout;
using std::endl;
using Eigen::VectorXd;
using Eigen::MatrixXd;
using Eigen::Matrix;

typedef Matrix< double, 1, 1 >  Vector1d;

class CostConstraintApproximationTest : public ::testing::Test {
};

TEST_F(CostConstraintApproximationTest, QPTest) {
  // Initialize the matrix size
  MatrixXd H_o = MatrixXd::Zero(2, 2);
  VectorXd b_o = VectorXd::Zero(2);
  VectorXd c_o = VectorXd::Zero(1);
  MatrixXd A_o = MatrixXd::Zero(2, 2);
  VectorXd lb_o = VectorXd::Zero(2);
  VectorXd ub_o = VectorXd::Zero(2);

  VectorXd w_sol;

  MatrixXd H_a;
  VectorXd b_a;
  Vector1d c_a;
  MatrixXd A_a;
  VectorXd lb_a;
  VectorXd ub_a;
  VectorXd y_a;

  // An example
  H_o <<  1.36075,  0.354964,
          0.354964, 1.19376;
  b_o = VectorXd::Ones(2);
  c_o << 0;
  A_o <<  0.411647, -0.164777,
         -0.302449, 0.26823;
  lb_o = 0.5 * VectorXd::Ones(2);
  ub_o = VectorXd::Ones(2);

  // solve optimization problem
  MathematicalProgram quadprog;
  auto w = quadprog.NewContinuousVariables(2, "w");
  // intentionally split the following constraint into two constraint bindings
  quadprog.AddLinearConstraint(A_o.row(0),
                               lb_o(0),
                               ub_o(0),
                               w);
  quadprog.AddLinearConstraint(A_o.row(1),
                               lb_o(1),
                               ub_o(1),
                               w);
  // Adds a cost term of the form 0.5*x'*H_o*x + b_o'x + c
  // intentionally split the following cost into two cost bindings
  quadprog.AddQuadraticCost(H_o * 1 / 3, b_o * 1 / 3, w);
  quadprog.AddQuadraticCost(H_o * 2 / 3, b_o * 2 / 3, w);
  const auto result = Solve(quadprog);
  w_sol = result.GetSolution(quadprog.decision_variables());

  // Approximation
  LinearizeConstraints(quadprog, w_sol, &y_a, &A_a, &lb_a, &ub_a);
  double c_doub = SecondOrderCost(quadprog, w_sol, &H_a, &b_a);
  c_a << c_doub;

  // Test the cost function
  std::vector<VectorXd> w_sample;
  w_sample.push_back(VectorXd::Random(2));
  w_sample.push_back(VectorXd::Random(2));
  w_sample.push_back(10 * VectorXd::Random(2));

  VectorXd original_cost_value;
  VectorXd approx_cost_value;
  for (int i = 0; i < 3; i++) {
    original_cost_value = 0.5 * w_sample[i].transpose() * H_o * w_sample[i] +
                          b_o.transpose() * w_sample[i] + c_o;
    approx_cost_value =
        0.5 * (w_sample[i] - w_sol).transpose() * H_a * (w_sample[i] - w_sol)
        + b_a.transpose() * (w_sample[i] - w_sol) + c_a;
    EXPECT_TRUE(CompareMatrices(original_cost_value, approx_cost_value, 1e-4));
  }

  // Test A, lb, and ub
  EXPECT_EQ(A_o, A_a);
  EXPECT_EQ(lb_o, lb_a);
  EXPECT_EQ(ub_o, ub_a);
}

}  // namespace
}  // namespace solvers
}  // namespace dairlib

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


/*
 * LineIterator.hpp
 *
 *  Created on: Nov 13, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/SubmapIterator.hpp"

#include <Eigen/Core>

namespace grid_map {

/*!
 * Iterator class to iterate over a line in the map.
 * Based on Bresenham Line Drawing algorithm.
 */
class LineIterator
{
public:

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param start the starting point of the line.
   * @param end the ending point of the line.
   */
  LineIterator(const grid_map::GridMap& gridMap, const Position& start, const Position& end);

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param start the starting index of the line.
   * @param end the ending index of the line.
   */
  LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end);

  /*!
   * Assignment operator.
   * @param iterator the iterator to copy data from.
   * @return a reference to *this.
   */
  LineIterator& operator =(const LineIterator& other);

  /*!
   * Compare to another iterator.
   * @return whether the current iterator points to a different address than the other one.
   */
  bool operator !=(const LineIterator& other) const;

  /*!
   * Dereference the iterator with const.
   * @return the value to which the iterator is pointing.
   */
  const Index& operator *() const;

  /*!
   * Increase the iterator to the next element.
   * @return a reference to the updated iterator.
   */
  LineIterator& operator ++();

  /*!
   * Indicates if iterator is past end.
   * @return true if iterator is out of scope, false if end has not been reached.
   */
  bool isPastEnd() const;

private:

  /*!
   * Construct function.
   * @param gridMap the grid map to iterate on.
   * @param start the starting index of the line.
   * @param end the ending index of the line.
   * @return true if successful, false otherwise.
   */
  bool initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end);

  /*!
   * Computes the parameters requires for the line drawing algorithm.
   */
  void initializeIterationParameters();

  /*!
   * Finds the index of a position on a line within the limits of the map.
   * @param[in] gridMap the grid map that defines the map boundaries.
   * @param[in] start the position that will be limited to the map range.
   * @param[in] end the ending position of the line.
   * @param[out] index the index of the moved start position.
   * @return true if successful, false otherwise.
   */
  bool getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start,
                                 const Position& end, Index& index);

  //! Current index.
  Index index_;

  //! Starting index of the line.
  Index start_;

  //! Ending index of the line.
  Index end_;

  //! Current cell number.
  unsigned int iCell_;

  //! Number of cells in the line.
  unsigned int nCells_;

  //! Helper variables for Bresenham Line Drawing algorithm.
  Size increment1_, increment2_;
  int denominator_, numerator_, numeratorAdd_;

  //! Map information needed to get position from iterator.
  Length mapLength_;
  Position mapPosition_;
  double resolution_;
  Size bufferSize_;
  Index bufferStartIndex_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} /* namespace */