text:
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <ctime>
#include <algorithm>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <ros/ros.h>
#include <ros/package.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"

#include <object_slam/Object_landmark.h>
#include <object_slam/Frame.h>
#include <object_slam/g2o_Object.h>

#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/detect_3d_cuboid.h"

#include "line_lbd/line_lbd_allclass.h"

using namespace std;
using namespace Eigen;

typedef pcl::PointCloud<pcl::PointXYZRGB> CloudXYZRGB;

#define CV_LOAD_IMAGE_ANYDEPTH cv::IMREAD_ANYDEPTH

// global variable
std::string base_folder;
bool online_detect_mode;
bool save_results_to_txt;
cv::Mat_<float> matx_to3d_, maty_to3d_;

// for converting depth image to point cloud.
void set_up_calibration(const Eigen::Matrix3f& calibration_mat,const int im_height,const int im_width)
{
    matx_to3d_.create(im_height, im_width);
    maty_to3d_.create(im_height, im_width);
    float center_x=calibration_mat(0,2);  //cx
    float center_y=calibration_mat(1,2);  //cy
    float fx_inv=1.0/calibration_mat(0,0);  // 1/fx
    float fy_inv=1.0/calibration_mat(1,1);  // 1/fy
    for (int v = 0; v < im_height; v++) {
	for (int u = 0; u < im_width; u++) {
	  matx_to3d_(v,u) = (u - center_x) * fx_inv;
	  maty_to3d_(v,u) = (v - center_y) * fy_inv;
	}
    }
}

// depth img is already in m unit.
void depth_to_cloud(const cv::Mat& rgb_img, const cv::Mat& depth_img,const Eigen::Matrix4f transToWorld, CloudXYZRGB::Ptr& point_cloud,bool downsample=false)
{
    pcl::PointXYZRGB pt;
    pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> vox_grid_;
    float close_depth_thre = 0.1;
    float far_depth_thre = 3.0;
      far_depth_thre = 3;
    int im_width = rgb_img.cols; int im_height= rgb_img.rows;
    for (int32_t i=0; i<im_width*im_height; i++) {      // row by row
	int ux=i % im_width; int uy=i / im_width;
	float pix_depth= depth_img.at<float>(uy,ux);
	if (pix_depth>close_depth_thre && pix_depth<far_depth_thre){
	      pt.z=pix_depth; pt.x=matx_to3d_(uy,ux)*pix_depth; pt.y=maty_to3d_(uy,ux)*pix_depth;
	      Eigen::VectorXf global_pt=homo_to_real_coord_vec<float>(transToWorld*Eigen::Vector4f(pt.x,pt.y,pt.z,1));  // change to global position
	      pt.x=global_pt(0); pt.y=global_pt(1); pt.z=global_pt(2);
	      pt.r = rgb_img.at<cv::Vec3b>(uy,ux)[2]; pt.g = rgb_img.at<cv::Vec3b>(uy,ux)[1]; pt.b = rgb_img.at<cv::Vec3b>(uy,ux)[0];
	      point_cloud->points.push_back(pt);
	}
    }
    if (downsample)
    {
	vox_grid_.setLeafSize(0.02,0.02,0.02);
	vox_grid_.setDownsampleAllData(true);
	vox_grid_.setInputCloud(point_cloud);
	vox_grid_.filter(*point_cloud);
    }
}

// one cuboid need front and back markers...
void cuboid_corner_to_marker(const Matrix38d& cube_corners,visualization_msgs::Marker& marker, int bodyOrfront)
{
    Eigen::VectorXd edge_pt_ids;
    if (bodyOrfront==0) { // body edges
	edge_pt_ids.resize(16); edge_pt_ids<<1,2,3,4,1,5,6,7,8,5,6,2,3,7,8,4;edge_pt_ids.array()-=1;
    }else { // front face edges
	edge_pt_ids.resize(5); edge_pt_ids<<1,2,6,5,1;edge_pt_ids.array()-=1;
    }
    marker.points.resize(edge_pt_ids.rows());
    for (int pt_id=0; pt_id<edge_pt_ids.rows(); pt_id++)
    {
	marker.points[pt_id].x = cube_corners(0, edge_pt_ids(pt_id));
	marker.points[pt_id].y = cube_corners(1, edge_pt_ids(pt_id));
	marker.points[pt_id].z = cube_corners(2, edge_pt_ids(pt_id));
    }
}

// one cuboid need front and back markers...  rgbcolor is 0-1 based
visualization_msgs::MarkerArray cuboids_to_marker(object_landmark* obj_landmark, Vector3d rgbcolor)
{
    visualization_msgs::MarkerArray plane_markers;  visualization_msgs::Marker marker;
    if (obj_landmark==nullptr)
	return plane_markers;

    marker.header.frame_id="/world";  marker.header.stamp=ros::Time::now();
    marker.id = 0; //0
    marker.type = visualization_msgs::Marker::LINE_STRIP;   marker.action = visualization_msgs::Marker::ADD;
    marker.color.r = rgbcolor(0); marker.color.g = rgbcolor(1); marker.color.b = rgbcolor(2); marker.color.a = 1.0;
    marker.scale.x = 0.02;

    g2o::cuboid cube_opti = obj_landmark->cube_vertex->estimate();
    Eigen::MatrixXd cube_corners = cube_opti.compute3D_BoxCorner();

    for (int ii=0;ii<2;ii++) // each cuboid needs two markers!!! one for all edges, one for front facing edge, could with different color.
    {
	marker.id++;
	cuboid_corner_to_marker(cube_corners,marker, ii);
	plane_markers.markers.push_back(marker);
    }
    return plane_markers;
}

geometry_msgs::Pose posenode_to_geomsgs(const g2o::SE3Quat &pose_Twc)
{
    geometry_msgs::Pose pose_msg;
    Eigen::Vector3d pose_trans = pose_Twc.translation();
    pose_msg.position.x=pose_trans(0);
    pose_msg.position.y=pose_trans(1);
    pose_msg.position.z=pose_trans(2);
    Eigen::Quaterniond pose_quat = pose_Twc.rotation();
    pose_msg.orientation.x = pose_quat.x();
    pose_msg.orientation.y = pose_quat.y();
    pose_msg.orientation.z = pose_quat.z();
    pose_msg.orientation.w = pose_quat.w();
    return pose_msg;
}

nav_msgs::Odometry posenode_to_odommsgs(const g2o::SE3Quat &pose_Twc,const std_msgs::Header &img_header)
{
    nav_msgs::Odometry odom_msg;
    odom_msg.pose.pose=posenode_to_geomsgs(pose_Twc);
    odom_msg.header=img_header;
    return odom_msg;
}

