#include "locate/ceres_scan_match.h"
#include<ceres/ceres.h>

using namespace std;
using namespace cv;

//主函数
int main(int argc, char** argv)
{
  ros::init(argc, argv, "ceres_scan_matcher");
  Eigen::Vector3d origin_trans(0,0,0);
  if(argc >= 4){
    origin_trans << stod(argv[1]), stod(argv[2]), stod(argv[3]);
  }
  Ceres_scan_match sm("/home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/201113map_origin.yaml", origin_trans);
  // sm.Solve();

  ros::Rate loop_rate(100);
  while(ros::ok()){

    // sm.Solve();
    // std::cout << trans << std::endl;
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

