#include "Autonomous.h"

#include "Globals.h"
#include "Util.h"
#include "log.h"
#include "rospub.h"
#include "world_interface/world_interface.h"

#include <cmath>
#include <iostream>

#include <Eigen/LU>

using namespace navtypes;
using namespace robot::types;
using util::toTransform;
using util::toPose;

namespace pose_graph = filters::pose_graph;

constexpr float PI = M_PI;
constexpr double KP_ANGLE = 2.0;
constexpr double FAST_DRIVE_SPEED = 1.5;
constexpr double CAREFUL_DRIVE_SPEED = 1.0;
const Eigen::Vector3d gpsStdDev = {2, 2, 3};
constexpr int numSamples = 1;

constexpr double FOLLOW_DIST = 2.0;
constexpr double PLANNING_GOAL_REGION_RADIUS = 1.0;
constexpr double PLAN_COLLISION_DIST = 1.3;
constexpr double PLAN_COLLISION_STOP_DIST = 4.0;
constexpr double INFINITE_COST = 1e10;
constexpr int REPLAN_PERIOD = 20;

constexpr bool USE_MAP = false;
constexpr bool MOVING_LANDMARKS = false;

constexpr bool LEFT = true;
constexpr bool RIGHT = false;

constexpr auto ZERO_DURATION = std::chrono::microseconds(0);

// twice the scan period, should be adjusted as necessary
constexpr std::chrono::milliseconds LIDAR_FRESH_PERIOD(200);
constexpr std::chrono::milliseconds GPS_FRESH_PERIOD(2000); // update to match gps specs

const transform_t VIZ_BASE_TF = toTransform({0, 0, M_PI / 2}); // TODO: remove this

/**
	 @brief Prints a list of landmarks using the logging mechanism.

	 The message is formatted as "Landmarks: [i: {x, y, z}, ...]" where i is the index of the
	 landmark in the list.

	 @param landmarks The list of landmarks to print.
	 @param log_level The optional log level to use; defaults to LOG_DEBUG so the message isn't
	 seen if on a higher logging level.
 */
static bool printLandmarks(const points_t& landmarks, int log_level = LOG_DEBUG);

/**
	 @brief Prints a list of landmarks using the logging mechanism.

	 The message is formatted as "Landmarks: [i: {x, y, z}, ...]" where i is the index of the
	 landmark in the list.

	 This is a duplicate of above, but modified to handle the DataPoint class.

	 @param landmarks The landmarks data to print.
	 @param log_level The optional log level to use; defaults to LOG_DEBUG so the message isn't
	 seen if on a higher logging level.
 */
static bool printLandmarks(const landmarks_t& landmarks, int log_level = LOG_DEBUG);

Autonomous::Autonomous(const std::vector<URCLegGPS>& gpsTargets, double controlHz)
	: initialized(false), urc_targets({}), urc_targets_gps(gpsTargets), leg_idx(0),
	  search_target({}),
	  gate_targets({NAN, NAN, NAN}, {NAN, NAN, NAN}), gate_direction(true),
	  poseEstimator({0.2, 0.2}, gpsStdDev, Constants::WHEEL_BASE, 1.0 / controlHz),
	  map(10000), // TODO: the stride should be set higher when using the real lidar
	  calibrated(false), calibrationPoses({}), nav_state(NavState::GPS),
	  control_state(ControlState::FAR_FROM_TARGET_POSE), time_since_plan(0), plan(0, 2),
	  pending_plan(), pending_solve(), plan_cost(INFINITE_COST), plan_base({0, 0, 0}),
	  plan_idx(0), search_theta_increment(PI / 4), mapLoopCounter(0), mapBlindPeriod(15),
	  mapDoesOverlap(false), mapOverlapSampleThreshold(15),
	  pose_graph(0, 0, 0, 0, 0), // We'll re-initialize this in the function body
	  pose_id(0), prev_odom(toTransform({0, 0, 0})), smoothed_traj({}),
	  smoothed_landmarks({}) {
}

void Autonomous::init() {
	if (!robot::gpsHasFix()) {
		return;
	}
	this->urc_targets.clear();
	// convert gps targets to map space
	for (const auto& gpsTarget : urc_targets_gps) {
		point_t p = robot::gpsToMeters(gpsTarget.gps).value();
		urc_targets.push_back({gpsTarget.left_post_id, gpsTarget.right_post_id, p});
	}
	search_target = {urc_targets[0].approx_GPS(0), urc_targets[0].approx_GPS(1), -M_PI / 2};
	int num_landmarks = 0;
	for (auto target : urc_targets) {
		int num_landmarks_for_target = (target.right_post_id == -1) ? 1 : 2;
		num_landmarks += num_landmarks_for_target;
	}
	// We use a gps_xy_std that's larger than the true std because empirically
	// that leads to smoother changes in the pose estimate (at least in the simulator),
	// which are easier for the controller to work with.
	pose_graph = pose_graph::FriendlyGraph(num_landmarks, 10, 0.3, 5.0, 0.05);
	prev_odom = robot::readOdom();
	// TODO how to make this start pose configurable?
	transform_t start_pose_guess = toTransform({0, 0, 0});
	double pose_prior_xy_std = 3.0;
	double pose_prior_th_std = 1.0;
	pose_graph::covariance<3> pose_prior_cov = pose_graph::covariance<3>::Zero();
	pose_prior_cov << pose_prior_xy_std * pose_prior_xy_std, 0, 0, 0,
		pose_prior_xy_std * pose_prior_xy_std, 0, 0, 0, pose_prior_th_std * pose_prior_th_std;
	pose_graph.addPosePrior(0, start_pose_guess, pose_prior_cov);
	double lm_std = 100.0;
	for (auto target : urc_targets) {
		pose_graph.addLandmarkPrior(target.left_post_id, target.approx_GPS, lm_std);
		if (target.right_post_id != -1) {
			pose_graph.addLandmarkPrior(target.right_post_id, target.approx_GPS, lm_std);
		}
	}
	pose_graph.solve();
	smoothed_traj = pose_graph.getSmoothedTrajectory();
	smoothed_landmarks = pose_graph.getLandmarkLocations();
	initialized = true;
}

double dist(const pose_t& p1, const pose_t& p2, double theta_weight) {
	pose_t diff = p1 - p2;
	// angles are modular in nature, so wrap at 2pi radians
	double thetaDiff = std::fmod(fabs(diff(2)), 2 * PI);
	// change domain from [0, 2pi) to (-pi, pi]
	if (thetaDiff > PI) {
		thetaDiff -= 2 * PI;
	}
	diff(2) = thetaDiff * theta_weight;
	return diff.norm();
}

void Autonomous::endCurrentLeg() {
	log(LOG_INFO, "Leg complete, exiting autonomous mode\n");
	robot::setCmdVel(0, 0);
	Globals::AUTONOMOUS = false;
	leg_idx += 1;
	// clear the cached data points

	if (leg_idx == urc_targets.size()) {
		log(LOG_INFO, "All legs complete\n");
	} else {
		search_target = {urc_targets[leg_idx].approx_GPS(0) - PI,
						 urc_targets[leg_idx].approx_GPS(1), -PI / 2};
		setNavState(NavState::GPS);
		log(LOG_INFO, "Enter autonomous mode to start next leg\n");
	}
}

void Autonomous::setNavState(NavState s) {
	NavState old_state = nav_state;
	nav_state = s;
	if (old_state != s) {
		log(LOG_INFO, "Changing navigation state %s -> %s\n", NAV_STATE_NAMES[old_state],
			NAV_STATE_NAMES[nav_state]);
		plan_cost = INFINITE_COST;
	}
}

pose_t Autonomous::poseToDraw(const pose_t& pose, const pose_t& current_pose) const {
	// Both poses are given in the same frame. (usually GPS frame)
	// `current_pose` is the current location of the robot, `pose` is the pose we wish to draw
	transform_t inv_curr = toTransform(current_pose).inverse();
	return toPose(toTransform(pose) * inv_curr * VIZ_BASE_TF, 0);
}

void Autonomous::update_nav_state(const pose_t& pose, const pose_t& plan_target) {
	if (nav_state == NavState::GPS || nav_state == NavState::SEARCH_PATTERN ||
		nav_state == NavState::SEARCH_PATTERN_SECOND_POST ||
		nav_state == NavState::POST_VISIBLE) {
		bool foundLeftPost = getPostVisibility(LEFT);
		bool foundRightPost = getPostVisibility(RIGHT);
		if (foundLeftPost && foundRightPost) {
			setNavState(NavState::GATE_ALIGN);
		} else if ((foundLeftPost || foundRightPost) &&
				   (nav_state != SEARCH_PATTERN_SECOND_POST)) {
			setNavState(NavState::POST_VISIBLE);
		}
	}

	if (dist(pose, plan_target, 1.0) < 0.5) {
		if (nav_state == NavState::GPS) {
			log(LOG_WARN, "Reached GPS target without seeing post!\n");
			setNavState(NavState::SEARCH_PATTERN);
		} else if (nav_state == NavState::POST_VISIBLE) {
			if (urc_targets[leg_idx].right_post_id == -1) {
				log(LOG_INFO, "Arrived at post\n");
				setNavState(NavState::DONE);
			} else {
				log(LOG_WARN, "Reached left post without seeing right post!\n");
				setNavState(NavState::SEARCH_PATTERN_SECOND_POST);
			}
		} else if (nav_state == NavState::GATE_ALIGN) {
			setNavState(NavState::GATE_TRAVERSE);
		} else if (nav_state == NavState::GATE_TRAVERSE) {
			log(LOG_INFO, "Arrived at gate\n");
			setNavState(NavState::DONE);
		} else if (nav_state == NavState::SEARCH_PATTERN ||
				   nav_state == NavState::SEARCH_PATTERN_SECOND_POST) {
			// Current search point has been reached
			updateSearchTarget();
		}
	}
}

bool calibratePeriodic(std::vector<pose_t>& poses, const pose_t& pose, pose_t& out) {
	poses.push_back(pose);
	if (poses.size() == numSamples) {
		pose_t sum = pose_t::Zero();
		for (const pose_t& p : poses) {
			sum += p;
		}
		sum /= poses.size();
		out = sum;
		return true;
	}
	return false;
}

