/*

Copyright (c) 2013, Project OSRM, Dennis Luxen, others
All rights reserved.

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

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <osrm/Coordinate.h>

#include "DouglasPeucker.h"
#include "../DataStructures/SegmentInformation.h"

#include <boost/assert.hpp>

#include <cmath>

#include <algorithm>

struct CoordinatePairCalculator
{
    CoordinatePairCalculator() = delete;
    CoordinatePairCalculator(const FixedPointCoordinate &coordinate_a,
                             const FixedPointCoordinate &coordinate_b)
    {
        // initialize distance calculator with two fixed coordinates a, b
        const float RAD = 0.017453292519943295769236907684886f;
        first_lat = (coordinate_a.lat / COORDINATE_PRECISION) * RAD;
        first_lon = (coordinate_a.lon / COORDINATE_PRECISION) * RAD;
        second_lat = (coordinate_b.lat / COORDINATE_PRECISION) * RAD;
        second_lon = (coordinate_b.lon / COORDINATE_PRECISION) * RAD;
    }

    int operator()(FixedPointCoordinate &other) const
    {
        // set third coordinate c
        const float RAD = 0.017453292519943295769236907684886f;
        const float earth_radius = 6372797.560856f;
        const float float_lat1 = (other.lat / COORDINATE_PRECISION) * RAD;
        const float float_lon1 = (other.lon / COORDINATE_PRECISION) * RAD;

        // compute distance (a,c)
        const float x_value_1 = (first_lon - float_lon1) * cos((float_lat1 + first_lat) / 2.f);
        const float y_value_1 = first_lat - float_lat1;
        const float dist1 = sqrt(std::pow(x_value_1, 2) + std::pow(y_value_1, 2)) * earth_radius;

        // compute distance (b,c)
        const float x_value_2 = (second_lon - float_lon1) * cos((float_lat1 + second_lat) / 2.f);
        const float y_value_2 = second_lat - float_lat1;
        const float dist2 = sqrt(std::pow(x_value_2, 2) + std::pow(y_value_2, 2)) * earth_radius;

        // return the minimum
        return static_cast<int>(std::min(dist1, dist2));
    }

    float first_lat;
    float first_lon;
    float second_lat;
    float second_lon;
};

DouglasPeucker::DouglasPeucker()
    : douglas_peucker_thresholds({512440, // z0
                                  256720, // z1
                                  122560, // z2
                                  56780,  // z3
                                  28800,  // z4
                                  14400,  // z5
                                  7200,   // z6
                                  3200,   // z7
                                  2400,   // z8
                                  1000,   // z9
                                  600,    // z10
                                  120,    // z11
                                  60,     // z12
                                  45,     // z13
                                  36,     // z14
                                  20,     // z15
                                  8,      // z16
                                  6,      // z17
                                  4       // z18
      })
{
}

void DouglasPeucker::Run(std::vector<SegmentInformation> &input_geometry, const unsigned zoom_level)
{
    // check if input data is invalid
    BOOST_ASSERT_MSG(!input_geometry.empty(), "geometry invalid");

    if (input_geometry.size() < 2)
    {
        return;
    }

    input_geometry.front().necessary = true;
    input_geometry.back().necessary = true;

    {
        BOOST_ASSERT_MSG(zoom_level < 19, "unsupported zoom level");
        unsigned left_border = 0;
        unsigned right_border = 1;
        // Sweep over array and identify those ranges that need to be checked
        do
        {
            // traverse list until new border element found
            if (input_geometry[right_border].necessary)
            {
                // sanity checks
                BOOST_ASSERT(input_geometry[left_border].necessary);
                BOOST_ASSERT(input_geometry[right_border].necessary);
                recursion_stack.emplace(left_border, right_border);
                left_border = right_border;
            }
            ++right_border;
        } while (right_border < input_geometry.size());
    }

    // mark locations as 'necessary' by divide-and-conquer
    while (!recursion_stack.empty())
    {
        // pop next element
        const GeometryRange pair = recursion_stack.top();
        recursion_stack.pop();
        // sanity checks
        BOOST_ASSERT_MSG(input_geometry[pair.first].necessary, "left border mus be necessary");
        BOOST_ASSERT_MSG(input_geometry[pair.second].necessary, "right border must be necessary");
        BOOST_ASSERT_MSG(pair.second < input_geometry.size(), "right border outside of geometry");
        BOOST_ASSERT_MSG(pair.first < pair.second, "left border on the wrong side");

        int max_int_distance = 0;
        unsigned farthest_entry_index = pair.second;
        const CoordinatePairCalculator DistCalc(input_geometry[pair.first].location,
                                                input_geometry[pair.second].location);

        // sweep over range to find the maximum
        for (unsigned i = pair.first + 1; i < pair.second; ++i)
        {
            const int distance = DistCalc(input_geometry[i].location);
            // found new feasible maximum?
            if (distance > max_int_distance && distance > douglas_peucker_thresholds[zoom_level])
            {
                farthest_entry_index = i;
                max_int_distance = distance;
            }
        }

        // check if maximum violates a zoom level dependent threshold
        if (max_int_distance > douglas_peucker_thresholds[zoom_level])
        {
            //  mark idx as necessary
            input_geometry[farthest_entry_index].necessary = true;
            if (1 < (farthest_entry_index - pair.first))
            {
                recursion_stack.emplace(pair.first, farthest_entry_index);
            }
            if (1 < (pair.second - farthest_entry_index))
            {
                recursion_stack.emplace(farthest_entry_index, pair.second);
            }
        }
    }
}


#include <iostream>
#include <fstream>
#include <vector>
#include <ctime>
#include <stdlib.h>
#include <time.h>
#include <algorithm>
#include <deque>
#include <unordered_set>
#include <math.h>
//PCL
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>

//Eigen
#include <Eigen/Dense>
#include <queue>

#include <unordered_map>

#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
using namespace std;

struct PointXYZIR{
PCL_ADD_POINT4D;
float intensity;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR,
(float,x,x)
(float,y,y)
(float,z,z)
(float,intensity,intensity)
(uint16_t,ring,ring)
)

static double pi = 3.1415926;

static int64_t gtm() {
	struct timeval tm;
	gettimeofday(&tm, 0);
	// return ms
	int64_t re = (((int64_t) tm.tv_sec) * 1000 * 1000 + tm.tv_usec);
	return re;
}

template<typename T> string toString(const T& t) {
	ostringstream oss;
	oss << t;
	return oss.str();
}

float Polar_angle_cal(float x, float y) {
	float temp_tangle = 0;
	if (x == 0 && y == 0) {
		temp_tangle = 0;
	} else if (y >= 0) {
		temp_tangle = (float) atan2(y, x);
	} else if (y <= 0) {
		temp_tangle = (float) atan2(y, x) + 2 * pi;
	}
	return temp_tangle;
}

double calRadius(double theta, double alpha, double beta){
	double r = (1.0/(tan(theta)+ tan(beta))) - (1.0/(tan(alpha+theta) + tan(beta)));
	return r;
}

Eigen::MatrixXd Radius_table(int model, double height, double slope){

	double step = 0.0;
	double initial_angle = 0.0;

	if(model == 64){
		step = 1.0/3.0;
		initial_angle = -2.0;
	}

	Eigen::MatrixXd rtable(model,2);
	rtable.fill(0.0);

	double alpha = step * pi/180;
	double beta = slope * pi/180;

	for(int i=0; i<model; ++i){
		double theta = (i * step + initial_angle);
		if(i==31){
			rtable(i,0) = height * calRadius(theta* pi/180, -alpha, beta);
			if(theta !=0) rtable(i,1)= height/tan(theta* pi/180);
			step = 0.5;
			alpha = step * pi/180;
			initial_angle = -15.0 + 8.83;
		}else{
			rtable(i,0) = height * calRadius(theta* pi/180, alpha, beta);
			if(theta !=0) rtable(i,1) = height/tan(theta* pi/180);
		}
	}

	return rtable;
}

double cal_range(PointXYZIR& p){
	return sqrt(p.x*p.x + p.y*p.y);
}

vector<vector<double>> depth_image(pcl::PointCloud<PointXYZIR>& cloud_in, int model){
	double width = 2083;
	cv::Mat bvimage = cv::Mat::zeros(model, (int)width, CV_8UC1);
	vector<vector<double>> depthimg((int)width, vector<double>(model,-1));
	for(int i=0; i<cloud_in.points.size(); ++i){
		double u = Polar_angle_cal(cloud_in.points[i].x,cloud_in.points[i].y);
		int col = width - (int)(width*(u *180/pi)/360.0) - 1;
		int ind = model - cloud_in.points[i].ring -1;
		depthimg[col][ind] = i;
		bvimage.at<uchar>(ind,col) = (cal_range(cloud_in.points[i])/100)*255;
	}
	cv::Mat image_ColorMap;
	cv::applyColorMap(bvimage, image_ColorMap, cv::COLORMAP_JET);
	cv::resize(image_ColorMap, image_ColorMap, cv::Size(1000, 100));
	//cv::imshow("test",image_ColorMap);
	//cv::waitKey(0);
	return depthimg;
}

void cascased_ground_seg(pcl::PointCloud<PointXYZIR>& cloud_in, int model, double height, double slope){
   	int64_t tm0 = gtm();

	Eigen::MatrixXd rtable = Radius_table(model, height, slope);
	vector<vector<double>> depthimg = depth_image(cloud_in, model);
	double vertical_thresh = 0.08;

	pcl::PointCloud<PointXYZIR>::Ptr ground(
			new pcl::PointCloud<PointXYZIR>);

	pcl::PointCloud<PointXYZIR>::Ptr nground(
			new pcl::PointCloud<PointXYZIR>);

	pcl::PointCloud<PointXYZIR>::Ptr finalground(
			new pcl::PointCloud<PointXYZIR>);

	pcl::PointCloud<PointXYZIR>::Ptr finalnground(
			new pcl::PointCloud<PointXYZIR>);

	//####################################### inner ring segmentaiton #################################
	for(int i=0; i<depthimg.size(); ++i){
		deque<int> ring;
		deque<int> index;
		pcl::PointCloud<PointXYZIR>::Ptr candidate(
			new pcl::PointCloud<PointXYZIR>);

		for(int j=0; j<depthimg[0].size(); ++j){
			if(depthimg[i][j] == -1)
				continue;
			else{
				ring.push_back(j);
				index.push_back(depthimg[i][j]);
			}
		}

		PointXYZIR p0 = cloud_in.points[index[0]];
		double zmax = p0.z;
		double zmin = p0.z;
		candidate->points.push_back(cloud_in.points[index[0]]);
		//cout<<"########################### "<<index.size()<<" "<<rtable<<"\n "<<endl;
		for(int id=0; id<index.size(); ++id){
			if(id != (index.size()-1)){

				double thresh = 0.0;
				for(int ti = ring[id]; ti < ring[id+1]; ++ti){
					thresh += rtable(ti,0);
				}

				double range = cal_range(cloud_in.points[index[id]]);
				double rangen = cal_range(cloud_in.points[index[id+1]]);
				if(fabs(rangen - range) < thresh){
					candidate->points.push_back(cloud_in.points[index[id+1]]);
					if(cloud_in.points[index[id+1]].z > zmax)
						zmax = cloud_in.points[index[id+1]].z;
					if(cloud_in.points[index[id+1]].z < zmin)
						zmin = cloud_in.points[index[id+1]].z;
				}else{
					if(candidate->points.size()>1 && (zmax - zmin) > vertical_thresh){
						*nground += *candidate;
						candidate->points.clear();
						candidate->points.swap(candidate->points);
					}else{
						*ground += *candidate;
						candidate->points.clear();
						candidate->points.swap(candidate->points);
					}
					candidate->points.push_back(cloud_in.points[index[id+1]]);
					zmax = cloud_in.points[index[id+1]].z;
					zmin = cloud_in.points[index[id+1]].z;
				}
			}else{

				if(candidate->points.size()>1 && (zmax - zmin) > vertical_thresh){
					*nground += *candidate;
					candidate->points.clear();
					candidate->points.swap(candidate->points);
				}else{
					*ground += *candidate;
					candidate->points.clear();
					candidate->points.swap(candidate->points);
				}
			}

		}

	}
	//################################### multi plane segmeantation ####################################

	int section = 4;
	int width = 2083;
	double seg_section = (int)(ceil((2083.0)/8.0));
	vector<double> angle_boundary = {width -seg_section, seg_section, 3*seg_section, 5*seg_section, width - seg_section};

	int ring_section = model/4;
	vector<double> ring_boundary = {0, rtable(63-ring_section,1),rtable(63- 2*ring_section,1), rtable(63-3*ring_section,1), rtable(7,1)};

	vector<vector<double>> uint = {{1,0},{0,-1},{-1,0},{0,1}};
	vector<vector<int>> plane_section(4);

	for(int i=0; i < ground->points.size(); ++i){
		double u = Polar_angle_cal(ground->points[i].x,ground->points[i].y);
		int col = width - (int)(width*(u *180/pi)/360.0) - 1;
		if(col< angle_boundary[1]){
			plane_section[0].push_back(i);

		}else if(col >= angle_boundary[1] && col<angle_boundary[2]){
			plane_section[1].push_back(i);

		}else if(col >= angle_boundary[2] && col<angle_boundary[3]){
			plane_section[2].push_back(i);

		}else if(col >= angle_boundary[3]){
			plane_section[3].push_back(i);

		}
	}

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
			new pcl::visualization::PCLVisualizer("pcd")); //PCLVisualizer 可视化类
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addCoordinateSystem(1);
        cv::RNG rng(12345);

	for(int i=0; i<4; ++i){
		vector<vector<int>> ratio_section(4);
		vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> section_point;
		vector<pcl::PointCloud<PointXYZIR>::Ptr> section_point_origin;
		for(int k=0; k<4; ++k){
			pcl::PointCloud<pcl::PointXYZ>::Ptr x(new pcl::PointCloud<pcl::PointXYZ>);
			section_point.push_back(x);
			pcl::PointCloud<PointXYZIR>::Ptr xt(new pcl::PointCloud<PointXYZIR>);
			section_point_origin.push_back(xt);
		}
		for(int j=0; j<plane_section[i].size(); ++j){
			double u = cal_range(ground->points[plane_section[i][j]]);
			if(u >= ring_boundary[0] && u<ring_boundary[1]){
				pcl::PointXYZ p;
				p.x = ground->points[plane_section[i][j]].x;
				p.y = ground->points[plane_section[i][j]].y;
				p.z = ground->points[plane_section[i][j]].z;
				section_point[0]->points.push_back(p);

				PointXYZIR po;
				po = ground->points[plane_section[i][j]];
				section_point_origin[0]->points.push_back(po);

			}else if(u >= ring_boundary[1] && u<ring_boundary[2]){
				pcl::PointXYZ p;
				p.x = ground->points[plane_section[i][j]].x;
				p.y = ground->points[plane_section[i][j]].y;
				p.z = ground->points[plane_section[i][j]].z;
				section_point[1]->points.push_back(p);

				PointXYZIR po;
				po = ground->points[plane_section[i][j]];
				section_point_origin[1]->points.push_back(po);

			}else if(u >= ring_boundary[2] && u<ring_boundary[3]){
				pcl::PointXYZ p;
				p.x = ground->points[plane_section[i][j]].x;
				p.y = ground->points[plane_section[i][j]].y;
				p.z = ground->points[plane_section[i][j]].z;
				section_point[2]->points.push_back(p);

				PointXYZIR po;
				po = ground->points[plane_section[i][j]];
				section_point_origin[2]->points.push_back(po);

			}else if(u >= ring_boundary[3] && u<ring_boundary[4]){
				pcl::PointXYZ p;
				p.x = ground->points[plane_section[i][j]].x;
				p.y = ground->points[plane_section[i][j]].y;
				p.z = ground->points[plane_section[i][j]].z;
				section_point[3]->points.push_back(p);

				PointXYZIR po;
				po = ground->points[plane_section[i][j]];
				section_point_origin[3]->points.push_back(po);

			}
		}

		for(int ci =0; ci<4; ++ci){
			int r = rng.uniform(0, 255) ;
			int b = rng.uniform(0, 255) ;
			int g = rng.uniform(0, 255) ;
			pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ> color2(section_point[ci], (b), (g),(r));
			viewer->addPointCloud(section_point[ci],color2, "cloud" + toString(ci + 4* i));
		}

		pcl::ModelCoefficients::Ptr precoefficients (new pcl::ModelCoefficients);

		//######################### plane fitting ############################
		for(int pid=0; pid<4; ++pid){
			if(section_point[pid]->points.size()>0){
				pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm;
				//创建法向量计算对象
				norm.setInputCloud (section_point[pid]);                  //设置输入点云	//
   				pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
				norm.setSearchMethod (tree); //设置搜索方法
				norm.setKSearch(50); //设置半径邻域搜索
				pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); //创建法向量点云
				norm.compute (*normals); //计算法向量

         			pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
          			pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
          			// Create the segmentation object
          			//pcl::SACSegmentation<pcl::PointXYZ> seg;
       				pcl::SACSegmentationFromNormals<pcl::PointXYZ,pcl::Normal> seg;    //分割对象

          			// Optional
         	 		seg.setOptimizeCoefficients (true);
          			// Mandatory
          			seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
          			seg.setMethodType (pcl::SAC_RANSAC);
          			seg.setDistanceThreshold (0.3);
    				seg.setNormalDistanceWeight(0.5);
    				seg.setMaxIterations(100);
         			seg.setInputCloud (section_point[pid]);
    				seg.setInputNormals(normals);

          			seg.segment (*inliers, *coefficients);
				pcl::PointCloud<PointXYZIR>::Ptr ground_temp(
					new pcl::PointCloud<PointXYZIR>);
				pcl::PointCloud<PointXYZIR>::Ptr nground_temp(
					new pcl::PointCloud<PointXYZIR>);

				unordered_set<int> hash;
				for (size_t j = 0; j < inliers->indices.size (); ++j){
					hash.insert(inliers->indices[j]);
				}

				if(pid==0){
					precoefficients = coefficients;
					for(int li =0; li<section_point_origin[pid]->points.size(); ++li){
						if(hash.count(li)){
							finalground->points.push_back(section_point_origin[pid]->points[li]);
						}else{
							finalnground->points.push_back(section_point_origin[pid]->points[li]);
						}
					}
				}else{

					double x = ring_boundary[pid] * uint[i][0];
					double y = ring_boundary[pid] * uint[i][1];
					Eigen::VectorXd pre(2);
					Eigen::VectorXd now(2);
					pre<<precoefficients->values[0],precoefficients->values[1];
					now<<coefficients->values[0],coefficients->values[1];
					double angle_diff = acos(pre.dot(now));

					Eigen::VectorXd preh(4);
					Eigen::VectorXd nowh(4);
					preh<<precoefficients->values[0],precoefficients->values[1],precoefficients->values[2],precoefficients->values[3];
					nowh<<coefficients->values[0],coefficients->values[1],coefficients->values[2],coefficients->values[3];

					Eigen::VectorXd temp = nowh/nowh(2) - preh/preh(2);
					double height_diff = fabs(temp(0) * x + temp(1) * y + temp(3));

					if(height_diff >=  0.3 || angle_diff >= 5*pi/180){
						for(int li =0; li<section_point_origin[pid]->points.size(); ++li){
							if(hash.count(li)){
								finalground->points.push_back(section_point_origin[pid]->points[li]);
							}else{
								finalnground->points.push_back(section_point_origin[pid]->points[li]);
							}
							precoefficients = coefficients;
						}
					}else{
						for(int li =0; li<section_point_origin[pid]->points.size(); ++li){
							double px(0),py(0),pz(0);
							px = section_point_origin[pid]->points[li].x;
							py = section_point_origin[pid]->points[li].y;
							pz = section_point_origin[pid]->points[li].z;
							double distance = fabs(preh(0) * px + preh(1)*py + preh(2)*pz + \
										preh(3))/sqrt(preh(0)*preh(0) +preh(1)*preh(1)+preh(2)*preh(2));
							if(distance < 0.3){
								finalground->points.push_back(section_point_origin[pid]->points[li]);
							}else{
								finalnground->points.push_back(section_point_origin[pid]->points[li]);
							}

						}
					}
				}

			}

		}

	}

	pcl::PointCloud<pcl::PointXYZI>::Ptr show(
			new pcl::PointCloud<pcl::PointXYZI>);

	for(int si = 0; si<finalground->points.size(); ++si){
		pcl::PointXYZI p;
		p.x = finalground->points[si].x;
		p.y = finalground->points[si].y;
		p.z = finalground->points[si].z;
		p.intensity = finalground->points[si].intensity;
		show->points.push_back(p);
	}

	pcl::PointCloud<pcl::PointXYZI>::Ptr showob(
			new pcl::PointCloud<pcl::PointXYZI>);

	for(int si = 0; si<finalnground->points.size(); ++si){
		pcl::PointXYZI p;
		p.x = finalnground->points[si].x;
		p.y = finalnground->points[si].y;
		p.z = finalnground->points[si].z;
		p.intensity = finalnground->points[si].intensity;
		showob->points.push_back(p);
	}

	*finalnground = *finalnground + *nground;
   	int64_t tm1 = gtm();
  	printf("[INFO]seg cast time:%ld us\n",  tm1-tm0);

	pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZI> color2(show, (255), (0),(0));
	string cs = "cloudfinal";
	//viewer->addPointCloud(show,color2, cs);

	pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZI> colorob(showob, (0), (255),(0));
	string csob = "cloudfinalob";
	//viewer->addPointCloud(showob,colorob, csob);

	while (!viewer->wasStopped()) {
		viewer->spin();
	}

}

int main(int argc, char** argv){
	int model = 64;
	double height = 2.0;
	double slope = 10.0;
    	pcl::PointCloud<PointXYZIR>::Ptr cloud(new pcl::PointCloud<PointXYZIR>);//初始化点云
    	pcl::io::loadPCDFile<PointXYZIR>(argv[1], *cloud);//加载pcd点云并放入cloud中
    	cloud->height = 1;
    	cloud->width = cloud->points.size();
    	cloud->is_dense = false;//最终优化结果
	cascased_ground_seg(*cloud, model,height, slope);

	return 0;
}


/* solve_chol - solve a linear system A*X = B using the cholesky factorization
 of A (where A is square, symmetric and positive definite.

 Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch 2010-12-16. */