// publish every frame's raw and optimized results
void publish_all_poses(std::vector<tracking_frame*> all_frames,std::vector<object_landmark*> cube_landmarks_history,
		       std::vector<object_landmark*> all_frame_rawcubes, Eigen::MatrixXd& truth_frame_poses)
{
    ros::NodeHandle n;
    ros::Publisher pub_slam_all_poses = n.advertise<geometry_msgs::PoseArray>("/slam_pose_array", 10);
    ros::Publisher pub_slam_odompose = n.advertise<nav_msgs::Odometry>("/slam_odom_pose", 10);
    ros::Publisher pub_truth_all_poses = n.advertise<geometry_msgs::PoseArray>("/truth_pose_array", 10);
    ros::Publisher pub_truth_odompose = n.advertise<nav_msgs::Odometry>("/truth_odom_pose", 10);
    ros::Publisher pub_slam_path = n.advertise<nav_msgs::Path>( "/slam_pose_paths", 10 );
    ros::Publisher pub_truth_path = n.advertise<nav_msgs::Path>( "/truth_pose_paths", 10 );
    ros::Publisher pub_final_opti_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_opti", 10); //final unique cube pose
    ros::Publisher pub_history_opti_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_opti_hist", 10); // landmark cube pose after each optimization
    ros::Publisher pub_frame_raw_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_raw_frame", 10);
    ros::Publisher pub_2d_cuboid_project = n.advertise<sensor_msgs::Image>("/cuboid_project_img", 10);
    ros::Publisher raw_cloud_pub = n.advertise<CloudXYZRGB> ("/raw_point_cloud", 50);

    int total_frame_number = all_frames.size();

    // prepare all paths messages.
    geometry_msgs::PoseArray all_pred_pose_array;    std::vector<nav_msgs::Odometry> all_pred_pose_odoms;
    geometry_msgs::PoseArray all_truth_pose_array;    std::vector<nav_msgs::Odometry> all_truth_pose_odoms;
    nav_msgs::Path path_truths,path_preds;
    std_msgs::Header pose_header;    pose_header.frame_id = "/world";    pose_header.stamp = ros::Time::now();
    path_preds.header = pose_header;    path_truths.header = pose_header;
    for (int i = 0; i < total_frame_number; i++)
    {
	all_pred_pose_array.poses.push_back(posenode_to_geomsgs(all_frames[i]->cam_pose_Twc));
	all_pred_pose_odoms.push_back(posenode_to_odommsgs(all_frames[i]->cam_pose_Twc,pose_header) );

	geometry_msgs::PoseStamped postamp;
	postamp.pose = posenode_to_geomsgs(all_frames[i]->cam_pose_Twc);
	postamp.header = pose_header;
	path_preds.poses.push_back(postamp);
    }
    if (truth_frame_poses.rows()>0)
    {
	for (int i=0; i < total_frame_number;i++)
	{
	    geometry_msgs::Pose pose_msg;
	    pose_msg.position.x=truth_frame_poses(i,1);    pose_msg.position.y=truth_frame_poses(i,2);    pose_msg.position.z=truth_frame_poses(i,3);
	    pose_msg.orientation.x = truth_frame_poses(i,4);	pose_msg.orientation.y = truth_frame_poses(i,5);
	    pose_msg.orientation.z = truth_frame_poses(i,6);	pose_msg.orientation.w = truth_frame_poses(i,7);
	    all_truth_pose_array.poses.push_back(pose_msg);
	    nav_msgs::Odometry odom_msg;odom_msg.pose.pose=pose_msg;
	    odom_msg.header = pose_header;
	    all_truth_pose_odoms.push_back(odom_msg);

	    geometry_msgs::PoseStamped postamp;
	    postamp.pose = pose_msg;
	    postamp.header = pose_header;
	    path_truths.poses.push_back(postamp);
	}
    }
    all_pred_pose_array.header.stamp=ros::Time::now();    all_pred_pose_array.header.frame_id="/world";
    all_truth_pose_array.header.stamp=ros::Time::now();    all_truth_pose_array.header.frame_id="/world";

    if (save_results_to_txt)  // record cam pose and object pose
    {
	ofstream resultsFile;
	string resultsPath = base_folder + "output_cam_poses.txt";
	cout<<"resultsPath  "<<resultsPath<<endl;
	resultsFile.open(resultsPath.c_str());
	resultsFile << "# timestamp tx ty tz qx qy qz qw"<<"\n";
	for (int i=0;i<total_frame_number;i++)
	{
	    double time_string=truth_frame_poses(i,0);
	    ros::Time time_img(time_string);
	    resultsFile << time_img<<"  ";
	    resultsFile << all_frames[i]->cam_pose_Twc.toVector().transpose()<<"\n";
	}
	resultsFile.close();

	ofstream objresultsFile;
	string objresultsPath = base_folder + "output_obj_poses.txt";
	objresultsFile.open(objresultsPath.c_str());
	for (size_t j=0;j<cube_landmarks_history.size();j++)
	{
	    g2o::cuboid cube_opti = cube_landmarks_history[j]->cube_vertex->estimate();
	    // transform it to local ground plane.... suitable for matlab processing.
	    objresultsFile << cube_opti.toMinimalVector().transpose()<<" "<<"\n";
	}
	objresultsFile.close();
    }

    // sensor parameter for TUM cabinet data!
    Eigen::Matrix3f calib;
    float depth_map_scaling = 5000;
    calib<<535.4,  0, 320.1,
	    0,  539.2, 247.6,
	    0,      0,     1;
    set_up_calibration(calib,480,640);

    visualization_msgs::MarkerArray finalcube_markers = cuboids_to_marker(cube_landmarks_history.back(),Vector3d(0,1,0));

    bool show_truth_cloud = true;  // show point cloud using camera pose. for visualization purpose

    pcl::PCLPointCloud2 pcd_cloud2;

    ros::Rate loop_rate(5);  //5
    int frame_number = -1;
    while ( n.ok() )
    {
	frame_number++;

	if (0) // directly show final results
	{
	    pub_slam_all_poses.publish(all_pred_pose_array);	pub_truth_all_poses.publish(all_truth_pose_array);
	    pub_slam_path.publish(path_preds);	pub_truth_path.publish(path_truths);
	}
	pub_final_opti_cube.publish(finalcube_markers);

	if (frame_number<total_frame_number)
	{
	    // publish cuboid landmarks, after each frame's g2o optimization
	    if (cube_landmarks_history[frame_number]!=nullptr)
		pub_history_opti_cube.publish(cuboids_to_marker(cube_landmarks_history[frame_number],Vector3d(1,0,0)));

	    // publish raw detected cube in each frame, before optimization
	    if (all_frame_rawcubes.size()>0 && all_frame_rawcubes[frame_number]!=nullptr)
		pub_frame_raw_cube.publish(cuboids_to_marker(all_frame_rawcubes[frame_number],Vector3d(0,0,1)));

	    // publish camera pose estimation of this frame
	    pub_slam_odompose.publish(all_pred_pose_odoms[frame_number]);
	    pub_truth_odompose.publish(all_truth_pose_odoms[frame_number]);

// 	    std::cout<<"Frame position x/y:   "<<frame_number<<"        "<<all_pred_pose_odoms[frame_number].pose.pose.position.x<<"  "<<
// 			  all_pred_pose_odoms[frame_number].pose.pose.position.y <<std::endl;

	    char frame_index_c[256];	sprintf(frame_index_c,"%04d",frame_number);  // format into 4 digit

	    cv::Mat cuboid_2d_proj_img = all_frames[frame_number]->cuboids_2d_img;

	    std::string raw_rgb_img_name = base_folder+"raw_imgs/" + std::string(frame_index_c) + "_rgb_raw.jpg";
	    cv::Mat raw_rgb_img = cv::imread(raw_rgb_img_name, 1);

	    if (show_truth_cloud && (truth_frame_poses.rows()>0))
		if (frame_number%2==0) // show point cloud every N frames
		{
		    std::string raw_depth_img_name = base_folder+"depth_imgs/" + std::string(frame_index_c) + "_depth_raw.png";
		    cv::Mat raw_depth_img = cv::imread(raw_depth_img_name, CV_LOAD_IMAGE_ANYDEPTH);
		    raw_depth_img.convertTo(raw_depth_img, CV_32FC1, 1.0/depth_map_scaling,0);

		    CloudXYZRGB::Ptr point_cloud(new CloudXYZRGB());
		    Eigen::Matrix4f truth_pose_matrix=g2o::SE3Quat(truth_frame_poses.row(frame_number).segment<7>(1)).to_homogeneous_matrix().cast<float>();

		    depth_to_cloud(raw_rgb_img, raw_depth_img, truth_pose_matrix, point_cloud, true); // need to downsample cloud, otherwise too many
		    ros::Time curr_time=ros::Time::now();

		    point_cloud->header.frame_id = "/world";
		    point_cloud->header.stamp = (curr_time.toNSec() / 1000ull);
		    raw_cloud_pub.publish(point_cloud);
		}

	    cv_bridge::CvImage out_image;
	    out_image.header.stamp=ros::Time::now();
	    out_image.image=cuboid_2d_proj_img;
	    out_image.encoding=sensor_msgs::image_encodings::TYPE_8UC3;
	    pub_2d_cuboid_project.publish(out_image.toImageMsg());
	}

	if (frame_number==int(all_pred_pose_odoms.size()))
	{
	    cout<<"Finish all visulialization!"<<endl;
	}

	ros::spinOnce();
	loop_rate.sleep();
    }
}