double getRelativeAngle(double currAngle, double targetAngle) {
	double range = 2 * PI;
	double dist = std::fmod(targetAngle - currAngle, range);
	double absDist = fabs(dist);

	int sign = dist > 0 ? 1 : (dist < 0 ? -1 : 0);

	return currAngle + ((absDist > (range / 2)) ? -sign * (range - absDist) : dist);
}

double Autonomous::getLinearVel(const pose_t& drive_target, const pose_t& pose,
								double thetaErr) const {
	double speed = FAST_DRIVE_SPEED;
	if (dist(drive_target, pose, 0) < 1.0 || nav_state == NavState::GATE_TRAVERSE) {
		// We may need to drive slower near the goal so that we don't overshoot the target
		// (given our relatively low control frequency of 10 Hz)
		speed = CAREFUL_DRIVE_SPEED;
	}
	if (fabs(thetaErr) > PI / 4 || control_state == ControlState::NEAR_TARGET_POSE) {
		// don't drive forward if pointing away
		// or if we're already very close to the target
		speed = 0;
	}
	return speed;
}

double Autonomous::getThetaVel(const pose_t& drive_target, const pose_t& pose,
							   double& thetaErr) const {
	// If we're within 20cm of the target location, we want to turn the rover until
	// we reach the target orientation. Otherwise, we want to turn the rover to
	// aim it at the target location.
	double targetAngle = drive_target(2);
	if (control_state == ControlState::FAR_FROM_TARGET_POSE) {
		double dx = drive_target(0) - pose(0);
		double dy = drive_target(1) - pose(1);
		targetAngle = atan2(dy, dx);
	}
	double relativeTargetAngle = getRelativeAngle(pose(2), targetAngle);
	thetaErr = relativeTargetAngle - pose(2);

	return KP_ANGLE * thetaErr;
}

int Autonomous::getPostID(bool left) {
	int post_id =
		left ? urc_targets[leg_idx].left_post_id : urc_targets[leg_idx].right_post_id;
	return post_id;
}

point_t Autonomous::getPostLocation(bool left, bool verbose) {
	int post_id = getPostID(left);
	if (post_id < 0 || static_cast<unsigned>(post_id) > smoothed_landmarks.size()) {
		if (verbose) {
			log(LOG_ERROR, "Invalid post_id %d\n", urc_targets[leg_idx].left_post_id);
		}
		return {0, 0, 0};
	}
	return smoothed_landmarks[post_id];
}

bool Autonomous::getPostVisibility(bool left) {
	// This is a bit of a hack. We decide whether a post is currently visible based
	// on whether its current estimated location is different from its approximate
	// GPS coordinates. Really what we should do is implement some pose graph function
	// that tells us how uncertain we are about the location of the post, but that's
	// more complicated to implement.
	point_t prior_location = urc_targets[leg_idx].approx_GPS;
	point_t location = getPostLocation(left, false);
	double diff = (location - prior_location).norm();
	bool different_from_prior = (diff > 0.001);
	bool visible = different_from_prior && (location(2) != 0);
	log(LOG_DEBUG,
		"Visibility %s is %d: diff %.3f loc (%.3f %.3f %.3f) prior (%.3f %.3f %.3f)\n",
		left ? "LEFT " : "RIGHT", visible, diff, location(0), location(1), location(2),
		prior_location(0), prior_location(1), prior_location(2));
	return visible;
}

pose_t Autonomous::choose_plan_target(const pose_t& pose) {
	// When traversing the gate, we don't want to reverse the order of the gate targets
	computeGateTargets(pose, nav_state != NavState::GATE_TRAVERSE);

	if (nav_state == NavState::GPS) {
		return getGPSTargetPose();
	} else if (nav_state == NavState::SEARCH_PATTERN ||
			   nav_state == NavState::SEARCH_PATTERN_SECOND_POST) {
		return search_target.value();
	} else if (nav_state == NavState::POST_VISIBLE) {
		bool post_to_get = getPostVisibility(LEFT) ? LEFT : RIGHT;
		point_t post = getPostLocation(post_to_get);
		double angle = std::atan2(post(1) - pose(1), post(0) - pose(0));
		// Offset the target by two meters to avoid crashing into the post
		pose_t plan_target({post(0) - 2.0 * cos(angle), post(1) - 2.0 * sin(angle), angle});
		return plan_target;
	} else if (nav_state == NavState::GATE_ALIGN) {
		return gate_targets.first;
	} else if (nav_state == NavState::GATE_TRAVERSE) {
		return gate_targets.second;
	} else { // probably NavState::DONE
		return {0, 0, 0};
	}
}

void Autonomous::computeGateTargets(const pose_t& pose, bool choose_direction) {
	if (!(getPostVisibility(LEFT) && getPostVisibility(RIGHT)))
		return;

	point_t post_1 = getPostLocation(LEFT);
	point_t post_2 = getPostLocation(RIGHT);
	point_t center = (post_1 + post_2) / 2;

	point_t offset{(post_1(1) - post_2(1)) / 2, (post_2(0) - post_1(0)) / 2, 0};
	pose_t goal_1 = center + 2 * offset;
	pose_t goal_2 = center - 2 * offset;

	// Choose the closest goal to be the first target
	if (choose_direction) {
		gate_direction = (dist(pose, goal_1, 1.0) < dist(pose, goal_2, 1.0));
	}

	if (gate_direction == true) {
		gate_targets = std::make_pair(goal_1, goal_2);
	} else {
		gate_targets = std::make_pair(goal_2, goal_1);
	}

	// Angle between targets
	double angle = std::atan2(gate_targets.second(1) - gate_targets.first(1),
							  gate_targets.second(0) - gate_targets.first(0));
	gate_targets.first(2) = angle;
	gate_targets.second(2) = angle;
}

void Autonomous::updateSearchTarget() {
	// Replan to avoid waiting at current search point
	plan_cost = INFINITE_COST;
	// Set the search target to the next point in the search pattern
	pose_t& search_target = this->search_target.value();
	search_target -= urc_targets[leg_idx].approx_GPS;
	double radius = hypot(search_target(0), search_target(1));
	double scale = (radius + search_theta_increment) / radius;
	// Rotate the target counterclockwise by the theta increment
	search_target.topRows(2) =
		(Eigen::Matrix2d() << cos(search_theta_increment), -sin(search_theta_increment),
		 sin(search_theta_increment), cos(search_theta_increment))
			.finished() *
		search_target.topRows(2) * scale;
	search_target += urc_targets[leg_idx].approx_GPS;
	// Adjust the target angle
	search_target(2) += search_theta_increment;
	if (search_target(2) > PI) {
		search_target(2) -= 2 * PI;
		// Decrease the theta increment every time a full rotation is made so the
		// spiral shape is followed better
		// This increases the denominator of the theta increment by 4
		search_theta_increment = PI / ((int)(PI / search_theta_increment + 4));
	}
}

plan_t computePlan(transform_t invTransform, point_t goal) {
	// We need to readLidar again from within the planning thread, for thread safety
	auto lidarData = robot::readLidarScan();
	points_t lidar_scan;
	std::function<bool(double, double, double)> collide_func;
	if (lidarData.isFresh(LIDAR_FRESH_PERIOD)) {
		lidar_scan = lidarData.getData();
		collide_func = [&](double x, double y, double radius) -> bool {
			// transform the point to check into map space
			point_t relPoint = {x, y, 1};
			point_t p = invTransform * relPoint;
			transform_t coll_tf = toTransform(relPoint);
			// TODO implement thread-safe access to `map` if `USE_MAP` is true
			// if (USE_MAP) return map.hasPointWithin(p, radius);
			return util::collides(coll_tf, lidar_scan, radius);
		};
	} else {
		if (!lidarData) {
			log(LOG_ERROR, "Tried to read from the lidar before it was initialized!\n");
		} else {
			log(LOG_WARN, "Data is older than %dms!\n", LIDAR_FRESH_PERIOD);
		}
		collide_func = [](double, double, double) -> bool { return false; };
	}
	return getPlan(collide_func, goal, PLANNING_GOAL_REGION_RADIUS);
}

transform_t Autonomous::optimizePoseGraph(transform_t current_odom) {
	DataPoint<point_t> gpsData = robot::readGPS();
	bool new_gps_data = gpsData.isFresh(GPS_FRESH_PERIOD) && lastGPSTime != gpsData.getTime();
	if (new_gps_data) {
		lastGPSTime = gpsData.getTime();
	}

	landmarks_t landmarks = robot::readLandmarks();
	printLandmarks(landmarks, LOG_DEBUG);
	// check if any of the landmark data is new
	bool new_landmark_data = false;
	for (int i = 0; i < landmarks.size(); i++) {
		const auto& lm = landmarks[i];
		// we don't check isFresh() because that's handled in the AR code
		if (lm) {
			auto lastTimeItr = lastLandmarkTimes.find(i);
			// if we haven't gotten any data before, or if the current data is more recent,
			// mark as new
			if (lastTimeItr == lastLandmarkTimes.end() ||
				lastTimeItr->second != lm.getTime()) {
				new_landmark_data = true;
				lastLandmarkTimes[i] = lm.getTime();
			} else {
				// we've seen this data point before, so mark as empty (so it won't be used
				// later)
				landmarks[i] = {};
			}
		}
	}

	double odom_diff = toPose(current_odom * prev_odom.inverse(), 0.0).norm();
	bool overwrite_landmark = (current_odom == prev_odom) && new_landmark_data;
	bool new_data =
		new_gps_data || (odom_diff > 0.5 && new_landmark_data) || overwrite_landmark;

	if (new_data) {
		if (current_odom != prev_odom) { // Don't add poses to pose graph if we haven't moved
			pose_id += 1;
			pose_graph.addOdomMeasurement(pose_id, pose_id - 1, current_odom, prev_odom);
		}
		for (size_t lm_id = 0; lm_id < landmarks.size(); lm_id++) {
			auto lmData = landmarks[lm_id];
			if (lmData) {
				// data is assumed to be new here (since it should have been checked above)
				point_t lm = lmData.getData();
				bool overwrite = overwrite_landmark || MOVING_LANDMARKS;
				pose_graph.addLandmarkMeasurement(pose_id, (int)lm_id, lm, overwrite);
			}
		}
		// TODO: change addGPSMeasurement() to accept a point_t instead of transform_t
		if (new_gps_data)
			pose_graph.addGPSMeasurement(pose_id, toTransform(gpsData.getData()));

		pose_graph.solve();
		int log_level = LOG_DEBUG;
		log(log_level, "Found pose graph solution: ");
		bool found_one = printLandmarks(pose_graph.getLandmarkLocations(), log_level);
		if (!found_one)
			log(log_level, "No landmarks found.\n");
		return current_odom;
	} else {
		return prev_odom;
	}
}

void Autonomous::autonomyIter() {
	if (robot::gpsHasFix() && !initialized) {
		init();
	} else if (!robot::gpsHasFix()) {
		log(LOG_WARN, "Waiting for GPS fix...\n");
		return;
	}

	transform_t odom = robot::readOdom();
	pose_t pose{0, 0, 0};

	if (!pending_solve.valid()) {
		// TODO even if we already have a pose graph optimization running, we should still
		// add data to the pose graph (e.g. if we have new camera or GPS readings).
		// However, this would be more complicated to implement, and the improvement is
		// likely insignificant as long as pose graph solves take significantly less time
		// than one second.
		pending_solve =
			std::async(std::launch::async, &Autonomous::optimizePoseGraph, this, odom);
	}

	if (pending_solve.valid() &&
		pending_solve.wait_for(ZERO_DURATION) == std::future_status::ready) {
		prev_odom = pending_solve.get();
		smoothed_traj = pose_graph.getSmoothedTrajectory();
		smoothed_landmarks = pose_graph.getLandmarkLocations();
	}

	transform_t current_tf =
		odom * prev_odom.inverse() * smoothed_traj[smoothed_traj.size() - 1];
	pose = toPose(current_tf, 0.0);
	log(LOG_DEBUG, "pose %f %f %f\n", pose(0), pose(1), pose(2));

	for (transform_t& sm_tf : smoothed_traj) {
		const pose_t sm_tf_transformed = poseToDraw(toPose(sm_tf, 0.0), pose);
		rospub::publish(sm_tf_transformed, rospub::PointPub::POSE_GRAPH);
	}

	transform_t invTransform = toTransform(pose).inverse();

	const points_t landmarks_transformed =
		util::transformReadings(smoothed_landmarks, invTransform * VIZ_BASE_TF);
	rospub::publish_array(landmarks_transformed, rospub::ArrayPub::LANDMARKS);

	if (nav_state == NavState::DONE && Globals::AUTONOMOUS) {
		endCurrentLeg();
		return;
	}

	pose_t plan_target = choose_plan_target(pose);
	log(LOG_DEBUG, "plan_target %f %f %f\n", plan_target(0), plan_target(1), plan_target(2));
	update_nav_state(pose, plan_target);

	auto lidarData = robot::readLidarScan();
	points_t lidar_scan;
	if (lidarData.isFresh(LIDAR_FRESH_PERIOD)) {
		if (lastLidarTime != lidarData.getTime()) {
			lidar_scan = lidarData.getData();
			lastLidarTime = lidarData.getTime();
		} else {
			log(LOG_WARN, "No new lidar data available!\n");
		}
	} else if (!lidarData) {
		log(LOG_ERROR, "Tried to read from the lidar before it was initialized!\n");
	} else {
		log(LOG_WARN, "Data is older than %dms!\n", LIDAR_FRESH_PERIOD);
	}

	if (USE_MAP) {
		if (mapLoopCounter++ > mapBlindPeriod) {
			map.addPoints(toTransform(pose), lidar_scan, mapDoesOverlap ? 0.4 : 0);
			mapDoesOverlap = lidar_scan.size() > mapOverlapSampleThreshold;
		}
	}

	// if we don't have a plan pending, we should start computing one.
	if (!pending_plan.valid()) {
		point_t point_t_goal;
		point_t_goal.topRows(2) = plan_target.topRows(2);
		point_t_goal(2) = 1.0;
		point_t_goal = toTransform(pose) * point_t_goal;
		log(LOG_DEBUG, "point_t_goal %f %f %f\n", point_t_goal(0), point_t_goal(1),
			point_t_goal(2));
		pending_plan =
			std::async(std::launch::async, &computePlan, invTransform, point_t_goal);
	}
	// if there is a plan pending, check if it is ready.
	else if (pending_plan.wait_for(ZERO_DURATION) == std::future_status::ready) {
		// plan is ready; retrieve it and check if the cost is satisfactory.
		// upon calling get(), pending_plan.valid() will return false, so if we reject this
		// plan another will be computed.
		plan_t new_plan = pending_plan.get();
		double new_plan_cost = planCostFromIndex(new_plan, 0);
		// we want a significant improvement to avoid thrash
		if (new_plan_cost < plan_cost * 0.8) {
			log(LOG_INFO, "Got new plan: %ld steps, %f cost (previous: %ld steps, %f cost)\n",
				new_plan.size(), new_plan_cost, plan.size(), plan_cost);
			plan_idx = 0;
			plan_base = pose;
			plan_cost = new_plan_cost;
			plan = new_plan;
			time_since_plan = 0;
		}
	}

	// Send lidar points to visualization
	const points_t lidar_scan_transformed = util::transformReadings(lidar_scan, VIZ_BASE_TF);
	log(LOG_DEBUG, "Publishing %d lidar points\n", lidar_scan_transformed.size());
	rospub::publish_array(lidar_scan_transformed, rospub::ArrayPub::LIDAR_SCAN);

	// Send current pose and plan target to visualization
	const pose_t curr_pose_transformed = poseToDraw(pose, pose);
	rospub::publish(curr_pose_transformed, rospub::PointPub::CURRENT_POSE);
	const pose_t plan_target_transformed = poseToDraw(plan_target, pose);
	rospub::publish(plan_target_transformed, rospub::PointPub::PLAN_TARGET);

	pose_t driveTarget = pose;
	if (nav_state == NavState::GATE_TRAVERSE || dist(plan_target, pose, 0.0) < 2.0) {
		// We're probably within planning resolution of the goal,
		// so using the goal as the drive target makes sense.
		driveTarget = plan_target;
	} else {
		driveTarget = getDriveTargetFromPlan(pose, plan_target, lidar_scan);
	}
	// Send drive target to visualization
	const pose_t drive_target_transformed = poseToDraw(driveTarget, pose);
	rospub::publish(drive_target_transformed, rospub::PointPub::DRIVE_TARGET);

	// Send message to visualization that all data has been sent
	rospub::publish({NAN, NAN, NAN}, rospub::PointPub::PLAN_VIZ);

	double d = dist(driveTarget, pose, 0);
	// There's an overlap where either state might apply, to prevent rapidly switching
	// back and forth between these two states.
	if (d < 0.2 && control_state == ControlState::FAR_FROM_TARGET_POSE) {
		control_state = ControlState::NEAR_TARGET_POSE;
	}
	if (d > 0.5 && control_state == ControlState::NEAR_TARGET_POSE) {
		control_state = ControlState::FAR_FROM_TARGET_POSE;
	}
	double thetaErr;
	double thetaVel = getThetaVel(driveTarget, pose, thetaErr);
	double driveSpeed = getLinearVel(driveTarget, pose, thetaErr);

	if (!Globals::E_STOP && Globals::AUTONOMOUS) {
		double scale_factor = robot::setCmdVel(thetaVel, driveSpeed);
	}
}

pose_t Autonomous::getDriveTargetFromPlan(const pose_t& pose, const pose_t& plan_target,
										  const points_t& lidar_scan) {
	// Roll out the plan until we find a target pose a certain distance in front
	// of the robot position `pose`.
	//
	// This code also handles visualizing the plan.
	//
	// It also handles estimating the cost of following the current plan, so that
	// we can replace it with a better plan if we find one.
	int plan_size = plan.rows();

	pose_t plan_pose = plan_base;
	pose_t driveTarget = pose;
	transform_t invTransform = toTransform(pose).inverse();

	bool found_target = false;
	double accumulated_cost = 0; // Estimates the cost-to-go for following the current plan
	for (int i = 0; i <= plan_size; i++) {
		transform_t tf_plan_pose = toTransform(plan_pose) * invTransform;
		// We don't care about collisions on already-executed parts of the plan
		bool plan_pose_collides = (i >= plan_idx);
		if (USE_MAP) {
			plan_pose_collides &= map.hasPointWithin(plan_pose, PLAN_COLLISION_DIST);
		} else {
			plan_pose_collides &= util::collides(tf_plan_pose, lidar_scan, PLAN_COLLISION_DIST);
		}
		if (plan_pose_collides) {
			// We'll replan next timestep
			accumulated_cost += INFINITE_COST;
			// TODO we should have a more sophisticated way of deciding whether a collision
			// is imminent.
			if (dist(plan_pose, pose, 0.0) < PLAN_COLLISION_STOP_DIST) {
				log(LOG_WARN, "Collision imminent! Stopping the rover!\n");
				driveTarget = pose; // Don't move
				found_target = true;
			}
		}
		if (i >= plan_idx && !found_target && dist(plan_pose, pose, 1.0) > FOLLOW_DIST) {
			found_target = true;
			plan_idx = i;
			accumulated_cost += planCostFromIndex(plan, i);
			driveTarget = plan_pose;
			if (dist(plan_pose, pose, 1.0) > 2 * FOLLOW_DIST) {
				log(LOG_WARN,
					"Drifted too far from plan. Stopping the rover and replanning.\n");
				driveTarget = pose; // Don't move
				accumulated_cost += INFINITE_COST;
			}
		} else {
			// Send current plan_pose to visualization
			const pose_t curr_plan_transformed = poseToDraw(plan_pose, pose);
			rospub::publish(curr_plan_transformed, rospub::PointPub::PLAN_VIZ);
		}
		if (i < plan_size) {
			action_t action = plan.row(i);
			plan_pose(2) += action(0);
			plan_pose(0) += action(1) * cos(plan_pose(2));
			plan_pose(1) += action(1) * sin(plan_pose(2));
		}
	}
	pose_t final_plan_pose = plan_pose; // Renaming for clarity
	if (!found_target) {
		// We reached the end of the plan without finding a drive target far enough away.
		// Set the drive target to the last point on the plan.
		plan_idx = plan_size - 1;
		driveTarget = final_plan_pose;
	}

	if (dist(final_plan_pose, plan_target, 0.0) > 2 * PLANNING_GOAL_REGION_RADIUS) {
		log(LOG_WARN, "Plan target moved (%f %f %f -> %f %f %f). Replanning.\n",
			final_plan_pose(0), final_plan_pose(1), final_plan_pose(2), plan_target(0),
			plan_target(1), plan_target(2));
		accumulated_cost += INFINITE_COST;
	}
	plan_cost = accumulated_cost;

	return driveTarget;
}