#include "mex.h"
#include <math.h>
#include <string.h>
#include "basis_functions/bf_fast_food.h"
#include "gp_deg.h"
#include <Eigen/Dense>
#include <iostream>

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	//TODO: create abstract class
	size_t M;
	size_t D;
	size_t n;
	size_t p;
	if (nrhs != 4 || nlhs < 2) { /* check the input */
		mexErrMsgTxt(
				"Usage: [alpha, L, nlZ, dnlZ, s, g, pi, b] = infFastFoodmex(M, unwrap(hyp), x, y)");
	}
	M = (size_t) mxGetScalar(prhs[0]);
	n = mxGetM(prhs[2]);
	D = mxGetN(prhs[2]);
	std::cout << "input dimensionality: " << D << std::endl;
	std::cout << "number of basis function: " << M << std::endl;
	std::cout << "number of points: " << n << std::endl;
	p = mxGetM(prhs[1]);
	libgp::DegGaussianProcess gp(D, "CovSum ( CovSEard, CovNoise)", M,
			"FastFood");
	Eigen::VectorXd params = Eigen::Map<const Eigen::VectorXd>(mxGetPr(prhs[1]),
			p);

	gp.covf().set_loghyper(params);

	Eigen::VectorXd y = Eigen::Map<const Eigen::VectorXd>(mxGetPr(prhs[3]), n);
	Eigen::MatrixXd X = Eigen::Map<const Eigen::MatrixXd>(mxGetPr(prhs[2]), n,
			D);

	for (size_t i = 0; i < n; i++) {
		gp.add_pattern(X.row(i), y(i));
	}

	plhs[0] = mxCreateDoubleMatrix(M, 1, mxREAL); /* allocate space for output */
	Eigen::Map<Eigen::VectorXd>(mxGetPr(plhs[0]), M) = gp.getAlpha();
	plhs[1] = mxCreateDoubleMatrix(M, M, mxREAL);
	Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[1]), M, M) = gp.getL();
	if (nlhs >= 3) {
		double nlZ = gp.log_likelihood();
		plhs[2] = mxCreateDoubleScalar(nlZ);
		if (nlhs >= 4) {
			std::cout << "infFastFood: computing gradients" << std::endl;
			Eigen::VectorXd grads = gp.log_likelihood_gradient();
			size_t params = grads.size();
			plhs[3] = mxCreateDoubleMatrix(params, 1, mxREAL); /* allocate space for output */
			Eigen::Map<Eigen::VectorXd>(mxGetPr(plhs[3]), params) = grads;
			if (nlhs >= 5) {
				std::cout << "infFastFood: gathering data..." << std::endl;
				M = floor(M / 2 / D);
				int out;
				std::frexp(D - 1, &out);
				D = pow(2, out);
				libgp::FastFood * bf = (libgp::FastFood *) &gp.covf();
				plhs[4] = mxCreateDoubleMatrix(M, D, mxREAL);
				plhs[5] = mxCreateDoubleMatrix(M, D, mxREAL);
				plhs[6] = mxCreateDoubleMatrix(M, D, mxREAL);
				plhs[7] = mxCreateDoubleMatrix(M, D, mxREAL);
				Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[4]), M, D) =
						bf->getS();
				Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[5]), M, D) =
						bf->getG();
				Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[6]), M, D) =
						bf->getPI();
				Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[7]), M, D) =
						bf->getB();
				std::cout << "infFastFood: done." << std::endl;
			}
		}
	}
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP
#define BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/concept_check.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace aitoff{

            struct par_aitoff
            {
                double    cosphi1;
                int        mode;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_aitoff_spheroid : public base_t_f<base_aitoff_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_aitoff m_proj_parm;

                inline base_aitoff_spheroid(const Parameters& par)
                    : base_t_f<base_aitoff_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double c, d;

                    if((d = acos(cos(lp_lat) * cos(c = 0.5 * lp_lon)))) {/* basic Aitoff */
                        xy_x = 2. * d * cos(lp_lat) * sin(c) * (xy_y = 1. / sin(d));
                        xy_y *= d * sin(lp_lat);
                    } else
                        xy_x = xy_y = 0.;
                    if (this->m_proj_parm.mode) { /* Winkel Tripel */
                        xy_x = (xy_x + lp_lon * this->m_proj_parm.cosphi1) * 0.5;
                        xy_y = (xy_y + lp_lat) * 0.5;
                    }
                }
            };

            template <typename Parameters>
            void setup(Parameters& par, par_aitoff& proj_parm)
            {
                boost::ignore_unused_variable_warning(par);
                boost::ignore_unused_variable_warning(proj_parm);
                // par.inv = 0;
                // par.fwd = s_forward;
                par.es = 0.;
            }

            // Aitoff
            template <typename Parameters>
            void setup_aitoff(Parameters& par, par_aitoff& proj_parm)
            {
                proj_parm.mode = 0;
                setup(par, proj_parm);
            }

            // Winkel Tripel
            template <typename Parameters>
            void setup_wintri(Parameters& par, par_aitoff& proj_parm)
            {
                proj_parm.mode = 1;
                if (pj_param(par.params, "tlat_1").i)
                    {
                    if ((proj_parm.cosphi1 = cos(pj_param(par.params, "rlat_1").f)) == 0.)
                        throw proj_exception(-22);
                    }
                else /* 50d28' or acos(2/pi) */
                    proj_parm.cosphi1 = 0.636619772367581343;
                setup(par, proj_parm);
            }

        }} // namespace detail::aitoff
    #endif // doxygen

    /*!
        \brief Aitoff projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Example
        \image html ex_aitoff.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct aitoff_spheroid : public detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>
    {
        inline aitoff_spheroid(const Parameters& par) : detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aitoff::setup_aitoff(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Winkel Tripel projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - lat_1
        \par Example
        \image html ex_wintri.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct wintri_spheroid : public detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>
    {
        inline wintri_spheroid(const Parameters& par) : detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aitoff::setup_wintri(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class aitoff_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_f<aitoff_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class wintri_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_f<wintri_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void aitoff_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("aitoff", new aitoff_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("wintri", new wintri_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP


#include <iostream>
#include <fstream>
#include <string>
#include <stdexcept>
#include <cmath>

#include <boost/format.hpp>
#include "path_optimization.h"

using namespace planner;
using namespace std;

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

  NonHolonomicPath::State lc_start(14.254, 125.566, -96.9345/(2*M_PI), 0.0461623);
  NonHolonomicPath::State lc_end(12.2967, 75.585, -90.2891/(2*M_PI), 0.0);

  NonHolonomicPath path;
  const bool success = path.optimizePath(lc_start, lc_end);

  if (!success) {
    boost::format state_format("%1% %2% %3% %4%\n");
    std::string error_msg("Path optimization failed.\n");
    error_msg += "lc start state (x y theta kappa): " + (
        state_format % lc_start.x
                     % lc_start.y
                     % lc_start.theta
                     % lc_start.kappa).str();
    error_msg += "lc end state (x y theta kappa): " + (
        state_format % lc_end.x
                     % lc_end.y
                     % lc_end.theta
                     % lc_end.kappa).str();
    throw std::runtime_error(error_msg);
    return -1;
  }

  return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/models/adjusterhelper.hpp>
#include <ql/pricingengines/swaption/gaussian1dfloatfloatswaptionengine.hpp>
#include <ql/cashflows/cashflows.hpp>
#include <ql/cashflows/couponpricer.hpp>

#include <boost/assign/list_of.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

void AdjusterHelper::performCalculations() const {
    // set up exotic coupon as float float swap and option on it
    Date start = index_->valueDate(fixingDate_);
    Date end = paymentDate_;
    Schedule sched(start, end, (end - start) * Days, NullCalendar(), Unadjusted,
                   Unadjusted, DateGeneration::Forward, false);
    instrument_ = boost::shared_ptr<FloatFloatSwap>(new FloatFloatSwap(
        VanillaSwap::Receiver, 100000000.0, 0.0, sched, index_,
        Actual365Fixed(), sched, index_, Actual365Fixed(), false, false, 1.0,
        0.0, cappedRate_, flooredRate_));
    boost::shared_ptr<Exercise> dummyExercise =
        boost::make_shared<EuropeanExercise>(fixingDate_);
    dummyOption_ =
        boost::make_shared<FloatFloatSwaption>(instrument_, dummyExercise);

    // use the same discounting curve for the reference price as in the model
    boost::shared_ptr<Gaussian1dFloatFloatSwaptionEngine> tmpEngine =
        boost::dynamic_pointer_cast<Gaussian1dFloatFloatSwaptionEngine>(
            modelEngine_);
    QL_REQUIRE(tmpEngine != NULL,
               "engine must be a Gaussian1dFloatFloatSwaptionEngine");
    Handle<YieldTermStructure> yts_discount = tmpEngine->discountingCurve();

    // calculate reference price
    Leg exotic = instrument_->leg(0);
    QuantLib::setCouponPricer(exotic, referencePricer_);
    referenceValue_ = CashFlows::npv(exotic, **yts_discount, false);
}

Real AdjusterHelper::modelValue() const {
    calculate();
    dummyOption_->setPricingEngine(modelEngine_);
    return dummyOption_->result<Real>("underlyingValue");
}
} // namespace QuantLib


#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

int colorWidth, colorHeight, depthWidth, depthHeight;
Eigen::Matrix4f colorIntrinsic;
Eigen::Matrix4f depthIntrinsic;
int frame_size;

void ParseInfo(const char* filename) {
	std::ifstream is(filename);
	char buffer[1024];
	is.getline(buffer, 1024);
	is.getline(buffer, 1024);
	is >> buffer >> buffer >> colorWidth;
	is >> buffer >> buffer >> colorHeight;
	is >> buffer >> buffer >> depthWidth;
	is >> buffer >> buffer >> depthHeight;
	is.getline(buffer, 1024);
	is.getline(buffer, 1024);
	is >> buffer >> buffer;
	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			is >> colorIntrinsic(i, j);
		}
	}
	is.getline(buffer, 1024);
	is.getline(buffer, 1024);
	is >> buffer >> buffer;
	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			is >> depthIntrinsic(i, j);
		}
	}
	is.getline(buffer, 1024);
	is.getline(buffer, 1024);
	is >> buffer >> buffer >> frame_size;

}

void LoadFrames(const char* folder, int i, cv::Mat& color, cv::Mat& depth, Eigen::Matrix4f& pose) {
	char buffer[1024];
	sprintf(buffer, "%s/image_data/frame-%06d.color.jpg", folder, i);
	color = cv::imread(buffer);
	sprintf(buffer, "%s/image_data/frame-%06d.depth.pgm", folder, i);
	depth = cv::imread(buffer, cv::IMREAD_UNCHANGED);
	sprintf(buffer, "%s/image_data/frame-%06d.pose.txt", folder, i);
	std::ifstream is(buffer);
	for (int j = 0; j < 4; ++j) {
		for (int k = 0; k < 4; ++k) {
			is >> pose(j, k);
		}
	}
	is.close();
	pose = pose.inverse().eval();
}
int main(int argc, char** argv)
{
	char buffer[1024];
	sprintf(buffer, "%s/image_data/_info.txt", argv[1]);
	printf("%s\n", buffer);
	ParseInfo(buffer);
	sprintf(buffer, "%s/%s", argv[1], argv[2]);
	std::ifstream is(buffer);
	float x, y, z;
	std::vector<Eigen::Vector3f> vertices;
	while (is >> x) {
		is >> y >> z;
		vertices.push_back(Eigen::Vector3f(x, y, z));
	}
	is.close();
	sprintf(buffer, "%s/%s", argv[1], argv[3]);
	is.open(buffer);
	float tx, ty, tz, nx, ny, nz;
	std::vector<Eigen::Vector3f> orients(vertices.size()), normals(vertices.size());
	for (int i = 0; i < vertices.size(); ++i) {
		is >> orients[i][0] >> orients[i][1] >> orients[i][2] >> normals[i][0] >> normals[i][1] >> normals[i][2];
	}
	is.close();
	for (int i = 0; i < vertices.size(); ++i) {
		Eigen::Vector3f qx = orients[i];
		Eigen::Vector3f n = normals[i];
		Eigen::Vector3f qy = n.cross(qx);
		if (qx[1] > 0)
			qx = -qx;
		if (qy[1] > 0)
			qy = -qy;
		if (qx[1] > qy[1])
			qx = qy;
		orients[i] = qx;
	}

	int half_kernel = 5;
	int kernel = 2 * half_kernel;
	float pixel_width = 0.004;
	int kernel2 = kernel * kernel;
	std::vector<Eigen::Vector3f> textiles(vertices.size() * kernel2, Eigen::Vector3f(0, 0, 0));
	std::vector<cv::Vec3f> textile_colors(vertices.size() * kernel2, cv::Vec3f(0, 0, 0));
	std::vector<int> textile_count(vertices.size() * kernel2, 0);

	for (int i = 0; i < vertices.size(); ++i) {
		Eigen::Vector3f p = vertices[i];
		Eigen::Vector3f tx = orients[i];
		Eigen::Vector3f ty = normals[i].cross(orients[i]);
		for (int j = -half_kernel; j < half_kernel; ++j) {
			for (int k = -half_kernel; k < half_kernel; ++k) {
				Eigen::Vector3f pt = p + tx * (j + 0.5f) * pixel_width + ty * (k + 0.5f) * pixel_width;
				textiles[i * kernel2 + (j + half_kernel) * kernel + k + half_kernel] = pt;
			}
		}
	}

	std::vector<cv::Vec3b> vertices_colors(vertices.size(), cv::Vec3b(0, 0, 0));
	std::vector<cv::Vec3b> temp_colors(kernel2);
	std::vector<int> temp_mask(kernel2);
	std::vector<double> variance(vertices.size(), 1e30);

	for (int j = 0; j < frame_size; ++j) {
		printf("%d of %d\n", j, frame_size);
		cv::Mat color, depth;
		Eigen::Matrix4f pose;
		LoadFrames(argv[1], j, color, depth, pose);
		for (int i = 0; i < vertices.size(); ++i) {
			Eigen::Vector4f p(vertices[i][0], vertices[i][1], vertices[i][2], 1);
			p = pose * p;
			float z = p[2];
			if (z <= 0.3)
				continue;
			float visual = p[0] * p[0] + p[1] * p[1] + p[2] * p[2];
			p = p / p[2];
			Eigen::Vector4f pd = depthIntrinsic * p;
			int px = pd[0] + 0.5;
			int py = pd[1] + 0.5;
			if (px >= 0 && px < depthWidth && py >= 0 && py < depthHeight) {
				double d = depth.at<unsigned short>(py, px) * 1e-3;
				double dis = std::abs(d - z);
				if (dis < 0.1) {
					float r = 0.0f, g = 0.0f, b = 0.0f, a = 0.0f;
					for (int k = 0; k < kernel2; ++k) {
						Eigen::Vector3f pc3 = textiles[i * kernel2 + k];
						Eigen::Vector4f pc(pc3[0], pc3[1], pc3[2], 1);
						pc = pose * pc;
						pc = pc / pc[2];
						pc = colorIntrinsic * pc;
						px = pc[0] + 0.5;
						py = pc[1] + 0.5;
						if (px < colorWidth - 100 && py < colorHeight - 100 && px >= 100 && py >= 100) {
							temp_colors[k] = color.at<cv::Vec3b>(py, px);
							temp_mask[k] = 1;
							r += temp_colors[k].val[0];
							g += temp_colors[k].val[1];
							b += temp_colors[k].val[2];
							a += 1;
						} else {
							temp_mask[k] = 0;
						}
					}
					if (a == 0.0)
						continue;

					r /= a;
					g /= a;
					b /= a;
					for (int k = 0; k < kernel2; ++k) {
						if (temp_mask[k] == 0) {
							temp_colors[k] = cv::Vec3b(r, g, b);
						}
					}
					if (visual < variance[i]) {
						variance[i] = visual;
						for (int k = 0; k < kernel2; ++k) {
							auto p = temp_colors[k];
							textile_colors[i * kernel2 + k] = cv::Vec3f(p.val[0], p.val[1], p.val[2]);
							//textile_count[i * kernel2 + k] += 1;
						}
					}

				}
			}
		}
	}

	for (int i = 0; i < textile_colors.size(); ++i) {
		if (textile_count[i] != 0) {
			//textile_colors[i] /= (float)textile_count[i];
		}
	}
	for (int i = 0; i < textile_colors.size(); ++i) {
		textile_colors[i] /= 255.0f;
	}
	sprintf(buffer, "%s/%s", argv[1], argv[4]);
	FILE* fp = fopen(buffer, "wb");
	fwrite(textile_colors.data(), sizeof(cv::Vec3f), textile_colors.size(), fp);
	fclose(fp);
	return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Polyhedron_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>

#include <OpenMesh/Core/IO/MeshIO.hh>

#if 1
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>
#include <CGAL/boost/graph/graph_traits_PolyMesh_ArrayKernelT.h>
#else
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <CGAL/boost/graph/graph_traits_TriMesh_ArrayKernelT.h>
#endif

#include <CGAL/boost/graph/convert_surface_mesh.h>

#include <boost/unordered_map.hpp>

#include <iostream>
#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel  Kernel;
typedef Kernel::Vector_3                                     Vector;
typedef Kernel::Point_3                                      Point;
typedef CGAL::Polyhedron_3<Kernel>                           Source;

#if 1
typedef OpenMesh::PolyMesh_ArrayKernelT</* MyTraits*/> Target;
#else
typedef OpenMesh::TriMesh_ArrayKernelT</* MyTraits*/> Target;
#endif
typedef boost::graph_traits<Source>::vertex_descriptor sm_vertex_descriptor;
typedef boost::graph_traits<Target>::vertex_descriptor tm_vertex_descriptor;

