#include <stdlib.h>
#include <ostream>
#include <stdio.h>
#include <fstream>
#include <yaml-cpp/yaml.h>
#include <vector>
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include <iostream>
#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>

#include <industrial_extrinsic_cal/basic_types.h>
#include <industrial_extrinsic_cal/camera_definition.h>
#include <industrial_extrinsic_cal/camera_yaml_parser.h>
#include <industrial_extrinsic_cal/targets_yaml_parser.h>
#include <industrial_extrinsic_cal/points_yaml_parser.h>
#include <industrial_extrinsic_cal/observation_data_point.h>
#include <industrial_extrinsic_cal/ceres_costs_utils.hpp>
#include <industrial_extrinsic_cal/ceres_costs_utils.h>
#define SHOW_DEBUG false

using std::ifstream;
using std::string;
using std::vector;
using boost::shared_ptr;
using industrial_extrinsic_cal::Pose6d;
using industrial_extrinsic_cal::P_BLOCK;
using industrial_extrinsic_cal::Camera;
using industrial_extrinsic_cal::Point3d;
using industrial_extrinsic_cal::ObservationDataPoint;
using industrial_extrinsic_cal::extractCameraIntrinsics;
using industrial_extrinsic_cal::transformPoint;
using industrial_extrinsic_cal::projectPntNoDistortion;
using industrial_extrinsic_cal::string2CostType;
using industrial_extrinsic_cal::Cost_function;

typedef boost::minstd_rand base_gen_type;
base_gen_type gen(42);
boost::normal_distribution<> normal_dist(0, 1);  // zero mean unit variance
boost::variate_generator<base_gen_type&, boost::normal_distribution<> > randn(gen, normal_dist);

typedef struct observation
{
  double x;
  double y;
} Observation;

typedef struct scene
{
  int scene_id;
  Pose6d target_pose;
  std::vector<std::string> camera_names;
} Scene;

/*! Brief defines a camera with extra structures to maintain statistics */
class CameraWithHistory : public Camera
{
public:
  int height;
  int width;
  vector<Pose6d> pose_history;
  double pose_position_mean_error;
  double pose_position_sigma;
  double pose_orientation_mean_error;
  double pose_orientation_sigma;
  double sigma_ax, sigma_ay, sigma_az;
  int num_observations;
};

/*! Brief defines a 3d point with extra structures to maintain statistics*/
class Point3dWithHistory
{
public:
  Point3d point;
  vector<double> x_history;
  vector<double> y_history;
  vector<double> z_history;
  double mean_x;
  double mean_y;
  double mean_z;
  double sigma_x;
  double sigma_y;
  double sigma_z;
  int num_observations;
};

// local prototypes
void printQTasH(double qx, double qy, double qz, double qw, double tx, double ty, double tz);
void printAATasH(double x, double y, double z, double tx, double ty, double tz);
void printAATasHI(double x, double y, double z, double tx, double ty, double tz);
void printAAasEuler(double x, double y, double z);
void printCamera(CameraWithHistory C, string words);
void computeObservations(vector<CameraWithHistory>& cameras, vector<Point3dWithHistory>& points, double noise,
                         double max_dist, vector<ObservationDataPoint>& observations);
void perturbCameras(vector<CameraWithHistory>& cameras, double position_noise, double degrees_noise);
void perturbPoints(vector<Point3dWithHistory>& points, double position_noise);
void perturbPose(Pose6d& pose, double position_noise, double degree_noise);
void independentlyPerturbCameras(vector<CameraWithHistory>& cameras);
void copyCamerasWoHistory(vector<CameraWithHistory>& original_cameras, vector<CameraWithHistory>& cameras);
void copyPoints(vector<Point3dWithHistory>& original_points, vector<Point3dWithHistory>& points);
void addPoseToHistory(vector<CameraWithHistory>& cameras, vector<CameraWithHistory>& original_cameras);
void computeHistoricPoseStatistics(vector<CameraWithHistory>& cameras);
void addPointsToHistory(vector<Point3dWithHistory>& points, vector<Point3dWithHistory>& original_points);
void computeHistoricPointStatistics(vector<Point3dWithHistory>& points);
void compareCameras(vector<CameraWithHistory>& C1, vector<CameraWithHistory>& C2);
void compareObservations(vector<ObservationDataPoint>& O1, vector<ObservationDataPoint>& O2);
bool parseScenes(std::string& scene_file_name, std::vector<Scene>& scenes);
void showScenes(vector<Scene>& scenes);
void addTargetPoints(int rows, int cols, double spacing, vector<Point3dWithHistory>& points, Pose6d& target_pose);
void computeObservationsFromScenes(vector<Scene>& scenes,               // pose of target, and which cameras observed it
                                   vector<CameraWithHistory>& cameras,  // list of cameras
                                   double target_pos_noise,  // perturb position of target by this amount then compute
                                   double target_degree_noise,  // peturb orientation of target by this amount then
                                                                // compute
                                   double image_noise,          // amount of noise to add to each observation
                                   vector<ObservationDataPoint>& observations,  // returned data
                                   vector<Point3dWithHistory>& points);         // returned target points
ObservationDataPoint predictObservationOfPoint(CameraWithHistory& C, Point3dWithHistory& P, Pose6d& target_pose,
                                               int scene_id, int point_id, double image_noise,
                                               bool& results_within_image);
void computeObservationsOfPoints(vector<CameraWithHistory>& cameras, vector<Point3dWithHistory>& points,
                                 Pose6d& target_pose, int scene_id, vector<ObservationDataPoint>& observations,
                                 double camera_position_noise = 0.0, double camera_degree_noise = 0.0,
                                 double image_noise = 0.0);

int main(int argc, char** argv)
{
  ros::init(argc, argv, "nist_analysis");

  vector<Point3d> original_points_nh;               // original as read in, no history
  vector<Point3dWithHistory> original_points;       // working copy of original points
  vector<shared_ptr<Camera> > original_cameras_sp;  // cameras with given poses
  vector<CameraWithHistory> original_cameras;       // cameras with given poses

  vector<Point3dWithHistory> points;                 // working copy of original points
  vector<Point3d> original_field_points_nh;          // test points for accuracy estimation
  vector<Point3dWithHistory> original_field_points;  // test points for accuracy estimation
  vector<Point3dWithHistory> field_points;           // working copy of original field points

  vector<CameraWithHistory> cameras;                         // working copy of original cameras
  vector<ObservationDataPoint> original_observations;        // noisless observations of original points
  vector<ObservationDataPoint> original_field_observations;  // noisless observations field points
  vector<ObservationDataPoint> observations;                 // working observations

  // TODO use a parameter file for these, or make them arguments
  // hard coded constants
  double camera_pos_noise = 0.10;   // 25 cm
  double camera_or_noise = 3.0;     // degrees
  double point_pos_noise = 0.1;     // 10cm perturbation prior to triangulation
  double target_pos_noise = 0.003;  // 3mm
  double target_or_noise = 1.0;     // degrees
  double image_noise = 0.1;         // pixels
  int num_test_cases = 100;         // each test case is a statistical sample

  google::InitGoogleLogging(argv[0]);
  if (argc != 4)
  {
    std::cerr << "usage: NistAnalysis <scene_file> <cameras_file> <fieldpoints_file\n";
    return 1;
  }

  std::string scene_file_name;
  ifstream scene_file(argv[1]);
  scene_file_name = argv[1];
  if (scene_file.fail())
  {
    ROS_ERROR_STREAM("ERROR can't open scene_file:  " << scene_file_name.c_str());
    return (false);
  }

  std::string cameras_file(argv[2]);
  std::string field_points_file(argv[3]);

  // read in the camera data, observation data and field points from yaml files
  parseCameras(cameras_file, original_cameras_sp);

  // copy each into a history capable version
  // NOTE: pullTransform() is called which installs the results from extrinsic calibration in the camera data
  std::string ref_frame("world");
  BOOST_FOREACH (shared_ptr<Camera>& current_camera, original_cameras_sp)
  {
    CameraWithHistory temp_camera;
    temp_camera.setTransformInterface(current_camera->getTransformInterface());
    temp_camera.camera_observer_ = current_camera->camera_observer_;
    temp_camera.trigger_ = current_camera->trigger_;
    temp_camera.camera_parameters_ = current_camera->camera_parameters_;
    temp_camera.camera_name_ = current_camera->camera_name_;
    temp_camera.intermediate_frame_ = current_camera->intermediate_frame_;
    temp_camera.is_moving_ = current_camera->is_moving_;
    temp_camera.height = current_camera->camera_parameters_.height;
    temp_camera.width = current_camera->camera_parameters_.width;
    temp_camera.setTIReferenceFrame(ref_frame);
    temp_camera.pullTransform();
    original_cameras.push_back(temp_camera);
  }

  // parse the scenes.
  // This is the output from the extrinsic calibration script
  // every scene has an id, a location of the target, and a list of cameras that observed the target
  std::vector<Scene> scenes;
  parseScenes(scene_file_name, scenes);
  if (SHOW_DEBUG) showScenes(scenes);

  // create original_points and original_observations from scenes
  // Note, these are nominal, no observation data is kept from calibration
  // it is expected that the nominal is good enough for analyis of the gometry
  computeObservationsFromScenes(scenes, original_cameras, 0.0, 0.0, 0.0, original_observations, original_points);

  // parse the field points used to generate the accuracy map
  parsePoints(field_points_file, original_field_points_nh);
  original_field_points.clear();
  for (int i = 0; i < original_field_points_nh.size(); i++)
  {
    Point3dWithHistory temp_point;
    temp_point.point.x = original_field_points_nh[i].x;
    temp_point.point.y = original_field_points_nh[i].y;
    temp_point.point.z = original_field_points_nh[i].z;
    original_field_points.push_back(temp_point);
  }

  // Setup problem 1: A test to see if camera extrinsics may be
  // recovered with fixed fiducials.  Create nominal observations of
  // points using camera parameters. Perturb camera positions and
  // orientations. Then, add all observation cost functions to problem
  // and solve.
  copyCamerasWoHistory(original_cameras, cameras);
  copyPoints(original_points, points);
  // compute noise free observations
  // these should be exactly the same as the original observations and original points
  computeObservationsFromScenes(scenes, cameras, 0.0, 0.0, 0.0, observations, points);

  // compute noisy observations
  computeObservationsFromScenes(scenes, cameras, 0.0, 0.0, image_noise, observations, points);

  // compare noisy observations to noise free observations
  ROS_INFO("Comparing %d noiseless observations to %d noisy observations", (int)original_observations.size(),
           (int)observations.size());
  compareObservations(original_observations, observations);  // shows noise is correct

  // regenerate noiseless observations with unperturbed cameras, note observations point to cameras parameters
  computeObservationsFromScenes(scenes, cameras, 0.0, 0.0, 0.0, observations, points);

  // perturb cameras
  ROS_INFO("perturbing cameras");
  perturbCameras(cameras, camera_pos_noise, camera_or_noise);
  // compare the original camera's locations to the perturbed ones
  ROS_INFO("comparing perturbed cameras to original cameras");
  compareCameras(original_cameras, cameras);  // shows how far apart they started

  // Create residuals for each observation in the bundle adjustment problem. The
  // parameters for cameras and points are added automatically.
  ceres::Problem problem1;
  // each observation is noisy and points to cameras whose extrinsics are perturbed
  // each point is a fiducial in a known/surveyed location
  ROS_INFO("solving with %d observations", (int)observations.size());
  int b1o = 0, b2o = 0, b3o = 0, b4o = 0, b5o = 0, b6o = 0, b7o = 0;
  BOOST_FOREACH (ObservationDataPoint obs, observations)
  {
    double* extrinsics = obs.camera_extrinsics_;
    Point3d point;
    point.x = obs.point_position_[0];
    point.y = obs.point_position_[1];
    point.z = obs.point_position_[2];
    double fx, fy, cx, cy, k1, k2, k3, p1, p2;
    industrial_extrinsic_cal::extractCameraIntrinsics(obs.camera_intrinsics_, fx, fy, cx, cy, k1, k2, k3, p1, p2);
    ceres::CostFunction* cost_function =
        industrial_extrinsic_cal::CameraReprjErrorPK::Create(obs.image_x_, obs.image_y_, fx, fy, cx, cy, point);
    problem1.AddResidualBlock(cost_function, NULL, extrinsics);
  }
  BOOST_FOREACH (CameraWithHistory& C, cameras)
  {
    ROS_INFO("%s has %d observations", C.camera_name_.c_str(), C.num_observations);
  }

  // solve problem
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 1000;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem1, &summary);
  if (SHOW_DEBUG)
  {  // display results
    std::cout << summary.FullReport() << "\n";
  }

  // see how well original camera poses were recoved with noisy observations
  ROS_INFO("comparing camera poses derived from noiseless measurements to actual");
  compareCameras(original_cameras, cameras);  // shows that cameras were recovered

  // compute observations of cameras at solved poses to noiseless observations
  computeObservationsFromScenes(scenes, cameras, 0.0, 0.0, 0.0, observations, points);
  ROS_INFO("comparing original observations to those made from poses derived from noisy measurments");
  compareObservations(original_observations, observations);  // ceres minimized observation noise?

  ROS_ERROR("verify recovery of camera poses for cameras with observations");

  // now, add noise to observations and see how well camera poses are recoverd
  copyCamerasWoHistory(original_cameras, cameras);
  computeObservationsFromScenes(scenes, cameras, 0.0, 0.0, image_noise, observations, points);
  perturbCameras(cameras, camera_pos_noise, camera_or_noise);
  ceres::Problem problem2;
  BOOST_FOREACH (ObservationDataPoint obs, observations)
  {
    double* extrinsics = obs.camera_extrinsics_;
    Point3d point;
    point.x = obs.point_position_[0];
    point.y = obs.point_position_[1];
    point.z = obs.point_position_[2];
    double fx, fy, cx, cy, k1, k2, k3, p1, p2;
    industrial_extrinsic_cal::extractCameraIntrinsics(obs.camera_intrinsics_, fx, fy, cx, cy, k1, k2, k3, p1, p2);
    ceres::CostFunction* cost_function =
        industrial_extrinsic_cal::CameraReprjErrorPK::Create(obs.image_x_, obs.image_y_, fx, fy, cx, cy, point);
    problem2.AddResidualBlock(cost_function, NULL, extrinsics);
  }
  // solve problem
  ceres::Solve(options, &problem2, &summary);
  if (SHOW_DEBUG)
  {  // display results
    std::cout << summary.FullReport() << "\n";
  }
  // see how well original camera poses were recoved with noisy observations
  ROS_INFO("comparing camera poses derived from noisy measurements to actual");
  compareCameras(original_cameras, cameras);  // shows that cameras were recovered

  ROS_ERROR("verify recovery of camera poses with reduced accuracy for cameras with observations");

  // Now we are ready to compute each camera's statistics
  // We answer the following questions
  // 1. what is the mean error in pose estimate for each camera
  // 2. what is the variance in pose estimate for each camera
  // These two values will be stored in camera.pos so that we can
  // use them to generate estimates of the accuracy of field point localization

  // compute poses for cameras for a bunch of test cases
  options.minimizer_progress_to_stdout = false;
  for (int test_case = 0; test_case < num_test_cases; test_case++)
  {
    copyCamerasWoHistory(original_cameras, cameras);
    computeObservationsFromScenes(scenes, cameras, 0.0, 0.0, image_noise, observations, points);
    perturbCameras(cameras, camera_pos_noise, camera_or_noise);

    ceres::Problem problem;
    BOOST_FOREACH (ObservationDataPoint obs, observations)
    {
      double x = obs.image_x_;
      double y = obs.image_y_;
      double* extrinsics = obs.camera_extrinsics_;
      Point3d point;
      point.x = obs.point_position_[0];
      point.y = obs.point_position_[1];
      point.z = obs.point_position_[2];
      double fx, fy, cx, cy, k1, k2, k3, p1, p2;
      industrial_extrinsic_cal::extractCameraIntrinsics(obs.camera_intrinsics_, fx, fy, cx, cy, k1, k2, k3, p1, p2);
      ceres::CostFunction* cost_function =
          industrial_extrinsic_cal::CameraReprjErrorPK::Create(x, y, fx, fy, cx, cy, point);
      problem.AddResidualBlock(cost_function, NULL, extrinsics);
    }
    ceres::Solve(options, &problem, &summary);
    addPoseToHistory(cameras, original_cameras);
  }  // end of test cases
  computeHistoricPoseStatistics(original_cameras);

  BOOST_FOREACH (CameraWithHistory& C, original_cameras)
  {
    ROS_INFO("%s\t:mean_pos_error = %7.5lf sigma=  %7.5lf angular %7.5lf number_observations = %d",
             C.camera_name_.c_str(), C.pose_position_mean_error, C.pose_position_sigma,
             C.pose_orientation_sigma * 180 / 3.1415, C.num_observations);
  }

  // at this point cameras each have an uncertianty model.
  // the uncertianty model is the variance in both position and orientation
  // We can now sample from this model to get uncertianty estimates for field points.
  // for .25 pixel error, most cameras have between  in position variance
  // and have less than .9 degrees in angular variance

  // Now we are ready to compute the field accuracy of the working volume
  // To to this we do the following.
  // Create a grid of locations representative of the field accuracy
  // For this optimization, the points will be solve, while the cameras are fixed
  // The cameras will be fixed at their original/nominal locations
  // But the observations will be made from the perturbed/real locations with each test case
  // Loop:
  //    Perturb the cameras using each camera's pose accuracy estimate
  //    compute noisy observations from those perturbed cameras
  //    Reset cameras to nominal locations, and hold fixed (at incorrect locations)
  //    optimize to find locations of all field points
  // end Loop
  // Compute point statistics for each field point

  // Compute noiseless observations of field points
  // determine how many times each point gets observed (need at least 2 to be triangulated)
  copyCamerasWoHistory(original_cameras, cameras);
  Pose6d target_pose;  // not used, but needed to fill out the observation data structure
  int scene_id = 0;    // not used
  computeObservationsOfPoints(cameras, original_field_points, target_pose, scene_id, original_field_observations);

  independentlyPerturbCameras(cameras);
  ROS_ERROR("comparing original camera poses to those using independent perturbations\n");
  compareCameras(original_cameras, cameras);  // will have some agregate overall statistics

  for (int test_case = 0; test_case < num_test_cases; test_case++)
  {
    copyPoints(original_field_points, field_points);  // working copy of field points
    copyCamerasWoHistory(original_cameras, cameras);  // working copy of cameras

    // noiseless ideal observations of cameras at their actual locations, these observation now point toward field
    // points
    computeObservationsOfPoints(cameras, field_points, target_pose, scene_id, observations);
    // noisey ideal observations of cameras at their actual locations
    // computeObservationsOfPoints(cameras, field_points, target_pose, scene_id, observations, 0.0, 0.0, image_noise);

    // cameras independently perturbed from their actual locations for triangulation
    independentlyPerturbCameras(cameras);
    perturbPoints(field_points, point_pos_noise);

    ceres::Problem problem;
    options.minimizer_progress_to_stdout = false;
    BOOST_FOREACH (ObservationDataPoint obs, observations)
    {
      double x = obs.image_x_;
      double y = obs.image_y_;
      double* points = obs.point_position_;  // this is perturbed from ideal by point_pos_noise
      double fx, fy, cx, cy, k1, k2, k3, p1, p2;
      industrial_extrinsic_cal::extractCameraIntrinsics(obs.camera_intrinsics_, fx, fy, cx, cy, k1, k2, k3, p1, p2);
      double tx, ty, tz, ax, ay, az;
      industrial_extrinsic_cal::extractCameraExtrinsics(obs.camera_extrinsics_, tx, ty, tz, ax, ay, az);
      Pose6d camera_pose(tx, ty, tz, ax, ay, az);  // note this is the pose from camera to world
      ceres::CostFunction* cost_function =
          industrial_extrinsic_cal::TriangulationError::Create(x, y, fx, fy, cx, cy, camera_pose);
      problem.AddResidualBlock(cost_function, NULL, points);
    }
    ceres::Solve(options, &problem, &summary);

    addPointsToHistory(field_points, original_field_points);
  }
  computeHistoricPointStatistics(original_field_points);
  FILE* fp = fopen("field_results.m", "w");
  fprintf(fp, "f = [\n");
  BOOST_FOREACH (Point3dWithHistory P, original_field_points)
  {
    printf("Mean Error= %9.5lf %9.5lf %9.5lf sigma= %9.5lf %9.5lf %9.5lf num_obs=%d\n", P.mean_x - P.point.x,
           P.mean_y - P.point.y, P.mean_z - P.point.z, P.sigma_x, P.sigma_y, P.sigma_z, P.num_observations);
    fprintf(fp, "%9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf %9.5lf;\n", P.point.x, P.point.y, P.point.z,
            P.mean_x, P.mean_y, P.mean_z, P.sigma_x, P.sigma_y, P.sigma_z);
  }
  fprintf(fp, "];\n");
  fclose(fp);
}