pose_t Autonomous::getGPSTargetPose() const {
	pose_t ret{urc_targets[leg_idx].approx_GPS(0), urc_targets[leg_idx].approx_GPS(1), 0.0};
	return ret;
}

static bool printLandmarks(const points_t& landmarks, int log_level) {
	std::ostringstream stream;
	bool found_one = false;
	for (size_t i = 0; i < landmarks.size(); i++) {
		point_t lm = landmarks[i];
		if (lm[2] != 0 && lm.topRows(2).norm() != 0) {
			found_one = true;
			stream << "Landmark " << i << " at {" << lm[0] << ", " << lm[1] << "} ";
		}
	}
	if (found_one)
		stream << std::endl;
	log(log_level, stream.str().c_str());
	return found_one;
}

static bool printLandmarks(const landmarks_t& landmarks, int log_level) {
	std::ostringstream stream;
	bool found_one = false;
	for (size_t i = 0; i < landmarks.size(); i++) {
		auto lmData = landmarks[i];
		if (lmData && lmData.getData().topRows(2).norm() != 0) {
			point_t lm = lmData.getData();
			found_one = true;
			stream << "Landmark " << i << " at {" << lm[0] << ", " << lm[1] << "} ";
		}
	}
	if (found_one)
		stream << std::endl;
	log(log_level, stream.str().c_str());
	return found_one;
}


#include <memory>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <ceres-error-terms/common.h>
#include <ceres-error-terms/inertial-error-term.h>
#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <imu-integrator/imu-integrator.h>
#include <maplab-common/gravity-provider.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

using namespace ceres_error_terms;  // NOLINT

class PosegraphErrorTerms : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    rot0_.coeffs() << 0, 0, sqrt(2) / 2, sqrt(2) / 2;
    pos0_ << 0, 0, 0;
    velocity0_ << 0, 2, 0;

    common::GravityProvider gravity_provider(
        common::locations::kAltitudeZurichMeters,
        common::locations::kLatitudeZurichDegrees);
    gravity_magnitude_ = gravity_provider.getGravityMagnitude();

    // Use integrator to obtain ground truth states for vertices.
    imu_integrator::ImuIntegratorRK4 integrator(0, 0, 0, 0, gravity_magnitude_);

    Eigen::Matrix<double, imu_integrator::kStateSize, 1> current_state;
    Eigen::Matrix<double, 2 * imu_integrator::kImuReadingSize, 1>
        debiased_imu_readings;
    Eigen::Matrix<double, imu_integrator::kStateSize, 1> next_state;
    Eigen::Matrix<double, imu_integrator::kErrorStateSize,
                  imu_integrator::kErrorStateSize>
        next_phi;
    Eigen::Matrix<double, imu_integrator::kErrorStateSize,
                  imu_integrator::kErrorStateSize>
        next_cov;

    current_state.setZero();
    current_state.block<4, 1>(imu_integrator::kStateOrientationOffset, 0) =
        rot0_.coeffs();
    current_state.block<3, 1>(imu_integrator::kStateVelocityOffset, 0) =
        velocity0_;
    current_state.block<3, 1>(imu_integrator::kStatePositionOffset, 0) = pos0_;

    debiased_imu_readings << 3, 0, gravity_magnitude_, 0, 0.2, 0, 3, 0,
        gravity_magnitude_, 0, 0.2, 0;

    const double delta_time_seconds = 2.0;
    integrator.integrate(
        current_state, debiased_imu_readings, delta_time_seconds, &next_state,
        &next_phi, &next_cov);

    rot1_.coeffs() =
        next_state.block<4, 1>(imu_integrator::kStateOrientationOffset, 0);
    velocity1_ =
        next_state.block<3, 1>(imu_integrator::kStateVelocityOffset, 0);
    pos1_ = next_state.block<3, 1>(imu_integrator::kStatePositionOffset, 0);

    current_state.swap(next_state);
    integrator.integrate(
        current_state, debiased_imu_readings, delta_time_seconds, &next_state,
        &next_phi, &next_cov);

    rot2_.coeffs() =
        next_state.block<4, 1>(imu_integrator::kStateOrientationOffset, 0);
    velocity2_ =
        next_state.block<3, 1>(imu_integrator::kStateVelocityOffset, 0);
    pos2_ = next_state.block<3, 1>(imu_integrator::kStatePositionOffset, 0);

    accel_bias0_.setZero();
    accel_bias1_.setZero();
    accel_bias2_.setZero();
    gyro_bias0_.setZero();
    gyro_bias1_.setZero();
    gyro_bias2_.setZero();

    imu_timestamps_01_ << 0, delta_time_seconds * 1e9;
    imu_data_01_ << 3, 3, 0, 0, gravity_magnitude_, gravity_magnitude_, 0, 0,
        0.2, 0.2, 0, 0;

    imu_timestamps_12_ << delta_time_seconds * 1e9,
        2 * delta_time_seconds * 1e9;
    imu_data_12_ << 3, 3, 0, 0, gravity_magnitude_, gravity_magnitude_, 0, 0,
        0.2, 0.2, 0, 0;
  }

  void addResidual();
  void solve();

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;

  Eigen::Matrix<int64_t, 1, 2> imu_timestamps_01_;
  Eigen::Matrix<double, 6, 2> imu_data_01_;

  Eigen::Matrix<int64_t, 1, 2> imu_timestamps_12_;
  Eigen::Matrix<double, 6, 2> imu_data_12_;

  Eigen::Quaterniond rot0_;
  Eigen::Quaterniond rot1_;
  Eigen::Quaterniond rot2_;
  Eigen::Vector3d pos0_;
  Eigen::Vector3d pos1_;
  Eigen::Vector3d pos2_;
  Eigen::Matrix<double, 7, 1> pose0_;
  Eigen::Matrix<double, 7, 1> pose1_;
  Eigen::Matrix<double, 7, 1> pose2_;

  Eigen::Vector3d accel_bias0_;
  Eigen::Vector3d accel_bias1_;
  Eigen::Vector3d accel_bias2_;
  Eigen::Vector3d gyro_bias0_;
  Eigen::Vector3d gyro_bias1_;
  Eigen::Vector3d gyro_bias2_;
  Eigen::Vector3d velocity0_;
  Eigen::Vector3d velocity1_;
  Eigen::Vector3d velocity2_;

  double gravity_magnitude_;
};

void PosegraphErrorTerms::addResidual() {
  rot0_.normalize();
  rot1_.normalize();
  rot2_.normalize();
  pose0_ << rot0_.coeffs(), pos0_;
  pose1_ << rot1_.coeffs(), pos1_;
  pose2_ << rot2_.coeffs(), pos2_;

  ceres::CostFunction* inertial_term_cost01 =
      new ceres_error_terms::InertialErrorTerm(
          imu_data_01_, imu_timestamps_01_, 1, 1, 1, 1, gravity_magnitude_);

  problem_.AddResidualBlock(
      inertial_term_cost01, NULL, pose0_.data(), gyro_bias0_.data(),
      velocity0_.data(), accel_bias0_.data(), pose1_.data(), gyro_bias1_.data(),
      velocity1_.data(), accel_bias1_.data());

  ceres::CostFunction* inertial_term_cost12 =
      new ceres_error_terms::InertialErrorTerm(
          imu_data_12_, imu_timestamps_12_, 1, 1, 1, 1, gravity_magnitude_);

  problem_.AddResidualBlock(
      inertial_term_cost12, NULL, pose1_.data(), gyro_bias1_.data(),
      velocity1_.data(), accel_bias1_.data(), pose2_.data(), gyro_bias2_.data(),
      velocity2_.data(), accel_bias2_.data());

  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;
  problem_.SetParameterization(pose0_.data(), pose_parameterization);
  problem_.SetParameterization(pose1_.data(), pose_parameterization);
  problem_.SetParameterization(pose2_.data(), pose_parameterization);

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(gyro_bias2_.data());
  problem_.SetParameterBlockConstant(accel_bias2_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity2_.data());
  problem_.SetParameterBlockConstant(pose0_.data());
  problem_.SetParameterBlockConstant(pose2_.data());
}

void PosegraphErrorTerms::solve() {
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 500;
  options.gradient_tolerance = 1e-50;
  options.function_tolerance = 1e-50;
  options.parameter_tolerance = 1e-50;
  options.num_threads = 8;
  options.num_linear_solver_threads = 8;

  ceres::Solve(options, &problem_, &summary_);

  LOG(INFO) << summary_.message;
  LOG(INFO) << summary_.BriefReport();
}

TEST_F(PosegraphErrorTerms, InertialTerm3KeyframesZeroCost) {
  addResidual();
  solve();

  EXPECT_LT(summary_.final_cost, 1e-12);
}

TEST_F(PosegraphErrorTerms, InertialTerm3KeyframesPositionNoise) {
  Eigen::Vector3d true_pos1 = pos1_;
  pos1_ += Eigen::Vector3d(0.1, -0.2, 0.13);

  addResidual();
  solve();

  EXPECT_NEAR_EIGEN(pose1_.tail(3), true_pos1, 1e-6);
  EXPECT_LT(summary_.final_cost, 1e-12);
}

TEST_F(PosegraphErrorTerms, InertialTerm3KeyframesRotationNoise) {
  Eigen::Vector4d true_rot1 = rot1_.coeffs();
  rot1_.coeffs() += Eigen::Vector4d(0.1, -0.2, 0.13, 0.2);
  rot1_.normalize();

  addResidual();
  solve();

  EXPECT_NEAR_EIGEN(pose1_.head(4), true_rot1, 1e-6);
  EXPECT_LT(summary_.final_cost, 1e-12);
}

TEST_F(PosegraphErrorTerms, InertialTerm3KeyframesVelocityNoise) {
  Eigen::Vector3d true_velocity1 = velocity1_;
  velocity1_ += Eigen::Vector3d(0.1, -0.2, 0.13);

  addResidual();
  solve();

  EXPECT_NEAR_EIGEN(velocity1_, true_velocity1, 1e-6);
  EXPECT_LT(summary_.final_cost, 1e-12);
}

TEST_F(PosegraphErrorTerms, InertialTerm3KeyframesRotVelPosNoise) {
  Eigen::Vector3d true_velocity1 = velocity1_;
  velocity1_ += Eigen::Vector3d(0.1, -0.2, 0.13);

  Eigen::Vector4d true_rot1 = rot1_.coeffs();
  rot1_.coeffs() += Eigen::Vector4d(0.1, -0.2, 0.13, 0.2);
  rot1_.normalize();

  Eigen::Vector3d true_pos1 = pos1_;
  pos1_ += Eigen::Vector3d(0.1, -0.2, 0.13);

  addResidual();
  solve();

  EXPECT_NEAR_EIGEN(velocity1_, true_velocity1, 1e-6);
  EXPECT_NEAR_EIGEN(pose1_.head(4), true_rot1, 1e-6);
  EXPECT_NEAR_EIGEN(pose1_.tail(3), true_pos1, 1e-6);
  EXPECT_LT(summary_.final_cost, 1e-12);
}

MAPLAB_UNITTEST_ENTRYPOINT


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <nt2/table.hpp>
#include <nt2/include/functions/any.hpp>
#include <nt2/include/functions/seladd.hpp>
#include <nt2/include/functions/plus.hpp>
#include <nt2/include/functions/minus.hpp>
#include <nt2/include/functions/times.hpp>
#include <nt2/include/functions/is_less.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/expand_to.hpp>
#include <nt2/include/functions/colvect.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/arrayfun.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <vector>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

namespace mandelbrot
{

  struct step
  {
    template<class Sig> struct result;
    template<class This, class A0, class A1>
    struct result<This(A0,A1)>
    {
      typedef typename boost::dispatch::meta::
              as_integer<typename boost::dispatch::meta::strip<A0>::type
                        >::type type;
    };

    step(std::size_t const& n) : max_iter_(n) {}

    template<class T>
    typename result<step(T,T)>::type operator()(T const& a, T const& b) const
    {
      typedef typename result<step(T const&, T const&)>::type iter_type;
      typedef typename boost::simd::meta::scalar_of<T>::type s_type;
      iter_type iter = nt2::Zero<iter_type>();
      iter_type const o = nt2::One<iter_type>();
      T x = nt2::Zero<T>();
      T y = nt2::Zero<T>();
      T x2,y2,xy,m2;
      typename boost::simd::meta::as_logical<T>::type mask;
      std::size_t i = 0;
      bool flag;
      do
      {
        x2 = x*x;
        y2 = y*y;
        xy = s_type(2)  *x*y;
        x = x2 - y2 + a;
        y = xy + b;
        m2 = x2 + y2;
        mask= m2<s_type(4);
        iter = nt2::seladd(mask, iter, o);
        flag = nt2::any(mask);
        i++;
      }while(flag && i < 256);

      return iter;
    }

    std::size_t max_iter_;
  };
}

template<typename T> struct mandelbrot_nt2
{
  typedef T value_type;
  template<typename Setup>
  mandelbrot_nt2(Setup const& s)
                    :  h_(boost::fusion::at_c<0>(s))
                    ,  w_(boost::fusion::at_c<1>(s))
                    ,  a0_(boost::fusion::at_c<2>(s))
                    ,  a1_(boost::fusion::at_c<3>(s))
                    ,  b0_(boost::fusion::at_c<4>(s))
                    ,  b1_(boost::fusion::at_c<5>(s))
                    ,  max_iter_(boost::fusion::at_c<6>(s))
                    ,  size_(h_*w_)
                    ,  julia(max_iter_)
  {
    A.resize(nt2::of_size(h_,w_));
    B.resize(nt2::of_size(h_,w_));
    C.resize(nt2::of_size(h_,w_));

    A=nt2::expand_to(nt2::linspace(a0_,a1_,h_),h_,w_);
    B=nt2::expand_to(nt2::colvect(nt2::linspace(b0_,b1_,w_)),h_,w_);
  }

  void operator()()
  {
    C = arrayfun(julia, A, B);
  }

  friend std::ostream& operator<<(std::ostream& os, mandelbrot_nt2<T> const& p)
  {
    return os << "(" << p.h_ << " x " << p.w_ << ")";
  }

  std::size_t size() const { return size_; }

  private:
    std::size_t h_, w_;
    value_type a0_, a1_, b0_, b1_;
    std::size_t max_iter_, size_, step_size_, aligned_sz, it;
    mandelbrot::step julia;
    nt2::table<value_type> A, B;
    nt2::table<int> C;
};

NT2_REGISTER_BENCHMARK_TPL( mandelbrot_nt2, (float) )
{

  std::size_t hmin = args("hmin", 100);
  std::size_t hmax = args("hmax", 1600);
  std::size_t hstep = args("hstep", 2);
  std::size_t wmin = args("wmin", 100);
  std::size_t wmax = args("wmax",1600);
  std::size_t wstep = args("wstep", 2);
  T xmin = args("xmin", -1.5);
  T xmax = args("xmax", 1.5);
  T ymin = args("ymin", -1.5);
  T ymax = args("ymax", 1.5);
  T max_iter = args("max_iter", 256);

  run_during_with< mandelbrot_nt2<float> > ( 1.
                                          , and_( geometric(hmin,hmax,hstep)
                                                , geometric(wmin,wmax,wstep)
                                                , constant(xmin)
                                                , constant(xmax)
                                                , constant(ymin)
                                                , constant(ymax)
                                                , constant(max_iter)
                                                )
                                          , cycles_per_element<stats::median_>()
                                          );
}


#include "Calibration.h"
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <QDir>
#include <QTime>

using namespace std;
using namespace cv;

extern struct VISION_PARAM vision_param;

//https://github.com/CompileSense/StereoCamera
const int imageWidth = 752; //摄像头的分辨率
const int imageHeight = 480;
const int boardWidth = 9;                         //横向的角点数目
const int boardHeight = 6;                        //纵向的角点数据
const int boardCorner = boardWidth * boardHeight; //总的角点数据
const int squareSize = 25;                        //标定板黑白格子的大小 单位mm
const Size imageSize = Size(imageWidth, imageHeight);

const Size boardSize = Size(boardWidth, boardHeight);
Mat intrinsicL;                   //相机内参数
Mat distortion_coeffL;            //相机畸变参数
vector<Mat> rvecsL;               //旋转向量
vector<Mat> tvecsL;               //平移向量
vector<vector<Point2f>> cornersL; //各个图像找到的角点的集合 和objRealPoint 一一对应

Mat intrinsicR;                   //相机内参数
Mat distortion_coeffR;            //相机畸变参数
vector<Mat> rvecsR;               //旋转向量
vector<Mat> tvecsR;               //平移向量
vector<vector<Point2f>> cornersR; //各个图像找到的角点的集合 和objRealPoint 一一对应
vector<Mat> intrinsics, distortion_coeffs;

vector<vector<Point3f>> objRealPoint; //各副图像的角点的实际物理坐标集合

vector<Point2f> cornerL; //某一副图像找到的角点
vector<Point2f> cornerR; //某一副图像找到的角点

Mat R, T, E, F;                 //R 旋转矢量 T平移矢量 E本征矩阵 F基础矩阵
Mat Rl, Rr, Pl, Pr, Q;          //校正旋转矩阵R，投影矩阵P 重投影矩阵Q (下面有具体的含义解释）
Mat mapLx, mapLy, mapRx, mapRy; //映射表
Rect validROIL, validROIR;      //图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域

//相机内参矩阵
Mat cameraMatrixL;
Mat distCoeffL;
Mat cameraMatrixR;
Mat distCoeffR;

Matrix34d PS[CAM_NUM_MAX];
Matrix33d RS[CAM_NUM_MAX];
Vector3d TS[CAM_NUM_MAX];
Matrix34d RTS34d[CAM_NUM_MAX];
Matrix44d RTS44d[CAM_NUM_MAX];
vector<vector<Point2f>> corners; //各个图像找到的角点的集合 和objRealPoint 一一对应
vector<float> vrms;

Calibration::Calibration()
{
}

/*计算标定板上模块的实际物理坐标*/
static void calRealPoint(vector<vector<Point3f>> &obj, int boardwidth, int boardheight, int imgNumber, int squaresize)
{
    //  Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));
    vector<Point3f> imgpoint;
    for (int rowIndex = 0; rowIndex < boardheight; rowIndex++)
    {
        for (int colIndex = 0; colIndex < boardwidth; colIndex++)
        {
            //  imgpoint.at<Vec3f>(rowIndex, colIndex) = Vec3f(rowIndex * squaresize, colIndex*squaresize, 0);
            imgpoint.push_back(Point3f(rowIndex * squaresize, colIndex * squaresize, 0));
        }
    }
    for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
    {
        obj.push_back(imgpoint);
    }
}

