/*
    create by user  2021/05/17
*/
# include <mylib/planar.h>
# include <mylib/commen_include.h>
/*
#include <iostream>
#include <fstream>
#include <memory>
#include <thread>
#include <vector>
#include <map>
#include <string>
#include <unordered_map>
#include <condition_variable>
#include <atomic>
#include <set>
#include <list>
#include <random>
#include <unistd.h>
#include <mutex>
// eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
//typedef Eigen::Matrix<double, 3, 1> Vec3;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > VecVector3d;
*/
int main(int argc, char** argv)
{
    std::cout<<"Hello SLAM!"<<std::endl;
    Plane plane1;
    plane1.createPlanePoints();
    Eigen::Vector3d norm;
    norm << 0,0,9;
    double dis = 4;
    Eigen::Vector4d homo_para;
    Eigen::Quaterniond quat_;
    plane1.HesseToHomogenous(norm,dis,homo_para);
    quat_ = plane1.HesseToQuaternion(norm,dis);
    std::cout << "homogeneous representation is "<<homo_para << std::endl;
    std::cout << "quat representation is "<< quat_ << std::endl;
    norm = plane1.SpherToHesse(0.4,0.5,4);
    std::cout << norm << std::endl;
    return 0;
}