//NOTE offline_pred_objects and init_frame_poses are not used in online_detect_mode! truth cam pose of first frame is used.
void incremental_build_graph(Eigen::MatrixXd& offline_pred_frame_objects, Eigen::MatrixXd& init_frame_poses, Eigen::MatrixXd& truth_frame_poses)
{
    Eigen::Matrix3d calib;
    calib<<535.4,  0,  320.1,   // for TUM cabinet data.
	    0,  539.2, 247.6,
	    0,      0,     1;

    int total_frame_number = truth_frame_poses.rows();

    // detect all frames' cuboids.
    detect_3d_cuboid detect_cuboid_obj;
    detect_cuboid_obj.whether_plot_detail_images = false;
    detect_cuboid_obj.whether_plot_final_images = false;
    detect_cuboid_obj.print_details = false;  // false  true
    detect_cuboid_obj.set_calibration(calib);
    detect_cuboid_obj.whether_sample_bbox_height = false;
    detect_cuboid_obj.nominal_skew_ratio = 2;
    detect_cuboid_obj.whether_save_final_images = true;

    line_lbd_detect line_lbd_obj;
    line_lbd_obj.use_LSD = true;
    line_lbd_obj.line_length_thres = 15;  // remove short edges

    // graph optimization.
    //NOTE in this example, there is only one object!!! perfect association
    g2o::SparseOptimizer graph;
    g2o::BlockSolverX::LinearSolverType* linearSolver;
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    graph.setAlgorithm(solver);    graph.setVerbose(false);

    // only first truth pose is used. to directly visually compare with truth pose. also provide good roll/pitch
    g2o::SE3Quat fixed_init_cam_pose_Twc(truth_frame_poses.row(0).tail<7>());

    // save optimization results of each frame
    std::vector<object_landmark*> cube_pose_opti_history(total_frame_number, nullptr);  //landmark pose after each frame's optimization
    std::vector<object_landmark*> cube_pose_raw_detected_history(total_frame_number, nullptr); //raw detected cuboid frame each frame. before optimization

    int offline_cube_obs_row_id = 0;

    std::vector<tracking_frame*> all_frames(total_frame_number);
    g2o::VertexCuboid* vCube;

    // process each frame online and incrementally
    for (int frame_index=0;frame_index<total_frame_number;frame_index++)
    {
	  g2o::SE3Quat curr_cam_pose_Twc;
	  g2o::SE3Quat odom_val; // from previous frame to current frame

	  if (frame_index==0)
		curr_cam_pose_Twc = fixed_init_cam_pose_Twc;
	  else
	  {
		g2o::SE3Quat prev_pose_Tcw = all_frames[frame_index-1]->cam_pose_Tcw;
		if (frame_index>1)  // from third frame, use constant motion model to initialize camera.
		{
		    g2o::SE3Quat prev_prev_pose_Tcw = all_frames[frame_index-2]->cam_pose_Tcw;
		    odom_val = prev_pose_Tcw*prev_prev_pose_Tcw.inverse();
		}
		curr_cam_pose_Twc = (odom_val*prev_pose_Tcw).inverse();
	  }

	  tracking_frame* currframe = new tracking_frame();
	  currframe->frame_seq_id = frame_index;
	  all_frames[frame_index] = currframe;

	  bool has_detected_cuboid = false;
	  g2o::cuboid cube_local_meas; double proposal_error;
	  char frame_index_c[256];	sprintf(frame_index_c,"%04d",frame_index);  // format into 4 digit

	  // read or detect cuboid
	  if (online_detect_mode)
	  {
	      //start detect cuboid
	      cv::Mat raw_rgb_img = cv::imread(base_folder+"raw_imgs/"+frame_index_c+"_rgb_raw.jpg", 1);

	      //edge detection
	      cv::Mat all_lines_mat;
	      line_lbd_obj.detect_filter_lines(raw_rgb_img, all_lines_mat);
	      Eigen::MatrixXd all_lines_raw(all_lines_mat.rows,4);
	      for (int rr=0;rr<all_lines_mat.rows;rr++)
		for (int cc=0;cc<4;cc++)
		  all_lines_raw(rr,cc) = all_lines_mat.at<float>(rr,cc);

	      //read cleaned yolo 2d object detection
	      Eigen::MatrixXd raw_2d_objs(10,5);  // 2d rect [x1 y1 width height], and prob
	      if (!read_all_number_txt(base_folder+"/filter_2d_obj_txts/"+frame_index_c+"_yolo2_0.15.txt", raw_2d_objs))
		  return;
	      raw_2d_objs.leftCols<2>().array() -=1;   // change matlab coordinate to c++, minus 1

	      Matrix4d transToWolrd;
	      detect_cuboid_obj.whether_sample_cam_roll_pitch = (frame_index!=0); // first frame doesn't need to sample cam pose. could also sample. doesn't matter much
	      if (detect_cuboid_obj.whether_sample_cam_roll_pitch) //sample around first frame's pose
		  transToWolrd = fixed_init_cam_pose_Twc.to_homogeneous_matrix();
	      else
		  transToWolrd = curr_cam_pose_Twc.to_homogeneous_matrix();

	      std::vector<ObjectSet> frames_cuboids; // each 2d bbox generates an ObjectSet, which is vector of sorted proposals
	      detect_cuboid_obj.detect_cuboid(raw_rgb_img,transToWolrd,raw_2d_objs,all_lines_raw, frames_cuboids);
	      currframe->cuboids_2d_img = detect_cuboid_obj.cuboids_2d_img;

	      has_detected_cuboid = frames_cuboids.size()>0 && frames_cuboids[0].size()>0;
	      if (has_detected_cuboid)  // prepare object measurement
	      {
		  cuboid* detected_cube = frames_cuboids[0][0];  // NOTE this is a simple dataset, only one landmark

		  g2o::cuboid cube_ground_value; //cuboid in the local ground frame.
		  Vector9d cube_pose;cube_pose<<detected_cube->pos(0),detected_cube->pos(1),detected_cube->pos(2),
			  0,0,detected_cube->rotY,detected_cube->scale(0),detected_cube->scale(1),detected_cube->scale(2);  // xyz roll pitch yaw scale
		  cube_ground_value.fromMinimalVector(cube_pose);
		  cube_local_meas = cube_ground_value.transform_to(curr_cam_pose_Twc); // measurement is in local camera frame

		  if (detect_cuboid_obj.whether_sample_cam_roll_pitch)  //if camera roll/pitch is sampled, transform to the correct camera frame.
		  {
		      Vector3d new_camera_eulers =  detect_cuboid_obj.cam_pose_raw.euler_angle;
		      new_camera_eulers(0) += detected_cube->camera_roll_delta; new_camera_eulers(1) += detected_cube->camera_pitch_delta;
		      Matrix3d rotation_new = euler_zyx_to_rot<double>(new_camera_eulers(0),new_camera_eulers(1),new_camera_eulers(2));
		      Vector3d trans = transToWolrd.col(3).head<3>();
		      g2o::SE3Quat curr_cam_pose_Twc_new(rotation_new,trans);
		      cube_local_meas = cube_ground_value.transform_to(curr_cam_pose_Twc_new);
		  }
		  proposal_error = detected_cube->normalized_error;
	      }
	  }
	  else
	  {
	      int cube_obs_frame_id = offline_pred_frame_objects(offline_cube_obs_row_id,0);
	      has_detected_cuboid = cube_obs_frame_id==frame_index;
	      if (has_detected_cuboid)  // prepare object measurement   not all frame has observation!!
	      {
		  VectorXd measure_data = offline_pred_frame_objects.row(offline_cube_obs_row_id);
		  g2o::cuboid cube_ground_value;
		  Vector9d cube_pose;cube_pose<<measure_data(1),measure_data(2),measure_data(3),0,0,measure_data(4),
						measure_data(5),measure_data(6),measure_data(7);  // xyz roll pitch yaw scale
		  cube_ground_value.fromMinimalVector(cube_pose);
		  Eigen::VectorXd cam_pose_vec = init_frame_poses.row(frame_index);
		  g2o::SE3Quat cam_val_Twc(cam_pose_vec.segment<7>(1)); // time x y z qx qy qz qw
		  cube_local_meas = cube_ground_value.transform_to(cam_val_Twc); // measurement is in local camera frame
		  proposal_error = measure_data(8);

		  // read offline saved 2d image
		  std::string detected_cube_2d_img_name = base_folder+"pred_3d_obj_overview/" + std::string(frame_index_c) + "_best_objects.jpg";
		  currframe->cuboids_2d_img = cv::imread(detected_cube_2d_img_name, 1);

		  offline_cube_obs_row_id++; // switch to next row  NOTE at most one object one frame in this data
	      }
	  }

	  if (has_detected_cuboid)
	  {
	      object_landmark* localcuboid = new object_landmark();

	      localcuboid->cube_meas = cube_local_meas;
	      localcuboid->meas_quality = (1-proposal_error+0.5)/2;  // initial error 0-1, higher worse,  now change to [0.5,1] higher, better
	      currframe->observed_cuboids.push_back(localcuboid);
	  }

	  // set up g2o cube vertex. only one in this dataset
	  if (frame_index==0)
	  {
	      g2o::cuboid init_cuboid_global_pose = cube_local_meas.transform_from(curr_cam_pose_Twc);
	      vCube = new g2o::VertexCuboid();
	      vCube->setEstimate(init_cuboid_global_pose);
	      vCube->setId(0);
	      vCube->setFixed(false);
	      graph.addVertex(vCube);
	  }

	  // set up g2o camera vertex
	  g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
	  currframe->pose_vertex = vSE3;
	  vSE3->setId(frame_index+1);
	  graph.addVertex(vSE3);

	  vSE3->setEstimate(curr_cam_pose_Twc.inverse()); //g2o vertex usually stores world to camera pose.
	  vSE3->setFixed(frame_index==0);

	  // add g2o camera-object measurement edges, if there is
	  if (currframe->observed_cuboids.size()>0)
	  {
		object_landmark* cube_landmark_meas = all_frames[frame_index]->observed_cuboids[0];
		g2o::EdgeSE3Cuboid* e = new g2o::EdgeSE3Cuboid();
		e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>( vSE3 ));
		e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>( vCube ));
		e->setMeasurement(cube_landmark_meas->cube_meas);
		e->setId(frame_index);
		Vector9d inv_sigma;inv_sigma<<1,1,1,1,1,1,1,1,1;
		inv_sigma = inv_sigma*2.0*cube_landmark_meas->meas_quality;
		Matrix9d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
		e->setInformation(info);
		graph.addEdge(e);
	  }

	  // camera vertex, add cam-cam odometry edges
	  if (frame_index>0)
	  {
		g2o::EdgeSE3Expmap* e = new g2o::EdgeSE3Expmap();
		e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>( all_frames[frame_index-1]->pose_vertex ));
		e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>( all_frames[frame_index]->pose_vertex ));
		e->setMeasurement(odom_val);

		e->setId(total_frame_number+frame_index);
		Vector6d inv_sigma;inv_sigma<<1,1,1,1,1,1;
		inv_sigma = inv_sigma*1.0;
		Matrix6d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
		e->setInformation(info);
		graph.addEdge(e);
	  }
	  graph.initializeOptimization();
	  graph.optimize(5); // do optimization!

	  // retrieve the optimization result, for debug visualization
	  for (int j=0;j<=frame_index;j++)
	  {
		all_frames[j]->cam_pose_Tcw = all_frames[j]->pose_vertex->estimate();
		all_frames[j]->cam_pose_Twc = all_frames[j]->cam_pose_Tcw.inverse();
	  }
	  object_landmark* current_landmark = new object_landmark();  current_landmark->cube_vertex = new g2o::VertexCuboid();
	  current_landmark->cube_vertex->setEstimate(vCube->estimate());
	  cube_pose_opti_history[frame_index] = current_landmark;

	  if (all_frames[frame_index]->observed_cuboids.size()>0)
	  {
	      object_landmark* cube_landmark_meas = all_frames[frame_index]->observed_cuboids[0];
	      g2o::cuboid local_cube = cube_landmark_meas->cube_meas;

	      g2o::cuboid global_cube = local_cube.transform_from(all_frames[frame_index]->cam_pose_Twc);
	      object_landmark* tempcuboids2 = new object_landmark();	  tempcuboids2->cube_vertex = new g2o::VertexCuboid();
	      tempcuboids2->cube_vertex->setEstimate(global_cube);
	      cube_pose_raw_detected_history[frame_index] = tempcuboids2;
	  }
	  else
	      cube_pose_raw_detected_history[frame_index] = nullptr;
    }

    cout<<"Finish all optimization! Begin visualization."<<endl;

    publish_all_poses(all_frames, cube_pose_opti_history,cube_pose_raw_detected_history,truth_frame_poses);
}