QStringList Calibration::checkFile(QString path)
{
    char buff[100];
    QString dirPath;
    QStringList nameFilters;
    nameFilters << "*.png";
    QStringList files;
    for (int i = 0; i < setting->camNumber; i++)
    {
        dirPath = path + "/camera" + QString::number(i);
        if (EasyTool::isDirExist(dirPath) == false)
        {
            msg("calibr check not found " + dirPath + ",exit");
            return files;
        }
        else
        {
            if (i == 0)
            {
                QDir dir(dirPath);
                files = dir.entryList(nameFilters, QDir::Files | QDir::Readable, QDir::Name);
                if (files.size() < 1)
                {
                    msg("calibr check error,files.size < 1,exit");
                }
            }
            else
            {
                for (int ii = 0; ii < files.size(); ii++)
                {
                    if (EasyTool::isFileExist(dirPath + "/" + files.at(ii)) == false)
                    {
                        msg("calibr check not found " + dirPath + "/" + files.at(ii) + ",exit");
                        return files;
                    }
                }
            }
        }
    }
    return files;
}

void Calibration::calibrStart(QString path)
{
    int goodFrameCount = 0;
    Mat imgL, boardimgL;
    Mat imgR, boardimgR;

    Mat img;

    vector<QMap<uint8_t, vector<Point2f>>> camcorners;

    //指定亚像素计算迭代标注
    cv::TermCriteria criteria = cv::TermCriteria(
        cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS,
        40,
        0.01);

    msg("calibr start.");

    QStringList files = checkFile(path);
    if (files.size() == 0)
    {
        return;
    }

    for (int i = 0; i < setting->camNumber; i++)
    //for (int i = 0; i < 1; i++)
    {
        QMap<uint8_t, vector<Point2f>> corners;
        for (int ii = 0; ii < files.size(); ii++)
        {
            QString imgPath = path + "/camera" + QString::number(i) + "/" + files.at(ii);
            if (EasyTool::isFileExist(imgPath) == false)
            {
                msg("calibr not not found " + imgPath + ",exit");
                return;
            }
            img = imread(string((const char *)imgPath.toLocal8Bit()), IMREAD_GRAYSCALE);
            if (img.data)
            {
                vector<Point2f> corner;

                bool isFind = findChessboardCornersSB(img, boardSize, corner,
                                                      CALIB_CB_NORMALIZE_IMAGE | CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY);
                if (isFind)
                {
                    //cornerSubPix(img, corner, Size(5, 5), Size(-1, -1), criteria);
                    //在图像上画出角点
                    //drawChessboardCorners(img, boardSize, corner, isFind);
                    corners.insert(ii, corner);
                    msg("findChessboardCorners " + imgPath + " success " + QString::number(corner.size()));
                    // namedWindow(string((const char *)imgPath.toLocal8Bit()));
                    // imshow(string((const char *)imgPath.toLocal8Bit()), img);
                }
                else
                {
                    msg("findChessboardCorners " + imgPath + " fail");
                }
            }
        }
        camcorners.push_back(corners);
    }

    msg("camcorners size " + QString::number(camcorners.size()));

    if (camcorners.size() != setting->camNumber)
    {
        return;
    }

    for (int i = 0; i < camcorners.size(); i++)
    {
        msg("corners " + QString::number(i) + " : " + QString::number(camcorners.at(i).size()));
    }

    intrinsics.clear();
    distortion_coeffs.clear();
    for (int i = 0; i < camcorners.size(); i++)
    {
        goodFrameCount = camcorners.at(i).size();
        /*计算实际的校正点的三维坐标*/
        objRealPoint.clear();
        calRealPoint(objRealPoint, boardWidth, boardHeight, goodFrameCount, squareSize);
        corners.clear();
        for (int ii = 0; ii < files.size(); ii++)
        {
            if (camcorners.at(i).contains(ii))
            {
                corners.push_back(camcorners.at(i).find(ii).value());
            }
        }
        Mat intrinsic, distortion_coeff;
        vector<Mat> rvecs; //旋转向量
        vector<Mat> tvecs; //平移向量
        //calibrateCamera(objRealPoint, corners, Size(boardWidth, boardHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);
        intrinsics.push_back(intrinsic);
        distortion_coeffs.push_back(distortion_coeff);
    }

    for (int i = 0; i < camcorners.size() - 1; i++)
    {
        cornersL.clear();
        cornersR.clear();
        for (int ii = 0; ii < files.size(); ii++)
        {
            if (camcorners.at(i).contains(ii) && camcorners.at(i + 1).contains(ii))
            {
                cornersL.push_back(camcorners.at(i).find(ii).value());
                cornersR.push_back(camcorners.at(i + 1).find(ii).value());
            }
        }

        msg("cailbr : camera" + QString::number(i) + " to camera" + QString::number(i + 1) + " start");
        goodFrameCount = cornersL.size();

        /*计算实际的校正点的三维坐标*/
        objRealPoint.clear();
        calRealPoint(objRealPoint, boardWidth, boardHeight, goodFrameCount, squareSize);

        // calibrateCamera(objRealPoint, cornersL, Size(boardWidth, boardHeight), intrinsicL, distortion_coeffL, rvecsL, tvecsL, 0);
        // calibrateCamera(objRealPoint, cornersR, Size(boardWidth, boardHeight), intrinsicR, distortion_coeffR, rvecsR, tvecsR, 0);

        //标定摄像头
        // float rms = stereoCalibrate(objRealPoint, cornersL, cornersR,
        //                             intrinsics.at(i), distortion_coeffs.at(i),
        //                             intrinsics.at(i+1), distortion_coeffs.at(i+1),
        //                             Size(imageWidth, imageHeight), R, T, E, F,
        //                             CALIB_USE_INTRINSIC_GUESS,
        //                             TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 10000, 1e-20));

        QTime time;
        time.start();
        float rms = stereoCalibrate(objRealPoint, cornersL, cornersR,
                                    intrinsics.at(i), distortion_coeffs.at(i),
                                    intrinsics.at(i + 1), distortion_coeffs.at(i + 1),
                                    Size(imageWidth, imageHeight), R, T, E, F,
                                    0,
                                    TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 10, 1e-2));

        vrms.push_back(rms);

        msg("take time " + QString::number(time.elapsed() / 1000.0) + "s");

        msg("Stereo Calibration done with RMS error = " + QString::number(rms, 'f', 6));

        //立体校正的时候需要两幅图像共面并且行对准 以使得立体匹配更加的可靠
        //使得两幅图像共面的方法就是把两个摄像头的图像投影到一个公共成像面上，这样每幅图像从本图像平面投影到公共图像平面都需要一个旋转矩阵R
        //stereoRectify 这个函数计算的就是从图像平面投影都公共成像平面的旋转矩阵Rl,Rr。 Rl,Rr即为左右相机平面行对准的校正旋转矩阵。
        //左相机经过Rl旋转，右相机经过Rr旋转之后，两幅图像就已经共面并且行对准了。
        //其中Pl,Pr为两个相机的投影矩阵，其作用是将3D点的坐标转换到图像的2D点的坐标:P*[X Y Z 1]' =[x y w]
        //Q矩阵为重投影矩阵，即矩阵Q可以把2维平面(图像平面)上的点投影到3维空间的点:Q*[x y d 1] = [X Y Z W]。其中d为左右两幅图像的时差

        stereoRectify(intrinsics.at(i), distortion_coeffs.at(i), intrinsics.at(i + 1), distortion_coeffs.at(i + 1), imageSize, R, T, Rl, Rr, Pl, Pr, Q,
                      CALIB_ZERO_DISPARITY, -1, imageSize, &validROIL, &validROIR);

        // cout << "R: " << R << endl;
        // cout << "T: " << T << endl;
        // cout << "Pl: " << Pl << endl;
        // cout << "Pr: " << Pr << endl;

        if (i == 0)
        {
            RS[0] << 1, 0, 0,
                0, 1, 0,
                0, 0, 1;
            TS[0] << (double)0.0, (double)0.0, (double)0.0;
            RTS34d[0] = EasyTool::getRT34d(RS[0], TS[0]);
            RTS44d[0] = EasyTool::getRT44d(RS[0], TS[0]);

            Matrix33d cameraMatrix;
            cv2eigen(intrinsics.at(i), cameraMatrix);
            PS[i] = cameraMatrix * RTS34d[0];
        }

        cv2eigen(R, RS[i + 1]);
        cv2eigen(T, TS[i + 1]);
        //cv2eigen(Pr, PS[i+1]);

        {
            RTS44d[i + 1] = EasyTool::getRT44d(RS[i + 1], TS[i + 1]) * RTS44d[i];
            EasyTool::RT44d(RTS44d[i + 1], RS[i + 1], TS[i + 1]);
        }

        //Vector3d TMPT = TS[i + 1];
        RTS34d[i + 1] = EasyTool::getRT34d(RS[i + 1], TS[i + 1]);
        Matrix33d cameraMatrix;
        cv2eigen(intrinsics.at(i + 1), cameraMatrix);
        PS[i + 1] = cameraMatrix * RTS34d[i + 1];

        if (i == 0)
        {
            vision_param.R[i] << RS[i].transpose();
            vision_param.T[i] << TS[i];
            vision_param.P[i] << PS[i];

            msg("P" + QString::number(i) + ": " + EasyTool::MatToString(vision_param.P[i]));
            msg("R" + QString::number(i) + ": " + EasyTool::MatToString(vision_param.R[i]));
            msg("T" + QString::number(i) + ": " + EasyTool::MatToString(vision_param.T[i]));

            // cout << std::setprecision(16) << "P" << i << ": " << vision_param.P[i] << endl;
            // cout << std::setprecision(16) << "R" << i << ": " << vision_param.R[i] << endl;
            // cout << std::setprecision(16) << "T" << i << ": " << vision_param.T[i] << endl;
        }

        vision_param.R[i + 1] << RS[i + 1].transpose();
        vision_param.T[i + 1] << TS[i + 1];
        vision_param.P[i + 1] << PS[i + 1];

        msg("P" + QString::number(i + 1) + ": " + EasyTool::MatToString(vision_param.P[i + 1]));
        msg("R" + QString::number(i + 1) + ": " + EasyTool::MatToString(vision_param.R[i + 1]));
        msg("T" + QString::number(i + 1) + ": " + EasyTool::MatToString(vision_param.T[i + 1]));

        // cout << std::setprecision(16) << "P" << i + 1 << ": " << vision_param.P[i + 1] << endl;
        // cout << std::setprecision(16) << "R" << i + 1 << ": " << vision_param.R[i + 1] << endl;
        // cout << std::setprecision(16) << "T" << i + 1 << ": " << vision_param.T[i + 1] << endl;
    }

    for (int i = 0; i < camcorners.size() - 1; i++)
    {
        msg(QString::number(i) + " to " + QString::number(i+1) + " rms error = " + QString::number(vrms.at(i), 'f', 6));
    }
}