// print a quaternion plus position as a homogeneous transform
void printQTasH(double qx, double qy, double qz, double qw, double tx, double ty, double tz)
{
  double Rs11 = qw * qw + qx * qx - qy * qy - qz * qz;
  double Rs21 = 2.0 * qx * qy + 2.0 * qw * qz;
  double Rs31 = 2.0 * qx * qz - 2.0 * qw * qy;

  double Rs12 = 2.0 * qx * qy - 2.0 * qw * qz;
  double Rs22 = qw * qw - qx * qx + qy * qy - qz * qz;
  double Rs32 = 2.0 * qy * qz + 2.0 * qw * qx;

  double Rs13 = 2.0 * qx * qz + 2.0 * qw * qy;
  double Rs23 = 2.0 * qy * qz - 2.0 * qw * qx;
  double Rs33 = qw * qw - qx * qx - qy * qy + qz * qz;

  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", Rs11, Rs12, Rs13, tx);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", Rs21, Rs22, Rs23, ty);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", Rs31, Rs32, Rs33, tz);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", 0.0, 0.0, 0.0, 1.0);
}

// angle axis to homogeneous transform inverted
void printAATasH(double x, double y, double z, double tx, double ty, double tz)
{
  double R[9];
  double aa[3];
  aa[0] = x;
  aa[1] = y;
  aa[2] = z;
  ceres::AngleAxisToRotationMatrix(aa, R);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[0], R[3], R[6], tx);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[1], R[4], R[7], ty);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[2], R[5], R[8], tz);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", 0.0, 0.0, 0.0, 1.0);
}

// angle axis to homogeneous transform
void printAATasHI(double x, double y, double z, double tx, double ty, double tz)
{
  double R[9];
  double aa[3];
  aa[0] = x;
  aa[1] = y;
  aa[2] = z;
  ceres::AngleAxisToRotationMatrix(aa, R);
  double ix = -(tx * R[0] + ty * R[1] + tz * R[2]);
  double iy = -(tx * R[3] + ty * R[4] + tz * R[5]);
  double iz = -(tx * R[6] + ty * R[7] + tz * R[8]);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[0], R[1], R[2], ix);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[3], R[4], R[5], iy);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[6], R[7], R[8], iz);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", 0.0, 0.0, 0.0, 1.0);
}

void printAAasEuler(double x, double y, double z)
{
  double R[9];
  double aa[3];
  aa[0] = x;
  aa[1] = y;
  aa[2] = z;
  ceres::AngleAxisToRotationMatrix(aa, R);
  double rx = atan2(R[7], R[8]);
  double ry = atan2(-R[6], sqrt(R[7] * R[7] + R[8] * R[8]));
  double rz = atan2(R[3], R[0]);
  printf("rpy = %8.4f %8.4f %8.4f\n", rx, ry, rz);
}
void printCamera(CameraWithHistory C, string words)
{
  printf("%s\n", words.c_str());
  printf("Point in Camera frame to points in World frame Transform:\n");
  printAATasHI(C.camera_parameters_.angle_axis[0], C.camera_parameters_.angle_axis[1],
               C.camera_parameters_.angle_axis[2], C.camera_parameters_.position[0], C.camera_parameters_.position[1],
               C.camera_parameters_.position[2]);

  printf("Points in World frame to points in Camera frame transform\n");
  printAATasH(C.camera_parameters_.angle_axis[0], C.camera_parameters_.angle_axis[1],
              C.camera_parameters_.angle_axis[2], C.camera_parameters_.position[0], C.camera_parameters_.position[1],
              C.camera_parameters_.position[2]);

  printAAasEuler(C.camera_parameters_.angle_axis[0], C.camera_parameters_.angle_axis[1],
                 C.camera_parameters_.angle_axis[2]);
  printf("fx = %8.3lf fy = %8.3lf\n", C.camera_parameters_.focal_length_x, C.camera_parameters_.focal_length_y);
  printf("cx = %8.3lf cy = %8.3lf\n", C.camera_parameters_.center_x, C.camera_parameters_.center_y);
}

void computeObservations(vector<CameraWithHistory>& cameras, vector<Point3dWithHistory>& points, double noise,
                         double max_dist, vector<ObservationDataPoint>& observations)
{
  double pnoise = noise / sqrt(1.58085);  // magic number found by trial and error
  int n_observations = 0;
  observations.clear();

  // reset number of observations for the points
  BOOST_FOREACH (Point3dWithHistory& P, points)
  {
    P.num_observations = 0;
  }

  // create nominal observations of points using camera parameters
  BOOST_FOREACH (CameraWithHistory& C, cameras)
  {
    C.num_observations = 0;
    int j = 0;
    BOOST_FOREACH (Point3dWithHistory& P, points)
    {
      j++;
      // find image of point in camera
      double dx = C.camera_parameters_.position[0] - P.point.x;
      double dy = C.camera_parameters_.position[1] - P.point.y;
      double dz = C.camera_parameters_.position[2] - P.point.z;
      double distance = sqrt(dx * dx + dy * dy + dz * dz);
      if (distance < max_dist)
      {
        double fx, fy, cx, cy, k1, k2, k3, p1, p2;
        Observation obs;
        double* intrinsics = C.camera_parameters_.pb_intrinsics;
        industrial_extrinsic_cal::extractCameraIntrinsics(intrinsics, fx, fy, cx, cy, k1, k2, k3, p1, p2);
        P_BLOCK point = &P.point.pb[0];
        industrial_extrinsic_cal::projectPntNoDistortion(point, fx, fy, cx, cy, obs.x, obs.y);
        obs.x += pnoise * randn();  // add observation noise
        obs.y += pnoise * randn();
        if (obs.x >= 0 && obs.x < C.width && obs.y >= 0 && obs.y < C.height)
        {
          // save observation
          Pose6d target_pose, intermediate_frame;
          industrial_extrinsic_cal::Cost_function cost_type;
          ObservationDataPoint new_obs(C.camera_name_,
                                       "who_cares",                         // target name
                                       2,                                   // target type
                                       0,                                   // scene id (who cares)
                                       C.camera_parameters_.pb_intrinsics,  // camera intrinsics
                                       C.camera_parameters_.pb_extrinsics,  // camera extrinsics
                                       0,                                   // point_id
                                       target_pose.pb_pose,                 // target_pose
                                       point,                               // point position
                                       obs.x,                               // observation x
                                       obs.x,                               // observation y
                                       cost_type,                           // const Cost_function
                                       intermediate_frame);                 // Pose6d of intermediate frame
          observations.push_back(new_obs);
          C.num_observations++;
          P.num_observations++;
          n_observations++;
        }  // end if observation within field of view
      }    // end if point close enough to camera
    }      // end for each fiducial point
  }        // end for each camera
}  // end compute observations

void perturbCameras(vector<CameraWithHistory>& cameras, double position_noise, double degrees_noise)
{
  double pos_noise = position_noise / sqrt(2.274625);
  double radian_noise = degrees_noise * 3.1415 / (180.0 * sqrt(2.58047));
  BOOST_FOREACH (CameraWithHistory& C, cameras)
  {
    C.camera_parameters_.position[0] += pos_noise * randn();
    C.camera_parameters_.position[1] += pos_noise * randn();
    C.camera_parameters_.position[2] += pos_noise * randn();
    C.camera_parameters_.angle_axis[0] += radian_noise * randn();
    C.camera_parameters_.angle_axis[1] += radian_noise * randn();
    C.camera_parameters_.angle_axis[2] += radian_noise * randn();
  }
}

void perturbPoints(vector<Point3dWithHistory>& points, double position_noise)
{
  double pos_noise = position_noise / sqrt(2.274625);
  BOOST_FOREACH (Point3dWithHistory& P, points)
  {
    P.point.x += pos_noise * randn();
    P.point.y += pos_noise * randn();
    P.point.z += pos_noise * randn();
  }
}

void independentlyPerturbCameras(vector<CameraWithHistory>& cameras)
{
  BOOST_FOREACH (CameraWithHistory& C, cameras)
  {
    double pos_noise = C.pose_position_sigma / sqrt(2.274625);
    double radian_noise = C.pose_orientation_sigma / sqrt(2.58047);
    C.camera_parameters_.position[0] += pos_noise * randn();
    C.camera_parameters_.position[1] += pos_noise * randn();
    C.camera_parameters_.position[2] += pos_noise * randn();
    C.camera_parameters_.angle_axis[0] += radian_noise * randn();
    C.camera_parameters_.angle_axis[1] += radian_noise * randn();
    C.camera_parameters_.angle_axis[2] += radian_noise * randn();
  }
}

void copyCamerasWoHistory(vector<CameraWithHistory>& original_cameras, vector<CameraWithHistory>& cameras)
{
  cameras.clear();
  BOOST_FOREACH (CameraWithHistory& C, original_cameras)
  {
    CameraWithHistory newC = C;
    newC.pose_history.clear();
    newC.num_observations = 0;
    cameras.push_back(newC);
  }
}

void copyPoints(vector<Point3dWithHistory>& original_points, vector<Point3dWithHistory>& points)
{
  points.clear();
  BOOST_FOREACH (Point3dWithHistory& P, original_points)
  {
    points.push_back(P);
  }
}

void addPoseToHistory(vector<CameraWithHistory>& cameras, vector<CameraWithHistory>& original_cameras)
{
  if (cameras.size() != original_cameras.size()) ROS_ERROR_STREAM("number of cameras in vectors do not match");

  for (int i = 0; i < (int)cameras.size(); i++)
  {
    Pose6d pose;
    pose.x = cameras[i].camera_parameters_.position[0];
    pose.y = cameras[i].camera_parameters_.position[1];
    pose.z = cameras[i].camera_parameters_.position[2];
    pose.ax = cameras[i].camera_parameters_.angle_axis[0];
    pose.ay = cameras[i].camera_parameters_.angle_axis[1];
    pose.az = cameras[i].camera_parameters_.angle_axis[2];
    original_cameras[i].pose_history.push_back(pose);
  }
}

void computeHistoricPoseStatistics(vector<CameraWithHistory>& cameras)
{
  // calculate statistics of test case
  double mean_x, mean_y, mean_z, mean_ax, mean_ay, mean_az;
  double sigma_x, sigma_y, sigma_z, sigma_ax, sigma_ay, sigma_az;
  BOOST_FOREACH (CameraWithHistory& C, cameras)
  {
    mean_x = 0.0;
    mean_y = 0.0;
    mean_z = 0.0;
    mean_ax = 0.0;
    mean_ay = 0.0;
    mean_az = 0.0;
    BOOST_FOREACH (Pose6d& p, C.pose_history)
    {
      mean_x += p.x;
      mean_y += p.y;
      mean_z += p.z;
      mean_ax += p.ax;
      mean_ay += p.ay;
      mean_az += p.az;
    }
    int num_poses = (int)C.pose_history.size();
    mean_x = mean_x / num_poses;
    mean_y = mean_y / num_poses;
    mean_z = mean_z / num_poses;
    mean_ax = mean_ax / num_poses;
    mean_ay = mean_ay / num_poses;
    mean_az = mean_az / num_poses;

    double d_x = mean_x - C.camera_parameters_.position[0];
    double d_y = mean_y - C.camera_parameters_.position[1];
    double d_z = mean_z - C.camera_parameters_.position[2];
    C.pose_position_mean_error = sqrt(d_x * d_x + d_y * d_y + d_z * d_z);
    double d_ax = mean_ax - C.camera_parameters_.angle_axis[0];
    double d_ay = mean_ay - C.camera_parameters_.angle_axis[1];
    double d_az = mean_az - C.camera_parameters_.angle_axis[2];
    C.pose_orientation_mean_error = sqrt(d_ax * d_ax + d_ay * d_ay + d_az * d_az);

    sigma_x = 0.0;
    sigma_y = 0.0;
    sigma_z = 0.0;
    sigma_ax = 0.0;
    sigma_ay = 0.0;
    sigma_az = 0.0;
    BOOST_FOREACH (Pose6d& p, C.pose_history)
    {
      sigma_x += (mean_x - p.x) * (mean_x - p.x);
      ;
      sigma_y += (mean_y - p.y) * (mean_y - p.y);
      ;
      sigma_z += (mean_z - p.z) * (mean_z - p.z);
      ;
      sigma_ax += (mean_ax - p.ax) * (mean_ax - p.ax);
      ;
      sigma_ay += (mean_ay - p.ay) * (mean_ay - p.ay);
      ;
      sigma_az += (mean_az - p.az) * (mean_az - p.az);
      ;
    }
    sigma_x = sqrt(sigma_x / (num_poses - 1.0));
    sigma_y = sqrt(sigma_y / (num_poses - 1.0));
    sigma_z = sqrt(sigma_z / (num_poses - 1.0));
    C.sigma_ax = sigma_ax = sqrt(sigma_ax / (num_poses - 1.0));
    C.sigma_ay = sigma_ay = sqrt(sigma_ay / (num_poses - 1.0));
    C.sigma_az = sigma_az = sqrt(sigma_az / (num_poses - 1.0));

    double dv = sqrt(sigma_x * sigma_x + sigma_y * sigma_y + sigma_z * sigma_z);
    double av = sqrt(sigma_ax * sigma_ax + sigma_ay * sigma_ay + sigma_az * sigma_az);

    C.pose_position_sigma = dv;
    C.pose_orientation_sigma = av;
  }  // end for each camera
}

void computeHistoricPointStatistics(vector<Point3dWithHistory>& points)
{
  // calculate statistics of test case
  double mean_x, mean_y, mean_z;

  BOOST_FOREACH (Point3dWithHistory& P, points)
  {
    P.mean_x = P.mean_y = P.mean_z = 0.0;
    BOOST_FOREACH (double& p, P.x_history)
    {
      P.mean_x += p;
    }
    BOOST_FOREACH (double& p, P.y_history)
    {
      P.mean_y += p;
    }
    BOOST_FOREACH (double& p, P.z_history)
    {
      P.mean_z += p;
    }
    int num_values = (int)P.x_history.size();
    if (num_values == 0)
    {
      num_values = 1.0;
    }
    P.mean_x = P.mean_x / num_values;
    P.mean_y = P.mean_y / num_values;
    P.mean_z = P.mean_z / num_values;

    P.sigma_x = P.sigma_y = P.sigma_z = 0.0;
    BOOST_FOREACH (double& p, P.x_history)
    {
      P.sigma_x += (P.mean_x - p) * (P.mean_x - p);
      ;
    }
    BOOST_FOREACH (double& p, P.y_history)
    {
      P.sigma_y += (P.mean_y - p) * (P.mean_y - p);
      ;
    }
    BOOST_FOREACH (double& p, P.z_history)
    {
      P.sigma_z += (P.mean_z - p) * (P.mean_z - p);
      ;
    }
    double denom = num_values - 1.0;
    if (denom == 0 || P.num_observations == 0)
    {
      P.sigma_x = 1000;
      P.sigma_y = 1000;
      P.sigma_z = 1000;
    }
    else if (P.num_observations == 1)
    {  // can't triangulate with only one observation
      P.sigma_x = 0.4;
      P.sigma_y = 0.4;
      P.sigma_z = 0.4;
    }
    else
    {
      P.sigma_x = sqrt(P.sigma_x / denom);
      P.sigma_y = sqrt(P.sigma_y / denom);
      P.sigma_z = sqrt(P.sigma_z / denom);
    }
  }  // end for each point
}

void compareCameras(vector<CameraWithHistory>& C1, vector<CameraWithHistory>& C2)
{
  if (C1.size() != C2.size())
  {
    ROS_ERROR_STREAM("compareCameras() camera vectors different in length");
  }
  double d_x, d_y, d_z, dist;
  double mean_dist = 0;
  double sigma_dist = 0;
  double d_ax, d_ay, d_az, angle;
  double mean_angle = 0;
  double sigma_angle = 0;
  for (int i = 0; i < (int)C1.size(); i++)
  {
    if (C1[i].camera_name_ == C2[i].camera_name_)
    {
      d_x = C1[i].camera_parameters_.position[0] - C2[i].camera_parameters_.position[0];
      d_y = C1[i].camera_parameters_.position[1] - C2[i].camera_parameters_.position[1];
      d_z = C1[i].camera_parameters_.position[2] - C2[i].camera_parameters_.position[2];
      d_ax = C1[i].camera_parameters_.angle_axis[0] - C2[i].camera_parameters_.angle_axis[0];
      d_ay = C1[i].camera_parameters_.angle_axis[1] - C2[i].camera_parameters_.angle_axis[1];
      d_az = C1[i].camera_parameters_.angle_axis[2] - C2[i].camera_parameters_.angle_axis[2];
      dist = sqrt(d_x * d_x + d_y * d_y + d_z * d_z);
      angle = sqrt(d_ax * d_ax + d_ay * d_ay + d_az * d_az);
      ROS_INFO("camera %s position_diff = %7.5lf meters, orientation diff = %7.5lf degrees", C1[i].camera_name_.c_str(),
               dist, angle * 180 / 3.14);
      mean_dist += dist;
      mean_angle += angle;
    }
    else
    {
      printf("cameras not same\n");
    }
  }
  mean_dist = mean_dist / (int)C1.size();
  mean_angle = mean_angle / (int)C1.size();
  for (int i = 0; i < (int)C1.size(); i++)
  {
    d_x = C1[i].camera_parameters_.position[0] - C2[i].camera_parameters_.position[0];
    d_y = C1[i].camera_parameters_.position[1] - C2[i].camera_parameters_.position[1];
    d_z = C1[i].camera_parameters_.position[2] - C2[i].camera_parameters_.position[2];
    d_ax = C1[i].camera_parameters_.angle_axis[0] - C2[i].camera_parameters_.angle_axis[0];
    d_ay = C1[i].camera_parameters_.angle_axis[1] - C2[i].camera_parameters_.angle_axis[1];
    d_az = C1[i].camera_parameters_.angle_axis[2] - C2[i].camera_parameters_.angle_axis[2];
    dist = sqrt(d_x * d_x + d_y * d_y + d_z * d_z);
    angle = sqrt(d_ax * d_ax + d_ay * d_ay + d_az * d_az);
    sigma_dist += (dist - mean_dist) * (dist - mean_dist);
    sigma_angle += (angle - mean_angle) * (angle - mean_angle);
  }
  sigma_dist = sqrt(sigma_dist / C1.size());
  sigma_angle = sqrt(sigma_angle / C1.size());
  ROS_INFO("mean distance between camera poses = %9.5lf meters sigma= %9.5lf", mean_dist, sigma_dist);
  ROS_INFO("mean angle between camera poses = %9.5lf degrees sigma= %9.5lf", mean_angle * 180 / 3.1415,
           sigma_angle * 180 / 3.1415);
}

void compareObservations(vector<ObservationDataPoint>& O1, vector<ObservationDataPoint>& O2)
{
  if (O1.size() != O2.size())
  {
    ROS_ERROR_STREAM("compareObservations() vectors different in length");
  }
  double d_x, d_y, dist;
  double mean_dist = 0.0;
  double sigma_dist = 0.0;
  int n_compared = 0;
  for (int i = 0; i < (int)O1.size(); i++)
  {
    if (O1[i].camera_name_ == O2[i].camera_name_ && O1[i].point_id_ == O2[i].point_id_)
    {
      d_x = O1[i].image_x_ - O2[i].image_x_;
      d_y = O1[i].image_y_ - O2[i].image_y_;
      dist = sqrt(d_x * d_x + d_y * d_y);
      mean_dist += dist;
      n_compared++;
    }
  }
  mean_dist = mean_dist / n_compared;
  for (int i = 0; i < (int)O1.size(); i++)
  {
    if (O1[i].camera_name_ == O2[i].camera_name_ && O1[i].point_id_ == O2[i].point_id_)
    {
      d_x = O1[i].image_x_ - O2[i].image_x_;
      d_y = O1[i].image_y_ - O2[i].image_y_;
      dist = sqrt(d_x * d_x + d_y * d_y);
      sigma_dist += (dist - mean_dist) * (dist - mean_dist);
    }
  }
  sigma_dist = sqrt(sigma_dist / n_compared);
  ROS_INFO("mean distance between observations = %9.5lf pixels sigma= %9.5lf ", mean_dist, sigma_dist);
}

void addPointsToHistory(vector<Point3dWithHistory>& points, vector<Point3dWithHistory>& original_points)
{
  if (points.size() != original_points.size()) ROS_ERROR_STREAM("number of points in vectors do not match");

  for (int i = 0; i < (int)points.size(); i++)
  {
    original_points[i].x_history.push_back(points[i].point.x);
    original_points[i].y_history.push_back(points[i].point.y);
    original_points[i].z_history.push_back(points[i].point.z);
  }
}