int main(int argc,char* argv[])
{
    ros::init(argc, argv, "object_slam");

    ros::NodeHandle nh;

    nh.param ("/base_folder", base_folder, ros::package::getPath("object_slam")+"/data/");
    nh.param ("/online_detect_mode", online_detect_mode, true);
    nh.param ("/save_results_to_txt", save_results_to_txt, false);

    cout<<""<<endl;
    cout<<"base_folder   "<<base_folder<<endl;
    if (online_detect_mode)
	ROS_WARN_STREAM("Online detect object mode !!\n");
    else
	ROS_WARN_STREAM("Offline read object mode !!\n");

    //NOTE important
    // in online mode, pred_frame_objects and init_frame_poses are not used. only first frame of truth_frame_poses is used.

    std::string pred_objs_txt = base_folder+"detect_cuboids_saved.txt";  // saved cuboids in local ground frame.
    std::string init_camera_pose = base_folder+"pop_cam_poses_saved.txt"; // offline camera pose for cuboids detection (x y yaw=0, truth roll/pitch/height)
    std::string truth_camera_pose = base_folder+"truth_cam_poses.txt";
    Eigen::MatrixXd pred_frame_objects(100,10);  // 100 is some large row number, each row in txt has 10 numbers
    Eigen::MatrixXd init_frame_poses(100,8);
    Eigen::MatrixXd truth_frame_poses(100,8);

    if (!read_all_number_txt(pred_objs_txt,pred_frame_objects))
	return -1;
    if (!read_all_number_txt(init_camera_pose,init_frame_poses))
	return -1;
    if (!read_all_number_txt(truth_camera_pose,truth_frame_poses))
	return -1;

    std::cout<<"read data size:  "<<pred_frame_objects.rows()<<"  "<<init_frame_poses.rows()<<"  "<<truth_frame_poses.rows()<<std::endl;

    incremental_build_graph(pred_frame_objects,init_frame_poses,truth_frame_poses);

    return 0;
}


text: #include <boost/range/algorithm/replace_copy.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/**
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or  (at your option) any later version.
 *
 *  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
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 **/
/**
 * @file imgScale
 * @ingroup converters
 * @author David Coeurjolly (\c david.coeurjolly@liris.cnrs.fr)
 *
 * @date 2015/03/24
 *
 *
 * This file is part of the DGtal library.
 */

#include <DGtal/base/Common.h>
#include "DGtal/io/readers/GenericReader.h"
#include "DGtal/io/writers/GenericWriter.h"
#include "DGtal/images/ImageContainerBySTLVector.h"
#include "DGtal/images/ImageSelector.h"
#include <DGtal/images/IntervalForegroundPredicate.h>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>

#include <DGtal/images/ConstImageAdapter.h>
#include <DGtal/kernel/BasicPointFunctors.h>

#include <vector>
#include <string>
#include <climits>

using namespace DGtal;

///////////////////////////////////////////////////////////////////////////////
namespace po = boost::program_options;

/**
 * Missing parameter error message.
 *
 * @param param
 */
void missingParam ( std::string param )
{
  trace.error() <<" Parameter: "<<param<<" is required..";
  trace.info() <<std::endl;
  exit ( 1 );
}

typedef ImageSelector < Z2i::Domain, unsigned char>::Type MyImage;

int main( int argc, char** argv )
{
  // parse command line ----------------------------------------------
  po::options_description general_opt("Allowed options are: ");
  general_opt.add_options()
    ("help,h", "display this message")
    ("input,i", po::value<std::string>(), "input image file name (any 2D image format accepted by DGtal::GenericReader)")
    ("output,o", po::value<std::string>(), "output image file name (any 2D image format accepted by DGtal::GenericWriter)")
    ("scale,s", po::value<double>()->default_value(1.0), "Scale factor (default 1.0)")  ;

  bool parseOK=true;
  po::variables_map vm;
  try{
    po::store(po::parse_command_line(argc, argv, general_opt), vm);
  }catch(const std::exception& ex){
    trace.info()<< "Error checking program options: "<< ex.what()<< std::endl;
    parseOK=false;
  }
  po::notify(vm);
  if(vm.count("help")||argc<=1|| !parseOK)
    {
      trace.info()<< "Scale an image by a given scale factor." <<std::endl << "Basic usage: "<<std::endl
      << "\t imgScale [options] --input <imageName> --output <outputImage> -noise 0.3"<<std::endl
      << general_opt << "\n";
      return 0;
    }

  //Parameters
  if ( ! ( vm.count ( "input" ) ) ) missingParam ( "--input" );
  const std::string input = vm["input"].as<std::string>();
  if ( ! ( vm.count ( "output" ) ) ) missingParam ( "--output" );
  const std::string  output = vm["output"].as<std::string>();
  const double scale = vm["scale"].as<double>();

  MyImage image = GenericReader<MyImage>::import( input );
  trace.info() <<"Input image: "<< image<<std::endl;

  std::vector<double> scales(2,1.0/scale);

  typedef functors::BasicDomainSubSampler< MyImage::Domain ,Z2i::Integer, double > ReSampler;
  ReSampler reSampler(image.domain(),
                      scales,
                      Z2i::Point(0,0));

  typedef DGtal::ConstImageAdapter<MyImage , MyImage::Domain, ReSampler,MyImage::Value,DGtal::functors::Identity >  SamplerImageAdapter;

  SamplerImageAdapter sampledImage (image,reSampler.getSubSampledDomain(), reSampler, functors::Identity());
  trace.info()<< "Transformed domain= "<<sampledImage.domain()<<std::endl;

  sampledImage >> output;

  return 0;
}

ext: cpp

max_stars_repo_name: Ezibenroc/shape-indexing

max_issues_repo_name: Ezibenroc/shape-indexing

max_forks_repo_name: Ezibenroc/shape-indexing


text: #include <boost/fusion/algorithm/query/find.hpp>


text:
//
//  Copyright Markus Rickert 2008
//
// 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)
//

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_DETAIL_DENSE_ORDERING_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_DETAIL_DENSE_ORDERING_H

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

namespace boost { namespace numeric { namespace bindings { namespace traits {

  namespace detail {

    template <typename StOrdTag>
    struct dense_ordering {};

    template<>
    struct dense_ordering<row_major_t> {
      typedef row_major_t type;

      template <typename M>
      static std::ptrdiff_t stride1( M const& m ) {
        return leading_dimension (m) ;
      }

      template <typename M>
      static std::ptrdiff_t stride2( M const& m ) {
        return 1 ;
      }
    };

    template<>
    struct dense_ordering<column_major_t> {
      typedef column_major_t type;

      template <typename M>
      static std::ptrdiff_t stride1( M const& m ) {
        return 1 ;
      }

      template <typename M>
      static std::ptrdiff_t stride2( M const& m ) {
        return leading_dimension (m) ;
      }
    };

  }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_DETAIL_DENSE_ORDERING_H

max_stars_repo_name: diku-dk/PROX

max_issues_repo_name: jdsika/holy

max_forks_repo_name: jdsika/holy


text:
// 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)
// (C) Copyright 2013,2014,2017 Vicente J. Botet Escriba

#ifndef JASEL_EXPERIMENTAL_V3_UTILITY_STATIC_ADDRESSOF_HPP
#define JASEL_EXPERIMENTAL_V3_UTILITY_STATIC_ADDRESSOF_HPP

// todo move this to memory folder

#include <experimental/fundamental/v3/config/requires.hpp>

//#ifdef JASEL_EXPECTED_USE_BOOST_HPP
//#include <boost/type_traits.hpp>
//#include <boost/utility/enable_if.hpp>
//#endif
#include <memory>
#include <utility>

namespace std
{
namespace experimental
{
inline namespace fundamental_v3
{
namespace detail {

#if defined JASEL_USE_STD_ADDRESSOF

template <typename T>
constexpr T* static_addressof(T& ref)
{
  return std::addressof(ref);
}

#else

template <typename T>
struct has_overloaded_addressof
{
  template <class X>
  static constexpr bool has_overload(...) { return false; }

  template <class X, size_t S = sizeof(std::declval< X&>().operator&()) >
  static constexpr bool has_overload(bool) { return true; }

  static constexpr bool value = has_overload<T>(true);
};

template <typename T>
constexpr T* static_addressof(T& ref,
    JASEL_REQUIRES(!has_overloaded_addressof<T>::value))
{
  return &ref;
}

template <typename T>
constexpr T* static_addressof(T& ref,
    JASEL_REQUIRES(has_overloaded_addressof<T>::value))
{
  return std::addressof(ref);
}

#endif

} // namespace detail
}}
} // namespace

#endif // JASEL_EXPERIMENTAL_V3_EXPECTED_DETAIL_STATIC_ADDRESSOF_HPP

max_stars_repo_name: jwakely/std-make

max_issues_repo_name: jwakely/std-make

max_forks_repo_name: jwakely/std-make


text:
// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// 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
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_DISTANCE_SAMPLED_HH
#define GRAPH_DISTANCE_SAMPLED_HH

#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <boost/python/object.hpp>
#include <boost/python/list.hpp>
#include <boost/python/extract.hpp>

#include "histogram.hh"
#include "numpy_bind.hh"
#include "hash_map_wrap.hh"

namespace graph_tool
{
using namespace std;
using namespace boost;

// retrieves the sampled vertex-vertex distance histogram

struct no_weightS {};

template <class Map>
struct get_val_type
{
    typedef typename property_traits<Map>::value_type type;
};

template <>
struct get_val_type<no_weightS>
{
    typedef size_t type;
};

struct get_sampled_distance_histogram
{

    template <class Graph, class VertexIndex, class WeightMap, class RNG>
    void operator()(const Graph& g, VertexIndex vertex_index, WeightMap weights,
                    size_t n_samples, const vector<long double>& obins,
                    python::object& phist, RNG& rng) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

        // select get_vertex_dists based on the existence of weights
        typedef typename mpl::if_<std::is_same<WeightMap, no_weightS>,
                                  get_dists_bfs,
                                  get_dists_djk>::type get_vertex_dists_t;

        // distance type
        typedef typename get_val_type<WeightMap>::type val_type;
        typedef Histogram<val_type, size_t, 1> hist_t;

        std::array<vector<val_type>,1> bins;
        bins[0].resize(obins.size());
        for (size_t i = 0; i < obins.size(); ++i)
            bins[0][i] = obins[i];

        hist_t hist(bins);
        SharedHistogram<hist_t> s_hist(hist);

        vector<vertex_t> sources;
        sources.reserve(num_vertices(g));
        for (auto v : vertices_range(g))
            sources.push_back(v);
        n_samples = min(n_samples, sources.size());

        typename hist_t::point_t point;
        get_vertex_dists_t get_vertex_dists;

        #pragma omp parallel for default(shared) private(point) \
            firstprivate(s_hist) schedule(runtime) \
            if (num_vertices(g) * n_samples > OPENMP_MIN_THRESH)
        for (size_t i = 0; i < n_samples; ++i)
        {
            vertex_t v;
            #pragma omp critical
            {
                uniform_int_distribution<size_t> randint(0, sources.size()-1);
                size_t j = randint(rng);
                v = sources[j];
                swap(sources[j], sources.back());
                sources.pop_back();
            }

            unchecked_vector_property_map<val_type,VertexIndex>
                dist_map(vertex_index, num_vertices(g));

            for (auto u : vertices_range(g))
                dist_map[u] = numeric_limits<val_type>::max();

            dist_map[v] = 0;
            get_vertex_dists(g, v, vertex_index, dist_map, weights);

            for (auto v2 : vertices_range(g))
            {
                if (v2 != v && dist_map[v2] != numeric_limits<val_type>::max())
                {
                    point[0] = dist_map[v2];
                    s_hist.put_value(point);
                }
            }
        }
        s_hist.gather();

        python::list ret;
        ret.append(wrap_multi_array_owned<size_t,1>(hist.get_array()));
        ret.append(wrap_vector_owned<val_type>(hist.get_bins()[0]));
        phist = ret;
    }

    // weighted version. Use dijkstra_shortest_paths()
    struct get_dists_djk
    {
        template <class Graph, class Vertex, class VertexIndex,
                  class DistanceMap, class WeightMap>
        void operator()(const Graph& g, Vertex s, VertexIndex vertex_index,
                        DistanceMap dist_map, WeightMap weights) const
        {
            dijkstra_shortest_paths(g, s, vertex_index_map(vertex_index).
                                    weight_map(weights).distance_map(dist_map));
        }
    };

    // unweighted version. Use BFS.
    struct get_dists_bfs
    {
        template <class Graph, class Vertex, class VertexIndex,
                  class DistanceMap>
        void operator()(const Graph& g, Vertex s, VertexIndex vertex_index,
                        DistanceMap dist_map, no_weightS) const
        {
            typedef typename vprop_map_t<default_color_type>::type::unchecked_t vmap_t;
            vmap_t color_map(vertex_index, num_vertices(g));
            parallel_vertex_loop
                (g,
                 [&](auto v)
                 {
                     color_map[v] = color_traits<default_color_type>::white();
                 });
            breadth_first_visit(g, s,
                                visitor(make_bfs_visitor
                                        (record_distances(dist_map,
                                                          on_tree_edge()))).
                                color_map(color_map));
        }
    };
};

} // boost namespace

#endif // GRAPH_DISTANCE_SAMPLED_HH

ext: hh


text:
#include "utility/volume_image.hpp"
#include <mip/imgio.h>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <range/v3/algorithm.hpp>
#include <range/v3/utility/iterator.hpp>
#include <fmt/ostream.h>

namespace fs = boost::filesystem;
namespace po = boost::program_options;
using namespace jhmi;

template <typename T>
void writeBinz(fs::path const& output_path, std::vector<std::string> const& ims) {
  for (auto& im : ims) {
    int x, y, z;
    auto img = readimage3d(const_cast<char*>(im.c_str()), &x, &y, &z);
    volume_image<T> vi{int3{x,y,z}, cube<m3>{m3{}, 4.7951998710632 * int3{x,y,z} * mm}};
    //Make the data a contiguous float array
    std::copy(img, img + x * y * z, vi.begin());

    vi.write(output_path / fs::path{im}.stem().replace_extension(".binz"));
  }
}

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