void Calibration::msg(QString msg)
{
    mlog->show(msg);
    emit logSignal(msg);
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <tf/tf.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

using namespace Eigen;

namespace DarkRoom {

    class Transform {
    public:
        /**
         * Queries the tf listener for the specified transform
         * @param from we whant the transformation from this frame
         * @param to another frame
         * @param transform the transform if available
         * @return true if available
         */
        bool getTransform(const char *from, const char *to, Matrix4d &transform);

        /**
         * Queries the tf listener for the specified transform
         * @param lighthouse
         * @param to another frame
         * @param transform the transform if available
         * @return true if available
         */
        bool getTransform(bool lighthouse, const char *to, Matrix4d &transform);

        /**
         * Queries the tf listener for the specified transform
         * @param lighthouse
         * @param from another frame
         * @param transform the transform if available
         * @return true if available
         */
        bool getTransform(const char *from, bool lighthouse, Matrix4d &transform);

        /**
         * Constructs 4x4 RT matrix from 6 pose parameters
         * @param RT filled with
         * @param pose the pose parameters (r,p,y,x,y,z)
         */
        void getRTmatrix(Matrix4d &RT, VectorXd &pose);

        void getTFtransform(VectorXd &x, tf::Transform &tf);

        void publishTF(tf::Transform &tf, const char *frame, const char *name);

    private:
        tf::TransformListener tf_listener;
        tf::TransformBroadcaster tf_broadcaster;
    };

}


//---------------------------------------------------------------------------------------------------------------------
//  Vertical Engineering Solutions
//---------------------------------------------------------------------------------------------------------------------
//
//  Copyright 2020 Vertical Engineering Solutions  - All Rights Reserved
//
//  Unauthorized copying of this file, via any medium is strictly prohibited Proprietary and confidential.
//
//  All information contained herein is, and remains the property of Vertical Engineering Solutions.  The
//  intellectual and technical concepts contained herein are proprietary to Vertical Engineering Solutions
//  and its suppliers and may be covered by UE and Foreign Patents, patents in process, and are protected
//  by trade secret or copyright law. Dissemination of this information or reproduction of this material is
//  strictly forbidden unless prior written permission is obtained from Adobe Systems Incorporated.
//
//---------------------------------------------------------------------------------------------------------------------
//
//  Maintainer: pramon@vengineerings.com
//
//---------------------------------------------------------------------------------------------------------------------

#include <QApplication>
#include <QVector>
#include "qcustomplot.h"

#include <pidpp/PID.h>

#include<iostream>

#include <boost/numeric/odeint.hpp>
using namespace boost::numeric::odeint;

typedef std::vector<double> state_type;
/* The rhs of x' = f(x) */
class FirstOrderSystem {
    public:
        FirstOrderSystem( double _c ) : c_(_c) { }

        void operator() ( const state_type &x , state_type &dxdt, const double _t ) {
            dxdt[0] = -1/c_ * x[0] + x[1];
        }

    private:
        double c_;
};

class SecondOrderSystem {
    public:
        SecondOrderSystem( double _a, double _b ) : a_(_a), b_(_b) { }

        void setU(double _u){
            u_ = _u;
        }

        void operator() ( const state_type &x , state_type &dxdt, const double _t ) {
            dxdt[0] = x[1];
            dxdt[1] = u_ - b_*x[0] -a_* x[1];
        }

    private:
        double a_, b_;
        double u_;
};

int main(int _argc, char** _argv){
    QApplication a(_argc, _argv);

    //-----------------------------------------------------------------------------------------------------------------
    // Simulation
    //-----------------------------------------------------------------------------------------------------------------

    float kp = 4.0;
    float ki = 5.0;
    float kd = 0.0;

    //-----------------------------------------------------------------------------------------------------------------
    // Free run
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateFree = {0, 0};
    std::vector<double> valuesFree;
    std::vector<double> stepFree;
    std::vector<double> timeFree;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateFree , 0.0 , 10.0 , 0.01 );

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                system.setU(1);
                stepFree.push_back(1);
            }else{
                stepFree.push_back(0);
            }

            stepper.do_step( system , stateFree , t , dt );
            timeFree.push_back(t);
            valuesFree.push_back(stateFree[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID no windup
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateNoWindup = {0, 0};
    std::vector<double> valuesNoWindup;
    std::vector<double> stepNoWindup;
    std::vector<double> timeNoWindup;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateNoWindup , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepNoWindup.push_back(1);
                system.setU(pid.update(stateNoWindup[0], dt));
            }else{
                stepNoWindup.push_back(0);
            }

            stepper.do_step( system , stateNoWindup , t , dt );
            timeNoWindup.push_back(t);
            valuesNoWindup.push_back(stateNoWindup[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID windup sat
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateWindupSat = {0, 0};
    std::vector<double> valuesWindupSat;
    std::vector<double> stepWindupSat;
    std::vector<double> timeWindupSat;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateWindupSat , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);
        pid.setAntiWindup(pidpp::PID::AntiWindupMethod::Saturation, {-0.2, 0.2});

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepWindupSat.push_back(1);
                system.setU(pid.update(stateWindupSat[0], dt));
            }else{
                stepWindupSat.push_back(0);
            }

            stepper.do_step( system , stateWindupSat , t , dt );
            timeWindupSat.push_back(t);
            valuesWindupSat.push_back(stateWindupSat[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID windup Back calculation
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateBackCalc = {0, 0};
    std::vector<double> valuesBackCalc;
    std::vector<double> stepBackCalc;
    std::vector<double> timeBackCalc;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateBackCalc , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);
        pid.setAntiWindup(pidpp::PID::AntiWindupMethod::BackCalculation, {1.2});

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepBackCalc.push_back(1);
                system.setU(pid.update(stateBackCalc[0], dt));
            }else{
                stepBackCalc.push_back(0);
            }

            stepper.do_step( system , stateBackCalc , t , dt );
            timeBackCalc.push_back(t);
            valuesBackCalc.push_back(stateBackCalc[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID windup clamp
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateClamp = {0, 0};
    std::vector<double> valuesClamp;
    std::vector<double> stepClamp;
    std::vector<double> timeClamp;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateClamp , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);
        pid.setAntiWindup(pidpp::PID::AntiWindupMethod::Clamping, {1});

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepClamp.push_back(1);
                system.setU(pid.update(stateClamp[0], dt));
            }else{
                stepClamp.push_back(0);
            }

            stepper.do_step( system , stateClamp , t , dt );
            timeClamp.push_back(t);
            valuesClamp.push_back(stateClamp[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PLOT
    //-----------------------------------------------------------------------------------------------------------------
    QCustomPlot plot;
    plot.setInteraction(QCP::iSelectPlottables, true);
    plot.legend->setVisible(true);
    plot.legend->setBrush(QBrush(QColor(255,255,255,230)));
    plot.setMinimumWidth(800);
    plot.setMinimumHeight(500);
    // Step
    QPen graphPen;
    graphPen.setColor(QColor(255,0,0));
    graphPen.setWidthF(4);
    plot.addGraph()->setPen(graphPen);
    plot.graph(0)->setData(QVector<double>::fromStdVector(timeFree), QVector<double>::fromStdVector(stepFree));
    plot.graph(0)->setName("Target Step");
    // Open step response
    graphPen.setColor(QColor(0,255,0));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(1)->setData(QVector<double>::fromStdVector(timeFree), QVector<double>::fromStdVector(valuesFree));
    plot.graph(1)->setName("Free form");
    // No wind up PID
    graphPen.setColor(QColor(0,0,255));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(2)->setData(QVector<double>::fromStdVector(timeNoWindup), QVector<double>::fromStdVector(valuesNoWindup));
    plot.graph(2)->setName("PID no antiwindup");

    // wind up sat PID
    graphPen.setColor(QColor(0,255,255));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(3)->setData(QVector<double>::fromStdVector(timeWindupSat), QVector<double>::fromStdVector(valuesWindupSat));
    plot.graph(3)->setName("with saturation");

    // wind up back calculation
    graphPen.setColor(QColor(255,0,255));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(4)->setData(QVector<double>::fromStdVector(timeBackCalc), QVector<double>::fromStdVector(valuesBackCalc));
    plot.graph(4)->setName("Back Calculation");

    // wind up Clamp
    graphPen.setColor(QColor(255,255,0));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(5)->setData(QVector<double>::fromStdVector(timeClamp), QVector<double>::fromStdVector(valuesClamp));
    plot.graph(5)->setName("Clamping");

    // give the axes some labels:
    plot.xAxis->setLabel("t (s)");
    plot.yAxis->setLabel("value (u)");
    // set axes ranges, so we see all data:
    plot.xAxis->setRange(0, 10);
    plot.yAxis->setRange(   -0.5, 2);
    plot.replot();
    plot.show();

    return a.exec();

}


#include <boost/math/distributions/find_location.hpp>


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#undef MTL_ASSERT_FOR_THROW // Don't wont to abort program when one solver fail

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

// #include <boost/test/minimal.hpp>

#define MTL_RUN_SOLVER( name, solver, args)				\
    {									\
	std::cout << "\n\n" << name << "\n";				\
	xs= 0.01;							\
        int codes= 0;							\
        itl::cyclic_iteration<double> iters(bs, 150, 1.e-4, 0.0, 10);     \
        try {								\
	    codes= solver args;						\
	} catch (const itl::unexpected_orthogonality&) {		\
	    std::cerr << "Unexpected_Orthogonality in solver!\n";	\
	}								\
        if (codes != 0) {						\
	    std::cerr << "Solver doesn't converge!!!\n";		\
	    failed++;							\
        } else								\
	    succeed++;							\
    }

int main(int, char**)
{
    // For a more realistic example set size to 1000 or larger
    const int size = 4, N = size * size;
    int       succeed= 0, failed= 0;

    typedef mtl::compressed2D<double>  matrix_s_type;
    matrix_s_type                                           As(N, N);
    laplacian_setup(As, size, size);
    mtl::dense_vector<double>                               xs(N, 1.0), bs(N);
    bs= As * xs;
    itl::pc::identity<matrix_s_type>                        I(As);
    itl::pc::ic_0<matrix_s_type>                            IC(As);
    itl::pc::ilu_0<matrix_s_type>                           ILU(As);
    const unsigned                                          ell= 6, restart= 8, s= ell;

    MTL_RUN_SOLVER("Bi-Conjugate Gradient", bicg, (As, xs, bs, I, iters));
    MTL_RUN_SOLVER("Bi-Conjugate Gradient Stabilized", bicgstab, (As, xs, bs, ILU, iters));
    MTL_RUN_SOLVER("Bi-Conjugate Gradient Stabilized(2)", bicgstab_2, (As, xs, bs, ILU, iters));
    MTL_RUN_SOLVER("Bi-Conjugate Gradient Stabilized(ell)", bicgstab_ell, (As, xs, bs, ILU, I, iters, ell));
    MTL_RUN_SOLVER("Conjugate Gradient", cg, (As, xs, bs, IC, iters));
    MTL_RUN_SOLVER("Conjugate Gradient Squared", cgs, (As, xs, bs, ILU, iters));
//     MTL_RUN_SOLVER("Generalized Minimal Residual method (without restart)", gmres_full, (As, xs, bs, I, I, iters, size));  //only N iterations
    MTL_RUN_SOLVER("Generalized Minimal Residual method with restart", gmres, (As, xs, bs, I, I, iters, restart));
    MTL_RUN_SOLVER("Induced Dimension Reduction on s dimensions (IDR(s))", idr_s, (As, xs, bs, ILU, I, iters, s));
    MTL_RUN_SOLVER("Quasi-minimal residual", qmr, (As, xs, bs, ILU, I, iters));
    MTL_RUN_SOLVER("Transposed-free Quasi-minimal residual", tfqmr, (As, xs, bs, ILU, I, iters));
    std::cout << succeed << " solvers succeeded and " << failed << " solvers failed.\n";

    return 0;
}


#include "gr/algorithms/match4pcsBase.h"
#include "gr/algorithms/FunctorSuper4pcs.h"
#include "gr/io/io.h"
#include "gr/utils/geometry.h"
#include <gr/algorithms/PointPairFilter.h>

#include <Eigen/Dense>

int main(int argc, char **argv) {
  using namespace gr;
  using namespace std;

  using TrVisitor = gr::DummyTransformVisitor;

  using MatcherType = gr::Match4pcsBase<gr::FunctorSuper4PCS, TrVisitor, gr::AdaptivePointFilter, gr::AdaptivePointFilter::Options>;
  using OptionType  = typename MatcherType::OptionsType;
  using SamplerType = gr::UniformDistSampler;

  vector<Point3D> set1, set2;
  vector<Eigen::Matrix2f> tex_coords1, tex_coords2;
  vector<typename Point3D::VectorType> normals1, normals2;
  vector<tripple> tris1, tris2;
  vector<std::string> mtls1, mtls2;

  IOManager iomanager;

  // dummy call, to test symbols accessibility
  iomanager.ReadObject("", set1, tex_coords1, normals1, tris1, mtls1);

  // check availability of the Utils functions
  if (tris1.size() == 0)
    Utils::CleanInvalidNormals(set1, normals1);

  // Our matcher.
  OptionType options;

  // Set parameters.
  typename MatcherType::MatrixType mat;
  double overlap (1);
  options.configureOverlap(overlap);

  typename Point3D::Scalar score = 0;

  constexpr Utils::LogLevel loglvl = Utils::Verbose;
  Utils::Logger logger(loglvl);
  SamplerType sampler;
  TrVisitor visitor;

  MatcherType matcher(options, logger);
  score = matcher.ComputeTransformation(set1, set2, mat, sampler, visitor);

  logger.Log<Utils::Verbose>( "Score: ", score );

  iomanager.WriteMatrix("output.map", mat.cast<double>(), IOManager::POLYWORKS);

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_NORM_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_NORM_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag norm_ of functor norm
     *        in namespace nt2::tag for toolbox algebra
    **/
    struct norm_ :  ext::abstract_<norm_> { typedef ext::abstract_<norm_> parent;   template<class... Args>   static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)   BOOST_AUTO_DECLTYPE_BODY( dispatching_norm_( ext::adl_helper(), static_cast<Args&&>(args)... ) ) };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::norm_, Site> dispatching_norm_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::norm_, Site>();
    }
    template<class... Args>
    struct impl_norm_;
  }

  /*!
    @brief Matricial or vectorial norm

    norm mimicks the behaviour of the correponding Matlab function

    @code
    norm(x, p)
    @endcode

    computes

    @code
    |--------------|-------------------|-----------------------|----------------------|
    |    Matlab p  |      NT2 p        |     matrix            |     vector           |
    |--------------|-------------------|-----------------------|----------------------|
    |  1           | 1 or nt2::one_    | max(sum(abs(x)))      |     sum(abs(x))      |
    |  2           | 2 or nt2::two_    |   max(svd(x))         | sum(abs(x).^2)^(1/2) |
    |  q           | q                 |       _               | sum(abs(x).^q)^(1/q) |
    |  inf         | nt2::inf_         | max(sum(abs(x')))     |    max(abs(x))       |
    |  -inf        | nt2::minf_        |       _               |    min(abs(x))       |
    |  'fro'       | nt2::fro_         | sqrt(sum(diag(x'*x))) | sum(abs(x).^2)^(1/2) |
    |--------------|-------------------|-----------------------|----------------------|
    @endcode

    where q is numeric finite positive different from the other possible values

    except 1 2 and q the NT2 calls are statically chosen at compile time.
    If you know exactly what you need, use preferentially mnorm for matrices
    and globalnorm for vectors.

    @see{globalnorm}, @see{mnorm}
  **/

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::norm_, norm, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::norm_, norm, 1)

}

