

In [1]:

    
%load_ext autoreload
%autoreload 2



In [2]:

    
from __future__ import division, print_function
import itertools as it
from collections import OrderedDict

import matplotlib.pyplot as plt
import numpy as np
import tqdm

from ..src.ros.spirit.src.helpers import Pose, Frame
%matplotlib inline

Let's generate the combinations:



In [3]:

    
def combos(combo_min, combo_max, combo_len):
    for combo in it.product(xrange(combo_min, combo_max + 1),
                            repeat=combo_len):
        yield combo
        
def combo_dicts(param_names, combo_min, combo_max, combo_len):
    for d in (OrderedDict(it.izip(param1_names, combo)) for combo in combos(combo_min, combo_max, combo_len)):
        yield d

We have 5 coefficients. If we make the maximum ratio a 10:1 ratio, and use integer steps, that's 100,000 tests we need to do. 200,000 if we consider that centrality and centrality2 are mutually exclusive. Can we reduce them by taking equivalent ratios? (e.g., [1, 2, 3, 4, 5] and [2, 4, 6, 8, 10] are equivalent.)



In [4]:

    
def reduction_percentage(max_ratio, n_slots=5):
    all_combos = list(combos(1, max_ratio, n_slots))
    return 100*(len(all_combos) - len(set(tuple(np.array(i) / max(i)) for i in all_combos))) / len(all_combos)



In [5]:

    
response = raw_input("This cell takes about 4 minutes to run. Press 'y' to continue: ").lower()
if response == "y":
    x = np.arange(1, 25)
    y = [reduction_percentage(i) for i in tqdm.tqdm_notebook(x)]









    



This cell takes about 4 minutes to run. Press 'y' to continue:



In [6]:

    
if response == "y":
    plt.plot(x, y)
    plt.title("Maximum iteration reduction")
    plt.ylabel("Reduction (%)")
    plt.xlabel("Maximum ratio")

Well, that's not very efficient. As the ratio increases, the computation takes an exponentially longer time. Checking up to 24:1 takes us almost 4 minutes, with almost 55 seconds needed for the last iteration. Even early on, the best improvement was about 3.5%, and that stayed consistent. It's not worth keeping the entire list in memory just for this tiny gain.



In [7]:

    
combo_min = 1
combo_max = 10
combo_len = 5
param1_names = ["centrality", "direction", "distance", "direction_with_current", "distance_with_current"]
param2_names = ["centrality2", "direction", "distance", "direction_with_current", "distance_with_current"]
params1 = combo_dicts(param1_names, combo_min, combo_max, combo_len)
params2 = combo_dicts(param2_names, combo_min, combo_max, combo_len)



In [8]:

    
combo = next(params1)



In [9]:

    
import rospy
rospy.init_node("selector", log_level=rospy.INFO)
rospy.set_param("~ref_distance", 1.5)
rospy.set_param("~image_queue_length", 60)
rospy.set_param("~eval_method", "Spirit")
rospy.set_param("~thresh_distance", 0.01)
rospy.set_param("~thresh_yaw", 0.02)



In [10]:

    
from ..src.ros.spirit.src.past_image_selector import Selector
from ..src.ros.spirit.src.evaluators import Spirit

my_selector = Selector(debug=True)
spirit = Spirit(my_selector)



In [11]:

    
p = Pose.from_components([0, 1, 2], [3, 4, 5, 6])
f = Frame(Pose.generate_stamped([0, 1, 3], [3, 4, 5, 6]), 1)



In [12]:

    
# Create first frame
my_selector.tracked = True
my_selector.pose_callback(p.pose_stamped)
my_selector.image_callback(1)

# Select current frame
my_selector.pose_callback(p.pose_stamped)

# Update current frame
my_selector.pose_callback(p.pose_stamped)



In [13]:

    
my_selector.frames









    Out[13]:





deque([<Frame(<Pose ([0, 1, 2], [3, 4, 5, 6]): 1483629558888520956>)>])



In [14]:

    
spirit.select_best_frame()









    Out[14]:





<Frame(<Pose ([0, 1, 2], [3, 4, 5, 6]): 1483629558888520956>)>



In [15]:

    
spirit._evaluate_frame(p, f)









    Out[15]:





2.0413436692506459



In [16]:

    
sum(coeff * getattr(spirit, fn)(p, f) for fn, coeff in combo.iteritems())









    Out[16]:





0.97777777777777775



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]:



In [ ]: