├── src └── goggles │ ├── __init__.py │ ├── azimuth_bins_test.py │ ├── radar_utilities.py │ ├── base_estimator.py │ ├── base_estimator_mlesac.py │ ├── radar_doppler_model_3D.py │ ├── mlesac.py │ ├── radar_doppler_model_2D.py │ └── orthogonal_distance_regression.py ├── .gitignore ├── radar_odom.png ├── setup.py ├── data ├── 1642_azimuth_bins.csv └── bruteForce.csv ├── README.md ├── package.xml ├── launch ├── goggles_octomap_vrpn.launch └── goggles.launch ├── nodes ├── rscan_octomap_filter.py └── radar_velocity.py ├── CMakeLists.txt └── LICENSE /src/goggles/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | src/goggles/*.pyc 2 | CMakeLists.txt.user 3 | -------------------------------------------------------------------------------- /radar_odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cstahoviak/goggles/HEAD/radar_odom.png -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup() 6 | d['packages'] = ['goggles'] 7 | d['package_dir'] = {'': 'src'} 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /src/goggles/azimuth_bins_test.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import rospy 3 | import numpy as np 4 | import scipy as sp 5 | 6 | from goggles.radar_utilities import RadarUtilities 7 | 8 | def main(): 9 | radar_azimuth_bins = np.genfromtxt('../../data/1642_azimuth_bins.csv', delimiter=',') 10 | # print(radar_azimuth_bins) 11 | 12 | utils = RadarUtilities() 13 | 14 | numAzimuthBins = utils.getNumAzimuthBins( radar_azimuth_bins ) 15 | print(numAzimuthBins) 16 | 17 | 18 | if __name__ == '__main__': 19 | main() 20 | -------------------------------------------------------------------------------- /data/1642_azimuth_bins.csv: -------------------------------------------------------------------------------- 1 | -1.3202 2 | -1.2153 3 | -1.1342 4 | -1.0655 5 | -1.0042 6 | -0.94841 7 | -0.89668 8 | -0.8481 9 | -0.80205 10 | -0.75799 11 | -0.71583 12 | -0.67522 13 | -0.66964 14 | -0.63573 15 | -0.59747 16 | -0.56007 17 | -0.52354 18 | -0.48787 19 | -0.45281 20 | -0.4183 21 | -0.38438 22 | -0.35097 23 | -0.31786 24 | -0.2851 25 | -0.25269 26 | -0.22053 27 | -0.18863 28 | -0.15692 29 | -0.12534 30 | -0.093883 31 | -0.062568 32 | -0.031294 33 | 0 34 | 0.031252 35 | 0.062542 36 | 0.09389 37 | 0.12533 38 | 0.15691 39 | 0.18864 40 | 0.22054 41 | 0.25271 42 | 0.2851 43 | 0.31786 44 | 0.35096 45 | 0.3844 46 | 0.41835 47 | 0.45286 48 | 0.48787 49 | 0.52355 50 | 0.56005 51 | 0.59744 52 | 0.63574 53 | 0.6751 54 | 0.67883 55 | 0.71582 56 | 0.75798 57 | 0.80206 58 | 0.8481 59 | 0.89669 60 | 0.9484 61 | 1.0042 62 | 1.0655 63 | 1.134 64 | 1.2153 65 | 1.3202 66 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # goggles 2 | __A radar-based 3D linear velocity estimation ROS package__ 3 | 4 | The figure below compares the Goggles _radar-only_ odometry method to a newly commercially available [Intel T265 Tracking Camera](https://www.intelrealsense.com/tracking-camera-t265/) visual-inertial odometry system. The Goggles radar-only method in comparable to and in some cases _outperforms_ the T265! 5 | 6 | 7 | 8 | ## The Algorithm 9 | 10 | Goggles estimates the 2D or 3D body-frame linear velocity vector of a sensor platfrom given input data from a single 11 | [TI mmWave Series](http://www.ti.com/sensors/mmwave/overview.html) radar (`/mmWaveDataHdl/RScan` topic). The velocity 12 | estimation scheme takes the following approach: 13 | 14 | 1. Near-field targets are removed from the target list. Many of these targets are artifacts of antenna interference at 15 | the senor origin, and are not representative of real targets in the scene. These near-field targets also exist in the 16 | zero-Doppler bin and thus would currupt the quality of the velocity estimate. 17 | 18 | 2. A Maximum Likelihood Sample and Concesus (MLESAC) outlier rejection method is used to filter targets that can be 19 | attributed to noise or dynamic targets in the environment. MLESAC generates an inlier set of targets and a first-pass 20 | linear velocity estimate derived from the inlier set. 21 | 22 | 3. An Orthogonal Distance Regression ([ODR](http://scholar.colorado.edu/cgi/viewcontent.cgi?article=1311&context=csci_techreports)) 23 | algortihm is seeded with the MLESAC linear velocity estimate and generates a final estimate of the body-frame linear 24 | velocity components. Orthogonal Distance Regression _"is the name given to the computational problem associated with finding the maximum likelihood estimators of parameters in measurement error models in the case of normally distributed errors."_ 25 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | goggles 4 | 0.0.0 5 | The goggles package 6 | 7 | 8 | 9 | 10 | carl 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | sensor_msgs 54 | std_msgs 55 | rospy 56 | sensor_msgs 57 | std_msgs 58 | rospy 59 | sensor_msgs 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /src/goggles/radar_utilities.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Carl Stahoviak 3 | Date Created: Apr 22, 2019 4 | Last Edited: Apr 22, 2019 5 | 6 | Description: 7 | 8 | """ 9 | 10 | from __future__ import division 11 | import rospy 12 | import numpy as np 13 | from functools import reduce 14 | 15 | class RadarUtilities: 16 | 17 | def __init__(self): 18 | pass 19 | 20 | ## filtering of 2D and 3D radar data 21 | def AIRE_filtering(self, data_AIRE, thresholds): 22 | ## unpack data (into row vectors) 23 | radar_azimuth = data_AIRE[:,0] # [rad] 24 | radar_intensity = data_AIRE[:,1] # [dB] 25 | radar_range = data_AIRE[:,2] # [m] 26 | radar_elevation = data_AIRE[:,3] # [rad] 27 | 28 | azimuth_thres = thresholds[0]; # [deg] 29 | intensity_thres = thresholds[1]; # [dB] 30 | range_thres = thresholds[2]; # [m] 31 | elevation_thres = thresholds[3]; # [deg] 32 | 33 | ## Indexing in Python example 34 | ## print("Values bigger than 10 =", x[x>10]) 35 | ## print("Their indices are ", np.nonzero(x > 10)) 36 | idx_azimuth = np.nonzero(np.abs(np.rad2deg(radar_azimuth)) < azimuth_thres); 37 | idx_intensity = np.nonzero(radar_intensity > intensity_thres); 38 | idx_range = np.nonzero(radar_range > range_thres); 39 | 40 | ## combine filters 41 | if np.all(np.isnan(radar_elevation)): 42 | ## 2D radar data 43 | idx_AIRE = reduce(np.intersect1d, (idx_azimuth,idx_intensity,idx_range)) 44 | else: 45 | ## 3D radar data 46 | idx_elevation = np.nonzero(np.abs(np.rad2deg(radar_elevation)) < elevation_thres); 47 | idx_AIRE = reduce(np.intersect1d, (idx_azimuth,idx_intensity,idx_range,idx_elevation)) 48 | 49 | return idx_AIRE 50 | 51 | def getNumAzimuthBins(self, radar_azimuth): 52 | bin_thres = 0.009; # [rad] - empirically determined 53 | 54 | azimuth_bins = np.unique(radar_azimuth); 55 | 56 | bins = []; 57 | current_bin = azimuth_bins[0]; 58 | begin_idx = 0; 59 | 60 | for i in range(azimuth_bins.shape[0]): 61 | if np.abs(current_bin - azimuth_bins[i]) > bin_thres: 62 | ## update location of last angle to be averaged 63 | end_idx = i-1; 64 | 65 | ## add single averaged value to table 66 | azimuth_bin = np.mean(azimuth_bins[begin_idx:end_idx]); 67 | bins.append(azimuth_bin) 68 | 69 | ## set new beginning index 70 | begin_idx = i; 71 | 72 | ## set new current bin 73 | current_bin = azimuth_bins[i]; 74 | 75 | if i == azimuth_bins.shape[0]-1: 76 | ## update location of last angle to be averaged 77 | end_idx = i; 78 | 79 | ## add single averaged value to table 80 | azimuth_bin = np.mean(azimuth_bins[begin_idx:end_idx]); 81 | bins.append(azimuth_bin) 82 | 83 | bins = np.array(bins) 84 | numAzimuthBins = bins.size 85 | 86 | return numAzimuthBins 87 | -------------------------------------------------------------------------------- /launch/goggles_octomap_vrpn.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /nodes/rscan_octomap_filter.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | """ 3 | Author: Carl Stahoviak 4 | Date Created: Nov 07, 2019 5 | Last Edited: Nov 07, 2019 6 | 7 | Task: To remove additional points from the /mmWaveDataHdl/RScanInliers topic for 8 | use with the Octomap mapping package 9 | 10 | """ 11 | 12 | import rospy 13 | import numpy as np 14 | import sensor_msgs.point_cloud2 as pc2 15 | from sensor_msgs.msg import PointCloud2, PointField 16 | 17 | class RScanFilter(): 18 | 19 | def __init__(self): 20 | 21 | ns = rospy.get_namespace() 22 | rospy.loginfo("INIT: namespace = %s", ns) 23 | if ns =='/': 24 | ## empty namespace 25 | ns = ns[1:] 26 | 27 | ## read input parameters 28 | input_pcl_topic = rospy.get_param('~input_pcl') 29 | self.z_thres_ = rospy.get_param('~z_thres') 30 | self.intensity_thres_ = rospy.get_param('~intensity_thres') 31 | 32 | self.pcl_sub = rospy.Subscriber(ns + input_pcl_topic, PointCloud2, self.ptcloud_cb) 33 | rospy.loginfo("INIT: RScanFilter Node subcribed to:\t" + ns + input_pcl_topic) 34 | 35 | rscan_filtered_topic = input_pcl_topic + '/filtered' 36 | self.pc_pub = rospy.Publisher(ns + rscan_filtered_topic, PointCloud2, queue_size=10) 37 | rospy.loginfo("INIT: RScanFilter Node publishing on:\t" + ns + rscan_filtered_topic) 38 | 39 | rospy.loginfo("INIT: RScanFilter Node Initialized") 40 | 41 | def ptcloud_cb(self, radar_msg): 42 | pts_list = list(pc2.read_points(radar_msg, field_names=["x", "y", "z", "intensity", "range", "doppler"])) 43 | pts = np.array(pts_list, dtype=np.float32) 44 | 45 | ## pts.shape = (Ntargets, 6) 46 | if pts.shape[0] == 0: 47 | rospy.logwarn("filter_rscan: EMPTY RADAR MESSAGE") 48 | else: 49 | self.filter_rscan(pts, radar_msg) 50 | 51 | def filter_rscan(self, pts, radar_msg): 52 | 53 | ## remove points outside of specified z threshold 54 | radar_z = pts[:,2] 55 | radar_intensity = pts[:,3] 56 | 57 | idx_z = np.nonzero( np.abs(radar_z) < self.z_thres_ ) 58 | idx_int = np.nonzero( radar_intensity > self.intensity_thres_ ) 59 | idx = reduce(np.intersect1d,(idx_z,idx_int)) 60 | pts_filtered = pts[idx,:] 61 | 62 | self.publish_filtered_rscan(pts_filtered, radar_msg) 63 | 64 | def publish_filtered_rscan(self, pts_filtered, radar_msg): 65 | 66 | rscan_filtered = PointCloud2() 67 | 68 | rscan_filtered.header = radar_msg.header 69 | 70 | rscan_filtered.height = 1 71 | rscan_filtered.width = pts_filtered.shape[0] 72 | rscan_filtered.is_bigendian = False 73 | rscan_filtered.point_step = 24 74 | rscan_filtered.row_step = rscan_filtered.width * rscan_filtered.point_step 75 | rscan_filtered.is_dense = True 76 | 77 | ## this is not being done correctly... 78 | rscan_filtered.fields = [ 79 | PointField('x', 0, PointField.FLOAT32, 1), 80 | PointField('y', 4, PointField.FLOAT32, 1), 81 | PointField('z', 8, PointField.FLOAT32, 1), 82 | PointField('intensity', 12, PointField.FLOAT32, 1), 83 | PointField('range', 16, PointField.FLOAT32, 1), 84 | PointField('doppler', 20, PointField.FLOAT32, 1), 85 | ] 86 | 87 | data = np.reshape(pts_filtered,(pts_filtered.shape[0]*pts_filtered.shape[1],)) 88 | rscan_filtered.data = data.tostring() 89 | self.pc_pub.publish(rscan_filtered) 90 | 91 | def main(): 92 | ## anonymous=True ensures that your node has a unique name by adding random numbers to the end of NAME 93 | rospy.init_node('rscan_filter_node') 94 | rscan_filter = RScanFilter() 95 | 96 | try: 97 | rospy.spin() 98 | except rospy.ROSInterruptException: 99 | sys.exit() 100 | 101 | if __name__ == '__main__': 102 | main() 103 | -------------------------------------------------------------------------------- /launch/goggles.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/goggles/base_estimator.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Carl Stahoviak 3 | Date Created: Apr 30, 2019 4 | Last Edited: Apr 30, 2019 5 | 6 | Description: 7 | Base Estimator class for RANSAC Regression. 8 | 9 | """ 10 | 11 | import rospy 12 | import numpy as np 13 | from sklearn.base import BaseEstimator, RegressorMixin 14 | from goggles.radar_utilities import RadarUtilities 15 | 16 | class dopplerRANSAC(BaseEstimator, RegressorMixin): 17 | 18 | def __init__(self, model): 19 | # ascribe doppler velocity model (2D, 3D) to the class 20 | self.model = model 21 | self.utils = RadarUtilities() 22 | 23 | self.sample_size = self.model.min_pts # the minimum number of data values required to fit the model 24 | self.max_iterations = 25 # the maximum number of iterations allowed in the algorithm 25 | self.max_distance = 0.15 # a threshold value for determining when a data point fits a model 26 | 27 | # body-frame velocity vector - to be estimated by RANSAC 28 | self.param_vec_ = None 29 | 30 | # fit(X,y): Fit model to given training data and target values 31 | def fit(self, X, y): 32 | radar_azimuth = np.squeeze(X) 33 | radar_doppler = np.squeeze(y) 34 | 35 | rospy.loginfo("base_estimator.fit: sample size = " + str(radar_doppler.shape[0])) 36 | model = self.model.doppler2BodyFrameVelocity(radar_doppler, radar_azimuth) 37 | # rospy.loginfo("fit: model = " + str(model)) 38 | self.param_vec_ = model 39 | return self 40 | 41 | # predict(X): Returns predicted values used to compute residual error using loss function 42 | def predict(self, X): 43 | radar_azimuth = np.squeeze(X) 44 | Ntargets = radar_azimuth.shape[0] 45 | 46 | doppler_predicted = self.model.simulateRadarDoppler(self.param_vec_, \ 47 | radar_azimuth, np.zeros((Ntargets,), dtype=np.float32), \ 48 | np.zeros((Ntargets,), dtype=np.float32)) 49 | 50 | # rospy.loginfo("predict: doppler_predicted = \n" + str(doppler_predicted)) 51 | return doppler_predicted 52 | 53 | def loss(self, y, y_pred): 54 | dist = np.sqrt(np.square(np.squeeze(y) - y_pred)) 55 | # rospy.loginfo("loss: dist.shape = " + str(dist.shape)) 56 | return dist 57 | 58 | 59 | # Don't need to define score(X,y) if inherited from RegressorMixin... I think 60 | # score(X,y): Returns the mean accuracy on the given test data, which is used 61 | # for the stop criterion defined by stop_score 62 | # def score(self, X, y): 63 | # radar_azimuth = np.squeeze(X) 64 | # radar_doppler = np.squeeze(y) 65 | # Ntargets = radar_azimuth.shape[0] 66 | # 67 | # rospy.loginfo("score: radar_azimuth.shape = " + str(radar_azimuth.shape)) 68 | # rospy.loginfo("score: radar_doppler.shape = " + str(radar_doppler.shape)) 69 | # 70 | # doppler_predicted = self.model.simulateRadarDoppler(self.param_vec_, \ 71 | # radar_azimuth, np.zeros((Ntargets,), dtype=float), \ 72 | # np.zeros((Ntargets,), dtype=float)) 73 | # 74 | # rospy.loginfo("score: radar_doppler = " + str(radar_doppler)) 75 | # rospy.loginfo("score: doppler_predicted = " + str(doppler_predicted)) 76 | # 77 | # dist = np.sqrt(np.square(radar_doppler - doppler_predicted)) 78 | # rospy.loginfo("score: dist = \n" + str(dist)) 79 | # return np.mean(dist, axis=0) 80 | 81 | 82 | def is_data_valid(self, X, y): 83 | radar_azimuth = np.squeeze(X) 84 | radar_doppler = np.squeeze(y) 85 | 86 | numAzimuthBins = self.utils.getNumAzimuthBins(radar_azimuth) 87 | # rospy.loginfo("is_data_valid: numAzimuthBins = " + str(numAzimuthBins)) 88 | 89 | ## BUG: (2019-07-18) On the final iteration of RANSAC, the sample size 90 | ## exceeds the value defined by the sampleSize parameter. The number of 91 | ## data points in this larger sample is not consistent from one radar 92 | ## scan to the next. This bug acually does not originate from here... 93 | 94 | ## This function is NOT called prior to all Ninlier data points being 95 | ## passed to the fit fcn prior to RANSAC exiting... why is this 96 | ## behavior happening? 97 | 98 | # if numAzimuthBins > self.model.sampleSize: 99 | if numAzimuthBins == self.model.sampleSize : 100 | is_valid = True 101 | else: 102 | is_valid = False 103 | 104 | rospy.loginfo("base_estimator.is_data_valid: is_valid = " + str(is_valid)) 105 | return is_valid 106 | -------------------------------------------------------------------------------- /src/goggles/base_estimator_mlesac.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Carl Stahoviak 3 | Date Created: July 25, 2019 4 | Last Edited: July 25, 2019 5 | 6 | Description: 7 | Base Estimator class for MLESAC Regression. 8 | 9 | """ 10 | from __future__ import division 11 | import numpy as np 12 | from goggles.radar_utilities import RadarUtilities 13 | 14 | class dopplerMLESAC(): 15 | 16 | def __init__(self, model): 17 | ## ascribe doppler velocity model (2D, 3D) to the class 18 | self.model = model 19 | self.utils = RadarUtilities() 20 | 21 | ## define MLESAC parameters 22 | self.sample_size = self.model.min_pts # the minimum number of data values required to fit the model 23 | self.max_iterations = 40 # the maximum number of iterations allowed in the algorithm 24 | self.max_distance = 0.15 # a threshold value for determining when a data point fits a model 25 | self.converge_thres = 10 # change in data log likelihood fcn required to indicate convergence 26 | 27 | self.param_vec_ = None # body-frame velocity vector - to be estimated by MLESAC 28 | self.param_vec_mlesac_ = None # array_like param_vec_ with shape (n,) 29 | self.param_vec_ols_ = None # array_like param_vec_ with shape (n,) 30 | self.covariance_ = None # covariance of parameter estimate, shape (p,p) 31 | 32 | ## model fit fcn 33 | def fit(self, data): 34 | self.param_vec_ = self.model.doppler2BodyFrameVelocity(data) 35 | return self 36 | 37 | ## distance(s) from data point(s) to model 38 | def distance(self, data): 39 | ## TODO: use residual function as part of computing the distances 40 | Ntargets = data.shape[0] 41 | p = self.sample_size 42 | 43 | radar_doppler = data[:,0] # [m/s] 44 | radar_azimuth = data[:,1] # [rad] 45 | radar_elevation = data[:,2] # [rad] 46 | 47 | ## do NOT corrupt measurements with noise 48 | eps = np.zeros((Ntargets,), dtype=np.float32) 49 | delta = np.zeros(((p-1)*Ntargets,), dtype=np.float32) 50 | 51 | ## compute distances via residual 52 | eps_sq = np.square( self.residual(self.param_vec_, data) ) 53 | distances = np.sqrt(eps_sq) 54 | 55 | ## distance per data point (column vector) 56 | return distances 57 | 58 | ## evaluate the data log likelihood of the data given the model - P(evidence | model) 59 | def score(self, data, type): 60 | ## TODO: use residual function as part of computing the score 61 | Ntargets = data.shape[0] 62 | p = self.sample_size 63 | 64 | radar_doppler = data[:,0] 65 | radar_azimuth = data[:,1] 66 | radar_elevaton = data[:,2] 67 | 68 | if type == 'mlesac': 69 | model = self.param_vec_mlesac_ 70 | elif type == 'ols': 71 | model = self.param_vec_ols_ 72 | else: 73 | model = self.param_vec_ 74 | 75 | ## evaluate the data log-likelihood given the model 76 | eps_sq = np.square( self.residual(self.param_vec_, data) ) 77 | score = -1/(2*self.model.sigma_vr**2)*np.sum(eps_sq) 78 | 79 | return score 80 | 81 | ## returns residuals vector (n,) 82 | def residual(self, X, data): 83 | Ntargets = data.shape[0] 84 | p = self.sample_size 85 | 86 | radar_doppler = data[:,0] 87 | radar_azimuth = data[:,1] 88 | radar_elevaton = data[:,2] 89 | 90 | doppler_predicted = self.model.simulateRadarDoppler( 91 | X, \ 92 | np.column_stack((radar_azimuth,radar_elevaton)), \ 93 | np.zeros((Ntargets,), dtype=np.float32), \ 94 | np.zeros(((p-1)*Ntargets,), dtype=np.float32)) 95 | 96 | eps = doppler_predicted - radar_doppler 97 | return eps 98 | 99 | ## returns Jacobain matrix, partial_eps/partial_beta (n,p) 100 | def jac(self, X, data): 101 | ## TODO: move this calculation over to the radar_doppler_model class 102 | Ntargets = data.shape[0] # X is a column vector of azimuth values 103 | 104 | theta = data[:,1] # azimuth angle column vector [rad] 105 | phi = data[:,2] # elevation angle column vector [rad] 106 | 107 | # initialize 108 | J = np.zeros((Ntargets,self.sample_size), dtype=np.float32) 109 | 110 | for i in range(Ntargets): 111 | if self.sample_size == 2: 112 | J[i,:] = np.array([np.cos(theta[i]),np.sin(theta[i])]) 113 | 114 | elif self.sample_size == 3: 115 | J[i,:] = np.array([np.cos(theta[i])*np.cos(phi[i]), \ 116 | np.sin(theta[i])*np.sin(phi[i]), \ 117 | np.sin(phi[i]) ]) 118 | else: 119 | raise ValueError("Jacobian Error") 120 | 121 | return J 122 | 123 | 124 | def is_data_valid(self, data): 125 | if data.shape[0] != self.sample_size: 126 | raise ValueError("data must be an 2x2 or 3x3 square matrix") 127 | 128 | if self.sample_size == 2: 129 | radar_doppler = data[:,0] 130 | radar_azimuth = data[:,1] 131 | 132 | numAzimuthBins = self.utils.getNumAzimuthBins(radar_azimuth) 133 | 134 | if numAzimuthBins > 1: 135 | is_valid = True 136 | else: 137 | is_valid = False 138 | 139 | elif self.sample_size == 3: 140 | radar_doppler = data[:,0] 141 | radar_azimuth = data[:,1] 142 | radar_elevation = data[:,2] 143 | 144 | numAzimuthBins = self.utils.getNumAzimuthBins(radar_azimuth) 145 | numElevBins = self.utils.getNumAzimuthBins(radar_elevation) 146 | 147 | if numAzimuthBins + numElevBins > 4: 148 | is_valid = True 149 | else: 150 | is_valid = False 151 | else: 152 | raise ValueError("data must be an Nx2 or Nx3 matrix") 153 | 154 | return is_valid 155 | -------------------------------------------------------------------------------- /src/goggles/radar_doppler_model_3D.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Carl Stahoviak 3 | Date Created: Apr 22, 2019 4 | Last Edited: Apr 22, 2019 5 | 6 | Description: 7 | 8 | """ 9 | from __future__ import division 10 | import rospy 11 | import numpy as np 12 | from goggles.radar_utilities import RadarUtilities 13 | 14 | class RadarDopplerModel3D: 15 | 16 | def __init__(self): 17 | self.utils = RadarUtilities() 18 | 19 | ## define radar uncertainty parameters 20 | self.sigma_vr = 0.044 # [m/s] 21 | self.sigma_theta = 0.0426 # [rad] 22 | self.sigma_phi = self.sigma_theta # [rad] 23 | self.sigma = np.array((self.sigma_theta,self.sigma_phi)) 24 | 25 | ## define ODR error variance ratio 26 | self.d = np.array((self.sigma_vr / self.sigma_theta, \ 27 | self.sigma_vr / self.sigma_phi)) 28 | 29 | self.min_pts = 3 # minimum number of data points to fit the model 30 | 31 | # inverse measurement model: measurements->model 32 | def doppler2BodyFrameVelocity(self, data): 33 | radar_doppler = data[:,0] # doppler velocity [m/s] 34 | theta = data[:,1] # azimuth angle column vector [rad] 35 | phi = data[:,2] # elevation angle column vector [rad] 36 | 37 | numAzimuthBins = self.utils.getNumAzimuthBins(theta) 38 | numElevBins = self.utils.getNumAzimuthBins(phi) 39 | 40 | if numAzimuthBins + numElevBins > 4: 41 | ## solve uniquely-determined problem for three targets 42 | M = np.array([ [np.cos(theta[0])*np.cos(phi[0]), np.sin(theta[0])*np.cos(phi[0]), np.sin(phi[0])], \ 43 | [np.cos(theta[1])*np.cos(phi[1]), np.sin(theta[1])*np.cos(phi[1]), np.sin(phi[1])], \ 44 | [np.cos(theta[2])*np.cos(phi[2]), np.sin(theta[2])*np.cos(phi[2]), np.sin(phi[2])] ]) 45 | 46 | b = np.array([ [radar_doppler[0]], \ 47 | [radar_doppler[1]], \ 48 | [radar_doppler[2]] ]) 49 | 50 | model = np.squeeze(np.linalg.solve(M,b)) 51 | else: 52 | model = float('nan')*np.ones((3,)) 53 | 54 | return model 55 | 56 | # measurement generative (forward) model: model->measurements 57 | def simulateRadarDoppler(self, model, data, eps, delta): 58 | Ntargets = data.shape[0] 59 | m = int(delta.shape[0] / Ntargets) 60 | 61 | # unpack radar data 62 | radar_azimuth = data[:,0] 63 | radar_elevation = data[:,1] 64 | 65 | ## "un-interleave" delta vector into (Ntargets x m) matrix 66 | delta = delta.reshape((Ntargets,m)) 67 | delta_theta = delta[:,0] 68 | delta_phi = delta[:,1] 69 | 70 | ## init radar Doppler vector 71 | radar_doppler = np.zeros((Ntargets,), dtype=np.float32) 72 | 73 | for i in range(Ntargets): 74 | ## add measurement noise distributed as N(0,sigma_theta) 75 | theta = radar_azimuth[i] + delta_theta[i] 76 | 77 | ## add measurement noise distributed as N(0,sigma_phi) 78 | phi = radar_elevation[i] + delta_phi[i] 79 | 80 | ## add meaurement noise epsilon distributed as N(0,sigma_vr) 81 | radar_doppler[i] = model[0]*np.cos(theta)*np.cos(phi) + \ 82 | model[1]*np.sin(theta)*np.cos(phi) + model[2]*np.sin(phi) 83 | 84 | ## add meaurement noise distributed as N(0,sigma_vr) 85 | radar_doppler[i] = radar_doppler[i] + eps[i] 86 | 87 | return radar_doppler 88 | 89 | 90 | def getBruteForceEstimate(self, radar_doppler, radar_azimuth): 91 | pass 92 | 93 | def getSimulatedRadarMeasurements(self, Ntargets, model, radar_angle_bins, \ 94 | sigma_vr, debug=False): 95 | p = model.shape[0] 96 | 97 | # generate ptcloud of simulated targets 98 | ptcloud = self.generatePointcloud3D(Ntargets) 99 | 100 | radar_x = ptcloud[:,0] 101 | radar_y = ptcloud[:,1] 102 | radar_z = ptcloud[:,2] 103 | 104 | ## generate truth data 105 | radar_range = np.sqrt(radar_x**2 + radar_y**2 + radar_z**2) 106 | true_azimuth = np.arctan(np.divide(radar_y,radar_x)) 107 | true_elevation = np.arcsin(np.divide(radar_z,radar_range)) 108 | 109 | ## init simulated data vectors 110 | radar_azimuth = np.zeros((Ntargets,), dtype=np.float32) 111 | radar_elevation = np.zeros((Ntargets,), dtype=np.float32) 112 | 113 | # bin azimuth and elevation data 114 | for i in range(Ntargets): 115 | bin_idx = (np.abs(radar_angle_bins - true_azimuth[i])).argmin() 116 | ## could additionally add Gaussian noise here 117 | radar_azimuth[i] = radar_angle_bins[bin_idx] 118 | 119 | bin_idx = (np.abs(radar_angle_bins - true_elevation[i])).argmin() 120 | ## could additionally add Gaussian noise here 121 | radar_elevation[i] = radar_angle_bins[bin_idx] 122 | 123 | ## define AGWN vector for doppler velocity measurements 124 | if debug: 125 | eps = np.ones((Ntargets,), dtype=np.float32)*sigma_vr 126 | else: 127 | eps = np.random.normal(0,sigma_vr,(Ntargets,)) 128 | 129 | ## get true radar doppler measurements 130 | true_doppler = self.simulateRadarDoppler(model, np.column_stack((true_azimuth,true_elevation)), \ 131 | np.zeros((Ntargets,), dtype=np.float32), np.zeros(((p-1)*Ntargets,), dtype=np.float32)) 132 | 133 | # get noisy radar doppler measurements 134 | radar_doppler = self.simulateRadarDoppler(model, np.column_stack((radar_azimuth,radar_elevation)), \ 135 | eps, np.zeros(((p-1)*Ntargets,), dtype=np.float32)) 136 | 137 | data_truth = np.column_stack((true_doppler,true_azimuth,true_elevation)) 138 | data_sim = np.column_stack((radar_doppler,radar_azimuth,radar_elevation)) 139 | 140 | return data_truth, data_sim 141 | 142 | def generatePointcloud3D(self, Ntargets): 143 | ## need to update this function to only generate targets within a 144 | ## specifed azimuth and elevation FOV - these ranges taken as inputs 145 | ## to this function 146 | 147 | min_x = 0 148 | max_x = 10 149 | ptcloud_x = (max_x - min_x)*np.random.random((Ntargets,)) + min_x 150 | 151 | min_y = -10 152 | max_y = 10 153 | ptcloud_y = (max_y - min_y)*np.random.random((Ntargets,)) + min_y 154 | 155 | min_z = -10 156 | max_z = 10 157 | ptcloud_z = (max_z - min_z)*np.random.random((Ntargets,)) + min_z 158 | 159 | pointcloud = np.column_stack((ptcloud_x, ptcloud_y, ptcloud_z)) 160 | 161 | return pointcloud 162 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(goggles) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | sensor_msgs 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # sensor_msgs# std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES goggles 108 | # CATKIN_DEPENDS rospy sensor_msgs std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/goggles.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/goggles_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_goggles.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /src/goggles/mlesac.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | """ 3 | Author: Carl Stahoviak 4 | Date Created: July 23, 2019 5 | Last Edited: July 24, 2019 6 | 7 | Description: 8 | 9 | """ 10 | from __future__ import division 11 | import time 12 | import numpy as np 13 | from functools import partial, reduce 14 | 15 | from scipy.optimize import least_squares 16 | from goggles.radar_utilities import RadarUtilities 17 | from goggles.radar_doppler_model_2D import RadarDopplerModel2D 18 | from goggles.radar_doppler_model_3D import RadarDopplerModel3D 19 | from goggles.base_estimator_mlesac import dopplerMLESAC 20 | # from sklearn.linear_model import RANSACRegressor 21 | # from goggles.base_estimator import dopplerRANSAC 22 | 23 | class MLESAC: 24 | 25 | def __init__(self, base_estimator, report_scores=False, ols_flag=False, get_covar=False): 26 | self.estimator_ = base_estimator 27 | 28 | self.inliers_ = None # inlier data points 29 | self.scores_ = None # data log likelihood associated with each iteration 30 | self.iter_ = None # number of iterations until convergence 31 | 32 | self.report_scores = report_scores # report data log likelihood of each iteration 33 | self.ols_flag = ols_flag # enable OLS solution on inlier set 34 | self.get_covar = get_covar # return estimate covariance? 35 | 36 | def mlesac(self, data): 37 | 38 | Ntargets = data.shape[0] # data.shape = (Ntargets,p) 39 | 40 | bestScore = -np.inf 41 | bestInliers = [] 42 | bestModel = [] 43 | scores = [] 44 | 45 | dll_incr = np.inf # increase in data log likelihood function 46 | iter = 0 # algorithm iteration Number 47 | 48 | while np.abs(dll_incr) > self.estimator_.converge_thres and \ 49 | iter < self.estimator_.max_iterations: 50 | 51 | ## randomly sample from data 52 | idx = np.random.randint(Ntargets,high=None,size=(self.estimator_.sample_size,)) 53 | sample = data[idx,:] 54 | 55 | is_valid = self.estimator_.is_data_valid(sample) 56 | if is_valid: 57 | ## estimate model parameters from sampled data points 58 | param_vec_temp = self.estimator_.param_vec_ 59 | self.estimator_.fit(sample) 60 | 61 | ## score the model - evaluate the data log likelihood fcn 62 | score = self.estimator_.score(data,type=None) 63 | 64 | if score > bestScore: 65 | ## this model better explains the data 66 | distances = self.estimator_.distance(data) 67 | 68 | dll_incr = score - bestScore # increase in data log likelihood fcn 69 | bestScore = score 70 | bestInliers = np.nonzero((distances < self.estimator_.max_distance)) 71 | 72 | if self.report_scores: 73 | scores.append(score) 74 | 75 | # evaluate stopping criteria - not yet used 76 | # Ninliers = sum(bestInliers) 77 | # w = Ninliers/Ntargets 78 | # k = np.log(1-0.95)*np.log(1-w**2) 79 | else: 80 | ## candidate param_vec_ did NOT have a higher score 81 | self.estimator_.param_vec_ = param_vec_temp 82 | 83 | iter+=1 84 | # print("iter = " + str(iter) + "\tscore = " + str(score)) 85 | else: 86 | ## do nothing - cannot derive a valid model from targets in 87 | ## the same azimuth/elevation bins 88 | 89 | # print("mlesac: INVALID DATA SAMPLE") 90 | pass 91 | 92 | self.estimator_.param_vec_mlesac_ = self.estimator_.param_vec_ 93 | self.inliers_ = reduce(np.intersect1d,(bestInliers)) 94 | self.scores_ = np.array(scores) 95 | self.iter_ = iter 96 | 97 | ## get OLS solution on inlier set 98 | if self.ols_flag: 99 | # callable = partial(self.estimator_.residual, data=data) 100 | # ols_soln = least_squares(callable, self.estimator_.param_vec_) 101 | 102 | ols_soln = least_squares(self.estimator_.residual, \ 103 | self.estimator_.param_vec_, self.estimator_.jac, \ 104 | kwargs={"data": data[self.inliers_,:]}) 105 | self.estimator_.param_vec_ols_ = ols_soln.x 106 | 107 | ## score both estimates 108 | score_mlesac = self.estimator_.score(data[self.inliers_,:],'mlesac') 109 | score_ols = self.estimator_.score(data[self.inliers_,:],'ols') 110 | 111 | if score_ols > score_mlesac: 112 | ## OLS solution is better than MLESAC solution 113 | self.estimator_.param_vec_ = self.estimator_.param_vec_ols_ 114 | 115 | ##TODO: maybe re-evaulate inliers?? 116 | else: 117 | ## do nothing - MLESAC solution is better than OLS solution 118 | pass 119 | 120 | if self.get_covar: 121 | eps = ols_soln.fun # residual vector at solution 122 | jac = ols_soln.jac # modified Jacobian matrix at solution 123 | 124 | self.estimator_.covariance_ = np.matmul(eps.T,eps) * \ 125 | np.linalg.inv(np.matmul(jac.T,jac)) 126 | 127 | else: 128 | self.estimator_.param_vec_ols_ = \ 129 | float('nan')*np.ones((self.estimator_.sample_size,)) 130 | 131 | return 132 | 133 | 134 | def test(model): 135 | ## define MLESAC parameters 136 | report_scores = False 137 | ols_flag = True 138 | get_covar = True 139 | 140 | # init instance of MLESAC class 141 | base_estimator_mlesac = dopplerMLESAC(model) 142 | mlesac = MLESAC(base_estimator_mlesac, report_scores, ols_flag, get_covar) 143 | 144 | ## instantiate scikit-learn RANSAC object with base_estimator class object 145 | # base_estimator_ransac = dopplerRANSAC(model=model) 146 | # ransac = RANSACRegressor(base_estimator=base_estimator_ransac, \ 147 | # min_samples=base_estimator_ransac.sample_size, \ 148 | # residual_threshold=base_estimator_ransac.max_distance, \ 149 | # is_data_valid=base_estimator_ransac.is_data_valid, \ 150 | # max_trials=base_estimator_ransac.max_iterations, \ 151 | # loss=base_estimator_ransac.loss) 152 | 153 | ## outlier std deviation 154 | sigma_vr_outlier = 1.5 155 | 156 | radar_angle_bins = np.genfromtxt('../../data/1642_azimuth_bins.csv', delimiter=',') 157 | 158 | ## simulated 'true' platform velocity range 159 | min_vel = -2.5 # [m/s] 160 | max_vel = 2.5 # [m/s] 161 | 162 | ## number of simulated targets 163 | Ninliers = 125 164 | Noutliers = 35 165 | 166 | ## generate truth velocity vector 167 | velocity = (max_vel-min_vel)*np.random.random((base_estimator_mlesac.sample_size,)) + min_vel 168 | 169 | ## create noisy INLIER simulated radar measurements 170 | _, inlier_data = model.getSimulatedRadarMeasurements(Ninliers, \ 171 | velocity,radar_angle_bins,model.sigma_vr) 172 | 173 | ## create noisy OUTLIER simulated radar measurements 174 | _, outlier_data = model.getSimulatedRadarMeasurements(Noutliers, \ 175 | velocity,radar_angle_bins,sigma_vr_outlier) 176 | 177 | ## combine inlier and outlier data sets 178 | Ntargets = Ninliers + Noutliers 179 | radar_doppler = np.concatenate((inlier_data[:,0],outlier_data[:,0]),axis=0) 180 | radar_azimuth = np.concatenate((inlier_data[:,1],outlier_data[:,1]),axis=0) 181 | radar_elevation = np.concatenate((inlier_data[:,2],outlier_data[:,2]),axis=0) 182 | 183 | ## get MLESAC estimate + inlier set 184 | start_time = time.time() 185 | radar_data = np.column_stack((radar_doppler,radar_azimuth,radar_elevation)) 186 | mlesac.mlesac(radar_data) 187 | model_mlesac = mlesac.estimator_.param_vec_mlesac_ 188 | model_ols = mlesac.estimator_.param_vec_ols_ 189 | inliers = mlesac.inliers_ 190 | time_mlesac = time.time() - start_time 191 | 192 | ## get scikit-learn RANSAC estimate + inlier set 193 | ## NOTE: DOES NOT WORK YET 194 | # start_time = time.time() 195 | # ransac.fit(radar_data) 196 | # model_ransac = np.squeeze(self.ransac.estimator_.param_vec_) 197 | # inlier_mask = self.ransac.inlier_mask_ 198 | # outlier_mask = np.logical_not(inlier_mask) 199 | # time_ransac = time.time() - start_time 200 | 201 | print("\nMLESAC Velocity Profile Estimation:\n") 202 | print("Truth\t MLESAC\t\tMLESAC+OLS") 203 | for i in range(base_estimator_mlesac.sample_size): 204 | print(str.format('{0:.4f}',velocity[i]) + "\t " + str.format('{0:.4f}',model_mlesac[i]) \ 205 | + " \t" + str.format('{0:.4f}',model_ols[i])) 206 | 207 | rmse_mlesac = np.sqrt(np.mean(np.square(velocity - model_mlesac))) 208 | print("\nRMSE (MLESAC)\t= " + str.format('{0:.4f}',rmse_mlesac) + " m/s") 209 | 210 | if mlesac.ols_flag: 211 | rmse_ols = np.sqrt(np.mean(np.square(velocity - model_ols))) 212 | print("RMSE (OLS)\t= " + str.format('{0:.4f}',rmse_ols) + " m/s") 213 | 214 | print("\nExecution Time = %s" % time_mlesac) 215 | 216 | def test_montecarlo(model): 217 | pass 218 | 219 | if __name__=='__main__': 220 | # model = RadarDopplerModel2D() 221 | model = RadarDopplerModel3D() 222 | test(model) 223 | -------------------------------------------------------------------------------- /src/goggles/radar_doppler_model_2D.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Carl Stahoviak 3 | Date Created: Apr 22, 2019 4 | Last Edited: Apr 22, 2019 5 | 6 | Description: 7 | 8 | """ 9 | 10 | from __future__ import division 11 | import rospy 12 | import numpy as np 13 | from goggles.radar_utilities import RadarUtilities 14 | 15 | class RadarDopplerModel2D: 16 | 17 | def __init__(self): 18 | self.utils = RadarUtilities() 19 | 20 | ## define radar uncertainty parameters 21 | self.sigma_vr = 0.044 # [m/s] 22 | self.sigma_theta = 0.0426 # [rad] 23 | self.sigma = self.sigma_theta 24 | 25 | ## define ODR error variance ratio 26 | self.d = self.sigma_vr / self.sigma_theta 27 | 28 | self.min_pts = 2 # minimum number of data points to fit the model of close data values required to assert that a model fits well to data 29 | 30 | # defined for RANSAC - not used 31 | def fit(self, data): 32 | radar_doppler = data[:,0] # [m/s] 33 | radar_azimuth = data[:,1] # [rad] 34 | 35 | model = self.doppler2BodyFrameVelocity(radar_doppler, radar_azimuth) 36 | return model 37 | 38 | 39 | # defined for RANSAC - not used 40 | def get_error(self, data, model): 41 | ## number of targets in scan 42 | Ntargets = data.shape[0] 43 | error = np.zeros((Ntargets,), dtype=np.float32) 44 | 45 | radar_doppler = data[:,0] # [m/s] 46 | radar_azimuth = data[:,1] # [rad] 47 | 48 | ## do NOT corrupt measurements with noise 49 | eps = np.zeros((Ntargets,), dtype=np.float32) 50 | delta = np.zeros((Ntargets,), dtype=np.float32) 51 | 52 | ## radar doppler generative model 53 | doppler_predicted = self.simulateRadarDoppler(model, radar_azimuth, eps, delta) 54 | 55 | for i in range(Ntargets): 56 | error[i] = np.sqrt((doppler_predicted[i] - radar_doppler[i])**2) 57 | 58 | ## error per data point (column vector) 59 | return error 60 | # return np.squeeze(error) 61 | 62 | 63 | # inverse measurement model: measurements->model 64 | def doppler2BodyFrameVelocity(self, data): 65 | radar_doppler = data[:,0] # doppler velocity [m/s] 66 | radar_azimuth = data[:,1] # azimuth angle column vector [rad] 67 | 68 | numAzimuthBins = self.utils.getNumAzimuthBins(radar_azimuth) 69 | 70 | # rospy.loginfo("doppler2BodyFrameVelocity: numAzimuthBins = %d", numAzimuthBins) 71 | # rospy.loginfo(['{0:5.4f}'.format(i) for i in radar_azimuth]) # 'list comprehension' 72 | # # rospy.loginfo(['{0:5.4f}'.format(i) for i in radar_doppler]) # 'list comprehension' 73 | 74 | if numAzimuthBins > 1: 75 | ## solve uniquely-determined problem for pair of targets (i,j) 76 | M = np.array([[np.cos(radar_azimuth[0]), np.sin(radar_azimuth[0])], \ 77 | [np.cos(radar_azimuth[1]), np.sin(radar_azimuth[1])]]) 78 | 79 | b = np.array([[radar_doppler[0]], \ 80 | [radar_doppler[1]]]) 81 | 82 | model = np.squeeze(np.linalg.solve(M,b)) 83 | else: 84 | model = float('nan')*np.ones((2,)) 85 | 86 | return model 87 | 88 | 89 | # measurement generative (forward) model: model->measurements 90 | def simulateRadarDoppler(self, model, data, eps, delta): 91 | Ntargets = data.shape[0] 92 | radar_doppler = np.zeros((Ntargets,), dtype=np.float32) 93 | 94 | radar_azimuth = data[:,0] 95 | 96 | for i in range(Ntargets): 97 | ## add measurement noise distributed as N(0,sigma_theta_i) 98 | theta = radar_azimuth[i] + delta[i] 99 | 100 | ## add meaurement noise epsilon distributed as N(0,sigma_vr) 101 | radar_doppler[i] = model[0]*np.cos(theta) + model[1]*np.sin(theta) + eps[i] 102 | 103 | return radar_doppler 104 | 105 | 106 | def getBruteForceEstimate(self, radar_doppler, radar_azimuth): 107 | ## number of targets in scan 108 | Ntargets = radar_doppler.shape[0] 109 | iter = (Ntargets-1)*Ntargets/2 110 | 111 | if Ntargets > 1: 112 | ## initialize velocity estimate vector 113 | v_hat = np.zeros((2,iter), dtype=np.float32) 114 | 115 | k = 0 116 | for i in range(Ntargets-1): 117 | for j in range(i+1,Ntargets): 118 | doppler = np.array([radar_doppler[i], radar_doppler[j]]) 119 | azimuth = np.array([radar_azimuth[i], radar_azimuth[j]]) 120 | 121 | model = self.doppler2BodyFrameVelocity(doppler, azimuth) 122 | ## don't understand why I need to transpose an ndarray of shape (2,1) 123 | v_hat[:,k] = np.transpose(model) 124 | 125 | k+=1 126 | 127 | ## identify non-NaN solutions to uniquely-determined problem 128 | idx_nonNaN = np.isfinite(v_hat[1,:]) 129 | v_hat_nonNaN = v_hat[:,idx_nonNaN] 130 | 131 | if v_hat_nonNaN.shape[1] == 0: 132 | ## there exists no unique solution to the uniquely-determined 133 | ## problem for any two targets in the scan. This is the result of M 134 | ## being close to singular for all pairs of targets, i.e. the 135 | ## targets have identical angular locations. 136 | v_hat_all = float('nan')*np.ones((2,)) 137 | 138 | else: 139 | ## cannot solve uniquely-determined problem for a single target 140 | ## (solution requires 2 non-identical targets) 141 | v_hat_nonNaN = [] 142 | v_hat_all = float('nan')*np.ones((2,)) 143 | 144 | if ( Ntargets > 2 ) and ( v_hat_nonNaN.shape[1] > 0 ): 145 | ## if there are more than 2 targets in the scan (resulting in a minimum 146 | ## of 3 estimates of the velocity vector), AND there exists at least one 147 | ## non-singular solution to the uniquely-determined problem 148 | 149 | ## remove k-sigma outliers from data and return sample mean as model 150 | sigma = np.std(v_hat_nonNaN, axis=1) # sample std. dev. 151 | mu = np.mean(v_hat_nonNaN, axis=1) # sample mean 152 | 153 | ## 2 targets will result in a single solution, and a variance of 0. 154 | ## k-sigma inliers should only be identified for more than 2 targets. 155 | k = 1 156 | if sigma[0] > 0: 157 | idx_inlier_x = np.nonzero(np.abs(v_hat_nonNaN[0,:]-mu[0]) < k*sigma[0]) 158 | else: 159 | idx_inlier_x = np.linspace(0,v_hat_nonNaN.shape[1]-1,v_hat_nonNaN.shape[1],dtype=int) 160 | 161 | if sigma[1] > 0: 162 | idx_inlier_y = np.nonzero(np.abs(v_hat_nonNaN[1,:]-mu[1]) < k*sigma[1]) 163 | else: 164 | idx_inlier_y = np.linspace(0,v_hat_nonNaN.shape[1]-1,v_hat_nonNaN.shape[1],dtype=int) 165 | 166 | ## remove k-sigma outliers 167 | idx_inlier = reduce(np.intersect1d, (idx_inlier_x, idx_inlier_y)) 168 | model = np.mean(v_hat_nonNaN[:,idx_inlier], axis=1) 169 | v_hat_all = v_hat_nonNaN[:,idx_inlier] 170 | 171 | elif ( Ntargets > 1 ) and ( v_hat_nonNaN.shape[1] > 0 ): 172 | # there are 2 targets in the scan, AND their solution to the 173 | # uniquely-determined problem produced a non-singular matrix M 174 | model = v_hat 175 | v_hat_all = v_hat 176 | 177 | elif ( Ntargets > 1 ) and ( v_hat_nonNaN.shape[1] == 0 ): 178 | # there are 2 targets in the scan, AND their solution to the 179 | # uniquely-determined problem produced a singular matrix M, i.e. the 180 | # targets have identical angular locations. 181 | model = float('nan')*np.ones((2,)) 182 | v_hat_all = float('nan')*np.ones((2,)) 183 | 184 | else: 185 | # there is a single target in the scan, and the solution to the 186 | # uniquely-determined problem is not possible 187 | model = float('nan')*np.ones((2,)) 188 | 189 | return model, v_hat_all 190 | 191 | def getSimulatedRadarMeasurements(self, Ntargets, model, radar_azimuth_bins, \ 192 | sigma_vr, debug=False): 193 | 194 | radar_azimuth = np.zeros((Ntargets,), dtype=np.float32) 195 | 196 | # simulated true target angles 197 | min_azimuth = np.deg2rad(-75) # [rad] 198 | max_azimuth = np.deg2rad(75) # [rad] 199 | if debug: 200 | true_azimuth = np.linspace(min_azimuth, max_azimuth, Ntargets) 201 | else: 202 | true_azimuth = (max_azimuth-min_azimuth)*np.random.random(Ntargets,) + min_azimuth 203 | 204 | # bin angular data 205 | for i in range(Ntargets): 206 | bin_idx = (np.abs(radar_azimuth_bins - true_azimuth[i])).argmin() 207 | ## could Additionally add some noise here 208 | radar_azimuth[i] = radar_azimuth_bins[bin_idx] 209 | 210 | true_elevation = np.zeros((Ntargets,)) 211 | radar_elevation = np.zeros((Ntargets,)) 212 | 213 | ## define AGWN vector for doppler velocity measurements 214 | if debug: 215 | eps = np.ones((Ntargets,), dtype=np.float32)*sigma_vr 216 | else: 217 | eps = np.random.normal(0,sigma_vr,(Ntargets,)) 218 | 219 | ## get true radar doppler measurements 220 | true_doppler = self.simulateRadarDoppler(model, np.column_stack((true_azimuth,true_elevation)), \ 221 | np.zeros((Ntargets,), dtype=np.float32), np.zeros((Ntargets,), dtype=np.float32)) 222 | 223 | # get noisy radar doppler measurements 224 | radar_doppler = self.simulateRadarDoppler(model, np.column_stack((radar_azimuth,radar_elevation)), \ 225 | eps, np.zeros((Ntargets,), dtype=np.float32)) 226 | 227 | data_truth = np.column_stack((true_doppler,true_azimuth,true_elevation)) 228 | data_sim = np.column_stack((radar_doppler,radar_azimuth,radar_elevation)) 229 | 230 | return data_truth, data_sim 231 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /nodes/radar_velocity.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | """ 3 | Author: Carl Stahoviak 4 | Date Created: Apr 21, 2019 5 | Last Edited: Apr 21, 2019 6 | 7 | Task: To estimate the 2D or 3D body-frame velocity vector of the sensor 8 | platfrom given input data from a single radar (/mmWaveDataHdl/RScan topic). 9 | The velocity estimation scheme takes the following approach: 10 | 11 | 1. Near-field targets are removed from the target list. Many of these targets 12 | are artifacts of antenna interference at the senor origin, and are not 13 | representative of real targets in the scene. These near-field targets also exist 14 | in the zero-doppler bin and thus would currupt the quality of the velocity 15 | estimate. 16 | 2. A RANSAC (or MLESAC) outlier rejection method is used to filter targets that 17 | can be attributed to noise or dynamic targets in the environment. RANSAC 18 | generates an inlier set of targets and a first-pass velocity estimate derived 19 | from the inlier set. 20 | 3. Orthogonal Distance Regression (ODR) is seeded with the RANSAC velocity 21 | estimate and generates a final estimate of the body frame linear velocity 22 | components. 23 | 24 | Implementation: 25 | - The goal is for the VelocityEstimator class (node) to be model dependent 26 | (e.g. 2D or 3D). In both cases the node is subscribed to the same topic 27 | (/mmWaveDataHdl/RScan), and publishes a TwistWithCovarianceStamped message. 28 | - I will need to write to separate classes (RadarDopplerModel2D and 29 | RadarDopplerModel3D) that both define the same methods (e.g. 30 | doppler2BodyFrameVelocity, simulateRadarDoppler, etc.) such that the 31 | VelocityEstimator class is composed of either one of these models. This can be 32 | thought of as designing an "implied interface" to the RadarDopplerModel subset 33 | of classes. 34 | - Additionally, a RadarUtilities class should be implemetned to allow access to 35 | other functions that are model-independent. 36 | 37 | """ 38 | from __future__ import division 39 | 40 | import rospy 41 | import numpy as np 42 | import sensor_msgs.point_cloud2 as pc2 43 | from sensor_msgs.msg import PointCloud2, PointField 44 | from geometry_msgs.msg import TwistWithCovarianceStamped 45 | from geometry_msgs.msg import TwistStamped 46 | 47 | # from sklearn.linear_model import RANSACRegressor 48 | # from goggles.base_estimator import dopplerRANSAC 49 | from goggles.mlesac import MLESAC 50 | from goggles.base_estimator_mlesac import dopplerMLESAC 51 | from goggles.radar_doppler_model_2D import RadarDopplerModel2D 52 | from goggles.radar_doppler_model_3D import RadarDopplerModel3D 53 | from goggles.orthogonal_distance_regression import OrthogonalDistanceRegression 54 | from goggles.radar_utilities import RadarUtilities 55 | import csv 56 | 57 | WRITE_DATA = False 58 | 59 | class VelocityEstimator(): 60 | 61 | def __init__(self, model): 62 | if WRITE_DATA: 63 | csv_file = open('odr.csv', 'a') 64 | self.writer = csv.writer(csv_file, delimiter=',') 65 | 66 | ## prescribe velocity estimator model {2D, 3D} and utils objects 67 | self.model = model 68 | self.odr = OrthogonalDistanceRegression(model) 69 | self.utils = RadarUtilities() 70 | 71 | ## additonal ROS params 72 | self.type_ = rospy.get_param('~type') # 2D or 3D pointcloud data 73 | self.publish_inliers_ = rospy.get_param('~publish_inliers') # publish MLESAC inliers? 74 | self.use_const_cov_ = rospy.get_param('~use_const_cov') 75 | self.cov_scale_factor_ = rospy.get_param('~cov_scale_factor') 76 | 77 | ## instantiate mlesac object with base_estimator_mlesac class object' 78 | self.base_estimator = dopplerMLESAC(model) 79 | # self.mlesac = MLESAC(self.base_estimator,report_scores=False,ols_flag=False,get_covar=False) 80 | self.mlesac = MLESAC(self.base_estimator,report_scores=False,ols_flag=True,get_covar=True) 81 | 82 | ## velocity estimate parameters 83 | self.vel_estimate_ = None 84 | self.vel_covariance_ = None 85 | 86 | self.scan_count = 0 87 | 88 | ns = rospy.get_namespace() 89 | rospy.loginfo("INIT: namespace = %s", ns) 90 | if ns =='/': 91 | ## empty namespace 92 | ns = ns[1:] 93 | 94 | ## init subscriber 95 | mmwave_topic = rospy.get_param('~mmwave_topic') 96 | self.radar_sub = rospy.Subscriber(ns + mmwave_topic, PointCloud2, self.ptcloud_cb) 97 | rospy.loginfo("INIT: VelocityEstimator Node subcribed to:\t" + ns + mmwave_topic) 98 | 99 | ## init MLESAC filtered pointcloud topic 100 | if self.publish_inliers_: 101 | rscan_inlier_topic = mmwave_topic + 'Inliers' 102 | self.pc_pub = rospy.Publisher(ns + rscan_inlier_topic, PointCloud2, queue_size=10) 103 | rospy.loginfo("INIT: RScanInliers publishing on:\t\t" + ns + rscan_inlier_topic) 104 | 105 | ## init publisher 106 | twist_topic = '/' + mmwave_topic.split('/')[1] + '/velocity' 107 | self.twist_pub = rospy.Publisher(ns + twist_topic, TwistWithCovarianceStamped, queue_size=10) 108 | rospy.loginfo("INIT: VelocityEstimator Node publishing on:\t" + ns + twist_topic) 109 | 110 | ## define filtering threshold parameters - taken from velocity_estimation.m 111 | self.azimuth_thres_ = rospy.get_param('~azimuth_thres') 112 | self.elevation_thres_ = rospy.get_param('~elevation_thres') 113 | self.range_thres_ = rospy.get_param('~range_thres') 114 | self.intensity_thres_ = rospy.get_param('~intensity_thres') 115 | self.thresholds_ = np.array([self.azimuth_thres_, self.intensity_thres_, \ 116 | self.range_thres_, self.elevation_thres_]) 117 | 118 | rospy.loginfo("INIT: " + ns + mmwave_topic + " azimuth_thres\t" + "= " + str(self.azimuth_thres_)) 119 | rospy.loginfo("INIT: " + ns + mmwave_topic + " elevation_thres\t"+ "= " + str(self.elevation_thres_)) 120 | rospy.loginfo("INIT: " + ns + mmwave_topic + " range_thres\t"+ "= " + str(self.range_thres_)) 121 | rospy.loginfo("INIT: " + ns + mmwave_topic + " intensity_thres\t"+ "= " + str(self.intensity_thres_)) 122 | 123 | rospy.loginfo("INIT: VelocityEstimator Node Initialized") 124 | 125 | def ptcloud_cb(self, radar_msg): 126 | self.scan_count += 1 127 | # rospy.loginfo("GOT HERE: ptcloud_cb") 128 | pts_list = list(pc2.read_points(radar_msg, field_names=["x", "y", "z", "intensity", "range", "doppler"])) 129 | pts = np.array(pts_list, dtype=np.float32) 130 | 131 | ## pts.shape = (Ntargets, 6) 132 | if pts.shape[0] < self.base_estimator.sample_size: 133 | ## do nothing - do NOT publish a twist message: no useful velocity 134 | ## estimate can be derived from less than sample_size targets 135 | rospy.logwarn("ptcloud_cb: EMPTY RADAR MESSAGE") 136 | else: 137 | # pts[:,1] = -pts[:,1] ## ROS standard coordinate system Y-axis is left, NED frame Y-axis is to the right 138 | # pts[:,2] = -pts[:,2] ## ROS standard coordinate system Z-axis is up, NED frame Z-axis is down 139 | 140 | self.estimate_velocity(pts, radar_msg) 141 | 142 | def estimate_velocity(self, pts, radar_msg): 143 | # rospy.loginfo("GOT HERE: estimate_velocity") 144 | Ntargets = pts.shape[0] 145 | 146 | ## create target azimuth vector (in radians) 147 | azimuth = np.arctan(np.divide(-pts[:,1],pts[:,0])) # [rad] 148 | elevation = np.arcsin(np.divide(-pts[:,2],pts[:,4])) # [rad] 149 | 150 | ## apply AIRE thresholding to remove near-field targets 151 | data_AIRE = np.column_stack((azimuth, pts[:,3], pts[:,4], elevation)) 152 | idx_AIRE = self.utils.AIRE_filtering(data_AIRE, self.thresholds_) 153 | Ntargets_valid = idx_AIRE.shape[0] 154 | 155 | ## define pre-filtered radar data for further processing 156 | # radar_intensity = pts[idx_AIRE,3] 157 | # radar_range = pts[idx_AIRE,4] 158 | radar_doppler = pts[idx_AIRE,5] 159 | radar_azimuth = azimuth[idx_AIRE] 160 | radar_elevation = elevation[idx_AIRE] 161 | 162 | if Ntargets_valid < self.base_estimator.sample_size: 163 | ## do nothing - do NOT publish a twist message: no useful velocity 164 | ## estimate can be derived from less than 2 targets 165 | rospy.logwarn("estimate_velocity: < %d TARGETS AFTER AIR THRESHOLDING" % self.base_estimator.sample_size) 166 | else: 167 | ## get MLESAC estimate + inlier set 168 | radar_data = np.column_stack((radar_doppler,radar_azimuth,radar_elevation)) 169 | self.mlesac.mlesac(radar_data) 170 | # rospy.loginfo("RScan: %d\t Ninliers = %d", self.scan_count, self.mlesac.inliers_.shape[0]) 171 | 172 | ## this data to be published on filtered ptcloud topic 173 | # intensity_inlier = radar_intensity[self.mlesac.inliers_] 174 | # range_inlier = radar_range[self.mlesac.inliers_] 175 | doppler_inlier = radar_doppler[self.mlesac.inliers_] 176 | azimuth_inlier = radar_azimuth[self.mlesac.inliers_] 177 | elevation_inlier = radar_elevation[self.mlesac.inliers_] 178 | 179 | if self.publish_inliers_: 180 | pts_AIRE = pts[idx_AIRE,:] # [x, y, z, intensity, range, doppler] 181 | pts_inliers = pts_AIRE[self.mlesac.inliers_,:] 182 | 183 | rscan_inliers = PointCloud2() 184 | 185 | rscan_inliers.header = radar_msg.header 186 | 187 | rscan_inliers.height = 1 188 | rscan_inliers.width = self.mlesac.inliers_.shape[0] 189 | rscan_inliers.is_bigendian = False 190 | rscan_inliers.point_step = 24 191 | rscan_inliers.row_step = rscan_inliers.width * rscan_inliers.point_step 192 | rscan_inliers.is_dense = True 193 | 194 | rscan_inliers.fields = [ 195 | PointField('x', 0, PointField.FLOAT32, 1), 196 | PointField('y', 4, PointField.FLOAT32, 1), 197 | PointField('z', 8, PointField.FLOAT32, 1), 198 | PointField('intensity', 12, PointField.FLOAT32, 1), 199 | PointField('range', 16, PointField.FLOAT32, 1), 200 | PointField('doppler', 20, PointField.FLOAT32, 1), 201 | ] 202 | 203 | data = np.reshape(pts_inliers, \ 204 | (pts_inliers.shape[0]*pts_inliers.shape[1],)) 205 | rscan_inliers.data = data.tostring() 206 | 207 | self.pc_pub.publish(rscan_inliers) 208 | 209 | ## get ODR estimate\ 210 | odr_data = np.column_stack((doppler_inlier,azimuth_inlier,elevation_inlier)) 211 | weights = (1/self.model.sigma_vr) * \ 212 | np.ones((self.mlesac.inliers_.shape[0],), dtype=np.float32) 213 | self.odr.odr(odr_data, self.mlesac.estimator_.param_vec_, weights, True) 214 | self.vel_estimate_ = -self.odr.param_vec_ 215 | self.vel_covariance_ = self.odr.covariance_ 216 | 217 | if WRITE_DATA: 218 | self.writer.writerow(-self.odr.param_vec_.tolist()) 219 | 220 | ## publish velocity estimate 221 | if np.isnan(self.vel_estimate_[0]): 222 | rospy.logwarn("estimate_velocity: VELOCITY ESTIMATE IS NANs") 223 | else: 224 | if self.type_ == '2d' or self.type_ == '2D': 225 | self.vel_estimate_ = np.stack((self.vel_estimate_,np.zeros((1,)))) 226 | self.publish_twist_estimate(radar_msg) 227 | 228 | def publish_twist_estimate(self, radar_msg): 229 | 230 | twist_estimate = TwistWithCovarianceStamped() 231 | 232 | ns = rospy.get_namespace() 233 | if ns == '/': 234 | ## single radar frame_id to comply with TI naming convention 235 | twist_estimate.header.frame_id = "base_radar_link" 236 | else: 237 | ## multi-radar frame_id 238 | twist_estimate.header.frame_id = ns[1:] + "_link" 239 | 240 | twist_estimate.header.stamp = radar_msg.header.stamp 241 | 242 | twist_estimate.twist.twist.linear.x = self.vel_estimate_[0] 243 | twist_estimate.twist.twist.linear.y = self.vel_estimate_[1] 244 | twist_estimate.twist.twist.linear.z = self.vel_estimate_[2] 245 | 246 | K = self.cov_scale_factor_ 247 | if self.use_const_cov_: 248 | twist_estimate.twiist.covariance[0] = K * 0.01 249 | twist_estimate.twiist.covariance[7] = K * 0.015 250 | twist_estimate.twiist.covariance[14] = K * 0.05 251 | else: 252 | for i in range(self.vel_covariance_.shape[0]): 253 | for j in range(self.vel_covariance_.shape[1]): 254 | twist_estimate.twist.covariance[(i*6)+j] = K*self.vel_covariance_[i,j] 255 | 256 | self.twist_pub.publish(twist_estimate) 257 | 258 | def main(): 259 | ## anonymous=True ensures that your node has a unique name by adding random numbers to the end of NAME 260 | rospy.init_node('goggles_node') 261 | 262 | if WRITE_DATA: 263 | csv_file = open('odr.csv', 'w+') 264 | csv_file.close() 265 | 266 | type = rospy.get_param('~type') 267 | if type == '2D' or type == '2d': 268 | model = RadarDopplerModel2D() 269 | elif type == '3D' or type == '3d': 270 | model = RadarDopplerModel3D() 271 | else: 272 | rospy.logerr("velocity_estimator_node main(): ESTIMATOR TYPE IMPROPERLY SPECIFIED") 273 | 274 | ## use composition to ascribe a model to the VelocityEstimator class 275 | velocity_estimator = VelocityEstimator(model=model) 276 | 277 | rospy.loginfo("MAIN: End of main()") 278 | 279 | try: 280 | rospy.spin() 281 | except rospy.ROSInterruptException: 282 | sys.exit() 283 | 284 | if __name__ == '__main__': 285 | main() 286 | -------------------------------------------------------------------------------- /src/goggles/orthogonal_distance_regression.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | """ 3 | Author: Carl Stahoviak 4 | Date Created: Apr 28, 2019 5 | Last Edited: Apr 28, 2019 6 | 7 | Description: 8 | 9 | """ 10 | 11 | from __future__ import division 12 | 13 | import time 14 | import rospy 15 | import numpy as np 16 | from numpy.linalg import inv, multi_dot 17 | from scipy.linalg import block_diag 18 | from scipy.linalg.blas import sgemm, sgemv 19 | from goggles.radar_doppler_model_2D import RadarDopplerModel2D 20 | from goggles.radar_doppler_model_3D import RadarDopplerModel3D 21 | from goggles.base_estimator_mlesac import dopplerMLESAC 22 | from goggles.mlesac import MLESAC 23 | 24 | class OrthogonalDistanceRegression(): 25 | 26 | def __init__(self, model, converge_thres=0.0005, max_iter=50, debug=False): 27 | self.model = model # radar Doppler model (2D or 3D) 28 | self.converge_thres = converge_thres # ODR convergence threshold on step size s 29 | self.max_iterations = max_iter # max number of ODR iterations 30 | self.debug = debug # for comparison to MATLAB implementation 31 | 32 | self.param_vec_ = None # body-frame velocity vector - to be estimated by ODR 33 | self.covariance_ = None # covariance of parameter estimate, shape (p,p) 34 | self.iter_ = 0 # number of iterations till convergence 35 | 36 | self.s_ = 10*np.ones((model.min_pts,), dtype=np.float32) # step size scale factor 37 | 38 | def odr(self, data, beta0, weights, get_covar): 39 | ## unpack radar data (into colum vectors) 40 | radar_doppler = data[:,0] 41 | radar_azimuth = data[:,1] 42 | radar_elevation = data[:,2] 43 | 44 | ## dimensionality of data 45 | Ntargets = data.shape[0] 46 | p = beta0.shape[0] 47 | m = self.model.d.shape[0] 48 | 49 | # [ S, T ] = self.getScalingMatrices() 50 | S = np.diag(self.s_) # s scaling matrix - 10 empirically chosen 51 | T = np.eye(Ntargets*m, dtype=np.float32) # t scaling matrix 52 | alpha = 1 # Lagrange multiplier 53 | 54 | if p == 2: 55 | ## init delta vector 56 | delta0 = np.random.normal(0,self.model.sigma,(Ntargets,)).astype(np.float32) 57 | 58 | ## construct weighted diagonal matrix D 59 | D = np.diag(np.multiply(weights,d)).astype(np.float32) 60 | 61 | elif p == 3: 62 | ## init delta vector - "interleaved" vector 63 | delta0_theta = np.random.normal(0,self.model.sigma[0],(Ntargets,)).astype(np.float32) 64 | delta0_phi = np.random.normal(0,self.model.sigma[1],(Ntargets,)).astype(np.float32) 65 | delta0 = np.column_stack((delta0_theta, delta0_phi)) 66 | delta0 = delta0.reshape((m*Ntargets,)) 67 | 68 | ## construct weighted block-diagonal matrix D 69 | D_i = np.diag(self.model.d).astype(np.float32) 70 | Drep = D_i.reshape(1,m,m).repeat(Ntargets,axis=0) 71 | Dblk = block_diag(*Drep) 72 | weights_diag = np.diag(np.repeat(weights,m)).astype(np.float32) 73 | D = np.matmul(weights_diag,Dblk) 74 | 75 | else: 76 | rospy.logerr("odr: initial guess must be a 2D or 3D vector") 77 | 78 | ## construct E matrix - E = D^2 + alpha*T^2 (ODR-1987 Prop. 2.1) 79 | E = np.matmul(D,D) + alpha*np.matmul(T,T) 80 | Einv = inv(E) 81 | 82 | ## initialize 83 | beta = beta0 84 | delta = delta0 85 | s = np.ones((p,), dtype=np.float32) 86 | 87 | self.iter_ = 1 88 | while np.linalg.norm(s) > self.converge_thres: 89 | 90 | if p == 2: 91 | G, V, M = self._getJacobian2D(data[:,1], delta, beta, weights, E) 92 | elif p == 3: 93 | G, V, M = self._getJacobian3D(data[:,1:3], delta, beta, weights, E) 94 | else: 95 | rospy.logerr("odr: initial guess must be a 2D or 3D vector") 96 | 97 | doppler_predicted = self.model.simulateRadarDoppler(beta, \ 98 | np.column_stack((data[:,1],data[:,2])), \ 99 | np.zeros((Ntargets,), dtype=np.float32), delta) 100 | 101 | ## update epsilon 102 | eps = np.subtract(doppler_predicted, radar_doppler) 103 | 104 | ## defined to reduce the number of times certain matrix products are computed 105 | prod1 = np.matmul(D,delta) 106 | prod2 = multi_dot([V,Einv,prod1]) 107 | 108 | ## form the elements of the linear least squares problem 109 | Gbar = np.matmul(M,G) 110 | y = np.matmul(-M,np.subtract(eps,prod2)) 111 | 112 | ## Compute step s via QR factorization of Gbar 113 | Q,R = np.linalg.qr(Gbar,mode='reduced') 114 | s = np.squeeze(np.linalg.solve(R,np.matmul(Q.T,y))) 115 | 116 | # t = -Einv*(V'*M^2*(eps + G*s - V*Einv*D*delta) + D*delta) 117 | t = np.matmul(-Einv, np.add( multi_dot( [V.T, np.matmul(M,M), \ 118 | eps+np.matmul(G,s)-prod2] ), prod1 )) 119 | 120 | # use s and t to iteratively update beta and delta, respectively 121 | beta = beta + np.matmul(S,s) 122 | delta = delta + np.matmul(T,t) 123 | 124 | self.iter_ += 1 125 | if self.iter_ > self.max_iterations: 126 | rospy.loginfo('ODR: max iterations reached') 127 | break 128 | 129 | self.param_vec_ = beta 130 | if get_covar: 131 | self._getCovariance( Gbar, D, eps, delta, weights ) 132 | else: 133 | self.covariance_ = float('nan')*np.ones((p,)) 134 | 135 | return 136 | 137 | 138 | def _getJacobian2D(self, X, delta, beta, weights, E): 139 | """ 140 | NOTE: We will use ODRPACK95 notation where the total Jacobian J has 141 | block components G, V and D: 142 | 143 | J = [G, V; 144 | zeros(n,p), D] 145 | 146 | G - the Jacobian matrix of epsilon wrt/ beta and has no special properites 147 | V - the Jacobian matrix of epsilon wrt/ delta and is a diagonal matrix 148 | D - the Jacobian matrix of delta wrt/ delta and is a diagonal matrix 149 | """ 150 | 151 | Ntargets = X.shape[0] # X is a column vector of azimuth values 152 | p = beta.shape[0] 153 | 154 | # initialize 155 | G = np.zeros((Ntargets,p), dtype=np.float32) 156 | V = np.zeros((Ntargets,Ntargets), dtype=np.float32) 157 | M = np.zeros((Ntargets,Ntargets), dtype=np.float32) 158 | 159 | for i in range(Ntargets): 160 | G[i,:] = weights[i]*np.array([np.cos(X[i] + delta[i]), np.sin(X[i] + delta[i])]) 161 | V[i,i] = weights[i]*(-beta[0]*np.sin(X[i] + delta[i]) + beta[1]*np.cos(X[i] + delta[i])) 162 | 163 | ## (ODR-1987 Prop. 2.1) 164 | w = V[i,i]**2 / E[i,i] 165 | M[i,i] = np.sqrt(1/(1+w)); 166 | 167 | return G, V, M 168 | 169 | def _getJacobian3D(self, X, delta, beta, weights, E): 170 | """ 171 | NOTE: We will use ODRPACK95 notation where the total Jacobian J has 172 | block components G, V and D: 173 | 174 | J = [G, V; 175 | zeros(n,p), D] 176 | 177 | G - the Jacobian matrix of epsilon wrt/ beta and has no special properites 178 | V - the Jacobian matrix of epsilon wrt/ delta and is a diagonal matrix 179 | D - the Jacobian matrix of delta wrt/ delta and is a diagonal matrix 180 | """ 181 | 182 | Ntargets = X.shape[0] # X is a column vector of azimuth values 183 | p = beta.shape[0] 184 | m = int(delta.shape[0] / Ntargets) 185 | 186 | theta = X[:,0] 187 | phi = X[:,1] 188 | 189 | ## "un-interleave" delta vector into (Ntargets x m) matrix 190 | delta = delta.reshape((Ntargets,m)) 191 | delta_theta = delta[:,0] 192 | delta_phi = delta[:,1] 193 | 194 | ## defined to simplify the following calculations 195 | x1 = theta + delta_theta 196 | x2 = phi + delta_phi 197 | 198 | # initialize 199 | G = np.zeros((Ntargets,p), dtype=np.float32) 200 | V = np.zeros((Ntargets,Ntargets*m), dtype=np.float32) 201 | M = np.zeros((Ntargets,Ntargets), dtype=np.float32) 202 | 203 | for i in range(Ntargets): 204 | G[i,:] = weights[i] * np.array([np.cos(x1[i])*np.cos(x2[i]), \ 205 | np.sin(x1[i])*np.cos(x2[i]), \ 206 | np.sin(x2[i])]) 207 | 208 | # V[i,2*i:2*i+2] = weights[i] * np.array([ 209 | # -beta[0]*np.sin(x1[i])*np.cos(x2[i]) + beta[1]*np.cos(x1[i])*np.cos(x2[i]), \ 210 | # -beta[0]*np.cos(x1[i])*np.sin(x2[i]) - beta[1]*np.sin(x1[i])*np.sin(x2[i]) + \ 211 | # beta[2]*np.cos(x2[i])]) 212 | 213 | V[i,2*i] = weights[i]*(-beta[0]*np.sin(x1[i])*np.cos(x2[i]) + \ 214 | beta[1]*np.cos(x1[i])*np.cos(x2[i])) 215 | 216 | V[i,2*i+1] = weights[i]*(-beta[0]*np.cos(x1[i])*np.sin(x2[i]) - \ 217 | beta[1]*np.sin(x1[i])*np.sin(x2[i]) + \ 218 | beta[2]*np.cos(x2[i])) 219 | 220 | ## (ODR-1987 Prop. 2.1) 221 | w = (V[i,2*i]**2 / E[2*i,2*i]) + (V[i,2*i+1]**2 / E[2*i+1,2*i+1]) 222 | M[i,i] = np.sqrt(1/(1+w)); 223 | 224 | return G, V, M 225 | 226 | def _getWeights(self): 227 | pass 228 | 229 | def _getCovariance(self, Gbar, D, eps, delta, weights): 230 | """ 231 | Computes the (pxp) covariance of the model parameters beta according to 232 | the method described in "The Computation and Use of the Asymtotic 233 | Covariance Matrix for Measurement Error Models", Boggs & Rogers (1989). 234 | """ 235 | 236 | n = Gbar.shape[0] # number of targets in the scan 237 | p = Gbar.shape[1] # dimension of the model parameters 238 | m = int(delta.shape[0] / n) # dimension of 'explanatory variable' vector 239 | 240 | ## form complete residual vector, g 241 | g = np.vstack((np.reshape(eps,(n,1)),np.reshape(delta,(n*m,1)))) 242 | 243 | ## residual weighting matrix, Omega 244 | W = np.diag(np.square(weights)).astype(np.float32) 245 | Omega1 = np.column_stack((W, np.zeros((n,n*m), dtype=np.float32))) 246 | Omega2 = np.column_stack((np.zeros((n*m,n), dtype=np.float32), np.matmul(D,D))) 247 | Omega = np.vstack((Omega1, Omega2)) 248 | 249 | ## compute total weighted covariance matrix of model parameters (pxp) 250 | self.covariance_ = ( 1/(n-p) * multi_dot([g.T,Omega,g]) ) * inv(np.matmul(Gbar.T,Gbar)) 251 | 252 | return 253 | 254 | 255 | def test_odr(model): 256 | import pylab 257 | 258 | ## define MLESAC parameters 259 | report_scores = False 260 | 261 | ## define ODR parameters 262 | converge_thres = 0.0005 263 | max_iter = 50 264 | get_covar = True 265 | 266 | ## init instances of MLESAC class 267 | base_estimator1 = dopplerMLESAC(model) 268 | base_estimator2 = dopplerMLESAC(model) 269 | mlesac = MLESAC(base_estimator1,report_scores) 270 | mlesac_ols = MLESAC(base_estimator2,report_scores,ols_flag=True,get_covar=True) 271 | 272 | ## init instance of ODR class 273 | odr = OrthogonalDistanceRegression(model,converge_thres,max_iter,debug=False) 274 | 275 | ## outlier std deviation 276 | sigma_vr_outlier = 1.5 277 | 278 | radar_angle_bins = np.genfromtxt('../../data/1642_azimuth_bins.csv', delimiter=',') 279 | 280 | ## simulated 'true' platform velocity range 281 | min_vel = -2.5 # [m/s] 282 | max_vel = 2.5 # [m/s] 283 | 284 | ## number of simulated targets 285 | Ninliers = 125 286 | Noutliers = 35 287 | 288 | mc_iter = 300 289 | 290 | rmse = float('nan')*np.ones((mc_iter,3)) # [mlesac, mlesac+pls, odr] 291 | timing = float('nan')*np.ones((mc_iter,3)) # [mlesac, mlesac+pls, odr] 292 | odr_iter = float('nan')*np.ones(mc_iter,); 293 | 294 | for i in range(mc_iter): 295 | 296 | print "MC iter: " + str(i) 297 | 298 | ## generate truth velocity vector 299 | velocity = (max_vel-min_vel)*np.random.random((base_estimator1.sample_size,)) + min_vel 300 | 301 | ## create noisy INLIER simulated radar measurements 302 | _, inlier_data = model.getSimulatedRadarMeasurements(Ninliers, \ 303 | velocity,radar_angle_bins,model.sigma_vr) 304 | 305 | ## create noisy OUTLIER simulated radar measurements 306 | _, outlier_data = model.getSimulatedRadarMeasurements(Noutliers, \ 307 | velocity,radar_angle_bins,sigma_vr_outlier) 308 | 309 | ## combine inlier and outlier data sets 310 | Ntargets = Ninliers + Noutliers 311 | radar_doppler = np.concatenate((inlier_data[:,0],outlier_data[:,0]),axis=0) 312 | radar_azimuth = np.concatenate((inlier_data[:,1],outlier_data[:,1]),axis=0) 313 | radar_elevation = np.concatenate((inlier_data[:,2],outlier_data[:,2]),axis=0) 314 | 315 | ## concatrnate radar data 316 | radar_data = np.column_stack((radar_doppler,radar_azimuth,radar_elevation)) 317 | 318 | ## get MLSESAC solution 319 | start_time = time.time() 320 | mlesac.mlesac(radar_data) 321 | timing[i,0] = time.time() - start_time 322 | 323 | ## get MLSESAC + OLS solution 324 | start_time = time.time() 325 | mlesac_ols.mlesac(radar_data) 326 | timing[i,1] = time.time() - start_time 327 | 328 | ## concatenate inlier data for ODR regression 329 | odr_data = np.column_stack((radar_doppler[mlesac.inliers_], \ 330 | radar_azimuth[mlesac.inliers_], \ 331 | radar_elevation[mlesac.inliers_])) 332 | 333 | ## get MLESAC + ODR solution 334 | start_time = time.time() 335 | weights = (1/model.sigma_vr)*np.ones((mlesac.inliers_.shape[0],), dtype=np.float32) 336 | odr.odr(odr_data, mlesac.estimator_.param_vec_, weights, get_covar) 337 | timing[i,2] = (time.time() - start_time) + timing[i,0] 338 | 339 | rmse[i,0] = np.sqrt(np.mean(np.square(velocity - mlesac.estimator_.param_vec_))) 340 | rmse[i,1] = np.sqrt(np.mean(np.square(velocity - mlesac_ols.estimator_.param_vec_))) 341 | rmse[i,2] = np.sqrt(np.mean(np.square(velocity - odr.param_vec_))) 342 | 343 | ## convert time to milliseconds 344 | timing = 1000*timing 345 | 346 | ## compute RMSE and timing statistics 347 | rmse_stats = np.column_stack((np.mean(rmse,axis=0), \ 348 | np.std(rmse,axis=0), \ 349 | np.min(rmse,axis=0), \ 350 | np.max(rmse,axis=0))) 351 | 352 | time_stats = np.column_stack((np.mean(timing,axis=0), \ 353 | np.std(timing,axis=0), \ 354 | np.min(timing,axis=0), \ 355 | np.max(timing,axis=0))) 356 | 357 | types = ['MLESAC\t\t', 'MLESAC + OLS\t', 'MLESAC + ODR\t'] 358 | 359 | print("\nAlgorithm Evaluation - Ego-Velocity RMSE [m/s]:") 360 | print("\t\tmean\tstd\tmin\tmax\n") 361 | for i in range(len(types)): 362 | print types[i] + str.format('{0:.4f}',rmse_stats[i,0]) + "\t" + \ 363 | str.format('{0:.4f}',rmse_stats[i,1]) + "\t" + \ 364 | str.format('{0:.4f}',rmse_stats[i,2]) + "\t" + \ 365 | str.format('{0:.4f}',rmse_stats[i,3]) 366 | 367 | print("\nAlgorithm Evaluation - Execution time [ms]:") 368 | print("\t\tmean\tstd\tmin\tmax\n") 369 | for i in range(len(types)): 370 | print types[i] + str.format('{0:.2f}',time_stats[i,0]) + "\t" + \ 371 | str.format('{0:.2f}',time_stats[i,1]) + "\t" + \ 372 | str.format('{0:.2f}',time_stats[i,2]) + "\t" + \ 373 | str.format('{0:.2f}',time_stats[i,3]) 374 | 375 | if __name__=='__main__': 376 | ## define Radar Doppler model to be used 377 | # model = RadarDopplerModel2D() 378 | model = RadarDopplerModel3D() 379 | 380 | test_odr(model) 381 | -------------------------------------------------------------------------------- /data/bruteForce.csv: -------------------------------------------------------------------------------- 1 | 0.2337159945333692,0.402892574969578 2 | 0.5570672681510366,0.9921062414540668 3 | -0.05071873614677447,0.029310956391761955 4 | 0.0,0.0 5 | 0.0,0.0 6 | 0.026040163605019487,-0.03195549372016716 7 | -0.019455542026500842,-0.07874626577348688 8 | 0.005876024121300485,-0.03316590057378717 9 | -0.0222188420268101,-0.14788986565374995 10 | -0.02732591553212168,-0.20000189097812696 11 | -0.0051971345070868065,-0.1849728819454538 12 | -0.07155231745244788,-0.3081816633201145 13 | -0.01833524941348962,-0.26094231443442667 14 | -0.07173305448263867,-0.23702365670615785 15 | -0.09742462908402626,-0.21325665253557385 16 | -0.22739288003657346,-0.2847618371517552 17 | -0.2713261250915785,-0.16369896216933347 18 | -0.34368868731997393,-0.09802349228751173 19 | -0.43966408660192413,0.007842042088059697 20 | -0.3889812603334465,-0.001873132044131788 21 | -0.5240413308779628,0.16970027835197019 22 | -0.7047056320307455,0.14462708644664507 23 | -0.7018687769644295,0.08008358168646305 24 | -0.7315493262793767,0.21899286462527448 25 | -0.7116595730423627,0.24938360734338014 26 | -0.8029533313319323,0.08459811571624086 27 | -0.7410504810157549,0.1342583094353662 28 | -0.7558176567455195,0.09989041160147268 29 | -0.6194473606157556,0.10255073496033901 30 | -0.6120424606803181,0.11747062846036907 31 | -0.6204322140068842,0.05982946697714683 32 | -0.5830395343907807,0.10196467640807505 33 | -0.4940864182470176,0.16364885440268329 34 | -0.5849587433207197,0.0679771616627891 35 | -0.5960435512288674,0.04828217285053169 36 | -0.6420862829190387,0.06829684408806955 37 | -0.5638113357161648,0.009490393473771539 38 | -0.6460594737876422,0.09129685526437649 39 | -0.6322672830828758,0.1205466081706156 40 | -0.6380065274486192,0.1163721492678514 41 | -0.6978162319432621,0.0185803912319738 42 | -0.6761348098832267,0.0734935575782644 43 | -0.7078410246033169,0.09816298665483057 44 | -0.720931407447335,0.06765356675230114 45 | -0.7045903479665551,0.05405793479915462 46 | -0.6214713073295302,0.050180772995476054 47 | -0.6713692647279086,0.04391305499779359 48 | -0.5850440477763927,0.10734149069512192 49 | -0.5640207739734712,0.041115685098779595 50 | -0.5670187719226105,0.07796099717355573 51 | -0.5158020613037925,-0.02593155571519958 52 | -0.5161886625003499,0.16588542882122254 53 | -0.49177860508354865,0.13782855488051127 54 | -0.6046819933847659,0.12160254838781095 55 | -0.6357391074845715,0.03289060971662602 56 | -0.6652672076149373,0.04912535457873737 57 | -0.5746396175779446,0.06908357317914784 58 | -0.7091720839721738,-0.03426499200145099 59 | -0.5958826748486199,-0.004102532979035894 60 | -0.6287239502560196,-0.012974839670531445 61 | -0.5942992947748345,-0.029298241981534027 62 | -0.6188396328740335,0.022833333455811344 63 | -0.5871186683574472,0.034343146957809864 64 | -0.62520421587794,0.027541279824704598 65 | -0.6136538074918176,0.04769300247707835 66 | -0.5773649148269717,0.020097344793939893 67 | -0.6128817903009562,-0.036857568332609475 68 | -0.628725135901293,-0.01479112573654391 69 | -0.5805996170389769,0.04686952010475574 70 | -0.584983098021804,-0.047086368619899135 71 | -0.6328760094979075,0.03987388568728118 72 | -0.6314850858082206,0.09841096589242496 73 | -0.5731781171965864,0.15035073392570952 74 | -0.5810186240699972,0.17628824672930699 75 | -0.4847005866391704,0.14154803076413247 76 | -0.5702598494580032,0.2708442661070533 77 | -0.6050443912015847,0.24074360983718981 78 | -0.5429916893338108,0.13377682680978145 79 | -0.6046547336520935,0.16895923192389592 80 | -0.5952318756746616,0.1397164600141484 81 | -0.624991354895974,0.024988127853866456 82 | -0.6034096629536969,-0.010222476684448084 83 | -0.604179495987812,-0.1467571615140157 84 | -0.5934054319211928,-0.17275021392754666 85 | -0.5010911832798391,-0.10677462109141911 86 | -0.5650459577443294,-0.22177323851924727 87 | -0.5335401799224486,-0.23269887018813412 88 | -0.5964174539743404,-0.14131449694809045 89 | -0.5764854621145812,-0.14179298124772372 90 | -0.5335848193923024,-0.1514043200795065 91 | -0.564772391626723,-0.08090433945585758 92 | -0.5742287896133986,0.10938848546696077 93 | -0.539736109601642,0.08721122271525097 94 | -0.5527918771518452,0.13309260082779473 95 | -0.5468487319871635,0.2260751323235764 96 | -0.5111125533935176,0.10929484914635766 97 | -0.5470937380086236,0.08003224763788495 98 | -0.4641156798376103,-0.025427662072044593 99 | -0.5657138832188133,0.10500235968140278 100 | -0.6004578894171733,0.02370294640063356 101 | -0.5583628669296096,-0.054911921984239935 102 | -0.5235131720644345,-0.254431502575076 103 | -0.4929617441942623,-0.21739648390437244 104 | -0.43878178536283113,-0.20594023624700486 105 | -0.3986833733588281,-0.23787705385517258 106 | -0.35248436906816555,-0.22592664688984124 107 | -0.310904583455484,-0.17083056833379368 108 | -0.2893390589878018,-0.13942939776254284 109 | -0.25182904294258596,-0.16083281726736787 110 | -0.26119653426924727,-0.03814142462711208 111 | -0.26088036991310526,0.0023647809882729374 112 | -0.2435582930900507,0.06401828616928136 113 | -0.23453674303187683,0.11814473623212277 114 | -0.2262003024788864,0.1648784514451091 115 | -0.22886833156447065,0.11267189465419829 116 | -0.23409601768518215,0.20327061418081685 117 | -0.20901783506090924,0.17256192369467663 118 | -0.16156073386110326,0.18668676690834118 119 | -0.13209444514679108,0.11864049325552369 120 | -0.12823971641944884,0.21333792534121568 121 | -0.12038584356444725,0.37096637447867836 122 | -0.10599558231265892,0.02505098228138551 123 | -0.09790431362583532,-0.007997645636045285 124 | -0.08945251733904035,-0.07861247546342391 125 | -0.093017921409669,-0.07253692397779278 126 | -0.11296811052738646,-0.12617153576421516 127 | -0.1111511905860262,0.14030087622758017 128 | -0.17719616928058982,-0.11064148114931562 129 | -0.19162388198486313,-0.1764500979377343 130 | -0.2363661512220255,-0.10110259122875935 131 | -0.22237461926511057,-0.15613285895243234 132 | -0.26466953199256016,-0.023374405965781254 133 | -0.2846252016211401,-0.032428174302645406 134 | -0.24653614288724968,-0.1731919967497711 135 | -0.23767332686265802,-0.06538966584940803 136 | -0.21415760617442944,-0.021659897927461667 137 | -0.17614839062043927,-0.005799984051765118 138 | -0.12952368592904676,0.02700793452821586 139 | -0.030762695473829125,0.18139915842791113 140 | -0.04353564840069097,0.057586361514374976 141 | -0.002826742971542238,0.0283958108981469 142 | 0.0,0.0 143 | 0.0,0.0 144 | -0.039510981970493034,0.04201966308624404 145 | -0.06556645766067777,0.025827123045262235 146 | -0.07057137470185805,-0.02064497948708535 147 | -0.08602623521489344,0.010236260506258464 148 | -0.09939441899456572,-0.05541041100190314 149 | -0.10100671709091165,-0.057202825172744984 150 | -0.1159013320932746,-0.12115547096356799 151 | -0.10060111762255024,-0.11185936325123953 152 | -0.09248232406857367,-0.09530262666757512 153 | -0.07253754936453351,-0.0469277854145064 154 | -0.06259100692536664,-0.039653798840516084 155 | -0.025386860006932622,0.008481252949202503 156 | -0.04616575116000777,-0.03863501503241534 157 | 0.011123939252411005,-0.1901905073370378 158 | -0.03465752939167652,-0.018026168086628266 159 | [nan],[nan] 160 | -0.032308629091885394,-0.008578650411168393 161 | -0.04545401468680312,0.015418636850825979 162 | -0.06107684410584323,0.04498260045574717 163 | -0.03855675235381943,0.014556261502204904 164 | -0.04140169804651879,0.003911879527759839 165 | -0.05189382147684016,0.002000663742675634 166 | -0.053214716746145974,0.0017133139148518928 167 | -0.051124975373381155,-0.0027626840408129243 168 | -0.028415644566206984,-0.02720175955865767 169 | -0.027955804267005113,-0.02916555485821829 170 | 0.0,0.0 171 | 0.0,0.0 172 | -0.025818525133541173,0.03992293376794032 173 | 0.008940978530965628,-0.024064829847771453 174 | 0.0,0.0 175 | 0.0,0.0 176 | 0.0,0.0 177 | 0.0,0.0 178 | 0.0,0.0 179 | [nan],[nan] 180 | 0.0,0.0 181 | [nan],[nan] 182 | 0.0,0.0 183 | 0.0,0.0 184 | 0.0,0.0 185 | 0.06630102004430864,0.10092437546406235 186 | 1.051764063566665,3.6049258300765126 187 | -0.06022802299645763,0.0394957861178238 188 | 0.0,0.0 189 | 0.0,0.0 190 | 0.0,0.0 191 | 0.0,0.0 192 | 0.0,0.0 193 | 0.0,0.0 194 | 0.03898593573545383,-0.003350298155688807 195 | 0.03879811400208779,-0.0042691295555284565 196 | 0.041165603614059285,0.032891052128834405 197 | 0.03892940257001288,0.032639507768260315 198 | 0.037605098790030295,0.027733053689492942 199 | 0.0,0.0 200 | 0.0,0.0 201 | -0.0011638993590297334,0.027140835636081197 202 | -0.001510776951995154,0.008892419710570521 203 | -0.0026786409100141672,0.01910485236048705 204 | -0.007567885254060403,0.0529799597694186 205 | -0.008380067195532071,0.05438510965767561 206 | -0.011129734842992782,0.052304394355717515 207 | 0.0,0.0 208 | 0.0,0.0 209 | 0.0,0.0 210 | 0.0,0.0 211 | 0.0,0.0 212 | 0.0,0.0 213 | 0.0,0.0 214 | 0.0,0.0 215 | 0.0,0.0 216 | -0.047059912191609155,0.02500719392482762 217 | 0.0,0.0 218 | 0.0,0.0 219 | 0.0,0.0 220 | 0.0,0.0 221 | -0.029240731045354126,0.047956926930251276 222 | -0.005000004492637745,-0.010489718612550538 223 | -0.018592312101158432,0.025814157240465672 224 | -0.027770711178137817,0.02646526637316421 225 | 0.0,0.0 226 | 0.0,0.0 227 | 0.07573808864571827,0.021292834260236954 228 | 0.0,0.0 229 | 0.0,0.0 230 | 0.0,0.0 231 | 0.035560616268667265,0.0741982659085061 232 | 0.03383325201894033,0.07791391950964989 233 | 0.029653847752687878,0.4952123217660661 234 | 0.08964359880840844,-0.6935192768796012 235 | 0.0,0.0 236 | 0.0,0.0 237 | 0.0,0.0 238 | 0.0,0.0 239 | 0.00317405271707257,-0.3921390947506474 240 | 0.0,0.0 241 | 0.0,0.0 242 | 0.04030480835768822,-0.6889577811430087 243 | 0.0,0.0 244 | 0.06340573125676846,1.0112962977211277 245 | 0.004691492181569653,0.1504887139162945 246 | 0.0,0.0 247 | 0.0,0.0 248 | 0.0027570953750305307,-0.0011594696215532592 249 | -0.07000103136922366,0.04770298278650566 250 | -0.16727489256839997,0.11521240630737985 251 | -0.2505137451190383,0.17276423942378105 252 | -0.40051405660148504,0.45437742499104794 253 | -0.5394285365481188,0.21684512993777946 254 | -0.5769458923031887,0.18731808320095156 255 | -0.7075418139869062,0.18561804350899047 256 | -0.808125872265674,0.06702410617630326 257 | -0.7983535903817027,-0.015543434221545062 258 | -0.8596795531202246,-0.05693208866508022 259 | -0.7576632760769719,-0.047941601570011344 260 | -0.8097642141547984,-0.0846023078619578 261 | -0.7503387827895898,-0.16695475078042596 262 | -0.6712458233284149,-0.16758930023173293 263 | -0.648125629714898,-0.10275546116599034 264 | -0.5927551756346169,-0.10295832873682975 265 | -0.5901196368377266,-0.020298594535068902 266 | -0.5704087984790744,0.06640161937101562 267 | -0.4914040557161571,-0.03329724680636156 268 | -0.6107228884086904,0.10997790791635147 269 | -0.6141317895094136,0.11939559853426852 270 | -0.623252332045957,0.09390534167900264 271 | -0.5747404332432657,0.03651429162197835 272 | -0.647912106286017,0.08464914806736988 273 | -0.5681627127909771,-0.043320364870671035 274 | -0.6544321673220235,0.09083815073172703 275 | -0.6197839520916686,-0.0474662643163676 276 | -0.6958352158943956,0.1702791238789683 277 | -0.6067548877614618,-0.08565836753853935 278 | -0.6138125456310957,-0.040689343869406064 279 | -0.6414734191120518,-0.055426146049377394 280 | -0.6489456484782021,-0.041987632433541845 281 | -0.6401714820561217,-0.00295901323493179 282 | -0.6301673665643657,-0.007121908443866378 283 | -0.6450900712088782,0.011640694371496025 284 | -0.5821774258309169,0.016567530333045914 285 | -0.6246430222071037,0.02675425848487689 286 | -0.6125067313287259,0.041557890892762246 287 | -0.5936478544687673,0.04659286552493004 288 | -0.5981109588481994,0.026642576283724895 289 | -0.5007287603261757,-0.0046604184436913965 290 | -0.5124405267723966,0.019457981350414058 291 | -0.5533207777618189,0.0027135937536224 292 | -0.5288030704496527,-0.04058697421187031 293 | -0.5177834933415518,-0.04437632420931245 294 | -0.5165395637275972,-0.054816599589250004 295 | -0.5362790284045116,-0.04837109054876662 296 | -0.5177901184355258,-0.07174791364903674 297 | -0.5528986863987528,-0.07089994487987733 298 | -0.5406404323416866,-0.05971735009764959 299 | -0.5178727161533934,-0.08239355081640143 300 | -0.5723156190540946,0.034571623265690056 301 | -0.5946613108142779,0.017568863525516447 302 | -0.5415709631092133,0.07975778870122598 303 | -0.5723069747004237,0.06251664819654802 304 | -0.5975384516131433,0.019921853062901267 305 | -0.5957499446914036,0.04919720777956337 306 | -0.5365572667819586,-0.008160745498880831 307 | -0.5687848983443884,0.029287332387105992 308 | -0.5361955152450446,0.04323473209216777 309 | -0.5518282958801518,0.0432524697954564 310 | -0.4673578565419368,0.05331527178306584 311 | -0.47634501763288845,0.09067362751900035 312 | -0.5351319378001144,0.033699956933281594 313 | -0.49263865055528544,0.040134658050020415 314 | -0.5442458857084495,0.052780508225391494 315 | -0.5400326137779887,0.0572379041905505 316 | -0.5378843365451306,0.09470084364386136 317 | -0.5709149377885709,0.0660600423629752 318 | -0.4900678567641984,0.08080619907524832 319 | -0.4967352029845325,0.014450354167257099 320 | -0.5349615973041206,0.035532925339793096 321 | -0.5000654037850478,-0.02060179006846396 322 | -0.4918971466821233,-0.002912099175256817 323 | -0.4548769377080017,0.010906452209662621 324 | -0.5006099927771945,-0.01152179220958301 325 | -0.49288289250849027,-0.04017987148209026 326 | -0.47617933113399397,0.03246111956513325 327 | -0.4824158466554661,0.003424905996563449 328 | -0.4879135468510299,0.0014669629145063646 329 | -0.4372749815756344,0.011594574753682932 330 | -0.5607946387774176,0.14431111519303563 331 | -0.485797019658928,0.028996878562615687 332 | -0.5705069759405134,0.12456742714487562 333 | -0.4976843656861166,0.22742939098157017 334 | -0.47347439778850886,0.05515648125981198 335 | -0.442986097036422,0.034826775103208946 336 | -0.3330850687221758,-0.16077224923286185 337 | -0.38631897842911705,0.004607259559418539 338 | -0.3557025399937437,0.046650510613622354 339 | -0.24530663732609045,0.04115856489197596 340 | -0.18409121715596122,0.04809945140229246 341 | -0.20796511518045982,-0.10451778971989084 342 | -0.28066836243894083,-0.06016019661727875 343 | -0.27159487239013663,-0.10163318057605919 344 | -0.182604076120759,-0.09392700545703968 345 | -0.14705357707088318,-0.06643854751736376 346 | -0.1932082326682982,-0.08099072054065412 347 | -0.1596494153015403,-0.22465640132375883 348 | -0.2209873388854303,-0.19252673954482724 349 | -0.1956711238993724,-0.13861055784640552 350 | -0.19213949196442306,-0.1915221939978369 351 | -0.26379290154665747,-0.034282388659738657 352 | -0.23779272958980116,-0.16126362385374549 353 | -0.1912623694159181,0.00461261617564671 354 | -0.20587840995304052,-0.3476217272776328 355 | -0.21034294895222583,0.011897575437884965 356 | -0.17029416961446872,-0.01879370811775125 357 | -0.13395979454369816,-0.035530541676428794 358 | -0.11333073531258929,-0.08414221473185979 359 | -0.09915786225615854,-0.10183569203625834 360 | -0.11916486754090169,-0.08390620646706154 361 | -0.13372065076326792,-0.10749251258094593 362 | -0.13673295077823422,-0.11655991455114334 363 | -0.15304631869179092,0.002412248582725129 364 | -0.14505788432864264,-0.1378354865179743 365 | -0.12445760152278554,-0.04297776362024534 366 | -0.11018973039920622,0.01501368675642819 367 | -0.12382776363579165,-0.020253809509565242 368 | -0.1288856435802385,0.035347185439642624 369 | -0.11978394821762697,0.014882597111920122 370 | -0.10712003798096238,0.10087495831939527 371 | -0.09938897357922172,0.10112167532543255 372 | -0.1176210905027288,0.1173997518477315 373 | -0.08056973464237886,0.20446422990423424 374 | -0.11977253051818362,0.11913276970170487 375 | -0.10405554016687506,0.13689321818018876 376 | -0.09105272044263499,0.058385013299652 377 | -0.0843953887756167,0.06111572790350861 378 | -0.09234133319524448,0.021297635205488653 379 | -0.10347983790383149,0.10222227899938836 380 | -0.07160533131170316,-0.029462369327792576 381 | -0.03504613450596208,-0.15509054105513284 382 | -0.022615377500860078,-0.07451966583860631 383 | -0.038486396074888955,-0.006735783535057835 384 | -0.03902747854590416,-0.004321156208761295 385 | nan,nan 386 | -0.04345300387666841,0.1279184230766727 387 | 0.0,0.0 388 | -0.001980905573343513,-0.003677895635642661 389 | -0.027451234108641878,0.0440123540629108 390 | -0.042299433875151046,0.08988382049362371 391 | -0.037959146172784224,0.05341236720229517 392 | -0.038255336110966015,-0.008318447888637554 393 | -0.00994106332196163,-0.06507457581326803 394 | -0.006107884772208105,-0.05456384422657796 395 | -0.0030440232747060026,-0.05161292372773099 396 | -0.00588141359174817,-0.031876575731195296 397 | 0.005121129056773303,-0.06447090164862225 398 | 0.02568951948558719,-0.06517528787619867 399 | 0.02742267961183314,-0.057525672261989465 400 | 0.04429793661444695,-0.07079372350087827 401 | 0.02528300928681159,-0.027232927483896538 402 | 0.0,0.0 403 | 0.03170540264674355,-0.04930821879240368 404 | [0.0],[0.0] 405 | 0.0,0.0 406 | 0.0,0.0 407 | -0.0341288471914169,0.041540114840415206 408 | -0.03188801176744224,0.03668376129431062 409 | -0.03370687178559718,0.030527916022910054 410 | [-0.030946954838762836],[-0.02379121600066898] 411 | -0.001864364736419479,-0.060114797092951616 412 | [nan],[nan] 413 | nan,nan 414 | 0.01626185274059336,-0.05385195328069302 415 | 0.0341596703322694,-0.029303184178411264 416 | 0.04552594891126967,-0.040908456257146376 417 | 0.03806982680169369,-0.016776449616404204 418 | 0.043542141549662586,-0.03667049650914374 419 | 0.0321750824453204,-0.025708821566701855 420 | [nan],[nan] 421 | [nan],[nan] 422 | [0.0],[-0.0] 423 | -0.039308560469969174,0.00515135496756119 424 | -0.049227667646493584,0.03218610835354018 425 | -0.1110277007665416,0.07742627720618624 426 | -0.27497161409154947,0.13459560302666979 427 | -0.368036927927251,0.2280722498678445 428 | -0.5257365941123369,0.19570476579943838 429 | -0.6817785709729245,0.1565943144083702 430 | -0.7327078224083963,0.10514718923610866 431 | -0.7377897736938079,-0.10071149121286081 432 | -0.8556272984322338,0.09025102268058587 433 | -0.8965326810114705,-0.14108453869353407 434 | -0.8186051453068294,-0.40632463541753105 435 | -0.7063228347417436,-0.3912701708311514 436 | -0.5921546825531852,-0.4257399958773443 437 | -0.5409342324953941,-0.6104354510337012 438 | -0.47869614634353175,-0.6371876938116494 439 | -0.5258084580786303,-0.3946953517347186 440 | -0.3698097900183195,-0.3455283322244836 441 | -0.3284346201463151,-0.2503509024606139 442 | -0.26865856814351624,-0.15406652706569546 443 | -0.26460842449141025,-0.17116237521610875 444 | -0.3387579088256668,-0.09517968106373066 445 | -0.31935914714289537,-0.05436548219889175 446 | -0.368669277363089,-0.002885625809538033 447 | -0.3919059240072344,0.0507437394579113 448 | -0.41423397117679295,0.012715466358063567 449 | -0.49221887721261176,0.250123798641221 450 | -0.46735239226155123,0.21200907398145252 451 | -0.46464153527548535,0.01759685124384754 452 | -0.4773615822831977,-0.03924105464736482 453 | -0.4215656866718084,-0.04828781797345623 454 | -0.3463820436191507,-0.043295449186644 455 | -0.3246794839545512,-0.13494575890300287 456 | -0.26734976328182325,-0.04663082841190873 457 | -0.25100789092494397,0.24420973897086165 458 | -0.18418731039050984,-0.08525602514632424 459 | -0.14440860319402526,-0.04324738624432687 460 | -0.12573593537343652,-0.030277311425348744 461 | -0.148790015335467,0.07518071976615764 462 | -0.1407720026226115,0.07927496652936276 463 | -0.14068379550442708,0.0986856776341478 464 | -0.12879600791751752,0.11748218642828841 465 | -0.14151298919305036,0.10133109540380202 466 | -0.21468614791243326,0.16403952292739593 467 | -0.18585999374658288,0.11294548130326776 468 | -0.20689271457492622,0.12701255517203072 469 | -0.2119421062191923,0.1399758254043546 470 | -0.17176796679910425,0.12850086296829205 471 | -0.20215574852888885,0.05980772142937708 472 | -0.1755263569718669,0.0033674625501882516 473 | -0.11039169354507185,-0.016078197378472617 474 | -0.08764267479023895,-0.0381296062757432 475 | -0.1421323976813967,0.6181656950646404 476 | -0.17893199007623461,-0.17496446504214935 477 | -0.07726980269415137,-0.0315048987729217 478 | -0.059291958935999485,-0.051712626243991394 479 | -0.09609974585317421,0.2541808974811124 480 | -0.04793982086373503,-0.07654185572871011 481 | -0.07434708210262993,-0.053462505385688676 482 | -0.05482322202454265,-0.10329082873136024 483 | -0.0843579415251179,0.06717399829520962 484 | -0.12520691009250842,-0.005902293692519986 485 | -0.10553856076687576,-0.044711265857793815 486 | -0.12468645394433095,-0.011348839308932473 487 | -0.1875186546694814,-0.0063748970525738784 488 | -0.12458448068030477,-0.07527083769421211 489 | -0.19717776716553462,-0.007767016210803219 490 | -0.2679881523634864,0.06492093935174438 491 | -0.22366324175005883,0.03478447210050964 492 | -0.10243894733136552,-0.11558849272389878 493 | -0.1993744394760431,-0.010986896310491226 494 | -0.08516188101647808,-0.0005888987606235507 495 | -0.07913704130252577,0.05247065424020098 496 | -0.11549455157446112,0.4338675174754128 497 | -0.0742870559566064,0.0585946949345203 498 | -0.07323216152853465,0.054274012662130895 499 | -0.04597505460506681,0.011855941981761847 500 | -0.08340458416689873,0.0900676187520467 501 | -0.055719910849644005,-0.05958814857475321 502 | -0.09900784366548117,0.45798121274838993 503 | -0.07557212961249064,0.01978659896739272 504 | -0.04409865719976351,0.05465913165456017 505 | -0.07728730252744899,-0.01318718608557617 506 | -0.057282180393168645,-0.057038366515991375 507 | 0.006147672705621967,-0.09711584630945652 508 | -0.029417453483963796,-0.10616915343427273 509 | -0.07224009680442624,-0.031060406553407097 510 | -0.004898672476685653,-0.010086007872400165 511 | -0.023058747580352836,-0.04944036049840081 512 | -0.05423150480292029,-0.1075231459035788 513 | -0.039692326106549304,0.024964292292458168 514 | -0.09233846628977736,-0.12578278559572004 515 | -0.02914990908556907,-0.007521270069402419 516 | -0.06913321735929512,-0.03820500875243608 517 | -0.009163283122291598,-0.02926626091721303 518 | -0.06349069793680996,-0.04674392381447683 519 | -0.049854885229696,-0.03778536886969777 520 | [nan],[nan] 521 | -0.03916210909030041,-0.003709763062056433 522 | nan,nan 523 | [-0.009894916157221927],[-0.3121018175554664] 524 | [-0.037823769092493756],[-0.011986824472313317] 525 | -0.021412526811282362,-0.04401045712793449 526 | -0.023923457976996618,0.027497383172716643 527 | 0.36172401723411773,-1.1019398914772178 528 | -0.01609925296221017,0.011549425025123164 529 | -0.01610050141411649,0.011552837267224432 530 | -0.007948900755931,0.014138178840381307 531 | -0.024366765518523903,0.047674994944693 532 | -0.026974310587355367,0.046710626613116125 533 | -0.1870073520139539,-0.5683556776894677 534 | nan,nan 535 | nan,nan 536 | 0.0,0.0 537 | 0.19971229096967436,1.0723572373898071 538 | -0.049131029692897066,0.07216448874983725 539 | -0.1503369947301662,0.1478661680532026 540 | -0.28254736540240727,0.17632196723585064 541 | -0.429499197015972,0.2931827135287673 542 | -0.5640119451973543,0.3448607589051131 543 | -0.7148695231623291,0.40148802390801874 544 | -0.7553438132512732,0.32296653807076947 545 | -0.906281116343641,0.42423196665664564 546 | -0.9596526136590211,0.3796305038676193 547 | -0.9432375932584675,0.30588271614129997 548 | -0.9727013033562246,0.2077828113700958 549 | -0.9011368279016514,0.18400181574892294 550 | -0.7897810396284304,-0.0642610313207902 551 | -0.7315920720412579,-0.03466912410035382 552 | -0.6953012393045375,-0.0001561797502369627 553 | -0.6345804365494858,-0.05654179275806782 554 | -0.6489508839857283,0.006188067342854058 555 | -0.5994370589473285,-0.03630406734395092 556 | -0.5585587217069707,-0.023350521509712938 557 | -0.5972049543752341,0.035518823110887554 558 | -0.5890272012646139,0.08935628055223979 559 | -0.6081680569341974,0.04473536475888678 560 | -0.6265297362023231,0.11155274236147801 561 | -0.5641208294713533,0.041081218234003444 562 | -0.6280502004270312,0.12395961784567222 563 | -0.5292835121617939,0.00969923219726019 564 | -0.6680512049896105,0.03050534450856587 565 | -0.7410827739469743,0.005980217092028028 566 | -0.6738477972272581,-0.06573488010252673 567 | -0.6695304864943781,-0.07756888986595038 568 | -0.5849630508087557,-0.12243947650190783 569 | -0.5746704162438828,-0.15293009527390755 570 | -0.559873144099712,-0.22025445273311528 571 | -0.5462532385848641,-0.2423621100494205 572 | -0.5108828932210245,-0.14230331118656941 573 | -0.6231340516345146,-0.05155200236638983 574 | -0.6082232461066016,0.03852556843426479 575 | -0.5455628749350492,0.003812230967114796 576 | -0.5440544732504276,-0.021156152590768935 577 | -0.6444635613566324,0.16138693960687983 578 | -0.5978621817129234,0.08951026544479374 579 | -0.6394593698639538,0.19628427041198182 580 | -0.7171035073670191,0.26840468484661456 581 | -0.623255533990931,0.12881274602916964 582 | -0.6774010285113721,0.18191798070880044 583 | -0.5693122532773465,0.1411899629385151 584 | -0.6619307065571054,-0.04480798963023164 585 | -0.5511607691170682,-0.12240895592957175 586 | -0.48925866115275046,-0.3625289405614361 587 | -0.5637095843705238,-0.2373221642574003 588 | -0.4502438780674616,-0.26569272691585816 589 | -0.48049475148637283,-0.34139609096750184 590 | -0.5259198034482719,-0.2947146995191405 591 | -0.5096988974906931,-0.24249910101476196 592 | -0.5344598774461622,-0.2240823611208547 593 | -0.5577394017207129,-0.1340861009518869 594 | -0.5018214569426936,-0.05207565288164316 595 | -0.5409317099205018,-0.08793261087645865 596 | -0.5784483091599738,0.008735099407082603 597 | -0.607861905261508,0.13694254077502702 598 | -0.6331153210812437,0.20731430958408872 599 | -0.6076952233203666,0.1665078425399328 600 | -0.6166262425046738,0.18502305164325905 601 | -0.550272185979435,0.09419080132775962 602 | -0.5802842257349794,0.14289819696043923 603 | -0.6027073530403246,0.1514886921864317 604 | -0.5305647055975916,0.11162469880097114 605 | -0.5789470524730699,0.11226616182986507 606 | -0.5241537931373029,0.05356930046362923 607 | -0.5112175049559309,0.15293009620162107 608 | -0.49872095300133046,0.13005311195829533 609 | -0.47269226456663715,-0.07780978150466845 610 | -0.565773610420642,-0.0376093237396317 611 | -0.5417904432565249,0.13462018557276909 612 | -0.552160223969637,-0.11671199210909558 613 | -0.5352056421471658,-0.18290052927754546 614 | -0.4217467282387324,-0.13450576068483572 615 | -0.47640451747008133,-0.012428748216871262 616 | -0.4862723806241278,-0.10142649924277745 617 | -0.4333068652511248,-0.14466309233826777 618 | -0.4107537986213623,-0.13157270903476703 619 | -0.3267737942097485,0.008007303707568485 620 | -0.2980635832878006,-0.06726563936828003 621 | -0.24020027015547252,-0.12187941826922274 622 | -0.31517423226992997,-0.035401776674369244 623 | -0.2724132634143673,-0.03983243714591609 624 | -0.27092071349612923,0.048998677977971805 625 | -0.2960942340166437,0.07465662756779776 626 | -0.289907817570931,0.1348699205879181 627 | -0.07729916286802012,0.24221422827631214 628 | -0.3054984929345583,0.1756532572541128 629 | -0.28684226292410836,0.0873034040579531 630 | -0.31971461416770175,0.034031245940222644 631 | -0.34512082324409044,0.04484539026032978 632 | -0.3461148851770825,-0.04239041893636404 633 | -0.3274021657599081,-0.03528535013652686 634 | -0.31846620798366626,-0.08760550268372427 635 | -0.3102312294297187,-0.13868048751831832 636 | -0.27838814541653084,-0.17794288368041825 637 | -0.24532134970172273,-0.19609604005451714 638 | -0.22215078010022046,-0.2179431219842991 639 | -0.17161854603253038,-0.15714095370624806 640 | -0.17165141549818363,-0.16100110065418555 641 | -0.15148426279842842,-0.1177547797002048 642 | -0.15795326602442933,-0.10329191923587752 643 | -0.16261171297504842,-0.100153482600344 644 | -0.18275074948920636,-0.08008063694747393 645 | -0.16930953585135347,-0.021475505798482834 646 | -0.20284256661974176,-0.032678139368677875 647 | -0.23793250010861244,0.014677600738287706 648 | -0.2307875193177966,-0.01765498316490243 649 | -0.21198449371216083,0.005597095582755219 650 | -0.24588558196912724,0.0011686970932268195 651 | -0.25695285478600854,0.043375169845688434 652 | -0.20968967990907988,0.05598253471911849 653 | -0.136875157742142,0.05497447342974812 654 | -0.12925079154487482,0.06540326137536863 655 | -0.08749236087511468,0.06089901914579475 656 | -0.007572495837264986,0.010737709395541134 657 | -0.037385174659357076,0.03428292120042458 658 | 0.0,0.0 659 | 0.0,0.0 660 | 0.0,0.0 661 | 0.0,0.0 662 | 0.14548278056951044,-0.21560842162446514 663 | 0.0,0.0 664 | 0.0,0.0 665 | 0.0,0.0 666 | -0.9151000238959708,0.019708549573718648 667 | -0.7147698254194772,0.8844702475169907 668 | -0.7770412277207783,0.8720083829073862 669 | 0.5096962957680959,-0.7688091117825203 670 | 1.7492511284490178,-1.3331969522417606 671 | -0.11048988838455717,0.08233505031475512 672 | 2.776619891809838,-1.9825777627679624 673 | 2.2749197496505245,-1.5016065998630916 674 | 0.46372283112501983,-0.21505842770527803 675 | -0.13919395889996494,0.07026721052351234 676 | -0.16784037575890362,0.09245602788507833 677 | -0.11011133730642603,0.0060529449454512245 678 | 0.6059837695153674,-0.18089353304511382 679 | -0.11387119972844498,0.08162094101747137 680 | -0.07807997065426736,0.0034047491588885866 681 | -0.040910175842640624,-0.12105732576843692 682 | -0.03922080453494921,-0.001292879839923002 683 | -0.03586523169294533,-0.021727807696346024 684 | [nan],[nan] 685 | 0.0,0.0 686 | 0.0,0.0 687 | 0.0,0.0 688 | 0.0,0.0 689 | 0.0,0.0 690 | -0.018692445601960674,0.0269159237314722 691 | 0.02678178006189851,-0.047910431269920194 692 | 0.028796162380601648,-0.03605307770236494 693 | 0.025549560445176422,-0.0465537190580496 694 | -0.0008221520264041995,-0.013145528248896029 695 | 0.0,0.0 696 | -0.030183992118681877,-0.0208208111458211 697 | -0.03917894347757959,-0.007933132657186166 698 | -0.05800576302973185,0.005345060570353747 699 | -0.09004325081456924,-0.11951455476741012 700 | -0.12814794066580745,-0.17476135248093286 701 | -0.04353003421639455,0.002399882669221626 702 | -0.0423217000403336,-0.04803719193004086 703 | -0.06493154892819117,-0.0049183084934287715 704 | -0.021108539728529708,-0.010495674529251991 705 | 0.0,0.0 706 | 0.0,0.0 707 | 0.038423794731786386,-0.011841868430474245 708 | 0.038699673172309546,-0.011023055003757623 709 | 0.03924597573687725,0.006323434198240994 710 | 0.03942449956856417,-0.005069187214273899 711 | 0.0329491976600705,0.006998003803146935 712 | 0.044886917317578955,-0.09420411447639138 713 | 0.018549180607878593,-0.07517490083458941 714 | 0.013095361429672013,-0.1038862782177417 715 | 0.01313639403198034,-0.04437111320806608 716 | 0.0,0.0 717 | -0.08729188906049384,0.05886680872848283 718 | -0.20135632109979046,0.11221522341136199 719 | -0.33637361132933813,0.2177786917683139 720 | -0.46654659982671187,0.27922973312834176 721 | -0.5668232558830717,0.33460272043604417 722 | -0.7016906126249237,0.4005192337898131 723 | -0.744846267996993,0.37349138035954543 724 | -0.8563698697859013,0.36081643452804374 725 | -0.8518441524100481,0.1870210806488318 726 | -0.9088022592422599,0.2154273753712614 727 | -0.797157697559084,0.22348282308176934 728 | -0.7491841106331271,0.18785468602729657 729 | -0.7191183195251603,0.18249687868315081 730 | -0.6306248956648871,-0.0028461532259783447 731 | -0.5333870215429134,-0.14209782354566522 732 | -0.43659898433598693,0.16209057148881212 733 | -0.4376576885343948,-0.008633184174580654 734 | -0.34690398194849936,-0.05891129140875943 735 | -0.29379390179079423,0.00991650194376192 736 | -0.30846099907770014,-0.13994234720393822 737 | -0.2732004793987116,0.09825003237230484 738 | -0.3350976378433492,-0.1754679999022642 739 | -0.34547399962525255,0.11943806118534804 740 | -0.30659392108351796,0.17030065956214807 741 | -0.39627487474380424,0.12197661856389004 742 | -0.32543642788282917,0.14714307817254255 743 | -0.40934946859119387,0.15280322332296623 744 | -0.4623781790596409,0.09176397687477979 745 | -0.40584574955842156,-0.004603057240281267 746 | -0.3677124819407027,0.039973277568244585 747 | -0.34857348862868615,0.046454471137426956 748 | -0.30385129379589687,-0.1625643370689988 749 | -0.3192536981073918,-0.09011142420101846 750 | -0.2919452039260527,-0.20781215772763625 751 | -0.2131765246647517,-0.24802088392958266 752 | -0.24688209519603768,-0.3500397490294816 753 | -0.24417365323363954,-0.1629485889352214 754 | -0.24269525106951625,-0.17500022676969856 755 | -0.2722893717164837,-0.5296197781859532 756 | -0.13342290940752688,-0.10859483963070166 757 | -0.18239358662249935,-0.09221171182178671 758 | -0.14591593263097277,-0.07293593745285464 759 | -0.13354099339225917,-0.04557082900249258 760 | -0.144499336150263,0.012130656906746697 761 | -0.12176337068375856,0.07776416330862154 762 | -0.10922947942712767,-0.07313961440627684 763 | -0.1521664847542495,-0.2821798606289497 764 | -0.1827805430705821,-0.08570932530182769 765 | -0.17599768952050643,-0.1176594453529745 766 | -0.16266124176361546,0.01217934610354773 767 | -0.12149832498400266,-0.05466587584376043 768 | -0.06209555438629784,-0.07167197941186305 769 | -0.02959798569046857,-0.040520558170772936 770 | -0.03930765072080126,0.004657296895988802 771 | -0.03689932545693997,-0.04284269384718432 772 | -0.03769005171138594,-0.01901257416782995 773 | -0.02949544215714652,0.015200973561839224 774 | -0.07034330427344376,0.30341902476206223 775 | -0.07075144911490641,0.10453202877190874 776 | -0.054236474989752995,0.03867181443741292 777 | -0.042279340856630605,0.02908616109610707 778 | -0.06765374113365492,0.039812006506404624 779 | -0.07790399392583082,0.007069138799395355 780 | -0.08168913270333578,-0.031981421754280606 781 | -0.08078389842107643,-0.028430966887303788 782 | -0.09574451279457652,0.02612000313234707 783 | -0.0781492636240832,0.024661125359132306 784 | -0.0718535740887729,0.05429247173817652 785 | -0.06136718810671677,0.0871337678699072 786 | -0.07335109588735073,0.003170641015462012 787 | -0.03367835882824791,0.0034301269104356057 788 | -0.03886594170476651,0.05231936505534606 789 | -0.040111628058737026,0.005535613592492207 790 | -0.03222585100066184,-0.03493155575791373 791 | -0.0409136811629569,0.004515295177934068 792 | -0.043255573803669574,-0.021013810839862073 793 | -0.027443524221894215,0.04293623339926063 794 | -0.032620956785578935,-0.06107924309688277 795 | -0.00984109554642898,-0.034912171060543076 796 | -0.09251631858496505,0.05032591432763169 797 | -0.06163071055935162,-0.05337061293198071 798 | -0.03110123377546016,-0.044083310716768485 799 | -0.058911145174066835,-0.05086564176444057 800 | -0.06270712938020463,0.020799167469840653 801 | -0.07744008988056188,-0.041429849227984046 802 | -0.055648898540622875,0.06377207143591745 803 | 0.004734958317930631,0.07944424082104486 804 | -0.024939172903561294,0.048453977497436254 805 | -0.002834893549221812,0.00845395477584562 806 | -0.016104969258806917,0.4184951954566413 807 | 0.026702499612546313,0.5888099578374557 808 | -0.03976620319993363,-0.023382306294162797 809 | -0.03823639472857604,-0.058508700646782326 810 | -0.038681841538690726,-0.08863189672892133 811 | -0.03358878016570211,-0.09190582702865673 812 | -0.032262267933095314,-0.08013404729783473 813 | -0.02872692288420028,-0.11125397555736377 814 | 0.0,0.0 815 | 0.0,0.0 816 | 0.0,0.0 817 | 0.0,0.0 818 | -0.00929196411383411,-0.015846799142429476 819 | -0.033558801526663934,-0.007216918702598407 820 | -0.039556407888444604,-0.003794495307896065 821 | -0.03727192058753319,-0.011591216281987086 822 | -0.03687149937000413,-0.012808205380438678 823 | -0.008500415467874155,-0.09028819672633899 824 | 0.0,0.0 825 | 0.0,0.0 826 | [nan],[nan] 827 | 0.0,0.0 828 | 0.0,0.0 829 | -0.03916608255697915,-0.0049678481815058845 830 | -0.14059677811936835,0.07189988385417734 831 | -0.2500049033080088,0.13270176588177912 832 | -0.3561456414479235,0.12304530073538493 833 | -0.48093900613896645,0.12463501324772644 834 | -0.6719820668131079,0.23623382873404025 835 | -0.7282441405575257,0.14278807698928728 836 | -0.8792793429877281,0.20077715267574803 837 | -0.8982304399191731,0.18346387089917524 838 | -0.8936708416358964,0.08230436225540022 839 | -0.9755686820096053,0.1800226576965673 840 | -0.9089446189105079,0.12807400311395062 841 | -0.8220209723476358,-0.0032957554020603035 842 | -0.8115710267212808,0.0654743608401387 843 | -0.7735440354041928,0.09186431416285215 844 | -0.7224824444406668,-0.01305805095089261 845 | -0.5908789592181186,-0.10503281242480049 846 | -0.6245047892021635,-0.0930781718247989 847 | -0.5650874796850343,-0.09948695417111317 848 | -0.6330828704855941,-0.10395900118097659 849 | -0.6019694705560344,-0.20809691483638976 850 | -0.6352907836347879,-0.19203829455020055 851 | -0.6670607096840433,-0.11732542328691006 852 | -0.6279017945917029,-0.14007907651711093 853 | -0.6766908468608808,-0.008388785235169526 854 | -0.7614786769469649,0.015686935916304826 855 | -0.8108399653813371,-0.020617464749926744 856 | -0.7821070050499344,-0.0550014783469502 857 | -0.8270466638065126,0.030025802107272917 858 | -0.7694229258981411,-0.00013477644691168065 859 | -0.7723643963289192,0.04093514536139542 860 | -0.745625656270128,0.07018348218890198 861 | -0.7279091609901455,0.049402335011830355 862 | -0.6385449464730621,0.034862923374970343 863 | -0.6456297025801571,0.02535303417069375 864 | -0.649360721123076,-0.008669309701588438 865 | -0.6123507891321753,-0.058761524552953526 866 | -0.5748036934835471,-0.07013332363976456 867 | -0.580073379535367,-0.04743425725101201 868 | -0.6157107472328135,-0.04475278015130566 869 | -0.6472722720810642,-0.0744104402846171 870 | -0.6157472704283614,-0.013158922434596109 871 | -0.7043262939454541,-0.01788308847161736 872 | -0.6777221037755963,-0.07482157260635221 873 | -0.609864110603113,-0.050916627752748446 874 | -0.5882432819317143,-0.04966071554539328 875 | -0.6464313201463383,-0.04707276651810656 876 | -0.5325547224442029,-0.07411559352033761 877 | -0.5996361260183776,-0.043766591529455355 878 | -0.5477859984453091,-0.051539917918175596 879 | -0.6130721525171346,-0.008688600078402454 880 | -0.5784192031726294,-0.0004207642192196236 881 | -0.5935790818664315,0.0009442339432734126 882 | -0.5708044650146242,-0.028869210793420738 883 | -0.5995016268437341,0.013504031112663016 884 | -0.6312541573484713,0.06451203503457045 885 | -0.692955345745658,0.14589450658812952 886 | -0.6337828142303148,0.10094886696175039 887 | -0.5804848084252622,0.08404378990589974 888 | -0.5839350718891858,0.06107923661704 889 | -0.5749799213452891,-0.000690578096123864 890 | -0.5879801831701362,0.0393186144178162 891 | -0.586130651086732,0.048867572797790436 892 | -0.5247337560298618,-0.009720654597931828 893 | -0.4926408694555945,-0.014066145930748064 894 | -0.4485209927152929,-0.0028460075837425406 895 | -0.43405043242450525,-0.02170836055637077 896 | -0.5246238903844512,0.0023171998538795034 897 | -0.465114500548948,0.010530871384199097 898 | -0.4693574625694953,0.05858174268641298 899 | -0.38154943731308366,0.018117384466568975 900 | -0.44069310207532636,0.09470051098487207 901 | -0.4691057674073264,0.12372973736784502 902 | -0.39481831975058307,0.05468596597895195 903 | -0.3623354527082213,0.036632299526694924 904 | -0.4307552474197904,0.03499407462028322 905 | -0.3432180556421748,0.027305625578591994 906 | -0.3917388211414236,-0.003621256868919867 907 | -0.34595344240126197,-0.0010626778420340726 908 | -0.3233001058643296,-0.048629823496551844 909 | -0.2718931944348159,-0.18335382117764418 910 | -0.34643938828313514,-0.002270366899495796 911 | -0.31701701239103486,-0.11512087367958548 912 | -0.2990338473344191,-0.05954481891573283 913 | -0.3389886688947468,-0.06524197183664399 914 | -0.34008295973965424,0.13198926525310614 915 | -0.3177613900524256,0.03298544567944079 916 | -0.3271893755456631,-0.034336194822760055 917 | -0.3481680696503027,0.20025194422842982 918 | -0.2676156984247028,0.03742287667367601 919 | -0.25589688299745666,0.10506668886529698 920 | -0.23907844624614843,0.14006186869479675 921 | -0.20279646706695065,-0.17728320211538 922 | -0.14720672928919387,0.03826939717398023 923 | -0.15619254303332675,-0.15214982452250803 924 | -0.1846454453841807,0.016152298781578242 925 | -0.17226300177505208,-0.07822379214191692 926 | -0.17966957192082816,-0.04539502584048834 927 | -0.18830250426065961,-0.04383659918706776 928 | -0.14047059054717304,-0.0635801204539497 929 | -0.1840036711614035,-0.021574027391706985 930 | -0.1861817185174265,-0.1691037014883028 931 | -0.19267197464456853,-0.062432932330138226 932 | -0.1579136705059037,-0.10025170160607727 933 | -0.14655799412673007,-0.050335510351473885 934 | -0.1140033877639818,-0.13437252197094046 935 | -0.12891766022223294,-0.08889558965077805 936 | -0.07625677402104232,-0.08508196560423559 937 | -0.08735358765551889,0.0013499893407162295 938 | -0.04391405082298088,-0.04249050012251034 939 | -0.03902740315756107,0.0006824481095811135 940 | -0.03041720267475183,0.0798849956678203 941 | -0.06720589874798746,-0.15117773421178016 942 | -0.061087069426981146,-0.046243163324931426 943 | -0.08550561655857736,-0.16164132167838413 944 | -0.09709546361050807,-0.01954277092310945 945 | -0.13794178381154096,0.05953267486657327 946 | -0.12653081552759748,0.0492952993790026 947 | -0.13440056081179377,0.04927532071900344 948 | -0.12025655216968564,-0.059555560715325515 949 | -0.10020184741543987,-0.02494207620339886 950 | -0.134826931218795,0.07477685700288222 951 | -0.08882504515397709,-0.03709058552940521 952 | -0.09473891907975913,-0.010372201602670545 953 | -0.0969642671575808,-0.010676524820052484 954 | -0.04875096625902098,-0.010140192953762494 955 | -0.009892522206866648,0.02289088041887588 956 | -0.051889198574744444,-0.007628179438561845 957 | -0.042825767621207614,-0.17693221498868314 958 | -0.029087725235317923,-0.03282868674117871 959 | -0.04216798229361789,-0.08282150309077277 960 | -0.04490208496633237,-0.17369560346061533 961 | -0.012021066737324705,0.012611617414856311 962 | -0.07285763691533746,-0.1731818423266176 963 | -0.05943371319909296,0.08639984467018576 964 | 0.001890019281825071,0.013359419031126989 965 | -0.016429489193074045,-0.11000753189739379 966 | -0.03372859658081777,-0.03358682478172424 967 | -0.04366550411652759,0.016492547172792107 968 | -0.03918996385672144,-0.0027844809479750246 969 | -0.0390834150250541,-0.0026823941956428433 970 | 0.0,0.0 971 | 0.0,0.0 972 | 0.0,0.0 973 | -0.010982838274239006,0.059622996304325465 974 | 0.0,0.0 975 | 0.0,0.0 976 | 0.0,0.0 977 | 0.0,0.0 978 | -0.027933179845482957,-0.018898409358844362 979 | nan,nan 980 | -0.03445792421426596,0.03726954663902265 981 | -0.015837648338900022,0.016877849601928167 982 | 0.0010249591499291303,-0.04097031644522575 983 | nan,nan 984 | nan,nan 985 | -0.039595110340517534,-0.006343336789851102 986 | -0.034781648327659814,0.04575569212861052 987 | -0.03766391255375614,-0.06816593886141507 988 | -0.034889071659303744,-0.07529992180511279 989 | -0.02166030670986408,-0.08110734261078048 990 | -0.010511601564696082,-0.09741242944007132 991 | -0.0069510178499656685,-0.10526760572818868 992 | 0.005388663410827372,-0.047985665381398535 993 | 0.002596584289522307,-0.020608731879779085 994 | 0.011677836165526626,-0.059807756431070634 995 | 0.02068230827610605,-0.06287577124639628 996 | 0.0,0.0 997 | 0.10433725502775971,0.30036476084351316 998 | 0.0,0.0 999 | 0.0,0.0 1000 | -0.0383360892531698,0.00984697740429629 1001 | -0.03979525200436884,0.0012825632520970162 1002 | -0.03590040909808812,-0.018898802360147005 1003 | -0.03914246816072897,-1.7834883690955093e-05 1004 | -0.011542381729197655,-0.16231682032113146 1005 | 0.0,0.0 1006 | 0.0,0.0 1007 | 0.03398997219300862,-0.06573520963295776 1008 | 0.07061769711526937,-0.05898328501243136 1009 | 0.07085233701427118,-0.041078080362320336 1010 | 0.07966891428021163,0.00613085833581542 1011 | 0.0903975968589971,-0.03106433822771176 1012 | 0.08027472148158808,-0.0107103775976605 1013 | 0.052208364892406794,-0.03945753540748411 1014 | [-0.051639271647293886],[-0.20063126853128935] 1015 | [-0.03746266102161897],[0.05077299763146858] 1016 | -0.03895175669658996,0.005027431937583229 1017 | nan,nan 1018 | [0.03920059482982964],[-0.005009090177333119] 1019 | 0.03940346125297644,0.03737303251884243 1020 | 0.0,0.0 1021 | [nan],[nan] 1022 | 0.0,0.0 1023 | 0.0,0.0 1024 | 0.0,0.0 1025 | [nan],[nan] 1026 | 0.03879675217133544,0.0042808906872463075 1027 | 0.03683725673837128,0.02318377365471316 1028 | [0.03730008542571277],[0.08392519220785374] 1029 | 0.040245403500006195,0.011190140032948668 1030 | [0.037343499028192124],[0.015801607633614313] 1031 | -0.023162645504765902,0.050258827978127175 1032 | 0.0,0.0 1033 | -0.024815898504164546,0.030151017315371616 1034 | 0.0,0.0 1035 | 0.0,0.0 1036 | 0.0024166541869101287,-0.001988816672896484 1037 | 0.021841220265951547,-0.03247813035525398 1038 | -0.11913618932703735,-0.005428668903117619 1039 | -0.15111888262331394,-0.1266525196046837 1040 | 0.006983690526379452,-0.08866972573452214 1041 | -0.07598433964939255,-0.0481244150666371 1042 | -0.21481855881089937,-0.1465904694434698 1043 | -0.050218821915400336,-0.03779703517393743 1044 | -0.016658960104192457,0.05964952247513414 1045 | -0.0021598607852722235,-0.027066698107799533 1046 | 0.07600408073287528,0.010348544856539382 1047 | 0.05342054976231192,-0.054633504552914186 1048 | 0.09523169225128619,0.05071697563360004 1049 | 0.06646489594965882,0.07204877646187816 1050 | 0.03462236092120582,-0.019152213599212126 1051 | 0.7579901845874184,0.17268983642918753 1052 | 0.0,0.0 1053 | 0.11654462887766948,0.1340909832633415 1054 | -0.05410305025509444,-0.12164285906533343 1055 | 0.08636357183806202,0.003697573413666882 1056 | [0.0],[-0.0] 1057 | 0.0,0.0 1058 | --------------------------------------------------------------------------------