#endif


// Copyright (C) 2013 The Regents of the University of California (Regents).
// 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.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/util.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <glog/logging.h>

#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/util/random.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

// For an E or F that is defined such that y^t * E * x = 0
double SquaredSampsonDistance(const Matrix3d& F,
                              const Vector2d& x,
                              const Vector2d& y) {
  const Vector3d epiline_x = F * x.homogeneous();
  const double numerator_sqrt = y.homogeneous().dot(epiline_x);
  const Vector4d denominator(y.homogeneous().dot(F.col(0)),
                             y.homogeneous().dot(F.col(1)),
                             epiline_x[0],
                             epiline_x[1]);

  // Finally, return the complete Sampson distance.
  return numerator_sqrt * numerator_sqrt / denominator.squaredNorm();
}

Eigen::Matrix3d CrossProductMatrix(const Vector3d& cross_vec) {
  Matrix3d cross;
  cross << 0.0, -cross_vec.z(), cross_vec.y(), cross_vec.z(), 0.0,
      -cross_vec.x(), -cross_vec.y(), cross_vec.x(), 0.0;
  return cross;
}

// Computes the normalization matrix transformation that centers image points
// around the origin with an average distance of sqrt(2) to the centroid.
// Returns the transformation matrix and the transformed points. This assumes
// that no points are at infinity.
bool NormalizeImagePoints(const std::vector<Vector2d>& image_points,
                          std::vector<Vector2d>* normalized_image_points,
                          Matrix3d* normalization_matrix) {
  Eigen::Map<const Matrix<double, 2, Eigen::Dynamic> > image_points_mat(
      image_points[0].data(), 2, image_points.size());

  // Allocate the output vector and map an Eigen object to the underlying data
  // for efficient calculations.
  normalized_image_points->resize(image_points.size());
  Eigen::Map<Matrix<double, 2, Eigen::Dynamic> > normalized_image_points_mat(
      (*normalized_image_points)[0].data(), 2, image_points.size());

  // Compute centroid.
  const Vector2d centroid(image_points_mat.rowwise().mean());

  // Calculate average RMS distance to centroid.
  const double rms_mean_dist =
      sqrt((image_points_mat.colwise() - centroid).squaredNorm() /
           image_points.size());

  // Create normalization matrix.
  const double norm_factor = sqrt(2.0) / rms_mean_dist;
  *normalization_matrix << norm_factor, 0, -1.0 * norm_factor * centroid.x(), 0,
      norm_factor, -1.0 * norm_factor * centroid.y(), 0, 0, 1;

  // Normalize image points.
  const Matrix<double, 3, Eigen::Dynamic> normalized_homog_points =
      (*normalization_matrix) * image_points_mat.colwise().homogeneous();
  normalized_image_points_mat = normalized_homog_points.colwise().hnormalized();

  return true;
}

// Projects a 3x3 matrix to the rotation matrix in SO3 space with the closest
// Frobenius norm. For a matrix with an SVD decomposition M = USV, the nearest
// rotation matrix is R = UV'.
Matrix3d ProjectToRotationMatrix(const Matrix3d& matrix) {
  Eigen::JacobiSVD<Matrix3d> svd(matrix,
                                 Eigen::ComputeFullU | Eigen::ComputeFullV);
  Matrix3d rotation_mat = svd.matrixU() * (svd.matrixV().transpose());

  // The above projection will give a matrix with a determinant +1 or -1. Valid
  // rotation matrices have a determinant of +1.
  if (rotation_mat.determinant() < 0) {
    rotation_mat *= -1.0;
  }

  return rotation_mat;
}

}  // namespace theia