  try {
    po::options_description desc("Allowed options");
    std::string type, output_path;
    std::vector<std::string> vimgs;
    desc.add_options()
      ("help,h", "produce help message")
      ("im-files", po::value<std::vector<std::string>>(&vimgs)->required(),
       "im files to convert")
      ("type", po::value<std::string>(&type)->required(),
       "output type may be float32, float64, uint8, or uint32")
      ("output-path,o", po::value<std::string>(&output_path)->required(),
       "path where convert images will be saved");
    po::positional_options_description p;
    p.add("im-files", -1);
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);

    if (vm.count("help")) {
      std::cout << desc << std::endl;
      return 1;
    }
    po::notify(vm);

    if (type == "float32") { writeBinz<float>(fs::path{output_path}, vimgs); }
    else if (type == "float64") { writeBinz<double>(fs::path{output_path}, vimgs); }
    else if (type == "uint8") { writeBinz<std::uint8_t>(fs::path{output_path}, vimgs); }
    else if (type == "uint32") { writeBinz<std::uint32_t>(fs::path{output_path}, vimgs); }
    else {
      std::cerr << "Invalid output format given\n";
      return 1;
    }
  }
  catch (std::exception const& ex) {
    std::cerr << ex.what() << std::endl;
    return 1;
  }
  return 0;
}


text:
// ADMM
#include <ADMM.h>
#include <ADMMUtils.h>

// GTSAM
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>

// Boost
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include <fstream>
#include <sstream>

using namespace std;
using namespace gtsam;

// To be used when you already have the partitioning (bunch of g2o files + separator file)
int main(int argc, char* argv[])
{

  int num_subgraphs;
  string output_dir; // output_dir
  string input_dir; // input dir

  double rho = 0.1; // penalty parameter in augmented lagrangian
  double mu = 10.0; // parameter used to decide whether to increase or not the rho (adaptive penalty)
  double tau = 2.0; // if we have to increase rho, we do rho <- rho * tau, otherwise rho <- rho / tau
  int maxIter = 1000; // max number of ADMM iterations
  bool isParallel = false; // if true ADMM runs in parallel mode, this is usually less stable
  bool adaptivePenalty = true;
  bool useFlaggedInitialization = false;
  bool computeSubgraphGradient = true;
  int solver = 0; // GN
  double min_p_res = 0.1f;
  double min_d_res = 0.1f;

  // Verbosity
  int verbosity = 0;

  try{
    // Parse program options
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("help", "Print help messages")
        ("input_dir,i",po::value<string>(&input_dir)->required(), "Input director (required)")
        ("num_subgraphs,n", po::value<int>(&num_subgraphs)->required(), "Num Subgraphs (required)")
        ("result_dir,r", po::value<string>(&output_dir)->required(), "Results Directory (required)")
        ("rho_val", po::value<double>(&rho), "rho value (default: 0.1)")
        ("mu_val,mu", po::value<double>(&mu), "mu value (default: 10.0)")
        ("tau_val,tau", po::value<double>(&tau), "tau value (default: 2.0)")
        ("max_iter", po::value<int>(&maxIter), "Maximum number of iterations (default: 1000)")
        ("adaptive_penalty,adpt_pen", po::value<bool>(&adaptivePenalty), "To use adaptive penalty or not (default: true)")
        ("use_flagged_init", po::value<bool>(&useFlaggedInitialization), "To use Flagged Initialization or not (default: true)")
        ("is_parallel", po::value<bool>(&isParallel), "To use parallel multiblock ADMM or not (default: false)")
        ("compute_subgraph_gradient", po::value<bool>(&computeSubgraphGradient), "To compute subgraph gradient  or not (default: true)")
        ("min_p_res", po::value<double>(&min_p_res), "min_p_res value (default: 0.01f)")
        ("min_d_res", po::value<double>(&min_d_res), "min_d_res value (default: 0.01f)")
        ("solver", po::value<int>(&solver), "Specify solver type (default: GN)")
        ("verbosity", po::value<int>(&verbosity), "Specify verbosity level (default: SILENT)");

    po::variables_map vm;
    try{
      po::store(po::parse_command_line(argc, argv, desc), vm); // can throw
      if ( vm.count("help")  ){ // --help option
        cout << "Partition and Run ADMM" << endl << desc << endl;
        return 0;
      }
      po::notify(vm); // throws on error, so do after help in case
    }
    catch(po::error& e){
      cerr << "ERROR: " << e.what() << endl << endl;
      cerr << desc << endl;
      return 1;
    }
  }
  catch(exception& e){
    cerr << "Unhandled Exception reached the top of main: "
              << e.what() << ", application will now exit" << endl;
    return 2;
  }

  // Create the output directory if it does not exist
  if( !(boost::filesystem::exists(output_dir))){
    boost::filesystem::create_directory(output_dir);
  }

  // Subgraph and Subinitials
  vector<NonlinearFactorGraph> sub_graphs;
  vector<Values> sub_initials;
  PriorFactor<Pose2> posePrior;

  // Read subgraphs
  for(int sub_id = 0; sub_id < num_subgraphs; sub_id++)
  {
    string filepath = input_dir + boost::lexical_cast<string>(sub_id) + string(".g2o");
    GraphAndValues readGraph = readG2o(filepath);
    NonlinearFactorGraph graph = *(readGraph.first);
    Values initial = *(readGraph.second);
    if (sub_id == 0){

      posePrior = PriorFactor<Pose2>(0, initial.at<Pose2>(0),noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05,  5*M_PI/180)));
      graph.add(posePrior);
    }