typedef boost::graph_traits<Source>::halfedge_descriptor sm_halfedge_descriptor;
typedef boost::graph_traits<Target>::halfedge_descriptor tm_halfedge_descriptor;

namespace OpenMesh {

inline  std::size_t hash_value(const VertexHandle&  i)
  {
    return i.idx();
  }

inline  std::size_t hash_value(const HalfedgeHandle&  i)
  {
    return i.idx();
  }

inline  std::size_t hash_value(const FaceHandle&  i)
  {
    return i.idx();
  }

}

int main(int argc, char* argv[])
{
  Source S;
  Target T;
  std::ifstream in((argc>1)?argv[1]:"cube.off");
  in >> S;

  {
    boost::unordered_map<sm_vertex_descriptor, tm_vertex_descriptor> v2v;
    boost::unordered_map<sm_halfedge_descriptor, tm_halfedge_descriptor> h2h;

    convert_surface_mesh(S,T,v2v,h2h);
    OpenMesh::IO::write_mesh(T, "om.off");
  }
  S.clear();
  {
    boost::unordered_map<tm_vertex_descriptor, sm_vertex_descriptor> v2v;
    boost::unordered_map<tm_halfedge_descriptor, sm_halfedge_descriptor> h2h;

    convert_surface_mesh(T,S,v2v,h2h);
    std::ofstream out("reverse.off");
    out << S << std::endl;
  }

  return 0;
}


