diff --git a/offline/controller/.gitignore b/offline/controller/.gitignore new file mode 100644 index 00000000..7b080b98 --- /dev/null +++ b/offline/controller/.gitignore @@ -0,0 +1,6 @@ +*.mat +*.jpg +*.png +.DS_Store +*.pyc + diff --git a/offline/controller/controller.m b/offline/controller/controller.m new file mode 100644 index 00000000..f785a122 --- /dev/null +++ b/offline/controller/controller.m @@ -0,0 +1,125 @@ +function [trajectory] = controller() + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + global velocity + global steering_vel + global dt + + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.441670, -79.9416362]; + dt = 0.001; % 1000Hz + m = 50; % 20Hz + velocity = 8; % m/s, 17.9mph, forward velocity + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds + + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + velocity; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints.mat'); + desired_traj = processWaypoints(logs); + % time = linspace(0, 240, size(trajectory,2)); + time = 0:dt:240; + u = 0; % commanded steering angle + steering = u; % steering angle + trajectory = [X; lat_long(1); lat_long(2); steering]; + + for i = 1:size(time, 2) + t = time(i); + A = model(X, steering); + X = A*X; + steering = updateSteering(steering, u); + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + + if(mod(i, m) == 0) + u = control(desired_traj, X); + end + + % trajectory = [trajectory, X]; + snapshot = summarize(X, utm_zone, steering); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('controller_v2.mat', 'trajectory'); + end +end + +function [desired] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long); + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; x(1); x(2); steeringAngle]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + +function a = clampSteeringAngle(a) + if(a < -deg2rad(10)) + a = -deg2rad(10); + end + if(a > deg2rad(10)) + a = deg2rad(10); + end +end + +function [A] = model(x, steering) + global wheel_base + global dt + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steering)/wheel_base, 0, 0]; +end + +function [u] = control(desired_traj, X) + pos = X(1:2); + b = repmat(pos, size(desired_traj, 1), 1); + delta = 15*15; + possible = find(sum((desired_traj-b).^2, 2) < delta); + if isempty(possible) + u = 0; + else + target = desired_traj(possible(end), :); + deltaPath = target - pos; + u = atan2(deltaPath(2), deltaPath(1))-X(4); + end + u = clampSteeringAngle(clampAngle(u)); +end + + + diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m new file mode 100644 index 00000000..ec6b89ef --- /dev/null +++ b/offline/controller/controller_analysis.m @@ -0,0 +1,48 @@ +% graph results + +clear; +close all; + +addpath('../localizer/latlonutm/Codes/matlab'); +addpath('../localizer/altmany-export_fig'); + +file = 'controller_v2.mat'; +load(file, 'trajectory'); +save_plot = false; +show_maps = true; + +load('./waypoints_course_v2.mat'); +[x, y, zone] = ll2utm(logs); +desired = [x y]; +desired = desired(112:(end-50), :); + +k = 1000; +if show_maps + wmline(trajectory(6,1:k:end), trajectory(7,1:k:end), 'Color', 'r') +end + +heading = trajectory(4,1:k:end); +figure(); +hold on; +plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) +quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(heading), sin(heading)) +plot(desired(:,1), desired(:,2), 'g') +hold off; +title(['Map ', file]); + +headingd = rad2deg(heading); +figure(); +hold on; +plot(1:length(headingd), headingd) +title(['Heading ', file]); + +figure(); +p = k/20; +plot(1:p:size(trajectory,2), trajectory(8,1:p:end)) +title('Control input'); + +% plot on google maps +if show_maps + xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; + fprintf(1, '%5.20f, %5.20f\n', xy); +end diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m new file mode 100644 index 00000000..f68a1ce2 --- /dev/null +++ b/offline/controller/controller_pure.m @@ -0,0 +1,137 @@ +function [trajectory] = controller_pure() +% https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf +% section 2.2 + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + global velocity + global steering_vel + global dt + + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362]; + dt = 0.001; % 1000Hz + m = 50; % 20Hz + velocity = 8; % m/s, 17.9mph, forward velocity + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds + % full range in 0.5s + + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + velocity; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints_tri.mat'); + desired_traj = processWaypoints(logs); + % time = linspace(0, 240, size(trajectory,2)); + time = 0:dt:60; % 240; + u = 0; % commanded steering angle + steering = u; % steering angle + trajectory = [X; lat_long(1); lat_long(2); steering]; + + for i = 1:size(time, 2) + t = time(i); + A = model(X, steering); + X = A*X; + steering = updateSteering(steering, u); + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + + if(mod(i, m) == 0) + u = control(desired_traj, X); + end + + % trajectory = [trajectory, X]; + snapshot = summarize(X, utm_zone, steering); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('controller_tri_v1.mat', 'trajectory'); + end +end + +function [desired] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long); + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; x(1); x(2); steeringAngle]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + +function a = clampSteeringAngle(a) + if(a < -deg2rad(10)) + a = -deg2rad(10); + end + if(a > deg2rad(10)) + a = deg2rad(10); + end +end + +function [A] = model(x, steering) + global wheel_base + global dt + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steering)/wheel_base, 0, 0]; +end + +function [u] = control(desired_traj, X) + global wheel_base + + pos = X(1:2)'; + pos_b = repmat(pos, size(desired_traj, 1), 1); + delta = 15; + cutoff = 100; + delta = delta * delta; + + distances = sum((desired_traj - pos_b).^2, 2); + [~, closest_idx] = min(distances); + last_idx = min([length(distances), closest_idx + cutoff]); + possible = find(distances(1:last_idx) < delta); + if isempty(possible) + u = 0; + else + target = desired_traj(possible(end), :); + deltaPath = target - pos; + + k = 0.8; + a = atan2(deltaPath(2), deltaPath(1))-X(4); + u = atan2(2*wheel_base*sin(a), k*X(3)); + end + u = clampSteeringAngle(clampAngle(u)); +end + diff --git a/offline/controller/controller_pure_cont.m b/offline/controller/controller_pure_cont.m new file mode 100644 index 00000000..8f89175f --- /dev/null +++ b/offline/controller/controller_pure_cont.m @@ -0,0 +1,167 @@ +function [trajectory] = controller_pure_cont() +% https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf +% section 2.2 + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + global velocity + global steering_vel + global dt + global last_closest_idx + global last_u + + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.441670, -79.9416362]; % tri [40.442867, -79.9427395]; + dt = 0.001; % 1000Hz + m = 50; % 20Hz + velocity = 3.6; % m/s, 8mph, forward velocity + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds + % full range in 0.5s + last_closest_idx = 1; + last_u = 0; + total_time = 6; % min + + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + velocity; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints_course_v2.mat'); + desired_traj = processWaypoints(logs); + time = 0:dt:(total_time*60); + u = 0; % commanded steering angle + steering = u; % steering angle + trajectory = [X; lat_long(1); lat_long(2); steering]; + + for i = 1:size(time, 2) + t = time(i); + A = model(X, steering); + X = A*X; + steering = updateSteering(steering, u); + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + + if(mod(i, m) == 0) + u = control(desired_traj, X); + end + + snapshot = summarize(X, utm_zone, steering); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('controller_v3.mat', 'trajectory'); + end +end + +function [desired] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long); + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; lat; lon; steeringAngle]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + +function a = clampSteeringAngle(a) + b = deg2rad(10); + if(a < -b) + a = -b; + elseif(a > b) + a = b; + end +end + +function [A] = model(x, steering) + global wheel_base + global dt + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steering)/wheel_base, 0, 0]; +end + +function [u] = control(desired_traj, X) + global wheel_base + global last_closest_idx + global last_u + + K = 4; + vel = X(3); + pos = X(1:2)'; + lookahead_bounds = [3 25]; + lookahead = K * vel; + if(lookahead < lookahead_bounds(1)) + lookahead = lookahead_bounds(1); + elseif(lookahead > lookahead_bounds(2)) + lookahead = lookahead_bounds(1); + end + + closest_idx = last_closest_idx; + min_dist = 100000; + for k=last_closest_idx:length(desired_traj) + distp = norm(desired_traj(k,:) - pos); + if(distp < min_dist) + min_dist = distp; + closest_idx = k; + end + % cut off search somehow + end + + lookahead_idx = 0; + for k=closest_idx:length(desired_traj) + distp = norm(desired_traj(k,:) - pos); + if(distp > lookahead) + lookahead_idx = k; + break; + end + end + + if(lookahead_idx == 0) + u = 0; + else + deltaPath = desired_traj(lookahead_idx,:) - pos; + a = atan2(deltaPath(2), deltaPath(1)) - X(4); + u = atan2(2 * wheel_base * sin(a), lookahead); + end + + % try moving average + % temp_u = u; + % u = (last_u + u) / 2; + % last_u = temp_u; + + u = clampSteeringAngle(u); + last_closest_idx = closest_idx; +end + diff --git a/offline/controller/controller_stanley.m b/offline/controller/controller_stanley.m new file mode 100644 index 00000000..8db24bd9 --- /dev/null +++ b/offline/controller/controller_stanley.m @@ -0,0 +1,160 @@ +function [trajectory] = controller_stanley() +% https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf +% section 2.3 + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + global velocity + global steering_vel + global dt + global last_closest_idx + global last_u + + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.441670, -79.9416362]; % tri [40.442867, -79.9427395]; + dt = 0.001; % 1000Hz + m = 50; % 20Hz + velocity = 3.6; % m/s, 8mph, forward velocity + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds + % full range in 0.5s + last_closest_idx = 1; + last_u = 0; + total_time = 6; % min + + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + velocity; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints_course_v2.mat'); + desired_traj = processWaypoints(logs); + desired_traj = desired_traj(112:(end-50), :); + + time = 0:dt:(total_time*60); + u = 0; % commanded steering angle + steering = u; % steering angle + trajectory = [X; lat_long(1); lat_long(2); steering]; + + for i = 1:size(time, 2) + t = time(i); + A = model(X, steering); + X = A*X; + steering = updateSteering(steering, u); + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + + if(mod(i, m) == 0) + u = control(desired_traj, X); + end + + snapshot = summarize(X, utm_zone, steering); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('controller_v2.mat', 'trajectory'); + end +end + +function [desired] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long); + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; lat; lon; steeringAngle]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + +function a = clampSteeringAngle(a) + b = deg2rad(10); + if(a < -b) + a = -b; + elseif(a > b) + a = b; + end +end + +function [A] = model(x, steering) + global wheel_base + global dt + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steering)/wheel_base, 0, 0]; +end + +function [u] = control(desired_traj, X) + global wheel_base + global last_closest_idx + global last_u + + K = 0.1; + K2 = 0.8; + pos = X(1:2)'; + + closest_idx = last_closest_idx; + min_dist = 100000; + for k=last_closest_idx:length(desired_traj) + distp = norm(desired_traj(k,:) - pos); + if(distp < min_dist) + min_dist = distp; + closest_idx = k; + end + % cut off search somehow + end + + if(closest_idx == length(desired_traj)) + u = 0; + return; + end + + ptA = desired_traj(closest_idx, :); + ptB = desired_traj(closest_idx+1, :); + + p = ptB - ptA; % path + path_heading = atan2(p(2), p(1)); + heading_error = clampAngle(path_heading) - clampAngle(X(4)); + crosstrack_error = - det([p; pos - ptA]) / norm(p); + + u = K2*heading_error + atan2(K * crosstrack_error, X(3)); + + % try moving average + % temp_u = u; + % u = (last_u + u) / 2; + % last_u = temp_u; + + u = clampSteeringAngle(u); + last_closest_idx = closest_idx; +end + diff --git a/offline/controller/get_logs.py b/offline/controller/get_logs.py new file mode 100644 index 00000000..61fc811d --- /dev/null +++ b/offline/controller/get_logs.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python + +import json +import numpy as np +import scipy.io + +combined = True +d = None +file = 'waypoints_course_v2.txt' +logs = [] +with open(file) as json_data: + for line in json_data: + d = json.loads(line) + lat = d['latitude'] + lng = d['longitude'] + logs.append([lat, lng]) + + +logs = np.array(logs) +print(logs.shape) +vars_map = {'logs': logs} +scipy.io.savemat('./waypoints_course_v2.mat', mdict=vars_map) diff --git a/offline/controller/waypoint_analysis.m b/offline/controller/waypoint_analysis.m new file mode 100644 index 00000000..a977b37d --- /dev/null +++ b/offline/controller/waypoint_analysis.m @@ -0,0 +1,29 @@ +% graph results + +clear; +close all; + +addpath('../localizer/latlonutm/Codes/matlab'); +show_maps = false; + +load('./waypoints_course.mat'); +[x, y, zone] = ll2utm(logs); +desired = [x y]; + +size(desired) + +sumdist = 0; +for k=1:(length(desired) - 1) + sumdist = sumdist + norm(desired(k,:) - desired(k+1,:)); +end +avgdist = sumdist ./ length(desired) + +% figure(); +% plot(1:length(trajectory), trajectory(8, 1:end)) +% title('Control input'); + +% plot on google maps +if show_maps + xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; + fprintf(1, '%5.20f, %5.20f\n', xy); +end diff --git a/offline/localizer/.gitignore b/offline/localizer/.gitignore new file mode 100644 index 00000000..f05d0f63 --- /dev/null +++ b/offline/localizer/.gitignore @@ -0,0 +1,5 @@ +*.mat +*.jpg +*.png +.DS_Store +*.pyc diff --git a/offline/localizer/README.md b/offline/localizer/README.md new file mode 100644 index 00000000..512bbe5e --- /dev/null +++ b/offline/localizer/README.md @@ -0,0 +1,18 @@ +## All files in offline/localizer + +### 1. process json with get_logs.py +change name of log file +will write roll_logs_combined.mat -> array of all different values + +### 2. localizer.m +reads in .mat file and runs algorithm and saves resuts to localizer_v4.mat + +### 3. run trajectory_analysis.m +save plots (screenshot and save different folders for each roll) + +### 4. copy all numbers from matlab command line +go to hamstermap.com/quickmap.php +paste numbers in white box +click regenerate +click satellite +Screenshot image diff --git a/offline/localizer/altmany-export_fig/.gitignore b/offline/localizer/altmany-export_fig/.gitignore new file mode 100755 index 00000000..68c9c928 --- /dev/null +++ b/offline/localizer/altmany-export_fig/.gitignore @@ -0,0 +1,5 @@ +/.ignore +*.txt +*.asv +*~ +*.mex* diff --git a/offline/localizer/altmany-export_fig/ImageSelection.java b/offline/localizer/altmany-export_fig/ImageSelection.java new file mode 100755 index 00000000..7ec32fd3 --- /dev/null +++ b/offline/localizer/altmany-export_fig/ImageSelection.java @@ -0,0 +1,38 @@ +/* + * Based on code snippet from + * http://java.sun.com/developer/technicalArticles/releases/data/ + * + * Copyright © 2008, 2010 Oracle and/or its affiliates. All rights reserved. Use is subject to license terms. + */ + +import java.awt.image.BufferedImage; +import java.awt.datatransfer.*; + +public class ImageSelection implements Transferable { + + private static final DataFlavor flavors[] = + {DataFlavor.imageFlavor}; + + private BufferedImage image; + + public ImageSelection(BufferedImage image) { + this.image = image; + } + + // Transferable + public Object getTransferData(DataFlavor flavor) throws UnsupportedFlavorException { + if (flavor.equals(flavors[0]) == false) { + throw new UnsupportedFlavorException(flavor); + } + return image; + } + + public DataFlavor[] getTransferDataFlavors() { + return flavors; + } + + public boolean isDataFlavorSupported(DataFlavor + flavor) { + return flavor.equals(flavors[0]); + } +} \ No newline at end of file diff --git a/offline/localizer/altmany-export_fig/LICENSE b/offline/localizer/altmany-export_fig/LICENSE new file mode 100755 index 00000000..1a058308 --- /dev/null +++ b/offline/localizer/altmany-export_fig/LICENSE @@ -0,0 +1,27 @@ +Copyright (c) 2014, Oliver J. Woodford, Yair M. Altman +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 {organization} 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 HOLDER 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. diff --git a/offline/localizer/altmany-export_fig/README.md b/offline/localizer/altmany-export_fig/README.md new file mode 100755 index 00000000..c6e68de7 --- /dev/null +++ b/offline/localizer/altmany-export_fig/README.md @@ -0,0 +1,270 @@ +export_fig +========== + +A toolbox for exporting figures from MATLAB to standard image and document formats nicely. + +### Overview +Exporting a figure from MATLAB the way you want it (hopefully the way it looks on screen), can be a real headache for the unitiated, thanks to all the settings that are required, and also due to some eccentricities (a.k.a. features and bugs) of functions such as `print`. The first goal of export_fig is to make transferring a plot from screen to document, just the way you expect (again, assuming that's as it appears on screen), a doddle. + +The second goal is to make the output media suitable for publication, allowing you to publish your results in the full glory that you originally intended. This includes embedding fonts, setting image compression levels (including lossless), anti-aliasing, cropping, setting the colourspace, alpha-blending and getting the right resolution. + +Perhaps the best way to demonstrate what export_fig can do is with some examples. + +### Examples +**Visual accuracy** - MATLAB's exporting functions, namely `saveas` and `print`, change many visual properties of a figure, such as size, axes limits and ticks, and background colour, in unexpected and unintended ways. Export_fig aims to faithfully reproduce the figure as it appears on screen. For example: +```Matlab +plot(cos(linspace(0, 7, 1000))); +set(gcf, 'Position', [100 100 150 150]); +saveas(gcf, 'test.png'); +export_fig test2.png +``` +generates the following: + +| Figure: | test.png: | test2.png: | +|:-------:|:---------:|:----------:| +|![](https://farm6.staticflickr.com/5616/15589249291_16e485c29a_o_d.png)|![](https://farm4.staticflickr.com/3944/15406302850_4d2e1c7afa_o_d.png)|![](https://farm6.staticflickr.com/5607/15568225476_8ce9bd5f6b_o_d.png)| + +Note that the size and background colour of test2.png (the output of export_fig) are the same as those of the on screen figure, in contrast to test.png. Of course, if you want the figure background to be white (or any other colour) in the exported file then you can set this prior to exporting using: +```Matlab +set(gcf, 'Color', 'w'); +``` + +Notice also that export_fig crops and anti-aliases (smooths, for bitmaps only) the output by default. However, these options can be disabled; see the Tips section below for details. + +**Resolution** - by default, export_fig exports bitmaps at screen resolution. However, you may wish to save them at a different resolution. You can do this using either of two options: `-m`, where is a positive real number, magnifies the figure by the factor for export, e.g. `-m2` produces an image double the size (in pixels) of the on screen figure; `-r`, again where is a positive real number, specifies the output bitmap to have pixels per inch, the dimensions of the figure (in inches) being those of the on screen figure. For example, using: +```Matlab +export_fig test.png -m2.5 +``` +on the figure from the example above generates: + +![](https://farm4.staticflickr.com/3937/15591910915_dc7040c477_o_d.png) + +Sometimes you might have a figure with an image in. For example: +```Matlab +imshow(imread('cameraman.tif')) +hold on +plot(0:255, sin(linspace(0, 10, 256))*127+128); +set(gcf, 'Position', [100 100 150 150]); +``` +generates this figure: + +![](https://farm4.staticflickr.com/3942/15589249581_ff87a56a3f_o_d.png) + +Here the image is displayed in the figure at resolution lower than its native resolution. However, you might want to export the figure at a resolution such that the image is output at its native (i.e. original) size (in pixels). Ordinarily this would require some non-trivial computation to work out what that resolution should be, but export_fig has an option to do this for you. Using: +```Matlab +export_fig test.png -native +``` +produces: + +![](https://farm6.staticflickr.com/5604/15589249591_da2b2652e4_o_d.png) + +with the image being the size (in pixels) of the original image. Note that if you want an image to be a particular size, in pixels, in the output (other than its original size) then you can resize it to this size and use the `-native` option to achieve this. + +All resolution options (`-m`, `-q` and `-native`) correctly set the resolution information in PNG and TIFF files, as if the image were the dimensions of the on screen figure. + +**Shrinking dots & dashes** - when exporting figures with dashed or dotted lines using either the ZBuffer or OpenGL (default for bitmaps) renderers, the dots and dashes can appear much shorter, even non-existent, in the output file, especially if the lines are thick and/or the resolution is high. For example: +```Matlab +plot(sin(linspace(0, 10, 1000)), 'b:', 'LineWidth', 4); +hold on +plot(cos(linspace(0, 7, 1000)), 'r--', 'LineWidth', 3); +grid on +export_fig test.png +``` +generates: + +![](https://farm4.staticflickr.com/3956/15592747732_f943d4aa0a_o_d.png) + +This problem can be overcome by using the painters renderer. For example: +```Matlab +export_fig test.png -painters +``` +used on the same figure generates: + +![](https://farm4.staticflickr.com/3945/14971168504_77692f11f5_o_d.png) + +Note that not only are the plot lines correct, but the grid lines are too. + +**Transparency** - sometimes you might want a figure and axes' backgrounds to be transparent, so that you can see through them to a document (for example a presentation slide, with coloured or textured background) that the exported figure is placed in. To achieve this, first (optionally) set the axes' colour to 'none' prior to exporting, using: +```Matlab +set(gca, 'Color', 'none'); % Sets axes background +``` + +then use export_fig's `-transparent` option when exporting: +```Matlab +export_fig test.png -transparent +``` + +This will make the background transparent in PDF, EPS and PNG outputs. You can additionally save fully alpha-blended semi-transparent patch objects to the PNG format. For example: + +```Matlab +logo; +alpha(0.5); +``` + +generates a figure like this: + +![](https://farm4.staticflickr.com/3933/15405290339_b08de33528_o_d.png) + +If you then export this to PNG using the `-transparent` option you can then put the resulting image into, for example, a presentation slide with fancy, textured background, like so: + +![](https://farm6.staticflickr.com/5599/15406302920_59beaefff1_o_d.png) + +and the image blends seamlessly with the background. + +**Image quality** - when publishing images of your results, you want them to look as good as possible. By default, when outputting to lossy file formats (PDF, EPS and JPEG), export_fig uses a high quality setting, i.e. low compression, for images, so little information is lost. This is in contrast to MATLAB's print and saveas functions, whose default quality settings are poor. For example: +```Matlab +A = im2double(imread('peppers.png')); +B = randn(ceil(size(A, 1)/6), ceil(size(A, 2)/6), 3) * 0.1; +B = cat(3, kron(B(:,:,1), ones(6)), kron(B(:,:,2), ones(6)), kron(B(:,:,3), ones(6))); +B = A + B(1:size(A, 1),1:size(A, 2),:); +imshow(B); +print -dpdf test.pdf +``` +generates a PDF file, a sub-window of which looks (when zoomed in) like this: + +![](https://farm6.staticflickr.com/5613/15405290309_881b2774d6_o_d.png) + +while the command + +```Matlab +export_fig test.pdf +``` +on the same figure produces this: + +![](https://farm4.staticflickr.com/3947/14971168174_687473133f_o_d.png) + +While much better, the image still contains some compression artifacts (see the low level noise around the edge of the pepper). You may prefer to export with no artifacts at all, i.e. lossless compression. Alternatively, you might need a smaller file, and be willing to accept more compression. Either way, export_fig has an option that can suit your needs: `-q`, where is a number from 0-100, will set the level of lossy image compression (again in PDF, EPS and JPEG outputs only; other formats are lossless), from high compression (0) to low compression/high quality (100). If you want lossless compression in any of those formats then specify a greater than 100. For example: +```Matlab +export_fig test.pdf -q101 +``` +again on the same figure, produces this: + +![](https://farm6.staticflickr.com/5608/15405803908_934512c1fe_o_d.png) + +Notice that all the noise has gone. + +### Tips +**Anti-aliasing** - the anti-aliasing which export_fig applies to bitmap outputs by default makes the images look nice, but it can also blur images and increase exporting time and memory requirements, so you might not always want it. You can set the level of anti-aliasing by using the `-a` option, where is 1 (no anti-aliasing), 2, 3 (default) or 4 (maximum anti-aliasing). + +**Cropping** - by default, export_fig crops its output to minimize the amount of empty space around the figure. If you'd prefer the figure to be uncropped, and instead have the same appearance (in terms of border width) as the on screen figure, then use the `-nocrop` option. + +**Colourspace** - by default, export_fig generates files in the RGB [colourspace](https://en.wikipedia.org/wiki/Color_space). However, you can also export in greyscale or the CMYK colourspace, using the `-grey` (or `-gray`) and `-cmyk` options respectively. The CMYK option is useful for publishers who require documents in this colourspace, but the option is only supported for PDF, EPS and TIFF files. + +**Specifying a target directory** - you can get export_fig to save output files to any directory (for which you have write permission), simply by specifying the full or relative path in the filename. For example: +```Matlab +export_fig ../subdir/fig.png; +export_fig('C:/Users/Me/Documents/figures/myfig', '-pdf', '-png'); +``` + +**Variable file names** - often you might want to save a series of figures in a for loop, each with a different name. For this you can use the functional form of input arguments, i.e. `export_fig(arg1, arg2)`, and construct the filename string in a variable. Here's an example of this: +```Matlab +for a = 1:5 + plot(rand(5, 2)); + export_fig(sprintf('plot%d.png', a)); +end +``` +When using the functional form like this, be sure to put string variables in quotes: +```Matlab +export_fig(sprintf('plot%d', a), '-a1', '-pdf', '-png'); +``` + +**Specifying the figure/axes** - if you have mutiple figures open you can specify which figure to export using its handle: +```Matlab +export_fig(figure_handle, filename); +``` +Equally, if your figure contains several subplots then you can export just one of them by giving export_fig the handle to the relevant axes: +```Matlab +export_fig(axes_handle, filename); +``` + +**Multiple formats** - save time by exporting to multiple formats simultaneously. E.g.: +```Matlab +export_fig filename -pdf -eps -png -jpg -tiff +``` + +**Other file formats** - if you'd like to save your figure to a bitmap format that is not supported by export_fig, e.g. animated GIF, PPM file or a frame in a movie, then you can use export_fig to output the image, and optionally an alpha-matte, to the workspace. E.g.: +```Matlab +frame = export_fig; +``` +or +```Matlab +[frame, alpha] = export_fig; +``` +These variables can then be saved to other image formats using other functions, such as imwrite. + +**Appending to a file** - you can use the `-append` option to append the figure to the end of an image/document, if it already exists. This is supported for PDF and TIFF files only. Note that if you wish to append a lot of figures consecutively to a PDF, it can be more efficient to save all the figures to PDF separately then append them all in one go at the end (e.g. using [append_pdfs](http://www.mathworks.com/matlabcentral/fileexchange/31215-appendpdfs)). + +**Output to clipboard** - you can use the `-clipboard` option to copy the specified figure or axes to the system clipboard, for easy paste into other documents (e.g., Word or PowerPoint). Note that the image is copied in bitmap (not vector) format. + +**Font size** - if you want to place an exported figure in a document with the font a particular size then you need to set the font to that size in the figure, and not resize the output of export_fig in the document. To avoid resizing, simply make sure that the on screen figure is the size you want the output to be in the document before exporting. + +**Renderers** - MATLAB has three renderers for displaying and exporting figures: painters, OpenGL and ZBuffer. The different renderers have different [features](http://www.mathworks.com/access/helpdesk/help/techdoc/creating_plots/f3-84337.html#f3-102410), so if you aren't happy with the result from one renderer try another. By default, vector formats (i.e. PDF and EPS outputs) use the painters renderer, while other formats use the OpenGL renderer. Non-default renderers can be selected by using one of these three export_fig input options: `-painters`, `-opengl`, `-zbuffer`: +```Matlab +export_fig test.png -painters +``` + +**Artifacts** - sometimes the output that you get from export_fig is not what you expected. If an output file contains artifacts that aren't in the on screen figure then make sure that the renderer used for rendering the figure on screen is the same as that used for exporting. To set the renderer used to display the figure, use: +```Matlab +set(figure_handle, 'Renderer', 'opengl'); +``` +After matching the two renderers, if the artifact appears in the on screen figure then you'll need to fix that before exporting. Alternatively you can try changing the renderer used by export_fig. Finally check that it isn't one of the known issues mentioned in the section below. + +**Smoothed/interpolated images in output PDF** - if you produce a PDF using export_fig and images in the PDF look overly smoothed or interpolated, this is because the software you are using to view the PDF is smoothing or interpolating the image data. The image is not smoothed in the PDF file itself. If the software has an option to disable this feature, you should select it. Alternatively, use another PDF viewer that doesn't exhibit this problem. + +**Locating Ghostscript/pdftops** - You may find a dialogue box appears when using export_fig, asking you to locate either [Ghostscript](http://www.ghostscript.com) or [pdftops](http://www.foolabs.com/xpdf). These are separate applications which export_fig requires to perform certain functions. If such a dialogue appears it is because export_fig can't find the application automatically. This is because you either haven't installed it, or it isn't in the normal place. Make sure you install the applications correctly first. They can be downloaded from the following places: + 1. Ghostscript: [www.ghostscript.com](http://www.ghostscript.com) + 2. pdftops (install the Xpdf package): [www.foolabs.com/xpdf](http://www.foolabs.com/xpdf) + +If you choose to install them in a non-default location then point export_fig +to this location using the dialogue box. + +**Undefined function errors** - If you download and run export_fig and get an error similar to this: +``` +??? Undefined function or method 'print2array' for input arguments of type 'double'. +``` +then you are missing one or more of the files that come in the export_fig package. Make sure that you click the "Get from GitHub" button at the top-right of the download [page](http://www.mathworks.co.uk/matlabcentral/fileexchange/23629-exportfig), then extract all the files in the zip file to the same directory. You should then have all the necessary files. + +### Known issues +There are lots of problems with MATLAB's exporting functions, especially `print`. Export_fig is simply a glorified wrapper for MATLAB's `print` function, and doesn't solve all of its bugs (yet?). Some of the problems I know about are: + +**Fonts** - when using the painters renderer, MATLAB can only export a small number of fonts, details of which can be found [here](http://www.mathworks.com/help/releases/R2014a/matlab/creating_plots/choosing-a-printer-driver.html#f3-96545). Export_fig attempts to correct font names in the resulting EPS file (up to a maximum of 11 different fonts in one figure), but this is not always guaranteed to work. In particular, the text positions will be affected. It also does not work for text blocks where the 'Interpreter' property is set to 'latex'. + +Also, when using the painters renderer, ghostscript will sometimes throw an error such as `Error: /undefined in /findfont`. This suggests that ghostscript could not find a definition file for one of your fonts. One possible fix for this is to make sure the file `EXPORT_FIG_PATH/.ignore/gs_font_path.txt` exists and contains a list of paths to the folder(s) containing the necessary font definitions (make sure that they are TrueType definitions!), separated by a semicolon. + +**RGB color data not yet supported in Painter's mode** - you will see this as a warning if you try to export a figure which contains patch objects whose face or vertex colors are specified as an RGB colour, rather than an index into the colormap, using the painters renderer (the default renderer for vector output). This problem can arise if you use `pcolor`, for example. This is a problem with MATLAB's painters renderer, which also affects `print`; there is currently no fix available in export_fig (other than to export to bitmap). The suggested workaround is to avoid colouring patches using RGB. First, try to use colours in the figure's colourmap (instructions [here](http://www.mathworks.co.uk/support/solutions/en/data/1-6OTPQE/)) - change the colourmap, if necessary. If you are using `pcolor`, try using [uimagesc](http://www.mathworks.com/matlabcentral/fileexchange/11368) (on the file exchange) instead. + +**Dashed contour lines appear solid** - when using the painters renderer, MATLAB cannot generate dashed lines using the `contour` function (either on screen or in exported PDF and EPS files). Details can be found [here](http://www.mathworks.com/support/solutions/en/data/1-14PPHB/?solution=1-14PPHB). + +**Text size** - when using the OpenGL or ZBuffer renderers, large text can be resized relative to the figure when exporting at non-screen-resolution (including using anti-alising at screen resolution). This is a feature of MATLAB's `print `function. In this case, try using the `-painters` option. + +**Lighting and transparency** - when using the painters renderer, transparency and lighting effects are not supported. Sorry, but this is an inherent feature of MATLAB's painters renderer. To find out more about the capabilities of each rendering method, see [here](http://www.mathworks.com/access/helpdesk/help/techdoc/creating_plots/f3-84337.html#f3-102410). You can still export transparent objects to vector format (SVG) using the excellent [plot2svg](http://www.mathworks.com/matlabcentral/fileexchange/7401) package, then convert this to PDF, for example using [Inkscape](http://inkscape.org/). However, it can't handle lighting. + +**Lines in patch objects** - when exporting patch objects to PDF using the painters renderer (default), sometimes the output can appear to have lines across the middle of rectangular patches; these lines are the colour of the background, as if there is a crack in the patch, allowing you to see through. This appears to be due to bugs in MATLAB's internal vector rendering code. These lines can often be removed from the PDF using software such as [InkScape](https://inkscape.org). Sometimes disabling anti-aliasing in the PDF-reader software can get rid of the lines ([discussion](https://github.com/altmany/export_fig/issues/44)). + +**Out of memory** - if you run into memory issues when using export_fig, some ways to get round this are: + 1. Reduce the level of anti-aliasing. + 2. Reduce the size of the figure. + 3. Reduce the export resolution (dpi). + 4. Change the renderer to painters or ZBuffer. + +**Errors** - the other common type of errors people get with export_fig are OpenGL errors. This isn't a fault of export_fig, but either a bug in MATLAB's `print`, or your graphics driver getting itself into a state. Always make sure your graphics driver is up-to-date. If it still doesn't work, try using the ZBuffer renderer. + +### Raising issues +If you think you have found a genuine error or issue with export_fig **that is not listed above**, first ensure that the figure looks correct on screen when rendered using the renderer that export_fig is set to use (e.g. if exporting to PDF or EPS, does the figure look correct on screen using the painters renderer, or if exporting to bitmap, does the figure look correct on screen using the OpenGL renderer?). If it looks wrong then the problem is there, and I cannot help (other than to suggest you try exporting using a different renderer). + +Secondly, if exporting to bitmap, do try all the renderers (i.e. try the options `-opengl`, `-zbuffer` and `-painters` separately), to see if one of them does produce an acceptable output, and if so, use that. + +If this still does not help, then ensure that you are using the latest version of export_fig, which is available [here](https://github.com/altmany/export_fig/archive/master.zip). + +If the figure looks correct on screen, but an error exists in the exported output (which cannot be solved using a different renderer) then please feel free to raise an [issue](https://github.com/altmany/export_fig/issues). Please be sure to include the .fig file, the export_fig command you use, the output you get, and a description of what you expected. I can't promise anything, but if it's easy to fix I may indeed do it. Often I will find that the error is due to a bug in MATLAB's `print` function, in which case I will suggest you submit it as a bug to TheMathWorks, and inform me of any fix they suggest. Also, if there's a feature you'd like that isn't supported please tell me what it is and I'll consider implementing it. + +### And finally... + +![](https://farm4.staticflickr.com/3956/15591911455_b9008bd77e_o_d.jpg) + +If you've ever wondered what's going on in the logo on the export_fig download page (reproduced here), then this explanantion is for you. The logo is designed to demonstrate as many of export_fig's features as possible: + +Given a figure containing a translucent mesh (top right), export_fig can export to pdf (bottom centre), which allows the figure to be zoomed-in without losing quality (because it's a vector graphic), but isn't able to reproduce the translucency. Also, depending on the PDF viewer program, small gaps appear between the patches, which are seen here as thin white lines. + +By contrast, when exporting to png (top left), translucency is preserved (see how the graphic below shows through), and the figure is anti-aliased. However, zooming-in does not reveal more detail since png is a bitmap format. Also, lines appear less sharp than in the pdf output. + diff --git a/offline/localizer/altmany-export_fig/append_pdfs.m b/offline/localizer/altmany-export_fig/append_pdfs.m new file mode 100755 index 00000000..9d050e71 --- /dev/null +++ b/offline/localizer/altmany-export_fig/append_pdfs.m @@ -0,0 +1,81 @@ +%APPEND_PDFS Appends/concatenates multiple PDF files +% +% Example: +% append_pdfs(output, input1, input2, ...) +% append_pdfs(output, input_list{:}) +% append_pdfs test.pdf temp1.pdf temp2.pdf +% +% This function appends multiple PDF files to an existing PDF file, or +% concatenates them into a PDF file if the output file doesn't yet exist. +% +% This function requires that you have ghostscript installed on your +% system. Ghostscript can be downloaded from: http://www.ghostscript.com +% +% IN: +% output - string of output file name (including the extension, .pdf). +% If it exists it is appended to; if not, it is created. +% input1 - string of an input file name (including the extension, .pdf). +% All input files are appended in order. +% input_list - cell array list of input file name strings. All input +% files are appended in order. + +% Copyright: Oliver Woodford, 2011 + +% Thanks to Reinhard Knoll for pointing out that appending multiple pdfs in +% one go is much faster than appending them one at a time. + +% Thanks to Michael Teo for reporting the issue of a too long command line. +% Issue resolved on 5/5/2011, by passing gs a command file. + +% Thanks to Martin Wittmann for pointing out the quality issue when +% appending multiple bitmaps. +% Issue resolved (to best of my ability) 1/6/2011, using the prepress +% setting + +% 26/02/15: If temp dir is not writable, use the output folder for temp +% files when appending (Javier Paredes); sanity check of inputs + +function append_pdfs(varargin) + +if nargin < 2, return; end % sanity check + +% Are we appending or creating a new file +append = exist(varargin{1}, 'file') == 2; +output = [tempname '.pdf']; +try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(output,'w'); + fwrite(fid,1); + fclose(fid); + delete(output); + isTempDirOk = true; +catch + % Temp dir is not writable, so use the output folder + [dummy,fname,fext] = fileparts(output); %#ok + fpath = fileparts(varargin{1}); + output = fullfile(fpath,[fname fext]); + isTempDirOk = false; +end +if ~append + output = varargin{1}; + varargin = varargin(2:end); +end +% Create the command file +if isTempDirOk + cmdfile = [tempname '.txt']; +else + cmdfile = fullfile(fpath,[fname '.txt']); +end +fh = fopen(cmdfile, 'w'); +fprintf(fh, '-q -dNOPAUSE -dBATCH -sDEVICE=pdfwrite -dPDFSETTINGS=/prepress -sOutputFile="%s" -f', output); +fprintf(fh, ' "%s"', varargin{:}); +fclose(fh); +% Call ghostscript +ghostscript(['@"' cmdfile '"']); +% Delete the command file +delete(cmdfile); +% Rename the file if needed +if append + movefile(output, varargin{1}); +end +end diff --git a/offline/localizer/altmany-export_fig/copyfig.m b/offline/localizer/altmany-export_fig/copyfig.m new file mode 100755 index 00000000..1746bbdb --- /dev/null +++ b/offline/localizer/altmany-export_fig/copyfig.m @@ -0,0 +1,51 @@ +function fh = copyfig(fh) +%COPYFIG Create a copy of a figure, without changing the figure +% +% Examples: +% fh_new = copyfig(fh_old) +% +% This function will create a copy of a figure, but not change the figure, +% as copyobj sometimes does, e.g. by changing legends. +% +% IN: +% fh_old - The handle of the figure to be copied. Default: gcf. +% +% OUT: +% fh_new - The handle of the created figure. + +% Copyright (C) Oliver Woodford 2012 + +% 26/02/15: If temp dir is not writable, use the dest folder for temp +% destination files (Javier Paredes) +% 15/04/15: Suppress warnings during copyobj (Dun Kirk comment on FEX page 2013-10-02) + + % Set the default + if nargin == 0 + fh = gcf; + end + % Is there a legend? + if isempty(findall(fh, 'Type', 'axes', 'Tag', 'legend')) + % Safe to copy using copyobj + oldWarn = warning('off'); %#ok %Suppress warnings during copyobj (Dun Kirk comment on FEX page 2013-10-02) + fh = copyobj(fh, 0); + warning(oldWarn); + else + % copyobj will change the figure, so save and then load it instead + tmp_nam = [tempname '.fig']; + try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); % cleanup + catch + % Temp dir is not writable, so use the current folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = pwd; + tmp_nam = fullfile(fpath,[fname fext]); + end + hgsave(fh, tmp_nam); + fh = hgload(tmp_nam); + delete(tmp_nam); + end +end diff --git a/offline/localizer/altmany-export_fig/crop_borders.m b/offline/localizer/altmany-export_fig/crop_borders.m new file mode 100755 index 00000000..21524960 --- /dev/null +++ b/offline/localizer/altmany-export_fig/crop_borders.m @@ -0,0 +1,152 @@ +function [A, vA, vB, bb_rel] = crop_borders(A, bcol, padding, crop_amounts) +%CROP_BORDERS Crop the borders of an image or stack of images +% +% [B, vA, vB, bb_rel] = crop_borders(A, bcol, [padding]) +% +%IN: +% A - HxWxCxN stack of images. +% bcol - Cx1 background colour vector. +% padding - scalar indicating how much padding to have in relation to +% the cropped-image-size (0<=padding<=1). Default: 0 +% crop_amounts - 4-element vector of crop amounts: [top,right,bottom,left] +% where NaN/Inf indicate auto-cropping, 0 means no cropping, +% and any other value mean cropping in pixel amounts. +% +%OUT: +% B - JxKxCxN cropped stack of images. +% vA - coordinates in A that contain the cropped image +% vB - coordinates in B where the cropped version of A is placed +% bb_rel - relative bounding box (used for eps-cropping) + +%{ +% 06/03/15: Improved image cropping thanks to Oscar Hartogensis +% 08/06/15: Fixed issue #76: case of transparent figure bgcolor +% 21/02/16: Enabled specifying non-automated crop amounts +% 04/04/16: Fix per Luiz Carvalho for old Matlab releases +%} + + if nargin < 3 + padding = 0; + end + if nargin < 4 + crop_amounts = nan(1,4); % =auto-cropping + end + crop_amounts(end+1:4) = NaN; % fill missing values with NaN + + [h, w, c, n] = size(A); + if isempty(bcol) % case of transparent bgcolor + bcol = A(ceil(end/2),1,:,1); + end + if isscalar(bcol) + bcol = bcol(ones(c, 1)); + end + + % Crop margin from left + if ~isfinite(crop_amounts(4)) + bail = false; + for l = 1:w + for a = 1:c + if ~all(col(A(:,l,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + l = 1 + abs(crop_amounts(4)); + end + + % Crop margin from right + if ~isfinite(crop_amounts(2)) + bcol = A(ceil(end/2),w,:,1); + bail = false; + for r = w:-1:l + for a = 1:c + if ~all(col(A(:,r,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + r = w - abs(crop_amounts(2)); + end + + % Crop margin from top + if ~isfinite(crop_amounts(1)) + bcol = A(1,ceil(end/2),:,1); + bail = false; + for t = 1:h + for a = 1:c + if ~all(col(A(t,:,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + t = 1 + abs(crop_amounts(1)); + end + + % Crop margin from bottom + bcol = A(h,ceil(end/2),:,1); + if ~isfinite(crop_amounts(3)) + bail = false; + for b = h:-1:t + for a = 1:c + if ~all(col(A(b,:,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + b = h - abs(crop_amounts(3)); + end + + if padding == 0 % no padding + if ~isequal([t b l r], [1 h 1 w]) % Check if we're actually croppping + padding = 1; % Leave one boundary pixel to avoid bleeding on resize + end + elseif abs(padding) < 1 % pad value is a relative fraction of image size + padding = sign(padding)*round(mean([b-t r-l])*abs(padding)); % ADJUST PADDING + else % pad value is in units of 1/72" points + padding = round(padding); % fix cases of non-integer pad value + end + + if padding > 0 % extra padding + % Create an empty image, containing the background color, that has the + % cropped image size plus the padded border + B = repmat(bcol,[(b-t)+1+padding*2,(r-l)+1+padding*2,1,n]); % Fix per Luiz Carvalho + % vA - coordinates in A that contain the cropped image + vA = [t b l r]; + % vB - coordinates in B where the cropped version of A will be placed + vB = [padding+1, (b-t)+1+padding, padding+1, (r-l)+1+padding]; + % Place the original image in the empty image + B(vB(1):vB(2), vB(3):vB(4), :, :) = A(vA(1):vA(2), vA(3):vA(4), :, :); + A = B; + else % extra cropping + vA = [t-padding b+padding l-padding r+padding]; + A = A(vA(1):vA(2), vA(3):vA(4), :, :); + vB = [NaN NaN NaN NaN]; + end + + % For EPS cropping, determine the relative BoundingBox - bb_rel + bb_rel = [l-1 h-b-1 r+1 h-t+1]./[w h w h]; +end + +function A = col(A) + A = A(:); +end diff --git a/offline/localizer/altmany-export_fig/eps2pdf.m b/offline/localizer/altmany-export_fig/eps2pdf.m new file mode 100755 index 00000000..6c109642 --- /dev/null +++ b/offline/localizer/altmany-export_fig/eps2pdf.m @@ -0,0 +1,189 @@ +function eps2pdf(source, dest, crop, append, gray, quality, gs_options) +%EPS2PDF Convert an eps file to pdf format using ghostscript +% +% Examples: +% eps2pdf source dest +% eps2pdf(source, dest, crop) +% eps2pdf(source, dest, crop, append) +% eps2pdf(source, dest, crop, append, gray) +% eps2pdf(source, dest, crop, append, gray, quality) +% eps2pdf(source, dest, crop, append, gray, quality, gs_options) +% +% This function converts an eps file to pdf format. The output can be +% optionally cropped and also converted to grayscale. If the output pdf +% file already exists then the eps file can optionally be appended as a new +% page on the end of the eps file. The level of bitmap compression can also +% optionally be set. +% +% This function requires that you have ghostscript installed on your +% system. Ghostscript can be downloaded from: http://www.ghostscript.com +% +% Inputs: +% source - filename of the source eps file to convert. The filename is +% assumed to already have the extension ".eps". +% dest - filename of the destination pdf file. The filename is assumed +% to already have the extension ".pdf". +% crop - boolean indicating whether to crop the borders off the pdf. +% Default: true. +% append - boolean indicating whether the eps should be appended to the +% end of the pdf as a new page (if the pdf exists already). +% Default: false. +% gray - boolean indicating whether the output pdf should be grayscale +% or not. Default: false. +% quality - scalar indicating the level of image bitmap quality to +% output. A larger value gives a higher quality. quality > 100 +% gives lossless output. Default: ghostscript prepress default. +% gs_options - optional ghostscript options (e.g.: '-dNoOutputFonts'). If +% multiple options are needed, enclose in call array: {'-a','-b'} + +% Copyright (C) Oliver Woodford 2009-2014, Yair Altman 2015- + +% Suggestion of appending pdf files provided by Matt C at: +% http://www.mathworks.com/matlabcentral/fileexchange/23629 + +% Thank you to Fabio Viola for pointing out compression artifacts, leading +% to the quality setting. +% Thank you to Scott for pointing out the subsampling of very small images, +% which was fixed for lossless compression settings. + +% 9/12/2011 Pass font path to ghostscript. +% 26/02/15: If temp dir is not writable, use the dest folder for temp +% destination files (Javier Paredes) +% 28/02/15: Enable users to specify optional ghostscript options (issue #36) +% 01/03/15: Upon GS error, retry without the -sFONTPATH= option (this might solve +% some /findfont errors according to James Rankin, FEX Comment 23/01/15) +% 23/06/15: Added extra debug info in case of ghostscript error; code indentation +% 04/10/15: Suggest a workaround for issue #41 (missing font path; thanks Mariia Fedotenkova) +% 22/02/16: Bug fix from latest release of this file (workaround for issue #41) + + % Intialise the options string for ghostscript + options = ['-q -dNOPAUSE -dBATCH -sDEVICE=pdfwrite -dPDFSETTINGS=/prepress -sOutputFile="' dest '"']; + % Set crop option + if nargin < 3 || crop + options = [options ' -dEPSCrop']; + end + % Set the font path + fp = font_path(); + if ~isempty(fp) + options = [options ' -sFONTPATH="' fp '"']; + end + % Set the grayscale option + if nargin > 4 && gray + options = [options ' -sColorConversionStrategy=Gray -dProcessColorModel=/DeviceGray']; + end + % Set the bitmap quality + if nargin > 5 && ~isempty(quality) + options = [options ' -dAutoFilterColorImages=false -dAutoFilterGrayImages=false']; + if quality > 100 + options = [options ' -dColorImageFilter=/FlateEncode -dGrayImageFilter=/FlateEncode -c ".setpdfwrite << /ColorImageDownsampleThreshold 10 /GrayImageDownsampleThreshold 10 >> setdistillerparams"']; + else + options = [options ' -dColorImageFilter=/DCTEncode -dGrayImageFilter=/DCTEncode']; + v = 1 + (quality < 80); + quality = 1 - quality / 100; + s = sprintf('<< /QFactor %.2f /Blend 1 /HSample [%d 1 1 %d] /VSample [%d 1 1 %d] >>', quality, v, v, v, v); + options = sprintf('%s -c ".setpdfwrite << /ColorImageDict %s /GrayImageDict %s >> setdistillerparams"', options, s, s); + end + end + % Enable users to specify optional ghostscript options (issue #36) + if nargin > 6 && ~isempty(gs_options) + if iscell(gs_options) + gs_options = sprintf(' %s',gs_options{:}); + elseif ~ischar(gs_options) + error('gs_options input argument must be a string or cell-array of strings'); + else + gs_options = [' ' gs_options]; + end + options = [options gs_options]; + end + % Check if the output file exists + if nargin > 3 && append && exist(dest, 'file') == 2 + % File exists - append current figure to the end + tmp_nam = tempname; + try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); + catch + % Temp dir is not writable, so use the dest folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = fileparts(dest); + tmp_nam = fullfile(fpath,[fname fext]); + end + % Copy the file + copyfile(dest, tmp_nam); + % Add the output file names + options = [options ' -f "' tmp_nam '" "' source '"']; + try + % Convert to pdf using ghostscript + [status, message] = ghostscript(options); + catch me + % Delete the intermediate file + delete(tmp_nam); + rethrow(me); + end + % Delete the intermediate file + delete(tmp_nam); + else + % File doesn't exist or should be over-written + % Add the output file names + options = [options ' -f "' source '"']; + % Convert to pdf using ghostscript + [status, message] = ghostscript(options); + end + % Check for error + if status + % Retry without the -sFONTPATH= option (this might solve some GS + % /findfont errors according to James Rankin, FEX Comment 23/01/15) + orig_options = options; + if ~isempty(fp) + options = regexprep(options, ' -sFONTPATH=[^ ]+ ',' '); + status = ghostscript(options); + if ~status, return; end % hurray! (no error) + end + % Report error + if isempty(message) + error('Unable to generate pdf. Check destination directory is writable.'); + elseif ~isempty(strfind(message,'/typecheck in /findfont')) + % Suggest a workaround for issue #41 (missing font path) + font_name = strtrim(regexprep(message,'.*Operand stack:\s*(.*)\s*Execution.*','$1')); + fprintf(2, 'Ghostscript error: could not find the following font(s): %s\n', font_name); + fpath = fileparts(mfilename('fullpath')); + gs_fonts_file = fullfile(fpath, '.ignore', 'gs_font_path.txt'); + fprintf(2, ' try to add the font''s folder to your %s file\n\n', gs_fonts_file); + error('export_fig error'); + else + fprintf(2, '\nGhostscript error: perhaps %s is open by another application\n', dest); + if ~isempty(gs_options) + fprintf(2, ' or maybe the%s option(s) are not accepted by your GS version\n', gs_options); + end + fprintf(2, 'Ghostscript options: %s\n\n', orig_options); + error(message); + end + end +end + +% Function to return (and create, where necessary) the font path +function fp = font_path() + fp = user_string('gs_font_path'); + if ~isempty(fp) + return + end + % Create the path + % Start with the default path + fp = getenv('GS_FONTPATH'); + % Add on the typical directories for a given OS + if ispc + if ~isempty(fp) + fp = [fp ';']; + end + fp = [fp getenv('WINDIR') filesep 'Fonts']; + else + if ~isempty(fp) + fp = [fp ':']; + end + fp = [fp '/usr/share/fonts:/usr/local/share/fonts:/usr/share/fonts/X11:/usr/local/share/fonts/X11:/usr/share/fonts/truetype:/usr/local/share/fonts/truetype']; + end + user_string('gs_font_path', fp); +end diff --git a/offline/localizer/altmany-export_fig/export_fig.m b/offline/localizer/altmany-export_fig/export_fig.m new file mode 100755 index 00000000..06b249e9 --- /dev/null +++ b/offline/localizer/altmany-export_fig/export_fig.m @@ -0,0 +1,1282 @@ +function [imageData, alpha] = export_fig(varargin) +%EXPORT_FIG Exports figures in a publication-quality format +% +% Examples: +% imageData = export_fig +% [imageData, alpha] = export_fig +% export_fig filename +% export_fig filename -format1 -format2 +% export_fig ... -nocrop +% export_fig ... -c[,,,] +% export_fig ... -transparent +% export_fig ... -native +% export_fig ... -m +% export_fig ... -r +% export_fig ... -a +% export_fig ... -q +% export_fig ... -p +% export_fig ... -d +% export_fig ... -depsc +% export_fig ... - +% export_fig ... - +% export_fig ... -append +% export_fig ... -bookmark +% export_fig ... -clipboard +% export_fig ... -update +% export_fig ... -nofontswap +% export_fig(..., handle) +% +% This function saves a figure or single axes to one or more vector and/or +% bitmap file formats, and/or outputs a rasterized version to the workspace, +% with the following properties: +% - Figure/axes reproduced as it appears on screen +% - Cropped borders (optional) +% - Embedded fonts (vector formats) +% - Improved line and grid line styles +% - Anti-aliased graphics (bitmap formats) +% - Render images at native resolution (optional for bitmap formats) +% - Transparent background supported (pdf, eps, png, tif) +% - Semi-transparent patch objects supported (png & tif only) +% - RGB, CMYK or grayscale output (CMYK only with pdf, eps, tiff) +% - Variable image compression, including lossless (pdf, eps, jpg) +% - Optionally append to file (pdf, tiff) +% - Vector formats: pdf, eps +% - Bitmap formats: png, tiff, jpg, bmp, export to workspace +% +% This function is especially suited to exporting figures for use in +% publications and presentations, because of the high quality and +% portability of media produced. +% +% Note that the background color and figure dimensions are reproduced +% (the latter approximately, and ignoring cropping & magnification) in the +% output file. For transparent background (and semi-transparent patch +% objects), use the -transparent option or set the figure 'Color' property +% to 'none'. To make axes transparent set the axes 'Color' property to +% 'none'. PDF, EPS, TIF & PNG are the only formats that support a transparent +% background; only TIF & PNG formats support transparency of patch objects. +% +% The choice of renderer (opengl, zbuffer or painters) has a large impact +% on the quality of output. The default value (opengl for bitmaps, painters +% for vector formats) generally gives good results, but if you aren't +% satisfied then try another renderer. Notes: 1) For vector formats (EPS, +% PDF), only painters generates vector graphics. 2) For bitmaps, only +% opengl can render transparent patch objects correctly. 3) For bitmaps, +% only painters will correctly scale line dash and dot lengths when +% magnifying or anti-aliasing. 4) Fonts may be substitued with Courier when +% using painters. +% +% When exporting to vector format (PDF & EPS) and bitmap format using the +% painters renderer, this function requires that ghostscript is installed +% on your system. You can download this from: +% http://www.ghostscript.com +% When exporting to eps it additionally requires pdftops, from the Xpdf +% suite of functions. You can download this from: +% http://www.foolabs.com/xpdf +% +% Inputs: +% filename - string containing the name (optionally including full or +% relative path) of the file the figure is to be saved as. If +% a path is not specified, the figure is saved in the current +% directory. If no name and no output arguments are specified, +% the default name, 'export_fig_out', is used. If neither a +% file extension nor a format are specified, a ".png" is added +% and the figure saved in that format. +% -format1, -format2, etc. - strings containing the extensions of the +% file formats the figure is to be saved as. +% Valid options are: '-pdf', '-eps', '-png', +% '-tif', '-jpg' and '-bmp'. All combinations +% of formats are valid. +% -nocrop - option indicating that the borders of the output are not to +% be cropped. +% -c[,,,] - option indicating crop amounts. Must be +% a 4-element vector of numeric values: [top,right,bottom,left] +% where NaN/Inf indicate auto-cropping, 0 means no cropping, +% and any other value mean cropping in pixel amounts. +% -transparent - option indicating that the figure background is to be +% made transparent (png, pdf, tif and eps output only). +% -m - option where val indicates the factor to magnify the +% on-screen figure pixel dimensions by when generating bitmap +% outputs (does not affect vector formats). Default: '-m1'. +% -r - option val indicates the resolution (in pixels per inch) to +% export bitmap and vector outputs at, keeping the dimensions +% of the on-screen figure. Default: '-r864' (for vector output +% only). Note that the -m option overides the -r option for +% bitmap outputs only. +% -native - option indicating that the output resolution (when outputting +% a bitmap format) should be such that the vertical resolution +% of the first suitable image found in the figure is at the +% native resolution of that image. To specify a particular +% image to use, give it the tag 'export_fig_native'. Notes: +% This overrides any value set with the -m and -r options. It +% also assumes that the image is displayed front-to-parallel +% with the screen. The output resolution is approximate and +% should not be relied upon. Anti-aliasing can have adverse +% effects on image quality (disable with the -a1 option). +% -a1, -a2, -a3, -a4 - option indicating the amount of anti-aliasing to +% use for bitmap outputs. '-a1' means no anti- +% aliasing; '-a4' is the maximum amount (default). +% - - option to force a particular renderer (painters, opengl or +% zbuffer). Default value: opengl for bitmap formats or +% figures with patches and/or transparent annotations; +% painters for vector formats without patches/transparencies. +% - - option indicating which colorspace color figures should +% be saved in: RGB (default), CMYK or gray. CMYK is only +% supported in pdf, eps and tiff output. +% -q - option to vary bitmap image quality (in pdf, eps and jpg +% files only). Larger val, in the range 0-100, gives higher +% quality/lower compression. val > 100 gives lossless +% compression. Default: '-q95' for jpg, ghostscript prepress +% default for pdf & eps. Note: lossless compression can +% sometimes give a smaller file size than the default lossy +% compression, depending on the type of images. +% -p - option to pad a border of width val to exported files, where +% val is either a relative size with respect to cropped image +% size (i.e. p=0.01 adds a 1% border). For EPS & PDF formats, +% val can also be integer in units of 1/72" points (abs(val)>1). +% val can be positive (padding) or negative (extra cropping). +% If used, the -nocrop flag will be ignored, i.e. the image will +% always be cropped and then padded. Default: 0 (i.e. no padding). +% -append - option indicating that if the file (pdfs only) already +% exists, the figure is to be appended as a new page, instead +% of being overwritten (default). +% -bookmark - option to indicate that a bookmark with the name of the +% figure is to be created in the output file (pdf only). +% -clipboard - option to save output as an image on the system clipboard. +% Note: background transparency is not preserved in clipboard +% -d - option to indicate a ghostscript setting. For example, +% -dMaxBitmap=0 or -dNoOutputFonts (Ghostscript 9.15+). +% -depsc - option to use EPS level-3 rather than the default level-2 print +% device. This solves some bugs with Matlab's default -depsc2 device +% such as discolored subplot lines on images (vector formats only). +% -update - option to download and install the latest version of export_fig +% -nofontswap - option to avoid font swapping. Font swapping is automatically +% done in vector formats (only): 11 standard Matlab fonts are +% replaced by the original figure fonts. This option prevents this. +% handle - The handle of the figure, axes or uipanels (can be an array of +% handles, but the objects must be in the same figure) to be +% saved. Default: gcf. +% +% Outputs: +% imageData - MxNxC uint8 image array of the exported image. +% alpha - MxN single array of alphamatte values in the range [0,1], +% for the case when the background is transparent. +% +% Some helpful examples and tips can be found at: +% https://github.com/altmany/export_fig +% +% See also PRINT, SAVEAS, ScreenCapture (on the Matlab File Exchange) + +%{ +% Copyright (C) Oliver Woodford 2008-2014, Yair Altman 2015- + +% The idea of using ghostscript is inspired by Peder Axensten's SAVEFIG +% (fex id: 10889) which is itself inspired by EPS2PDF (fex id: 5782). +% The idea for using pdftops came from the MATLAB newsgroup (id: 168171). +% The idea of editing the EPS file to change line styles comes from Jiro +% Doke's FIXPSLINESTYLE (fex id: 17928). +% The idea of changing dash length with line width came from comments on +% fex id: 5743, but the implementation is mine :) +% The idea of anti-aliasing bitmaps came from Anders Brun's MYAA (fex id: +% 20979). +% The idea of appending figures in pdfs came from Matt C in comments on the +% FEX (id: 23629) + +% Thanks to Roland Martin for pointing out the colour MATLAB +% bug/feature with colorbar axes and transparent backgrounds. +% Thanks also to Andrew Matthews for describing a bug to do with the figure +% size changing in -nodisplay mode. I couldn't reproduce it, but included a +% fix anyway. +% Thanks to Tammy Threadgill for reporting a bug where an axes is not +% isolated from gui objects. +%} +%{ +% 23/02/12: Ensure that axes limits don't change during printing +% 14/03/12: Fix bug in fixing the axes limits (thanks to Tobias Lamour for reporting it). +% 02/05/12: Incorporate patch of Petr Nechaev (many thanks), enabling bookmarking of figures in pdf files. +% 09/05/12: Incorporate patch of Arcelia Arrieta (many thanks), to keep tick marks fixed. +% 12/12/12: Add support for isolating uipanels. Thanks to michael for suggesting it. +% 25/09/13: Add support for changing resolution in vector formats. Thanks to Jan Jaap Meijer for suggesting it. +% 07/05/14: Add support for '~' at start of path. Thanks to Sally Warner for suggesting it. +% 24/02/15: Fix Matlab R2014b bug (issue #34): plot markers are not displayed when ZLimMode='manual' +% 25/02/15: Fix issue #4 (using HG2 on R2014a and earlier) +% 25/02/15: Fix issue #21 (bold TeX axes labels/titles in R2014b) +% 26/02/15: If temp dir is not writable, use the user-specified folder for temporary EPS/PDF files (Javier Paredes) +% 27/02/15: Modified repository URL from github.com/ojwoodford to /altmany +% Indented main function +% Added top-level try-catch block to display useful workarounds +% 28/02/15: Enable users to specify optional ghostscript options (issue #36) +% 06/03/15: Improved image padding & cropping thanks to Oscar Hartogensis +% 26/03/15: Fixed issue #49 (bug with transparent grayscale images); fixed out-of-memory issue +% 26/03/15: Fixed issue #42: non-normalized annotations on HG1 +% 26/03/15: Fixed issue #46: Ghostscript crash if figure units <> pixels +% 27/03/15: Fixed issue #39: bad export of transparent annotations/patches +% 28/03/15: Fixed issue #50: error on some Matlab versions with the fix for issue #42 +% 29/03/15: Fixed issue #33: bugs in Matlab's print() function with -cmyk +% 29/03/15: Improved processing of input args (accept space between param name & value, related to issue #51) +% 30/03/15: When exporting *.fig files, then saveas *.fig if figure is open, otherwise export the specified fig file +% 30/03/15: Fixed edge case bug introduced yesterday (commit #ae1755bd2e11dc4e99b95a7681f6e211b3fa9358) +% 09/04/15: Consolidated header comment sections; initialize output vars only if requested (nargout>0) +% 14/04/15: Workaround for issue #45: lines in image subplots are exported in invalid color +% 15/04/15: Fixed edge-case in parsing input parameters; fixed help section to show the -depsc option (issue #45) +% 21/04/15: Bug fix: Ghostscript croaks on % chars in output PDF file (reported by Sven on FEX page, 15-Jul-2014) +% 22/04/15: Bug fix: Pdftops croaks on relative paths (reported by Tintin Milou on FEX page, 19-Jan-2015) +% 04/05/15: Merged fix #63 (Kevin Mattheus Moerman): prevent tick-label changes during export +% 07/05/15: Partial fix for issue #65: PDF export used painters rather than opengl renderer (thanks Nguyenr) +% 08/05/15: Fixed issue #65: bad PDF append since commit #e9f3cdf 21/04/15 (thanks Robert Nguyen) +% 12/05/15: Fixed issue #67: exponent labels cropped in export, since fix #63 (04/05/15) +% 28/05/15: Fixed issue #69: set non-bold label font only if the string contains symbols (\beta etc.), followup to issue #21 +% 29/05/15: Added informative error message in case user requested SVG output (issue #72) +% 09/06/15: Fixed issue #58: -transparent removed anti-aliasing when exporting to PNG +% 19/06/15: Added -update option to download and install the latest version of export_fig +% 07/07/15: Added -nofontswap option to avoid font-swapping in EPS/PDF +% 16/07/15: Fixed problem with anti-aliasing on old Matlab releases +% 11/09/15: Fixed issue #103: magnification must never become negative; also fixed reported error msg in parsing input params +% 26/09/15: Alert if trying to export transparent patches/areas to non-PNG outputs (issue #108) +% 04/10/15: Do not suggest workarounds for certain errors that have already been handled previously +% 01/11/15: Fixed issue #112: use same renderer in print2eps as export_fig (thanks to Jesús Pestana Puerta) +% 10/11/15: Custom GS installation webpage for MacOS. Thanks to Andy Hueni via FEX +% 19/11/15: Fixed clipboard export in R2015b (thanks to Dan K via FEX) +% 21/02/16: Added -c option for indicating specific crop amounts (idea by Cedric Noordam on FEX) +% 08/05/16: Added message about possible error reason when groot.Units~=pixels (issue #149) +% 17/05/16: Fixed case of image YData containing more than 2 elements (issue #151) +% 08/08/16: Enabled exporting transparency to TIF, in addition to PNG/PDF (issue #168) +%} + + if nargout + [imageData, alpha] = deal([]); + end + hadError = false; + displaySuggestedWorkarounds = true; + + % Ensure the figure is rendered correctly _now_ so that properties like axes limits are up-to-date + drawnow; + pause(0.05); % this solves timing issues with Java Swing's EDT (http://undocumentedmatlab.com/blog/solving-a-matlab-hang-problem) + + % Parse the input arguments + fig = get(0, 'CurrentFigure'); + [fig, options] = parse_args(nargout, fig, varargin{:}); + + % Ensure that we have a figure handle + if isequal(fig,-1) + return; % silent bail-out + elseif isempty(fig) + error('No figure found'); + end + + % Isolate the subplot, if it is one + cls = all(ismember(get(fig, 'Type'), {'axes', 'uipanel'})); + if cls + % Given handles of one or more axes, so isolate them from the rest + fig = isolate_axes(fig); + else + % Check we have a figure + if ~isequal(get(fig, 'Type'), 'figure'); + error('Handle must be that of a figure, axes or uipanel'); + end + % Get the old InvertHardcopy mode + old_mode = get(fig, 'InvertHardcopy'); + end + + % Hack the font units where necessary (due to a font rendering bug in print?). + % This may not work perfectly in all cases. + % Also it can change the figure layout if reverted, so use a copy. + magnify = options.magnify * options.aa_factor; + if isbitmap(options) && magnify ~= 1 + fontu = findall(fig, 'FontUnits', 'normalized'); + if ~isempty(fontu) + % Some normalized font units found + if ~cls + fig = copyfig(fig); + set(fig, 'Visible', 'off'); + fontu = findall(fig, 'FontUnits', 'normalized'); + cls = true; + end + set(fontu, 'FontUnits', 'points'); + end + end + + try + % MATLAB "feature": axes limits and tick marks can change when printing + Hlims = findall(fig, 'Type', 'axes'); + if ~cls + % Record the old axes limit and tick modes + Xlims = make_cell(get(Hlims, 'XLimMode')); + Ylims = make_cell(get(Hlims, 'YLimMode')); + Zlims = make_cell(get(Hlims, 'ZLimMode')); + Xtick = make_cell(get(Hlims, 'XTickMode')); + Ytick = make_cell(get(Hlims, 'YTickMode')); + Ztick = make_cell(get(Hlims, 'ZTickMode')); + Xlabel = make_cell(get(Hlims, 'XTickLabelMode')); + Ylabel = make_cell(get(Hlims, 'YTickLabelMode')); + Zlabel = make_cell(get(Hlims, 'ZTickLabelMode')); + end + + % Set all axes limit and tick modes to manual, so the limits and ticks can't change + % Fix Matlab R2014b bug (issue #34): plot markers are not displayed when ZLimMode='manual' + set(Hlims, 'XLimMode', 'manual', 'YLimMode', 'manual'); + set_tick_mode(Hlims, 'X'); + set_tick_mode(Hlims, 'Y'); + if ~using_hg2(fig) + set(Hlims,'ZLimMode', 'manual'); + set_tick_mode(Hlims, 'Z'); + end + catch + % ignore - fix issue #4 (using HG2 on R2014a and earlier) + end + + % Fix issue #21 (bold TeX axes labels/titles in R2014b when exporting to EPS/PDF) + try + if using_hg2(fig) && isvector(options) + % Set the FontWeight of axes labels/titles to 'normal' + % Fix issue #69: set non-bold font only if the string contains symbols (\beta etc.) + texLabels = findall(fig, 'type','text', 'FontWeight','bold'); + symbolIdx = ~cellfun('isempty',strfind({texLabels.String},'\')); + set(texLabels(symbolIdx), 'FontWeight','normal'); + end + catch + % ignore + end + + % Fix issue #42: non-normalized annotations on HG1 (internal Matlab bug) + annotationHandles = []; + try + if ~using_hg2(fig) + annotationHandles = findall(fig,'Type','hggroup','-and','-property','Units','-and','-not','Units','norm'); + try % suggested by Jesús Pestana Puerta (jespestana) 30/9/2015 + originalUnits = get(annotationHandles,'Units'); + set(annotationHandles,'Units','norm'); + catch + end + end + catch + % should never happen, but ignore in any case - issue #50 + end + + % Fix issue #46: Ghostscript crash if figure units <> pixels + oldFigUnits = get(fig,'Units'); + set(fig,'Units','pixels'); + + % Set to print exactly what is there + set(fig, 'InvertHardcopy', 'off'); + % Set the renderer + switch options.renderer + case 1 + renderer = '-opengl'; + case 2 + renderer = '-zbuffer'; + case 3 + renderer = '-painters'; + otherwise + renderer = '-opengl'; % Default for bitmaps + end + + % Handle transparent patches + hasTransparency = ~isempty(findall(fig,'-property','FaceAlpha','-and','-not','FaceAlpha',1)); + hasPatches = ~isempty(findall(fig,'type','patch')); + if hasTransparency + % Alert if trying to export transparent patches/areas to non-supported outputs (issue #108) + % http://www.mathworks.com/matlabcentral/answers/265265-can-export_fig-or-else-draw-vector-graphics-with-transparent-surfaces + % TODO - use transparency when exporting to PDF by not passing via print2eps + msg = 'export_fig currently supports transparent patches/areas only in PNG output. '; + if options.pdf + warning('export_fig:transparency', '%s\nTo export transparent patches/areas to PDF, use the print command:\n print(gcf, ''-dpdf'', ''%s.pdf'');', msg, options.name); + elseif ~options.png && ~options.tif % issue #168 + warning('export_fig:transparency', '%s\nTo export the transparency correctly, try using the ScreenCapture utility on the Matlab File Exchange: http://bit.ly/1QFrBip', msg); + end + end + + try + % Do the bitmap formats first + if isbitmap(options) + if abs(options.bb_padding) > 1 + displaySuggestedWorkarounds = false; + error('For bitmap output (png,jpg,tif,bmp) the padding value (-p) must be between -1 100 + imwrite(A, [options.name '.jpg'], 'Mode', 'lossless'); + else + imwrite(A, [options.name '.jpg'], 'Quality', quality); + end + end + % Save tif images in cmyk if wanted (and possible) + if options.tif + if options.colourspace == 1 && size(A, 3) == 3 + A = double(255 - A); + K = min(A, [], 3); + K_ = 255 ./ max(255 - K, 1); + C = (A(:,:,1) - K) .* K_; + M = (A(:,:,2) - K) .* K_; + Y = (A(:,:,3) - K) .* K_; + A = uint8(cat(3, C, M, Y, K)); + clear C M Y K K_ + end + append_mode = {'overwrite', 'append'}; + imwrite(A, [options.name '.tif'], 'Resolution', options.magnify*get(0, 'ScreenPixelsPerInch'), 'WriteMode', append_mode{options.append+1}); + end + end + + % Now do the vector formats + if isvector(options) + % Set the default renderer to painters + if ~options.renderer + if hasTransparency || hasPatches + % This is *MUCH* slower, but more accurate for patches and transparent annotations (issue #39) + renderer = '-opengl'; + else + renderer = '-painters'; + end + end + options.rendererStr = renderer; % fix for issue #112 + % Generate some filenames + tmp_nam = [tempname '.eps']; + try + % Ensure that the temp dir is writable (Javier Paredes 30/1/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); + isTempDirOk = true; + catch + % Temp dir is not writable, so use the user-specified folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = fileparts(options.name); + tmp_nam = fullfile(fpath,[fname fext]); + isTempDirOk = false; + end + if isTempDirOk + pdf_nam_tmp = [tempname '.pdf']; + else + pdf_nam_tmp = fullfile(fpath,[fname '.pdf']); + end + if options.pdf + pdf_nam = [options.name '.pdf']; + try copyfile(pdf_nam, pdf_nam_tmp, 'f'); catch, end % fix for issue #65 + else + pdf_nam = pdf_nam_tmp; + end + % Generate the options for print + p2eArgs = {renderer, sprintf('-r%d', options.resolution)}; + if options.colourspace == 1 % CMYK + % Issue #33: due to internal bugs in Matlab's print() function, we can't use its -cmyk option + %p2eArgs{end+1} = '-cmyk'; + end + if ~options.crop + % Issue #56: due to internal bugs in Matlab's print() function, we can't use its internal cropping mechanism, + % therefore we always use '-loose' (in print2eps.m) and do our own cropping (in crop_borders) + %p2eArgs{end+1} = '-loose'; + end + if any(strcmpi(varargin,'-depsc')) + % Issue #45: lines in image subplots are exported in invalid color. + % The workaround is to use the -depsc parameter instead of the default -depsc2 + p2eArgs{end+1} = '-depsc'; + end + try + % Generate an eps + print2eps(tmp_nam, fig, options, p2eArgs{:}); + % Remove the background, if desired + if options.transparent && ~isequal(get(fig, 'Color'), 'none') + eps_remove_background(tmp_nam, 1 + using_hg2(fig)); + end + % Fix colorspace to CMYK, if requested (workaround for issue #33) + if options.colourspace == 1 % CMYK + % Issue #33: due to internal bugs in Matlab's print() function, we can't use its -cmyk option + change_rgb_to_cmyk(tmp_nam); + end + % Add a bookmark to the PDF if desired + if options.bookmark + fig_nam = get(fig, 'Name'); + if isempty(fig_nam) + warning('export_fig:EmptyBookmark', 'Bookmark requested for figure with no name. Bookmark will be empty.'); + end + add_bookmark(tmp_nam, fig_nam); + end + % Generate a pdf + eps2pdf(tmp_nam, pdf_nam_tmp, 1, options.append, options.colourspace==2, options.quality, options.gs_options); + % Ghostscript croaks on % chars in the output PDF file, so use tempname and then rename the file + try movefile(pdf_nam_tmp, pdf_nam, 'f'); catch, end + catch ex + % Delete the eps + delete(tmp_nam); + rethrow(ex); + end + % Delete the eps + delete(tmp_nam); + if options.eps + try + % Generate an eps from the pdf + % since pdftops can't handle relative paths (e.g., '..\'), use a temp file + eps_nam_tmp = strrep(pdf_nam_tmp,'.pdf','.eps'); + pdf2eps(pdf_nam, eps_nam_tmp); + movefile(eps_nam_tmp, [options.name '.eps'], 'f'); + catch ex + if ~options.pdf + % Delete the pdf + delete(pdf_nam); + end + try delete(eps_nam_tmp); catch, end + rethrow(ex); + end + if ~options.pdf + % Delete the pdf + delete(pdf_nam); + end + end + end + + % Revert the figure or close it (if requested) + if cls || options.closeFig + % Close the created figure + close(fig); + else + % Reset the hardcopy mode + set(fig, 'InvertHardcopy', old_mode); + % Reset the axes limit and tick modes + for a = 1:numel(Hlims) + try + set(Hlims(a), 'XLimMode', Xlims{a}, 'YLimMode', Ylims{a}, 'ZLimMode', Zlims{a},... + 'XTickMode', Xtick{a}, 'YTickMode', Ytick{a}, 'ZTickMode', Ztick{a},... + 'XTickLabelMode', Xlabel{a}, 'YTickLabelMode', Ylabel{a}, 'ZTickLabelMode', Zlabel{a}); + catch + % ignore - fix issue #4 (using HG2 on R2014a and earlier) + end + end + % Revert the tex-labels font weights + try set(texLabels, 'FontWeight','bold'); catch, end + % Revert annotation units + for handleIdx = 1 : numel(annotationHandles) + try + oldUnits = originalUnits{handleIdx}; + catch + oldUnits = originalUnits; + end + try set(annotationHandles(handleIdx),'Units',oldUnits); catch, end + end + % Revert figure units + set(fig,'Units',oldFigUnits); + end + + % Output to clipboard (if requested) + if options.clipboard + % Delete the output file if unchanged from the default name ('export_fig_out.png') + if strcmpi(options.name,'export_fig_out') + try + fileInfo = dir('export_fig_out.png'); + if ~isempty(fileInfo) + timediff = now - fileInfo.datenum; + ONE_SEC = 1/24/60/60; + if timediff < ONE_SEC + delete('export_fig_out.png'); + end + end + catch + % never mind... + end + end + + % Save the image in the system clipboard + % credit: Jiro Doke's IMCLIPBOARD: http://www.mathworks.com/matlabcentral/fileexchange/28708-imclipboard + try + error(javachk('awt', 'export_fig -clipboard output')); + catch + warning('export_fig -clipboard output failed: requires Java to work'); + return; + end + try + % Import necessary Java classes + import java.awt.Toolkit + import java.awt.image.BufferedImage + import java.awt.datatransfer.DataFlavor + + % Get System Clipboard object (java.awt.Toolkit) + cb = Toolkit.getDefaultToolkit.getSystemClipboard(); + + % Add java class (ImageSelection) to the path + if ~exist('ImageSelection', 'class') + javaaddpath(fileparts(which(mfilename)), '-end'); + end + + % Get image size + ht = size(imageData, 1); + wd = size(imageData, 2); + + % Convert to Blue-Green-Red format + try + imageData2 = imageData(:, :, [3 2 1]); + catch + % Probably gray-scaled image (2D, without the 3rd [RGB] dimension) + imageData2 = imageData(:, :, [1 1 1]); + end + + % Convert to 3xWxH format + imageData2 = permute(imageData2, [3, 2, 1]); + + % Append Alpha data (unused - transparency is not supported in clipboard copy) + alphaData2 = uint8(permute(255*alpha,[3,2,1])); %=255*ones(1,wd,ht,'uint8') + imageData2 = cat(1, imageData2, alphaData2); + + % Create image buffer + imBuffer = BufferedImage(wd, ht, BufferedImage.TYPE_INT_RGB); + imBuffer.setRGB(0, 0, wd, ht, typecast(imageData2(:), 'int32'), 0, wd); + + % Create ImageSelection object from the image buffer + imSelection = ImageSelection(imBuffer); + + % Set clipboard content to the image + cb.setContents(imSelection, []); + catch + warning('export_fig -clipboard output failed: %s', lasterr); %#ok + end + end + + % Don't output the data to console unless requested + if ~nargout + clear imageData alpha + end + catch err + % Display possible workarounds before the error message + if displaySuggestedWorkarounds && ~strcmpi(err.message,'export_fig error') + if ~hadError, fprintf(2, 'export_fig error. '); end + fprintf(2, 'Please ensure:\n'); + fprintf(2, ' that you are using the latest version of export_fig\n'); + if ismac + fprintf(2, ' and that you have Ghostscript installed\n'); + else + fprintf(2, ' and that you have Ghostscript installed\n'); + end + try + if options.eps + fprintf(2, ' and that you have pdftops installed\n'); + end + catch + % ignore - probably an error in parse_args + end + fprintf(2, ' and that you do not have multiple versions of export_fig installed by mistake\n'); + fprintf(2, ' and that you did not made a mistake in the expected input arguments\n'); + try + % Alert per issue #149 + if ~strncmpi(get(0,'Units'),'pixel',5) + fprintf(2, ' or try to set groot''s Units property back to its default value of ''pixels'' (details)\n'); + end + catch + % ignore - maybe an old MAtlab release + end + fprintf(2, '\nIf the problem persists, then please report a new issue.\n\n'); + end + rethrow(err) + end +end + +function options = default_options() + % Default options used by export_fig + options = struct(... + 'name', 'export_fig_out', ... + 'crop', true, ... + 'crop_amounts', nan(1,4), ... % auto-crop all 4 image sides + 'transparent', false, ... + 'renderer', 0, ... % 0: default, 1: OpenGL, 2: ZBuffer, 3: Painters + 'pdf', false, ... + 'eps', false, ... + 'png', false, ... + 'tif', false, ... + 'jpg', false, ... + 'bmp', false, ... + 'clipboard', false, ... + 'colourspace', 0, ... % 0: RGB/gray, 1: CMYK, 2: gray + 'append', false, ... + 'im', false, ... + 'alpha', false, ... + 'aa_factor', 0, ... + 'bb_padding', 0, ... + 'magnify', [], ... + 'resolution', [], ... + 'bookmark', false, ... + 'closeFig', false, ... + 'quality', [], ... + 'update', false, ... + 'fontswap', true, ... + 'gs_options', {{}}); +end + +function [fig, options] = parse_args(nout, fig, varargin) + % Parse the input arguments + + % Set the defaults + native = false; % Set resolution to native of an image + options = default_options(); + options.im = (nout == 1); % user requested imageData output + options.alpha = (nout == 2); % user requested alpha output + + % Go through the other arguments + skipNext = false; + for a = 1:nargin-2 + if skipNext + skipNext = false; + continue; + end + if all(ishandle(varargin{a})) + fig = varargin{a}; + elseif ischar(varargin{a}) && ~isempty(varargin{a}) + if varargin{a}(1) == '-' + switch lower(varargin{a}(2:end)) + case 'nocrop' + options.crop = false; + options.crop_amounts = [0,0,0,0]; + case {'trans', 'transparent'} + options.transparent = true; + case 'opengl' + options.renderer = 1; + case 'zbuffer' + options.renderer = 2; + case 'painters' + options.renderer = 3; + case 'pdf' + options.pdf = true; + case 'eps' + options.eps = true; + case 'png' + options.png = true; + case {'tif', 'tiff'} + options.tif = true; + case {'jpg', 'jpeg'} + options.jpg = true; + case 'bmp' + options.bmp = true; + case 'rgb' + options.colourspace = 0; + case 'cmyk' + options.colourspace = 1; + case {'gray', 'grey'} + options.colourspace = 2; + case {'a1', 'a2', 'a3', 'a4'} + options.aa_factor = str2double(varargin{a}(3)); + case 'append' + options.append = true; + case 'bookmark' + options.bookmark = true; + case 'native' + native = true; + case 'clipboard' + options.clipboard = true; + options.im = true; + options.alpha = true; + case 'svg' + msg = ['SVG output is not supported by export_fig. Use one of the following alternatives:\n' ... + ' 1. saveas(gcf,''filename.svg'')\n' ... + ' 2. plot2svg utility: http://github.com/jschwizer99/plot2svg\n' ... + ' 3. export_fig to EPS/PDF, then convert to SVG using generic (non-Matlab) tools\n']; + error(sprintf(msg)); %#ok + case 'update' + % Download the latest version of export_fig into the export_fig folder + try + zipFileName = 'https://github.com/altmany/export_fig/archive/master.zip'; + folderName = fileparts(which(mfilename('fullpath'))); + targetFileName = fullfile(folderName, datestr(now,'yyyy-mm-dd.zip')); + urlwrite(zipFileName,targetFileName); + catch + error('Could not download %s into %s\n',zipFileName,targetFileName); + end + + % Unzip the downloaded zip file in the export_fig folder + try + unzip(targetFileName,folderName); + catch + error('Could not unzip %s\n',targetFileName); + end + case 'nofontswap' + options.fontswap = false; + otherwise + try + wasError = false; + if strcmpi(varargin{a}(1:2),'-d') + varargin{a}(2) = 'd'; % ensure lowercase 'd' + options.gs_options{end+1} = varargin{a}; + elseif strcmpi(varargin{a}(1:2),'-c') + if numel(varargin{a})==2 + skipNext = true; + vals = str2num(varargin{a+1}); %#ok + else + vals = str2num(varargin{a}(3:end)); %#ok + end + if numel(vals)~=4 + wasError = true; + error('option -c cannot be parsed: must be a 4-element numeric vector'); + end + options.crop_amounts = vals; + options.crop = true; + else % scalar parameter value + val = str2double(regexp(varargin{a}, '(?<=-(m|M|r|R|q|Q|p|P))-?\d*.?\d+', 'match')); + if isempty(val) || isnan(val) + % Issue #51: improved processing of input args (accept space between param name & value) + val = str2double(varargin{a+1}); + if isscalar(val) && ~isnan(val) + skipNext = true; + end + end + if ~isscalar(val) || isnan(val) + wasError = true; + error('option %s is not recognised or cannot be parsed', varargin{a}); + end + switch lower(varargin{a}(2)) + case 'm' + % Magnification may never be negative + if val <= 0 + wasError = true; + error('Bad magnification value: %g (must be positive)', val); + end + options.magnify = val; + case 'r' + options.resolution = val; + case 'q' + options.quality = max(val, 0); + case 'p' + options.bb_padding = val; + end + end + catch err + % We might have reached here by raising an intentional error + if wasError % intentional raise + rethrow(err) + else % unintentional + error(['Unrecognized export_fig input option: ''' varargin{a} '''']); + end + end + end + else + [p, options.name, ext] = fileparts(varargin{a}); + if ~isempty(p) + options.name = [p filesep options.name]; + end + switch lower(ext) + case {'.tif', '.tiff'} + options.tif = true; + case {'.jpg', '.jpeg'} + options.jpg = true; + case '.png' + options.png = true; + case '.bmp' + options.bmp = true; + case '.eps' + options.eps = true; + case '.pdf' + options.pdf = true; + case '.fig' + % If no open figure, then load the specified .fig file and continue + if isempty(fig) + fig = openfig(varargin{a},'invisible'); + varargin{a} = fig; + options.closeFig = true; + else + % save the current figure as the specified .fig file and exit + saveas(fig(1),varargin{a}); + fig = -1; + return + end + case '.svg' + msg = ['SVG output is not supported by export_fig. Use one of the following alternatives:\n' ... + ' 1. saveas(gcf,''filename.svg'')\n' ... + ' 2. plot2svg utility: http://github.com/jschwizer99/plot2svg\n' ... + ' 3. export_fig to EPS/PDF, then convert to SVG using generic (non-Matlab) tools\n']; + error(sprintf(msg)); %#ok + otherwise + options.name = varargin{a}; + end + end + end + end + + % Quick bail-out if no figure found + if isempty(fig), return; end + + % Do border padding with repsect to a cropped image + if options.bb_padding + options.crop = true; + end + + % Set default anti-aliasing now we know the renderer + if options.aa_factor == 0 + try isAA = strcmp(get(ancestor(fig, 'figure'), 'GraphicsSmoothing'), 'on'); catch, isAA = false; end + options.aa_factor = 1 + 2 * (~(using_hg2(fig) && isAA) | (options.renderer == 3)); + end + + % Convert user dir '~' to full path + if numel(options.name) > 2 && options.name(1) == '~' && (options.name(2) == '/' || options.name(2) == '\') + options.name = fullfile(char(java.lang.System.getProperty('user.home')), options.name(2:end)); + end + + % Compute the magnification and resolution + if isempty(options.magnify) + if isempty(options.resolution) + options.magnify = 1; + options.resolution = 864; + else + options.magnify = options.resolution ./ get(0, 'ScreenPixelsPerInch'); + end + elseif isempty(options.resolution) + options.resolution = 864; + end + + % Set the default format + if ~isvector(options) && ~isbitmap(options) + options.png = true; + end + + % Check whether transparent background is wanted (old way) + if isequal(get(ancestor(fig(1), 'figure'), 'Color'), 'none') + options.transparent = true; + end + + % If requested, set the resolution to the native vertical resolution of the + % first suitable image found + if native && isbitmap(options) + % Find a suitable image + list = findall(fig, 'Type','image', 'Tag','export_fig_native'); + if isempty(list) + list = findall(fig, 'Type','image', 'Visible','on'); + end + for hIm = list(:)' + % Check height is >= 2 + height = size(get(hIm, 'CData'), 1); + if height < 2 + continue + end + % Account for the image filling only part of the axes, or vice versa + yl = get(hIm, 'YData'); + if isscalar(yl) + yl = [yl(1)-0.5 yl(1)+height+0.5]; + else + yl = [min(yl), max(yl)]; % fix issue #151 (case of yl containing more than 2 elements) + if ~diff(yl) + continue + end + yl = yl + [-0.5 0.5] * (diff(yl) / (height - 1)); + end + hAx = get(hIm, 'Parent'); + yl2 = get(hAx, 'YLim'); + % Find the pixel height of the axes + oldUnits = get(hAx, 'Units'); + set(hAx, 'Units', 'pixels'); + pos = get(hAx, 'Position'); + set(hAx, 'Units', oldUnits); + if ~pos(4) + continue + end + % Found a suitable image + % Account for stretch-to-fill being disabled + pbar = get(hAx, 'PlotBoxAspectRatio'); + pos = min(pos(4), pbar(2)*pos(3)/pbar(1)); + % Set the magnification to give native resolution + options.magnify = abs((height * diff(yl2)) / (pos * diff(yl))); % magnification must never be negative: issue #103 + break + end + end +end + +function A = downsize(A, factor) + % Downsample an image + if factor == 1 + % Nothing to do + return + end + try + % Faster, but requires image processing toolbox + A = imresize(A, 1/factor, 'bilinear'); + catch + % No image processing toolbox - resize manually + % Lowpass filter - use Gaussian as is separable, so faster + % Compute the 1d Gaussian filter + filt = (-factor-1:factor+1) / (factor * 0.6); + filt = exp(-filt .* filt); + % Normalize the filter + filt = single(filt / sum(filt)); + % Filter the image + padding = floor(numel(filt) / 2); + for a = 1:size(A, 3) + A(:,:,a) = conv2(filt, filt', single(A([ones(1, padding) 1:end repmat(end, 1, padding)],[ones(1, padding) 1:end repmat(end, 1, padding)],a)), 'valid'); + end + % Subsample + A = A(1+floor(mod(end-1, factor)/2):factor:end,1+floor(mod(end-1, factor)/2):factor:end,:); + end +end + +function A = rgb2grey(A) + A = cast(reshape(reshape(single(A), [], 3) * single([0.299; 0.587; 0.114]), size(A, 1), size(A, 2)), class(A)); %#ok +end + +function A = check_greyscale(A) + % Check if the image is greyscale + if size(A, 3) == 3 && ... + all(reshape(A(:,:,1) == A(:,:,2), [], 1)) && ... + all(reshape(A(:,:,2) == A(:,:,3), [], 1)) + A = A(:,:,1); % Save only one channel for 8-bit output + end +end + +function eps_remove_background(fname, count) + % Remove the background of an eps file + % Open the file + fh = fopen(fname, 'r+'); + if fh == -1 + error('Not able to open file %s.', fname); + end + % Read the file line by line + while count + % Get the next line + l = fgets(fh); + if isequal(l, -1) + break; % Quit, no rectangle found + end + % Check if the line contains the background rectangle + if isequal(regexp(l, ' *0 +0 +\d+ +\d+ +r[fe] *[\n\r]+', 'start'), 1) + % Set the line to whitespace and quit + l(1:regexp(l, '[\n\r]', 'start', 'once')-1) = ' '; + fseek(fh, -numel(l), 0); + fprintf(fh, l); + % Reduce the count + count = count - 1; + end + end + % Close the file + fclose(fh); +end + +function b = isvector(options) + b = options.pdf || options.eps; +end + +function b = isbitmap(options) + b = options.png || options.tif || options.jpg || options.bmp || options.im || options.alpha; +end + +% Helper function +function A = make_cell(A) + if ~iscell(A) + A = {A}; + end +end + +function add_bookmark(fname, bookmark_text) + % Adds a bookmark to the temporary EPS file after %%EndPageSetup + % Read in the file + fh = fopen(fname, 'r'); + if fh == -1 + error('File %s not found.', fname); + end + try + fstrm = fread(fh, '*char')'; + catch ex + fclose(fh); + rethrow(ex); + end + fclose(fh); + + % Include standard pdfmark prolog to maximize compatibility + fstrm = strrep(fstrm, '%%BeginProlog', sprintf('%%%%BeginProlog\n/pdfmark where {pop} {userdict /pdfmark /cleartomark load put} ifelse')); + % Add page bookmark + fstrm = strrep(fstrm, '%%EndPageSetup', sprintf('%%%%EndPageSetup\n[ /Title (%s) /OUT pdfmark',bookmark_text)); + + % Write out the updated file + fh = fopen(fname, 'w'); + if fh == -1 + error('Unable to open %s for writing.', fname); + end + try + fwrite(fh, fstrm, 'char*1'); + catch ex + fclose(fh); + rethrow(ex); + end + fclose(fh); +end + +function set_tick_mode(Hlims, ax) + % Set the tick mode of linear axes to manual + % Leave log axes alone as these are tricky + M = get(Hlims, [ax 'Scale']); + if ~iscell(M) + M = {M}; + end + M = cellfun(@(c) strcmp(c, 'linear'), M); + set(Hlims(M), [ax 'TickMode'], 'manual'); + %set(Hlims(M), [ax 'TickLabelMode'], 'manual'); % this hides exponent label in HG2! +end + +function change_rgb_to_cmyk(fname) % convert RGB => CMYK within an EPS file + % Do post-processing on the eps file + try + % Read the EPS file into memory + fstrm = read_write_entire_textfile(fname); + + % Replace all gray-scale colors + fstrm = regexprep(fstrm, '\n([\d.]+) +GC\n', '\n0 0 0 ${num2str(1-str2num($1))} CC\n'); + + % Replace all RGB colors + fstrm = regexprep(fstrm, '\n[0.]+ +[0.]+ +[0.]+ +RC\n', '\n0 0 0 1 CC\n'); % pure black + fstrm = regexprep(fstrm, '\n([\d.]+) +([\d.]+) +([\d.]+) +RC\n', '\n${sprintf(''%.4g '',[1-[str2num($1),str2num($2),str2num($3)]/max([str2num($1),str2num($2),str2num($3)]),1-max([str2num($1),str2num($2),str2num($3)])])} CC\n'); + + % Overwrite the file with the modified contents + read_write_entire_textfile(fname, fstrm); + catch + % never mind - leave as is... + end +end diff --git a/offline/localizer/altmany-export_fig/fix_lines.m b/offline/localizer/altmany-export_fig/fix_lines.m new file mode 100755 index 00000000..b160fd8d --- /dev/null +++ b/offline/localizer/altmany-export_fig/fix_lines.m @@ -0,0 +1,151 @@ +%FIX_LINES Improves the line style of eps files generated by print +% +% Examples: +% fix_lines fname +% fix_lines fname fname2 +% fstrm_out = fixlines(fstrm_in) +% +% This function improves the style of lines in eps files generated by +% MATLAB's print function, making them more similar to those seen on +% screen. Grid lines are also changed from a dashed style to a dotted +% style, for greater differentiation from dashed lines. +% +% The function also places embedded fonts after the postscript header, in +% versions of MATLAB which place the fonts first (R2006b and earlier), in +% order to allow programs such as Ghostscript to find the bounding box +% information. +% +%IN: +% fname - Name or path of source eps file. +% fname2 - Name or path of destination eps file. Default: same as fname. +% fstrm_in - File contents of a MATLAB-generated eps file. +% +%OUT: +% fstrm_out - Contents of the eps file with line styles fixed. + +% Copyright: (C) Oliver Woodford, 2008-2014 + +% The idea of editing the EPS file to change line styles comes from Jiro +% Doke's FIXPSLINESTYLE (fex id: 17928) +% The idea of changing dash length with line width came from comments on +% fex id: 5743, but the implementation is mine :) + +% Thank you to Sylvain Favrot for bringing the embedded font/bounding box +% interaction in older versions of MATLAB to my attention. +% Thank you to D Ko for bringing an error with eps files with tiff previews +% to my attention. +% Thank you to Laurence K for suggesting the check to see if the file was +% opened. + +% 01/03/15: Issue #20: warn users if using this function in HG2 (R2014b+) +% 27/03/15: Fixed out of memory issue with enormous EPS files (generated by print() with OpenGL renderer), related to issue #39 + +function fstrm = fix_lines(fstrm, fname2) + +% Issue #20: warn users if using this function in HG2 (R2014b+) +if using_hg2 + warning('export_fig:hg2','The fix_lines function should not be used in this Matlab version.'); +end + +if nargout == 0 || nargin > 1 + if nargin < 2 + % Overwrite the input file + fname2 = fstrm; + end + % Read in the file + fstrm = read_write_entire_textfile(fstrm); +end + +% Move any embedded fonts after the postscript header +if strcmp(fstrm(1:15), '%!PS-AdobeFont-') + % Find the start and end of the header + ind = regexp(fstrm, '[\n\r]%!PS-Adobe-'); + [ind2, ind2] = regexp(fstrm, '[\n\r]%%EndComments[\n\r]+'); + % Put the header first + if ~isempty(ind) && ~isempty(ind2) && ind(1) < ind2(1) + fstrm = fstrm([ind(1)+1:ind2(1) 1:ind(1) ind2(1)+1:end]); + end +end + +% Make sure all line width commands come before the line style definitions, +% so that dash lengths can be based on the correct widths +% Find all line style sections +ind = [regexp(fstrm, '[\n\r]SO[\n\r]'),... % This needs to be here even though it doesn't have dots/dashes! + regexp(fstrm, '[\n\r]DO[\n\r]'),... + regexp(fstrm, '[\n\r]DA[\n\r]'),... + regexp(fstrm, '[\n\r]DD[\n\r]')]; +ind = sort(ind); +% Find line width commands +[ind2, ind3] = regexp(fstrm, '[\n\r]\d* w[\n\r]'); +% Go through each line style section and swap with any line width commands +% near by +b = 1; +m = numel(ind); +n = numel(ind2); +for a = 1:m + % Go forwards width commands until we pass the current line style + while b <= n && ind2(b) < ind(a) + b = b + 1; + end + if b > n + % No more width commands + break; + end + % Check we haven't gone past another line style (including SO!) + if a < m && ind2(b) > ind(a+1) + continue; + end + % Are the commands close enough to be confident we can swap them? + if (ind2(b) - ind(a)) > 8 + continue; + end + % Move the line style command below the line width command + fstrm(ind(a)+1:ind3(b)) = [fstrm(ind(a)+4:ind3(b)) fstrm(ind(a)+1:ind(a)+3)]; + b = b + 1; +end + +% Find any grid line definitions and change to GR format +% Find the DO sections again as they may have moved +ind = int32(regexp(fstrm, '[\n\r]DO[\n\r]')); +if ~isempty(ind) + % Find all occurrences of what are believed to be axes and grid lines + ind2 = int32(regexp(fstrm, '[\n\r] *\d* *\d* *mt *\d* *\d* *L[\n\r]')); + if ~isempty(ind2) + % Now see which DO sections come just before axes and grid lines + ind2 = repmat(ind2', [1 numel(ind)]) - repmat(ind, [numel(ind2) 1]); + ind2 = any(ind2 > 0 & ind2 < 12); % 12 chars seems about right + ind = ind(ind2); + % Change any regions we believe to be grid lines to GR + fstrm(ind+1) = 'G'; + fstrm(ind+2) = 'R'; + end +end + +% Define the new styles, including the new GR format +% Dot and dash lengths have two parts: a constant amount plus a line width +% variable amount. The constant amount comes after dpi2point, and the +% variable amount comes after currentlinewidth. If you want to change +% dot/dash lengths for a one particular line style only, edit the numbers +% in the /DO (dotted lines), /DA (dashed lines), /DD (dot dash lines) and +% /GR (grid lines) lines for the style you want to change. +new_style = {'/dom { dpi2point 1 currentlinewidth 0.08 mul add mul mul } bdef',... % Dot length macro based on line width + '/dam { dpi2point 2 currentlinewidth 0.04 mul add mul mul } bdef',... % Dash length macro based on line width + '/SO { [] 0 setdash 0 setlinecap } bdef',... % Solid lines + '/DO { [1 dom 1.2 dom] 0 setdash 0 setlinecap } bdef',... % Dotted lines + '/DA { [4 dam 1.5 dam] 0 setdash 0 setlinecap } bdef',... % Dashed lines + '/DD { [1 dom 1.2 dom 4 dam 1.2 dom] 0 setdash 0 setlinecap } bdef',... % Dot dash lines + '/GR { [0 dpi2point mul 4 dpi2point mul] 0 setdash 1 setlinecap } bdef'}; % Grid lines - dot spacing remains constant + +% Construct the output +% This is the original (memory-intensive) code: +%first_sec = strfind(fstrm, '% line types:'); % Isolate line style definition section +%[second_sec, remaining] = strtok(fstrm(first_sec+1:end), '/'); +%[remaining, remaining] = strtok(remaining, '%'); +%fstrm = [fstrm(1:first_sec) second_sec sprintf('%s\r', new_style{:}) remaining]; +fstrm = regexprep(fstrm,'(% line types:.+?)/.+?%',['$1',sprintf('%s\r',new_style{:}),'%']); + +% Write the output file +if nargout == 0 || nargin > 1 + read_write_entire_textfile(fname2, fstrm); +end +end diff --git a/offline/localizer/altmany-export_fig/ghostscript.m b/offline/localizer/altmany-export_fig/ghostscript.m new file mode 100755 index 00000000..17608774 --- /dev/null +++ b/offline/localizer/altmany-export_fig/ghostscript.m @@ -0,0 +1,196 @@ +function varargout = ghostscript(cmd) +%GHOSTSCRIPT Calls a local GhostScript executable with the input command +% +% Example: +% [status result] = ghostscript(cmd) +% +% Attempts to locate a ghostscript executable, finally asking the user to +% specify the directory ghostcript was installed into. The resulting path +% is stored for future reference. +% +% Once found, the executable is called with the input command string. +% +% This function requires that you have Ghostscript installed on your +% system. You can download this from: http://www.ghostscript.com +% +% IN: +% cmd - Command string to be passed into ghostscript. +% +% OUT: +% status - 0 iff command ran without problem. +% result - Output from ghostscript. + +% Copyright: Oliver Woodford, 2009-2015, Yair Altman 2015- +%{ +% Thanks to Jonas Dorn for the fix for the title of the uigetdir window on Mac OS. +% Thanks to Nathan Childress for the fix to default location on 64-bit Windows systems. +% 27/04/11 - Find 64-bit Ghostscript on Windows. Thanks to Paul Durack and +% Shaun Kline for pointing out the issue +% 04/05/11 - Thanks to David Chorlian for pointing out an alternative +% location for gs on linux. +% 12/12/12 - Add extra executable name on Windows. Thanks to Ratish +% Punnoose for highlighting the issue. +% 28/06/13 - Fix error using GS 9.07 in Linux. Many thanks to Jannick +% Steinbring for proposing the fix. +% 24/10/13 - Fix error using GS 9.07 in Linux. Many thanks to Johannes +% for the fix. +% 23/01/14 - Add full path to ghostscript.txt in warning. Thanks to Koen +% Vermeer for raising the issue. +% 27/02/15 - If Ghostscript croaks, display suggested workarounds +% 30/03/15 - Improved performance by caching status of GS path check, if ok +% 14/05/15 - Clarified warning message in case GS path could not be saved +% 29/05/15 - Avoid cryptic error in case the ghostscipt path cannot be saved (issue #74) +% 10/11/15 - Custom GS installation webpage for MacOS. Thanks to Andy Hueni via FEX +%} + + try + % Call ghostscript + [varargout{1:nargout}] = system([gs_command(gs_path()) cmd]); + catch err + % Display possible workarounds for Ghostscript croaks + url1 = 'https://github.com/altmany/export_fig/issues/12#issuecomment-61467998'; % issue #12 + url2 = 'https://github.com/altmany/export_fig/issues/20#issuecomment-63826270'; % issue #20 + hg2_str = ''; if using_hg2, hg2_str = ' or Matlab R2014a'; end + fprintf(2, 'Ghostscript error. Rolling back to GS 9.10%s may possibly solve this:\n * %s ',hg2_str,url1,url1); + if using_hg2 + fprintf(2, '(GS 9.10)\n * %s (R2014a)',url2,url2); + end + fprintf('\n\n'); + if ismac || isunix + url3 = 'https://github.com/altmany/export_fig/issues/27'; % issue #27 + fprintf(2, 'Alternatively, this may possibly be due to a font path issue:\n * %s\n\n',url3,url3); + % issue #20 + fpath = which(mfilename); + if isempty(fpath), fpath = [mfilename('fullpath') '.m']; end + fprintf(2, 'Alternatively, if you are using csh, modify shell_cmd from "export..." to "setenv ..."\nat the bottom of %s\n\n',fpath,fpath); + end + rethrow(err); + end +end + +function path_ = gs_path + % Return a valid path + % Start with the currently set path + path_ = user_string('ghostscript'); + % Check the path works + if check_gs_path(path_) + return + end + % Check whether the binary is on the path + if ispc + bin = {'gswin32c.exe', 'gswin64c.exe', 'gs'}; + else + bin = {'gs'}; + end + for a = 1:numel(bin) + path_ = bin{a}; + if check_store_gs_path(path_) + return + end + end + % Search the obvious places + if ispc + default_location = 'C:\Program Files\gs\'; + dir_list = dir(default_location); + if isempty(dir_list) + default_location = 'C:\Program Files (x86)\gs\'; % Possible location on 64-bit systems + dir_list = dir(default_location); + end + executable = {'\bin\gswin32c.exe', '\bin\gswin64c.exe'}; + ver_num = 0; + % If there are multiple versions, use the newest + for a = 1:numel(dir_list) + ver_num2 = sscanf(dir_list(a).name, 'gs%g'); + if ~isempty(ver_num2) && ver_num2 > ver_num + for b = 1:numel(executable) + path2 = [default_location dir_list(a).name executable{b}]; + if exist(path2, 'file') == 2 + path_ = path2; + ver_num = ver_num2; + end + end + end + end + if check_store_gs_path(path_) + return + end + else + executable = {'/usr/bin/gs', '/usr/local/bin/gs'}; + for a = 1:numel(executable) + path_ = executable{a}; + if check_store_gs_path(path_) + return + end + end + end + % Ask the user to enter the path + while true + if strncmp(computer, 'MAC', 3) % Is a Mac + % Give separate warning as the uigetdir dialogue box doesn't have a + % title + uiwait(warndlg('Ghostscript not found. Please locate the program.')) + end + base = uigetdir('/', 'Ghostcript not found. Please locate the program.'); + if isequal(base, 0) + % User hit cancel or closed window + break; + end + base = [base filesep]; %#ok + bin_dir = {'', ['bin' filesep], ['lib' filesep]}; + for a = 1:numel(bin_dir) + for b = 1:numel(bin) + path_ = [base bin_dir{a} bin{b}]; + if exist(path_, 'file') == 2 + if check_store_gs_path(path_) + return + end + end + end + end + end + if ismac + error('Ghostscript not found. Have you installed it (http://pages.uoregon.edu/koch)?'); + else + error('Ghostscript not found. Have you installed it from www.ghostscript.com?'); + end +end + +function good = check_store_gs_path(path_) + % Check the path is valid + good = check_gs_path(path_); + if ~good + return + end + % Update the current default path to the path found + if ~user_string('ghostscript', path_) + filename = fullfile(fileparts(which('user_string.m')), '.ignore', 'ghostscript.txt'); + warning('Path to ghostscript installation could not be saved in %s (perhaps a permissions issue). You can manually create this file and set its contents to %s, to improve performance in future invocations (this warning is safe to ignore).', filename, path_); + return + end +end + +function good = check_gs_path(path_) + persistent isOk + if isempty(path_) + isOk = false; + elseif ~isequal(isOk,true) + % Check whether the path is valid + [status, message] = system([gs_command(path_) '-h']); %#ok + isOk = status == 0; + end + good = isOk; +end + +function cmd = gs_command(path_) + % Initialize any required system calls before calling ghostscript + % TODO: in Unix/Mac, find a way to determine whether to use "export" (bash) or "setenv" (csh/tcsh) + shell_cmd = ''; + if isunix + shell_cmd = 'export LD_LIBRARY_PATH=""; '; % Avoids an error on Linux with GS 9.07 + end + if ismac + shell_cmd = 'export DYLD_LIBRARY_PATH=""; '; % Avoids an error on Mac with GS 9.07 + end + % Construct the command string + cmd = sprintf('%s"%s" ', shell_cmd, path_); +end diff --git a/offline/localizer/altmany-export_fig/im2gif.m b/offline/localizer/altmany-export_fig/im2gif.m new file mode 100755 index 00000000..a6c5975d --- /dev/null +++ b/offline/localizer/altmany-export_fig/im2gif.m @@ -0,0 +1,186 @@ +%IM2GIF Convert a multiframe image to an animated GIF file +% +% Examples: +% im2gif infile +% im2gif infile outfile +% im2gif(A, outfile) +% im2gif(..., '-nocrop') +% im2gif(..., '-nodither') +% im2gif(..., '-ncolors', n) +% im2gif(..., '-loops', n) +% im2gif(..., '-delay', n) +% +% This function converts a multiframe image to an animated GIF. +% +% To create an animation from a series of figures, export to a multiframe +% TIFF file using export_fig, then convert to a GIF, as follows: +% +% for a = 2 .^ (3:6) +% peaks(a); +% export_fig test.tif -nocrop -append +% end +% im2gif('test.tif', '-delay', 0.5); +% +%IN: +% infile - string containing the name of the input image. +% outfile - string containing the name of the output image (must have the +% .gif extension). Default: infile, with .gif extension. +% A - HxWxCxN array of input images, stacked along fourth dimension, to +% be converted to gif. +% -nocrop - option indicating that the borders of the output are not to +% be cropped. +% -nodither - option indicating that dithering is not to be used when +% converting the image. +% -ncolors - option pair, the value of which indicates the maximum number +% of colors the GIF can have. This can also be a quantization +% tolerance, between 0 and 1. Default/maximum: 256. +% -loops - option pair, the value of which gives the number of times the +% animation is to be looped. Default: 65535. +% -delay - option pair, the value of which gives the time, in seconds, +% between frames. Default: 1/15. + +% Copyright (C) Oliver Woodford 2011 + +function im2gif(A, varargin) + +% Parse the input arguments +[A, options] = parse_args(A, varargin{:}); + +if options.crop ~= 0 + % Crop + A = crop_borders(A, A(ceil(end/2),1,:,1)); +end + +% Convert to indexed image +[h, w, c, n] = size(A); +A = reshape(permute(A, [1 2 4 3]), h, w*n, c); +map = unique(reshape(A, h*w*n, c), 'rows'); +if size(map, 1) > 256 + dither_str = {'dither', 'nodither'}; + dither_str = dither_str{1+(options.dither==0)}; + if options.ncolors <= 1 + [B, map] = rgb2ind(A, options.ncolors, dither_str); + if size(map, 1) > 256 + [B, map] = rgb2ind(A, 256, dither_str); + end + else + [B, map] = rgb2ind(A, min(round(options.ncolors), 256), dither_str); + end +else + if max(map(:)) > 1 + map = double(map) / 255; + A = double(A) / 255; + end + B = rgb2ind(im2double(A), map); +end +B = reshape(B, h, w, 1, n); + +% Bug fix to rgb2ind +map(B(1)+1,:) = im2double(A(1,1,:)); + +% Save as a gif +imwrite(B, map, options.outfile, 'LoopCount', round(options.loops(1)), 'DelayTime', options.delay); +end + +%% Parse the input arguments +function [A, options] = parse_args(A, varargin) +% Set the defaults +options = struct('outfile', '', ... + 'dither', true, ... + 'crop', true, ... + 'ncolors', 256, ... + 'loops', 65535, ... + 'delay', 1/15); + +% Go through the arguments +a = 0; +n = numel(varargin); +while a < n + a = a + 1; + if ischar(varargin{a}) && ~isempty(varargin{a}) + if varargin{a}(1) == '-' + opt = lower(varargin{a}(2:end)); + switch opt + case 'nocrop' + options.crop = false; + case 'nodither' + options.dither = false; + otherwise + if ~isfield(options, opt) + error('Option %s not recognized', varargin{a}); + end + a = a + 1; + if ischar(varargin{a}) && ~ischar(options.(opt)) + options.(opt) = str2double(varargin{a}); + else + options.(opt) = varargin{a}; + end + end + else + options.outfile = varargin{a}; + end + end +end + +if isempty(options.outfile) + if ~ischar(A) + error('No output filename given.'); + end + % Generate the output filename from the input filename + [path, outfile] = fileparts(A); + options.outfile = fullfile(path, [outfile '.gif']); +end + +if ischar(A) + % Read in the image + A = imread_rgb(A); +end +end + +%% Read image to uint8 rgb array +function [A, alpha] = imread_rgb(name) +% Get file info +info = imfinfo(name); +% Special case formats +switch lower(info(1).Format) + case 'gif' + [A, map] = imread(name, 'frames', 'all'); + if ~isempty(map) + map = uint8(map * 256 - 0.5); % Convert to uint8 for storage + A = reshape(map(uint32(A)+1,:), [size(A) size(map, 2)]); % Assume indexed from 0 + A = permute(A, [1 2 5 4 3]); + end + case {'tif', 'tiff'} + A = cell(numel(info), 1); + for a = 1:numel(A) + [A{a}, map] = imread(name, 'Index', a, 'Info', info); + if ~isempty(map) + map = uint8(map * 256 - 0.5); % Convert to uint8 for storage + A{a} = reshape(map(uint32(A{a})+1,:), [size(A) size(map, 2)]); % Assume indexed from 0 + end + if size(A{a}, 3) == 4 + % TIFF in CMYK colourspace - convert to RGB + if isfloat(A{a}) + A{a} = A{a} * 255; + else + A{a} = single(A{a}); + end + A{a} = 255 - A{a}; + A{a}(:,:,4) = A{a}(:,:,4) / 255; + A{a} = uint8(A(:,:,1:3) .* A{a}(:,:,[4 4 4])); + end + end + A = cat(4, A{:}); + otherwise + [A, map, alpha] = imread(name); + A = A(:,:,:,1); % Keep only first frame of multi-frame files + if ~isempty(map) + map = uint8(map * 256 - 0.5); % Convert to uint8 for storage + A = reshape(map(uint32(A)+1,:), [size(A) size(map, 2)]); % Assume indexed from 0 + elseif size(A, 3) == 4 + % Assume 4th channel is an alpha matte + alpha = A(:,:,4); + A = A(:,:,1:3); + end +end +end diff --git a/offline/localizer/altmany-export_fig/isolate_axes.m b/offline/localizer/altmany-export_fig/isolate_axes.m new file mode 100755 index 00000000..4e71e69e --- /dev/null +++ b/offline/localizer/altmany-export_fig/isolate_axes.m @@ -0,0 +1,130 @@ +function fh = isolate_axes(ah, vis) +%ISOLATE_AXES Isolate the specified axes in a figure on their own +% +% Examples: +% fh = isolate_axes(ah) +% fh = isolate_axes(ah, vis) +% +% This function will create a new figure containing the axes/uipanels +% specified, and also their associated legends and colorbars. The objects +% specified must all be in the same figure, but they will generally only be +% a subset of the objects in the figure. +% +% IN: +% ah - An array of axes and uipanel handles, which must come from the +% same figure. +% vis - A boolean indicating whether the new figure should be visible. +% Default: false. +% +% OUT: +% fh - The handle of the created figure. + +% Copyright (C) Oliver Woodford 2011-2013 + +% Thank you to Rosella Blatt for reporting a bug to do with axes in GUIs +% 16/03/12: Moved copyfig to its own function. Thanks to Bob Fratantonio +% for pointing out that the function is also used in export_fig.m +% 12/12/12: Add support for isolating uipanels. Thanks to michael for suggesting it +% 08/10/13: Bug fix to allchildren suggested by Will Grant (many thanks!) +% 05/12/13: Bug fix to axes having different units. Thanks to Remington Reid for reporting +% 21/04/15: Bug fix for exporting uipanels with legend/colorbar on HG1 (reported by Alvaro +% on FEX page as a comment on 24-Apr-2014); standardized indentation & help section +% 22/04/15: Bug fix: legends and colorbars were not exported when exporting axes handle in HG2 + + % Make sure we have an array of handles + if ~all(ishandle(ah)) + error('ah must be an array of handles'); + end + % Check that the handles are all for axes or uipanels, and are all in the same figure + fh = ancestor(ah(1), 'figure'); + nAx = numel(ah); + for a = 1:nAx + if ~ismember(get(ah(a), 'Type'), {'axes', 'uipanel'}) + error('All handles must be axes or uipanel handles.'); + end + if ~isequal(ancestor(ah(a), 'figure'), fh) + error('Axes must all come from the same figure.'); + end + end + % Tag the objects so we can find them in the copy + old_tag = get(ah, 'Tag'); + if nAx == 1 + old_tag = {old_tag}; + end + set(ah, 'Tag', 'ObjectToCopy'); + % Create a new figure exactly the same as the old one + fh = copyfig(fh); %copyobj(fh, 0); + if nargin < 2 || ~vis + set(fh, 'Visible', 'off'); + end + % Reset the object tags + for a = 1:nAx + set(ah(a), 'Tag', old_tag{a}); + end + % Find the objects to save + ah = findall(fh, 'Tag', 'ObjectToCopy'); + if numel(ah) ~= nAx + close(fh); + error('Incorrect number of objects found.'); + end + % Set the axes tags to what they should be + for a = 1:nAx + set(ah(a), 'Tag', old_tag{a}); + end + % Keep any legends and colorbars which overlap the subplots + % Note: in HG1 these are axes objects; in HG2 they are separate objects, therefore we + % don't test for the type, only the tag (hopefully nobody but Matlab uses them!) + lh = findall(fh, 'Tag', 'legend', '-or', 'Tag', 'Colorbar'); + nLeg = numel(lh); + if nLeg > 0 + set([ah(:); lh(:)], 'Units', 'normalized'); + try + ax_pos = get(ah, 'OuterPosition'); % axes and figures have the OuterPosition property + catch + ax_pos = get(ah, 'Position'); % uipanels only have Position, not OuterPosition + end + if nAx > 1 + ax_pos = cell2mat(ax_pos(:)); + end + ax_pos(:,3:4) = ax_pos(:,3:4) + ax_pos(:,1:2); + try + leg_pos = get(lh, 'OuterPosition'); + catch + leg_pos = get(lh, 'Position'); % No OuterPosition in HG2, only in HG1 + end + if nLeg > 1; + leg_pos = cell2mat(leg_pos); + end + leg_pos(:,3:4) = leg_pos(:,3:4) + leg_pos(:,1:2); + ax_pos = shiftdim(ax_pos, -1); + % Overlap test + M = bsxfun(@lt, leg_pos(:,1), ax_pos(:,:,3)) & ... + bsxfun(@lt, leg_pos(:,2), ax_pos(:,:,4)) & ... + bsxfun(@gt, leg_pos(:,3), ax_pos(:,:,1)) & ... + bsxfun(@gt, leg_pos(:,4), ax_pos(:,:,2)); + ah = [ah; lh(any(M, 2))]; + end + % Get all the objects in the figure + axs = findall(fh); + % Delete everything except for the input objects and associated items + delete(axs(~ismember(axs, [ah; allchildren(ah); allancestors(ah)]))); +end + +function ah = allchildren(ah) + ah = findall(ah); + if iscell(ah) + ah = cell2mat(ah); + end + ah = ah(:); +end + +function ph = allancestors(ah) + ph = []; + for a = 1:numel(ah) + h = get(ah(a), 'parent'); + while h ~= 0 + ph = [ph; h]; + h = get(h, 'parent'); + end + end +end diff --git a/offline/localizer/altmany-export_fig/pdf2eps.m b/offline/localizer/altmany-export_fig/pdf2eps.m new file mode 100755 index 00000000..c0766124 --- /dev/null +++ b/offline/localizer/altmany-export_fig/pdf2eps.m @@ -0,0 +1,51 @@ +%PDF2EPS Convert a pdf file to eps format using pdftops +% +% Examples: +% pdf2eps source dest +% +% This function converts a pdf file to eps format. +% +% This function requires that you have pdftops, from the Xpdf suite of +% functions, installed on your system. This can be downloaded from: +% http://www.foolabs.com/xpdf +% +%IN: +% source - filename of the source pdf file to convert. The filename is +% assumed to already have the extension ".pdf". +% dest - filename of the destination eps file. The filename is assumed to +% already have the extension ".eps". + +% Copyright (C) Oliver Woodford 2009-2010 + +% Thanks to Aldebaro Klautau for reporting a bug when saving to +% non-existant directories. + +function pdf2eps(source, dest) +% Construct the options string for pdftops +options = ['-q -paper match -eps -level2 "' source '" "' dest '"']; +% Convert to eps using pdftops +[status, message] = pdftops(options); +% Check for error +if status + % Report error + if isempty(message) + error('Unable to generate eps. Check destination directory is writable.'); + else + error(message); + end +end +% Fix the DSC error created by pdftops +fid = fopen(dest, 'r+'); +if fid == -1 + % Cannot open the file + return +end +fgetl(fid); % Get the first line +str = fgetl(fid); % Get the second line +if strcmp(str(1:min(13, end)), '% Produced by') + fseek(fid, -numel(str)-1, 'cof'); + fwrite(fid, '%'); % Turn ' ' into '%' +end +fclose(fid); +end + diff --git a/offline/localizer/altmany-export_fig/pdftops.m b/offline/localizer/altmany-export_fig/pdftops.m new file mode 100755 index 00000000..0f9aa478 --- /dev/null +++ b/offline/localizer/altmany-export_fig/pdftops.m @@ -0,0 +1,150 @@ +function varargout = pdftops(cmd) +%PDFTOPS Calls a local pdftops executable with the input command +% +% Example: +% [status result] = pdftops(cmd) +% +% Attempts to locate a pdftops executable, finally asking the user to +% specify the directory pdftops was installed into. The resulting path is +% stored for future reference. +% +% Once found, the executable is called with the input command string. +% +% This function requires that you have pdftops (from the Xpdf package) +% installed on your system. You can download this from: +% http://www.foolabs.com/xpdf +% +% IN: +% cmd - Command string to be passed into pdftops (e.g. '-help'). +% +% OUT: +% status - 0 iff command ran without problem. +% result - Output from pdftops. + +% Copyright: Oliver Woodford, 2009-2010 + +% Thanks to Jonas Dorn for the fix for the title of the uigetdir window on Mac OS. +% Thanks to Christoph Hertel for pointing out a bug in check_xpdf_path under linux. +% 23/01/2014 - Add full path to pdftops.txt in warning. +% 27/05/2015 - Fixed alert in case of missing pdftops; fixed code indentation +% 02/05/2016 - Added possible error explanation suggested by Michael Pacer (issue #137) +% 02/05/2016 - Search additional possible paths suggested by Jonas Stein (issue #147) +% 03/05/2016 - Display the specific error message if pdftops fails for some reason (issue #148) + + % Call pdftops + [varargout{1:nargout}] = system(sprintf('"%s" %s', xpdf_path, cmd)); +end + +function path_ = xpdf_path + % Return a valid path + % Start with the currently set path + path_ = user_string('pdftops'); + % Check the path works + if check_xpdf_path(path_) + return + end + % Check whether the binary is on the path + if ispc + bin = 'pdftops.exe'; + else + bin = 'pdftops'; + end + if check_store_xpdf_path(bin) + path_ = bin; + return + end + % Search the obvious places + if ispc + paths = {'C:\Program Files\xpdf\pdftops.exe', 'C:\Program Files (x86)\xpdf\pdftops.exe'}; + else + paths = {'/usr/bin/pdftops', '/usr/local/bin/pdftops'}; + end + for a = 1:numel(paths) + path_ = paths{a}; + if check_store_xpdf_path(path_) + return + end + end + + % Ask the user to enter the path + errMsg1 = 'Pdftops not found. Please locate the program, or install xpdf-tools from '; + url1 = 'http://foolabs.com/xpdf'; + fprintf(2, '%s\n', [errMsg1 '' url1 '']); + errMsg1 = [errMsg1 url1]; + %if strncmp(computer,'MAC',3) % Is a Mac + % % Give separate warning as the MacOS uigetdir dialogue box doesn't have a title + % uiwait(warndlg(errMsg1)) + %end + + % Provide an alternative possible explanation as per issue #137 + errMsg2 = 'If you have pdftops installed, perhaps Matlab is shaddowing it as described in '; + url2 = 'https://github.com/altmany/export_fig/issues/137'; + fprintf(2, '%s\n', [errMsg2 'issue #137']); + errMsg2 = [errMsg2 url1]; + + state = 0; + while 1 + if state + option1 = 'Install pdftops'; + else + option1 = 'Issue #137'; + end + answer = questdlg({errMsg1,'',errMsg2},'Pdftops error',option1,'Locate pdftops','Cancel','Cancel'); + drawnow; % prevent a Matlab hang: http://undocumentedmatlab.com/blog/solving-a-matlab-hang-problem + switch answer + case 'Install pdftops' + web('-browser',url1); + case 'Issue #137' + web('-browser',url2); + state = 1; + case 'Locate pdftops' + base = uigetdir('/', errMsg1); + if isequal(base, 0) + % User hit cancel or closed window + break + end + base = [base filesep]; %#ok + bin_dir = {'', ['bin' filesep], ['lib' filesep]}; + for a = 1:numel(bin_dir) + path_ = [base bin_dir{a} bin]; + if exist(path_, 'file') == 2 + break + end + end + if check_store_xpdf_path(path_) + return + end + + otherwise % User hit Cancel or closed window + break + end + end + error('pdftops executable not found.'); +end + +function good = check_store_xpdf_path(path_) + % Check the path is valid + good = check_xpdf_path(path_); + if ~good + return + end + % Update the current default path to the path found + if ~user_string('pdftops', path_) + warning('Path to pdftops executable could not be saved. Enter it manually in %s.', fullfile(fileparts(which('user_string.m')), '.ignore', 'pdftops.txt')); + return + end +end + +function good = check_xpdf_path(path_) + % Check the path is valid + [good, message] = system(sprintf('"%s" -h', path_)); %#ok + % system returns good = 1 even when the command runs + % Look for something distinct in the help text + good = ~isempty(strfind(message, 'PostScript')); + + % Display the error message if the pdftops executable exists but fails for some reason + if ~good && exist(path_,'file') % file exists but generates an error + fprintf('Error running %s:\n', path_); + fprintf(2,'%s\n\n',message); + end +end diff --git a/offline/localizer/altmany-export_fig/print2array.m b/offline/localizer/altmany-export_fig/print2array.m new file mode 100755 index 00000000..5f85c7c9 --- /dev/null +++ b/offline/localizer/altmany-export_fig/print2array.m @@ -0,0 +1,244 @@ +function [A, bcol] = print2array(fig, res, renderer, gs_options) +%PRINT2ARRAY Exports a figure to an image array +% +% Examples: +% A = print2array +% A = print2array(figure_handle) +% A = print2array(figure_handle, resolution) +% A = print2array(figure_handle, resolution, renderer) +% A = print2array(figure_handle, resolution, renderer, gs_options) +% [A bcol] = print2array(...) +% +% This function outputs a bitmap image of the given figure, at the desired +% resolution. +% +% If renderer is '-painters' then ghostcript needs to be installed. This +% can be downloaded from: http://www.ghostscript.com +% +% IN: +% figure_handle - The handle of the figure to be exported. Default: gcf. +% resolution - Resolution of the output, as a factor of screen +% resolution. Default: 1. +% renderer - string containing the renderer paramater to be passed to +% print. Default: '-opengl'. +% gs_options - optional ghostscript options (e.g.: '-dNoOutputFonts'). If +% multiple options are needed, enclose in call array: {'-a','-b'} +% +% OUT: +% A - MxNx3 uint8 image of the figure. +% bcol - 1x3 uint8 vector of the background color + +% Copyright (C) Oliver Woodford 2008-2014, Yair Altman 2015- +%{ +% 05/09/11: Set EraseModes to normal when using opengl or zbuffer +% renderers. Thanks to Pawel Kocieniewski for reporting the issue. +% 21/09/11: Bug fix: unit8 -> uint8! Thanks to Tobias Lamour for reporting it. +% 14/11/11: Bug fix: stop using hardcopy(), as it interfered with figure size +% and erasemode settings. Makes it a bit slower, but more reliable. +% Thanks to Phil Trinh and Meelis Lootus for reporting the issues. +% 09/12/11: Pass font path to ghostscript. +% 27/01/12: Bug fix affecting painters rendering tall figures. Thanks to +% Ken Campbell for reporting it. +% 03/04/12: Bug fix to median input. Thanks to Andy Matthews for reporting it. +% 26/10/12: Set PaperOrientation to portrait. Thanks to Michael Watts for +% reporting the issue. +% 26/02/15: If temp dir is not writable, use the current folder for temp +% EPS/TIF files (Javier Paredes) +% 27/02/15: Display suggested workarounds to internal print() error (issue #16) +% 28/02/15: Enable users to specify optional ghostscript options (issue #36) +% 10/03/15: Fixed minor warning reported by Paul Soderlind; fixed code indentation +% 28/05/15: Fixed issue #69: patches with LineWidth==0.75 appear wide (internal bug in Matlab's print() func) +% 07/07/15: Fixed issue #83: use numeric handles in HG1 +%} + + % Generate default input arguments, if needed + if nargin < 2 + res = 1; + if nargin < 1 + fig = gcf; + end + end + % Warn if output is large + old_mode = get(fig, 'Units'); + set(fig, 'Units', 'pixels'); + px = get(fig, 'Position'); + set(fig, 'Units', old_mode); + npx = prod(px(3:4)*res)/1e6; + if npx > 30 + % 30M pixels or larger! + warning('MATLAB:LargeImage', 'print2array generating a %.1fM pixel image. This could be slow and might also cause memory problems.', npx); + end + % Retrieve the background colour + bcol = get(fig, 'Color'); + % Set the resolution parameter + res_str = ['-r' num2str(ceil(get(0, 'ScreenPixelsPerInch')*res))]; + % Generate temporary file name + tmp_nam = [tempname '.tif']; + try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); % cleanup + isTempDirOk = true; + catch + % Temp dir is not writable, so use the current folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = pwd; + tmp_nam = fullfile(fpath,[fname fext]); + isTempDirOk = false; + end + % Enable users to specify optional ghostscript options (issue #36) + if nargin > 3 && ~isempty(gs_options) + if iscell(gs_options) + gs_options = sprintf(' %s',gs_options{:}); + elseif ~ischar(gs_options) + error('gs_options input argument must be a string or cell-array of strings'); + else + gs_options = [' ' gs_options]; + end + else + gs_options = ''; + end + if nargin > 2 && strcmp(renderer, '-painters') + % Print to eps file + if isTempDirOk + tmp_eps = [tempname '.eps']; + else + tmp_eps = fullfile(fpath,[fname '.eps']); + end + print2eps(tmp_eps, fig, 0, renderer, '-loose'); + try + % Initialize the command to export to tiff using ghostscript + cmd_str = ['-dEPSCrop -q -dNOPAUSE -dBATCH ' res_str ' -sDEVICE=tiff24nc']; + % Set the font path + fp = font_path(); + if ~isempty(fp) + cmd_str = [cmd_str ' -sFONTPATH="' fp '"']; + end + % Add the filenames + cmd_str = [cmd_str ' -sOutputFile="' tmp_nam '" "' tmp_eps '"' gs_options]; + % Execute the ghostscript command + ghostscript(cmd_str); + catch me + % Delete the intermediate file + delete(tmp_eps); + rethrow(me); + end + % Delete the intermediate file + delete(tmp_eps); + % Read in the generated bitmap + A = imread(tmp_nam); + % Delete the temporary bitmap file + delete(tmp_nam); + % Set border pixels to the correct colour + if isequal(bcol, 'none') + bcol = []; + elseif isequal(bcol, [1 1 1]) + bcol = uint8([255 255 255]); + else + for l = 1:size(A, 2) + if ~all(reshape(A(:,l,:) == 255, [], 1)) + break; + end + end + for r = size(A, 2):-1:l + if ~all(reshape(A(:,r,:) == 255, [], 1)) + break; + end + end + for t = 1:size(A, 1) + if ~all(reshape(A(t,:,:) == 255, [], 1)) + break; + end + end + for b = size(A, 1):-1:t + if ~all(reshape(A(b,:,:) == 255, [], 1)) + break; + end + end + bcol = uint8(median(single([reshape(A(:,[l r],:), [], size(A, 3)); reshape(A([t b],:,:), [], size(A, 3))]), 1)); + for c = 1:size(A, 3) + A(:,[1:l-1, r+1:end],c) = bcol(c); + A([1:t-1, b+1:end],:,c) = bcol(c); + end + end + else + if nargin < 3 + renderer = '-opengl'; + end + err = false; + % Set paper size + old_pos_mode = get(fig, 'PaperPositionMode'); + old_orientation = get(fig, 'PaperOrientation'); + set(fig, 'PaperPositionMode', 'auto', 'PaperOrientation', 'portrait'); + try + % Workaround for issue #69: patches with LineWidth==0.75 appear wide (internal bug in Matlab's print() function) + fp = []; % in case we get an error below + fp = findall(fig, 'Type','patch', 'LineWidth',0.75); + set(fp, 'LineWidth',0.5); + % Fix issue #83: use numeric handles in HG1 + if ~using_hg2(fig), fig = double(fig); end + % Print to tiff file + print(fig, renderer, res_str, '-dtiff', tmp_nam); + % Read in the printed file + A = imread(tmp_nam); + % Delete the temporary file + delete(tmp_nam); + catch ex + err = true; + end + set(fp, 'LineWidth',0.75); % restore original figure appearance + % Reset paper size + set(fig, 'PaperPositionMode', old_pos_mode, 'PaperOrientation', old_orientation); + % Throw any error that occurred + if err + % Display suggested workarounds to internal print() error (issue #16) + fprintf(2, 'An error occured with Matlab''s builtin print function.\nTry setting the figure Renderer to ''painters'' or use opengl(''software'').\n\n'); + rethrow(ex); + end + % Set the background color + if isequal(bcol, 'none') + bcol = []; + else + bcol = bcol * 255; + if isequal(bcol, round(bcol)) + bcol = uint8(bcol); + else + bcol = squeeze(A(1,1,:)); + end + end + end + % Check the output size is correct + if isequal(res, round(res)) + px = round([px([4 3])*res 3]); % round() to avoid an indexing warning below + if ~isequal(size(A), px) + % Correct the output size + A = A(1:min(end,px(1)),1:min(end,px(2)),:); + end + end +end + +% Function to return (and create, where necessary) the font path +function fp = font_path() + fp = user_string('gs_font_path'); + if ~isempty(fp) + return + end + % Create the path + % Start with the default path + fp = getenv('GS_FONTPATH'); + % Add on the typical directories for a given OS + if ispc + if ~isempty(fp) + fp = [fp ';']; + end + fp = [fp getenv('WINDIR') filesep 'Fonts']; + else + if ~isempty(fp) + fp = [fp ':']; + end + fp = [fp '/usr/share/fonts:/usr/local/share/fonts:/usr/share/fonts/X11:/usr/local/share/fonts/X11:/usr/share/fonts/truetype:/usr/local/share/fonts/truetype']; + end + user_string('gs_font_path', fp); +end diff --git a/offline/localizer/altmany-export_fig/print2eps.m b/offline/localizer/altmany-export_fig/print2eps.m new file mode 100755 index 00000000..dc7deb1d --- /dev/null +++ b/offline/localizer/altmany-export_fig/print2eps.m @@ -0,0 +1,529 @@ +function print2eps(name, fig, export_options, varargin) +%PRINT2EPS Prints figures to eps with improved line styles +% +% Examples: +% print2eps filename +% print2eps(filename, fig_handle) +% print2eps(filename, fig_handle, export_options) +% print2eps(filename, fig_handle, export_options, print_options) +% +% This function saves a figure as an eps file, with two improvements over +% MATLAB's print command. First, it improves the line style, making dashed +% lines more like those on screen and giving grid lines a dotted line style. +% Secondly, it substitutes original font names back into the eps file, +% where these have been changed by MATLAB, for up to 11 different fonts. +% +%IN: +% filename - string containing the name (optionally including full or +% relative path) of the file the figure is to be saved as. A +% ".eps" extension is added if not there already. If a path is +% not specified, the figure is saved in the current directory. +% fig_handle - The handle of the figure to be saved. Default: gcf(). +% export_options - array or struct of optional scalar values: +% bb_padding - Scalar value of amount of padding to add to border around +% the cropped image, in points (if >1) or percent (if <1). +% Can be negative as well as positive; Default: 0 +% crop - Cropping flag. Deafult: 0 +% fontswap - Whether to swap non-default fonts in figure. Default: true +% renderer - Renderer used to generate bounding-box. Default: 'opengl' +% crop_amounts - 4-element vector of crop amounts: [top,right,bottom,left] +% (available only via the struct alternative) +% print_options - Additional parameter strings to be passed to the print command + +%{ +% Copyright (C) Oliver Woodford 2008-2014, Yair Altman 2015- + +% The idea of editing the EPS file to change line styles comes from Jiro +% Doke's FIXPSLINESTYLE (fex id: 17928) +% The idea of changing dash length with line width came from comments on +% fex id: 5743, but the implementation is mine :) +%} +%{ +% 14/11/11: Fix a MATLAB bug rendering black or white text incorrectly. +% Thanks to Mathieu Morlighem for reporting the issue and +% obtaining a fix from TMW. +% 08/12/11: Added ability to correct fonts. Several people have requested +% this at one time or another, and also pointed me to printeps +% (fex id: 7501), so thank you to them. My implementation (which +% was not inspired by printeps - I'd already had the idea for my +% approach) goes slightly further in that it allows multiple +% fonts to be swapped. +% 14/12/11: Fix bug affecting font names containing spaces. Thanks to David +% Szwer for reporting the issue. +% 25/01/12: Add a font not to be swapped. Thanks to Anna Rafferty and Adam +% Jackson for reporting the issue. Also fix a bug whereby using a +% font alias can lead to another font being swapped in. +% 10/04/12: Make the font swapping case insensitive. +% 26/10/12: Set PaperOrientation to portrait. Thanks to Michael Watts for +% reporting the issue. +% 26/10/12: Fix issue to do with swapping fonts changing other fonts and +% sizes we don't want, due to listeners. Thanks to Malcolm Hudson +% for reporting the issue. +% 22/03/13: Extend font swapping to axes labels. Thanks to Rasmus Ischebeck +% for reporting the issue. +% 23/07/13: Bug fix to font swapping. Thanks to George for reporting the +% issue. +% 13/08/13: Fix MATLAB feature of not exporting white lines correctly. +% Thanks to Sebastian Hesslinger for reporting it. +% 24/02/15: Fix for Matlab R2014b bug (issue #31): LineWidths<0.75 are not +% set in the EPS (default line width is used) +% 25/02/15: Fixed issue #32: BoundingBox problem caused uncropped EPS/PDF files +% 05/03/15: Fixed issue #43: Inability to perform EPS file post-processing +% 06/03/15: Improved image padding & cropping thanks to Oscar Hartogensis +% 21/03/15: Fixed edge-case of missing handles having a 'FontName' property +% 26/03/15: Attempt to fix issue #45: white lines in subplots do not print correctly +% 27/03/15: Attempt to fix issue #44: white artifact lines appearing in patch exports +% 30/03/15: Fixed issue #52: improved performance on HG2 (R2014b+) +% 09/04/15: Comment blocks consolidation and minor code cleanup (no real code change) +% 12/04/15: Fixed issue #56: bad cropping +% 14/04/15: Workaround for issue #45: lines in image subplots are exported in invalid color +% 07/07/15: Added option to avoid font-swapping in EPS/PDF +% 07/07/15: Fixed issue #83: use numeric handles in HG1 +% 22/07/15: Fixed issue #91 (thanks to Carlos Moffat) +% 28/09/15: Fixed issue #108 (thanks to JacobD10) +% 01/11/15: Fixed issue #112: optional renderer for bounding-box computation (thanks to Jesús Pestana Puerta) +% 21/02/16: Enabled specifying non-automated crop amounts +% 22/02/16: Better support + backward compatibility for transparency (issue #108) +% 10/06/16: Fixed issue #159: text handles get cleared by Matlab in the print() command +% 12/06/16: Improved the fix for issue #159 (in the previous commit) +% 12/06/16: Fixed issue #158: transparent patch color in PDF/EPS +%} + + options = {'-loose'}; + if nargin > 3 + options = [options varargin]; + elseif nargin < 3 + export_options = 0; + if nargin < 2 + fig = gcf(); + end + end + + % Retrieve padding, crop & font-swap values + crop_amounts = nan(1,4); % auto-crop all 4 sides by default + if isstruct(export_options) + try fontswap = export_options.fontswap; catch, fontswap = true; end + try bb_crop = export_options.crop; catch, bb_crop = 0; end + try crop_amounts = export_options.crop_amounts; catch, end + try bb_padding = export_options.bb_padding; catch, bb_padding = 0; end + try renderer = export_options.rendererStr; catch, renderer = 'opengl'; end % fix for issue #110 + if renderer(1)~='-', renderer = ['-' renderer]; end + else + if numel(export_options) > 2 % font-swapping + fontswap = export_options(3); + else + fontswap = true; + end + if numel(export_options) > 1 % cropping + bb_crop = export_options(2); + else + bb_crop = 0; % scalar value, so use default bb_crop value of 0 + end + if numel(export_options) > 0 % padding + bb_padding = export_options(1); + else + bb_padding = 0; + end + renderer = '-opengl'; + end + + % Construct the filename + if numel(name) < 5 || ~strcmpi(name(end-3:end), '.eps') + name = [name '.eps']; % Add the missing extension + end + + % Set paper size + old_pos_mode = get(fig, 'PaperPositionMode'); + old_orientation = get(fig, 'PaperOrientation'); + set(fig, 'PaperPositionMode', 'auto', 'PaperOrientation', 'portrait'); + + % Find all the used fonts in the figure + font_handles = findall(fig, '-property', 'FontName'); + fonts = get(font_handles, 'FontName'); + if isempty(fonts) + fonts = {}; + elseif ~iscell(fonts) + fonts = {fonts}; + end + + % Map supported font aliases onto the correct name + fontsl = lower(fonts); + for a = 1:numel(fonts) + f = fontsl{a}; + f(f==' ') = []; + switch f + case {'times', 'timesnewroman', 'times-roman'} + fontsl{a} = 'times-roman'; + case {'arial', 'helvetica'} + fontsl{a} = 'helvetica'; + case {'newcenturyschoolbook', 'newcenturyschlbk'} + fontsl{a} = 'newcenturyschlbk'; + otherwise + end + end + fontslu = unique(fontsl); + + % Determine the font swap table + if fontswap + matlab_fonts = {'Helvetica', 'Times-Roman', 'Palatino', 'Bookman', 'Helvetica-Narrow', 'Symbol', ... + 'AvantGarde', 'NewCenturySchlbk', 'Courier', 'ZapfChancery', 'ZapfDingbats'}; + matlab_fontsl = lower(matlab_fonts); + require_swap = find(~ismember(fontslu, matlab_fontsl)); + unused_fonts = find(~ismember(matlab_fontsl, fontslu)); + font_swap = cell(3, min(numel(require_swap), numel(unused_fonts))); + fonts_new = fonts; + for a = 1:size(font_swap, 2) + font_swap{1,a} = find(strcmp(fontslu{require_swap(a)}, fontsl)); + font_swap{2,a} = matlab_fonts{unused_fonts(a)}; + font_swap{3,a} = fonts{font_swap{1,a}(1)}; + fonts_new(font_swap{1,a}) = font_swap(2,a); + end + else + font_swap = []; + end + + % Swap the fonts + if ~isempty(font_swap) + fonts_size = get(font_handles, 'FontSize'); + if iscell(fonts_size) + fonts_size = cell2mat(fonts_size); + end + M = false(size(font_handles)); + + % Loop because some changes may not stick first time, due to listeners + c = 0; + update = zeros(1000, 1); + for b = 1:10 % Limit number of loops to avoid infinite loop case + for a = 1:numel(M) + M(a) = ~isequal(get(font_handles(a), 'FontName'), fonts_new{a}) || ~isequal(get(font_handles(a), 'FontSize'), fonts_size(a)); + if M(a) + set(font_handles(a), 'FontName', fonts_new{a}, 'FontSize', fonts_size(a)); + c = c + 1; + update(c) = a; + end + end + if ~any(M) + break; + end + end + + % Compute the order to revert fonts later, without the need of a loop + [update, M] = unique(update(1:c)); + [M, M] = sort(M); + update = reshape(update(M), 1, []); + end + + % MATLAB bug fix - black and white text can come out inverted sometimes + % Find the white and black text + black_text_handles = findall(fig, 'Type', 'text', 'Color', [0 0 0]); + white_text_handles = findall(fig, 'Type', 'text', 'Color', [1 1 1]); + % Set the font colors slightly off their correct values + set(black_text_handles, 'Color', [0 0 0] + eps); + set(white_text_handles, 'Color', [1 1 1] - eps); + + % MATLAB bug fix - white lines can come out funny sometimes + % Find the white lines + white_line_handles = findall(fig, 'Type', 'line', 'Color', [1 1 1]); + % Set the line color slightly off white + set(white_line_handles, 'Color', [1 1 1] - 0.00001); + + % Workaround for issue #45: lines in image subplots are exported in invalid color + % In this case the -depsc driver solves the problem, but then all the other workarounds + % below (for all the other issues) will fail, so it's better to let the user decide by + % just issuing a warning and accepting the '-depsc' input parameter + epsLevel2 = ~any(strcmpi(options,'-depsc')); + if epsLevel2 + % Use -depsc2 (EPS color level-2) if -depsc (EPS color level-3) was not specifically requested + options{end+1} = '-depsc2'; + % Issue a warning if multiple images & lines were found in the figure, and HG1 with painters renderer is used + isPainters = any(strcmpi(options,'-painters')); + if isPainters && ~using_hg2 && numel(findall(fig,'Type','image'))>1 && ~isempty(findall(fig,'Type','line')) + warning('YMA:export_fig:issue45', ... + ['Multiple images & lines detected. In such cases, the lines might \n' ... + 'appear with an invalid color due to an internal MATLAB bug (fixed in R2014b). \n' ... + 'Possible workaround: add a ''-depsc'' or ''-opengl'' parameter to the export_fig command.']); + end + end + + % Fix issue #83: use numeric handles in HG1 + if ~using_hg2(fig), fig = double(fig); end + + % Workaround for when transparency is lost through conversion fig>EPS>PDF (issue #108) + % Replace transparent patch RGB values with an ID value (rare chance that ID color is being used already) + if using_hg2 + origAlphaColors = eps_maintainAlpha(fig); + end + + % Print to eps file + print(fig, options{:}, name); + + % Do post-processing on the eps file + try + % Read the EPS file into memory + fstrm = read_write_entire_textfile(name); + catch + fstrm = ''; + end + + % Restore colors for transparent patches/lines and apply the + % setopacityalpha setting in the EPS file (issue #108) + if using_hg2 + [~,fstrm,foundFlags] = eps_maintainAlpha(fig, fstrm, origAlphaColors); + + % If some of the transparencies were not found in the EPS file, then rerun the + % export with only the found transparencies modified (backward compatibility) + if ~isempty(fstrm) && ~all(foundFlags) + foundIdx = find(foundFlags); + for objIdx = 1 : sum(foundFlags) + colorsIdx = foundIdx(objIdx); + colorsData = origAlphaColors{colorsIdx}; + hObj = colorsData{1}; + propName = colorsData{2}; + newColor = colorsData{4}; + hObj.(propName).ColorData = newColor; + end + delete(name); + print(fig, options{:}, name); + fstrm = read_write_entire_textfile(name); + [~,fstrm] = eps_maintainAlpha(fig, fstrm, origAlphaColors(foundFlags)); + end + end + + % Fix for Matlab R2014b bug (issue #31): LineWidths<0.75 are not set in the EPS (default line width is used) + try + if ~isempty(fstrm) && using_hg2(fig) + % Convert miter joins to line joins + %fstrm = regexprep(fstrm, '\n10.0 ML\n', '\n1 LJ\n'); + % This is faster (the original regexprep could take many seconds when the axes contains many lines): + fstrm = strrep(fstrm, sprintf('\n10.0 ML\n'), sprintf('\n1 LJ\n')); + + % In HG2, grid lines and axes Ruler Axles have a default LineWidth of 0.5 => replace en-bulk (assume that 1.0 LineWidth = 1.333 LW) + % hAxes=gca; hAxes.YGridHandle.LineWidth, hAxes.YRuler.Axle.LineWidth + %fstrm = regexprep(fstrm, '(GC\n2 setlinecap\n1 LJ)\nN', '$1\n0.667 LW\nN'); + % This is faster: + fstrm = strrep(fstrm, sprintf('GC\n2 setlinecap\n1 LJ\nN'), sprintf('GC\n2 setlinecap\n1 LJ\n0.667 LW\nN')); + + % This is more accurate but *MUCH* slower (issue #52) + %{ + % Modify all thin lines in the figure to have 10x LineWidths + hLines = findall(fig,'Type','line'); + hThinLines = []; + for lineIdx = 1 : numel(hLines) + thisLine = hLines(lineIdx); + if thisLine.LineWidth < 0.75 && strcmpi(thisLine.Visible,'on') + hThinLines(end+1) = thisLine; %#ok + thisLine.LineWidth = thisLine.LineWidth * 10; + end + end + + % If any thin lines were found + if ~isempty(hThinLines) + % Prepare an EPS with large-enough line widths + print(fig, options{:}, name); + % Restore the original LineWidths in the figure + for lineIdx = 1 : numel(hThinLines) + thisLine = handle(hThinLines(lineIdx)); + thisLine.LineWidth = thisLine.LineWidth / 10; + end + + % Compare the original and the new EPS files and correct the original stream's LineWidths + fstrm_new = read_write_entire_textfile(name); + idx = 500; % skip heading with its possibly-different timestamp + markerStr = sprintf('10.0 ML\nN'); + markerLen = length(markerStr); + while ~isempty(idx) && idx < length(fstrm) + lastIdx = min(length(fstrm), length(fstrm_new)); + delta = fstrm(idx+1:lastIdx) - fstrm_new(idx+1:lastIdx); + idx = idx + find(delta,1); + if ~isempty(idx) && ... + isequal(fstrm(idx-markerLen+1:idx), markerStr) && ... + ~isempty(regexp(fstrm_new(idx-markerLen+1:idx+12),'10.0 ML\n[\d\.]+ LW\nN')) %#ok + value = str2double(regexprep(fstrm_new(idx:idx+12),' .*','')); + if isnan(value), break; end % something's wrong... - bail out + newStr = sprintf('%0.3f LW\n',value/10); + fstrm = [fstrm(1:idx-1) newStr fstrm(idx:end)]; + idx = idx + 12; + else + break; + end + end + end + %} + + % This is much faster although less accurate: fix all non-gray lines to have a LineWidth of 0.75 (=1 LW) + % Note: This will give incorrect LineWidth of 075 for lines having LineWidth<0.75, as well as for non-gray grid-lines (if present) + % However, in practice these edge-cases are very rare indeed, and the difference in LineWidth should not be noticeable + %fstrm = regexprep(fstrm, '([CR]C\n2 setlinecap\n1 LJ)\nN', '$1\n1 LW\nN'); + % This is faster (the original regexprep could take many seconds when the axes contains many lines): + fstrm = strrep(fstrm, sprintf('\n2 setlinecap\n1 LJ\nN'), sprintf('\n2 setlinecap\n1 LJ\n1 LW\nN')); + end + catch err + fprintf(2, 'Error fixing LineWidths in EPS file: %s\n at %s:%d\n', err.message, err.stack(1).file, err.stack(1).line); + end + + % Reset the font and line colors + try + set(black_text_handles, 'Color', [0 0 0]); + set(white_text_handles, 'Color', [1 1 1]); + catch + % Fix issue #159: redo findall() '*text_handles' + black_text_handles = findall(fig, 'Type', 'text', 'Color', [0 0 0]+eps); + white_text_handles = findall(fig, 'Type', 'text', 'Color', [1 1 1]-eps); + set(black_text_handles, 'Color', [0 0 0]); + set(white_text_handles, 'Color', [1 1 1]); + end + set(white_line_handles, 'Color', [1 1 1]); + + % Reset paper size + set(fig, 'PaperPositionMode', old_pos_mode, 'PaperOrientation', old_orientation); + + % Reset the font names in the figure + if ~isempty(font_swap) + for a = update + set(font_handles(a), 'FontName', fonts{a}, 'FontSize', fonts_size(a)); + end + end + + % Bail out if EPS post-processing is not possible + if isempty(fstrm) + warning('Loading EPS file failed, so unable to perform post-processing. This is usually because the figure contains a large number of patch objects. Consider exporting to a bitmap format in this case.'); + return + end + + % Replace the font names + if ~isempty(font_swap) + for a = 1:size(font_swap, 2) + %fstrm = regexprep(fstrm, [font_swap{1,a} '-?[a-zA-Z]*\>'], font_swap{3,a}(~isspace(font_swap{3,a}))); + fstrm = regexprep(fstrm, font_swap{2,a}, font_swap{3,a}(~isspace(font_swap{3,a}))); + end + end + + % Move the bounding box to the top of the file (HG2 only), or fix the line styles (HG1 only) + if using_hg2(fig) + % Move the bounding box to the top of the file (HG2 only) + [s, e] = regexp(fstrm, '%%BoundingBox: [^%]*%%'); + if numel(s) == 2 + fstrm = fstrm([1:s(1)-1 s(2):e(2)-2 e(1)-1:s(2)-1 e(2)-1:end]); + end + else + % Fix the line styles (HG1 only) + fstrm = fix_lines(fstrm); + end + + % Apply the bounding box padding & cropping, replacing Matlab's print()'s bounding box + if bb_crop + % Calculate a new bounding box based on a bitmap print using crop_border.m + % 1. Determine the Matlab BoundingBox and PageBoundingBox + [s,e] = regexp(fstrm, '%%BoundingBox: [^%]*%%'); % location BB in eps file + if numel(s)==2, s=s(2); e=e(2); end + aa = fstrm(s+15:e-3); % dimensions bb - STEP1 + bb_matlab = cell2mat(textscan(aa,'%f32%f32%f32%f32')); % dimensions bb - STEP2 + + [s,e] = regexp(fstrm, '%%PageBoundingBox: [^%]*%%'); % location bb in eps file + if numel(s)==2, s=s(2); e=e(2); end + aa = fstrm(s+19:e-3); % dimensions bb - STEP1 + pagebb_matlab = cell2mat(textscan(aa,'%f32%f32%f32%f32')); % dimensions bb - STEP2 + + % 2. Create a bitmap image and use crop_borders to create the relative + % bb with respect to the PageBoundingBox + [A, bcol] = print2array(fig, 1, renderer); + [aa, aa, aa, bb_rel] = crop_borders(A, bcol, bb_padding, crop_amounts); + + % 3. Calculate the new Bounding Box + pagew = pagebb_matlab(3)-pagebb_matlab(1); + pageh = pagebb_matlab(4)-pagebb_matlab(2); + %bb_new = [pagebb_matlab(1)+pagew*bb_rel(1) pagebb_matlab(2)+pageh*bb_rel(2) ... + % pagebb_matlab(1)+pagew*bb_rel(3) pagebb_matlab(2)+pageh*bb_rel(4)]; + bb_new = pagebb_matlab([1,2,1,2]) + [pagew,pageh,pagew,pageh].*bb_rel; % clearer + bb_offset = (bb_new-bb_matlab) + [-1,-1,1,1]; % 1px margin so that cropping is not TOO tight + + % Apply the bounding box padding + if bb_padding + if abs(bb_padding)<1 + bb_padding = round((mean([bb_new(3)-bb_new(1) bb_new(4)-bb_new(2)])*bb_padding)/0.5)*0.5; % ADJUST BB_PADDING + end + add_padding = @(n1, n2, n3, n4) sprintf(' %d', str2double({n1, n2, n3, n4}) + [-bb_padding -bb_padding bb_padding bb_padding] + bb_offset); + else + add_padding = @(n1, n2, n3, n4) sprintf(' %d', str2double({n1, n2, n3, n4}) + bb_offset); % fix small but noticeable bounding box shift + end + fstrm = regexprep(fstrm, '%%BoundingBox:[ ]+([-]?\d+)[ ]+([-]?\d+)[ ]+([-]?\d+)[ ]+([-]?\d+)', '%%BoundingBox:${add_padding($1, $2, $3, $4)}'); + end + + % Fix issue #44: white artifact lines appearing in patch exports + % Note: the problem is due to the fact that Matlab's print() function exports patches + % as a combination of filled triangles, and a white line appears where the triangles touch + % In the workaround below, we will modify such dual-triangles into a filled rectangle. + % We are careful to only modify regexps that exactly match specific patterns - it's better to not + % correct some white-line artifacts than to change the geometry of a patch, or to corrupt the EPS. + % e.g.: '0 -450 937 0 0 450 3 MP PP 937 0 0 -450 0 450 3 MP PP' => '0 -450 937 0 0 450 0 0 4 MP' + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\2 \1 \3 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\2 \3 \1 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\3 \1 \2 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\3 \2 \1 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + + % Write out the fixed eps file + read_write_entire_textfile(name, fstrm); +end + +function [StoredColors, fstrm, foundFlags] = eps_maintainAlpha(fig, fstrm, StoredColors) + if nargin == 1 % in: convert transparency in Matlab figure into unique RGB colors + hObjs = findall(fig); %findobj(fig,'Type','Area'); + StoredColors = {}; + propNames = {'Face','Edge'}; + for objIdx = 1:length(hObjs) + hObj = hObjs(objIdx); + for propIdx = 1 : numel(propNames) + try + propName = propNames{propIdx}; + if strcmp(hObj.(propName).ColorType, 'truecoloralpha') + nColors = length(StoredColors); + oldColor = hObj.(propName).ColorData; + newColor = uint8([101; 102+floor(nColors/255); mod(nColors,255); 255]); + StoredColors{end+1} = {hObj, propName, oldColor, newColor}; + hObj.(propName).ColorData = newColor; + end + catch + % Never mind - ignore (either doesn't have the property or cannot change it) + end + end + end + else % restore transparency in Matlab figure by converting back from the unique RGBs + %Find the transparent patches + wasError = false; + nColors = length(StoredColors); + foundFlags = false(1,nColors); + for objIdx = 1 : nColors + colorsData = StoredColors{objIdx}; + hObj = colorsData{1}; + propName = colorsData{2}; + origColor = colorsData{3}; + newColor = colorsData{4}; + try + %Restore the EPS files patch color + colorID = num2str(round(double(newColor(1:3)') /255,3),'%.3g %.3g %.3g'); %ID for searching + origRGB = num2str(round(double(origColor(1:3)')/255,3),'%.3g %.3g %.3g'); %Replace with original color + origAlpha = num2str(round(double(origColor(end)) /255,3),'%.3g'); %Convert alpha value for EPS + + %Find and replace the RGBA values within the EPS text fstrm + if strcmpi(propName,'Face') + oldStr = sprintf(['\n' colorID ' RC\nN\n']); + newStr = sprintf(['\n' origRGB ' RC\n' origAlpha ' .setopacityalpha true\nN\n']); + else %'Edge' + oldStr = sprintf(['\n' colorID ' RC\n1 LJ\n']); + newStr = sprintf(['\n' origRGB ' RC\n' origAlpha ' .setopacityalpha true\n']); + end + foundFlags(objIdx) = ~isempty(strfind(fstrm, oldStr)); + fstrm = strrep(fstrm, oldStr, newStr); + + %Restore the figure object's original color + hObj.(propName).ColorData = origColor; + catch err + % something is wrong - cannot restore transparent color... + if ~wasError + fprintf(2, 'Error maintaining transparency in EPS file: %s\n at %s:%d\n', err.message, err.stack(1).file, err.stack(1).line); + wasError = true; + end + end + end + end +end diff --git a/offline/localizer/altmany-export_fig/read_write_entire_textfile.m b/offline/localizer/altmany-export_fig/read_write_entire_textfile.m new file mode 100755 index 00000000..9fabb22d --- /dev/null +++ b/offline/localizer/altmany-export_fig/read_write_entire_textfile.m @@ -0,0 +1,37 @@ +%READ_WRITE_ENTIRE_TEXTFILE Read or write a whole text file to/from memory +% +% Read or write an entire text file to/from memory, without leaving the +% file open if an error occurs. +% +% Reading: +% fstrm = read_write_entire_textfile(fname) +% Writing: +% read_write_entire_textfile(fname, fstrm) +% +%IN: +% fname - Pathname of text file to be read in. +% fstrm - String to be written to the file, including carriage returns. +% +%OUT: +% fstrm - String read from the file. If an fstrm input is given the +% output is the same as that input. + +function fstrm = read_write_entire_textfile(fname, fstrm) +modes = {'rt', 'wt'}; +writing = nargin > 1; +fh = fopen(fname, modes{1+writing}); +if fh == -1 + error('Unable to open file %s.', fname); +end +try + if writing + fwrite(fh, fstrm, 'char*1'); + else + fstrm = fread(fh, '*char')'; + end +catch ex + fclose(fh); + rethrow(ex); +end +fclose(fh); +end diff --git a/offline/localizer/altmany-export_fig/user_string.m b/offline/localizer/altmany-export_fig/user_string.m new file mode 100755 index 00000000..8d35b3d5 --- /dev/null +++ b/offline/localizer/altmany-export_fig/user_string.m @@ -0,0 +1,105 @@ +function string = user_string(string_name, string) +%USER_STRING Get/set a user specific string +% +% Examples: +% string = user_string(string_name) +% isSaved = user_string(string_name, new_string) +% +% Function to get and set a string in a system or user specific file. This +% enables, for example, system specific paths to binaries to be saved. +% +% The specified string will be saved in a file named .txt, +% either in a subfolder named .ignore under this file's folder, or in the +% user's prefdir folder (in case this file's folder is non-writable). +% +% IN: +% string_name - String containing the name of the string required, which +% sets the filename storing the string: .txt +% new_string - The new string to be saved in the .txt file +% +% OUT: +% string - The currently saved string. Default: '' +% isSaved - Boolean indicating whether the save was succesful + +% Copyright (C) Oliver Woodford 2011-2014, Yair Altman 2015- + +% This method of saving paths avoids changing .m files which might be in a +% version control system. Instead it saves the user dependent paths in +% separate files with a .txt extension, which need not be checked in to +% the version control system. Thank you to Jonas Dorn for suggesting this +% approach. + +% 10/01/2013 - Access files in text, not binary mode, as latter can cause +% errors. Thanks to Christian for pointing this out. +% 29/05/2015 - Save file in prefdir if current folder is non-writable (issue #74) + + if ~ischar(string_name) + error('string_name must be a string.'); + end + % Create the full filename + fname = [string_name '.txt']; + dname = fullfile(fileparts(mfilename('fullpath')), '.ignore'); + file_name = fullfile(dname, fname); + if nargin > 1 + % Set string + if ~ischar(string) + error('new_string must be a string.'); + end + % Make sure the save directory exists + %dname = fileparts(file_name); + if ~exist(dname, 'dir') + % Create the directory + try + if ~mkdir(dname) + string = false; + return + end + catch + string = false; + return + end + % Make it hidden + try + fileattrib(dname, '+h'); + catch + end + end + % Write the file + fid = fopen(file_name, 'wt'); + if fid == -1 + % file cannot be created/updated - use prefdir if file does not already exist + % (if file exists but is simply not writable, don't create a duplicate in prefdir) + if ~exist(file_name,'file') + file_name = fullfile(prefdir, fname); + fid = fopen(file_name, 'wt'); + end + if fid == -1 + string = false; + return; + end + end + try + fprintf(fid, '%s', string); + catch + fclose(fid); + string = false; + return + end + fclose(fid); + string = true; + else + % Get string + fid = fopen(file_name, 'rt'); + if fid == -1 + % file cannot be read, try to read the file in prefdir + file_name = fullfile(prefdir, fname); + fid = fopen(file_name, 'rt'); + if fid == -1 + string = ''; + return + end + end + string = fgetl(fid); + fclose(fid); + end +end diff --git a/offline/localizer/altmany-export_fig/using_hg2.m b/offline/localizer/altmany-export_fig/using_hg2.m new file mode 100755 index 00000000..ba72228b --- /dev/null +++ b/offline/localizer/altmany-export_fig/using_hg2.m @@ -0,0 +1,36 @@ +%USING_HG2 Determine if the HG2 graphics engine is used +% +% tf = using_hg2(fig) +% +%IN: +% fig - handle to the figure in question. +% +%OUT: +% tf - boolean indicating whether the HG2 graphics engine is being used +% (true) or not (false). + +% 19/06/2015 - Suppress warning in R2015b; cache result for improved performance +% 06/06/2016 - Fixed issue #156 (bad return value in R2016b) + +function tf = using_hg2(fig) + persistent tf_cached + if isempty(tf_cached) + try + if nargin < 1, fig = figure('visible','off'); end + oldWarn = warning('off','MATLAB:graphicsversion:GraphicsVersionRemoval'); + try + % This generates a [supressed] warning in R2015b: + tf = ~graphicsversion(fig, 'handlegraphics'); + catch + tf = ~verLessThan('matlab','8.4'); % =R2014b + end + warning(oldWarn); + catch + tf = false; + end + if nargin < 1, delete(fig); end + tf_cached = tf; + else + tf = tf_cached; + end +end diff --git a/offline/localizer/covariance.m b/offline/localizer/covariance.m new file mode 100644 index 00000000..830c2ef0 --- /dev/null +++ b/offline/localizer/covariance.m @@ -0,0 +1,27 @@ +% graph results + +clear; +close all; + +addpath('./latlonutm/Codes/matlab'); + +file = 'localizer_v3.mat'; +load(file, 'trajectory', 'P'); + +k = 100; +t1 = 1476008235676; +t2 = 1476009019782; +t = linspace(0, (t2 - t1)/1000, size(trajectory,2)); + +trajectory(5,:) = deg2rad( trajectory(5,:) ); + +Q = cov(trajectory(1:7,:)') + +for i = 1:size(trajectory, 1) + fprintf(1, '%d %5.5f, %5.5f\n', i, mean(trajectory(i,1:k:end)), std(trajectory(i,1:k:end))); +end + +fprintf(1, '\n'); +for i = 1:size(trajectory, 1) + fprintf(1, '%d %5.5f, %5.5f\n', i, mean(trajectory(i,1:k)), std(trajectory(i,1:k))); +end diff --git a/offline/localizer/get_logs.py b/offline/localizer/get_logs.py new file mode 100644 index 00000000..7e86161d --- /dev/null +++ b/offline/localizer/get_logs.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python + +import json +import numpy as np +import scipy.io + +combined = True +d = None +# file = '../../../rolls_logs/sensors_2016-10-09-06-17-15.txt' +file = '/Users/babraham/Desktop/sensors_2017-01-28-19-19-04.txt' +with open(file) as json_data: + d = json.load(json_data) + +if d is None: + print('No log file found') + exit() + +messages = d['sensor_data'] +map_names = ['imu_linear_no_grav', 'imu_ang_pos', 'imu_ang_vel', + 'gps', 'encoder', 'steering', 'imu_temp'] +map_names_nd = np.asarray(map_names, dtype='object') +start_time = messages[0]['timestamp'] + +if combined: + logs = [] + for msg in messages: + try: + name = msg['topicName'] + name = str(name[8:]) + ind = map_names.index(name) + except: + continue + + if name == 'imu_linear_no_grav': + logs.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z'], 0, 0, 0, 0, 0, 0]) + elif name == 'imu_ang_pos': + line = [ind, msg['timestamp']] + R = msg['rot'][0] + R.extend(msg['rot'][1]) + R.extend(msg['rot'][2]) + line.extend(R) + logs.append(line) + elif name == 'imu_ang_vel': + logs.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z'], 0, 0, 0, 0, 0, 0]) + elif name == 'gps': + logs.append([ + ind, msg['timestamp'], + msg['latitude'], msg['longitude'], + msg['antennaAltitude'], 0, 0, 0, 0, 0, 0]) + elif name == 'encoder': + logs.append([ + ind, msg['timestamp'], + msg['distance'], msg['velocity'], 0, 0, 0, 0, 0, 0, 0]) + elif name == 'steering': + logs.append([ + ind, msg['timestamp'], + msg['angle'], 0, 0, 0, 0, 0, 0, 0, 0]) + elif name == 'imu_temp': + logs.append([ + ind, msg['timestamp'], + msg['temperature'], 0, 0, 0, 0, 0, 0, 0, 0]) + + logs = np.array(logs) + print(logs.shape) + vars_map = {'map_names': map_names_nd, 'start_time': start_time, + 'logs': logs} + scipy.io.savemat('./roll_logs_combined.mat', mdict=vars_map) + +else: + imu = [] + imu_ang_pos = [] + imu_ang_vel = [] + gps = [] + encoder = [] + steering = [] + compass = [] + + for msg in messages: + name = msg['topicName'] + name = name[8:] + ind = map_names.index(name) + + if name == 'imu_linear_no_grav': + imu.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z']]) + elif name == 'imu_ang_pos': + line = [ind, msg['timestamp']] + line.extend(msg['rot']) + imu_ang_pos.append(line) + elif name == 'imu_ang_vel': + imu_ang_vel.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z']]) + elif name == 'gps': + gps.append([ + ind, msg['timestamp'], + msg['latitude'], msg['longitude']], + msg['antennaAltitude']) + elif name == 'encoder': + encoder.append([ + ind, msg['timestamp'], + msg['distance'], msg['velocity']]) + elif name == 'steering': + steering.append([ + ind, msg['timestamp'], + msg['angle']]) + elif name == 'imu_temp': + compass.append([ + ind, msg['timestamp'], + msg['temperature']]) + + imu = np.array(imu) + gps = np.array(gps) + encoder = np.array(encoder) + steering = np.array(steering) + compass = np.array(compass) + + vars_map = {'map_names': map_names_nd, 'start_time': start_time, + 'imu': imu, 'imu_ang_pos': imu_ang_pos, + 'imu_ang_vel': imu_ang_vel, 'gps': gps, 'encoder': encoder, + 'steering': steering, 'compass': compass} + scipy.io.savemat('./roll_logs_separate.mat', mdict=vars_map) diff --git a/offline/localizer/latlonutm/Codes/matlab/ll2utm.m b/offline/localizer/latlonutm/Codes/matlab/ll2utm.m new file mode 100644 index 00000000..0e26214b --- /dev/null +++ b/offline/localizer/latlonutm/Codes/matlab/ll2utm.m @@ -0,0 +1,225 @@ +function [x,y,f]=ll2utm(varargin) +%LL2UTM Lat/Lon to UTM coordinates precise conversion. +% [X,Y]=LL2UTM2(LAT,LON) or LL2UTM([LAT,LON]) converts coordinates +% LAT,LON (in degrees) to UTM X and Y (in meters). Default datum is WGS84. +% +% LAT and LON can be scalars, vectors or matrix. Outputs X and Y will +% have the same size as inputs. +% +% LL2UTM(...,DATUM) uses specific DATUM for conversion. DATUM can be one +% of the following char strings: +% 'wgs84': World Geodetic System 1984 (default) +% 'nad27': North American Datum 1927 +% 'clk66': Clarke 1866 +% 'nad83': North American Datum 1983 +% 'grs80': Geodetic Reference System 1980 +% 'int24': International 1924 / Hayford 1909 +% or DATUM can be a 2-element vector [A,F] where A is semimajor axis (in +% meters) and F is flattening of the user-defined ellipsoid. +% +% LL2UTM(...,ZONE) forces the UTM ZONE (scalar integer or same size as +% LAT and LON) instead of automatic set. +% +% [X,Y,ZONE]=LL2UTM(...) returns also the computed UTM ZONE (negative +% value for southern hemisphere points). +% +% +% XY=LL2UTM(...) or without any output argument returns a 2-column +% matrix [X,Y]. +% +% Note: +% - LL2UTM does not perform cross-datum conversion. +% - precision is near a millimeter. +% +% +% Reference: +% I.G.N., Projection cartographique Mercator Transverse: Algorithmes, +% Notes Techniques NT/G 76, janvier 1995. +% +% Acknowledgments: Mathieu, Frederic Christen. +% +% +% Author: Francois Beauducel, +% Created: 2003-12-02 +% Updated: 2015-01-29 + + +% Copyright (c) 2001-2015, François Beauducel, covered by BSD License. +% 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 +% +% 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 OWNER 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. + +% Available datums +datums = [ ... + { 'wgs84', 6378137.0, 298.257223563 }; + { 'nad83', 6378137.0, 298.257222101 }; + { 'grs80', 6378137.0, 298.257222101 }; + { 'nad27', 6378206.4, 294.978698214 }; + { 'int24', 6378388.0, 297.000000000 }; + { 'clk66', 6378206.4, 294.978698214 }; +]; + +% constants +D0 = 180/pi; % conversion rad to deg +K0 = 0.9996; % UTM scale factor +X0 = 500000; % UTM false East (m) + +% defaults +datum = 'wgs84'; +zone = []; + +if nargin < 1 + error('Not enough input arguments.') +end + +if nargin > 1 && isnumeric(varargin{1}) && isnumeric(varargin{2}) && all(size(varargin{1})==size(varargin{2})) + lat = varargin{1}; + lon = varargin{2}; + v = 2; +elseif isnumeric(varargin{1}) && size(varargin{1},2) == 2 + lat = varargin{1}(:,1); + lon = varargin{1}(:,2); + v = 1; +else + error('Single input argument must be a 2-column matrix [LAT,LON].') +end + +if all([numel(lat),numel(lon)] > 1) && any(size(lat) ~= size(lon)) + error('LAT and LON must be the same size or scalars.') +end + +for n = (v+1):nargin + % LL2UTM(...,DATUM) + if ischar(varargin{n}) || (isnumeric(varargin{n}) && numel(varargin{n})==2) + datum = varargin{n}; + % LL2UTM(...,ZONE) + elseif isnumeric(varargin{n}) && (isscalar(varargin{n}) || all(size(varargin{n})==size(lat))) + zone = round(varargin{n}); + else + error('Unknown argument #%d. See documentation.',n) + end +end + +if ischar(datum) + % LL2UTM(...,DATUM) with DATUM as char + if ~any(strcmpi(datum,datums(:,1))) + error('Unkown DATUM name "%s"',datum); + end + k = find(strcmpi(datum,datums(:,1))); + A1 = datums{k,2}; + F1 = datums{k,3}; +else + % LL2UTM(...,DATUM) with DATUM as [A,F] user-defined + A1 = datum(1); + F1 = datum(2); +end + +p1 = lat/D0; % Phi = Latitude (rad) +l1 = lon/D0; % Lambda = Longitude (rad) + +% UTM zone automatic setting +if isempty(zone) + F0 = round((l1*D0 + 183)/6); +else + F0 = zone; +end + +B1 = A1*(1 - 1/F1); +E1 = sqrt((A1*A1 - B1*B1)/(A1*A1)); +P0 = 0/D0; +L0 = (6*F0 - 183)/D0; % UTM origin longitude (rad) +Y0 = 1e7*(p1 < 0); % UTM false northern (m) +N = K0*A1; + +C = coef(E1,0); +B = C(1)*P0 + C(2)*sin(2*P0) + C(3)*sin(4*P0) + C(4)*sin(6*P0) + C(5)*sin(8*P0); +YS = Y0 - N*B; + +C = coef(E1,2); +L = log(tan(pi/4 + p1/2).*(((1 - E1*sin(p1))./(1 + E1*sin(p1))).^(E1/2))); +z = complex(atan(sinh(L)./cos(l1 - L0)),log(tan(pi/4 + asin(sin(l1 - L0)./cosh(L))/2))); +Z = N.*C(1).*z + N.*(C(2)*sin(2*z) + C(3)*sin(4*z) + C(4)*sin(6*z) + C(5)*sin(8*z)); +xs = imag(Z) + X0; +ys = real(Z) + YS; + +% outputs zone if needed: scalar value if unique, or vector/matrix of the +% same size as x/y in case of crossed zones +if nargout > 2 + f = F0.*sign(lat); + fu = unique(f); + if isscalar(fu) + f = fu; + end +end + +if nargout < 2 + x = [xs(:),ys(:)]; +else + x = xs; + y = ys; +end + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function c = coef(e,m) +%COEF Projection coefficients +% COEF(E,M) returns a vector of 5 coefficients from: +% E = first ellipsoid excentricity +% M = 0 for transverse mercator +% M = 1 for transverse mercator reverse coefficients +% M = 2 for merdian arc + + +if nargin < 2 + m = 0; +end + +switch m + case 0 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -105/4096, 0, -45/1024, 0, -3/32, 0, -3/8, 0, 0; + 525/16384, 0, 45/1024, 0, 15/256, 0, 0, 0, 0; + -175/12288, 0, -35/3072, 0, 0, 0, 0, 0, 0; + 315/131072, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 1 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + 1/61440, 0, 7/2048, 0, 1/48, 0, 1/8, 0, 0; + 559/368640, 0, 3/1280, 0, 1/768, 0, 0, 0, 0; + 283/430080, 0, 17/30720, 0, 0, 0, 0, 0, 0; + 4397/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 2 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -901/184320, 0, -9/1024, 0, -1/96, 0, 1/8, 0, 0; + -311/737280, 0, 17/5120, 0, 13/768, 0, 0, 0, 0; + 899/430080, 0, 61/15360, 0, 0, 0, 0, 0, 0; + 49561/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + +end +c = zeros(size(c0,1),1); + +for i = 1:size(c0,1) + c(i) = polyval(c0(i,:),e); +end diff --git a/offline/localizer/latlonutm/Codes/matlab/utm2ll.m b/offline/localizer/latlonutm/Codes/matlab/utm2ll.m new file mode 100644 index 00000000..24057ef0 --- /dev/null +++ b/offline/localizer/latlonutm/Codes/matlab/utm2ll.m @@ -0,0 +1,191 @@ +function [lat,lon]=utm2ll(x,y,f,datum,varargin) +%UTM2LL UTM to Lat/Lon coordinates precise conversion. +% [LAT,LON]=UTM2LL(X,Y,ZONE) converts UTM coordinates X,Y (in meters) +% defined in the UTM ZONE (integer) to latitude LAT and longitude LON +% (in degrees). Default datum is WGS84. +% +% X, Y and F can be scalars, vectors or matrix. Outputs LAT and LON will +% have the same size as inputs. +% +% For southern hemisphere points, use negative zone -ZONE. +% +% UTM2LL(X,Y,ZONE,DATUM) uses specific DATUM for conversion. DATUM can be +% a string in the following list: +% 'wgs84': World Geodetic System 1984 (default) +% 'nad27': North American Datum 1927 +% 'clk66': Clarke 1866 +% 'nad83': North American Datum 1983 +% 'grs80': Geodetic Reference System 1980 +% 'int24': International 1924 / Hayford 1909 +% or DATUM can be a 2-element vector [A,F] where A is semimajor axis (in +% meters) and F is flattening of the user-defined ellipsoid. +% +% Notice: +% - UTM2LL does not perform cross-datum conversion. +% - precision is near a millimeter. +% +% +% Reference: +% I.G.N., Projection cartographique Mercator Transverse: Algorithmes, +% Notes Techniques NT/G 76, janvier 1995. +% +% Author: Francois Beauducel, +% Created: 2001-08-23 +% Updated: 2015-01-29 + + +% Copyright (c) 2001-2015, François Beauducel, covered by BSD License. +% 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 +% +% 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 OWNER 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. + +% Available datums +datums = [ ... + { 'wgs84', 6378137.0, 298.257223563 }; + { 'nad83', 6378137.0, 298.257222101 }; + { 'grs80', 6378137.0, 298.257222101 }; + { 'nad27', 6378206.4, 294.978698214 }; + { 'int24', 6378388.0, 297.000000000 }; + { 'clk66', 6378206.4, 294.978698214 }; +]; + +if nargin < 3 + error('Not enough input arguments.') +end + +if all([numel(x),numel(y)] > 1) && any(size(x) ~= size(y)) + error('X and Y must be the same size or scalars.') +end + +if ~isnumeric(f) || any(f ~= round(f)) || (~isscalar(f) && any(size(f) ~= size(x))) + error('ZONE must be integer value, scalar or same size as X and/or Y.') +end + +if nargin < 4 + datum = 'wgs84'; +end + +if ischar(datum) + if ~any(strcmpi(datum,datums(:,1))) + error('Unkown DATUM name "%s"',datum); + end + k = find(strcmpi(datum,datums(:,1))); + A1 = datums{k,2}; + F1 = datums{k,3}; +else + if numel(datum) ~= 2 + error('User defined DATUM must be a vector [A,F].'); + end + A1 = datum(1); + F1 = datum(2); +end + +% constants +D0 = 180/pi; % conversion rad to deg +maxiter = 100; % maximum iteration for latitude computation +eps = 1e-11; % minimum residue for latitude computation + +K0 = 0.9996; % UTM scale factor +X0 = 500000; % UTM false East (m) +Y0 = 1e7*(f < 0); % UTM false North (m) +P0 = 0; % UTM origin latitude (rad) +L0 = (6*abs(f) - 183)/D0; % UTM origin longitude (rad) +E1 = sqrt((A1^2 - (A1*(1 - 1/F1))^2)/A1^2); % ellpsoid excentricity +N = K0*A1; + +% computing parameters for Mercator Transverse projection +C = coef(E1,0); +YS = Y0 - N*(C(1)*P0 + C(2)*sin(2*P0) + C(3)*sin(4*P0) + C(4)*sin(6*P0) + C(5)*sin(8*P0)); + +C = coef(E1,1); +zt = complex((y - YS)/N/C(1),(x - X0)/N/C(1)); +z = zt - C(2)*sin(2*zt) - C(3)*sin(4*zt) - C(4)*sin(6*zt) - C(5)*sin(8*zt); +L = real(z); +LS = imag(z); + +l = L0 + atan(sinh(LS)./cos(L)); +p = asin(sin(L)./cosh(LS)); + +L = log(tan(pi/4 + p/2)); + +% calculates latitude from the isometric latitude +p = 2*atan(exp(L)) - pi/2; +p0 = NaN; +n = 0; +while any(isnan(p0) | abs(p - p0) > eps) && n < maxiter + p0 = p; + es = E1*sin(p0); + p = 2*atan(((1 + es)./(1 - es)).^(E1/2).*exp(L)) - pi/2; + n = n + 1; +end + +if nargout < 2 + lat = D0*[p(:),l(:)]; +else + lat = p*D0; + lon = l*D0; +end + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function c = coef(e,m) +%COEF Projection coefficients +% COEF(E,M) returns a vector of 5 coefficients from: +% E = first ellipsoid excentricity +% M = 0 for transverse mercator +% M = 1 for transverse mercator reverse coefficients +% M = 2 for merdian arc + + +if nargin < 2 + m = 0; +end + +switch m + case 0 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -105/4096, 0, -45/1024, 0, -3/32, 0, -3/8, 0, 0; + 525/16384, 0, 45/1024, 0, 15/256, 0, 0, 0, 0; + -175/12288, 0, -35/3072, 0, 0, 0, 0, 0, 0; + 315/131072, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 1 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + 1/61440, 0, 7/2048, 0, 1/48, 0, 1/8, 0, 0; + 559/368640, 0, 3/1280, 0, 1/768, 0, 0, 0, 0; + 283/430080, 0, 17/30720, 0, 0, 0, 0, 0, 0; + 4397/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 2 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -901/184320, 0, -9/1024, 0, -1/96, 0, 1/8, 0, 0; + -311/737280, 0, 17/5120, 0, 13/768, 0, 0, 0, 0; + 899/430080, 0, 61/15360, 0, 0, 0, 0, 0, 0; + 49561/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + +end +c = zeros(size(c0,1),1); + +for i = 1:size(c0,1) + c(i) = polyval(c0(i,:),e); +end diff --git a/offline/localizer/latlonutm/license.txt b/offline/localizer/latlonutm/license.txt new file mode 100644 index 00000000..0355b586 --- /dev/null +++ b/offline/localizer/latlonutm/license.txt @@ -0,0 +1,24 @@ +Copyright (c) 2015, François Beauducel +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 + +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 OWNER 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. diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m new file mode 100644 index 00000000..f93f2fbd --- /dev/null +++ b/offline/localizer/localizer.m @@ -0,0 +1,190 @@ +function [trajectory] = localizer() + % localizer code, as seen in java codebase + % Bereket Abraham + + % TODO: compare to raw latlon, granular position truth? + % TODO: UNITS, use IMU body accel + + addpath('./latlonutm/Codes/matlab'); + + global wheel_base + global last_gps + global last_encoder + global last_encoder_time + global last_time + global R + global steeringAngle + global first_gps + global first_theta + save_data = true; + + % constants + ref_dt = (1/10) * 1000; % 10 Hz in ms + % dt of the controller, not the filter + wheel_base = 1.13; + utm_zone = 17; + first_theta = deg2rad(250); + lat_long = [40.441670, -79.9416362]; + + [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); + first_gps = [x y]; + last_gps = [x y]; + utm_zone = zone; % fixed + last_encoder = 0; + steeringAngle = 0; + + % output matrices + C_encoder = [0 0 1 0 0]; + C_gps = [1 0 0 0 0; + 0 1 0 0 0; + 0 0 0 1 0]; + + % measurement noise covariance + R = diag([4, 4, 0.25, 0.01, 0.01]); + % model noise covariance + Q_gps = diag([4, 4, 0.02]); + Q_encoder = [0.25]; + % initialize Kalman filter + % covariance matrix + P = diag([25, 25, 0.25, 2.46, 2.46]); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + 0; % d_Yb, body velocity, m/s + first_theta; % heading, deg, global + 0]; % d_heading, deg/s + + trajectory = [X; lat_long'; norm(P)]; + + load('./roll_logs_combined.mat'); + start_time = double(start_time); + last_encoder_time = start_time; + last_time = start_time; + + for i = 1:size(logs, 1) + llog = logs(i, :); + name = map_names{llog(1) + 1}; + time = llog(2); + + if strcmp(name, 'gps') + [z] = gps_meaurement(llog(3), llog(4), X(4)); + C = C_gps; + Q = Q_gps; + elseif strcmp(name,'encoder') + [z] = encoder_meaurement(llog(3), time); + C = C_encoder; + Q = Q_encoder; + if isempty(z) + continue; + end + elseif strcmp(name, 'steering') + steeringAngle = deg2rad(llog(3)); + continue; + else + continue; + end + + [P_pre, X_pre] = predict_step(P, X, time); + [P, X] = update_step(C, Q, z, P_pre, X_pre); + snapshot = summarize(P, X, utm_zone); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('localizer_v4.mat', 'trajectory', 'P'); + end +end + +function snapshot = summarize(P, x, utm_zone) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; lat; lon; norm(P)]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +% generate dynamic model +function [A] = model(x, time) + global wheel_base + global last_time + global steeringAngle + + dt = (time - last_time) / 1000.0; + last_time = time; + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steeringAngle)/wheel_base, 0, 0]; +end + +function [P_pre, x_pre] = predict_step(P, x, time) + global R + A = model(x, time); + x_pre = A * x; + P_pre = (A * P * A') + R; + + x_pre(4) = clampAngle(x_pre(4)); + x_pre(5) = clampAngle(x_pre(5)); +end + +function [P, x] = update_step(C, Q, z, P_pre, x_pre) + residual = z - (C * x_pre); + K = P_pre * C' * inv((C * P_pre * C') + Q); % gain + x = x_pre + (K * residual); + P = (eye(5) - (K * C)) * P_pre; + + x(4) = clampAngle(x(4)); + x(5) = clampAngle(x(5)); +end + +function [z] = gps_meaurement(lat, long, last_heading) + global last_gps + global first_gps + global first_theta + + [x, y, ~] = ll2utm(lat, long); + dx = x - last_gps(1); + dy = y - last_gps(2); + last_gps = [x y]; + heading = atan2(dy, dx); + + % ignore small strides + if ((dx*dx + dy*dy) < 0.25) + heading = last_heading; + end + % close the loop + d = abs(x - first_gps(1)) + abs(y - first_gps(2)); + if (d < 10.0) + heading = first_theta; + end + + z = [x; y; heading]; +end + +function [z] = encoder_meaurement(encoder_dist, encoder_time) + global last_encoder + global last_encoder_time + + dt = encoder_time - last_encoder_time; + + if (dt < 10) + z = []; + else + dx = encoder_dist - last_encoder; + last_encoder = encoder_dist; + last_encoder_time = encoder_time; + bodySpeed = dx / (dt / 1000.0); + z = [bodySpeed]; + end +end + + + diff --git a/offline/localizer/plot_sensor.m b/offline/localizer/plot_sensor.m new file mode 100644 index 00000000..f31718be --- /dev/null +++ b/offline/localizer/plot_sensor.m @@ -0,0 +1,36 @@ +clear; +close all; + +load('./roll_logs_combined.mat'); +start_time = double(start_time); + +% ['imu_linear_no_grav', = 0 linear accel +% 'imu_ang_pos', = 1 angle position +% 'imu_ang_vel', = 2 angular velocity +% 'gps', = 3 +% 'encoder', = 4 encoder distance in m +% 'steering', = 5 steering wheel angle +% 'imu_temp'] = 6 compass angle, bugs + +% imu_linear_acc = logs(logs(:,1) == 0, :); +% time = (imu_linear_acc(:, 2) - start_time) / 1000.0; +% figure(); +% plot(time, imu_linear_acc(:, 3), 'r'); +% figure(); +% plot(time, imu_linear_acc(:, 4), 'b'); + + +% imu_ang_pos = logs(logs(:,1) == 1, :); +% time2 = (imu_ang_pos(:, 2) - start_time) / 1000.0; +% r21 = imu_ang_pos(:, 6); +% r11 = imu_ang_pos(:, 3); +% yaw = atan2(r21, r11); +% figure(); +% plot(time2, yaw, 'r'); + +% load('localizer_v3.mat', 'trajectory'); + +gps = logs(logs(:,1) == 3, :); +time3 = (gps(:, 2) - start_time) / 1000.0; +wmline(gps(:, 3), gps(:, 4), 'Color', 'r') + diff --git a/offline/localizer/trajectory_analysis.m b/offline/localizer/trajectory_analysis.m new file mode 100644 index 00000000..03c81259 --- /dev/null +++ b/offline/localizer/trajectory_analysis.m @@ -0,0 +1,42 @@ +% graph results + +clear; +close all; + +addpath('./latlonutm/Codes/matlab'); +% addpath('./zoharby-plot_google_map'); +addpath('./altmany-export_fig'); + +file = 'localizer_v4.mat'; +load(file, 'trajectory', 'P'); +save_plot = false; + +k = 100; +wmline(trajectory(6,1:k:end), trajectory(7,1:k:end), 'Color', 'r') + +t1 = 1476008235676; +t2 = 1476009019782; +t = linspace(0, (t2 - t1)/1000, size(trajectory,2)); +figure(); +plot(t, trajectory(8,:)) +title(['Uncertainty ', file]); + +% plot(trajectory(8,:), trajectory(9,:), '.r', 'MarkerSize', 20) +% plot_google_map('maptype','roadmap', 'APIKey','AIzaSyBYLaDqr2QKRCCRAHhrOf21DVJL-kkkyr8') + +if save_plot + set(gcf,'renderer','zbuffer') + export_fig('out.jpg') +end + +heading = trajectory(4,1:k:end); +figure(); +hold on; +plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) +quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(heading), sin(heading)) +hold off; +title(['Heading ', file]); + +% plot on google maps +ss = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; +fprintf(1, '%5.20f, %5.20f\n', ss); diff --git a/offline/localizer/zoharby-plot_google_map/.gitattributes b/offline/localizer/zoharby-plot_google_map/.gitattributes new file mode 100755 index 00000000..412eeda7 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/.gitattributes @@ -0,0 +1,22 @@ +# Auto detect text files and perform LF normalization +* text=auto + +# Custom for Visual Studio +*.cs diff=csharp +*.sln merge=union +*.csproj merge=union +*.vbproj merge=union +*.fsproj merge=union +*.dbproj merge=union + +# Standard to msysgit +*.doc diff=astextplain +*.DOC diff=astextplain +*.docx diff=astextplain +*.DOCX diff=astextplain +*.dot diff=astextplain +*.DOT diff=astextplain +*.pdf diff=astextplain +*.PDF diff=astextplain +*.rtf diff=astextplain +*.RTF diff=astextplain diff --git a/offline/localizer/zoharby-plot_google_map/.gitignore b/offline/localizer/zoharby-plot_google_map/.gitignore new file mode 100755 index 00000000..b9d6bd92 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/.gitignore @@ -0,0 +1,215 @@ +################# +## Eclipse +################# + +*.pydevproject +.project +.metadata +bin/ +tmp/ +*.tmp +*.bak +*.swp +*~.nib +local.properties +.classpath +.settings/ +.loadpath + +# External tool builders +.externalToolBuilders/ + +# Locally stored "Eclipse launch configurations" +*.launch + +# CDT-specific +.cproject + +# PDT-specific +.buildpath + + +################# +## Visual Studio +################# + +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. + +# User-specific files +*.suo +*.user +*.sln.docstates + +# Build results + +[Dd]ebug/ +[Rr]elease/ +x64/ +build/ +[Bb]in/ +[Oo]bj/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +*_i.c +*_p.c +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*.log +*.vspscc +*.vssscc +.builds +*.pidb +*.log +*.scc + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opensdf +*.sdf +*.cachefile + +# Visual Studio profiler +*.psess +*.vsp +*.vspx + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# NCrunch +*.ncrunch* +.*crunch*.local.xml + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.Publish.xml +*.pubxml + +# NuGet Packages Directory +## TODO: If you have NuGet Package Restore enabled, uncomment the next line +#packages/ + +# Windows Azure Build Output +csx +*.build.csdef + +# Windows Store app package directory +AppPackages/ + +# Others +sql/ +*.Cache +ClientBin/ +[Ss]tyle[Cc]op.* +~$* +*~ +*.dbmdl +*.[Pp]ublish.xml +*.pfx +*.publishsettings + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file to a newer +# Visual Studio version. Backup files are not needed, because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm + +# SQL Server files +App_Data/*.mdf +App_Data/*.ldf + +############# +## Windows detritus +############# + +# Windows image file caches +Thumbs.db +ehthumbs.db + +# Folder config file +Desktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Mac crap +.DS_Store + + +############# +## Python +############# + +*.py[co] + +# Packages +*.egg +*.egg-info +dist/ +build/ +eggs/ +parts/ +var/ +sdist/ +develop-eggs/ +.installed.cfg + +# Installer logs +pip-log.txt + +# Unit test / coverage reports +.coverage +.tox + +#Translations +*.mo + +#Mr Developer +.mr.developer.cfg diff --git a/offline/localizer/zoharby-plot_google_map/README.md b/offline/localizer/zoharby-plot_google_map/README.md new file mode 100755 index 00000000..8c2a5e16 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/README.md @@ -0,0 +1,26 @@ +# plot_google_map +#### MATLAB function for plotting a Google map on the background of a figure + +plot_google_map.m uses the Google Maps API to plot a map in the background of the current figure. +It assumes the coordinates of the current figure are in the WGS84 datum, and uses a conversion code to convert and project the image from the coordinate system used by Google into WGS84 coordinates. +The zoom level of the map is automatically determined to cover the entire area of the figure. Additionally, it has the option to auto-refresh the map upon zooming in the figure, revealing more details as one zooms in. +The following code produces the screenshot: + +``` +lat = [48.8708 51.5188 41.9260 40.4312 52.523 37.982]; +lon = [2.4131 -0.1300 12.4951 -3.6788 13.415 23.715]; +plot(lon,lat,'.r','MarkerSize',20) +plot_google_map +``` + +Known Issues: + 1. The static maps API is limited to 1000 requests per day when used with no API key. If you use this function a lot, you can obtain an API key and easily set the function to use it (see help for more details) + + 2. Saving the map with an image/matrix overlay drawn on top of it (especially a semi-transparent one) can sometimes cause unexpected results (map not showing etc.). If you're encountering such problems, it's recommended to use the export_fig submission: + http://www.mathworks.com/matlabcentral/fileexchange/23629-exportfig + The combination that seems to work best: + ``` + set(gcf,'renderer','zbuffer') + export_fig('out.jpg') + ``` + diff --git a/offline/localizer/zoharby-plot_google_map/license.txt b/offline/localizer/zoharby-plot_google_map/license.txt new file mode 100755 index 00000000..52dbe68f --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/license.txt @@ -0,0 +1,24 @@ +Copyright (c) 2015, Zohar Bar-Yehuda +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 + +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 OWNER 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. diff --git a/offline/localizer/zoharby-plot_google_map/plot_google_map.m b/offline/localizer/zoharby-plot_google_map/plot_google_map.m new file mode 100755 index 00000000..645392a6 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/plot_google_map.m @@ -0,0 +1,647 @@ +function varargout = plot_google_map(varargin) +% function h = plot_google_map(varargin) +% Plots a google map on the current axes using the Google Static Maps API +% +% USAGE: +% h = plot_google_map(Property, Value,...) +% Plots the map on the given axes. Used also if no output is specified +% +% Or: +% [lonVec latVec imag] = plot_google_map(Property, Value,...) +% Returns the map without plotting it +% +% PROPERTIES: +% Axis - Axis handle. If not given, gca is used. +% Height (640) - Height of the image in pixels (max 640) +% Width (640) - Width of the image in pixels (max 640) +% Scale (2) - (1/2) Resolution scale factor. Using Scale=2 will +% double the resulotion of the downloaded image (up +% to 1280x1280) and will result in finer rendering, +% but processing time will be longer. +% Resize (1) - (recommended 1-2) Resolution upsampling factor. +% Increases image resolution using imresize(). This results +% in a finer image but it needs the image processing +% toolbox and processing time will be longer. +% MapType - ('roadmap') Type of map to return. Any of [roadmap, +% satellite, terrain, hybrid]. See the Google Maps API for +% more information. +% Alpha (1) - (0-1) Transparency level of the map (0 is fully +% transparent). While the map is always moved to the +% bottom of the plot (i.e. will not hide previously +% drawn items), this can be useful in order to increase +% readability if many colors are plotted +% (using SCATTER for example). +% ShowLabels (1) - (0/1) Controls whether to display city/street textual labels on the map +% Style - (string) A style configuration string. See: +% https://developers.google.com/maps/documentation/static-maps/?csw=1#StyledMaps +% http://instrument.github.io/styled-maps-wizard/ +% Language - (string) A 2 letter ISO 639-1 language code for displaying labels in a +% local language instead of English (where available). +% For example, for Chinese use: +% plot_google_map('language','zh') +% For the list of codes, see: +% http://en.wikipedia.org/wiki/List_of_ISO_639-1_codes +% Marker - The marker argument is a text string with fields +% conforming to the Google Maps API. The +% following are valid examples: +% '43.0738740,-70.713993' (default midsize orange marker) +% '43.0738740,-70.713993,blue' (midsize blue marker) +% '43.0738740,-70.713993,yellowa' (midsize yellow +% marker with label "A") +% '43.0738740,-70.713993,tinyredb' (tiny red marker +% with label "B") +% Refresh (1) - (0/1) defines whether to automatically refresh the +% map upon zoom/pan action on the figure. +% AutoAxis (1) - (0/1) defines whether to automatically adjust the axis +% of the plot to avoid the map being stretched. +% This will adjust the span to be correct +% according to the shape of the map axes. +% FigureResizeUpdate (1) - (0/1) defines whether to automatically refresh the +% map upon resizing the figure. This will ensure map +% isn't stretched after figure resize. +% APIKey - (string) set your own API key which you obtained from Google: +% http://developers.google.com/maps/documentation/staticmaps/#api_key +% This will enable up to 25,000 map requests per day, +% compared to a few hundred requests without a key. +% To set the key, use: +% plot_google_map('APIKey','SomeLongStringObtaindFromGoogle') +% You need to do this only once to set the key. +% To disable the use of a key, use: +% plot_google_map('APIKey','') +% +% OUTPUT: +% h - Handle to the plotted map +% +% lonVect - Vector of Longidute coordinates (WGS84) of the image +% latVect - Vector of Latidute coordinates (WGS84) of the image +% imag - Image matrix (height,width,3) of the map +% +% EXAMPLE - plot a map showing some capitals in Europe: +% lat = [48.8708 51.5188 41.9260 40.4312 52.523 37.982]; +% lon = [2.4131 -0.1300 12.4951 -3.6788 13.415 23.715]; +% plot(lon,lat,'.r','MarkerSize',20) +% plot_google_map +% +% References: +% http://www.mathworks.com/matlabcentral/fileexchange/24113 +% http://www.maptiler.org/google-maps-coordinates-tile-bounds-projection/ +% http://developers.google.com/maps/documentation/staticmaps/ +% +% Acknowledgements: +% Val Schmidt for his submission of get_google_map.m +% +% Author: +% Zohar Bar-Yehuda +% +% Version 1.8 - 25/04/2016 - By Hannes Diethelm +% - Add resize parameter to resize image using imresize() +% - Fix scale parameter +% Version 1.7 - 14/04/2016 +% - Add custom style support +% Version 1.6 - 12/11/2015 +% - Use system temp folder for writing image files (with fallback to current dir if missing write permissions) +% Version 1.5 - 20/11/2014 +% - Support for MATLAB R2014b +% - several fixes for complex layouts: several maps in one figure, +% map inside a panel, specifying axis handle as input (thanks to Luke Plausin) +% Version 1.4 - 25/03/2014 +% - Added the language parameter for showing labels in a local language +% - Display the URL on error to allow easier debugging of API errors +% Version 1.3 - 06/10/2013 +% - Improved functionality of AutoAxis, which now handles any shape of map axes. +% Now also updates the extent of the map if the figure is resized. +% - Added the showLabels parameter which allows hiding the textual labels on the map. +% Version 1.2 - 16/06/2012 +% - Support use of the "scale=2" parameter by default for finer rendering (set scale=1 if too slow). +% - Auto-adjust axis extent so the map isn't stretched. +% - Set and use an API key which enables a much higher usage volume per day. +% Version 1.1 - 25/08/2011 + +persistent apiKey useTemp +if isnumeric(apiKey) + % first run, check if API key file exists + if exist('api_key.mat','file') + load api_key + else + apiKey = ''; + end +end + +if isempty(useTemp) + % first run, check we we have wrtie access to the temp folder + try + tempfilename = tempname; + fid = fopen(tempfilename, 'w'); + if fid > 0 + fclose(fid); + useTemp = true; + delete(tempfilename); + else + % Don't have write access to temp folder or it doesn't exist, fallback to current dir + useTemp = false; + end + catch + % in case tempname fails for some reason + useTemp = false; + end +end + +hold on + +% Default parametrs +axHandle = gca; +height = 640; +width = 640; +scale = 2; +resize = 1; +maptype = 'roadmap'; +alphaData = 1; +autoRefresh = 1; +figureResizeUpdate = 1; +autoAxis = 1; +showLabels = 1; +language = ''; +markeridx = 1; +markerlist = {}; +style = ''; + +% Handle input arguments +if nargin >= 2 + for idx = 1:2:length(varargin) + switch lower(varargin{idx}) + case 'axis' + axHandle = varargin{idx+1}; + case 'height' + height = varargin{idx+1}; + case 'width' + width = varargin{idx+1}; + case 'scale' + scale = round(varargin{idx+1}); + if scale < 1 || scale > 2 + error('Scale must be 1 or 2'); + end + case 'resize' + resize = varargin{idx+1}; + case 'maptype' + maptype = varargin{idx+1}; + case 'alpha' + alphaData = varargin{idx+1}; + case 'refresh' + autoRefresh = varargin{idx+1}; + case 'showlabels' + showLabels = varargin{idx+1}; + case 'figureresizeupdate' + figureResizeUpdate = varargin{idx+1}; + case 'language' + language = varargin{idx+1}; + case 'marker' + markerlist{markeridx} = varargin{idx+1}; + markeridx = markeridx + 1; + case 'autoaxis' + autoAxis = varargin{idx+1}; + case 'apikey' + apiKey = varargin{idx+1}; % set new key + % save key to file + funcFile = which('plot_google_map.m'); + pth = fileparts(funcFile); + keyFile = fullfile(pth,'api_key.mat'); + save(keyFile,'apiKey') + case 'style' + style = varargin{idx+1}; + otherwise + error(['Unrecognized variable: ' varargin{idx}]) + end + end +end +if height > 640 + height = 640; +end +if width > 640 + width = 640; +end + +% Store paramters in axis handle (for auto refresh callbacks) +ud = get(axHandle, 'UserData'); +if isempty(ud) + % explicitly set as struct to avoid warnings + ud = struct; +end +ud.gmap_params = varargin; +set(axHandle, 'UserData', ud); + +curAxis = axis(axHandle); +if max(abs(curAxis)) > 500 + return; +end + +% Enforce Latitude constraints of EPSG:900913 +if curAxis(3) < -85 + curAxis(3) = -85; +end +if curAxis(4) > 85 + curAxis(4) = 85; +end +% Enforce longitude constrains +if curAxis(1) < -180 + curAxis(1) = -180; +end +if curAxis(1) > 180 + curAxis(1) = 0; +end +if curAxis(2) > 180 + curAxis(2) = 180; +end +if curAxis(2) < -180 + curAxis(2) = 0; +end + +if isequal(curAxis,[0 1 0 1]) % probably an empty figure + % display world map + curAxis = [-200 200 -85 85]; + axis(curAxis) +end + + +if autoAxis + % adjust current axis limit to avoid strectched maps + [xExtent,yExtent] = latLonToMeters(curAxis(3:4), curAxis(1:2) ); + xExtent = diff(xExtent); % just the size of the span + yExtent = diff(yExtent); + % get axes aspect ratio + drawnow + org_units = get(axHandle,'Units'); + set(axHandle,'Units','Pixels') + ax_position = get(axHandle,'position'); + set(axHandle,'Units',org_units) + aspect_ratio = ax_position(4) / ax_position(3); + + if xExtent*aspect_ratio > yExtent + centerX = mean(curAxis(1:2)); + centerY = mean(curAxis(3:4)); + spanX = (curAxis(2)-curAxis(1))/2; + spanY = (curAxis(4)-curAxis(3))/2; + + % enlarge the Y extent + spanY = spanY*xExtent*aspect_ratio/yExtent; % new span + if spanY > 85 + spanX = spanX * 85 / spanY; + spanY = spanY * 85 / spanY; + end + curAxis(1) = centerX-spanX; + curAxis(2) = centerX+spanX; + curAxis(3) = centerY-spanY; + curAxis(4) = centerY+spanY; + elseif yExtent > xExtent*aspect_ratio + + centerX = mean(curAxis(1:2)); + centerY = mean(curAxis(3:4)); + spanX = (curAxis(2)-curAxis(1))/2; + spanY = (curAxis(4)-curAxis(3))/2; + % enlarge the X extent + spanX = spanX*yExtent/(xExtent*aspect_ratio); % new span + if spanX > 180 + spanY = spanY * 180 / spanX; + spanX = spanX * 180 / spanX; + end + + curAxis(1) = centerX-spanX; + curAxis(2) = centerX+spanX; + curAxis(3) = centerY-spanY; + curAxis(4) = centerY+spanY; + end + % Enforce Latitude constraints of EPSG:900913 + if curAxis(3) < -85 + curAxis(3:4) = curAxis(3:4) + (-85 - curAxis(3)); + end + if curAxis(4) > 85 + curAxis(3:4) = curAxis(3:4) + (85 - curAxis(4)); + end + axis(axHandle, curAxis); % update axis as quickly as possible, before downloading new image + drawnow +end + +% Delete previous map from plot (if exists) +if nargout <= 1 % only if in plotting mode + curChildren = get(axHandle,'children'); + map_objs = findobj(curChildren,'tag','gmap'); + bd_callback = []; + for idx = 1:length(map_objs) + if ~isempty(get(map_objs(idx),'ButtonDownFcn')) + % copy callback properties from current map + bd_callback = get(map_objs(idx),'ButtonDownFcn'); + end + end + delete(map_objs) + +end + +% Calculate zoom level for current axis limits +[xExtent,yExtent] = latLonToMeters(curAxis(3:4), curAxis(1:2) ); +minResX = diff(xExtent) / width; +minResY = diff(yExtent) / height; +minRes = max([minResX minResY]); +tileSize = 256; +initialResolution = 2 * pi * 6378137 / tileSize; % 156543.03392804062 for tileSize 256 pixels +zoomlevel = floor(log2(initialResolution/minRes)); + +% Enforce valid zoom levels +if zoomlevel < 0 + zoomlevel = 0; +end +if zoomlevel > 19 + zoomlevel = 19; +end + +% Calculate center coordinate in WGS1984 +lat = (curAxis(3)+curAxis(4))/2; +lon = (curAxis(1)+curAxis(2))/2; + +% Construct query URL +preamble = 'http://maps.googleapis.com/maps/api/staticmap'; +location = ['?center=' num2str(lat,10) ',' num2str(lon,10)]; +zoomStr = ['&zoom=' num2str(zoomlevel)]; +sizeStr = ['&scale=' num2str(scale) '&size=' num2str(width) 'x' num2str(height)]; +maptypeStr = ['&maptype=' maptype ]; +if ~isempty(apiKey) + keyStr = ['&key=' apiKey]; +else + keyStr = ''; +end +markers = '&markers='; +for idx = 1:length(markerlist) + if idx < length(markerlist) + markers = [markers markerlist{idx} '%7C']; + else + markers = [markers markerlist{idx}]; + end +end + +if showLabels == 0 + if ~isempty(style) + style(end+1) = '|'; + end + style = [style 'feature:all|element:labels|visibility:off']; +end + +if ~isempty(language) + languageStr = ['&language=' language]; +else + languageStr = ''; +end + +if ismember(maptype,{'satellite','hybrid'}) + filename = 'tmp.jpg'; + format = '&format=jpg'; + convertNeeded = 0; +else + filename = 'tmp.png'; + format = '&format=png'; + convertNeeded = 1; +end +sensor = '&sensor=false'; + +if ~isempty(style) + styleStr = ['&style=' style]; +else + styleStr = ''; +end + +url = [preamble location zoomStr sizeStr maptypeStr format markers languageStr sensor keyStr styleStr]; + +% Get the image +if useTemp + filepath = fullfile(tempdir, filename); +else + filepath = filename; +end + +try + urlwrite(url,filepath); +catch % error downloading map + warning(['Unable to download map form Google Servers.\n' ... + 'Matlab error was: %s\n\n' ... + 'Possible reasons: missing write permissions, no network connection, quota exceeded, or some other error.\n' ... + 'Consider using an API key if quota problems persist.\n\n' ... + 'To debug, try pasting the following URL in your browser, which may result in a more informative error:\n%s'], lasterr, url); + varargout{1} = []; + varargout{2} = []; + varargout{3} = []; + return +end +[M Mcolor] = imread(filepath); +M = cast(M,'double'); +delete(filepath); % delete temp file +width = size(M,2); +height = size(M,1); + +% We now want to convert the image from a colormap image with an uneven +% mesh grid, into an RGB truecolor image with a uniform grid. +% This would enable displaying it with IMAGE, instead of PCOLOR. +% Advantages are: +% 1) faster rendering +% 2) makes it possible to display together with other colormap annotations (PCOLOR, SCATTER etc.) + +% Convert image from colormap type to RGB truecolor (if PNG is used) +if convertNeeded + imag = zeros(height,width,3); + for idx = 1:3 + imag(:,:,idx) = reshape(Mcolor(M(:)+1+(idx-1)*size(Mcolor,1)),height,width); + end +else + imag = M/255; +end +% Resize if needed +if resize ~= 1 + imag = imresize(imag, resize, 'bilinear'); +end + +% Calculate a meshgrid of pixel coordinates in EPSG:900913 +width = size(imag,2); +height = size(imag,1); +centerPixelY = round(height/2); +centerPixelX = round(width/2); +[centerX,centerY] = latLonToMeters(lat, lon ); % center coordinates in EPSG:900913 +curResolution = initialResolution / 2^zoomlevel / scale / resize; % meters/pixel (EPSG:900913) +xVec = centerX + ((1:width)-centerPixelX) * curResolution; % x vector +yVec = centerY + ((height:-1:1)-centerPixelY) * curResolution; % y vector +[xMesh,yMesh] = meshgrid(xVec,yVec); % construct meshgrid + +% convert meshgrid to WGS1984 +[lonMesh,latMesh] = metersToLatLon(xMesh,yMesh); + +% Next, project the data into a uniform WGS1984 grid +uniHeight = round(height*resize); +uniWidth = round(width*resize); +latVect = linspace(latMesh(1,1),latMesh(end,1),uniHeight); +lonVect = linspace(lonMesh(1,1),lonMesh(1,end),uniWidth); +[uniLonMesh,uniLatMesh] = meshgrid(lonVect,latVect); +uniImag = zeros(uniHeight,uniWidth,3); + +% old version (projection using INTERP2) +% for idx = 1:3 +% % 'nearest' method is the fastest. difference from other methods is neglible +% uniImag(:,:,idx) = interp2(lonMesh,latMesh,imag(:,:,idx),uniLonMesh,uniLatMesh,'nearest'); +% end +uniImag = myTurboInterp2(lonMesh,latMesh,imag,uniLonMesh,uniLatMesh); + +if nargout <= 1 % plot map + % display image + hold(axHandle, 'on'); + cax = caxis; + h = image(lonVect,latVect,uniImag, 'Parent', axHandle); + caxis(cax); % Preserve caxis that is sometimes changed by the call to image() + set(axHandle,'YDir','Normal') + set(h,'tag','gmap') + set(h,'AlphaData',alphaData) + + % add a dummy image to allow pan/zoom out to x2 of the image extent + h_tmp = image(lonVect([1 end]),latVect([1 end]),zeros(2),'Visible','off', 'Parent', axHandle); + set(h_tmp,'tag','gmap') + + % older version (display without conversion to uniform grid) + % h =pcolor(lonMesh,latMesh,(M)); + % colormap(Mcolor) + % caxis([0 255]) + % warning off % to avoid strange rendering warnings + % shading flat + + uistack(h,'bottom') % move map to bottom (so it doesn't hide previously drawn annotations) + axis(axHandle, curAxis) % restore original zoom + if nargout == 1 + varargout{1} = h; + end + + % if auto-refresh mode - override zoom callback to allow autumatic + % refresh of map upon zoom actions. + figHandle = axHandle; + while ~strcmpi(get(figHandle, 'Type'), 'figure') + % Recursively search for parent figure in case axes are in a panel + figHandle = get(figHandle, 'Parent'); + end + + zoomHandle = zoom(axHandle); + panHandle = pan(figHandle); % This isn't ideal, doesn't work for contained axis + if autoRefresh + set(zoomHandle,'ActionPostCallback',@update_google_map); + set(panHandle, 'ActionPostCallback', @update_google_map); + else % disable zoom override + set(zoomHandle,'ActionPostCallback',[]); + set(panHandle, 'ActionPostCallback',[]); + end + + % set callback for figure resize function, to update extents if figure + % is streched. + if figureResizeUpdate &&isempty(get(figHandle, 'ResizeFcn')) + % set only if not already set by someone else + set(figHandle, 'ResizeFcn', @update_google_map_fig); + end + + % set callback properties + set(h,'ButtonDownFcn',bd_callback); +else % don't plot, only return map + varargout{1} = lonVect; + varargout{2} = latVect; + varargout{3} = uniImag; +end + + +% Coordinate transformation functions + +function [lon,lat] = metersToLatLon(x,y) +% Converts XY point from Spherical Mercator EPSG:900913 to lat/lon in WGS84 Datum +originShift = 2 * pi * 6378137 / 2.0; % 20037508.342789244 +lon = (x ./ originShift) * 180; +lat = (y ./ originShift) * 180; +lat = 180 / pi * (2 * atan( exp( lat * pi / 180)) - pi / 2); + +function [x,y] = latLonToMeters(lat, lon ) +% Converts given lat/lon in WGS84 Datum to XY in Spherical Mercator EPSG:900913" +originShift = 2 * pi * 6378137 / 2.0; % 20037508.342789244 +x = lon * originShift / 180; +y = log(tan((90 + lat) * pi / 360 )) / (pi / 180); +y = y * originShift / 180; + + +function ZI = myTurboInterp2(X,Y,Z,XI,YI) +% An extremely fast nearest neighbour 2D interpolation, assuming both input +% and output grids consist only of squares, meaning: +% - uniform X for each column +% - uniform Y for each row +XI = XI(1,:); +X = X(1,:); +YI = YI(:,1); +Y = Y(:,1); + +xiPos = nan*ones(size(XI)); +xLen = length(X); +yiPos = nan*ones(size(YI)); +yLen = length(Y); +% find x conversion +xPos = 1; +for idx = 1:length(xiPos) + if XI(idx) >= X(1) && XI(idx) <= X(end) + while xPos < xLen && X(xPos+1)= Y(end) + while yPos < yLen && Y(yPos+1)>YI(idx) + yPos = yPos + 1; + end + diffs = abs(Y(yPos:yPos+1)-YI(idx)); + if diffs(1) < diffs(2) + yiPos(idx) = yPos; + else + yiPos(idx) = yPos + 1; + end + end +end +ZI = Z(yiPos,xiPos,:); + + +function update_google_map(obj,evd) +% callback function for auto-refresh +drawnow; +try + axHandle = evd.Axes; +catch ex + % Event doesn't contain the correct axes. Panic! + axHandle = gca; +end +ud = get(axHandle, 'UserData'); +if isfield(ud, 'gmap_params') + params = ud.gmap_params; + plot_google_map(params{:}); +end + + +function update_google_map_fig(obj,evd) +% callback function for auto-refresh +drawnow; +axes_objs = findobj(get(gcf,'children'),'type','axes'); +for idx = 1:length(axes_objs) + if ~isempty(findobj(get(axes_objs(idx),'children'),'tag','gmap')); + ud = get(axes_objs(idx), 'UserData'); + if isfield(ud, 'gmap_params') + params = ud.gmap_params; + else + params = {}; + end + + % Add axes to inputs if needed + if ~sum(strcmpi(params, 'Axis')) + params = [params, {'Axis', axes_objs(idx)}]; + end + plot_google_map(params{:}); + end +end diff --git a/real_time/surface_src/java_src/Alice/build.gradle b/real_time/surface_src/java_src/Alice/build.gradle index 8cd833b7..84aa75eb 100644 --- a/real_time/surface_src/java_src/Alice/build.gradle +++ b/real_time/surface_src/java_src/Alice/build.gradle @@ -42,8 +42,9 @@ dependencies { compile 'org.jcodec:jcodec-javase:0.1.9' compile project (':BuggyRos') - //for jason object extraction from strings - compile 'com.google.code.gson:gson:2.2.+' + //for json object extraction from strings + + compile group: 'com.google.code.gson', name: 'gson', version: '2.2.+' compile 'jfreechart:jfreechart:1.0.0' compile 'gov.nist.math:jama:1.0.2' compile 'org.eclipse.jetty.aggregate:jetty-all:9.2.7.v20150116' diff --git a/real_time/surface_src/java_src/Alice/images/.lat_long_course_map.png.icloud b/real_time/surface_src/java_src/Alice/images/.lat_long_course_map.png.icloud new file mode 100644 index 00000000..5c034483 Binary files /dev/null and b/real_time/surface_src/java_src/Alice/images/.lat_long_course_map.png.icloud differ diff --git a/real_time/surface_src/java_src/Alice/images/lat_long_course_map.png b/real_time/surface_src/java_src/Alice/images/lat_long_course_map.png deleted file mode 100644 index d991173a..00000000 Binary files a/real_time/surface_src/java_src/Alice/images/lat_long_course_map.png and /dev/null differ diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java index c894d38b..02e136df 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java @@ -32,6 +32,8 @@ public final class RobobuggyConfigFile { public static final boolean LOGGING = true; public static final String LOG_FILE_LOCATION = "logs"; public static final String LOG_FILE_NAME = "sensors"; + public static final double INITIAL_POS_LAT = 40.441670; + public static final double INITIAL_POS_LON = -79.9416362; //Autonomous controls public static final int RBSM_COMMAND_PERIOD = 50; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 9b7ca14e..a711fda0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,8 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.TransistorDataCollection; +import com.roboclub.robobuggy.robots.SimRobot; +import com.roboclub.robobuggy.robots.TransistorAuton; import com.roboclub.robobuggy.ui.Gui; @@ -29,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = TransistorDataCollection.getInstance(); + robot = TransistorAuton.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); @@ -58,11 +59,6 @@ public static AbstractRobot getCurrentRobot() { */ public static void resetSystem() { robot.shutDown(); - // Gui.close(); - // Gui.getInstance(); -// robot.getInstance(); - //TODO make this work for real - } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java index 4e8beda9..5ebee217 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java @@ -73,7 +73,7 @@ public static JsonObject readJSONFile(String path) throws UnsupportedEncodingExc /** - * Normalizes an an input angle in degrees to be between -180 and 180 + * Clamp an an input angle in degrees to be between -180 and 180 * * @param degrees input angle in degrees * @return normalized angle in degrees @@ -92,7 +92,7 @@ public static double normalizeAngleDeg(double degrees) { } /** - * Normalizes an an input angle in degrees to be between -pi and pi + * Clamp an an input angle in degrees to be between -pi and pi * * @param radians input angle in radians * @return normalized angle in radians diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java index 0857f10f..ff2cc553 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java @@ -9,7 +9,11 @@ public class DriveControlMessage extends BaseMessage { public static final String VERSION_ID = "drive_control_message"; + /** + * Angle is supposed to be in radians + */ private final double angle; + private GpsMeasurement currentWaypoint; /** * Construct a new DriveControlMessage @@ -22,6 +26,19 @@ public DriveControlMessage(Date timestamp, double angle) { this.timestamp = new Date(timestamp.getTime()).getTime(); } + /** + * Construct a new DriveControlMessage + * + * @param timestamp {@link Date} representing the creation time + * @param angle the commanded angle of the front wheel + * @param currentWaypoint gps measurement representing the current waypoint + */ + public DriveControlMessage(Date timestamp, double angle, GpsMeasurement currentWaypoint) { + this.angle = angle; + this.timestamp = new Date(timestamp.getTime()).getTime(); + this.currentWaypoint = currentWaypoint; + } + /** * Returns the commanded angle of the steering as a double (in degrees) * @@ -32,12 +49,13 @@ public double getAngleDouble() { } /** - * Returns the commanded angle of the steering as an int (in hundredths of degrees) + * Returns the GpsMeasurement representing the current waypoint + * from this DriveControlMessage * - * @return the commanded angle of the steering as an int (in hundredths of degrees) + * @return the GpsMeasurement representing the current waypoint + * from this DriveControlMessage */ - public int getAngleInt() { - return (int) (angle * 100.0); + public GpsMeasurement getWaypoint() { + return currentWaypoint; } - } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java index e6d6ffa0..6a8ff943 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java @@ -16,6 +16,7 @@ public class GPSPoseMessage extends BaseMessage { private final double latitude; private final double longitude; + private final double velocity; private final double heading; /** @@ -27,10 +28,24 @@ public class GPSPoseMessage extends BaseMessage { * @param heading of the buggy (in degrees from North) */ public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading) { + this(timestamp, latitude, longitude, heading, 0.0); + } + + /** + * Constructs a new {@link GPSPoseMessage} + * + * @param timestamp {@link Date} representing the creation time + * @param latitude of the buggy (negative is South) + * @param longitude of the buggy (negative is West) + * @param heading of the buggy (in degrees from North) + * @param velocity of the buggy + */ + public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading, double velocity) { this.latitude = latitude; this.longitude = longitude; this.heading = heading; this.timestamp = new Date(timestamp.getTime()).getTime(); + this.velocity = velocity; } /** @@ -52,14 +67,21 @@ public double getLongitude() { } /** - * Returns the heading of the {@link GPSPoseMessage} (in degrees from North) + * Returns the heading of the {@link GPSPoseMessage} (in RADIANS from North) * - * @return the heading of the {@link GPSPoseMessage} (in degrees from North) + * @return the heading of the {@link GPSPoseMessage} (in RADIANS from North) */ public double getHeading() { return heading; } + /** + * @return the current estimated velocity (m/s) + */ + public double getVelocity() { + return velocity; + } + /** * evaluates to the distance between two gps points based on an L2 metric * diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java index 6bfcd2ab..33e4f193 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java @@ -1,5 +1,7 @@ package com.roboclub.robobuggy.messages; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; + import java.util.Date; /** @@ -161,5 +163,4 @@ public boolean getNorth() { public GPSPoseMessage toGpsPoseMessage(double heading) { return new GPSPoseMessage(gpsTimestamp, latitude, longitude, heading); } - } \ No newline at end of file diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java index c696bba7..396f3ec3 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java @@ -83,170 +83,94 @@ public KfLocalizer(int period) { // Initialize subscriber to GPS measurements new Subscriber("htGpsLoc", NodeChannel.GPS.getMsgPath(), - new MessageListener() { - @Override - public synchronized void actionPerformed(String topicName, Message m) { - GpsMeasurement newGPSData = (GpsMeasurement) m; - LocTuple gpsLatLng = new LocTuple(newGPSData - .getLatitude(), newGPSData.getLongitude()); - UTMTuple gpsUTM = LocalizerUtil.deg2UTM(gpsLatLng); - double dx = gpsUTM.getEasting() - lastGPS.getEasting(); - double dy = gpsUTM.getNorthing() - lastGPS.getNorthing(); - double th = Math.toDegrees(Math.atan2(dy, dx)); - lastGPS = gpsUTM; - - double[][] observationModel = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - // don't update angle if we did not move a lot - if (Math.sqrt(dx * dx + dy * dy) < .5) {// || - // Math.abs(th- - // state.get(4, - // 0)) > 90){ - observationModel[4][4] = 0; - } - - if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) - + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { - th = startAngle; - } - - double[][] meassurement = { { gpsUTM.getEasting() }, - { gpsUTM.getNorthing() }, { 0 }, { 0 }, { th }, - { 0 }, { 0 } }; - double[][] updateCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - updateStep(new Matrix(observationModel), new Matrix( - meassurement), new Matrix(updateCovariance)); + new MessageListener() { + @Override + public synchronized void actionPerformed(String topicName, Message m) { + GpsMeasurement newGPSData = (GpsMeasurement) m; + LocTuple gpsLatLng = new LocTuple(newGPSData + .getLatitude(), newGPSData.getLongitude()); + UTMTuple gpsUTM = LocalizerUtil.deg2UTM(gpsLatLng); + double dx = gpsUTM.getEasting() - lastGPS.getEasting(); + double dy = gpsUTM.getNorthing() - lastGPS.getNorthing(); + double th = Math.toDegrees(Math.atan2(dy, dx)); + lastGPS = gpsUTM; + + double[][] observationModel = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 0 } // Heading + }; + + // don't update angle if we did not move a lot + if (Math.sqrt(dx * dx + dy * dy) < .5) {// || + // Math.abs(th- + // state.get(4, + // 0)) > 90){ + observationModel[4][4] = 0; } - }); - - /* - new Subscriber( - "HighTrustGpsLoc", - NodeChannel.IMU_ANG_POS.getMsgPath(), - ((topicName, m) -> { - IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); - // double y = mes.getRot()[0][1]; - // double x = mes.getRot()[0][0]; - double[][] xVar = { { 1 }, { 0 }, { 0 } }; - double[][] yVar = { { 0 }, { 1 }, { 0 } }; - Matrix xMat = new Matrix(xVar); - Matrix yMat = new Matrix(yVar); - Matrix rot = new Matrix(mes.getRot()); - double x = rot.times(xMat).get(0, 0); - double y = rot.times(yMat).get(0, 0); - double th = -(Math.toDegrees(Math.atan2(y, x)) - 90); - double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, - { th }, { 0 }, { 0 } }; - double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - // updateStep(new Matrix(observationModel),new - // Matrix(meassurement),new Matrix(updateCovariance)); + if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) + + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { + th = startAngle; + } - })); - */ + double[][] meassurement = { { gpsUTM.getEasting() }, + { gpsUTM.getNorthing() }, { 0 }, { 0 }, { th }, + { 0 }, { 0 } }; + double[][] updateCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + updateStep(new Matrix(observationModel), new Matrix( + meassurement), new Matrix(updateCovariance)); + } + }); // TODO note that we will probably run into precision errors since the // changes are so small // would be good to batch up the encoder updates until we get a margin // that we know can be represented proeprly new Subscriber("htGpsLoc", NodeChannel.ENCODER.getMsgPath(), - new MessageListener() { - @Override - public synchronized void actionPerformed(String topicName, - Message m) { - EncoderMeasurement measurement = (EncoderMeasurement) m; - - // convert the feet from the last message into a delta - // degree, and update our position - double currentEncoderMeasurement = measurement - .getDistance(); - double deltaDistance = currentEncoderMeasurement - - lastEncoderReading; - long currentTime = new Date().getTime(); - long dt = currentTime - lastEncoderReadingTime; - if (dt > 1) { // to remove numeric instability - double bodySpeed = deltaDistance / (dt / 1000.0); - lastEncoderReadingTime = currentTime; - lastEncoderReading = currentEncoderMeasurement; - - double[][] observationModel = { - { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; + new MessageListener() { + @Override + public synchronized void actionPerformed(String topicName, + Message m) { + EncoderMeasurement measurement = (EncoderMeasurement) m; + + // convert the feet from the last message into a delta + // degree, and update our position + double currentEncoderMeasurement = measurement + .getDistance(); + double deltaDistance = currentEncoderMeasurement + - lastEncoderReading; + long currentTime = new Date().getTime(); + long dt = currentTime - lastEncoderReadingTime; + if (dt > 1) { // to remove numeric instability + double bodySpeed = deltaDistance / (dt / 1000.0); + lastEncoderReadingTime = currentTime; + lastEncoderReading = currentEncoderMeasurement; - double[][] meassurement = { { 0 }, { 0 }, - { bodySpeed }, { 0 }, { 0 }, { 0 }, { 0 } }; - double[][] updateCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - updateStep(new Matrix(observationModel), - new Matrix(meassurement), new Matrix( - updateCovariance)); - } - } - }); - - new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), - new MessageListener() { - - @Override - public synchronized void actionPerformed(String topicName, - Message m) { - SteeringMeasurement steerM = (SteeringMeasurement) m; double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot_b { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b { 0, 0, 0, 0, 0, 0, 0 }, // th { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // Heading + { 0, 0, 0, 0, 0, 0, 0 } // Heading }; - double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, - { 0 }, { 0 }, { steerM.getAngle() } }; + + double[][] meassurement = { { 0 }, { 0 }, + { bodySpeed }, { 0 }, { 0 }, { 0 }, { 0 } }; double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x { 0, 1, 0, 0, 0, 0, 0 }, // y @@ -256,10 +180,44 @@ public synchronized void actionPerformed(String topicName, { 0, 0, 0, 0, 0, 1, 0 }, // th_dot { 0, 0, 0, 0, 0, 0, 1 } // heading }; - updateStep(new Matrix(observationModel), new Matrix( - meassurement), new Matrix(updateCovariance)); + updateStep(new Matrix(observationModel), + new Matrix(meassurement), new Matrix( + updateCovariance)); } - }); + } + }); + + new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), + new MessageListener() { + + @Override + public synchronized void actionPerformed(String topicName, + Message m) { + SteeringMeasurement steerM = (SteeringMeasurement) m; + double[][] observationModel = { + { 0, 0, 0, 0, 0, 0, 0 }, // x + { 0, 0, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // Heading + }; + double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, + { 0 }, { 0 }, { steerM.getAngle() } }; + double[][] updateCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + updateStep(new Matrix(observationModel), new Matrix( + meassurement), new Matrix(updateCovariance)); + } + }); resume(); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java new file mode 100644 index 00000000..86e21af2 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -0,0 +1,341 @@ +package com.roboclub.robobuggy.nodes.localizers; + +import Jama.Matrix; +import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.messages.SteeringMeasurement; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; + +import java.util.Date; + +/** + * Created by vivaanbahl on 11/4/16. + */ +public class RobobuggyKFLocalizer extends PeriodicNode { + + // our transmitter for position estimate messages + private Publisher posePub; + private boolean gpsMessagesReceived; + + // constants we use throughout the file + private static final int X_GLOBAL_ROW = 0; + private static final int Y_GLOBAL_ROW = 1; + private static final int VELOCITY_ROW = 2; + private static final int HEADING_GLOBAL_ROW = 3; + private static final int HEADING_VEL_ROW = 4; + + public static final double WHEELBASE_IN_METERS = 1.13; // meters + private static final int UTMZONE = 17; + private final UTMTuple initialLocationGPS; // new LocTuple(40.441670, -79.9416362)); + private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad + + // The state consists of 4 elements: + // x - x position in the world frame, in meters + // y - y position in the world frame, in meters + // dy_body - forward velocity in the body frame, in meters/s + // heading - heading, or yaw angle, in the world frame, in rad + // dheading - angular velocity in the world frame, in rad/s + private Matrix x; // current state + private Matrix rMatrix; // measurement noise covariance matrix + private Matrix pMatrix; // covariance matrix + private Matrix qGPS; // model noise covariance matrix + private Matrix qEncoder; + + // output matrices + private Matrix cGPS; // a description of how the GPS impacts x + private Matrix cEncoder; // how the encoder affects x + + // Q is the variance of new measurements + // pMatrix is the variance of old measurements + // C is the observation matrix - what variables can we measure, since we can't directly measure everything + // in the state + // rMatrix = covariance of measurements + // if rMatrix is small, that means the "kalman gain" is high, means you weight measurement more than model + // if rMatrix is large, kalman gain low, weight model more than measurements + + private long lastTime; // the last time we updated the current position estimate + private UTMTuple lastGPS; // the most recent value of the GPS coordinates, expressed as a UTM measurement + private double lastEncoder; // deadreckoning value + private long lastEncoderTime; // the most recent time of the encoder reading, used for getting buggy velocity + private double steeringAngle = 0; // current steering angle of the buggy + + /** + * Create a new {@link PeriodicNode} decorator + * + * @param period of the periodically executed portion of the node + * @param name the name of the node + * @param initialPosition the initial position of the localizer + */ + public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { + super(new BuggyBaseNode(NodeChannel.POSE), period, name); + posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + + initialLocationGPS = LocalizerUtil.deg2UTM(initialPosition); + + // set initial state + lastTime = new Date().getTime(); + lastEncoder = 0; + lastEncoderTime = lastTime; + lastGPS = initialLocationGPS; + + double[][] x2D = { + { initialLocationGPS.getEasting() }, + { initialLocationGPS.getNorthing() }, + { 0 }, + { INITIAL_HEADING_IN_RADS }, + { 0 } + }; + x = new Matrix(x2D); + + double[] rArray = {4, 4, 0.25, 0.01, 0.01}; + double[] pArray = {25, 25, 0.25, 2.46, 2.46}; + + rMatrix = arrayToMatrix(rArray); + pMatrix = arrayToMatrix(pArray); + + double[][] qGPS2D = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 0.01}, + }; + qGPS = new Matrix(qGPS2D); + + double[][] qEncoder2D = { + {0.25}, + }; + qEncoder = new Matrix(qEncoder2D); + + double[][] cGPS2D = { + {1, 0, 0, 0, 0}, + {0, 1, 0, 0, 0}, + {0, 0, 0, 1, 0}, + }; + cGPS = new Matrix(cGPS2D); + + double[][] cEncoder2D = { + {0, 0, 1, 0, 0}, + }; + cEncoder = new Matrix(cEncoder2D); + + // add all our subscribers for our current state update stream + // Every time we get a new sensor update, trigger the new kalman update + setupGPSSubscriber(); + setupEncoderSubscriber(); + setupSteeringSubscriber(); + resume(); + } + + private void setupEncoderSubscriber() { + new Subscriber("KF Localizer", NodeChannel.ENCODER.getMsgPath(), ((topicName, m) -> { + long currentTime = new Date().getTime(); + long dt = currentTime - lastEncoderTime; + // to remove numeric instability, limit rate to 10ms, 100Hz + if (dt < 50) { + return; + } + + EncoderMeasurement odometry = (EncoderMeasurement) m; + double currentEncoder = odometry.getDistance(); + + double dx = currentEncoder - lastEncoder; + double bodySpeed = dx / (dt / 1000.0); + lastEncoderTime = currentTime; + lastEncoder = currentEncoder; + + // measurement + double[][] z2D = { + { bodySpeed }, + }; + Matrix z = new Matrix(z2D); + + kalmanFilter(cEncoder, qEncoder, z); + })); + } + + private void setupGPSSubscriber() { + new Subscriber("KF Localizer", NodeChannel.GPS.getMsgPath(), ((topicName, m) -> { + gpsMessagesReceived = true; + GpsMeasurement gpsLoc = (GpsMeasurement) m; + LocTuple loc = new LocTuple(gpsLoc.getLatitude(), gpsLoc.getLongitude()); + UTMTuple gps = LocalizerUtil.deg2UTM(loc); + double dx = gps.getEasting() - lastGPS.getEasting(); + double dy = gps.getNorthing() - lastGPS.getNorthing(); + lastGPS = gps; + + // don't update angle if we did not move a lot + double heading = Math.atan2(dy, dx); + if ((dx * dx + dy * dy) < 0.25) { + heading = x.get(HEADING_GLOBAL_ROW, 0); + } + // close the loop, lock initial angle + if (Math.abs(gps.getEasting() - initialLocationGPS.getEasting()) + + Math.abs(gps.getNorthing() - initialLocationGPS.getNorthing()) < 10.0) { + // todo should this be previous heading? + heading = INITIAL_HEADING_IN_RADS; + } + + // measurement + double[][] z2D = { + { gps.getEasting() }, + { gps.getNorthing() }, + { heading }, + }; + + Matrix z = new Matrix(z2D); + + kalmanFilter(cGPS, qGPS, z); + })); + } + + private void setupSteeringSubscriber() { + new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), ((topicName, m) -> { + SteeringMeasurement steerM = (SteeringMeasurement) m; + steeringAngle = Math.toRadians(steerM.getAngle()); + })); + } + + @Override + protected void update() { + Matrix xPredict = propagate(); + + UTMTuple utm = new UTMTuple(UTMZONE, 'T', xPredict.get(X_GLOBAL_ROW, 0), + xPredict.get(Y_GLOBAL_ROW, 0)); + LocTuple latLon = LocalizerUtil.utm2Deg(utm); + if (gpsMessagesReceived) { + posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(), + latLon.getLongitude(), xPredict.get(HEADING_GLOBAL_ROW, 0), xPredict.get(VELOCITY_ROW, 0))); + } + } + + // Kalman filter step 0: Generate the motion model for the buggy + private Matrix getMotionModel(double dt) { + double[][] motionModel2D = { + // x y dy_b heading dheading + { 1, 0, dt * Math.cos(x.get(HEADING_GLOBAL_ROW, 0)), 0, 0 }, // x + { 0, 1, dt * Math.sin(x.get(HEADING_GLOBAL_ROW, 0)), 0, 0 }, // y + { 0, 0, 1, 0, 0 }, // dy_b + { 0, 0, 0, 1, dt }, // heading + { 0, 0, Math.tan(steeringAngle) / WHEELBASE_IN_METERS, 0, 0 }, // dheading + }; + + return new Matrix(motionModel2D); + } + + /* + Kalman filter step 1: Use the motion model to predict the next state + and covariance matrix. (_pre = predicted) + Kalman filter step 2: Update the prediction based off of measurements / + sensor readings + + ---> {A} ---> ---> | + A |---> {filter} ---> ---| + | z ---> | | + | | + ------------------------------------------------------------------------------------| + */ + private void kalmanFilter(Matrix cMatrix, Matrix qMatrix, Matrix z) { + // update time + Date now = new Date(); + double dt = (now.getTime() - lastTime) / 1000.0; + lastTime = now.getTime(); + + Matrix aMatrix = getMotionModel(dt); + + /* + the predict step is responsible for determining the estimate of the next state + What we do is we take our current state (x[k-1]) and current noise estimate + (p[k-1]), and plug them into the motion model (A). This lets us determine our + prediction state (xhat[k]) and our prediction noise (phat[k]) + + Predict: + x_pre = A * x + P_pre = A * pMatrix * A' + rMatrix + */ + Matrix xPre = aMatrix.times(x); + Matrix pPre = aMatrix.times(pMatrix).times(aMatrix.transpose()); + pPre = pPre.plus(rMatrix); + + xPre.set(HEADING_GLOBAL_ROW, 0, clampAngle(xPre.get(HEADING_GLOBAL_ROW, 0))); + xPre.set(HEADING_VEL_ROW, 0, clampAngle(xPre.get(HEADING_VEL_ROW, 0))); + + /* + the update step is responsible for updating the current state, and resolving + discrepancies between the prediction and actual state + + What we do is we take our measurements (z) that were captured from sensors, + as well as the predictions from the previous step (xhat[k], phat[k]) and run + them through the filter + + This produces your next state (x[k]) and your next noise estimation (p[k]), + which connect in a feedback loop to become the new current state + + Update: + r = z - (C * x_pre) + K = P_pre * C' * inv((C * P_pre * C') + Q) // gain + x = x_pre + (K * r) + pMatrix = (I - (K * C)) * P_pre + */ + Matrix residual = z.minus(cMatrix.times(xPre)); + Matrix kMatrix = cMatrix.times(pPre); + kMatrix = kMatrix.times(cMatrix.transpose()); + kMatrix = kMatrix.plus(qMatrix); + kMatrix = pPre.times(cMatrix.transpose()).times(kMatrix.inverse()); + + x = xPre.plus(kMatrix.times(residual)); + + pMatrix = Matrix.identity(5, 5).minus(kMatrix.times(cMatrix)); + pMatrix = pMatrix.times(pPre); + + x.set(HEADING_GLOBAL_ROW, 0, clampAngle(x.get(HEADING_GLOBAL_ROW, 0))); + x.set(HEADING_VEL_ROW, 0, clampAngle(x.get(HEADING_VEL_ROW, 0))); + } + + // Propagate the motion model forward in time since the last sensor reading + // not part of the Kalman algorithm + private Matrix propagate() { + Date now = new Date(); + double dt = (now.getTime() - lastTime) / 1000.0; + + // x_pre = A * x + Matrix aMatrix = getMotionModel(dt); + return aMatrix.times(x); + } + + + @Override + protected boolean startDecoratorNode() { + return false; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } + + private static Matrix arrayToMatrix(double[] arr) { + double[][] arr2D = { + {arr[0], 0, 0, 0, 0}, + {0, arr[1], 0, 0, 0}, + {0, 0, arr[2], 0, 0}, + {0, 0, 0, arr[3], 0}, + {0, 0, 0, 0, arr[4]} + }; + return new Matrix(arr2D); + } + + // clamp all the angles between -pi and +pi + private double clampAngle(double theta) { + while (theta < -Math.PI) { + theta = theta + (2 * Math.PI); + } + while (theta > Math.PI) { + theta = theta - (2 * Math.PI); + } + return theta; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java index a4adf1e8..87e05d9c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java @@ -3,6 +3,7 @@ import com.roboclub.robobuggy.messages.BrakeControlMessage; import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; @@ -48,7 +49,7 @@ protected final boolean startDecoratorNode() { public void actionPerformed(String topicName, Message m) { updatePositionEstimate((GPSPoseMessage) m); steeringCommandPub.publish(new DriveControlMessage(new Date(), - getCommandedSteeringAngle())); + getCommandedSteeringAngle(), getTargetWaypoint())); brakingCommandPub.publish(new BrakeControlMessage(new Date(), getDeployBrakeValue())); } @@ -78,7 +79,7 @@ protected boolean shutdownDecoratorNode() { * Returns the steering angle to which the {@link PathPlanner} thinks the * buggy's steering should be commanded to follow the desired path. * - * @return desired commanded steering angle + * @return desired commanded steering angle IN RADIANS */ protected abstract double getCommandedSteeringAngle(); @@ -89,4 +90,8 @@ protected boolean shutdownDecoratorNode() { * @return desired commanded brake value (true is deployed) */ protected abstract boolean getDeployBrakeValue(); + + protected GpsMeasurement getTargetWaypoint() { + return null; + } } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java index 84cc21f3..c160d46b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java @@ -14,9 +14,9 @@ public class SweepNode extends PathPlannerNode { private double currentCommandedSteeringAngle = 0; - private static final double STEERING_ANGLE_LOWER_BOUND = -20; - private static final double STEERING_ANGLE_UPPER_BOUND = 20; - private static final double STEERING_ANGLE_INCREMENT = 1; + private static final double STEERING_ANGLE_LOWER_BOUND = Math.toRadians(-20); + private static final double STEERING_ANGLE_UPPER_BOUND = Math.toRadians(20); + private static final double STEERING_ANGLE_INCREMENT = Math.toRadians(1); private Publisher steeringPublisher; private Publisher brakePublisher; private Thread t1; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 4bbef890..f9da87c6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -1,9 +1,12 @@ package com.roboclub.robobuggy.nodes.planners; +import com.roboclub.robobuggy.main.RobobuggyLogicNotification; +import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.main.Util; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.ros.NodeChannel; import java.util.ArrayList; @@ -15,6 +18,11 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type + private int lastClosestIndex = 0; + private GpsMeasurement target; + + // we only want to look at the next 10 waypoints as possible candidates + private static final int WAY_POINT_LOOKAHEAD_MAX = 50; /** * @param wayPoints the list of waypoints to follow @@ -22,6 +30,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { public WayPointFollowerPlanner(ArrayList wayPoints) { super(NodeChannel.PATH_PLANNER); this.wayPoints = wayPoints; + target = wayPoints.get(0); pose = new GPSPoseMessage(new Date(0), 0, 0, 0);// temp measurment } @@ -31,76 +40,88 @@ public void updatePositionEstimate(GPSPoseMessage m) { } + @Override + protected GpsMeasurement getTargetWaypoint() { + return target; + } + //find the closest way point - //TODO turn into a binary search - private static int getClosestIndex(ArrayList wayPoints, GPSPoseMessage currentLocation) { + private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage currentLocation) { double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this - int closestIndex = -1; - for (int i = 0; i < wayPoints.size(); i++) { - double d = GPSPoseMessage.getDistance(currentLocation, wayPoints.get(i).toGpsPoseMessage(0)); + int closestIndex = lastClosestIndex; + for (int i = lastClosestIndex; i < (lastClosestIndex + WAY_POINT_LOOKAHEAD_MAX) && i < wayPoints.size(); i++) { + GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); + double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { min = d; closestIndex = i; } + // eventually cut off search somehow } + lastClosestIndex = closestIndex; return closestIndex; } @Override public double getCommandedSteeringAngle() { + // determines the angle at which to move every 50 milliseconds + //PD control of DC steering motor handled by low level + + double commandedAngle; + commandedAngle = purePursuitController(); + + return commandedAngle; + } + + private double purePursuitController() { + // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf + // section 2.2 + int closestIndex = getClosestIndex(wayPoints, pose); - if (closestIndex == -1) { - return 17433504; //A dummy value that we can never get - } + double k = 2.5; + double velocity = pose.getVelocity(); + double lookaheadLowerBound = 5.0; + double lookaheadUpperBound = 25.0; + double lookahead = (k * velocity)/2; + if(lookahead < lookaheadLowerBound) { + lookahead = lookaheadLowerBound; + } + else if(lookahead > lookaheadUpperBound) { + lookahead = lookaheadUpperBound; + } - double delta = 10; //meters - //pick the first point that is at least delta away - //pick the point to follow - int targetIndex = closestIndex; - while (targetIndex < wayPoints.size() && GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)) < delta) { - targetIndex = targetIndex + 1; + //pick the first point that is at least lookahead away, then point buggy toward it + int lookaheadIndex = 0; + for(lookaheadIndex = closestIndex; lookaheadIndex < wayPoints.size(); lookaheadIndex++) { + if(GPSPoseMessage.getDistance(pose, wayPoints.get(lookaheadIndex).toGpsPoseMessage(0)) > lookahead) { + break; + } } //if we are out of points then just go straight - if (targetIndex >= wayPoints.size()) { + if (lookaheadIndex >= wayPoints.size()) { + new RobobuggyLogicNotification("HELP out of points", RobobuggyMessageLevel.EXCEPTION); return 0; } - - GpsMeasurement targetPoint = wayPoints.get(targetIndex); - //find a path from our current location to that point - double dLon = targetPoint.getLongitude() - pose.getLongitude(); - double dLat = targetPoint.getLatitude() - pose.getLatitude(); - double desiredHeading = Math.toDegrees(Math.atan2(LocalizerUtil.convertLatToMeters(dLat), LocalizerUtil.convertLonToMeters(dLon))); - - // basically we want all of our angles to be in the same range, so that we don't - // have weird wraparound - desiredHeading = Util.normalizeAngleDeg(desiredHeading); - double poseHeading = Util.normalizeAngleDeg(pose.getHeading()); - - //find the angle we need to reach that point - return Util.normalizeAngleDeg(desiredHeading - poseHeading); - + GpsMeasurement target = wayPoints.get(lookaheadIndex); + double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); + double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); + + //Pure Pursuit steering controller + double commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), lookahead); + commandedAngle = Util.normalizeAngleRad(commandedAngle); + return commandedAngle; } @Override protected boolean getDeployBrakeValue() { + // need to brake when 15 m off course return false; - /* - int closestIndex = getClosestIndex(wayPoints,pose); - - if(closestIndex == -1){ - return true; - } - - // if closest point is too far away throw breaks - - boolean shouldBrake = GPSPoseMessage.getDistance(pose, wayPoints.get(closestIndex).toGpsPoseMessage(0)) >= 5.0; - return shouldBrake; - */ } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java index b663afaf..9022f308 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java @@ -179,7 +179,7 @@ private static String formatDateIntoFile(Date d) { * @return the status of the operation - true if it succeeded, false if it didn't */ private boolean createNewLogFile() { - if (!outputDirectory.exists() || !outputDirectory.isDirectory()) { + if ((!outputDirectory.exists() && !outputDirectory.mkdirs()) || !outputDirectory.isDirectory()) { new RobobuggyLogicNotification("Output directory path isn't a folder!", RobobuggyMessageLevel.EXCEPTION); return false; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java index a6d42008..c9c32dd6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java @@ -313,9 +313,11 @@ public int peel(byte[] buffer, int start, int bytesAvailable) { private final class RBSMPeriodicNode extends PeriodicNode { //Stored commanded values - private int commandedAngle = 0; + private double commandedAngle = 0; private boolean commandedBrakeEngaged = true; + private static final int MAX_STEERING_ANGLE_HUNDREDTHS_DEG = 1500; + /** * Create a new {@link RBSMPeriodicNode} object * @@ -333,14 +335,14 @@ private final class RBSMPeriodicNode extends PeriodicNode { */ @Override protected void update() { - int outputAngle = commandedAngle;//allows for commandedAngle to be read only in this function - if (outputAngle > 1000) { - outputAngle = 1000; - } else if (outputAngle < -1000) { - outputAngle = -1000; + int outputAngleHundredthsDegrees = (int) (Math.toDegrees(commandedAngle) * 100);//allows for commandedAngle to be read only in this function + if (outputAngleHundredthsDegrees > MAX_STEERING_ANGLE_HUNDREDTHS_DEG) { + outputAngleHundredthsDegrees = MAX_STEERING_ANGLE_HUNDREDTHS_DEG; + } else if (outputAngleHundredthsDegrees < -MAX_STEERING_ANGLE_HUNDREDTHS_DEG) { + outputAngleHundredthsDegrees = -MAX_STEERING_ANGLE_HUNDREDTHS_DEG; } - RBSMSteeringMessage msgSteer = new RBSMSteeringMessage(outputAngle); + RBSMSteeringMessage msgSteer = new RBSMSteeringMessage(outputAngleHundredthsDegrees); send(msgSteer.getMessageBytes()); RBSMBrakeMessage msgBrake = new RBSMBrakeMessage(commandedBrakeEngaged); send(msgBrake.getMessageBytes()); @@ -356,7 +358,7 @@ protected boolean startDecoratorNode() { new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commandedAngle = -((DriveControlMessage) m).getAngleInt(); + commandedAngle = -((DriveControlMessage) m).getAngleDouble(); } }); new Subscriber("rbsm", NodeChannel.BRAKE_CTRL.getMsgPath(), diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java new file mode 100644 index 00000000..647db616 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java @@ -0,0 +1,54 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.nodes.planners.SweepNode; +import com.roboclub.robobuggy.nodes.sensors.RBSMNode; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.MainGuiWindow; + + +/** + * Created by Robot on 1/24/2017. + */ +public final class CommTestRobot extends AbstractRobot { + private static CommTestRobot instance; + private static final int ARDUINO_BOOTLOADER_TIMEOUT_MS = 2000; + + /** + * Returns a reference to the one instance of the {@link Robot} object. + * If no instance exists, a new one is created. + * + * @return a reference to the one instance of the {@link Robot} object + */ + public static CommTestRobot getInstance() { + if (instance == null) { + instance = new CommTestRobot(); + } + return instance; + } + + private CommTestRobot() { + super(); + try { + Thread.sleep(ARDUINO_BOOTLOADER_TIMEOUT_MS); + } catch (InterruptedException e) { + e.printStackTrace(); + } + + nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, + RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); + nodeList.add(new SweepNode(NodeChannel.DRIVE_CTRL)); + + //setup the gui + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + tabs.addTab(new MainGuiWindow(), "Home"); + + } + +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ConfigRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ConfigRobot.java new file mode 100644 index 00000000..f4e1f507 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ConfigRobot.java @@ -0,0 +1,43 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.ui.ConfigurationPanel; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.PathPanel; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; + +/** + * Runs playback + * + * @author Trevor Decker + */ +public final class ConfigRobot extends AbstractRobot { + + private static ConfigRobot instance; + + /** + * Returns a reference to the one instance of the {@link Robot} object. + * If no instance exists, a new one is created. + * + * @return a reference to the one instance of the {@link Robot} object + */ + public static AbstractRobot getInstance() { + if (instance == null) { + instance = new ConfigRobot(); + } + return instance; + } + + private ConfigRobot() { + super(); + + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + tabs.addTab(new ConfigurationPanel(), "Configuration"); + tabs.addTab(new PathPanel(), "Path Visualizer"); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + Gui.getInstance().fixPaint(); + } +} + diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java new file mode 100644 index 00000000..66ee000e --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -0,0 +1,60 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; +import com.roboclub.robobuggy.simulation.ControllerTester; +import com.roboclub.robobuggy.ui.ConfigurationPanel; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.MainGuiWindow; +import com.roboclub.robobuggy.ui.PathPanel; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; + +import java.io.IOException; +import java.util.ArrayList; + +/** + * Created by vivaanbahl on 1/26/17. + */ +public final class ControllerTesterRobot extends AbstractRobot { + private static ControllerTesterRobot instance; + + /** + * Returns a reference to the one instance of the {@link Robot} object. + * If no instance exists, a new one is created. + * + * @return a reference to the one instance of the {@link Robot} object + */ + public static ControllerTesterRobot getInstance() { + if (instance == null) { + instance = new ControllerTesterRobot(); + } + return instance; + } + + private ControllerTesterRobot() { + super(); + + ArrayList wayPoints = new ArrayList<>(); + try { + wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); + nodeList.add(new WayPointFollowerPlanner(wayPoints)); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + nodeList.add(new ControllerTester("Testing the controller", new LocTuple(wayPoints.get(0).getLatitude(), wayPoints.get(0).getLongitude()))); + + //setup the gui + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + tabs.addTab(new MainGuiWindow(), "Home"); + tabs.add(new PathPanel(), "Path Visualizer"); + tabs.addTab(new ConfigurationPanel(), "Configuration"); + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java new file mode 100644 index 00000000..d91082aa --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java @@ -0,0 +1,65 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; +import com.roboclub.robobuggy.simulation.LocalizerTester; +import com.roboclub.robobuggy.ui.ConfigurationPanel; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.MainGuiWindow; +import com.roboclub.robobuggy.ui.PathPanel; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; + +import java.io.FileNotFoundException; +import java.util.ArrayList; + +/** + * Created by vivaanbahl on 1/27/17. + */ +public class LocalizerTesterRobot extends AbstractRobot { + + private static LocalizerTesterRobot instance; + + /** + * Returns a reference to the one instance of the {@link Robot} object. + * + * @return a reference to the one instance of the {@link Robot} object + */ + protected LocalizerTesterRobot() { + super(); + + ArrayList waypoints = null; + try { + waypoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + + nodeList.add(new LocalizerTester("localizer tester", waypoints)); + nodeList.add(new RobobuggyKFLocalizer(10, "localizer", new LocTuple(40.441670, -79.9416362))); + + //setup the gui + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + tabs.addTab(new MainGuiWindow(), "Home"); + tabs.add(new PathPanel(), "Path Visualizer"); + tabs.addTab(new ConfigurationPanel(), "Configuration"); + } + + /** + * Returns a reference to the one instance of the {@link Robot} object. + * + * @return a reference to the one instance of the {@link Robot} object + */ + public static LocalizerTesterRobot getInstance() { + if (instance == null) { + instance = new LocalizerTesterRobot(); + } + return instance; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index e25cf39f..83d9d4fa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -1,7 +1,10 @@ package com.roboclub.robobuggy.robots; import com.roboclub.robobuggy.main.RobobuggyConfigFile; -import com.roboclub.robobuggy.nodes.localizers.KfLocalizer; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.simulation.LineByLineSensorPlayer; import com.roboclub.robobuggy.ui.AutonomousPanel; import com.roboclub.robobuggy.ui.ConfigurationPanel; @@ -11,6 +14,8 @@ import com.roboclub.robobuggy.ui.RobobuggyGUITabs; import com.roboclub.robobuggy.ui.RobobuggyJFrame; +import java.io.FileNotFoundException; + /** * Runs playback * @@ -36,16 +41,12 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); -// new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); -// new HighTrustGPSLocalizer(); - new KfLocalizer(10); + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); Gui.getInstance().addWindow(mainWindow); RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - tabs.addTab(new AutonomousPanel(), "Autonomous"); tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java index 14577f56..baf05e51 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java @@ -3,11 +3,13 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.HighTrustGPSLocalizer; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.simulation.FullSimRunner; import com.roboclub.robobuggy.simulation.SimulatedBuggy; import com.roboclub.robobuggy.simulation.SimulatedGPSNode; import com.roboclub.robobuggy.simulation.SimulatedImuNode; @@ -24,6 +26,9 @@ import java.io.FileNotFoundException; import java.util.ArrayList; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LAT; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LON; + /** * A robot file for a simulated robot that can be used for internal testing of nodes along simulated paths @@ -49,32 +54,7 @@ public static AbstractRobot getInstance() { private SimRobot() { super(); - nodeList.add(new HighTrustGPSLocalizer()); - // nodeList.add(new KfLocalizer(100)); - nodeList.add(new SimulatedImuNode(100)); - nodeList.add(new SimulatedGPSNode(500)); - nodeList.add(new SimulatedRBSMNode()); - - try { - ArrayList wayPoints = - WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); - nodeList.add(new WayPointFollowerPlanner(wayPoints)); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - - SimulatedBuggy simBuggy = SimulatedBuggy.getInstance(); - simBuggy.setY(LocalizerUtil.convertLatToMeters(40.441705)); - simBuggy.setX(LocalizerUtil.convertLonToMeters(-79.941585)); - simBuggy.setTh(-110); - simBuggy.setDx(10); - - - nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, - NodeChannel.getLoggingChannels())); - //simBuggy.setDth(1); - // simBuggy.setDth(0.10); - + nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(INITIAL_POS_LAT, INITIAL_POS_LON))); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); @@ -82,13 +62,8 @@ private SimRobot() { RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - // tabs.addTab(new ImuPanel(),"IMU"); - tabs.addTab(new AutonomousPanel(), "Autonomous"); - tabs.addTab(new SimulationPanel(), "Simulation"); - tabs.addTab(new PathPanel(), "Path Panel"); + tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); - } } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 751b602e..fc421c02 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -4,11 +4,11 @@ import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.nodes.localizers.KfLocalizer; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -22,7 +22,8 @@ import java.io.IOException; import java.util.ArrayList; - +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LAT; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LON; /** * A robot class for having transistor drive itself * @@ -57,18 +58,14 @@ private TransistorAuton() { shutDown(); } new RobobuggyLogicNotification("Logic Exception Setup properly", RobobuggyMessageLevel.NOTE); - // Initialize Nodes - - nodeList.add(new KfLocalizer(10)); + // Initialize Nodes + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(INITIAL_POS_LAT, INITIAL_POS_LON))); nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, NodeChannel.getLoggingChannels())); nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); -// nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillCrestImuNode()); - try { ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); @@ -83,8 +80,6 @@ private TransistorAuton() { RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - // tabs.addTab(new AutonomousPanel(),"Autonomous"); tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java index 2980405d..68fc11ea 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java @@ -3,6 +3,8 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.nodes.sensors.CameraNode; import com.roboclub.robobuggy.nodes.sensors.GpsNode; import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; @@ -57,6 +59,8 @@ private TransistorDataCollection() { RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); nodeList.add(new HillCrestImuNode()); + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(0, 0))); + //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java new file mode 100644 index 00000000..1f9852f3 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -0,0 +1,109 @@ +package com.roboclub.robobuggy.simulation; + +import Jama.Matrix; +import com.roboclub.robobuggy.messages.DriveControlMessage; +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.UTMTuple; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; + +import java.util.Date; + +/** + * Created by vivaanbahl on 1/25/17. + * + * adapted from @babraham's MATLAB script found in offline/controller + */ +public class ControllerTester extends PeriodicNode { + + // run every 10 ms + private static final int SIM_PERIOD = 10; + // controller runs every 10 iterations + private static final int CONTROLLER_PERIOD = 1; + // wheelbase in meters + private static final double WHEELBASE = 1.13; + // assume a velocity of 8 m/s + private static final double VELOCITY = 1; + // assume we are facing up hill 1 + private static final double INITIAL_HEADING_RAD = Math.toRadians(250); + + private Matrix x; + private double commandedSteeringAngle = 0; + private Publisher simulatedPosePub; + private int simCounter; + + /** + * Create a new {@link PeriodicNode} decorator + * + * @param name - name of tester + * @param initialPosition - represents initial position of tester + */ + public ControllerTester(String name, LocTuple initialPosition) { + super(new BuggyBaseNode(NodeChannel.AUTO), SIM_PERIOD, name); + + double[][] xAsDoubleArr = { + { LocalizerUtil.deg2UTM(initialPosition).getEasting() }, + { LocalizerUtil.deg2UTM(initialPosition).getNorthing() }, + { VELOCITY }, + { INITIAL_HEADING_RAD }, + { 0 } + }; + + x = new Matrix(xAsDoubleArr); + + new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { + commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); + })); + + simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); + new Publisher(NodeChannel.GPS.getMsgPath()); + } + + @Override + protected void update() { + simCounter++; + + Matrix motionModel = getNewModel(x); + + // update simulated position + x = motionModel.times(x); + + UTMTuple t = new UTMTuple(17, 'T', x.get(0, 0), x.get(1, 0)); + LocTuple lt = LocalizerUtil.utm2Deg(t); + + // if it's time to run the controller, update the controller's understanding of where we are + if (simCounter == CONTROLLER_PERIOD) { + simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), x.get(3, 0), x.get(2, 0))); + simCounter = 0; + } + + } + + private Matrix getNewModel(Matrix x) { + double dt = 1.0/SIM_PERIOD; + double[][] matrixAsDoubleArr = { + { 1, 0, dt * Math.cos((x.get(3, 0))), 0, 0 }, + { 0, 1, dt * Math.sin((x.get(3, 0))), 0, 0 }, + { 0, 0, 1, 0, 0 }, + { 0, 0, 0, 1, dt }, + { 0, 0, Math.tan(commandedSteeringAngle)/WHEELBASE, 0, 0 } + }; + return new Matrix(matrixAsDoubleArr); + } + + @Override + protected boolean startDecoratorNode() { + resume(); + return true; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java new file mode 100644 index 00000000..5850c4fc --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -0,0 +1,142 @@ +package com.roboclub.robobuggy.simulation; + +import Jama.Matrix; +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.messages.DriveControlMessage; +import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.messages.SteeringMeasurement; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; +import com.roboclub.robobuggy.nodes.localizers.UTMTuple; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; + +import java.io.FileNotFoundException; +import java.util.Date; +import java.util.Timer; +import java.util.TimerTask; + +/** + * Created by vivaanbahl on 4/2/17. + */ +public class FullSimRunner extends BuggyBaseNode { + + private static final double WHEELBASE_IN_METERS = 1.13; // meters + private static final int UTMZONE = 17; + private static final int VELOCITY = 4; + private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad + private static final int LOCALIZER_UPDATE_PERIOD = 10; + private static final int GPS_UPDATE_PERIOD = 500; + private static final int ENC_UPDATE_PERIOD = 100; + private static final int SIM_UPDATE_PERIOD = 100; + + private Matrix motionModel; + private Matrix state; + private double totalDistance = 0; + + private Publisher gpsPub; + private Publisher encPub; + private Publisher steerPub; + + private RobobuggyKFLocalizer localizer; + private WayPointFollowerPlanner controller; + + private Timer gpsTimer; + private Timer encTimer; + + /** + * Create a new {@link PeriodicNode} decorator + * + * @param name - name of full simulation runner + * @param initialPos - initial position of full simulation runner + */ + public FullSimRunner(String name, LocTuple initialPos) { + super(NodeChannel.SIMULATION); + + // initialized to the identity matrix except for dHeading (goes nowhere) + double[][] motionModelArr = { + {1, 0, 0, 0, 0}, + {0, 1, 0, 0, 0}, + {0, 0, 1, 0, 0}, + {0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0}, + }; + motionModel = new Matrix(motionModelArr); + + UTMTuple t = LocalizerUtil.deg2UTM(initialPos); + double[][] stateArr = { + { t.getEasting() }, + { t.getNorthing() }, + { VELOCITY }, + { INITIAL_HEADING_IN_RADS }, + { 0 }, + }; + state = new Matrix(stateArr); + + gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); + encPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); + steerPub = new Publisher(NodeChannel.STEERING.getMsgPath()); + new Subscriber("Full Sim Toolbox", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { + updateMotionModel(((DriveControlMessage) m).getAngleDouble()); + steerPub.publish(new SteeringMeasurement(Math.toDegrees(((DriveControlMessage) m).getAngleDouble()))); + }); + updateMotionModel(0); + + try { + localizer = new RobobuggyKFLocalizer(LOCALIZER_UPDATE_PERIOD, "Simulated Localization", initialPos); + localizer.startNode(); + controller = new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile())); + controller.startNode(); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + + gpsTimer = new Timer(); + gpsTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + Matrix nextStep = generateNextStep(); + UTMTuple nextLocUTM = new UTMTuple(UTMZONE, 'T', state.get(0, 0), state.get(1, 0)); + nextLocUTM.setEasting(nextLocUTM.getEasting()); + nextLocUTM.setNorthing(nextLocUTM.getNorthing()); + LocTuple nextLocLL = LocalizerUtil.utm2Deg(nextLocUTM); + gpsPub.publish(new GpsMeasurement(new Date(), nextLocLL.getLatitude(), true, nextLocLL.getLongitude(), true, 0, 0, 0, 0, 0, 0)); + state = nextStep; + + } + }, 0, GPS_UPDATE_PERIOD); + + encTimer = new Timer(); + encTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + state = generateNextStep(); + double deltaDist = ENC_UPDATE_PERIOD / 1000.0 * state.get(2, 0); + totalDistance += deltaDist; + encPub.publish(new EncoderMeasurement(totalDistance, state.get(2, 0))); + } + }, 0, ENC_UPDATE_PERIOD); + + } + + private void updateMotionModel(double v) { + double dt = ENC_UPDATE_PERIOD / 1000.0; + motionModel.set(0, 2, dt*Math.cos(state.get(3, 0))); + motionModel.set(1, 2, dt*Math.sin(state.get(3, 0))); + motionModel.set(3, 4, dt); + motionModel.set(4, 2, Math.tan(v) / WHEELBASE_IN_METERS); + + } + + private Matrix generateNextStep() { + return motionModel.times(state); + } + +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java new file mode 100644 index 00000000..455570bb --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -0,0 +1,106 @@ +package com.roboclub.robobuggy.simulation; + +import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; + +import java.util.ArrayList; +import java.util.Timer; +import java.util.TimerTask; + +/** + * Created by vivaanbahl on 1/27/17. + */ +public class LocalizerTester extends BuggyDecoratorNode { + + private static final int GPS_UPDATE_PERIOD = 500; + private static final int ODOM_UPDATE_PERIOD = 10; + private static final double POSITION_UPDATE_M = 1; // move this many meters every tick + + private int targetWaypointIndex = 0; + private ArrayList waypoints; + private Timer gpsTimer; + private Timer odomTimer; + private LocTuple currentPosition = new LocTuple(40.441670, -79.9416362); + private double heading = Math.toRadians(90); + // TODO simulate noise using Gaussians and actual covariances + + private Publisher gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); + private Publisher odomPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); + + /** + * Creates a new decorator for the given Node + * + * @param name the name we want for this node to store so that it can be referenced later + * @param waypoints the array list of GpsMeasurements representing the waypoints on the map + */ + public LocalizerTester(String name, ArrayList waypoints) { + super(new BuggyBaseNode(NodeChannel.POSE), name); + + gpsTimer = new Timer("GPS"); + odomTimer = new Timer("odom"); + this.waypoints = waypoints; + } + + /** + * returns the target waypoint + * @return a GpsMeasurement representing the waypoint + */ + public GpsMeasurement getTargetWaypoint() { + GpsMeasurement currentTarget = waypoints.get(targetWaypointIndex); + GpsMeasurement currentPositionMeas = new GpsMeasurement(currentPosition.getLatitude(), currentPosition + .getLongitude()); + GPSPoseMessage currentTargetPM = currentTarget.toGpsPoseMessage(0); + GPSPoseMessage currentPositionPM = currentPositionMeas.toGpsPoseMessage(0); + + double distInM = (GPSPoseMessage.getDistance(currentTargetPM, currentPositionPM )); + if (distInM < 5) { + targetWaypointIndex++; + } + return waypoints.get(targetWaypointIndex); + } + + private GpsMeasurement updateSimulatedPosition() { + GpsMeasurement targetWaypoint = getTargetWaypoint(); + double dlat = currentPosition.getLatitude() - targetWaypoint.getLatitude(); + double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); + heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); + + double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random()); + double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random()); + double newLat = currentPosition.getLatitude() + updateLat; + double newLon = currentPosition.getLongitude() + updateLon; + currentPosition = new LocTuple(newLat, newLon); + return new GpsMeasurement(newLat, newLon); + } + + @Override + protected boolean startDecoratorNode() { + gpsTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + gpsPub.publish(updateSimulatedPosition()); + } + }, 0, GPS_UPDATE_PERIOD); + + odomTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + odomPub.publish(new EncoderMeasurement(0.01, 8)); + } + }, 0, ODOM_UPDATE_PERIOD); + + return true; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 281443ef..64a6cafa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -42,7 +42,8 @@ public final class PlayBackUtil { private Publisher steeringCommandPub; private Publisher loggingButtonPub; private Publisher logicNotificationPub; - + private Publisher posePub; + private Publisher driveCtrlPub; /** * Gets the PlaybackUtil instance @@ -69,6 +70,8 @@ private PlayBackUtil() { steeringCommandPub = new Publisher(NodeChannel.STEERING_COMMANDED.getMsgPath()); loggingButtonPub = new Publisher(NodeChannel.GUI_LOGGING_BUTTON.getMsgPath()); logicNotificationPub = new Publisher(NodeChannel.LOGIC_NOTIFICATION.getMsgPath()); + posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + driveCtrlPub = new Publisher(NodeChannel.DRIVE_CTRL.getMsgPath()); } /** @@ -113,7 +116,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, long sensorTime = sensorDataJson.get("timestamp").getAsLong(); long sensorDt = (sensorTime - sensorStartTime); long dt = (long) (sensorDt / playBackSpeed) - playBacktime; - if (dt > 10) { //Milliseconds + if (dt > 10) { // Milliseconds Thread.sleep(dt); } @@ -134,6 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); + getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); @@ -156,6 +160,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case GPSPoseMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, GPSPoseMessage.class); + getPrivateInstance().posePub.publish(transmitMessage); break; case RemoteWheelAngleRequest.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, RemoteWheelAngleRequest.class); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java index f87e2752..5b0bb936 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java @@ -29,7 +29,7 @@ public class SimulatedRBSMNode extends PeriodicNode { private Publisher messagePubControllerSteering; private Publisher messagePubBrakeState; private Publisher messagePubSteering; - private int commandedAngle = 0; + private double commandedAngle = 0; private boolean commandedBrakeEngaged = true; @@ -53,7 +53,7 @@ public SimulatedRBSMNode() { new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commandedAngle = ((DriveControlMessage) m).getAngleInt(); + commandedAngle = ((DriveControlMessage) m).getAngleDouble(); } }); @@ -96,7 +96,7 @@ protected void update() { messagePubControllerSteering.publish(new SteeringMeasurement(potAngle)); messagePubSteering.publish(new SteeringMeasurement(potAngle)); - int outputAngle = commandedAngle; + int outputAngle = (int) commandedAngle; //update the commanded angle if (outputAngle > 1000) { outputAngle = 1000; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java index 9e6b4fda..3d423bbf 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java @@ -1,5 +1,7 @@ package com.roboclub.robobuggy.ui; +import java.awt.Color; + /** * {@link RobobuggyGUIContainer} used to display a {@link DataPanel} and a * {@link GraphPanel} @@ -10,6 +12,7 @@ public final class AnalyticsPanel extends RobobuggyGUIContainer { private DataPanel dataPanel; private static AnalyticsPanel instance; + private Map map; /** * @return a reference to the analytics panel @@ -28,7 +31,10 @@ public static synchronized AnalyticsPanel getInstance() { private AnalyticsPanel() { name = "analytics"; dataPanel = new DataPanel(); + map = new Map(); + this.addComponent(map, 0, 0, 1, 0.75); this.addComponent(dataPanel, 0, 0, 1, 1); + this.setBackground(Color.RED); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java index 48f6cc51..f505199f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java @@ -1,6 +1,7 @@ package com.roboclub.robobuggy.ui; import com.roboclub.robobuggy.messages.BrakeControlMessage; +import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.EncoderMeasurement; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.messages.ImuMeasurement; @@ -195,10 +196,10 @@ public void actionPerformed(String topicName, Message m) { panel.add(commandAng); // Subscriber for drive control updates - new Subscriber("uiDataPan", NodeChannel.STEERING_COMMANDED.getMsgPath(), new MessageListener() { + new Subscriber("uiDataPan", NodeChannel.DRIVE_CTRL.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commandAng.setText(Double.toString(((SteeringMeasurement) m).getAngle())); + commandAng.setText(Double.toString(Math.toDegrees(((DriveControlMessage) m).getAngleDouble()))); Gui.getInstance().fixPaint(); } }); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index e81de9dc..3ab3ac55 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -2,7 +2,9 @@ import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.ros.Message; import com.roboclub.robobuggy.ros.MessageListener; @@ -48,6 +50,11 @@ public class Map extends JPanel { private double mapDragY = -1; private static final int MAX_POINT_BUF_SIZE = 3000; + private MapMarkerDot currentWaypoint = new MapMarkerDot(0, 0); + private MapPolygonImpl currentSteeringCommandMapObj = new MapPolygonImpl(); + private MapPolygonImpl currentHeadingMapObj = new MapPolygonImpl(); + private double currentCommandedAngle; + /** * initializes a new Map with cache loaded */ @@ -56,6 +63,11 @@ public Map() { addCacheToTree(); this.add(getMapTree()); + currentWaypoint.setColor(Color.BLUE); + getMapTree().getViewer().addMapMarker(currentWaypoint); + getMapTree().getViewer().addMapPolygon(currentSteeringCommandMapObj); + getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); + //adds track buggy new Subscriber("Map", NodeChannel.POSE.getMsgPath(), new MessageListener() { //TODO make this optional @@ -65,8 +77,45 @@ public void actionPerformed(String topicName, Message m) { zoomLevel = getMapTree().getViewer().getZoom(); getMapTree().getViewer().setDisplayPosition(new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), zoomLevel); + addPointsToMapTree(Color.RED, new LocTuple(gpsM.getLatitude(), gpsM.getLongitude())); + + getMapTree().getViewer().removeMapMarker(currentWaypoint); + getMapTree().getViewer().addMapMarker(currentWaypoint); + + double currentHeading = gpsM.getHeading(); + getMapTree().getViewer().removeMapPolygon(currentHeadingMapObj); + currentHeadingMapObj = new MapPolygonImpl( + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), + new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(currentHeading), gpsM.getLongitude() + 0.0001 * Math.cos(currentHeading)), + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) + ); + currentHeadingMapObj.setColor(Color.RED); + getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); + + getMapTree().getViewer().removeMapPolygon(currentSteeringCommandMapObj); + currentSteeringCommandMapObj = new MapPolygonImpl( + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), + new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(currentCommandedAngle + currentHeading), + gpsM.getLongitude() + 0.0001 * Math.cos(currentCommandedAngle + currentHeading)), + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) + ); + currentSteeringCommandMapObj.setColor(Color.BLUE); + getMapTree().getViewer().addMapPolygon(currentSteeringCommandMapObj); + } }); + + new Subscriber("map", NodeChannel.GPS.getMsgPath(), ((topicName, m) -> { + GpsMeasurement gps = ((GpsMeasurement) m); + addPointsToMapTree(Color.BLACK, new LocTuple(gps.getLatitude(), gps.getLongitude())); + })); + new Subscriber("map", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { + DriveControlMessage dcm = ((DriveControlMessage) m); + currentWaypoint.setLat(dcm.getWaypoint().getLatitude()); + currentWaypoint.setLon(dcm.getWaypoint().getLongitude()); + currentCommandedAngle = dcm.getAngleDouble(); + }); + } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java index 2c14ec18..4d5edc6d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java @@ -99,7 +99,7 @@ public void actionPerformed(String topicName, Message m) { new Subscriber("uiDriveAngle", NodeChannel.DRIVE_CTRL.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commtheta = ((DriveControlMessage) m).getAngleInt(); + commtheta = (int) Math.toDegrees(((DriveControlMessage) m).getAngleDouble()); } });