    sub_graphs.push_back(graph);
    sub_initials.push_back(initial);
  }

  // Read separators
  string separator_file = input_dir + "separators.txt";
  vector < vector <string> > separators_str;
  ifstream infile;
  infile.open(separator_file.c_str(), ifstream::in);

  while (infile){
    string s;
    if (!getline( infile, s )) break;

    istringstream ss(s);
    vector <string> record;

    while (ss){
      string s;
      if (!getline( ss, s, ',' )) break;
      record.push_back(s);
    }

    separators_str.push_back( record );
  }
  if (!infile.eof()){
    cerr << "File still not read completely!\n";
  }

  // Load the separators
  vector <int>  separators;
  for(size_t i=0; i < separators_str.size(); i++){
    for(size_t j=0; j< separators_str[i].size(); j++){
      separators.push_back(boost::lexical_cast<int>(separators_str[i][j]));
    }
  }

  // Read centralized graph from g2o file
  string filepath = input_dir + "/fullGraph.g2o";
  GraphAndValues readGraph = readG2o(filepath);
  NonlinearFactorGraph graph = *(readGraph.first);
  Values initial = *(readGraph.second);

  // Write down the G2o files
  for(size_t sub_id =0; sub_id < sub_initials.size(); sub_id++){
    string filepath = output_dir + "/" + boost::lexical_cast<string>(sub_id) + "_initial.g2o";
    writeG2o(sub_graphs[sub_id], sub_initials[sub_id], filepath);
  }

  filepath = output_dir + "/separators.txt";
  fstream separatorFile(filepath.c_str(), fstream::out);
  for(size_t i = 0; i < separators.size(); i += 3){
    separatorFile << separators[i] <<  "," << separators[i+1] <<  "," << separators[i+2] << endl;
  }
  separatorFile.close();

  // Run ADMM
  // Construct ADMM
  ADMM admm(rho,
            mu,
            tau,
            maxIter,
            isParallel,
            adaptivePenalty,
            useFlaggedInitialization,
            computeSubgraphGradient,
            min_p_res,
            min_d_res,
            output_dir);

  admm.setSolver(ADMM::Solver(solver));
  admm.setVerbosity(ADMM::Verbosity(verbosity));

  // Load structures
  admm.load(sub_graphs, sub_initials, separators, graph);

  cout << "Running on " << input_dir << endl;
  // Run ADMM
  pair<vector<Values>, vector<double> > primalDualResult = admm.optimize();
  vector<Values> sub_results = primalDualResult.first;

  // Pack ADMM results into a single Value structure, to check centralized error
  logResults(sub_graphs, sub_results, graph, initial, posePrior, output_dir);

  return 0;
}

max_stars_repo_name: itzsid/admm-slam

max_issues_repo_name: itzsid/admm-slam

max_forks_repo_name: itzsid/admm-slam


text:
#ifndef CLIENT_HTTPS_HPP
#define	CLIENT_HTTPS_HPP

#include "client_http.hpp"
#include <boost/asio/ssl.hpp>

namespace SimpleWeb {
    typedef boost::asio::ssl::stream<boost::asio::ip::tcp::socket> HTTPS;

    template<>
    class Client<HTTPS> : public ClientBase<HTTPS> {
    public:
        Client(const std::string& server_port_path, bool verify_certificate=true,
                const std::string& cert_file=std::string(), const std::string& private_key_file=std::string(),
                const std::string& verify_file=std::string()) :
                ClientBase<HTTPS>::ClientBase(server_port_path, 443), context(boost::asio::ssl::context::tlsv12) {
            if(cert_file.size()>0 && private_key_file.size()>0) {
                context.use_certificate_chain_file(cert_file);
                context.use_private_key_file(private_key_file, boost::asio::ssl::context::pem);
            }

            if(verify_certificate)
                context.set_verify_callback(boost::asio::ssl::rfc2818_verification(host));

            if(verify_file.size()>0)
                context.load_verify_file(verify_file);
            else
                context.set_default_verify_paths();

            if(verify_file.size()>0 || verify_certificate)
                context.set_verify_mode(boost::asio::ssl::verify_peer);
            else
                context.set_verify_mode(boost::asio::ssl::verify_none);
        }

    protected:
        boost::asio::ssl::context context;

        void connect() {
            if(!socket || !socket->lowest_layer().is_open()) {
                std::unique_ptr<boost::asio::ip::tcp::resolver::query> query;
                if(config.proxy_server.empty())
                    query=std::unique_ptr<boost::asio::ip::tcp::resolver::query>(new boost::asio::ip::tcp::resolver::query(host, std::to_string(port)));
                else {
                    auto proxy_host_port=parse_host_port(config.proxy_server, 8080);
                    query=std::unique_ptr<boost::asio::ip::tcp::resolver::query>(new boost::asio::ip::tcp::resolver::query(proxy_host_port.first, std::to_string(proxy_host_port.second)));
                }
                resolver.async_resolve(*query, [this]
                                       (const boost::system::error_code &ec, boost::asio::ip::tcp::resolver::iterator it){
                    if(!ec) {
                        {
                            std::lock_guard<std::mutex> lock(socket_mutex);
                            socket=std::unique_ptr<HTTPS>(new HTTPS(io_service, context));
                        }

                        auto timer=get_timeout_timer(config.timeout_connect);
                        boost::asio::async_connect(socket->lowest_layer(), it, [this, timer]
                                                   (const boost::system::error_code &ec, boost::asio::ip::tcp::resolver::iterator /*it*/){
                            if(timer)
                                timer->cancel();
                            if(!ec) {
                                boost::asio::ip::tcp::no_delay option(true);
                                this->socket->lowest_layer().set_option(option);
                            }
                            else {
                                std::lock_guard<std::mutex> lock(socket_mutex);
                                this->socket=nullptr;
                                throw boost::system::system_error(ec);
                            }
                        });
                    }
                    else {
                        std::lock_guard<std::mutex> lock(socket_mutex);
                        socket=nullptr;
                        throw boost::system::system_error(ec);
                    }
                });
                io_service.reset();
                io_service.run();

                if(!config.proxy_server.empty()) {
                    boost::asio::streambuf write_buffer;
                    std::ostream write_stream(&write_buffer);
                    auto host_port=host+':'+std::to_string(port);
                    write_stream << "CONNECT "+host_port+" HTTP/1.1\r\n" << "Host: " << host_port << "\r\n\r\n";
                    auto timer=get_timeout_timer();
                    boost::asio::async_write(socket->next_layer(), write_buffer,
                                             [this, timer](const boost::system::error_code &ec, size_t /*bytes_transferred*/) {
                        if(timer)
                            timer->cancel();
                        if(ec) {
                            std::lock_guard<std::mutex> lock(socket_mutex);
                            socket=nullptr;
                            throw boost::system::system_error(ec);
                        }
                    });
                    io_service.reset();
                    io_service.run();

                    std::shared_ptr<Response> response(new Response());
                    timer=get_timeout_timer();
                    boost::asio::async_read_until(socket->next_layer(), response->content_buffer, "\r\n\r\n",
                                                  [this, timer](const boost::system::error_code& ec, size_t /*bytes_transferred*/) {
                        if(timer)
                            timer->cancel();
                        if(ec) {
                            std::lock_guard<std::mutex> lock(socket_mutex);
                            socket=nullptr;
                            throw boost::system::system_error(ec);
                        }
                    });
                    io_service.reset();
                    io_service.run();
                    parse_response_header(response);
                    if (response->status_code.empty() || response->status_code.compare(0, 3, "200") != 0) {
                        std::lock_guard<std::mutex> lock(socket_mutex);
                        socket=nullptr;
                        throw boost::system::system_error(boost::system::error_code(boost::system::errc::permission_denied, boost::system::generic_category()));
                    }
                }

                auto timer=get_timeout_timer();
                this->socket->async_handshake(boost::asio::ssl::stream_base::client,
                                              [this, timer](const boost::system::error_code& ec) {
                    if(timer)
                        timer->cancel();
                    if(ec) {
                        std::lock_guard<std::mutex> lock(socket_mutex);
                        socket=nullptr;
                        throw boost::system::system_error(ec);
                    }
                });
                io_service.reset();
                io_service.run();
            }
        }
    };
}

#endif	/* CLIENT_HTTPS_HPP */

max_stars_repo_name: edmont/ot-br-posix

max_issues_repo_name: edmont/ot-br-posix

max_forks_repo_name: edmont/ot-br-posix