//
// Created by Sylvain  on 6/1/20.
//

#include "SatOps/GravitationalField.h"
#include "SatOps/constants.h"
#include "SatOps/ReferenceFrame.h"
#include <cmath>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <boost/math/special_functions/factorials.hpp>

GravitationalField::GravitationalField(Model model, unsigned int max_degree, const std::filesystem::path& file)
    :
    PotentialField(max_degree),
    m_model(model)
{
    // Allocates and initializes vectors containing the coefficients
    unsigned int num_entries = (max_degree + 1) * (max_degree + 2) / 2;
    m_c_coeffs = std::vector(num_entries, 0.0);
    m_s_coeffs = std::vector(num_entries, 0.0);

    switch (m_model) {
        case Model::EGM2008:
            loadEGM08Coeffs(file);
            m_radius = constants::R_EARTH_EGM08;
            m_scale_factor = -constants::MU_EARTH_EGM08;
            break;
        case Model::GEM10:
            if (max_degree > 5)
                throw std::invalid_argument("GravitationalField: The degree of expansion of the GEM10 model is too high (max 5).");

            loadEGM08Coeffs(file);
            m_radius = constants::R_EARTH_GEM10;
            m_scale_factor = -constants::MU_EARTH_GEM10;
            break;
    }
}

void GravitationalField::loadEGM08Coeffs(const std::filesystem::path& path) {
    std::ifstream data_file(path);
    if (data_file.is_open()) {
        unsigned int num_entries = (m_max_degree + 1) * (m_max_degree + 2) / 2;

        // Reads required lines
        for (std::size_t i = 3; i < num_entries; ++i) {
            std::string line;
            getline(data_file, line);

            // Parses the line, converts to double, and stores into arrays
            unsigned long k = line.find('D');
            while (k != std::string::npos) {
                line.replace(k, 1, "E");
                k = line.find('D');
            }
            std::istringstream iss(line);
            std::vector<std::string> results(std::istream_iterator<std::string>{iss}, std::istream_iterator<std::string>());
            int n = stoi(results[0]);
            int m = stoi(results[1]);
            double kron_delta = (0 == m) ? 1. : 0.;
            long double unnormalization_factor = std::sqrt(boost::math::factorial<long double>(n - m) * (2. * n + 1) * (2. - kron_delta) / boost::math::factorial<long double>(n + m));
            m_c_coeffs[i] = unnormalization_factor * stod(results[2]);
            m_s_coeffs[i] = unnormalization_factor * stod(results[3]);
        }
        data_file.close();
    } else {
        throw std::invalid_argument("GravitationalField: The file containing the spherical harmonics coefficients could not be opened.");
    }
}