bool parseScenes(std::string& scene_file_name, std::vector<Scene>& scenes)
{
  FILE* fp = fopen(scene_file_name.c_str(), "r");
  if (fp == NULL)
  {
    ROS_ERROR("Can't open file %s", scene_file_name.c_str());
    return (false);
  }
  int scene_id = 10;
  scenes.clear();
  while (fscanf(fp, "scene_id = %d/n", &scene_id) == 1)
  {
    Scene temp_scene;
    temp_scene.scene_id = scene_id;
    tf::Matrix3x3 R;
    double x, y, z;
    char dum[100], dum2[100], dum3[100];
    double z1, z2, z3, z4;
    fscanf(fp, "%s %s %s %lf %lf %lf %lf;/n", dum, dum2, dum3, &R[0][0], &R[0][1], &R[0][2], &x);
    fscanf(fp, "%lf %lf %lf %lf;/n", &R[1][0], &R[1][1], &R[1][2], &y);
    fscanf(fp, "%lf %lf %lf %lf;/n", &R[2][0], &R[2][1], &R[2][2], &z);
    fscanf(fp, "%lf %lf %lf %lf];/n", &z1, &z2, &z3, &z4);  // gobble last row
    temp_scene.target_pose.setBasis(R);
    tf::Vector3 v(x, y, z);
    temp_scene.target_pose.setOrigin(v);
    temp_scene.camera_names.clear();
    char temp_char_data[100];
    fscanf(fp, "%s %s %s %s", dum, dum2, dum3, temp_char_data);
    temp_scene.camera_names.push_back(std::string(temp_char_data));
    bool done = false;
    do
    {
      fscanf(fp, "%s", temp_char_data);
      if (temp_char_data[0] == ']')
      {
        done = true;
        char c = 'a';
        while (c != '\n')
          fscanf(fp, "%c", &c);
      }
      else
      {
        temp_scene.camera_names.push_back(std::string(temp_char_data));
      }
    } while (!done);
    ROS_INFO("scene %d has %d cameras", (int)scene_id, (int)temp_scene.camera_names.size());
    scenes.push_back(temp_scene);
  }  // end while there are more scenes
  fclose(fp);
  return (true);
}

void show_scenes(vector<Scene>& scenes)
{
  BOOST_FOREACH (Scene S, scenes)
  {
    ROS_INFO("scene_id %d", S.scene_id);
    S.target_pose.show("target pose:");
    BOOST_FOREACH (string s, S.camera_names)
    {
      ROS_INFO("%s", s.c_str());
    }
  }
}
void perturbPose(Pose6d& pose, double position_noise, double degree_noise)
{
  double pos_noise = position_noise / sqrt(2.274625);
  double radian_noise = degree_noise * 3.1415 / (180.0 * sqrt(2.58047));
  pose.x += pos_noise * randn();
  pose.y += pos_noise * randn();
  pose.z += pos_noise * randn();
  pose.ax += radian_noise * randn();
  pose.ay += radian_noise * randn();
  pose.ay += radian_noise * randn();
}

void computeObservationsOfPoints(vector<CameraWithHistory>& cameras, vector<Point3dWithHistory>& points,
                                 Pose6d& target_pose, int scene_id, vector<ObservationDataPoint>& observations,
                                 double camera_position_noise, double camera_degree_noise, double image_noise)

{
  observations.clear();
  BOOST_FOREACH (CameraWithHistory& C, cameras)
  {
    int q = 0;
    BOOST_FOREACH (Point3dWithHistory& P, points)
    {
      bool good_observation;
      ObservationDataPoint obs =
          predictObservationOfPoint(C, P, target_pose, scene_id, q++, image_noise, good_observation);
      if (good_observation)
      {
        observations.push_back(obs);
        P.num_observations++;
      }
    }
  }
}
void computeObservationsFromScenes(vector<Scene>& scenes,               // pose of target, and which cameras observed it
                                   vector<CameraWithHistory>& cameras,  // list of cameras
                                   double target_pos_noise,  // perturb position of target by this amount then compute
                                   double target_degree_noise,  // peturb orientation of target by this amount then
                                                                // compute
                                   double image_noise,          // amount of noise to add to each observation
                                   vector<ObservationDataPoint>& observations,  // returned data
                                   vector<Point3dWithHistory>& points)          // returned target points
{
  observations.clear();
  BOOST_FOREACH (CameraWithHistory& C, cameras)
    C.num_observations = 0;
  double pnoise = image_noise / sqrt(1.58085);  // magic number found by trial and error
  int rows = 10;
  int cols = 10;
  double spacing = .025;
  points.clear();
  BOOST_FOREACH (Scene& S, scenes)
  {
    // add a new set of target points to the vector of all points
    int target_point_offset = points.size();  // points[target_point_offset] is the first of the new points being added
    // add any target position and pose noise to target's pose
    if (target_pos_noise > 0.0 || target_degree_noise > 0.0)
    {
      perturbPose(S.target_pose, target_pos_noise, target_degree_noise);
    }
    addTargetPoints(rows, cols, spacing, points, S.target_pose);  // added in world coordinates
    // for each camera in scene
    BOOST_FOREACH (string camera_name, S.camera_names)
    {
      CameraWithHistory* current_camera_ptr = NULL;
      for (int i = 0; i < cameras.size(); i++)
      {
        if (cameras[i].camera_name_ == camera_name)
        {
          current_camera_ptr = &(cameras[i]);
        }
      }
      if (current_camera_ptr == NULL)
      {
        ROS_ERROR("Couldn't find camera %s", camera_name.c_str());
      }
      for (int i = target_point_offset; i < (int)points.size(); i++)
      {  // for each new point
        bool good_observation;
        ObservationDataPoint obs = predictObservationOfPoint(*current_camera_ptr, points[i], S.target_pose, S.scene_id,
                                                             i, image_noise, good_observation);
        if (good_observation)
        {
          observations.push_back(obs);
          current_camera_ptr->num_observations++;
          points[i].num_observations++;
        }
      }
    }  // end for each camera in scene
  }    // end for each scene
}  // end

ObservationDataPoint predictObservationOfPoint(CameraWithHistory& C, Point3dWithHistory& P, Pose6d& target_pose,
                                               int scene_id, int point_id, double image_noise,
                                               bool& results_within_image)
{
  double pnoise = image_noise / sqrt(1.58085);  // magic number found by trial and error
  double fx, fy, cx, cy, k1, k2, k3, p1, p2;
  double aa[3], tx[3];
  double ox, oy;
  double world_point[3];
  std::string t_name("modified_circle_grid");
  Pose6d dummy_target_pose;
  Pose6d dummy_intermediate_pose;
  std::string cost_type_string("CameraReprjErrorPK");
  Cost_function cost_type = string2CostType(cost_type_string);

  extractCameraIntrinsics(C.camera_parameters_.pb_intrinsics, fx, fy, cx, cy, k1, k2, k3, p1, p2);
  aa[0] = C.camera_parameters_.angle_axis[0];
  aa[1] = C.camera_parameters_.angle_axis[1];
  aa[2] = C.camera_parameters_.angle_axis[2];
  tx[0] = C.camera_parameters_.position[0];
  tx[1] = C.camera_parameters_.position[1];
  tx[2] = C.camera_parameters_.position[2];
  transformPoint(aa, tx, P.point.pb, world_point);              // move to world coordinates
  projectPntNoDistortion(world_point, fx, fy, cx, cy, ox, oy);  // project point into camera's image plane
  ox += pnoise * randn();                                       // add observation noise
  oy += pnoise * randn();
  results_within_image = true;
  if (ox < 0.0 || ox > C.camera_parameters_.width || oy < 0.0 || oy > C.camera_parameters_.height)
  {
    results_within_image = false;
  }
  ObservationDataPoint obs(C.camera_name_,
                           t_name,  // target_name
                           2,       // target type
                           scene_id, C.camera_parameters_.pb_intrinsics, C.camera_parameters_.pb_extrinsics, point_id,
                           target_pose.pb_pose, P.point.pb, ox, oy, cost_type, dummy_intermediate_pose, 0.0);
  return (obs);
}

void addTargetPoints(int rows, int cols, double spacing, vector<Point3dWithHistory>& points, Pose6d& target_pose)
{
  for (int i = 0; i < rows; i++)
  {
    for (int j = 0; j < cols; j++)
    {
      double target_point[3];
      double world_point[3];
      target_point[0] = j * spacing;
      target_point[1] = (rows - 1 - i) * spacing;
      target_point[2] = 0.0;
      poseTransformPoint(target_pose, target_point, world_point);  // move to world coordinates
      Point3dWithHistory point;
      point.point.x = world_point[0];
      point.point.y = world_point[1];
      point.point.z = world_point[2];
      points.push_back(point);
    }
  }
}


//------------------------------------------------------------------------------
/*
  This file is part of rippled: https://github.com/ripple/rippled
  Copyright (c) 2021 Ripple Labs Inc.

  Permission to use, copy, modify, and/or distribute this software for any
  purpose  with  or without fee is hereby granted, provided that the above
  copyright notice and this permission notice appear in all copies.

  THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
  WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
  MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
  ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
  ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

#include <ripple/app/tx/impl/NFTokenMint.h>
#include <ripple/basics/Expected.h>
#include <ripple/basics/Log.h>
#include <ripple/ledger/View.h>
#include <ripple/protocol/Feature.h>
#include <ripple/protocol/InnerObjectFormats.h>
#include <ripple/protocol/Rate.h>
#include <ripple/protocol/TxFlags.h>
#include <ripple/protocol/st.h>
#include <boost/endian/conversion.hpp>
#include <array>

namespace ripple {

NotTEC
NFTokenMint::preflight(PreflightContext const& ctx)
{
    if (!ctx.rules.enabled(featureNonFungibleTokensV1))
        return temDISABLED;

    if (auto const ret = preflight1(ctx); !isTesSuccess(ret))
        return ret;

    if (ctx.tx.getFlags() & tfNFTokenMintMask)
        return temINVALID_FLAG;

    if (auto const f = ctx.tx[~sfTransferFee])
    {
        if (f > maxTransferFee)
            return temBAD_NFTOKEN_TRANSFER_FEE;

        // If a non-zero TransferFee is set then the tfTransferable flag
        // must also be set.
        if (f > 0u && !ctx.tx.isFlag(tfTransferable))
            return temMALFORMED;
    }

    // An issuer must only be set if the tx is executed by the minter
    if (auto iss = ctx.tx[~sfIssuer]; iss == ctx.tx[sfAccount])
        return temMALFORMED;

    if (auto uri = ctx.tx[~sfURI])
    {
        if (uri->length() == 0 || uri->length() > maxTokenURILength)
            return temMALFORMED;
    }

    return preflight2(ctx);
}

uint256
NFTokenMint::createNFTokenID(
    std::uint16_t flags,
    std::uint16_t fee,
    AccountID const& issuer,
    nft::Taxon taxon,
    std::uint32_t tokenSeq)
{
    // An issuer may issue several NFTs with the same taxon; to ensure that NFTs
    // are spread across multiple pages we lightly mix the taxon up by using the
    // sequence (which is not under the issuer's direct control) as the seed for
    // a simple linear congruential generator.  cipheredTaxon() does this work.
    taxon = nft::cipheredTaxon(tokenSeq, taxon);

    // The values are packed inside a 32-byte buffer, so we need to make sure
    // that the endianess is fixed.
    flags = boost::endian::native_to_big(flags);
    fee = boost::endian::native_to_big(fee);
    taxon = nft::toTaxon(boost::endian::native_to_big(nft::toUInt32(taxon)));
    tokenSeq = boost::endian::native_to_big(tokenSeq);

    std::array<std::uint8_t, 32> buf{};

    auto ptr = buf.data();

    // This code is awkward but the idea is to pack these values into a single
    // 256-bit value that uniquely identifies this NFT.
    std::memcpy(ptr, &flags, sizeof(flags));
    ptr += sizeof(flags);

    std::memcpy(ptr, &fee, sizeof(fee));
    ptr += sizeof(fee);

    std::memcpy(ptr, issuer.data(), issuer.size());
    ptr += issuer.size();

    std::memcpy(ptr, &taxon, sizeof(taxon));
    ptr += sizeof(taxon);

    std::memcpy(ptr, &tokenSeq, sizeof(tokenSeq));
    ptr += sizeof(tokenSeq);
    assert(std::distance(buf.data(), ptr) == buf.size());

    return uint256::fromVoid(buf.data());
}

TER
NFTokenMint::preclaim(PreclaimContext const& ctx)
{
    // The issuer of the NFT may or may not be the account executing this
    // transaction. Check that and verify that this is allowed:
    if (auto issuer = ctx.tx[~sfIssuer])
    {
        auto const sle = ctx.view.read(keylet::account(*issuer));

        if (!sle)
            return tecNO_ISSUER;

        if (auto const minter = (*sle)[~sfNFTokenMinter];
            minter != ctx.tx[sfAccount])
            return tecNO_PERMISSION;
    }

    return tesSUCCESS;
}

TER
NFTokenMint::doApply()
{
    auto const issuer = ctx_.tx[~sfIssuer].value_or(account_);

    auto const tokenSeq = [this, &issuer]() -> Expected<std::uint32_t, TER> {
        auto const root = view().peek(keylet::account(issuer));
        if (root == nullptr)
            // Should not happen.  Checked in preclaim.
            return Unexpected(tecNO_ISSUER);

        // Get the unique sequence number for this token:
        std::uint32_t const tokenSeq = (*root)[~sfMintedNFTokens].value_or(0);
        {
            std::uint32_t const nextTokenSeq = tokenSeq + 1;
            if (nextTokenSeq < tokenSeq)
                return Unexpected(tecMAX_SEQUENCE_REACHED);

            (*root)[sfMintedNFTokens] = nextTokenSeq;
        }
        ctx_.view().update(root);
        return tokenSeq;
    }();

    if (!tokenSeq.has_value())
        return (tokenSeq.error());

    std::uint32_t const ownerCountBefore =
        view().read(keylet::account(account_))->getFieldU32(sfOwnerCount);

    // Assemble the new NFToken.
    SOTemplate const* nfTokenTemplate =
        InnerObjectFormats::getInstance().findSOTemplateBySField(sfNFToken);

    if (nfTokenTemplate == nullptr)
        // Should never happen.
        return tecINTERNAL;

    STObject newToken(
        *nfTokenTemplate,
        sfNFToken,
        [this, &issuer, &tokenSeq](STObject& object) {
            object.setFieldH256(
                sfNFTokenID,
                createNFTokenID(
                    static_cast<std::uint16_t>(ctx_.tx.getFlags() & 0x0000FFFF),
                    ctx_.tx[~sfTransferFee].value_or(0),
                    issuer,
                    nft::toTaxon(ctx_.tx[sfNFTokenTaxon]),
                    tokenSeq.value()));

            if (auto const uri = ctx_.tx[~sfURI])
                object.setFieldVL(sfURI, *uri);
        });

    if (TER const ret =
            nft::insertToken(ctx_.view(), account_, std::move(newToken));
        ret != tesSUCCESS)
        return ret;

    // Only check the reserve if the owner count actually changed.  This
    // allows NFTs to be added to the page (and burn fees) without
    // requiring the reserve to be met each time.  The reserve is
    // only managed when a new NFT page is added.
    if (auto const ownerCountAfter =
            view().read(keylet::account(account_))->getFieldU32(sfOwnerCount);
        ownerCountAfter > ownerCountBefore)
    {
        if (auto const reserve = view().fees().accountReserve(ownerCountAfter);
            mPriorBalance < reserve)
            return tecINSUFFICIENT_RESERVE;
    }
    return tesSUCCESS;
}

}  // namespace ripple


#include "formats/cp.hpp"
#include "atomistic.hpp"
#include "types.hpp"
#include "io.hpp"

#include <boost/format.hpp>
#include <boost/regex.hpp>
#include <boost/lexical_cast.hpp>

namespace formats {
namespace cp {

typedef boost::sregex_iterator RegIt;
typedef boost::smatch Match;
typedef const boost::regex Regex;

using boost::lexical_cast;
using namespace types;
namespace at = atomistic;

Spectrum & Spectrum::operator*=(Real factor){
    for(std::vector<at::EnergyLevels>::iterator it = this->spins.begin(); it != this->spins.end(); it++) {
        *it *= factor;
    }

    return *this;
}

void Spectrum::shift(Real deltaE) {
    for(std::vector<at::EnergyLevels>::iterator it = this->spins.begin(); it != this->spins.end(); it++) {
        it->shift(deltaE);
    }
}

void Spectrum::setFermiZero() {
    for(std::vector<at::EnergyLevels>::iterator it = this->spins.begin(); it != this->spins.end(); it++) {
        it->setFermiZero();
    }
}

at::EnergyLevels Spectrum::sumSpins() const {
    at::EnergyLevels e = at::EnergyLevels();
    // Spins may have different Fermi. The new Fermi is the average Fermi of all spins
    // and energy levels are shifted accordingly
    Uint counter = 0;
    Real fermiSum = 0;

    for(std::vector<at::EnergyLevels>::const_iterator it = this->spins.begin(); it != this->spins.end(); it++) {
        e.join(*it);
        ++counter;
    }

    return e;
}

void Spectrum::print() const {
    for(std::vector<at::EnergyLevels>::const_iterator it = this->spins.begin(); it != this->spins.end(); it++) {
        it->print();
    }
}

bool Spectrum::readFromCp(String filename) {
    String content;
    io::readFile(filename, content);

    RegIt occIt(
        content.begin(),
        content.end(),
        boost::regex("Eigenvalues (eV), kp =   1 , spin =  ([\\.\\-\\d\\s]*)"));
//    RegIt fermiIt(
//        content.begin(),
//        content.end(),
//        boost::regex("Fermi Energy \\[eV\\] :([\\.\\-\\d\\s]*)"));
//    RegIt unoccIt(
//        content.begin(),
//        content.end(),
//        boost::regex("Eigenvalues of the unoccupied subspace spin.*?iterations([\\.\\-\\d\\s]*)"));
    RegIt dummyIt;

    Regex fermiRegex("\\-?\\d\\.\\d{6}");
    Regex levelRegex("\\-?\\d+\\.\\d+");

    // Iterate over spins
    while(occIt != dummyIt) {
        String levelData = occIt->str();
        std::vector<Real> levels;
        RegIt levelIt(levelData.begin(), levelData.end(), levelRegex);
        while(levelIt != dummyIt) {
            levels.push_back(lexical_cast<Real>(levelIt->str()));
            ++levelIt;
        }

//        Match fermiMatch;
//        regex_search(fermiIt->str(), fermiMatch, fermiRegex);
//        Real fermi = lexical_cast<Real>(fermiMatch);
//        // fermi given in eV, want Ha = 2 Ry
//        fermi *= GSL_CONST_MKSA_ELECTRON_VOLT /
//            (2* GSL_CONST_MKSA_RYDBERG);

        at::EnergyLevels energyLevels = at::EnergyLevels(levels, 0);
        this->spins.push_back(energyLevels);

        ++occIt;
    }

    return true;
}

}
}


#include <openvslam_ros.h>

#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <openvslam/publish/map_publisher.h>
#include <Eigen/Geometry>

namespace openvslam_ros {
system::system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : SLAM_(cfg, vocab_file_path), cfg_(cfg), node_(std::make_shared<rclcpp::Node>("run_slam")), custom_qos_(rmw_qos_profile_default),
      mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
      pose_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("~/camera_pose", 1)) {
    custom_qos_.depth = 1;
    exec_.add_node(node_);
}

void system::publish_pose() {
    // SLAM get the motion matrix publisher
    auto cam_pose_wc = SLAM_.get_map_publisher()->get_current_cam_pose_wc();

    // Extract rotation matrix and translation vector from
    Eigen::Matrix3d rot = cam_pose_wc.block<3, 3>(0, 0);
    Eigen::Vector3d trans = cam_pose_wc.block<3, 1>(0, 3);
    Eigen::Matrix3d cv_to_ros;
    cv_to_ros << 0, 0, 1,
        -1, 0, 0,
        0, -1, 0;

    // Transform from CV coordinate system to ROS coordinate system on camera coordinates
    Eigen::Quaterniond quat(cv_to_ros * rot * cv_to_ros.transpose());
    trans = cv_to_ros * trans;

    // Create odometry message and update it with current camera pose
    nav_msgs::msg::Odometry pose_msg;
    pose_msg.header.stamp = node_->now();
    pose_msg.header.frame_id = "map";
    pose_msg.child_frame_id = "camera_link";
    pose_msg.pose.pose.orientation.x = quat.x();
    pose_msg.pose.pose.orientation.y = quat.y();
    pose_msg.pose.pose.orientation.z = quat.z();
    pose_msg.pose.pose.orientation.w = quat.w();
    pose_msg.pose.pose.position.x = trans(0);
    pose_msg.pose.pose.position.y = trans(1);
    pose_msg.pose.pose.position.z = trans(2);
    pose_pub_->publish(pose_msg);
}

mono::mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : system(cfg, vocab_file_path, mask_img_path) {
    sub_ = image_transport::create_subscription(
        node_.get(), "camera/image_raw", [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { callback(msg); }, "raw", custom_qos_);
}
void mono::callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
    const rclcpp::Time tp_1 = node_->now();
    const double timestamp = tp_1.seconds();

    // input the current frame and estimate the camera pose
    SLAM_.feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_);

    const rclcpp::Time tp_2 = node_->now();
    const double track_time = (tp_2 - tp_1).seconds();

    //track times in seconds
    track_times_.push_back(track_time);
}

stereo::stereo(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path,
               const bool rectify)
    : system(cfg, vocab_file_path, mask_img_path),
      rectifier_(rectify ? std::make_shared<openvslam::util::stereo_rectifier>(cfg) : nullptr),
      left_sf_(node_, "camera/left/image_raw"),
      right_sf_(node_, "camera/right/image_raw"),
      sync_(left_sf_, right_sf_, 10) {
    sync_.registerCallback(&stereo::callback, this);
}

void stereo::callback(const sensor_msgs::msg::Image::ConstPtr& left, const sensor_msgs::msg::Image::ConstPtr& right) {
    auto leftcv = cv_bridge::toCvShare(left)->image;
    auto rightcv = cv_bridge::toCvShare(right)->image;
    if (leftcv.empty() || rightcv.empty()) {
        return;
    }

    if (rectifier_) {
        rectifier_->rectify(leftcv, rightcv, leftcv, rightcv);
    }

    const rclcpp::Time tp_1 = node_->now();
    const double timestamp = tp_1.seconds();

    // input the current frame and estimate the camera pose
    SLAM_.feed_stereo_frame(leftcv, rightcv, timestamp, mask_);

    const rclcpp::Time tp_2 = node_->now();
    const double track_time = (tp_2 - tp_1).seconds();

    //track times in seconds
    track_times_.push_back(track_time);
}
} // namespace openvslam_ros


#include "trajopt/common.hpp"
#include <openrave-core.h>
#include "trajopt/collision_checker.hpp"
#include "osgviewer/osgviewer.hpp"
#include "utils/eigen_conversions.hpp"
#include "trajopt/utils.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

using namespace OpenRAVE;
using namespace std;
using namespace trajopt;
using namespace util;
using namespace Eigen;

typedef Matrix<double,6,1> Vector6d;
typedef Matrix<double,6,Eigen::Dynamic> Matrix6Xd;
typedef Matrix<double,6,6> Matrix6d;
typedef Matrix<double,12,1> Vector12d;
typedef Matrix<double,12,Eigen::Dynamic> Matrix12Xd;
typedef Matrix<double,12,12> Matrix12d;

CollisionCheckerPtr cc;
vector<GraphHandlePtr> handles;
EnvironmentBasePtr env;
OSGViewerPtr viewer;

// copied from rave_utils
RobotBase::ManipulatorPtr GetManipulatorByName(RobotBase& robot, const std::string& name) {
	vector<RobotBase::ManipulatorPtr> manips = robot.GetManipulators();
	BOOST_FOREACH(RobotBase::ManipulatorPtr& manip, manips) {
		if (manip->GetName()==name) return manip;
	}
	return RobotBase::ManipulatorPtr();
}

// copied from problem_description
BeliefRobotAndDOFPtr RADFromName(const string& name, RobotBasePtr robot) {
	if (name == "active") {
		return BeliefRobotAndDOFPtr(new BeliefRobotAndDOF(robot, robot->GetActiveDOFIndices(), robot->GetAffineDOF(), robot->GetAffineRotationAxis()));
	}
	vector<int> dof_inds;
	int affinedofs = 0;
	Vector rotationaxis(0,0,1);
	vector<string> components;
	boost::split(components, name, boost::is_any_of("+"));
	for (int i=0; i < components.size(); ++i) {
		std::string& component = components[i];
		if (RobotBase::ManipulatorPtr manip = GetManipulatorByName(*robot, component)) {
			vector<int> inds = manip->GetArmIndices();
			dof_inds.insert(dof_inds.end(), inds.begin(), inds.end());
		}
		else if (component == "base3d") {
			affinedofs |= DOF_XYZ | DOF_Rotation3D;
		}
		else if (component == "base") {
			affinedofs |= DOF_X | DOF_Y | DOF_RotationAxis;
		}
		else if (component == "base_point") {
			affinedofs |= DOF_X | DOF_Y;
		}
		else if (KinBody::JointPtr joint = robot->GetJoint(component)) {
			dof_inds.push_back(joint->GetDOFIndex());
		}
		else PRINT_AND_THROW( boost::format("error in reading manip description: %s must be a manipulator, link, or 'base'")%component );
	}
	return BeliefRobotAndDOFPtr(new BeliefRobotAndDOF(robot, dof_inds, affinedofs, rotationaxis));
}

void DiscreteCheckSigmaSigma(BeliefRobotAndDOFPtr rad0, const MatrixXd& sigma_pts0, BeliefRobotAndDOFPtr rad1, const MatrixXd& sigma_pts1, vector<Collision>& collisions) {
	OR::RobotBase::RobotStateSaver saver0 = rad0->Save();
	OR::RobotBase::RobotStateSaver saver1 = rad1->Save();

	vector<int> inds;

	KinBody::LinkPtr link0, link1;
	vector<KinBody::LinkPtr> links;
	rad0->GetAffectedLinks(links, true, inds);
	assert(links.size() == 1);
	link0 = links[0];
	rad1->GetAffectedLinks(links, true, inds);
	assert(links.size() == 1);
	link1 = links[0];

	vector<OR::Transform> tf0(sigma_pts0.cols()), tf1(sigma_pts1.cols());

	for (int i=0; i<sigma_pts0.cols(); i++) {
		rad0->SetDOFValues(toDblVec(sigma_pts0.col(i)));
		tf0[i] = link0->GetTransform();
	}
	for (int i=0; i<sigma_pts1.cols(); i++) {
		rad1->SetDOFValues(toDblVec(sigma_pts1.col(i)));
		tf1[i] = link1->GetTransform();
	}
	cc->MultiCastVsMultiCast(link0, tf0, link1, tf1, collisions);
}

float alpha = 1;
void AdjustTransparency(float da) {
	alpha += da;
	alpha = fmin(alpha, 1);
	alpha = fmax(alpha, 0);
	viewer->SetAllTransparency(alpha);
}

bool isTrajectoryInCollision(CollisionCheckerPtr cc, TrajArray traj, BeliefRobotAndDOFPtr rad) {
	vector<Collision> collisions;
	cc->DiscreteCheckTrajectory(traj, rad, collisions);
	for (int i=0; i<collisions.size(); i++) {
		if (collisions[i].distance < 0) return true;
	}
	return false;
}

OR::Vector toQuat(const Matrix3d& m) {
	Quaterniond rq(m);
	return OR::Vector(rq.w(), rq.x(), rq.y(), rq.z());
}
OR::Vector toQuat(const AngleAxisd& aa) {
	return toQuat(aa.toRotationMatrix());
}
Eigen::Matrix3d toMatrix3d(OR::RaveVector<float> rq) {
	return Quaterniond(rq[0], rq[1], rq[2], rq[3]).toRotationMatrix();
}

void renderSigmaPts(BeliefRobotAndDOFPtr rad, const MatrixXd& sigma_pts, const osg::Vec4f& colorvec) {
	OR::RobotBase::RobotStateSaver saver = rad->Save();

	vector<KinBody::LinkPtr> links;
	vector<int> joint_inds;
	rad->GetAffectedLinks(links, true, joint_inds);

	// render sigma points
	for (int j=0; j<sigma_pts.cols(); j++) {
		rad->SetDOFValues(toDblVec(sigma_pts.col(j)));
		handles.push_back(viewer->PlotKinBody(rad->GetRobot()));
		SetColor(handles.back(), colorvec);
	}

	// render convex hulls of sigma points
	vector<DblVec> dofvals(sigma_pts.cols());
	for (int i=0; i<sigma_pts.cols(); i++)
		dofvals[i] = toDblVec(sigma_pts.col(i));
	cc->SetContactDistance(100);
	cc->PlotCastHull(*rad, links, dofvals, handles);
	SetTransparency(handles.back(), 0.2);
}

Matrix3d skewSymmetric(const Vector3d& v) {
	Matrix3d m;
	m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
	return m;
}

Matrix3d poseToRot(VectorXd x) {
	Vector3d aa;
	if (x.rows() == 3) aa = x;
	else if (x.rows() == 6) aa = x.bottomRows(3);
	Matrix3d rot;
	if (aa.norm() > 1e-5)
		rot = (Matrix3d) AngleAxisd(aa.norm(), aa.normalized());
	else
		rot = Matrix3d::Identity();
	return rot;
}
Vector3d toEigenVector(OR::Vector v) {
	return Vector3d(v[0], v[1], v[2]);
}
OR::Vector toORVector(Vector3d v) {
	return OR::Vector(v[0], v[1], v[2]);
}

Collision transformedCollision(const Collision& col_rhs, const Vector6d& pose) {
	Matrix3d rot = poseToRot(pose);
	Vector3d pos = pose.topRows(3);
	Collision col = col_rhs;
	col.ptA = toORVector(rot * toEigenVector(col.ptA) + pos);
	col.ptB = toORVector(rot * toEigenVector(col.ptB) + pos);
	col.normalB2A = toORVector(rot * toEigenVector(col.normalB2A));
	return col;
}

//VectorXd psi0, VectorXd psi1,
// psi0 is robot
// psi1 is obstacle
Vector6d delta(const Vector6d& psi0, const Vector6d& psi1) {
	Vector3d pos0 = psi0.topRows(3);
	Vector3d pos1 = psi1.topRows(3);
	Matrix3d rot0 = poseToRot(psi0);
	Matrix3d rot1 = poseToRot(psi1);
//	if (psi0.bottomRows(3).norm() > 1e-5)
//		rot0 = (Matrix3d) AngleAxisd(psi0.bottomRows(3).norm(), psi0.bottomRows(3).normalized());
//	else
//		rot0 = Matrix3d::Identity();
//	if (psi1.bottomRows(3).norm() > 1e-5)
//		rot1 = (Matrix3d) AngleAxisd(psi1.bottomRows(3).norm(), psi1.bottomRows(3).normalized());
//	else
//		rot1 = Matrix3d::Identity();

//	Vector6d rel_old;
//	rel_old.topRows(3) = rot1 * (pos0 - pos1);
//	Vector3d rel_rot_old = Vector3d::Zero();
//	for (int j=0; j<3; j++) {
//		rel_rot_old += skewSymmetric(rot0.col(j)) * rot1.col(j);
//	}
//	rel_old.bottomRows(3) = 0.5 * rel_rot_old;

	Matrix4d tf0 = Matrix4d::Identity();
	tf0.topLeftCorner(3,3) = rot0;
	tf0.topRightCorner(3,1) = pos0;
	Matrix4d tf1 = Matrix4d::Identity();
	tf1.topLeftCorner(3,3) = rot1;
	tf1.topRightCorner(3,1) = pos1;
	Matrix4d rel_tf = tf1.inverse() * tf0;

	Vector6d rel;
	Matrix3d rel_rot = rot1.transpose() * rot0;
//	Matrix3d rel_rot = rel_tf.topLeftCorner(3,3);
	Vector3d rel_pos = rot1.transpose() * (pos0 - pos1);
//	Vector3d rel_pos = rel_tf.topRightCorner(3,1);
	AngleAxisd rel_rot_aa(rel_rot);
	rel.topRows(3) = rel_pos;
	rel.bottomRows(3) = rel_rot_aa.axis() * rel_rot_aa.angle();

	return rel;
}

Matrix6Xd jointSigmaPts(const Vector12d& mean, const Matrix12d& rt_Sigma, BeliefRobotAndDOFPtr rad) {
	double lambda = 2;
	Matrix12d rt_scaled_cov = lambda * rt_Sigma;

	Matrix6d cov = Matrix6d::Zero();
	Matrix6d var1 = Matrix6d::Zero();
	Matrix6d var2 = Matrix6d::Zero();

	double w = (1/(2*lambda*lambda));

	Matrix6Xd sigmapts(6,13);
	for(int i = 0; i < 12; ++i) {
		Vector12d sp = mean + rt_scaled_cov.col(i);
		Vector12d sm = mean - rt_scaled_cov.col(i);

		cov += w * delta(sp.topRows(6), mean.topRows(6)) * delta(sp.bottomRows(6), mean.bottomRows(6)).transpose();
		cov += w * delta(sm.topRows(6), mean.topRows(6)) * delta(sm.bottomRows(6), mean.bottomRows(6)).transpose();

		var1 += w * delta(sp.topRows(6), mean.topRows(6)) * delta(sp.topRows(6), mean.topRows(6)).transpose();
		var1 += w * delta(sm.topRows(6), mean.topRows(6)) * delta(sm.topRows(6), mean.topRows(6)).transpose();

		var2 += w * delta(sp.bottomRows(6), mean.bottomRows(6)) * delta(sp.bottomRows(6), mean.bottomRows(6)).transpose();
		var2 += w * delta(sm.bottomRows(6), mean.bottomRows(6)) * delta(sm.bottomRows(6), mean.bottomRows(6)).transpose();
	}

	Vector6d rel_mean = delta(mean.topRows(6), mean.bottomRows(6));
	Matrix6d rel_var = var1+var2-2*cov;

	cout << "rel_var" << endl;
	cout << rel_var << endl;

	Eigen::SelfAdjointEigenSolver<MatrixXd> es(rel_var);
	Matrix6d sqrt_rel_var = es.eigenvectors().real() * es.eigenvalues().real().cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
	sigmapts = rad->sigmaPoints(rel_mean, sqrt_rel_var);

	cout << "cov" << endl;
	cout << cov << endl;
	cout << "var1" << endl;
	cout << var1 << endl;
	cout << "var2" << endl;
	cout << var2 << endl;
	cout << "sigmapts" << endl;
	cout << sigmapts << endl;
	return sigmapts;
}

Quaterniond quatAdd(const Quaterniond& q0, const Quaterniond& q1) {
	return Quaterniond(q0.w()+q1.w(), q0.x()+q1.x(), q0.y()+q1.y(), q0.z()+q1.z());
}

class DualQuat {
	Quaterniond m_qr;
	Quaterniond m_qt;
public:
	DualQuat(const Quaterniond& qr, const Quaterniond& qt) : m_qr(qr), m_qt(qt) {}
	DualQuat operator* (const DualQuat& dq) {
		return DualQuat(m_qr*dq.m_qr, quatAdd(m_qr*dq.m_qt, m_qt*dq.m_qr));
	}
	VectorXd asVectorXd() {
		VectorXd v(8);
		v << m_qr.w(), m_qr.x(), m_qr.y(), m_qr.z(), m_qt.w(), m_qt.x(), m_qt.y(), m_qt.z();
		return v;
	}
};

DualQuat DOFToDualQuat(const VectorXd& x) {
	DualQuat dqt(Quaterniond::Identity(), Quaterniond(0, x(0)/2.0, x(1)/2.0, x(2)/2.0));
	Quaterniond qr = Quaterniond::Identity();
	if (x.bottomRows(3).norm() > 1e-5)
		qr = Quaterniond(AngleAxisd(x.bottomRows(3).norm(), x.bottomRows(3).normalized()));
	DualQuat dqr(qr, Quaterniond(0,0,0,0));
	cout << "---" << endl;
	cout << dqt.asVectorXd().transpose() << endl;
	cout << dqr.asVectorXd().transpose() << endl;
	cout << "---" << endl;
	return dqt*dqr;
}

int main() {
	RaveInitialize(false, OpenRAVE::Level_Debug);
	env = RaveCreateEnvironment();
	env->StopSimulation();
	env->Load(DATA_DIR"/boxes.env.xml");

	vector<RobotBasePtr> robots;
	env->GetRobots(robots);
	RobotBasePtr robot = robots[0];
	vector<RobotBase::ManipulatorPtr> manips = robot->GetManipulators();

	BeliefRobotAndDOFPtr rad = RADFromName("base3d", robot);
	const int n_steps = 10;

	BeliefRobotAndDOFPtr obstacle = RADFromName("base3d", robots[1]);

	cc = CollisionChecker::GetOrCreate(*env);
	viewer.reset(new OSGViewer(env));
	env->AddViewer(viewer);
	dynamic_cast<OSGViewer::EventHandler*>(viewer->GetViewer().getCameraManipulator())->setTransformation(osg::Vec3d(0,0,2), osg::Vec3d(0,0,0), osg::Vec3d(0,1,0));

	viewer->AddKeyCallback('=', boost::bind(&AdjustTransparency, .05));
	viewer->AddKeyCallback('-', boost::bind(&AdjustTransparency, -.05));

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

	VectorXd x = toVectorXd(rad->GetDOFValues());
	//  x << -0.3,0.4,-0.4,0,M_PI_4,M_PI_4/2.0;
	//x << -0.3,0.5+0.4,0,0,0,0;
	rad->SetDOFValues(toDblVec(x));
	VectorXd rt_Sigma_diag(6);
	rt_Sigma_diag << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
	MatrixXd rt_Sigma = rt_Sigma_diag.asDiagonal();
	VectorXd theta;
	rad->composeBelief(x, rt_Sigma, theta);

	VectorXd obstacle_x = toVectorXd(obstacle->GetDOFValues());
	//obstacle_x << 0.2,0.5+0.25,0,0,0,0;
	obstacle->SetDOFValues(toDblVec(obstacle_x));
	VectorXd obstacle_rt_Sigma_diag(6);
	obstacle_rt_Sigma_diag << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
	MatrixXd obstacle_rt_Sigma = obstacle_rt_Sigma_diag.asDiagonal();
	VectorXd obstacle_theta;
	obstacle->composeBelief(obstacle_x, obstacle_rt_Sigma, obstacle_theta);

	renderSigmaPts(rad, rad->sigmaPoints(theta), osg::Vec4f(0,0,1,0.5));
	renderSigmaPts(obstacle, obstacle->sigmaPoints(obstacle_theta), osg::Vec4f(0,0,1,0.5));

	// plot collisions
	vector<Collision> collisions;
	DiscreteCheckSigmaSigma(rad, rad->sigmaPoints(theta), obstacle, obstacle->sigmaPoints(obstacle_theta), collisions);
	cout << "collisions " << collisions.size() << ": ";
	for (int i=0; i<collisions.size(); i++) cout << collisions[i].distance << " "; cout << endl;

	{
	Vector12d joint_x;
	joint_x.topRows(6) = x;
	joint_x.bottomRows(6) = obstacle_x;
	Matrix12d joint_rt_sigma = Matrix12d::Identity();
	joint_rt_sigma.topLeftCorner(6,6) = rt_Sigma;
	joint_rt_sigma.bottomRightCorner(6,6) = obstacle_rt_Sigma;
	//joint_rt_sigma.bottomLeftCorner(6,6) = 0.099*Matrix6d::Identity();
	//joint_rt_sigma.topRightCorner(6,6) = 0.099*Matrix6d::Identity();

	Matrix6Xd  rel_sigmapts = jointSigmaPts(joint_x, joint_rt_sigma, rad);

	renderSigmaPts(rad, rel_sigmapts, osg::Vec4f(0,0,1,0.5));
	renderSigmaPts(obstacle, Vector6d::Zero(), osg::Vec4f(0,0,1,0.5));

	DiscreteCheckSigmaSigma(rad, rel_sigmapts, obstacle, Vector6d::Zero(), collisions);
	collisions.push_back(transformedCollision(collisions.back(), obstacle_x));
	cout << "collisions " << collisions.size() << ": ";
	for (int i=0; i<collisions.size(); i++) cout << collisions[i].distance << " "; cout << endl;
	}

	{
	Vector12d joint_x;
	joint_x.topRows(6) = obstacle_x;
	joint_x.bottomRows(6) = x;
	Matrix12d joint_rt_sigma = Matrix12d::Identity();
	joint_rt_sigma.topLeftCorner(6,6) = obstacle_rt_Sigma;
	joint_rt_sigma.bottomRightCorner(6,6) = rt_Sigma;

	Matrix6Xd  rel_sigmapts = jointSigmaPts(joint_x, joint_rt_sigma, obstacle);

	renderSigmaPts(rad, Vector6d::Zero(), osg::Vec4f(0,0,1,0.5));
	renderSigmaPts(obstacle, rel_sigmapts, osg::Vec4f(0,0,1,0.5));

	DiscreteCheckSigmaSigma(obstacle, rel_sigmapts, rad, Vector6d::Zero(), collisions);
	collisions.push_back(transformedCollision(collisions.back(), x));
	cout << "collisions " << collisions.size() << ": ";
	for (int i=0; i<collisions.size(); i++) cout << collisions[i].distance << " "; cout << endl;
	}

	PlotCollisions(collisions, *env, handles, 0);

	viewer->Idle();

	env.reset();
	viewer.reset();
	RaveDestroy();
}