Vector3d GravitationalField::getAcceleration(const Vector3d& itrf93_position, Matrix3d* jacobian) {
    double x = itrf93_position[0];
    double y = itrf93_position[1];
    double z = itrf93_position[2];

    std::array<double, 12> sums{};
    sums.fill(0.0);
    sums[0] = 1.0;
    sums[4] = 2.0;

    Vector3d gravity_field = computeGradient(x, y, z, sums, jacobian);
    gravity_field.setFrame(ReferenceFrame::ITRF93);
    return gravity_field;
}


/*
 * camera_pinhole_test.cpp
 *
 *  Created on: Oct 26, 2012
 *      Author: cforster
 */

#include <string>
#include <stdint.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <vikit/math_utils.h>
#include <vikit/timer.h>
#include <vikit/sample.h>
#include <vikit/pinhole_camera.h>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
  vk::PinholeCamera cam(752, 480, 414.5, 414.2, 348.8, 240.0);
  Matrix4d T_ref_cur(Matrix4d::Identity());
  T_ref_cur(0,3) = 0.5;
  Vector2d u_cur(200, 300);
  Vector3d f_cur(cam.cam2world(u_cur));
  double depth_cur = 2.0;
  Vector3d f_ref(vk::project3d(T_ref_cur*vk::unproject3d(f_cur*depth_cur)));
  double depth_ref = f_ref.norm();
  Vector2d u_ref(cam.world2cam(f_ref));
  double z_ref, z_cur;
  vk::depthFromTriangulationExact(T_ref_cur.topLeftCorner<3,3>(), T_ref_cur.topRightCorner<3,1>(), f_ref, f_cur, z_ref, z_cur);
  printf("depth = %f, triangulated depth = %f\n", depth_cur, z_cur);
  printf("depth = %f, triangulated depth = %f\n", depth_ref, z_ref);

  return 0;
}


//#########################################################//
//#                                                       #//
//# gaussian_mixture_models  gmm_classifier.cpp           #//
//# Roberto Capobianco  <capobianco@dis.uniroma1.it>      #//
//#                                                       #//
//#########################################################//

#include <Eigen/Core>
#include <fstream>
#include <iostream>
#include <set>
#include <stdexcept>
#include <vector>

#include <omp.h>

#include <particle_filter/gmm_classifier.h>
#include <particle_filter/matrix_io.h>

namespace gmms {
void GMMClassifier::train(const Eigen::MatrixXd& dataset,
        bool evaluate_bic,
        int gmm_components) {
    int dataset_size = dataset.rows();

    Eigen::VectorXi labels = dataset.rightCols(1).cast<int>();
    Eigen::MatrixXd training_data = dataset.leftCols(dataset.cols() - 1);
    std::set<int> classes(labels.data(), labels.data() + labels.size());
    classes_.clear();
    classes_.insert(classes_.begin(), classes.begin(), classes.end());

    gmm_vec_.clear();
    gmm_vec_.resize(classes_.size());

    input_size_ = training_data.cols();
    int class_idx = 0;

    for (std::vector<int>::iterator it = classes_.begin(); it != classes_.end();
            ++it) {
        Eigen::MatrixXd class_data(
                (labels.array() == *it).count(), training_data.cols());

        std::cout << "Training class number " << class_idx << ": " << *it
                  << std::endl;
        std::cout << "\t\tClass data size: " << class_data.rows() << std::endl;

        if (class_data.rows() < 10) {
            std::cerr << notsufficient_() << std::endl;
            gmm_vec_[class_idx] = nullptr;
            continue;
        }

        int class_data_idx = 0;

        for (int data_idx = 0; data_idx < labels.size(); ++data_idx) {
            if (labels(data_idx) == *it) {
                class_data.row(class_data_idx++) = training_data.row(data_idx);
            }
        }

        int num_components;
        double bic = 0.0;
        bool first_trial = true;

        std::cout << "\t\tEvaluate BIC: ";

        if (evaluate_bic) {
            std::cout << "ON" << std::endl;
            num_components = 1;
        } else {
            std::cout << "OFF" << std::endl;
            num_components = gmm_components;
        }

#pragma parallel for
        for (int c = num_components; c <= gmm_components; ++c) {
            std::cout << "\t\t\t\tUsing " << c << " GMM components"
                      << std::endl;

            std::shared_ptr<GaussianMixtureModel> model(
                    new GaussianMixtureModel);
            model->setNumComponents(c);
            model->initialize(class_data);
            model->setNumIterations(max_iterations_);
            model->setDelta(delta_);

            try {
                model->expectationMaximization(class_data);
            } catch (std::runtime_error e) {
                std::cout << "\t\t\t\t" << c << " components are not usable"
                          << std::endl;
                break;
            }

            double trial_bic = model->bayesianInformationCriterion(class_data);
            std::cout << "\t\t\t\tTrial BIC: " << trial_bic << std::endl;

            if (first_trial || trial_bic < bic) {
                bic = trial_bic;
                gmm_vec_[class_idx] = model;

                if (first_trial) {
                    first_trial = false;
                }
            }
        }

        std::cout << "\t\tFinal class model with "
                  << gmm_vec_[class_idx]->numComponents() << "; ";
        std::cout << "BIC " << bic << std::endl;
        ++class_idx;
    }

    trained_ = true;
}

std::vector<int> GMMClassifier::predict(const Eigen::MatrixXd& dataset) const {
    if (!trained_) {
        throw std::runtime_error(nottrained_());
    }

    if (dataset.cols() != input_size_) {
        throw std::runtime_error(dimensionality_mismatch_());
    }

    int dataset_size = dataset.rows();
    std::vector<int> assignments(dataset_size);

    int num_threads = omp_get_max_threads();
    int thread_dataset_size = floor(double(dataset_size) / double(num_threads));

    int thread_log_likelihood[num_threads];

#pragma omp parallel for
    for (int i = 0; i < dataset_size; ++i) {
        bool first = true;
        double log_likelihood = 0.0;

        for (int k = 0; k < gmm_vec_.size(); ++k) {
            if (gmm_vec_[k] == nullptr) {
                continue;
            }

            double class_log_likelihood =
                    gmm_vec_[k]->logLikelihood(dataset.row(i));

            if (first || log_likelihood < class_log_likelihood) {
                assignments[i] = classes_[k];
                log_likelihood = class_log_likelihood;

                if (first) {
                    first = false;
                }
            }
        }
    }

    return assignments;
}

void GMMClassifier::load(const std::string filename) {
    MatrixIO mio;
    Eigen::MatrixXd model;

    mio.readFromFile(filename, model);

    trained_ = model(0, 0);
    delta_ = model(1, 0);
    max_iterations_ = model(2, 0);
    input_size_ = model(3, 0);

    classes_.clear();
    gmm_vec_.clear();
    int idx = 4;

    while (idx < model.rows() - 1) {
        classes_.push_back(model(idx, 0));
        int rows = model(idx + 1, 0);
        gmm_vec_.push_back(std::shared_ptr<GaussianMixtureModel>(
                new GaussianMixtureModel));
        gmm_vec_[gmm_vec_.size() - 1]->load(
                model.block(idx + 2, 0, rows, model.cols()));

        idx += rows + 2;
    }
}

void GMMClassifier::save(const std::string filename) {
    MatrixIO mio;
    int rows = 0;
    int cols = 0;
    std::vector<Eigen::MatrixXd> gmm_models;

    for (int k = 0; k < gmm_vec_.size(); ++k) {
        if (gmm_vec_[k] == nullptr) {
            continue;
        }

        gmm_models.push_back(gmm_vec_[k]->save());
        rows += gmm_models[k].rows();

        if (cols == 0) {
            cols = gmm_models[k].cols();
        }
    }

    // gmm model + classes + num rows per classes
    Eigen::MatrixXd model =
            Eigen::MatrixXd::Zero(rows + 2 * classes_.size() + 4, cols);

    model(0, 0) = trained_;
    model(1, 0) = delta_;
    model(2, 0) = max_iterations_;
    model(3, 0) = input_size_;

    int idx = 4;

    for (int k = 0; k < gmm_vec_.size(); ++k) {
        if (gmm_vec_[k] == nullptr) {
            continue;
        }

        model(idx, 0) = classes_[k];
        model(idx + 1, 0) = gmm_models[k].rows();
        model.block(idx + 2, 0, gmm_models[k].rows(), gmm_models[k].cols()) =
                gmm_models[k];
        idx += gmm_models[k].rows() + 2;
    }

    mio.writeToFile(filename, model);
}
}  // namespace gmms


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#pragma once

#include <array>
#include <memory>
#include <Eigen/Dense>

#include "libvideostitch/config.hpp"
#include "libvideostitch/inputDef.hpp"

#include "boundedValue.hpp"

namespace VideoStitch {

namespace Core {
class GeometryDefinition;
class RigCameraDefinition;
}  // namespace Core

namespace Calibration {

/**
Used to simulate a rig's camera.
*/
class VS_EXPORT Camera {
 public:
  Camera();
  virtual ~Camera();
  virtual Camera* clone() const = 0;

  /**
  @brief Setup camera instance using parameters from a camera rigcamera definition.
  @note These parameters will be transformed to optimize computations
  @param rigcamdef input camera definition
  */
  void setupWithRigCameraDefinition(Core::RigCameraDefinition& rigcamdef);

  /**
  @brief Effectively lift a point on the sphere of radius sphereScale in the reference frame
  @param refpt the result lifted point
  @param Jhfocal jacobian of the function wrt horizontal focal
  @param Jvfocal jacobian of the function wrt vertical focal
  @param Jhcenter jacobian of the function wrt horizontal center
  @param Jvcenter jacobian of the function wrt vertical center
  @param JdistortA jacobian of the function wrt distortion param A
  @param JdistortB jacobian of the function wrt distortion param B
  @param JdistortC jacobian of the function wrt distortion param C
  @param Jrotation jacobian of the function wrt rotation params
  @param JtX jacobian of the function wrt translation X param
  @param JtY jacobian of the function wrt translation Y param
  @param JtZ jacobian of the function wrt translation Z param
  @param impt the input image point
  @param sphereScale the sphere radius in the reference frame
  @return false if some numerical problem happened
  */
  bool lift(Eigen::Vector3d& refpt, Eigen::Matrix<double, 3, 4>& Jhfocal, Eigen::Matrix<double, 3, 4>& Jvfocal,
            Eigen::Matrix<double, 3, 4>& Jhcenter, Eigen::Matrix<double, 3, 4>& Jvcenter,
            Eigen::Matrix<double, 3, 4>& JdistortA, Eigen::Matrix<double, 3, 4>& JdistortB,
            Eigen::Matrix<double, 3, 4>& JdistortC, Eigen::Matrix<double, 3, 9>& Jrotation,
            Eigen::Matrix<double, 3, 4>& JtX, Eigen::Matrix<double, 3, 4>& JtY, Eigen::Matrix<double, 3, 4>& JtZ,
            const Eigen::Vector2d& impt, const double sphereScale);

  /**
   @brief Effectively lift a point on the sphere of radius sphereScale in the reference frame
   @param refpt the result lifted point
   @param impt the input image point
   @param sphereScale the sphere radius in the reference frame
   @return false if some numerical problem happened
   */
  bool lift(Eigen::Vector3d& refpt, const Eigen::Vector2d& impt, const double sphereScale);

  /**
  @brief Effectively lift a point on the unit sphere in the camera frame
  @param campt the result lifted point
  @param impt the input image point
  @return false if some numerical problem happened
  */
  bool quicklift(Eigen::Vector3d& campt, const Eigen::Vector2d& impt);

  /**
  @brief Project a point on the unit sphere in the reference space into the camera plane
  @param impt_pixels the result projected point
  @param Jpoint jacobian of the function wrt source point
  @param Jhfocal jacobian of the function wrt horizontal focal
  @param Jvfocal jacobian of the function wrt vertical focal
  @param Jhcenter jacobian of the function wrt horizontal center
  @param Jvcenter jacobian of the function wrt vertical center
  @param JdistortA jacobian of the function wrt lens distortion param A
  @param JdistortB jacobian of the function wrt lens distortion param B
  @param JdistortC jacobian of the function wrt lens distortion param C
  @param Jrotation jacobian of the function wrt rotation params
  @param JtX jacobian of the function wrt translation X param
  @param JtY jacobian of the function wrt translation Y param
  @param JtZ jacobian of the function wrt translation Z param
  @param refpt the input point in the reference space
  @return false if some numerical problem happened
  */
  bool project(Eigen::Vector2d& impt_pixels, Eigen::Matrix<double, 2, 3>& Jpoint, Eigen::Matrix<double, 2, 4>& Jhfocal,
               Eigen::Matrix<double, 2, 4>& Jvfocal, Eigen::Matrix<double, 2, 4>& Jhcenter,
               Eigen::Matrix<double, 2, 4>& Jvcenter, Eigen::Matrix<double, 2, 4>& JdistortA,
               Eigen::Matrix<double, 2, 4>& JdistortB, Eigen::Matrix<double, 2, 4>& JdistortC,
               Eigen::Matrix<double, 2, 9>& Jrotation, Eigen::Matrix<double, 2, 4>& JtX,
               Eigen::Matrix<double, 2, 4>& JtY, Eigen::Matrix<double, 2, 4>& JtZ, const Eigen::Vector3d& refpt);

  /**
   @brief Project a point on the unit sphere in the reference space into the camera plane
   @param impt_pixels the result projected point
   @param refpt the input point in the reference space
   @return false if some numerical problem happened
   */
  bool project(Eigen::Vector2d& impt_pixels, const Eigen::Vector3d& refpt);

  /**
   @brief Project a point on the unit sphere in the camera space into the camera plane
   @param impt_pixels the result projected point
   @param refpt the input point in the reference space
   @return false if some numerical problem happened
   */
  bool quickproject(Eigen::Vector2d& impt_pixels, const Eigen::Vector3d& campt);

  /**
  @brief Get point lift mean and covariance matrix
  @param mean the lifted point
  @param hfocal_variance the variance of horizontal focal parameter
  @param vfocal_variance the variance of vertical focal parameter
  @param hcenter_variance the variance of horizontal center parameter
  @param vcenter_variance the variance of vertical center parameter
  @param distorta_variance the variance of distortion A parameter
  @param distortb_variance the variance of distortion B parameter
  @param distortc_variance the variance of distortion C parameter
  @param tx_variance the variance of translation X parameter
  @param ty_variance the variance of translation Y parameter
  @param tz_variance the variance of translation Z parameter
  @param impt the original 2d point to lift
  @param sphereScale the sphere radius in the reference frame
  @return false if an error occured
  */
  bool getLiftCovariance(Eigen::Vector3d& mean, Eigen::Matrix3d& cov, const double hfocal_variance,
                         const double vfocal_variance, const double hcenter_variance, const double vcenter_variance,
                         const double distorta_variance, const double distortb_variance, const double distortc_variance,
                         const double tx_variance, const double ty_variance, const double tz_variance,
                         const Eigen::Vector2d& impt, const double sphereScale);

  /**
  @brief Get point projection mean and covariance matrix
  @param mean the lifted point
  @param hfocal_variance the variance of horizontal focal parameter
  @param vfocal_variance the variance of vertical focal parameter
  @param hcenter_variance the variance of horizontal center parameter
  @param vcenter_variance the variance of vertical center parameter
  @param distorta_variance the variance of distortion A parameter
  @param distortb_variance the variance of distortion B parameter
  @param distortc_variance the variance of distortion C parameter
  @param tx_variance the variance of translation X parameter
  @param ty_variance the variance of translation Y parameter
  @param tz_variance the variance of translation Z parameter
  @param refpt the original 3d point to project
  @param refpt_cov the original 2d point covariance to project
  @return false if an error occured
  */
  bool getProjectionCovariance(Eigen::Vector2d& mean, Eigen::Matrix2d& cov, const double hfocal_variance,
                               const double vfocal_variance, const double hcenter_variance,
                               const double vcenter_variance, const double distorta_variance,
                               const double distortb_variance, const double distortc_variance, const double tx_variance,
                               const double ty_variance, const double tz_variance, const Eigen::Vector3d& refpt,
                               const Eigen::Matrix3d& refpt_cov);

  /**
  @brief Returns whether the horizontal focal is constant
  */
  bool isHorizontalFocalConstant() const;

  /**
  @brief Get a pointer on horizontal focal parameters vector
  @return a pointer to double vector (4 elements)
  */
  double* getHorizontalFocalPtr();

  /**
  @brief Set lens horizontal focal vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setHorizontalFocal(const double* ptr);

  /**
  @brief Returns whether the vertical focal is constant
  */
  bool isVerticalFocalConstant() const;

  /**
  @brief Get a pointer on horizontal focal parameters vector
  @return a pointer to double vector (4 elements)
  */
  double* getVerticalFocalPtr();

  /**
  @brief Set lens vertical focal vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setVerticalFocal(const double* ptr);

  /**
  @brief Returns whether the horizontal center is constant
  */
  bool isHorizontalCenterConstant() const;

  /**
  @brief Get a pointer on horizontal center parameters vector
  @return a pointer to double vector (4 elements)
  */
  double* getHorizontalCenterPtr();

  /**
  @brief Set lens horizontal center vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setHorizontalCenter(const double* ptr);

  /**
  @brief Returns whether the vertical center is constant
  */
  bool isVerticalCenterConstant() const;

  /**
  @brief Get a pointer on horizontal center parameters vector
  @return a pointer to double vector (4 elements)
  */
  double* getVerticalCenterPtr();

  /**
  @brief Set lens vertical center vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setVerticalCenter(const double* ptr);

  /**
  @brief Returns whether the First distortion parameter is constant
  */
  bool isDistortionAConstant() const;

  /**
  @brief Get a pointer on First distortion parameter parameters vector
  @return a pointer to double vector (4 elements)
  */
  double* getDistortionAPtr();

  /**
  @brief Set First distortion parameter vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setDistortionA(const double* ptr);

  /**
  @brief Returns whether the Second distortion parameter is constant
  */
  bool isDistortionBConstant() const;

  /**
  @brief Get a pointer on Second distortion parameter parameters vector
  @return a pointer to double vector (4 elements)
  */
  double* getDistortionBPtr();

  /**
  @brief Set Second distortion parameter vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setDistortionB(const double* ptr);

  /**
  @brief Returns whether the Third distortion parameter is constant
  */
  bool isDistortionCConstant() const;

  /**
  @brief Get a pointer on Third distortion parameter parameters vector
  @return a pointer to double vector (4 elements)
  */
  double* getDistortionCPtr();

  /**
  @brief Set Third distortion parameter vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setDistortionC(const double* ptr);

  /**
  @brief Returns whether the camera rotation is constant
  */
  bool isRotationConstant() const;

  /**
  @brief Get a pointer on camera rotation vector
  @return a pointer to double vector (9 elements)
  */
  double* getRotationPtr();

  /**
  @brief Set rotation vector from input vector pointer
  @param ptr pointer to the input data
  */
  void setRotation(const double* ptr);

  /**
  @brief Get rotation matrix
  @return 3x3 matrix
  */
  Eigen::Matrix3d getRotation() const;

  /**
  @brief Set rotation matrix directly
  @param R the input matrix to copy
  */
  void setRotationMatrix(const Eigen::Matrix3d& R);

  /**
  @brief Set rotation matrix from the presets
  */
  void setRotationFromPresets();

  /**
   @brief Checks if given rotation is within presets
   @param R the input matrix to check
   @return boolean if rotation is within presets
   */
  bool isRotationWithinPresets(const Eigen::Matrix3d& R) const;

  /**
   @brief Get reference pose matrix from presets
   @return 3x3 matrix
   */
  Eigen::Matrix3d getRotationFromPresets() const;

  /**
   @brief Returns whether the camera translation X is constant
   */
  bool isTranslationXConstant() const;

  /**
   @brief Get a pointer camera translation X component
   @return a pointer to double
   */
  double* getTranslationXPtr();

  /**
   @brief Set camera translation X from input vector pointer
   @param ptr pointer to the input data
   */
  void setTranslationX(const double* ptr);

  /**
   @brief Returns whether the camera translation Y is constant
   */
  bool isTranslationYConstant() const;

  /**
   @brief Get a pointer camera translation Y component
   @return a pointer to double
   */
  double* getTranslationYPtr();

  /**
   @brief Set camera translation Y from input vector pointer
   @param ptr pointer to the input data
   */
  void setTranslationY(const double* ptr);

  /**
   @brief Returns whether the camera translation Z is constant
   */
  bool isTranslationZConstant() const;

  /**
   @brief Get a pointer camera translation Z component
   @return a pointer to double
   */
  double* getTranslationZPtr();

  /**
   @brief Set camera translation Z from input vector pointer
   @param ptr pointer to the input data
   */
  void setTranslationZ(const double* ptr);

  /**
   @brief Get camera translation in a single output vector
   @return camera translation vector
   */
  Eigen::Vector3d getTranslation() const;

  /**
   @brief Set camera translations from input vector
   @param translation input translation vector
   */
  void setTranslation(const Eigen::Vector3d& translation);

  /**
  @brief Fill Geometry
  @param geometry the to update geometry
  @param width width of the input image
  @param height height of the input image
  */
  void fillGeometry(Core::GeometryDefinition& geometry, int width, int height);

  /**
  @brief Get Image width
  @return width of captured image
  */
  size_t getWidth();

  /**
  @brief Get Image height
  @return height of captured image
  */
  size_t getHeight();

  /**
   @brief Get the rotation covariance matrix
   @param cov the estimated covariance
   */
  void getRotationCovarianceMatrix(Eigen::Matrix<double, 9, 9>& cov) const;

  /**
   @brief Get the yaw/pitch/roll covariance matrix
   @param cov the covariance
   */
  void getYawPitchRollCovarianceMatrix(Eigen::Matrix<double, 3, 3>& cov) const;

  /**
  @brief Get relative rotation
  @param second_Rmean_first relative rotation mean between 2 cameras
  @param second_axisAngleRCov_first relative rotation variance between 2 cameras in axis-angle representation
  @param first the first camera
  @param second the second camera
  */
  static void getRelativeRotation(Eigen::Matrix3d& second_Rmean_first, Eigen::Matrix3d& second_axisAngleRCov_first,
                                  const Camera& first, const Camera& second);

  /*
  @brief Get Format for this camera
  @return format for this camera class
  */
  Core::InputDefinition::Format getFormat();

  /**
  @brief Set Format for this camera
  @param format this camera format
  */
  void setFormat(Core::InputDefinition::Format format);

  /**
  @brief Ties the focal to the focal of another camera
  @param other camera to have the focal tied to
  */
  void tieFocalTo(const Camera& other);

  /**
  @brief Untie the focal values (i.e. will have its own values independent from other objects)
  */
  void untieFocal();

 protected:
  /**
  @brief Effectively lift a point on the sphere of radius sphereScale in the reference frame
  @param refpt the result lifted point
  @param Jhfocal jacobian of the function wrt horizontal focal
  @param Jvfocal jacobian of the function wrt vertical focal
  @param Jhcenter jacobian of the function wrt horizontal center
  @param Jvcenter jacobian of the function wrt vertical center
  @param JdistortA jacobian of the function wrt distortion param A
  @param JdistortB jacobian of the function wrt distortion param B
  @param JdistortC jacobian of the function wrt distortion param C
  @param Jrotation jacobian of the function wrt lens distortion params
  @param Jpose jacobian of the function wrt pose params
  @param impt the input image point
  @param sphereScale the sphere radius in the reference frame
  @return false if some numerical problem happened
  */
  bool lift_unbounded(Eigen::Vector3d& refpt, Eigen::Matrix<double, 3, 1>& Jhfocal,
                      Eigen::Matrix<double, 3, 1>& Jvfocal, Eigen::Matrix<double, 3, 1>& Jhcenter,
                      Eigen::Matrix<double, 3, 1>& Jvcenter, Eigen::Matrix<double, 3, 1>& JdistortA,
                      Eigen::Matrix<double, 3, 1>& JdistortB, Eigen::Matrix<double, 3, 1>& JdistortC,
                      Eigen::Matrix<double, 3, 9>& Jrotation, Eigen::Matrix<double, 3, 1>& JtranslationX,
                      Eigen::Matrix<double, 3, 1>& JtranslationY, Eigen::Matrix<double, 3, 1>& JtranslationZ,
                      const Eigen::Vector2d& impt, const double sphereScale);

  /**
  @brief Project a point on the unit sphere in the reference space into the camera plane
  @param impt the result projected point
  @param Jpoint jacobian of the function wrt source point
  @param Jhfocal jacobian of the function wrt horizontal focal
  @param Jvfocal jacobian of the function wrt vertical focal
  @param Jhcenter jacobian of the function wrt horizontal center
  @param Jvcenter jacobian of the function wrt vertical center
  @param JdistortA jacobian of the function wrt lens distortion param A
  @param JdistortB jacobian of the function wrt lens distortion param B
  @param JdistortC jacobian of the function wrt lens distortion param C
  @param Jrotation jacobian of the function wrt pose params
  @param refpt the input point in the reference space
  @return false if some numerical problem happened
  */
  bool project_unbounded(Eigen::Vector2d& impt_pixels, Eigen::Matrix<double, 2, 3>& Jpoint,
                         Eigen::Matrix<double, 2, 1>& Jhfocal, Eigen::Matrix<double, 2, 1>& Jvfocal,
                         Eigen::Matrix<double, 2, 1>& Jhcenter, Eigen::Matrix<double, 2, 1>& Jvcenter,
                         Eigen::Matrix<double, 2, 1>& JdistortA, Eigen::Matrix<double, 2, 1>& JdistortB,
                         Eigen::Matrix<double, 2, 1>& JdistortC, Eigen::Matrix<double, 2, 9>& Jrotation,
                         Eigen::Matrix<double, 2, 1>& JtranslationX, Eigen::Matrix<double, 2, 1>& JtranslationY,
                         Eigen::Matrix<double, 2, 1>& JtranslationZ, const Eigen::Vector3d& refpt);

  /**
  @brief Backproject from image plane to unit sphere space
  @param campt 3d point on the camera frame's unit sphere
  @param jacobian the transformation jacobian
  @param impt the input image coordinates (in meters)
  @return false if some numerical problem happened
  */
  virtual bool backproject(Eigen::Vector3d& campt, Eigen::Matrix<double, 3, 2>& jacobian,
                           const Eigen::Vector2d& impt) = 0;

  /**
  @brief Project from unit sphere space to image plane in meters
  @param impt_meters projected point in meters
  @param jacobian the transformation jacobian
  @param campt the input 3d coordinates (in camera space)
  @return false if some numerical problem happened
  */
  virtual bool project(Eigen::Vector2d& impt_meters, Eigen::Matrix<double, 2, 3>& jacobian,
                       const Eigen::Vector3d& campt) = 0;

  /**
  @brief Undistort a point
  @param impt_undistorted output undistorted
  @param impt_distorted input image coordinates
  */
  bool undistort(Eigen::Vector2d& impt_undistorted, const Eigen::Vector2d& impt_distorted);

  /**
  @brief Undistort a point
  @param impt_undistorted output undistorted
  @param Jundistortwrtdistorted jacobian of undistorted wrt distorted coordinates
  @param JparameterA jacobian of undistorted wrt A
  @param JparameterB jacobian of undistorted wrt B
  @param JparameterC jacobian of undistorted wrt C
  @param impt_distorted input image coordinates
  */
  bool undistort(Eigen::Vector2d& impt_undistorted, Eigen::Matrix<double, 2, 2>& Jundistortwrtdistorted,
                 Eigen::Matrix<double, 2, 1>& JparameterA, Eigen::Matrix<double, 2, 1>& JparameterB,
                 Eigen::Matrix<double, 2, 1>& JparameterC, const Eigen::Vector2d& impt_distorted);

  /**
  @brief Undistort a point
  @param impt_distorted output undistorted
  @param impt_undistorted input image coordinates
  */
  bool distort(Eigen::Vector2d& impt_distorted, const Eigen::Vector2d& impt_undistorted);

  /**
  @brief Undistort a point
  @param impt_distorted output undistorted
  @param Jdistortedwrtundistorted jacobian of undistorted wrt distorted coordinates
  @param JparameterA jacobian of undistorted wrt A
  @param JparameterB jacobian of undistorted wrt B
  @param JparameterC jacobian of undistorted wrt C
  @param impt_undistorted input image coordinates
  */
  bool distort(Eigen::Vector2d& impt_distorted, Eigen::Matrix<double, 2, 2>& Jdistortedwrtundistorted,
               Eigen::Matrix<double, 2, 1>& JparameterA, Eigen::Matrix<double, 2, 1>& JparameterB,
               Eigen::Matrix<double, 2, 1>& JparameterC, const Eigen::Vector2d& impt_undistorted);

 protected:
  /*
  WARNING: if you add a member here, do not forget to update the clone() method in all derived classes
  */
  BoundedValue horizontal_focal;
  BoundedValue vertical_focal;
  BoundedValue horizontal_center;
  BoundedValue vertical_center;
  BoundedValue distort_A;
  BoundedValue distort_B;
  BoundedValue distort_C;
  BoundedValue t_x;
  BoundedValue t_y;
  BoundedValue t_z;

  Eigen::Vector3d yprReference;
  Eigen::Matrix<double, 3, 3> yprCovariance;
  Eigen::Matrix<double, 3, 3> cameraRreference;
  Eigen::Matrix<double, 9, 9> cameraRreference_covariance;
  std::array<double, 9> cameraR_data;
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > cameraR;

  Core::InputDefinition::Format format;
  size_t width;
  size_t height;
};

}  // namespace Calibration
}  // namespace VideoStitch


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "robert/lapamg.h"

using namespace Eigen;
using namespace std;

LaplacianAMGStuff* S;
int expect_cycle;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex;

    double *nnz, *dof, *rows, *cols, *vals, *msg, *rhs;

    double *output;

    nnz = mxGetPr(prhs[0]);
    dof = mxGetPr(prhs[1]);
    rows = mxGetPr(prhs[2]);
    cols = mxGetPr(prhs[3]);
    vals = mxGetPr(prhs[4]);
    msg = mxGetPr(prhs[5]);
    rhs = mxGetPr(prhs[6]);

    int nnz_num = nnz[0];
    int dof_num = dof[0];
    int status = msg[0];

    output_mex = plhs[0] = mxCreateDoubleMatrix(dof_num, 1, mxREAL);

    output = mxGetPr(output_mex);

    if(status == 0)
    {
        int* rowbegin = (int*)malloc((dof_num + 1) * sizeof(int));
        int* column = (int*)malloc(nnz_num * sizeof(int));
        double* value = vals;
        int nmin = 1000;

        for(int i = 0; i < nnz_num; i++)
        {
            column[i] = cols[i] - 1;
            rows[i]--;
        }

        ///////////////////////////////////////////////////////////////

        FILE* file_a = fopen("matlab.txt", "wt");

        for(int i = 0; i < nnz_num; i++)
        {
            int tr = rows[i];
            int tc = cols[i] - 1;
            fprintf(file_a, "%d %d %f\n", tr, tc, vals[i]);

        }

        fclose(file_a);

        ///////////////////////////////////////////////////////////////

        rowbegin[0] = 0;

        int index = 0;
        int count = 0;

        for(int i = 0; i < nnz_num; i++)
        {
            if(rows[i] - index > 0.5)
            {
                index++;
                rowbegin[index] = rowbegin[index - 1] + count;
                count = 0;
            }

            count++;
        }

        rowbegin[dof_num] = nnz_num;

        ///////////////////////////////////////////////////////////////

        FILE* file_b = fopen("csr.txt", "wt");

        for(int i = 0; i < dof_num + 1; i++)
        {

            fprintf(file_b, "%d ", rowbegin[i]);

        }

        fprintf(file_b, "\n\n");

        for(int i = 0; i < nnz_num; i++)
        {

            fprintf(file_b, "%d ", column[i]);

        }

        fprintf(file_b, "\n\n");

        for(int i = 0; i < nnz_num; i++)
        {

            fprintf(file_b, "%f ", value[i]);

        }

        fclose(file_b);

        ///////////////////////////////////////////////////////////////

        S = prepareLaplacianAMG(dof_num, rowbegin, column, value, nmin);

        expect_cycle = solveLaplacianAMG(S, rhs, output, 0, 100, 1e-8);

        printf("iteration %d\n", expect_cycle);

        free(rowbegin);
        free(column);

    }else{

        solveLaplacianAMG(S, rhs, output, expect_cycle, expect_cycle, 1e-8);
    }

    return;

}