From 92c2d414475bffde09bd9e4cf6a1e5bfca0ee6fd Mon Sep 17 00:00:00 2001 From: Simone Zandara Date: Wed, 16 Mar 2016 09:15:12 +0100 Subject: [PATCH] Initial commit --- Bin/.DS_Store | Bin 0 -> 6148 bytes Bin/slam.m | 8 + Bin/slamsm.m | 206 ++++++++++ Bin/wall_generator.m | 188 ++++++++++ EKF/Cloning.m | 24 ++ EKF/Correction_ScanMatching.m | 17 + EKF/Correction_ScanMatching.m~ | 23 ++ EKF/KalmanFilter4D_predict.m | 22 ++ EKF/KalmanFilter4D_predict.m~ | 33 ++ EKF/KalmanFilter_init.m | 9 + EKF/Prediction_ScanMatching_v1.m | 57 +++ EKF/Prediction_ScanMatching_v1.m~ | 54 +++ EKF/invert.m | 13 + Error/errorReport.m | 78 ++++ Etc/cfg.m | 153 ++++++++ Etc/cfgRobot.m | 19 + Etc/cfgSensor.m | 86 +++++ Graphics/displayAssociations.m | 15 + Graphics/displayMap.m | 5 + Graphics/displayMultOccGrid.m | 26 ++ Graphics/displayOccGrid.m | 24 ++ Graphics/displayOccGrid.m~ | 8 + Graphics/displayPoints.m | 15 + Graphics/displayRobot.m~ | 17 + Graphics/displayTrajectory.m | 31 ++ Graphics/displayTrajectory.m~ | 23 ++ Graphics/displayWorld.m | 24 ++ Graphics/draw_ellipse.m | 41 ++ Graphics/draw_ellipse.m~ | 42 +++ Graphics/draw_ellipse_v2.m | 44 +++ Graphics/graphicsRobot.m | 17 + Graphics/pol2cartM.m~ | 6 + Init/createMap.m | 70 ++++ Init/createPlot.m | 44 +++ Init/createRobots.m | 123 ++++++ Init/createSensors.m | 123 ++++++ Init/createSimStructures.m | 30 ++ Init/createSlamStructures.m | 11 + Init/createTime.m | 8 + Init/createWorld.m | 66 ++++ Init/createWorld.m~ | 36 ++ Kinematics/addPoseToMap.m | 7 + Kinematics/calcOdo2D.m | 15 + Kinematics/correctPose.m | 19 + Kinematics/correctPose.m~ | 20 + Kinematics/odo2.m | 20 + Kinematics/odo2_cart.m | 13 + Kinematics/simMotion.m | 88 +++++ Kinematics/simMotion.m~ | 101 +++++ Map/fillOccupancyGrid.m | 158 ++++++++ Map/fillOccupancyGrid.m~ | 156 ++++++++ Map/getCoordinates.m~ | 26 ++ Map/makeLocalOccupancyGrid.m | 145 +++++++ Map/makeOccupancyGrid.m | 39 ++ Map/makeOccupancyGrid.m~ | 91 +++++ Math/.DS_Store | Bin 0 -> 6148 bytes Math/Lines/inSegment.m | 11 + Math/Lines/inSegment.m~ | 11 + Math/Lines/intersectPoint2DWall.m | 88 +++++ Math/Lines/line2lineSIDistance.m | 22 ++ Math/Lines/or2rot.m | 0 Math/Lines/pt2lineDistance.asv | 56 +++ Math/Lines/pt2lineDistance.m | 79 ++++ Math/Lines/pt2lineDistance.m~ | 70 ++++ Math/Lines/pts2line.asv | 11 + Math/Lines/pts2line.m | 11 + Math/Lines/pts2lineSI.m | 10 + Math/Lines/pts2lineSIDistance.m | 22 ++ Math/Lines/ptsDistance.m | 7 + Math/Lines/r2Rt.m~ | 6 + Math/Lines/segseginter.m | 62 +++ Math/Lines/segseginter.m~ | 61 +++ Math/Matrix/calcCov.m | 12 + Math/Matrix/checkSingular.m | 39 ++ Math/Matrix/checkSingular.m~ | 30 ++ Math/Matrix/gridOverlap.m | 17 + Math/Matrix/gridOverlap.m~ | 11 + Math/Matrix/nMaxima.m | 37 ++ Math/Matrix/nMaxima.m~ | 31 ++ Math/Matrix/remapMatrix.m | 10 + Math/Matrix/repairSingular.m | 17 + Math/Rotations/Contents.m | 31 ++ Math/Rotations/Normalize.m | 2 + Math/Rotations/R2e.m | 56 +++ Math/Rotations/R2q.m | 88 +++++ Math/Rotations/Rotations.tar.gz | Bin 0 -> 8955 bytes Math/Rotations/au2q.m | 50 +++ Math/Rotations/e2R.m | 88 +++++ Math/Rotations/e2q.m | 79 ++++ Math/Rotations/e2v.m | 6 + Math/Rotations/e2v.m~ | 6 + Math/Rotations/eulerAngles.m | 69 ++++ Math/Rotations/flu2rdf.m | 44 +++ Math/Rotations/normAngle.m | 3 + Math/Rotations/pi2pc.m | 47 +++ Math/Rotations/polarReference.m | 12 + Math/Rotations/q2Pi.m | 59 +++ Math/Rotations/q2Q.m | 57 +++ Math/Rotations/q2R.m | 86 +++++ Math/Rotations/q2au.m | 46 +++ Math/Rotations/q2e.m | 103 +++++ Math/Rotations/q2eG.m | 45 +++ Math/Rotations/q2qc.m | 54 +++ Math/Rotations/q2v.m | 43 +++ Math/Rotations/qInv.m | 9 + Math/Rotations/qInv.m~ | 9 + Math/Rotations/qProd.m | 79 ++++ Math/Rotations/qRot.m | 48 +++ Math/Rotations/quaternion.m | 98 +++++ Math/Rotations/r2R.m | 6 + Math/Rotations/r2Rt.m | 6 + Math/Rotations/r2RtI.m | 8 + Math/Rotations/split.m | 65 ++++ Math/Rotations/v2R.m | 47 +++ Math/Rotations/v2au.m | 78 ++++ Math/Rotations/v2q.m | 74 ++++ Math/Rotations/w2omega.m | 57 +++ Math/Scan/cartProject.m | 14 + Math/Scan/cartProject.m~ | 12 + Math/Scan/polarProject.m | 13 + Math/Scan/polarProject.m~ | 13 + Math/Signals/fastsmooth.m | 54 +++ Math/Utils/Contents.m | 20 + Math/Utils/addRow.m | 8 + Math/Utils/applyTransform2Scan2D.m | 15 + Math/Utils/applyTransform2Scan2DPolar.m | 4 + Math/Utils/arrayshift.m | 10 + Math/Utils/checkConv.m | 31 ++ Math/Utils/checkConv.m~ | 6 + Math/Utils/chi2.m | 355 ++++++++++++++++++ Math/Utils/ci.m | 91 +++++ Math/Utils/compound.m | 48 +++ Math/Utils/dec2gc.m | 7 + Math/Utils/dec2gc.m~ | 15 + Math/Utils/deg2rad.m | 40 ++ Math/Utils/epose2qpose.m | 53 +++ Math/Utils/filterAssMatrix.m | 15 + Math/Utils/findLocalMaxima.m | 19 + Math/Utils/fitnessGrid2.m | 55 +++ Math/Utils/frameInv.m | 11 + Math/Utils/frameRef.m | 17 + Math/Utils/gc2dec.m | 14 + Math/Utils/gc2dec.m~ | 7 + Math/Utils/getCoordinates.m | 26 ++ Math/Utils/gridShift.m | 27 ++ Math/Utils/gridShift.m~ | 23 ++ Math/Utils/inverse_transformation.m | 6 + Math/Utils/invert.m | 13 + Math/Utils/max2d.m | 18 + Math/Utils/mean_last.mat | Bin 0 -> 438 bytes Math/Utils/minmaxScan.m | 33 ++ Math/Utils/minmaxScan.m~ | 21 ++ Math/Utils/multRow.m | 8 + Math/Utils/normAngle.m | 61 +++ Math/Utils/normvec.m | 74 ++++ Math/Utils/odoPolar2Cart.m | 8 + Math/Utils/perturbate3Dpoint.m | 14 + Math/Utils/perturbatePoint.m | 13 + Math/Utils/polyadd.m | 18 + Math/Utils/qpose2epose.m | 55 +++ Math/Utils/rad2deg.m | 40 ++ Math/Utils/searchPair.m | 13 + Math/Utils/stdErr.m | 27 ++ Math/Utils/transPolarCartScan.m | 289 ++++++++++++++ Math/Utils/transPolarCartScan.m~ | 215 +++++++++++ Math/Utils/transPolarCartScan3.m | 201 ++++++++++ Math/minmaxScan.m | 33 ++ ScanMatching/2D/AngleHistogram/ahsm.m | 218 +++++++++++ ScanMatching/2D/AngleHistogram/ahsm.m~ | 215 +++++++++++ .../2D/AngleHistogram/makeAnglehistogram.m | 19 + .../2D/AngleHistogram/makeAnglehistogram.m~ | 18 + .../2D/AngleHistogram/makeAxishistogram.m | 33 ++ .../2D/AngleHistogram/makeAxishistogram.m~ | 34 ++ ScanMatching/2D/Association/cpAssociation.m | 80 ++++ ScanMatching/2D/Association/cpAssociation.m~ | 84 +++++ ScanMatching/2D/Association/mahaAssociation.m | 203 ++++++++++ .../2D/Association/mahaAssociation.m~ | 203 ++++++++++ ScanMatching/2D/Association/mbAssociation.m | 185 +++++++++ ScanMatching/2D/Association/mbAssociation.m~ | 185 +++++++++ ScanMatching/2D/Association/mpAssociation.m | 104 +++++ ScanMatching/2D/Association/mpAssociation.m~ | 142 +++++++ ScanMatching/2D/Association/normAssociation.m | 116 ++++++ .../2D/Association/normAssociation.m~ | 118 ++++++ ScanMatching/2D/Association/p2lAssociation.m | 95 +++++ ScanMatching/2D/Association/p2lAssociation.m~ | 95 +++++ ScanMatching/2D/Association/plAssociation.m | 63 ++++ ScanMatching/2D/Association/plAssociation.m~ | 61 +++ ScanMatching/2D/FMT-Based/fmtsm.m | 316 ++++++++++++++++ ScanMatching/2D/FMT-Based/hann2d.m | 47 +++ ScanMatching/2D/FMT-Based/pprocess.m | 23 ++ ScanMatching/2D/FMT-Based/resamplePolar.m | 19 + ScanMatching/2D/GA/Selection/select_duckett.m | 53 +++ ScanMatching/2D/GA/Selection/select_rw.m | 42 +++ .../2D/GA/Selection/select_tournament.m | 54 +++ .../2D/GA/Selection/select_truncate.m | 20 + ScanMatching/2D/GA/fitnessGrid.m | 45 +++ ScanMatching/2D/GA/fitnessGrid.m~ | 40 ++ ScanMatching/2D/GA/ga.m | 259 +++++++++++++ ScanMatching/2D/GA/ga.m~ | 260 +++++++++++++ ScanMatching/2D/GA/popCovariance.m | 17 + ScanMatching/2D/GMapping/gmapping.m | 175 +++++++++ ScanMatching/2D/GMapping/gmapping.m~ | 132 +++++++ .../2D/Hough/circularCrossCorrelation.m | 17 + ScanMatching/2D/Hough/circularRotation.m | 7 + ScanMatching/2D/Hough/computeHoughSpectrum.m | 13 + ScanMatching/2D/Hough/computeHoughTransform.m | 36 ++ ScanMatching/2D/Hough/findLocalMaxima.m | 19 + ScanMatching/2D/Hough/findLocalMaxima2.m | 22 ++ .../2D/Hough/findLocalMaximaCircular.m | 11 + ScanMatching/2D/Hough/hough.m~ | 63 ++++ ScanMatching/2D/Hough/houghSM.m | 291 ++++++++++++++ ScanMatching/2D/Hough/houghSM.m~ | 298 +++++++++++++++ ScanMatching/2D/Hough/mean_last.mat | Bin 0 -> 438 bytes ScanMatching/2D/Hough/normalize.m | 6 + ScanMatching/2D/Hough/rotationMatrix.m | 2 + ScanMatching/2D/Hough/simpleLowPassFilter.m | 5 + ScanMatching/2D/Hough/simpleLowPassFilter2.m | 13 + .../2D/Hough/simpleLowPassFilterCircular.m | 7 + ScanMatching/2D/Hough/std_last.mat | Bin 0 -> 205 bytes ScanMatching/2D/ICP/icp.m | 145 +++++++ ScanMatching/2D/IDC/idc.m | 163 ++++++++ ScanMatching/2D/IDC/idc.m~ | 152 ++++++++ ScanMatching/2D/LF_SOG/lf_sof.m~ | 137 +++++++ ScanMatching/2D/LF_SOG/lf_sog.m | 190 ++++++++++ ScanMatching/2D/LF_SOG/lf_sog.m~ | 186 +++++++++ ScanMatching/2D/MbICP/MbICP.m | 148 ++++++++ ScanMatching/2D/MbICP/Normalize.m | 2 + ScanMatching/2D/MbICP/Precompute.m | 23 ++ ScanMatching/2D/Montecarlo/montecarlo.m | 165 ++++++++ ScanMatching/2D/Montecarlo/montecarlo.m~ | 158 ++++++++ ScanMatching/2D/NDT/NDT.m | 260 +++++++++++++ ScanMatching/2D/NDT/NDT.m~ | 266 +++++++++++++ ScanMatching/2D/NDT/NDTTest.mat | Bin 0 -> 30301 bytes ScanMatching/2D/NDT/build_NDT.m | 43 +++ ScanMatching/2D/NDT/getCoordinatesSubgrid.m | 32 ++ ScanMatching/2D/NDT/makeLocalOccupancyGrid.m~ | 135 +++++++ ScanMatching/2D/NDT/mean_last.mat | Bin 0 -> 438 bytes ScanMatching/2D/NDT/std_last.mat | Bin 0 -> 274 bytes ScanMatching/2D/Registration/LMSMatrix.m | 58 +++ .../2D/Registration/acc_registration.m | 21 ++ .../2D/Registration/pICRegistration.m | 54 +++ ScanMatching/2D/Registration/plRegistration.m | 59 +++ ScanMatching/2D/Registration/regist_besl.m | 47 +++ ScanMatching/2D/Registration/regist_trucco.m | 31 ++ ScanMatching/2D/Registration/registerCensi.m | 81 ++++ ScanMatching/2D/Registration/registerMbICP.m | 68 ++++ ScanMatching/2D/Registration/registerSVD.m | 34 ++ .../2D/Registration/register_martinez.m | 39 ++ .../2D/Registration/register_matlab.m | 107 ++++++ ScanMatching/2D/Rejection/x84.m | 25 ++ ScanMatching/2D/Rejection/x84.m~ | 12 + ScanMatching/2D/icpbSM.m | 226 +++++++++++ ScanMatching/2D/pIC/Composition.m | 19 + ScanMatching/2D/pIC/MStep.asv | 50 +++ ScanMatching/2D/pIC/mahaAssociation.asv | 113 ++++++ ScanMatching/2D/pIC/mahaAssociation.m~ | 158 ++++++++ ScanMatching/2D/pIC/pIC.asv | 87 +++++ ScanMatching/2D/pIC/pIC.m | 144 +++++++ ScanMatching/2D/pIC/pIC.m~ | 118 ++++++ ScanMatching/2D/pIC/pICRegistration.asv | 54 +++ ScanMatching/2D/pIC/pICScanMatching.m | 112 ++++++ ScanMatching/performSM.m | 29 ++ ScanMatching/performSM.m~ | 24 ++ Sensors/Sonar/multibeamComplex2D.m | 95 +++++ Sensors/Sonar/multibeamWall2D.m | 53 +++ Sensors/Sonar/singlebeamWall2D.m | 42 +++ Sensors/addPointsToMap.m | 38 ++ Sensors/calculateGlobalObs.m | 21 ++ Sensors/filterScan.m | 90 +++++ Sensors/getScan.m | 20 + Sensors/simObservation.m | 35 ++ TechSheet.odt | Bin 0 -> 89844 bytes TechSheet.pdf | Bin 0 -> 228377 bytes 273 files changed, 16261 insertions(+) create mode 100644 Bin/.DS_Store create mode 100644 Bin/slam.m create mode 100644 Bin/slamsm.m create mode 100644 Bin/wall_generator.m create mode 100644 EKF/Cloning.m create mode 100644 EKF/Correction_ScanMatching.m create mode 100644 EKF/Correction_ScanMatching.m~ create mode 100644 EKF/KalmanFilter4D_predict.m create mode 100644 EKF/KalmanFilter4D_predict.m~ create mode 100644 EKF/KalmanFilter_init.m create mode 100644 EKF/Prediction_ScanMatching_v1.m create mode 100644 EKF/Prediction_ScanMatching_v1.m~ create mode 100644 EKF/invert.m create mode 100644 Error/errorReport.m create mode 100644 Etc/cfg.m create mode 100644 Etc/cfgRobot.m create mode 100644 Etc/cfgSensor.m create mode 100644 Graphics/displayAssociations.m create mode 100644 Graphics/displayMap.m create mode 100644 Graphics/displayMultOccGrid.m create mode 100644 Graphics/displayOccGrid.m create mode 100644 Graphics/displayOccGrid.m~ create mode 100644 Graphics/displayPoints.m create mode 100644 Graphics/displayRobot.m~ create mode 100644 Graphics/displayTrajectory.m create mode 100644 Graphics/displayTrajectory.m~ create mode 100644 Graphics/displayWorld.m create mode 100644 Graphics/draw_ellipse.m create mode 100644 Graphics/draw_ellipse.m~ create mode 100644 Graphics/draw_ellipse_v2.m create mode 100644 Graphics/graphicsRobot.m create mode 100644 Graphics/pol2cartM.m~ create mode 100644 Init/createMap.m create mode 100644 Init/createPlot.m create mode 100644 Init/createRobots.m create mode 100644 Init/createSensors.m create mode 100644 Init/createSimStructures.m create mode 100644 Init/createSlamStructures.m create mode 100644 Init/createTime.m create mode 100644 Init/createWorld.m create mode 100644 Init/createWorld.m~ create mode 100644 Kinematics/addPoseToMap.m create mode 100644 Kinematics/calcOdo2D.m create mode 100644 Kinematics/correctPose.m create mode 100644 Kinematics/correctPose.m~ create mode 100644 Kinematics/odo2.m create mode 100644 Kinematics/odo2_cart.m create mode 100644 Kinematics/simMotion.m create mode 100644 Kinematics/simMotion.m~ create mode 100644 Map/fillOccupancyGrid.m create mode 100644 Map/fillOccupancyGrid.m~ create mode 100644 Map/getCoordinates.m~ create mode 100644 Map/makeLocalOccupancyGrid.m create mode 100644 Map/makeOccupancyGrid.m create mode 100644 Map/makeOccupancyGrid.m~ create mode 100644 Math/.DS_Store create mode 100644 Math/Lines/inSegment.m create mode 100644 Math/Lines/inSegment.m~ create mode 100644 Math/Lines/intersectPoint2DWall.m create mode 100644 Math/Lines/line2lineSIDistance.m create mode 100644 Math/Lines/or2rot.m create mode 100644 Math/Lines/pt2lineDistance.asv create mode 100644 Math/Lines/pt2lineDistance.m create mode 100644 Math/Lines/pt2lineDistance.m~ create mode 100644 Math/Lines/pts2line.asv create mode 100644 Math/Lines/pts2line.m create mode 100644 Math/Lines/pts2lineSI.m create mode 100644 Math/Lines/pts2lineSIDistance.m create mode 100644 Math/Lines/ptsDistance.m create mode 100644 Math/Lines/r2Rt.m~ create mode 100644 Math/Lines/segseginter.m create mode 100644 Math/Lines/segseginter.m~ create mode 100644 Math/Matrix/calcCov.m create mode 100644 Math/Matrix/checkSingular.m create mode 100644 Math/Matrix/checkSingular.m~ create mode 100644 Math/Matrix/gridOverlap.m create mode 100644 Math/Matrix/gridOverlap.m~ create mode 100644 Math/Matrix/nMaxima.m create mode 100644 Math/Matrix/nMaxima.m~ create mode 100644 Math/Matrix/remapMatrix.m create mode 100644 Math/Matrix/repairSingular.m create mode 100644 Math/Rotations/Contents.m create mode 100644 Math/Rotations/Normalize.m create mode 100644 Math/Rotations/R2e.m create mode 100644 Math/Rotations/R2q.m create mode 100644 Math/Rotations/Rotations.tar.gz create mode 100644 Math/Rotations/au2q.m create mode 100644 Math/Rotations/e2R.m create mode 100644 Math/Rotations/e2q.m create mode 100644 Math/Rotations/e2v.m create mode 100644 Math/Rotations/e2v.m~ create mode 100644 Math/Rotations/eulerAngles.m create mode 100644 Math/Rotations/flu2rdf.m create mode 100644 Math/Rotations/normAngle.m create mode 100644 Math/Rotations/pi2pc.m create mode 100644 Math/Rotations/polarReference.m create mode 100644 Math/Rotations/q2Pi.m create mode 100644 Math/Rotations/q2Q.m create mode 100644 Math/Rotations/q2R.m create mode 100644 Math/Rotations/q2au.m create mode 100644 Math/Rotations/q2e.m create mode 100644 Math/Rotations/q2eG.m create mode 100644 Math/Rotations/q2qc.m create mode 100644 Math/Rotations/q2v.m create mode 100644 Math/Rotations/qInv.m create mode 100644 Math/Rotations/qInv.m~ create mode 100644 Math/Rotations/qProd.m create mode 100644 Math/Rotations/qRot.m create mode 100644 Math/Rotations/quaternion.m create mode 100644 Math/Rotations/r2R.m create mode 100644 Math/Rotations/r2Rt.m create mode 100644 Math/Rotations/r2RtI.m create mode 100644 Math/Rotations/split.m create mode 100644 Math/Rotations/v2R.m create mode 100644 Math/Rotations/v2au.m create mode 100644 Math/Rotations/v2q.m create mode 100644 Math/Rotations/w2omega.m create mode 100644 Math/Scan/cartProject.m create mode 100644 Math/Scan/cartProject.m~ create mode 100644 Math/Scan/polarProject.m create mode 100644 Math/Scan/polarProject.m~ create mode 100644 Math/Signals/fastsmooth.m create mode 100644 Math/Utils/Contents.m create mode 100644 Math/Utils/addRow.m create mode 100644 Math/Utils/applyTransform2Scan2D.m create mode 100644 Math/Utils/applyTransform2Scan2DPolar.m create mode 100644 Math/Utils/arrayshift.m create mode 100644 Math/Utils/checkConv.m create mode 100644 Math/Utils/checkConv.m~ create mode 100644 Math/Utils/chi2.m create mode 100644 Math/Utils/ci.m create mode 100644 Math/Utils/compound.m create mode 100644 Math/Utils/dec2gc.m create mode 100644 Math/Utils/dec2gc.m~ create mode 100644 Math/Utils/deg2rad.m create mode 100644 Math/Utils/epose2qpose.m create mode 100644 Math/Utils/filterAssMatrix.m create mode 100644 Math/Utils/findLocalMaxima.m create mode 100644 Math/Utils/fitnessGrid2.m create mode 100644 Math/Utils/frameInv.m create mode 100644 Math/Utils/frameRef.m create mode 100644 Math/Utils/gc2dec.m create mode 100644 Math/Utils/gc2dec.m~ create mode 100644 Math/Utils/getCoordinates.m create mode 100644 Math/Utils/gridShift.m create mode 100644 Math/Utils/gridShift.m~ create mode 100644 Math/Utils/inverse_transformation.m create mode 100644 Math/Utils/invert.m create mode 100644 Math/Utils/max2d.m create mode 100644 Math/Utils/mean_last.mat create mode 100644 Math/Utils/minmaxScan.m create mode 100644 Math/Utils/minmaxScan.m~ create mode 100644 Math/Utils/multRow.m create mode 100644 Math/Utils/normAngle.m create mode 100644 Math/Utils/normvec.m create mode 100644 Math/Utils/odoPolar2Cart.m create mode 100644 Math/Utils/perturbate3Dpoint.m create mode 100644 Math/Utils/perturbatePoint.m create mode 100644 Math/Utils/polyadd.m create mode 100644 Math/Utils/qpose2epose.m create mode 100644 Math/Utils/rad2deg.m create mode 100644 Math/Utils/searchPair.m create mode 100644 Math/Utils/stdErr.m create mode 100644 Math/Utils/transPolarCartScan.m create mode 100644 Math/Utils/transPolarCartScan.m~ create mode 100644 Math/Utils/transPolarCartScan3.m create mode 100644 Math/minmaxScan.m create mode 100644 ScanMatching/2D/AngleHistogram/ahsm.m create mode 100644 ScanMatching/2D/AngleHistogram/ahsm.m~ create mode 100644 ScanMatching/2D/AngleHistogram/makeAnglehistogram.m create mode 100644 ScanMatching/2D/AngleHistogram/makeAnglehistogram.m~ create mode 100644 ScanMatching/2D/AngleHistogram/makeAxishistogram.m create mode 100644 ScanMatching/2D/AngleHistogram/makeAxishistogram.m~ create mode 100644 ScanMatching/2D/Association/cpAssociation.m create mode 100644 ScanMatching/2D/Association/cpAssociation.m~ create mode 100644 ScanMatching/2D/Association/mahaAssociation.m create mode 100644 ScanMatching/2D/Association/mahaAssociation.m~ create mode 100644 ScanMatching/2D/Association/mbAssociation.m create mode 100644 ScanMatching/2D/Association/mbAssociation.m~ create mode 100644 ScanMatching/2D/Association/mpAssociation.m create mode 100644 ScanMatching/2D/Association/mpAssociation.m~ create mode 100644 ScanMatching/2D/Association/normAssociation.m create mode 100644 ScanMatching/2D/Association/normAssociation.m~ create mode 100644 ScanMatching/2D/Association/p2lAssociation.m create mode 100644 ScanMatching/2D/Association/p2lAssociation.m~ create mode 100644 ScanMatching/2D/Association/plAssociation.m create mode 100644 ScanMatching/2D/Association/plAssociation.m~ create mode 100644 ScanMatching/2D/FMT-Based/fmtsm.m create mode 100644 ScanMatching/2D/FMT-Based/hann2d.m create mode 100644 ScanMatching/2D/FMT-Based/pprocess.m create mode 100644 ScanMatching/2D/FMT-Based/resamplePolar.m create mode 100644 ScanMatching/2D/GA/Selection/select_duckett.m create mode 100644 ScanMatching/2D/GA/Selection/select_rw.m create mode 100644 ScanMatching/2D/GA/Selection/select_tournament.m create mode 100644 ScanMatching/2D/GA/Selection/select_truncate.m create mode 100644 ScanMatching/2D/GA/fitnessGrid.m create mode 100644 ScanMatching/2D/GA/fitnessGrid.m~ create mode 100644 ScanMatching/2D/GA/ga.m create mode 100644 ScanMatching/2D/GA/ga.m~ create mode 100644 ScanMatching/2D/GA/popCovariance.m create mode 100644 ScanMatching/2D/GMapping/gmapping.m create mode 100644 ScanMatching/2D/GMapping/gmapping.m~ create mode 100644 ScanMatching/2D/Hough/circularCrossCorrelation.m create mode 100644 ScanMatching/2D/Hough/circularRotation.m create mode 100644 ScanMatching/2D/Hough/computeHoughSpectrum.m create mode 100644 ScanMatching/2D/Hough/computeHoughTransform.m create mode 100644 ScanMatching/2D/Hough/findLocalMaxima.m create mode 100644 ScanMatching/2D/Hough/findLocalMaxima2.m create mode 100644 ScanMatching/2D/Hough/findLocalMaximaCircular.m create mode 100644 ScanMatching/2D/Hough/hough.m~ create mode 100644 ScanMatching/2D/Hough/houghSM.m create mode 100644 ScanMatching/2D/Hough/houghSM.m~ create mode 100644 ScanMatching/2D/Hough/mean_last.mat create mode 100644 ScanMatching/2D/Hough/normalize.m create mode 100644 ScanMatching/2D/Hough/rotationMatrix.m create mode 100644 ScanMatching/2D/Hough/simpleLowPassFilter.m create mode 100644 ScanMatching/2D/Hough/simpleLowPassFilter2.m create mode 100644 ScanMatching/2D/Hough/simpleLowPassFilterCircular.m create mode 100644 ScanMatching/2D/Hough/std_last.mat create mode 100644 ScanMatching/2D/ICP/icp.m create mode 100644 ScanMatching/2D/IDC/idc.m create mode 100644 ScanMatching/2D/IDC/idc.m~ create mode 100644 ScanMatching/2D/LF_SOG/lf_sof.m~ create mode 100644 ScanMatching/2D/LF_SOG/lf_sog.m create mode 100644 ScanMatching/2D/LF_SOG/lf_sog.m~ create mode 100644 ScanMatching/2D/MbICP/MbICP.m create mode 100644 ScanMatching/2D/MbICP/Normalize.m create mode 100644 ScanMatching/2D/MbICP/Precompute.m create mode 100644 ScanMatching/2D/Montecarlo/montecarlo.m create mode 100644 ScanMatching/2D/Montecarlo/montecarlo.m~ create mode 100644 ScanMatching/2D/NDT/NDT.m create mode 100644 ScanMatching/2D/NDT/NDT.m~ create mode 100644 ScanMatching/2D/NDT/NDTTest.mat create mode 100644 ScanMatching/2D/NDT/build_NDT.m create mode 100644 ScanMatching/2D/NDT/getCoordinatesSubgrid.m create mode 100644 ScanMatching/2D/NDT/makeLocalOccupancyGrid.m~ create mode 100644 ScanMatching/2D/NDT/mean_last.mat create mode 100644 ScanMatching/2D/NDT/std_last.mat create mode 100644 ScanMatching/2D/Registration/LMSMatrix.m create mode 100644 ScanMatching/2D/Registration/acc_registration.m create mode 100644 ScanMatching/2D/Registration/pICRegistration.m create mode 100644 ScanMatching/2D/Registration/plRegistration.m create mode 100644 ScanMatching/2D/Registration/regist_besl.m create mode 100644 ScanMatching/2D/Registration/regist_trucco.m create mode 100644 ScanMatching/2D/Registration/registerCensi.m create mode 100644 ScanMatching/2D/Registration/registerMbICP.m create mode 100644 ScanMatching/2D/Registration/registerSVD.m create mode 100644 ScanMatching/2D/Registration/register_martinez.m create mode 100644 ScanMatching/2D/Registration/register_matlab.m create mode 100644 ScanMatching/2D/Rejection/x84.m create mode 100644 ScanMatching/2D/Rejection/x84.m~ create mode 100644 ScanMatching/2D/icpbSM.m create mode 100644 ScanMatching/2D/pIC/Composition.m create mode 100644 ScanMatching/2D/pIC/MStep.asv create mode 100644 ScanMatching/2D/pIC/mahaAssociation.asv create mode 100644 ScanMatching/2D/pIC/mahaAssociation.m~ create mode 100644 ScanMatching/2D/pIC/pIC.asv create mode 100644 ScanMatching/2D/pIC/pIC.m create mode 100644 ScanMatching/2D/pIC/pIC.m~ create mode 100644 ScanMatching/2D/pIC/pICRegistration.asv create mode 100644 ScanMatching/2D/pIC/pICScanMatching.m create mode 100644 ScanMatching/performSM.m create mode 100644 ScanMatching/performSM.m~ create mode 100644 Sensors/Sonar/multibeamComplex2D.m create mode 100644 Sensors/Sonar/multibeamWall2D.m create mode 100644 Sensors/Sonar/singlebeamWall2D.m create mode 100644 Sensors/addPointsToMap.m create mode 100644 Sensors/calculateGlobalObs.m create mode 100644 Sensors/filterScan.m create mode 100644 Sensors/getScan.m create mode 100644 Sensors/simObservation.m create mode 100644 TechSheet.odt create mode 100644 TechSheet.pdf diff --git a/Bin/.DS_Store b/Bin/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..5008ddfcf53c02e82d7eee2e57c38e5672ef89f6 GIT binary patch literal 6148 zcmeH~Jr2S!425mzP>H1@V-^m;4Wg<&0T*E43hX&L&p$$qDprKhvt+--jT7}7np#A3 zem<@ulZcFPQ@L2!n>{z**++&mCkOWA81W14cNZlEfg7;MkzE(HCqgga^y>{tEnwC%0;vJ&^%eQ zLs35+`xjp>T01 + [Rob] = Cloning(Rob,3); + end + [Rob] = Prediction_ScanMatching_v1(Rob); + Rob.state.x_full = reshape(Rob.state.x,3,size(Rob.state.x,1)/3); + Rob.state.dr_full(:,Tim.currentFrame) = Rob.state.dr; + Rob.state.gt_full(:,Tim.currentFrame) = Rob.state.gt; + else + % If no filter is used, the estimation is simple dead reckoning + Rob.state.x = odo2_cart(Rob.state.x,Rob.con.u); + Rob.state.x(3) = normAngle(Rob.state.x(3)); + Rob = addPoseToMap(Rob,Tim); + end + + %DEBUG + DISPLACEMENT_EU = ( Rob.state.d ) + DISPLACEMENT_AMOUNT = sqrt(Rob.state.d(1)^2 +Rob.state.d(2)^2) + + %% Observation Reading + + %Observe data (noise is added) + [RawL, SimSen] = simObservation(Rob, SimSen, SimWorld); + + SimSen.raw.timed(currentFrame).localCart = RawL.data.localCart; + SimSen.raw.timed(currentFrame).localPolar = RawL.data.localPolar; + + SimSen.raw.localCart = [ SimSen.raw.localCart; RawL.data.localCart]; + SimSen.raw.localPolar = [ SimSen.raw.localPolar; RawL.data.localPolar]; + %Rob.raw.covm = cat(3, Rob.raw.covm, RawL.data.covm(:,:,:) ); + + + % Filter the scan readings + if size(SimSen.raw.localCart,1) >= Opt.scan.maxscanpoints + Rob = filterScan(Rob, SimSen, Tim,Opt); + SimSen.raw.localCart = []; + SimSen.raw.localPolar = []; + end + + + %% Scanmatching + % retrieve a new map S and its applied displacement R + % and t DEBUG VERSION + if performSM(Rob,Tim,Opt) + + RefScan = getScan(Rob.Map,nIter-1); + + % Correction is performed only if points are present + if ~isempty(RefScan) && size(RefScan.data.localCart,1) > 1 &&... + size(Rob.filtered.localCart,1) > 1 + + if Opt.filter.usefilter + + N = size(Rob.state.x,1); + H = zeros(3,N); + + %find test scan position + j = currentFrame-1; + elm_x = N -(3*j-1); elm_y = N -(3*j-2); elm_th = N -(3*j-3); + testX = [Rob.state.x(elm_x); Rob.state.x(elm_y); Rob.state.x(elm_th)]; + + %Predict displacement + testXinv = invert(testX); %do the inversion transformation + res = compound(testXinv.x,Rob.state.x(1:3)); %do the compounding + motion.q = res.x; + + %Build H matrix + H(1:3,1:3) = res.Jb; + H(1:3,elm_x:elm_th) = (res.Ja * testXinv.oJ); + + %Predicted covariance + motion.Pq = H*Rob.state.P*H'; + motion.H = H; + + + Rob.con.u(1:2)=res.x(1:2); + Rob.con.Pq = motion.Pq; + + else + u_inv = frameInv(Rob.state.x_full(: , currentFrame-1)); + u_r = frameRef(Rob.state.x,u_inv); + Rob.con.u(1:2)=u_r(1:2); + + end + + [R t NI] = Opt.scanmatcher.handle(RefScan.data, Rob.filtered, Rob); + + R + t + + if Opt.filter.usefilter + [Rob.state.x,Rob.state.P] = Correction_ScanMatching([t R]',motion, Rob.state.x,Rob.state.P); + else + Rob = correctPose(Rob,R,t); + Rob = addPoseToMap(Rob,Tim); + end + + Rob.lastcorrectionP = [t R']; + end + + Rob.Map.x = Rob.state.x; + [Rob.Map Rob.filtered] = addPointsToMap(Rob.filtered,Rob.Map,Tim,Rob); + Rob.lastcorrection = Tim.currentFrame; + + else + [Rob.Map Rob.filtered] = addPointsToMap(Rob.filtered,Rob.Map,Tim,Rob); + end + + %% PLOTTING + + if (size(Rob.Map.grid,1) > 0) && Opt.error.display_result == 1 + + if Opt.plot.points_cr + set(Opt.plot.points_cr_h,'XData',Rob.Map.grid(:,1),'YData',Rob.Map.grid(:,2)); + end + + if Opt.plot.ground_truth + Opt.plot.ground_truth_data = [Opt.plot.ground_truth_data; Rob.state.gt(:,end)' ]; + set(Opt.plot.ground_truth_h,'XData',Opt.plot.ground_truth_data(:,1),'YData',Opt.plot.ground_truth_data(:,2)); + grob = graphicsRobot(Opt.plot.ground_truth_data(end,:)'); + set(Opt.plot.ground_truth_h_rob,'XData',grob(1,:),'YData',grob(2,:)); + end + + if Opt.plot.dead_reckoning + Opt.plot.dead_reckoning_data = [Opt.plot.dead_reckoning_data ; frameRef(Rob.state.dr(:,end),Rob.state0([1 2 6]),0)']; + set(Opt.plot.dead_reckoning_h,'XData',Opt.plot.dead_reckoning_data(:,1),'YData',Opt.plot.dead_reckoning_data(:,2)); + grob = graphicsRobot(Opt.plot.dead_reckoning_data(end,:)'); + set(Opt.plot.dead_reckoning_h_rob,'XData',grob(1,:),'YData',grob(2,:)); + end + + if Opt.plot.corrected + Opt.plot.corrected_data = [Opt.plot.corrected_data; frameRef(Rob.state.x(:,end),Rob.state0([1 2 6]),0)']; + set(Opt.plot.corrected_h,'XData',Opt.plot.corrected_data(:,1),'YData',Opt.plot.corrected_data(:,2)); + grob = graphicsRobot(Opt.plot.corrected_data(end,:)'); + set(Opt.plot.corrected_h_rob,'XData',grob(1,:),'YData',grob(2,:)); + + if Opt.plot.correction_uncertainty + draw_ellipse(Opt.plot.corrected_data(end,:),Rob.state.P,'r',Opt.plot.correction_uncertainty_h); + end + end + + drawnow; + end + out = 1; +end + + +if ~isempty(NI) + error_gt = errorReport(Rob,SimSen,Opt, NI); + save('std_last','error_gt'); +end diff --git a/Bin/wall_generator.m b/Bin/wall_generator.m new file mode 100644 index 0000000..bad8d2e --- /dev/null +++ b/Bin/wall_generator.m @@ -0,0 +1,188 @@ +%% Random world generator +%% SYNTAX POLYGON = wall_generator(0.3);figure; axis equal; plot(POLYGON(1,:),POLYGON(2,:)) + +classdef wall_generator + + properties + POLYGON=[]; + TRAJ = []; + end + + methods + + %constructor + function obj = wall_generator(varargin) + if nargin == 0 + + + obj.POLYGON=wall_generator.random_world(0.1); + else + if varargin{1}>0 + obj.POLYGON=wall_generator.random_world(varargin{1}); + else + obj.POLYGON=wall_generator.random_world_unstruct(varargin{1}); + end + + if nargin>1 && varargin{2}==1 + disp(obj); + traj_i = ginput; + if ( size(traj_i,1)>0) + obj.TRAJ = wall_generator.calc_traj(traj_i'); + end + end + end + + + end + + + function disp(obj) + figure; plot(obj.POLYGON(1,:),obj.POLYGON(2,:),'LineWidth',3); axis equal; + + end + + + + end + + methods (Static) + + function [traj] = calc_traj(traj) + for x = 2:size(traj,2) + + + dif = traj(:,x) - traj(:,x-1); + + if( sum(dif(1:2)) ~= 0 ) + traj(6,x) = atan2(dif(2,:), dif(1,:)); + else + traj(6,x) = traj(6,x-1); + end + end + end + + function [pol] = draw_ellipse(x0,y0,a,b,theta) % angular positions of vertices + t = linspace(0, 2*pi, 30); + + for i = 1:length(x0) + % pre-compute rotation angles (given in degrees) + cot = cosd(theta(i)); + sit = sind(theta(i)); + + % compute position of points used to draw current ellipse + xt = x0(i) + a(i) * cos(t) * cot - b(i) * sin(t) * sit; + yt = y0(i) + a(i) * cos(t) * sit + b(i) * sin(t) * cot; + + end + + [x_e,y_e] = poly2cw(xt, yt); + pol=[x_e;y_e]; + + end + + + function [pol] = draw_rect(x0,y0,a,b,theta) % angular positions of vertices + + x_rect(1,1) = x0-a/2; x_rect(1,2) = x0+a/2; x_rect(1,3) = x0+a/2; x_rect(1,4) = x0-a/2; x_rect(1,5) = x0-a/2; + x_rect(2,1) = y0+b/2; x_rect(2,2) = y0+b/2; x_rect(2,3) = y0-b/2; x_rect(2,4) = y0-b/2; x_rect(2,5) = y0+b/2; + + T=[cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1]; + + for l = 1:size(x_rect,2) + rotated_rect(:,l) = T(1:2,1:2)*[x_rect(1,l);x_rect(2,l)]; + end + [x_e,y_e] = poly2cw(rotated_rect(1,:),rotated_rect(2,:)); + + + + pol=[x_e;y_e]; + end + + %Generator + function polygon = random_world(ell_sigma) + + npoly = round(rand*10)+4; + polygon = []; + + + for n = 1:npoly + type = rand; + add=rand; + if add>0.8 + add='-'; + else + add='+'; + end + this_pol = []; + + x0_r = rand*5+1; + y0_r = rand*5+1; + a_r = rand*3+1; + b_r = rand*3+1; + theta_r = 0; %rand*(pi/2); + + if type > ell_sigma %rect + this_pol = wall_generator.draw_rect(x0_r,y0_r,a_r,b_r,theta_r); + else %ellipse + this_pol = wall_generator.draw_ellipse(x0_r,y0_r,a_r/2,b_r/2,theta_r); + end + + if ~isempty(polygon) + [polygon_x,polygon_y] = polybool(add,polygon(1,:),polygon(2,:),this_pol(1,:),this_pol(2,:)); + polygon=[polygon_x;polygon_y]; + else + polygon = this_pol; + end + end + + end + + %Generator + function polygon = random_world_unstruct(ell_sigma) + + last_p = [(rand*3)-1;(rand*3)-1]; + last_angle = 0; + angle = pi*2; + polygon = last_p; + init_p=last_p; + + for l = 1:1 %round(rand*2)+2 + npoly = 40; + for n = 1:npoly + angle = rand*(pi*2); + while angle>last_angle+pi/1.5 || angle < last_angle-pi/1.5 + angle = rand*(pi*2); + end + last_angle = angle; + d = rand*2; + + polygon_x = last_p(1) + d*cos(angle); + polygon_y = last_p(2) + d*sin(angle); + + polygon=[polygon [polygon_x;polygon_y] ]; + + last_p = [polygon_x; polygon_y]; + + end + + polygon=[polygon [NaN;NaN] ]; + + last_p = init_p + [(rand*8)-4;(rand*8)-4]; + init_p = last_p; + + last_angle = 0; + angle = pi*2; + polygon = [polygon last_p]; + + end + + end + + end + +end + + + + + diff --git a/EKF/Cloning.m b/EKF/Cloning.m new file mode 100644 index 0000000..9d7e3be --- /dev/null +++ b/EKF/Cloning.m @@ -0,0 +1,24 @@ +function [R]=Cloning(R,tam_x) +%Clone the positions btwn the scans +state = R.state.x; +P_state = R.state.P; + + +if tam_x == 8 +X=[state(1:tam_x); state([1 2 4]); state(tam_x+1:end)]; + +P=[P_state(1:tam_x,1:tam_x) P_state(1:tam_x,[1 2 4]) P_state(1:tam_x,(tam_x+1):end); + P_state([1 2 4],1:tam_x) P_state([1 2 4],[1 2 4]) P_state([1 2 4],(tam_x+1):end); + P_state((tam_x+1):end,1:tam_x) P_state((tam_x+1):end,[1 2 4]) P_state((tam_x+1):end,(tam_x+1):end)]; +end + +if tam_x == 3 +X=[state(1:tam_x); state([1 2 3]); state(tam_x+1:end)]; + +P=[P_state(1:tam_x,1:tam_x) P_state(1:tam_x,[1 2 3]) P_state(1:tam_x,(tam_x+1):end); + P_state([1 2 3],1:tam_x) P_state([1 2 3],[1 2 3]) P_state([1 2 3],(tam_x+1):end); + P_state((tam_x+1):end,1:tam_x) P_state((tam_x+1):end,[1 2 3]) P_state((tam_x+1):end,(tam_x+1):end)]; +end + +R.state.x=X; +R.state.P=P; \ No newline at end of file diff --git a/EKF/Correction_ScanMatching.m b/EKF/Correction_ScanMatching.m new file mode 100644 index 0000000..4eac105 --- /dev/null +++ b/EKF/Correction_ScanMatching.m @@ -0,0 +1,17 @@ +function [ x, P ] = Correction_ScanMatching( v, motion, x, P ) +%CORRECTION_SCANMATCHING Summary of this function goes here +% Detailed explanation goes here + +v(3) = normAngle(v(3)); + +S = motion.Pq + diag(diag(ones(3)))*0.01; + +K = (P*motion.H')/(S); +x = x + K * v; + +%Joseph Form +P=(eye(length(x))-K*motion.H)*P; +x(3) = normAngle(x(3)); + +end + diff --git a/EKF/Correction_ScanMatching.m~ b/EKF/Correction_ScanMatching.m~ new file mode 100644 index 0000000..c17ea95 --- /dev/null +++ b/EKF/Correction_ScanMatching.m~ @@ -0,0 +1,23 @@ +function [ x, P ] = Correction_ScanMatching( z, motion, x, P ) +%CORRECTION_SCANMATCHING Summary of this function goes here +% Detailed explanation goes here +z = q_corrected; +z(3) = normAngle(z(3)); + +%Do EKF update +v = z - motion.q; +v(3) = normAngle(v(3)); + +% S = motion.Pq + R_sm; +S = motion.Pq + diag(diag(ones(3)))*0.1; + + +K = (P*H')/(S); +x = x + K * v; + +%Joseph Form +P=(eye(length(x))-K*H)*P*(eye(length(x))-K*H)'; +x(3) = normAngle(x(3)); + +end + diff --git a/EKF/KalmanFilter4D_predict.m b/EKF/KalmanFilter4D_predict.m new file mode 100644 index 0000000..873738a --- /dev/null +++ b/EKF/KalmanFilter4D_predict.m @@ -0,0 +1,22 @@ +function [robot]=KalmanFilter4D_predict(robot) +%[X,P]=KalmanFilter(state,P,At,Z,H,R) +% +%If state, P and At are input of the function it performs a prediction. +%If Z,H and R are also present, an additional correction step is performed. +%The function returns the new state X and its uncertainty P + +global PARAM; +state = robot.state.x; +P = robot.state.P; +u = robot.con.u; + +rott=u(2)+state(4); + +J = [ 1 0 0 -sin(rott); + 0 1 0 cos(rott); + 0 0 1 0; + 0 0 0 1]; + +robot.state.x = odo2_cart(robot.state.x,u); + +robot.state.P = (J\P)*J' + robot.state.P_Added; \ No newline at end of file diff --git a/EKF/KalmanFilter4D_predict.m~ b/EKF/KalmanFilter4D_predict.m~ new file mode 100644 index 0000000..50479b4 --- /dev/null +++ b/EKF/KalmanFilter4D_predict.m~ @@ -0,0 +1,33 @@ +function [robot]=KalmanFilter4D_predict(robot) +%[X,P]=KalmanFilter(state,P,At,Z,H,R) +% +%If state, P and At are input of the function it performs a prediction. +%If Z,H and R are also present, an additional correction step is performed. +%The function returns the new state X and its uncertainty P + +global PARAM; +state = robot.state.x; +P = robot.state.P; +u = robot.con.u; +%--RENAME THE STATE VARS--------------------------------------------------- +x=cos(u(1))*u(2); +y=sin(u(1))*u(2); +yw=u(3); +z=0; + +rott=u(2)+state(4); + +J = [ 1 0 0 -sin(rott); + 0 1 0 cos(rott); + 0 0 1 0; + 0 0 0 1]; + +V = [ cos(rott) -u(1)*sin(rott) 0; + sin(rott) u(1)*cos(rott) 0; + 0 0 1 ]; + +robot.state.x(1) = state(1) + x; +robot.state.x(2) = state(2) + y; +robot.state.x(3) = state(3) + z; +robot.state.x(4) = normAngle(yw+state(4)); +robot.state.P = (J\P)*J' + (V\robot.state.P_Added([1 2 4],[1 2 4]))*V'; \ No newline at end of file diff --git a/EKF/KalmanFilter_init.m b/EKF/KalmanFilter_init.m new file mode 100644 index 0000000..6bbef47 --- /dev/null +++ b/EKF/KalmanFilter_init.m @@ -0,0 +1,9 @@ +function [ robot ] = KalmanFilter_init( robot ) +%KALMANFILTER_INIT Init the structures used to perform SLAM. In this case +%we add the velocity to the state and uncertainty. + +% robot.state.x = [robot.state.x; zeros(4,1) ]; +% robot.state.P = [robot.state.P zeros(4,4) ; zeros(4,4) zeros(4,4) ]; + +end + diff --git a/EKF/Prediction_ScanMatching_v1.m b/EKF/Prediction_ScanMatching_v1.m new file mode 100644 index 0000000..60113f0 --- /dev/null +++ b/EKF/Prediction_ScanMatching_v1.m @@ -0,0 +1,57 @@ +function [R]=Prediction_ScanMatching_v1(R) +% +%state, P and dis are inputs of the function to perform prediction. +%The function returns the new state X and its uncertainty P + +state = R.state.x; +P= R.state.P; + +dis.pos = R.con.u; +dis.cov = R.con.U; + +%--RENAME THE STATE VARS--------------------------------------------------- +x = state(1); +y = state(2); +% th = 0; + +x1 = dis.pos(1); +y1 = dis.pos(2); +th1 = normAngle(dis.pos(3)); +Q1 = dis.cov; + +% x2 = dis.pos(1); +% y2 = dis.pos(2); +% th2 = normAngle(dis.pos(3)); +% Q2 = dis.cov; + +tamany = length(state)-3; + +%--PREDICTION-------------------------------------------------------------- + +%Is the previous position plus the displacement (all should referenced in the same frame) +X = [x + x1; + y + y1; + state(3) + th1]; + +%The rest of the state (poses) is constant and doesn't changes +X=[X; state(4:end)]; + +%Matrix with respect to state vector X +A=[ 1, 0, 0; + 0, 1, 0; + 0, 0, 0 ]; + +%Matrix with respect to new displacement +B=[ 1, 0, 0; + 0, 1, 0; + 0, 0, 1 ]; + +%Build the Jacobians +J1=sparse(blkdiag(A, eye(tamany))); +J2=sparse([B;zeros(tamany,3)]); + +%Uncertainty update +P = J1*P*J1' + J2*Q1*J2'; + +R.state.x=X; +R.state.P=P; diff --git a/EKF/Prediction_ScanMatching_v1.m~ b/EKF/Prediction_ScanMatching_v1.m~ new file mode 100644 index 0000000..1dcac3e --- /dev/null +++ b/EKF/Prediction_ScanMatching_v1.m~ @@ -0,0 +1,54 @@ +function [R]=Prediction_ScanMatching_v1(R, dis) +% +%state, P and dis are inputs of the function to perform prediction. +%The function returns the new state X and its uncertainty P + +state = R.state.x; +P= R.state.P; + +%--RENAME THE STATE VARS--------------------------------------------------- +x = state(1); +y = state(2); +% th = 0; + +x1 = dis.pos(1); +y1 = dis.pos(2); +th1 = normAngle(dis.pos(3)); +Q1 = dis.cov; + +% x2 = dis.pos(1); +% y2 = dis.pos(2); +% th2 = normAngle(dis.pos(3)); +% Q2 = dis.cov; + +tamany = length(state)-3; + +%--PREDICTION-------------------------------------------------------------- + +%Is the previous position plus the displacement (all should referenced in the same frame) +X = [x + x1; + y + y1; + th1]; + +%The rest of the state (poses) is constant and doesn't changes +X=[X; state(4:end)]; + +%Matrix with respect to state vector X +A=[ 1, 0, 0; + 0, 1, 0; + 0, 0, 0 ]; + +%Matrix with respect to new displacement +B=[ 1, 0, 0; + 0, 1, 0; + 0, 0, 1 ]; + +%Build the Jacobians +J1=sparse(blkdiag(A, eye(tamany))); +J2=sparse([B;zeros(tamany,3)]); + +%Uncertainty update +P = J1*P*J1' + J2*Q1*J2'; + +R.state.x=state; +R.state.P=P; diff --git a/EKF/invert.m b/EKF/invert.m new file mode 100644 index 0000000..1071400 --- /dev/null +++ b/EKF/invert.m @@ -0,0 +1,13 @@ +function res = invert(p) + +x = p(1); y = p(2); +cosy = cos(p(3)); siny = sin(p(3)); + +res.x = [-p(1)*cosy - p(2)*siny; + p(1)*siny - p(2)*cosy; + -p(3)]; + +%Jacobian for the Inverse function (-)X +res.oJ = [-cosy, -siny, x*siny - y*cosy; + siny, -cosy, x*cosy + y*siny; + 0, 0, -1]; diff --git a/Error/errorReport.m b/Error/errorReport.m new file mode 100644 index 0000000..d520ead --- /dev/null +++ b/Error/errorReport.m @@ -0,0 +1,78 @@ +function [ err_mat ] = errorReport( Rob, Sen, Opt, NI ) +%ERRORREPORT Generate a report on the error using the ground truth + +%Find the angular shift in the yaw using quaternion mathematics + +Map = Rob.Map; + +ID = Opt.random.seed; +sm_iterations = Opt.scanmatcher.iterations; +sm_br = Opt.scanmatcher.Br; + + + +t1 = Rob.state.gt; +t2 = Rob.state.x +q1 = Rob.state.x(3); +q2 = Rob.state.gt(3); + +err_q = normAngle(q1 - q2) +err_x = t1(1) - t2(1) +err_y = t1(2) - t2(2) + +err_mat = [ err_x err_y 0; + [0 0 err_q] ; + Rob.state.d(1:2)' 0; + [0 0 Rob.state.d(3)]; + Rob.lastcorrectionP(1:2) 0; + [0 0 Rob.lastcorrectionP(3)]; + NI NI NI]; + +terr_x = 0; +terr_y = 0; +terr_z = 0; +terr_r = 0; +terr_p = 0; +terr_yaw = 0; + +out = 1; + +% fid = fopen('exp.txt', 'a'); +% fprintf(fid, '\n\n Scan SEED: %6.4f \n\n', ID); +% +% fprintf(fid, 'Parameters of the computation' ); +% fprintf(fid, 'Linear Error (Odometry): %6.4f %6.4f %6.4f \n', errRob(1:3)); +% fprintf(fid, 'Angular Error (Odometry): %6.4f %6.4f %6.4f \n', errRob(4:6)); +% fprintf(fid, 'Orientation Error Robot: %6.4f %6.4f %6.4f \n', errRob(7:9)); +% fprintf(fid, 'Error Sensor: %6.4f %6.4f %6.4f \n', errSen'); +% fprintf(fid, 'ScanMatcher Type: %s \n', sm_type); +% fprintf(fid, 'ScanMatcher Max Angle: %6.4f \n', sm_br); +% fprintf(fid, 'ScanMatcher Iterations: %6.4f \n', sm_iterations'); +% +% fprintf(fid, 'Corrected Robot Pose \n' ); +% +% fprintf(fid, 'Error X: %6.4f \n', err_x); +% fprintf(fid, 'Error Y: %6.4f \n', err_y); +% fprintf(fid, 'Error Z: %6.4f \n', err_z); +% fprintf(fid, 'Error Angle_Roll: %6.4f \n', err_q(1) ); +% fprintf(fid, 'Error Angle_Pitch: %6.4f \n', err_q(2) ); +% fprintf(fid, 'Error Angle_Yaw: %6.4f \n', err_q(3) ); +% +% fclose(fid); + +% sf = ['mean_last']; +% +% if exist( [sf '.mat']) +% load(sf,'terr*'); +% end +% terr_x = terr_x + err_x; +% terr_y = terr_y + err_y; +% terr_z = terr_z + err_z; +% terr_r = terr_r + err_q(1); +% terr_p = terr_p + err_q(2); +% terr_yaw = terr_yaw + err_q(3); +% +% save(sf,'terr*'); + +end + diff --git a/Etc/cfg.m b/Etc/cfg.m new file mode 100644 index 0000000..84a7c9c --- /dev/null +++ b/Etc/cfg.m @@ -0,0 +1,153 @@ +%Simone Zandara @ VICOROB +% 2D SLAM Simulator for ScanMatching configuration file. + +input_traj = 1; + +% Options +global Opt; +global DEBUG; + +% single algorithm debug option +DEBUG = struct(... + 'p2lAssociation',0,... + 'cpAssociation',0,... + 'mpAssociation',0,... + 'mbAssociation',0,... + 'mahaAssociation',0,... + 'normAssociation',0,... + 'gmapping',0,... + 'montecarlo',0,... + 'ga',0,... + 'houghSM',0,... + 'fmtsm',0,... + 'NDT',0,... + 'lf_sog',0,... + 'ahsm',0,... + 'all',1); + +Opt = struct(... + ...% Map parameters + 'map', struct(... % options for the map + 'type', 'occgrid',... % type of map. only occgrid + 'size', 1000,... + 'resolution', 4,... % cell size + 'realdata','../Input/Marina/sonar_marina_proc'),... %real data input + ... + ...% Plot + 'plot', struct(... + 'figure',[],... + 'ground_truth', 1, 'ground_truth_h', [],'ground_truth_h_rob', [] , 'ground_truth_data', [],... %Plot Ground Truth ? + 'corrected', 1, 'corrected_h', [],'corrected_h_rob', [] , 'corrected_data', [],... %Plot Corrected Pose ? + 'dead_reckoning', 1, 'dead_reckoning_h', [] ,'dead_reckoning_h_rob',[], 'dead_reckoning_data', [], ... %Plot Dead Reckoning Pose ? + 'correction_uncertainty',0,'correction_uncertainty_h',[],... + 'points_gt',1, 'points_gt_h',[],...%Plot Ground Truth points ? + 'points_dr',1, 'points_dr_h',[],...%Plot DeadReckoning points ? + 'points_cr',1, 'points_cr_h',[],...%Plot Corrected points ? + 'robot_display_n',1,... + 'robot_scale',0.1),... + ...% Randomizer parameters + 'random', struct(... % random generator options + 'newSeed', true,... % select new random seed? + 'fixedSeed', 215264,... % random seed for non-random runs + 'seed', [] ),... + ... + ...% Stochastic Filter + 'filter', struct(... + 'usefilter',1,... + 'type','ekf2d'... + ),... + ...% Scan matcher parameters + 'scanmatcher', struct(... + 'usematcher',1,... + 'motionamount', 0.0,... % amount of motion after which performing scan matching + 'distancethreshold', 1,... % NOT USED + ...% Possible values: icp, IDC, + ...% pIC,MbICP,ga,gmapping,montecarlo,ahsm,fmtsm,houghSM,NDT,lf_sog, icpbSM. + 'handle', @icp,... % scan matching algorithm in use + ... + ...% If using mixed algorithm 'icpbSM', you have to fill the next two with function handles. + ...% Possible values: mahaAssociation (mahalanobis distance), mbAssociation (metric based ), cpAssociation (closest Point) + ...% p2lAssociation (point to line),normAssociation (normal lines association). mpAssociation (range based association) + 'associate', @mahaAssociation,... % association function + ... + ...% Possible Values: regist_besl (Besley Unit Vector), register_martinez (Closed form Least Square) + ...% register_matlab (MATLAB minimization), registerSVD (Singular Value Decomposition), registerCensi (Censi's Lagrange Multiplier) + 'register', @ga,... % registration function + ... + 'rejection_rule',[],... % handle to rejection rule if wanted: only X84 available + 'projfilter', 0,... % use projection filter ? + 'iterations', 50,... % max number of iterations + 'Br', [0.1 0.5], ... % angular and radial trehshold used by algorithms + 'convalue', 0.00001,... % below this value the result is acceptable + 'niterconv', 3,... % minimum number of iterations before convergence check + 'chival', chi2inv(0.95,0.1)),... % tolerance value, different applications + ... + ...%Scan forming parameters + 'scan', struct(... % minimum number of points before scan composition + 'maxscanpoints', 1),... + ... + ...%Error parameters + 'error', struct(... + 'display_result',1 )... % show result? + ); + + + +% Random generator +if Opt.random.newSeed + Opt.random.seed = sum(100*clock); + randn('seed',Opt.random.seed); + rand('seed',Opt.random.seed); + fprintf('Random seed: %6.0f.\n',Opt.random.seed) + disp('To repeat this run, edit userData.m,') + disp(' add this seed to SimOpt.random.fixedSeed,') + disp(' and set SimOpt.random.newSeed to false.') +else + Opt.random.seed = Opt.random.fixedSeed; + randn('seed',Opt.random.seed); + rand('seed',Opt.random.seed); + fprintf('Fixed random seed: %6.0f.\n',Opt.random.seed) + disp('To make further runs truly random, edit userData.m,') + disp(' and set SimOpt.random.newSeed to true.') +end + +if ~input_traj + + trajr =[0, 0.2, 0.3;... % x + 0, 0.0, 0.5;... % y + 0, 0, 0;... % 0 + 0, 0, 0;... % 0 + 0, 0, 0;... % 0 + 0.2 ,0.4, 0.6]; % theta + + world = wall_generator(0.1); + random_world= world.POLYGON'; + +else + + world = wall_generator(0.1,1); + random_world= world.POLYGON'; + trajr = world.TRAJ; +end + + + + +Time = struct(... + 'dt', .1,... % sampling time, seconds + 'firstFrame', 1,... % first frame # + 'lastFrame', size(trajr,2),... + 'currentFrame', 1,... + 'step',1); + +%World information +RealWorld = struct(... + 'xMin', min(random_world(1,:)),... % playground limits + 'xMax', max(random_world(1,:)),... + 'yMin', min(random_world(2,:)),... + 'yMax', max(random_world(2,:)),... + 'zMin', -20,... + 'zMax', 20,... + 'points', [],... + 'segments', random_world,... + 'surface', []); diff --git a/Etc/cfgRobot.m b/Etc/cfgRobot.m new file mode 100644 index 0000000..f01c8d2 --- /dev/null +++ b/Etc/cfgRobot.m @@ -0,0 +1,19 @@ +%Robot configuration + +global MotionUncertainty; +global Opt; + + +Robot{1} = struct(... % DEFINED TRAJECTORY + 'id', 1,... % robot identifier + 'name', 'Dala',... % robot name + 'type', 'atrv',... % type of robot + 'motion', 'trajectory',... % motion model (trajectory, realdata) + 'data', Opt.map.realdata,... % file containing information + 'trajectory', trajr,... % ground truth trajectory if available + 'odoTrajectory', [],... % odometry trajectory if available + 'dxStd', 0.1*[1;1],... % odo linear and angular error std + 'errtype', 'gaussian',... % added motion noise type + 'doStd',0.0001); % orientation error [roll pitch yaw] + +MotionUncertainty = [ 0.1 0.1 0.00 ]; \ No newline at end of file diff --git a/Etc/cfgSensor.m b/Etc/cfgSensor.m new file mode 100644 index 0000000..f4b5889 --- /dev/null +++ b/Etc/cfgSensor.m @@ -0,0 +1,86 @@ +%Sensor Configuration +% +% Sensor{1} = struct(... +% 'id', 1,... % sensor identifier +% 'name', 'Sonar 2D',... % sensor name +% 'type', 'Singlebeam2D',... % type of sensor +% 'robot', 1,... % robot where it is mounted +% 'position', [0;0;0],... % position in robot +% 'orientationDegrees', [0;0;0],... % orientation in robot, roll pitch yaw +% 'positionStd', [0;0;0],... % position error std +% 'beamStd', [0.0524; 0.0524; 0.0524],... +% 'orientationStd', [0;0;0],... % orient. error std +% 'nBeams', 1,... %image size +% 'maxWidth', 120,... %multibeam angle coverage +% 'beamWidth', deg2rad(3),... %single beam width +% 'minRange', 0.5,... %beam strength in meters +% 'maxRange', 100,... +% 'dataPerScan', 1,... %data points returned each scan +% 'frameInMap', false); % Angular error on each beam + +% + +% %MULTIBEAM +% Sensor{1} = struct(... +% 'id', 1,... % sensor identifier +% 'name', 'Imagenex Delta T',... % sensor name +% 'type', 'Multibeam',... % type of sensor +% 'robot', 1,... % robot where it is mounted +% 'position', [0;0;0],... % position in robot +% 'orientationDegrees', [0;1.5708;0],... % orientation in robot, roll pitch yaw +% 'positionStd', [0;0;0],... % position error std +% 'beamStd', [0; 0; 0],... % [0.0524; 0.0524; 0.0524],... +% 'orientationStd', [0;0;0],... % orient. error std +% 'nBeams', 120,... %image size +% 'maxWidth', 120,... %multibeam angle coverage +% 'beamWidth', 3,... %single beam width +% 'minRange', 0.5,... %beam strength in meters +% 'maxRange', 100,... +% 'dataPerScan', 120,... %data points returned each scan +% 'frameInMap', false,... % Angular error on each beam +% 'outliers', 0.001); + +global Opt; + +%SINGLEBEAM 2D +SensorMultibeam = struct(... + 'id', 1,... % sensor identifier + 'name', 'Sonar 2D',... % sensor name + 'type', 'Multibeam2D',... % type of sensor + 'robot', 1,... % robot where it is mounted + 'position', [0;0;0],... % position in robot + 'orientationDegrees', [0;0;0],... % orientation in robot, roll pitch yaw + 'positionStd', [0;0;0],... % position error std + 'beamStd', 1*[0.00; 0.000000; 0.000000],... % [0.0524; 0.0524; 0.0524],... + 'orientationStd', [0;0;0],... % orient. error std + 'nBeams', 360,... %image size + 'maxWidth', 360,... %multibeam angle coverage + 'beamWidth', deg2rad(3),... %single beam width + 'minRange', 0.3,... %beam strength in meters + 'maxRange', 100,... + 'dataPerScan', 360,... %data points returned each scan + 'frameInMap', false,... % Angular error on each beam + 'outliers', 0.000); %Outliers probability + +% %REALDATA +SensorReal = struct(... + 'id', 1,... % sensor identifier + 'name', 'Laser',... % sensor name + 'type', 'RealData',... % type of sensor + 'robot', 1,... % robot where it is mounted + 'position', [0;0;0],... % position in robot + 'orientationDegrees', [0;0;0],... % orientation in robot, roll pitch yaw + 'positionStd', [0;0;0],... % position error std + 'beamStd', 1*[0.09; 0.000005; 0.000005],... % [0.0524; 0.0524; 0.0524],... + 'orientationStd', [0;0;0],... % orient. error std + 'nBeams', 360,... %image size + 'maxWidth', 360,... %multibeam angle coverage + 'beamWidth', deg2rad(3),... %single beam width + 'minRange', 0.5,... %beam strength in meters + 'maxRange', 300,... + 'dataPerScan', 360,... %data points returned each scan + 'frameInMap', false,... % Angular error on each beam + 'outliers', 0.000,... + 'data', Opt.map.realdata); % file containing information) + +Sensor{1} = SensorMultibeam; diff --git a/Graphics/displayAssociations.m b/Graphics/displayAssociations.m new file mode 100644 index 0000000..b662075 --- /dev/null +++ b/Graphics/displayAssociations.m @@ -0,0 +1,15 @@ +function [ o ] = displayAssociations( assp, hp ) +%DISPLAYASSOCIATIONS Summary of this function goes here +% Detailed explanation goes here +o=1; +if ~isempty(assp) + plot_assoc = []; + for i = 1:size(assp.new,1) + plot_assoc = [plot_assoc; [assp.new(i,1:2); assp.ref(i,1:2); NaN NaN ] ]; + end + + set(hp,'XData',plot_assoc(:,1),'YData',plot_assoc(:,2)); +end + +end + diff --git a/Graphics/displayMap.m b/Graphics/displayMap.m new file mode 100644 index 0000000..32735cd --- /dev/null +++ b/Graphics/displayMap.m @@ -0,0 +1,5 @@ +function displayMap(Map,c) + +plot3(Map.grid(:,1),Map.grid(:,2),Map.grid(:,3), ['.' c], 'MarkerSize', 6); + +end \ No newline at end of file diff --git a/Graphics/displayMultOccGrid.m b/Graphics/displayMultOccGrid.m new file mode 100644 index 0000000..c141282 --- /dev/null +++ b/Graphics/displayMultOccGrid.m @@ -0,0 +1,26 @@ +function [ out ] = displayMultOccGrid( grids ) +%DISPLAYOCCGRID display the sum of occupancy grids + +sx = size(grids(1).grid,1); +sy = size(grids(1).grid,2); +vals = zeros(sx,sy); + +for i = 1: sx + for j = 1: sy + for k = 1:size(grids,2) + vals(i,j) = sum(sum(grids(k).grid(i,j).mean)); + if vals(i,j) == 0 + vals(i,j) = -1; + else + vals(i,j) = 1; + end + end + end +end + +surf(1:sx,1:sy,vals'); +shading interp +axis equal +out=1; +end + diff --git a/Graphics/displayOccGrid.m b/Graphics/displayOccGrid.m new file mode 100644 index 0000000..d793591 --- /dev/null +++ b/Graphics/displayOccGrid.m @@ -0,0 +1,24 @@ +function [ out ] = displayOccGrid( grids ) +%DISPLAYOCCGRID display the occupancy grid + +sx = size(grids.grid,1); +sy = size(grids.grid,2); +vals = zeros(sx,sy); + +for i = 1: sx + for j = 1: sy + vals(i,j) = grids.grid(i,j); + end +end + +surf(1:sx,1:sy,vals'); +imshow(vals,[-1 1]); +g = colormap('gray'); +g(1,:) = [ 1 0 0]; +%g(2:127,:) = [ 0.2 0.2 0.7]; +colormap(g); +% shading interp +% axis equal +out=1; +end + diff --git a/Graphics/displayOccGrid.m~ b/Graphics/displayOccGrid.m~ new file mode 100644 index 0000000..d2fce1d --- /dev/null +++ b/Graphics/displayOccGrid.m~ @@ -0,0 +1,8 @@ +function [ out ] = displayOccGrid( grid ) +%DISPLAYOCCGRID display the occupancy grid + +vals = grid(:,:).cov; +pcolor(grid); +out=1; +end + diff --git a/Graphics/displayPoints.m b/Graphics/displayPoints.m new file mode 100644 index 0000000..e5d631d --- /dev/null +++ b/Graphics/displayPoints.m @@ -0,0 +1,15 @@ +function displayPoints(Map,c,ppolar) + + + +if(nargin==3 && ppolar==1) + polar(Map(:,1), Map(:,2), ['.' c]); +else + + if(size(Map,1) == 3) + plot3(Map(:,1),Map(:,2),Map(:,3), ['.' c], 'MarkerSize', 6); + else + plot(Map(:,1),Map(:,2), ['.' c], 'MarkerSize', 6); + end + +end \ No newline at end of file diff --git a/Graphics/displayRobot.m~ b/Graphics/displayRobot.m~ new file mode 100644 index 0000000..8b6d418 --- /dev/null +++ b/Graphics/displayRobot.m~ @@ -0,0 +1,17 @@ +function [ out ] = displayRobot( Rob, c ) +%DISPLAYROBOT Display robot information gathered so far and project it onto +%the real world +global Opt; + hold on + displayMap(Rob.Map,c); + trajs = [Rob.Map.prev.x Rob.Map.x ]; + + + for jj = 1:size(trajs,2) + trajs(:,jj) = frameRef(trajs(:,jj),Rob.state0,0); + end + displayTrajectory(trajs ,c); + + out = 1; +end + diff --git a/Graphics/displayTrajectory.m b/Graphics/displayTrajectory.m new file mode 100644 index 0000000..c65efb4 --- /dev/null +++ b/Graphics/displayTrajectory.m @@ -0,0 +1,31 @@ +function displayTrajectory(Traj,c,scale) + +if nargin==2 + scale = 0.4; +end + +trp = [ -1 -0.5 0 ; 1 0 0 ; -1 0.5 0]; +trp = (trp * 0.5) * scale; +Traj = Traj'; +robs = size(Traj,1); +nsteps = floor(max(log(robs^3),1)); +plot(Traj(:,1),Traj(:,2),'.c'); + +for i = 1 : 1 : size(Traj,1) + + q = Traj(i,4); + pos = Traj(i,1:3)'; + + trpt = [ e2R([0 0 q]) * trp(1,:)' + pos ... + e2R([0 0 q]) * trp(2,:)' + pos ... + e2R([0 0 q]) * trp(3,:)' + pos ]; + + fill3(trpt(1,:), trpt(2,:), trpt(3,:), c); + + %draw_ellipse(pose,,'c' +end + + + + +end \ No newline at end of file diff --git a/Graphics/displayTrajectory.m~ b/Graphics/displayTrajectory.m~ new file mode 100644 index 0000000..b5cb155 --- /dev/null +++ b/Graphics/displayTrajectory.m~ @@ -0,0 +1,23 @@ +function displayTrajectory(Traj,c) + +trp = [ -1 -0.5 0 ; 1 0 0 ; -1 0.5 0]; +trp = trp * 0.5; +Traj = Traj'; + +for i = 1 : 1 : size(Traj,1) + + q = Traj(i,4:7)'; + pos = Traj(i,1:3)'; + + trpt = [ qRot( trp(1,:)', q ) + pos ... + qRot( trp(2,:)', q ) + pos ... + qRot( trp(3,:)', q ) + pos ]; + + fill3(trpt(1,:), trpt(2,:), trpt(3,:), c); + + draw_ellipse(pose,,'c' +end + + + +end \ No newline at end of file diff --git a/Graphics/displayWorld.m b/Graphics/displayWorld.m new file mode 100644 index 0000000..cc817a1 --- /dev/null +++ b/Graphics/displayWorld.m @@ -0,0 +1,24 @@ +function displayWorld(W) + + + [Xmesh Ymesh] = meshgrid(W.df:W.df:size(W.Surface,1)*W.df, W.df:W.df:size(W.Surface,2)*W.df); + + %TextureImage=imread('mosaic.jpg'); + %Resize texture image at size of surface and set it to the correct + %orientation for matching terrain bumps + %TextureImage=imresize(TextureImage,size(Surface)); + %TextureImage=flipdim(W.TextureImage,1); + + % [Xmesh Ymesh] = meshgrid(min(profile(:,1)):0.1:max(profile(:,1)),min(profile(:,2)):0.1:max(profile(:,2))); + % Z = griddata(profile(:,1),profile(:,2),profile(:,3),Xmesh,Ymesh); + %TextureImage=imresize(TextureImage,size(Z)); + %TextureImage=flipdim(TextureImage,1); + + + + + h = surface(Ymesh,Xmesh,W.Surface'); + shading flat + + %set(h,'CData',TextureImage,'FaceColor','texturemap'); +end \ No newline at end of file diff --git a/Graphics/draw_ellipse.m b/Graphics/draw_ellipse.m new file mode 100644 index 0000000..3a2571b --- /dev/null +++ b/Graphics/draw_ellipse.m @@ -0,0 +1,41 @@ +function hp = draw_ellipse(pos, cov, color, ObjectHandle) +%Two ways to call the function: +% +%[ObjectHandle] = draw_ellipse(pos, cov, color) Plots a ellipse with center at [pos], +% shape determined by [cov] and color +% by [color]. Returns figure handler +% +%[]=draw_ellipse(pos, cov, color,ObjectHandle) Redefines shape and position of a +% previously defined ellipse called "handle". + +%chi2=9.2103; %confian�a del 99% +%chi2=5.991; %confian�a del 95% +persistent CIRCLE + +numero=size(cov,3); +p=[]; + + +if isempty(CIRCLE) + tita = linspace(0, 2*pi,40); %es similar a usar :, genera un vector de 0 a 2pi amb 40 dades + CIRCLE = [cos(tita); sin(tita)]; +end + +for i=1:numero + [V,D]=eig(full(cov(1:2,1:2,i))); + ejes=sqrt(chi2inv(0.95,2)*diag(D)); + P = (V*diag(ejes))*CIRCLE; + p=[p [P(1,:)+pos(i,1);P(2,:)+pos(i,2)] [NaN; NaN]]; +end + +if nargin==3 + hp = line(p(1,:), p(2,:)); + set(hp,'Color', color); + legend('A') +elseif nargin==4 + set(ObjectHandle, 'XData',p(1,:),'YData',p(2,:),'Color', color); +end + + + + diff --git a/Graphics/draw_ellipse.m~ b/Graphics/draw_ellipse.m~ new file mode 100644 index 0000000..830eccd --- /dev/null +++ b/Graphics/draw_ellipse.m~ @@ -0,0 +1,42 @@ +function hp = draw_ellipse(pos, cov, color,Opt,ObjectHandle) +%Two ways to call the function: +% +%[ObjectHandle] = draw_ellipse(pos, cov, color) Plots a ellipse with center at [pos], +% shape determined by [cov] and color +% by [color]. Returns figure handler +% +%[]=draw_ellipse(pos, cov, color,ObjectHandle) Redefines shape and position of a +% previously defined ellipse called "handle". + +%chi2=9.2103; %confian�a del 99% +%chi2=5.991; %confian�a del 95% +persistent CIRCLE + +numero=size(cov,3); +p=[]; + + +if isempty(CIRCLE) + tita = linspace(0, 2*pi,40); %es similar a usar :, genera un vector de 0 a 2pi amb 40 dades + CIRCLE = [cos(tita); sin(tita)]; +end + +for i=1:numero + [V,D]=eig(full(cov(1:2,1:2,i))); + ejes=sqrt(chi2inv(0.95,2)*diag(D)); + P = (V*diag(ejes))*CIRCLE; + p=[p [P(1,:)+pos(i,1);P(2,:)+pos(i,2)] [NaN; NaN]]; +end + +if nargin==3 + hp = line(p(1,:), p(2,:)); + set(hp,'Color', color); + + +elseif nargin==4 + set(ObjectHandle, 'XData',p(1,:),'YData',p(2,:),'Color', color); +end + + + + diff --git a/Graphics/draw_ellipse_v2.m b/Graphics/draw_ellipse_v2.m new file mode 100644 index 0000000..657a1c6 --- /dev/null +++ b/Graphics/draw_ellipse_v2.m @@ -0,0 +1,44 @@ +function hp = draw_ellipse_v2(pos, cov, color,ObjectHandle) +%Two ways to call the function: +% +%[ObjectHandle] = draw_ellipse(pos, cov, color) Plots a ellipse with center at [pos], +% shape determined by [cov] and color +% by [color]. Returns figure handler +% +%[]=draw_ellipse(pos, cov, color,ObjectHandle) Redefines shape and position of a +% previously defined ellipse called "handle". + +global PARAM; +%chi2=9.2103; %confian�a del 99% +%chi2=5.991; %confian�a del 95% +persistent CIRCLE + +PARAM.chi2 = chi2inv(0.99,2); +numero=size(cov,3); +p=[]; + + +if isempty(CIRCLE) + tita = linspace(0, 2*pi,40); %es similar a usar :, genera un vector de 0 a 2pi amb 40 dades + CIRCLE = [cos(tita); sin(tita)]; +end + +for i=1:numero + if ~isnan(pos(i,1)) + [V,D]=eig(full(cov(1:2,1:2,i))); + ejes=sqrt(PARAM.chi2*diag(D)); + P = (V*diag(ejes))*CIRCLE; + p=[p [P(1,:)+pos(i,1);P(2,:)+pos(i,2)] [NaN; NaN]]; + end +end + +if (nargin==3) && ~isempty(p) + hp = line(p(1,:), p(2,:)); + set(hp,'Color', color); +elseif nargin==4 + set(ObjectHandle, 'XData',p(1,:),'YData',p(2,:),'Color', color); +end + + + + diff --git a/Graphics/graphicsRobot.m b/Graphics/graphicsRobot.m new file mode 100644 index 0000000..5bb4c31 --- /dev/null +++ b/Graphics/graphicsRobot.m @@ -0,0 +1,17 @@ +function [ trpt ] = graphicsRobot( pos) +%DISPLAYROBOT Display robot shape + +global Opt +scale = Opt.plot.robot_scale; + +q = pos(3); +pos = [pos(1:2); 1]; +trp = [ -1 -0.5 1; 1 0 1; -1 0.5 1]; +trp = (trp) * scale; + +trpt = [ e2R([0 0 q]) * trp(1,:)' + pos ... + e2R([0 0 q]) * trp(2,:)' + pos ... + e2R([0 0 q]) * trp(3,:)' + pos ]; + +end + diff --git a/Graphics/pol2cartM.m~ b/Graphics/pol2cartM.m~ new file mode 100644 index 0000000..a09cda7 --- /dev/null +++ b/Graphics/pol2cartM.m~ @@ -0,0 +1,6 @@ +function B = pol2cartM(A) +%Convert a row ordered matrix of polar coordinates to cartesian coordinates +B = zeros(size(A,1),2); +for i = 1:size(A,1) + [B(i,1) B(i,2)] = 2polcart(A(i,1), A(i,1)); +end \ No newline at end of file diff --git a/Init/createMap.m b/Init/createMap.m new file mode 100644 index 0000000..ed6646a --- /dev/null +++ b/Init/createMap.m @@ -0,0 +1,70 @@ +function [Mapm Lmk Obs] = createMap(Rob,Sen,Tim) + +% CREATEMAP Create an empty Map structure. +% Map = CREATEMAP(Rob,Sen,Lmk,Opt) creates the structure Map from the +% information contained in Rob, The map is saved as a list of point. +% 'occgrid' is cited but not working. +%Simone Zandara @ VICOROB TODO + +global Opt + +Map.type = Opt.map.type; + +switch(Map.type) + + case 'occgrid' + Map.x = zeros(size(Rob.state.x)); %6D current pose (correspond to Robot.frame) + Map.P = zeros(size(Rob.state.P)); + Map.grid = []; %zeros(Sen.par.dataPerScan* (Tim.firstFrame-Tim.lastFrame + 1),3 ); + Map.datasize = Sen.par.dataPerScan; + Map.used = 1; %unused + + MM = struct('points',[]); + + steps = ceil((Tim.lastFrame - Tim.firstFrame) / Tim.step); + + Map.prev.gridlocal(1:steps) = MM; + Map.prev.gridpolar(1:steps) = MM; + Map.prev.gridlocalpolar(1:steps) = MM; + Map.prev.gridcovm(1:steps) = MM; + Map.prev.written(1:steps) = 0; + + otherwise + +end +Mapm = Map; +% Map.size = n; + + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Init/createPlot.m b/Init/createPlot.m new file mode 100644 index 0000000..b2328d9 --- /dev/null +++ b/Init/createPlot.m @@ -0,0 +1,44 @@ +function [ O ] = createPlot( ) +%CREATEPLOT Generates the plot to show the SLAM + +global Opt + + +scrsz = get(0,'ScreenSize'); +Opt.plot.figure=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); +%axis([-20 20 -20 20]); +axis equal; +xlabel('X (m)'); ylabel('Y (m)'); +hold all + +if Opt.plot.ground_truth + Opt.plot.ground_truth_h = plot(NaN,NaN,'r'); + Opt.plot.ground_truth_h_rob = fill(NaN,NaN,'r'); +end +if Opt.plot.dead_reckoning + Opt.plot.dead_reckoning_h = plot(NaN,NaN,'k'); + Opt.plot.dead_reckoning_h_rob = fill(NaN,NaN,'k'); +end +if Opt.plot.corrected + Opt.plot.corrected_h = plot(NaN,NaN,'b'); + Opt.plot.corrected_h_rob = fill(NaN,NaN,'b'); +end + +if Opt.plot.correction_uncertainty + Opt.plot.correction_uncertainty_h = plot(NaN,NaN,'--b'); +end + +if Opt.plot.points_gt + Opt.plot.points_gt_h = plot(NaN,NaN,['.r'], 'MarkerSize', 6); +end +if Opt.plot.points_dr + Opt.plot.points_dt_h = plot(NaN,NaN,['.k'], 'MarkerSize', 6); +end +if Opt.plot.points_cr + Opt.plot.points_cr_h = plot(NaN,NaN,['.b'], 'MarkerSize', 6); +end + + + +end + diff --git a/Init/createRobots.m b/Init/createRobots.m new file mode 100644 index 0000000..68ddbd3 --- /dev/null +++ b/Init/createRobots.m @@ -0,0 +1,123 @@ +function Rob = createRobots(Robot,Tim) + +% CREATEROBOTS Create robots structure array. +% Rob = CREATEROBOTS(Robot) Generates the Rob structure that contains all +% the information about the robot and the state vector and uncertainty. + +global MotionUncertainty; + +for rob = 1:numel(Robot) + + Ri = Robot{rob}; % input robot structure + + % identification + Ro.rob = rob; + Ro.id = Ri.id; + Ro.name = Ri.name; + Ro.type = Ri.type; + Ro.motion = Ri.motion; + + Ro.state0 = [Ri.trajectory(1:3,1); Ri.trajectory(4:6,1) ]; + + %Ro.raw.globalCart2D = zeros(Opt.scan.maxscanpoints,3); + %Ro.raw.globalPolar2D = zeros(Opt.scan.maxscanpoints,3); + Ro.raw.localCart = []; %zeros(Opt.scan.maxscanpoints,3); + Ro.raw.localPolar = []; %zeros(Opt.scan.maxscanpoints,2); + Ro.raw.covm = []; + + %Ro.filtered.globalCart2D = zeros(Opt.scan.maxscanpoints,3); + %Ro.filtered.globalPolar2D = zeros(Opt.scan.maxscanpoints,3); + Ro.filtered.localCart = []; %zeros(Opt.scan.maxscanpoints,3); + Ro.filtered.localPolar = []; %zeros(Opt.scan.maxscanpoints,2); + Ro.filtered.covm = []; + Ro.filtered.last = 0; + Ro.filtered.pos = [0 0 0]'; + Ro.filtered.poses = []; + Ro.filtered.timePoses = []; + + Ro.Map = []; + Ro.lastcorrection = 0; + + Ro.error_type = Ri.errtype; + + % control + switch Ri.motion + + case {'trajectory'} + + Ro.con.u = [0 0 0]; + Ro.con.incr = 1; + Ro.con.U = diag([Ri.dxStd; Ri.doStd]); + Ro.con.d = [0 0 0]; + Ro.vel.x = []; + Ro.vel.P = []; + Ro.trajectory = Ri.trajectory; + + otherwise + error('Unknown motion model %s from robot %d.',Robot.motion,Robot.id); + end + + Ro.sensors = []; + + % Robot frame + ep = [0; 0; 0]; + EP = Ro.con.U; + + %[qp,QP] = propagateUncertainty(ep,EP,@epose2qpose); % frame and cov. in quaternion + + % Define the 4D state and Uncertainty + Ro.state.x = ep; %SLAM State and Covariance + Ro.state.P = EP; + + Ro.state.dr = ep; % Dead Reckoning + Ro.state.gt = ep; %Ground Truth + + Ro.state.P_Added = Ro.state.P; + + + + steps = ceil((Tim.lastFrame - Tim.firstFrame) / Tim.step); + + Ro.state.x_full = [];% zeros(size(Ro.state.x,1), steps); + Ro.state.dr_full = zeros(3,steps); + Ro.state.gt_full = zeros(3,steps); + Ro.state.P_full = []; %zeros(size(Ro.state.x,1),size(Ro.state.x,1), steps); + + + Rob(rob) = Ro; % output robot structure + +end + + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Init/createSensors.m b/Init/createSensors.m new file mode 100644 index 0000000..f022a6e --- /dev/null +++ b/Init/createSensors.m @@ -0,0 +1,123 @@ +function [Sen] = createSensors(Sensor) + +% CREATESENSORS Create sensors structure array. +% Sen = CREATESENSORS(Sensor) Creates the sensors simulator and +% initializes error and type. + +global Opt + + +for sen = 1:numel(Sensor) + + Si = Sensor{sen}; % input sensor structure + + % identification + So.sen = sen; + So.id = Si.id; + So.name = Si.name; + So.type = Si.type; + So.robot = Si.robot; + % transducer parameters + + So.frame.q = e2q(Si.orientationDegrees); + So.frame.t = Si.position; + + So.raw.localCart = []; + So.raw.localPolar = []; + + switch So.type + + + case 'Multibeam' + So.par.maxWidth = Si.maxWidth; + So.par.beamWidth = Si.beamWidth; + So.par.nBeams = Si.nBeams; + So.par.minRange = Si.minRange; + So.par.maxRange = Si.maxRange; + So.par.beamStd = Si.beamStd; + So.par.dataPerScan = Si.dataPerScan; + So.par.outliers = Si.outliers; + case 'Multibeam2D' + So.par.maxWidth = Si.maxWidth; + So.par.beamWidth = Si.beamWidth; + So.par.nBeams = Si.nBeams; + So.par.minRange = Si.minRange; + So.par.maxRange = Si.maxRange; + So.par.beamStd = Si.beamStd; + So.par.dataPerScan = Si.dataPerScan; + So.par.outliers = Si.outliers; + case 'Singlebeam2D' + So.par.maxWidth = Si.maxWidth; + So.par.beamWidth = Si.beamWidth; + So.par.nBeams = Si.nBeams; + So.par.minRange = Si.minRange; + So.par.maxRange = Si.maxRange; + So.par.beamStd = Si.beamStd; + So.par.i = 0; + So.par.dir = 1; + So.par.dataPerScan = Si.dataPerScan; + So.par.outliers = Si.outliers; + + case 'RealData' + So.par.maxWidth = Si.maxWidth; + So.par.beamWidth = Si.beamWidth; + So.par.nBeams = Si.nBeams; + So.par.minRange = Si.minRange; + So.par.maxRange = Si.maxRange; + So.par.beamStd = Si.beamStd; + So.par.i = 0; + So.par.dir = 1; + So.par.dataPerScan = Si.dataPerScan; + So.par.outliers = Si.outliers; + + load(Si.data,'beams'); + So.par.i = 0; + So.data = beams; + clear 'beams'; + + + otherwise + error('??? Unknown sensor type ''%s''.',Si.type) + + end + + + Sen(sen) = So; % output sensor structure + + + +end + + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Init/createSimStructures.m b/Init/createSimStructures.m new file mode 100644 index 0000000..f34dbcb --- /dev/null +++ b/Init/createSimStructures.m @@ -0,0 +1,30 @@ +function [Rob,SimSen,SimW,Tim] = createSimStructures(Robot,Sensor,World,Time) + +% CREATESIMSTRUCTURES Create simulation data structures. + +global Opt + +Tim = createTime(Time); + +% Create robots and controls +Rob = createRobots(Robot,Tim); % Ground Truth + + +if Opt.filter.usefilter + Rob = KalmanFilter_init(Rob); +end + +% Create sensors +SimSen = createSensors(Sensor); % Range Finder + +% Create world +SimW = createWorld(World); + +% Create time variables + + +% Create Map Structs + +Rob.Map = createMap(Rob,SimSen,Tim); + + diff --git a/Init/createSlamStructures.m b/Init/createSlamStructures.m new file mode 100644 index 0000000..7926c9e --- /dev/null +++ b/Init/createSlamStructures.m @@ -0,0 +1,11 @@ +function [Rob,CRob] = createSlamStructures(Robot,Sen,Opt,Time) + +% CREATESLAMSTRUCTURES Initialize SLAM data structures from user data. + +% Create robots and controls +Rob = createRobots(Robot,Opt); +CRob = createRobots(Robot,Opt); + +Rob.Map = createMap(Rob,Sen,Opt,Time); +CRob.Map = createMap(Rob,Sen,Opt,Time); + diff --git a/Init/createTime.m b/Init/createTime.m new file mode 100644 index 0000000..40424d3 --- /dev/null +++ b/Init/createTime.m @@ -0,0 +1,8 @@ +function Tim = createTime(Time) + +% CREATETIME Create Tim structure. + + +Tim = Time; + + diff --git a/Init/createWorld.m b/Init/createWorld.m new file mode 100644 index 0000000..a90c259 --- /dev/null +++ b/Init/createWorld.m @@ -0,0 +1,66 @@ +function SimWorld = createWorld(World) + +% CREATEWORLD Create a world for simulation. +% SIMLMK = CREATEWORLD(World) defines limits and mesh for the world. + + +if isempty(World.points) + World.points = zeros(3,0); +end +if isempty(World.segments) + World.segments = zeros(6,0); +end + +PN = size(World.points,2); % number of points in the simulated world +SN = size(World.segments,2); % number of segments + +SimWorld.points.id = (1:PN); +SimWorld.points.coord = World.points; +SimWorld.segments.id = PN+(1:SN); +SimWorld.segments.coord = World.segments; +SimWorld.surface = World.surface; +SimWorld.lims.xMin = World.xMin; +SimWorld.lims.xMax = World.xMax; +SimWorld.lims.yMin = World.yMin; +SimWorld.lims.yMax = World.yMax; +SimWorld.lims.zMin = World.zMin; +SimWorld.lims.zMax = World.zMax; +SimWorld.dims.l = SimWorld.lims.xMax - SimWorld.lims.xMin; % playground dimensions +SimWorld.dims.w = SimWorld.lims.yMax - SimWorld.lims.yMin; +SimWorld.dims.h = SimWorld.lims.zMax - SimWorld.lims.zMin; +SimWorld.center.xMean = (SimWorld.lims.xMax + SimWorld.lims.xMin)/2; % center fo playground +SimWorld.center.yMean = (SimWorld.lims.yMax + SimWorld.lims.yMin)/2; +SimWorld.center.zMean = (SimWorld.lims.zMax + SimWorld.lims.zMin)/2; + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Init/createWorld.m~ b/Init/createWorld.m~ new file mode 100644 index 0000000..4db710c --- /dev/null +++ b/Init/createWorld.m~ @@ -0,0 +1,36 @@ +function SimWorld = createWorld(World) + +% CREATEWORLD Create a world for simulation. +% SIMLMK = CREATEWORLD(World) defines limits and mesh for the world. + + +if isempty(World.points) + World.points = zeros(3,0); +end +if isempty(World.segments) + World.segments = zeros(6,0); +end + +PN = size(World.points,2); % number of points in the simulated world +SN = size(World.segments,2); % number of segments + +SimWorld.points.id = (1:PN); +SimWorld.points.coord = World.points; +SimWorld.segments.id = PN+(1:SN); +SimWorld.segments.coord = World.segments; +SimWorld.surface = World.surface; +SimWorld.lims.xMin = World.xMin; +SimWorld.lims.xMax = World.xMax; +SimWorld.lims.yMin = World.yMin; +SimWorld.lims.yMax = World.yMax; +SimWorld.lims.zMin = World.zMin; +SimWorld.lims.zMax = World.zMax; +SimWorld.dims.l = SimWorld.lims.xMax - SimWorld.lims.xMin; % playground dimensions +SimWorld.dims.w = SimWorld.lims.yMax - SimWorld.lims.yMin; +SimWorld.dims.h = SimWorld.lims.zMax - SimWorld.lims.zMin; +SimWorld.center.xMean = (SimWorld.lims.xMax + SimWorld.lims.xMin)/2; % center fo playground +SimWorld.center.yMean = (SimWorld.lims.yMax + SimWorld.lims.yMin)/2; +SimWorld.center.zMean = (SimWorld.lims.zMax + SimWorld.lims.zMin)/2; + + + diff --git a/Kinematics/addPoseToMap.m b/Kinematics/addPoseToMap.m new file mode 100644 index 0000000..802a603 --- /dev/null +++ b/Kinematics/addPoseToMap.m @@ -0,0 +1,7 @@ +function Rob = addPoseToMap(Rob,Tim) +%Update the pose vectors + + Rob.state.x_full(:,Tim.currentFrame) = Rob.state.x; + Rob.state.dr_full(:,Tim.currentFrame) = Rob.state.dr; + Rob.state.gt_full(:,Tim.currentFrame) = Rob.state.gt; +end \ No newline at end of file diff --git a/Kinematics/calcOdo2D.m b/Kinematics/calcOdo2D.m new file mode 100644 index 0000000..10c4fee --- /dev/null +++ b/Kinematics/calcOdo2D.m @@ -0,0 +1,15 @@ +function [u] = calcOdo2D(pos1,pos2) +%Calculate the 2D odometry shift from two 4D poses keeping fixed the z-axis +%and using the yaw as orientation. The information returned is a polar +%odometry information and an orientation change. +% u = [v x yaw] + +% Displacement +dif = pos1(1:2) - pos2(1:2); +difa = (atan2(dif(2),dif(1)) ); +dify = normAngle(pos1(3) - pos2(3)); +dif = sqrt( (dif(1))^2 + (dif(2))^2); + +u = [ dif difa dify]; + +end diff --git a/Kinematics/correctPose.m b/Kinematics/correctPose.m new file mode 100644 index 0000000..fe3dc90 --- /dev/null +++ b/Kinematics/correctPose.m @@ -0,0 +1,19 @@ +function [ Rob ] = correctPose( Rob, R, t ) +%CORRECTPOSE Apply the 2D rotation R and translation t to the robot and +%generates the uncertainty + +global currentFrame + +if currentFrame==1 + return +end + +oldor = Rob.state.x_full(3,currentFrame-1); + +% reference in the correct axis. ie. the Reference Scan point of view +reff = r2Rt(oldor,[0 0])*[t(1); t(2); 1]; + +Rob.state.x(1:2) = Rob.state.x(1:2) + reff(1:2); +Rob.state.x(3) = normAngle(Rob.state.x(3) + R); +end + diff --git a/Kinematics/correctPose.m~ b/Kinematics/correctPose.m~ new file mode 100644 index 0000000..43e83c3 --- /dev/null +++ b/Kinematics/correctPose.m~ @@ -0,0 +1,20 @@ +function [ Rob ] = correctPose( Rob, R, t ) +%CORRECTPOSE Apply the 2D rotation R and translation t to the robot and +%generates the uncertainty + +global currentFrame + +if currentFrame==1 + return +end + +oldor = Rob.state.x_full(3,currentFrame-1); + +u = Rob.con.u; +% reference in the correct axis. ie. the Reference Scan point of view +reff = r2Rt(oldor,[0 0])*[t(1); t(2); 1]; + +Rob.state.x(1:2) = Rob.state.x(1:2) + reff(1:2); %Rob.frame.t + [t(1) t(2) 0 ]'; +Rob.state.x(3) = normAngle(Rob.state.x(3) + R); +end + diff --git a/Kinematics/odo2.m b/Kinematics/odo2.m new file mode 100644 index 0000000..6987ced --- /dev/null +++ b/Kinematics/odo2.m @@ -0,0 +1,20 @@ +function [G, incr] = odo2(F,u) + +% ODO2 Odometry evolution: The odometry model is in the form +% u = [v x yaw] +% with v and x being the angular and radial motion information in polar +% coordinates and yaw is the orientation shift. Note that the angular +% odometry and yaw are indipendent even though they are similar. + +dv = u(2); % Linear and Angular odometry model +dx = u(1); +dy = u(3); % Yaw shift + +incr(1) = dx*cos(dv+F(4)); +incr(2) = dx*sin(dv+F(4)); +incr(3) = dy; + +G(1:3) = [F(1) + incr(1); F(2) + incr(2); F(3)]; % Position update +G(4) = normAngle(incr(3)+F(4)); %Orientation update + +G=G'; \ No newline at end of file diff --git a/Kinematics/odo2_cart.m b/Kinematics/odo2_cart.m new file mode 100644 index 0000000..8225a60 --- /dev/null +++ b/Kinematics/odo2_cart.m @@ -0,0 +1,13 @@ +function [G] = odo2_cart(F,u) + +% ODO2_CART Odometry evolution: The odometry model is in the form +% u = [x y yaw] + +dv = u(3); +dx = u(1); +dy = u(2); + +G(1:2) = [F(1) + dx; F(2) + dy]; % Position update +G(3) = normAngle(dv+F(3)); %Orientation update + +G=G'; \ No newline at end of file diff --git a/Kinematics/simMotion.m b/Kinematics/simMotion.m new file mode 100644 index 0000000..1b964c7 --- /dev/null +++ b/Kinematics/simMotion.m @@ -0,0 +1,88 @@ +function Rob = simMotion(Rob, Tim, Noise) +% SIMMOTION Simulated robot motion. +% Rob = SIMMOTION(Rob, Tim) performs one motion step to robot Rob, +% following the motion model in Rob.motion. The time information Tim is +% used only if the motion model requires it, but it has to be provided +% because MOTION is a generic method. + +global MotionUncertainty + +if(Noise) + %errStd retrieve an error from a multivariate Gaussian + %errStd(1) contains the linear error and errStd the angular over the + %odometry shift. errStd(4) contains the orientation error with respect + %to the final orientation of the robot + errd = MotionUncertainty; + for nn = 1:size(errd,2) + errStd(nn) = stdErr(0, errd(nn) ); + end + +else + %No error is added + errStd = zeros(1,6); +end + +% motion model of the robot: +switch Rob.motion + + + case {'trajectory'} + + %Trajectory odometry. The position are taken from the data field + %trajectory and are in a 6D form [x y z e1 e2 e3 ] with q being a + %quaternion form. Conversion is necessary to a 4D state [x y z yaw] + if(Rob.con.incr > 1) + % Extract the position from the ground truth trajectory and + % apply proper conversion to 3D. + oldp = Rob.trajectory(:,Rob.con.incr - Tim.step); + newp = Rob.trajectory(:,Rob.con.incr); + initp = [Rob.state0(1:2); Rob.state0(6)]; + oldp = [oldp(1:2); oldp(6)]; + newp = [newp(1:2); newp(6)]; + + Rob.state.gt = newp; + + % Refer the pose to the initial pose + + + oldp_r = frameRef( oldp, initp, 1); + oldp_r(3)=oldp(3); + newp_r = frameRef( newp ,initp, 1); + newp_r(3)=newp(3); + + + %calculate the displacement caused by the error Rob.state.d and + %update the struct frame containing the latest state + %information + + if strcmp(Rob.error_type,'gaussian') + disp = [errStd(1) errStd(2) errStd(3)] ; + newp_r(1:2) = newp_r(1:2)+[disp(1) disp(2)]' ; + newp_r(3) = normAngle(newp_r(3)+disp(3)) ; + end + + Rob.con.u = newp_r-oldp_r; + + % DEBUG + + Rob.con.incr = Rob.con.incr+Tim.step; + + if(Noise) + Rob.state.d = (Rob.state.gt - newp) ; + else + Rob.state.d = [0; 0; 0]; % no displacement + end + else + %If it's the first iteration, no information on odometry are + %passed nor error displacement + + Rob.con.incr = Rob.con.incr+Tim.step; + Rob.state.d = [0; 0; 0]; + Rob.state.u = [0 0 0]; + Rob.state.gt = Rob.state0([1 2 6]); + end + otherwise + error('??? Unknown motion model ''%s''.',Rob.motion); + +end + diff --git a/Kinematics/simMotion.m~ b/Kinematics/simMotion.m~ new file mode 100644 index 0000000..1aed620 --- /dev/null +++ b/Kinematics/simMotion.m~ @@ -0,0 +1,101 @@ +function Rob = simMotion(Rob, Tim, Noise) +% SIMMOTION Simulated robot motion. +% Rob = SIMMOTION(Rob, Tim) performs one motion step to robot Rob, +% following the motion model in Rob.motion. The time information Tim is +% used only if the motion model requires it, but it has to be provided +% because MOTION is a generic method. + +global MotionUncertainty + +if(Noise) + %errStd retrieve an error from a multivariate Gaussian + %errStd(1) contains the linear error and errStd the angular over the + %odometry shift. errStd(4) contains the orientation error with respect + %to the final orientation of the robot + errd = MotionUncertainty; + for nn = 1:size(errd,2) + errStd(nn) = stdErr(0, errd(nn) ); + end + +else + %No error is added + errStd = zeros(1,6); +end + +% motion model of the robot: +switch Rob.motion + + + case {'trajectory'} + + %Trajectory odometry. The position are taken from the data field + %trajectory and are in a 6D form [x y z e1 e2 e3 ] with q being a + %quaternion form. Conversion is necessary to a 4D state [x y z yaw] + if(Rob.con.incr > 1) + % Extract the position from the ground truth trajectory and + % apply proper conversion to 3D. + oldp = Rob.trajectory(:,Rob.con.incr - Tim.step); + newp = Rob.trajectory(:,Rob.con.incr); + initp = [Rob.state0(1:2); Rob.state0(6)]; + oldp = [oldp(1:2); oldp(6)]; + newp = [newp(1:2); newp(6)]; + + Rob.state.gt = newp; + + % Refer the pose to the initial pose + + + oldp_r = frameRef( oldp, initp, 1); + oldp_r(3)=oldp(3); + newp_r = frameRef( newp ,initp, 1); + newp_r(3)=newp(3); + + %Rob.state.x = newp; + %gt = newp; + + + % calculate a 2D odometry step and add it to the robot position + % including the error got from a gaussian + %shift = calcOdo2D(newp, oldp); + %Rob.state = odo2(Rob.state,shift); + + %REALODO = calcOdo2D(newp, oldp)' + + + + %calculate the displacement caused by the error Rob.state.d and + %update the struct frame containing the latest state + %information + + if strcmp(Rob.error_type,'gaussian') + disp = [errStd(1) errStd(2) errStd(3)] ; + newp_r(1:2) = newp_r(1:2)+[disp(1) disp(2)]' ; + newp_r(3) = normAngle(newp_r(3)+disp(3)) ; + end + + Rob.con.u = newp_r-oldp_r; %calcOdo2D(newp,oldp ) ; + + % DEBUG + + Rob.con.incr = Rob.con.incr+Tim.step; + + %only fill Rob.state.d is error was chosen to be added + if(Noise) + Rob.state.d = (Rob.state.gt - newp) ; + else + Rob.state.d = [0; 0; 0]; % no displacement + end + else + %If it's the first iteration, no information on odometry are + %passed nor error displacement + + Rob.con.incr = Rob.con.incr+Tim.step; + Rob.state.d = [0; 0; 0]; + Rob.state.u = [0 0 0]; + Rob.state.gt = Rob.state0([1 2 6]); + end + otherwise + error('??? Unknown motion model ''%s''.',Rob.motion); + +end + diff --git a/Map/fillOccupancyGrid.m b/Map/fillOccupancyGrid.m new file mode 100644 index 0000000..8f860ef --- /dev/null +++ b/Map/fillOccupancyGrid.m @@ -0,0 +1,158 @@ +function [ Occ ] = fillOccupancyGrid( A, Occ, motion ) +%FILLOCCUPANCYGRID update an occupancy OCC with a scan A using different +%types of models. For probabilistic sonar, A must be in polar coordinates +% locally referenced from the robot position. The shift will be calculated +% using the motion. + + + +L = Occ.PARAM.L; +xmin = Occ.PARAM.xmin; +xmax = Occ.PARAM.xmax; +ymin = Occ.PARAM.ymin; +ymax = Occ.PARAM.ymax; + +sscan = size(A,1); +xwide = abs(xmax - xmin); +ywide = abs(ymax - ymin); + +xrob = motion(1); +yrob = motion(2); +throb = motion(3); + + +% We need these coordinates to reference the cell in global coordinates +[rx ry] = getCoordinates( [xrob yrob] , Occ.PARAM); + +NX = floor(xwide / L)+1; +NY = floor(ywide / L)+1; + +%%Occ.grid = zeros(NX,NY); + +for i = 1:sscan + x = A(i,1); + y = A(i,2); + + switch(Occ.PARAM.type) + + % In the binary model, only the cell believed to be hit is filled + case 'binary' + + %Calculate the cell where the point (x,y) should fall in. + [ix1 iy1] = getCoordinates( [x y], Occ.PARAM); + + % global reference the scan shifting of a number of + % cells equal to the robot's position cell + Occ.grid(ix1,iy1) = 1; + case 'binary sonar' + + + case 'prob sonar' + + rangeb = A(i,2); + angleb = A(i,1) + throb; % global reference the scan + bwdth = Occ.PARAM.bwidth; + % calculate the length of the chord by the size of the cell as + % approximate number of cells to check + + rwidth = floor( rangeb / L ); + for j = 1:rwidth + + incrr = (rangeb / rwidth) * j; + if j == rwidth + incrr = rangeb; + end + nrangeb = incrr; + lwidth = floor( ( 2*nrangeb*cos(bwdth/2) ) / L ) +1; + for k = -lwidth:1:lwidth + + % calculate an angular shift and its relative occupancy + incra = (bwdth/2)/lwidth * k; + nangleb = angleb + incra; + [xp yp] = pol2cart(nangleb ,nrangeb); + + % global reference the scan shifting of a number of + % cells equal to the robot's position cell + + ix1 = min(floor( ( abs(xmin) + xp + xrob) / L)+1,NX); + iy1 = min(floor( ( abs(ymin) + yp + yrob) / L)+1,NY); + pocc = ( ( (Occ.PARAM.maxr - nrangeb ) / Occ.PARAM.maxr ) + ... + ( (bwdth - abs(incra) ) / bwdth ) ) / 2 + + pemp = 1 - pocc; + + % this area is behind the occupied chord so the + % probability is inverse + if j < rwidth + pocc = 1 - pocc; + pemp = pocc; + end + + % The cell was not visited yet + if Occ.grid(ix1,iy1) == -2 + Occ.grid(ix1,iy1) = pocc ; + else + % Update the cell using the bayes rule + Occ.grid(ix1,iy1) = (pocc * Occ.grid(ix1,iy1)) / ... + (pocc * Occ.grid(ix1,iy1) + pemp * ( 1 - Occ.grid(ix1,iy1) ) ) ; + end + + end + end + + case 'logodds sonar' + rangeb = A(i,2); + angleb = A(i,1) + throb; % global reference the scan + bwdth = Occ.PARAM.bwidth; + % calculate the length of the chord by the size of the cell as + % approximate number of cells to check + + rwidth = floor( rangeb / L ); + for j = 1:rwidth + + incrr = (rangeb / rwidth) * j; + if j == rwidth + incrr = rangeb; + end + nrangeb = incrr; + lwidth = floor( ( 2*nrangeb*cos(bwdth/2) ) / L ) +1; + for k = -lwidth:1:lwidth + + % calculate an angular shift and its relative occupancy + incra = (bwdth/2)/lwidth * k; + nangleb = angleb + incra; + [xp yp] = pol2cart(nangleb ,nrangeb); + + % global reference the scan shifting of a number of + % cells equal to the robot's position cell + + ix1 = min(floor( ( abs(xmin) + xp + xrob) / L)+1,NX); + iy1 = min(floor( ( abs(ymin) + yp + yrob) / L)+1,NY); + pocc = ( ( (Occ.PARAM.maxr - nrangeb ) / Occ.PARAM.maxr ) + ... + ( (bwdth - abs(incra) ) / bwdth ) ) / 2; + + + pemp = 1 - pocc; + + % this area is behind the occupied chord so the + % probability is inverse + if j < rwidth + pocc = 1 - pocc; + pemp = pocc; + end + + % The cell was not visited yet + if Occ.grid(ix1,iy1) == -2 + Occ.grid(ix1,iy1) = pocc ; + else + % Update the cell using the bayes rule + Occ.grid(ix1,iy1) = (pocc * Occ.grid(ix1,iy1)) / ... + (pocc * Occ.grid(ix1,iy1) + pemp * ( 1 - Occ.grid(ix1,iy1) ) ) ; + end + + end + end + end + +end + diff --git a/Map/fillOccupancyGrid.m~ b/Map/fillOccupancyGrid.m~ new file mode 100644 index 0000000..0fe4cb0 --- /dev/null +++ b/Map/fillOccupancyGrid.m~ @@ -0,0 +1,156 @@ +function [ Occ ] = fillOccupancyGrid( A, Occ, motion ) +%FILLOCCUPANCYGRID update an occupancy OCC with a scan A using different +%types of models. For probabilistic sonar, A must be in polar coordinates +% locally referenced from the robot position. The shift will be calculated +% using the motion. + + + +L = Occ.PARAM.L; +xmin = Occ.PARAM.xmin; +xmax = Occ.PARAM.xmax; +ymin = Occ.PARAM.ymin; +ymax = Occ.PARAM.ymax; + +sscan = size(A,1); +xwide = abs(xmax - xmin); +ywide = abs(ymax - ymin); + +xrob = motion(1); +yrob = motion(2); +throb = motion(3); + +% We need these coordinates to reference the cell in global coordinates +[rx ry] = getCoordinates( [xrob yrob] , Occ); + +NX = floor(xwide / L)+1; +NY = floor(ywide / L)+1; + +%%Occ.grid = zeros(NX,NY); + +for i = 1:sscan + x = A(i,1); + y = A(i,2); + + switch(Occ.PARAM.type) + + % In the binary model, only the cell believed to be hit is filled + case 'binary' + + %Calculate the cell where the point (x,y) should fall in. + [ix1 iy1] = getCoordinates( [x y], A); + + % global reference the scan shifting of a number of + % cells equal to the robot's position cell + Occ.grid(ix1 + rx,iy1 + ry) = 1; + case 'binary sonar' + + + case 'prob sonar' + + rangeb = A(i,2); + angleb = A(i,1) + throb; % global reference the scan + bwdth = Occ.PARAM.bwidth; + % calculate the length of the chord by the size of the cell as + % approximate number of cells to check + + rwidth = floor( rangeb / L ); + for j = 1:rwidth + + incrr = (rangeb / rwidth) * j; + if j == rwidth + incrr = rangeb; + end + nrangeb = incrr; + lwidth = floor( ( 2*nrangeb*cos(bwdth/2) ) / L ) +1; + for k = -lwidth:1:lwidth + + % calculate an angular shift and its relative occupancy + incra = (bwdth/2)/lwidth * k; + nangleb = angleb + incra; + [xp yp] = pol2cart(nangleb ,nrangeb); + + % global reference the scan shifting of a number of + % cells equal to the robot's position cell + + ix1 = min(floor( ( abs(xmin) + xp + xrob) / L)+1,NX); + iy1 = min(floor( ( abs(ymin) + yp + yrob) / L)+1,NY); + pocc = ( ( (Occ.PARAM.maxr - nrangeb ) / Occ.PARAM.maxr ) + ... + ( (bwdth - abs(incra) ) / bwdth ) ) / 2; + + pemp = 1 - pocc; + + % this area is behind the occupied chord so the + % probability is inverse + if j < rwidth + pocc = 1 - pocc; + pemp = pocc; + end + + % The cell was not visited yet + if Occ.grid(ix1,iy1) == -2 + Occ.grid(ix1,iy1) = pocc ; + else + % Update the cell using the bayes rule + Occ.grid(ix1,iy1) = (pocc * Occ.grid(ix1,iy1)) / ... + (pocc * Occ.grid(ix1,iy1) + pemp * ( 1 - Occ.grid(ix1,iy1) ) ) ; + end + + end + end + + case 'logodds sonar' + rangeb = A(i,2); + angleb = A(i,1) + throb; % global reference the scan + bwdth = Occ.PARAM.bwidth; + % calculate the length of the chord by the size of the cell as + % approximate number of cells to check + + rwidth = floor( rangeb / L ); + for j = 1:rwidth + + incrr = (rangeb / rwidth) * j; + if j == rwidth + incrr = rangeb; + end + nrangeb = incrr; + lwidth = floor( ( 2*nrangeb*cos(bwdth/2) ) / L ) +1; + for k = -lwidth:1:lwidth + + % calculate an angular shift and its relative occupancy + incra = (bwdth/2)/lwidth * k; + nangleb = angleb + incra; + [xp yp] = pol2cart(nangleb ,nrangeb); + + % global reference the scan shifting of a number of + % cells equal to the robot's position cell + + ix1 = min(floor( ( abs(xmin) + xp + xrob) / L)+1,NX); + iy1 = min(floor( ( abs(ymin) + yp + yrob) / L)+1,NY); + pocc = ( ( (Occ.PARAM.maxr - nrangeb ) / Occ.PARAM.maxr ) + ... + ( (bwdth - abs(incra) ) / bwdth ) ) / 2; + + pemp = 1 - pocc; + + % this area is behind the occupied chord so the + % probability is inverse + if j < rwidth + pocc = 1 - pocc; + pemp = pocc; + end + + % The cell was not visited yet + if Occ.grid(ix1,iy1) == -2 + Occ.grid(ix1,iy1) = pocc ; + else + % Update the cell using the bayes rule + Occ.grid(ix1,iy1) = (pocc * Occ.grid(ix1,iy1)) / ... + (pocc * Occ.grid(ix1,iy1) + pemp * ( 1 - Occ.grid(ix1,iy1) ) ) ; + end + + end + end + end + +end + diff --git a/Map/getCoordinates.m~ b/Map/getCoordinates.m~ new file mode 100644 index 0000000..bd78f5f --- /dev/null +++ b/Map/getCoordinates.m~ @@ -0,0 +1,26 @@ +function [ ix iy ] = getCoordinates( p, A ) +%GETCOORDINATES given a point P on the grid map A, it returns its grid +%position. It uses a default values of definition of 0.1 and size +%0.1*size(A) + + +L = A.L; +xmin = A.xmin; +xmax = A.xmax; +ymin = A.ymin; +ymax = A.ymax; + +xwide = xmax - xmin; +ywide = ymax - ymin; +NX = floor(xwide / L)+1; +NY = floor(ywide / L)+1; + +x = p(1); +y = p(2); + +ix = min(floor( ( abs(xmin) + x) / L)+1,NX); %+ (abs(xmin) / L); +iy = min(floor( ( abs(ymin) + y) / L)+1,NY); %+ (abs(ymin) / L); + + +end + diff --git a/Map/makeLocalOccupancyGrid.m b/Map/makeLocalOccupancyGrid.m new file mode 100644 index 0000000..76c4f30 --- /dev/null +++ b/Map/makeLocalOccupancyGrid.m @@ -0,0 +1,145 @@ +function [ SubGrids ] = makeLocalOccupancyGrid( Scan, PARAM ) +%MAKELOCALOCCUPANCYGRID returns an occupancy grid out of a range finder +%scan. The grid size is dynamic and depends on the cartesian area covered +%by the points in Scan. We suppose the points to be ordered by angle. +%Adjacent points are neighbours. The grid has a probabilistic fashion, each +%cell is filled with its occupancy probability modeled by a gaussian +%derivied by the number of n points which fall inside it. + +% SUBGRIDS is a set of 4 occupancy grid maps defined using a small shift +% over the original center of the grid. + +L = PARAM.L; +xmin = PARAM.xmin; +xmax = PARAM.xmax; +ymin = PARAM.ymin; +ymax = PARAM.ymax; + +sscan = size(Scan,1); + + +xwide = (xmax - xmin); +ywide = (ymax - ymin); + + +NX = ceil(xwide / L); +NY = ceil(ywide / L); + +% we create the occupancy grid structures, each cell is defined by a mean +% and covariance pdf +ogrid.mean = 0; +ogrid.cov = zeros(2,2); + +ogrid1.grid(1:NX, 1:NY) = ogrid; +ogrid2.grid(1:NX, 1:NY) = ogrid; +ogrid3.grid(1:NX, 1:NY) = ogrid; +ogrid4.grid(1:NX, 1:NY) = ogrid; +ogrid_tot.grid(1:NX, 1:NY) = ogrid; + +%Initialize the cells to group the points togheter +pgrid0.points = []; + +pgrid1.grid(1:NX, 1:NY) = pgrid0; +pgrid2.grid(1:NX, 1:NY) = pgrid0; +pgrid3.grid(1:NX, 1:NY) = pgrid0; +pgrid4.grid(1:NX, 1:NY) = pgrid0; + +%Group scans according to the cell they fall in, Calculates 4 grids. The +%second grid is shifted horizontally of L/2, the third is shifted +%vertically of the same amount, and the last one is shifted both vertically +%and horizontally of L/2 as well. + +for i = 1:sscan + x = Scan(i,1); + y = Scan(i,2); + + + %Calculate the cell where the point (x,y) should fall in. + + ix1 = min(floor( (-(xmin) + x) / L)+1,NX); + iy1 = min(floor( (-(ymin) + y) / L)+1,NY); + + ix2 = min(floor( (-(xmin) + x + L/2) / L)+1,NX); + iy2 = iy1; + + ix3 = ix1; + iy3 = min(floor( (-(ymin) + y + L/2) / L)+1,NY); + + ix4 = ix2; + iy4 = iy3; + + % and add it to the respective map + rest = pgrid1.grid(ix1,iy1).points; + pgrid1.grid(ix1,iy1).points = [rest; x y]; + + rest = pgrid2.grid(ix2,iy2).points; + pgrid2.grid(ix2,iy2).points = [rest; x y]; + + rest = pgrid3.grid(ix3,iy3).points; + pgrid3.grid(ix3,iy3).points = [rest; x y]; + + rest = pgrid4.grid(ix4,iy4).points; + pgrid4.grid(ix4,iy4).points = [rest; x y]; +end + +grids = [pgrid1 pgrid2 pgrid3 pgrid4]; +ogrids = [ogrid1 ogrid2 ogrid3 ogrid4]; + + +% for each cell, calculate its mean and covariance using its containing +% points. If the number of points is smaller than 3 the cell results +% unoccupied. +hh = 0; +tots = 0; +for i = 1:NX + + for j = 1:NY + + %For every cell calculate the four occupancy maps + for kk = 1:size(grids,2) + pgrid = grids(kk).grid; + + np = size(pgrid(i,j).points,1); + mpoints = 0; + covpoints = zeros(2,2); + tots = tots +np; + + if np >= 3 + + hh = hh+1; + %Calculate mean and covariance of all the points in the cell + mpoints = mean(pgrid(i,j).points); + covpoints = cov(pgrid(i,j).points); + %covpoints = zeros(2,2); +% for k = 1:np +% rest = pgrid(i,j).points(k,:) - mpoints; +% rest = rest'*rest; +% covpoints = covpoints + rest; +% end +% covpoints = covpoints/(np-1); + +% mpoints +% covpoints +% pgrid(i,j).points +% j +% i + end + + %update the points in each map... + mpoints; + covpoints; + ogrids(kk).grid(i,j).mean = mpoints; + ogrids(kk).grid(i,j).cov = covpoints; + end + + %and to the total map which is the sum of the previous four +% ogrid_tot.grid(i,j).mean = sum(ogrids(1:4).mean); +% ogrid_tot.grid(i,j).cov = sum(ogrids(1:4).cov); + end + + +end + +%displayMultOccGrid(ogrids); +SubGrids = [ogrids(1) ogrids(2) ogrids(3) ogrids(4)]; +end diff --git a/Map/makeOccupancyGrid.m b/Map/makeOccupancyGrid.m new file mode 100644 index 0000000..6a4c508 --- /dev/null +++ b/Map/makeOccupancyGrid.m @@ -0,0 +1,39 @@ +function [ O ] = makeOccupancyGrid( A, PARAM ) +%OCCUPANCYGRID Initialize an occupancy grid map using a (might be empty) clouds +%of points A and a resolution res. The size is enough to cover all the +%points plus a margin of size 1. The grid expans from xmin to xmax and ymin +%ymax. Values can be negatives and are relatives to robot position. the +%map is represented as a grid of xwide cells from 0 to xwide and y +%calculated as the same. Then, the respective cell where a point P falls is +%calculated taking into account the area represented by the map stored into +%the parameters PARAM. The robot is believed to be in the middle cell. +%Usually xmin and ymin are negative and equal -xmax and -ymax repsectively + +% Depending on the type of the map we create in different ways. For +% probabilistic mode, the scan A needs to be presented in Polar +% Coordinates. + +L = PARAM.L; +xmin = PARAM.xmin; +xmax = PARAM.xmax; +ymin = PARAM.ymin; +ymax = PARAM.ymax; + + +xwide = abs(xmax - xmin); +ywide = abs(ymax - ymin); + + +NX = floor(xwide / L)+1; +NY = floor(ywide / L)+1; + +O.grid = ones(NX,NY)*0; +O.PARAM = PARAM; + +O = fillOccupancyGrid(A,O, [0 0 0]); + +end + + + + diff --git a/Map/makeOccupancyGrid.m~ b/Map/makeOccupancyGrid.m~ new file mode 100644 index 0000000..a07486a --- /dev/null +++ b/Map/makeOccupancyGrid.m~ @@ -0,0 +1,91 @@ +function [ O ] = makeOccupancyGrid( A, PARAM ) +%OCCUPANCYGRID Initialize an occupancy grid map using a (might be empty) clouds +%of points A and a resolution res. The size is enough to cover all the +%points plus a margin of size 1. The grid expans from xmin to xmax and ymin +%ymax. Values can be negatives and are relatives to robot position. the +%map is represented as a grid of xwide cells from 0 to xwide and y +%calculated as the same. Then, the respective cell where a point P falls is +%calculated taking into account the area represented by the map stored into +%the parameters PARAM. The robot is believed to be in the middle cell. +%Usually xmin and ymin are negative and equal -xmax and -ymax repsectively + +% Depending on the type of the map we create in different ways. For +% probabilistic mode, the scan A needs to be presented in Polar +% Coordinates. + +L =PARAM.L; +xmin = PARAM.xmin; +xmax = PARAM.xmax; +ymin = PARAM.ymin; +ymax = PARAM.ymax; + + +sscan = size(A,1); + + +xwide = abs(xmax - xmin); +ywide = abs(ymax - ymin); + + +NX = floor(xwide / L)+1; +NY = floor(ywide / L)+1; + +O.grid = ones(NX,NY)*-1; +O.PARAM = PARAM; +for i = 1:sscan + x = A(i,1); + y = A(i,2); + + switch(O.type) + + % In the binary model, only the cell believed to be hit is filled + case 'binary' + %Calculate the cell where the point (x,y) should fall in. + ix1 = min(floor( (abs(xmin) + x) / L)+1,NX); + iy1 = min(floor( (abs(ymin) + y) / L)+1,NY); + O.grid(ix1,iy1) = 1; + case 'probabilistic sonar' + rangeb = A(i,2); + angleb = A(i,1); + + % calculate the length of the chord by the size of the cell as + % approximate number of cells to check + lwidth = floor( ( 2*rangeb*cos(angleb/2) ) / (L*2) ); + rwidth = floor( rangeb / L ); + for j = 1:rwidth + + incrr = (rangeb / rwidth) * j; + if j == rwidth + incrr = rangeb; + end + nrangeb = rangeb - incrr; + + for k = -lwidth:1:lwidth + + % calculate an angular shift and its relative occupancy + incra = (PARAM.bwidth/2)/lwidth * k; + nangleb = angleb + incra; + [ xp yp] = pol2cart(nangleb ,nrangeb); + [ix1 iy1] = getCoordinates( [xp yp], A); + pocc = ( ( (PARAM.maxr - nrangeb ) / PARAM.maxr ) + ... + ( (PARAM.bwidth - nangleb ) / PARAM.bwidth ) ) / 2; + + + if j == rwidth + O.grid(ix1,iy1) = pocc; + else + % this area is behind the occupied chord so the probability is inverse + O.grid(ix1,iy1) = 1 - pocc; + end + + end + end + end + +end + +end + + + + diff --git a/Math/.DS_Store b/Math/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..0fa43d65746db70afaf65d0781b799e5d8a8a287 GIT binary patch literal 6148 zcmeHKOHRWu5S@WiRboMxEOP|7K~x1+ERf&;khVdQ(gYD8+nkKE@#ce?h-|t+2xcVb z+4Hmgq*cl4wap3p7EN&V-11(sbm`b0BMuE#1=#JyK2WM55oQlC_`FwXS+c z!}S-`K)a)M)2^$$X)0O%3EfbM?QQpJ?0l#$y7D5gnjBgoRi#|l@4^sdkf7+=u4?Kh zXg|i5s%@V=hZD~kJmz$ZHOcg^`{|(%2?m0JU?3O>1`c7sDiSSD4+#q2U?3Ry7YxYx zke~^sj=iBC9r)-I062qM1%18t0uxaHQ^(#A8VFk`&_dat80@!Zws6=}_wNlYoLFZ@ z9-sN^@xp%T=0x3zQ^WWM1Hr(UfnyskrT$;xSDJj}k3(V<3!zVNV@= pp1(1) && p1(1) <= pp1(2) &&... + p1(2) >= pp2(1) && p1(2) <= pp2(2); + +end + diff --git a/Math/Lines/inSegment.m~ b/Math/Lines/inSegment.m~ new file mode 100644 index 0000000..24ad2b4 --- /dev/null +++ b/Math/Lines/inSegment.m~ @@ -0,0 +1,11 @@ +function [ out ] = inSegment( p1, p2, p3 ) +%INSEGMENT returns 1 if the point P1 is part the segment P2, P3 + +pp1 = min(p2(1),p3(1)); +pp1 = min(p2(1),p3(1)); + +out = p1(1) >= p2(1) && p1(1) <= p3(1) &&... + p1(2) >= p2(2) && p1(2) <= p3(2) + +end + diff --git a/Math/Lines/intersectPoint2DWall.m b/Math/Lines/intersectPoint2DWall.m new file mode 100644 index 0000000..bb144d0 --- /dev/null +++ b/Math/Lines/intersectPoint2DWall.m @@ -0,0 +1,88 @@ +% Calculate intersection between a single beam and the walls defined by the +% world. The beam is selected using an index nbeam. Every sonar has +% S.maxWidth coverage, divided into S.nBeams, S.beamWidth wide. + +function [simulatedScan, realPoints] = intersectPoint2DWall(TrajScan,nbeam,Xmax,Ymax,S,noise) + + + +sonarRange = S.maxRange; +sonarWidth = S.maxWidth; +sonarAngle = 1; + +simulatedScan=[-1 -1]; +realPoints=[0 0]; +% for i = 1:nBeams %360 is even number WARNING: this works for 360DEG! +% + x=TrajScan(1); + y=TrajScan(2); + yaw=TrajScan(3); +% +% %calculate intersection between y = mx + q (beam line) with the 4 wall +% %lines + angle = -sonarWidth / 2; + angle = deg2rad(angle + nbeam*sonarAngle)+yaw; %yaw is in rad CHECK IF - or + + m = tan(angle); + q = y-m*x; + + intersection=zeros(4,2); + noise = stdErr(0,noise); + + + intersection(1,1)=0; intersection(1,2)=q; % x=0 + intersection(2,1)=(Ymax-q)/m; intersection(2,2)=Ymax; % x=Ymax + intersection(3,1)=Xmax; intersection(3,2)=m*Xmax+q; % y=Xmax + intersection(4,1)=-q/m; intersection(4,2)=0; % y=0 + intersection = addRow(intersection,[noise noise]); + + for j=1:4 + %intersection is outside the pool/room/environment + if (intersection(j,1)<0 || intersection(j,1)>Xmax || intersection(j,2)<0 || intersection(j,2)>Ymax) + continue; + end + + distance=sqrt((intersection(j,1)-x)^2+(intersection(j,2)-y)^2); + xSign=cos(angle); + ySign=sin(angle); + + %intersection on direction given by angle + if( (intersection(j,1)-x)*xSign>=0 && (intersection(j,2)-y)*ySign>=0) + if(distance>sonarRange) + simulatedScan=[-1 -1]; + else + simulatedScan = [ angle-yaw distance ]; + realPoints = intersection(j,:); + end + end + end +% end +% indexDelete = (simulatedScan == 0); +% realPoints([indexDelete;indexDelete])=[]; +% realPoints = reshape(realPoints,2,[]); +% +% [r,c,v] = find(simulatedScan); +% simulatedScan = [sonarAngle*c;v]; + +% if DEBUG.plotscans +% figure(nu_scan) +% subplot(2,3,1) +% pp=zeros(2,size(simulatedScan,2)); +% for i=1:size(simulatedScan,2) +% pp(1,i)=simulatedScan(2,i)*cos(deg2rad(simulatedScan(1,i))); +% pp(2,i)=simulatedScan(2,i)*sin(deg2rad(simulatedScan(1,i))); +% end +% +% subplot(2,3,1) +% axis equal +% plot(pp(1,:),pp(2,:),'r.','DisplayName','simulatedScan'); +% title(['Simulated Scan ' num2str(nu_scan)]) +% +% subplot(2,3,4) +% axis equal +% plot(realPoints(1,:),realPoints(2,:),'r*','DisplayName','realPoints'); +% hold all +% plot(TrajScan(1),TrajScan(2),'b.','DisplayName','trajectory'); +% title(['Real Map ' num2str(nu_scan)]) +% end + + diff --git a/Math/Lines/line2lineSIDistance.m b/Math/Lines/line2lineSIDistance.m new file mode 100644 index 0000000..13a1def --- /dev/null +++ b/Math/Lines/line2lineSIDistance.m @@ -0,0 +1,22 @@ +function [ d P] = line2lineSIDistance(p1, m1, q1, m, q ) +%PTS2LINESIDISTANCE Returns the distance from P1 to the line defined by M +%and Q +% m1 = -m; +% q1 = p1(2) - m1*p1(2); + +A=[1 -m;1 -m1]; + +P = (A\[q;q1])'; + +if isnan(P(1)) || isinf(P(1)) + d = inf; + P= []; + return +end + +dff = P - p1; +d = sqrt(dff(1)^2 +dff(2)^2); +%if inSegment(ip,p2,p3) + +end + diff --git a/Math/Lines/or2rot.m b/Math/Lines/or2rot.m new file mode 100644 index 0000000..e69de29 diff --git a/Math/Lines/pt2lineDistance.asv b/Math/Lines/pt2lineDistance.asv new file mode 100644 index 0000000..b5b1b31 --- /dev/null +++ b/Math/Lines/pt2lineDistance.asv @@ -0,0 +1,56 @@ +function [ d ] = pt2lineDistance( p1, p2,p3 ) +%PT2LINEDISTANCE return the distance between 1 point and a line give in +%slope intercept form. + + +sl = pts2line(p2,p3); +q = sl * p2(1) + p2(2); +[a l] = cart2pol(p1(1),p2(2)); + +A = (p1(1)-p2(1))*(p3(1)-p2(1)) + (p1(2)-p2(2))*(p3(2)-p2(2)); +B = (p3(1)-p2(1))*(p3(1)-p2(1)) + (p3(2)-p3(2))*(p3(2)-p2(2)) ; +R = A/B; + +PX = p2(1) + R*(p3(1) - p2(1)); +PY = p2(2) + R*(p3(2) - p2(2)); + +S = (p2(2)-p1(2))*(p3(1)-p2(1))-(p2(1)-p1(1))*(p3(2)-p2(2)) / B; +dL = abs(S)*sqrt(B); + + + if ( (R >= 0) && (R <= 1) ) + + d = dL; + + else + + + dist1 = (p1(1)-p2(1))*(p1(1)-p2(1)) + (p1(2)-p2(2))*(p1(2)-p2(2)); + dist2 = (p1(1)-p3(1))*(p1(1)-p3(1)) + (p1(2)-p3(2))*(p1(2)-p3(2)); + + if (dist1 < dist2) + P = p2; + distanceSegment = sqrt(dist1); + else + + P = p3; + distanceSegment = sqrt(dist2); + end + + + end + + + + + +d = abs(det([p3-p2;p1-p2]))/norm(p3-p2); % for row vectors. +proj = [cos(a)*(d+l) sin(a)*(d+l)]; + + if proj(1) < p2(1) && proj(2) < p2(2) + + proj(2) < p2(2) && proj(2) < p3(2) + + end + +end diff --git a/Math/Lines/pt2lineDistance.m b/Math/Lines/pt2lineDistance.m new file mode 100644 index 0000000..a7da6f8 --- /dev/null +++ b/Math/Lines/pt2lineDistance.m @@ -0,0 +1,79 @@ +function [ d P ] = pt2lineDistance( p1, p2, p3 ) +%PT2LINEDISTANCE return the distance between 1 point and a line give in +%slope intercept form. + +% [m q] = pts2lineSI(p2,p3); +% m1 = -1; +% q1 = p1(2) - m1*p1(2); +% +% A=[1 -m;1 -m1]; +% +% ip = (A\[q;q1])'; +% +% if isnan(ip(1)) || isinf(ip(1)) +% d = inf; +% P= []; +% return +% end +% +% dff = ip - p2; +% d1 = sqrt(dff(1)^2 +dff(2)^2); +% +% dff = ip - p3; +% d2 = sqrt(dff(1)^2 +dff(2)^2); +% +% [mm im] = min([d1,d2]); +% +% dff = ip - p1; +% d3 = sqrt(dff(1)^2 +dff(2)^2); +% +% d = d3; +% +% +% if im == 1 +% P = p2; +% dff = p1 - p2; +% d = sqrt(dff(1)^2 +dff(2)^2); +% else +% P = p3; +% dff = p1 - p3; +% d = sqrt(dff(1)^2 +dff(2)^2); +% end +% +% if inSegment(ip,p2,p3) +% d = d3; +% end +% + + + +xu = p1(1) - p2(1); +yu = p1(2) - p2(2); +xv = p3(1) - p2(1); +yv = p3(2) - p2(2); + +d1 = sqrt( (xu)^2 + (yu)^2); +if (xu * xv + yu * yv < 0) +d=d1; +P=p2; +return +end + +xu = p1(1) - p3(1); +yu = p1(2) - p3(2); +xv = -xv; +yv = -yv; + +d2=sqrt( (xu)^2 + (yu)^2 ); +if (xu * xv + yu * yv < 0) +d=d2; +P=p3; +return +end + +pts = [p2; p3]; ds=[d1 d2]; + +d = abs( ( p1(1) * ( p2(2) - p3(2) ) + p1(2) * ( p3(1) - p2(1) ) + ( p2(1) * p3(2) - p2(1) * p2(2) ) ) / sqrt( ( p3(1) - p2(1) )^2 + ( p3(2) - p2(2) )^2 ) ); +[mm ii] = min(ds); +P=pts(ii,:); +end diff --git a/Math/Lines/pt2lineDistance.m~ b/Math/Lines/pt2lineDistance.m~ new file mode 100644 index 0000000..fa790c0 --- /dev/null +++ b/Math/Lines/pt2lineDistance.m~ @@ -0,0 +1,70 @@ +function [ d P ] = pt2lineDistance( p1, p2, p3 ) +%PT2LINEDISTANCE return the distance between 1 point and a line give in +%slope intercept form. + +% [m q] = pts2lineSI(p2,p3); +% m1 = -1; +% q1 = p1(2) - m1*p1(2); +% +% A=[1 -m;1 -m1]; +% +% ip = (A\[q;q1])'; +% +% if isnan(ip(1)) || isinf(ip(1)) +% d = inf; +% P= []; +% return +% end +% +% dff = ip - p2; +% d1 = sqrt(dff(1)^2 +dff(2)^2); +% +% dff = ip - p3; +% d2 = sqrt(dff(1)^2 +dff(2)^2); +% +% [mm im] = min([d1,d2]); +% +% dff = ip - p1; +% d3 = sqrt(dff(1)^2 +dff(2)^2); +% +% d = d3; +% +% +% if im == 1 +% P = p2; +% dff = p1 - p2; +% d = sqrt(dff(1)^2 +dff(2)^2); +% else +% P = p3; +% dff = p1 - p3; +% d = sqrt(dff(1)^2 +dff(2)^2); +% end +% +% if inSegment(ip,p2,p3) +% d = d3; +% end +% + + +xu = p1(1) - p2(1); +yu = p1(2) - p2(2); +xv = p3(1) - p2(1); +yv = p3(2) - p2(2); + +if (xu * xv + yu * yv < 0) +d = sqrt( (xu)^2 + (yu)^2); +return +end + +xu = p1(1) - p3(1); +yu = p1(2) - p3(2); +xv = -xv; +yv = -yv; +if (xu * xv + yu * yv < 0) +d=sqrt( (xu)^2 + (yu)^2 ); +return +end + +d = Abs( ( Xp * ( Ya - Yb ) + Yp * ( Xb - Xa ) + ( Xa * Yb - Xb * Ya ) ) / Sqrt( ( Xb - Xa )^2 + ( Yb - Ya )^2 ) ); + +end diff --git a/Math/Lines/pts2line.asv b/Math/Lines/pts2line.asv new file mode 100644 index 0000000..22928f3 --- /dev/null +++ b/Math/Lines/pts2line.asv @@ -0,0 +1,11 @@ +function [ m q ] = pts2line( p2,p3 ) +%PTS2LINE Summary of this function goes here +% Find the line passing between two points. Return in slope intercept +% form. +dx = p3(1) - p2(1); +dy = p3(2) - p2(2); +m = dy / dx ; +%d = sqrt(dx^2 + dy^2); +q = m*p3(1) - p3(2); + +end diff --git a/Math/Lines/pts2line.m b/Math/Lines/pts2line.m new file mode 100644 index 0000000..22fdfeb --- /dev/null +++ b/Math/Lines/pts2line.m @@ -0,0 +1,11 @@ +function [ m d ] = pts2line( p2,p3 ) +%PTS2LINE Summary of this function goes here +% Find the line passing between two points. Return in slope point +% form. +dx = p3(1) - p2(1); +dy = p3(2) - p2(2); +m = dy / dx ; +%d = sqrt(dx^2 + dy^2); +d = -m*p3(1) + p3(2); + +end diff --git a/Math/Lines/pts2lineSI.m b/Math/Lines/pts2lineSI.m new file mode 100644 index 0000000..46b936d --- /dev/null +++ b/Math/Lines/pts2lineSI.m @@ -0,0 +1,10 @@ +function [ s in ] = pts2lineSI( p1, p2 ) +%PTS2LINESI Returns the slope intercept of the line passing between the two +%points P1 and P2 + +d = p1 - p2; +s = d(1) / d(2); +in = p1(2) - s*p1(1); + +end + diff --git a/Math/Lines/pts2lineSIDistance.m b/Math/Lines/pts2lineSIDistance.m new file mode 100644 index 0000000..cfa33f4 --- /dev/null +++ b/Math/Lines/pts2lineSIDistance.m @@ -0,0 +1,22 @@ +function [ d P] = pts2lineSIDistance( p1, m, q ) +%PTS2LINESIDISTANCE Returns the distance from P1 to the line defined by M +%and Q +m1 = -m; +q1 = p1(2) - m1*p1(2); + +A=[1 -m;1 -m1]; + +P = (A\[q;q1])'; + +if isnan(P(1)) || isinf(P(1)) + d = inf; + P= []; + return +end + +dff = P - p1; +d = sqrt(dff(1)^2 +dff(2)^2); +%if inSegment(ip,p2,p3) + +end + diff --git a/Math/Lines/ptsDistance.m b/Math/Lines/ptsDistance.m new file mode 100644 index 0000000..31944fa --- /dev/null +++ b/Math/Lines/ptsDistance.m @@ -0,0 +1,7 @@ +function [ d ] = ptsDistance( p1, p2 ) +%2PTSDISTANCE distance between 2 points + +dx = p2(1) - p1(1); +dy = p2(2) - p1(2); +d = sqrt(dx^2 + dy^2); +end diff --git a/Math/Lines/r2Rt.m~ b/Math/Lines/r2Rt.m~ new file mode 100644 index 0000000..42a0c7b --- /dev/null +++ b/Math/Lines/r2Rt.m~ @@ -0,0 +1,6 @@ +%Return a 2D transformation matrix from angle a +function R = r2Rt(a,t) + +R = [ cos(a) -sin(a) t(1); sin(a) cos(a) t(2); 0 0 1]; + +end \ No newline at end of file diff --git a/Math/Lines/segseginter.m b/Math/Lines/segseginter.m new file mode 100644 index 0000000..f7b3c0e --- /dev/null +++ b/Math/Lines/segseginter.m @@ -0,0 +1,62 @@ +function [ out , pout, di] = segseginter( p1,p2,p3,p4 ) +%SEGSEGINTER Summary of this function goes here +% Detailed explanation goes here + +% Calculate the line segment PaPb that is the shortest route between +% two lines P1P2 and P3P4. Calculate also the values of mua and mub where +% Pa = P1 + mua (P2 - P1) +% Pb = P3 + mub (P4 - P3) +% Return FALSE if no solution exists. + +% XYZ p13,p43,p21; +% double d1343,d4321,d1321,d4343,d2121; +% double numer,denom; +% EPS = 1; +% p13(1) = p1(1) - p3(1); +% p13(2) = p1(2) - p3(2); +% p43(1) = p4(1) - p3(1); +% p43(2) = p4(2) - p3(2); +% +% +% if (abs(p43(1)) < EPS && abs(p43(2)) < EPS) +% out = (0); +% return; +% end +% p21(1)= p2(1)- p1(1); +% p21(2) = p2(2) - p1(2); +% +% if (abs(p21(1)) < EPS && abs(p21(2)) < EPS ) +% out = 0; +% return +% end +pout = []; +di = 0; + d = (p4(2) - p3(2)) * (p2(1) - p1(1)) - (p4(1) - p3(1)) * (p2(2) - p1(2)); + n_a = (p4(1) - p3(1)) * (p1(2) - p3(2)) - (p4(2) - p3(2)) * (p1(1) - p3(1)); + n_b = (p2(1) - p1(1)) * (p1(2) - p3(2)) - (p2(2) - p1(2)) * (p1(1) - p3(1)); + + + %denom = d2121 * d4343 - d4321 * d4321; + if (abs(d) == 0) + out = 0; + return + end + + ua = n_a / d; + ub = n_b / d; + + + if (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1) + + pout(1) = p1(1) + (ua * (p2(1) - p1(1))); + pout(2) = p1(2) + (ua * (p2(2) - p1(2))); + di = sqrt( (pout(1) - p2(1))^2 + (pout(2) - p2(2))^2 ); + out = 1; + return + end + out = 0; + + + +end + diff --git a/Math/Lines/segseginter.m~ b/Math/Lines/segseginter.m~ new file mode 100644 index 0000000..8dcbc85 --- /dev/null +++ b/Math/Lines/segseginter.m~ @@ -0,0 +1,61 @@ +function [ out , mua, mub] = segseginter( p1,p2,p3,p4 ) +%SEGSEGINTER Summary of this function goes here +% Detailed explanation goes here + +% Calculate the line segment PaPb that is the shortest route between +% two lines P1P2 and P3P4. Calculate also the values of mua and mub where +% Pa = P1 + mua (P2 - P1) +% Pb = P3 + mub (P4 - P3) +% Return FALSE if no solution exists. + +% XYZ p13,p43,p21; +% double d1343,d4321,d1321,d4343,d2121; +% double numer,denom; +EPS = 1; + p13(1) = p1(1) - p3(1); + p13(2) = p1(2) - p3(2); + p43(1) = p4(1) - p3(1); + p43(2) = p4(2) - p3(2); + + + if (abs(p43(1)) < EPS && abs(p43(2)) < EPS) + out = (0); + return; + end + p21(1)= p2(1)- p1(1); + p21(2) = p2(2) - p1(2); + + if (abs(p21(1)) < EPS && abs(p21(2)) < EPS ) + out = 0; + return + end + + denom = (p4(2) - p3(2)) * (p2(1) - p1(1)) - (p4(1) - P3(1)) * (p2(2) - p(2)); + n_a = (p4(2) - p3(1)) * (p1(2) - p3(2)) - (p4(2) - p3(2)) * (p1(1) - p3(1)); + n_b = (p2(1) - p1(1)) * (p1(2) - p3(2)) - (p2(2) - p1(2)) * (p1(1) - p3(1)); + + + %denom = d2121 * d4343 - d4321 * d4321; + if (abs(denom) == 0) + out = 0; + return + end + + ua = n_a / d; + ub = n_b / d; + + + if (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1d) + + ptIntersection.X = L1.X1 + (ua * (L1.X2 - L1.X1)); + ptIntersection.Y = L1.Y1 + (ua * (L1.Y2 - L1.Y1)); + out = 1; + end + out = 0 + + + out = 1; + + +end + diff --git a/Math/Matrix/calcCov.m b/Math/Matrix/calcCov.m new file mode 100644 index 0000000..555bbfc --- /dev/null +++ b/Math/Matrix/calcCov.m @@ -0,0 +1,12 @@ +function [ P ] = calcCov( X ) +%CALCCOV calculate the covariance matrix of a set of values X, row oriented + +sizeX = size(X,1); + +for i = 1:sizeX + + +end + +end + diff --git a/Math/Matrix/checkSingular.m b/Math/Matrix/checkSingular.m new file mode 100644 index 0000000..7a74b68 --- /dev/null +++ b/Math/Matrix/checkSingular.m @@ -0,0 +1,39 @@ +function [ out, A ] = checkSingular( A, repair ) +%CHECKSINGULAR Check if a matrix is singular by comparing its eigen values. +%If the smallest eigenvalue must be at least 0.001 times the biggest one. + +% WARNING, v can also be singular....... + +[v e] = eig(A); + +[emin imin] = min(diag(e)); +[emax imax] = max(diag(e)); + +out = 0; + +if emin/emax < 0.001 + % in this case the matrix is singular + out = 1; +else + return +end + + +if nargin > 1 && repair == 1 + %we want to se the smallest eigen values to be 0.001 times the biggest one + while emin/emax < 0.001 + + A=A+eye(2)*0.0001; + + [v e] = eig(A); + + [emin imin] = min(diag(e)); + [emax imax] = max(diag(e)); +% e(imin,imin) = e(imax,imax) * 0.0001; +% A = (v*e)\v; + end +end + + +end + diff --git a/Math/Matrix/checkSingular.m~ b/Math/Matrix/checkSingular.m~ new file mode 100644 index 0000000..e187766 --- /dev/null +++ b/Math/Matrix/checkSingular.m~ @@ -0,0 +1,30 @@ +function [ out, A ] = checkSingular( A, repair ) +%CHECKSINGULAR Check if a matrix is singular by comparing its eigen values. +%If the smallest eigenvalue must be at least 0.001 times the biggest one. + +% WARNING, v can also be singular....... + +[v e] = eig(A); + +[emin imin] = min(diag(e)); +[emax imax] = max(diag(e)); + +out = 0; + +if emin < emax*0.001 + % in this case the matrix is singular + out = 1; +else + return +end + + +if nargin > 1 && repair == 1 + %we want to se the smallest eigen values to be 0.001 times the biggest one + e(imin,imin) = e(imax,imax) * 0.001; + A = (v*e)/v; +end + + +end + diff --git a/Math/Matrix/gridOverlap.m b/Math/Matrix/gridOverlap.m new file mode 100644 index 0000000..d10a25b --- /dev/null +++ b/Math/Matrix/gridOverlap.m @@ -0,0 +1,17 @@ +function [ score ] = gridOverlap( A, B ) +%GRIDOVERLAP returns a score based on the overlap of two grids. The grids A +%and B must be the same size + +gsx = size(B,1); +gsy = size(B,2); +score =0; +for i=1:gsx + for j=1:gsy + if A(i,j) > 0 && B(i,j) > 0 + score = score+1; + end + end +end + +end + diff --git a/Math/Matrix/gridOverlap.m~ b/Math/Matrix/gridOverlap.m~ new file mode 100644 index 0000000..fbefe5c --- /dev/null +++ b/Math/Matrix/gridOverlap.m~ @@ -0,0 +1,11 @@ +function [ score ] = gridOverlap( A, B ) +%GRIDOVERLAP returns a score based on the overlap of two grids. The grids A +%and B must be the same size + +gsx = size(B,1); +gsy = size(B,2); + +fi + +end + diff --git a/Math/Matrix/nMaxima.m b/Math/Matrix/nMaxima.m new file mode 100644 index 0000000..c16acd0 --- /dev/null +++ b/Math/Matrix/nMaxima.m @@ -0,0 +1,37 @@ +function [ c, in ] = nMaxima( A, n ) +%NMAXIMA Returns the first n maxima of a array/matrix. If it is a matrix +% It opereates through all the matrixignoring its dimension. + +if nargin==1 + n = 1; +end + +rows = size(A,1); +cols = size(A,2); + +if size(A,2) > 1 + + for i=1:size(A,2) + B = [B A(1,:)]; + end +else + B = A; +end + +c = zeros(n); +in = zeros(n,2); + +for i=1:n + + [C I] = max(B); + B(I) = -inf; + ii(1,2) = floor(I/rows); + ii(1,1) = I - i(2) * cols; + + c(i) = C; + in(i) = ii; +end + + +end + diff --git a/Math/Matrix/nMaxima.m~ b/Math/Matrix/nMaxima.m~ new file mode 100644 index 0000000..fddac5e --- /dev/null +++ b/Math/Matrix/nMaxima.m~ @@ -0,0 +1,31 @@ +function [ c, i ] = nMaxima( A ) +%NMAXIMA Returns the first n maxima of a array/matrix. If it is a matrix +% It opereates through all the matrixignoring its dimension. + +rows = size(A,1); +cols = size(A,2); + +if size(A,2) > 1 + + for i=1:size(A,2) + B = [B A(1,:)]; + end +else + B = A; +end + +c = zeros(n); +i = zeros(n,2) +for i=1:n + + [C I] = max(B); + ii(1,2) = floor(I/rows); + ii(1,1) = I - i(2) * cols; + + c = [c C]; + i = [i ii]; +end + + +end + diff --git a/Math/Matrix/remapMatrix.m b/Math/Matrix/remapMatrix.m new file mode 100644 index 0000000..934baa0 --- /dev/null +++ b/Math/Matrix/remapMatrix.m @@ -0,0 +1,10 @@ +function [ C ] = remapMatrix( A, B ) +%REMAPMATRIX given a matrix A and a map schema B, reorder the elements +%according to that schema +C = zeros(size(A,1),size(A,2)); +for j = 1:size(A,1) + C(j,:) = A(B(j),:); +end + +end + diff --git a/Math/Matrix/repairSingular.m b/Math/Matrix/repairSingular.m new file mode 100644 index 0000000..299008a --- /dev/null +++ b/Math/Matrix/repairSingular.m @@ -0,0 +1,17 @@ +function [ A ] = repairSingular( A ) +%REPAIRSINGULAR Fix a non singular matrix. Uses the idea of + +% P. Biber and W. Straβer. The normal distribution transform: a new approach +% to laser scan matching. In Proceedings of the IEEE/RSJ International Con- +% ference on Intelligent Robots and Systems (IROS), volume 3, pages 2743 – +% 2748, October 2003. + + +if nargin > 1 && repair == 1 +%we want to se the smallest eigen values to be 0.001 times the biggest one + e(imin) = e(imax) * 0.001; + A = (e*D)\e; +end + +end + diff --git a/Math/Rotations/Contents.m b/Math/Rotations/Contents.m new file mode 100644 index 0000000..d72c4d2 --- /dev/null +++ b/Math/Rotations/Contents.m @@ -0,0 +1,31 @@ +% Rotations. +% +% Help +% quaternion - Help on quaternions for the Rotations/ toolbox. +% eulerAngles - Help on Euler angles for the Rotations/ toolbox. +% +% Conversion between rotation specifications +% au2q - Rotated angle and rotation axis vector to quaternion. +% e2q - Euler angles to quaternion conversion. +% e2R - Euler angles to rotation matrix. +% q2au - quaternion to rotated angle and rotation axis vector. +% q2e - Quaternion to Euler angles conversion. +% q2eG - Transform quaternion Gaussian to Euler Gaussian. +% q2Q - Quaternion to quaternion matrix conversion. +% q2R - Quaternion to rotation matrix conversion. +% q2v - Quaternion to rotation vector conversion. +% R2e - Rotation matrix to Euler angles conversion. +% R2q - rotation matrix to quaternion conversion. +% v2au - Rotation vector to rotation axis and angle conversion. +% v2q - Rotaiton vector to quaternion conversion. +% v2R - Rotation vector to rotation matrix conversion +% +% Other +% flu2rdf - Camera body to camera sensor rotation matrix. +% q2Pi - Pi matrix construction from quaternion. +% q2qc - Conjugate quaternion. +% pi2pc - Pi matrix to conjugated Pi matrix conversion. +% qProd - Quaternion product. +% w2omega - Skew symetric matrix OMEGA from angular rates vector W. +% qRot - Vector rotation via quaternion algebra. + diff --git a/Math/Rotations/Normalize.m b/Math/Rotations/Normalize.m new file mode 100644 index 0000000..179e3c7 --- /dev/null +++ b/Math/Rotations/Normalize.m @@ -0,0 +1,2 @@ +function [out] = Normalize(angle) +out = angle+2*pi*floor((pi-angle)/(2*pi)); \ No newline at end of file diff --git a/Math/Rotations/R2e.m b/Math/Rotations/R2e.m new file mode 100644 index 0000000..a92d48f --- /dev/null +++ b/Math/Rotations/R2e.m @@ -0,0 +1,56 @@ +function e = R2e(R) + +% R2E Rotation matrix to Euler angles conversion. +% +% See also FRAME. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + + +s = whos('R'); + +if (strcmp(s.class,'sym')) + roll = atan(R(3,2)/R(3,3)); + pitch = asin(-R(3,1)); + yaw = atan(R(2,1)/R(1,1)); +else + roll = atan2(R(3,2),R(3,3)); + pitch = asin(-R(3,1)); + yaw = atan2(R(2,1),R(1,1)); +end + +e = [roll;pitch;yaw]; + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/R2q.m b/Math/Rotations/R2q.m new file mode 100644 index 0000000..16f2c8a --- /dev/null +++ b/Math/Rotations/R2q.m @@ -0,0 +1,88 @@ +function q = R2q(R) + +% R2Q rotation matrix to quaternion conversion. +% +% See also QUATERNION, Q2R, Q2E, Q2V. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + + +T = trace(R) + 1; + +if isa(T,'sym') || ( T > 0.00000001 ) % to avoid large distortions! + + S = 2 * sqrt(T); + a = 0.25 * S; + b = ( R(2,3) - R(3,2) ) / S; + c = ( R(3,1) - R(1,3) ) / S; + d = ( R(1,2) - R(2,1) ) / S; + +else + if ( R(1,1) > R(2,2) && R(1,1) > R(3,3) ) + % Column 1: + % tested with R2 = diag([1 -1 -1]) + + S = 2 * sqrt( 1.0 + R(1,1) - R(2,2) - R(3,3) ); + a = (R(2,3) - R(3,2) ) / S; + b = 0.25 * S; + c = (R(1,2) + R(2,1) ) / S; + d = (R(3,1) + R(1,3) ) / S; + + elseif ( R(2,2) > R(3,3) ) + % Column 2: + % tested with R3 = [0 1 0;1 0 0;0 0 -1] + + S = 2 * sqrt( 1.0 + R(2,2) - R(1,1) - R(3,3) ); + a = (R(3,1) - R(1,3) ) / S; + b = (R(1,2) + R(2,1) ) / S; + c = 0.25 * S; + d = (R(2,3) + R(3,2) ) / S; + + else + % Column 3: + % tested with R4 = [-1 0 0;0 0 1;0 1 0] + + S = 2 * sqrt( 1.0 + R(3,3) - R(1,1) - R(2,2) ); + a = (R(1,2) - R(2,1) ) / S; + b = (R(3,1) + R(1,3) ) / S; + c = (R(2,3) + R(3,2) ) / S; + d = 0.25 * S; + + end +end + +q = [a -b -c -d]'; + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/Rotations.tar.gz b/Math/Rotations/Rotations.tar.gz new file mode 100644 index 0000000000000000000000000000000000000000..907bd2f74cc53f7308d0c8a68daf53121266a1a7 GIT binary patch literal 8955 zcmV@Jod9zQG=pfxvx#?^~Q4NVB$ zb{qd1yPanKF9vgWxBJ2{%-v4MZ0@$ZFPetgG+QrN^KsP@VIIX+$k+?p3;*PCd@J33 z=|5-~&!5+;Pu}nU zcKR<3-X~r`&G09-zhq;0KfhV=#In53>j;nal$58 z-BaH6XU`tdW!{v;Uive5noAG{&$sX<6J|p_kN9{#(TE$U*{8$PUq76kvi+ma?9={m zxPNr|xd+toCDu%S&4n!QYBupeG(1yiP2&VqClSB(hX=m`&;Gv--yfcSh9_We4o{Ez zCnxO9haohGW2icZ2WRj1hwS)lc>Lj{-vG5aD#l5|9VI-bw*-ag^4RhwQTo)M;l)Ov z-^68?)-{J0>~QE8pp8R=n?{_G z#>I5r@WYE8*+``ivxceL*E-cCI+~7{5RId|J}A(j{i>)UW&mTdL`LF zf8~=IRhQfl4MPn^W5vs2BdT-QRF#CCw4)bwmCq+U+(*n1HT%d+FHqAP9VjSMVsItGZpu3l) z6qwdLW68-*8SZ_8R1VK&Jt)X2D6*XfCM%6eMGS7u zJ10gw_67xpw{ou(*#_S0`+4PyUgaGYN^H22(|yG@EYy)zJ@2k2*M)aJT+uVHI=yCm z@uCW>3kp^q!HWxm?o?v9>G3xxu~zofHa{R9B1z-P+zj2ZlyqQS!Klq_-%U`8BW?lV zMg99fV<<+zglakff+a%C-fG7%rAgqjc(?dGiO5%s9(zD7IQ9E zv*XZrH&wG4#Dju@YGL|U{KAq{Cx7wMkN1YxQ(`Xuv% z*O5KfN0FA*XV1#NSZi{@?a*rAc>cA4KmPRpzVW}^YVG3q--7>)P6y}=qidGq|C5Bm z{Ga1g8EQn8*S0EKP!apkuYcL-x22~0*T7Wm-!j=B08a?h{h2>?alQ}okRqRlkR+Q! zA1BG7KbiDqUhG`<5^FKSytdLA{yS7z=9o_Htx4n;=J#pidk3`cV8`ueA`nj#{ori> zv_CvL{BWeP{@MFJ%%$GG@1JPU&kr;)U#FP?~N8DH7!`ydYGIua_sFf6Qi&{ib7yRTLnJJj)}AsJyX$UJ~~m+ zEg3Bm92A3gk)jx9rlRv=oJ2(z#SAzkG#j1z`IfS`oHN}?>dvelh1*e5k7jiz)F_TF zF^&=;PFzBqNQ}Z!UJP^6^l^BRi^BApa6Q$Zn7A0EGiB6T>PSr1pS<6Ms z9#qdBbY1J5m=Tm5pSoClqP)TCQTVHL{Qjh-4R;{h?Vb&7re5Y2i`c7{(ZEbzKZ7Um$zuLH! z^${!PElhZBc=fjr+#EhP{_k{ljaB^L>9$M!{}ds^|3hskNB1F&?iE7+!!I*o0b#XY zSl?SU9i+y9g&$5_;@LP4ATl4ih^b^piPN-uK?X3s z9~ZL(q2;C(Kw1FotEEFVD;S!6X+8gPz+b6Uph7`G0 zzLkoC)nTnyQQ#PF6tbRSN{Wy1MuF=DQ&NbGHfCEHO*_%J~LkN1we zm#7<}5R*fMq=b7tqI*Z)6QvH?sDSCYTjHM)$gFH$BOMZ`Y&05qkbQ4xYMbMAQ7Cos z(6fe{DtVdiKL`L#-e^ny|+aLtKVIUlBMu4n$RFj z@AaJAjYX~Jq^xCpODPsJPKop!FGwn8{G~;mQ~uUcEI!lM7j;2TeUqMRG7D;DM1``` zH_hhVy?yoKP7PVe@i+45AuPkciBq6$U=%im(R_Cyl!WMK()FTry(nETO4o}=b-nmo1a4kG_x!JEz?{!8TD#4T+3a?k<@(PP1~sH{ ziw4K*YJ92T`arGnGIIqeOzGezw%F;f%)gdBKP^)4|9*G^Jt)lNtksow7!Yx+e(q;2 z!+Oa1=sa3KaNqjxZmUD~pLVm|Zd3b*%hLb*Ny5wgVkdi;lNMv2xb((K)|PV-?jgLG z%&!2s>9Ru*Y#4aD!Yh|GafSDlCJ7b*`#4jyM8M(&yo`%f#Sy1(=W>R-^ASzoD!n*f7LZoZ7br)X-6TkogMM|X18$f?`%m-~ECaQz3i``UWT%SJ zwOa4FAEpdn9j5TQ$^8GE?SB{Xe;Z)G(Qb8H)c)I@vi(0rSZn{PLa6rtQgsOojncFa zRl*&>B^R6t6x<^STy_^fR-lcskL&?(Py27d?dtsBXqD@KPZCu7Kla$3*d|>)_T=`@ z<3scKfPLL|X|Wjt*!Bc__4u$R(_>)>5IkGTR)2g5v}j2;j}NPZn%oD4js!WHgFMZF zBofPm14=Uo2f(a}CA3V(+zN%|Q<2YwaWe)u-qErt<^1+S=dO-UR|;6f?f%{I4}<&%4k3lCGd1ALQC; zF~ST6-~n4)nz%(@T(;oS6_*GpDv1HtfFhi_%eE0s)a_nnNs-onOom4Q+VZahf47Oj#~dJ)dcgOlFi4Bcmh}wA^s1is(@K)ulo8B7r}(o=O+> zYI#e#=~4}1%-D&7Fs>TJ>7uSJH>N8c=y`;)by4=)Qnxyl zai)!=M78Qvu31hp3010#vfP#$)TzvJdG^JDs8EKKdygddNC*_)%8qgNhvmp!;})}- z!(6d%JlH$8dUnt0;hS0M8GG5GXZ;{M^o(l(?Hez#e~JA|>|bL49{~Gfz{9_v-~QKJ z#s9lzY5#kYP{99kkb7GVv_U!pc*`DkBz!6lMUXt)tS99NGoX@?lX@gb#Wr%1QBQgMw}Vh-gznzTzDQk7+U-#iJS`8QYl9B;0kD1z3E=U>{h&Um_k&kt_$x>M za`Z1p|8n&IK}LTZ|Bt^Mry{J&$M5X^W)SDyb0u(ru@oD`#2(l2+ z@;MZz#y`v@j=qJ6R!^p*!4rSmHFXRkt{2QpH-@)qA@BQEL8wfvq5Rr5C zlC%RB`Pt!0!P*DZNI+n)B`C4UtxE}#^!dYzQno$$1b`jtaFrf|G-g2&Pg1j&;?UVl z2^xs?N0wr?705+Ui{RQy0>Chd(^e8(rXbp=ch~B3@gq|YSa24qXSgYkx+*EOGHdI| zOC<_;RYYGZv0Yn7U}D9!w0V^_uhQmK+Pr=sn^zgKfFDqOZu?)evxfh6cgyepJW0rX z|I^V12Xb{Qa55Zt!0!02uH1G>4SOI^7Q!6_M5ELYD5%?uo8!rIIP5BB@QdYmxti_zyH`ayPb9W-)@Qj zo+1>!|0vPlP=UYUsu6JY5YJT;;E+Jp5Fw#G_a^SgT+haN3!+qQMb(SZU7Hb=MI zoL$x3ww#Jc*v=SC&sk^ph>TYVUZ zB)XJRNGLF5;zN|V!9BRm@PuA@ z(09}65H|g)156kshbybTjhz6EM`TBM?vnZc5rX5;cc1tC?{?F~{a@`)tJP_lT{{2K zG|K(oPZDzeAKD-=;2#-6sajOwJ}}eWV8_D`zvL$c(&8>1>nXW900Cw#b^KWO@?`^q z)nTrnHZ`0N^0^8jxA8JA$?%x_JrJTM}OG4-dmeGH3JXsK3`}{Ezxdr@uoP? zJ2eoCeM?q-foC_>mw4P#J;CFa>Njc968l6TfIhK)LrF=Rln~bqB_V-ZU#3Yn#Fdc1 zB_%DyR;Z$lTaZK97W9!8+=a&UCHza^-_59y1ksqxZZNr+MAR~QSxmy{LKZ;{m3aM;ZaDgOty9Sek|t}}HqP@n~PE@*+kTp=)P<$3Vs zeztNyTe+XD+|O2qCk(;RkDvSer`_h-_kYnPyu|-c5HkBe&c(4dRIfF40>tpcDf=kR z^vZ1SS$Sk{O)j_{TIm50j!n)HURWhB>O>Zn~pwi2?HkgbGlKQ3g;ZRmM4 z6mZY@-`zD^H2$}n?RLA{#P5Fs5-7+2rwA`|11e*`^2v-9PFs(tUSg{r${{=2Nyi7V zc1Ay+xE#q7RzL^T$Sv~i)u1|0=2uhJ)&&&E@8BtuUeVs3Ar?`B9nTu>y;iKJVzW@7gT> zh(B%i5j`dK?_?`~J~=x)Af;!qWtvM6#TS_-PYl<*IGMyD1?J&Z;~dK-=>peG$Yaf2 zXYO?!T+1~V3OVH!Pl!f=HR_c>CRjPuIssC`GB%;!5G7$sn%fe^<@6LZcl~+Y_2x}F zou*67L24rjlIWV+u7f6qd5n-CV9PX{VrlGln*Iup`6feUkV~o~VAC{I*Nz;m5YK_9 z0#BOQgojI9My)^sh|G9q+D{gRJM7Iul{5<;aM{M*v4Eu*ggAeHS*y#}7*5h9uqDtf z0?P7}gT11BaIjyLpPjqm5{o3> zb0L`;f4ihN4+to}0p5=;@emH%(Ctva?PI^~W54Y~zir!T3PlF$=#a~1%ZL_ybrlnG z_KNN^huC$fss_yw(EtYyWpF5i#_66KpDSE`OyeX9GP`9zz~3oqHC+%lW8|g>KaLtV zkLV__(IZN`%1j-#S(Hs#WICu+lK5k$QFT~^Q|JJ&-usijm>;xKh!tT?fsz-cG@8=L zawHqkajJ?*r|Le%Zr#YOR9?~qujN=qRw*-cpDsQ0=%TWY>2FgWYWR%# z;=yfMY&9DpqcEK=;wV3;i(br;D1PBUW^C!q3ZhOIzYtNk)4186Yd1Akqh>4D$5GL8 zl`C?sSHe>n$KD*?d+?+5=1dw))0_dqu!$XQ2Ys_h!Q z=>I%RQVS2+%6gGC7JL6=4E{E50Pja4z_oYk`3m_|)tkR;rB8u z_S*7UNXMyFZCYQmdwf_(~!)g-6Q-88BA)@;}S_G+!SrzFjMOoGjg+!YxZ%?KJNFk zkNea_)@v_o|LJ4QKg9n7-`$7DLH3_}1B3scBUSp}K_(qe3zNnXy$QT~3$qV&ZM>wZ zKLD`K!vK16j&m!2LkJ^rCrg)-R0IRgT|4(|$8qdDLE||p)w+;J^A}2k##W9K1DI8D z(BN@#nncNhYjD8~EGlNX%b-M-C#b!H;~U0jlw~;BV6rJ@(G+T6KT5Mm4oD zM%^xY1?PSYH(@1+Wn4p2fLH`~5j_IZ5I_{i-4!ONX*P|L+rmIm0}4uu+Yj1fz()`#82OM_+7V^d03=O$sJ?d~%2#(Vsm}JF_`g6jvSJPOA>>l< zOO`2`sp)x8C*{v2O{Nc88@Z4`;@{dvDonMG*Y$sCoqe#bea3UUfszJF8YuZvP_p!b z;)U)1b_RRg;gQ|yzoZnl^%dlJk)g&gHJKn z|0bP)mSe!H(ZApRb7Y@i)Bf`493ffF3wKkg@q%N>c+t|>r<%}|;6c)!Q^l&3NHRfn zO1LK&s2Yb&TJNNrRu#%ag`bm}_ywQUeohy#WZ^<2l+0nS&b*+cNg7&pQ{>9(;Xcjt z_*Y+U9>qiA^Xd+i$1$6{fD3AtS84=v1)eylncm#&pHp?Vw*+Zub{&8|E@b-?xKaVR?X=t6zSX zT{m*pNku`PD2Tp(Ij7&{b$ZoLUXD-YFzI`mfRw^$0q!^shC&$J$P@m%epDi`Y9?`V zYjw^}UY{I5-zej}>K2`DU36Et=P656^7|ZR@WA};u#WI}KZDV*WKGO_P@`PYX1KO^q+I&kH#^C{NqIp`LELfVceacfY_%9^mTk) z^99Z$0ELYZK=M_91AnsLq-Cx3ieM|;4=6YdV}kz5z~~g=)Z}z~a^W;1Db`}Y*oF40 z!x0`W7R+Hf(#>!*&(a_^ZCVz0*UYPA%KO+3iIS z5dhR{dB%+mtX%1>dI_v5!9Eh$rzAE_O0}v_gB}3YEp}S;&dcUOx(0+f2+kOE4i18e z?`)V47l?tIy|MD0SJqwU!ILn6*8pAvcn#n+fcM)|`SS4}{Q>!Z_x4?X*YnB$ySuw@ z{J%d-QfuUx7TYl{wut=3kM8-lqCe8Gfn0odm%PS*edtHZ+`@PcVkJCypTN*8B637C zgW;-0caZ=Ea8*Ia?$c%Pkkj-?%8Qjs!EKODrctieyYNGh1>qutr3Sre(6}cD3Nu(l z(E$Q5SxguCMo3$UVVVQB8*b!5LFdZ}{Gwnon0<{GWHpWUlHw=dK|21aA{7@s6fJI=PUCeK`_zdDPh{qrvgLq8(ooVT% zGguw}Pz?CR{l5Y3|M}<%c-P&f^Z%Va)4h_VS(hq0IKLC*%H|iTiIXeF2JoYQ zX`yk*rRCA5-&JHX5dkM=NWh6#kjWA%Vs{l85=dfMGMe7E91(kO-`MQr^@;yP9|ugh zGzg|suGhIFwazP_x!1y>L0uF}62+2uTaqYr&X<}TmL!KI$)Qdo^H!8@iw5sXN?ih# z3k0D$`G)i)C-DrHPV)LRNs~?oaM&#%B?mw$0vrF6TgT&_uJUqVoEaEr2F96zab{qg V8JJ{}NhUpa`X8r)AxHoi0RZzQs5Srq literal 0 HcmV?d00001 diff --git a/Math/Rotations/au2q.m b/Math/Rotations/au2q.m new file mode 100644 index 0000000..d167a65 --- /dev/null +++ b/Math/Rotations/au2q.m @@ -0,0 +1,50 @@ +function [q,Qa,Qu] = au2q(a,u) +%AU2Q Rotated angle and rotation axis vector to quaternion. +% Q = AU2Q(A,U) gives the quaternion representing a rotation +% of A rad around the axis defined by the unit vector U. +% +% [Q,Qa,Qu] = AU2Q(...) returns the Jacobians wrt A and U. + +q = [ cos(a/2) + u*sin(a/2)]; + +if nargout > 1 + + Qa = [-sin(a/2)/2 + u*cos(a/2)/2]; + + Qu = [0 0 0;eye(3)*sin(a/2)]; + +end + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/e2R.m b/Math/Rotations/e2R.m new file mode 100644 index 0000000..d712426 --- /dev/null +++ b/Math/Rotations/e2R.m @@ -0,0 +1,88 @@ +function [R,Re] = e2R(e) + +% E2R Euler angles to rotation matrix. +% E2R(EU) gives the rotation matrix body to world +% corresponding to the body orientation given +% by the euler angles vector EU (roll, pitch, yaw). +% +% [R,R_e] = E2R(EU) returns the Jacobian wrt EU. +% +% See also R2E. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +r = e(1); %roll +p = e(2); %pitch +y = e(3); %yaw + +sr = sin(r); +cr = cos(r); +sp = sin(p); +cp = cos(p); +sy = sin(y); +cy = cos(y); + +R= [cp*cy -cr*sy+sr*sp*cy sr*sy+cr*sp*cy + cp*sy cr*cy+sr*sp*sy -sr*cy+cr*sp*sy + -sp sr*cp cr*cp ]; + + +if nargout > 1 + + Re = [... + [ 0, -sin(p)*cos(y), -cos(p)*sin(y)] + [ 0, -sin(p)*sin(y), cos(p)*cos(y)] + [ 0, -cos(p), 0] + [ sin(r)*sin(y)+cos(r)*sin(p)*cos(y), sin(r)*cos(p)*cos(y), -cos(r)*cos(y)-sin(r)*sin(p)*sin(y)] + [ -sin(r)*cos(y)+cos(r)*sin(p)*sin(y), sin(r)*cos(p)*sin(y), -cos(r)*sin(y)+sin(r)*sin(p)*cos(y)] + [ cos(r)*cos(p), -sin(r)*sin(p), 0] + [ cos(r)*sin(y)-sin(r)*sin(p)*cos(y), cos(r)*cos(p)*cos(y), sin(r)*cos(y)-cos(r)*sin(p)*sin(y)] + [ -cos(r)*cos(y)-sin(r)*sin(p)*sin(y), cos(r)*cos(p)*sin(y), sin(r)*sin(y)+cos(r)*sin(p)*cos(y)] + [ -sin(r)*cos(p), -cos(r)*sin(p), 0]]; + +end + +return + +%% + +syms r p y real + +e = [r;p;y]; + +R = e2R(e); + +Re = simplify(jacobian(R,e)) + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/e2q.m b/Math/Rotations/e2q.m new file mode 100644 index 0000000..4a68f12 --- /dev/null +++ b/Math/Rotations/e2q.m @@ -0,0 +1,79 @@ +function [q,Qe] = e2q(e) + +% E2Q Euler angles to quaternion conversion. +% Q = vE2Q(E) gives the quaternion Q corresponding to the Euler angles +% vector E = [roll;pitch;yaw]. +% +% [Q,J] = E2Q(E) returns also the Jacobian matrix J = dQ/dE. +% +% See also QUATERNION, EULERANGLES, R2Q, Q2E, Q2V. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +qx = au2q(e(1),[1;0;0]); % roll rot. on X axis +qy = au2q(e(2),[0;1;0]); % pitch rot. on Y axis +qz = au2q(e(3),[0;0;1]); % yaw rot. on Z axis + +q = qProd(qProd(qz,qy),qx); + +if nargout == 2 + + sr = sin(e(1)/2); + sp = sin(e(2)/2); + sy = sin(e(3)/2); + + cr = cos(e(1)/2); + cp = cos(e(2)/2); + cy = cos(e(3)/2); + + Qe = 0.5*[ + [ -cy*cp*sr+sy*sp*cr, -cy*sp*cr+sy*cp*sr, -sy*cp*cr+cy*sp*sr] + [ cy*cp*cr+sy*sp*sr, -cy*sp*sr-sy*cp*cr, -sy*cp*sr-cy*sp*cr] + [ -cy*sp*sr+sy*cp*cr, cy*cp*cr-sy*sp*sr, -sy*sp*cr+cy*cp*sr] + [ -sy*cp*sr-cy*sp*cr, -cy*cp*sr-sy*sp*cr, cy*cp*cr+sy*sp*sr] + ]; +end + +return + +%% +syms r p y real +e = [r;p;y]; + +[q,Qe] = e2q(e); + +simplify(Qe-jacobian(q,e)) + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/e2v.m b/Math/Rotations/e2v.m new file mode 100644 index 0000000..096e03a --- /dev/null +++ b/Math/Rotations/e2v.m @@ -0,0 +1,6 @@ +function ea = e2v(th,axis) +%Transform the rotation theta TH over the axis AXIS into a rotation vector + + + +end \ No newline at end of file diff --git a/Math/Rotations/e2v.m~ b/Math/Rotations/e2v.m~ new file mode 100644 index 0000000..e76d08a --- /dev/null +++ b/Math/Rotations/e2v.m~ @@ -0,0 +1,6 @@ +function ea = e2v(th,axis) +%Transform the rotation theta TH over the axis AXIS into a rotation vector + +x(1) = + +end \ No newline at end of file diff --git a/Math/Rotations/eulerAngles.m b/Math/Rotations/eulerAngles.m new file mode 100644 index 0000000..412cc7f --- /dev/null +++ b/Math/Rotations/eulerAngles.m @@ -0,0 +1,69 @@ +% EULERANGLES Help on Euler angles for the Rotations/ toolbox. +% +% We specify the Euler angles as a column 3-vector +% +% e = [roll pitch yaw]', +% +% where +% roll is a positive rotation around the X-axis, +% pitch is a positive rotation around the Y-axis, and +% yaw is a positive rotation around the Z-axis. +% +% The orientation of a 3D solid with respect to the cartesian axes XYZ is +% given by the following ordered sequence of three elementary rotations: +% +% 1. rotation around the Z-axis - the yaw angle; +% 2. rotation around the rotated Y-axis - the pitch angle; +% 3. rotation around the doubly rotated X-axis - the roll angle. +% +% The three Euler angles are limited by gimbal lock. It is up to the user +% to ensure that the following conditions are met before calling any +% function with Euler angles: +% +% -pi <= roll < pi +% -pi/2 <= pitch < pi/2 +% -pi <= yaw < pi , or 0 <= yaw < 2pi +% +% Interesting functions involving Euler angles are: +% +% e2q, q2e conversion to and from quaternion. +% e2R, R2e conversion to and from rotation matrix. +% epose2qpose Euler-based frame to quaternion-based frame. +% qpose2epose the inverse of the above. +% +% See also E2Q, Q2E, E2R, R2E, EPOSE2QPOSE, QPOSE2EPOSE, QUATERNION. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/flu2rdf.m b/Math/Rotations/flu2rdf.m new file mode 100644 index 0000000..eba3a62 --- /dev/null +++ b/Math/Rotations/flu2rdf.m @@ -0,0 +1,44 @@ +function Rb2s = flu2rdf + +% FLU2RDF Camera body to camera sensor rotation matrix. +% FLU2RDF computes the rotation matrix for a camera whos body +% is in the FLU frame (x-front, y-left, z-up) and its sensor in +% the RDF frame (x-right, y-down, z-front). + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +Rb2s = e2R([-pi/2 0 -pi/2]'); + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/normAngle.m b/Math/Rotations/normAngle.m new file mode 100644 index 0000000..b9a1438 --- /dev/null +++ b/Math/Rotations/normAngle.m @@ -0,0 +1,3 @@ +function [out] = normAngle(angle) +%Normalize an angle in the range [-pi/2;pi/2] +out = angle+2*pi*floor((pi-angle)/(2*pi)); \ No newline at end of file diff --git a/Math/Rotations/pi2pc.m b/Math/Rotations/pi2pc.m new file mode 100644 index 0000000..a52ead3 --- /dev/null +++ b/Math/Rotations/pi2pc.m @@ -0,0 +1,47 @@ +function Pi = pi2pc(Pi) + +% PI2PC Pi matrix to conjugated Pi matrix conversion. +% PC = PI2PC(PI) converts the matrix +% PI = QUAT2PI(Q) +% into +% PC = QUAT2PI(QC) +% where QC = Q2QC(Q), the conjugated quaternion. +% +% See also Q2QC, Q2PI, TOFRAME + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +Pi([1 3 4 5 6 8 9 10 11]) = -Pi([1 3 4 5 6 8 9 10 11]); + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/polarReference.m b/Math/Rotations/polarReference.m new file mode 100644 index 0000000..3d7c875 --- /dev/null +++ b/Math/Rotations/polarReference.m @@ -0,0 +1,12 @@ +function [ a r ] = polarReference( a,r,t,rot ) +%POLARREFERENCE given a point in polar coordinate a, r, apply an axis +%change using the translation t and rotation rot + + rc = cos(rot+a)*r + t(1); + rs = sin(rot+a)*r + t(2); + + r = sqrt(rc^2 + rs^2); + a = atan2(rs,rc); + +end + diff --git a/Math/Rotations/q2Pi.m b/Math/Rotations/q2Pi.m new file mode 100644 index 0000000..1c2cc08 --- /dev/null +++ b/Math/Rotations/q2Pi.m @@ -0,0 +1,59 @@ +function Pi = q2Pi(q) + +% Q2PI Pi matrix construction from quaternion. +% PI = Q2PI(Q) Jacobian submatrix PI from quaternion +% +% Given: Q = [a b c d]' the attitude quaternion +% W = [p q r]' the angular rates vector +% OMEGA = W2OMEGA(W) a skew symetric matrix +% +% The output matrix: +% +% |-b -c -d | +% PI = | a -d c | +% | d a -b | +% |-c b a | +% +% is the Jacobian of OMEGA*Q with respect to W +% +% See also W2OMEGA, Q2R + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +Pi = [-q(2) -q(3) -q(4) + q(1) -q(4) q(3) + q(4) q(1) -q(2) + -q(3) q(2) q(1)]; + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/q2Q.m b/Math/Rotations/q2Q.m new file mode 100644 index 0000000..d154954 --- /dev/null +++ b/Math/Rotations/q2Q.m @@ -0,0 +1,57 @@ +function Q = q2Q(q) + +% Q2Q Quaternion to quaternion matrix conversion. +% Q = Q2Q(q) gives the matrix Q so that the quaternion product +% q1 x q2 is equivalent to the matrix product: +% +% q1 x q2 == q2Q(q1) * q2 +% +% If q = [a b c d]' this matrix is +% +% Q = [ a -b -c -d +% b a -d c +% c d a -b +% d -c b a ]; +% +% See also QUATERNION, R2Q, Q2E, Q2V. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +Q = [ q(1) -q(2) -q(3) -q(4) + q(2) q(1) -q(4) q(3) + q(3) q(4) q(1) -q(2) + q(4) -q(3) q(2) q(1) ]; + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/q2R.m b/Math/Rotations/q2R.m new file mode 100644 index 0000000..2402dfe --- /dev/null +++ b/Math/Rotations/q2R.m @@ -0,0 +1,86 @@ +function [R,Rq] = q2R(q) + +% Q2R Quaternion to rotation matrix conversion. +% R = Q2R(Q) builds the rotation matrix corresponding to the unit +% quaternion Q. The obtained matrix R is such that the product: +% +% re = R * rb +% +% converts the body referenced vector rb +% into the earth referenced vector re +% +% [R,Rq] = (...) returns the Jacobian wrt q. +% +% See also QUATERNION, R2Q, Q2E, Q2V. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +[a,b,c,d] = split(q); + +aa = a^2; +ab = 2*a*b; +ac = 2*a*c; +ad = 2*a*d; +bb = b^2; +bc = 2*b*c; +bd = 2*b*d; +cc = c^2; +cd = 2*c*d; +dd = d^2; + +R = [... + aa+bb-cc-dd bc-ad bd+ac + bc+ad aa-bb+cc-dd cd-ab + bd-ac cd+ab aa-bb-cc+dd]; + + +if nargout > 1 + + [a2,b2,c2,d2] = deal(2*a,2*b,2*c,2*d); + + Rq = [... + [ a2, b2, -c2, -d2] + [ d2, c2, b2, a2] + [ -c2, d2, -a2, b2] + [ -d2, c2, b2, -a2] + [ a2, -b2, c2, -d2] + [ b2, a2, d2, c2] + [ c2, d2, a2, b2] + [ -b2, -a2, d2, c2] + [ a2, -b2, -c2, d2]]; + +end + + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/q2au.m b/Math/Rotations/q2au.m new file mode 100644 index 0000000..1165b1a --- /dev/null +++ b/Math/Rotations/q2au.m @@ -0,0 +1,46 @@ +function [a,u] = q2au(q) + +%Q2AU quaternion to rotated angle and rotation axis vector. +% [A,U] = Q2AU(Q) gives the rotation of A rad around the axis +% defined by the unity vector U, that is equivalent to that +% defined by the quaternion Q + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +u = q(2:4); +u = u/norm(u); + +a = 2*acos(q(1)); + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/q2e.m b/Math/Rotations/q2e.m new file mode 100644 index 0000000..eeb92e4 --- /dev/null +++ b/Math/Rotations/q2e.m @@ -0,0 +1,103 @@ +function [e,Eq] = q2e(q) + +% Q2E Quaternion to Euler angles conversion. +% Q2E(Q) returns an Euler angles vector [roll;pitch;yaw] corresponding to +% the orientation quaternion Q. +% +% [E,Jq] = Q2E(Q) returns also the Jacobian matrix. +% +% See also QUATERNION, EULERANGLES, R2Q, E2Q, Q2V. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + + +a = q(1); +b = q(2); +c = q(3); +d = q(4); + +y1 = 2*c*d + 2*a*b; +x1 = a^2 - b^2 - c^2 + d^2; +z2 = -2*b*d + 2*a*c; +y3 = 2*b*c + 2*a*d; +x3 = a^2 + b^2 - c^2 - d^2; + +w = whos('q'); + +if strcmp(w.class,'sym') + + e = [ atan(y1/x1) + asin(z2) + atan(y3/x3) ]; + +else + + e = [ atan2(y1,x1) + asin(z2) + atan2(y3,x3) ]; + +end + +if nargout >1 + + dx1dq = [ 2*a, -2*b, -2*c, 2*d]; + dy1dq = [ 2*b, 2*a, 2*d, 2*c]; + dz2dq = [ 2*c, -2*d, 2*a, -2*b]; + dx3dq = [ 2*a, 2*b, -2*c, -2*d]; + dy3dq = [ 2*d, 2*c, 2*b, 2*a]; + + de1dx1 = -y1/(x1^2 + y1^2); + de1dy1 = x1/(x1^2 + y1^2); + de2dz2 = 1/sqrt(1-z2^2); + de3dx3 = -y3/(x3^2 + y3^2); + de3dy3 = x3/(x3^2 + y3^2); + + de1dq = de1dx1*dx1dq + de1dy1*dy1dq; + de2dq = de2dz2*dz2dq; + de3dq = de3dx3*dx3dq + de3dy3*dy3dq; + + Eq = [de1dq;de2dq;de3dq]; +end + +return + +%% jac + +syms a b c d real +q=[a;b;c;d]; +[e,Eq] = q2e(q); +simplify(Eq-jacobian(e,q)) + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/q2eG.m b/Math/Rotations/q2eG.m new file mode 100644 index 0000000..63588a7 --- /dev/null +++ b/Math/Rotations/q2eG.m @@ -0,0 +1,45 @@ +function [e,E] = q2eG(q,Q) + +% Q2EG Transform quaternion Gaussian to Euler Gaussian. +% [e,E] = Q2EG(q,Q) thransforms the Gaussian quaternion N(q,Q) into a +% Euler angles Gaussian N(e,E) representing the same rotation with the +% same uncertainty. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +[e,Eq] = q2e(q); +E = Eq*Q*Eq'; + + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/q2qc.m b/Math/Rotations/q2qc.m new file mode 100644 index 0000000..648b1cc --- /dev/null +++ b/Math/Rotations/q2qc.m @@ -0,0 +1,54 @@ +function [qc,QCq] = q2qc(q) + +% Q2QC Conjugate quaternion. +% Q2QC(Q) for Q = [a b c d] is the conjugate quaternion +% QC = [a -b -c -d] corresponding to the inverse +% rotation of that represented by Q. +% +% [qc,QCq] = Q2QC(...) gives the Jacobian wrt Q. +% +% See also QUATERNION, R2Q, Q2E, Q2V. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + + +qc = q; +qc(2:end) = -qc(2:end); + +if nargout == 2 + QCq = diag([1 -1 -1 -1]); +end + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/q2v.m b/Math/Rotations/q2v.m new file mode 100644 index 0000000..bdcd3ca --- /dev/null +++ b/Math/Rotations/q2v.m @@ -0,0 +1,43 @@ +function v = q2v(q) + +%Q2V Quaternion to rotation vector conversion. +% Q2V(Q) transforms the quaternion Q into a rotation vector representing +% the same rotation. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +[a,u] = q2au(q); +v = a*u; + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/qInv.m b/Math/Rotations/qInv.m new file mode 100644 index 0000000..0da8db6 --- /dev/null +++ b/Math/Rotations/qInv.m @@ -0,0 +1,9 @@ +function [ r ] = qInv( q ) +% Compute the invere of a given quaternion q + +[a,b,c,d] = split(q); +q2 = (a^2 + b^2 + c^2 + d^2 ); +r = [a -b -c -d]./q2; + +end + diff --git a/Math/Rotations/qInv.m~ b/Math/Rotations/qInv.m~ new file mode 100644 index 0000000..b0bc548 --- /dev/null +++ b/Math/Rotations/qInv.m~ @@ -0,0 +1,9 @@ +function [ r ] = qInv( q ) +% Compute the invere of a given quaternion q + +[a,b,c,d] = split(q1); +q2 = (a+b+c+d)^2; +r = a-b-c-d + +end + diff --git a/Math/Rotations/qProd.m b/Math/Rotations/qProd.m new file mode 100644 index 0000000..a1fd869 --- /dev/null +++ b/Math/Rotations/qProd.m @@ -0,0 +1,79 @@ +function [q,Qq1,Qq2] = qProd(q1,q2) + +% QPROD Quaternion product. +% QPROD(Q1,Q2) is the quaternion product Q1 * Q2 +% +% [Q,Qq1,Qq2] = QPROD(...) gives the Jacobians wrt Q1 and Q2. +% +% See also QUATERNION, R2Q, Q2E, Q2V. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +[a,b,c,d] = split(q1); +[w,x,y,z] = split(q2); + +q = [... + a*w - b*x - c*y - d*z + a*x + b*w + c*z - d*y + a*y - b*z + c*w + d*x + a*z + b*y - c*x + d*w]; + +if nargout > 1 + Qq1 = [... + [ w, -x, -y, -z] + [ x, w, z, -y] + [ y, -z, w, x] + [ z, y, -x, w]]; + + Qq2 = [... + [ a, -b, -c, -d] + [ b, a, -d, c] + [ c, d, a, -b] + [ d, -c, b, a]]; +end + +return + +%% +syms a b c d w x y z real +q1=[a b c d]'; +q2 = [w x y z]'; + +[q,Qq1,Qq2] = qProd(q1,q2); + +simplify(Qq1 - jacobian(q,q1)); +simplify(Qq2 - jacobian(q,q2)); + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/qRot.m b/Math/Rotations/qRot.m new file mode 100644 index 0000000..5cb11e5 --- /dev/null +++ b/Math/Rotations/qRot.m @@ -0,0 +1,48 @@ +function vr = qRot(v,q) + +% QROT Vector rotation via quaternion algebra. +% PR = QROT(V,Q) performs to vector V the rotation specified by +% quaternion Q. +% +% See also QUATERNION, Q2Q, QPROD. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +v = [0;v]; + +vr = qProd(qProd(q,v),q2qc(q)); + +vr = vr(2:end); + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/quaternion.m b/Math/Rotations/quaternion.m new file mode 100644 index 0000000..2373b4f --- /dev/null +++ b/Math/Rotations/quaternion.m @@ -0,0 +1,98 @@ +% QUATERNION Help on quaternions for the Rotations/ toolbox. +% +% We specify a quaternion with a unit, column 4-vector +% +% q = [a b c d]' , norm(q) = 1. +% +% with [a] the real part and [b c d]' the imaginary parts. This is +% equivalent to writing the true quaternion in quaternion space +% +% q = a + bi + cj + dk, +% +% with +% +% i^2 = j^2 = k^2 = -1, ij = k, ji = -k. +% +% NOTE: The products performed in quaternion space are indicated with +% a dot as in (q1.q2). Matrix products are with a star (A*v). +% +% Quaternions are used for encoding rotations and orientations with the +% following convention: +% +% 1. Consider a world or global frame W and a local frame F. +% +% 2. q is used to encode the orientation of frame F wrt frame W. It +% may be written as q_WF. +% +% 3. Consider now a vector v = vx.i + vy.j + vz.k in quaternion +% space. +% +% 4. We name v_F and v_W the coordinates of v in frames W and F. +% +% 5. Then, if q_FW = (q_WF)' = a - bi - cj - dk is the conjugate of +% q_WF, we have +% v_W = q_WF . v_F . q_FW +% v_F = q_FW . v_W . q_WF +% +% 6. This is equivalent to the linear, rotation matrix forms +% V_W = R_WF * V_F +% V_F = R_FW * V_W , +% with +% V_W, V_F : the same vectors in Euclidean 3d-space +% R_WF = q2R(q_WF) : the rotation matrix +% R_FW = R_WF'. : the transposed rotation matrix. +% +% Some interesting functions involving quaternions are: +% +% q2qc conjugate quaternion, q' = q2qc(q). +% qProd product of quaternions, q1.q2 = qProd(q1,q2) +% q2Q quaternion matrix, q1.q2 = q2Q(q1)*q2 +% q2R rotation matrix. We name R(q) = q2R(q), with the properties: +% R(q1.q2) = R(q1) * R(q2) +% R(q') = R(q)' +% R(-q) = R(q) +% q2e quaternion to Euler angles conversion +% q2v quaternion to rotation vector conversion +% R2q, e2q, v2q inverses of q2R, q2e and q2v. +% qpredict constant angular velocity prediction model. +% +% Most of these functions are equipped with Jacobian computation for +% their use in algorithms requiring linearization, such as EKF. +% +% See also Q2QC, QPROD, Q2Q, Q2R, Q2E, Q2V, R2Q, E2Q, V2Q, QPREDICT, +% EULERANGLES, EPOSE2QPOSE, QPOSE2EPOSE, FRAME. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/r2R.m b/Math/Rotations/r2R.m new file mode 100644 index 0000000..ba55759 --- /dev/null +++ b/Math/Rotations/r2R.m @@ -0,0 +1,6 @@ +%Return a 2D transformation matrix from angle a +function R = r2R(a) + +R = [ cos(a) -sin(a) 0; sin(a) cos(a) 0; 0 0 1]; + +end \ No newline at end of file diff --git a/Math/Rotations/r2Rt.m b/Math/Rotations/r2Rt.m new file mode 100644 index 0000000..42a0c7b --- /dev/null +++ b/Math/Rotations/r2Rt.m @@ -0,0 +1,6 @@ +%Return a 2D transformation matrix from angle a +function R = r2Rt(a,t) + +R = [ cos(a) -sin(a) t(1); sin(a) cos(a) t(2); 0 0 1]; + +end \ No newline at end of file diff --git a/Math/Rotations/r2RtI.m b/Math/Rotations/r2RtI.m new file mode 100644 index 0000000..213e8f8 --- /dev/null +++ b/Math/Rotations/r2RtI.m @@ -0,0 +1,8 @@ +function [ R ] = r2RtI( a, t ) +%R2RTI Summary of this function goes here +% Detailed explanation goes here + +R = inv(r2Rt(a,t)); + +end + diff --git a/Math/Rotations/split.m b/Math/Rotations/split.m new file mode 100644 index 0000000..ead2fcc --- /dev/null +++ b/Math/Rotations/split.m @@ -0,0 +1,65 @@ +function varargout = split(A) + +% SPLIT Split vectors into scalars, or matrices into row vectors. +% [s1,s2,...,sn] = SPLIT(V), with V a vector, returns all its components +% in scalars s1 ... sn. It is an error if numel(V) < nargout. +% +% [v1,...,vn] = SPLIT(M), with M a matrix, returns its rows as separate +% vectors v1 ... vn. It is an error if size(M,2) < nargout. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +if min(size(A)) == 1 + % A is vector. We take row or column. + ni = length(A); + if nargout > ni + error('not enough components in input vector'); + else + for i = 1:nargout + varargout{i} = A(i); + end + end +else + % A is matrix. We split rows only. + ni = size(A,2); + if nargout > ni + error('not enough rows in input matrix'); + else + for i = 1:nargout + varargout{i} = A(i,:); + end + end +end + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/v2R.m b/Math/Rotations/v2R.m new file mode 100644 index 0000000..df31654 --- /dev/null +++ b/Math/Rotations/v2R.m @@ -0,0 +1,47 @@ +function R = v2R(v) +% V2R Rotation vector to rotation matrix conversion +% V2R(V) computes the rotation matrix corresponding to the +% rotation vector V. Uses rodrigues formula. + +[a,u] = v2au(v); % u is always a column vector, regardless of the charactrer of v. + +% intermediate results +ca = cos(a); +sau = sin(a)*u; + +% R = cos(a)*eye(3) + sin(a)*hat(u) + (1-cos(a))*u*u'; A shortcut is: +R = diag([ca;ca;ca]) + hat(sau) + ((1-ca)*u)*u'; + + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/v2au.m b/Math/Rotations/v2au.m new file mode 100644 index 0000000..aa501e6 --- /dev/null +++ b/Math/Rotations/v2au.m @@ -0,0 +1,78 @@ +function [a,u,Av,Uv] = v2au(v) + +%V2AU Rotation vector to rotation axis and angle conversion. +% [ALPHA,U] = R2AU(V) converts the rotation vector V into its +% equivalent axis unity vector U and the rotated angle ALPHA +% +% [ALPHA,U,Av,Uv] = V2AU(...) returns the Jacobians of ALPHA and U wrt V. +% +% Note: it used to be r2av but confusion with possible R2av led to name change + +v = v(:); +a = sqrt(dot(v,v)); + +s = whos('a'); + +if (strcmp(s.class,'sym')) || (a>eps) + u = v/a; + if nargout > 2 + Av = u'; + Uv = [... + [ 1/a-u(1)^2/a, -u(1)/a*u(2), -u(1)/a*u(3) ] + [ -u(1)/a*u(2), 1/a-u(2)^2/a, -u(2)/a*u(3) ] + [ -u(1)/a*u(3), -u(2)/a*u(3), 1/a-u(3)^2/a ]]; + end + +else + a = 0; + u = [0;0;0]; + if nargout > 2 + warning('TODO: revise Jacobian') + Av = [0 0 0]; + Uv = zeros(3); + end +end + +return + +%% jac +syms u v w real +rv = [u;v;w]; +[a,u,Av,Au] = v2au(rv) + +Av - jacobian(a,rv) +Au - jacobian(u,rv) + + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/v2q.m b/Math/Rotations/v2q.m new file mode 100644 index 0000000..c14aa33 --- /dev/null +++ b/Math/Rotations/v2q.m @@ -0,0 +1,74 @@ +function [q,Qv] = v2q(v) + +% V2Q Rotaiton vector to quaternion conversion. +% [Q,Qv] = V2Q(V) returns the quaternion Q correscponding to the rotation +% encoded in rotation vector V, and the associated Jacobian Qv = dQ/dV. + +if nargout == 1 + + [a,u] = v2au(v); + q = au2q(a,u); + +else + a = sqrt(dot(v,v)); + + if isnumeric(a) && a < 1e-6 + + % Use small signal approximation: + q = [1-norm(v)^2/8 + v(:)/2]; + + Qv = [-1/4*v(:)' + 0.5*eye(3)]; + + else + + [a,u,Av,Uv] = v2au(v); + [q,Qa,Qu] = au2q(a,u); + Qv = Qa*Av + Qu*Uv; + + end +end + + +return + +%% +syms r s t real +v = [r;s;t]; +[q,Qv] = v2q(v) + +simplify(Qv - jacobian(q,v)) + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Rotations/w2omega.m b/Math/Rotations/w2omega.m new file mode 100644 index 0000000..fbcaae3 --- /dev/null +++ b/Math/Rotations/w2omega.m @@ -0,0 +1,57 @@ +function Omega=w2omega(w) + +% W2OMEGA Skew symetric matrix OMEGA from angular rates vector W. +% OMEGA = W2OMEGA(W) builds OMEGA as +% +% Omega=[ 0 -W(1) -W(2) -W(3) +% W(1) 0 W(3) -W(2) +% W(2) -W(3) 0 W(1) +% W(3) W(2) -W(1) 0 ]; +% +% See also Q2PI, ODO3. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + + +if all(size(w) == [3 1]) + Omega=[ 0 -w(1) -w(2) -w(3) + w(1) 0 w(3) -w(2) + w(2) -w(3) 0 w(1) + w(3) w(2) -w(1) 0 ]; +else + error('Input dimensions don''t agree. Enter 3x1 column vector') +end + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Scan/cartProject.m b/Math/Scan/cartProject.m new file mode 100644 index 0000000..417aa63 --- /dev/null +++ b/Math/Scan/cartProject.m @@ -0,0 +1,14 @@ +function [ np ] = cartProject( p, r, t ) +%CARTPROJECT Using rotation r and translation t project the Polar point p +% P has the form [x y] + +if size(p,2) == 2 + p = [p 1]; +end + +rTn = [cos(r) -sin(r) t(1); sin(r) cos(r) t(2); 0 0 1]; +np = (rTn*p')'; +np(3) = []; + +end + diff --git a/Math/Scan/cartProject.m~ b/Math/Scan/cartProject.m~ new file mode 100644 index 0000000..f403e21 --- /dev/null +++ b/Math/Scan/cartProject.m~ @@ -0,0 +1,12 @@ +function [ np ] = cartProject( p, r, t ) +%CARTPROJECT Using rotation r and translation t project the Polar point p +% P has the form [x y] + +if size( + +rTn = [cos(r) -sin(r) t(1); sin(r) cos(r) t(2); 0 0 1]; +np = (rTn*p)'; + + +end + diff --git a/Math/Scan/polarProject.m b/Math/Scan/polarProject.m new file mode 100644 index 0000000..19026c8 --- /dev/null +++ b/Math/Scan/polarProject.m @@ -0,0 +1,13 @@ +function [ np ] = polarProject( p, r, t) +%POLARPROJECT Using rotation r and translation t project the Polar point p +% P has the form [theta range] + + th = normAngle(p(1) + r); + + rc = cos(th)*p(2) + t(1); + rs = sin(th)*p(2) + t(2); + + np(2) = sqrt(rc^2 + rs^2); + np(1) = atan2(rs,rc); +end + diff --git a/Math/Scan/polarProject.m~ b/Math/Scan/polarProject.m~ new file mode 100644 index 0000000..8499f2b --- /dev/null +++ b/Math/Scan/polarProject.m~ @@ -0,0 +1,13 @@ +function [ output_args ] = polarProject(r, ) +%POLARPROJECT Summary of this function goes here +% Detailed explanation goes here + + th = normAngle(B.polar(i,1) + yaw); + ro = B.polar(i,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + B.polar(i,2) = sqrt(rc^2 + rs^2); + B.polar(i,1) = atan2(rs,rc); +end + diff --git a/Math/Signals/fastsmooth.m b/Math/Signals/fastsmooth.m new file mode 100644 index 0000000..00a1cf6 --- /dev/null +++ b/Math/Signals/fastsmooth.m @@ -0,0 +1,54 @@ +function SmoothY=fastsmooth(Y,w,type,ends) +% fastbsmooth(Y,w,type,ends) smooths vector Y with smooth +% of width w. Version 2.0, May 2008. +% The argument "type" determines the smooth type: +% If type=1, rectangular (sliding-average or boxcar) +% If type=2, triangular (2 passes of sliding-average) +% If type=3, pseudo-Gaussian (3 passes of sliding-average) +% The argument "ends" controls how the "ends" of the signal +% (the first w/2 points and the last w/2 points) are handled. +% If ends=0, the ends are zero. (In this mode the elapsed +% time is independent of the smooth width). The fastest. +% If ends=1, the ends are smoothed with progressively +% smaller smooths the closer to the end. (In this mode the +% elapsed time increases with increasing smooth widths). +% fastsmooth(Y,w,type) smooths with ends=0. +% fastsmooth(Y,w) smooths with type=1 and ends=0. +% Example: +% fastsmooth([1 1 1 10 10 10 1 1 1 1],3)= [0 1 4 7 10 7 4 1 1 0] +% fastsmooth([1 1 1 10 10 10 1 1 1 1],3,1,1)= [1 1 4 7 10 7 4 1 1 1] +% T. C. O'Haver, 2008. +if nargin==2, ends=0; type=1; end +if nargin==3, ends=0; end + switch type + case 1 + SmoothY=sa(Y,w,ends); + case 2 + SmoothY=sa(sa(Y,w,ends),w,ends); + case 3 + SmoothY=sa(sa(sa(Y,w,ends),w,ends),w,ends); + end + +function SmoothY=sa(Y,smoothwidth,ends) +w=round(smoothwidth); +SumPoints=sum(Y(1:w)); +s=zeros(size(Y)); +halfw=round(w/2); +L=length(Y); +for k=1:L-w, + s(k+halfw-1)=SumPoints; + SumPoints=SumPoints-Y(k); + SumPoints=SumPoints+Y(k+w); +end +s(k+halfw)=sum(Y(L-w+1:L)); +SmoothY=s./w; +% Taper the ends of the signal if ends=1. + if ends==1, + startpoint=(smoothwidth + 1)/2; + SmoothY(1)=(Y(1)+Y(2))./2; + for k=2:startpoint, + SmoothY(k)=mean(Y(1:(2*k-1))); + SmoothY(L-k+1)=mean(Y(L-2*k+2:L)); + end + SmoothY(L)=(Y(L)+Y(L-1))./2; + end diff --git a/Math/Utils/Contents.m b/Math/Utils/Contents.m new file mode 100644 index 0000000..e22931b --- /dev/null +++ b/Math/Utils/Contents.m @@ -0,0 +1,20 @@ +% Some mathematical functions. +% This folder adds Jacobian support to existing Matlab functions, and +% other functionnalities. +% +% Unit conversions +% deg2rad - Degrees to radians conversion. +% rad2deg - Radians to degrees conversion. +% +% Jacobian extensions to algebraic functions +% crossJ - Cross product with Jacobians output +% dotJ - Dot product with Jacobian return +% hat - Hat operator. +% normAngle - Normalize angle to (-pi .. pi] values. +% normquat - Normalize quaternion to unit length +% normvec - Normalize vector. +% +% Functions on Gaussian data +% mahalanobis - Mahalanobis distance +% propagateUncertainty - Non-linear propagation of Gaussian uncertainty. +% nees - Normalized Estimation Error Squared. diff --git a/Math/Utils/addRow.m b/Math/Utils/addRow.m new file mode 100644 index 0000000..55a0541 --- /dev/null +++ b/Math/Utils/addRow.m @@ -0,0 +1,8 @@ +%Multiply to every row + +function M = addRow(A,v) + + for i = 1:size(A,1) + M(i,:) = v+A(i,:); + end +end diff --git a/Math/Utils/applyTransform2Scan2D.m b/Math/Utils/applyTransform2Scan2D.m new file mode 100644 index 0000000..7e968f6 --- /dev/null +++ b/Math/Utils/applyTransform2Scan2D.m @@ -0,0 +1,15 @@ +function [ A ] = applyTransform2Scan2D( B, R, t ) +%APPLYTRANSFORM2SCAN Apply a transformation t and rotation R to the set of +%points defined in B +yaw = R; +x = t(1); +y = t(2); +A = zeros(size(B,1),size(B,2)); +sizeB = size(B,1); + rTn = [cos(R) -sin(R) x; sin(yaw) cos(yaw) y; 0 0 1]; + + for i = 1:sizeB + A(i,1:3) = (rTn*[B(i,1:2) 1]')' ; + end +end + diff --git a/Math/Utils/applyTransform2Scan2DPolar.m b/Math/Utils/applyTransform2Scan2DPolar.m new file mode 100644 index 0000000..f81ecc5 --- /dev/null +++ b/Math/Utils/applyTransform2Scan2DPolar.m @@ -0,0 +1,4 @@ +function [ A ] = applyTransform2Scan2DPolar( B, R, t ) + +end + diff --git a/Math/Utils/arrayshift.m b/Math/Utils/arrayshift.m new file mode 100644 index 0000000..4057645 --- /dev/null +++ b/Math/Utils/arrayshift.m @@ -0,0 +1,10 @@ +function [ b ] = arrayshift( a , n ) +%ARRAYSHIFT Summary of this function goes here +% Detailed explanation goes here + +for j = 1:size(a,2) + b(j) = a( mod(j+n, size(a,2) )+1); +end + +end + diff --git a/Math/Utils/checkConv.m b/Math/Utils/checkConv.m new file mode 100644 index 0000000..74a88b3 --- /dev/null +++ b/Math/Utils/checkConv.m @@ -0,0 +1,31 @@ +function [ out ] = checkConv( err, t, minerr ) +%CHECKCONV Given an input of values consevutive representing error function outputs, returns +%whether the function is converging. The convergence tolerance is given as +%input + +global Opt; +out = 0; + +if nargin==2 + minerr = 1; +end + +if size(err,2) > 0 + +% if minerr && err(end) >= Opt.scanmatcher.chival +% return; +% end + + dt = sqrt(t(:,1).^2 + t(:,2).^2); + dth = t(:,3); + + if size(err,2) >= Opt.scanmatcher.niterconv &&... + (std(err) < Opt.scanmatcher.convalue ||... + std(dt) < Opt.scanmatcher.convalue*10 && std(dth) < Opt.scanmatcher.convalue*10) + out = 1; + end + +end + +end + diff --git a/Math/Utils/checkConv.m~ b/Math/Utils/checkConv.m~ new file mode 100644 index 0000000..7477db1 --- /dev/null +++ b/Math/Utils/checkConv.m~ @@ -0,0 +1,6 @@ +function [ output_args ] = checkConv( input_args ) +%CHECKCONV Given an input of values, + + +end + diff --git a/Math/Utils/chi2.m b/Math/Utils/chi2.m new file mode 100644 index 0000000..730316b --- /dev/null +++ b/Math/Utils/chi2.m @@ -0,0 +1,355 @@ +function chi2 = chi2(n,p) + +% CHI2 Chi square distribution +% TH = CHI2(N,P) gives the critical values of the N-dimensional +% Chi-squared distribuiton function for a right-tail probability area P + +% Copyright 2009 Joan Sola @ LAAS-CNRS. + +[nTab,pTab,Chi2Tab] = chi2tab(); + +if (n == floor(n)) + + + % values of p + if p < .001 + warning('Too small probability. Assuming P=0.001') + p = 0.001; + elseif (p > 0.2) && (p<0.975) && (p~=0.5) + warning('Poor data in table. Inaccurate results. Use P<0.2 or P>0.975.') + elseif p > 0.995 + warning('Too big probability. Assuming P=0.995') + p = 0.995; + end + + % values of n + if (n > 1000) + error('Chi2 Lookup table only up to N=1000 DOF') + elseif (n > 250) && (mod(n,50)~=0) + warning('Value of N-DOF not found. Innacurate results. Use N in [1:1:250,300:50:1000]') + end + + % 2-D interpolation + chi2 = interp2(pTab,nTab,Chi2Tab,p,n); + +else + error('Dimension N must be a positive integer value in [1:1000]') +end + +end + +%% Lookup tables + +function [DOF,PRB,TAB] = chi2tab() + +PRB = [0.995 0.975 0.20 0.10 0.05 0.025 0.02 0.01 0.005 0.002 0.001]; + +DOF = [1:250 300:50:1000]; + +TAB = [... +1 0.0000393 0.000982 1.642 2.706 3.841 5.024 5.412 6.635 7.879 9.550 10.828 +2 0.0100 0.0506 3.219 4.605 5.991 7.378 7.824 9.210 10.597 12.429 13.816 +3 0.0717 0.216 4.642 6.251 7.815 9.348 9.837 11.345 12.838 14.796 16.266 +4 0.207 0.484 5.989 7.779 9.488 11.143 11.668 13.277 14.860 16.924 18.467 +5 0.412 0.831 7.289 9.236 11.070 12.833 13.388 15.086 16.750 18.907 20.515 +6 0.676 1.237 8.558 10.645 12.592 14.449 15.033 16.812 18.548 20.791 22.458 +7 0.989 1.690 9.803 12.017 14.067 16.013 16.622 18.475 20.278 22.601 24.322 +8 1.344 2.180 11.030 13.362 15.507 17.535 18.168 20.090 21.955 24.352 26.124 +9 1.735 2.700 12.242 14.684 16.919 19.023 19.679 21.666 23.589 26.056 27.877 +10 2.156 3.247 13.442 15.987 18.307 20.483 21.161 23.209 25.188 27.722 29.588 +11 2.603 3.816 14.631 17.275 19.675 21.920 22.618 24.725 26.757 29.354 31.264 +12 3.074 4.404 15.812 18.549 21.026 23.337 24.054 26.217 28.300 30.957 32.909 +13 3.565 5.009 16.985 19.812 22.362 24.736 25.472 27.688 29.819 32.535 34.528 +14 4.075 5.629 18.151 21.064 23.685 26.119 26.873 29.141 31.319 34.091 36.123 +15 4.601 6.262 19.311 22.307 24.996 27.488 28.259 30.578 32.801 35.628 37.697 +16 5.142 6.908 20.465 23.542 26.296 28.845 29.633 32.000 34.267 37.146 39.252 +17 5.697 7.564 21.615 24.769 27.587 30.191 30.995 33.409 35.718 38.648 40.790 +18 6.265 8.231 22.760 25.989 28.869 31.526 32.346 34.805 37.156 40.136 42.312 +19 6.844 8.907 23.900 27.204 30.144 32.852 33.687 36.191 38.582 41.610 43.820 +20 7.434 9.591 25.038 28.412 31.410 34.170 35.020 37.566 39.997 43.072 45.315 +21 8.034 10.283 26.171 29.615 32.671 35.479 36.343 38.932 41.401 44.522 46.797 +22 8.643 10.982 27.301 30.813 33.924 36.781 37.659 40.289 42.796 45.962 48.268 +23 9.260 11.689 28.429 32.007 35.172 38.076 38.968 41.638 44.181 47.391 49.728 +24 9.886 12.401 29.553 33.196 36.415 39.364 40.270 42.980 45.559 48.812 51.179 +25 10.520 13.120 30.675 34.382 37.652 40.646 41.566 44.314 46.928 50.223 52.620 +26 11.160 13.844 31.795 35.563 38.885 41.923 42.856 45.642 48.290 51.627 54.052 +27 11.808 14.573 32.912 36.741 40.113 43.195 44.140 46.963 49.645 53.023 55.476 +28 12.461 15.308 34.027 37.916 41.337 44.461 45.419 48.278 50.993 54.411 56.892 +29 13.121 16.047 35.139 39.087 42.557 45.722 46.693 49.588 52.336 55.792 58.301 +30 13.787 16.791 36.250 40.256 43.773 46.979 47.962 50.892 53.672 57.167 59.703 +31 14.458 17.539 37.359 41.422 44.985 48.232 49.226 52.191 55.003 58.536 61.098 +32 15.134 18.291 38.466 42.585 46.194 49.480 50.487 53.486 56.328 59.899 62.487 +33 15.815 19.047 39.572 43.745 47.400 50.725 51.743 54.776 57.648 61.256 63.870 +34 16.501 19.806 40.676 44.903 48.602 51.966 52.995 56.061 58.964 62.608 65.247 +35 17.192 20.569 41.778 46.059 49.802 53.203 54.244 57.342 60.275 63.955 66.619 +36 17.887 21.336 42.879 47.212 50.998 54.437 55.489 58.619 61.581 65.296 67.985 +37 18.586 22.106 43.978 48.363 52.192 55.668 56.730 59.893 62.883 66.633 69.346 +38 19.289 22.878 45.076 49.513 53.384 56.896 57.969 61.162 64.181 67.966 70.703 +39 19.996 23.654 46.173 50.660 54.572 58.120 59.204 62.428 65.476 69.294 72.055 +40 20.707 24.433 47.269 51.805 55.758 59.342 60.436 63.691 66.766 70.618 73.402 +41 21.421 25.215 48.363 52.949 56.942 60.561 61.665 64.950 68.053 71.938 74.745 +42 22.138 25.999 49.456 54.090 58.124 61.777 62.892 66.206 69.336 73.254 76.084 +43 22.859 26.785 50.548 55.230 59.304 62.990 64.116 67.459 70.616 74.566 77.419 +44 23.584 27.575 51.639 56.369 60.481 64.201 65.337 68.710 71.893 75.874 78.750 +45 24.311 28.366 52.729 57.505 61.656 65.410 66.555 69.957 73.166 77.179 80.077 +46 25.041 29.160 53.818 58.641 62.830 66.617 67.771 71.201 74.437 78.481 81.400 +47 25.775 29.956 54.906 59.774 64.001 67.821 68.985 72.443 75.704 79.780 82.720 +48 26.511 30.755 55.993 60.907 65.171 69.023 70.197 73.683 76.969 81.075 84.037 +49 27.249 31.555 57.079 62.038 66.339 70.222 71.406 74.919 78.231 82.367 85.351 +50 27.991 32.357 58.164 63.167 67.505 71.420 72.613 76.154 79.490 83.657 86.661 +51 28.735 33.162 59.248 64.295 68.669 72.616 73.818 77.386 80.747 84.943 87.968 +52 29.481 33.968 60.332 65.422 69.832 73.810 75.021 78.616 82.001 86.227 89.272 +53 30.230 34.776 61.414 66.548 70.993 75.002 76.223 79.843 83.253 87.507 90.573 +54 30.981 35.586 62.496 67.673 72.153 76.192 77.422 81.069 84.502 88.786 91.872 +55 31.735 36.398 63.577 68.796 73.311 77.380 78.619 82.292 85.749 90.061 93.168 +56 32.490 37.212 64.658 69.919 74.468 78.567 79.815 83.513 86.994 91.335 94.461 +57 33.248 38.027 65.737 71.040 75.624 79.752 81.009 84.733 88.236 92.605 95.751 +58 34.008 38.844 66.816 72.160 76.778 80.936 82.201 85.950 89.477 93.874 97.039 +59 34.770 39.662 67.894 73.279 77.931 82.117 83.391 87.166 90.715 95.140 98.324 +60 35.534 40.482 68.972 74.397 79.082 83.298 84.580 88.379 91.952 96.404 99.607 +61 36.301 41.303 70.049 75.514 80.232 84.476 85.767 89.591 93.186 97.665 100.888 +62 37.068 42.126 71.125 76.630 81.381 85.654 86.953 90.802 94.419 98.925 102.166 +63 37.838 42.950 72.201 77.745 82.529 86.830 88.137 92.010 95.649 100.182 103.442 +64 38.610 43.776 73.276 78.860 83.675 88.004 89.320 93.217 96.878 101.437 104.716 +65 39.383 44.603 74.351 79.973 84.821 89.177 90.501 94.422 98.105 102.691 105.988 +66 40.158 45.431 75.424 81.085 85.965 90.349 91.681 95.626 99.330 103.942 107.258 +67 40.935 46.261 76.498 82.197 87.108 91.519 92.860 96.828 100.554 105.192 108.526 +68 41.713 47.092 77.571 83.308 88.250 92.689 94.037 98.028 101.776 106.440 109.791 +69 42.494 47.924 78.643 84.418 89.391 93.856 95.213 99.228 102.996 107.685 111.055 +70 43.275 48.758 79.715 85.527 90.531 95.023 96.388 100.425 104.215 108.929 112.317 +71 44.058 49.592 80.786 86.635 91.670 96.189 97.561 101.621 105.432 110.172 113.577 +72 44.843 50.428 81.857 87.743 92.808 97.353 98.733 102.816 106.648 111.412 114.835 +73 45.629 51.265 82.927 88.850 93.945 98.516 99.904 104.010 107.862 112.651 116.092 +74 46.417 52.103 83.997 89.956 95.081 99.678 101.074 105.202 109.074 113.889 117.346 +75 47.206 52.942 85.066 91.061 96.217 100.839 102.243 106.393 110.286 115.125 118.599 +76 47.997 53.782 86.135 92.166 97.351 101.999 103.410 107.583 111.495 116.359 119.850 +77 48.788 54.623 87.203 93.270 98.484 103.158 104.576 108.771 112.704 117.591 121.100 +78 49.582 55.466 88.271 94.374 99.617 104.316 105.742 109.958 113.911 118.823 122.348 +79 50.376 56.309 89.338 95.476 100.749 105.473 106.906 111.144 115.117 120.052 123.594 +80 51.172 57.153 90.405 96.578 101.879 106.629 108.069 112.329 116.321 121.280 124.839 +81 51.969 57.998 91.472 97.680 103.010 107.783 109.232 113.512 117.524 122.507 126.083 +82 52.767 58.845 92.538 98.780 104.139 108.937 110.393 114.695 118.726 123.733 127.324 +83 53.567 59.692 93.604 99.880 105.267 110.090 111.553 115.876 119.927 124.957 128.565 +84 54.368 60.540 94.669 100.980 106.395 111.242 112.712 117.057 121.126 126.179 129.804 +85 55.170 61.389 95.734 102.079 107.522 112.393 113.871 118.236 122.325 127.401 131.041 +86 55.973 62.239 96.799 103.177 108.648 113.544 115.028 119.414 123.522 128.621 132.277 +87 56.777 63.089 97.863 104.275 109.773 114.693 116.184 120.591 124.718 129.840 133.512 +88 57.582 63.941 98.927 105.372 110.898 115.841 117.340 121.767 125.913 131.057 134.745 +89 58.389 64.793 99.991 106.469 112.022 116.989 118.495 122.942 127.106 132.273 135.978 +90 59.196 65.647 101.054 107.565 113.145 118.136 119.648 124.116 128.299 133.489 137.208 +91 60.005 66.501 102.117 108.661 114.268 119.282 120.801 125.289 129.491 134.702 138.438 +92 60.815 67.356 103.179 109.756 115.390 120.427 121.954 126.462 130.681 135.915 139.666 +93 61.625 68.211 104.241 110.850 116.511 121.571 123.105 127.633 131.871 137.127 140.893 +94 62.437 69.068 105.303 111.944 117.632 122.715 124.255 128.803 133.059 138.337 142.119 +95 63.250 69.925 106.364 113.038 118.752 123.858 125.405 129.973 134.247 139.546 143.344 +96 64.063 70.783 107.425 114.131 119.871 125.000 126.554 131.141 135.433 140.755 144.567 +97 64.878 71.642 108.486 115.223 120.990 126.141 127.702 132.309 136.619 141.962 145.789 +98 65.694 72.501 109.547 116.315 122.108 127.282 128.849 133.476 137.803 143.168 147.010 +99 66.510 73.361 110.607 117.407 123.225 128.422 129.996 134.642 138.987 144.373 148.230 +100 67.328 74.222 111.667 118.498 124.342 129.561 131.142 135.807 140.169 145.577 149.449 +101 68.146 75.083 112.726 119.589 125.458 130.700 132.287 136.971 141.351 146.780 150.667 +102 68.965 75.946 113.786 120.679 126.574 131.838 133.431 138.134 142.532 147.982 151.884 +103 69.785 76.809 114.845 121.769 127.689 132.975 134.575 139.297 143.712 149.183 153.099 +104 70.606 77.672 115.903 122.858 128.804 134.111 135.718 140.459 144.891 150.383 154.314 +105 71.428 78.536 116.962 123.947 129.918 135.247 136.860 141.620 146.070 151.582 155.528 +106 72.251 79.401 118.020 125.035 131.031 136.382 138.002 142.780 147.247 152.780 156.740 +107 73.075 80.267 119.078 126.123 132.144 137.517 139.143 143.940 148.424 153.977 157.952 +108 73.899 81.133 120.135 127.211 133.257 138.651 140.283 145.099 149.599 155.173 159.162 +109 74.724 82.000 121.192 128.298 134.369 139.784 141.423 146.257 150.774 156.369 160.372 +110 75.550 82.867 122.250 129.385 135.480 140.917 142.562 147.414 151.948 157.563 161.581 +111 76.377 83.735 123.306 130.472 136.591 142.049 143.700 148.571 153.122 158.757 162.788 +112 77.204 84.604 124.363 131.558 137.701 143.180 144.838 149.727 154.294 159.950 163.995 +113 78.033 85.473 125.419 132.643 138.811 144.311 145.975 150.882 155.466 161.141 165.201 +114 78.862 86.342 126.475 133.729 139.921 145.441 147.111 152.037 156.637 162.332 166.406 +115 79.692 87.213 127.531 134.813 141.030 146.571 148.247 153.191 157.808 163.523 167.610 +116 80.522 88.084 128.587 135.898 142.138 147.700 149.383 154.344 158.977 164.712 168.813 +117 81.353 88.955 129.642 136.982 143.246 148.829 150.517 155.496 160.146 165.900 170.016 +118 82.185 89.827 130.697 138.066 144.354 149.957 151.652 156.648 161.314 167.088 171.217 +119 83.018 90.700 131.752 139.149 145.461 151.084 152.785 157.800 162.481 168.275 172.418 +120 83.852 91.573 132.806 140.233 146.567 152.211 153.918 158.950 163.648 169.461 173.617 +121 84.686 92.446 133.861 141.315 147.674 153.338 155.051 160.100 164.814 170.647 174.816 +122 85.520 93.320 134.915 142.398 148.779 154.464 156.183 161.250 165.980 171.831 176.014 +123 86.356 94.195 135.969 143.480 149.885 155.589 157.314 162.398 167.144 173.015 177.212 +124 87.192 95.070 137.022 144.562 150.989 156.714 158.445 163.546 168.308 174.198 178.408 +125 88.029 95.946 138.076 145.643 152.094 157.839 159.575 164.694 169.471 175.380 179.604 +126 88.866 96.822 139.129 146.724 153.198 158.962 160.705 165.841 170.634 176.562 180.799 +127 89.704 97.698 140.182 147.805 154.302 160.086 161.834 166.987 171.796 177.743 181.993 +128 90.543 98.576 141.235 148.885 155.405 161.209 162.963 168.133 172.957 178.923 183.186 +129 91.382 99.453 142.288 149.965 156.508 162.331 164.091 169.278 174.118 180.103 184.379 +130 92.222 100.331 143.340 151.045 157.610 163.453 165.219 170.423 175.278 181.282 185.571 +131 93.063 101.210 144.392 152.125 158.712 164.575 166.346 171.567 176.438 182.460 186.762 +132 93.904 102.089 145.444 153.204 159.814 165.696 167.473 172.711 177.597 183.637 187.953 +133 94.746 102.968 146.496 154.283 160.915 166.816 168.600 173.854 178.755 184.814 189.142 +134 95.588 103.848 147.548 155.361 162.016 167.936 169.725 174.996 179.913 185.990 190.331 +135 96.431 104.729 148.599 156.440 163.116 169.056 170.851 176.138 181.070 187.165 191.520 +136 97.275 105.609 149.651 157.518 164.216 170.175 171.976 177.280 182.226 188.340 192.707 +137 98.119 106.491 150.702 158.595 165.316 171.294 173.100 178.421 183.382 189.514 193.894 +138 98.964 107.372 151.753 159.673 166.415 172.412 174.224 179.561 184.538 190.688 195.080 +139 99.809 108.254 152.803 160.750 167.514 173.530 175.348 180.701 185.693 191.861 196.266 +140 100.655 109.137 153.854 161.827 168.613 174.648 176.471 181.840 186.847 193.033 197.451 +141 101.501 110.020 154.904 162.904 169.711 175.765 177.594 182.979 188.001 194.205 198.635 +142 102.348 110.903 155.954 163.980 170.809 176.882 178.716 184.118 189.154 195.376 199.819 +143 103.196 111.787 157.004 165.056 171.907 177.998 179.838 185.256 190.306 196.546 201.002 +144 104.044 112.671 158.054 166.132 173.004 179.114 180.959 186.393 191.458 197.716 202.184 +145 104.892 113.556 159.104 167.207 174.101 180.229 182.080 187.530 192.610 198.885 203.366 +146 105.741 114.441 160.153 168.283 175.198 181.344 183.200 188.666 193.761 200.054 204.547 +147 106.591 115.326 161.202 169.358 176.294 182.459 184.321 189.802 194.912 201.222 205.727 +148 107.441 116.212 162.251 170.432 177.390 183.573 185.440 190.938 196.062 202.390 206.907 +149 108.291 117.098 163.300 171.507 178.485 184.687 186.560 192.073 197.211 203.557 208.086 +150 109.142 117.985 164.349 172.581 179.581 185.800 187.678 193.208 198.360 204.723 209.265 +151 109.994 118.871 165.398 173.655 180.676 186.914 188.797 194.342 199.509 205.889 210.443 +152 110.846 119.759 166.446 174.729 181.770 188.026 189.915 195.476 200.657 207.054 211.620 +153 111.698 120.646 167.495 175.803 182.865 189.139 191.033 196.609 201.804 208.219 212.797 +154 112.551 121.534 168.543 176.876 183.959 190.251 192.150 197.742 202.951 209.383 213.973 +155 113.405 122.423 169.591 177.949 185.052 191.362 193.267 198.874 204.098 210.547 215.149 +156 114.259 123.312 170.639 179.022 186.146 192.474 194.384 200.006 205.244 211.710 216.324 +157 115.113 124.201 171.686 180.094 187.239 193.584 195.500 201.138 206.390 212.873 217.499 +158 115.968 125.090 172.734 181.167 188.332 194.695 196.616 202.269 207.535 214.035 218.673 +159 116.823 125.980 173.781 182.239 189.424 195.805 197.731 203.400 208.680 215.197 219.846 +160 117.679 126.870 174.828 183.311 190.516 196.915 198.846 204.530 209.824 216.358 221.019 +161 118.536 127.761 175.875 184.382 191.608 198.025 199.961 205.660 210.968 217.518 222.191 +162 119.392 128.651 176.922 185.454 192.700 199.134 201.076 206.790 212.111 218.678 223.363 +163 120.249 129.543 177.969 186.525 193.791 200.243 202.190 207.919 213.254 219.838 224.535 +164 121.107 130.434 179.016 187.596 194.883 201.351 203.303 209.047 214.396 220.997 225.705 +165 121.965 131.326 180.062 188.667 195.973 202.459 204.417 210.176 215.539 222.156 226.876 +166 122.823 132.218 181.109 189.737 197.064 203.567 205.530 211.304 216.680 223.314 228.045 +167 123.682 133.111 182.155 190.808 198.154 204.675 206.642 212.431 217.821 224.472 229.215 +168 124.541 134.003 183.201 191.878 199.244 205.782 207.755 213.558 218.962 225.629 230.383 +169 125.401 134.897 184.247 192.948 200.334 206.889 208.867 214.685 220.102 226.786 231.552 +170 126.261 135.790 185.293 194.017 201.423 207.995 209.978 215.812 221.242 227.942 232.719 +171 127.122 136.684 186.338 195.087 202.513 209.102 211.090 216.938 222.382 229.098 233.887 +172 127.983 137.578 187.384 196.156 203.602 210.208 212.201 218.063 223.521 230.253 235.053 +173 128.844 138.472 188.429 197.225 204.690 211.313 213.311 219.189 224.660 231.408 236.220 +174 129.706 139.367 189.475 198.294 205.779 212.419 214.422 220.314 225.798 232.563 237.385 +175 130.568 140.262 190.520 199.363 206.867 213.524 215.532 221.438 226.936 233.717 238.551 +176 131.430 141.157 191.565 200.432 207.955 214.628 216.641 222.563 228.074 234.870 239.716 +177 132.293 142.053 192.610 201.500 209.042 215.733 217.751 223.687 229.211 236.023 240.880 +178 133.157 142.949 193.654 202.568 210.130 216.837 218.860 224.810 230.347 237.176 242.044 +179 134.020 143.845 194.699 203.636 211.217 217.941 219.969 225.933 231.484 238.328 243.207 +180 134.884 144.741 195.743 204.704 212.304 219.044 221.077 227.056 232.620 239.480 244.370 +181 135.749 145.638 196.788 205.771 213.391 220.148 222.185 228.179 233.755 240.632 245.533 +182 136.614 146.535 197.832 206.839 214.477 221.251 223.293 229.301 234.891 241.783 246.695 +183 137.479 147.432 198.876 207.906 215.563 222.353 224.401 230.423 236.026 242.933 247.857 +184 138.344 148.330 199.920 208.973 216.649 223.456 225.508 231.544 237.160 244.084 249.018 +185 139.210 149.228 200.964 210.040 217.735 224.558 226.615 232.665 238.294 245.234 250.179 +186 140.077 150.126 202.008 211.106 218.820 225.660 227.722 233.786 239.428 246.383 251.339 +187 140.943 151.024 203.052 212.173 219.906 226.761 228.828 234.907 240.561 247.532 252.499 +188 141.810 151.923 204.095 213.239 220.991 227.863 229.935 236.027 241.694 248.681 253.659 +189 142.678 152.822 205.139 214.305 222.076 228.964 231.040 237.147 242.827 249.829 254.818 +190 143.545 153.721 206.182 215.371 223.160 230.064 232.146 238.266 243.959 250.977 255.976 +191 144.413 154.621 207.225 216.437 224.245 231.165 233.251 239.386 245.091 252.124 257.135 +192 145.282 155.521 208.268 217.502 225.329 232.265 234.356 240.505 246.223 253.271 258.292 +193 146.150 156.421 209.311 218.568 226.413 233.365 235.461 241.623 247.354 254.418 259.450 +194 147.020 157.321 210.354 219.633 227.496 234.465 236.566 242.742 248.485 255.564 260.607 +195 147.889 158.221 211.397 220.698 228.580 235.564 237.670 243.860 249.616 256.710 261.763 +196 148.759 159.122 212.439 221.763 229.663 236.664 238.774 244.977 250.746 257.855 262.920 +197 149.629 160.023 213.482 222.828 230.746 237.763 239.877 246.095 251.876 259.001 264.075 +198 150.499 160.925 214.524 223.892 231.829 238.861 240.981 247.212 253.006 260.145 265.231 +199 151.370 161.826 215.567 224.957 232.912 239.960 242.084 248.329 254.135 261.290 266.386 +200 152.241 162.728 216.609 226.021 233.994 241.058 243.187 249.445 255.264 262.434 267.541 +201 153.112 163.630 217.651 227.085 235.077 242.156 244.290 250.561 256.393 263.578 268.695 +202 153.984 164.532 218.693 228.149 236.159 243.254 245.392 251.677 257.521 264.721 269.849 +203 154.856 165.435 219.735 229.213 237.240 244.351 246.494 252.793 258.649 265.864 271.002 +204 155.728 166.338 220.777 230.276 238.322 245.448 247.596 253.908 259.777 267.007 272.155 +205 156.601 167.241 221.818 231.340 239.403 246.545 248.698 255.023 260.904 268.149 273.308 +206 157.474 168.144 222.860 232.403 240.485 247.642 249.799 256.138 262.031 269.291 274.460 +207 158.347 169.047 223.901 233.466 241.566 248.739 250.900 257.253 263.158 270.432 275.612 +208 159.221 169.951 224.943 234.529 242.647 249.835 252.001 258.367 264.285 271.574 276.764 +209 160.095 170.855 225.984 235.592 243.727 250.931 253.102 259.481 265.411 272.715 277.915 +210 160.969 171.759 227.025 236.655 244.808 252.027 254.202 260.595 266.537 273.855 279.066 +211 161.843 172.664 228.066 237.717 245.888 253.122 255.302 261.708 267.662 274.995 280.217 +212 162.718 173.568 229.107 238.780 246.968 254.218 256.402 262.821 268.788 276.135 281.367 +213 163.593 174.473 230.148 239.842 248.048 255.313 257.502 263.934 269.912 277.275 282.517 +214 164.469 175.378 231.189 240.904 249.128 256.408 258.601 265.047 271.037 278.414 283.666 +215 165.344 176.283 232.230 241.966 250.207 257.503 259.701 266.159 272.162 279.553 284.815 +216 166.220 177.189 233.270 243.028 251.286 258.597 260.800 267.271 273.286 280.692 285.964 +217 167.096 178.095 234.311 244.090 252.365 259.691 261.898 268.383 274.409 281.830 287.112 +218 167.973 179.001 235.351 245.151 253.444 260.785 262.997 269.495 275.533 282.968 288.261 +219 168.850 179.907 236.391 246.213 254.523 261.879 264.095 270.606 276.656 284.106 289.408 +220 169.727 180.813 237.432 247.274 255.602 262.973 265.193 271.717 277.779 285.243 290.556 +221 170.604 181.720 238.472 248.335 256.680 264.066 266.291 272.828 278.902 286.380 291.703 +222 171.482 182.627 239.512 249.396 257.758 265.159 267.389 273.939 280.024 287.517 292.850 +223 172.360 183.534 240.552 250.457 258.837 266.252 268.486 275.049 281.146 288.653 293.996 +224 173.238 184.441 241.592 251.517 259.914 267.345 269.584 276.159 282.268 289.789 295.142 +225 174.116 185.348 242.631 252.578 260.992 268.438 270.681 277.269 283.390 290.925 296.288 +226 174.995 186.256 243.671 253.638 262.070 269.530 271.777 278.379 284.511 292.061 297.433 +227 175.874 187.164 244.711 254.699 263.147 270.622 272.874 279.488 285.632 293.196 298.579 +228 176.753 188.072 245.750 255.759 264.224 271.714 273.970 280.597 286.753 294.331 299.723 +229 177.633 188.980 246.790 256.819 265.301 272.806 275.066 281.706 287.874 295.465 300.868 +230 178.512 189.889 247.829 257.879 266.378 273.898 276.162 282.814 288.994 296.600 302.012 +231 179.392 190.797 248.868 258.939 267.455 274.989 277.258 283.923 290.114 297.734 303.156 +232 180.273 191.706 249.908 259.998 268.531 276.080 278.354 285.031 291.234 298.867 304.299 +233 181.153 192.615 250.947 261.058 269.608 277.171 279.449 286.139 292.353 300.001 305.443 +234 182.034 193.524 251.986 262.117 270.684 278.262 280.544 287.247 293.472 301.134 306.586 +235 182.915 194.434 253.025 263.176 271.760 279.352 281.639 288.354 294.591 302.267 307.728 +236 183.796 195.343 254.063 264.235 272.836 280.443 282.734 289.461 295.710 303.400 308.871 +237 184.678 196.253 255.102 265.294 273.911 281.533 283.828 290.568 296.828 304.532 310.013 +238 185.560 197.163 256.141 266.353 274.987 282.623 284.922 291.675 297.947 305.664 311.154 +239 186.442 198.073 257.179 267.412 276.062 283.713 286.016 292.782 299.065 306.796 312.296 +240 187.324 198.984 258.218 268.471 277.138 284.802 287.110 293.888 300.182 307.927 313.437 +241 188.207 199.894 259.256 269.529 278.213 285.892 288.204 294.994 301.300 309.058 314.578 +242 189.090 200.805 260.295 270.588 279.288 286.981 289.298 296.100 302.417 310.189 315.718 +243 189.973 201.716 261.333 271.646 280.362 288.070 290.391 297.206 303.534 311.320 316.859 +244 190.856 202.627 262.371 272.704 281.437 289.159 291.484 298.311 304.651 312.450 317.999 +245 191.739 203.539 263.409 273.762 282.511 290.248 292.577 299.417 305.767 313.580 319.138 +246 192.623 204.450 264.447 274.820 283.586 291.336 293.670 300.522 306.883 314.710 320.278 +247 193.507 205.362 265.485 275.878 284.660 292.425 294.762 301.626 307.999 315.840 321.417 +248 194.391 206.274 266.523 276.935 285.734 293.513 295.855 302.731 309.115 316.969 322.556 +249 195.276 207.186 267.561 277.993 286.808 294.601 296.947 303.835 310.231 318.098 323.694 +250 196.161 208.098 268.599 279.050 287.882 295.689 298.039 304.940 311.346 319.227 324.832 +300 240.663 253.912 320.397 331.789 341.395 349.874 352.425 359.906 366.844 375.369 381.425 +350 285.608 300.064 372.051 384.306 394.626 403.723 406.457 414.474 421.900 431.017 437.488 +400 330.903 346.482 423.590 436.649 447.632 457.305 460.211 468.724 476.606 486.274 493.132 +450 376.483 393.118 475.035 488.849 500.456 510.670 513.736 522.717 531.026 541.212 548.432 +500 422.303 439.936 526.401 540.930 553.127 563.852 567.070 576.493 585.207 595.882 603.446 +550 468.328 486.910 577.701 592.909 605.667 616.878 620.241 630.084 639.183 650.324 658.215 +600 514.529 534.019 628.943 644.800 658.094 669.769 673.270 683.516 692.982 704.568 712.771 +650 560.885 581.245 680.134 696.614 710.421 722.542 726.176 736.807 746.625 758.639 767.141 +700 607.380 628.577 731.280 748.359 762.661 775.211 778.972 789.974 800.131 812.556 821.347 +750 653.997 676.003 782.386 800.043 814.822 827.785 831.670 843.029 853.514 866.336 875.404 +800 700.725 723.513 833.456 851.671 866.911 880.275 884.279 895.984 906.786 919.991 929.329 +850 747.554 771.099 884.492 903.249 918.937 932.689 936.808 948.848 959.957 973.534 983.133 +900 794.475 818.756 935.499 954.782 970.904 985.032 989.263 1001.630 1013.036 1026.974 1036.826 +950 841.480 866.477 986.478 1006.272 1022.816 1037.311 1041.651 1054.334 1066.031 1080.320 1090.418 +1000 888.564 914.257 1037.431 1057.724 1074.679 1089.531 1093.977 1106.969 1118.948 1133.579 1143.917]; + +% remove DOF column +TAB = TAB(:,2:end); + +% add the P=0.5 column +PRB = [PRB(1:2) 0.500 PRB(3:end)]; +TAB = [TAB(:,1:2) DOF' TAB(:,3:end)]; + +end + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Utils/ci.m b/Math/Utils/ci.m new file mode 100644 index 0000000..c4a0336 --- /dev/null +++ b/Math/Utils/ci.m @@ -0,0 +1,91 @@ +function interval=ci(x,confidence,dim); + +% interval=ci(x,confidence,dim); +% +% 90%, 95% or 99% confidence interval of a vector or 2d matrix +% Optional dim refers to dimension and must be 1 or 2 (default is 1). +% Optional confidence must either 90, 95 or 99 (default is 95). +% +% Calculated as the standard error multiplied by the critical two-tailed value of t for +% a=0.10, 0.05 or 0.01 (see Statistical methods in psychology, D.C. Howell). +% This formula applies when the Population standard deviation is unknown. +% As n gets larger then the t distribution approaches the normal distribution. +% So, if n>120, uses z instead of t (e.g. 1.96 standard errors, for 95% interval) + +% Raymond Reynolds 23/11/06 + +if nargin<3 + dim = min(find(size(x)~=1)); + if isempty(dim), dim = 1; end +end +if nargin<2 + dim = min(find(size(x)~=1)); + if isempty(dim), dim = 1; end + confidence=95; +end + +n=size(x,dim); +sd=std(x,0,dim); +sem=sd./sqrt(n); + +% t table for p=0.10 +ttable90=[6.314 2.92 2.353 2.132 2.015 1.943 1.895 1.86 1.833 1.812... + 1.796 1.782 1.771 1.761 1.753 1.746 1.74 1.734 1.729 1.725... + 1.721 1.717 1.714 1.711 1.708 1.706 1.703 1.701 1.699 1.697... + 1.684 1.676 1.671 1.664 1.66 1.658 1.645]; +% t table for p=0.05 +ttable95=[12.71 4.303 3.182 2.776 2.571 2.447 2.365 2.306 2.262 2.228... + 2.201 2.179 2.16 2.145 2.131 2.12 2.11 2.101 2.093 2.086... + 2.08 2.074 2.069 2.064 2.06 2.056 2.052 2.048 2.045 2.042... + 2.021 2.009 2.00 1.99 1.984 1.98 1.96]; +% t table for p=0.01 +ttable99=[63.66 9.925 5.841 4.604 4.032 3.707 3.499 3.355 3.25 3.169... + 3.106 3.055 3.012 2.977 2.947 2.921 2.898 2.878 2.861 2.845... + 2.831 2.819 2.807 2.797 2.787 2.779 2.771 2.763 2.756 2.75... + 2.704 2.678 2.66 2.639 2.626 2.617 2.576]; + +if confidence==90; + if n<30 + t=ttable90(n-1); + elseif n<30; t=ttable90(n-1); + elseif n<40; t=ttable90(30); + elseif n<50; t=ttable90(31); + elseif n<60; t=ttable90(32); + elseif n<80; t=ttable90(33); + elseif n<100; t=ttable90(34); + elseif n<120; t=ttable90(35); + elseif n==120; t=ttable90(36); + elseif n>120; t=ttable90(37); + end +elseif confidence==95; + if n<30 + t=ttable95(n-1); + elseif n<30; t=ttable95(n-1); + elseif n<40; t=ttable95(30); + elseif n<50; t=ttable95(31); + elseif n<60; t=ttable95(32); + elseif n<80; t=ttable95(33); + elseif n<100; t=ttable95(34); + elseif n<120; t=ttable95(35); + elseif n==120; t=ttable95(36); + elseif n>120; t=ttable95(37); + end +elseif confidence==99; + if n<30 + t=ttable99(n-1); + elseif n<30; t=ttable99(n-1); + elseif n<40; t=ttable99(30); + elseif n<50; t=ttable99(31); + elseif n<60; t=ttable99(32); + elseif n<80; t=ttable99(33); + elseif n<100; t=ttable99(34); + elseif n<120; t=ttable99(35); + elseif n==120; t=ttable99(36); + elseif n>120; t=ttable99(37); + end +end + +interval=t.*sem; + + + diff --git a/Math/Utils/compound.m b/Math/Utils/compound.m new file mode 100644 index 0000000..bf951cb --- /dev/null +++ b/Math/Utils/compound.m @@ -0,0 +1,48 @@ +function res = compound(pa,pb) +%Compouting and relative Jacobians +%for feature and point(s) +%and for feature and feature(s) + +yaw = pa(3); +cosy = cos(yaw); siny = sin(yaw); +count = size(pb,2); +% Ja = zeros(2,3,count); + +%x,y: feature and point(s) +if size(pb,1) == 2 + r = zeros(2,count); + Ja = zeros(2,3,count); + for i=1:count + r(:,i) = [pb(1,i)*cosy-pb(2,i)*siny+pa(1); + pb(1,i)*siny+pb(2,i)*cosy+pa(2)]; + %first Jacobian + Ja(:,:,i) = [1, 0, -pb(1,i)*siny - pb(2,i)*cosy; + 0, 1, pb(1,i)*cosy - pb(2,i)*siny]; + end + %second Jacobian + Jb = [cosy, -siny; + siny, cosy]; + +%x,y,theta: feature and feature(s) +elseif size(pb,1) == 3 + r = zeros(3,count); + Ja = zeros(3,3,count); + for i=1:count + r(:,i) = [pb(1,i)*cosy - pb(2,i)*siny + pa(1); + pb(1,i)*siny + pb(2,i)*cosy + pa(2); + pb(3,i) + pa(3)]; + r(3,i) = normAngle(r(3,i)); + %first Jacobian + Ja(:,:,i) = [1, 0, -pb(1,i)*siny - pb(2,i)*cosy; + 0, 1, pb(1,i)*cosy - pb(2,i)*siny; + 0, 0, 1]; + end + %second Jacobian + Jb = [cosy, -siny, 0; + siny, cosy, 0; + 0, 0, 1]; +end + +res.x = r; +res.Ja = Ja; +res.Jb = Jb; diff --git a/Math/Utils/dec2gc.m b/Math/Utils/dec2gc.m new file mode 100644 index 0000000..d748bfb --- /dev/null +++ b/Math/Utils/dec2gc.m @@ -0,0 +1,7 @@ +function [ dec ] = dec2gc(gcd) +%GC2DEC Returns the inverse of the Gray code representation of a decimal + +dec = bitxor(gcd,bitshift(gcd,-1)); + +end + diff --git a/Math/Utils/dec2gc.m~ b/Math/Utils/dec2gc.m~ new file mode 100644 index 0000000..26d96a4 --- /dev/null +++ b/Math/Utils/dec2gc.m~ @@ -0,0 +1,15 @@ +function gcode = dec2gc(dec,N) + +% Description: This function converts a decimal number to its equivalent +% gray code representation. + +gcode = zeros(size(bin)); + +tmp = decnum; idv = 1; +while idc > 1 || + tmp = bitxor(tmp,bitshift(tmp,idv)); + idv = bitshift(idv,1); +end + + +gcode = tmp; \ No newline at end of file diff --git a/Math/Utils/deg2rad.m b/Math/Utils/deg2rad.m new file mode 100644 index 0000000..728b28f --- /dev/null +++ b/Math/Utils/deg2rad.m @@ -0,0 +1,40 @@ +function rad = deg2rad(deg) + +% DEG2RAD Degrees to radians conversion. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +rad = deg*pi/180; + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Utils/epose2qpose.m b/Math/Utils/epose2qpose.m new file mode 100644 index 0000000..67fbe5a --- /dev/null +++ b/Math/Utils/epose2qpose.m @@ -0,0 +1,53 @@ +function [qp,QP_ep] = epose2qpose(ep) + +% EPOSE2QPOSE Euler-specified to quaternion-specified pose conversion. +% +% QP = EPOSE2QPOSE(EP) returns a full 7-pose QP=[X;Q] from a full 6-pose +% QE=[X;E], where X is 3D opsition and Q and E are 3D orientations. +% +% [QP,Jep] = EPOSE2QPOSE(...) returns also the Jacobian matrix +% +% See also QPOSE2EPOSE, EULERANGLES, QUATERNION, FRAME. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +qp = zeros(7,1); % empty quaternion pose +qp(1:3) = ep(1:3); % position copy +P = eye(3); % Jacobian of copy function +[qp(4:7),Q_e] = e2q(ep(4:6));% orientation and Jacobian + +QP_ep = [P zeros(3);zeros(4,3) Q_e]; % Full Jacobian + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Utils/filterAssMatrix.m b/Math/Utils/filterAssMatrix.m new file mode 100644 index 0000000..9d6c7a5 --- /dev/null +++ b/Math/Utils/filterAssMatrix.m @@ -0,0 +1,15 @@ +function [ A B ] = filterAssMatrix( A, B, i ) +%FILTERASSMATRIX Given two matrices of points A and B with B representing +%the association set of A. Removes all the indeces from A and B which are +%not associated. ie when an element of i = -1 + +ix = find(i == -1); +if ~isempty(A) +A(ix,:) = []; +end +if ~isempty(B) +B(ix,:) = []; +end + +end + diff --git a/Math/Utils/findLocalMaxima.m b/Math/Utils/findLocalMaxima.m new file mode 100644 index 0000000..c882dae --- /dev/null +++ b/Math/Utils/findLocalMaxima.m @@ -0,0 +1,19 @@ +% Find the local maxima of a sequence; it returns +% the indexes in descending order +% +% nm maximum number of maxima returned +% +function [maxima values] = findLocalMaxima(sequence, nm) + + % This is a bit of black magic; these are the beauties of Matlab + indexes = find(diff(sign(diff(sequence,1)))==-2); + + values = sequence(indexes); + + [unused, sortedIndexes] = sort(values,'descend'); + + maxima = indexes(sortedIndexes); + + if size(maxima,2)>nm + maxima = maxima(1:nm); + end diff --git a/Math/Utils/fitnessGrid2.m b/Math/Utils/fitnessGrid2.m new file mode 100644 index 0000000..323a50a --- /dev/null +++ b/Math/Utils/fitnessGrid2.m @@ -0,0 +1,55 @@ +function [ score A] = fitnessGrid2( A, B, motion ) +%FITNESSGRID defines a fitness function for the grid B. It evaluates how +%good the scan A, applied the transformation MOTION, fits in the grid. It +%counts without any explicit association how many cells overlap. A cell is +% said to overlap with another if it falls under a range of a 5*5 area. +x0 = motion(1); +y0 = motion(2); +yaw0 = motion(3); +A = applyTransform2Scan2D(A, yaw0, [x0 y0]); +sizen = size(A,1); +score = 0; +gr = B.grid; +gsx = size(B.grid,1); +gsy = size(B.grid,2); +sb = 1; + + +% Check every point in the scan A +for i = 1:sizen + [a b] = getCoordinates(A(i,:),B.PARAM); + % Check the surraounding area + scored = 0; + for j = 1:sb*2 + for k = 1:sb*2 + % Restrict the search area inside the grid map + ix = a - sb + k ; + iy = b - sb + j ; + if ix > gsx || ix <= 0 || iy > gsy || iy <= 0 + continue + end + + if gr(ix,iy) == 1 + if ~((-sb + k) == 0) + penalty = 0.2; + else + penalty = 0; + end + + if ~((-sb + j) == 0) + penalty = penalty+ 0.2; + end + score = score + 1 - penalty; % - (0.1*(-sb+k))*(1*(-sb+j)) ; + scored = 1; + end + end + if scored + continue + end + end + + +end + +end + diff --git a/Math/Utils/frameInv.m b/Math/Utils/frameInv.m new file mode 100644 index 0000000..b53d494 --- /dev/null +++ b/Math/Utils/frameInv.m @@ -0,0 +1,11 @@ +function [ f ] = frameInv( p ) +%FRAMEINV frame inverse +x = p(1); y = p(2); +cosy = cos(p(3)); siny = sin(p(3)); + +f = [-p(1)*cosy - p(2)*siny; + p(1)*siny - p(2)*cosy; + -p(3)]; + +end + diff --git a/Math/Utils/frameRef.m b/Math/Utils/frameRef.m new file mode 100644 index 0000000..a5443a2 --- /dev/null +++ b/Math/Utils/frameRef.m @@ -0,0 +1,17 @@ +function [ f ] = frameRef( f1, f2, invs ) +%FRAMEREF Reference the frame f1 with respect to f2. f1 and f2 are vector +% containing a position x y z and a rotation Yaw + +if (nargin == 3 && invs == 0) || nargin == 2 + f = r2Rt(f2(3),f2(1:2))*[f1(1:2); 1]; + f = [f(1:2); normAngle(f1(3)+f2(3))]; +else + rt = r2R(-f2(3))*[f2(1:2); 1]; + f = r2Rt(-f2(3),-rt(1:2))*[f1(1:2); 1]; + f = [f(1:2); normAngle(f1(3)-f2(3))]; +end + +% Transform back to 4D state + +end + diff --git a/Math/Utils/gc2dec.m b/Math/Utils/gc2dec.m new file mode 100644 index 0000000..0882e0a --- /dev/null +++ b/Math/Utils/gc2dec.m @@ -0,0 +1,14 @@ +function gcode = gc2dec(dec) + +% Description: This function converts a decimal number to its equivalent +% gray code representation. +tmp = dec; idv = 1; +while 1 + dv = bitshift(tmp,-idv); + tmp = bitxor(tmp,dv); + if dv <= 1 || dv == 16 + break; + end + idv = bitshift(idv,1); +end +gcode = tmp; \ No newline at end of file diff --git a/Math/Utils/gc2dec.m~ b/Math/Utils/gc2dec.m~ new file mode 100644 index 0000000..91b116a --- /dev/null +++ b/Math/Utils/gc2dec.m~ @@ -0,0 +1,7 @@ +function [ dec ] = gc2dec(gcd) +%GC2DEC Returns the inverse of the Gray code representation of a decimal + +dec = bitxor(gcd,bitshift(gcd,1) + +end + diff --git a/Math/Utils/getCoordinates.m b/Math/Utils/getCoordinates.m new file mode 100644 index 0000000..51eea22 --- /dev/null +++ b/Math/Utils/getCoordinates.m @@ -0,0 +1,26 @@ +function [ ix iy ] = getCoordinates( p, A ) +%GETCOORDINATES given a point P on the grid map A, it returns its grid +%position. It uses a default values of definition of 0.1 and size +%0.1*size(A) + + +L = A.L; +xmin = A.xmin; +xmax = A.xmax; +ymin = A.ymin; +ymax = A.ymax; + +xwide = xmax - xmin; +ywide = ymax - ymin; +NX = floor(xwide / L)+1; +NY = floor(ywide / L)+1; + +x = p(1); +y = p(2); + +ix = min(floor( (-(xmin) + x) / L)+1,NX); %+ (abs(xmin) / L); +iy = min(floor( (-(ymin) + y) / L)+1,NY); %+ (abs(ymin) / L); + + +end + diff --git a/Math/Utils/gridShift.m b/Math/Utils/gridShift.m new file mode 100644 index 0000000..458ce88 --- /dev/null +++ b/Math/Utils/gridShift.m @@ -0,0 +1,27 @@ +function [ B ] = gridShift( A, x, y ) +%GRIDSHIFT Shift a matrix by x and y cells respectively in x and y +%direction. The rest is set to zero. The cells of A falling outside the +%size are lost. + +sizex = size(A,1); +sizey = size(A,2); + +B = zeros(sizex,sizey); + +for i=1:sizex + ii=i+x; + if ii>sizex || ii<=0 + continue + end + for j=1:sizey + jj = j + y; + if jj>sizey || jj<=0 + continue + end + + B(ii,jj) = A(i,j); + end +end + +end + diff --git a/Math/Utils/gridShift.m~ b/Math/Utils/gridShift.m~ new file mode 100644 index 0000000..569ad4a --- /dev/null +++ b/Math/Utils/gridShift.m~ @@ -0,0 +1,23 @@ +function [ A ] = gridShift( A, x, y ) +%GRIDSHIFT Summary of this function goes here +% Detailed explanation goes here + +sizex = size(A,1); +sizey = size(A,2); + +for i=1:sizex + if ii>sizex + continue + end + for j=1:sizey + yy = y + y; + if yy>sizey + continue + end + + A + end +end + +end + diff --git a/Math/Utils/inverse_transformation.m b/Math/Utils/inverse_transformation.m new file mode 100644 index 0000000..56bb361 --- /dev/null +++ b/Math/Utils/inverse_transformation.m @@ -0,0 +1,6 @@ +function [r] = inverse_transformation(p) + +cosy = cos(p(3)); siny = sin(p(3)); +r = [-p(1)*cosy-p(2)*siny; + p(1)*siny-p(2)*cosy; + -p(3)]; \ No newline at end of file diff --git a/Math/Utils/invert.m b/Math/Utils/invert.m new file mode 100644 index 0000000..1071400 --- /dev/null +++ b/Math/Utils/invert.m @@ -0,0 +1,13 @@ +function res = invert(p) + +x = p(1); y = p(2); +cosy = cos(p(3)); siny = sin(p(3)); + +res.x = [-p(1)*cosy - p(2)*siny; + p(1)*siny - p(2)*cosy; + -p(3)]; + +%Jacobian for the Inverse function (-)X +res.oJ = [-cosy, -siny, x*siny - y*cosy; + siny, -cosy, x*cosy + y*siny; + 0, 0, -1]; diff --git a/Math/Utils/max2d.m b/Math/Utils/max2d.m new file mode 100644 index 0000000..e753f72 --- /dev/null +++ b/Math/Utils/max2d.m @@ -0,0 +1,18 @@ +function [Val,Ind]=max2d(Mat); +%-------------------------------------------------------------------------- +% max2d function 2d maximum function. Return the maximum value +% and index in a 2d matrix. +% Input : - Matrix. +% Output : - Maximum value. +% - [I,J] index of maximum. +% Tested : Matlab 5.3 +% By : Eran O. Ofek October 2000 +% URL : http://wise-obs.tau.ac.il/~eran/matlab.html +%-------------------------------------------------------------------------- + +[V1,I1] = max(Mat); +[V2,I2] = max(V1); + +Val = V2; +Ind(1) = I1(I2); % I +Ind(2) = I2; % J \ No newline at end of file diff --git a/Math/Utils/mean_last.mat b/Math/Utils/mean_last.mat new file mode 100644 index 0000000000000000000000000000000000000000..1e303f9166f3b4d794f3aa94b292bfd1b2aa0a59 GIT binary patch literal 438 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i*PhE(NSP4;mcV1iP6G{ xmax + xmax = x; + end + + if x < xmin + xmin = x; + end + + if y < ymin + ymin = y; + end + + if y > ymax + ymax = y; + end +end + +end + diff --git a/Math/Utils/minmaxScan.m~ b/Math/Utils/minmaxScan.m~ new file mode 100644 index 0000000..6be6681 --- /dev/null +++ b/Math/Utils/minmaxScan.m~ @@ -0,0 +1,21 @@ +function [ xmin xmax ymin ymax ] = minmaxScan( S ) +%MINMAXSCAN return the minimumand maximum value of both components listed +%in a scan set. Returns 4 values representing the minimum x value, the +%maximum x value, the minimum y value and the maximum y value. + +xmax=inf; +xmin=-inf; +ymin=-inf; +ymax=inf; + +for i=1:size(S,1) + x=S(i,1); + y=S(i,2); + + if x > xmax + + +end + +end + diff --git a/Math/Utils/multRow.m b/Math/Utils/multRow.m new file mode 100644 index 0000000..c478562 --- /dev/null +++ b/Math/Utils/multRow.m @@ -0,0 +1,8 @@ +%Add to every row + +function M = multRow(A,v) + + for i = 1:size(A,1) + M(i,:) = A(i,:)*v; + end +end diff --git a/Math/Utils/normAngle.m b/Math/Utils/normAngle.m new file mode 100644 index 0000000..648036b --- /dev/null +++ b/Math/Utils/normAngle.m @@ -0,0 +1,61 @@ +function ao = normAngle(ai,deg) + +% NORMANGLE Normalize angle to (-pi .. pi] values. +% NORMANGLE(A) brings angle A to the range (-pi .. pi] by adding an +% integer number of turns. +% +% NORMANGLE(A,DEG) with DEG ~= false, uses degrees instead of radians, +% and thus the angle A is normalized to the range (-180 .. 180]. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +if nargin < 2 + deg = false; +end + +if deg + halfTurn = 180; +else + halfTurn = pi; +end + +ao = ai; +while any(ao <= -halfTurn) + ao(ao <= -halfTurn) = ao(ao <= -halfTurn) + 2*halfTurn; +end +while any(ao > halfTurn) + ao(ao > halfTurn) = ao(ao > halfTurn) - 2*halfTurn; +end + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Utils/normvec.m b/Math/Utils/normvec.m new file mode 100644 index 0000000..6117b2a --- /dev/null +++ b/Math/Utils/normvec.m @@ -0,0 +1,74 @@ +function [vn,VN_v] = normvec(v,jacMethod) + +% NORMVEC Normalize vector. +% NORMVEC(V) is the unit length vector in the same direction and sense as +% V. It is equal to V/norm(V). +% +% [NV,NV_v] = NORMVEC(V) returns the Jacobian of the normed vector wrt V. +% +% [NV,NV_v] = NORMVEC(V,method) , with method ~= 0, uses a scalar diagonal +% matrix as Jacobian, meaning that the vector has been just scaled to +% length one by a scalar factor. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +n2 = dot(v,v); +n = sqrt(n2); + +vn = v/n; + +if nargout > 1 + s = numel(v); + + if nargin > 1 && jacMethod ~= 0 % use scalar method (approx) + VN_v = eye(s)/n; + + else % use full vector method (exact) + n3 = n*n2; + VN_v = (n2*eye(s) - v*v')/n3; + + end + +end + +return + +%% +syms v1 v2 v3 v4 v5 real +v = [v1;v2;v3;v4;v5]; +[vn,VN_v] = normvec(v); + +simplify(VN_v - jacobian(vn,v)) + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Utils/odoPolar2Cart.m b/Math/Utils/odoPolar2Cart.m new file mode 100644 index 0000000..f1d4d91 --- /dev/null +++ b/Math/Utils/odoPolar2Cart.m @@ -0,0 +1,8 @@ +function [ u2 ] = odoPolar2Cart( u ) +%ODOPOLAR2CART transform an odometry in polar format to cartesian format + +u2(1) = cos(u(1))*u(2); +u2(2) = sin(u(1))*u(2); + +end + diff --git a/Math/Utils/perturbate3Dpoint.m b/Math/Utils/perturbate3Dpoint.m new file mode 100644 index 0000000..7a00c3f --- /dev/null +++ b/Math/Utils/perturbate3Dpoint.m @@ -0,0 +1,14 @@ +%Return a point perturbated by a Gaussian distributed value + +function ppoint = perturbate3Dpoint(p,m,N) + +%Return a 3D vector with added noise derived from three independent +%randomly generated deviations from Gaussian distribution. The std +%deviation of Gaussian are stored in the vector N and have mean m + + stdmatrix = [ stdErr(m(1),N(1)); ... + stdErr(m(2),N(2)); ... + stdErr(m(3),N(3)) ]; + ppoint = p + stdmatrix'; + +end \ No newline at end of file diff --git a/Math/Utils/perturbatePoint.m b/Math/Utils/perturbatePoint.m new file mode 100644 index 0000000..52aef4d --- /dev/null +++ b/Math/Utils/perturbatePoint.m @@ -0,0 +1,13 @@ +%Return a point perturbated by a Gaussian distributed value + +function ppoint = perturbatePoint(p,m,N) + +%Return a point with added noise derived from two independent +%randomly generated deviations from Gaussian distribution. The std +%deviation of Gaussian are stored in the vector N and have mean m + + stdmatrix = [ stdErr(m(1),N(1)) ... + stdErr(m(2),N(2)) ]; + ppoint = p + stdmatrix; + +end \ No newline at end of file diff --git a/Math/Utils/polyadd.m b/Math/Utils/polyadd.m new file mode 100644 index 0000000..6535f71 --- /dev/null +++ b/Math/Utils/polyadd.m @@ -0,0 +1,18 @@ +function[poly]=polyadd(poly1,poly2) + %Copyright 1996 Justin Shriver + %polyadd(poly1,poly2) adds two polynominals possibly of uneven length + if length(poly1)0 + poly=[zeros(1,mz),short]+long; + else + poly=long+short; + end + +end \ No newline at end of file diff --git a/Math/Utils/qpose2epose.m b/Math/Utils/qpose2epose.m new file mode 100644 index 0000000..fac743c --- /dev/null +++ b/Math/Utils/qpose2epose.m @@ -0,0 +1,55 @@ +function [ep,EPq] = qpose2epose(qp) + +% QPOSE2EPOSE Quaternion-specified to Euler-specified pose conversion. +% +% EP = QPOSE2EPOSE(QP) returns a full 6-pose EP=[X;E] from a full 7-pose +% QP=[X;Q] where X is 3D opsition and Q and E are 3D orientations. +% +% [EP,Jqp] = QPOSE2EPOSE(...) returns also the Jacobian matrix. +% +% See also EPOSE2QPOSE, EULERANGLES, QUATERNION, FRAME. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + +if any(size(qp) ~= [7,1]) + warning('Input pose should be a column 7-vector') +end + +ep = zeros(6,1); +ep(1:3) = qp(1:3); +P = eye(3); +[ep(4:6),Eq] = q2e(qp(4:7)); +EPq = [P zeros(3,4);zeros(3) Eq]; + + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Utils/rad2deg.m b/Math/Utils/rad2deg.m new file mode 100644 index 0000000..cc8d472 --- /dev/null +++ b/Math/Utils/rad2deg.m @@ -0,0 +1,40 @@ +function deg = rad2deg(rad) + +% RAD2DEG Radians to degrees conversion. + +% Copyright 2008-2009 Joan Sola @ LAAS-CNRS. + + +deg = rad/pi*180; + + +% ========== End of function - Start GPL license ========== + + +% # START GPL LICENSE + +%--------------------------------------------------------------------- +% +% This file is part of SLAMTB, a SLAM toolbox for Matlab. +% +% SLAMTB is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% SLAMTB is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with SLAMTB. If not, see . +% +%--------------------------------------------------------------------- + +% SLAMTB is Copyright 2007,2008,2009 +% by Joan Sola, David Marquez and Jean Marie Codol @ LAAS-CNRS. +% See on top of this file for its particular copyright. + +% # END GPL LICENSE + diff --git a/Math/Utils/searchPair.m b/Math/Utils/searchPair.m new file mode 100644 index 0000000..2a0bb7e --- /dev/null +++ b/Math/Utils/searchPair.m @@ -0,0 +1,13 @@ +function [ j ] = searchPair( A, P ) +%SEARCHPAIR Search a pair P of numbers in the array A. They must be +%consecutive. Returns I as the position within the array + +for j = 1:size(A,1) + if (P(1) == A(j,1) || P(2) == A(j,2)) &&... + (P(2) == A(j,1) || P(1) == A(j,2)) + return + end +end +j = -1; +end + diff --git a/Math/Utils/stdErr.m b/Math/Utils/stdErr.m new file mode 100644 index 0000000..1ccb50e --- /dev/null +++ b/Math/Utils/stdErr.m @@ -0,0 +1,27 @@ +%Return en error from a standard distribution + +function rerr = stdErr(m,N) + +%Return a random variable from a Gaussian with mean m and std deviation N +%using the Box-Muller transform + +rerr = 0; + +if(N==0) + rerr=0; + return; +end + +while rerr == 0 + +u1 = randn; +u2 = randn; + +z1 = sqrt(-2 * log(u1)) * sin(2 * pi * u2); +z2 = sqrt(-2 * log(u1)) * cos(2 * pi * u2); + +%x1 = mu + z1 * sigma; +rerr = real(m + z2 * N); +end + +end \ No newline at end of file diff --git a/Math/Utils/transPolarCartScan.m b/Math/Utils/transPolarCartScan.m new file mode 100644 index 0000000..e963ec1 --- /dev/null +++ b/Math/Utils/transPolarCartScan.m @@ -0,0 +1,289 @@ +function [ B Bf Af aixs bixs] = transPolarCartScan( Bl, Al, R, t, dir, threshold ) +%TRANSPOLARCARTSCAN Summary of this function goes here +% Detailed explanation goes here +% B referenced to A coordinate system + +x = t(1); +y = t(2); +yaw = R; + +%Bf = Bl; +Af = Al; + +aixs=[]; +bixs=[]; +bixs2=[]; +diffbixs = []; +diffbixs2 = []; +threshold = threshold * 1; + +global Opt; + + +switch(dir) + + case 1 + if Opt.scanmatcher.projfilter == 1 + irtn = inverse_transformation([x y yaw]); + irTn = [cos(irtn(3)) -sin(irtn(3)) irtn(1); sin(irtn(3)) cos(irtn(3)) irtn(2); 0 0 1]; + + for i = 1:size(Al.cart,1) + tmpc = (irTn*[Al.cart(i,1:2) 1]')' ; + [Ab.polar(i,1) Ab.polar(i,2)] = cart2pol(tmpc(1), tmpc(2)); + end + end + rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; + + sizeB = size(Bl.cart,1); + + %B referenced to A coordinate system + for i = 1:sizeB + B.cart(i,1:3) = (rTn*[Bl.cart(i,1:2) 1]')' ; + %B.polar(i,1:2) = [normAngle(B.polar(i,1) + yaw) B.polar(i,2)]; + th = normAngle(Bl.polar(i,1) + yaw); + ro = Bl.polar(i,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + B.polar(i,2) = sqrt(rc^2 + rs^2); + B.polar(i,1) = atan2(rs,rc); + + % bp1 = [x y]; + % bp2 = B.cart(i,:); + + %% Search for occluded points from the reference pose + if Opt.scanmatcher.projfilter == 1 + for rp =1:size(Al.polar,1) + + % ap1 = Al.cart(rp,:); + % ap2 = Al.cart(rp-1,:); + + %[sint p di ] = segseginter(bp1,bp2,ap1,ap2); + dangle = abs( normAngle(Al.polar(rp,1) - B.polar(i,1)) ); + + if( isempty(find(aixs==rp,1)) && dangle < 0.05 ) + %if di > threshold + if abs(Af.polar(rp,2) - B.polar(i,2) ) > threshold*0.3 + if(Af.polar(rp,2) > B.polar(i,2) ) + + else + bixs = [bixs; i]; + diffbixs = [diffbixs; Af.polar(rp,2) - B.polar(i,2)]; + break + end + %else + + end + end + end + + + + %% Search for occluded points from the new pose + for rp =1:size(Ab.polar,1) + + dangle = abs( normAngle(Ab.polar(rp,1) - Bl.polar(i,1)) ); + + if( isempty(find(aixs==rp,1)) && dangle < 0.05 ) + if abs(Ab.polar(rp,2) - Bl.polar(i,2) ) > threshold*0.3 + + + if(Ab.polar(rp,2) > Bl.polar(i,2) ) + bixs2=[bixs2 i]; + % + % ibixs = find(bixs==i,1); + diffbixs2 = Ab.polar(rp,2) - Bl.polar(i,2) ; + + % if ~isempty(ibixs) + % %if sign(diffbixs2) == sign(diffbixs(ibixs)) + % bixs2 = [bixs2; i ]; + % % diffbixs2(end) = []; + % %end + % end + break + else + % + % % ibixs = find(bixs==i,1); + % % diffbixs2 = Ab.polar(rp,2) - Bl.polar(i,2) ; + % % + % % if ~isempty(ibixs) + % % if sign(diffbixs2) == sign(diffbixs(ibixs)) + % % bixs2(end) = []; + % % % diffbixs2(end) = []; + % % end + % % end + end + %else + + end + end + end + end + + + end + % diffbixs2 = abs(diffbixs2); + % bixs2(diffbixs2 threshold + if abs(Af.polar(2,rp) - B.polar(2,i) ) > threshold*0.2 + if(Af.polar(2,rp) > B.polar(2,i) ) + + else + bixs = [bixs; i]; + diffbixs = [diffbixs; Af.polar(2,rp) - B.polar(2,i)]; + break + end + %else + + end + end + end + + + %% Search for occluded points from the new pose + for rp =1:size(Ab.polar,2) + + dangle = abs( normAngle(Ab.polar(1,rp) - Bl.polar(1,i)) ); + + if( isempty(find(aixs==rp,1)) && dangle < 0.05 ) + if abs(Ab.polar(2,rp) - Bl.polar(2,i) ) > threshold*0.2 + + + if(Ab.polar(2,rp) > Bl.polar(2,i) ) + bixs2=[bixs2 i]; + % + % ibixs = find(bixs==i,1); + diffbixs2 = Ab.polar(2,rp) - Bl.polar(2,i) ; + + % if ~isempty(ibixs) + % %if sign(diffbixs2) == sign(diffbixs(ibixs)) + % bixs2 = [bixs2; i ]; + % % diffbixs2(end) = []; + % %end + % end + break + else + % + % % ibixs = find(bixs==i,1); + % % diffbixs2 = Ab.polar(rp,2) - Bl.polar(i,2) ; + % % + % % if ~isempty(ibixs) + % % if sign(diffbixs2) == sign(diffbixs(ibixs)) + % % bixs2(end) = []; + % % % diffbixs2(end) = []; + % % end + % % end + end + %else + + end + end + end + + end + + end + % diffbixs2 = abs(diffbixs2); + % bixs2(diffbixs2 threshold + %if abs(Af.polar(rp,2) - B.polar(i,2) ) > threshold + if(Af.polar(rp,2) > B.polar(i,2) ) + + else + bixs = [bixs; i]; + diffbixs = [diffbixs; Af.polar(rp,2) - B.polar(i,2)]; + break + end + %else + + %end + end + end + + + + %% Search for occluded points from the new pose + for rp =1:size(Ab.polar,1) + + dangle = abs( normAngle(Ab.polar(rp,1) - Bl.polar(i,1)) ); + + if( isempty(find(aixs==rp,1)) && dangle < 0.05 ) + %if abs(Ab.polar(rp,2) - Bl.polar(i,2) ) > threshold + + + if(Ab.polar(rp,2) > Bl.polar(i,2) ) + bixs2=[bixs2 i]; + % + % ibixs = find(bixs==i,1); + diffbixs2 = Ab.polar(rp,2) - Bl.polar(i,2) ; + + % if ~isempty(ibixs) + % %if sign(diffbixs2) == sign(diffbixs(ibixs)) + % bixs2 = [bixs2; i ]; + % % diffbixs2(end) = []; + % %end + % end + break + else + % + % % ibixs = find(bixs==i,1); + % % diffbixs2 = Ab.polar(rp,2) - Bl.polar(i,2) ; + % % + % % if ~isempty(ibixs) + % % if sign(diffbixs2) == sign(diffbixs(ibixs)) + % % bixs2(end) = []; + % % % diffbixs2(end) = []; + % % end + % % end + end + %else + + %end + end + end + + + + end +% diffbixs2 = abs(diffbixs2); +% bixs2(diffbixs2 threshold + if(Af.polar(2,rp) > B.polar(2,i) ) + else + mthreshold = 1*mthreshold; + end + else + mthreshold = 0; + end + end + end + if mthreshold + bixs = [bixs; i]; + end + end + + for i = 1:size(B.polar,2) + if isempty(find(bixs == i, 1)) + for j = 1:size(B.polar,2) + if abs(normAngle(B.polar(1,j)) - B.polar(1,i)) < 0.001 && i~=j + [mm ii] = min( [ B.polar(2,j) B.polar(2,i) ]); + oi = j; + if ii == 2 + oi = i; + end + bixs = [bixs; oi]; + continue; + end + end + end + end + + Bf=B; + Bf.cart(:,bixs)= []; + Bf.polar(:,bixs)= []; + Af.cart(:,aixs)= []; + Af.polar(:,aixs)= []; +end + + diff --git a/Math/Utils/transPolarCartScan3.m b/Math/Utils/transPolarCartScan3.m new file mode 100644 index 0000000..a8441d9 --- /dev/null +++ b/Math/Utils/transPolarCartScan3.m @@ -0,0 +1,201 @@ +function [ B Bf Af aixs bixs] = transPolarCartScan( Bl, Al, R, t, dir, threshold ) +%TRANSPOLARCARTSCAN Summary of this function goes here +% Detailed explanation goes here +% B referenced to A coordinate system + +x = t(1); +y = t(2); +yaw = R; + +%Bf = Bl; +Af = Al; + +aixs=[]; +bixs=[]; +bixs2=[]; +diffbixs = []; +diffbixs2 = []; +threshold = threshold * 1.5; + +global Opt; + +irtn = inverse_transformation([x y yaw]); +irTn = [cos(irtn(3)) -sin(irtn(3)) irtn(1); sin(irtn(3)) cos(irtn(3)) irtn(2); 0 0 1]; + +% for i = 1:size(Al.cart,1) +% tmpc = (irTn*[Al.cart(i,1:2) 1]')' ; +% [Ab.polar(i,1) Ab.polar(i,2)] = cart2pol(tmpc(1), tmpc(2)); +% end + +switch(dir) + + case 1 + + rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; + + sizeB = size(Bl.cart,1); + + %B referenced to A coordinate system + for i = 1:sizeB + B.cart(i,1:3) = (rTn*[Bl.cart(i,1:2) 1]')' ; + %B.polar(i,1:2) = [normAngle(B.polar(i,1) + yaw) B.polar(i,2)]; + th = normAngle(Bl.polar(i,1) + yaw); + ro = Bl.polar(i,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + B.polar(i,2) = sqrt(rc^2 + rs^2); + B.polar(i,1) = atan2(rs,rc); + +% bp1 = [x y]; +% bp2 = B.cart(i,:); + + mthreshold = 1; + for rp =1:size(Al.polar,1) + +% ap1 = Al.cart(rp,:); +% ap2 = Al.cart(rp-1,:); + + %[sint p di ] = segseginter(bp1,bp2,ap1,ap2); + dangle = abs( normAngle(Al.polar(rp,1) - B.polar(i,1)) ); + + if( isempty(find(aixs==rp,1)) && dangle < 0.1 ) + %if di > threshold + if abs(Af.polar(rp,2) - B.polar(i,2) ) > threshold + if(Af.polar(rp,2) > B.polar(i,2) ) + + else + mthreshold = 1*mthreshold; + end + else + mthreshold = 0; + end + end + end + + if mthreshold + bixs = [bixs; i]; + diffbixs = [diffbixs; Af.polar(rp,2) - B.polar(i,2) ]; + end + +% +% mthreshold = 1; +% for rp =1:size(Ab.polar,1) +% +% % ap1 = Al.cart(rp,:); +% % ap2 = Al.cart(rp-1,:); +% +% %[sint p di ] = segseginter(bp1,bp2,ap1,ap2); +% dangle = abs( normAngle(Ab.polar(rp,1) - Bl.polar(i,1)) ); +% +% if( isempty(find(aixs==rp,1)) && dangle < 0.05 ) +% %if di > threshold +% if abs(Ab.polar(rp,2) - Bl.polar(i,2) ) > threshold +% if(Ab.polar(rp,2) > Bl.polar(i,2) ) +% +% else +% mthreshold = 1*mthreshold; +% end +% else +% mthreshold = 0; +% end +% end +% end +% +% if mthreshold +% bixs2 = [bixs2; i]; +% diffbixs2 = [diffbixs2; Ab.polar(rp,2) - Bl.polar(i,2) ]; +% ibixs = find(bixs==i,1); +% if ~isempty(ibixs) +% if sign(diffbixs2(end)) == sign(diffbixs(ibixs)) +% bixs2(end) = []; +% diffbixs2(end) = []; +% end +% end +% end + + + + end +% bixs = bixs2; + + for i = 1:size(B.polar,1) + if isempty(find(bixs == i, 1)) + for j = 1:size(B.polar,1) + if abs(normAngle(B.polar(j,1)) - B.polar(i,1)) < 0.001 && i~=j + [mm ii] = min( [ B.polar(j,2) B.polar(i,2) ]); + oi = j; + if ii == 2 + oi = i; + end + bixs = [bixs; oi]; + continue; + end + end + end + end + + Bf=B; + Bf.cart(bixs,:)= []; + Bf.polar(bixs,:)= []; + Af.cart(aixs,:)= []; + Af.polar(aixs,:)= []; + + case 2 + + rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; + + sizeB = size(Bl.cart,2); + + for i = 1:sizeB + B.cart(1:3,i) = (rTn*[Bl.cart(1:2,i); 1]) ; + th = normAngle(Bl.polar(1,i) + yaw); + ro = Bl.polar(2,i); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + B.polar(2,i) = sqrt(rc^2 + rs^2); + B.polar(1,i) = atan2(rs,rc); + + mthreshold = 1; + for rp =1:size(Al.polar,2) + if( isempty(find(aixs==rp,1)) && abs(normAngle(Al.polar(1,rp)) - B.polar(1,i)) < 0.05 ) + if abs(Af.polar(2,rp) - B.polar(2,i) ) > threshold + if(Af.polar(2,rp) > B.polar(2,i) ) + else + mthreshold = 1*mthreshold; + end + else + mthreshold = 0; + end + end + end + if mthreshold + bixs = [bixs; i]; + end + end + + for i = 1:size(B.polar,2) + if isempty(find(bixs == i, 1)) + for j = 1:size(B.polar,2) + if abs(normAngle(B.polar(1,j)) - B.polar(1,i)) < 0.001 && i~=j + [mm ii] = min( [ B.polar(2,j) B.polar(2,i) ]); + oi = j; + if ii == 2 + oi = i; + end + bixs = [bixs; oi]; + continue; + end + end + end + end + + Bf=B; + Bf.cart(:,bixs)= []; + Bf.polar(:,bixs)= []; + Af.cart(:,aixs)= []; + Af.polar(:,aixs)= []; +end + + diff --git a/Math/minmaxScan.m b/Math/minmaxScan.m new file mode 100644 index 0000000..81edb87 --- /dev/null +++ b/Math/minmaxScan.m @@ -0,0 +1,33 @@ +function [ xmin xmax ymin ymax ] = minmaxScan( S ) +%MINMAXSCAN return the minimumand maximum value of both components listed +%in a scan set. Returns 4 values representing the minimum x value, the +%maximum x value, the minimum y value and the maximum y value. + +xmax=-inf; +xmin=inf; +ymin=inf; +ymax=-inf; + +for i=1:size(S,1) + x=S(i,1); + y=S(i,2); + + if x > xmax + xmax = x; + end + + if x < xmin + xmin = x; + end + + if y < ymin + ymin = y; + end + + if y > ymax + ymax = y; + end +end + +end + diff --git a/ScanMatching/2D/AngleHistogram/ahsm.m b/ScanMatching/2D/AngleHistogram/ahsm.m new file mode 100644 index 0000000..a458cd2 --- /dev/null +++ b/ScanMatching/2D/AngleHistogram/ahsm.m @@ -0,0 +1,218 @@ +function [ R t NI] = ahsm( ScanRef, ScanNew, motion ) +%ahsm This methods uses a one dimensional signal-type representation of the scan +% points. The vector delimited by every two consecutive +% points of the scan is computed and represented in polar +% coordinates with respect to the global frame of reference. +% Then, a histogram of their angles is computed as a 1D +% signal which is invariant to translation but not to rotation. +% Given two rotated scans, the rotation angle can be +% computed as the signal shift which maximizes the cross +% correlation of the corresponding scan-signals. Once the +% rotation has been solved, the same procedure may be +% applied for the x and y to solve for the translation. + + +% Keeping Pack of Position and Orientation +% of Moving Indoor Systems +% by Correlation of Range-Finder Scans +% Gerhard We$, Christopher Wetzler, Ewald von Puttkamer + +global Opt; +%Select which points to use, in this case local cartesian point in +Al.cart = ScanRef.localCart; +Al.polar = ScanRef.localPolar; + +Bl.cart = ScanNew.localCart; +Bl.polar = ScanNew.localPolar; + +%Calculate the motion parameters q0 +u = motion.con.u; + + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; +NI = 1; + + + +% Apply the initial estimaion q +[B Bf Af] = transPolarCartScan(Bl, Al, yaw, [x y], 1, Opt.scanmatcher.Br(2)); + +% Apply projection filter? +if Opt.scanmatcher.projfilter + BA = Bf; + AlA = Af; +else + BA = B; + AlA = Al; +end + +global DEBUG +if DEBUG.ahsm || DEBUG.all + + figure; + hold on + axis equal + grid on + displayPoints(BA.cart,'g',0); + displayPoints(AlA.cart,'r',0); + +end + + t = [0 0 0]; + R = e2q([0 0 0]); + +% Generate the Angle histogram of both scans and normalize +Bh = makeAnglehistogram(BA.cart,yaw0); +Ah = makeAnglehistogram(AlA.cart,yaw0); +Bh = Bh./max(Bh); +Ah = Ah./max(Ah); + +if DEBUG.ahsm || DEBUG.all + figure + plot(1:size(Bh,2),Bh); + figure + plot(1:size(Ah,2),Ah); +end + +% Circular cross correlation and shift to have the 0 frequency in the +% middle +corrBA=zeros(1,size(Ah,2)); +for j=1:size(Ah,2) + for k=1:size(Bh,2) + corrBA(j) = corrBA(j) + Bh(k)*Ah( mod(j+k,size(Ah,2))+1 ); + end +end +sizec = size(corrBA,2); +corrBA = arrayshift(corrBA,round(sizec/2)); + +% Normalize the result and cut off the borders which usually contains +% artifacts +[mm ii] = max(corrBA); +corrBA=(corrBA.*hann(length(corrBA))' )./mm; + +% corrBA(1:30) = 0; +% corrBA(sizec-30:end) = 0; + +% Applying a fast smoothing +corrBASmooth = fastsmooth(corrBA,10); +if DEBUG.ahsm || DEBUG.all + figure + plot(1:size(corrBA,2),corrBA); +end + +% Locate local maxima and extract rotational hypotheses +maxs = findLocalMaxima(corrBASmooth,3); +angle_hyp = deg2rad((maxs - round(size(corrBA,2)/2) )) + +% Locate local maxima in the reference scan to extract the direction of +% correction +maxb = findLocalMaxima(Bh,1); +maxb = deg2rad((maxb - round(size(Bh,2)/2) )); + + +BAt = BA; +AAt = AlA; + +for q = size(maxb,1) + + % Transform both scan according to the direction of correction + [BAt] = transPolarCartScan(BAt, AlA, normAngle(maxb(q)+angle_hyp(1)), [0 0] , 1,Opt.scanmatcher.Br(2)); + [AAt] = transPolarCartScan(AAt, AlA, maxb(q) , [0 0] , 1,Opt.scanmatcher.Br(2)); + + if DEBUG.ahsm || DEBUG.all + + figure; + hold on + axis equal + grid on + displayPoints(BAt.cart,'g',0); + displayPoints(AAt.cart,'r',0); + + end + +bounds = [ max(max(BAt.cart(:,1)),min(AAt.cart(:,1))), min(min(BAt.cart(:,1)),min(AAt.cart(:,1))); + max(max(BAt.cart(:,2)),min(AAt.cart(:,2))), min(min(BAt.cart(:,2)),min(AAt.cart(:,2)))]; + + + % Create the Axis histogram for the X and Y components of the points + bxhist = makeAxishistogram(BAt.cart,'x',bounds); + byhist = makeAxishistogram(BAt.cart,'y',bounds); + + axhist = makeAxishistogram(AAt.cart,'x',bounds); + ayhist = makeAxishistogram(AAt.cart,'y',bounds); + + + % smooth the histograms and do the cross correlation +% bxhist =fastsmooth(bxhist,2); +% axhist =fastsmooth(axhist,2); +% byhist =fastsmooth(byhist,2); +% ayhist =fastsmooth(ayhist,2); + + corrX=zeros(1,size(bxhist,2)); + for j=1:size(bxhist,2) + for k=1:size(axhist,2) + corrX(j) = corrX(j) + axhist(k)*bxhist( mod(j+k,size(bxhist,2))+1 ); + end + end + + corrY=zeros(1,size(byhist,2)); + + + for j=1:size(byhist,2) + for k=1:size(ayhist,2) + corrY(j) = corrY(j) + ayhist(k)*byhist( mod(j+k,size(byhist,2))+1 ); + end + end + + corrY = circularCrossCorrelation(byhist,ayhist,1); + corrX = circularCrossCorrelation(bxhist,axhist,1); + + if DEBUG.ahsm || DEBUG.all + figure + subplot(2,1,1);plot(1:size(bxhist,2),bxhist); + subplot(2,1,2);plot(1:size(axhist,2),axhist); + figure + subplot(2,1,1);plot(1:size(byhist,2),byhist); + subplot(2,1,2);plot(1:size(ayhist,2),ayhist); + end + + % shift the results to have the 0 frequency in the middle + midvx = round( size(corrX,2) / 2 ); + corrX = arrayshift(corrX,midvx); + + midvy = round( size(corrY,2) / 2 ); + corrY = arrayshift(corrY,midvy); + + + if DEBUG.ahsm || DEBUG.all + figure + plot(1:size(corrX,2),corrX); + figure + plot(1:size(corrY,2),corrY); + end + + + % Find the translation from the local maxima of the cross correlation + % bouding the result according to the user input Opt.scanmatcher.Br(2) + Y = (findLocalMaxima(corrX,2) - midvx )*(Opt.scanmatcher.Br(2)/(midvx*2)); + X = (findLocalMaxima(corrY,2) - midvy )*(Opt.scanmatcher.Br(2)/(midvy*2)); + + if isempty(X) || isempty(Y) + return + end + + yaw = -maxb(q); + rt = [cos(yaw) -sin(yaw); sin(yaw) cos(yaw)]; + + % Finally Q = [t R] + t = (rt*[X(1); Y(1)])' ; + R = -angle_hyp(1); +end + +end diff --git a/ScanMatching/2D/AngleHistogram/ahsm.m~ b/ScanMatching/2D/AngleHistogram/ahsm.m~ new file mode 100644 index 0000000..52303e0 --- /dev/null +++ b/ScanMatching/2D/AngleHistogram/ahsm.m~ @@ -0,0 +1,215 @@ +function [ R t NI] = ahsm( ScanRef, ScanNew, motion ) +%ahsm This methods uses a one dimensional signal-type representation of the scan +% points. The vector delimited by every two consecutive +% points of the scan is computed and represented in polar +% coordinates with respect to the global frame of reference. +% Then, a histogram of their angles is computed as a 1D +% signal which is invariant to translation but not to rotation. +% Given two rotated scans, the rotation angle can be +% computed as the signal shift which maximizes the cross +% correlation of the corresponding scan-signals. Once the +% rotation has been solved, the same procedure may be +% applied for the x and y to solve for the translation. + + +% Keeping Pack of Position and Orientation +% of Moving Indoor Systems +% by Correlation of Range-Finder Scans +% Gerhard We$, Christopher Wetzler, Ewald von Puttkamer + +global Opt; +%Select which points to use, in this case local cartesian point in +Al.cart = ScanRef.localCart; +Al.polar = ScanRef.localPolar; + +Bl.cart = ScanNew.localCart; +Bl.polar = ScanNew.localPolar; + +%Calculate the motion parameters q0 +u = motion.con.u; + +lrangle = u(2); +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; +NI = 1; + + + +% Apply the initial estimaion q +[B Bf Af] = transPolarCartScan(Bl, Al, yaw, [x y], 1, Opt.scanmatcher.Br(2)); + +% Apply projection filter? +if Opt.scanmatcher.projfilter + BA = Bf; + AlA = Af; +else + BA = B; + AlA = Al; +end + +global DEBUG +if DEBUG.ahsm || DEBUG.all + + figure; + hold on + axis equal + grid on + displayPoints(BA.cart,'g',0); + displayPoints(AlA.cart,'r',0); + +end + + t = [(rt*[X(1); Y(1)])' 0]; + R = e2q([0 0 -angle_hyp(1)]); + +% Generate the Angle histogram of both scans and normalize +Bh = makeAnglehistogram(BA.cart,yaw0); +Ah = makeAnglehistogram(AlA.cart,yaw0); +Bh = Bh./max(Bh); +Ah = Ah./max(Ah); + +if DEBUG.ahsm || DEBUG.all + figure + plot(1:size(Bh,2),Bh); + figure + plot(1:size(Ah,2),Ah); +end + +% Circular cross correlation and shift to have the 0 frequency in the +% middle +corrBA=zeros(1,size(Ah,2)); +for j=1:size(Ah,2) + for k=1:size(Bh,2) + corrBA(j) = corrBA(j) + Bh(k)*Ah( mod(j+k,size(Ah,2))+1 ); + end +end +sizec = size(corrBA,2); +corrBA = arrayshift(corrBA,round(sizec/2)); + +% Normalize the result and cut off the borders which usually contains +% artifacts +[mm ii] = max(corrBA); +corrBA=(corrBA.*hann(length(corrBA))' )./mm; + +% corrBA(1:30) = 0; +% corrBA(sizec-30:end) = 0; + +% Applying a fast smoothing +corrBASmooth = fastsmooth(corrBA,10); +if DEBUG.ahsm || DEBUG.all + figure + plot(1:size(corrBA,2),corrBA); +end + +% Locate local maxima and extract rotational hypotheses +maxs = findLocalMaxima(corrBASmooth,3); +angle_hyp = deg2rad((maxs - round(size(corrBA,2)/2) )) + +% Locate local maxima in the reference scan to extract the direction of +% correction +maxb = findLocalMaxima(Bh,1); +maxb = deg2rad((maxb - round(size(Bh,2)/2) )); + + +BAt = BA; +AAt = AlA; + +for q = size(maxb,1) + + % Transform both scan according to the direction of correction + [BAt] = transPolarCartScan(BAt, AlA, normAngle(maxb(q)+angle_hyp(1)), [0 0] , 1,Opt.scanmatcher.Br(2)); + [AAt] = transPolarCartScan(AAt, AlA, maxb(q) , [0 0] , 1,Opt.scanmatcher.Br(2)); + + if DEBUG.ahsm || DEBUG.all + + figure; + hold on + axis equal + grid on + displayPoints(BAt.cart,'g',0); + displayPoints(AAt.cart,'r',0); + + end + +bounds = [ max(max(BAt.cart(:,1)),min(AAt.cart(:,1))), min(min(BAt.cart(:,1)),min(AAt.cart(:,1))); + max(max(BAt.cart(:,2)),min(AAt.cart(:,2))), min(min(BAt.cart(:,2)),min(AAt.cart(:,2)))]; + + + % Create the Axis histogram for the X and Y components of the points + bxhist = makeAxishistogram(BAt.cart,'x',bounds); + byhist = makeAxishistogram(BAt.cart,'y',bounds); + + axhist = makeAxishistogram(AAt.cart,'x',bounds); + ayhist = makeAxishistogram(AAt.cart,'y',bounds); + + + % smooth the histograms and do the cross correlation +% bxhist =fastsmooth(bxhist,2); +% axhist =fastsmooth(axhist,2); +% byhist =fastsmooth(byhist,2); +% ayhist =fastsmooth(ayhist,2); + + corrX=zeros(1,size(bxhist,2)); + for j=1:size(bxhist,2) + for k=1:size(axhist,2) + corrX(j) = corrX(j) + axhist(k)*bxhist( mod(j+k,size(bxhist,2))+1 ); + end + end + + corrY=zeros(1,size(byhist,2)); + + + for j=1:size(byhist,2) + for k=1:size(ayhist,2) + corrY(j) = corrY(j) + ayhist(k)*byhist( mod(j+k,size(byhist,2))+1 ); + end + end + + corrY = circularCrossCorrelation(byhist,ayhist,1); + corrX = circularCrossCorrelation(bxhist,axhist,1); + + if DEBUG.ahsm || DEBUG.all + figure + subplot(2,1,1);plot(1:size(bxhist,2),bxhist); + subplot(2,1,2);plot(1:size(axhist,2),axhist); + figure + subplot(2,1,1);plot(1:size(byhist,2),byhist); + subplot(2,1,2);plot(1:size(ayhist,2),ayhist); + end + + % shift the results to have the 0 frequency in the middle + midvx = round( size(corrX,2) / 2 ); + corrX = arrayshift(corrX,midvx); + + midvy = round( size(corrY,2) / 2 ); + corrY = arrayshift(corrY,midvy); + + + if DEBUG.ahsm || DEBUG.all + figure + plot(1:size(corrX,2),corrX); + figure + plot(1:size(corrY,2),corrY); + end + + + % Find the translation from the local maxima of the cross correlation + % bouding the result according to the user input Opt.scanmatcher.Br(2) + Y = (findLocalMaxima(corrX,2) - midvx )*(Opt.scanmatcher.Br(2)/(midvx*2)); + X = (findLocalMaxima(corrY,2) - midvy )*(Opt.scanmatcher.Br(2)/(midvy*2)); + + + yaw = -maxb(q); + rt = [cos(yaw) -sin(yaw); sin(yaw) cos(yaw)]; + + % Finally Q = [t R] + t = [(rt*[X(1); Y(1)])' 0]; + R = e2q([0 0 -angle_hyp(1)]); +end + +end diff --git a/ScanMatching/2D/AngleHistogram/makeAnglehistogram.m b/ScanMatching/2D/AngleHistogram/makeAnglehistogram.m new file mode 100644 index 0000000..2353c8e --- /dev/null +++ b/ScanMatching/2D/AngleHistogram/makeAnglehistogram.m @@ -0,0 +1,19 @@ +function [ A ] = makeAnglehistogram( Scan, Yaw ) +%MAKEANGLEHISTOGRAM Generates the Angle histogram of global orientations of +%the the vectors generated by consecutive points. + +angleres = 0.0175; +angles=zeros(size(Scan,1)-1,1); + for k=1:size(Scan,1)-1 + angles(k) = normAngle(atan2(Scan(k,1) - Scan(k+1,1),Scan(k,2) - Scan(k+1,2)) + Yaw); + end + + A = zeros(1,round((2*pi)/angleres)+ 1 ) ; + + for k = 1:size(angles,1) + j = round(angles(k) / angleres) + 1 + round((pi)/angleres); + j = min(j,size(A,2)); + A(j) = A(j) + 1; + end +end + diff --git a/ScanMatching/2D/AngleHistogram/makeAnglehistogram.m~ b/ScanMatching/2D/AngleHistogram/makeAnglehistogram.m~ new file mode 100644 index 0000000..6d022b0 --- /dev/null +++ b/ScanMatching/2D/AngleHistogram/makeAnglehistogram.m~ @@ -0,0 +1,18 @@ +function [ A ] = makeAnglehistogram( Scan, Yaw ) +%MAKEANGLEHISTOGRAM Generates the Angle histogram of the vector generated + +angleres = 0.0175; +angles=[]; + for k=1:size(Scan,1)-1 + angles = [angles; normAngle(atan2(Scan(k,1) - Scan(k+1,1),Scan(k,2) - Scan(k+1,2)) + Yaw)]; + end + + A = zeros(1,round((2*pi)/angleres)+ 1 ) ; + + for k = 1:size(angles,1) + j = round(angles(k) / angleres) + 1 + round((pi)/angleres); + j = min(j,size(A,2)); + A(j) = A(j) + 1; + end +end + diff --git a/ScanMatching/2D/AngleHistogram/makeAxishistogram.m b/ScanMatching/2D/AngleHistogram/makeAxishistogram.m new file mode 100644 index 0000000..a994558 --- /dev/null +++ b/ScanMatching/2D/AngleHistogram/makeAxishistogram.m @@ -0,0 +1,33 @@ +function [ A ] = makeAxishistogram( Scan, a, bounds ) +%MAKEANGLEHISTOGRAM Generates the X or Y histogram of the distribution of the +% points in the X and Y space using the resolution Opt.map.resolution + +global Opt +cmres = Opt.map.resolution; + + +switch a + + case 'x' + + maxs = bounds(1,1); + mins = bounds(1,2); + A = zeros(1,ceil((maxs-mins)/cmres) + 1 ) ; + for k = 1:size(Scan,1) + j = min(ceil( (Scan(k,1) - mins) / cmres)+1, length(A)); + A(j) = A(j) + 1; + end + + case 'y' + maxs = bounds(2,1); + mins = bounds(2,2); + A = zeros(1,ceil((maxs-mins)/cmres) + 1 ) ; + + for k = 1:size(Scan,1) + j = min(ceil( (Scan(k,2) - mins) / cmres)+1,length(A)); + A(j) = A(j) + 1; + end +end + +end + diff --git a/ScanMatching/2D/AngleHistogram/makeAxishistogram.m~ b/ScanMatching/2D/AngleHistogram/makeAxishistogram.m~ new file mode 100644 index 0000000..77eff82 --- /dev/null +++ b/ScanMatching/2D/AngleHistogram/makeAxishistogram.m~ @@ -0,0 +1,34 @@ +function [ A ] = makeAxishistogram( Scan, a ) +%MAKEANGLEHISTOGRAM + +cmres = 0.2; + +switch a + + case 'x' + + maxs = max(Scan(:,1)); + mins = min(Scan(:,1)); + + A = zeros(1,round((maxs-mins)/cmres) + 1 ) ; + + for k = 1:size(Scan,1) + j = round( (Scan(k,1) - mins) / cmres) + 1; + A(j) = A(j) + 1; + end + + case 'y' + + maxs = max(Scan(:,2)); + mins = min(Scan(:,2)); + + A = zeros(1,round((maxs-mins)/cmres) + 1 ) ; + + for k = 1:size(Scan,1) + j = round( (Scan(k,2) - mins) / cmres) +1; + A(j) = A(j) + 1; + end +end + +end + diff --git a/ScanMatching/2D/Association/cpAssociation.m b/ScanMatching/2D/Association/cpAssociation.m new file mode 100644 index 0000000..47d04ef --- /dev/null +++ b/ScanMatching/2D/Association/cpAssociation.m @@ -0,0 +1,80 @@ +function [mind assoc index] = cpAssociation(snew, sref, Opt) +%CPASSOCIATION. Given a point and a reference set, find the best matching association +%based on the Closest Point rule, search over the N points in pset the on +%with minor Euclidean distance. + +% In: +% SNEW.cart: Nx2 points +% SREF.cart: Nx2 points +% OPT: options and plot data +% Out: +% MIND mean error +% ASSOC association object +% INDEX index of the points in SREF associated with the points in SNEW + +pset = sref.cart; +pa = snew.cart; + +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + + set(Opt.plot_r,'XData',pset(:,1),'YData',pset(:,2)); + set(Opt.plot_n,'XData',pa(:,1),'YData',pa(:,2)); + drawnow +end + +index = []; +errtot = 0; + +assoc = struct('new',[],'ref',[],'dist',[]); + +% Loop through all the points in the new scan +for j = 1:size(pa,1) + + p = pa(j,1:2); + a = pa(j,3); + + + infd = inf; + mind = 0; + pf = []; + indexf = -1; + + for i = 1:size(pset,1) + p2 = pset(i,1:2); + + % classic Euler Distance + d = sqrt( (p(1)-p2(1))^2 + (p(2)-p2(2))^2 ); + + %we select the point with minor distance and only considering those + %points with distance less than a certain threshold Br(2). The last + %part of the IF is a switch to disable such threshold + if d < infd && d < (Opt.scanmatcher.Br(2)+(~Opt.scanmatcher.distancethreshold*10000)) + infd = d; + mind = infd; + pf = p2; + indexf = i; + end + + end + + % only generates the output if we found at least 1 association + if indexf > 0 + assoc.new = [assoc.new; pa(j,1:3)]; + assoc.ref = [assoc.ref; pf pa(j,3)]; + assoc.dist = [assoc.dist; mind]; + errtot = errtot + mind; + +% if DEBUG.cpAssociation || DEBUG.all +% plot( [p(1) pf(1) ],[p(2) pf(2) ], 'm' ) +% end + + end + + index = [index; indexf]; +end + +% The error is the sum of the residuals +mind = mean(errtot); + +end diff --git a/ScanMatching/2D/Association/cpAssociation.m~ b/ScanMatching/2D/Association/cpAssociation.m~ new file mode 100644 index 0000000..8af5409 --- /dev/null +++ b/ScanMatching/2D/Association/cpAssociation.m~ @@ -0,0 +1,84 @@ +function [mind assoc index] = cpAssociation(snew, sref, Opt) +%Given a point and a reference set, find the best matching association +%based on the Closest Point rule, search over the N points in pset the on +%with minor Euclidean distance. + +pset = sref.cart; +local = sref.polar; + +pa = snew.cart; +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + + figure; + hold on + axis equal + grid on + displayPoints(pset,'g',0); + displayPoints(pa,'r',0); + +end + +pret = []; +index = []; +errtot = 0; + +assoc = struct('new',[],'ref',[],'dist',[]); + +for j = 1:size(pa,1) + + p = pa(j,1:2); + a = pa(j,3); + + + infd = inf; + mind = 0; + pf = []; + indexf = -1; + + for i = 2:size(pset,1) + + ap = local(i,1); + w = abs(ap-a); + + p2 = pset(i-1,1:2); %Adjacent points in the reference scan + p3 = pset(i,1:2); + + % classic Euler Distance + d = sqrt( (p(1)-p2(1))^2 + (p(2)-p2(2))^2 ); + + %we select the point with minor distance + if d < infd + if (Opt.scanmatcher.distancethreshold && d < Opt.scanmatcher.Br(2) ) + infd = d; + mind = infd; + pf = p2; + indexf = i; + end + + end + + if indexf > 0 + assoc.new = [assoc.new; pa(j,1:3)]; + assoc.ref = [assoc.ref; pf pa(j,3)]; + assoc.dist = [assoc.dist; mind]; + + + errtot = errtot + mind; + + + if DEBUG.cpAssociation || DEBUG.all + plot( [p(1) pf(1) ],[p(2) pf(2) ], 'm' ) + end + + end +end + +if size(assoc.ref,1) < 5 + error(' Not enough associations fuond' ); +end + + +mind = mean(errtot); + +end diff --git a/ScanMatching/2D/Association/mahaAssociation.m b/ScanMatching/2D/Association/mahaAssociation.m new file mode 100644 index 0000000..4df90d3 --- /dev/null +++ b/ScanMatching/2D/Association/mahaAssociation.m @@ -0,0 +1,203 @@ +function [res,assoc,P_r_index,P_n_index] = mahaAssociation(n,r,Opt,motion) +% Function mahaAssociation +% Computes correspondences between two scans +% In: +% R: cart[2xN], P[2x2xN], points and uncertainty +% N: cart[2xN], P[2x2xN], points and uncertainty +% OPT: options and plot data +% MOTION: [x y yaw], motion estimation +% P[3x3] scan uncertainty +% Out: +% RES: estimated error +% ASSOC: association object + + +% which method to use? nearest neighbour(0) or virtual point(1) +PARAM.sm.estep_method = 0; +PARAM.debug = 0; + +% Mahalanobis distance confidence +confidence = 0.90; +chi2value = chi2inv(confidence,2); + +% Motion estimation + +yaw = motion.estimation(3); +siny = sin(yaw); +cosy = cos(yaw); + +% Jacobian of the motion +%Jp = [-cosy siny; -siny -cosy]; +Jp = [cosy -siny; siny cosy]; % MODIFICATO + +% Apply motion estimation +[Bnf Bf Af aixs bixs] = transPolarCartScan(n, r, motion.estimation(3), motion.estimation(1:2), 2, Opt.scanmatcher.Br(2)); +Bnf.cart(3,:) = 0; % Delete 1's from normalized 2D point +Bf.cart(3,:) = 0; + +if Opt.scanmatcher.projfilter + %c = Bf.cart(1:2,:); + + if ~isempty(bixs ) + n.P(:,:,bixs) = []; + n.cart(:,bixs) = []; + end + + if ~isempty(aixs ) + r.P(:,:,aixs) = []; + r.cart = Af.cart; + end + + c = Bf.cart(1:2,:); +else + c = Bnf.cart(1:2,:); + %r.cart = Al; +end + +if isempty(n.cart) || isempty(r.cart) + assoc = []; + res = []; + return +end + +global DEBUG +if DEBUG.mahaAssociation || DEBUG.all + + set(Opt.plot_r,'XData',r.cart(1,:),'YData',r.cart(2,:)); + set(Opt.plot_n,'XData',c(1,:),'YData',c(2,:)); + drawnow +end + +% Jacobian of both scans +j = 1:size(n.cart,2); +col3_tmp = [n.cart(1,j)*siny+n.cart(2,j)*cosy; -n.cart(1,j)*cosy+n.cart(2,j)*siny]; +col3 = reshape(col3_tmp,2,1,size(n.cart,2)); +diagonal = repmat(diag(-ones(1,2)),[1,1,size(n.cart,2)]); +Jq = [diagonal col3]; + +% Buffers initialization +a = zeros(2,size(n.cart,2)); % a buffer +Pe = zeros(2,2,size(n.cart,2)); % Pe buffer +indexBuffer = zeros(1,size(n.cart,2)); +Pc_j = zeros(2,2,size(n.cart,2)); %Pc buffer + +for j = 1:size(n.cart,2) + + + Pc_j(:,:,j) = Jq(:,:,j)*motion.P*Jq(:,:,j)'+ Jp*n.P(:,:,j)*Jp'; + + + % draw_ellipse(c(:,j)',Pc_j(:,:,j),'b'); + i = 1:size(r.cart,2); + d = [r.cart(1,i)-c(1,j); r.cart(2,i)-c(2,j)]; + + % Covariances of r and q-p composition + Pe_ij = [r.P(1,1,i)+Pc_j(1,1,j) r.P(1,2,i)+Pc_j(1,2,j); r.P(2,1,i)+Pc_j(2,1,j) r.P(2,2,i)+Pc_j(2,2,j)]; + + % inv(Pe_ij) for a 2x2xn matrix. Each matrix must be [a b;b d] form + det_Pe_ij = (Pe_ij(1,1,i).*Pe_ij(2,2,i)-Pe_ij(1,2,i).^2); + inv_Pe_ij = [Pe_ij(2,2,i)./det_Pe_ij(1,1,i) -Pe_ij(1,2,i)./det_Pe_ij(1,1,i);-Pe_ij(1,2,i)./det_Pe_ij(1,1,i) Pe_ij(1,1,i)./det_Pe_ij(1,1,i)]; + + inv_11 = squeeze(inv_Pe_ij(1,1,i))'; + inv_12 = squeeze(inv_Pe_ij(1,2,i))'; + inv_22 = squeeze(inv_Pe_ij(2,2,i))'; + + % Mahalanobis distance + dist = (d(1,i).^2).*inv_11+2.*d(1,i).*d(2,i).*inv_12 + (d(2,i).^2).*inv_22; + + if PARAM.sm.estep_method == 0 % ICNN + [d_value,d_index] = min(dist); + + if(d_value <= chi2value) + a_j = r.cart(:,d_index); + Pa_j = Pe_ij(:,:,d_index); + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + % save a_j & Pa_j + a(:,j) = a_j; + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + indexBuffer(1,j) = 1; +% if DEBUG.mahaAssociation || DEBUG.all +% plot( [c(1,j) a_j(1) ],[c(2,j) a_j(2) ], 'm' ) +% end + end + elseif PARAM.sm.estep_method == 1 % Virtual point + d_index = dist<=chi2value; + A = r.cart(:,d_index); + + Pe_ij = Pe_ij(:,:,d_index); + + % Build association struct + if (size(A,2) > 0) + i = 1:size(A,2); + + % prob_a using multivariate normal pdf + prob_a = mvnpdf(A',c(1:2,j)',Pe_ij); + prob_a = prob_a'; + + eta = 1/sum(prob_a,2); + prob_a = eta*prob_a; + + % a_j + a_j = sum(A.*[prob_a;prob_a],2); + + % Pa_j + err = [A(1,i)-a_j(1); A(2,i)-a_j(2)]; + err = reshape(err,2,1,size(A,2)); + errQuad = [err(1,1,i).^2 err(1,1,i).*err(2,1,i); err(2,1,i).*err(1,1,i) err(2,1,i).^2]; % error in 2x2 matrix + + prob_a = reshape(prob_a,[1,1,size(A,2)]); + errQuad(:,:,i) = errQuad(:,:,i).*[prob_a(:,:,i) prob_a(:,:,i); prob_a(:,:,i) prob_a(:,:,i)]; + + Pa_j = sum(errQuad(:,:,i),3); + + % save a_j & Pa_j + a(:,j) = a_j; + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + indexBuffer(1,j) = 1; + +% if DEBUG.mahaAssociation || DEBUG.all +% plot( [c(1,j) a_j(1) ],[c(2,j) a_j(2) ], 'm' ) +% end + + % Debug mode 3 + if PARAM.debug >= 3 + if assoc_index == PARAM.assoc_plot_interval + lines_assoc_scanK = repmat([NaN;NaN],1,size(A,2)*3); + a_rep = repmat(a(:,j),1,size(A,2)); + k = 1:3:size(A,2)*3; + m = 1:size(A,2); + %pause; + lines_assoc_scanK(:,k) = a_rep(:,m); + lines_assoc_scanK(:,k+1) = A(:,m); + lines_a = [lines lines_assoc_scanK]; + %pause; + %set(handle_assoc_scanK,'XData',lines_assoc_scanK(1,:), + %'YData',lines_assoc_scanK(2,:)); + + P_r_index = P_r_index + d_index; + P_n_index = [P_n_index j]; + + assoc_index = 1; + else + assoc_index = assoc_index + 1; + end + end + end + end +end + +index = find(indexBuffer == 1); + + +% Keep only the associaton necessary values +assoc.oldn = n.cart(1:2,index); +assoc.new = c(:,index)'; +assoc.ref = a(:,index)'; +assoc.Pe = Pe(:,:,index); +assoc.Jq = Jq(:,:,index); +dd = assoc.new - assoc.ref; +res = sum( sqrt(dd(:,1).^2 + dd(:,2).^2) ); +P_r_index = index; diff --git a/ScanMatching/2D/Association/mahaAssociation.m~ b/ScanMatching/2D/Association/mahaAssociation.m~ new file mode 100644 index 0000000..286f8b0 --- /dev/null +++ b/ScanMatching/2D/Association/mahaAssociation.m~ @@ -0,0 +1,203 @@ +function [res,assoc,P_r_index,P_n_index] = mahaAssociation(n,r,Opt,motion) +% Function mahaAssociation +% Computes correspondences between two scans +% In: +% R: cart[2xN], P[2x2xN], points and uncertainty +% N: cart[2xN], P[2x2xN], points and uncertainty +% OPT: options and plot data +% MOTION: [x y yaw], motion estimation + +% Out: +% RES: estimated error +% ASSOC: association object + + +% which method to use? nearest neighbour(0) or virtual point(1) +PARAM.sm.estep_method = 0; +PARAM.debug = 0; + +% Mahalanobis distance confidence +confidence = 0.70; +chi2value = chi2inv(confidence,(Opt.scanmatcher.Br(2))); + +% Motion estimation + +yaw = motion.estimation(3); +siny = sin(yaw); +cosy = cos(yaw); + +% Jacobian of the motion +%Jp = [-cosy siny; -siny -cosy]; +Jp = [cosy -siny; siny cosy]; % MODIFICATO + +% Apply motion estimation +[Bnf Bf Af aixs bixs] = transPolarCartScan(n, r, motion.estimation(3), motion.estimation(1:2), 2, Opt.scanmatcher.Br(2)); +Bnf.cart(3,:) = 0; % Delete 1's from normalized 2D point +Bf.cart(3,:) = 0; + +if Opt.scanmatcher.projfilter + %c = Bf.cart(1:2,:); + + if ~isempty(bixs ) + n.P(:,:,bixs) = []; + n.cart(:,bixs) = []; + end + + if ~isempty(aixs ) + r.P(:,:,aixs) = []; + r.cart = Af.cart; + end + + c = Bf.cart(1:2,:); +else + c = Bnf.cart(1:2,:); + %r.cart = Al; +end + +if isempty(n.cart) || isempty(r.cart) + assoc = []; + res = []; + return +end + +global DEBUG +if DEBUG.mahaAssociation || DEBUG.all + + set(Opt.plot_r,'XData',r.cart(1,:),'YData',r.cart(2,:)); + set(Opt.plot_n,'XData',c(1,:),'YData',c(2,:)); + drawnow +end + +% Jacobian of both scans +j = 1:size(n.cart,2); +col3_tmp = [n.cart(1,j)*siny+n.cart(2,j)*cosy; -n.cart(1,j)*cosy+n.cart(2,j)*siny]; +col3 = reshape(col3_tmp,2,1,size(n.cart,2)); +diagonal = repmat(diag(-ones(1,2)),[1,1,size(n.cart,2)]); +Jq = [diagonal col3]; + +% Buffers initialization +a = zeros(2,size(n.cart,2)); % a buffer +Pe = zeros(2,2,size(n.cart,2)); % Pe buffer +indexBuffer = zeros(1,size(n.cart,2)); +Pc_j = zeros(2,2,size(n.cart,2)); %Pc buffer + +for j = 1:size(n.cart,2) + + + Pc_j(:,:,j) = Jq(:,:,j)*motion.P*Jq(:,:,j)'+ Jp*n.P(:,:,j)*Jp'; + + +% draw_ellipse(c(:,j)',Pc_j(:,:,j),'b'); + i = 1:size(r.cart,2); + d = [r.cart(1,i)-c(1,j); r.cart(2,i)-c(2,j)]; + + % Covariances of r and q-p composition + Pe_ij = [r.P(1,1,i)+Pc_j(1,1,j) r.P(1,2,i)+Pc_j(1,2,j); r.P(2,1,i)+Pc_j(2,1,j) r.P(2,2,i)+Pc_j(2,2,j)]; + + % inv(Pe_ij) for a 2x2xn matrix. Each matrix must be [a b;b d] form + det_Pe_ij = (Pe_ij(1,1,i).*Pe_ij(2,2,i)-Pe_ij(1,2,i).^2); + inv_Pe_ij = [Pe_ij(2,2,i)./det_Pe_ij(1,1,i) -Pe_ij(1,2,i)./det_Pe_ij(1,1,i);-Pe_ij(1,2,i)./det_Pe_ij(1,1,i) Pe_ij(1,1,i)./det_Pe_ij(1,1,i)]; + + inv_11 = squeeze(inv_Pe_ij(1,1,i))'; + inv_12 = squeeze(inv_Pe_ij(1,2,i))'; + inv_22 = squeeze(inv_Pe_ij(2,2,i))'; + + % Mahalanobis distance + dist = (d(1,i).^2).*inv_11+2.*d(1,i).*d(2,i).*inv_12 + (d(2,i).^2).*inv_22; + + if PARAM.sm.estep_method == 0 % ICNN + [d_value,d_index] = min(dist); + + if(d_value <= chi2value) + a_j = r.cart(:,d_index); + Pa_j = Pe_ij(:,:,d_index); + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + % save a_j & Pa_j + a(:,j) = a_j; + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + indexBuffer(1,j) = 1; +% if DEBUG.mahaAssociation || DEBUG.all +% plot( [c(1,j) a_j(1) ],[c(2,j) a_j(2) ], 'm' ) +% end + end + elseif PARAM.sm.estep_method == 1 % Virtual point + d_index = dist<=chi2value; + A = r.cart(:,d_index); + + Pe_ij = Pe_ij(:,:,d_index); + + % Build association struct + if (size(A,2) > 0) + i = 1:size(A,2); + + % prob_a using multivariate normal pdf + prob_a = mvnpdf(A',c(1:2,j)',Pe_ij); + prob_a = prob_a'; + + eta = 1/sum(prob_a,2); + prob_a = eta*prob_a; + + % a_j + a_j = sum(A.*[prob_a;prob_a],2); + + % Pa_j + err = [A(1,i)-a_j(1); A(2,i)-a_j(2)]; + err = reshape(err,2,1,size(A,2)); + errQuad = [err(1,1,i).^2 err(1,1,i).*err(2,1,i); err(2,1,i).*err(1,1,i) err(2,1,i).^2]; % error in 2x2 matrix + + prob_a = reshape(prob_a,[1,1,size(A,2)]); + errQuad(:,:,i) = errQuad(:,:,i).*[prob_a(:,:,i) prob_a(:,:,i); prob_a(:,:,i) prob_a(:,:,i)]; + + Pa_j = sum(errQuad(:,:,i),3); + + % save a_j & Pa_j + a(:,j) = a_j; + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + indexBuffer(1,j) = 1; + +% if DEBUG.mahaAssociation || DEBUG.all +% plot( [c(1,j) a_j(1) ],[c(2,j) a_j(2) ], 'm' ) +% end + + % Debug mode 3 + if PARAM.debug >= 3 + if assoc_index == PARAM.assoc_plot_interval + lines_assoc_scanK = repmat([NaN;NaN],1,size(A,2)*3); + a_rep = repmat(a(:,j),1,size(A,2)); + k = 1:3:size(A,2)*3; + m = 1:size(A,2); + %pause; + lines_assoc_scanK(:,k) = a_rep(:,m); + lines_assoc_scanK(:,k+1) = A(:,m); + lines_a = [lines lines_assoc_scanK]; + %pause; + %set(handle_assoc_scanK,'XData',lines_assoc_scanK(1,:), + %'YData',lines_assoc_scanK(2,:)); + + P_r_index = P_r_index + d_index; + P_n_index = [P_n_index j]; + + assoc_index = 1; + else + assoc_index = assoc_index + 1; + end + end + end + end +end + +index = find(indexBuffer == 1); + + +% Keep only the associaton necessary values +assoc.oldn = n.cart(1:2,index); +assoc.new = c(:,index)'; +assoc.ref = a(:,index)'; +assoc.Pe = Pe(:,:,index); +assoc.Jq = Jq(:,:,index); +dd = assoc.new - assoc.ref; +res = sum( sqrt(dd(:,1).^2 + dd(:,2).^2) ); +P_r_index = index; diff --git a/ScanMatching/2D/Association/mbAssociation.m b/ScanMatching/2D/Association/mbAssociation.m new file mode 100644 index 0000000..c020b66 --- /dev/null +++ b/ScanMatching/2D/Association/mbAssociation.m @@ -0,0 +1,185 @@ + +function [erro, A, i] = mbAssociation(newPoints, refPoints, opt, motion,prec) +% MBASSOCIATION: Metric Based Scan matching (MbICP) association function +% In: +% NEWPOINTS: cart[2xN], P[2x2xN], points and uncertainty +% REFPOINTS: cart[2xN], P[2x2xN], points and uncertainty +% OPT: options and plot data +% MOTION: [x y yaw], motion estimation +% PREC: refPoint's precomputed stuff +% Out: +% ERRO: estimated error +% ASSOC: association object +% I: index of the points in SREF associated with the points +% in SNEW + +global Opt + +PARAM.Bw = 1.57/3.0; +PARAM.Br = 10.1; +PARAM.L = 3; +PARAM.step = 1; +PARAM.maxDistanceInterpolation = 0.5; +PARAM.filter = 1; +PARAM.projectionFilter = 0; %!! test with 0 +PARAM.associationError = 0.1; +PARAM.maxIterations = 250; +PARAM.errorRatio = 0.0001; +PARAM.error = [0.0001 0.0001 0.0001]; +PARAM.nIterationSmoothConvergence = 2; + +PARAM.deviation.sensor = [0.0262 0.05]; % std deviation [angular(rad) range(m)] [+/-1.5� +/-5cm] +PARAM.deviation.motion = [0.1 0.1 deg2rad(10)]; % motion std deviation [x(m) y(m) yaw(rad)] + +x = motion.estimation(1); +y = motion.estimation(2); +yaw = motion.estimation(3); + +L2 = PARAM.L^2; + +% newPoints referenced to reference coordinate system +%rTn = [1 0 0; 0 1 0; 0 0 1]; % +% rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; +% +% newCartesianRef = []; newPolarRef = []; +% for i=1:size(newPoints.cart,2) +% newCartesianRef = [newCartesianRef rTn*[newPoints.cart(:,i);1]]; +% [r,theta] = cart2pol(newCartesianRef(1,i), newCartesianRef(2,i)); +% newPolarRef = [newPolarRef [r,theta]']; +% end + +newCartesianRef = newPoints.cart; % (3,:) = []; % Delete 1's from normalized 2D point +newPolarRef = newPoints.polar; + +global DEBUG + + +if DEBUG.mbAssociation || DEBUG.all + + set(opt.plot_r,'XData',refPoints.cart(1,:),'YData',refPoints.cart(2,:)); + set(opt.plot_n,'XData',newPoints.cart(1,:),'YData',newPoints.cart(2,:)); + drawnow +end + +% Projection Filter (Necessari �?�?�?) +if (PARAM.projectionFilter == 1) + counter = 2; + for i = 2:size(newPolarRef,2) + if (newPolarRef(1,i) >= newPolarRef(1,counter-1)) + newPolarRef(:,counter) = newPolarRef(:,i); + newCartesianRef(:,counter) = newCartesianRef(:,i); + counter = counter+1; + end + end + newPolarRef(:,counter:size(newPolarRef,2)) = []; + newCartesianRef(:,counter:size(newCartesianRef,2)) = []; +end + +newPointsRef = struct('polar',newPolarRef,'cart',newCartesianRef); + + +% Build index for the windows +% L = 1; R = 1; +% Io = 1; +% +% if (newPointsRef.polar(1,Io) < refPoints.polar(1,L)) +% while(newPointsRef.polar(1,Io) + PARAM.Bw < refPoints.polar(1,L) && Io < size(newPointsRef.polar,2)-1) +% Io = Io+1; +% end +% while(newPointsRef.polar(1,Io) + PARAM.Bw > refPoints.polar(1,R+1) && R < size(refPoints.polar,2)-1) +% R = R+1; +% end +% else +% while(newPointsRef.polar(1,Io) - PARAM.Bw > refPoints.polar(1,L) && L < size(refPoints.polar,2)-1) +% L = L+1; +% end +% R = L; +% while(newPointsRef.polar(1,Io) + PARAM.Bw > refPoints.polar(1,R+1) && R < size(refPoints.polar,2)-1) +% R = R+1; +% end +% end + +% Look for potential correspondeces between scans +%Associations = []; + +assoc = struct('new',[],'ref',[],'dist',[],'R',[],'L',[]); + +counter = 1; +R = 0; +L = 0; +for i = 1:size(newPointsRef.polar,2) + % Keep index of the original scan ordering + + previousX = 0; + previousY = 0; + previousDist = 100000; + p2x = newPointsRef.cart(1,i); p2y = newPointsRef.cart(2,i); + + for j = 2:size(refPoints.polar,2) + + if abs(refPoints.polar(1,j) - newPointsRef.polar(1,i)) > PARAM.Bw + continue + end + q1x = refPoints.cart(1,j-1); q1y = refPoints.cart(2,j-1); + q2x = refPoints.cart(1,j); q2y = refPoints.cart(2,j); + + + dqx = prec.refdq(1,j-1); dqy = prec.refdq(2,j-1); + dqpx = q1x-p2x; dqpy = q1y-p2y; + a = 1/(p2x^2+p2y^2+L2); + b = 1-a*p2y^2; + c = 1-a*p2x^2; + d = a*p2x*p2y; + + landaMin = (d*(dqx*dqpy+dqy*dqpx)+b*dqx*dqpx+c*dqy*dqpy)/(b*prec.refdq2(1,j-1)+c*prec.refdq(2,j-1)+2*d*prec.refdqxdqy(j-1)); + + if (landaMin < 0) + qx = q1x; qy = q1y; + elseif (landaMin > 1) + qx = q2x; qy = q2y; + elseif (prec.distRef(j-1) < PARAM.maxDistanceInterpolation) + qx = (1-landaMin)*q1x+landaMin*q2x; + qy = (1-landaMin)*q1y+landaMin*q2y; + else + if (landaMin < 0.5) + qx = q1x; qy = q1y; + else + qx = q2x; qy = q2y; + end + end + + % Precompute stuff to see if we have the associations + dx = p2x-qx; + dy = p2y-qy; + dist = dx^2+dy^2-(dx*qy-dy*qx)^2/(qx^2+qy^2+L2); + + if (dist < previousDist) + previousX = qx; + previousY = qy; + previousDist = dist; + end + end + + % Association compatible in distance + if (previousDist < Opt.scanmatcher.Br(2)) + % assoc = struct('new',newPointsRef.cart(:,i),'ref',[previousX; previousY],'dist',previousDist,'R',R,'L',L); + % Associations = [Associations assoc]; + counter = counter+1; + + assoc.new = [assoc.new; newPointsRef.cart(1:2,i)']; + assoc.ref = [assoc.ref; [previousX previousY]]; + assoc.dist = [assoc.dist; previousDist]; + assoc.R = [assoc.R; R]; + assoc.L = [assoc.L; L]; + + end +end + + +A = assoc; +i=[]; +if(size(A.ref) < size(newPointsRef.cart,2)*PARAM.associationError) + erro = 0; +else + erro = mean(assoc.dist); +end diff --git a/ScanMatching/2D/Association/mbAssociation.m~ b/ScanMatching/2D/Association/mbAssociation.m~ new file mode 100644 index 0000000..d46a8fb --- /dev/null +++ b/ScanMatching/2D/Association/mbAssociation.m~ @@ -0,0 +1,185 @@ +% Function EStep +% Coputes correspondences between two scans +% In: +% refPoints +% newPoint +% motion: [x y yaw] +% prec: refPoint's precomputed stuff +% Out: +% out: +% A: associations +function [out, A, i] = mbAssociation(newPoints, refPoints, Opt, motion,prec) + +PARAM.Bw = 1.57/3.0; +PARAM.Br = 10.1; +PARAM.L = 3; +PARAM.step = 1; +PARAM.maxDistanceInterpolation = 0.5; +PARAM.filter = 1; +PARAM.projectionFilter = 0; %!! test with 0 +PARAM.associationError = 0.1; +PARAM.maxIterations = 250; +PARAM.errorRatio = 0.0001; +PARAM.error = [0.0001 0.0001 0.0001]; +PARAM.nIterationSmoothConvergence = 2; + +PARAM.deviation.sensor = [0.0262 0.05]; % std deviation [angular(rad) range(m)] [+/-1.5� +/-5cm] +PARAM.deviation.motion = [0.1 0.1 deg2rad(10)]; % motion std deviation [x(m) y(m) yaw(rad)] + +x = motion.estimation(1); +y = motion.estimation(2); +yaw = motion.estimation(3); + +L2 = PARAM.L^2; + +% newPoints referenced to reference coordinate system +%rTn = [1 0 0; 0 1 0; 0 0 1]; % +% rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; +% +% newCartesianRef = []; newPolarRef = []; +% for i=1:size(newPoints.cart,2) +% newCartesianRef = [newCartesianRef rTn*[newPoints.cart(:,i);1]]; +% [r,theta] = cart2pol(newCartesianRef(1,i), newCartesianRef(2,i)); +% newPolarRef = [newPolarRef [r,theta]']; +% end + +newCartesianRef = newPoints.cart; % (3,:) = []; % Delete 1's from normalized 2D point +newPolarRef = newPoints.polar; + +global DEBUG +if DEBUG.mbAssociation || DEBUG.all + + figure + hold on + axis equal + + displayPoints(newCartesianRef','g'); + displayPoints(newPoints.cart','r'); + %displayPoints(refPoints.cart','k'); +end + +% Projection Filter (Necessari �?�?�?) +if (PARAM.projectionFilter == 1) + counter = 2; + for i = 2:size(newPolarRef,2) + if (newPolarRef(1,i) >= newPolarRef(1,counter-1)) + newPolarRef(:,counter) = newPolarRef(:,i); + newCartesianRef(:,counter) = newCartesianRef(:,i); + counter = counter+1; + end + end + newPolarRef(:,counter:size(newPolarRef,2)) = []; + newCartesianRef(:,counter:size(newCartesianRef,2)) = []; +end + +newPointsRef = struct('polar',newPolarRef,'cart',newCartesianRef); + + +% Build index for the windows +% L = 1; R = 1; +% Io = 1; +% +% if (newPointsRef.polar(1,Io) < refPoints.polar(1,L)) +% while(newPointsRef.polar(1,Io) + PARAM.Bw < refPoints.polar(1,L) && Io < size(newPointsRef.polar,2)-1) +% Io = Io+1; +% end +% while(newPointsRef.polar(1,Io) + PARAM.Bw > refPoints.polar(1,R+1) && R < size(refPoints.polar,2)-1) +% R = R+1; +% end +% else +% while(newPointsRef.polar(1,Io) - PARAM.Bw > refPoints.polar(1,L) && L < size(refPoints.polar,2)-1) +% L = L+1; +% end +% R = L; +% while(newPointsRef.polar(1,Io) + PARAM.Bw > refPoints.polar(1,R+1) && R < size(refPoints.polar,2)-1) +% R = R+1; +% end +% end + +% Look for potential correspondeces between scans +%Associations = []; + +assoc = struct('new',[],'ref',[],'dist',[],'R',[],'L',[]); + +counter = 1; +R = 0; +L = 0; +for i = 1:size(newPointsRef.polar,2) + % Keep index of the original scan ordering + + previousX = 0; + previousY = 0; + previousDist = 100000; + p2x = newPointsRef.cart(1,i); p2y = newPointsRef.cart(2,i); + + for j = 2:size(refPoints.polar,2) + + if abs(refPoints.polar(1,j) - newPointsRef.polar(1,i)) > PARAM.Bw + continue + end + q1x = refPoints.cart(1,j-1); q1y = refPoints.cart(2,j-1); + q2x = refPoints.cart(1,j); q2y = refPoints.cart(2,j); + + + dqx = prec.refdq(1,j-1); dqy = prec.refdq(2,j-1); + dqpx = q1x-p2x; dqpy = q1y-p2y; + a = 1/(p2x^2+p2y^2+L2); + b = 1-a*p2y^2; + c = 1-a*p2x^2; + d = a*p2x*p2y; + + landaMin = (d*(dqx*dqpy+dqy*dqpx)+b*dqx*dqpx+c*dqy*dqpy)/(b*prec.refdq2(1,j-1)+c*prec.refdq(2,j-1)+2*d*prec.refdqxdqy(j-1)); + + if (landaMin < 0) + qx = q1x; qy = q1y; + elseif (landaMin > 1) + qx = q2x; qy = q2y; + elseif (prec.distRef(j-1) < PARAM.maxDistanceInterpolation) + qx = (1-landaMin)*q1x+landaMin*q2x; + qy = (1-landaMin)*q1y+landaMin*q2y; + else + if (landaMin < 0.5) + qx = q1x; qy = q1y; + else + qx = q2x; qy = q2y; + end + end + + % Precompute stuff to see if we have the associations + dx = p2x-qx; + dy = p2y-qy; + dist = dx^2+dy^2-(dx*qy-dy*qx)^2/(qx^2+qy^2+L2); + + if (dist < previousDist) + previousX = qx; + previousY = qy; + previousDist = dist; + end + end + + % Association compatible in distance + if (previousDist < PARAM.Br) + % assoc = struct('new',newPointsRef.cart(:,i),'ref',[previousX; previousY],'dist',previousDist,'R',R,'L',L); + % Associations = [Associations assoc]; + counter = counter+1; + + assoc.new = [assoc.new; newPointsRef.cart(1:2,i)']; + assoc.ref = [assoc.ref; [previousX previousY]]; + assoc.dist = [assoc.dist; previousDist]; + assoc.R = [assoc.R; R]; + assoc.L = [assoc.L; L]; + + if DEBUG.mbAssociation || DEBUG.all + plot( [newPointsRef.cart(1,i) previousX ],[newPointsRef.cart(2,i) previousY ], 'm' ) + end + end +end + + +A = assoc; +i=[]; +if(size(A.ref) < size(newPointsRef.cart,2)*PARAM.associationError) + out = sum; +else + out = 1; +end diff --git a/ScanMatching/2D/Association/mpAssociation.m b/ScanMatching/2D/Association/mpAssociation.m new file mode 100644 index 0000000..499970a --- /dev/null +++ b/ScanMatching/2D/Association/mpAssociation.m @@ -0,0 +1,104 @@ +function [minr assoc index] = mpAssociation(snew, sref, Opt) +%MPASSOCIATION: Given a point and a reference set, find the best matching association +%based on the Matching Point rule as defined in + +%Feng Lu and Evangelos Milios. 1997. Robot Pose Estimation in Unknown +%Environments by Matching 2D Range Scans. J. Intell. Robotics Syst. 18, 3 +%(March 1997), 249-275 + +%It works in 2D, Z is kept fixed +% MODEL = Reference Scan +% In: +% SNEW: cart[Nx2 points],polar[Nx2 points] +% SREF: cart[Nx2 points],polar[Nx2 points] +% OPT: options and plot data +% Out: +% MIND mean error +% ASSOC association object +% INDEX index of the points in SREF associated with the points in +% SNEW + + +pset = sref.cart; +local = sref.polar; +cart = sref.cart; +pa = snew.cart; + +errtot = 0; +index = []; + +% Orientation and distance threshold +Br = Opt.scanmatcher.Br; + +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + + set(Opt.plot_r,'XData',pset(:,1),'YData',pset(:,2)); + set(Opt.plot_n,'XData',pa(:,1),'YData',pa(:,2)); + drawnow +end + + +assoc = struct('new',[],'ref',[],'dist',[]); + +for j = 1:size(pa,1) + + p = pa(j,1:2); + [a l] = cart2pol(p(1),p(2)); + + % initialization + chosenp = []; %best fitting range point + minr = inf; %minor range shift found + m = size(pset,1); + + %We check all adjacent points for a possible association + for i = 1:m + + ap = local(i,1); + lp = local(i,2); + + pp = cart(i,1:2); + w = abs(ap-a); + %check the angular shift fits the threshold + if Br(1) > 0 && w > Br(1) + continue; + end + + % check the range and savfe the points having a closest range to + % P + mp1 = [ap lp]; + min_v = abs(l - lp); + + % minr stores the closest range we have found but the euclidean + % distance between the two points still has to be minor than the + % distance threshold + if ptsDistance(pp,p) < Br(2) && min_v < minr + chosenp = mp1; + minr = min_v; + + end + + end + + + if(~isempty(chosenp)) + [pret1 pret2] = pol2cart(chosenp(1),chosenp(2)); + errtot = errtot + minr; + assoc.new = [assoc.new; pa(j,1:3)]; + assoc.ref = [assoc.ref; [pret1 pret2] pa(j,3)]; + assoc.dist = [assoc.dist; minr]; +% if DEBUG.mpAssociation || DEBUG.all +% plot( [p(1) pret1 ],[p(2) pret2 ], 'k' ); +% end + end + + +end + +% error as the sum of residuals +minr = mean(errtot); + +end + + + diff --git a/ScanMatching/2D/Association/mpAssociation.m~ b/ScanMatching/2D/Association/mpAssociation.m~ new file mode 100644 index 0000000..6d96ef0 --- /dev/null +++ b/ScanMatching/2D/Association/mpAssociation.m~ @@ -0,0 +1,142 @@ +function [minr assoc index] = mpAssociation(snew, sref, Opt) +%MPASSOCIATION: Given a point and a reference set, find the best matching association +%based on the Matching Point rule as defined in + +%Feng Lu and Evangelos Milios. 1997. Robot Pose Estimation in Unknown +%Environments by Matching 2D Range Scans. J. Intell. Robotics Syst. 18, 3 +%(March 1997), 249-275 + +%It works in 2D, Z is kept fixed +% MODEL = Reference Scan +% In: +% SNEW.cart: Nx2 points +% SREF.cart: Nx2 points +% OPT: options and plot data +% Out: +% MIND mean error +% ASSOC association object +% INDEX index of the points in SREF associated with the points in +% SNEW + + +pset = sref.cart; +local = sref.polar; +cart = sref.cart; +pa = snew.cart; +papol = snew.polar; +pret = []; +errtot = 0; +index = []; + +Br = Opt.scanmatcher.Br; + +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + + set(Opt.plot_r,'XData',pset(:,1),'YData',pset(:,2)); + set(Opt.plot_n,'XData',pa(:,1),'YData',pa(:,2)); +% displayPoints(pset,'g',0); +% displayPoints(pa,'r',0); + drawnow +end + + +assoc = struct('new',[],'ref',[],'dist',[]); + +for j = 1:size(pa,1) + + p = pa(j,1:2); + [a l] = cart2pol(p(1),p(2)); %papol(j,1); + %l = papol(j,2); + + %[a l] = cart2pol(p(1),p(2)); %Transform in polar coord + chosenp = []; %best fitting range point + minr = inf; %minor range shift found + indexf = -1; + + m = size(pset,1); + + pret1=inf; + pret2=inf; + %Once we have the set, we check all adjacent points for a possible + %association + for i = 1:m + % [ap lp] = cart2pol(pset(i,1),pset(i,2)); + ap = local(i,1); + lp = local(i,2); + + pp = cart(i,1:2); + w = abs(ap-a); %normAngle(abs(ap-a)); %angular shift + if Br(1) > 0 && w > Br(1) + continue; + end + + mp1 = [ap lp]; %Adjacent points in the reference scan + %mp2 = seta(i,3:4); + %interval = [mp1; mp2]; + %r1 = mp1(2); r2 = mp2(2); th1 = mp1(1); th2 = mp2(1); + + %Interpolate to find how the rotation of p would fit in the model + % r = (r1*r2*(th2-th1)) / r1*(a - th1) + r2*(th2 - a); + + + + %Chooses the most similar point in the model,the one with range + %closest to p + %[minv index] = min([ abs(l - interval(1,2)) abs(l - interval(2,2)) ]); + min_v = abs(l - lp); + %candidate = interval(index,:); + + %We can inverse interpolate if the interpolated adjacent scan are known + %to intersect the point in the new scan we are consiering + % if (r1 > r && r2 < r) || (r2 > r && r1 < r) + % [mpp1x mpp1y] = pol2cart(th1,r1); + % [mpp2x mpp2y] = pol2cart(th2,r2); + % [mppx mppy] = pol2cart(a,r); + % figure + % plot([mpp1x mpp2x p(1)], [mpp1y mpp2y p(2)], '.r', mppx,mppy, '.k'); + % l=r; + % th = (l*(r1*th1 - r2*th2) + r1*r2*(th2-th1)) / l*(r1-r2); + % chosenp = [th l 0]; + % minr = 0; + % break; + % end + + + % minr stores the closest range we have found + if ptsDistance(pp,p) < Br(2) && min_v < minr + chosenp = mp1; + minr = min_v; + indexf = i; + end + + end + + %We return the minor range found and the chosen point + if(~isempty(chosenp)) + [pret1 pret2] = pol2cart(chosenp(1),chosenp(2)); + errtot = errtot + minr; + assoc.new = [assoc.new; pa(j,1:3)]; + assoc.ref = [assoc.ref; [pret1 pret2] pa(j,3)]; + assoc.dist = [assoc.dist; minr]; + +% if DEBUG.mpAssociation || DEBUG.all +% plot( [p(1) pret1 ],[p(2) pret2 ], 'k' ); +% end + + end + + +end + +% if size(assoc.ref,1) < 5 +% error(' Not enough associations fuond' ); +% end + + +minr = mean(errtot); + +end + + + diff --git a/ScanMatching/2D/Association/normAssociation.m b/ScanMatching/2D/Association/normAssociation.m new file mode 100644 index 0000000..4de204d --- /dev/null +++ b/ScanMatching/2D/Association/normAssociation.m @@ -0,0 +1,116 @@ +function [mind assoc index] = normAssociation(snew, sref, Opt) +%P2LASSOCIATION Point to line association. Tries to associate every point +%of SNEW to one in SREF. A point in SREF is chosen using a point to line +%metric, ie calculating the orthogonal distance beteween a point in SNEW +%and a line formed by consecutive points in the model SREF + +% In: +% SNEW: cart[Nx2 points],polar[Nx2 points] +% SREF: cart[Nx2 points],polar[Nx2 points] +% OPT: options and plot data +% Out: +% MIND mean error +% ASSOC association object +% INDEX index of the points in SREF associated with the points in +% SNEW + +pset = sref.cart; +local = sref.polar; + +pa = snew.cart; +Br = Opt.scanmatcher.Br; + +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + + set(Opt.plot_r,'XData',pset(:,1),'YData',pset(:,2)); + set(Opt.plot_n,'XData',pa(:,1),'YData',pa(:,2)); + drawnow +end + +pret = []; +index = []; +errtot = 0; +assoc = struct('new',[],'ref',[],'dist',[]); +lineset = zeros(size(pset,1),2); + +% Generates the set of lines passing through adjacent points +for i = 2:size(pset,1) + p2 = pset(i-1,1:2); %Adjacent points in the reference scan + p3 = pset(i,1:2); + [m q] = pts2lineSI(p2,p3); + lineset(i,:) = [m q]; +end + +% Cycle all the points in S new +for j = 2:size(pa,1) + + p = pa(j,1:2); + pm2 = pa(j-1,1:2); + a = snew.polar(j,1); + + + infd = inf; + indexf = -1; + pf = [-1 -1]; + + % Generate the line between the current points and its adjacent + [m q] = pts2lineSI(p,pm2); + + for i = 1:size(lineset,1) + + ap = local(i,1); + w = abs(ap-a); + + % Maximum rotation check + if Br(1) > 0 && w > Br(1) + continue; + end + + %Adjacent points in the reference scan + p2 = pset(i,1:2); + p3 = pset(i+1,1:2); + + pp = [p2;p3]; + + d1 = ptsDistance(p,p2); + d2 = ptsDistance(p,p3); + [mm im] = min([d1,d2]); + + %interpolate the points and find the segment distance from p + [d P] = line2lineSIDistance(p,m,q,lineset(i,1),lineset(i,2)); + + if ~inSegment(P,p2,p3) + d = mm; + end + + %we select the point with normal distance minor distance but has + %still euclidean distance lower than a threshold + if d < Br(2) && d < infd + infd = d; + pf = pp(im,:); + indexf = (i-1)+im; + end + + end + pret = [pret; pf]; + index = [index; indexf]; + %errtot = errtot + infd; + + if indexf > 0 + assoc.new = [assoc.new; pa(j,1:3)]; + assoc.ref = [assoc.ref; pf pa(j,3)]; + assoc.dist = [assoc.dist; infd]; + errtot = errtot + infd; +% if DEBUG.normAssociation || DEBUG.all +% plot( [p(1) pf(1) ],[p(2) pf(2) ], 'm' ) +% end + + end + +end + +% error as sum of residuals +mind = mean(errtot); + +end diff --git a/ScanMatching/2D/Association/normAssociation.m~ b/ScanMatching/2D/Association/normAssociation.m~ new file mode 100644 index 0000000..6ce132b --- /dev/null +++ b/ScanMatching/2D/Association/normAssociation.m~ @@ -0,0 +1,118 @@ +function [mind assoc index] = normAssociation(snew, sref, Opt) +%P2LASSOCIATION Point to line association. Tries to associate every point +%of SNEW to one in SREF. A point in SREF is chosen using a point to line +%metric, ie calculating the orthogonal distance beteween a point in SNEW +%and a line formed by consecutive points in the model SREF + +% In: +% SNEW: cart[Nx2 points],polar[Nx2 points] +% SREF: cart[Nx2 points],polar[Nx2 points] +% OPT: options and plot data +% Out: +% MIND mean error +% ASSOC association object +% INDEX index of the points in SREF associated with the points in +% SNEW + +pset = sref.cart; +local = sref.polar; + +pa = snew.cart; +Br = Opt.scanmatcher.Br; + +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + + set(Opt.plot_r,'XData',pset(:,1),'YData',pset(:,2)); + set(Opt.plot_n,'XData',pa(:,1),'YData',pa(:,2)); + drawnow +end + +pret = []; +index = []; +errtot = 0; +assoc = struct('new',[],'ref',[],'dist',[]); +lineset = zeros(size(pset,1),2); + +% Generates the set of lines passing through adjacent points +for i = 2:size(pset,1) + p2 = pset(i-1,1:2); %Adjacent points in the reference scan + p3 = pset(i,1:2); + [m q] = pts2lineSI(p2,p3); + lineset(i,:) = [m q]; +end + +% Cycle all the points in S new +for j = 2:size(pa,1) + + p = pa(j,1:2); + pm2 = pa(j-1,1:2); + a = snew.polar(j,1); + + + infd = inf; + indexf = -1; + pf = [-1 -1]; + + % Generate the line between the current points and its adjacent + [m q] = pts2lineSI(p,pm2); + + for i = 1:size(lineset,1) + + ap = local(i,1); + w = abs(ap-a); + + % Maximum rotation check + if Br(1) > 0 && w > Br(1) + continue; + end + + %Adjacent points in the reference scan + p2 = pset(i,1:2); + p3 = pset(i+1,1:2); + + pp = [p2;p3]; + + d1 = ptsDistance(p,p2); + d2 = ptsDistance(p,p3); + [mm im] = min([d1,d2]); + + %interpolate the points and find the segment distance from p + [d P] = line2lineSIDistance(p,m,q,lineset(i,1),lineset(i,2)); + + if ~inSegment(P,p2,p3) + d = mm; + end + + %we select the point with minor distance + if d < Br(2) && d < infd + infd = d; + pf = pp(im,:); + indexf = (i-1)+im; + end + + end + pret = [pret; pf]; + index = [index; indexf]; + %errtot = errtot + infd; + + if indexf > 0 + assoc.new = [assoc.new; pa(j,1:3)]; + assoc.ref = [assoc.ref; pf pa(j,3)]; + assoc.dist = [assoc.dist; infd]; + + + errtot = errtot + infd; + + + if DEBUG.normAssociation || DEBUG.all + plot( [p(1) pf(1) ],[p(2) pf(2) ], 'm' ) + end + + end + +end + +mind = mean(errtot); + +end diff --git a/ScanMatching/2D/Association/p2lAssociation.m b/ScanMatching/2D/Association/p2lAssociation.m new file mode 100644 index 0000000..250f0fc --- /dev/null +++ b/ScanMatching/2D/Association/p2lAssociation.m @@ -0,0 +1,95 @@ +function [mind assoc index] = p2lAssociation(snew, sref, Opt) +%P2LASSOCIATION Point to line association. Tries to associate every point +%of SNEW to one in SREF. A point in SREF is chosen using a point to line +%metric, ie calculating the orthogonal distance beteween a point in SNEW +%and a line formed by consecutive points in the model SREF + +% In: +% SNEW: cart[Nx2 points],polar[Nx2 points] +% SREF: cart[Nx2 points],polar[Nx2 points] +% OPT: options and plot data +% Out: +% MIND mean error +% ASSOC association object +% INDEX index of the points in SREF associated with the points in +% SNEW + + +pset = sref.cart; +local = sref.polar; + +pa = snew.cart; +Br = Opt.scanmatcher.Br; + +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + + set(Opt.plot_r,'XData',pset(:,1),'YData',pset(:,2)); + set(Opt.plot_n,'XData',pa(:,1),'YData',pa(:,2)); + drawnow +end + +pret = []; +index = []; +errtot = 0; + +assoc = struct('new',[],'ref',[],'dist',[]); + +% Cycle all the points in S ne +for j = 1:size(pa,1) + + p = pa(j,1:2); + a = snew.polar(j,1); + + + infd = inf; + indexf = -1; + pf = [-1 -1]; + + for i = 1:size(pset,1)-1 + + ap = local(i,1); + w = abs(ap-a); + + %Maximum rotation check + if Br(1) > 0 && w > Br(1) + continue; + end + + %Adjacent points in the reference scan + p2 = pset(i,1:2); + p3 = pset(i+1,1:2); + + %interpolate the points and find the segment distance from p + [d P] = pt2lineDistance(p(1:2),p2,p3); + + %we select the point with minor distance but has + %still euclidean distance lower than a threshold + if d < Br(2) && d < infd + infd = d; + pf = P; + indexf = i; + end + + end + pret = [pret; pf]; + index = [index; indexf]; + + if indexf > 0 + assoc.new = [assoc.new; pa(j,1:3)]; + assoc.ref = [assoc.ref; pf pa(j,3)]; + assoc.dist = [assoc.dist; infd]; + errtot = errtot + infd; +% if DEBUG.p2lAssociation || DEBUG.all +% plot( [p(1) pf(1) ],[p(2) pf(2) ], 'm' ) +% end + + end + +end + +% error as the sum of residuals +mind = mean(errtot); + +end + diff --git a/ScanMatching/2D/Association/p2lAssociation.m~ b/ScanMatching/2D/Association/p2lAssociation.m~ new file mode 100644 index 0000000..01a8ee5 --- /dev/null +++ b/ScanMatching/2D/Association/p2lAssociation.m~ @@ -0,0 +1,95 @@ +function [mind pret index] = p2lAssociation(snew, sref, Br) +%P2LASSOCIATION Point to line association. Tries to associate every point +%of SNEW to one in SREF. A point in SREF is chosen using a point to line +%metric, ie calculating the orthogonal distance beteween a point in SNEW +%and a line formed by consecutive points in the model SREF + +pset = sref.cart; +local = sref.polar; + +pa = snew.cart; + + + +%DEBUG + +figure; +hold on +axis equal +grid on +displayPoints(pset,'g',0); +displayPoints(pa,'r',0); + +pret = []; +index = []; +errtot = 0; + +for i = 2:size(pset,1) + p2 = pset(i-1,1:2); %Adjacent points in the reference scan + p3 = pset(i,1:2); + [m q] = pts2lineSI(p2,p3); + linset = [lineset; m q]; +end + +for j = 1:size(pa,1) + + p = pa(j,1:2); + a = pa(j,3); + + + infd = inf; + mind = 0; + indexf = -1; + pf = []; + + for i = 2:size(pset,1) + %[ap lp] = cart2pol(pset(i,1),pset(i,2)); + ap = local(i,1); + w = abs(ap-a); %normAngle(abs(ap-a)); %angular shift + + % Maximum rotation check + % if Br > 0 && w > Br + % continue; + % end + + p2 = pset(i-1,1:2); %Adjacent points in the reference scan + p3 = pset(i,1:2); + +dff = ip - p2; +d1 = sqrt(dff(1)^2 +dff(2)^2); + +dff = ip - p3; +d2 = sqrt(dff(1)^2 +dff(2)^2); + %interpolate the points and find the segment distance from p + %[d P] = pt2lineDistance(p(1:2),p2(1:2),p3(1:2)); + d = pt2lineSIDistance(p(1:2),p2(1:2),p3(1:2)); + % classic Euler Distance + %d = sqrt( (p(1)-p2(1))^2 + (p(2)-p2(2))^2 ); + + %we select the point with minor distance + if d < infd + infd = d; + mind = infd; + pf = p2; + indexf = i; + end + + end + pret = [pret; pf]; + index = [index; indexf]; + errtot = errtot + mind; + + + %DEBUG + plot( [p(1) pf(1) ],[p(2) pf(2) ], 'm' ) +end + +if size(pret,1) < 3 + error(' Not enough associations fuond' ); +end + + +mind = mean(errtot); + +end + diff --git a/ScanMatching/2D/Association/plAssociation.m b/ScanMatching/2D/Association/plAssociation.m new file mode 100644 index 0000000..b2f8daf --- /dev/null +++ b/ScanMatching/2D/Association/plAssociation.m @@ -0,0 +1,63 @@ +function [mind pret index] = plAssociation(pa, pdata) +%Given a point and a reference set, find the best matching association +%based on the two nearest closest points (in euclidean terms). It returns +%two points which represent the endpoints of the closest line + +pset = pdata.cart; +p = pa(1:2); +pret = []; +index = []; + + +% Cycle all the points and find the two nearest one + + infd = inf; + mind = sqrt( (p(1)-pret(1))^2 + (p(2)-pret(2))^2 ); + + for i = 1:size(pset,1) + + p2 = pset(i,1:2); %check the distance and save the best match + d = sqrt( (p(1)-p2(1))^2 + (p(2)-p2(2))^2 ); + + if d < infd + infd = d; + mind = infd; + pret = p2; + index = i; + end + + end + + % Check the nearest 5 neighbours in both directions to find the second + % best match + infd = inf; + for i = 1:5 + + + p2 = pset(index+i,1:2); %check the distance and save the best match + p3 = pset(index-i,1:2); + + % We check in both directions and save the minimum + d = sqrt( (p(1)-p2(1))^2 + (p(2)-p2(2))^2 ); + d2 = sqrt( (p(1)-p3(1))^2 + (p(2)-p3(2))^2 ); + + [d im] = min(d,d2); + + if d < infd + infd = d; + mind2 = infd; + pret2 = p2; + index2 = index+(i * (-2*(im-1) + 1) ) ; + end + + + end + + pret = [pret pret2]; + index = [index index2]; + mind = [mind mind2]; +if size(pret,1) < 2 + error('No associations found'); +end + +end diff --git a/ScanMatching/2D/Association/plAssociation.m~ b/ScanMatching/2D/Association/plAssociation.m~ new file mode 100644 index 0000000..b34fdbf --- /dev/null +++ b/ScanMatching/2D/Association/plAssociation.m~ @@ -0,0 +1,61 @@ +function [mind pret index] = plAssociation(pa, pdata, Br) +%Given a point and a reference set, find the best matching association +%based on the two nearest closest points (in euclidean terms). It returns +%two points which represent the endpoints of the closest line + +pset = pdata.cart; +p = pa(1:2); +pret = []; +index = []; + + +% Cycle all the points and find the two nearest one + + + infd = inf; + mind = sqrt( (p(1)-pret(1))^2 + (p(2)-pret(2))^2 ); + + for i = 1:size(pset,1) + + p2 = pset(i,1:2); %check the distance and save the best match + d = sqrt( (p(1)-p2(1))^2 + (p(2)-p2(2))^2 ); + + if d < infd + infd = d; + mind = infd; + pret = p2; + index = i; + end + + end + + % Check the nearest 5 neighbours in both directions to find the second + % best match + infd = inf; + for i = 1:5 + + + p2 = pset(index+i,1:2); %check the distance and save the best match + p3 = pset(index-i,1:2); %check the distance and save the best match + d = sqrt( (p(1)-p2(1))^2 + (p(2)-p2(2))^2 ); + d2 = sqrt( (p(1)-p3(1))^2 + (p(2)-p3(2))^2 ); + + [d im] = min(d,d2); + + if d < infd + infd = d; + mind2 = infd; + pret2 = p2; + index2 = index-(i * (-1+(im-1)) );; + end + + + end + + pret = [pret pret2]; + index = [index index2]; +if size(pret,1) < 2 + error('No associations found'); +end + +end diff --git a/ScanMatching/2D/FMT-Based/fmtsm.m b/ScanMatching/2D/FMT-Based/fmtsm.m new file mode 100644 index 0000000..2e822be --- /dev/null +++ b/ScanMatching/2D/FMT-Based/fmtsm.m @@ -0,0 +1,316 @@ +%FMTSM Spectral registration takes proï¬t of the fact that image translation in +% the space domain manifest itself as a phase shift in the +% frequency domain. Hence, given the Fourier transform +% of two translated but not rotated images, corresponding +% to the Sref and Snew scans, it is possible to compute +% their phase shift using the Phase Only Matched Filter +% (POMF). Next, the inverse Fourier transform of the +% phase difference is a Dirac delta signal centred in the +% corresponding image translation. Computing its peak, +% means solving for the image translation. On the other hand, +% a rotation applied to an image in the space +% domain is manifested as a rotation of the Fourier spectra. +% However, the rotation is manifested as a signal shift in +% the polar representation of the Fourier Magnitude, which +% may again be computed using a POMF over the polar re- +% sampled Fourier transform of the images and computing +% the peak of the corresponding Dirac Delta signal. It is +% worth noting, that it is necessary to ï¬rst solve for the +% rotation and next, solve for the translation. + +% [25] A. B. Heiko Bulow, Max Pï¬ngsthorn, “Using Robust Spectral Regis- +% tration for Scan Matching of Sonar Range Data ,†in The 7th IFAC +% Symposium on Intelligent Autonomous Vehicles, IAV 2010. IEEE, 2010. +function [ R t NI] = fmtsm( ScanRef, ScanNew, motion ) +% In: +% ScanRef: localCart[2xN], points cartesian +% ScanRef: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + + + + + +global DEBUG Opt; + +%Calculate the motion parameters +u = motion.con.u; +pose(3) = 0; + + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; +NI = 1; +borders = 0; + +rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; +Snew_Or =ScanNew.localCart; +magnThreshold = 10; + +% S New referenced to S Ref coordinate system +for z = 1:size(ScanNew.localCart,1) + ScanNew.localCart(z,1:3) = (rTn*[ScanNew.localCart(z,1:2) 1]')' ; + th = normAngle(ScanNew.localPolar(z,1) + yaw); + ro = ScanNew.localPolar(z,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + ScanNew.localPolar(z,2) = sqrt(rc^2 + rs^2); + ScanNew.localPolar(z,1) = atan2(rs,rc); +end + +ScanNew.localCart(:,3) = pose(3); + + +% Make the occupancy grid as big as to contain both scans. The occupancy +% grid is also our binary image that will be processed through the fourier +% mellin transform. Other methods might be used which take into +% consideration the sensor uncertainty and sensor type. +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef.localCart); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNew.localCart); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - borders; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + borders; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - borders; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + borders; + +OccRef = makeOccupancyGrid(ScanRef.localCart, PARAM); +OccNew = makeOccupancyGrid(ScanNew.localCart, PARAM); + +DEBUGALL = 0; + +Sref = ScanRef.localCart; +bmw = blackman(128); + + +t_res(1) = Opt.scanmatcher.Br(2) / 128; +t_res(2) = Opt.scanmatcher.Br(2) / 128; + +gridsize = 128; + +%% Reference Scan (S REF) + +%OccRef.grid = OccRef.grid .* hann2d(imagesize(1),imagesize(2)); +%OccNew.grid = OccNew.grid .* hann2d(imagesize(1),imagesize(2)); + + +% 2D FFT of S Ref +fftImage1 = fftshift(fft2(OccRef.grid)); +magn1 = (abs((fftImage1))); +phase1 = angle(fftImage1); + +if (DEBUG.all || DEBUG.fmtsm) && DEBUGALL + % plot the image, its magnitude and its phase + figure; + subplot(1,3,1), imshow(OccRef.grid, [0 1]); + subplot(1,3,2), imshow(magn1,[0 15]); + subplot(1,3,3), imshow(phase1,[-pi/2 pi/2]); +end + +% resample into polar coordinates the magnitude so to have a FMT descriptor +% in MAGNPOLAR1 and filter it +magnPolar1 = resamplePolar(magn1,gridsize); +magnPolar1(find(magnPolar1 bestval) + bestval = score; + besti = k; + bestx = thyp(1); + besty = thyp(2); + bestr = thyp(3); + + if DEBUG.fmtsm || DEBUG.all + % display the new best match found + figure(fmt_res); + hold on + axis equal + grid on + displayPoints(Sref,'g',0); + displayPoints(lastgrid,'r',0); + end + end + + end + end +end + +%% Result +if DEBUG.fmtsm || DEBUG.all + % Display the result + figure('Name','FMT RES'); hold on + title( ['\theta = ' num2str(bestr) ', x=' num2str(bestx) ', y=' num2str(besty) ', score= ' num2str(bestval) ]); + displayPoints(Sref,'g',0); + displayPoints(applyTransform2Scan2D(Snew_Or, bestr+yaw0, [x0 y0] + [bestx besty]),'r',0); + axis equal +end + +t = [bestx besty] ; +R = (bestr); + +if DEBUG.fmtsm || DEBUG.all + close(fmt_res); +end + +end diff --git a/ScanMatching/2D/FMT-Based/hann2d.m b/ScanMatching/2D/FMT-Based/hann2d.m new file mode 100644 index 0000000..2f61cc5 --- /dev/null +++ b/ScanMatching/2D/FMT-Based/hann2d.m @@ -0,0 +1,47 @@ +% HANNING.M +% +% COPYRIGHT : (c) NUHAG, Dept.Math., University of Vienna, AUSTRIA +% http://nuhag.eu/ +% Permission is granted to modify and re-distribute this +% code in any manner as long as this notice is preserved. +% All standard disclaimers apply. +% +% HANNING.M - returns the N-point Hanning window in a column vector. +% +% Input : n = signal length (or column vector?) +% +% Output : N-point Hanning window +% +% Usage : w = hanning(n); or 2D: hanning(n1,n2); +% +% Comments : empty argument shows plot, modification of original MATLAB M-file + +% HGFei + +function w = hann2d(n,n2) + +if nargin == 2; + w1 = hann2d(n); + w2 = hann2d(n2); + w = w1(:) * w2; + return; +end; + +if nargin == 0 + help hann2d; + return; +end; + +[h,w] = size(n); +if h*w > 1; +n = max(h,w); +end; + +u = .5*(1 - cos(2*pi*(0:n-1)'/(n-1))); + +if h*w > 1; +w = zeros(h,w); +w(:) = u; +else +w = u(:).'; +end; diff --git a/ScanMatching/2D/FMT-Based/pprocess.m b/ScanMatching/2D/FMT-Based/pprocess.m new file mode 100644 index 0000000..e9d9487 --- /dev/null +++ b/ScanMatching/2D/FMT-Based/pprocess.m @@ -0,0 +1,23 @@ +function [ B ] = pprocess( A, thr, ncolors ) +%PPROCESS Filter the image A using a threshold THR and resampling in +%NCOLORS + + + +mx = max(max(A)); + +dif = ncolors/(mx - thr); + +for k = 1:size(A,1) + for v = 1:size(A,2) + if A(k,v) > thr + B(k,v) = floor((A(k,v) - thr) * (dif)); + else + B(k,v) = 0; + end + end +end + + +end + diff --git a/ScanMatching/2D/FMT-Based/resamplePolar.m b/ScanMatching/2D/FMT-Based/resamplePolar.m new file mode 100644 index 0000000..b21bbae --- /dev/null +++ b/ScanMatching/2D/FMT-Based/resamplePolar.m @@ -0,0 +1,19 @@ +function [ B ] = resamplePolar( A, N ) +%RESAMPLEPOLAR Resample a magnitude spectra into a polar spectra of size N +gridsize = [N N]; +imagesize = [size(A,1) size(A,2)]; +B = zeros(N,N); + +% Resample into polar coordinates +for k = 1:gridsize(1) + for j = 1:gridsize(2) + K = floor( (((imagesize(1)/2)*k)/gridsize(1))*cos((pi*j)/gridsize(2)) + imagesize(1)/2 ) + 1; + J = floor( (((imagesize(1)/2)*k)/gridsize(1))*sin((pi*j)/gridsize(2)) + imagesize(1)/2 ) + 1; + K = min(K,size(A,1)); + J = min(J,size(A,2)); + B(k,j) = A(K,J); + end +end + +end + diff --git a/ScanMatching/2D/GA/Selection/select_duckett.m b/ScanMatching/2D/GA/Selection/select_duckett.m new file mode 100644 index 0000000..34ca173 --- /dev/null +++ b/ScanMatching/2D/GA/Selection/select_duckett.m @@ -0,0 +1,53 @@ +function [ B ] = select_duckett( A, fit ) +%SELECT_DUCKETT selection process for genetic algorithms. Given a set A and +% the fitness values, a new population of same size is sorted according to +% the method used by + +% Genetic Algorithm for Simultaneous Localization and Mapping +% Tom Duckett +% Centre for Applied Autonomous Sensor Systems +% Dept. of Technology, Orebro University +% SE-70182 Orebro, Sweden + +% Values are normalized and resampled in the interval [ 0.5 1.5] + +nchr = size(A,1); +sfit = sum(fit); +fit = fit./sfit; +[fit ifit] = sort(fit,'descend'); +minf = fit(size(fit,1)); +maxf = fit(1); + +intv = (maxf - minf); + +fit = 0.5 + ( (fit-minf) / intv); + +% entries with integer part greater than 0 are automatically selected. +% A is remapped with the sorting information +A = remapMatrix(A,ifit); +B = zeros(nchr,3); +nchos = 0; +for j = 1:size(A,1) + if (fit(j) >= 1) + nchos = nchos+1; + B(nchos,:) = A(j,:); + end +end + +remtc = size(A,1) - nchos; + +% the remaining entries are drawn using the fractional part as probability +% to be selected +for k = 1:remtc-1 + for j = 1: (nchr-k+1) + v = rand; + if v < fit(j) + B(nchos+k+1,:) = A(j,:); + A(j,:) = []; + break + end + end +end + +end + diff --git a/ScanMatching/2D/GA/Selection/select_rw.m b/ScanMatching/2D/GA/Selection/select_rw.m new file mode 100644 index 0000000..6b6bb65 --- /dev/null +++ b/ScanMatching/2D/GA/Selection/select_rw.m @@ -0,0 +1,42 @@ +function [ B ] = select_rw( A, fit ) +% SELECT_RW Roulette Wheel selection for Genetic Algorithms. Given a set A of +% points (chromosomes) and a fitting array representing the fitness value +% of each chromosome within our problem. Draw N new points using the +% Roulette Wheel system. The points are selected in couples. + + +% Normalize the fit function. Each entry contains the sum of the previous +% fits normalized. + +nchr = size(A,1); + +sfit = sum(fit);sumfit = 0; +wfit = zeros(nchr,1); + +for j = 1:nchr + wfit(j) = sumfit + fit(j)/sfit; + sumfit = wfit(j); +end + +c1 = -1; + +B = zeros(nchr,3); + +for j = 1:nchr + v = rand(1); % Using a uniformely generated number, draw one point + if v < wfit(1) + c1=1; + end + for h = 1:nchr-1 + % let's check in which interval it falls + if v > wfit(h) && v < wfit(h+1) + c1 = h+1; + end + end + B(j,:) = A(c1,:); +end + + + +end + diff --git a/ScanMatching/2D/GA/Selection/select_tournament.m b/ScanMatching/2D/GA/Selection/select_tournament.m new file mode 100644 index 0000000..4561ab6 --- /dev/null +++ b/ScanMatching/2D/GA/Selection/select_tournament.m @@ -0,0 +1,54 @@ +function [ B ] = select_tournament( A, fit ) +%SELECT_TOURNAMENT Tournament selection for Genetic Algorithms. Given a set A of +% points (chromosomes) and a fitting array representing the fitness value +% of each chromosome within our problem. Draw N new points using the +% Roulette Wheel system. The points are selected in couples. + +% A good reference +% http://www.edc.ncl.ac.uk/highlight/rhjanuary2007g02.php/ + +nchr = size(A,1); + +% We select only the best indexes according to a pressure (threshold) value +% to improve the goodness of the breeding population + +press = mean(fit)*0.8 +ksize = 10; % Tournament size + + B = zeros(nchr,3); +% We now want to generate a new set of surviving chromosomes with +% replacement (ie. a chromosomes can be chosen multiple times) +for j = 1:nchr + + mpool = []; + + + % Choose KSIZE random elements to create a tournament + for k = 1:ksize + mpool(k) = fit(floor(rand*nchr)+1); + end + + % Normalize the fitness value using the new surviving population + sfit = sum(mpool); + nfit = mpool./sfit; + [nfit infit] = sort(nfit,'descend'); + + nmp = size(mpool,1); + + % Select the best candidate using a probability to give chance to worse + % candidates + for k = 1:nmp + v = rand(1) % Using a uniformely generated number, draw one value + p = nfit(k) + if v > p*((1-p)^(k-1)) % a candidate has probability to be chosen as p*((1-p)^j) + break; + end + + end + B(j,:) = A(infit(k),:); + +end + +end + + diff --git a/ScanMatching/2D/GA/Selection/select_truncate.m b/ScanMatching/2D/GA/Selection/select_truncate.m new file mode 100644 index 0000000..691719c --- /dev/null +++ b/ScanMatching/2D/GA/Selection/select_truncate.m @@ -0,0 +1,20 @@ +function [ B ] = select_truncate( A, fit ) +%SELECT_TRUNCATE Simply use a threshold to eliminate the values with worse +%entry. The value is calculated using the mean of the population and it is +%scaled by a factor SC, the smaller it gets the more tollerant the +%selection will be. Using strict tolerance increase fast convergence + +sc = 0.8; +sfit = sum(fit); +m = mean(fit./sfit) * sc; +B = []; +nchos = 0; +for j = 1:size(A,1) + if (fit >= m) + nchos = nchos+1; + B(nchos,:) = A(j,:); + end +end + +end + diff --git a/ScanMatching/2D/GA/fitnessGrid.m b/ScanMatching/2D/GA/fitnessGrid.m new file mode 100644 index 0000000..0471d48 --- /dev/null +++ b/ScanMatching/2D/GA/fitnessGrid.m @@ -0,0 +1,45 @@ +function [ score A] = fitnessGrid( A, B, motion ) +%FITNESSGRID defines a fitness function for the grid B. It evaluates how +%good the scan A, applied the transformation MOTION, fits in the grid. It +%counts without any explicit association how many cells overlap. A cell is +% said to overlap with another if it falls under a range of a 5*5 area. +x0 = motion(1); +y0 = motion(2); +yaw0 = motion(3); +A = applyTransform2Scan2D(A, yaw0, [x0 y0]); +sizen = size(A,1); +score = 0; +gr = B.grid; +gsx = size(B.grid,1); +gsy = size(B.grid,2); +sb = 2; + + +% Check every point in the scan A +for i = 1:sizen + [a b] = getCoordinates(A(i,:),B.PARAM); + % Check the surraounding area + scored = 0; + for j = 1:sb*2 + for k = 1:sb*2 + % Restrict the search area inside the grid map + ix = a - sb + k -1; + iy = b - sb + j -1; + if ix > gsx || ix <= 0 || iy > gsy || iy <= 0 + continue + end + if gr(ix,iy) == 1 + score = score + 1 ; + scored = 1; + end + end + if scored + continue + end + end + + +end + +end + diff --git a/ScanMatching/2D/GA/fitnessGrid.m~ b/ScanMatching/2D/GA/fitnessGrid.m~ new file mode 100644 index 0000000..a0b181e --- /dev/null +++ b/ScanMatching/2D/GA/fitnessGrid.m~ @@ -0,0 +1,40 @@ +function [ score ] = fitnessGrid( A, B, motion ) +%FITNESSGRID defines a fitness function for the grid B. It evaluates how +%good the scan A, applied the transformation MOTION, fits in the grid. It +%counts without any explicit association how many cells overlap. A cell is +% said to overlap with another if it falls under a range of a 5*5 area. +x0 = motion(1); +y0 = motion(2); +yaw0 = motion(3); +A = applyTransform2Scan2D(A, yaw0, [x0 y0]); +sizen = size(A,1); +score = 0; +gr = B.grid; +gsx = size(B.grid,1); +gsy = size(B.grid,2); +sb = 5; + +% Check every point in the scan A +for i = 1:sizen + [a b] = getCoordinates(A(i,:),B); + % Check the surraounding area + for j = 1:sb*2 + for k = 1:sb*2 + % Restrict the search area inside the grid map + ix = a - sb + k -1; + iy = b - sb + j -1; + if ix > gsx || ix <= 0 || iy > gsy || iy <= 0 + continue + end + if gr(ix,iy) == 1 + score = score + 1 - (((sb*2-(j + k)))/10)*0. ; + + end + end + end + + +end + +end + diff --git a/ScanMatching/2D/GA/ga.m b/ScanMatching/2D/GA/ga.m new file mode 100644 index 0000000..c35afa6 --- /dev/null +++ b/ScanMatching/2D/GA/ga.m @@ -0,0 +1,259 @@ +% GA It generates poses +% according to the genetic evolution. At every iteration, a +% number of poses is generated evolving from the previous +% set of poses. The poses that do not have a certain score +% are discarded. The discarded poses are replaced with +% genetic variations of the best poses. + +% [1] Mobile robot motion estimation by 2D scan matching with genetic and iterative closest point algorithms +% Martinez, J. L. Gonzalez, J. Morales, J. Mandow, A. Garcia-Cerezo, A. J. +% JOURNAL OF FIELD ROBOTICS +% 2006, VOL 23; NUMBER 1, pages 21-34 + +% [2] Genetic Algorithm for Simultaneous Localization and Mapping +% Tom Duckett +% Centre for Applied Autonomous Sensor Systems +% Dept. of Technology, Orebro University +% SE-70182 Orebro, Sweden + +% [3] Fast Genetic Scan Matching Using +% Corresponding Point Measurements in Mobile Robotics +% Kristijan Lenac1,2 , Enzo Mumolo1 , and Massimiliano Nolich1 +% DEEI, University of Trieste, Via Valerio 10, Trieste, Italy +% AIBS Lab, Via del Follatoio 12, Trieste, Italy +function [ R t NI ] = ga( ScanRef, ScanNew, motion ) +% In: +% ScanRef: localCart[2xN], points cartesian +% ScanRef: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + +global Opt DEBUG + +niter = Opt.scanmatcher.iterations; + +nchr = 50; % N chromosomes. ONLY EVEN NUMBERS +gnsz = 24; % 8-bit representation +chsz = gnsz*3; % Chromosome size (pose) +ScanNew = ScanNew.localCart; +ScanRef = ScanRef.localCart; + +sizenew = size(ScanNew,1); + +% motion estimation q0 +u = motion.con.u; + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); +% reference S New in S Ref coordinate system +ScanNewTR = applyTransform2Scan2D(ScanNew, yaw0, [x0 y0]); + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNewTR); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + +% define the boundaries of search space +bound_x=Opt.scanmatcher.Br(2); +bound_y=Opt.scanmatcher.Br(2); +bound_th=Opt.scanmatcher.Br(1); + +bounds_rand = [bound_x bound_y bound_th]; + +popchr = zeros(nchr,3); + +% randomly generate thee initial population +for j = 1:nchr + tmpx = (rand(1)-0.5)*bound_x*2; + tmpy = (rand(1)-0.5)*bound_y*2; + tmpth = (rand(1)-0.5)*bound_th*2; + + popchr(j,:) = [tmpx tmpy tmpth]; + +end + +% Loop limit as ending criteria +it = 0; +bestfit = sizenew*0.9; + + +lasterror=[]; +corr=[]; +lastbest = [x0 y0 yaw0]; + + +if DEBUG.ga || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + +while it < niter + it = it+1; + % Evaluate our chromosomes using our fitness function FITNESSGRID which + % checks a small area around each point using a direct lookup. No + % association is needed + maxfit = -inf; + fit = zeros(nchr,1); + + selchr = zeros(nchr,2); + newpopchr = zeros(nchr,3); + + + + for j = 1:nchr + [fit(j) TRF(j).grid] = fitnessGrid2(ScanNewTR, Occ, popchr(j,:)); + if fit(j) > maxfit + maxfit = fit(j); + maxfiti = j; + end + end + + corr = [corr; lastbest - popchr(maxfiti,:) ]; + + if DEBUG.ga || DEBUG.all + set(opts.plot_r,'XData',ScanRef(:,1),'YData',ScanRef(:,2)); + set(opts.plot_n,'XData',TRF(maxfiti).grid(:,1),'YData',TRF(maxfiti).grid(:,2)); + drawnow + end + + + lasterror = [lasterror abs(log(maxfit/bestfit))-1 ] + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + lastbest = popchr(maxfiti,:); + + %return if convergence is achieved + if checkConv(lasterror, corr,0) || isinf(lasterror(end)) || sum(diag(cov(popchr)))<0.1 + break; + end + + + %% SELECTION: we have to generate a new population of surviving NCHR chromosomes + % depending on the fitness function. There are different methods to + % achieve this + + survpop = select_duckett(popchr, fit); + + + %% CROSSOVER: the crossover operation merge the two fathers with a + % probability of 0.7 (common value). The merging is done bitwise, + % exchanging all genes in the chromosomes sequence after a randomly + % selected gene. We use a 8-bit representation chromosomes + for j = 1:nchr/2 + + % Search for janitors, without reselecting the same couple twice. + % This ensures diversity. Also try to avoid breeding the same + % chromosome + nbreeding = size(survpop,1); + conds = 1; + while conds > 0 + + r1 = floor(rand(1)*nbreeding)+1; + p1 = survpop((r1),:); + + r2 = r1; + % Draw a new point and check dat it's not too close (or the + % same) of the first one. + while r2 == r1 + r2 = floor(rand(1)*nbreeding)+1; + end + p2 = survpop((r2),:); + ddiff = p1 - p2; + dstc = sqrt(ddiff(1)^2 + ddiff(2)^2) <= 0.000; + + if sum(dstc) > 0 + conds = 1; + continue + end + + % we don't want to select the same pair multiple times. + conds = searchPair(selchr,[r1 r2]); + end + + p1 = max(0,floor( ((p1 + bounds_rand))*100000)) ; % We need integer representation. We do a cast assuming a single precision float + p2 = max(0,floor( ((p2 + bounds_rand))*100000)) ; + selchr(j,:) = [ r1 r2 ]; % We keep track of the already selected pairs + + + % Not always a crossover operator is applied. In this case we just + % select the first choice + % Generate the binary representation in Gray code + chgc1 = [dec2bin(dec2gc(p1(1)),gnsz) dec2bin(dec2gc(p1(2)),gnsz) dec2bin(dec2gc(p1(3)),gnsz)]; + chgc2 = [dec2bin(dec2gc(p2(1)),gnsz) dec2bin(dec2gc(p2(2)),gnsz) dec2bin(dec2gc(p2(3)),gnsz)]; + + % Decides whether to use the crossover operator or not + crossrate = rand(1); + if crossrate > 0.9 + chnew1 = chgc1; + chnew2 = chgc2; + else + sgene = floor(rand(1) * (chsz-2)) + 1; + slength = floor(rand(1) * (chsz - (sgene+2) ) ) + 1; + bounds = sgene+1:sgene+1+slength; + % Apply the crossover operator and reconverts in Decimal + chnew1 = [ chgc1(1:sgene) chgc2(bounds) chgc1(bounds(end)+1:end)]; + chnew2 = [ chgc2(1:sgene) chgc1(bounds) chgc2(bounds(end)+1:end)]; + end + + + %% MUTATION: Once we have our new chromosome, we might apply a mutation operator + % with very low rate. Mutation might have bad effects on diversity. + % An application of mutation usually brings to significant changes + % and improbable solutions. IMPROVE to avoid this + mutationrate = rand(1); + if mutationrate < 0.001 + % we select one bit and flip it + sgene = floor(rand(1) * chsz) + 1; + b = num2str(bitxor( sscanf(chnew1(sgene),'%i') ,1)); + chnew1(sgene) = b; + end + mutationrate = rand(1); + if mutationrate < 0.001 + % we select one bit and flip it + sgene = floor(rand(1) * chsz) + 1; + b = num2str(bitxor( sscanf(chnew2(sgene),'%i') ,1)); + chnew2(sgene) = b; + end + + nch1 = [ gc2dec(bin2dec(chnew1(1:gnsz))) /100000 , ... + gc2dec(bin2dec(chnew1(gnsz+1:gnsz*2)))/100000, ... + gc2dec(bin2dec(chnew1(gnsz*2+1:gnsz*3)))/100000]-bounds_rand; + nch2 = [gc2dec(bin2dec(chnew2(1:gnsz)))/100000, ... + gc2dec(bin2dec(chnew2(gnsz+1:gnsz*2)))/100000, ... + gc2dec(bin2dec(chnew2(gnsz*2+1:gnsz*3)))/100000]-bounds_rand; + + newpopchr( ((j-1)*2) +1,:) = nch1; + newpopchr( ((j-1)*2) +2,:) = nch2; + + end + + popchr = single(newpopchr); +end + +%% result +t = popchr(maxfiti,1:2); +R = popchr(maxfiti,3); +NI = it; +end + diff --git a/ScanMatching/2D/GA/ga.m~ b/ScanMatching/2D/GA/ga.m~ new file mode 100644 index 0000000..8a3186d --- /dev/null +++ b/ScanMatching/2D/GA/ga.m~ @@ -0,0 +1,260 @@ +function [ R t NI ] = ga( ScanRef, ScanNew, motion ) +% GA returns a rotational and translational estimation using a genetic +% algorithm search. The algorithm is based on a series of publications. + +% [1] Mobile robot motion estimation by 2D scan matching with genetic and iterative closest point algorithms +% Martinez, J. L. Gonzalez, J. Morales, J. Mandow, A. Garcia-Cerezo, A. J. +% JOURNAL OF FIELD ROBOTICS +% 2006, VOL 23; NUMBER 1, pages 21-34 + +% [2] Genetic Algorithm for Simultaneous Localization and Mapping +% Tom Duckett +% Centre for Applied Autonomous Sensor Systems +% Dept. of Technology, Orebro University +% SE-70182 Orebro, Sweden + +% [3] Fast Genetic Scan Matching Using +% Corresponding Point Measurements in Mobile Robotics +% Kristijan Lenac1,2 , Enzo Mumolo1 , and Massimiliano Nolich1 +% DEEI, University of Trieste, Via Valerio 10, Trieste, Italy +% AIBS Lab, Via del Follatoio 12, Trieste, Italy + + +% The idea is to apply a genetic evuolution to a random sample obtained +% from a roto/translational space. This space is defined using a bound +% given by the error bound. +global Opt + +convg = Opt.scanmatcher.convalue; +niter = Opt.scanmatcher.iterations; + +nchr = 200; % N chromosomes. ONLY EVEN NUMBERS +gnsz = 24; % 8-bit representation +chsz = gnsz*3; +ScanNew = ScanNew.localCart; +ScanRef = ScanRef.localCart; + +sizenew = size(ScanNew,1); +sizeref = size(ScanRef,1); + +u = motion.con.u; +lrangle = u(2); +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +ScanNewTR = applyTransform2Scan2D(ScanNew, yaw0, [x0 y0]); + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNewTR); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + +% Make Search Space using noise information. TODO +%Use uniform random sampling to draw the initial population of chromosomes +global DEBUG + +bound_x = motion.state.P_Added(1,1); +bound_y = motion.state.P_Added(2,2); +bound_th = motion.state.P_Added(3,3); + + +bound_x=Opt.scanmatcher.Br(2); +bound_y=Opt.scanmatcher.Br(2); +bound_th=Opt.scanmatcher.Br(1); + +bounds_rand = [bound_x bound_y bound_th]; + +popchr = zeros(nchr,3); + +% Select initial population +for j = 1:nchr + tmpx = (rand(1)-0.5)*bound_x*2; + tmpy = (rand(1)-0.5)*bound_y*2; + tmpth = (rand(1)-0.5)*bound_th*2; + + popchr(j,:) = [tmpx tmpy tmpth]; + +end + +% Loop limit as ending criteria +it = 0; +bestfit = sizenew*0.9; + + +lasterror=[]; +corr=[]; +lastbest = [x0 y0 yaw0]; +%% Improve error check + +while it < niter + it = it+1; + % Evaluate our chromosomes using our fitness function FITNESSGRID which + % checks a small area around each point using a direct lookup. No + % association is needed + maxfit = -inf; + fit = zeros(nchr,1); + + selchr = zeros(nchr,2); + newpopchr = zeros(nchr,3); + + + + for j = 1:nchr + [fit(j) TRF(j).grid] = fitnessGrid2(ScanNewTR, Occ, popchr(j,:)); + if fit(j) > maxfit + maxfit = fit(j); + maxfiti = j; + end + end + + maxfit + + corr = [corr; lastbest - popchr(maxfiti,:) ] + +if DEBUG.ga || DEBUG.all + figure; + hold on + axis equal + grid on + displayPoints(ScanRef,'g',0); + displayPoints(TRF(maxfiti).grid,'r',0); +end + + lasterror = [lasterror abs(log(maxfit/bestfit))-1 ] + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + lastbest = popchr(maxfiti,:); + + %return if convergence is achieved + if checkConv(lasterror, corr,0) + break; + end + + + %% SELECTION: we have to generate a new population of surviving NCHR chromosomes + % depending on the fitness function. There are different methods to + % achieve this + + survpop = select_duckett(popchr, fit); + + % Convergence check done on covariance +% covc = popCovariance(survpop(1:10,:)) +% if sum(diag(covc)) < 0.05 %convg*3 +% break; +% end + + %% CROSSOVER: the crossover operation merge the two fathers with a + % probability of 0.7 (common value). The merging is done bitwise, + % exchanging all genes in the chromosomes sequence after a randomly + % selected gene. We use a 8-bit representation chromosomes + for j = 1:nchr/2 + + % Search for janitors, without reselecting the same couple twice. + % This ensures diversity. Also try to avoid breeding the same + % chromosome + nbreeding = size(survpop,1); + conds = 1; + while conds > 0 + + r1 = floor(rand(1)*nbreeding)+1; + p1 = survpop((r1),:); + + r2 = r1; + % Draw a new point and check dat it's not too close (or the + % same) of the first one. + while r2 == r1 + r2 = floor(rand(1)*nbreeding)+1; + end + p2 = survpop((r2),:); + ddiff = p1 - p2; + dstc = sqrt(ddiff(1)^2 + ddiff(2)^2) <= 0.000; + + if sum(dstc) > 0 + conds = 1; + continue + end + + % we don't want to select the same pair multiple times. + conds = searchPair(selchr,[r1 r2]); + end + + p1 = mafloor( ((p1 + bounds_rand))*100000) ; % We need integer representation. We do a cast assuming a single precision float + p2 = floor( ((p2 + bounds_rand))*100000) ; + selchr(j,:) = [ r1 r2 ]; % We keep track of the already selected pairs + + + % Not always a crossover operator is applied. In this case we just + % select the first choice + % Generate the binary representation in Gray code + chgc1 = [dec2bin(dec2gc(p1(1)),gnsz) dec2bin(dec2gc(p1(2)),gnsz) dec2bin(dec2gc(p1(3)),gnsz)]; + chgc2 = [dec2bin(dec2gc(p2(1)),gnsz) dec2bin(dec2gc(p2(2)),gnsz) dec2bin(dec2gc(p2(3)),gnsz)]; + + % Decides whether to use the crossover operator or not + crossrate = rand(1); + if crossrate > 0.9 + chnew1 = chgc1; + chnew2 = chgc2; + else + sgene = floor(rand(1) * (chsz-2)) + 1; + slength = floor(rand(1) * (chsz - (sgene+2) ) ) + 1; + bounds = sgene+1:sgene+1+slength; + % Apply the crossover operator and reconverts in Decimal + chnew1 = [ chgc1(1:sgene) chgc2(bounds) chgc1(bounds(end)+1:end)]; + chnew2 = [ chgc2(1:sgene) chgc1(bounds) chgc2(bounds(end)+1:end)]; + end + + + %% MUTATION: Once we have our new chromosome, we might apply a mutation operator + % with very low rate. Mutation might have bad effects on diversity. + % An application of mutation usually brings to significant changes + % and improbable solutions. IMPROVE to avoid this + mutationrate = rand(1); + if mutationrate < 0.001 + % we select one bit and flip it + sgene = floor(rand(1) * chsz) + 1; + b = num2str(bitxor( sscanf(chnew1(sgene),'%i') ,1)); + chnew1(sgene) = b; + end + mutationrate = rand(1); + if mutationrate < 0.001 + % we select one bit and flip it + sgene = floor(rand(1) * chsz) + 1; + b = num2str(bitxor( sscanf(chnew2(sgene),'%i') ,1)); + chnew2(sgene) = b; + end + + nch1 = [ gc2dec(bin2dec(chnew1(1:gnsz))) /100000 , ... + gc2dec(bin2dec(chnew1(gnsz+1:gnsz*2)))/100000, ... + gc2dec(bin2dec(chnew1(gnsz*2+1:gnsz*3)))/100000]-bounds_rand; + nch2 = [gc2dec(bin2dec(chnew2(1:gnsz)))/100000, ... + gc2dec(bin2dec(chnew2(gnsz+1:gnsz*2)))/100000, ... + gc2dec(bin2dec(chnew2(gnsz*2+1:gnsz*3)))/100000]-bounds_rand; + +% nch1 +% nch2 + + newpopchr( ((j-1)*2) +1,:) = nch1; + newpopchr( ((j-1)*2) +2,:) = nch2; + %newpopchr; + end + + popchr = single(newpopchr); +end + +t = [popchr(maxfiti,1:2) 0] +R = e2q([0 0 popchr(maxfiti,3)]) +NI = it; +end + diff --git a/ScanMatching/2D/GA/popCovariance.m b/ScanMatching/2D/GA/popCovariance.m new file mode 100644 index 0000000..40b1025 --- /dev/null +++ b/ScanMatching/2D/GA/popCovariance.m @@ -0,0 +1,17 @@ +function [ B ] = popCovariance( A ) +%CHECKCOVARIANCE Given a population of chromosomes, generate a covariance +% matrix useful to analyze convergence + +meana = mean(A); + +B = zeros(3,3); +for j= 1: size(A,1) + t = A(j,:) - meana; + t = t'*t; + B = B+t; +end +B = B/size(A,1); + +end + + diff --git a/ScanMatching/2D/GMapping/gmapping.m b/ScanMatching/2D/GMapping/gmapping.m new file mode 100644 index 0000000..2d69f93 --- /dev/null +++ b/ScanMatching/2D/GMapping/gmapping.m @@ -0,0 +1,175 @@ +% GMAPPING: Gmapping/VascoThis method was +% presented as the VASCO scan matcher in the CARMEN +% framework. Later, it was used by GMapping with minor +% changes. The idea is to search for a better pose in the +% borders of the axis of an uncertainty ellipse. The axis of +% the ellipse have ï¬xed length δd . At every iteration the +% axis length is divided by 2. In [20] authors redeï¬ned the +% ellipse size using the pose uncertainty thus reducing the +% computational time by skipping unlikely poses. + +% [1] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Improved +% Techniques for Grid Mapping with Rao-Blackwellized Particle Filters, +% IEEE Transactions on Robotics, 2006 (link) + +% [2] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: +% Improving Grid-based SLAM with Rao-Blackwellized Particle Filters +% by Adaptive Proposals and Selective Resampling, +% In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2005 (link) + +% The idea is to apply use a gradient descent method to search for possible +% good poses. The space of search is limited by a boundary + +function [ R t NI ] = gmapping( ScanRef, ScanNew, motion ) +% In: +% ScanRef: localCart[2xN], points cartesian +% ScanRef: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + +global Opt DEBUG + + +niter = Opt.scanmatcher.iterations; + +ScanNew = ScanNew.localCart; +ScanRef = ScanRef.localCart; + +sizenew = size(ScanNew,1); + +% Initial motion estiamtion q0 +u = motion.con.u; + + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +%Apply the initial estimation to S NEW +ScanNewTR = applyTransform2Scan2D(ScanNew, yaw0, [x0 y0]); + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNewTR); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + +it = 0; +% some initialization about +% score +lastbest =[x0 y0 yaw0]; +lastgrid = ScanNewTR; +bestfit = sizenew*0.9; +initfit = fitnessGrid2(ScanNewTR, Occ, [x0 y0 yaw0]); +maxfit = initfit; + +% and search procedures +incr_x=Opt.scanmatcher.Br(2); +incr_y=Opt.scanmatcher.Br(2); +incr_th=Opt.scanmatcher.Br(1); + +lasterror=[]; +corr=[]; + +if DEBUG.gmapping || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + +while it < niter + it = it+1; + + + %return if convergence is achieved + if checkConv(lasterror, corr, 0) + break; + end + + maxfiti = -1; + its = 0; + + % we loop until we find a better solution or a certain number of + % iterations is reached + while maxfiti < 0 && incr_x > 0.0001 && its < niter*2 + + % we define a set of steps around the axis considering the + % uncertainty bounds in INCR. + + dirs = [ + [incr_x 0 0]; + + [-incr_x 0 0] ; + + [0 incr_y 0] ; + + [0 -incr_y 0] ; + + [0 0 incr_th] ; + + [0 0 -incr_th] ]; + + % Evaluate our pose using our fitness function FITNESSGRID which + % checks a small area around each point using a direct lookup. No + % association is needed + + for j=1:size(dirs,1) + [fit(j) TRF(j).grid] = fitnessGrid2(ScanNewTR, Occ, dirs(j,:) ); + if fit(j) > maxfit + maxfit = fit(j); + maxfiti = j; + end + end + + % at each iteration we divide by 2 the bounds + incr_x = incr_x*0.5; + incr_y = incr_y*0.5; + incr_th = incr_th*0.5; + + its = its+1; + end + + % if a solution better than the current one is found, save the result + if maxfiti > 0 + corr = [corr; lastbest - dirs(maxfiti,:) ]; + lastbest = dirs(maxfiti,:); + lastgrid = TRF(maxfiti).grid; + else + corr = [corr; 0 0 0]; + end + + if DEBUG.gmapping || DEBUG.all + set(opts.plot_r,'XData',ScanRef(:,1),'YData',ScanRef(:,2)); + set(opts.plot_n,'XData',lastgrid(:,1),'YData',lastgrid(:,2)); + drawnow + end + + + lasterror = [lasterror abs(log(maxfit/bestfit))-1 ]; + + % update error + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + +end + +%% Result +% Returns the minimizing Q = [t R] +t = lastbest(1:2) ; +R = lastbest(3); + +NI = it; + +end + diff --git a/ScanMatching/2D/GMapping/gmapping.m~ b/ScanMatching/2D/GMapping/gmapping.m~ new file mode 100644 index 0000000..0cfa625 --- /dev/null +++ b/ScanMatching/2D/GMapping/gmapping.m~ @@ -0,0 +1,132 @@ +function [ R t NI ] = gmapping( ScanRef, ScanNew, motion, Opt ) +% GA returns a rotational and translational estimation using a genetic +% algorithm search. The algorithm is based on a series of publications. + +% [1] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Improved +% Techniques for Grid Mapping with Rao-Blackwellized Particle Filters, +% IEEE Transactions on Robotics, 2006 (link) + +% [2] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: +% Improving Grid-based SLAM with Rao-Blackwellized Particle Filters +% by Adaptive Proposals and Selective Resampling, +% In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2005 (link) + +% The idea is to apply use a gradient descent method to search for possible +% good poses. The space of search is limited by a boundary + +convg = Opt.scanmatcher.convalue; +niter = Opt.scanmatcher.iterations; + +ScanNew = ScanNew.localCart; +ScanRef = ScanRef.localCart; + +sizenew = size(ScanNew,1); +sizeref = size(ScanRef,1); + +u = motion.con.u; +pose = motion.frame.t; +lrangle = u(2); +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +ScanNewTR = applyTransform2Scan2D(ScanNew, yaw0, [x0 y0]); + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNewTR); +PARAM.L = 0.1; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + +% Make Search Space using noise information. TODO +%Use uniform random sampling to draw the initial population of chromosomes +global DEBUG + +bound_x = 2.5; %USING fixed values, MUST BE CHANGED +bound_y = 2.5; +bound_th = deg2rad(25); +delta_t=bound_x*0.1; +delta_th=bound_th*0.05; +bounds_rand = [bound_x bound_y bound_th]; + +popchr = zeros(nchr,3); + +it = 0; + +bestfit =[x0 y0 yaw0]; + +initfit = fitnessGrid(ScanNewTR, Occ, [x0 y0 yaw0]); + +maxfit = initfit; + +incr_x=bound_x; +incr_y=bound_y; +incr_th=bound_th; + +%% Improve error check + +while it < niter + it = it+1; + % Evaluate our chromosomes using our fitness function FITNESSGRID which + % checks a small area around each point using a direct lookup. No + % association is needed + + fit = zeros(nchr,1); + + maxfiti = -1; + + incr_x = incr_x*0.5; + + dirs = [ bestfit + [incr_x 0 0]; + bestfit + [-incr_x 0 0] ; + bestfit + [0 incr_y 0] ; + bestfit + [0 incr_y 0] ; + bestfit + [0 0 incr_th] ; + bestfit + [0 0 -incr_th] ]; + + for j=1:size(dirs,1) + [fit(j) TRF(j).grid] = fitnessGrid(ScanNewTR, Occ, dirs(j,:) ); + if fit(j) > maxfit + maxfit = fit(j); + maxfiti = j; + end + end + + if maxfiti > 0 + diffpose = bestfit - dirs(maxfiti,:); + bestfit = dirs(maxfiti,:); + else + diffpose = [diffpo0 0 0]; + end + + + if DEBUG.gmapping || DEBUG.all + figure; + hold on + axis equal + grid on + displayPoints(ScanRef,'g',0); + displayPoints(TRF(maxfiti).grid,'r',0); + end + + err = abs(log(maxfit/bestfit)); + + if err < 0.15 + break; + end + + + +end + +t = [popchr(maxfiti,1:2) 0] +R = e2q([0 0 popchr(maxfiti,3)]) +NI = it; +end + diff --git a/ScanMatching/2D/Hough/circularCrossCorrelation.m b/ScanMatching/2D/Hough/circularCrossCorrelation.m new file mode 100644 index 0000000..cafd5ea --- /dev/null +++ b/ScanMatching/2D/Hough/circularCrossCorrelation.m @@ -0,0 +1,17 @@ +% Computes the circular cross-correlation between two sequences +% +% a,b the two sequences +% normalize if true, normalize in [0,1] +% +function c = circularCrossCorrelation(a,b,normalize) + +for k=1:length(a) + c(k)=a*b'; + b=[b(end),b(1:end-1)]; % circular shift +end + +if normalize + minimum = min(c); + maximum = max(c); + c = (c - minimum) / (maximum-minimum); +end diff --git a/ScanMatching/2D/Hough/circularRotation.m b/ScanMatching/2D/Hough/circularRotation.m new file mode 100644 index 0000000..105aeff --- /dev/null +++ b/ScanMatching/2D/Hough/circularRotation.m @@ -0,0 +1,7 @@ +function b = circularRotation(a, cells) + cells=round(cells); + if cells>0 + b=[a(end-cells:end) a(1:end-cells-1)]; + else + b=a; + end diff --git a/ScanMatching/2D/Hough/computeHoughSpectrum.m b/ScanMatching/2D/Hough/computeHoughSpectrum.m new file mode 100644 index 0000000..fd3a317 --- /dev/null +++ b/ScanMatching/2D/Hough/computeHoughSpectrum.m @@ -0,0 +1,13 @@ +% Computes the Hough Spectrum from the buffer. +% +% shouldNormalize if true, normalize result in [0,1] +function spectrum = computeHoughSpectrum(HTbuffer, shouldNormalize) + +% Computing the spectrum is very easy: +% we square each cell and then sum over rho +spectrum = sum(( HTbuffer .^ 2 )'); + +if shouldNormalize + spectrum = normalize(spectrum); +end + diff --git a/ScanMatching/2D/Hough/computeHoughTransform.m b/ScanMatching/2D/Hough/computeHoughTransform.m new file mode 100644 index 0000000..60b2702 --- /dev/null +++ b/ScanMatching/2D/Hough/computeHoughTransform.m @@ -0,0 +1,36 @@ +% Computes the Hough Transform +function buffer = computeHoughTransform(points, thetaCells, rhoCells, rhoScale) + +% Initialize buffer +buffer = zeros(thetaCells, rhoCells); + +% For each input point +for i=1:size(points,2) + point = points(:,i); + + % For each theta cell + for thetaCell=1:thetaCells + + % Corresponding angle + theta = thetaCell * pi * 2 / thetaCells; + + % Find rho = p.x * cos(theta) + p.y*sin(theta) + rho = [cos(theta) sin(theta)] * point; + + % Transform into cells + rhoCell = rho * rhoCells / rhoScale; + + % Put the cells with rho=0 at the middle of the buffer + rhoCell = rhoCell + rhoCells/2; + + % We need an integer + rhoCell = round(rhoCell); + + % If (thetaCell, rhoCell) is inside the buffer.. + if (rhoCell>=1) && (rhoCell<=rhoCells) + + % increase the cell value + buffer(thetaCell, rhoCell) = buffer(thetaCell, rhoCell) + 1; + end + end +end diff --git a/ScanMatching/2D/Hough/findLocalMaxima.m b/ScanMatching/2D/Hough/findLocalMaxima.m new file mode 100644 index 0000000..c8774ca --- /dev/null +++ b/ScanMatching/2D/Hough/findLocalMaxima.m @@ -0,0 +1,19 @@ +% Find the local maxima of a sequence; it returns +% the indexes in descending order +% +% nm maximum number of maxima returned +% +function maxima = findLocalMaxima(sequence, nm) + + % This is a bit of black magic; these are the beauties of Matlab + indexes = find(diff(sign(diff(sequence,1)))==-2); + + values = sequence(indexes); + + [unused, sortedIndexes] = sort(values,'descend'); + + maxima = indexes(sortedIndexes); + + if size(maxima,2)>nm + maxima = maxima(1:nm); + end diff --git a/ScanMatching/2D/Hough/findLocalMaxima2.m b/ScanMatching/2D/Hough/findLocalMaxima2.m new file mode 100644 index 0000000..621bd08 --- /dev/null +++ b/ScanMatching/2D/Hough/findLocalMaxima2.m @@ -0,0 +1,22 @@ +% Find the local maxima of a sequence; it returns +% the indexes in descending order +% +% nm maximum number of maxima returned +% +function maxima = findLocalMaxima2(matrix, nm) + + [i,j] = find(imregionalmax(matrix)); + + for a=1:size(i,1) + values(a)=matrix(i(a),j(a)); + end + + + [unused, sortedIndexes] = sort(values,'descend'); + + maxima = [ i(sortedIndexes)'; j(sortedIndexes)']; + + if size(maxima,2)>nm + maxima = maxima(:,1:nm); + end + diff --git a/ScanMatching/2D/Hough/findLocalMaximaCircular.m b/ScanMatching/2D/Hough/findLocalMaximaCircular.m new file mode 100644 index 0000000..d72630f --- /dev/null +++ b/ScanMatching/2D/Hough/findLocalMaximaCircular.m @@ -0,0 +1,11 @@ +% Find the local maxima of a sequence; it returns +% the indexes in descending order +% +% nm maximum number of maxima returned +% +function maxima = findLocalMaxima(sequence, nm) + + extended=[sequence(end) sequence sequence(1)]; + m=findLocalMaxima(extended, nm); + maxima = m-1; + diff --git a/ScanMatching/2D/Hough/hough.m~ b/ScanMatching/2D/Hough/hough.m~ new file mode 100644 index 0000000..0785cfa --- /dev/null +++ b/ScanMatching/2D/Hough/hough.m~ @@ -0,0 +1,63 @@ +function [ R t ] = hough( ScanRef, ScanNew, motion, Opt ) +%HOUGH Uses the hough trasnform to detect the displacement as proposed in +%the paper + +% A. Censi, L. Iocchi, G. Grisetti - +% Scan Matching in the Hough Domain (PDF), presented at ICRA 2005. + +%Calculate the motion parameters +u = motion.con.u; +pose = motion.frame.t; +lrangle = u(2); +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; + +rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; + +% B referenced to A coordinate system +for i = 1:sizeB + B.cart(i,1:3) = (rTn*[B.cart(i,1:2) 1]')' ; + th = normAngle(B.polar(i,1) + yaw); + ro = B.polar(i,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + B.polar(i,2) = sqrt(rc^2 + rs^2); + B.polar(i,1) = atan2(rs,rc); +end + +B.cart(:,3) = pose(3); % Delete 1's from normalized 2D point + + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNew); +PARAM.L = 0.5; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNew); +PARAM.L = 0.5; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + + +end + diff --git a/ScanMatching/2D/Hough/houghSM.m b/ScanMatching/2D/Hough/houghSM.m new file mode 100644 index 0000000..42f92a6 --- /dev/null +++ b/ScanMatching/2D/Hough/houghSM.m @@ -0,0 +1,291 @@ + +%HOUGH The discrete Hough +% Transform (HT ) of both scans Sref and Snew is com- +% puted. For each point in a scan set, HT maps a set +% of possible regular features passing through that point. +% The features are parametrized, as a set of (Ï,θ) values. +% The transform returns a 2D matrix of accumulators +% which have higher values when a possible feature passes +% through more points in the scan set. This domain is +% invariant to the translation, but not to the rotation. Cal- +% culating the magnitude of the HT , it is possible to have +% a 1D signal carrying rotational information. Correlating +% the two signals of two scan sets, it is possible to extract +% a series of rotational correction hypotheses. To ï¬nd the +% translation a different procedure is applied. We begin +% by rotating Snew with one of the rotational hypotheses +% previously found and correlating its HT with the one +% of Sref . The correlation maxima will identify a set of +% column indexes C (or directions of correction) which we +% will use to ï¬nd the translation hypotheses. A discretized +% space of translational hypotheses with resolution νd and +% bounded at [-δd ,+δd ]is generated. Each cell represents +% a hypothesis and it is ï¬lled with the sum (for each +% direction) of the cross correlation value (related with +% the cell hypothesis) of the C indexed columns extracted +% from the HT of Sref and the HT of Snew rotated. +% From the transational buffer a series of x, y hypotheses +% is extracted. + +% A. Censi, L. Iocchi, G. Grisetti - +% Scan Matching in the Hough Domain (PDF), presented at ICRA 2005. + +function [ R t NI] = houghSM( ScanRef, ScanNew, motion ) + +% In: +% ScanRef: localCart[2xN], points cartesian +% ScanRef: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + + +global Opt + +%Calculate the motion parameters +u = motion.con.u; +lrangle = u(2); + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; + +borders = 2; +NI = 0; +rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; + +% B referenced to A coordinate system +for i = 1:size(ScanNew.localCart,1) + ScanNew.localCart(i,1:3) = (rTn*[ScanNew.localCart(i,1:2) 1]')' ; + th = normAngle(ScanNew.localPolar(i,1) + yaw); + ro = ScanNew.localPolar(i,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + ScanNew.localPolar(i,2) = sqrt(rc^2 + rs^2); + ScanNew.localPolar(i,1) = atan2(rs,rc); +end + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef.localCart); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNew.localCart); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - borders; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + borders; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - borders; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + borders; + +% Some parameter for the algorithm +maxPhiHypotheses=4; +rhoScale=Opt.map.resolution; +thetaScale=1; +rhoCells = 200; +thetaCells = round(360/thetaScale); +maxTHypotheses = 6; +phiFilterSigma = 15; +rhoFilterSigma = 4; +maxRhoCorrelations = 8; +maxTBound = Opt.scanmatcher.Br(2); + + +global DEBUG + +Sref = ScanRef.localCart; +if(size(Sref,2)>2) + Sref(:,3) = []; +end +Snew = ScanNew.localCart; +if(size(Snew,2)>2) + Snew(:,3) = []; +end + +Sref = Sref'; +Snew = Snew'; + +HoughRef = computeHoughTransform(Sref, thetaCells, rhoCells, rhoScale); +HoughNew = computeHoughTransform(Snew, thetaCells, rhoCells, rhoScale); + +if DEBUG.houghSM || DEBUG.all + % show the Hough transform + figure; + subplot(4,2,3); imshow(HoughRef, [ 0 14]); + subplot(4,2,4); imshow(HoughNew, [ 0 14]); + colormap(hot); + +end + +% Compute the discrete hough transform using the algorithm provided by the +% author +HoughSpectrumRef = computeHoughSpectrum(HoughRef,true); +HoughSpectrumNew = computeHoughSpectrum(HoughNew,true); + +% and calculate the cross correlation which is shown if on debug +ccHough = circularCrossCorrelation(HoughSpectrumNew,HoughSpectrumRef,true); + +if DEBUG.houghSM || DEBUG.all + subplot(4,2,5); plot(1:size(HoughSpectrumRef',1),HoughSpectrumRef,'-r', 1:size(HoughSpectrumNew',1),HoughSpectrumNew,'-b');grid on; + subplot(4,2,6); plot(ccHough);grid on; +end + +%% Rotation + +% To find an estimate of the rotation, we do a +% circular cross-correlation of the two spectra +crossCorrelation = circularCrossCorrelation(HoughSpectrumNew, HoughSpectrumRef, true); + +% We then do simple low-pass filtering to eliminate noise +crossCorrelationFiltered = simpleLowPassFilterCircular(crossCorrelation, phiFilterSigma); + +% We then extract the maxima of the filtered cross correlation: +% these are our hypotheses for the rotation +phiIndexes = findLocalMaximaCircular(crossCorrelationFiltered, maxPhiHypotheses); +phiHypotheses = normAngle(phiIndexes * 2 * pi / thetaCells); + +tvec=[]; + +%% Translation + +% Now we find a translation for each rotation hypothesis +for i=1:size(phiHypotheses,2) + phi=(phiHypotheses(i)); + + + % Assuming phi, we rotate the second spectrum + % radians->cells + phiCell = phi / (2*pi) * thetaCells; + % circular rotation + HoughSpectrumNewR = circularRotation(HoughSpectrumNew, phiCell); + + % We have to choose the columns (=which directions) of the HT to correlate + % the best way to choose is to use the maxima of the spectrum; + % therefore (to suppress noise) we multiply the two spectra, filter + % and get the maxima + product = HoughSpectrumRef .* HoughSpectrumNewR; + productFiltered = simpleLowPassFilterCircular(product, phiFilterSigma); + + corrIndexes = findLocalMaxima(productFiltered(1:floor(thetaCells/2)), maxRhoCorrelations); + + if (size(corrIndexes,2)==1) + corrIndexes(2) = round(corrIndexes(1)+thetaCells/4); + end + + cells = round(maxTBound * (rhoCells/rhoScale)); + + % the buffer of translational hypotheses + tbuffer = zeros(cells*2+1,cells*2+1); + + % For each direction, fill the buffer with the correlation of the + % columns defined before + for j=1:size(corrIndexes,2) + index=corrIndexes(j); + direction=index*2*pi/thetaCells; + + % We extract the two columns of the HT + col1 = HoughRef(index,:); + % we must "rotate" by phi + index2 = min(round(mod(index+phiCell, thetaCells))+1,size(HoughNew,1) ); + col2 = HoughNew(index2,:); + + % We do a cross correlation between the two columns + crossCorrelationRange = cells; + crossCorrelation = xcorr(col2,col1,crossCorrelationRange); + + % Now for each element in the T buffer, + % we increase the value by this observation + for a=1:size(tbuffer,1) + for b=1:size(tbuffer,2) + % Which value of the translation T is associated to this cell? + T = [ (a-cells) (b-cells) ]' * rhoScale / rhoCells; + + % We project T on the correlation direction + pT = [cos(direction) sin(direction)] * T; + + % We transform the projection to cells of the correlation + % buffer + pTcell = round(pT * rhoCells / rhoScale + cells); + + if (pTcell>=1) && (pTcell<=size(crossCorrelation,2)) + value = crossCorrelation(pTcell); + tbuffer(a,b)=tbuffer(a,b)+value; + end + end + end + end + + + % Now the buffer is complete, to find the T we extract the maxima + % (for simplicity, we use only one maximum) + + tbufferFiltered = simpleLowPassFilter2(tbuffer, rhoFilterSigma); + Tcells = findLocalMaxima2(tbufferFiltered, maxTHypotheses); + num=size(Tcells,2); + % Convert back to world-coordinates + THypotheses = (Tcells-repmat([cells cells]',1,num)) * rhoScale / rhoCells; + + % Saving debug info into main structure + THypotheses(3,:) = repmat(phi,1,size(THypotheses,2)); + tvec = [tvec THypotheses]; + +end + +% some initialization for the best correction search +maxf = inf; +maxi=1; + + +% we use nearest neighbour to find the best correction among those +% extracted from the Hough domain +for s=1:size(tvec,2) + + + rot = -tvec(3,s); + t = -[tvec(1:2,s)]; + R = e2q([0 0 rot]); + corrs=[]; + rTn = [cos(rot) -sin(rot) t(1); sin(rot) cos(rot) t(2); 0 0 1]; + + % B referenced to A coordinate system + for i = 1:size(Snew,2) + corrs(:,i) = (rTn*[Snew(1:2,i); 1]) ; + end + corrs(3,:) = []; + % returns the nearest neighbour error + [~, err] = dsearchn(corrs', Sref'); + err = mean(err); + if err < maxf + maxf = err; + maxi = s; + %maxf = j; + end +end + + + +%% Result + +t = -tvec(1:2,maxi)'; +R=-tvec(3,maxi); + +if DEBUG.houghSM || DEBUG.all + % fisplay the final result + figure; + hold on + axis equal + grid on + displayPoints(Sref','g',0); + displayPoints(Snew','b',0); + displayPoints(applyTransform2Scan2D(Snew', R, [t(1) t(2)]),'r',0); +end + +end + + + diff --git a/ScanMatching/2D/Hough/houghSM.m~ b/ScanMatching/2D/Hough/houghSM.m~ new file mode 100644 index 0000000..b803824 --- /dev/null +++ b/ScanMatching/2D/Hough/houghSM.m~ @@ -0,0 +1,298 @@ +function [ R t NI] = houghSM( ScanRef, ScanNew, motion ) +%HOUGH The discrete Hough +% Transform (HT ) of both scans Sref and Snew is com- +% puted. For each point in a scan set, HT maps a set +% of possible regular features passing through that point. +% The features are parametrized, as a set of (Ï,θ) values. +% The transform returns a 2D matrix of accumulators +% which have higher values when a possible feature passes +% through more points in the scan set. This domain is +% invariant to the translation, but not to the rotation. Cal- +% culating the magnitude of the HT , it is possible to have +% a 1D signal carrying rotational information. Correlating +% the two signals of two scan sets, it is possible to extract +% a series of rotational correction hypotheses. To ï¬nd the +% translation a different procedure is applied. We begin +% by rotating Snew with one of the rotational hypotheses +% previously found and correlating its HT with the one +% of Sref . The correlation maxima will identify a set of +% column indexes C (or directions of correction) which we +% will use to ï¬nd the translation hypotheses. A discretized +% space of translational hypotheses with resolution νd and +% bounded at [-δd ,+δd ]is generated. Each cell represents +% a hypothesis and it is ï¬lled with the sum (for each +% direction) of the cross correlation value (related with +% the cell hypothesis) of the C indexed columns extracted +% from the HT of Sref and the HT of Snew rotated. +% From the transational buffer a series of x, y hypotheses +% is extracted. + +% A. Censi, L. Iocchi, G. Grisetti - +% Scan Matching in the Hough Domain (PDF), presented at ICRA 2005. + +global Opt + +%Calculate the motion parameters +u = motion.con.u; +lrangle = u(2); +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; + +borders = 2; +NI = 0; +rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; + +% B referenced to A coordinate system +for i = 1:size(ScanNew.localCart,1) + ScanNew.localCart(i,1:3) = (rTn*[ScanNew.localCart(i,1:2) 1]')' ; + th = normAngle(ScanNew.localPolar(i,1) + yaw); + ro = ScanNew.localPolar(i,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + ScanNew.localPolar(i,2) = sqrt(rc^2 + rs^2); + ScanNew.localPolar(i,1) = atan2(rs,rc); +end + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef.localCart); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNew.localCart); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - borders; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + borders; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - borders; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + borders; + +% Some parameter for the algorithm +maxPhiHypotheses=4; +rhoScale=Opt.map.resolution; +thetaScale=1; +rhoCells = 200; +thetaCells = round(360/thetaScale); +maxTHypotheses = 6; +phiFilterSigma = 15; +rhoFilterSigma = 4; +maxRhoCorrelations = 8; +maxTBound = Opt.scanmatcher.Br(2); + + +global DEBUG + +Sref = ScanRef.localCart; +if(size(Sref,2)>2) + Sref(:,3) = []; +end +Snew = ScanNew.localCart; +if(size(Snew,2)>2) + Snew(:,3) = []; +end + +Sref = Sref'; +Snew = Snew'; + +HoughRef = computeHoughTransform(Sref, thetaCells, rhoCells, rhoScale); +HoughNew = computeHoughTransform(Snew, thetaCells, rhoCells, rhoScale); + +if DEBUG.houghSM || DEBUG.all + % show the Hough transform + figure; + subplot(4,2,3); imshow(HoughRef, [ 0 14]); + subplot(4,2,4); imshow(HoughNew, [ 0 14]); + colormap(hot); + +end + +% Compute the discrete hough transform using the algorithm provided by the +% author +HoughSpectrumRef = computeHoughSpectrum(HoughRef,true); +HoughSpectrumNew = computeHoughSpectrum(HoughNew,true); + +% and calculate the cross correlation which is shown if on debug +ccHough = circularCrossCorrelation(HoughSpectrumNew,HoughSpectrumRef,true); + +if DEBUG.houghSM || DEBUG.all + subplot(4,2,5); plot(1:size(HoughSpectrumRef',1),HoughSpectrumRef,'-r', 1:size(HoughSpectrumNew',1),HoughSpectrumNew,'-b');grid on; + subplot(4,2,6); plot(ccHough);grid on; +end + +%% Rotation + +% To find an estimate of the rotation, we do a +% circular cross-correlation of the two spectra +crossCorrelation = circularCrossCorrelation(HoughSpectrumNew, HoughSpectrumRef, true); + +% We then do simple low-pass filtering to eliminate noise +crossCorrelationFiltered = simpleLowPassFilterCircular(crossCorrelation, phiFilterSigma); + +% We then extract the maxima of the filtered cross correlation: +% these are our hypotheses for the rotation +phiIndexes = findLocalMaximaCircular(crossCorrelationFiltered, maxPhiHypotheses); +phiHypotheses = normAngle(phiIndexes * 2 * pi / thetaCells); + +tvec=[]; + +%% Translation + +% Now we find a translation for each rotation hypothesis +for i=1:size(phiHypotheses,2) + phi=(phiHypotheses(i)); + + + % Assuming phi, we rotate the second spectrum + % radians->cells + phiCell = phi / (2*pi) * thetaCells; + % circular rotation + HoughSpectrumNewR = circularRotation(HoughSpectrumNew, phiCell); + + % We have to choose the columns (=which directions) of the HT to correlate + % the best way to choose is to use the maxima of the spectrum; + % therefore (to suppress noise) we multiply the two spectra, filter + % and get the maxima + product = HoughSpectrumRef .* HoughSpectrumNewR; + productFiltered = simpleLowPassFilterCircular(product, phiFilterSigma); + + corrIndexes = findLocalMaxima(productFiltered(1:floor(thetaCells/2)), maxRhoCorrelations); + + if (size(corrIndexes,2)==1) + corrIndexes(2) = round(corrIndexes(1)+thetaCells/4); + end + + cells = round(maxTBound * (rhoCells/rhoScale)); + + % the buffer of translational hypotheses + tbuffer = zeros(cells*2+1,cells*2+1); + + % For each direction, fill the buffer with the correlation of the + % columns defined before + for j=1:size(corrIndexes,2) + index=corrIndexes(j); + direction=index*2*pi/thetaCells; + + % We extract the two columns of the HT + col1 = HoughRef(index,:); + % we must "rotate" by phi + index2 = min(round(mod(index+phiCell, thetaCells))+1,size(HoughNew,1) ); + col2 = HoughNew(index2,:); + + % We do a cross correlation between the two columns + crossCorrelationRange = cells; + crossCorrelation = xcorr(col2,col1,crossCorrelationRange); + + % Now for each element in the T buffer, + % we increase the value by this observation + for a=1:size(tbuffer,1) + for b=1:size(tbuffer,2) + % Which value of the translation T is associated to this cell? + T = [ (a-cells) (b-cells) ]' * rhoScale / rhoCells; + + % We project T on the correlation direction + pT = [cos(direction) sin(direction)] * T; + + % We transform the projection to cells of the correlation + % buffer + pTcell = round(pT * rhoCells / rhoScale + cells); + + if (pTcell>=1) && (pTcell<=size(crossCorrelation,2)) + value = crossCorrelation(pTcell); + tbuffer(a,b)=tbuffer(a,b)+value; + end + end + end + end + + + % Now the buffer is complete, to find the T we extract the maxima + % (for simplicity, we use only one maximum) + + tbufferFiltered = simpleLowPassFilter2(tbuffer, rhoFilterSigma); + Tcells = findLocalMaxima2(tbufferFiltered, maxTHypotheses); + num=size(Tcells,2); + % Convert back to world-coordinates + THypotheses = (Tcells-repmat([cells cells]',1,num)) * rhoScale / rhoCells; + + % Saving debug info into main structure + THypotheses(3,:) = repmat(phi,1,size(THypotheses,2)); + tvec = [tvec THypotheses]; + +end + +% some initialization for the best correction search +maxf = inf; +maxi=1; + + +% we use nearest neighbour to find the best correction among those +% extracted from the Hough +for s=1:size(tvec,2) + + + rot = -tvec(3,s); + t = -[tvec(1:2,s)]; + R = e2q([0 0 rot]); + corrs=[]; + rTn = [cos(rot) -sin(rot) t(1); sin(rot) cos(rot) t(2); 0 0 1]; + + % B referenced to A coordinate system + for i = 1:size(Snew,2) + corrs(:,i) = (rTn*[Snew(1:2,i); 1]) ; + + + end + + corrs(3,:) = []; + + [as err] = dsearchn(corrs', Sref'); + err = mean(err); + % if DEBUG.houghSM || DEBUG.all + % figure; + % title(err) + % hold on + % axis equal + % grid on + % displayPoints(Sref','g',0); + % displayPoints(corrs','r',0); + % end + + + if err < maxf + maxf = err; + maxi = s; + %maxf = j; + end + + + +end + + + + + +t = -[tvec(1:2,maxi); 0]; +t=t' +% tm = e2R([0 0 Rvec(maxi)]); +% +% t = tm*t'; +% t=t'; + +R=-tvec(3,maxi) + +if DEBUG.houghSM || DEBUG.all + figure; + hold on + axis equal + grid on + displayPoints(Sref','g',0); + displayPoints(applyTransform2Scan2D(Snew', R, [t(1) t(2)]),'r',0); +end +R = e2q([0 0 R]); +end + + + diff --git a/ScanMatching/2D/Hough/mean_last.mat b/ScanMatching/2D/Hough/mean_last.mat new file mode 100644 index 0000000000000000000000000000000000000000..86a6b56edd464f6e085a37ffe094f62cfc86f465 GIT binary patch literal 438 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i*PhE(NSP4;mcV1iP6G{ zC4~cAPmLK3jRlOIey((N{ptGgrRp4qZHxl(8JG56T(UdaK1BL3qlq1l#Ot{ilGg9q LE6 opts.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + +end + +%% Result +% Returns the minimizing Q = [t R] +R = ([normAngle(yaw-yaw0)] ); +t = [x y] - [x0 y0]; +NI = it; \ No newline at end of file diff --git a/ScanMatching/2D/IDC/idc.m b/ScanMatching/2D/IDC/idc.m new file mode 100644 index 0000000..0f4ab29 --- /dev/null +++ b/ScanMatching/2D/IDC/idc.m @@ -0,0 +1,163 @@ +% It works similarly to the original ICP, +% but solving the association at two different stages. First, +% the Closest Point Rule is used to compute the corre- +% spondence set needed for solving for the translation +% registering through the Unit Quaternions [5]. Next, the +% Matching Range Rule is used to associate points having +% the closest range when represented in polar coordinates. +% It is worth noting that polar range is invariant to rotation. +% Next, the new association set is used to register (again +% with the Unit Quaternions [5]) solving for the rotation. + +%Feng Lu and Evangelos Milios. 1997. Robot Pose Estimation in Unknown +%Environments by Matching 2D Range Scans. J. Intell. Robotics Syst. 18, 3 +%(March 1997), 249-275. + + +function [R, t, NI] = idc(Ai, Bi, motion) + +% In: +% Ai: localCart[2xN], points cartesian, localPolar[2xN], points +% polar +% Bi: localCart[2xN], points cartesian, localPolar[2xN], points +% polar +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + + +global DEBUG Opt +opts = Opt; + +% we need both cartesian and polar +Al.cart = Ai.localCart; +Al.polar = Ai.localPolar; +Bl.cart = Bi.localCart; +Bl.polar = Bi.localPolar; + +itMax = opts.scanmatcher.iterations; +Br = opts.scanmatcher.Br; % thresholds +alpha = 0.05; % angular threshold decreasing factor +it=1; %iteration control + +sizeB = size(Bl.cart,1); + +% Matrix initialization +B.cart = zeros(sizeB, 2); +B.polar = zeros(sizeB, 2); + + +%Motion estimation q0 +u = motion.con.u; +pose(3) = 0; + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; + +lasterror = []; +corr=[]; + +% Format the two scans we are going to use +B.cart(:,1:2) = Bl.cart(:,1:2); +B.polar(:,1:2) = Bl.polar(:,1:2); + +A.cart(:,1:2) = Al.cart(:,1:2); +A.polar(:,1:2) = Al.polar(:,1:2); + +if DEBUG.cpAssociation || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + +while it < itMax + + %% Apply q + % return if convergence is achieved + if checkConv(lasterror, corr) + break + end + + % apply the motion to S New + [Bnf Bf Af] = transPolarCartScan(B, Al, yaw, [x y], 1,opts.scanmatcher.Br(2)); + Bnf.cart(:,3) = pose(3); + Bf.cart(:,3) = pose(3); + + if opts.scanmatcher.projfilter + BA = Bf; + AlA = Af; + else + BA = Bnf; + AlA = Al; + end + + %% Association + + %Compute the associated point with the Closest Point rule + [merr assp ] = cpAssociation(BA,AlA, opts); + + + %Compute the associated point with the Matching Range rule + [~, assp2 ] = mpAssociation(BA,AlA, opts); + + % Decrement the angular threshold + opts.scanmatcher.Br(1) = max(Br(1)*exp(-alpha*it),0.1); + + %Use rejection rule? + if ~isempty(opts.scanmatcher.rejection_rule) + assp = opts.scanmatcher.rejection_rule(assp); + assp2 = opts.scanmatcher.rejection_rule(assp2); + end + + %% Registration + + if size(assp.new,1) < 5 || size(assp2.new,1) < 5 + break; + end + + + %Registrate the scan, Use rotation from IMRP rule if selected + %[BB ASS] = filterAssMatrix(B.cart, assp,i1); + [~, t] = regist_besl(assp.new', assp.ref'); + + %[BB ASS] = filterAssMatrix(B.cart, assp2.ref,i2); + [R] = regist_besl(assp2.new', assp2.ref'); + + + % update q + yawR = R2e(R); + yaw = yaw + yawR(3); + x = x + t(1); + y = y + t(2); + + %Error estimation, keep a vector of the last error estimation + it=it+1; + lasterror = [lasterror merr/10]; + corr = [corr; t(1)/10 t(2)/10 yaw/10]; + + % error estimation update + if size(lasterror,2) > opts.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + +end + +%% Result +% Returns the minimizing Q = [t R] +R = normAngle(yaw-yaw0); +t = [x y] - [x0 y0]; +NI = it; diff --git a/ScanMatching/2D/IDC/idc.m~ b/ScanMatching/2D/IDC/idc.m~ new file mode 100644 index 0000000..c081efa --- /dev/null +++ b/ScanMatching/2D/IDC/idc.m~ @@ -0,0 +1,152 @@ +%Given two scan, performs an pIC scanmatching. It returns the displacement +%between adiacent points. Being R the 3D rotation in quaternion form and t +%the translation in 3D Being A the old scan and B the new one. The +%point set size does not match. The idea is to find the correnspondence +%between all points of B with a point of A. + +%Feng Lu and Evangelos Milios. 1997. Robot Pose Estimation in Unknown +%Environments by Matching 2D Range Scans. J. Intell. Robotics Syst. 18, 3 +%(March 1997), 249-275. + + +function [R, t, NI] = idc(Ai, Bi, motion) + +global Opt + +%Select which points to use, in this case local cartesian point in +Al.cart = Ai.localCart; +Al.polar = Ai.localPolar; + +Bl.cart = Bi.localCart; +Bl.polar = Bi.localPolar; + +itMax = Opt.scanmatcher.iterations; +Br = Opt.scanmatcher.Br; % Angular tolerance +% Br0 = Br; +% alfaBr = 1/(2.5*itMax); +% Br = Br0*exp(1)^alfaBr; +alpha = 0.1; +it=1; %iteration control + +sizeB = size(Bl.cart,1); + +% Matrix initialization +B.cart = zeros(sizeB, 2); +B.polar = zeros(sizeB, 2); + + +%Motion extraction from robot + +u = motion.con.u; +%pose = motion.state.x; +pose(3) = 0; +lrangle = u(2); + +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; + +lasterror = []; +corr=[]; + +B.cart(:,1:2) = Bl.cart(:,1:2); +B.polar(:,1:2) = Bl.polar(:,1:2); + +A.cart(:,1:2) = Al.cart(:,1:2); +A.polar(:,1:2) = Al.polar(:,1:2); + +BA = B; + +xtot = 0; +ytot = 0; +rtot = 0; +global DEBUG +if DEBUG.cpAssociation || DEBUG.all + scrsz = get(0,'ScreenSize'); + Opt.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + Opt.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + Opt.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + +while it < itMax + + % reeturn if convergence is achieved + if checkConv(lasterror, corr) + break + end + + [Bnf Bf Af] = transPolarCartScan(B, Al, yaw, [x y], 1,Opt.scanmatcher.Br(2)); + Bnf.cart(:,3) = pose(3); % Delete 1's from normalized 2D point + Bf.cart(:,3) = pose(3); + + if Opt.scanmatcher.projfilter + BA = Bf; + AlA = Af; + else + BA = Bnf; + AlA = Al; + end + + %Compute the associated point with the Closest Point rule + [merr assp i1] = cpAssociation(BA,AlA, Opt); + + + %Compute the associated point with the Matching Range rule + [merr2 assp2 i2] = mpAssociation(BA,AlA, Opt); + + Opt.scanmatcher.Br(1) = max(Br(1)*exp(-alpha*it),0.01); + Br(1)*exp(-alpha*it) + + %Decrease the maximum angular error factor + + + if ~isempty(Opt.scanmatcher.rejection_rule) + assp = Opt.scanmatcher.rejection_rule(assp); + assp2 = Opt.scanmatcher.rejection_rule(assp2); + end + + if size(assp.new,1) < 5 || size(assp2.new,1) < 5 + break; + end + + + %Registrate the scan, Use rotation from IMRP rule if selected + %[BB ASS] = filterAssMatrix(B.cart, assp,i1); + [R2 t] = regist_besl(assp.new', assp.ref'); + + %[BB ASS] = filterAssMatrix(B.cart, assp2.ref,i2); + [R t2] = regist_besl(assp2.new', assp2.ref'); + + + % Apply the shift found to the pose + yawR = R2e(R); + yaw = yaw + yawR(3); + x = x + t(1); + y = y + t(2); + + %Error estimation, keep a vector of the last error estimation + it=it+1; + lasterror = [lasterror merr/10]; + corr = [corr; t(1)/10 t(2)/10 yaw/10]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + +end + +%Final result in quaternion +R = e2q( [0 0 normAngle(yaw-yaw0)] ); + +t = [x y 0] - [x0 y0 0]; + +NI = it; diff --git a/ScanMatching/2D/LF_SOG/lf_sof.m~ b/ScanMatching/2D/LF_SOG/lf_sof.m~ new file mode 100644 index 0000000..bec6312 --- /dev/null +++ b/ScanMatching/2D/LF_SOG/lf_sof.m~ @@ -0,0 +1,137 @@ +function [R t] = lf_sog(ScanNew, ScanRef, motion, Opt) + +%NDT LF-SOG algorithm. It returns a rototranslation [R t] which estimates +%the current robot displacement from the ground truth. The algorithm +%follows the paper + +% Antoni BURGUERA, Yolanda GONZÃLEZ, Gabriel OLIVER +% The sNDT: A Grid-Based Likelihood Field Approach to Robust and Accurate Sonar Scan Matching Localization. +% Technical Report A-06-2007 +% Departament de Matemàtiques i Informàtica (UIB) + +% TODO: OPTIMIZE SCAN, RESAMPLING SCAN TO DISCARD CELL WITH TOO MANY +% READINGS + +% N ITERATIONS %%%%%%%%%% TO DO: USE OPT +niter = Opt.scanmatcher.iterations; + +ScanNew = ScanNew.data.localCart2D; +ScanRef = ScanRef.data.localCart2D; + +sizenew = size(ScanNew,1); +sizeref = size(ScanRef,1); + +u = motion.con.u; +originalp = [cos(u(2))*u(1) sin(u(2))*u(1) u(3)]; +%originalp = [motion.frame.x(1) motion.frame.x(2) motion.frame.epose(6)]; +p = originalp; + +%We are going to use Newton's algorithm to minimize the score function. The +%idea is that the score function returns a worse value when the point is +%more distant from the Reference Scan + +for i = 1:niter + + x = p(1); + y = p(2); + yaw = p(3); + cosy = cos(yaw); + siny = sin(yaw); + + grad = zeros(1,3); + H = zeros(3,3); + score = 0; +%DEBUG +figure +hold on +axis equal +displayPoints(ScanRef,'k'); +displayPoints(ScanNew,'g'); + + for j = 1:sizenew + pn = ScanNew(j,1:2); + + for k = 1:sizeref + + qn = ScanRef(j,1:2); + %add the point pn from the new scan to the approximated minimum + %of the function (the odometry estimation). + v(1) = x + cosy*pn(1) - siny*pn(2); + v(2) = y + siny*pn(1) + cosy*pn(2); + + plot(v(1),v(2),'.b'); + + %Jacobian of the transformation function having point pn and + %translation x + dv = [ 1, 0, -pn(1)*siny-cosy*pn(2);... + 0, 1, pn(1)*cosy-siny*pn(2)]; + + + %difference with mean + dnm = (v - qn)'; + + grad = grad + 2*exp( ((-dnm'*dnm)*dnm')*dv); + + % Calculate the Hessian of the likelihood function + vH = [ -pn(1)*cosy + siny*pn(2); + -pn(1)*siny - cosy*pn(2) ]; + + + dnmdv1 = dnm'*dv(:,1); + dnmdv2 = dnm'*dv(:,2); + dnmdv3 = dnm'*dv(:,3); + expdnm = 2*exp(dnm*dnm'); + + + %Hessian of the function + Hp = [ expdnm*(-2*dnmdv1*dnmdv1)*(dv(:,1)'*dv(:,1)) ,...% D² xx + expdnm*(-2*dnmdv1*dnmdv2)*(dv(:,1)'*dv(:,2)),... % D² xy + expdnm*(-2*dnmdv1*dnmdv3)*(dv(:,1)*dv(:,3));... % D² xTHETA + ... + 0,...% D² yx + expdnm*(-2*dnmdv2*dnmdv2)*(dv(:,2)*dv(:,2)'),... % D² yy + expdnm*(-2*dnmdv2*dnmdv3)*(dv(:,2)*dv(:,3)');... % D² yTHETA + ... + 0,...% D² x THETA + 0,... % D² yTHETA + expdnm*(-2*dnmdv3*dnmdv3)*(dv(:,3)*dv(:,3)' + vH)... + % D² THETA THETA. The second derivative is defined only in + % this point + ]; + + %H is symmetric, let's fill the remaining part + Hp(2,1) = Hp(1,2); + Hp(3,1) = Hp(1,3); + Hp(3,2) = Hp(2,3); + + H = H + Hp; + end + + + + end + + % Once we have the Hessian and Gradient of the function we calculate + % the new translation + H + grad + dt = -H\grad'; + + + % Motion referenced to computed estimation + eTm = [cos(dt(3)) -sin(dt(3)) dt(1); sin(dt(3)) cos(dt(3)) dt(2); 0 0 1]; + p = eTm*[p(1:2) 1]'; + %solution(3,1) = estimation(3)+motion(3); + %solution = solution + estimation; + p(3) = normAngle(dt(3)+yaw); + + p = p'; + %p = p + dt'; + +end +p = originalp + p; +t = [p(1:2) 0]; +R = e2q([0 0 (p(3)) ]); +end + + diff --git a/ScanMatching/2D/LF_SOG/lf_sog.m b/ScanMatching/2D/LF_SOG/lf_sog.m new file mode 100644 index 0000000..29f2cd0 --- /dev/null +++ b/ScanMatching/2D/LF_SOG/lf_sog.m @@ -0,0 +1,190 @@ +% This algorithm works similarly to NDT but it does not work with grids. +% Likelihood ï¬elds are, in this case, deï¬ned using the two +% scans. The minimizing function is, this time, deï¬ned as +% the exponential of the distances between a point in Snew +% and a set of points in Sref . The set of points in Sref is +% decided using an euclidean distance threshold. + +% P. Biber, S. Fleck, and W. Strasser, “A probabilistic framework for robust +% and accurate matching of point clouds,†Pattern Recognition, pp. 480– +% 487, 2004. + +function [R t NI] = lf_sog(ScanRef, ScanNew, motion) +% In: +% Ai: localCart[2xN], points cartesian +% Bi: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + +global DEBUG Opt + +itMax = Opt.scanmatcher.iterations; + +% we only use cartesian points +ScanNew = downsample(ScanNew.localCart,3); +ScanRef = downsample(ScanRef.localCart,3); +sizenew = size(ScanNew,1); +sizeref = size(ScanRef,1); + +%Motion estimation q0 +u = motion.con.u; + + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +originalp = [x0 y0 yaw0]; + +p = originalp; +lasterror = []; +corr=[]; +merr=0; + + +if DEBUG.lf_sog || DEBUG.all + figure + hold on + axis equal + hr = plot(ScanRef(:,1),ScanRef(:,2),'.k'); + hs = plot(NaN,NaN,'.b'); + title(['err = ' num2str(1/merr)]); +end + +for i = 1:itMax + + %return if convergence is achieved + if checkConv(lasterror, corr) + break; + end + + x = p(1); + y = p(2); + yaw = p(3); + cosy = cos(yaw); + siny = sin(yaw); + + % inialize grediant and hessian + grad = zeros(1,3); + H = zeros(3,3); + dist_th = Opt.scanmatcher.Br(2); + + curscan = []; + + + for j = 1:sizenew + pn = ScanNew(j,1:2); + + %transform the point using the latest estimation + v(1) = x + cosy*pn(1) - siny*pn(2); + v(2) = y + siny*pn(1) + cosy*pn(2); + + %Jacobian of the transformation function having point pn and + %translation x + dv = [ 1, 0, -pn(1)*siny-cosy*pn(2);... + 0, 1, pn(1)*cosy-siny*pn(2)]; + + curscan = [curscan [v(1); v(2) ] ]; + + for k = 1:sizeref + + qn = ScanRef(k,1:2); + + %difference with mean + dnm = (v - qn)'; + + if sqrt(dnm(1)^2 + dnm(2)^2) > dist_th + continue + end + + expdnm = 2*exp(-dnm'*dnm); + grad = grad + expdnm * (dnm'*dv); + + + % Calculate the score + merr = merr - exp( -(dnm(1)^2 + dnm(2)^2)); + + % Calculate the Hessian of the likelihood function + vH = [ -pn(1)*cosy+siny*pn(2); + -pn(1)*siny-cosy*pn(2)]; + + + dnmdv1 = dnm'*dv(:,1); + dnmdv2 = dnm'*dv(:,2); + dnmdv3 = dnm'*dv(:,3); + + + + %Hessian of the function + Hp = [ ( (-2*dnmdv1*dnmdv1)+(dv(:,1)'*dv(:,1)) ),...% D² xx + ( (-2*dnmdv2*dnmdv1)+(dv(:,1)'*dv(:,2)) ),... % D² xy + ( (-2*dnmdv3*dnmdv1)+(dv(:,1)'*dv(:,3)) );... % D² xTHETA + ... + 0,...% D² yx + ( (-2*dnmdv2*dnmdv2)+(dv(:,2)'*dv(:,2)) ),... % D² yy + ( (-2*dnmdv3*dnmdv2)+(dv(:,2)'*dv(:,3)) );... % D² yTHETA + ... + 0,...% D² x THETA + 0,... % D² y THETA + ( (-2*dnmdv3*dnmdv3)+( dnm'*vH + dv(:,3)'*dv(:,3) ) )... + % D² THETA THETA. The second derivative is defined only in + % this point + ]; + + %H is symmetric, let's fill the remaining part + Hp(2,1) = Hp(1,2); + Hp(3,1) = Hp(1,3); + Hp(3,2) = Hp(2,3); + + H = H + expdnm*Hp; + end + + + + end + + % Once we have the Hessian and Gradient of the function we calculate + % the new translation + invh = inv(H); + if( sum(sum(isinf(invh))) == 0) + dt = -invh*(grad'); + else + % the Hessian inversion has failed + break; + end + + if DEBUG.lf_sog || DEBUG.all + set(hs,'XData',curscan(1,:),'YData',curscan(2,:)); + drawnow + end + p = p + dt'; + p(3) = normAngle(p(3)); + + + % Maintain last errors to check convergence + + lasterror = [lasterror merr]; + corr = [corr; dt(1) dt(2) dt(3)]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + +end + + +%Final result in quaternion +R = normAngle(p(3)-originalp(3) ); + +t = [p(1)-originalp(1) p(2)-originalp(2)]; + +NI = i; + +end + + diff --git a/ScanMatching/2D/LF_SOG/lf_sog.m~ b/ScanMatching/2D/LF_SOG/lf_sog.m~ new file mode 100644 index 0000000..d9e452f --- /dev/null +++ b/ScanMatching/2D/LF_SOG/lf_sog.m~ @@ -0,0 +1,186 @@ +% This algorithm works similarly to NDT but it does not work with grids. +% Likelihood ï¬elds are, in this case, deï¬ned using the two +% scans. The minimizing function is, this time, deï¬ned as +% the exponential of the distances between a point in Snew +% and a set of points in Sref . The set of points in Sref is +% decided using an euclidean distance threshold. + +% P. Biber, S. Fleck, and W. Strasser, “A probabilistic framework for robust +% and accurate matching of point clouds,†Pattern Recognition, pp. 480– +% 487, 2004. + +function [R t NI] = lf_sog(ScanRef, ScanNew, motion) +% In: +% Ai: localCart[2xN], points cartesian +% Bi: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + +global DEBUG Opt + +itMax = Opt.scanmatcher.iterations; + +% we only use cartesian points +ScanNew = downsample(ScanNew.localCart; +ScanRef = ScanRef.localCart; + +sizenew = size(ScanNew,1); +sizeref = size(ScanRef,1); + +%Motion estimation q0 +u = motion.con.u; +originalp = [cos(u(2))*u(1) sin(u(2))*u(1) u(3)]; + + +p = originalp; +lasterror = []; +corr=[]; +merr=0; + + +if DEBUG.lf_sog || DEBUG.all + figure + hold on + axis equal + hr = plot(ScanRef(:,1),ScanRef(:,2),'.k'); + hs = plot(NaN,NaN,'.b'); + title(['err = ' num2str(1/merr)]); +end + +for i = 1:itMax + + %return if convergence is achieved + if checkConv(lasterror, corr) + break; + end + + x = p(1); + y = p(2); + yaw = p(3); + cosy = cos(yaw); + siny = sin(yaw); + + % inialize grediant and hessian + grad = zeros(1,3); + H = zeros(3,3); + dist_th = Opt.scanmatcher.Br(2); + + curscan = []; + + + for j = 1:sizenew + pn = ScanNew(j,1:2); + + %transform the point using the latest estimation + v(1) = x + cosy*pn(1) - siny*pn(2); + v(2) = y + siny*pn(1) + cosy*pn(2); + + %Jacobian of the transformation function having point pn and + %translation x + dv = [ 1, 0, -pn(1)*siny-cosy*pn(2);... + 0, 1, pn(1)*cosy-siny*pn(2)]; + + curscan = [curscan [v(1); v(2) ] ]; + + for k = 1:sizeref + + qn = ScanRef(k,1:2); + + %difference with mean + dnm = (v - qn)'; + + if sqrt(dnm(1)^2 + dnm(2)^2) > dist_th + continue + end + + expdnm = 2*exp(-dnm'*dnm); + grad = grad + expdnm * (dnm'*dv); + + + % Calculate the score + merr = merr - exp( -(dnm(1)^2 + dnm(2)^2)); + + % Calculate the Hessian of the likelihood function + vH = [ -pn(1)*cosy+siny*pn(2); + -pn(1)*siny-cosy*pn(2)]; + + + dnmdv1 = dnm'*dv(:,1); + dnmdv2 = dnm'*dv(:,2); + dnmdv3 = dnm'*dv(:,3); + + + + %Hessian of the function + Hp = [ ( (-2*dnmdv1*dnmdv1)+(dv(:,1)'*dv(:,1)) ),...% D² xx + ( (-2*dnmdv2*dnmdv1)+(dv(:,1)'*dv(:,2)) ),... % D² xy + ( (-2*dnmdv3*dnmdv1)+(dv(:,1)'*dv(:,3)) );... % D² xTHETA + ... + 0,...% D² yx + ( (-2*dnmdv2*dnmdv2)+(dv(:,2)'*dv(:,2)) ),... % D² yy + ( (-2*dnmdv3*dnmdv2)+(dv(:,2)'*dv(:,3)) );... % D² yTHETA + ... + 0,...% D² x THETA + 0,... % D² y THETA + ( (-2*dnmdv3*dnmdv3)+( dnm'*vH + dv(:,3)'*dv(:,3) ) )... + % D² THETA THETA. The second derivative is defined only in + % this point + ]; + + %H is symmetric, let's fill the remaining part + Hp(2,1) = Hp(1,2); + Hp(3,1) = Hp(1,3); + Hp(3,2) = Hp(2,3); + + H = H + expdnm*Hp; + end + + + + end + + % Once we have the Hessian and Gradient of the function we calculate + % the new translation + invh = inv(H); + if( sum(sum(isinf(invh))) == 0) + dt = -invh*(grad'); + else + % the Hessian inversion has failed + break; + end + + if DEBUG.lf_sog || DEBUG.all + set(hs,'XData',curscan(1,:),'YData',curscan(2,:)); + drawnow + end + p = p + dt'; + p(3) = normAngle(p(3)); + + + % Maintain last errors to check convergence + + lasterror = [lasterror merr]; + corr = [corr; dt(1) dt(2) dt(3)]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + +end + + +%Final result in quaternion +R = e2q( [0 0 normAngle(p(3)-originalp(3))] ); + +t = [p(1)-originalp(1) p(2)-originalp(2) 0]; + +NI = i; + +end + + diff --git a/ScanMatching/2D/MbICP/MbICP.m b/ScanMatching/2D/MbICP/MbICP.m new file mode 100644 index 0000000..567051a --- /dev/null +++ b/ScanMatching/2D/MbICP/MbICP.m @@ -0,0 +1,148 @@ +% The association set is created using +% a different metric distance between a reference scan +% point ri , and its corresponding point ci . The distance +% function is defined as the norm of the transformation +% vector q which maps ci into ri . Being defined as q = +% x2 + y 2 + θ2 L2 , the new norm definition captures not +% only the points distance, but also the rotation (governed +% by the L parameter). Assuming θ small enough, the +% distance function is linearized around zero and the +% registration is solved using LMS over the linearized +% correspondence equation, to solve for qmin . + +% J. Minguez, F. Lamiraux, and L. Montesano, “Metric-based scan match- +% ing algorithms for mobile robot displacement estimation,†in IEEE IN- +% TERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION, +% vol. 4. Citeseer, 2005, p. 3557. + + +function [R, t, NI] = MbICP(A, B, motion) + +% In: +% Ai: localCart[2xN], points cartesian +% Bi: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + +global Opt DEBUG + + +it=1; + + +%Preprocess +ScA.cart = A.localCart(:,1:2)'; +ScB.cart = B.localCart(:,1:2)'; +ScA.polar = A.localPolar'; +ScB.polar = B.localPolar'; + + +itMax = Opt.scanmatcher.iterations; + +%Motion estimation q0 +u = motion.con.u; + + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +originalpose = [x0 y0 yaw0]; + + +motion2D.estimation = originalpose; +motion2D.P = motion.state.P(1:3,1:3); + +% initialization and some pre calculations +prec = Precompute(ScA); +lasterror = []; +corr =[]; +y = motion2D.estimation(2); +yaw = motion2D.estimation(3); +x = motion2D.estimation(1); + +opts =[]; + +if DEBUG.mbAssociation || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + + + +while it < itMax + + %% Apply q + % return if convergence is achieved + if checkConv(lasterror, corr) + break; + end + + % apply the motion to S New + [ScBnf ScBf ScAf] = transPolarCartScan(ScB, ScA, yaw, [x y], 2,Opt.scanmatcher.Br(2)); + ScBnf.cart(3,:) = []; + ScBf.cart(3,:) = []; + + if Opt.scanmatcher.projfilter + BA = ScBf; + AlA = ScAf; + else + BA = ScBnf; + AlA = ScA; + end + + %% Association + + %Compute the associated point with the Metric Based distance + [~,assoc] = mbAssociation(BA, AlA, opts, motion2D,prec); + + + %Use rejection rule? + if ~isempty(Opt.scanmatcher.rejection_rule) + assoc = Opt.scanmatcher.rejection_rule(assoc); + end + + if size(assoc.ref,1) < 5 + break; + end + + %% Registration + %using Least Square + [~,solution,merr] = registerMbICP(assoc,motion2D.estimation',10000,0); + motion2D.estimation = solution; + + % update q + yaw = normAngle(yaw + motion2D.estimation(3)); + x = x + motion2D.estimation(1); + y = y + motion2D.estimation(2); + + %% Error check + lasterror = [lasterror 1/merr]; + corr = [corr; solution]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + it=it+1; + +end + +%% Result +% Returns the minimizing Q = [t R] +R = normAngle(yaw-originalpose(3) ); + +t = [x y] - [originalpose(1:2)]; + +NI = it; + diff --git a/ScanMatching/2D/MbICP/Normalize.m b/ScanMatching/2D/MbICP/Normalize.m new file mode 100644 index 0000000..47d5a71 --- /dev/null +++ b/ScanMatching/2D/MbICP/Normalize.m @@ -0,0 +1,2 @@ +function [out] = Normalize(angle) %FOR BACK COMPATIBILITY....% TO REMOVE +out = angle+2*pi*floor((pi-angle)/(2*pi)); \ No newline at end of file diff --git a/ScanMatching/2D/MbICP/Precompute.m b/ScanMatching/2D/MbICP/Precompute.m new file mode 100644 index 0000000..01b2a53 --- /dev/null +++ b/ScanMatching/2D/MbICP/Precompute.m @@ -0,0 +1,23 @@ +% Function Precompute +% Preproces an Scan for being used in a scan matching algorithm +% In: +% Scan: TScan struct scan data in polar and cartesian +% Out: +% s: struct with precomputed stuff +% s.refdq +% s.refdq2 +% s.distRef +% s.refdqxdqy + +function [s] = Precompute(scan) + +Refdq = []; +for i = 1:size(scan.polar,2)-1 + Refdq = [Refdq scan.cart(:,i)-scan.cart(:,i+1)]; +end + +Refdq2 = Refdq.^2; +DistRef = Refdq2(1,:)+Refdq2(2,:); +Refdqxdqy = Refdq(1,:).*Refdq(2,:); + +s = struct('refdq',Refdq,'refdq2',Refdq2,'distRef',DistRef,'refdqxdqy',Refdqxdqy); \ No newline at end of file diff --git a/ScanMatching/2D/Montecarlo/montecarlo.m b/ScanMatching/2D/Montecarlo/montecarlo.m new file mode 100644 index 0000000..df94c56 --- /dev/null +++ b/ScanMatching/2D/Montecarlo/montecarlo.m @@ -0,0 +1,165 @@ +% MONTECARLO: The algorithm works similarly to +% VASCO but instead of searching over the axis of the +% ellipse, it randomly generats a set of possible poses +% inside the uncertainty ellipse and then assigns a score +% using a grid. +function [ R t NI ] = montecarlo( ScanRef, ScanNew, motion ) +% In: +% ScanRef: localCart[2xN], points cartesian +% ScanRef: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + + +global DEBUG Opt + +% maximum iterations +niter = Opt.scanmatcher.iterations; +% point sets +ScanNew = ScanNew.localCart; +ScanRef = ScanRef.localCart; + + +sizenew = size(ScanNew,1); + +% Initial motion estiamtion q0 +u = motion.con.u; + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +%Apply the initial estimation to S NEW +ScanNewTR = applyTransform2Scan2D(ScanNew, yaw0, [x0 y0]); + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNewTR); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + + +% Define the searches bound +bound_x=Opt.scanmatcher.Br(2); +bound_y=Opt.scanmatcher.Br(2); +bound_th=Opt.scanmatcher.Br(1); + + +it = 0; + + +lastbest =[0 0 0]; + + +% Initial score +bestfit = sizenew*2; +initfit = fitnessGrid2(ScanNewTR, Occ, [0 0 0]); +maxfit = initfit; + + +% Some initialization +lasterror=[]; +corr=[]; +xtot= x0; +ytot= y0; +yawtot= yaw0; + +%How many samples at each iteration +NSamples = 50; + +if DEBUG.montecarlo || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + +bound_d = 1; + +while it < niter + it = it+1; + + + %return if convergence is achieved + if checkConv(lasterror, corr,0) + break; + end + + maxfiti = -1; + ns=zeros(NSamples,3); + + + for j=1:NSamples + ns(j,:) = [stdErr(0,bound_x/bound_d) stdErr(0,bound_y/bound_d) stdErr(0,bound_th/(bound_d*2))]; + res = frameRef([xtot ytot yawtot]', [ns(j,1:2) ns(j,3)]' )'; + ns(j,1:3) = res(1:3); + end + + % Evaluate our pose using our fitness function FITNESSGRID which + % checks a small area around each point using a direct lookup. No + % association is needed + for j=1:size(ns,1) + [fit(j) TRF(j).grid] = fitnessGrid2(ScanNew, Occ, ns(j,:) ); + if fit(j) > maxfit + maxfit = fit(j); + maxfiti = j; + end + end + + % If we found a better correction we save the last pose + if maxfiti > 0 + corr = [corr; lastbest - ns(maxfiti,:) ]; + lastbest = ns(maxfiti,:); + lastgrid = TRF(maxfiti).grid; + bound_d = bound_d+0.5; + + + if DEBUG.montecarlo || DEBUG.all + set(opts.plot_r,'XData',ScanRef(:,1),'YData',ScanRef(:,2)); + set(opts.plot_n,'XData',lastgrid(:,1),'YData',lastgrid(:,2)); + drawnow + end + + % Last correction info + xtot = lastbest(1); + ytot = lastbest(2); + yawtot = lastbest(3); + + else + corr = [corr; 0 0 0]; + end + + + % Error information + err_l = exp(-log(maxfit/bestfit))-1; + lasterror = [lasterror err_l ]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + +end + +%% Result +% Returns the minimizing Q = [t R] +t = [ xtot(1) ytot(1) ] - [x0 y0 ]; +R = normAngle(yawtot(1)-yaw0); + +NI = it; + +end + diff --git a/ScanMatching/2D/Montecarlo/montecarlo.m~ b/ScanMatching/2D/Montecarlo/montecarlo.m~ new file mode 100644 index 0000000..84a492f --- /dev/null +++ b/ScanMatching/2D/Montecarlo/montecarlo.m~ @@ -0,0 +1,158 @@ +function [ R t NI ] = montecarlo( ScanRef, ScanNew, motion ) +% MONTECARLO: it searches for a correction inside a defined uncertainty +% ellipse. To search inside such space, random poses are drawed + +global DEBUG Opt + +% maximum iterations +niter = Opt.scanmatcher.iterations; +% point sets +ScanNew = ScanNew.localCart; +ScanRef = ScanRef.localCart; + + +sizenew = size(ScanNew,1); + +% Initial motion estiamtion q0 +u = motion.con.u; + +lrangle = u(2); +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +%Apply the initial estimation to S NEW +ScanNewTR = applyTransform2Scan2D(ScanNew, yaw0, [x0 y0]); + +% Make the occupancy grid as big as to contain both scans +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +[PARAMN.xmin PARAMN.xmax PARAMN.ymin PARAMN.ymax] = minmaxScan(ScanNewTR); +PARAM.L = Opt.map.resolution; %Occupancy resolution +PARAM.type = 'binary'; +PARAM.xmin = min(PARAM.xmin, PARAMN.xmin) - 1; +PARAM.xmax = max(PARAM.xmax, PARAMN.xmax) + 1; +PARAM.ymin = min(PARAM.ymin, PARAMN.ymin) - 1; +PARAM.ymax = max(PARAM.ymax, PARAMN.ymax) + 1; + +Occ = makeOccupancyGrid(ScanRef, PARAM); + + +% Define the searches bound +bound_x=Opt.scanmatcher.Br(2); +bound_y=Opt.scanmatcher.Br(2); +bound_th=Opt.scanmatcher.Br(1); + + +it = 0; + + +lastbest =[0 0 0]; + + +% Initial score +bestfit = sizenew*2; +initfit = fitnessGrid2(ScanNewTR, Occ, [0 0 0]); +maxfit = initfit; + + +% Some initialization +lasterror=[]; +corr=[]; +xtot= x0; +ytot= y0; +yawtot= yaw0; + +%How many samples at each iteration +NSamples = 50; + +if DEBUG.montecarlo || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + +bound_d = 1; + +while it < niter + it = it+1; + + + %return if convergence is achieved + if checkConv(lasterror, corr,0) + break; + end + + maxfiti = -1; + ns=zeroes(NSamples,3); + + + for j=1:NSamples + ns(j,:) = [stdErr(0,bound_x/bound_d) stdErr(0,bound_y/bound_d) stdErr(0,bound_th/(bound_d*2))]; + res = frameRef([xtot ytot 0 yawtot]', [ns(j,1:2) 0 ns(j,3)]' )'; + ns(j,1:2) = res(1:2); + ns(j,3) = res(4); + end + + % Evaluate our pose using our fitness function FITNESSGRID which + % checks a small area around each point using a direct lookup. No + % association is needed + for j=1:size(ns,1) + [fit(j) TRF(j).grid] = fitnessGrid2(ScanNew, Occ, ns(j,:) ); + if fit(j) > maxfit + maxfit = fit(j); + maxfiti = j; + end + end + + % If we found a better correction we save the last pose + if maxfiti > 0 + corr = [corr; lastbest - ns(maxfiti,:) ]; + lastbest = ns(maxfiti,:); + lastgrid = TRF(maxfiti).grid; + bound_d = bound_d+0.5; + + + if DEBUG.montecarlo || DEBUG.all + set(opts.plot_r,'XData',ScanRef(:,1),'YData',ScanRef(:,2)); + set(opts.plot_n,'XData',lastgrid(:,1),'YData',lastgrid(:,2)); + drawnow + end + + % Last correction info + xtot = lastbest(1); + ytot = lastbest(2); + yawtot = lastbest(3); + + else + corr = [corr; 0 0 0]; + end + + + % Error information + err_l = exp(-log(maxfit/bestfit))-1; + lasterror = [lasterror err_l ]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + +end + +t=[]; +t(1)=0; +t(2)=0; +yawtot=[yawtot]; +R=0; + +t = [ xtot(1) ytot(1) 0] - [x0 y0 0]; +R = e2q([0 0 normAngle(yawtot(1)-yaw0)]) +t(3)=0; +NI = it + +end + diff --git a/ScanMatching/2D/NDT/NDT.m b/ScanMatching/2D/NDT/NDT.m new file mode 100644 index 0000000..fac92fd --- /dev/null +++ b/ScanMatching/2D/NDT/NDT.m @@ -0,0 +1,260 @@ +% The idea of NDT is to model +% a scan distribution as a grid of normal distributions. +% First, a likelihood grid is created where each point in +% the reference scan is inserted. The grid behaves as a +% piecewise approximation of the likelihood field. Then +% the negative log likelihood function of observing the +% new scan, Snew , given a hypothetical roto-translation is +% defined. The function is defined evaluating the likelihood +% of observing each single point of the Snew scan falling in +% a particular grid cell (depending on the roto-translation +% hypothesis). The log likelihood is next simplified and +% converted into a score function which is next minimized +% using the Newton’s method. + +% Antoni BURGUERA, Yolanda GONZÃLEZ, Gabriel OLIVER +% The sNDT: A Grid-Based Likelihood Field Approach to Robust and Accurate Sonar Scan Matching Localization. +% Technical Report A-06-2007 +% Departament de Matemàtiques i Informàtica (UIB) + +function [ R t NI] = NDT( SRef, SNew, motion ) + +% In: +% Ai: localCart[2xN], points cartesian, +% Bi: localCart[2xN], points cartesian, +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + + +global DEBUG Opt + +itMax = Opt.scanmatcher.iterations; + +ScanNew = SNew.localCart; +ScanRef = SRef.localCart; + +sizenew = size(ScanNew,1); + +%Motion estimation q0 +u = motion.con.u; + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +originalp = [x0 y0 yaw0]; +p = originalp; + + +% Find the bounds +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +PARAM.L = Opt.map.resolution; %Occupancy resolution + +L = PARAM.L; +xmin = PARAM.xmin; +xmax = PARAM.xmax; +ymin = PARAM.ymin; +ymax = PARAM.ymax; + +xwide = xmax - xmin; +ywide = ymax - ymin; + +NX = floor(xwide / L); +NY = floor(ywide / L); + + +%generate the grids to fit both scans +[ subGrids] = makeLocalOccupancyGrid(ScanRef, PARAM); + +lasterror = []; +corr=[]; + + +%occGrid1-4 represents the 4 four different grids built over the map with +%L/2 shift on 3 directions. See the function for details. We are going to +%use Newton's algorithm to minimize the score function which uses the sum +%of the four different PDF defined for each cell. +merr=0; + +if DEBUG.NDT || DEBUG.all + figure + hold on + axis equal + hr = plot(ScanRef(:,1),ScanRef(:,2),'.k'); + hs = plot(NaN,NaN,'.b'); + title(['err = ' num2str(1/merr)]); +end + +for i = 1:itMax*2 + + %return if convergence is achieved + if checkConv(lasterror, corr) + break; + end + + x = p(1); + y = p(2); + yaw = p(3); + cosy = cos(yaw); + siny = sin(yaw); + + + % initialize the gradient and Hessian for this iteration + grad = zeros(1,3); + H = zeros(3,3); + + + + merr = 0; + totgrds = 0; + + curscan = []; + + % loop through all the points in S NEW and find the cell it falls in + % in the 4 grids. + for j = 1:sizenew + pn = ScanNew(j,1:2); + ngrds = 0; + + for k = 1:size(subGrids,2) + + ogrid = subGrids(k); + + gradS = zeros(1,3); + Hps = zeros(3,3); + %transform the point using the latest estimation + v(1) = x + cosy*pn(1) - siny*pn(2); + v(2) = y + siny*pn(1) + cosy*pn(2); + + curscan = [curscan [v(1); v(2) ] ]; + + %find in which cell of the grid it falls in + [ix iy] = getCoordinatesSubgrid(v(1),v(2),k,PARAM); + + %We need to be inside the grid to have overlap + if ix < 1 || ix > NX || iy < 1 || iy > NY + continue; + else + %retrieve mean and covariance and of that cell and find the + %gradient of the likelihood function. The likelihood function + %is a pdf defined by mean and covariance created by the + %occupancy grid. It is used to find the likelihood of a point + %projected of being in a particular area of the grid. We need + %to derive this function to find the new estimation + m = ogrid.grid(ix,iy).mean; + cov = ogrid.grid(ix,iy).cov; + end + + if m == 0 + continue + end + + % if the matrix is singular, we need a backup plan + [~, cov] = checkSingular(cov,1); + + invcov = inv(cov); + + %Gradient of the transformation function + dv = [ 1, 0, -pn(1)*siny-cosy*pn(2);... + 0, 1, pn(1)*cosy-siny*pn(2)]; + + + %difference with mean + dnm = (v - m)'; + + % Calculate the Hessian of the likelihood function + vH = [ -pn(1)*cosy+siny*pn(2); + -pn(1)*siny-cosy*pn(2) ]; + + covdnm = (dnm')*invcov; + exppart = exp( -(covdnm*dnm)/2 ) ; + + % Gradient vector + gradt = (covdnm * dv * exppart ); + ngrds = ngrds+1; + + + dnmdv1 = covdnm*dv(:,1); + dnmdv2 = covdnm*dv(:,2); + dnmdv3 = covdnm*dv(:,3); + + dnmtdv1 = dv(:,1)'*invcov; + dnmtdv2 = dv(:,2)'*invcov; + dnmtdv3 = dv(:,3)'*invcov; + + % Score function as error + merr = merr - exppart; + + % Hessian of the function + Hp = [ ( (-dnmdv1)*dnmdv1-dnmtdv1*dv(:,1) )*-exppart,...% D² xx + ( (-dnmdv1)*dnmdv2-dnmtdv2*dv(:,1) )*-exppart,... % D² xy + ( (-dnmdv1)*dnmdv3-dnmtdv3*dv(:,1) )*-exppart;... % D² xTHETA + ... + 0,...% D² yx + ( (-dnmdv2)*dnmdv2-dnmtdv2*dv(:,2) )*-exppart,... % D² yy + ( (-dnmdv2)*dnmdv3-dnmtdv3*dv(:,2) )*-exppart;... % D² yTHETA + ... + 0,...% D² x THETA + 0,... % D² yTHETA + ( (-dnmdv3)*dnmdv3-dnmtdv3*dv(:,3)-(covdnm*vH) )*-exppart... + % D² THETA THETA. The second derivative is defined only in this point + ]; + %H is symmetric, let's fill the remaining part + Hp(2,1) = Hp(1,2); + Hp(3,1) = Hp(1,3); + Hp(3,2) = Hp(2,3); + + gradS = gradS + gradt; + Hps = Hps + Hp; + + + end + + H = H + Hps; + grad = grad + gradS; + + end + + % Once we have the Hessian and Gradient of the function we calculate + % the new translation + invh = inv(H); + if( sum(sum(isinf(invh))) == 0) + dt = -invh*(grad'); + else + % the Hessian inversion has failed + break; + end + + if ( DEBUG.NDT || DEBUG.all ) && size(curscan,1)>0 + size(curscan) + set(hs,'XData',curscan(1,:),'YData',curscan(2,:)); + drawnow + end + + p = p + dt'; + p(3) = normAngle(p(3)); + + % Maintain last errors to check convergence + lasterror = [lasterror 1/merr ]; + corr = [corr; dt(1) dt(2) dt(3)]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end +end + +%Final result in quaternion +R = normAngle(p(3)-originalp(3)); + +t = [p(1)-originalp(1) p(2)-originalp(2) ]; + +NI = i; + +end + + diff --git a/ScanMatching/2D/NDT/NDT.m~ b/ScanMatching/2D/NDT/NDT.m~ new file mode 100644 index 0000000..7332ee5 --- /dev/null +++ b/ScanMatching/2D/NDT/NDT.m~ @@ -0,0 +1,266 @@ +% The idea of NDT is to model +% a scan distribution as a grid of normal distributions. +% First, a likelihood grid is created where each point in +% the reference scan is inserted. The grid behaves as a +% piecewise approximation of the likelihood field. Then +% the negative log likelihood function of observing the +% new scan, Snew , given a hypothetical roto-translation is +% defined. The function is defined evaluating the likelihood +% of observing each single point of the Snew scan falling in +% a particular grid cell (depending on the roto-translation +% hypothesis). The log likelihood is next simplified and +% converted into a score function which is next minimized +% using the Newton’s method. + +% Antoni BURGUERA, Yolanda GONZÃLEZ, Gabriel OLIVER +% The sNDT: A Grid-Based Likelihood Field Approach to Robust and Accurate Sonar Scan Matching Localization. +% Technical Report A-06-2007 +% Departament de Matemàtiques i Informàtica (UIB) + +function [ R t NI] = NDT( SRef, SNew, motion ) + +% In: +% Ai: localCart[2xN], points cartesian, +% Bi: localCart[2xN], points cartesian, +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + + +global DEBUG Opt + +itMax = Opt.scanmatcher.iterations; + +ScanNew = SNew.localCart; +ScanRef = SRef.localCart; + +sizenew = size(ScanNew,1); + +%Motion estimation q0 +u = motion.con.u; +originalp = [cos(u(2))*u(1) sin(u(2))*u(1) u(3)]; +p = originalp; + + +% Find the bounds +[PARAM.xmin PARAM.xmax PARAM.ymin PARAM.ymax] = minmaxScan(ScanRef); +PARAM.L = Opt.map.resolution; %Occupancy resolution + +L = PARAM.L; +xmin = PARAM.xmin; +xmax = PARAM.xmax; +ymin = PARAM.ymin; +ymax = PARAM.ymax; + +xwide = xmax - xmin; +ywide = ymax - ymin; + +NX = floor(xwide / L); +NY = floor(ywide / L); + + +%generate the grids to fit both scans +[ subGrids] = makeLocalOccupancyGrid(ScanRef, PARAM); + +lasterror = []; +corr=[]; + + +%occGrid1-4 represents the 4 four different grids built over the map with +%L/2 shift on 3 directions. See the function for details. We are going to +%use Newton's algorithm to minimize the score function which uses the sum +%of the four different PDF defined for each cell. +merr=0; + +if DEBUG.NDT || DEBUG.all + figure + hold on + axis equal + hr = plot(ScanRef(:,1),ScanRef(:,2),'.k'); + hs = plot(NaN,NaN,'.b'); + title(['err = ' num2str(1/merr)]); +end + +for i = 1:itMax*2 + + %return if convergence is achieved + if checkConv(lasterror, corr) + break; + end + + x = p(1); + y = p(2); + yaw = p(3); + cosy = cos(yaw); + siny = sin(yaw); + + + % initialize the gradient and Hessian for this iteration + grad = zeros(1,3); + H = zeros(3,3); + + + + merr = 0; + totgrds = 0; + + curscan = []; + + % loop through all the points in S NEW and find the cell it falls in + % in the 4 grids. + for j = 1:sizenew + pn = ScanNew(j,1:2); + ngrds = 0; + + for k = 1:size(subGrids,2) + + ogrid = subGrids(k); + + gradS = zeros(1,3); + Hps = zeros(3,3); + %transform the point using the latest estimation + v(1) = x + cosy*pn(1) - siny*pn(2); + v(2) = y + siny*pn(1) + cosy*pn(2); + + curscan = [curscan [v(1); v(2) ] ]; + + %find in which cell of the grid it falls in + [ix iy] = getCoordinatesSubgrid(v(1),v(2),k,PARAM); + + %We need to be inside the grid to have overlap + if ix < 1 || ix > NX || iy < 1 || iy > NY + continue; + else + %retrieve mean and covariance and of that cell and find the + %gradient of the likelihood function. The likelihood function + %is a pdf defined by mean and covariance created by the + %occupancy grid. It is used to find the likelihood of a point + %projected of being in a particular area of the grid. We need + %to derive this function to find the new estimation + m = ogrid.grid(ix,iy).mean; + cov = ogrid.grid(ix,iy).cov; + end + + if m == 0 + continue + end + + % if the matrix is singular, we need a backup plan + [~, cov] = checkSingular(cov,1); + + invcov = inv(cov); + + %Gradient of the transformation function + dv = [ 1, 0, -pn(1)*siny-cosy*pn(2);... + 0, 1, pn(1)*cosy-siny*pn(2)]; + + + %difference with mean + dnm = (v - m)'; + + % Calculate the Hessian of the likelihood function + vH = [ -pn(1)*cosy+siny*pn(2); + -pn(1)*siny-cosy*pn(2) ]; + + covdnm = (dnm')*invcov; + exppart = exp( -(covdnm*dnm)/2 ) ; + + % Gradient vector + gradt = (covdnm * dv * exppart ); + ngrds = ngrds+1; + + + dnmdv1 = covdnm*dv(:,1); + dnmdv2 = covdnm*dv(:,2); + dnmdv3 = covdnm*dv(:,3); + + dnmtdv1 = dv(:,1)'*invcov; + dnmtdv2 = dv(:,2)'*invcov; + dnmtdv3 = dv(:,3)'*invcov; + + % Score function as error + merr = merr - exppart; + + % Hessian of the function + Hp = [ ( (-dnmdv1)*dnmdv1-dnmtdv1*dv(:,1) )*-exppart,...% D² xx + ( (-dnmdv1)*dnmdv2-dnmtdv2*dv(:,1) )*-exppart,... % D² xy + ( (-dnmdv1)*dnmdv3-dnmtdv3*dv(:,1) )*-exppart;... % D² xTHETA + ... + 0,...% D² yx + ( (-dnmdv2)*dnmdv2-dnmtdv2*dv(:,2) )*-exppart,... % D² yy + ( (-dnmdv2)*dnmdv3-dnmtdv3*dv(:,2) )*-exppart;... % D² yTHETA + ... + 0,...% D² x THETA + 0,... % D² yTHETA + ( (-dnmdv3)*dnmdv3-dnmtdv3*dv(:,3)-(covdnm*vH) )*-exppart... + % D² THETA THETA. The second derivative is defined only in this point + ]; + %H is symmetric, let's fill the remaining part + Hp(2,1) = Hp(1,2); + Hp(3,1) = Hp(1,3); + Hp(3,2) = Hp(2,3); + + % leave computation, the Hessian is not positive definite + if isinf( Hp(3,3) ) + return; %error('Inf'); + end + gradS = gradS + gradt; + Hps = Hps + Hp; + + + end + + + % if ngrds > 0 + % totgrds = totgrds + ngrds; + % gradS = gradS/ngrds; + % Hps = Hps/ngrds; + + H = H + Hps; + grad = grad + gradS; + % end + end + + % Once we have the Hessian and Gradient of the function we calculate + % the new translation + invh = inv(H); + if( sum(sum(isinf(invh))) == 0) + dt = -invh*(grad'); + else + break; + end + + if ( DEBUG.NDT || DEBUG.all ) && size(curscan,1)>0 + size(curscan) + set(hs,'XData',curscan(1,:),'YData',curscan(2,:)); + drawnow + end + + p = p + dt'; + p(3) = normAngle(p(3)); + + % Maintain last errors to check convergence + lasterror = [lasterror 1/merr ]; + corr = [corr; dt(1) dt(2) dt(3)]; + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end +end + +%calculate the covariance + +%Final result in quaternion +R = e2q( [0 0 normAngle(p(3)-originalp(3))] ); + +t = [p(1)-originalp(1) p(2)-originalp(2) 0]; + +NI = i; + +end + + diff --git a/ScanMatching/2D/NDT/NDTTest.mat b/ScanMatching/2D/NDT/NDTTest.mat new file mode 100644 index 0000000000000000000000000000000000000000..fe8315b2a737179b34bf27c9cc100cf42ed16997 GIT binary patch literal 30301 zcmb4~<9Zzo!>rphP8zJ(wj0~Ft;V)(+gh=0Cyi~}ww?7o?>>V)f94eCnz=_-P(?;i zh>(?@kx*7ph0fH%+Ju%+!P?N-)Xvd{i%?ufPD7B5g_cm*(ZtZ%#F)^|mWxpRKbNy} zCH%ue$i&F?=MNVv8{r>DCPu>lPyP5QDU0~y#}664A3xl)FWukRmv&x2tTRd(LJr7( zM8jdd((*+@6Hz1oB%`8{Q3*?gj>aF8CMuPSV0ms+Vo?{#STN3XY@T$7!smYI-MqZ} z@?Sg*oFKkdt!Q4<v{3#et0Z%&NPZrp~7y33hkLN^W$VE`>_K-=CL3s+ws^d zy=@+A^zWH;1RURQD4x$ro>-`c4?f@T58ltquP>fhkk{vq?)XnolKA&QD)G2j&?_iz zzHii<1f?aJ;!l7R$n5;rmt*in|}ThGR8Z3tnYXk>PLY3jwL5R1-Hzv-btc8YmL<75mR=JnM%ej6rX_iLM*vPYy%cWyY3hf(Lm5DK1!VO7u!KmaBhJ z%BmJi>k&NWJ-bU>>(epJxOi7Jg*xVX&)3RU>=fI6?p`)aq=1DlF3Ux@hQC4qM z8oShvOEwcc>8-0SO(s);cWwu4!;|oB=uGph)TRFS-FHZ_6dT! zZH&DeJSh@BThC`0_1P14O=*pEW`&!Ev&Z7=QA(7B-y^_+M8Ws~LBTyz>)Byj zNC~=MEpmQ;!=dSW_#TE_f{|Xf9iIkE6!(oZ#d5vZG4qWUPMfz*5a$#e<%Cyu5e>T+ z$MgrH+{CVk0Ku!T7JX*C#Tmo;s>vvs*cf{z~ zOc*2RUDvJjv=SvK@m$ycn#&Xpl;{jONg#Ddyp(|{jt{o$%bJM{lMv#!Oz3k*mQ1&L zfTQU66VWnVD7;B0BlI{JQdwn8p6|nKM3k8;I$M{lcil&%p|f+Mc+u#*DjGGK}-3m*gTPONp7cbjbD_*zF1_lhfTCgl3VJCJ%`wGM~|us zA)A=b#$jEa~cUILXPa`N-0bRe;@n$kZE|2nF&4HHuE5< zkHdlZ6;|L;Z>y)zss6rq&c@^4pfc*z^48vJV*CuO^3gE(UM(bsKnW=q ziI5Se!M_Llzip7=M1}!mk3OaGvdy=ZQu zpHY^}%yl+;%s0zsIZB$;%@u7k|A0kY-^M#A>SnY99<7gxa^DKgJ&&=|XD&7=J=>CF zOc@#)Cc2Ta+gxq!U41mRo68m}-#>A;TR`Gu|Nba!v$(15YSaL3Hn{*6mV3=Nn|6}o zNV*9gW?9H~z{iIkW>p4&gKz}9`BUC7kU_FtO@}e#@4c!X7H4VS?wO4*`uTe3Gf@WA zp5b=Dv@RLZo$~ZBdRCRHLtH1$CJAI0+35G)bE^HEj{(U$KI@#@GCE2jh)_XQ=GpkdX4_EqmN{9-(#(qL(i1|fI`s{Heg&-H#> zvQ4^rdMsNi!!~OL;++p`V(&Ok)<*$Wrh6TXAb?+x?`4E`bF~klS!+~Wp)>frp0`8N zJYJZ6EMhOVj^t_mQLRv}t>S*Arm|0YC2Q2av?m?9O3d#@oKl z1{Q-4nv-kMcp^wYNC%sKXHRqnUf50S7lAEylLt=~gMU6ng!fN*L0os&P%1xN=pcOs zoH%vp15{n{CSxIReTxUIa<$0)=fD8rp{PN|>CVUTNF=5%Gio;bNUBhxe3S`p6m+BJ z{OrWket5P)( zrra=ZJ33v+O`Bxlv+jJE>p=5m>DwRp^u!mH4E<0GcT*COKm7eoq|=&I+7RE>zJm>Mhc<23re2MW& zx05#qwy>j9zKOV&tm?!Q2DtsMkhO)RW1-XZGVI<~WlWptlA9f5(#RL>BD*wHIwAF; zPS>>9sJ$?M)}orPzmwX#+LSQbfnIEMr3DtImsph|Ct zA*lIzp%3xX#DUstxb(7|7Ci#n4zDIvRJ=oIU|&#E=c7S7IiD^J?XwfGoSiU0M<7Sa zZ<{obTy@H>wLRf5X>nzWHI1f0$$UE&>;}OWP9Z)fTJ^6g1t*f?UEQFw!7ZVojVH9L z{f&8E75@tJC@q*{6@<+12UCrr_^&5TJabeR)?E2uqj^g3=ic8a8;x|ZZ7TFQef@lY z!}1%llUC|orwWtn@+lhoslg6F2$NlbikG4b^s;9!W}vCyeh^dyAWnAyed9R`oSh) zgR>u1pnfmJB%gs_+Tk106rzVvx({szLnv9e74b(is~7~M4!!A#wvTTY9ntpMJrHqa zTa#7#@OEF%Nz3XG9+AyU6yb{CNNjrlb$?QKQ$vt_^D#aM=WAHs1jE(gZ{6l-^Qlkv z>G`M6$O8echPI_cc*1l56o#wlA+&=*m4H@y9xH^!(#AFU5#5g?hwGpfD|7?@PQAUQ zN>1oQr-S!CcBo;;{<}R#Ck4hodA-YT4A$i=(p#InzmC$bjG9MIb%sCqlvTCs zQg?X%0yy~1+7vTA-_O{4kIkb2=TYmG@*cko1fXut3|*OXxJ?em5 z((LT&qpcn>8^c8ncXbV_&sH34Tkh!xh(X;=0a_-BH&w#;Hcto(`t8VFGU)IoH20hG#9DCnvfmm+!y+F+Bax!W4DHW5M+0)C{~tQB^GL#c7*!Q}nq71;15w@VX* zo30f<<(OtcNcqPE4J+m)++bJF^*J{wdQ>S6v_7k}OyhC0VQLQi8(ufQ+Cs zHW2*rwNN`A@(z7OS;-o;;EVDb!i6CREEA^wD4~YjL={|={&R6_D=ge^=Xmgr;5!Vo z=lA+QW4P-*&vZ@zb=SPruAlqWTFtLMUx!&uF7Brr5c_=Jo-K35e^%wMF|YjCJHWX9 zHzBqvS+zZl@(w}YyWVbQYKB6|mBqsP;0>V_vGRS^QEQO$NhEyy&Q92R!jgV%7uwOb zLDP|MT2)?CE`HPr&iGltu`z zj)c**^2!`4TScevK=tF-J;g;*_+~C6o|os#e7OE?nPSY2Fo=DWx115V?G7ij*@|QCH+76w z;5FTr!Zq@{n7I$4&dlm~eB0Wu&TGF$?4n1Z#jW39Q+nM6;6VMaNbcwE)l*fzN9kC`8IB7mnTJH()Ja9ZNf(F>0kCnrpB-UxIPdv}- z0uZoi+~Ic!_j11U6}wpg)M7mBsN(UrbCLVCI08an+eVJAK+^EyXc^13h7%W;rr=A- zc)47(xdzg`!yNW<6;{BZ8-D5aB6;7%zqvV-NYY@<9rup9$lxw4tw|RMK!fLl#1{-C z^gR@COPna*PJc_O`X{8y%8^s%Ba?wkOG}a!-mfZrO*HA1KL-Eo_GAs=ex<_eGUWL6 z`}4nNA1}vY8LYpQpR5Z*lHzKn6n#eKP!v{gjzPe;Wi)-aXmkC-B~kZvlQs2!43_&m z*{}wYzuh=a|JjwTaS^k}{DqG&9dc=K!bE7SI(zT~@^GT7vjA-zSPSq#!=L`~Z4{{4 z(~4$V-kAt4>=$=yKmmviZyK?k zC)wlV4KAubIPLOoVL66}IjC2~8QGK+D6Q^M50bcD1i2r_Lc&;%(EHVl5fix&T7M1j z0Om*N`PzU&f&V^x^3pC{)N0iUSx&^jbo|)coKRqJm@}fsu%A6EF!w!jMfz*DHn(Ae zNHI_FACNPM1rwIMS6PyRuW#A|TvJGSPGz~Xy8~ey4k50ii2<~>pXO<{WQGH4m@~tW zfbmP`-@xW117>$p4UG(#?Sz9zoCi`4b-mQrz8-bBoNvv>fee_FR;(k)F~+{`BCffG zBp4`!8@t!bfnblgr>&8$40}eJ<}~!aVBPuqD=vzRIz3YBv~M5sYzwZYl4BkesU~@Nqww%dM-pxgKCTydSjkzT8(8qEL;Q0k*K$HIw9K%0eMR%M5%Vx`F3XGKN?oQ z+;K;AXxt6Wto^}2lJBoI$^ImRmVw7a2@JSX@~~JH)H;JPAExa4tYGh_R#wR@iU2b2 zd=Wnz1O+OXUT!!7p~Ub;%h@e3i6l;|GS71eo{RUlr!*mC=x@%~^VRhgLMAEz9*r{O zl-y_in;JN!UUGz7vJV`e$?dSz~PsEJvX9BizUw?}tC5pucV>-C19Jg8!c0rSxcgWe~L2vnOg}_mY^T(4WpUnD6UiNR&PFdz4Quo1=yae<4XNqC zRWYqtKQ&3VJRpMXMJ@XRnXNKdwTQzrR2})Mt|#-cPej&5$b&EAQxPon^iq=_@@F0M zm0U!X%E*t~Y`<(wYV!9+8)Ajp{=L*gS4AbY`88Q%=VLT%#mOtoP-q#UsMKrQgeKY} zjFqAlyi%Vo4fCjr12qShJDuEaIc&|fd#}T5JuHw9uT%Xf6LvWxl=CaxX6X=Y|ANH zMcbp0mDI!^Rhg9S=THTuA!&+n;}ac~gmn&!OvBPh8aG;WJkK0(DcVgX&Za`E3oULcxr6q7iDUWR}?kFCQt`y=!f824*T2ZqHM7 zZlZ`q6APt@8HobP+WmK9VM%Kr1DY*5$_Nm|j~^hA^$HA&s{SYCtyR|wPnwW1wIbk8 zQO^!)X4#|)wQ>-Z3GmNM4U;kTRAgd(xKpJ*Y;j_uF?{|{#fzgl^HY;=^=IKCjR>0wTi##-BP-`snYPh;%Z_z>f!s7?0XN# z2?g#n;R^NlXreT|PeJSZ1*Z=UnjNM};C4vY{Z28&nBLs|23<1XtWF&#;Vj73>}p@+ z9s1x`EL0`D6~ykI{4-Y^_{dGYi@X#c;+P%+yheEvnEI5Obt`JHg+n$}ak%`3lUA|D z1RE65x;#e-n~?pO2ZtrSIr^9jQ9_qie6V{8UjYb9utb|kb_#nQlF)V-Np9>R(0dQj zNgH|_aC?cxIqAR#B*^7#Z-X(KU{ydG&$}2zeTDi0@T8t_S*>D=o)UR5VNGUDcrUPR z#uD*vT@i@@y;-~*8@eD@VEa2Uy(vmY9e(@QhRyO{;U*3GdlWpqzO z4%X%34-Ra@4jrrVL2;g{&UR3csBu^M5?b0{!C{7$ZiZe&$AaUsFD2w|kjKU^hKgj4 zdMSgxhX*nQZ=tOVUzN09DCLfav+9>^U$>z{4K$bL7M5h@^@E}9QiXk-5!UJNSP-qJ zb$d3sxIN3ARb|q8CmL%rfx<$^P`XAMw_`#>c1VexBg? zTP=_br`Lo}VKsdNJKMZ=Gx|Z-YLZDsavhzup}~|jLsvtgZ=?&&Wu6i(`|k5vP0vr0 z<8-=FG>^h2v=r>+5hb5uX_*iNm_FOmJ=h}E)>udHh_ zTufl&$n7~ZQl4SJLjH0W$9_g$|hM`?JgH#0adFHK;f|UqR`Rrma0a zTY2^od$^;O} zecL)`_G3HGtzU(f-=%VIFZ~bu$HxSlK1hwGOQDS(KD+KmGUK^VUv--2XSF{H2q@S7 zqDw>d2(;dgorixjC?u#f2JW-|s7LL|6Nwe9i{@CTOJ{|4jC#52UnhFK81Z!xokC)f z(Q-t+{DO_R7Ns!m?CFn4XdlM89;jQI6A{f|0n_14D}PZE&)YI})X}v?ja-MH#8-(H z`uBY)#XDeUu+7r(>F~iG&$xfKJ6P;+VQm^1Mm~@EEY4YT$H!p#2hen zxuH1SI*Uu9@^c#U_SuOl|Veauq!@d3sT&`Dj<*=)lg{lPs= zXU+Bh2h9iU_=LgCvsvcmXZcAUCBD^C`5ib;=Kx{t+bgi%d-=q?&JbRHPCDc$+y6@X zQNml2WKh}C7r^cY!0*&HY4Xkpb{Wc_QBo-(^eH-xD}Ox$iQ%`Y&tHy67i- zF`(YCUlMG;U<@Ymh+ONu!u83nac^~i`jOruTF)~rI1FyDhJ!~~RKlWsY5f>~S6XPd zdux~y`u1t6d(FVhoR_psmQ4L4>J9UV<1*W|FacE}y=iLjk2MC{HpaIrI+18E(bBf9 z;rX?2z)@Te77bxe^=<6Umv$;23y9w>pvsTn>bMvvTWj(nc7;%@QI($T>9mSgS-kOh zr*eQ4oPF_K*Eg}h>YRRFoAji#r8oVcSvP&bd$0R(5^^Y9RD@wTIQXhJ6Cuixe!8Ho z(E*K!!vMoMP1|qjSlCnW4IsVzbg|{|h4~Ja)am`Qg}KI0aao&`7aAF=2(OYQMV;Bn zVz-g(Pt*3Ku?CaGd>^||ogxERzB>Ju{1orsJA#N393Z7FTW{*hN`sK416BrN!zGp? zTy1esUm95^KqmcozruY}_YxX1?_V6yh zaD1uYJ9tdTLopOj_kQZLZLTed~{mDtGT@4os09`yU7?=DlMT@~%r~MY+>_5e#G^>vrfg?3?vIFte0LE$;cz(m7xvQHz)?@Oc|@^7A_uQ6OWgVBxNoWz`Y7%C z$G1^=UawYUSQ^d+s%+SpWrCTP^;lBunofTJ!8ozLhJ^9N_czwe8%!$POT41P+taa= zHR@&6B9^(fuwaG9MAtDN>~(q*#srseUvIhJioBUZox4#%{**8*z8{@aQ%O|d-|Lhf zK$Xs^b(buUKx7p8bn~c@b7o+t^{-gHFNu6A#1o!RZLz+xv~oe8F;p#wwujLHPS{M; z_L~&8g4s*z(j>4{$|le&S|vp%RnwI`%eH=X{_MEwL7S`b{D@PGPwzVqRZ|ScIhRlj zRgiRAc-=)7W9Lolj}6a5kghFyMvIAOXZ|(kAG+ZZ zW1ldXjAbnn`s3DE8ErBw&fe%G8E!P5=i-LAB()`*4Qss-Nu$2dhB0fo^&X+-Y!(JN zEjBCP^!1-|TJj`zKFy{y`HQZ9k85tVg&OG37khv!gPpC~o-PbM#Gj$$v?-IB5rt0d z`gA8&q^l5Du8w?mgw%(kR;5B965Df@a-TULp}~^`1U)<==r70>n|yIgn1TAZaq(?6 zWEO`Yv<^rUaJyvQozO z)C20U?h*%-7sw;~G~TcfzeBpUPe*enqQd!79X!DJdIlJ}v=Q=! z5y9uUqxx#QUY5Bp{`*|@Xr(OT@9l}(9cJF`r>6|Bvr|^N~wv{~KX?!w}GHBy^F)UYrOY!3R zII=t7x*n)BG5DTrpZ3~Kfs~O3O%%fbsQXh~#lf$fisq#GTytPHxFa2%~!K z!}Xps5f2KV+FvW0XjD*0Rpt>fW(mlYxJ3Z`OU%r!nE_*HN$--J0aE0y$;~q}boY-l zE|6uirw5*ZFEQ)lTq~9Y0%isQgftSyyLUxhNTcvQ_NV=66L7lpHg-%W3*cmT!$%)a z9$Y2)YUi$z+#fuzPw8n+bYCwVBI?xxye*-Ip&gSt97hCOb;h9Th4XY&46DL2=(^o? zSkQ(1@`0)iPM6p_ud#0k?L@^tMB5UWO73qBit@ACk=%>9i;xGrSKcO3dbkZ7sHUs5 z8{ThBskU9t29;paTO`YWE4q$oVW(BKF(+*n&y7&iZSGdE;CuMF2k0w7+O1gMho>Y6 zQrz8$@yPvg@3uQm42*%VxupmUA~9B|%$YcNSH{wzY*S_EPTOXnzB2BdqxJ@Fo^3GE z2MG7lx&W7|1ZL{OE3YBHXlpAGSFeX?8jB^6yd!?SA6edFa-+BjzLa*pQ{9mS7pj0VJ1at)4$)kMr%6s zxw4WlF0d1=_2>`!cINN{+_u_hMN9OZ0($svY8W50kqQabcVlPvn2duj=nE9bvfG?= zXm(6EDdZ9srHUar>!>atSZfe3IpD2^yLi^_LY`x7QG8vUzZI<(UvqxTb9C_QV~vN_ z8Azhp84aC_xdEPUy9`;eq;S5FhR4iEmM2%%!J0wm7VsLgUo^_QA09N#0v z&mK(zW1}enEm(Eaap1Iqg%b;gg>a7^&u6fLu7Uty-Je{kDy=~7ELrVV&beCY%ncWF zV;I~gqgV}Ey8f>6z0NoS^Z-_h!iPnVcW?TROFfxk0A(}YG)lTgU~=MXjl%n9F1w=Q z>{c)INsKDHd^BCZ+}(#ZHX>ahqUf`OLOuX)Kp61LR638{M4C^h1)QtD zVrQt9Hc->Tw>{_&Fa0>5=$U5Vn(515-vP4lT_u#4G97R&KKUnM2mt;Y?!er7Gx*3< zv+;)a4bFxB8=O!IL*eO&vxN97jH$Ptk#u5_JQJy zTbt%u*DV0zruvLsEtANw7s2fw!Uuhkh>R8M#;`AOPt%tH5j~n7ZZ}8rZ{Gt7+sOEa z2BY-8%MFI7Zc1~r#=&1M8dnuOHGR>U2YKzgum;|$(|sfZ3GV60Eav8t%jx?4huW}- zB5F;hx5KcvMe(ZDCZFd8e-@ncL-ffe#nt$)LyAHn3O1D#rCTP7#)DS85g$#KwY!P0 z@QDB5u1Ecs5|Q8-Jg0Z@xN1vN@r#y$n+0&rI=k#$yMx1jU_B;=PDHroH`|3d3H=dP zPoX#MWcCHwe4-9)(I^BE(;A-=LxIw4tH*pjA>5 z>9}6)EgQn9xKYkyhF``Gr;|!jsH*5rw8~b8<0gtPx;Hs}o|?hW;k)ua4II~BCwHAs zjvBJ%S4qmr1KPtwbfGPdlptPL-5M%hV86QakP-r-WNp%&M2avT`;83~`(a%>)ybd3 ziLaZEn}9)3@AZMnqIXsFd)y;v(J%k;0f8cC=SVFV`ZxQtXo8*M9@fDAnY0anjS8v0 z>+WNE%fp~UxlWp#`!Tm+ok5QEozs7zV?E(d7_B>kA1NpMFlfQLuxcNp(ID?lBBi@k zHf#U_(oH2YaTjUeWn8PCJg$JqAWHeH*~MbEM02&jP%h*Jex%!3$ESFI_oz7&gC6Sm6;BvhB=yW@@=PGY@nur&h0iu~Exf9siu7t5EAFBbBzW9)Cl{J}F7P7A= zi{lz}Z_{vujXfMW(R=o@oco7jj{E%Fhf$Yk4Ep{z1N-@K>|psPBWP15nhqf6S)BB9 z+nK-sf=zvFND{+Yjp5QPc9>91+sN}v+_l`Fe`%*X5iLmvn%1+QXYtPY5hHw6N;4IS zMlLR=V)e^+k*7Z{3h~jV8J#P%?oMW!IVkzQu5JuFWnSy(+TD{Jz`D#h>W!H_j%6t^ zuh#76_rFD7b-6kEm5{$?_8U^?g%3BEr=f_++)LlL(qr&olUgMwE zeh=IpI_pUWo}9O5ecaTcNM#t`n{u2O9U3;vYKNjoGHY>ejUu)7L2MV@C)c9J0WYP@ zYsgLx;L6`iee$_>@S%ti=Wpk{Si2||wrzau3Ad)J&QdjX-h^N^p|}i@Hu4YyX>M*& zr%Es-=j-%+he;kWpyLnt}FtL>|KJZ!aT*1nkAh~VoO`!KeUO5oY? zNm_00vQ$v_0k$jdL;WldTj7T*Q-pzK6YpE<+#SoR{z2{gIx+q$1cx-xx&$D@i6C3g zQzI9GrS~M#$#)uviRGffd;ltn4RRcDZc zBB0&lLBsxSx^h*4GMNOe9Qtgx*`#weGSPt#Lrwr6)^%62z&Qg5_YS4D{b=#CX!|h% zq%x{>!izK&2@GE^_HL!Xz@BW`mN-`uQ0_|JJS42Y%aBd z&fkkJ%&6MYXY#ta236vF#^-=I2^oum2h2372Vc<6cHlN4`y~3bdtSkR#+`Njo_%o^ zWyv49RrDEf8d+-;U`2GL_Q?9RHdjHXJuL!|dE&&*Jv8?4r`S`--gHIwrR{9zoutV$ z$V?_RZu}nObZFpEe%)2~Z>0sqg1g3!s z&yf+PqAsQd93&Ua7Jxd?-T1p)Yq(;JyoOYCu5bTfG&pZ5GhR&bgTb*nCICd>3)1vA zxm08LP#4t*8{=6I2D#_0zrLFLz!$o`3Y*W{V*|!m=Y3K~Ja(@KsvTo+g&0|FQK9Wz zIU&^I#D1+; z-#)!RbXH$1HlD7?($#!U6Rwz*f(z%#za|J!{Z~IMYtcSo!Ig#YRJ-Y@Vg0}TL}HUM z{=FK949Gm!_#sHdbd>rk1>*4P(&f^^61qFTXPu#AKpUEK(53zx@BU?c6XimsjxLbh zJnM26#SLA;{UYNev#AtNSotz^YKbOLgW_eFnGBUMaC^HZwuY@oayx~-XMQvj*1~)= z26YVukU46<5-l~p+--cW@*W)5agR^!@hRa7GC|r|LwVL}UbU-;pm=7SESKX*XyZqX)+JZ#iI#@)!pPDFjGP{T z4Zv(Ha;x)+4g|R%@&x@(%0D{Gcn9!P_ceW4K7G?Xx=4u`N$I{6V~)DcE5ON06Q@^H zQrRf!XCYh((JNI#3=j&o9Wpo#6A3Mp?fkzZ!&@|Svh!* z{HkIA(|bSUMfJs2IK|4XP~@o3G3?M(D)4roH?MiFapd)Y)-$`o{d!%_H8<2S*O%yM zqapY{tzo!PXAD05#V-Hi=Soz7iA}H^1@iO9WqZ%7*7D<-0NwlLxt``oyU%hKj0!@7)AZ6M~zI-$Pmc8p!a=!i5@0zX2eAH;7pmu`3 zw})MBMi}X;ZUjhLW=iRh;r_+Si+8`2^Vu*%8ssqX_H-%`;sZaDBbpg@`evw@u|Lt4 zhOn~z4X+`e3suX!nVhqVDINuh2ZCQdT&H!{uGLo0!hV@)hI%w>M+6TN%hUvYW#A%l z&b^aZa_#5-goIFg7oYKWC&(RwjPh0hU5V*&93p06ZhN|BX}w(_@ytG7dZdYhdU0a& z+pp&Q$ji+Tj$G{^ZvNd8FBE}=t59R4=dGe3!ym+4A?QYI!_H_$t6`lS3Xg=_Mr_t0 z-X-h>5#fam+rp(gC*zy1CPT5&0eG=%g**Z@zp2tG$LIro-xa+GV0pQ3^d^4 z#bz`&L4vjIkFEDo*Zq+?)p4YY5CN{d;tT?CMcGIb6Id*|ws>9j5u`AeL=Cg))vBSn_(cr>G59V-`r zfNtwS-w1B7RX+Rugqvc(^~l@Rq)v|3<>CDHS~CPTfo~M|^UoKDaKwa9@rd`rzn;ha zf15Yvb$UKtI3rJ_foIq3XEYMSX9iD)6$2Nq605pgM^q222?iBgpu4BaI`~m5ueDuL zq@U6hLQBzA0GJYN>b&E+E3ikK+z3tQNdMnb_+Z3+5rdwgnh^Xo>9D##5U|c8HjfPB z@j&KAd2V2R&V`2Q%Z^tv8o)dDwQont*X>c6sX19fZ%i%in?f|;R4~@gZd#|25Fzu% zq82|R(7;oQSA9{D9Ihqe2sc6-2dtP~q$ZjmisG8dd4;M%qc8A;IdIJtCft zLAs$urDe`Zx%FtAA2OcfAchXgia6r^5I{y8-fhh1JM(vCP|BomL-$_WjC4hb2Ylb6 zY46R4*88jGpM~>1RS$G@ol(6h>p5`c{{Y@A#3amHTwX-uL?IH6At=cQUk}#S4_NVzYqO}x+pTz zcd82>tA??los~!Hw>LhC32fk+&Imc_vc0S0*VO$S$NjaltM`w~U%(ii=Ut>u$*UgI zDd)EIutvMi7nB?CT|7NqvTA^^1Z~IkRLk6?c!rexlno+-1}hE2g|Zn6AfqzJ$XMte z`B=m9&9H;UwML8GwUr_=-rRtjQ;kDASd+r7%A)HEy05KvY^4cWXg{(@l643Pxk~Sp z1{0I(fIAB70vAp4laMaXQ8UkqxL?XIty`|g%YMe*wxq|9Ls5EM%OHg_fl8=dOl`fa z{IPJ|!Inid{_Uli;LS)>FW&2U_X~xI3(G`TU0>_q z?O$um3tYMgT>05~@Xa_KaXSB_XX#dL%#5cZj&n5v0Kmy=Wnr~2c-yN%0rEQIDl6;l z_ATp+i$68?6p>4y8Xm}NWqZr@6CO&7Jwm|nfr;QzbAKq7U~=`=sq0XEwOHV36~@+P z(?9~$x{b|ry9!6g#o(-r>BaYz1a>@POg{$?N;8@6+|x(p>z3o9+T@3FUTPoE(x?ml z4QQA8G!DSbhNv|4h|m`XbwR!Tr%GwM*NGp*K4a6zyyjqcX2vIfL~tr<9lTzMJ4rp z{ga~AS!z(ttEsX5+u8U*vbe6SevB+e-@mJFnWj%Z)SFQtIsdp2jKv-bb7)pEbOLr7 zvnLK%JXR|!>mfK4R*@D&&l56)heD5TmuuSuBBD!XKbZc|F7nw3rSHtwipQ)cx6O4K z7aj<|t+T<*h&EIbP7^Nbh4W|+oy|m4`)t;0!+TWMKJUwk7~b#8 zd8+yq5LQ}Loe3+4s1h=Irpj80!v&^a-M`RxazM4yxvCFT%ZbVM?{``E!eB;Z=xcz6oB5`wE0ulQaqMHPDc zSf$%Nh5ZMt16)tD5r%yho}=l`bjA@Ve2txRklw*%ztNas^IM<0I5T|`=ShUV>P^Ji zjFTDXGtpwvD;@tXzl-ot_-F|0e>`X+jAI^w)M^ccPrQ}_IAJA|h5sYczBzxY%?*c> z0hu^>-fS3O{{jLfRB1NbKaRld=lJs&-j6u4NA?xA?Gu$AV(aA(o2?IlAPt*_^qBk0 zf@7u((($c>rQcc7CHGP_Vl6g0pXU0m%OWA-XB6i=zAozI*KxV>2C4OX&iOf|`|%K8wn=mdS(P&i*v@wFN~ojD)Iiz+&-J`4QFMiTs72Venp|^pvB9a$-*(- zu}=244tQqyB)8|;&5U2!%9W^iVXkV}Q=x!7$~_aXCWkVA7k@`57*~-O=io{5jZ6ny z9A?eL-s}^VTBk6o^#$>qWvi}d3~ewYsn1>v0qpagGDX=__65_Lx+_2r1$i>yVzRi; z#biNtTJF-tgb)@**KukeXfUs{VBHN1AU$wtpo3SN#ttSj5oRpBH*OMrm=v5egOA!H`MFj!$T1>&Is}CW-Q>y%yG$o$OleSkd}-`Wl~kj|x?^8H z&dVbz;Lkg>86^WFUnUzIN+wjv`n8*ty@eCPW_If}Q1s=Du2T2M@>AUbEIl=&Qm%%u zJv%BIP4@nAL411~Xs=YsLpSgb6GM`ew6m|9)u{pR)uHXR7aqw7+&y25?DFXyHyloG zzL2S7)bv-t^{l~PkE^0ymj@2D)rwvue-vk}DbQ-h3Y*fr>Z`4Tqn`8mz1+Nt52jI) zT(`)m%7}gDPnHw3Goinh=D&ftf9hR6$FU0tGa4+zl~t1Qqk^OxhvWij!;RgZw>9G7 znaYK^Z$x(35F)iL@$pv1gt#W^cw#c5S>~;RsC?XiPX*^9<504KMjvMN_Libt^*4n`Z;BVkl^OO_F{?7~YXXqghtF-@ zlz;-7(%Ve}S)K7U*f;|BSs?{wPEDi2!2zn5mPFALrSPO8^=b2n0-0BZ3>$}~X+Atp zv|5BW^AaS$uLD12ybGB@hm=XZ)(ZH|dy8|OzFp*$B7z<^6WPh=k?b${oC39TD+bA5k=M3tzILv#;njsW<>hT5`N6T>bopd90SSRJRP-r!J(eEIXeP&)mU) zcLGQVX&WS_?-jL>a^OFos+5G&D_M!pDXrbE!*cx8vYGvM(eS&d%aHE`58rh8Ok4Sn z>;WCP<<1jlwK7a8;8Gl)&28!9G+VlMRU3Xa7gde*Njd+7Qk~4Rk)kK&*0-;qA%n)q z**Z&WIY0k^9Q9xBC?%jC$!Ud~5QnaEw~#QnOKrryw47q{w{zv#>BI-J%7)WRx)6SJ z)kKzR*k39mAhS&~Pta^@0+X&$K9cWm;`OJ~Y3+uzsIIe771e(CPn*?IrE`vA2re^fQE zTwMA*bD8Bf1I=o`F2yrNh~KahBpQQ@U`3N3cZ5F)bPxSRqMPhiA|CdIq&|%{sFD}x zN37*>)`54gW*s9(O;=d+OleMXAZzFK<*kF<^2#XgGrv-bZ$%#5Y`M!f0@Zi^s7Mgb zOcjU*$dn%s;so&rk)tBL+ilz26$TR{^szVPs6?CmQ+BqiH0+>l>u8UqO`i##7Y+bK zyT}-b-GCa8C{Z4#ZvY=w{oqUq=o%LT!A*U8ujt(5&SYfQaJ^%}yWBzQrBdkz)pb|y z?)WGG8eB6hZ$qI50bjxQg-Xh}Kx&;nGU!BH~1V@dVn} zF9b&<&7P&dEAnSk^l^i1K6%?@SZ=*)gRz%w<8wZj!*Q zQ4TcEPTv(}vLL#3hZl7f3pV}sYWIp@frk{oJ3W*IM}vq9^50X-zdp z9zLHZ4nCV;!4mQDq|eUs@P5AGwv09wFn3Q@C-uvLbHTil7K$9CNN()Y-N^*g@y!Uk z$%5}(@!xUz3}9q;_zmeYpyn7^({;5B_&I9VFgHkn(v0Q;Qj-J(Mt<~K&_D<8k`?Jx z4judsgg*{yrb5`dhSa@=RB*kx`@z$KS+HIy(Yt@gEU>&Y@08(2afn@~T8{n208$d( z%vwfdIB#87}e=VtF8oBm8p^%KG(O7O@ z%k7X!lrmbj;Wl*wDYZl!YtIOfCu&R9avwv{$FzJeY#2e*phHJQ7LFi}|M4eN3_h|s z(-i%b%0nqN(_IUshmp;|6Fs^W7cm|x+ex44M@!qzuQnl0N7&``7k`jV%kFsxAO1mHhb{wt zX*FtE<@?R{(l10TSad-?D<8p2E13`e-w?@~am}QdKz{G)ZDxAjA`&$sXtwr6OsbnF z7O^J{+dK?ByzyWz_Oo*Evn&6M9~m!@@BdkVxp9$)B-1N!q;;tly|oO(nECp;_BzaQ zt2dlEQjc9}>fyc;4Os8Q{m2~`o3Z`<*RejCZP@<8s>EQ^4y=7&gD|AVd%)nD?i7>HrwAHwsYWPYUQmSozxNR?VjaI(HX^IFa1>cX9U>Z zCs60&+i`3!cjd9!+)2#Q*_$!Ecnb64{CfKwrm=}ll)KsFG%iYSWkyC!ych)WUE zFB7dYQltqZX6L?*$uh)o-&>EuUNH!}^bF-qM`Q_|BRgV~Rxt@8PR&^0H-W{f$*T8b z1X$AK6cT(G#eUaPYd{T{~qUJ^*gOj%I5u8-9l!P3_X~uSrAaq zZ^lgxoZo4W8!*VGTOZRZ$9}Qy*{dV+u;00_*!R|ROyZfdk{{+Gm^EcKc&r$K#UYtP zij|1lQ~t9@p#gCj5?}k~v?Cji{?%8zdeHmMu!~Cz`cXn^&EC3pE}~v9U$OJfFiL1& zC^6>DM=9Gn{r6~(A*Gn7MG36}^q!rbLXDq7q*KNfH-@KC{;Z=f?DI$ve=9w+aF!_O zC4HSMZ$}2k)PBY09b%vzHDkX0syOs1URdWsqd?QP{Q*v;LO!(7Zn2_)_7hg-WkX@! zXlb3WUMvCHC*x==MJcdfK6}#xU1`|U+VC(9Xz;& z0h|ISKkbVQuvGkWt|5&9_D0Oz3#qao)|25kVI~W_O09?sfwB-g2r8Gq%7Tv7dxj#5 z3FTUAigLCK^Y!)9tPg2S_$spPN?9)xj%UwRCNYKimdE-QWhe*vb|<=ciE@x*kk@Y~ z&H^LTNj^D~1@Qv?6dkHO6h`<69tX$+$P%B=iz|T56}Ptb2n8ToixP_PaLZ8u=D%SGObASteDlgF4Wo{fQd> z!5t_iUN_r4rW3*LjKYABE);BD(Z3jXA=mFL|Aw+|6q)nTvfio(G1?tMosRb+BWiw= z#Kt~EWoI@XSo9a|DZk&6rO=N!lX7n*=M12lSF^VTGPua-!n)0zBw^nBdHU{qqd~-I zRF-IM9z-0bQIvPk5aO9CSKAm5Ba4r#I*#@XBT^LI*Ypby>E+F@(v0P!$rYpRf$1YC za>0yLP{kFmYvWkn52#i^DwV67Jg2(QlGo-&{P%opiV-JhN*es{*$3!UrmUC_Q&re28yt${^ zq)Sl(Y45^>RrN^w`O+v$@g}rh=Fk3tgDoh7?s)nXr45O#@$+|)=tf1xBn_>TJ*dbg zN{?kbfcDhc@rJH*5w9V4q2Gc*q~bA>wApkRZ61H>6L6A`d>@=#GubkNi1Sz9#B_`y z1Jvre^_>9K1y&lae>RRpzbA*KRZb#(r4}#u&(p~Liq0CACke`RZq>~)6M>-#ug32A zqL4b;D(EvMgOth^??nU|d?bHzU)qVoi+5Y+9o{ty&KVbtXdI!y4#n01+B_-*#TSQJ z`_Vuq;M%EeQFNd-@@VyBNjPs({Odef3f>!Qn3!N`2$fpKEczw`y8@mZDKTY0bwA_u znPm)6OWkEMN@Kv9HgTR=itxQy&>8RICj+_TIZ0NUGSHjUAJ|$d4O*KQFGvlOhNSr_ zlOplbkT>2W5$r1syq`<1-CZvY2?x(;e(R9})!4nOHHxL6dbnVK?=J;Ij2i>Geo{bA z_t(9=Mha%`s9aq$M+#D$WW;3#B*7x;+z*#7NjSteAU!CM1i8%UwnMihVLxl%rSE4X zp|^TzP?(`4G{}fg2X2sr2(!B7b6Ap);eo>TOi4h$>r9}1iv%pr>aAYB>>YtI&s}3Anl}$y0nD^z|jU}OC$khH967Z>JpIeu%GjkECEMTH*C-B zqQmz>?Nc^2bV#oA&JF%fhhGP@|Gao3v@^n&4&SE3e&jCN;~3_8IvrV+k76U5MQ{ zkVS*7R|j;}U(=vDRuL(8^{H_4^v$PlZ&SeEvtqY{GX=QjKX~RN6tImX@Iy^a=p3T2WgNkid;faNGcvbhlgswpcxjE}Em1WZbHwKz~&7#9*Tk7;* zQ#Ntth0>W<9&94n`)^KF44Wv=b+~pcl}$`L$WO|avWb3mu^nG{Y~lhf&N*9(Lr9tD zpS;iJ5V=RMEJ~cqAyQI(BqDSR z5H=BfS+@AP8=GjU{23Ex!X_S7dTjY>!6u4yR(bbt656v04ywrr?}y$TD%D~W9fE=Q zasE7_*go}^ofMl`H2rav!4fths&wlLjhIK|zqK^2^qWU)mRF2d9-2#BKC|fQYt6X? z813j-{z8TD7;Zk{(>{l|EU8djVLgXfzs)UPrB<0x_>nh8S*A?reYV|R8#bFbP}CYT zXZCDDJy%$HgewtU$`z*MK1E_)h0|#@4@F|vC$*JFpDGagVsGeb3l#_(`=dddisXrq z*0l4kHu8iH-AVl*!6FU}AD#^R!XgsC9^RBD&LVi8`T7UcS%g4y3uAAe98s4aok@8r zN3=hid1k&@j);slO?uGDBx2v4k#7xQ5;60Z9a-$cBpzQ}$_U^v32IK)5B|>ISWmel z3l}wDj=Vi?`PPU@R~qj6RMg-a%f(}v&W+e6DKh`>zWaav4g8;dcR!)OcGQV) zV<&E*o2^&MM^2<5Yn!z`8TMI-)%dlDyyPRAf+5uqaRM1!IiLdja*;-dH1Fe^Zz$P) zkEFr9?!*DV@t^tq4M|Vokw35p*b(V%=xBLin`;AaBW^0>YH8?9$s6H zepGI5_+t=Bf0qfSWtsBb3?;rl50`5rBvU$ zw{^(1WcvcunBVBt)}fNo<$uuQTPjhPR{ud2p;~p(_Zks@ZPqi|x+b(;F-iLY2bzW;9d5l@ir$L0%P(c0-db$VXy9@i4TagQ;d6L+jd5boo-*0}gG- zX+3$L!rFFJB6mM-by+(~HE`)GwC_N(-UPT^tn5I#hB02J{W?)_Ls_lPa3^x16YwLg_8(ua0c z$#?h~^r1~rNykO?{vtJg;?;-tf6?+A4=I0H{b)B$bj?(7KiV}U&i;F40A2g&R`N}r zi#BaMZof!}i&R3cux-wAQSN%~57kgET6QmflPh}=Iei(pZKF4c#$_a~W!xS_FXPvd zwYmpUC-3OOcSAR8t@6&J~M=v^{d483V9{+x_l2BM&+?7b)-wfXl!Z2 ziN@R-D(QEsts_YKLc`6~dq$D4kd;r4 z7)8e=H6Dd(jv-#ymz}mIW9Z~^zFD=`7z!7hO#7TMhCV2cSOzQ+pcBuJRqyc?lKUYe zrc8huU&r9TjRKUfX6GnaG>&3-tqGc)F^-V=6gMt(92Lu5Utqs|0=?L`EpPP11d^Md z5wkgH0uB9A6?yY_0_|uPwN|H_Z)I}agK6~5Q0(}!B{PUzxUy~c z`5A=IU$jV8nnw0=_BQH&X3)7EGLK}ROrv-utuQ+l37(1f@n2_6qrk@X)LRiG7>SAj z`N|ozWlW7B5;KG9LRnMOjWZ~uLWQKSN`j`C!_Qh4l0a#~_Wf8O2_nlyqY7VueDqYU~;uRpOY*TGl^S&%^PhspTrJwp+D@DC-K41omZN_ zPGCJVBUNSZ34C$iCB%2& z2!1l}lUQ9JA2+CvT1>n0ac$EQ>WDEP=h%HUP!jMkRex~7)kGdnRJ;5}c^?leG;T{O zVDs>UQ)x9R6~lOi8CluNeHa&2OXTz!4`VN%1M6M8hj7rAmU-(w4B?pbzFbbk5ca=& zWZyQ%5MHlWYJc(jAYSub$y@K{AZ{2Bx#PEf5VK}(FCC^1VtZ>{72P*nyyL@}6@zEF zcvJmvt9E@Z&M~)dbT1yjs@uH>)m98(2%t;N``V8;sK};BtnJ5J4nT%`_+PA}!cBj~ z{EH1kJVwuH_Tk?po?r5w_TnwWf1Y$3_Tsq3$4-0x=)n!8zpsW&bz`QK62_x)W^{k3ir(3Y6DF5-3+{K|S99AgFCA*fpB|X%FpS!9 zLY|qrM@t(X>ol8XvAqqeG{x`Vl+%LG;Rtihvu$`;*G{AOs5b0tY#!S+w+T(aq_{p{x&3MbH;%e0 z{^>cp4sRoA%zo@vi({*f_daZ?#b@gc-UQTD;)uGy{h{y6v3b$%V{hi$o4b_ZWv!m=+U;f7qfv9Oqgx?f;QBlxT(l7PY<*qNaQKd8oC9Ne z^$Kv3e)QXf#Ra%5x3A8k|1%aHpW}FA=GZ7vRW^OX4c`8{?~*mVAuK{lo_ zsAkDxnb_-q+=eydN%$dS-Svfy53pxIK%x!%5zc&bcx8;U2l~=^q1;gNIr=XE00960 zMArv6R$u%8aJyc6@A0Y#6|zG39@)tZB@%^}Jql&KM&@g;@V+w2$WMzy-B+NoEQC2Uj87n zl@ArAgge-p^C7xWelu2gKD0%6C8{Hu59w<6a17S-p;t>|TDWmOWO!plm1h?};?kcm zx)a8aT4RL2l~?eibmQAPJRkW{p$Zqrw4eaG`8~}>)Kvf#E~q!_-xWZQ1=YPQ<^_;k z(-(HDB)#w5JDDC_o{vm3nAjb(MAJBVKjB?zGzv6 zFgjR#J(`I_1YK#CJ;G5Ug0B79<08cZI1vl zVVWvBRLPG%Jh+k*p~a8lOySD!H+-noiZobtmJbQ`-1U4T!iNHH^rQrr@uFt>MPPH~ zMSIlMJ9L?Ok(}DxOZSI7XlaMK=b@uK=xlR&>uz=)R5Odp*~Yk$Q>lpI&jN0w@4wQy z6xL!G;t+@2g@L&AwO{U#q-5%bw}Eawg@(#aZ? zWy)qnnpU^|yz^s4vIYDTgb-Fl|D)?(k`61%n3ZN6#j_&*uh)eA`*El|_)cZUQygM= zGCzGD!=W!X7#~F3#-X^~X}oz6I3%~nX;I!ChbCVr=U5)XA#%c3fWfC{ZjzgK?zp^HcLp+NW3yu45s9Q%-uzNcWiQQ=5W3|2& z(HZa$E-~WJi^4UL3P&8ORjh0kKZ-*l+q3D6HvJYI(PU+gL!L##M*qBV$W_TlP|S9- z9czYq4^JEtRz%`4rJMZ_y&(N16^Dk4&RW`*N@W*WQBIzh_Q2-%7Sug+4u8jrYMdTrm0ZW6q1UJX+sny{7=Nb>N9<-rqdakh zF3ZjO8fJq7eXMA^&DmXnL#*g`NM>J&92*jQJV|B?WkZJr|I4MHWBOQ;rs#NM+ljn`+gZsQ4<2$&@k|`tbCh`NUT) zq%|>Dux`(d3^~j~`--^HR%5NOH!M8JEGNdOz>)_IP38XLc*KK>)#kPHC3w+GrFo~k zN?z39^;IyCjSt;=6I!=e!H2NzrT)Dud}v3q95A);qZduO>x?P_=&y#|5bv}AI^paR znUO7sID>RGX1C(eq1CN?-RyYu$c)>wm=}*EG9rWBx$$Us0$+^}KOS{h#q(NA;?b^w ztrdq9@TlqH&f-}$Jc>ENBU-f+kBBdHoS*9AQBKbL&d-2H*0>N~QxiN&a3;HS>EV%Z z!4VrxB|O@pA^S^iFCJBEm>0@x;8B5D?2SSdJi>Sv7G!1dsG`ev=w>^&W>>c+;E_Eei}KENJjx#v(T}{1 zN9PAQ=}I5qQB_+vk?;nO^v^u>*Zzk`eH(K3@C?%Ej#P~zJ+m|tGn8C$dyYrSy^ALt zH{%nX8LUj)Hv6+g?06`HM^@(VmU$MXkZNcsUVtoxeEvI~S*Vnwo5e>T1lcWJU2=t)+EqZfye##9!Vg*c`4Vc&JyT;io04RUIMA&&faqWBaVh< zzEswBi6gg$0dAj+Qv*kD3gKql~c{y*_CP6n^~e-!NMVH1ZZKObHTb z(Ei-e;hPf3&{eZ-VoU@Rp@jDRYPNn?|nTB17yLUXpPlJf&k3XdX)4)4bxv!RU7OMZd z!)a1E1Fc&39WNgI0agXWzUh2FV2B~WJGA=;bcx=O&Lqu&jsdyb_4quj@(TEb5#}L# zpT@4=UGq?Iy+-Tf>jh{oess+@@F%bzIdbu6+fV3vDYlJIXAyF?R{Z_L`wLitQj}#K zf5H2QR%(A$enYMN!1BM@->_{?NKaLE3APwSi0hayLjzlxg$VZw?9eFbvf*EaOgVPC z;Lg9`m21RC+E@edw>_>txdEr&(6=*s&>;q;6B_JR^yq}uQ|*f_{;;3l)6-!hn`t4r#vA-x<}5bkd{+W zKayn|a5w7zWyxacKt>Y6Z{Uowc^S9!&FF^J7PqW)fVkU4A9xVFLaA#fP=( z188C#BJp|aB>3|)<}W>^y*ozr?_V|1P-y3w(q=RgMj z;~5Vm7`DU51y1p^4_s+h$~#WhWs#wypKRal_fd~cLjS0_whB<)`pROkynze_-DB6h z%og-S?@eUJtLX#PITQX5-|9Ex1a=a>NKGD2^O^c%-O~4Bi4pRXR*x z^v(!&h1W+2VN@Mu`WK*)3_ntXGtBv?^zu)oDZiKy!SX0h#DogzY(B4PzbEoremewQ zXc~$*;)?lG%(2!;aLpmZ$r7HzR}Bbq)#a89kN9F#1AGFS)iub_{3UaWQw@RDB{7XR zoj#Z~MM+T&y+v&@AZCT3@9t^!{J-EXk zqdFeMtHdo%h7Xoq^e>)Sz`XMYFI!&_MiuRAuAqNT0=6A?8-Z^ffVh_sZCMtIrBD=k znHy|uLjqfci;XS{4xn-DgCVb0C`Og1ovXv}14qcN�+Y2b z9~Z(Wg~YST46g7tOYUDKTLd;v@x&6Y)vdJ%9Dme?HHLXYpV5QLm(Q+CEXt?SR%$qacDnX;$h`{I6hjHC%;U9GQPmq?5~nBs$ZtA5yJX&pqS{AAhjL| z_dKL~RELu=s)^?ciaq;s;VM`78Fr^D(3|2gG!RI{sQRZ9&x90bfwJx5*4O3X@VBH= zCv001wo1|AK#0ejdmgM2!@rd8JqbcDeY~Z!A~CA^%Wlt!@kQXu>s@M|vkg8A7BeR( zI$%`g_y60gT}cMsxlz{ZuMOd3u-u$=wja%dJ%m&aSu)5+{>~h$@&?T3K+{xT3{Cr@ z!i2LzgQ1lc`Sb_w3-N=6h*!~Twc@3 zf{@9Z8-2>@5GC$&?p8r65GYD99b7U{C&J%q;#EHTJa8u87}4361ji|U&XYLxX)GLu z8WLRC4iv)ekGyAQJ!5D-+^wvacF+rc9@k{yvLM0rgKw?#+d^q}MQ#)6cz*H`uK1zR8T*jVZV2X`wS@irM={Ms4O`^pldTB#bUbn%Y}97*4kIOjx$ zM({bdHSiqPL@{Bbv1XLGA7C4|=5F~SP}Qtqk^LQosZsoEHCxoN#RHUieRP4T5MF$H zoj17}gFUB6X#KtJx7!EC8?E|@-1%@@VATD*aUw=F4wK}&I2i`ZZVxqe({sQkceG!d zD+Qx^^tf(gU_dkoE6)83Do=-f=4~=!#8ga^qNPi8=r6MrxQ4SSlR1_IJ9mw)4A`Y& zaui>uoukt%%7CDVjQc%1<3W-5%8h)Rh*8zm`N}D$o&`DTKV}Wsqd|2;O3Ct5GDh{W zOafPXTMh{H9X2ZMj)e4@*TL<*NmvHO<-5FvaU;1<`2A!=_!eKl++JiZ>%?M@DekfI zif;H?0Cug01ruA(g4x^QWpj^cjB4ej`qg6sEbV8_;q71>_SvG+?-+KNqJ3#VRA31i zo@>9568Relfk#=d+gMZHJEKz?OeGX3J|F?1 zWJBNggAM%DIsHu7^#VNP|P6y9nO#n5(T5 z&j33?b@-cJG_X;O*q_zdRbL3EX)i9{6849Ah7Qd}i8xrGSg$M_i3kO763?SS-w_4d ztzYxj8zsXwiaN6`9tIw{5b~~N*||Fwrn?%RHLj(=6h%A5b5EB}X2ZohyGFHt#(}cH z{Z}*2M4H*$4Nv*6i_R}4~~3GnbUkL}Dlk)}r+L1`f?6Q)nsbCA4KK>0%?m~g*`YNd zkVcm&eWAJl>N-inJYBI+Pq9itTwKK055!Dd#?)&I;qJA}KTU_DX%chTl`F6_;86SY zFIV2?IOq(Cyd!pzX6SfvSmdlNFecZ~wZA1nrB#E^HLW11qPU`PYwYq43z+6sOK96m zhCwk_(dJQKn!yJ-v~I>6faJtqKHnqA@aog4eLP>?X_`rf?TvY`7rG6~Tvj&os55xG zyULsj*jb7Nv5&{Yt=T~Gr2ILxbTYh*C?m*3xM717)yy}lS1T02LxY!pMUD)R-KCc0 z<%dyy8uOggMl^tikb~$67a2wZProXE;*Ske)ZF;HH{iZ89Md-$O9&^yF5JJp8{8on z)rx|CotbbuSZS}nQ2w$jFVJ9iF z|a}YAQx`+lE=2>zPE@ zSc7ozPX5-IEjJY7n(Ax)x!r{_y zPd5vZB#i2E<0#XHUmnavx>z~YT>_;Ewu4u56ES6q_vKGdDvaiVR4f}`rELH_{k!~S zuqzJhp_o%Bo}j5z0OxB*Q`6p`0}fK{ms@Nx7}fW`^y@}W7eeH)%Sm>%SlHQi#MHPi z1(T&{|ISa8zd9Rid4|tqaJzt7`~FvBLg5(ImnwYfZJ&yOo}l)m_wi*|+R~9TIh%-4 z)vqv->&(i7Lt+wKO4W0fWIlfrGYgR41e(D{-{X6s21SU zI;#lz@ZneP>tPo`X#c!CI&N=ElNeTb{OEsV*zL_c*%{&ni+X>~C>@Bzs79?_ue_LD z1fdV2ilS=V;qX%6@bH%?%$A}_(_8&m#UhYkZ9LJ{cMQ~+68?C^Uc%HVPE7Q8IwWSJvlpe zogK7|-=8Y+G@wcR^xhh6?!WGT{K_8lS{;0gwU*kM&VV09Mq}KnlNTAjdnZ<@q}xDR z>dDH}M=sD@55v7|s3t+Jw6TGK_zBqIn?2Q(5l(Yh(D|eM%OZ%gXwFaFaSjsI_m{d1 zMAP)okO>HiECh}*`Umfx_yY4=1;d8$SYV=fcYeIRthWFPj-`D5*cb+G`35#A`ibC5 z5!dbKEG?K1cL&w|Ch20~xuoFfGfgQp@2{KZ<+f)-|5>k_i=VClTeUaIv78936rYXq z@7Z}c3xfJaWBvrC!o~L|7{A^8e?JM=x8CDR1A`t#r|V9c5W8rWr6iO}Q?mKd`?D_+ zK;Bi=Ugc&sTswYudc7wFN+@0m@a@~C8VeWH`>QQybHQFJn2hR@XufnWS-yQD5`M1! zdh>*(0QAZ~e>yF2h333JD$`jB0N;;WxCSQ+p}4|!y8aP?=J_OL)n4Y);6?v^qUAdY zth_|j?vw`7yp@?^6zFRXPgFy7f0dDe=hWh8_V1%KYdsnms|N6(_wzyYcqkdR4dEC_ zy&f3V#s%vz<|!5E;rn`QA)W+R2j@R+tG$TjQ?!zjF!Gr^3~hwg6p`@)D4V;T#^n%; zQ4P`~OxGLxLR<94Wz{YDK)x1T(rK57QEgA?U=!E~1KlMSSMi;hFzBE8l}{lRqk6^r zgT_AHM9_2;o7pyy0=$Wl(mq#HF{%#=jBzh9QW?`yoMj7pBfHLzaEQG9sT&NXkSwSFvzqP=-Hct%G~~<)tEqx>aQO|Ev7Rh zcu5}rX?eg3TCRNO?`!jiM-*SvcU->4Nd~6mo9A5LsRO>(``W~eInArml?-3+kb#kb z*;lVrAHtgB{pDmmFshs-`n#FJ$zXc6WI}Gt4gBvWTzVZ9j!{iAum}wvD1zq?Qi^4F z1cI@Rv65Fz9A-qZ=SfSl@__Vx&a>6pAs>uwP_m>Ea7?3Wl(M;Z661d z9aaLqy(t*g#1;W-waxsoDdm&E{U0fCl7s>u51xPGq(^nd@|+joxyhSu-bN#nto0mXOyo+IDt?4ey%$48}#3}o54X2Emz z7*(dKl+(JNS}<@T)T&2@48cqWOpI6jXg<5;7TCsW0ULD;t4ypUc+ObP-qd}GCf!qg zf|#@`koVGa)+ZK1dci85P#8_q-hM}6!wxUt2-{sZe<>gSS9;hNmnp!t^kk03DwXE9-dL&YW$Ca=Kl05W zAr?5p?pf5wY}TWQ1&oiVcV~my?XRzYbVh*Z?X`cVY)LfPbXPy{JLN%bQY*7WoDU4% zk>J;GjiDJN@1JJ2zYutwGw(~byFhhV$QZrkC7LJC`Rs0!B7q63+w+Xb{op>3OSpZ_ znPxaqnqWZM%o|0HUVJE}1!{ItrT9M%7}X1`>6hnjkl|k(`>?m82T1V0sokR+g;Bj8 z$C-4~r3l1=v~K>c_Jd6ADhpGMSS*X;z!t$>RqqR+MA)a2b3GEm=bHRv&L?41l^s() zC$Z+iwJ0S;slfy=@Ocp+yP4MQ;E4 z1lE#vs$!ou$2s>*#7IC2Ms;5Efd6@mSXf_A^tJw#4;r%P#d=5LF{&n9|E=j;hrptu z;wQe^A{d>%XK~mk0vn=^Va-QKar+k5joH7RRXA-Y)sAD%tNKIX#U(=YcA{?kqYGm1~MWHuJGa)80nt(rSH3hc{VZ!-=ifgQ#5ons~= zjk!>Noeo_*83JYvzgTy1CD1%~(8g|fG#|#T_@*PW{2}nsB^%w1Seg|>trar~1@J!r z00960rPFy(Q&$uR;1EU8PQeAmrC^nciXzlfgi84t5QHkYp-~ap(NL^G1jJ~OEdi1j zfv^Nf0*c7kDg!Q5S$t}-Y7lT?7b{SLg1DkkQEOx8z0}%H`$x~rneW{@bMCqC%-qMT zo_2Mf_!<{8x{tfuia`}+Nrq|7_OKk3It+c@I^>Iy^^K(}ZWMGVFH5uUcqYm4l*g;B zY;(tkw)JWGgW($AJo(IGOO_nVmM!~Ar^*?w;f#_OeO!&ly$+ z8_6e>?ykiyF?k9sZ|tqqcCmw{&#w`OW^+i|q9#{8xxWINg64&64_JpCea{n`-FK0p zlr=|vi&oi-&__x6#ru|8$W(z!BN+P|DSVR0lHWi20WPE8;iD7VgK zJups`LQfwrlPdOs?=AZDjIoduP%01JC|}FVfJ5PpyefqN?Ce^?q>LbGlcVo7hK`aT zWW>z6g>L(id8wP*m?+IO%Td@X$&kX~SbTP0shB_X=z~YI5zkYqE|bB%^VWqUwv$~@G z&&Ar8M=AG`pHnKrVawAK4QNb-kJgDL9P z)6XU1h36{gZH>t&53J_oOT?(46n-(T-mXdrYv(IpFS@lKHyytd{&ZZ7dP+lP=dQEq z1Zvw4TdfP{%G2E+Xz6MvBf{-R z@5+rLJPiIa@<^T-k0^^&W)p3N$ryd-QIk9%30D)`Z5lXYjeCMw!c*}n$n@EordyQ& z8}cdh`3$i}Lnc$qTbByAOqr8%q5!K}3aTaN39O{dbJgxJG?SpF-+G$2M?C!5#bzxm zLgT>wn1#)w)4-{7H{`~}!oJJbO^_|p$n3AY%Nj_-(*T>>R;AHk$LU8n)(bUOpI%|Q z_I5h*3jm4t4}`jeKwI;t`dZZy_-i4zZuYaX)Of<%qqmrPF@?9Rln z#Nw50L1DsZOdthRo}IGA%A2ErQIqy;6XYpLI*0tKpt4ug{_9FbL$HbpdM2T2?E z^S*VlqZP15Qaxu$c_i)iKyGQ!1UY)*dNZE11>?~Q-;Se~V#$Y;Nk5t1 zIQLNw)N|j7GF;G;6=s%Maw z6!W#($6K@~khH{yH$xSEkqDhtR6aXLhF1^l$Mj5zB41KYZd-C@M~^pj84QMnI!CBe zhw^P`Q){hv@@+8HUre=c>$d(&-N^UqMyb>K&-G%)UuEcfINO?Vdck-vHnIu~h_Ri2 q+%xZA`sw~dKkoa7{Vg8-uNB;Oh5374&My` literal 0 HcmV?d00001 diff --git a/ScanMatching/2D/NDT/build_NDT.m b/ScanMatching/2D/NDT/build_NDT.m new file mode 100644 index 0000000..3b1cc7e --- /dev/null +++ b/ScanMatching/2D/NDT/build_NDT.m @@ -0,0 +1,43 @@ +function [theNDT]=build_NDT(S, cellSize, offsetX, offsetY) + % Store parameters so that theNDT structure is self-contained. + theNDT.theScan=S; + theNDT.cellSize=cellSize; + % Compute the limit coordinates of the scan + theNDT.Xmin=min(S(1,:)); + theNDT.Ymin=min(S(2,:)); + theNDT.Xmax=max(S(1,:)); + theNDT.Ymax=max(S(2,:)); + theNDT.offsetX=offsetX; + theNDT.offsetY=offsetY; + % Compute the grid size + theNDT.gridWidth=ceil((theNDT.Xmax-theNDT.Xmin)/cellSize); + theNDT.gridHeight=ceil((theNDT.Ymax-theNDT.Ymin)/cellSize); + % For each grid cell, + for i=1:theNDT.gridWidth, + for j=1:theNDT.gridHeight, + % Look for the scan points lying inside. + theNDT.theGrid(j,i).theScanIndexes=find(S(1,:)>=theNDT.Xmin+(i-1)*cellSize-offsetX & S(1,:)<=theNDT.Xmin+i*cellSize-offsetX & S(2,:)>=theNDT.Ymin+(j-1)*cellSize-offsetY & S(2,:)<=theNDT.Ymin+j*cellSize-offsetY); + % Compute mean and covariance. + if (size(theNDT.theGrid(j,i).theScanIndexes,2)>2), + thePoints=[S(1,theNDT.theGrid(j,i).theScanIndexes);S(2,theNDT.theGrid(j,i).theScanIndexes)]; + theNDT.theGrid(j,i).theMean=mean(thePoints')'; + theNDT.theGrid(j,i).theCovariance=cov(thePoints')'; + + theMinEigVal=min(eig(theNDT.theGrid(j,i).theCovariance)); + theMaxEigVal=max(eig(theNDT.theGrid(j,i).theCovariance)); + while (theMinEigVal/theMaxEigVal<0.001), + theNDT.theGrid(j,i).theCovariance=theNDT.theGrid(j,i).theCovariance+eye(2)*0.0001; + theMinEigVal=min(eig(theNDT.theGrid(j,i).theCovariance)); + theMaxEigVal=max(eig(theNDT.theGrid(j,i).theCovariance)); + end; + theNDT.theGrid(j,i).theInvertedCovariance=inv(theNDT.theGrid(j,i).theCovariance); + theNDT.theGrid(j,i).exists=1; + else + theNDT.theGrid(j,i).theMean=-1; + theNDT.theGrid(j,i).theCovariance=-1; + theNDT.theGrid(j,i).theInvertedCovariance=-1; + theNDT.theGrid(j,i).exists=0; + end; + end; + end; +return; \ No newline at end of file diff --git a/ScanMatching/2D/NDT/getCoordinatesSubgrid.m b/ScanMatching/2D/NDT/getCoordinatesSubgrid.m new file mode 100644 index 0000000..39aed74 --- /dev/null +++ b/ScanMatching/2D/NDT/getCoordinatesSubgrid.m @@ -0,0 +1,32 @@ +function [ ix iy ] = getCoordinatesSubgrid( x, y, i, PARAM ) +%GETCOORDINATESSUBGRID Returns the correct coordinates according to the +%subgrid I. Every subgrid has an own shift +L = PARAM.L; +xmin = PARAM.xmin; +xmax = PARAM.xmax; +ymin = PARAM.ymin; +ymax = PARAM.ymax; + +xwide = xmax - xmin; +ywide = ymax - ymin; + +NX = floor(xwide / L); +NY = floor(ywide / L); + +%Calculate the cell where the point (x,y) should fall in. +if(i == 1) + ix = min(floor( (-(xmin) + x) / L)+1,NX); + iy = min(floor( (-(ymin) + y) / L)+1,NY); +elseif(i==2) + ix = min(floor( (-(xmin) + x + L/2) / L)+1,NX); + iy = min(floor( (-(ymin) + y) / L)+1,NY); +elseif(i==3) + ix = min(floor( (-(xmin) + x) / L)+1,NX); + iy = min(floor( (-(ymin) + y + L/2) / L)+1,NY); +else + ix = min(floor( (-(xmin) + x + L/2) / L)+1,NX); + iy = min(floor( (-(ymin) + y + L/2) / L)+1,NY); +end + +end + diff --git a/ScanMatching/2D/NDT/makeLocalOccupancyGrid.m~ b/ScanMatching/2D/NDT/makeLocalOccupancyGrid.m~ new file mode 100644 index 0000000..9473bb1 --- /dev/null +++ b/ScanMatching/2D/NDT/makeLocalOccupancyGrid.m~ @@ -0,0 +1,135 @@ +function [ SubGrids ] = makeLocalOccupancyGrid( Scan, PARAM ) +%MAKELOCALOCCUPANCYGRID returns an occupancy grid out of a range finder +%scan. The grid size is dynamic and depends on the cartesian area covered +%by the points in Scan. We suppose the points to be ordered by angle. +%Adjacent points are neighbours. + +L = PARAM.L; +xmin = PARAM.xmin; +xmax = PARAM.xmax; +ymin = PARAM.ymin; +ymax = PARAM.ymax; + +sscan = size(Scan,1); + + +xwide = abs(xmax - xmin); +ywide = abs(ymax - ymin); + +xrwide = xwide - abs(xmax); +yrwide = ywide - abs(ymax); + +NX = floor(xwide / L); +NY = floor(ywide / L); + +% we create the occupancy grid structures, each cell is defined by a mean +% and covariance pdf +ogrid.mean = 0; +ogrid.cov = zeros(2,2); + +ogrid1.grid(1:NX, 1:NY) = ogrid; +ogrid2.grid(1:NX, 1:NY) = ogrid; +ogrid3.grid(1:NX, 1:NY) = ogrid; +ogrid4.grid(1:NX, 1:NY) = ogrid; +ogrid_tot.grid(1:NX, 1:NY) = ogrid; + +%Initialize the cells to group the points togheter +pgrid0.points = []; + +pgrid1.grid(1:NX, 1:NY) = pgrid0; +pgrid2.grid(1:NX, 1:NY) = pgrid0; +pgrid3.grid(1:NX, 1:NY) = pgrid0; +pgrid4.grid(1:NX, 1:NY) = pgrid0; + +%Group scans according to the cell they fall in, Calculates 4 grids. The +%second grid is shifted horizontally of L/2, the third is shifted +%vertically of the same amount, and the last one is shifted both vertically +%and horizontally of L/2 as well. + +for i = 1:sscan + x = Scan(i,1); + y = Scan(i,2); + + + %Calculate the cell where the point (x,y) should fall in. + + ix1 = min(floor( (abs(xmin) + x) / L)+1,NX); + iy1 = min(floor( (abs(ymin) + y) / L)+1,NY); + + ix2 = min(floor( (abs(xmin) + x + L/2) / L)+1,NX); + iy2 = iy1; + + ix3 = ix1; + iy3 = min(floor( (abs(ymin) + y + L/2) / L)+1,NY); + + ix4 = ix2; + iy4 = iy3; + + % and add it to the respective map + rest = pgrid1.grid(ix1,iy1).points; + pgrid1.grid(ix1,iy1).points = [rest; x y]; + + rest = pgrid2.grid(ix2,iy2).points; + pgrid2.grid(ix2,iy2).points = [rest; x y]; + + rest = pgrid3.grid(ix3,iy3).points; + pgrid3.grid(ix3,iy3).points = [rest; x y]; + + rest = pgrid4.grid(ix4,iy4).points; + pgrid4.grid(ix4,iy4).points = [rest; x y]; +end + +grids = [pgrid1 pgrid2 pgrid3 pgrid4]; +ogrids = [ogrid1 ogrid2 ogrid3 ogrid4]; + + +% for each cell, calculate its mean and covariance using its containing +% points. If the number of points is smaller than 3 the cell results +% unoccupied. +hh = 0; +for i = 1:NX + + for j = 1:NY + + %For every cell calculate the four occupancy maps + for kk = 1:size(grids,1) + pgrid = grids(kk).grid; + + + np = size(pgrid(i,j).points,1); + mpoints = 0; + covpoints = zeros(2,2); + + if np >= 2 + hh = hh+1; + %Calculate mean and covariance of all the points in the cell + mpoints = mean(pgrid(i,j).points); + covpoints = zeros(2,2); + for k = 1:np + rest = pgrid(i,j).points(k,:) - mpoints; + rest = rest'*rest; + covpoints = covpoints + rest; + end + covpoints = covpoints/(np-1); + +% mpoints +% covpoints +% pgrid(i,j).points +% j +% i + end + + %update the points in each map... + ogrids(kk).grid(i,j).mean = mpoints; + ogrids(kk).grid(i,j).cov = covpoints; + end + + %and to the total map which is the sum of the previous four +% ogrid_tot.grid(i,j).mean = sum(ogrids(1:4).mean); +% ogrid_tot.grid(i,j).cov = sum(ogrids(1:4).cov); + end +displayOccGrid(ogrids(1)); +SubGrids = [ogrids(1) ogrids(2) ogrids(3) ogrids(4)]; + +end + diff --git a/ScanMatching/2D/NDT/mean_last.mat b/ScanMatching/2D/NDT/mean_last.mat new file mode 100644 index 0000000000000000000000000000000000000000..b1faa33d9ea7164ad95ba9329a534df33f378e86 GIT binary patch literal 438 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i*PhE(NS zC4~cAPmPrlC&YMW1qRlzGBc}($T8LZVP}Y#!756O7F8rIt69ZBS_}x-A_~&-FYx|k z_4_lH$uKY!1amAQ#wtOmRacl9!k4l16QhL@$#o2@ydW)FxLhZ&UuEkGjeSNN=Lnx_ NZ;=1WkS@bo002tueHs7& literal 0 HcmV?d00001 diff --git a/ScanMatching/2D/NDT/std_last.mat b/ScanMatching/2D/NDT/std_last.mat new file mode 100644 index 0000000000000000000000000000000000000000..e5bfd91c442c8a190098dd256e0cd5aa7687d92b GIT binary patch literal 274 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i*PhE(NS8$o zrH&1vr_$Mz(iGBKJ_UDoed>O6+1sM=ji|!jvh82Xs&qHDXC-?)*dS!HHS2esMEt`` zJ?FRY2s`|A;UYesvu)z$e{L~|>)$9{8gwCpIYz$u!kO#qTa3Razi4hc(^KNucHwWq g&reeI3)SPlA3i(#;sKT03X?a 0.01 + estimation = -A\b'; +%else +% estimation = [0 0 0] +%end + +out = 1; diff --git a/ScanMatching/2D/Registration/acc_registration.m b/ScanMatching/2D/Registration/acc_registration.m new file mode 100644 index 0000000..0ddb56c --- /dev/null +++ b/ScanMatching/2D/Registration/acc_registration.m @@ -0,0 +1,21 @@ +%Funcio per recalcular correspondencies, coneixen una estimacio inicial de +%la rotacio + +function [P1,P2,error]=acc_registration(P1i,P2i,R,t) +%R i t matrius de transformacio de P1 a P2. +% P1i i P2i 3xn : punts 3d dels dos nuvols +% P1 i P2 3xn : correspondencies segons ICP (ClosestPoint) +n1=size(P1i,1); +n2=size(P2i,1); +%tri = delaunayn(P2i')'; +for i=1:n1 + P1it(i,:)=P1i(i,:)*R+t; + %[k,d] = dsearchn(P2i',tri,P1it(:,i)'); + [k,d] = dsearchn(P2i,P1it(i,:)); + P1(i,:)=P1i(i,:); + P2(i,:)=P2i(k,:); + error(i)=d; +end + +error = mean(error); + diff --git a/ScanMatching/2D/Registration/pICRegistration.m b/ScanMatching/2D/Registration/pICRegistration.m new file mode 100644 index 0000000..251fc4e --- /dev/null +++ b/ScanMatching/2D/Registration/pICRegistration.m @@ -0,0 +1,54 @@ +function [out,solution,errorK1,nConverged] = pICRegistration(assoc,motion,errorK1,nConverged,tmp) + +errRatio = 0.0001; +error = [0.0001 0.0001 0.0001]; +nIterationSmoothConvergence = 2; +%motion_estimation = qpose3D2epose2D(motion.x); + +assoc.new = assoc.new'; +assoc.ref = assoc.ref'; + +if (size(assoc.new,2) == 0) + error('No associations avaiable.'); +end + + +E = zeros(2*size(assoc.new,2),1); +H = zeros(2*size(assoc.new,2),3); +C = zeros(2*size(assoc.new,2)); + +for i = 1:size(assoc.new,2) + E(2*i-1:2*i) = -(assoc.ref(:,i)-assoc.new(:,i))+assoc.Jq(:,:,i)*motion.estimation'; + H(2*i-1:2*i,:) = assoc.Jq(:,:,i); + C(2*i-1:2*i,2*i-1:2*i) = assoc.Pe(:,:,i); +end + +invC = C\eye(size(C)); +qmin = (H'*invC*H)\H'*invC*E; + +% Compute error +e = Composition(qmin,assoc.oldn); +e = (e - assoc.ref).^2; +err = sum(sum(e,2)); + +% Convergence criteria +errorRatio = err/errorK1; +diff = abs(motion.estimation - qmin'); +if (abs(1-errorRatio) <= errorRatio) || (diff(1) < error(1) && diff(2) < error(2) && diff(3) < error(3)) + nConverged = nConverged+1; +else + nConverged = 0; +end + +% Build solution +errorK1 = err; +solution = qmin; + +% Smooth convergence criterion +if (nConverged > nIterationSmoothConvergence) + out = 1; +else + out = 0; +end + + diff --git a/ScanMatching/2D/Registration/plRegistration.m b/ScanMatching/2D/Registration/plRegistration.m new file mode 100644 index 0000000..7249e82 --- /dev/null +++ b/ScanMatching/2D/Registration/plRegistration.m @@ -0,0 +1,59 @@ +function [R t] = plRegistration(P1,P2) +%PLASSOCIATION Summary of this function goes here +% Detailed explanation goes here + + g = zeros(4,1); + + for k=1:size(P1,1) + M_k = [eye(2) P1(k,1) -P1(k,2); P1(k,2) P1(k,1)]; + n = -(P2(k,1)-P2(k,3) ) / (P2(k,2)-P2(k,4) ); + n = [ cos(n) sin(n) ]; + nn = n*n'; + M = M + M_k'* nn *M_k; + g = g + (- 2 * P2(k,1:2)' * nn * M_k)'; + end + + W = [ zeros(2,2) zeros(2,2); zeros(2,2) eye(2)]; + + %% This is the function that we want to minimize + h = @(l) g' * inv(M+2*l*W) * W * inv(M+2*l*W)' * g - 1; + + %% Partition M in 4 submatrixes: + + M = 2*M; + % M = [A B; C D] + A = M(1:2,1:2); + B = M(1:2,3:4); + D = M(3:4,3:4); + inA = inv(A); + + S = D - B' * inA * B; + Sa = inv(S) / det(S); + + g1 = g(1:2); g2=g(3:4); + + p7 = [( g1'*(inA*B* 4 *B'*inA)*g1 + 2*g1'*(-inA*B* 4 )*g2 + g2'*( 4 )*g2) ... + ( g1'*(inA*B* 4*Sa *B'*inA)*g1 + 2*g1'*(-inA*B* 4*Sa)*g2 + g2'*( 4*Sa)*g2) ... + ( g1'*(inA*B* Sa*Sa *B'*inA)*g1 + 2*g1'*(-inA*B* Sa*Sa)*g2 + g2'*(Sa*Sa)*g2)]; + + p_lambda = [4 (2*S(1,1)+2*S(2,2)) (S(1,1)*S(2,2)-S(2,1)*S(1,2))]; + Ptot = polyadd(p7, -conv(p_lambda,p_lambda)) ; + + % Find largest real root of Ptot + r = roots(Ptot); + lambda = 0; found = 0; + for i=1:4 + if isreal(r(i)) && (not(found) || r(i)>0) + lambda = max(lambda, r(i)); + found = 1; + end + end + + x = -inv(M + 2 * lambda * W) * g; + theta = atan2(x(4),x(3)); + + R = e2q([0 0 theta]); + t = [x(1) x(2) 0]; + + + diff --git a/ScanMatching/2D/Registration/regist_besl.m b/ScanMatching/2D/Registration/regist_besl.m new file mode 100644 index 0000000..cbe21d2 --- /dev/null +++ b/ScanMatching/2D/Registration/regist_besl.m @@ -0,0 +1,47 @@ +function [R,t]=regist_besl(P1,P2) +%Given two sets of points, it computes the least square minimization to +%find the best rotation R and translation t that would match the two +%shapes. + +% The article follow the algorithm described in: +% Besl, P. J. and McKay, N. D.: A method for registration of 3-D shapes, IEEE Transactions on +% Pattern Analysis and Machine Intelligence 14(2) (1992), 239–256. + +n=min(size(P1,2),size(P2,2)); + +% Find the mean of both point set, which will be their center of mass. +mp1(1)=mean(P1(1,:)); +mp1(2)=mean(P1(2,:)); +mp1(3)=mean(P1(3,:)); + +mp2(1)=mean(P2(1,:)); +mp2(2)=mean(P2(2,:)); +mp2(3)=mean(P2(3,:)); + +% Find the cross-covariance matrix of the two set with respect to their +% mean +Cp1p2=zeros(3,3); +for i=1:n + Cp1p2=(P1(1:3,i)'-mp1)'*(P2(1:3,i)-mp2')'+Cp1p2; +end +Cp1p2=Cp1p2./n; + +% Find the anti-symmetric matrix +A=Cp1p2-Cp1p2'; + +% .. and the vector delta +D=[A(2,3);A(3,1);A(1,2)]; + +%.. to generate the matrix Q +Q=[trace(Cp1p2) D';D (Cp1p2+Cp1p2'-trace(Cp1p2)*eye(3))]; + +% we find the maximum eigenvalue which gives us the optimal rotation R +[eigVe,eigVa]=eig(Q); +eig_va=diag(eigVa); +[val,ind]=max(eig_va); +q=eigVe(:,ind); +R=q2R(q); + +% We find the translation t rotating the center of mass of P1 and +% calculating the shift with P2 +t=mp2'-R*mp1'; diff --git a/ScanMatching/2D/Registration/regist_trucco.m b/ScanMatching/2D/Registration/regist_trucco.m new file mode 100644 index 0000000..ba26fed --- /dev/null +++ b/ScanMatching/2D/Registration/regist_trucco.m @@ -0,0 +1,31 @@ +function [R,res]=regist_trucco(P2,P1) +% P1 3xn +% P2 3xn + +n=size(P1,2); +for i=1:n + A(i,:)=[P1(:,i)' 0 0 0 0 0 0]; + A(n+i,:)=[0 0 0 P1(:,i)' 0 0 0]; + A(2*n+i,:)=[0 0 0 0 0 0 P1(:,i)']; + b(i,1)=P2(1,i); + b(n+i,1)=P2(2,i); + b(2*n+i,1)=P2(3,i); +end + +if rcond(A'*A)>0.00001, + r = (inv(A'*A))*A'*b; +else + AA=[A -b]; + [V,D]=eig(AA'*AA); + [nm,im]=min(sum(D)); + r=V(:,im)./V(10,im); + r=r(1:9); +end + + +R=[r(1:3)';r(4:6)';r(7:9)']; +% size(A) +% size(r) +% size(b) +res=A*r-b; + diff --git a/ScanMatching/2D/Registration/registerCensi.m b/ScanMatching/2D/Registration/registerCensi.m new file mode 100644 index 0000000..82ae717 --- /dev/null +++ b/ScanMatching/2D/Registration/registerCensi.m @@ -0,0 +1,81 @@ +function [R t] = registerCensi(A,B) + % Input is: + % C + % A(:,k) + % B(:,k) + + C = [1 0; 0 1]; + + %% First we put the problem in a quadratic+constraint form. + M = zeros(4,4); + g = zeros(4,1); + + for k=1:size(A,2) + M_k = [eye(2) [A(1:2,k) (rot(pi/2)*A(1:2,k))]]; + M = M + M_k'* C *M_k; + g = g + (- 2 * B(1:2,k)' * C * M_k)'; + end + + W = [ zeros(2,2) zeros(2,2); zeros(2,2) eye(2)]; + + %% This is the function that we want to minimize + h = @(l) g' * inv(M+2*l*W) * W * inv(M+2*l*W)' * g - 1; + + %% Partition M in 4 submatrixes: + + M = 2*M; + % M = [A B; C D] + H = M(1:2,1:2); + G = M(1:2,3:4); + D = M(3:4,3:4); + invH = inv(H); + + S = D - G' * invH * G; + Sa = inv(S) * det(S); + + g1 = g(1:2); g2=g(3:4); + + p7 = [( g1'*(invH*G* 4 *G'*invH)*g1 + 2*g1'*(-invH*G* 4 )*g2 + g2'*( 4 )*g2) ... + ( g1'*(invH*G* 4*Sa *G'*invH)*g1 + 2*g1'*(-invH*G* 4*Sa)*g2 + g2'*( 4*Sa)*g2) ... + ( g1'*(invH*G* Sa*Sa *G'*invH)*g1 + 2*g1'*(-invH*G* Sa*Sa)*g2 + g2'*(Sa*Sa)*g2)]; + + p_lambda = [4 (2*S(1,1)+2*S(2,2)) (S(1,1)*S(2,2)-S(2,1)*S(1,2))]; + Ptot = polyadd(p7, -conv(p_lambda,p_lambda)) ; + + % Find largest real root of Ptot + r = roots(Ptot); + lambda = 0; found = 0; + for i=1:4 + if isreal(r(i)) & (not(found) | r(i)>0) + lambda = max(lambda, r(i)); + found = 1; + end + end + + x = -inv(M + 2 * lambda * W) * g; + theta = atan2(x(4),x(3)); + + + t = [x(1); x(2); 0]; + R = e2R([0 0 theta]); + +function R = rot(theta) + R = [cos(theta) -sin(theta); sin(theta) cos(theta)]; + +function[poly]=polyadd(poly1,poly2) + %Copyright 1996 Justin Shriver + %polyadd(poly1,poly2) adds two polynominals possibly of uneven length + if length(poly1)0 + poly=[zeros(1,mz),short]+long; + else + poly=long+short; + end + diff --git a/ScanMatching/2D/Registration/registerMbICP.m b/ScanMatching/2D/Registration/registerMbICP.m new file mode 100644 index 0000000..6d91988 --- /dev/null +++ b/ScanMatching/2D/Registration/registerMbICP.m @@ -0,0 +1,68 @@ +function [out,solution,errorK1,nConverged] = registerMbICP(assoc,motion,errorK1,nConverged) + +PARAM.Bw = 6.23; +PARAM.Br = 2.1; +PARAM.L = 3; +PARAM.step = 1; +PARAM.maxDistanceInterpolation = 0.5; +PARAM.filter = 1; +PARAM.projectionFilter = 0; %!! test with 0 +PARAM.associationError = 0.1; +PARAM.maxIterations = 250; +PARAM.errorRatio = 0.0001; +PARAM.error = [0.0001 0.0001 0.0001]; +PARAM.nIterationSmoothConvergence = 2; + +PARAM.deviation.sensor = [0.0262 0.05]; % std deviation [angular(rad) range(m)] [+/-1.5� +/-5cm] +PARAM.deviation.motion = [0.1 0.1 deg2rad(10)]; % motion std deviation [x(m) y(m) yaw(rad)] + +if (size(assoc,2) == 0) + error('No associations avaiable.'); + out = -1; + return +end + +cpAssoc = assoc; +[out,estimation] = LMSMatrix(cpAssoc); + +if(out == -1) + out = -1; + solution =[]; + return; +end + +% Compute associatons' error +cosy = cos(estimation(3)); siny = sin(estimation(3)); +dtx = estimation(1); dty = estimation(2); + +ref = [assoc.ref']; +new = [assoc.new']; + +errorTmp = 0; +for i = 1:size(ref,2) + tmp1 = new(:,i)-ref(:,i); %*cosy-new(2,i)*siny+dtx-ref(1,i); + %tmp2 = new(1,i)*siny-new(2,i)*cosy+dty-ref(2,i); + errorTmp = errorTmp+sqrt(tmp1(1)^2 + tmp1(2)^2); +end + +% Build solution +errorK1 = errorTmp; + +% Motion referenced to computed estimation +%eTm = [cos(estimation(3)) -sin(estimation(3)) estimation(1); sin(estimation(3)) cos(estimation(3)) estimation(2); 0 0 1]; + +eTm = [cos(estimation(3)) -sin(estimation(3)) 0; sin(estimation(3)) cos(estimation(3)) 0; 0 0 1]; +solution = eTm*[estimation(1:2,1); 1]; +%solution(3,1) = estimation(3)+motion(3); +%solution = solution + estimation; +solution(3,1) = Normalize(estimation(3)); + + +%solution = motion + estimation; +solution=solution'; +% Smooth convergence criterion +if (nConverged > PARAM.nIterationSmoothConvergence) + out = 1; +else + out = 0; +end diff --git a/ScanMatching/2D/Registration/registerSVD.m b/ScanMatching/2D/Registration/registerSVD.m new file mode 100644 index 0000000..19089e7 --- /dev/null +++ b/ScanMatching/2D/Registration/registerSVD.m @@ -0,0 +1,34 @@ +function [R,t]=registerSVD(P2,P1) +%Given two sets of points, it computes the least square minimization to +%find the best rotation R and translation t that would match the two +%shapes. It uses a singular value decomposition based method + +% The article follow the algorithm described in: +% Estimating 3-D rigid body transformations: a comparison +% of four major algorithms +% D.W. Eggert1 , A. Lorusso2 , R.B. Fisher3 + +% Find the mean of both point set, which will be their center of mass. + +sizeN = size(P1,2); + +mp1(1)=mean(P1(1,:)); +mp1(2)=mean(P1(2,:)); +mp1(3)=mean(P1(3,:)); + +mp2(1)=mean(P2(1,:)); +mp2(2)=mean(P2(2,:)); +mp2(3)=mean(P2(3,:)); + +cmp1 = P1-repmat(mp1',1,sizeN); +cmp2 = P2-repmat(mp2',1,sizeN); +H = zeros(3,3); +for i = 1:size(cmp1,2) + H = H + cmp2(:,i)*cmp1(:,i)'; +end + +[U,S,V] = svd(H); + +R = V*U'; + +t = mp1' - R*mp2'; \ No newline at end of file diff --git a/ScanMatching/2D/Registration/register_martinez.m b/ScanMatching/2D/Registration/register_martinez.m new file mode 100644 index 0000000..5e5ba7e --- /dev/null +++ b/ScanMatching/2D/Registration/register_martinez.m @@ -0,0 +1,39 @@ +function [ R, t ] = register_martinez( A, B ) +% REGISTER_MARTINEZ Closed form registration to solve the least square +% minimization over two set of 2D associated points. Closed form inspired +% by: + +% Mobile robot motion estimation by 2D scan matching with genetic and +% iterative closest point algorithms +% Martinez, J. L. Gonzalez, J. Morales, J. Mandow, A. Garcia-Cerezo, A. J. +% JOURNAL OF FIELD ROBOTICS +% 2006, VOL 23; NUMBER 1, pages 21-34 +A = A'; +B = B'; + +sizen = size(A,1); + +sumb = sum(B); +sumbx = sumb(:,1); +sumby = sumb(:,2); + +suma = sum(A); +sumax = suma(:,1); +sumay = suma(:,2); + +sumabx = sum(A(:,1).*B(:,1)); +sumaby = sum(A(:,2).*B(:,2)); + +sumabyx = sum(A(:,2).*B(:,1)); +sumabxy = sum(A(:,1).*B(:,2)); + +yaw = atan( (sumay*sumbx + sizen*(sumabxy - sumabyx) - sumby*sumax ) / ... + ( sizen*(sumabx + sumaby) - sumax*sumbx - sumby*sumay) ); + +x = (sumbx - cos(yaw)*sumax + sin(yaw)*sumay)/sizen; +y = (sumby - sin(yaw)*sumax - cos(yaw)*sumay)/sizen; + +t = [x y 0]; +R = e2R([0 0 yaw]); +end + diff --git a/ScanMatching/2D/Registration/register_matlab.m b/ScanMatching/2D/Registration/register_matlab.m new file mode 100644 index 0000000..34a96ef --- /dev/null +++ b/ScanMatching/2D/Registration/register_matlab.m @@ -0,0 +1,107 @@ +function [R t] = register_matlab(Ai, Bi, motion, Opt) +% REGISTER_MATLAB Uses Matlab Curve Fitting Toolbox to solve the +% minimization. It requires to have already an association set. +% P1(n) is associated with P2(n). +% Ai being the reference scan and Bi bein the new scan + +%Select which points to use, in this case local cartesian point in +Al.cart = Ai.localCart; +Al.polar = Ai.localPolar; + +Bl.cart = Bi.localCart; +Bl.polar = Bi.localPolar; + +R = []; %Final rotation correction +P = Bl.cart; %Initial points to be aligned +itMax = Opt.scanmatcher.iterations; + +Br = Opt.scanmatcher.Br; % Angular tolerance +Br0 = Br; +alfaBr = 1/(2.5*itMax); +Br = Br0*exp(1)^alfaBr; +Br = 0; +it=1; %iteration control + +sizeB = size(Bl.cart,1); + +%Matrix initialization +B.cart = zeros(sizeB, 2); +B.polar = zeros(sizeB, 2); +C = zeros(sizeB, 3); +err = zeros(sizeB, 1); + + + +%Motion extraction from robot + +u = motion.con.u; +pose = motion.frame.t; +lrangle = u(2); +x0 = cos(lrangle)*u(1); +y0 = sin(lrangle)*u(1); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; + +lasterror = []; +B.cart(:,1:2) = Bl.cart(:,1:2);B.polar(:,1:2) = Bl.polar(:,1:2); +xtot = 0; +ytot = 0; +rtot = 0; +% rTn = [cos(yaw) -sin(yaw) x; sin(yaw) cos(yaw) y; 0 0 1]; + + +% B referenced to A coordinate system +% for i = 1:sizeB +% D.cart(i,1:3) = (rTn*[B.cart(i,1:2) 1]')' ; +% th = normAngle(B.polar(i,1) + yaw); +% ro = B.polar(i,2); +% rc = cos(th)*ro + x; +% rs = sin(th)*ro + y; +% +% D.polar(i,2) = sqrt(rc^2 + rs^2); +% D.polar(i,1) = atan2(rs,rc); +% end +% +% % Compute the association set +% [merr assp] = cpAssociation(D,Al, Br); +% assp(:,3) = pose(3); + + + + function [v] = fmin(X) + rTn = [cos(X(3)) -sin(X(3)) X(1); sin(X(3)) cos(X(3)) X(2); 0 0 1]; + + % B referenced to A coordinate system + for i = 1:sizeB + D.cart(i,1:3) = (rTn*[B.cart(i,1:2) 1]')' ; + th = normAngle(B.polar(i,1) + yaw); + ro = B.polar(i,2); + rc = cos(th)*ro + x; + rs = sin(th)*ro + y; + + D.polar(i,2) = sqrt(rc^2 + rs^2); + D.polar(i,1) = atan2(rs,rc); + end + + % Compute the association set + [merr assp] = cpAssociation(D,Al, Br); + assp(:,3) = pose(3); + + v=[]; + % B referenced to A coordinate system + for k = 1:size(assp,1) +% F(k,1:3) = (rTn*[B.cart(k,1:2) 1]')' ; + v = [v; assp(k,1:2)-D.cart(k,1:2) ]; + end + + end + + +[T res] = lsqnonlin(@fmin, [x0 y0 yaw0]); +T = T - [x0 y0 yaw0]; +R = e2q([0 0 T(3)]); +t = [T(1:2) 0]; +end \ No newline at end of file diff --git a/ScanMatching/2D/Rejection/x84.m b/ScanMatching/2D/Rejection/x84.m new file mode 100644 index 0000000..9464a64 --- /dev/null +++ b/ScanMatching/2D/Rejection/x84.m @@ -0,0 +1,25 @@ +function [ assp ixs ] = x84( assp ) +%X84 This function post process a struct ASSP rejecting all the +%associations that do not match the Median Absolute Deviation rule over the +%residuals. + +if size(assp.ref,1) < 5 + ixs=[]; + return +end + +residuals = (assp.new - assp.ref).^2; +residuals = sqrt(residuals(:,1) + residuals(:,2) ); + + +madass = mad(residuals); +K = 4; +rej_val = (K)*madass; + +ixs = find(residuals > rej_val); + +assp.ref(ixs,:) = []; +assp.new(ixs,:) = []; + +end + diff --git a/ScanMatching/2D/Rejection/x84.m~ b/ScanMatching/2D/Rejection/x84.m~ new file mode 100644 index 0000000..3abceb4 --- /dev/null +++ b/ScanMatching/2D/Rejection/x84.m~ @@ -0,0 +1,12 @@ +function [ assp ] = x84( assp ) +%X84 This function post process a struct ASSP rejecting all the +%associations that do not match the Median Absolute Deviation rule over the +%residuals. + +residuals = assp.new - assp.ref; +madass = mad(residuals); + +rej_val = K* + +end + diff --git a/ScanMatching/2D/icpbSM.m b/ScanMatching/2D/icpbSM.m new file mode 100644 index 0000000..ffb6c92 --- /dev/null +++ b/ScanMatching/2D/icpbSM.m @@ -0,0 +1,226 @@ +function [R t NI] = icpbSM(Ai, Bi, motion) +% ICMBSM: ICP based Scan Matcher. This function lets you perform a Scan +% Matching between two clouds of points defined inside Ai and Bi using an +% initial estimation MOTION. Opt stores the scan matcher parameters and the +% stage functions to be used. ICP is a 2 stage algorithm defined by an +% association step and a registration step. + +% In: +% Ai: localCart[2xN], points cartesian +% Bi: localCart[2xN], points cartesian +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations +global Opt DEBUG +opts = Opt; + +Al.cart = Ai.localCart; +Al.polar = Ai.localPolar; + +Bl.cart = Bi.localCart; +Bl.polar = Bi.localPolar; + +Al.covm = Ai.covm; +Bl.covm = Bi.covm; + +itMax = Opt.scanmatcher.iterations; + +it=1; %iteration control + +sizeB = size(Bl.cart,1); + +%Matrix initialization +B.cart = zeros(sizeB, 2); +B.polar = zeros(sizeB, 2); + +err = zeros(sizeB, 1); +Br = opts.scanmatcher.Br; % thresholds + +if DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + + +%Motion estimation q0 + +u = motion.con.u; + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +x=x0; +y=y0; +yaw=yaw0; + +dirPoints = 1; + +originalodo = [cos(u(2))*u(1) sin(u(2))*u(1) u(3)]; +motion2D.estimation = originalodo; +motion2D.P = motion.state.P(1:3,1:3); + +par1=[]; +par2=[]; + +% depending on the algorithm we have the scan ordered differently + +if strcmp(func2str(Opt.scanmatcher.associate),'mahaAssociation') + par1 = motion2D; + par1.estimation = [0 0 0]; + + Bl.cart = Bl.cart'; + Al.cart = Al.cart(:,1:2)'; + Bl.polar = Bl.polar'; + Al.polar = Al.polar'; + dirPoints=2; + [B Bf Af aixs bixs] = transPolarCartScan(Bl, Al, yaw, [x y], dirPoints,Opt.scanmatcher.Br(2)); + %B = Bl; + B.cart(3,:) = []; + Bf.cart(3,:) = []; + + B.P = Bi.covm; + Al.P = Ai.covm; + + + Bf.P = Bi.covm; + Bf.P(:,:,bixs) = []; + Af.P = Ai.covm; + Af.P(:,:,aixs) = []; + +elseif strcmp(func2str(Opt.scanmatcher.associate),'mbAssociation') + + Bl.cart = Bl.cart'; + Al.cart = Al.cart(:,1:2)'; + Bl.polar = Bl.polar'; + Al.polar = Al.polar'; + + + + par1 = motion2D; + par2 = Precompute(Al); + + dirPoints=2; + + [B Bf Af] = transPolarCartScan(Bl, Al, yaw, [x y], dirPoints,opts.scanmatcher.Br(2)); + B.cart(3,:) = []; + Bf.cart(3,:) = []; + +else + + [B Bf Af] = transPolarCartScan(Bl, Al, yaw, [x y], dirPoints,opts.scanmatcher.Br(2)); +end + + +Cnf = B; +Cf = Bf; + + +lasterror = []; +corr=[]; + +while it < itMax + + %% Apply q + % reeturn if convergence is achieved + if checkConv(lasterror,corr) + break; + end + + + %% Association + + if isempty(Opt.scanmatcher.associate) + err('No association function defined') + end + + if Opt.scanmatcher.projfilter + CA = Cf; + AlA = Af; + else + CA = Cnf; + AlA = Al; + end + + + if ~isempty(par1) && ~isempty(par2) + [merr assp i1] = Opt.scanmatcher.associate(CA,AlA,Opt,par1,par2); + elseif ~isempty(par1) + [merr assp i1] = Opt.scanmatcher.associate(CA,AlA,Opt,par1); + else + [merr assp i1] = Opt.scanmatcher.associate(CA,AlA,Opt); + end + + + opts.scanmatcher.Br(1) = max(Br(1)*exp(-alpha*it),0.1); + + if isempty(Opt.scanmatcher.register) + err('No registration function defined') + end + + + %Use rejection rule? + if ~isempty(Opt.scanmatcher.rejection_rule) + assp = Opt.scanmatcher.rejection_rule(assp); + end + + + + %% Registration + if size(assp.ref,1) < 5 + break; + end + + if size(assp.new,2) == 2 + assp.new(:,3) = Ai.localCart(1,3); + assp.ref(:,3) = Ai.localCart(1,3); + end + + [R t] = Opt.scanmatcher.register(assp.new', assp.ref'); + + % update q + yawR = R2e(R); + + yaw = normAngle(yaw + yawR(3)); + x = x + t(1); + y = y + t(2); + + % apply the motion to S New + [Cnf Cf Af] = transPolarCartScan(Bl, Al, yaw, [x y], dirPoints,Opt.scanmatcher.Br(2)); + + if strcmp(func2str(Opt.scanmatcher.associate),'mahaAssociation') + Cnf.P = Bi.covm; + Cnf.cart(3,:) = []; + Cf.P = Bi.covm; + Cf.cart(3,:) = []; + end + + it=it+1; + + % error estimation update + lastyaw = yawR(3); + lasterror = [lasterror 1/merr] + corr = [corr; t(1) t(2) lastyaw] + + if size(lasterror,2) > Opt.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + +end +%% Result +% Returns the minimizing Q = [t R] +R = normAngle(yaw-yaw0); +t = [x y] - [x0 y0]; + + +NI = it; + diff --git a/ScanMatching/2D/pIC/Composition.m b/ScanMatching/2D/pIC/Composition.m new file mode 100644 index 0000000..1f200ee --- /dev/null +++ b/ScanMatching/2D/pIC/Composition.m @@ -0,0 +1,19 @@ +function [r] = Composition(pa,pb) + +yaw = pa(3); +cosy = cos(yaw); siny = sin(yaw); + +if size(pb,1) == 2 %x,y + r = []; + for i=1:size(pb,2) + np = [ pb(1,i)*cosy-pb(2,i)*siny+pa(1); + pb(1,i)*siny+pb(2,i)*cosy+pa(2)]; + r = [r np]; + end +elseif size(pb,1) == 3 %x,y,theta + r = [ pb(1)*cosy-pb(2)*siny+pa(1); + pb(1)*siny+pb(2)*cosy+pa(2); + pb(3)+pa(3)]; +end + + diff --git a/ScanMatching/2D/pIC/MStep.asv b/ScanMatching/2D/pIC/MStep.asv new file mode 100644 index 0000000..e7ac8d8 --- /dev/null +++ b/ScanMatching/2D/pIC/MStep.asv @@ -0,0 +1,50 @@ +unction [out,solution,errorK1,nConverged] = pICRegistration(assoc,motion,errorK1,nConverged,tmp) +global PARAM + +if (size(assoc,2) == 0) + error('No associations avaiable.'); + out = -1; + return +end + +E = []; H = []; +C = []; +for i = 1:size(assoc,2) + e = assoc(i).a-assoc(i).refNewPoint; + E = [E; (-e+assoc(i).Jq*motion.estimation')]; + H = [H; assoc(i).Jq]; + C = blkdiag(C,assoc(i).Pa+assoc(i).refNewP); +end + +invC = inv(C); +qmin = inv(H'*invC*H)*H'*invC*E; + +tx = qmin(1,1); ty = qmin(2,1); yaw = qmin(3,1); +cosy = cos(yaw); siny = sin(yaw); + +err = 0; +for i = 1:size(assoc,2) + errorX = assoc(i).newPoint(1,1)*cosy-assoc(i).newPoint(2,1)*siny+tx-assoc(i).a(1,1); + errorY = assoc(i).newPoint(1,1)*siny-assoc(i).newPoint(2,1)*cosy+ty-assoc(i).a(2,1); + err = err+errorX^2+errorY^2; +end + +errorRatio = err/errorK1; +dist = motion.estimation - qmin' +ERR = 1-errorRatio +if (abs(1-errorRatio) <= PARAM.errorRatio) || abs(dist(1)) < PARAM.error(1) && abs(dist(2)) < PARAM.error(2) && abs(dist(3) < PARAM.error(3)) + nConverged = nConverged+1; +else + nConverged = 0; +end + +% Build solution +errorK1 = err; +solution = qmin; + +% Smooth convergence criterion +if (nConverged > PARAM.nIterationSmoothConvergence) + out = 1; +else + out = 0; +end diff --git a/ScanMatching/2D/pIC/mahaAssociation.asv b/ScanMatching/2D/pIC/mahaAssociation.asv new file mode 100644 index 0000000..db7237b --- /dev/null +++ b/ScanMatching/2D/pIC/mahaAssociation.asv @@ -0,0 +1,113 @@ +function [out, A] = mahaAssociation(refPoints,newPoints,motion) +% Function mahaAssociation +% Computes correspondences between two scans +% In: +% refPoints +% newPoint +% motion: [x y yaw] +% prec: refPoint's precomputed stuff +% Out: +% out: +% A: associations + + +confidence = 0.95; +% Mahalanobis distance^2 +x = motion.estimation(1); +y = motion.estimation(2); +yaw = motion.estimation(3); +siny = sin(yaw); +cosy = cos(yaw); + +% newPoints referenced to reference coordinate system +rTn = [cosy -siny x; siny cosy y; 0 0 1]; + +% Look for points in Sref statistically compatible with pi +chi2value = chi2inv(confidence,2); + +A = []; +Jp = [-cosy siny; -siny -cosy]; +% hold on +% axis equal +% displayPoints(newPoints.cart','b'); +% B = multRow( [newPoints.cart' repmat(1, size(newPoints.cart,2) , 1) ],rTn); +% displayPoints(refPoints.cart','g'); +% displayPoints(B,'r'); + +for j = 1:size(newPoints.cart,2) + refp = rTn*[newPoints.cart(:,j);1]; % New point to Sref + Jq = [-1 0 newPoints.cart(1,j)*siny+newPoints.cart(2,j)*cosy; 0 -1 -newPoints.cart(1,j)*cosy+newPoints.cart(2,j)*siny]; + Pcompos = Jq*motion.P*Jq'+ Jp*newPoints.P(:,2*j-1:2*j)*Jp'; + refNewP = Pcompos; % Composition q-p covariance + + Eassoc = []; + Cov =[]; + for i = 1:size(refPoints.cart,2) + d = refPoints.cart(:,i)-refp(1:2,1); + C = Pcompos+refPoints.P(:,2*i-1:2*i); + dist = d'*inv(C)*d + if (dist < chi2value) + Eassoc = [Eassoc refPoints.cart(:,i)]; % Sref points statically compatible + Cov = [Cov C]; + %Passoc = [Passoc refPoints.P(:,2*i-1:2*i)]; + end + end + + Pr_temp = zeros(2,2,size(Eassoc,2)); + for i = 1:size(Eassoc,2) + Pr_temp(:,:,i) = refPoints.P(:,2*i-1:2*i); + end + + % Build association struct + if (size(Eassoc,2) > 0) + association.newPoint = newPoints.cart(:,j); + association.newP = newPoints.P(:,2*j-1:2*j); + association.refNewPoint = refp(1:2,1); + association.Cov = Cov; + association.refNewP = refNewP; + association.Jq = Jq; + %association.refPoint = Eassoc; + %association.refP = Passoc; + + prob_a =[]; + for i = 1:size(Eassoc,2) + %prob_a = [prob_a mvnpdf(Eassoc(:,i),association.newPoint,association.Cov(:,2*i-1:2*i))]; + prob_a = [prob_a mvnpdf(Eassoc(:,i)',association.refNewPoint',association.Cov(:,2*i-1:2*i))]; + end + eta = 1/sum(prob_a); + prob_a = eta*prob_a; + + association.a = [0;0]; + for i = 1:size(Eassoc,2) + association.a = association.a+Eassoc(:,i)*prob_a(i); + end + + association.Pa = zeros(2); + for i = 1:size(Eassoc,2) + diff_a = Eassoc(:,i)-association.a; + association.Pa = association.Pa+diff_a*diff_a'*prob_a(i); + end + +% % Display correspendences +% % figure; +% % hold on; +% % plot(refp(1,1),refp(2,1),'r.',Eassoc(1,:),Eassoc(2,:),'b.'); +% % draw_ellipse(refp(1:2,1)',refNewP,'r'); +% % draw_ellipse(Eassoc',Pr_temp,'c'); +% % plot(association.a(1,1),association.a(2,1),'g.'); +% % draw_ellipse(association.a',association.Pa,'g'); +% % hold off; +% % pause; +% figure; +% hold on; +% plot(refp(1,1),refp(2,1),'r.',Eassoc(1,:),Eassoc(2,:),'b.'); +% plot(association.a(1,1),association.a(2,1),'g.'); +% hold off; +% pause; + + A = [A association]; + end +end + +out = 0; + diff --git a/ScanMatching/2D/pIC/mahaAssociation.m~ b/ScanMatching/2D/pIC/mahaAssociation.m~ new file mode 100644 index 0000000..a94772d --- /dev/null +++ b/ScanMatching/2D/pIC/mahaAssociation.m~ @@ -0,0 +1,158 @@ +function [assoc,lines,P_r_index,P_n_index] = mahaAssociation(r,n,motion) +% Function mahaAssociation +% Computes correspondences between two scans +% In: +% refPoints +% newPoint +% motion: [x y yaw] +% prec: refPoint's precomputed stuff +% Out: +% out: +% A: associations + + +confidence = 0.95; +% Mahalanobis distance^2 +x = motion.estimation(1); +y = motion.estimation(2); +yaw = motion.estimation(3); +siny = sin(yaw); +cosy = cos(yaw); + + +% Look for points in Sref statistically compatible with pi +chi2value = chi2inv(confidence,2); + +A = []; +%Jp = [-cosy siny; -siny -cosy]; +Jp = [cosy -siny; siny cosy]; % MODIFICATO + + +% c_j n point to r +c = Composition(motion.estimation, n.cart); + + +figure +hold on +axis equal +displayPoints(n.cart','b'); +displayPoints(r.cart','g'); +displayPoints(c','r'); + +% Pq +j = 1:size(n.cart,2); +col3_tmp = [n.cart(1,j)*siny+n.cart(2,j)*cosy; -n.cart(1,j)*cosy+n.cart(2,j)*siny]; +col3 = reshape(col3_tmp,2,1,size(n.cart,2)); +diagonal = repmat(diag(-ones(1,2)),[1,1,size(n.cart,2)]); +Jq = [diagonal col3]; + +% Buffers initialization +a = zeros(2,size(n.cart,2)); % a buffer +Pe = zeros(2,2,size(n.cart,2)); % Pe buffer +indexBuffer = zeros(1,size(n.cart,2)); +Pc_j = zeros(2,2,size(n.cart,2)); %Pc buffer + +for j = 1:size(n.cart,2) + Pc_j(:,:,j) = Jq(:,:,j)*motion.P*Jq(:,:,j)'+ Jp*n.P(:,:,j)*Jp'; + + i = 1:size(r.cart,2); + d = [r.cart(1,i)-c(1,j); r.cart(2,i)-c(2,j)]; + + % Covariances of r and q-p composition + Pe_ij = [r.P(1,1,i)+Pc_j(1,1,j) r.P(1,2,i)+Pc_j(1,2,j); r.P(2,1,i)+Pc_j(2,1,j) r.P(2,2,i)+Pc_j(2,2,j)]; + + % inv(Pe_ij) for a 2x2xn matrix. Each matrix must be [a b;b d] form + det_Pe_ij = (Pe_ij(1,1,i).*Pe_ij(2,2,i)-Pe_ij(1,2,i).^2); + inv_Pe_ij = [Pe_ij(2,2,i)./det_Pe_ij(1,1,i) -Pe_ij(1,2,i)./det_Pe_ij(1,1,i);-Pe_ij(1,2,i)./det_Pe_ij(1,1,i) Pe_ij(1,1,i)./det_Pe_ij(1,1,i)]; + + inv_11 = squeeze(inv_Pe_ij(1,1,i))'; + inv_12 = squeeze(inv_Pe_ij(1,2,i))'; + inv_22 = squeeze(inv_Pe_ij(2,2,i))'; + + % Mahalanobis distance + dist = (d(1,i).^2).*inv_11+2.*d(1,i).*d(2,i).*inv_12 + (d(2,i).^2).*inv_22; + + if PARAM.sm.estep_method == 0 % ICNN + [d_value,d_index] = min(dist); + if(d_value <= chi2value) + a_j = r.cart(:,d_index); + Pa_j = Pe_ij(:,:,d_index); + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + % save a_j & Pa_j + a(:,j) = a_j; + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + indexBuffer(1,j) = 1; + end + elseif PARAM.sm.estep_method == 1 % Virtual point + d_index = dist<=chi2value; + A = r.cart(:,d_index); + + Pe_ij = Pe_ij(:,:,d_index); + + % Build association struct + if (size(A,2) > 0) + i = 1:size(A,2); + + % prob_a using multivariate normal pdf + prob_a = mvnpdf(A',c(1:2,j)',Pe_ij); + prob_a = prob_a'; + + eta = 1/sum(prob_a,2); + prob_a = eta*prob_a; + + % a_j + a_j = sum(A.*[prob_a;prob_a],2); + + % Pa_j + err = [A(1,i)-a_j(1); A(2,i)-a_j(2)]; + err = reshape(err,2,1,size(A,2)); + errQuad = [err(1,1,i).^2 err(1,1,i).*err(2,1,i); err(2,1,i).*err(1,1,i) err(2,1,i).^2]; % error in 2x2 matrix + + prob_a = reshape(prob_a,[1,1,size(A,2)]); + errQuad(:,:,i) = errQuad(:,:,i).*[prob_a(:,:,i) prob_a(:,:,i); prob_a(:,:,i) prob_a(:,:,i)]; + + Pa_j = sum(errQuad(:,:,i),3); + + % save a_j & Pa_j + a(:,j) = a_j; + Pe(:,:,j) = Pa_j + Pc_j(:,:,j); + + indexBuffer(1,j) = 1; + + % Debug mode 3 + if PARAM.debug >= 3 + if assoc_index == PARAM.assoc_plot_interval + lines_assoc_scanK = repmat([NaN;NaN],1,size(A,2)*3); + a_rep = repmat(a(:,j),1,size(A,2)); + k = 1:3:size(A,2)*3; + m = 1:size(A,2); + %pause; + lines_assoc_scanK(:,k) = a_rep(:,m); + lines_assoc_scanK(:,k+1) = A(:,m); + lines = [lines lines_assoc_scanK]; + %pause; + %set(handle_assoc_scanK,'XData',lines_assoc_scanK(1,:),'YData',lines_assoc_scanK(2,:)); + + P_r_index = P_r_index + d_index; + P_n_index = [P_n_index j]; + + assoc_index = 1; + else + assoc_index = assoc_index + 1; + end + end + end + end +end + +index = find(indexBuffer == 1); + +% Keep only the associaton necessary values +assoc.n = n.cart(:,index); +assoc.c = c(:,index); +assoc.a = a(:,index); +assoc.Pe = Pe(:,:,index); +assoc.Jq = Jq(:,:,index); + diff --git a/ScanMatching/2D/pIC/pIC.asv b/ScanMatching/2D/pIC/pIC.asv new file mode 100644 index 0000000..a0973fe --- /dev/null +++ b/ScanMatching/2D/pIC/pIC.asv @@ -0,0 +1,87 @@ +%Given two scan, performs an IDC scanmatching. It returns the displacement +%between adiacent points. Being A the old scan and B the new one. The +%point set size does not match. The idea is to find the correnspondence +%between all points of B with a point of A. + +%L. Montesano, J. Minguez, and L. Montano, “Probabilistic scan +%matching for motion estimation in unstructured environments,” in +%Proceedings of the IEEE/RSJ International conference on Intelligent +%Robots and Systems (IROS), (Edmonton, Canada), 2005. + + +function [R, t, S] = pIC(A, B, motion) + +nnb = 5; +threshold = 0.5; +merr = inf; +R = []; +t = []; +S = []; +P = B; +Br = 0.5; +alfa = 1; +it=0; + +% Fixed polar cov. matrix +stdsen = [0.01 0.01]; % std deviation [angular(rad) range(m)] [+/-1.5º +/-5cm] +Ppolar = [stdsen(1)^2 0; 0 stdsen(2)^2]; + +PA = []; +PB = []; + +%Preprocess +ScA.cart = A.data.localpoint(:,1:2)'; +ScB.cart = B.data.localpoint(:,1:2)'; + +for i = 1:size(ScA.cart,2); + % Create P (cov. matrix) for both scans + J = [cos(ScA.cart(1,i)) -ScA.cart(2,i)*sin(ScA.cart(1,i)); sin(ScA.cart(1,i)) ScA.cart(2,i)*cos(ScA.cart(1,i))]; + PA = [PA [J*Ppolar*J']]; + +end + +for i = 1:size(ScB.cart,2); + % Create P (cov. matrix) for both scans + J = [cos(ScB.cart(1,i)) -ScB.cart(2,i)*sin(ScB.cart(1,i)); sin(ScB.cart(1,i)) ScB.cart(2,i)*cos(ScB.cart(1,i))]; + PB = [PB [J*Ppolar*J']]; +end + + +ScA.P = PA; %A.data.covm'; +ScB.P = PB; %A.data.covm'; +u = motion.u; +%motion2D.estimation = [cos(u(2))*u(1) sin(u(2))*u(1) u(2)]; +motion2D.estimation = [motion.x(1) motion.x(2) motion.x]; +motion2D.P = motion.P2(1:3,1:3); + +while it < 10 + + %Compute the association set + + [resEStep,assoc] = mahaAssociation(ScA, ScB, motion2D); + + [resMStep,solution,errorK1,nConverged] = pICRegistration(assoc,motion2D,10000,0); + + motion2D.estimation = solution' + + merr = errorK1; + it=it+1; +end +t = [motion2D.estimation(1) motion2D.estimation(2)]; +R = r2R(motion2D.estimation(3)); + for i = 1:size(ScB.cart,2) + S(i,1:2) = (ScB.cart(:,i)') + t ; + S(i,3) = B.data.point(i,3); +% %err(i)=norm(B(i,:)-C(i,:)); + end + + + % %Calculate the fitline +% int = i-nnb:1:i+nnb; +% if (i<=nnb) +% int = int + (nnb - i + 1) ; +% end +% +% polyfit(B(int,1),B(int,2),1); +% +% C = newref(B,motion); \ No newline at end of file diff --git a/ScanMatching/2D/pIC/pIC.m b/ScanMatching/2D/pIC/pIC.m new file mode 100644 index 0000000..d4207da --- /dev/null +++ b/ScanMatching/2D/pIC/pIC.m @@ -0,0 +1,144 @@ +% Uses a probabilistic framework to keep +% into consideration the motion and sensor noise. Asso- +% ciation uses de Closest Point Rule but defined over +% the Mahalanobis distance of the points instead of the +% Euclidean counterpart. The original pIC algorithm uses +% a virtual point as the correspondence one. For a given +% reference point ri , its associated virtual point ai is placed +% in the probabilistic weight average of the statistically +% compatible points of the new scan (those which satisfy +% the Individual Compatibility test). pIC registration is +% done as in the M bICP but using the + +% L. Montesano, J. Minguez, and L. Montano, “Probabilistic scan match- +% ing for motion estimation in unstructured environments,†in Intelligent +% Robots and Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International +% Conference on. IEEE, 2005, pp. 3499–3504. + + +function [R, t, NI] = pIC(A, B, motion) + +% In: +% Ai: localCart[2xN], points cartesian, covm[2x2xN] points +% uncertainty +% Bi: localCart[2xN], points cartesian, covm[2x2xN] points +% uncertainty +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% STATE.P[3x3] scan uncertainty +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + +global Opt DEBUG +opts = Opt; + +t = []; +it=0; + +% Format the two scans we are going to use +ScA.cart = A.localCart(:,1:2)'; +ScB.cart = B.localCart(:,1:2)'; + +ScA.polar = A.localPolar(:,1:2)'; +ScB.polar = B.localPolar(:,1:2)'; + +itMax = opts.scanmatcher.iterations; + +ScA.P = A.covm; +ScB.P = B.covm; + +%Motion estimation q0 +u = motion.con.u; + +x0 = u(1); +y0 = u(2); +yaw0 = u(3); + +originalodo = [x0 y0 yaw0]; + +motion2D.estimation = originalodo; +solution = originalodo'; +motion2D.P = motion.state.P([1 2 4],[1 2 4]); %eye(3)*0.005; + +% error convergence init +lasterror = []; +corr = []; + + +if DEBUG.mahaAssociation || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); + opts.plot_a = plot(NaN,NaN,'-m','LineWidth',1); +end + +while it < itMax + + % Check convergence on previous error + if checkConv(lasterror, corr) + break + end + + %% Association + + %Compute the associated point with the Mahalanobis Distance + [merr assoc] = mahaAssociation(ScB, ScA, opts, motion2D); + + if isempty(assoc) + break; + end + + %Use rejection rule? + if ~isempty(opts.scanmatcher.rejection_rule) + [assoc ixs] = opts.scanmatcher.rejection_rule(assoc); + assoc.oldn(:,ixs) = []; + assoc.Pe(:,:,ixs) = []; + assoc.Jq(:,:,ixs) = []; + end + + if size(assoc.ref,1) < 5 + break; + end + + if DEBUG.all + displayAssociations(assoc,opts.plot_a); + + end + %% Registration + %using Least Square + [~, solution] = pICRegistration(assoc,motion2D,10000,0); + + motion2D.estimation = solution'; + + + %% Error check + lasterror = [lasterror 1/merr]; + corr = [corr; solution']; + + if (sum(diag(motion2D.P)) > 0.05) + + motion2D.P = motion2D.P*0.8; + + end + + if size(lasterror,2) > opts.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + it=it+1; +end + + +%% Result +% Returns the minimizing Q = [t R] +solution = solution' - originalodo; +t(1:2) = [solution(1) solution(2) ]; +R = solution(3); +NI = it; \ No newline at end of file diff --git a/ScanMatching/2D/pIC/pIC.m~ b/ScanMatching/2D/pIC/pIC.m~ new file mode 100644 index 0000000..c7bc36d --- /dev/null +++ b/ScanMatching/2D/pIC/pIC.m~ @@ -0,0 +1,118 @@ +% Uses a probabilistic framework to keep +% into consideration the motion and sensor noise. Asso- +% ciation uses de Closest Point Rule but defined over +% the Mahalanobis distance of the points instead of the +% Euclidean counterpart. The original pIC algorithm uses +% a virtual point as the correspondence one. For a given +% reference point ri , its associated virtual point ai is placed +% in the probabilistic weight average of the statistically +% compatible points of the new scan (those which satisfy +% the Individual Compatibility test). pIC registration is +% done as in the M bICP but using the + +% L. Montesano, J. Minguez, and L. Montano, “Probabilistic scan match- +% ing for motion estimation in unstructured environments,†in Intelligent +% Robots and Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International +% Conference on. IEEE, 2005, pp. 3499–3504. + + +function [R, t, NI] = pIC(A, B, motion) + +% In: +% Ai: localCart[2xN], points cartesian, covm[2x2xN] points +% uncertainty +% Bi: localCart[2xN], points cartesian, covm[2x2xN] points +% uncertainty +% MOTION: CON.U[d psi theta], motion estimation, translational, +% rotational and orientation +% STATE.P[3x3] scan uncertainty +% Out: +% R: rotational corrrection in quaternion +% t: translational corrrection in quaternion +% NI: number of iterations + +global Opt DEBUG +opts = Opt; + +t = []; +it=0; + +% Format the two scans we are going to use +ScA.cart = A.localCart(:,1:2)'; +ScB.cart = B.localCart(:,1:2)'; + +ScA.polar = A.localPolar(:,1:2)'; +ScB.polar = B.localPolar(:,1:2)'; + +itMax = opts.scanmatcher.iterations; + +ScA.P = A.covm; +ScB.P = B.covm; + +%Motion estimation q0 +u = motion.con.u; +originalodo = [cos(u(2))*u(1) sin(u(2))*u(1) u(3)]; +motion2D.estimation = originalodo; +solution = originalodo'; +motion2D.P = motion.state.P(1:3,1:3); + +% error convergence init +lasterror = []; +corr = []; + + +if DEBUG.mahaAssociation || DEBUG.all + scrsz = get(0,'ScreenSize'); + opts.fighandle=figure('Position',scrsz,'Renderer','zbuffer','doublebuffer','on'); + axis equal; + xlabel('X (m)'); ylabel('Y (m)'); + hold all + opts.plot_r = plot(NaN,NaN,'.r','MarkerSize',6); + opts.plot_n = plot(NaN,NaN,'.b','MarkerSize',6); +end + +while it < itMax + + % Check convergence on previous error + if checkConv(lasterror, corr) + break + end + + %% Association + + %Compute the associated point with the Mahalanobis Distance + [merr assoc] = mahaAssociation(ScB, ScA, opts, motion2D); + + if isempty(assoc) + break; + end + + if ~isempty(opts.scanmatcher.rejection_rule) + [assoc ixs] = opts.scanmatcher.rejection_rule(assoc); + assoc.oldn(:,ixs) = []; + assoc.Pe(:,:,ixs) = []; + assoc.Jq(:,:,ixs) = []; + end + + if size(assoc.ref,1) < 5 + break; + end + + [~, solution] = pICRegistration(assoc,motion2D,10000,0); + + motion2D.estimation = solution'; + + lasterror = [lasterror 1/merr]; + corr = [corr; solution']; + + if size(lasterror,2) > opts.scanmatcher.niterconv + lasterror = lasterror(2:end); + corr = corr(2:end,:); + end + + it=it+1; +end +solution = solution' - originalodo; +t(1:3) = [solution(1) solution(2) 0]; +R = e2q([0 0 solution(3)]); +NI = it; \ No newline at end of file diff --git a/ScanMatching/2D/pIC/pICRegistration.asv b/ScanMatching/2D/pIC/pICRegistration.asv new file mode 100644 index 0000000..4dd64b5 --- /dev/null +++ b/ScanMatching/2D/pIC/pICRegistration.asv @@ -0,0 +1,54 @@ +function [out,solution,errorK1,nConverged] = pICRegistration(assoc,motion,errorK1,nConverged,tmp) + +errRatio = 0.0001; +error = [0.0001 0.0001 0.0001]; +nIterationSmoothConvergence = 2; +%motion_estimation = qpose3D2epose2D(motion.x); + +if (size(assoc,2) == 0) + error('No associations avaiable.'); + out = -1; + return +end + +E = []; H = []; +C = []; +for i = 1:size(assoc,2) + e = assoc(i).a-assoc(i).refNewPoint; + E = [E; (-e+assoc(i).Jq*motion.estimation)]; + H = [H; assoc(i).Jq]; + C = blkdiag(C,assoc(i).Pa+assoc(i).refNewP); +end + +invC = inv(C); +qmin = inv(H'*invC*H)*H'*invC*E; + +tx = qmin(1,1); ty = qmin(2,1); yaw = qmin(3,1); +cosy = cos(yaw); siny = sin(yaw); + +err = 0; +for i = 1:size(assoc,2) + errorX = assoc(i).newPoint(1,1)*cosy-assoc(i).newPoint(2,1)*siny+tx-assoc(i).a(1,1); + errorY = assoc(i).newPoint(1,1)*siny-assoc(i).newPoint(2,1)*cosy+ty-assoc(i).a(2,1); + err = err+errorX^2+errorY^2; +end + +errorRatio = err/errorK1; +dist = motion.estimation - qmin' +ERR = 1-errorRatio +if (abs(1-errorRatio) <= errRatio) || abs(dist(1)) < error(1) && abs(dist(2)) < error(2) && abs(dist(3) < error(3)) + nConverged = nConverged+1; +else + nConverged = 0; +end + +% Build solution +errorK1 = err; +solution = qmin; + +% Smooth convergence criterion +if (nConverged > nIterationSmoothConvergence) + out = 1; +else + out = 0; +end diff --git a/ScanMatching/2D/pIC/pICScanMatching.m b/ScanMatching/2D/pIC/pICScanMatching.m new file mode 100644 index 0000000..51485e4 --- /dev/null +++ b/ScanMatching/2D/pIC/pICScanMatching.m @@ -0,0 +1,112 @@ +close all; clear all; + +global PARAM + +% pIC parameters RETOCAR!!! +PARAM.deviation.sensor = [0.05 deg2rad(1.5)] % std deviation [angular(rad) range(m)] [+/-1.5º +/-5cm] +%PARAM.deviation.sensor = [0 0] % std deviation [angular(rad) range(m)] [+/-1.5º +/-5cm] +PARAM.deviation.motion = [0.1 0.1 deg2rad(10)] % motion std deviation [x(m) y(m) yaw(rad)] +PARAM.step = 2; +PARAM.confidence = 0.95; +PARAM.maxDistanceInterpolation = 0.5; +PARAM.filter = 1; +PARAM.projectionFilter = 0; %!! test with 0 +PARAM.associationError = 0.1; +PARAM.maxIterations = 250; +PARAM.errorRatio = 0.0001; +PARAM.error = [0.0001 0.0001 0.0001]; +PARAM.nIterationSmoothConvergence = 2; + +LastScanK = load('scan5.mat'); +LastScanK1 = load('scan6.mat');%LastScanK; %load('scan6.mat'); + +motion.estimation = [-0.2 0.6 deg2rad(5)]; +%motion.estimation = [-0.2 1 deg2rad(-5)]; % Real data. No noise added + +threshold = 90; +maxRange = 18; + + +% World configuration +XWorld = 16; YWorld = 8; + +% Sensor configuration +sensor.range = 10; +sensor.rangeDev = PARAM.deviation.sensor(2); +sensor.sector = deg2rad(360); +sensor.thetaStep = deg2rad(1.8); +sensor.thetaDev = PARAM.deviation.sensor(1); + +positionK = [8,4,deg2rad(30)]; +figure +[DataK,TMP1,TMP2] = SimulateProfiler(LastScanK.scan,threshold,maxRange); +scanK = Preprocess(DataK); + + +% % Main While +errorK1 = 1000000; +nConverged = 0; +nIteration = 0; +% +positionK1 = [10,5,deg2rad(60)]; +%positionK1 = [9,4,0]; +figure +[DataK1,TMP1,TMP2] = SimulateProfiler(LastScanK1.scan,threshold,maxRange); +scanK1 = Preprocess(DataK1); + +% Struct for motion +% Estimation with noise +disp('motion.estimation'); +%motion.estimation = positionK1-positionK; +motion.estimation +% inici = motion.estimation; +% motion.estimation(1) = motion.estimation(1)+normrnd(0,PARAM.deviation.motion(1)); +% motion.estimation(2) = motion.estimation(2)+normrnd(0,PARAM.deviation.motion(2)); +% motion.estimation(3) = motion.estimation(3)+normrnd(0,PARAM.deviation.motion(3)); +% motion.estimation(1) = motion.estimation(1) - 0.5; +% motion.estimation(2) = motion.estimation(2) - 0.5; +% motion.estimation(3) = motion.estimation(3) - deg2rad(10); +% Covariance + +handle = figure; +hold on +hscanK = plot(scanK.cart(1,:),scanK.cart(2,:),'b.'); +hscanK1 = plot(scanK1.cart(1,:),scanK1.cart(2,:),'r.'); +hscanSol = plot(scanK1.cart(1,:),scanK1.cart(2,:),'g.'); +legend([hscanK,hscanK1,hscanSol],'scan k','skan k+1','solution',1); + +motion.P = [PARAM.deviation.motion(1)^2 0 0; 0 PARAM.deviation.motion(2)^2 0; 0 0 PARAM.deviation.motion(3)^2]; + +disp('noisy motion.estimation'); +motion.estimation +out = 0; +while (nIteration < PARAM.maxIterations && out == 0) + [resEStep,assoc] = EStep(scanK, scanK1, motion); + + [resMStep,solution,errorK1,nConverged] = MStep(assoc,motion,errorK1,nConverged); + motion.estimation = solution'; + + %DisplaySM(scanK1.cart,motion.estimation',hscanSol); + motion.estimation + %pause; + + if (resMStep == 1) + out = 1; + elseif (resMStep == -1) + out = -2; + else + nIteration = nIteration+1; + end +end +hold off; + +solution = motion.estimation' + +out = 2; +nIteration +% Graphical representation +if (out > 0) + DisplaySM(scanK1.cart,solution,hscanSol); +else + disp('Unable to display results'); +end \ No newline at end of file diff --git a/ScanMatching/performSM.m b/ScanMatching/performSM.m new file mode 100644 index 0000000..2bab62a --- /dev/null +++ b/ScanMatching/performSM.m @@ -0,0 +1,29 @@ +function [out] = performSM(Rob,Tim,Opt) +% PERFORMSM, using the option given by the user, says whether to perform +% scan match or not. We perform a Scan Matching after a certain distance +% has been travelled + +if Opt.scanmatcher.usematcher==0 + out = 0; + return +end + +Map = Rob.Map; + +% We have no points, no correction will be performed but the last points +% taken will be added +if Rob.lastcorrection == 0 + out = 1; + return +end + +% Distance travelled in meters +distance = ptsDistance(Rob.state.x_full(1:2,Rob.lastcorrection), Rob.state.x(1:2) ); + +if distance >= Opt.scanmatcher.motionamount + out = 1; +else + out = 0; +end + +end \ No newline at end of file diff --git a/ScanMatching/performSM.m~ b/ScanMatching/performSM.m~ new file mode 100644 index 0000000..a8a2d95 --- /dev/null +++ b/ScanMatching/performSM.m~ @@ -0,0 +1,24 @@ +function [out] = performSM(Rob,Tim,Opt) +% PERFORMSM, using the option given by the user, says whether to perform +% scan match or not. We perform a Scan Matching after a certain distance +% has been travelled + +Map = Rob.Map; + +% We have no points, no correction will be performed but the last points +% taken will be added +if Rob.lastcorrection == 0 + out = 1; + return +end + +% Distance travelled in meters +distance = ptsDistance(Map.prev.x(1:2,Rob.lastcorrection), Rob.state.x(1:2) ); + +if Opt.scanmatcher.distance > Opt.scanmatcher.motionamount + out = 1; +else + out = 0; +end + +end \ No newline at end of file diff --git a/Sensors/Sonar/multibeamComplex2D.m b/Sensors/Sonar/multibeamComplex2D.m new file mode 100644 index 0000000..9d67da5 --- /dev/null +++ b/Sensors/Sonar/multibeamComplex2D.m @@ -0,0 +1,95 @@ +function [ localCart2D localPolar2D ] = multibeamComplex2D( Rob,World,Sen ) +%MULTIBEAMCOMPLEX2D Summary of this function goes here +% Detailed explanation goes here +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Multibeam parameters + +Fr = Rob.state; +%Multibeam Coorindate System +Q = Fr.gt; %+Rob.state0([1 2 3 6]); %frame ground truth +T = Q(1:2); %robot pose +rad = Q(3); %orientation + +nBeams = Sen.par.nBeams; +S = Sen.par; %Sensor parameter + +sonarRange = S.maxRange; +sonarWidth = S.maxWidth; +skipped = 0; +localPolar2D=[]; +localCart2D=[]; +sonarAngle = sonarWidth/nBeams; +noise = Sen.par.beamStd(1); +angle = -sonarWidth / 2; + +for i = 1:nBeams + + beamangle = deg2rad(angle + i*sonarAngle); + angle = normAngle(beamangle+rad); + + Tr(1) = T(1) + cos(angle) * sonarRange; + Tr(2) = T(2) + sin(angle) * sonarRange; + + [xout yout] = polyxpoly(World.segments.coord(:,1),World.segments.coord(:,2),... + [T(1) Tr(1)],[T(2) Tr(2)]); + + + if(isempty(xout) ) + skipped = skipped +1; + continue; + end + + if size(xout,1) > 1 + mind = inf; + for k = 1:size(xout,1) + d = ptsDistance([xout(k) yout(k)],T(1:2)); + if(d < mind) + cxout = xout(k); + cyout = yout(k); + mind = d; + end + end + else + cxout = xout; + cyout = yout; + end + + d = norm([cxout-T(1),cyout-T(2)]); + + ns = stdErr(0,noise); + ns2 = stdErr(0,noise); + outl = randn^2 < Sen.par.outliers; + + if outl + outlp = randn ; + else + outlp = 1; + end + cxout = cxout+(ns*d)*cos(beamangle); + cyout = cyout+(ns*d)*sin(beamangle); + + localCart = cartProject([cxout cyout ], -rad, (e2R([0 0 -rad])*[-(T(1:2)*outlp)' 1]')' ); + + %localCart = cartProject([cxout cyout ], rad(3), (e2R([0 0 rad(3)])*[(T(1:2)*outlp)' 1]')' ); + + + % Return the points locally referenced + localCart2D(i-skipped,1:2) = [localCart(1:2)]; + [la lr] = cart2pol(localCart2D(i-skipped,1), localCart2D(i-skipped,2) ); + localPolar2D(i-skipped,1:2) = [ la lr ]; + +end + + +%Draw profile +% figure +% hold on +% axis equal +% displayPoints(localCart2D,'k'); +% % displayPoints(globalCart2D,'r'); +% % displayPoints(globalPolar2D,'g',1); +% displayPoints(localPolar2D,'b',1); + + +end + diff --git a/Sensors/Sonar/multibeamWall2D.m b/Sensors/Sonar/multibeamWall2D.m new file mode 100644 index 0000000..c601f15 --- /dev/null +++ b/Sensors/Sonar/multibeamWall2D.m @@ -0,0 +1,53 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Multibeam simulation in 2D +% Simulates a multibeam sonar scanning the wall of the world + +function [localCart2D localPolar2D ] = multibeamWall2D(Fr,World,Sen) + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Multibeam parameters + +Fr = Fr.frame; +%Multibeam Coorindate System +Q = Fr.x; %frame ground truth +T = Q(1:3); %robot pose +q = Q(4:7); %quaternion orientation +rad = q2e(q); %radians orientation +nBeams = Sen.par.nBeams; +S = Sen.par; %Sensor parameter + + +skipped = 0; +localPolar2D=[]; +localCart2D=[]; +for i = 1:nBeams + + %Calculate the intersection with the walls defined in the world point using the ground truth + [localPolar realPoints] = intersectPoint2DWall( [ T(1:2)' rad(3) ], i ,World.lims.xMax,World.lims.yMax,S,Sen.par.beamStd(1)); + + + + if(localPolar(1) == -1 && localPolar(2) == -1) + skipped = skipped +1; + continue; + end + % Return the points locally referenced + localPolar2D(i-skipped,1:2) = localPolar(1:2) ; + [lx ly] = pol2cart(localPolar2D(i-skipped,1), localPolar2D(i-skipped,2) ); + localCart2D(i-skipped,1:3) = [ lx ly T(3) ]; + +end + + +%Draw profile +% figure +% hold on +% axis equal +% displayPoints(localCart2D,'k'); +% displayPoints(globalCart2D,'r'); +% displayPoints(globalPolar2D,'g',1); +%displayPoints(localPolar2D,'b',1); + +end diff --git a/Sensors/Sonar/singlebeamWall2D.m b/Sensors/Sonar/singlebeamWall2D.m new file mode 100644 index 0000000..a91121e --- /dev/null +++ b/Sensors/Sonar/singlebeamWall2D.m @@ -0,0 +1,42 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Multibeam simulation in 2D +% Simulates a multibeam sonar scanning the wall of the world + +function [ localCart2D localPolar2D Sen] = multibeamWall2D(Fr,World,Sen) + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Multibeam parameters + + +%Multibeam Coorindate System +Q = Fr.x; %frame +T = Q(1:3); %robot pose +q = Q(4:7); %quaternion orientation +rad = q2e(q); %radians orientation + +S = Sen.par; %Sensor parameter +S.i = S.i + (1*S.dir); + +if(S.i >= S.maxWidth) + S.dir = -S.dir; +end + + +[localPolar realPoints] = intersectPoint2DWall( [ T(1:2)' rad(3) ], S.i ,World.lims.xMax,World.lims.yMax,S,Sen.par.beamStd(1)); + + % Return the points locally referenced + localPolar2D = localPolar(1:2) ; + [lx ly] = pol2cart(localPolar2D(1), localPolar2D(2) ); + localCart2D = [ lx ly T(3) ]; +%Draw profile + +Sen.par = S; + +if localPolar2D(1) == -1 && localPolar2D(2) == -1 + localCart2D = []; + localPolar2D = []; +end + +end diff --git a/Sensors/addPointsToMap.m b/Sensors/addPointsToMap.m new file mode 100644 index 0000000..eaa2ae0 --- /dev/null +++ b/Sensors/addPointsToMap.m @@ -0,0 +1,38 @@ + + +function [ Map filtered ] = addPointsToMap(Points,Map,Tim,Rob) + +%Add points to a map depending on its type. Only list of points. + + filtered = Rob.filtered; + filtered.localPolar=[]; + filtered.localCart=[]; +switch(Map.type) + + %For occupancy grid 2D. TO DO. Just point cloud so far + case 'occgrid' + + actualdata = size(Points.localCart,1); + + if(actualdata <= 0) + return + end + + [data polarp] = calculateGlobalObs(Points,Rob); + covm = Points.covm; + + Map.grid = [Map.grid; data]; %( ( (Tim.currentFrame-1) * datasize + 1) : (Tim.currentFrame*datasize) ,:) = data; + Map.prev.gridlocal(Tim.currentFrame).points = Points.localCart; + Map.prev.gridlocalpolar(Tim.currentFrame).points = Points.localPolar; + Map.prev.gridpolar(Tim.currentFrame).points = polarp; + Map.prev.gridcovm(Tim.currentFrame).points = covm; + Map.prev.written(Tim.currentFrame) = actualdata; + + + + + otherwise + + error('ERROR: Unknown Map Type ''%s''.',Rob.motion); + +end diff --git a/Sensors/calculateGlobalObs.m b/Sensors/calculateGlobalObs.m new file mode 100644 index 0000000..ede4f04 --- /dev/null +++ b/Sensors/calculateGlobalObs.m @@ -0,0 +1,21 @@ +function [ globalCart2D globalPolar2D ] = calculateGlobalObs( Raw, Rob ) +%CALCULATEGLOBALOBS Calculates the global coordinates of a scan + + Rob.state.x = frameRef(Rob.state.x,Rob.state0); + + x = Rob.state.x(1) ; + y = Rob.state.x(2) ; + rT = r2Rt(Rob.state.x(3), [x y]); + + globalCart2D = zeros(size(Raw.localCart,1),2); + globalPolar2D = zeros(size(Raw.localCart,1),2); + + for i=1:size(Raw.localCart,1) + + p_r = ( rT*[ Raw.localCart(i,1:2) 1]' )'; + globalCart2D(i,1:2) = p_r(1:2); + [a r] = cart2pol(globalCart2D(i,1), globalCart2D(i,2) ); + globalPolar2D(i,1:2) = [a r]; + end +end + diff --git a/Sensors/filterScan.m b/Sensors/filterScan.m new file mode 100644 index 0000000..37ba903 --- /dev/null +++ b/Sensors/filterScan.m @@ -0,0 +1,90 @@ +function [ Rob ] = filterScan( Rob, Sen, Tim, Opt ) +%FILTERSCAN Filters and form a Scan depending on the paremeters + +% Scan Building: compound the scan taken at different locations. +% The scan stored in Rob.raw represents the +% scan taken from the time frame Robot.filtered.last till now. If the +% number of scan taken per pose is bigger than one, it is implicit that +% those scan are taken at the same position. Only works for the 2D case. +% The points are shifted over the 2 axis and rotated using the yaw odometry + +lastfilter = Rob.filtered.last+1; %last filtering +npose = floor((Tim.currentFrame-lastfilter) / Tim.step) + 1; %number of pose involved +poses = Rob.state.x; +if( npose > 1) + poses = [Rob.Map.prev.x(:,lastfilter:lastfilter+npose-2) poses]; %poses involved +end +nscans = size(Sen.raw.localCart,1); % number of scans taken +scanpose = floor(nscans/npose); % number of scans per pose +centertraj = poses(:,floor(npose/2)+1); +x = centertraj(1:2)'; +yaw = centertraj(3); + +Rob.filtered.localCart = []; +Rob.filtered.localPolar = []; + + +% Reference every scan point to the same coordinate system, which is the +% center of the poses involved + + +for i = 1:npose + + curpose = poses(:,i); + + xn = curpose(1:2)'; + yawn = curpose(3); + u = calcOdo2D( [xn yawn], [x yaw] ); + rr = r2Rt(-u(3),-[u(1) u(2)]); + + intv = 1+(i-1)*scanpose: 1+(i)*scanpose-1; + % Less than one point per pose + if scanpose <= 1 %&& i~=npose + + localpoints = Sen.raw.timed(lastfilter+i-1).localCart; + polarlocalpoints = Sen.raw.timed(lastfilter+i-1).localPolar; + + for zz = 1:size(localpoints,1) + scanpol = polarProject(polarlocalpoints(zz,:), u(3), u(1:2) ); + scan = rr*localpoints(zz,:)' ; + + Rob.filtered.localCart = [Rob.filtered.localCart; scan']; + Rob.filtered.localPolar = [Rob.filtered.localPolar; scanpol]; + end + + + else + % 1 ore more points per pose + for k = intv + + Rob.filtered.localCart = [ Rob.filtered.localCart; (Sen.raw.localCart(k,:) ) ]; + Rob.filtered.localPolar = [Rob.filtered.localPolar; Sen.raw.localPolar(k,:) ]; %polarProject(Sen.raw.localPolar(k,:), u(3), u(1:2) ) ]; + + end + end + + + +end + +%Calculate the points uncertainty + +std = [Sen.par.beamStd(1) 0; 0 Sen.par.beamStd(2)]; +Rob.filtered.covm = []; + +%calculate the covariance matrix of the measurements +for j = 1:size(Rob.filtered.localCart,1) + J = [cos(Rob.filtered.localPolar(j,1)) ... + -Rob.filtered.localPolar(j,2)*sin(Rob.filtered.localPolar(j,1));... + sin(Rob.filtered.localPolar(j,1))... + Rob.filtered.localPolar(j,2)*cos(Rob.filtered.localPolar(j,1))]; + Rob.filtered.covm(:,:,j) = J*std*J'; +end + +Rob.filtered.last = Tim.currentFrame; +%Rob.state.x = centertraj; +Rob.filtered.pos = centertraj; +Rob.filtered.poses = [Rob.filtered.poses; centertraj]; +Rob.filtered.timePoses = [Rob.filtered.timePoses Tim.currentFrame]; + +% Filter SCAN : TO DO diff --git a/Sensors/getScan.m b/Sensors/getScan.m new file mode 100644 index 0000000..f3ebf46 --- /dev/null +++ b/Sensors/getScan.m @@ -0,0 +1,20 @@ +%Retrieves a previous scan with index i from a map object +function S = getScan(Map,i) + +S = []; + +if i <= 0 + return +end + +s = Map.prev.written(i); + +S.data.localCart = Map.prev.gridlocal(i).points; +S.data.globalPolar = Map.prev.gridpolar(i).points; +S.data.localPolar = Map.prev.gridlocalpolar(i).points; +S.data.covm = Map.prev.gridcovm(i).points; + +%S.data.globalCart = Map.grid( ( (i-1) *s) + 1 : (i*s) ,:); + + +end diff --git a/Sensors/simObservation.m b/Sensors/simObservation.m new file mode 100644 index 0000000..8ef6814 --- /dev/null +++ b/Sensors/simObservation.m @@ -0,0 +1,35 @@ +function [Raw, Sen] = simObservation(SimRob, SimSen, SimLmk) + +% SIMOBSERVATION Simulates range infer sensors + + + +Sen = SimSen; + +Raw.type = 'simu'; + + +% We need to global reference the robot in order to work with a real +% world. NOT ALL SENSORS SHALL USE THIS. TO FIX +SimRob.state.gt = SimRob.state.gt; + +switch Sen.type + + % Simone Zandara @ VICOROB + + case 'Multibeam2D' + + [Raw.data.localCart Raw.data.localPolar] = multibeamComplex2D(SimRob, SimLmk, SimSen); + + + case 'Singlebeam2D' + %TODO + + otherwise + % Print an error and exit + error('??? Unknown sensor type ''%s''.',Sen.type); + +end % end of the "switch" on sensor type + + +end diff --git a/TechSheet.odt b/TechSheet.odt new file mode 100644 index 0000000000000000000000000000000000000000..6f17b21d1a83d0e522fe401bb530f006bcca9b2c GIT binary patch literal 89844 zcmZU)1ymf*wmsbVK!QUE?ry=|-E|;%u;A`a2myjS1eaig1rH9vA-KD{yTdoXci;Qp z`@VP9>NP#pYkI1yt4^J>_dco$aPT+)00Mw}y;u>$UiP=N008`RK$`$ND?4*nFGq7@ zM@L&LQ)5>v2YXg`dovaXV;3tI76(Ujdou@9H#>8CR~A=uPgm9db+hMKIl_S>0Ql!% zhTcro%GA}(+1!Pd?ZbZ<5AVOeaB}c+andx|ulTM~A6+#ZKD&AtJDUR*_6~OD%r2(R z=H~V;mJY6`aP7hXKnBQ3ifMXg9A$d=YJwkLvt3RzlNA?!(o#_+s$ygDGbn}2U*vugaM*~02vnSTl^QAICa+&@^C(SCTeB|mKn0M zu!<(^zNCwML}h-J472`Na*2?)`0W3jbQz(;jNqv#e_x&*4-Yst9AOl!%rNdhdu4ec z)kO@@;I&`k{bcf>^ae)vCy4FyTo|K;fgm*_?FK*>^em1=d*W4OZuG_JvHpG z_V2#vf0`!;jNPV?cdf#Q;0IEU?QJQ0V=0xGvbI?F&Yia+eY}zmqCfytF_mCN zyp{qb&E>1v_cp=lU3R!i*0af8#H$uTvHoi9!Tnzbs?OJG@|6Lgp=?S|NTw7qaYm za(L35!mRh1lfdP6=S5Ki`jUb?pH&H{*YIfLT`{eY_xV~Mbtm4;P~q6&AnE78#?g7x z>EbRhOl~?$c78cXIc2AFgSDco)CeX^j#_UT4>;g(yo+4-OJK$%K|avkK6qDE53X*~ z%#DG-x(L zw>-{pz0~9vUggUF9v?p|D=RHEBQ+!AJs}<;9$wdZ#`4LY7c2a5)kJD(BVWy~@`;aw z)?cpY&Az_tp0h90&ny%_Qbb?Bt2@`z<~1a6^j8yVe$Qv%(}pqbk2Q`-5esSOMgaxx zNhuX|Z*6ULcXu~EJU}1}5Qxj}$n?JJn|Dl}$Jeu?Q#cYK?N}Fqn?hhocdn~x>MOJ_ z{ZIZj$VD) zH4dfxUiFZMNH3W)Anp57=wI@jcP))QII*oI({mnz-Dji7?<$8uOwCzGx7%SS-c-8j zb1X8~90OYGh^mVyEDnW3p480<`C4%Ol8K^tneBfq?D#Jwszai{yzRgOMPU&v~ z?ccOF$S7h^vvZ~NY$BT{{k$H+cKxb%u2kU6{V4P(o7ela9x{IYYEV_B%NNkXAHxQZ z>!B23{%Oykf-+o_YWCoWWc2(CwrwjrFUn^$kZ37NIo1q!SkC0`%zM^j%Kb5p;~2H| z(sXp4>4VR!zBGa&M`;93_d{MEI#TENJ2Ka4`x_d#oPCz z`>LVWiA|!=D-0w9ef-nErz+G@Z47}dW!N_yV|7}6t-W8XB*Mf1NFEh=Wr%Yx%nzq$ zj=aa8niZ=Cw4n#}JlSs=nky$?K56a5Mt*^_Z@%W1QUd}vJ>|g>)C{z=yA%5k%kDNfMO~sCUagoJe~?(5 zkY}<+$&@Q8Qapn*Z59^G7!Zz;`H+qMC3n4PqVzXuh3rz=kGqH z4C=}&wTJ!komA}mjR$n)Z0DEej0r|KQ}#FmA0*`YK~0q;0qQ(;Je<6UTCL!4Qgf+KcR1l-3DY4C>Mw1IxP#bN||x z{|Qdh&NTXu#(5Zl(g3@c|Z1y{kW zI98sL-NkD802vQY3I%lsrX<;W6E!#eu=-n_A>pAZ>ig=^YUl*eP$y=0b0U-MJ$O{C z4d-tA91zUlBJ@W4=9lsO{5*7>-6LJ7;K%CJd^R^T6IB7Lt6%)N9L*NFZf`7?>I zYjLUi6Mk}K`^y6f{Bi5;$}=^RCkaiC{~@SI&1ei6b3KlnhH}?{2Z+Tm+R;!0Hw>B9 z)XP`i%mx(FpUI^rwWKdB6F^}goMlzmVW70rPM!NOum2Ikx~vp;cb8&3t}4m?p5fIk#JVEFuc$YL zU#~KB|HW%Cr($Ze zr=L#7bK#cGjmiBI(ppkNlt-@#gsGU@e$Jf4fuk{~mL?RC6!FJzYYf0c?ZU=vhn1PJ z)n+pwh3+_3G+sJ&ogUM=n_MfoY1I6rL0`?~-G@ZB|Md0b6u@0{s%MM6uCuK?QIcQ> zIeD0-l$5j-tem)kh~HeRlw0K-**-HDh4CC_PfeS`Gd*22~2+e6ctr|nMEIuttlA^H|{I1Je72><^Jk`)u))jP7h(fUNGM z)5@m4&sEWQ{d2(3X0P+dAW3p2)KaoN$WBE=CvsqrQBmtYGI@D zXz}uStaa~7L0`I5YxJG5aBv^_b##;=5sK!{a;0-Y!44~Gt- zC`HD6Pk(4v_At5W^aQ)YY8NiX3MBTAL@X70lH5jT&|7 z@sin`Mvh8MLvFs;SNhzo#0(|cD`NVc?t6%waOGJS_-c;)5n=uD1HUwZ5Uo7m@LJ^} z-SV;CfEBaIULof&GLQc73l9lF?dyGW%C4?edP39IQ1;6#xM})!rM*!8q~)zUE=%ht zSy|aNUbGgCvgKFIFT0*Ke4JF>ni_hP5f}4%e;4(6oA>2OQG!@#Y5Q!s9TuCE-k`|b z4dAI*TU!e|l4Ywe80d-q3#l{`bj3wvV@~0quSQux#&L zJyyJCle7(y0iCYudfR#5y|MKZC78W0+v($uH~Rd4@?VuRZa!p3sTNLdnoub1fn;_=-cAH%1CT;@s` z29uCql|=S}Qvf=9H9u^5*M6T#2 ziQ*g4{iVRjC^T3#2=DVvAx`^(UI9fdZ})5)gQxv|Y6<0|e_8`6CVk#? z`NzNFt{*LxGzq3i`G$7G{+*#fcY?1J0NL6$UTQ*d>1YQPjzI?;#!t~N3^@?ZWYJ_b ztdc#kn9%SnVW$U$i6jqq9k?2#?pQ(Ro#|Ot0^&ESFe~T~%Y%S>RJ_m=gUi#`8L`1T zn~KH<7`oTXH;+e}m%$L6?;Y5G-Uy1xgW<8_tM*OxGSNJf6x;UZ7!M+qt*5=SI%9^S7FlU&vO#W!vP+1{b_2CN+N7 zpROx!I3W02s9d!;2IhB$xCwpT_!l?zJ;#BDc^w5I=;l(&B3I{wQyR@bLf z_V8-rGrb7Ihmme+B=#F@K_X4XYqOY+aG35f^6laZ@ayn9T@|j>Tv%{#mCR^jmOA3k zmyKjo;|Jki_VHQWlMKw`UpJB^Ku4MosiRD(Y~Wk(!_1dnkE$S)Ulz zQX7%N)8QDtldF2>RYR-WHCHIm|=` zZ=Q$5#<{;?|u;4yy}kIybUP0ziwQlJ7;x=lV^eWw*dW8%mdSbPT$0u>~S zB78ZP%bo8KFrVOP0N;u2S8u1CaTy0Aoh|Y}DvHaCg4q$GX@SW6kp#W0fTv?vHva>^ zm=`C>G8j>N;fLeLO8;LqhA_<5=Wnu%>Hr{TYnqVsd0dcn35>s1cQ&!ZDdm;iT!a>uNVh2=KJN@w?8(%_ga)uux2I+Dc}my+Ym8X<)i2bqygzXB&v59s=`glH7HbGK zo$s;jL2;xjVhWE+?7<;ZvwRumv{FTjhTsA+I_^XBA6YOZ-aUv9ywjGrebUIQQ-tJ& z0cF&AojYpaaCmz1;MnU&n+X#MYWX?=vd?0$f!^uG#S9eIe0$Q(v@HD9+!Z2Icv&Oh z-v5AKpqVbW^@DHd^SzW4U2{p0Cvyu!F1z>_vny|` zgr0iZ8+cSX&6rwz3{RWfVnm+C)r!JJK}Rc9$RG^ZqW5oLvK%F7{yOkp7_16!Qc(N> z^^q5h$r1JG9GAaS_{k(E8Fw8L`!}L;>Grxg{0RBT{Px5^DJn{IZLZ36?eH6e{yfRa z-tSDzXLT@+P`FZ|{f{P`gWMcdReA>Rl?-g3<&I)Lsd6Lyd&jLVjDrot`2)1z60F(h z5;>@;sLVcbrVqO}3$e1Yim<9gh=vY|F&9G)T={3q^(?ABF4;F2R*JaZ#qOO;%JfZ5 zO=U0%=;*6YwHv+0$HzNu_8!kSTl|s2#%ZqJEozCy&{)-h(*#eC7|09(Yh_Hr>T`M4 zK$vJyA!d`@&Qr^NT4rWKLP9=5QD%bhGwdt^08>^{lEv?~aC5w9V`CGf1@*TCBL07p znbW`aG0-{dzAcrH#^Ej%1p%-aWWL?Mq#9VVKKyRz0j;t4?tY==dE)|yt(rz}7aSd) z_ou5I6x)8=$DDq<+In=8?jYj4b?#sd2FS!}=-`8PzcsZwBa?@`XfF$pYKeHB@_^%S zU9tB}T6~YT7VUq`H96fw-|uqrUO8&_n8i*cXJ0+7p`Oni+|T}bo3Z?w&$o}>=58~~ zqpJdYW9=tC)Uz|wSYkm9e$Qb=2A`S->4Gpw@DQC7n0MlPHfKcYTEAn7PTl_SQ{uLB z45H?aM6zvo>mGm=B@LIcqR;;RtwB;6;?lz)cEX>9WQx%dT6Y;*Gp$PtH|{+)YsKs za4<0trzNIZ1^|M7aeb%!DXE)AI5h9Ud448S&w&p2xM|E^o@0aO1DS8kR1ETvi{iw3qWP

PWC%#w!297J4e>LRqPi@xOXsc zf_U)j;Ed{MY|n^;si>-39MhGV=;y!$dL=h@c>7*5XkipV z7;WW^7l{UB`xNcOhl>n92(2!|NrbW)f_5ddhKEPtvy*zA&5`va>~;i$48H43=(oW| z7CF<5!{^Fo(pFg~%k!Vg?%7mg7 zG~9dsITRV-fPwhyHcOv9iap4iV2_SKjjyDky)ZORFcCL5GQ5zqCmBj)6+I$4{b)m` zT$hnJa!uZnV$ZJEz z;xt}O17q=IJhFX+Z~|o6@o1Wo@B;kLyy&yDaono5;5sVPY~GrEGH+8yk13vD<^ynItsQfYrG)>pSDX zqKq|mUvScp5IOv^*H*7Tj~7tq(vCJ~R?V@>;y8#~f8Qqt8C7*(H+VDrsc#*Ip;KXG z&NgsHBqaSv&p8%Wrzx>=Z6yw$H801gFnRW!agGm)SGOVC#Ru$sRWW;;aB9sauS1i6 zB`6YsXo(Svd`1zn*qI5@mC~X>Wc;ig`8ajP53~zhg6!+P8b?^l^?u4I$)&bC zx-Fz4j-<(^K4VOL2PeoJ$Li;3EZYwi7lfE|bF*!d6owcYh-V2MRIbwUm@JyojZ)?7 zE|LqgP=0uP?3U70%ge~K0zWl6gtZ8deW)c{VD>(fqg%@s=T>Sn1|o+vuo+X+B#mZP zFfF`2WuGLZq`JZNu*1b_8pLp7HxyUkA!30fXW+GgCFJ57M&4tMO!jy0#hS=Y%rCfS zrl(C{8>4tIc*J)sEv1p2+h_bVX?@sq<{7u7bH8;;#VbCfMtGh&wfd$PMt_Kk6dLF8 zz{~z^5X?;K9G*%qWL6j+a!5I(fvqV?b=bV1IBYo1IIavTj$lU>yUeXeyn9}~+i*Uwf&7kpo8dk3R%L&P{ zkD)K~NH^zt+V_Rt->HIn62`}uDDV4A%S5ot4#`|$;TQ!uA~TIu%&v`9^P-YmoJ7Z= z@>_MyHS@pb16tZuUO@@bC!ORHB)SnEoKMbdS@^|@?Q?r-9!A*(KQK4Yps8|I`*Mdw zyUjQtD;ShSpeDq>GMtjVWIq=KaS7k?c{tEih5ip|?-(Unm#%B4Z9A*duCyv`+qP}n zR;6v*wr$(Cak6{wKD&GLyU#x7`+me2F(Sr{SP^T+T2IV%U(aQ7;L$|x&v2XB9*TBs zc@Nq_XpGZAYCUzFo^Cx8yzI?`pBH^B?-1ohWKHjY`jm6dmKp33`oYv1Rp|#BJy;%c zGzIUl@>X%Z`b@*UXK;I{fkDiPH-S)81pD4ul|fZ_{03$$ACL4`vx@P*zVZJ(ef|$k z0RE?0^p~9g=ve-x6Tm;n|G!iD-(3LyM&q;nCw>37=l+@Y|4)Mc@6`PF<4}Kl{%_;{ zot*#s58+?P`M=L5IjDao=b8TN)Bb~;|6AXX|06k1$IQz7FXa4&JG6_!Lc`alaTdds zgb@j(D1@jKQr}oifIv4$!iYW)@Xt_Dm&7F^hM&NqB$E;lB8XJzf{?kO$h;Kfilqrq z55BS%JP?aV->{mX`ya|hJ^AwnAEmy>%8sd&5&n6vrmrvOt}CPDl#VA=cTTUfnkBro z?5+WS2@_6&VXd`ev(2yZPfm!OCB)vw)jzy!d~2W37~UZ_(0-Bd&hCDoF?Th{l@?$g`u_ED0#VdumrI(cjWTlcU7Zt?Y} z(mpZ|BR9Ivd|$>OOp$e9`@%m`bc%KDeM91e%I2^m3L)(C2h765qA=00(YZNv1@z=< z)8kT|a9_DUmIAf*t~{T;o-6k#ogvR+5WF07_O3Oy9Wr{mQ7~BLlli zoDqv>;L`nsd18GHk|Q9QM52}krbN5`xQl-6k^|EL`nHs9RvWV}w%Lb0z*zG~gGU`y zATjsgINw1CIkM4gZdyqaSBskFpo(dnD@Rcy>?sGjyDrEB@(yL%0=)?%rzlXYM1lf1 z?M%lK{cF9{!;I<}_7-*9tBdExp(a2gkkq_i?*4}!COcFN@31h5j|09Mp{1;r34Z&a zsJh7S>m!qGjUAGD@#4NT-Ze^}!f2E{(I14ets%_2MZDJen&CjRnBQy3b!w4BNPu}T zD|Rv>&aLpP)+wZ29NdN)gE1u*Go#?L13e)Nc1tW<3ggJg`7huFh~T2Fm)XggPM2+X znb=8mb$XfWf%>=u6zW5>dMf3q;QY+~hAPe|cg7N@yyUo_Y)NS)O{Y_- zi;B{(F4BNJ>fG!npB~g`jP!M|^t$&sOG z>Y$#o4vYFMhwx!OvUa5W@ME%Ew3?#>Mq@M)3eeK03T@OlojB1aC`|8~H;SwCYaQf9dtTV<%_(CD;KzO9dMhh)4i(2&S96bA zTaY{oS@<#KCzP%61_3DuZ_OawbkaKU``XryYQ~H~DjYRdh)mQ#u0>_+x-%ni}v}Duv_N_0mcdAnhv?n8w{X_?)Yp15N7hI7(+nS zzCz<11S4OPR)6}?Whv}1YyEEBehk-GqwSo{_b&$XZNVN(;~aqJW&UgG~vJ;~A}AYLM=SmEWHi z%vn=-&GzPl{g(K*V@!{Zds@!M2eYGS^TrELNtIUAt?|j9sbrbCkqBwAQ#`TAu`v(q z(E}DRH3XTj?BUEx3N|0tcGK3t&%*-c{78*fScL$(l1G9`15JeNcv8FDw@2*ZAj}W z4*dy-E79fXG?c|55=&7vrB1Tu!IeE&<;N8vzz6n4C{j9#CA&w6irYARtbXerbDq>R zbZqWmL^r&=o4uQx#W?$LEZ0I~yG3ZQVEv&R2OuMO`CNeOJ!|LteBS*WD0FLU?#PgO zm(}mj#ma_M;a_BSM>_?(X0^dfl^|`R=kpL5BBZ?8$0|gJ0cU8?+%A19Zx7(-)X9&~ zfp0Dv+P=-?zN3~rgcmdU{3z`Cl4|ogO9iHJ5Mq5oIx}-c38mr@m?%7^d~P&*!4s@m zT)vcA1ITY$o1~2iP7}|k<6Okr%>G}--L9-qA88+j5b-+9g`~u!C-;7fF`4P<3MRB9 z#Nn)mCCA!>jw(0u$4Pp^Zf%Z3$3*^gY`oz@B>O2t!sd5!b;9be$v{Uvcdk$-Np7kF z8Z8iz8L&*+=5$}EL8nXa^Q4YIgW=~0wrb_5Axhoh(*!v2B|(DpQoVFD1OffUkfO zql4<^XLGxQT`%J)@ZZwFAytp3)Wbh^9!kzfV_!^A@3mkSS>QGJzz?&!_@P0F7R%*Z zxtLB6W(pu)be+_tzx9L(5QS9k`xuAWdW}*ths_nE7tm^NIG|z13c{Ha%VMs~1crDR z$A-;#Wh5HJSi|pLx!7;}+w&qf_qcPl=jv2G*VeQhLZ7Txo3{w4w6a|v53uhtJ3igg zgY&8(*G|0Cm+zi?G|gZo!8!M26Fr+v?fYe+S8==@RIqF6PufPJc4V26XA@wWG)_lO z6ccKv#nGvgei;=7*NgaaK=F^=BhMMXm&ZNMJlU|LS+A%bt~VYpP2q2L-S_x$$m|H= zz*b=o8MUj#eC=FGXQp}BA9G-Xz90u$Rl_GyE$9Ev@3B1oS$CMs8!}7Kkslhu2v=Eo zWUq3%zpUaKSXwIDPc=NJg2Y>T6qYBoFxl%o?WkGa%=MW-;xE`fE05CV%cA! zxg<(ub7D2Q`E#a$Dg?d@&WS%ADQ#C7)kOB(#c>*3Hx141Gsu<*OM)x!!bWoJPsJk`BDI$k$0KWWaY*G=6 zaM?Fs>NtnsXR6@19`<=n+SLMk%Qp5et9}Qzmi=DX^ncLi&;ozbhSzNSI_)s}GW_uU z6i4=aK-~1vbpX3n0Cr`EYFNH#PaG^QU_>)B)76v1sVby1Vc?<=vW{ZIS~5ain{CTU zC!si~=it?MGGm%4iJFO-=-?kM3Dh!DwG2P_?mHA$sx+T7AkR?kVgy<4BxTTW7kN8b z+F*{x^?UDvefq z_C`cMwXkMIt}t3LVsvE3*erUMR%KGdK@aqH4U~QMWrjRNe~TLeSE^UeY*_K2bxEO zn>9+P#FKtbyx6+!s1c+>rAe9hQ-hULV2*8ZNF1CC+H zpjPE1V42Y*E(aacG zK<^yVLnTYyCCP(`hMwu%W8}{Gb6V%UV2G|r34ixcx60aKjAH5L7iG<8{%QYIkW^}? zpDouXWtka_c5{P~X)M<3&7tW?VV^T?1!gp0&DE&Vsp65=S)O0_DNmENpV(^jJNg`W zZggWm>xH{RMe5TJ$-etfN%oI35wuMsk}ZD)UoV*MmpMN4CrOSbX1L^54z*6kadjJjC+&DeP7aGS+d_~P9f67BsAczFUk zQhr1em>E8V!@L|EFZ|FLMIXiJqMJg{Cj6ys;}I zu+}CqMzFS@id;OtU}Dzrqz*=Pu*8HSlR|=6exPJlVM5_TVMF0Vp-(}djbiA4uO(-_ z8HBn=Jb*ZecyPmnW%IjDcH6+ltks0oh1G`DhgF}|9-spNJ_1|}@Gjtq&!Nww&!x|& z&yLRx1iN1}gzzxpO^Ably@0!bvw(NMO~1;m=4f&5Fp_O(y+EBnn?RGm6o@x^-dT3H zzE)O3FKWSSdf;}qKp*}@s*Ap$>t-Yai`7cI>sUt`%hubR3+`tBM_XhjGS`r*2%oJJ;S1<$-cJIM zfFQ@cs<6Y@|U*IKbeIB_4vp1_`@UFQAk%K5U@Qv*gk4v ze~+UfQ=80@tdgw%*bd~NVgJd;*r>&R;BW6Bc9nG_j-N5~i`RsLJv5JplvAup_=JbF zaM=qOQI=ql#0;hzS4)EXffTZmK(}XXA;JejiIKP~ugE3RUrip;860=rU1&|85%*QH zm{882;6W`;AFE2T)?kfCUdwo%l5QP;KU8UKIDyd|#rnq(4+Uprr|ZKX>uEWbO#ko` zXa{`Gh9`3l+^P51GbQqHtD-Z&UGU{8af}Rr6hqp8yB;oyzDr7z_)7lwNI>z|8@0Fq zJINpPMO8(?BYP}?*B`+h(lLLO%L3i2UIPg|D`rzX>kYORqMtJYvpc%qlos$XHw>i1 zg>k_=gDsISY2Qg!%p#=pDev*?1zZkJ@s})KPB!>(J6c>X zDlR*IaoRui&Gbn*bvhx@E`zyxAs?qeVDt+iVI$Ra5?Td=T8o8A)JCDl$v7bman1)1 z26;V)929Nlr_sH=06Xj`@5!dPteF>zxIUz}l*tiDRvrF9xm3U0P~IO#w<3UlOu<4Q zkj|txw0rLbvn#0j+(R$=W+Ssa?jQ@$OMx{hvJZ)J$sZG*cWjn0H|g|Lz(ZX|@AC`z zx1^kii2F%Xru&O=CO|3`rM0D>(S)Vz7_ewqw>o9WRM}%Sx8?i8)V0(_r{?f1hC;)i zl-FymM?#T>wJ2^QUVt}Sj1H;ReLb5YuaaqR_NhoYMZa)s?iBMycfM($M;1)xb5n+)wbsaM9{VTJXeKMS+|pHIQEllsWSN^VMtmX?T)3=X4|Gi!u_$<90^vqcc*%izJ%?&@mSt2=9n8Me)DmfTJ~J`FR+ zNu;X-;^`WaO?Bhl-O0{|4+iO3BB zFf^S*fp|1%p$`Ltn5$!PO7beloX8(p0z?QiM0xmY9F##f0v(p6&-D7v8Ewu%s+#J9nT>LrINDL)fykGhhqk za2aK4TF7eLpPoSc#N>!aN&xmuFgmmWp{>92#kqxLYD#n)&T&yrb>g=AK9Sjr4E+Is z<6lS;2=GFFM}J>Zp1Oh?F4@tLFs$W=-&aU%Ex}>FmMTeQV8RwcL>&y z(^8^V{*$CIMP2MLJW0`#Tk8>3w=7Flgr?BL!Ni|=R-UQF)usj(HtJV<`eNrAsv*Oy zoq0kfH%bUtbLNs+`$gatk|a)q@k%QbDpdy!^Yrmsscx%Jep2~Tn~a* zs}HxP!PJpmuR!CVq^zjS4rN5A7BpF3d#ehQw_-Q|x-fZ5-!ctYh=P*Hj zR1VY9tfSP!D6H6pAXJz-2M@kRJ0W{mOhq;kQ$~R!D8&qblkq1i*Gvj>85pTRV53^YZCqC{ZasGUsG z3$ha}wlQ$#xT|4l=eV%N#o7#@f<=>=PEB4m3WgFLQ>P&G%z=vHnJHZDvQoS{d31XM z#RG%CaD^PmkANa3-dzfF4~QF>lmThHg5C#G^xts|^S5K8-~ue^LSiqDRrfroFx7HE zFFX1J`3u}~i0;rflb&>QPtJ9x=RYRf>C zJ~r3p^Omdd6#1!jrrJxOkHGZCY-a2+W|*D_(Ec?&QoRLkr}@4j!=;!Gy+;dO zoVu+EVU4eLweN*y;O4F+;ZPS8urJ*H*~BdESO~owA_p;gsh3P$bR*YV{%T)MpIMcrDaq zRjmD4G;g3Yr}v@kh1y^N;|+wpJ(uHq2`QE2O*N7vrp7P)rRvZtYo$Gn*lplS-i1Cb zJk&7i28!9AnY@()+ZfJ(iqz3DMt|Hxfb*0blnF-q>V!hK=K*EoMmdKbTVI@g&J>~D zHA+3jU>5llnC)nIKZydQaU-ziS@CceYqWvJ>z)oE!!4(a#URHU+`QAVLbLD0l1ND>X zx_?`&4}W0ZI(mPC={OHMFUKM_GK4STrr0ap^6p$H%Hp1XTs5%=q~}`Nv*!Bq=T>gz zDn$wJaTEVcfOTv)VlF{*0~2w$lO`eb_hukU1Qw(|Dw>9g*aK@23*t~1Gi+-a)KZ-D z$jn$cr9rp3zZspr^7m~h#`m>W26EB@S+IbB$%Q=-T*w3 zgB?eQlGP2r%2++v0xp;w{&uW}<}9?E*1jdb@L>jeYx98xegj(p z(i@98nwC=(B3$8|$%|)lpw#m6Id7gGUZUF;e5rGvBhTWfC*v3m|G>%CrUl>TsN2aF~ zd*j(?DT5aDE|Y*h130A*R#?#oBlOu)CtS%nD0bp+GT|vBn#PYTn30die9+gQK?V|S_g{_Iid87S|r!qF!r~~3Tk1VmmaqCJ<@5SNS5h#XrJm-q-*Gj z)Tz>zfqc#_6oGcHjl9r(FtEy5XrL|D2`tq6==ZZSWC~`>l4sQ{tXyjXIw;~y0a?}M zT4FZkfMilOvNnT-k)lL~ZMOj#FocE(miIysnGc*T|XXXymBZ&dxnKcja92mu!q~nCLv69eTFTLWA{chT3^O_hyJv zdarsHQa1GnNzq+*qHo)wj;YP#^1G6M6oflNOQ>yQnRsnMGTjF)k59P^ic#W&FH8ok zEalGDKCA_;hkfu{DRS@o$53+zbY;D@sdmV_vh|i{jVrvo${u?yMnm%Ryf~KGhAh41 zMca-E^S{Xi_0$5Uwkbwab|uiI1bOc~HBd!8LAe;);%>CxjqPi?FqFeylKuIj26;Vn zst3^AyJFMjjB>SmBPh`UiQ>Ho={|D%`~uITjoQs(Ht?6G#8-bdCR&BeGUAGIx&1Yl z@8^KyyVyG(FuOAsVz)il{c^(g2QFnteryO!itwgpcnMM)@foUdswhCt&XM{gZ{J+3 zbM)sIt3;=UcxUr0i6`#}y6^yH(vi6G>hPb3-2-vLg0Z_1!!{RFqL;TL)!D5sf-K|M zuiSm>1dl|18lm{E#&|86nxweRN^7Z=F2pbTNDsFjYR^?En&y!#@|S&S$XU{acXD43 ziIEr7eL}I%J6g6U-FAmd%3EEIm+XvpaANQL{f{TT{3mkPUqR`Xq~0zpLa)HYS5|al zSA#Lmi*ZeNBbdqQBcw&-u4j_|_h`h{Si|IEUeY5Y?&Y&1i2GZ?u`Fb+9U1M7Y?^M7 zamwVbhSaVpaa*#tNW`R0LL!_AI7Ryy#J}Y1P@ZukNrWZx-thn=z>R!|^)IdNR|nA8Wgho^YPCf0W>G z8I{DoVer(4X6nr0y3FTg^rMZ;6(WE`U}O@acCSam6r32|C=M=WYes4npOvaX{uZEr z5jc1R_-x?fsX%-UJ@&%M7Mqy6VBafw%R6~JNvaVDLmhg!L+7Gn#vnYlzK1U@%>rzi zxhUSfeBVlpS=J7n*x9OH%vr8>M(i^(4mRPvUybY|KWv?Mk4P zlpr!s6quOOZrDz&oh|>9vi6*|B(I-8G%ap5m8c{!4$C0cEG{HGt{{Ob$KVDiuYOqq zt!1P)X(U?bHqWHCTjWRXkzjmF!g!8>6JI;y?_dRaIYCik;-+$9)Pw}#czQ9T9otzd zF){e~l)Mob<8(%SBwMrg_3U zX*_{xodoh^DIBT$@^Rn*M=c|fd%@x?lSJ`kDXcVVsUjl*Df>a^JF~D1qBI@{a&f=` zYzYy;LwmvT=Q~8uLaMez^4t=m91??u0-}4lBh56vp%c|A1rcO%J@c06RrqN%n$jcR z*~v7iL5P=G3^5^j`mngzSRzlAoP^6COM-EXVseCp1qxzv`?hkUY6Q`QBMhQQB2u|T zv2ln#7Z74d-IS0w2CT$43H`E2&^UVH!_~>Qv(rvFAo7gTajRVX0nQfBvNDoMmG{L z@vE+Y@lQ1|#%?wKP!vfB4B<`!AS#F#5Pu*S99}fPwrcl1G>?z34DS1`iteuGq>xW+ zU1#jUzlwJK!-&Mj@U0;Ehe+t#SM-0E7Zm=Fyr6)gnXZzfysp)^KG@3o+gI{;8`5{6 z@1JZ)Of26vBo@XWEOgA@F+oPU|1KuTM$h`sG9-rYSNUfd663cj>91r+^nXij{Yy;n z-(^S)tn~j=OfYWZJ0=L@bHyPTwUOf&(51*HLQRbn{9$4N&l7qQBgEs2=khkdyq1s4 zqm8uYLGl=Ws*&P#qkXZwSRGQyvf9hBX$idx)wJm%2f5KS)|l)3n=$^VZQU&c9fM*n zQuXk~`qDz{oWe<%Q){Q^PP-gzEiG-?6Qx0Y>B;of4{)3S=+{(W(By)vS>cfpKvwPJ zAE%*@+Dpw*?d~^rUzz&E^4Tb?!xGaT=0h+oG?13zQTnIuV{c>KUh<{N4~uf>KQxRG zar~DmblNfeyNoDIbA7lUazJEFXFT#uerv0J+H|-C@ziWRO{%7qE11s%6hh;AmH>^<F}S{=`D&jl3az z%|5J5JqD)ZCFatB^~#6$BIZ=fV45u8VlTo+ zfbZ?=Opq0Js`rf+!rBLtycyf!ICyABcY2$*8DD`F9(`N=8EDhbc$+xcx9*zV#)Z^{ z3C97hEaknU!Z_G3*s+Z&$L7B&ovW_El*C6p) zC9F?Fhw|b9EpGePqT>%ycGFcQT!VhcYR=>^tv8=v*Uz}OyKxC|xVw>FMLSt?(db+w zQkqZ{BgD#lAi(j2g^AMKXYULGbZrP{oR#;ND~9)yq18qWU#1!xM6Kt&i7hnr{a(fO zi2{*qoIWpX^h3d?sjOzJjm>6fVo)8S8$SyZ9Be>-Gc%E$OKbS`S-)XbaV_)H_04&r zY8YyQ+T3bh9n24MU(|kJpr&otTc2*k`zuQwoK{<5CfXkUlaOvChwg-KdZ&Ahy^?_U zIw0`(Xplr5*mPu8mtJlrqac5%Bv2tPRq|?(vJo?1!{Z;EKND@6Q!J2^c_Jddot-hZ zzJ@A-=?Ijp>tcnVaQE7XDt=P8jotKiTs@@x3ER^fe4XgYJW2B;)gY9_lHVKS2}Bv< z46zQh5l3zV@*&J@z%u)}xe3Uw=6Hhl`|QuVXjd;z7F_E}W{2TnloUI-_yA&J6&m$b z>K@kW6vq}#OMO*Q*Hx;sdyU8t$yhx{jN2@44FdDz#CpQ z-61ceM{>V6C9FX#^KK0|}iSLW&>;uk`o2myYhGTttD#+ss~0W2BNy z8~nnXtwvZlWvwmAL5_Jz^ih$)j_Efhf)EXh?Z}tOa+b19*|T;7yv*dMv)T?TeP`Pj z;0+)NdTjF-XRN;;Z2t~mFnlZJ{!veTlM(-S^_1X$)Kk(nhF0>fmU`9}|EQ(@#Vkq3 z!uDND(ftj#V5R@TM)%)}C0W?M^?Ls$mi(6F{ZlRVJ?^hdslT5?{<)O;7TEpo&HqOt zp8kIn3xj{GwP5j!LOxI9?>+(=m|j4f{!tkN@A6y>TjPUAiZJ3b zReD|Ttly9S+}8OmLF-}{VXv}GaL?){X9zmER$tb>HX=|Ec(KT~Ge3)Ps&-Q5V!w+D zH$`**oPYoOw}CVvPUmFCu~p~|HfcUcpc$48>oDhchu0KH$HJ%z*yTXg)cqCv zzzbXH=QmEe{>afYIIl2E*T35F|BsBsU+w9?EK|Pg-T$mo{sVaJPamcK?Zzr&2islRERqJTx zHFgCZooc#=>h4n`zy+I+n^)0SPjBPZQ)=#44Ubyx6%9*TZxSL7I*?~??im>?UmPsn?265$b4ehe6`{%4;E)`u3DeM5TSe%Ls0U}laSZ06tS7$9*b3r_y^-buq@Ce{@M-l#->QLucoPu1A81fLQJ+b!mmS}lWG?Gn$ zq&&!b29;kg5_U=U!Ujo0=Ez&3?+__7$SQ$ZgHxlUhAbIf6t>d4!P~J`3Sd~OfOi%G zOWDRV(8p89zs4u~rRqT{d{eyr7^;0XvTv3!e%JZ0^mO`#={8vYxcI>yp{El|IwOC~ zdVIJwEqV{fyu;WIW8Dvat&PDnH`3m}IlwjAIjVaJINbz&;1iX5%yrLteuO?Ft^&4~ zv}^PZi_;5!<9sbjGT1)l(M#TJ-aOTONALV@@Lty+)-OC9gmX+^k9z^K;RPR=BRtcU ztDV=C{l$l`&zTwAcEiaUB)$8DNgO{=ZqPe^G444Izl?eg_^4VL_q{@UsI|YDph>zm z^!D=H^0wF1?~U-KxEtDE)$LqQ+lkHOCsYMg)eO(+ujM2H(Wwqvd9H{l*IZt%!YN)ZnvPY5NOZHaVfJLNs0^?;vSHoNH^lz$fgB?8}u48+VfR;nQu^?NojLsWL~H6{nXc4*nP)IEj1?$Y)~F^TQ3K3!t1uqlEV0L=l%+$wKZ zl>UWdY6tdDO{qRl=-oNiR$QuAS~qNNpU)t}(5IiRH3*UK2!>Vw-d$+1#YcGrn6G<267KoV4Bic+$Ll3M~~tJf~eYJSIDDvvMFm zPj>DWkK)gmuk^{N^iz?8Yiq@<5Y9Lj3vmNSs_EYdHAh-Q`gsq%{eex>@7+gOO*rVA z2lfHO#gfNz&9xky^f@Kv>q43;aHWeq*>g~hKRMhu{5Xb}z)A6jz7l>*KmBOsfbMdC z)t#nT?FV;H`E3{kq3mXMQV41Pam5kSf%-tp^~kCgflCJC&J4f%Okhfkh;d*xy}41H z72UVV3T?X!b^q(waa@P8ycqaUxtmoo`v{iDlUwB)ivx^01EB$WI!k9mjUzg=*MlI! z;kQ7AXv_)qJ@~Yf->4#lZ4O?Qe+?I}Hh%_fpT>Mdbk1P^9zuyyw%cP;_LW7saJUWH zC)agDAKXGNi~{u=r;&`VhoBxFvO#noX1vcMB5TL&G;uS2{&WxfBq4)VD8*ixYz|pi z1N5&FRoX5$EyaSS-1hSN{O00m`IxMvv&j)BLqn)ZHp~-KxV8y1x-_{+unws**`neE zC@q>j6|lTsFlGHV1pB5T)+Y0jI08B6mz*B%vK&ql9{IgMM!KAgI7oE4FrJkMHZeL; zaw&Pe4jF&5FsMMVBn0=GP2z~XhcY$5uA0pQWIbcD&h*_k@=8Ds24su?Fm>k+guV+t z{&HLKCmViuP8eq@!G`4q)@%w2fguHQpF#k%y4K#IYR8|25c-zM-VwY|un?&B&8OIU zPRg9IKL!sOpMUF5l&N8#&e1b?3I@`(H}^Hr1!;uqvBMMdhMzFfz3gI07a{r?Fqvf8 zl-t-n2ibD2E?>$zx3YgI*AJ?6MUU%Vceg*#ojuAorcl6GX{@&zJ6U7>Ue|$!%4g?j z7Y>oEG|SjX6V2qpj{t0!kkWs%A(Ze0CaW=x1gN81dR5bZR!f%qLHV;xtn$~k*$?WA zEd)KWB?9HAYnIg>#;6?(wGL|g`5gpOB+}SCUJNT!N5et0ISnpv&sREREUga9aT=&c zX!kIv=o47c0lc|WB7usONRYxeGNBR3#d?{&4&g6E0-3Z3j)4zA05bSyU>mQVcj7!BDJlKkZ-avMh!i!N zWoq$`fR+;2f_kAt==!UoQ~L_huGA+hObZMXo0mY+UJ09k<8*=UuUS&oqL#;*_kkmJS5*ar`~ zx!D{~Co!O@vuM8 zLf_h16_yx&Tvwm^bqYc7^z}v3eCZn8SG{Cak_T8!GBCU=;62N?gfm|nQ`RLJ%U4wPT zBE~{6N2Qmd8QTFBEZM~zSc9q*sL}2rLkT&s!x$%v^{e_*6@X&O1T9Ao_p^X8P;@5& zD7~j3#wd#6ZBW6`+yW0FH&UdAZ|jlnhYX-=h```!T%(syxr8=woFWGOL`pr5!Bo0- zw?k%^`s04l%^Me|H^9A*66xpg#t5|l7OOmbnD}SNmZC@8YiNpl=1X1#-KwDz@!njO zxTf*V3LJg#elQC7!rrJh!xxFW_>1{NTdnhf_xy1**|$_y?lXP2$ZREBugaszk12v- zMpiKl)yHJlMV{%(L>jt?&U976z`1@=_9iJyHRtNf&mHgDY&+;Q&UC>Zw z%kaDw#1hBq?x%^w1J;pJ0%09wOKt528;Q8!%eq+A+8<(VPLIz^Zhh+4otacfB5{eZ7cj*N zGV?^bHUqk08R=oJn342arxmFc2*%-V`EFc{#mE^}8A^t_j^W9L#c5O*B9>xM&b`k@ zKzOn>8|cQd%y)Id<3>=2v)~p7o38a=XqcXMS);}p#;V451^ByHGG3($K}YAQedER* zr!PiTo|x)YT1J&kTR94;AwY)YaJf8|P*n@Q#}wwBN_&HQtlIM@F?)w>E>T?ax>F_< zDFu_M2gZ|2@4ll%B@M7dz!heGx7idBZWpzna|f0-?MkBul~e}P*p#Cc+btw#5n62C z?{%&EXIUZMXkRnO3(H=cDso*L-XE8e8Sa;z>)=aJp!tH2>X;ub*ZxTl8&4mkmuxU+ z75)#0e!o&=*grQxW=XkFvE@szWHw7$G`$s^1RfG@$tRlmb7*&n#|v(S7Dm~GnWD$= zojIyrQ(l=C&Q7IYq#08kG+R|A#;#KZD|*Tpxb?k8##4D8^jp~)P|PX-Qm_RZq|7ZA z8_h8*CPH%dY~54Ql`%q72I)b51**P?9+Qjlsj~;>N|oJ3#l)0>*RWtjW*^eg444p} zPD<^_lF-l@SI+BIR5SlN3){T7S#+HhKAEWWxUIeW|rbiX)zM3bVO&yz>oPRY>biSk15x3x?_b@b0@D(|}Hcg3m|Wheoczs?~?R-pvm zBp+nB*DRti;tH75I{?7PrT|;sm;%Yrmiz%Tw2XR$F9r5ks8}(CVVfB}_;AN`Z2~c$ z6$iu-dfE&dP(dV%+l8w=xq+eG^Km>U&8-e0M=k#D`xbtV3X&4*G7_N6v3Jv+g=XhL zVcl4$Ho+{-gEkyK+GLjNYF(^tU*cKXu-c%?rQFmy)=bWj+*D#iw$~c5#;_o?s}9)31;>a6+4@y{7|?PnkHQ!y0dit0+GS>)0}zS-=&F}0H? zKeCPGoW;~?8RlfGwxfVOBc)=AoY}srU6Q8Zl1l9}Rgzer?iqWU33~faY`7J;HH@`* z34nrcyYuhT3{%#M;r?7=1M6^cT)!ri@McW<_79zFj7*TOH)$ycg)_EsR8%rce4_GY zibA!HMG~&1@AhDaQOEjjLR$gcv_w-Hbo#5+Dm2{URNU{Zt^160$(YJ+^!Ly(Z4UW^Uw(DT3mX1~6hkFG+>8ka9Z44R=* zv(G*X%a#Q(OE!!OiJ3JRKSoCfoOrPw+fm8EOcrE`{c6tZRy7>qGz~(SP~A2nJ<Wf0(DNYBw_#-QmD-H@yx5aU=QEdS`lHy9Zvf zRzJ{8HKNk0wpxSln{xa(I7}iF+FzF zh`3H1Q(lTfnv7d!G=0?RKJXqPEZ*N$y3PfBX6 z?6I>C5*_3aNJ7b|opWSDk)Frun%Ik0Zy&8s<8@hQnEqDB($nYsgNPfXh8X=Q`n=X++4X9BrCTBIj82(z31}mccVsQ6xAN!C`_)+V^-~#sJa=K#LDn#L zN|oRX@yo>Yjl}2L%dN*u>}5sZE6pnT9s}EEtZq&VOqS^P;R~mV1;cJ`c5b7ELM2z~ ziW#f_;m-va4v_CqijeG)h`n(>zd9LiQ_-G-i96%a#U%03BN<=IJ(>I>64@kZi@h(S za=E`8F!ArDG|I`Ye)YIFxy!Yda+;--Hp(Qy7CtLeD}to4hNBOz5}wmP_rg$4_)w*WB0AbvovxYzgm?|qpH5KgEEXlEL)^~t;&n;KZpWT%;Y&N@z8 z21^$OkxyE5F4kH??;8>8iqNg$dsmYs8R|=#DF2JHcMh`U+qy){wr$(4Q?_l}u2ZLM zow9A)wr$(CU0uI>Z^!G7?|ZLb|FOrMGgoHD$`u(qV(+=e7(U5P_-;8SmupF|Y76*W z*G6of%`K**UF-WW^KQ#;eM6hSGx)%D8|kjPI z`|rZ>y^VmFZ=H}U*{eaO8#WdiZ_m!T*mR~_uWhd)zCAuXKlWd+Bdw;hP05Gw-lkS! zJ>Ubnx4@1EG*%QSV?cF*Uiqp=VzD!sw5dFVhenLYjL)hSj|%vo=S0<~jw7bcytQc+0n+BJ=^`4gHYAJ#xN45&EoueyO_D`QLTJMHE!flYi(ibIN%Tb%C zN4M7A-oY9EDC=jWrlmp3C8_LZ-U7c^$$g{H|Hbd~*ca3FiFaVM{-DjkRm=KD)y4Ah zKQOQ&B(DzgG86kItg>8ujrb+iOlJ(}9d?7C|^(x^wVK zYisLxgiIE^9uMv}a#P0ZEce9wp_L{akK4!9xDe_09WhKX>XE|~VqQD8z;CObd)gD* z**DgHvi?gZRm$dZs}w9DlXm$HO ze_A-~u;-)->uc}z&`8MjskIb%>y;u&2Wr#@QJ@B5C{I;CRM&E6alz2!Ck}mlAqg-R z<@5)@{oYA<-Qdfq#YQPGsG854wMadm8&^{jTW(DC z8pj)ytiXMgrBmsTSWbd>xR;T?hF!iO7mA1HM^?!eUR+zYqHIfsqN+wz#^|C$fi@L! zYPXi_97B0<8e$cU)a!8ib7PvrKe|e1q|21XWOm9sPi3+SNhOj9RK3^)a^O&m(?e~Q zLybq8^9J+8WKEk5;|<%+ea=fHLS&s142fwR3?ECer2$oxs-UTFKqCv}F_erFb9+vs z73$x)nIz9Uc=(?b~+F!-rCYAG^TG*i}S6EWC!*)C0C0-9d`+LLjgFN)&)#dc(Manf6RjD~s zw>ZEF73Ef|Eu71qn>f}j!2nk;jx17A+6NfatoxUM6J+=bbPWJ6XvSfivz%Fo$?w-& zR&>oE7FDxI=*1wFtY*!~RWUGwH~Zn;IAJg!|LsoiEOWP55V~T1*R^PODvVFPx7XLb zznX19j;azv|9$;Mb_?ROr?a}ed$q$~9X|d;=w!WY#1RcSn5Nb<(UW-%RXL)#U-*;e zsnlg6&sNd_*XKiJ7i1O0noX(iWbBz$fmS{6djyV3Pv0l5Z-3GKP7BLybQJWqM_+2I zPX)`@t<$^_)d8v{AFFe%2$riuF~lWr^=L72pNDZ=y}-}~-+Vb29sy=lUyPDO>1YEH z@7#@CZ>)YthdrY3G6t^y3z}{p0iy>q4iG5j;Fm=JfB>^uC}gVtVw`mOAsFu}lQyj@ zU3mA4_ebT+qZ{{=;pFVp-Agy;)%snh-iNjEc!2cUp_$x%`mOs$h#mLGqxyc&)$^Xu z-|pwHOJO@U@r-W(_xcsR8J_3vyEBx!ld#tSnAk@^;(+}C61 zx=o!xt+S*9#$A6u;s$6DUk(2J1LygA_|pE{>FVg*wR-fcw#!{*-fo(=myYyzaMBCa z2qGF0`o*1Ntrvxn9GH=^RiDYSg=?Y%f;u6kG_Tkj{Fm}pI^s<$6+ zjM%@iiuH%#ZMlD+&dYh{o9>a(I!yv^w;s1YM|y%^G^5D28CrHbb_iY^;f6bF1P|8I z*hsw1W%$f@+sIEfvafX?UDeq+>K%VIeWDG2MujyTT$h~k{m3B1(tCcKOJ?)D{&bTu}*B#dY*Iw_+YWuaLguqE>3)>^Q)C!Wb@REfX}$e;#9fG_e=Sb?b9G0Xns=F4N43`xLh>*JHfAg^8b|=p-yL`^;LGTY; zg|r-05~Y>J^s)7k?LUa#(9(VP7Ihl6;OSv+Bl6*8hhF!X_;9#hJa>ETg~d;llfwsz z@!s!y-E!}5hlLQh{r+an5+|ARdczD`E!??xzj*~JlKZIIAoxw`SP52s9&;OWs()nz zD!H{_ZWtkFLDn#B*0ukpeD9FNqgStrJhPGHIO9fQB{;dGF@I22UstE?>6)aDpb!!J-FpT(N>0vcwSiZqc&e^e*&d62@) ztTn{!qX*t1F(8_u~tWj+`%3QyI73D-gX`2UyBrsed(x~p9D}AoPsryz% z+aG%+!tvS^X*Jw_qcK5LQGC6{A(?6kC@KEk7ut{AQjgQmdQ_+`jIpL@fNG#!4iTU^ zI7U^Sc;Q)|5)TiLp=RbXCo`n6%q2V$tf$5w~4e8v6G@O)jf4RYCi0eb*8^pW8kU-ED<# zda^E2icD}((D@aH_gj&d2{bw*BlG_T#t|`-$Z*#TC?t zX8Ws8FPWi=?NJTVpWn8C#Dx#s<;KMX-UgnanmqS^rLx~e@Y-?GTDJ28ktN%uu^e=VJdT0o-!Bs7|;$M;(4=V3(n2v&wV4`K9Q>U zOW(T1TaZ{WjFGAgKY~q=vi!#s@zcIwkHZ&!X3$^7eoCVKH@s3H2ESKp)()4NNi&2*&$&^K!*nXL#W_~`(sEjW))t$Te=e9jDrkvY zSym<8)z|YV4D9E4(u4$mU!za7$C&q4@9XV)V*ID}A^aJCzb3`GIOibxNr(R;!9B^m zrv(OhfwuoZkewvQirNeO&$&liq7JCDDp%R0IL?Xb+D1?f*!LGmQ@Naf@WRVSHSdxn zIYzRZ!}m1zUjG0>9?QFL5ez;$X8Ot{MoukVMqLPw-sX_Lw>Cj zo}Rmz5dHHUf?cV5XoA^#l)L=d-6?ynlG|jvfnC{q76agS@ZuaV{9igK(*$=1`0+p2 zeM5$GQ+`4t$)4!ty0P8*eas`l{fhZU9E1+_hGId2d!rN|(-0;S5al^KW&p~T|4N?GbA>=y)pvVEwX~?k+ zrxzn!04JqVfJE>TG6%4~dl2%7QG}S81O6EFU7UCQ{&p;gvlsB`g+f@n=C#lG2S^^Y(dqx> ztVH`Jkk)6h3T`eJ<9>$a2B%mWk0#-cT03Y+C6QqHN74?M;4gr(_b7Ti9%@0`bpf8J zXI1nR>-rV$oVqs^_U=U?F;Z*0!Iv(Vq2fDG;+ov?c}zv@1)-uCiO0(+R=nqW(ItLD z2QQ5V1HTJ$?pBKSWjo8J_JRvG0u@@C8%0&crU5<9{g0xmV@v$BWcx`G5T zZd|cw7e9IsBd{{HfbVE6cR`XsnZ=$0^Do>y?%%$a4Iy{P5@Ea^q*bMuTom7E7E3t| za|hle#1rPZ&=jNQqf3~bV5FD91&JV(p9GbY>a2pD4Iz@h9kNrxD`QU^W4d&?wk9x0 zbgh#L9;M!{a}?xaTu&3GxK20H&8_0HFKTj(`)%8<`{JSlcO+aN&?MGJ5l!MLK?mZ8a3eyA?G`jABKL7rD~5_U&G0Ipsl`Yk&ZtoT7=r!#y+VsT|G#T&q`S_ zZ()CKgrU4{ym1w9vdtlal@@W+-%J9Wg*;JCB5hx#=o+9u+W5WosFtlmJhq=W^GIlB zj%00ID^OaJ{8kdat4lpey|^qyyUo8$;G<;4Yz}yua?^Btk}a!b*d$#qQe$NOjUSm> zur0^RNH8hzB(L~aVT_OTcHzX}OsJE^R(4cXdb*3^VKRid5oCxo+fb5~yAN&tWC0pw z)0zGeUToh}r)0I+A${1RG$-#Q8T--von-9Hyy*}JTQy5aN*|$>FfUB18XC6*bq>?XzJ>q zUPiSN7ho8;Z9yy}<)i2*lh@n`dc8cI&SFrSr04;?`yjemR=5pG_^SooS%`)QiH%qR z#@+J~*_s|SEwh8`I=VGtOkKn(ry7&{d#SKN*=kP0=?5_zI*HyzI_i4S5ov5eyd@XM zISOVY=GbRJ{I@#P$(Lcpwx3>)E!8uTnJoXd9nUp=0tJCdtpbNfkto4)JXb;o|>&*tjGMZ z!FZE>&iFPW0v>{98*ty9ydDLVOyAxNKL^WBG?JTNCy03fkdMq2FX$2W#BEWB7qnTQmIpCwLc+&wek0V$D81+koS~k~PFY z=Qq!gGtH3|d=QrK{XoFDe&Qq`^Rb|>1|PjqwuBV0B4~Uo=m^37v$kcyTk<9G_Q*H` zQT_zDISV7oTFw+R=&)eEZNM?Tel~+Zi_Yd!`aSSZQE(t1v$`ilFt`Ew_YkQOD~t^M zA}3fEr+^7#Q1j2h`f>Vt$ND6a$yi~w=a3UiAh$~psiX`u*cbUIz`2tQ75E3&7=dAN zU>7iN>_Ot+^6V$tCvXpe0zV#nO7>E|Nb=3k(7ZWtx$FRK=g_VIt$a9ezF6;EKnxO! zn*w<>gf*yW^Vlb=WIby{JKVg+5nKB3Dt5|i;)oVqa62m)2w=oJipuGJUjjrI`rxzO zGpPQg{h>7_<_Ry5G+qP%uF?KhzNthxOlMj0mydRLcJNQVGk)e`28tFvTbL1n9v|x? zD1q(t3@DKA{RCJs&i>SXfk7i8xIy#0y$;xR;ld zr;%~p;EHqDt2^Y-^kk9W1pV?k;F`b*CWB*1g^^O|%Z)TTt%B;S6xvPB4*lV*u!5U~ z<=9wB>=ov-B`jr0w({o*OQGRmGW_!D0tuE6L|PGa!5mB)_d_;)O)~_$7X({i?mru6 zPLp?4gV{hSAX#r&@KeA#e(&p}1cNaO)#RJfQm`|TqHx?bP!*+yHA8BB@CJNE z?ihSQxQ$p}85-E&Kd#y+mHVRZe}Bwk*{ErK>JHjY|5MKJCU3gEZ|!hnL_}b?F?n<7o<-B8<8$5l|>ioF#jcsO2|#w_rnhXuKR$-jr}U1N2z zhd7wLS3QJQF}V+y>?|ylJX|9)r=g|3ci+`eQ6N>GtgNW2SStCNs(PQB6t^Ueb92V; zb73VQl^|J6{Y*RT%c8&dyHruZHbW zxm}oEFb~DKmwnA}=ua}Y(uZoff1^0&+oKHYE#hdU%x$2(wjB( z&6M;ie42^`ebhn-r$%WH(Cq)Vwr=pq9?Pd9dMR7g_7n|7$P0C}S7$xURP{2Udg3riOz{Ideq9rEEAs9;+ z(k=sk{vzRD1q~7*z8Pqs6C`|iA5x?>ez6jD0ULXsoK>_#!kk|pOFx&sPz_uKSP#)2 z8NCAF0M`faf$_y&l?_D}mUWOWrr!p=Uj+I?YZXjzV0^9)O$Dw#%_WaTijA{Kc8rNL zNlj6$RQp#0n{>7oD^yfgwf8RnP5b6@D)#zKp|kaa!-IC57+L-jh^k!a zcZNzX5`{4-t&yb6>lYX6I?%W^U?YddUHMzcsgm)#r~yGrwV#X_+R6HaDFJhjsOwb8 zD(5ST2Q=wB8z$(H7DrhLfxdJ;ZFYrH-GLAAz)Cv*KKtzIQ*`TbVm=ppdWd8$L(W2` zZ)~?zfmsM3HW)K(fJJaGOFzLNLtMb-v*5ID4y8eJJ_C2)lMp8i2e?q0m_PtB%IFkz zw*F-WEK9BrlygY#B4g14(x`R*kg}vHwvyGjRdhXK0}>Q)pV)9k)7^}cHK2~{3@q1GNv+cEf@I$<6Nz5U}`oqd8liOT8bX>dmLY%^e#Qg zOCHEKDOa=2#%R9*^RLd*FD^8b>4*CtU(XP5#l+pJ}^F@sFgk?xWt1F6J>ODvF#nq%jVDfo=jdCnMn z>;UzEzs9BWXndyu8UQnl^G?hF@QtWufqbJSZ-yu42Z~e4Q-*g3oSBUBHHieR6UcXB z6*vL;a70Emox<)djzT7cMgR-=FkHBE9#2xZ*GoO1hWaPGq@7v%Eh>~4pLbM02oWN4 zmLY{jkG5v1m`1*4i$RY;>a$nr*UAQPYQ;OUX(uUZsXDV!9RrDSC0q1L=9$?EF)85L zXzJm!?75hCc;$sD-cvjCwN)yvcq+mw{tC43a0=GCx&>1P@04a=@k-Td9%=rw@Drzw-nkxl3kZ+LIYoVqKsbIN&@lpF7{dM^IK;?};{KV_$Gz;UTzYUTR5R6%y@~6t zC)Dq}XUYvf-QpWb=ZHSL+;_?kvbMCo%aZUu!h<4&p?rq$3@Dznin&D%0+5QpIzUN? zF$0Qd{y$()a<$1h9k8Um%HbLrQF^2JNBFzhyC%^$CLdanv+((U7Cx&HrQD>0p&EV9n$)u+LX9N@@fSr8J?f4b)D1D{%Mp-v zu`%5sPNZ|pF*5;&E$ZS`(A09Sa;p=%pmE#j?Q`Xn!n5`OM*5+)Iefp z#whEVHUNRuf`&{9Dn&8|-narHsvKAlzbg#Ln?6vpqq?+1mc!PbC;rMeNI2-Tn-In^ zKt}>|)N&Zr@w~`3=pNUxd1m-b|Es;UKO+#n(^hu<@*nhwASb-vTe~!_Tgog|zDL*s zSDnt`3df6m9QQFPXB&q!E~0;BIfuJH)#4^=5vdIjI@NG@-+&5NXfa!u% z4kEbdCozkO*tnWGMa-(|HQmxq&?lv$v{W8gW&x<=rgS0RXh$%~e|0rk;$RN|ebIDh zcW3Ag(C_GYqVa`H@5n&q3s%oiEkGp-=EhKSfd06lBEQlv;@xFa%Q)VlbH-#4%SG+E z2lJ#Je21FU_zR57(EdUXu3MyZr94I5X9aF|*P%FzBbyyubG z_cU~=7@J4gPp7xz4ZfB6Wgf}69Bs5P#}$QKaJt5F_uLq<1h&$7}ZKC$MhKLhR}>pSH9TfZ3(aDS(98RkWgVQ2keqflG8=p0C#& z@}^PN-avG@Cau9~*Bw75GtYQuQj&D~CIT`%r* zEZ@r3l_dnfMfBzRoXy|MH42DfR~Sj3(ap`S+Wd5W9|E=$sD)wIma`xulWCoTcg{JT zqcXnWEGu&h-lq(V5`0?ZaV+gG_ic3>8LLfDr_0Q&^2Q^gfW??z%~9<}7xdFA;>||m zDlR59+|+W_ib5w=kg#p`W;~@Fk*+~Cl3k4WJRp2qx$y_ux|4qBFd;dbk%migk2s+h z3d!3SL(Ap76fme$sxs;_Vx5|4@#Lfr74OpVCL;mUwfL8kg~AZi04u~aMOq@^1Nt_$ zL8j#jJJKp7#2VY3I)TPpbE!jvMXCpM^KFGOH>W~u>g0u1< zBVQSxBJbs*j-uhBWd{dx5S#FkQ}3e+T$+x2fAg|N5{LjI-{kM)a{D}=BZ|ys!o(o< zZeMs1r5_(fO!g@Y<*3M-!lFXIsZ2ylOMmU|4zu{Up&qKjeYyy2$?W5{~v`+Wd89x3wqAz8+S(ZgY**? z6sLql`ukPV08a1}LxgICS-11_I5DGo2K2D%ZTE5EpuTjx_Tcn#Cc4-|)3#*}KJWG$ zY5>)?6VjN*x%$XO*oU=&4WhGIqX;v2&Dkgmd^cS$-LyG$QX*Q-4WbXdxcBjhj1Avp ze|U0pjUJBAg_O3N$iHA&~zTWfIY7B zMgO~(?f+7jYtkVhY0*n{Kq2gf9C&;v;W-hpZH%@MwtJL$_Vp+Q5j+RuPP%f z|3{S(Mix$H0{VaR$A2cn!Tw`b_@9!QfR&N+f2A2`G#fb=>eSF-t-GpPuj8(>safY6 zxvtdC*V_EHUYF%E9d3K=>U`#WzVPzia2Uq2ihP|ad*xAiRGz%0gF+%z0W)9*N6rZh z=%faQCT1oA4Ju)tSy>$G)ty*D9f6j2sRyUvXaNSe06@djMWfSIZK?*HPA29b7+V~C z9AAaYIXZ*@sR8%`1f<~PR_DN8_gWRf#p%qbq@mII<^^DI1;khf-nhWsl(w$artvcc zfCo3j_6=M9h8ADjN{GcHu(USYvo_{)pt7i3 zAhI|x2tYL#r)XKG_scR#l@0(M+(`}I$c&)pk($}bNNI;}#?-c`%9nnBdG7P?6SX(B z+W2=^FNFMWz&Rhn)K<xGc_p!{<&V<6KlKY#2&sH zI$N@(9{S+wp6)njmnNpq(dEIBU)#8irEn8QcMNj-Y1fO^Z-$f@gQbq?iBlRNUadVJz@RPF7Xn43PcT`ko1tE-TjnqMrbkG|I$9Aj(lK+-;;UsXKV zcYH7xE+>7bnpB_oNx{*}eL_O~Le70Kx`!4PhVV}IfObq@mmxkZzrO=s^b)^h{7(6l z^bNj}WrQOI26xK~zxt&{SH`v;x1l3meemC>p)Y-S$FY4T^U3*>Q)5$8565rM@AarZ zzlW%7?auT~P3&$LL-mj?F07xr*}l-zLPG<{dqVs&aeI2k`hj;2jDhW)?BBoG30yp= z$=P!nFnogo0wPmCtx~>M*<74lzv5oDo8Bc~yR#-MyDXtW+<{~e@8cc1K*gZyLCq2; z8RrVO*^pa&^c(~H%00`~dNPHNoky*cWU(4{{DYFYj?hF$Wv&b%54(BddU;$r#!;moq&rskI>aI*AZ`5F#W}+5;?nCArT_AxqcNjK=&j0&4C* zXg)PL0tG;OE}Zlv8yqd}go>Vv%@;`MY1c}Dx%X0BIf!vJ`@#uCqw~>uBI$B-kpc7L zCVvn+W8c_q6R?b#2EPH@@8(Zo6h1%s>Np(9BEY zZ)QF+cLxKIH#HFnII*t)cOV6T56s^{)I4^`!nuc`k%Fk>J<)g>%Q=tvsc=1$Xz&kQ z38*lRsd3gHm%521@jbgK%SD2E;{-CMZL%|AvFM^ZyJDpFQn^(Et739=q)B9$dS!QP zJk$G?#8-bs6mpF~HFHyaq`yumm({+2a z*gl`zQR}lfjOLCSSX0S`)Kluu<*WQe~<}Vy(5MG*@d;=zbYuEsoCyZ6Sj8LOPaVFZ++gtDQG(~M; zy&19~B#K7Ql!ypM&@XZor>G-P5(iI^_;`t@u;(TcnL+c@Yec)G!0Ql~33*Q~Tn~Yg zs#)*S6$FecP5GIv5F+8P)?4;gzSz)u9$)iNA-J+C40*@jgK7h)1IQG!{>ik`A4uH>@O4Oh4RzqdH?m3bZ`;C5K@|H55i(;+ge_HXT4+XVNJSP~Kbx-no!)st&IO&=; zo(oa%G)Sn|G$p$~V8ZnaW{{rmjWT7?0&1vA?Oy;EBaY{6&V9i`jx_)Uc16+<7L-Ek zhhMQQt!|{9dXcnDky_=OdRUs(TC2x6EBce)Puz81^T;>NnPs;$EPuh;beD0fypaTz%d7Gd=%;2l zEg6GO7csyTu6&w3&-kqOtS(ceP7XWDtc|wfl_X;bLM3jy8Fe#%S+H`-?wq@O@c@)4 zhUcIL>;9s1f_i62J^4})tZ=N7ior~60x7>jiHiWon zT?Z*JwpqW~k#ePZ>$>J$wIJ1G#%W!o?lE2{L^>$0qJCA;3E?Dh8OrdEpi+$>-CVP;ce&80+!pFjA_`jhMRy$sXm%4eHl>R8@019bcv!o`tTb!o02A}q zYEOzM_`R~Lt&EwQ+g5InH_bXz3Je#(YDD!A%C7$0Oy^M1&N^kjX0A;-vE&`%xF07y zP&wzrW}mH=)JXhP#@-&C+=@Xm7C+c469UHWa7+ePlEjM%p(aK`w)r`0HxUaq`rm6w z{xfsOEp})|ST;tsq`uUIFXuZbgj_Yqi;{z{A?l5?#dfxHE=sx%W(k-~a3ssnLuTm# z&hX-K2Dt!rova|qs4a>}DAkMzJGd~c&@nBIDOONuKA5vpsc;_h>Bco7(D}XtY*>BL z3BcKFIk&t_pJ_kNSv`)TP+ z#9yFCHL$|_>F=rQfzFm*Ffao=w_DP?Yhy8&^zzPu8-3bp^=D;tq4hdh$D5a*^1^es z$1P=Qz3-a7nO8H{w*!K(!Yx3iU3Oj2!}ee#Y+%g>k4B-3qmQ^2FK8nR;W1AL{mF58 ze)yR7_um%7`+4fICj~nY$2Jh~>ShRnqh09Nc_uvh)YlMU!}qEaACC%pA)j1vsjA+?_%YM20;mm23(Wx0^v9d7{wTOiQiR>V7j{)*0#Q*qc|riCtB?qy^HaEpG+gqhzcYdT~6Y`(~hLuV(9%`IuzE7@-P(2saKbqvI2pgC;~#8^M6bm zv}}9=*%jh?&zO(HG_PHcHx_mHj`@`7P0V;QNZypLmm1G1RMw|XuJ$ZeBpC${Rx+JL zH#LE9aWghpUoh2zPv=m(bebcj2Hz+?TQuCK-2}ybIQjCgZkXWf-zrB=G51+LlLDsyppJ5v|kB0vIihd_SZ%o z^;a(xCCZrw4HE+!S+#FKngvV#T($yf(`sJG1pao3yt6AIw-0Hy^T=GW|Kg`7;!-^? z^-bb41)EJ%4dG(11|XDE02ln0$-KaI@4x#TEbl|Qd+f-^q%!oF#t z;^<)XOd7W6XKKBg*JNc)T!u`xoKsAqqsgA>H@)*SQ(swIIA{qGT>M?oCzF*w6U=rs zpebxj7%;WNTgT46gb|P~^z|!{_spEY^=7XwCEv_FoX>0yhVSIkRZ+jBbOGt(0Y=jzTQp;`V*d{5$AY;0V@X@|7d1|Q(pDFPp1crp8R84^h zp6`9Dy)1k5fztpl@j#3CR&ikV-FWx!gVndTxxvu=`W6Lnp5tO_+3}sgnzW~MT}eQQ zO5Ys-gs|g<3aXmppP+SJu{PIU@{%ir$X`fdgP<#8+TRh>l--xtU!Zdq5+J-|60uG; zg+Du5bW?VB1jC`c;=|pn@@x0`7w<` zC78S%y^g*&LydIq-Yka4!bjPvYS46kdOORt=317p#VDS>_TEEiYm_>s_C@MGq}gqL zCjs8vIDLlq6eA!)q8clc!g^b!^ErocDUXYPx5Uxf_)?5TPv;Vgk|5;?ReKZTVnS`E zJ0*-Ur=SwqSfm;Nu-J(39xlvu9aR%fi+SB5Rs&GSI91gIdz8M&+# zEN7wsBw+ptDEd@U&Y{XN8QG46!0bm5%7m$uaAI9h2_}HyEsMu7PUlaz)yLG+8u8xH zhBR60V#zcw$W?%M8l>Vpf{>-TQXKq>N<9B&K_%!b4=w*GNrT8YozP5>U=K)jJUMj( zX}Qd903{;U$#A1LXHjZOK7|EB?Dr>k;SI5l|1xIp3pWF|5YM7nn3*_$4nEf#RDl(6 zyQVjLzSX+aH-RQ)Kt5T$P?BId@z>iE`pIg@7rnYLYJVehzVtOih#vq*&h+D?xm49;no- z?DGRfO2v-P;co&aP<)4**p=b~rxy(J6{xyb@D}bG0UU$}_ z|9BlM+$$a$@Q519H}qjgHxA3>O*AxrBJA^MJPm(dHsJO+nvIOpG;RXEj}7Nj@c?j{ z%E8Esa zCtq1}A-FMkIMDxI<#A3cEpH}9cYEb=rUKVPwzSeXwASEge$)VskT-8Hs^F#)RQxw8 zLhfx`?OlgL5pb!0idk8?-es7erhj_D=XG7*AVpBUC427*wUkK)q$VxFLLRBN?Au5^ z=y%}pL=2(${4c!^fd&SM&xATSdQ~A~bOCf{9i>Ge5hO^Qnx#pEVU93qb!Jzlrz?nq zD~3>;ox@(hj16!4jo}hUO=N5ap1@NgUKh_vd1j$#cgX9)Y(rdO9<1il+Ib5kiICrl zeq_3;m7Wl>t6uns{@tJk-rl2+vp_MGHMAOk9sTT_+PLE%w-;V0x&yMeYYt@tg9MO| zY3@R|W4p8Y6V=XH0i+0FuGK`%O_231;Ha#tUJJe(0yZfw1mzz5k|d6_Cso^7$8XNO zfv`D~j0ha)PgvmzNuKC{RK=8@wntX9+eO0=P$;Fvl@!|5Kb2Qp$?6nZaSfM$ZO_&l zMgX$Iq3YIKmi?i>i~6e5Zj2giA&{o*C7NyQpR^ahNNVn8jL~|H5%ib()CgM&ZCF=2 zR0O^T8~=1Z;4EyEpN^NrOvZ0|Ok$IR087@JJO~qvLoS}pa&)tcYlwgj!QHxcfn)nI2#quHT2L2o5_~2Y+ zrFYyui?h*vgm_@#^mI3~8Hhh+4pl-%>64dB+S_-TO~jzWY{qRdRu;V1KFksQW9;~0 zCQLDkDEF^1FdE&+ythYW?s%?&+jK>QJ-70Ns{aRTI?|t$l|GH}{dqZXHF4iwLez2KFlRY;*P z1$K@1J6q3GH>{n0ElPwq;!DpOfl7s!Cg}0?Sh3LIl_cW*us?S)#drP|-xysT{(S3_ zbLT9I3H{!^%B+5u21*AF$X~f^+t@Wg>COqo&k;LF59nY1*N>w*gy$AQLMkC)DIM_k zq5zbB8AJ#UmRqXO(c;8-*5{{lu}EFxVDIdz{VD<{y3H^a_!sDA0|O^@qAy(O#kOm9 zfA4Ux?8k-1%kIRHijotI9LQ_bNqx9tios}(SxQTp$F0{pg5{CFX*%k^TJh1W^pB?? z@7-p`xQ1@B4h6_frIx>Y5jxN#*Hv`sHR2Vq*J5712-PChFBN^{cy9-MM(=Zma*W?g zv&<^Jsf&L4y78{RXlKG+Qhjl4lro)hZ5ZC_0Nq*oBmInuW*0whUTel~Q9>ewy(NvP zrR`yp)}jRnP;4RCWxnc|Epw-7qQfT7O))T&To^P4ZL~$(Ccdk-wW9+U;KS+j&92El z+cqNn-zjGIGua&_*?PrSdM>^9lm|$n$HSMq^0n?xuURKE=gVUn$8=*M|E7>VfblT| z=VM&N?lxqK-t08zeHepVzo`s=bTyF1waCw5Ewi@z+3#Fc4I;8N;lr09QhKOq8ngl- zSKDPnG8^=M?(mPm4+0GIRalJMyZ+g^I88;-lGbnq$96WO=nXXQUMoMzEo@H5Ci?vW zcoNbO`b5y@tRoDwrxSZJaR9O8aLpg`yDh2|2tvKQP~E|Kd+GtOcN@Ui&pSAYVa!^a z{6>32mA$KGG8SF>BPxnL_Y8!yi`b{B602MyNY?-7%g^*HK;`)1S)0GCWtn7N`>U>KYrr!wxg)};Ar`>^hFUB*gYGU?Al^KFWeQjst#p+2K0=WtD&4h|WvnOMsR4)zVSj z!mEi7wEqXm&@Be>l@w+w{B3E%mo<^iiH(^0c04qGbA0Yefe|UgdL{c6IFwQ$ur}Vn zCP%Pn9el!y%`Q^@r#P?3ChppuF>qA~;#NvP$`?cUlBuurNY3IXU5Ke#WpbakjC_$` zPo7$(cO@KBcFDnjHm1zg-j<@I#A3fQkz~@UGSbSo6MPreCsjk3otO5HifcmqXI{*s>0R8&oO5a*!8?0exnYiJrz!5;fIT*i;U($6DS(MNIG( za9ji@_8bIxz=%RNP!qt&QRsILt{}*^db+Euz*VwO>|=Jy?Jt#*wG7_J?&qcHPvADr z{a&h)ng=6|J(gt(8o17+p+NrwFF?@0mYBTssOGa;8RK+BCR|qLLRLk0f46y$|M~YO zL%&!ufJH18K6+=Hw}{k7wH3?{Xd6F-6vLbwRLnQN*X9=xXfIoG9iI!ekR+dZYlkA6 z%_lGPr@zlcF#;(;@Ex0hQQL*pQJxRiOFmAIm$Jz(l}^!V;B4W!S+n@a<2qxo5-X~u zJ)ypx9m)xY1Vjd^_6Pqj?=p6cikIZ0xHp>*1CP??f5d&wo;()ho3dj*FPeUn={>Vn3L?$d^sIYOb$FS`$4tv{m9g* zlQhe$o=MEq0W<|-9Hk_@C_fu*Yo7l)xEiGtNF^qAdr0Tq-;-HOP}z7j0XAX zJ2)|yFbhj&v}NTq`7_YbPN{Y~IV?EyVTHWsL9nIy0wh5}`SdYrMl2fPJn@={dv!90 z$d~Fu#88n-kmVaoJ6F!7M`ha3w;xW{RlDV(_imj;{=C6XjDGx;;fBhi7*Mf%6-Ae! zfjutQlC=(=IVsxt-)nMXe%qBmvYrpCg`ZuGUJS(mf%)Y$(h~^eGx?R0&loj~Y~R^AfPOhMt&aO?Oe;>0oK6NAvwi zH3m6jpiyPJ(&f(<;s)!*j|g#kBvJjyCxio)G z9(xX>;O9^U@*_txAJ*^Do#1+WEIHi+cC^#b847en_U-kjAIf3mfly=JL+KKJ_XYWp z5V}om?WUwS1-g?#RF_#CuO+|J1|{*#W4RdH^bZWHDV1jFekjn}9@hx`JQ+)^kGD-P?%WWX06s>3z=-O ztw`QhGvFx5f8JOWjsd)kfod8Unyeml@zA)qH97b(3Rx{|GiQd;bDuG})f~eK{fdUg zx#;SqSEOzrftw+ti5v7tVccyE>j|m8>(40K&1v-{WZRdVedP*zel|h%49`cG^s! zPMsXZC8dJOKzuY`>XRR=$INEts4^JpJk#eh86vuRc=2#4 z`D9yE%ADE!(08nqor0Epw?*1ZCr6hagvJEs@sJTGLnzN!*e1Y#bkwW}g1ix*yrgrj zq+;3=iM67;#j6#XBFk}L$-*9&ldpSC1uZf_<{H&N_;};sinh8=x2)*3ZM_a=Y;5yn4`uvVYbTFJ3HlmZ4 zA+i;ILYO#8&+#v#2yWS*`oo>kplhRD?eiQjfgb2BG%`RelfAjkKF^0=!glt4m=q}y zE`2ed_w26h1zP-!B4b|sJ&a&-#E~@l2lX;JG9_WNLEQ;o6;A=gxR*r%6h8goM9=vq z=z`2Sli>VsH9M@&k7LX>%`TNUSFb;JDCcHo%qljx2j`foK4(aQ;h`52C#jI7zWma> zjZ;B1m3Z;UQXiB)OmmpZLT%Zg$)sF%OKzkDVAL+uy%=(NY{MZ#Bt_RSGeg0K!!5-S z;RquToOo*7$&bJGJyKjs_V)o7Xz6`6rF-=|x`1KSOF%g_h(sxr43kZ-FEsbAr$X#v z5IGgs^ey#wsNRzzY&MPy*+Bhmt!9=S-NOTcJD8K1?cLzEpMn?W7C;tDMK0>OgSEY( z49cLE2N5J?$rBW#!Vra;-_G*fpuc=uoNlRRoG_NgkwzdPmEla!1dupWin^pV4J^mZ zLf>!{el>kM45n9yQbaxic>a^(UHE8pAn~@5+VdCPzd-g#VF2(ke zJm3p9FLFFsizGY7uODuXTEku0J&VSze&i0q+LYZDTnrKoF{>R^Ym+=d?I>+Z3$eJx zrz#ZUd?bRaprMXZJDEdx=)+bz8>>iJKQrxIquZ>-CqZ2%i4XnJ(<_&&1|z6uX!}i% zDK60&gil{Q6C$Oh&wk}W*s$dn==0=qUKQ^=FESAr}zq!d_G!)7IoR2x}L|& z0&0-Np8C5_NkfWi)}O}Q-~uLgvR@gyJXJ zbY8=aP~$|0qDdi}&VrJ&q#hU*HlvHM7p&n&wNe|)M=KR6oS28R;b1~gVE z^v|SDtlpilfFneKNVFv`{$WP?ei5SeUsK`Lvy%{au2)2xd;tv1L{65jA{tl5u40db0P(7PMkcCJI%={@clVWw zK2qrSD9Wi6Pun+c$%)nvazRgYr{fGG{UP3LuiqHqC&bXMU$^~*>Rte3u!At=?~Y}% zM8e2=7$r41Y~Ce>Y^VEhbaVPJEE0Mcb`N>X!XMa74Oe8am;F3?p3(c?mUsB>0ED#C zQryhlLqru)R%HRPhYwkMK39_c;Oj;+$1&?o+?n_9eP}6SylJ?^ChU?Q0)ZP(gFTZoX^UB!|D9J%48F z9>He_hQ7c2C4rRsUbQY`9$(^Kaol42wkr4I)%q#6)8cnAzNS*uE?7g+Wach~W&wAH zr?a?Ug-&6vtRNm@>VduEpR&ZB-^_jO4pMRV?G_>fL)nb?)yiC=IytjGZ76lt)5HTL zBJ!Y#r$A2c$#^yezw%;xsK&7`_3 z7KwM6Q%gWPVuPUguEo_MZ^=Lj#AKYuCoRzLs+lWBFgy+aw0U7!>c4xmm+iz#D zMd8X=C&xuLvD0&;^Txwz2}GHuLnGhmn$)>LUsC){G5PnR#qAqUREk*M7Mvcesm8~3 z6vw_F7{i;I_={08UetZ3%e6Qytbm@2XL5Oh=Jkt`BFZ0$xbnmcD0MQd!$sOQL~+M% zl5+RuDT_lh$a% zVT?XWfX-|e3j&m5`br1VG+Tl!z>5!-^kWK0aT|7TF4-0GWK-IR@zw12LMc!mwGaw0 zwVDyK5TR$r7$q|#W-S+h+v_jHQdk(^)M7OAZv|ZaKvt5va=3g_@GTy|k3x?^uzg9~ zXksVNL>!-iimnrf{WeR1<^dM%*V$By7$phh9VQ)th}&v(>?X)QP#}rSCP2>B{CfcE z<;?0?ci>y_5>rh0oe!%F)fPvV>q^@cXA!5zhVN&J1{6;FHoRzY4VQDcm1VH&x)p7c z3p0yMxy+-k5K3AXhLmzdVY>G{0m=FJz-_g}G;@@Mk2~?I_@qVqQLvLJWn{FeyUsla zI8gM@u$|26d<9i|TfKN_xuHke%i#be6$1Bo*4&)yGUYy(OLYStKoQi7;_mffh?NL_ zHbd~lEJicX=8c^^$UnVLYk;x{xcF-9Fl&X_&e4rh__PE{Ti{@E?=U7S>%~Oa`RKOP z?wdu!4A++~H>kSS8jw4M0w<9Tti@|D{nDz;Oa{D$V@T{LxewEOE#=?DX0bl;0{oqGka894_L zQ=Q_D=oeVkMi<|4}&Cj0qH~8ArIy`#)>yQrf>0W3DjC1 zyPQWaw=r<4+4w^r3l@8VsJ$fn+#D6ud%)l9NV1~|L;xYc;h4y$ta^B5H63lnb__6l z6Yyc7)fbjsZ2?S9n%B*$!DTOg+8`NGSC~QDx1Gkcz|#BLA2I*DW17=gl=>A`%0Mm@ z3LTP`+nJv*W>e+c^N`E$g&7hIl0spU@Wj5YnQ2CD^(asnSrPv(jpQKHuGyb=6+?Ny z^r@ADB4=yL#aJ>SGFA{0HZRr9KU@)_kAnhyP;JX(6SQ_V;1!CE2?zQ^u<3vTKWi6d?#c0Zm~&qhTUGeq7@Z1nf? zjO$=!4!w7?S49p>k=9n^S5ro_9y27@!NZe1^pi>np@KnBVl~COLxdbt$4}N@rkybRR#I${SdwhnyyVOH_!N&M8&w`yxjkj7{h_ zGwFPjbr^E81w8t_x+PtH8dJ&s`@|EF5FhJr`QM(Pags1d^4~KD&}^<&;)4}a)-v$! zV#1oyy9qp~(5GT>Xq^&*LjJD9vCE3HV}R~D3Vc^NU%e7$Du~{mY(*f=A{}7`LcS?H zb||etB&S`=DMGdv80@At9>mzyW?YcAkK-|ncjrcA+^D0GXIw>Y{nFS=E9K=M>hsV6 z$M#7SIFIztY8kKc`+I490X5iuttQ6Wf+2vlz3pUls(7Fv;P6SV*YG`b0hK-@@!0?evD(stZG>?jS>$XW0=!p}xAGQjE9i*=EeV&W9f zk^%f?;9Lu~D#+&+g|dr69HHbr zL8k}|SzN`y1*;BN#N=G?4FP@^B*&~Ma{*SE*2+HC_tM7jZc?Y(9O&xw>n0A10~1qA zAWMC}sOzJ_@>P24n@=V2a$IV|Xh9j5rS1Dle+?GX6sW~E18=Tzd3Dy?M#fdWCv6?e z?JuYb(FTVc2D^wZ6B0>07>hzI6m7m6hz@=ocXO0Y)aqh^N00*q#w`Db(5YEOoV)rG z2e9z~4kyQZk~CmKf7$3mk$7xGh7M5;LiO*hlFqecx0q_1HQ^!094V*O+CR=7dwcm% zK|&94kPGwNDBzil3=GFt)}Dun%UXa6lb0Ys!4}!{{vr6_=F2<)qiH!}Tp+o0%J`l; z)b9#A&B%}vve~p0?Y=sFcY|HZZMX?4fQ^mVIIKH1w;bP5yTH|scm{C&l$7_34Xa$>g~gov&QUWfkTB`@`!}RAPT~25-vXA8m&m1E7$ReKY5$f5 zn=Dv&DkakDWSQ>z@OAicW?Cb{o%>Tc+oj89ntg2{yrCr%16?x1^z$cl1Gub~0q&91 zPC4c;RpxsBZzt}96D3AAJJsO)L-V>}uXv)CMgo?;PkLus_zuY)K2YBzcx#zoHz?N; zYm0R~K1a-*3eiQE2#4z5<2_aCa2x4xOm5B_XPfn4ep&9e2xhQl7F?k_>ExbRXP59b zNM|mY9SzzZ=LM1jltK|gGjsUx>WM2@9`5niRbt+p@M8({L`B6OpmFhxmSvh${nsIf z_$jKg4ed))m6b~boM`pZ1Z{Qp58G8tTZ=Z9W6y?4CT9iTkuA){r{o13(6cSvR7y16 z!tOr{Y2xrb!d3_QJl1J{qnZf4>dC5M*0j!y5DT?i3A>Y4fk2HdE;6iK`2a$7Y+hV& zk1Q~oeHg;)ZKsFW8WNxVU@K(~k(rzKk73Jf%>2PQ!yU3Pa?(5G2NoHQ0|J;sY9`bo z^F-Hnct?dgfz9cd%imfZL-GpiO^sKUr1{6p26i8Y$han|_g}3)gTjG}=D`joJ}ez( zn9@)Mpr8}|SdGUkMQOd8k{t))Y~heFZln94!v+t2m>V*%hvwsHDm)R=c=(4(g_jA` zha9FVCDH63cg|?R#OjZ6vc&`j9*O7$*(Gkp2k-k1Zep7xS=J3Oh+0~iiEMGX%m{0p z5ub3V#GHX+*7u{Do=e2qj%-9xWfxM=PKKG14C{~&c7An|ZBp?cB~~o=@+wrA#cg)x zrzjHBPsMeU*^PX$e}Y87=$(t+P!$r<=J2b6-NZ=|WiJmWIA83Z(SJhvmc zra$gMV#>C>y4ehZ<7G}l9jdVet44{pViPGWyvVLMNsG5U6~`PwM%#_Nl*kt`^{HFJ z)}z{Z8XlHC<@|iHzHZ{4Qo9uKCXtoemFn;t71@EN@0rY5$8y#l%8$^9?x#4>eMl=F zC2f1q?ter#_gOIr&rXUsCQs{-B41ev@)m4!j%<4Ef95D=)+PEwo{KzLU9Z3!y(=Xn z)(my}=()bJ7DIOL^F!!(_K34ehda1jOU-x-Yd34&+RBa9#$RQrgikVAUP%WJCWP+! z2Cs1e(@s@A=jVtE+3HycT0M2>GSwHWhZU=t!qYrvKi#Z3?il(ay?D&uuf7EaG%9f; zm~SN0G7FppNEjVJIcI$4O5s7UT5*mb-7Mn0A-qVQTCws&Gf9mT$68@wgEeVo^sb?$ zKU-hd(;h(^fW9WW*|dP$4eyBELp5Ytb?5jO$|vpy(l~n|>|z*&;TFuB`AqkjII$p6 zy6RU~5N9Kn+huX33R61q-!aysh~U2@JBJ)%+$Y7dzoYYuuWp;UjK~_tSEXFUbE9qu zfQRVaq1S8$X@IOv8YwGZH&AI{Fivwm?H)nozBF8qgrTwg5isnkZvWXLTfSQpzCst4 zdd3ymrdh_OC8&>>q!uamckez8BZ ztO_zPz(9THQ=G?x%$|G3!ykX#^vC9p;Qbuo_o_WO(bD9C-tttW(H=US(=|;z-L>bqgfERZZ#zoK;d}h z{oK-^zFeZGaD5XfCm%f0wwTpsme*kYx2#w07j{OF2->kJY$J6Kk6jfNmVZI%uJx)307>=XD{h`1g^ZNj!*Cv|*<}9ae^VFaZs>0C> zlNc()c3h~#Tcml8Hce@|lk=Fv(T-r03*8-QUH!bU&-;X-)$bD^crhB3OZUGv4zT*c zE*(SfbFX|o@mzM7_iEQ)&dS;}9m899TFFOd>>vkByW`~8!V1FC0}8l9v@ZFdTwEf* z+l7d~iOnQOrh;BD;^Fp{7RD9AO9;G9njmGH)#!B8OI!Moe&tDGewsu9*W+m0TF@F^G?xh~I4HI4s? zLgKjD*{Nj~L2OVA|FHi0;vm^Xai&QkjJ`drBYMzmXeY5FZ^TFX4q zf-W<@_ZS~J2I}d;HqhiXH-OW|(@V&lD2^@dcn&4xreVlWqFVN80E07FZr9z73Fx}^ ziKg{qt2E;GIvf)#DP-TM)~CBJgOrpDpI$^R>i550qvE`;`9siBL2bG})rZJ(yMFg< zghfnr(Iix)xI8Z*<=HMy+~jW<_>ze@2uz6g`N(>ucp_DkFzwS8t-AZZ1KM`l1^#_n zU2fRUk_%T2u!90w;`npLZo32h<-WK-9^n|$D1YcPj3ZQt7evYO^rP@MHQG8_A;_cT zQQiEy@~yW|k9h2ow@qQ&FUJ~mc1XS}(LtB8t4OU{i-ql@!C%0BUNh=;m7q$yPIQ#^ z+_!ndt8FB?Zvn806;Y*yE$ImMLM?|azPj9e*A1c`Sw`ho?eK9UNqZ@#ST&5LgKqEE z16QnxRrE^&8M1$2Ma<4H=HNMj)`IoYvJ%FAj!l2mFUHC_wdHheVLVYXLgS4oA^Z6` zfg?zFN1oa6HE6$^JG>+NUY!tR2qQD#%5;%0=cz_?HSBZ@!HqGwRotzJs49^|Pa3fN z5l5gzlh=;af^nA}oTr1=k&9Kt1PaC1)b>Ect8S%nojlqd6-l6>x!(+xBt3P4EP5PM zV_;-_wxI%d`V}olAc!8x1ny1k3BglI94+JL*?9PTMs3(-uOthw+QA= z#!bpkuj5DZ@Ve!}g;pdkarB)k(0PGf52_t|jSf=mF;jQJG>L8x;2}7!$IgNQ#hc?)R2;QUH+JcXZoz1fUpC! z^6?>-)hSzJPS1`!`kjd2I@7+PyxC}5`_+mU1-`>=ffm7za@6Q|*CGY36&ufm-lF&N zIx%S08h@^~mfjx-{IXV6dP=D|;@G60Ys^I)wv?b{Yut`;lvgz|_MF7AN|BAc{+gnS z6n_n)gvrVD1}%|)R_ZBh1!riza@2fvAh$4zA%Y{NbHVs=NY~D;aQQgw-Vaz`GFBC(RM= zb?`8h(Fh`EPGy@6>a;L~>MQ*D&svszxUViA>=Qo8g+! zP7#`upHhI|i6+rS@3dDSFm2;M8Q=3|)M>k9SsWS8Y~#j5*H%{T;^sk?|WN2mbsD zm*c`A{dWh|xaf=(D7$+J^Gbu^p%s-_^Le)JNRb9ECffMAGPkzEF}%m{sfdB*O7R^F z6;6j8L2Dn=;g9Cwd&45M8VbM~*nI`r!5>~Dg5<$xOb^yf@I^AkJYlPS5d7$fE+VIw z^{O%=3dC=o^&4W5!zKX=Sg8L6*GVSCT16l5ybiGPngsJsk)rTa|}y+1VS^oVSo zF{1u(*05

GWce|!i z)?Mh~$z$mLxz8$tD@_| z{B`6wrD1jo_&2sPSLFq=UnqdQ4JSE(pID{UgD;kXVj}s^@TpLHea{y)iJhgEyJ)>= zVm}ifY$=KCQc@o?|NfQf%5nzV9g6&)4Cqp(y<93!Zq`FhJg?{029`5VdKE8+c6AM` zi<26(Jv-Hise)GaBW(mU=ihOx7H8ri;S5d9_zrK-}hx$RIY3eoWaU$h1)&$P8*+2`aCNdWMKsqc< zw;A&6cvAv1mim>~MA~giI*~2u&TUQR(@DKla^+3v!HSWwY+Py9WFK7fS;0avM^slgu5T{ zT*T!5%S$LJDE6!$l;62_tVtc8E+bCQz>tK?(3^%>FHfu2lK{_)=9%^0d82|9gHK~L zlp-e_OA0qGEx}w_^w`Bu^);U&uQ!so#_RI47{-&;Oj%jk#Dw}nk4%3Kj2ajhlaPR5 zP&wm6=nwjWUWrfhE23e?;HIaGc*0&hp zX|s?D`>s#%7N_6r3GhgsM-TcLwcqX(-7|CE0n-bpF-!(3i?!eiK5lI@cV2ekq%po)~@}ny9*ZbdWXnt3kM4?)*m-6(xn1Y5%*K? zEVw^?DvghA3XR9BNpd(<*Znj`;tnVOIF`Dte(Y5qjm}g-GgZaw13V*S*$*q{ODv^M zXKn%QF5xXM(+Z1&Wb4%UKJzn>)0f8YOoT@F++qWBHo+P zH-3*dN#1T0&5HKrPEDbmpenO?Y>Za6$y>xh=G|&^=F`{FWc0Oi;d^&^R1Cn z4{o;K&AiJ{a=B6a%Y*_ev!Hrd7%@XO6Jt7;O)?n>2uu`TT!eA2RW#`v9=LcqnLuo=WR$;Cl33X=v|SiROcezNfX&cCwc^MTIpSLq;6InYP*MiP)u;=z%(Y22>b$ zQBg@0NVD(hiL)rcyMiTRytOT{d9_F&WUTX|21Nv^e5aBoO~YR<_mCFT=sK!!chndi zGT1w68|p%pCl-cTTjSkdnLS>7rh=Z1(|PRYsw*dyV2))PhzmT7V$F5*3{+4{C?^{) zVj`9cb9Gt2^<{wiS_&~RMD0tMSWC~cGS7^~GM}7CVFQZ}L|?v8V4}@R8*pAU&19yx zukHfQ-CUj%A|Ylymq;r8nSSnc6oU*{YseOBF~rVZN6r%BN6lBxBQzju#&r-NHs!=N zkhRfO%%B;E8-u%y(vN2CmM9?c^8-G80a2e*H)Sc%fhi8k1%fdEvS6|TY$WLdYRT58 z>v1@9r3LeX*4bhl5P6~_Jloy==l_J}yJ{iaD|_$d?c~sy8od3Ew{=Dp4AarP`5rT_{!jsrXTMkEs{CmqH*=W51TXr0 zy`)u9)#vVKrh0I60jOhNLfn-I02ty2_u-@AL^X+&(E-WsB9eP6KjoX)+FIAras&W! z$l#SVd-xWywR638F_SxGv4k$XtY0l|98^o^5y8ihg$M7VDR|xVHqBJ4)D7BwKvh-s zQ)I-Yk2r`cgd8TC5*Cmqe5CodbL8OJ$1?ppX-5sY0W~_U&&9j+qtirv7t1sm8IYC- zc;eC@ALwtr9hFm7&f;|1Xjt`=p(kcDfd>Fl6B7#ebIgfT>oqYj7+0!FS1&(5Ux=ik z^V1Y^P}e>bM`B`SCFM8kU;h&%_7#y>#J?=y^=-o(@3Xicr=dQ84 zUo2+~*F*tM23uN@!UcgbVVG}rg)R3M0T*a6RvP2TNRcOa%%76pzY~n8u5onSZNY#a z$;i>Jh!upij~JeH(Hd>fdQu>cwr+=nlDMuupDkDyl)_AKleSukt`M0b_F7#T~w0B{2ePx$!~))aseazH*+f#RpBWvNDqLK|WYz)(ODG%isp_}#R)9pnZ z;Ove)!Lfzf|IZNATO8K&c=`SqeSVBsdVx1(HAWxK65)e9B(%3Yd3}2;ib9}46HSD$ zGCvVT?;*9)1}F6*=rqQZ_o&G7E~4kpr7v^{7q=(mhlYlpyBw^nN=i!=6cr;u=(k2p z#EQr(o|picT|+zwh;4M)J#hWUvoibG>v&0htN-ApHt&H3LbjHFE*g&n{#y$mVQ|rn zzaOQiopA;Px;kI>hR?fLpgwJ_A`!J!4q%MkSbWv2_F>sam9EUC}Dg~EcFzfVg?(V|;l&b22!d~5u>W<`?c55<7Rx;zS@+PZC(W>AlbzFwx&|=K4#TDbc6YHh?ReGBwqTnF@Xo77t07t2u?w zM%wxAb;$xb5DNNmaWJgt7f*y0s#?)yh0lg(e=S09lDWUIJ|6kKBF5r;gP^Qhwfwgt z;$O^@XT@2cZ)mD0xaezM6ac&Hvxab2-vYq_A`>As^5JU8g z_gGLxQTprhwp|_sqTsY}R9;QN7*4cD0b^oQfxC>>I*vnQ8K%RQ zJ4HLO6yg!e3|macK!=in3?;VZaw)-yZRMW6l1mecLrK8g2K{5(L-u{tX@id7bR*OQ z0fDtwS8Dmv$MZE%8H(9hju;RK(}n)-AQ}k_p5AvmKR?$h0Da-xwO(sS$Gc@5jnnhdXb-RKCCV4MWvGA=W9&;0phPq*iXbgr5gx z$)Ze$&_{$1qH+-w%BnDcNYFn#KF<^Hzx9sZgl2GM+c|Nf&gvIGSU$q z(2)ay&S&#@J^v(~oyE8-O}-%g9Ptu=krH{OeiF-xz^-|S&N-wXL)qFQUz1oWNVDKjKMD_I|l{p_HfDNLw#W5sP-H$)we$9)|24IGyD zw0P0IXbZA*O?1q0NEze@1U zT)NGnuHZ|k0~stCEJZfE3}JuuKlfiZu_Mz=4>jytV4?)V9*w-NzHY`>QZm$Ys;V95 zCV+y;ybn+s{M3K@9XB-w1e`30ur7El4w~4!ggkG+X-N0j3h^@0LYzk12=KQrDzjSX zd0Z}zytJ3jNAF#P1Fqs?qMM${ud>D}GP~hdTSX+{kMHkRK6(Z)bn*x+2zaTo&u7v> z)md;~V!J(A;Bhi3Rw&B!%pN+GqpW<1$-V8@a+--Y+l8VbLslU>NWCdv^m5%BrYPzY zy`#DM$SLSpiWVhe8F2jOEsPKeKAAObuYC_$roMh@T+habl{vEKVW{Yk^BnB>PXSHr zdSwIv=H9606ZdS$;}Ae(A(ws=@T5?4OhyqT0&_gn0UM|gM1Y4!<+E0Ye>bj|;yWNT z`b2N{dJ$^4Jf0)!kySi1sEb*RNJB6?%m4LWr|F@jJ)m?K6nJ3!>(D=J3>y$94g>|V z=wVYFTzh}3H&<)M>~n(T5v{c87V%)%rW}BAx#-!*S-9W99J%{I_otgNLwHqlYnyoD zxH?4PC7Pt>L>l-s0{8VlFQnf<t4)>H1zy!*TbFkYyEYk520;s^gUI5>zUltmiw z#P(mzLS12D;ZRJhm}xO2ysWmiR#H-u*FgDeXAXAo&d$!>-kxcn{KL(m$J*Ez7<{;B zQpS?vvX9UEv_B*`OifIdNK`(4OjRz@26KJ%00CD^MP)u zUu&Bu7ea^-0LSUXcX$n|XDYO(HCz~9X>!IK23N7AL@UiZ*>fD|IFmX1!-&MsnB-xB zPkk+_7PN~-jk37m7z7o!-n85^ykx^z8>@j!QS=V{(K#j#j-r;4KW;xGd^`KC`fFMM zCt#;D{|07ZQwEc>MzqtM&1eBlFlt$g*#Oq@J>ja~Z6;l~m=Zb6hnsd+U)N#0SN*npxpDc% zuyGH;rcb?RXUlr&;~x7qBJM*vi0I*e4)Rec)K}UW$S88wm>~%mUPjf8%yE%nbUZlP zr8S7#R451q@uR2)Ph*CTPf;;E+g$2vw%f|9<|Xp>$XB@`^$27giJC?qSw;yGGdu7y zVlhR-1_r|mlfH3bQCOYUh=?i0VR_iCgz#LnFW&T}ysVaa;oH3Y*tI4_eG7%$)*uYR z-an;$2GQ^2pLIM$4a65kV;in7Q6(!_E3_<739^bA>5_btw|h6N`5C!z584=S;ja{c zU!lr~WKE2>yFB{MAGnl}q(~M!>qHi^n#@cJ{S{3vRYZ~%!hoFLHWzT-_gC3ox*fxa z%p=Qu^mnF^^6zam6ZIb-Pf2TCs*%58rGJVO4AR+F^!=z_Jbn!SW$W1TyS!^?$a!6L zQzO?&Iq{F-3^G3b@JV5hkJPKyQk1kaP2HbIXDC0%$O9M|O66Jt79ceoh|n#cBa&+* z^~J|lgXM!y?u(BAw9hgt~LR0u9;`OHJ1u1#;_5wZX zfL_Q6*I~phy6x;hVFE=&V7*g0i5OP~O{>`J17z|>JsGXgiw|A5Kr>VVh&@c0$3M-&UilJ#Vl@jECk=pD4qqGIXy!9o2MoqClJbL zZuZ5j{xX)(Fr~!0gJR1p%}4CO9JE0~gObd4>9^BDt<_?^eR)}h^rmUEgrwaq zzEmDMN8nbXX1Mcm{q$p{_a|rXeB=4wB6LgHaLeWQuho_-%Wczvu;X9^}IS9+`MDSd=+*j3A#<{-+Z|4sqpvqg?S}vamx%FIs2-pcKxz>x(8;{=#s8l0} zVA$fruE=onG@zH(vwf_`MdEcBeLdCY8XXDPW0`H%iyluLm}OD6Te`YCc{ly~6d%30 zz1gdu+?ABV<$0HCyl=c_?T)Y%eM~cQY*fz*_M=oPS(xn+o%)o&;$`wmn@*6Oae3x9 z2vI^hlrmM6*Id3C{&teemU7iE=lAfmxEuU#7f-t(T>s->7@y1L`qmD%Jv`ts&=kd` zQSsXkbG7zm+ZkUp>~QK0LaYG7ldL1{ac2H_9sgYm5+%)C23AtMqF@pB?=TQ?(Py(i z&5sljTX*@s3r>DUISfq-VmEsTUFG?u$T3;EMMW!{y-q9U*sfN3hLQ^cs*JVezJ_x2UE^m=)Vgo?LQeXE0IojtkYF9^6;|Ny*3r zJRUbhMMV+JOii^s-zM&6**XVc-lH{;a2l#&F&YjGB`6nvRcVs-+Wgbd@p3T zGl$bxp>4RNSqcl(K^Gx%8Xkk&#L05(P=hV6@NN^KL*~A|U$7Z-lYh?d$HwJh%{{;C z3%tr=znvJBBezWFmJ?H(*<^#3yAeH=w#EV$|AVN6Ve`^9rgKM3(^DP)*R`*sE6)=o zgsxSRuH!San0!*wx_oUOIB zyuADbR_^l9x}&SP{H)DPd5uZw>`M_p6E)XcK-IR`7Q$E&de8ArvHURh#}P%nC3K}V zK`eGfE-(!0$lH5O4^jvYnB!Fz6PKi*$qviA@YL*V5wD9adW}Ng%V8caF0Pk}pFe*_ z6e+?4LJ{Ztn{y<#?L`2f~&Lr9uA0vBjj$y(w_7Zx_N#AEb&@=_4+JAijAS)w%hS+%VG z2du}rOpGA?vhQ=tBMHaXX7APA(Tb;2vCx~y^LlIeC!Xrxzl2Tk{u&UJBjwy&2$g2( zftemo2;2xQ$Y7eitNg6iWQ}xcYzww$q+sCaJ?H{uzeYWH9Uy7w8LIF@xluB{ zXOy)E1FF?d6(%<~GaW~S^#81Ro#&f3m5!Nj_JCr9R_f|l0F)j}N=fk%5p;JW`HCQ^ zmN+w`HSw|=UW=#uP}b!nVyzD5nxoZ{1Y|Q@MmJp+9_(~)`y;Z*tl&v7nq>bGCp7uo z+DoZPM-%O*o3ud-l8jb@18*8aiDAgrpoE;VPGey`G)?PW>|QnXAg+6za9^Z`+vECj ze6n|EP3@=oAws^6$-4UDLDIo;snRbxQ;B6$7g#gNtwaWt=3>{1-upfmcZNJ z3i+BXRP%J{jNGUSeutBLDr}ngf0YMs|E(wzrWm^1Y(iqU9={oQU zg~PI4{Jkfan;(`CiHel@H|jwX6t;6B(%NcBu~vE=)JpF^4YYkg0CohgSXa$8Yo9_w z8$9BE1fyZYwN4T(^^}r${cYTe;YfLLt3VU2n|*gIY^;s3KQPH5CCWb!bxm(82RGt3 zQd3j^{{0J;V1uE;)tPDMU?^&}aG0SlaT$z<3ho^f2oWE|PsE-mP%eQ2QV2OTHj;hwf?GV>OpOzb;J;@ z5wk3#iyOSTi0WAu7=V_{{ExXNa~W?0)4rSs(}f3-k>xGh=O^}dcmLKu8I)MuycIks zm$^I8da4fgHTW!8^x{q{sflCW(GmIkcdFP|BqPUl1nK;7OHS3KnW>qrU1DB!&!3Hr z+oCUGS-Woh%Vr5!qvM`gZAX?+PYz;BA71cZe0uT??ZQmW+HvJHPjtD%wf6O88r?Ua zmQwr9g$i*YmDWUtpQph$KhoIle||dQ9cOW+Bj9s}%FiipPnMfKPFJ!;o=Pj4AOWv0 zFB#5j5gn{&4@RyXfByUdmufvTq5E$~M!l=RWz=$If!2Y+0H-6{B>TyF_WNR`IU`Rr zQh$1dUv7l1?h8~-sy9nYUwh4~J~F*~1JH%5>*$ow4L08F5rtu}TaM+#QObk_*Oq5< zr`w&bv{F-3ADSjJ(?q@{RBZL?bcMDXFV9{<;N)$#R#mt)1nTTHpn7A6`)GD|2NqwatUNh9KJm@;u;i2pgP25=q(?=)F!Yt*%!z zmT-uPH=X&U)S$YWs`(?d(B%xlkL=xhJC8>awll(Oq8U%2^j88PNRB`29ki-^Wca|BgTyC@&AJ zxi?>4T)@^SKqRfv2;Th3_)y2kM$e*&CllzYX%j5m(=1Zb>Y4XulA72-SP{;DT_qt| zd3bpES*m&z_yyHPLYEy0kAqK;UCcG3g{`xTi&qx9b@MTKs?hZ4pvg0unJhuF+3NEH zxq$f()WE_*8YwjsQ&SjGwZv76dL+y@C@A6K;aJ$%b(N8|_4UxGEJK+SUc94yKh3jU zX^fg7d}(5W-E=3icEzfiiCDn>El|;Vvk)5_EAnz4!>HBb|4_)ES8sw1fYc9~C1xpy z-j2c^U=^;MA5B|*VqSR~G(E*>I*;6$t_(fw+MtP$7iq|^o!WG!lPd|q7<8gRgoh2GO+Gtm~0-?M+pjIQcy0EsGMCq zEX57ov=qv+#N+{oBrft<;*Ne_{JDspH1|T*=Y;t6WosZIkpfmd$yy)~c|DTGlqY+L zQzINgmiG@uAGRPN2%8?}o@*;pY>CT3GkrDaC>cT(da!Y&PY1|O(Q9~weIoCG14fa_ z#TCmr+Fgqb<77X1P)bJ{X2E4 zY-TGeXf7)kSS6|~dzg8gAd$OC+U-;9b6;Iii<6^ zy$Mpt@%dfp?j!@@pC0!2H-v7NYCO+3&_$lkdhm>%eiXiEe==87!yVtm-?47RA)nI@ zbLgj%4m-xe0WS`M#5b2nyVZlXo5ME7?SHuh3#jCE{x9hzK(-fr_;T0x4<<2GKDnRw z$1Q4jL^U6#*wwesV)aBwi}pZ~pM|#TZqs5@UDmSA9j_!-NeCh8DlRr9hC2lZv68+RUhQ z`JAqbG-U~&k0~MXg@@c?$Ca&#J;h6b-ay+P{;OQ&q!(%OH+W>Sg0{Dus?qt$u3Uwz zwC}HTF(r#Od4&K~1~r}J%<|%}pXa~F^yTSbWq8Fw~|XFKm5xt)3h*6w05 zTD5jp>s%}J&7mcqJVR@Iw_6_H%=X5}!`l9G!jy;o^HA#Hngw}O^6@z66H>+up{Y1Q z-rf0hzd_nNTZ0)y79ZYUd1v9Z5GKPZs{I6k+5&P*f{S)|39`ZvIh^?cGEJJigc|Uh z5yTw!br1|dMzkK-;f0I(pDM4Htba=El%sw&^9Qz%$&}ms@EK6`9~S4xkbx2tsB;)( z&dUC)cCx+G{H!7PD-!xl9{K_~5b`#_Ck%vB0)fcVPiLVZ%7WH;qj!0uH(M8zR7P=N zDNI7unU58H(U*8DLq`ACTw2lI4W%uTJgP_IV{FaPz6#GN1!QO z4fV`I%0kx-_Uvt+k2rokeb)IX7!W|1_8%qD|2o}gibx&aaXE(jx9k7+*`$N2+4f-4 z?gA3c|Fy!^HgO4w+oO4?yp>W;9X*ZkZ3r|ei^AtX1l*4oxSY3S)o^@1$yCnnZf`?d zd)FN;WiYCseivG|81T~q1sRzST0*9)n-&l#?(hFHTGjtQ0mpxSPDMePJ3~onX)P_S zkdTlC+j^2YWOVdO2!!AJiVo1!*2crdy`EDxH#hh6^i)uYaPPL|*4EZ8g&c@Xa7al> zB_$=P$rBP1)=~cXA$*(se@5V+GVrVI(M}2|DVjKuWo}-#qpNo;l|w8^73@0w;a8YC zH1Tmok9c4>12czd6TT>j5}{%Kf#;|B}8qV17D{~QAOJej>KCE2i; znU(eOmt)k-%uF^OXHF8QCo(b;8d~xT2`wMHBc-9ZGJT*XrVH)b&%pZdKUx6eI$gNO z2`ahxX=_x@_m(mo74-dRdps+in~{G!CY5-e%g#91{;xfw1!r=DnwRaxFq*9L=)$2S z;~3GxY2eJh>-6+=Pfw4wzEe-bs(&*yffz1i8iJx5S=@HIPEJlxF@ti^6U5v=fd(3B*>SS|1dU`7;QVNyi)+`{16JF^m3f(s=8@EEwOiO zcy8Lj*ZE}fKgLfP7Yn1k60n4i`&Q(K~!&nLW=w37PQh) z1E)a3#WFjPCb+-)>Aa7WRsXl+U?P2PZmwRvZEm7+CjWKN8x}fxdYx8pu0E&WSbMB7 zv22*-g#`ja!e+C9_#{Scl%S5wmOQ$us;W15EUu0E%*@P!qaGxm(6eF6(Yes&<+E;! z;&eEim`H_=>67b`xNxM$YuX~3(1 z%g*2P&A!mkPz`w(XJ>b4!0G?&{W_W5?A-OL8ZGKXH2eRt_ZDDLt^K0-QWP)<3zU=% zf`Wib=O9=}DgsgtC@Gyo4=Rd^fP#SJfJ#Y8HwXwwBi$o84U@jJy)m^nd+rppSl_N?f)_6sa(CsBVWT#3kA`3g|UInD@!_Z>WSnO4`@)ks(rTRrrENrgfl;f>s(H=(M+gnIJQa(yEj@r4 zz{YJKK8(KjfEu#t|2)>fi4vB0%p9(&2ADn79Eo=AGomDrgV1Gw&$d#BEY zh6&79y05QfKRix$)S5Orlwx+!4f|zuD?}wBurc0oYFK&ToC%wsseP(^;Y2MtLpba= zA@>>F8J`4fa{~ulx7Fgj2lmk+#jifX!+6ic>(9%6G^Go*&7o!LuRvetGKJ+TK3#di zB0Vv;U(3le>P4p$ft-iHIJ&)ct=vXQSA-^6DTcN3>;o#o^XEri*X?`zhHLMh;cG68 zS6sfHVlP^`$E;xC=)%^p|0LterEf&DeO!KyCoi84^{?|wI7-E=bmO#3sCuWebmJ_x zE5#QXJL%9lKRcz&H%9bbX-fa3-lL_Vt-D?>Pi!sXz}Jzt;0&3=qqBjwQ`;i)I0;M$ z+BUe7uot>uY~%&MqocDqN+s>Wd@KO_e)Ms4bo6x&kB>=7XZ)A(XmTY`@aN^Hihi7J z`J9wA^^Dc7cpF%(kks_~GKmgW~IV~FSy6D7SI&qBg4Q)y4#EW* zT8RPk80>H%Ha^=MH>z7Z*}|1#=rEqjSV)|X7KW3*ZuV8+IkV-<63f0xjfmaVUmb{e zO5T1NL+-wJEBwGl3S53Zf$?7DL{ql#-4Vy-1!3=r?0d)-6en!@4Ze3NLIGi8=s8r* z_%PVly3z915~vu1&#Mu@Qxghu85NkR&t0n@!RpNne(y@LijJrZyWpaG%&7bO_ge-A z`0pXUE`ZvC3^B>AVP}f2f_A8-{Qi{R8m{FqmVZT;wKms|c)`sw6tC`h0oI5mS+FUnl$cHArJ(;nArzZvM~P7O0d(8ssBk%| z6{jhdaIq_RGnk6E+~htjS2^}^I(g#pINt_H;X6EEF|!ZyOVm+);9N2c6!oFOL24$2 zqN1XZlNaAJ%7t*qNyo6?@No#`0VY~bIZqcaFRzvU`{-Bd9}0?!IK7vvBjLWd>DCxZ zQoEs0(Ze2KPA(olhT8thfZVPHNgwK`Typl77AM6VoLRAVUwb8N)OXKEVX%-pFEec? zRU#jT$14?OWN==*zy4hH+f&UquVgPcZ*pcF7-n^ki~21Wir}(+v6skN-W|%re{8cjDq+{FE2cb zIDit=&$k3df}}XnrYL0tgCXa3ISK3D3_*j!x4w5~W^eJUbx|?ME`v0CJ1eUa($0LB zXl7Q{Xueff2$C=nC{#KVsv?8~XWhMV+^Te_XxVP>Og$R*1|LurzuB1G6U-7fIVeHI zHgcnUyoYB#Db0Pl7IagQB~;bv%jL#f1t+d^ijGGG4SCbhK*qFhNo!RjUJA_2gT1gS zc{Y)(&3A|vwxaQs-isSzb;PyI7zs0_RNHsHUmrzj8z7eWMx zBhcjWP4C-MrHNS;Ru8fQ>3bOP;eFXa`k1zRwt8PMZ|;9sY->~ML*Nvzy*l~b)hjWy)WvHxk-;am zE6T29Dx$L|$CajhnV1&!^0f^eoH!6+KT2M;vuHAvcldsls*)eTR!GYpx(AnHq6=O9 zKkm3L5zyrIN3~&UTSgl=108n+D9CfTAJpF9@R)of(kA(UxU)gU0VLy{ot?TsW|Z5Y z6|1<-S(^nwLo7!k*sE-8Vw`XHL}ppxda<+4w29m(-FbZ z>Mb!Y|K-&u3|S91f(KMP~3gcCio_Hnla;iNs_TUU-O<=aE7w*n}Q3&$7ffryf#D~*7^J0Ra&!~Y? z*ZE*}8FiV(d@6cL;|miY&?*C7RoOvZdK&kIM*hOg>>da-1q(B61*+Xo$nYL27Ro2T zM;>%cd-#-Z-__hhNBM#AyS3Dl6F+ae@b+bnGw{;lmeu#~-b}o0UQ8*m20>p)%Fyop z3di?a?*8(g@#jN~PobYZpWO$91<xOf zH#zfMHf)zpNf4W^mPMcuLxH&K&8X4%@`xJTy@5T@6QxCJ{Pt3lz=eV?%`+2>dL5-t3V{@|SOyk#D^YfKUb&R|L z%j1U_o-yiF9#xM!dCz;S`cxi$YpnamVA%ck-7|Qm3)6RSF_?RoY;EU(R`_JroHH{Q zxBs)Q;HWJ<1BSEWA)&6%KV~|Z-1|jc+M`!*EWR$iQ`G-;vNhA_v9>rdt_9zqIH63H z%hrvncsm2J4r{bWeNRz^#qjzw$1e0*n3znF@Q!)PrOB)!&#ebApMf7(Wq8;EK@00P zksja@MKq`uKzdE)cNxIk=~ERe*N?j(tM zd+y1n)CwysrlyFMTkFO&o>C7=XJP`p$)EXH?Fs4U z_*^}0Jd45UkRIxcRU42cgxO%vQ#@C@KX4Gb=4$LElPidyyCO67!;{lV`R?`I2g;WE zaeHXxVC1-IwzZsvYj7kwV4;@+37;daPfApPO&NF6DBIvrK#L*>-z$#iU{!nR>9`gm z7&hT6SF(FunbJd*Xj8m5rUj@)?KGt8+>Aby+6mjPX5`hNbOIQ4h*pj{2?b@cIaNY& zu%!{LP#)FjOAjy+ja}ww7qp#gw%o{^QNy*cViKyqs!j#~k7m(1#wgfk9? zpg~s_@tsyxZ43$;?Z_hM;N`m!B%fH{Z`*Mzh*MyZ*q@6;2z+e!G4H6L>4@Ca2G!Yc z0ZinPmJcdgpZ1)KPj%1gX@|LVn;3lx4d}=cVgn%Ve244%kP#wM+^Xr7HM7u7&a$OJ zm3G2IF1S}lnXKdIP}6Z5%m|y1Y^h0K%=|N~&eDOAR%FLL{Q{d!6D`G=O`Uaz`WO;6 z&++!MlUlquet?;Y+5BGVa0*G5tVE{F(y+2JXL}yyGs^`MZ6M?Jv*sNmET)Ho!HywX zgSU=66=DlyM$0|WL3ClKI{{3Jz#O}L;0ErkeCBbbL#rKd*Rf2$`AA=%fv4y}?Xfc9 zkvX9?&WxTzOFjo zdsHwIzNEL=^2;^&5TRGzb?tVQCEAQ}iB!4vpB^od0HiG?;DhJfoepAB z+@<-KjBXrGO&J0;c`^p$^l73#3!jVx>9e*6I1e%RyUNG$%JXb}G@#%Cz8>{`d7#Ea zW5(=(@Y+&UXUcddE=Yo@VHT?+J39L+=|p>D8vHJ2rN_7J=E}VF`c8xH+B1gI$XOFK zJ92%Pv1TC~CS|XwjGff8h~seEREwA0Nd0VGq9G?o9%=Uok&Q@P3UD}U&kha-!aw@B zMbEw*9SRtjnD1DWQrwR!reashC6}HD`(66l*x$ULvorAj8eHc!r;^(+zCFHgGae2d z#><`SQJHvGfeGHo+8UMcTGU>kL2!WQ{3-$t{$iOYyH$X}8MFkx|B#FFqkQXs-F4Da zIXbykj)w9Cw5b%%+AAkB?7aKU81|6{hz6kDqD^StcrriDF1wlPgYe$uh);U#V-`=j zbokxBXU(_tN^@u7Gtm!)~CnD0X>gEKVDT#no{GVW_SLsAoXXMG)(hYsJ+0irVlf#+s{3 zNDCxiSn_O%vpPN3aD}llD2QNSU_i^I913Eg!2Ql`zSc+!ry+KR&DB{iA<2p=s)%UB z5RuCtM3W;>Fs};}>sWSXw7lb7jOR|iV_j`k`Bd!_**X+*hP=LNg__v%bw^&ONH)gE z0t3jW15;UqLz5~$6Gk1#W9WC}x{)ms0jy<0rs|X`wNGm~pPQ*OP4K5UkA-dtb`N6| z9!8ifW>oz4tOnIOz?j?Gnv|Z|8l&Vsml$JTH9t|oPylK|%#}XNam}{Vb402%D^*GP zY%*S-gB9D2@{xClN;1g2qRl)HD8W?|S@5O)oDYn0%msyob|Bks)OHK#$)0U`e4EzyH#Rm# z?CkCAD%Y?9?XazU-yj{)r9yl|FngKpP$3{3Dvj8tFawis1?)cgQ zH^1dwIJDR|vbpF~Uq#+bZ<1=38TILT-Qs5KOF(VCty(*SbX6~pq5wv@_#%@6^Fvev z5{DWrgjKDe?H=ybF|f3eOjD@neJ7OB+ilQ%0&FW2?lV&!HtIh4jj={-E#AHBjXi0h z2#epuv@a~avq{a)Mn3e|UQY>(x!1}RlQ$Ns!tsfrmCN6rhzgJ=j}273T5m(Jt!#IO zcbTgS+C@ndx9iBnS}ul!T1|@%7+^=CWo2chrw`?tHOF(N zBv{WFY7XC3ryl9;jo?vMQBjFbj#352t>n^D>99tibQc1rZf`Ge+~yY*2^VkWkjy)P zT{o`q6^qy>IJe7fRaBAfkoY^tru@nH^e$03oT5B(@S8>6ef;d8YGi$uBsK2H<`+DG3ypf^& zdIsE|k+iX9FKkybPP8X&_c7&TnghS&wkDOVwRvDaJ-b)3nzb{C6}TtkVKw#XS#1iF z?mVxWk7G}A3Tg*wFKhRl)v*Oc=F^JIc+10qk>LCHH_29^Jixjt=wA+E-hdXkwxlPz z6{|&aK|MOT#Ly0!c$>X-(zCiE1jM~)>tMZUuZM7qoFl~`+uwsU3xJitQ@H6A!RqVM zFvVGt;Gz=B!+kw?A>SaBhZy7YqH|p;rqy>_X^CZ7vqC@=eG<8C#q2#t%0h`}xo^K9 z<1kz@dbP$hsXGDW6NdR4wYee4HO zo1vGPhvb-bifjft(~f-`c2=+OstyIJ>XcK-5Z3)Od@-iT9zAN+>?e@0b$5rY$+Gg| z+xo+Gr^`4#j@Ph;MBmxoxE9KzAqbb-7FoYjV`$#Gni#+VfMu}1-+d-Az>T;LJke}0 zU!WGY1`1b7Zl_V0eMh6+p7m)Y9!~{Z6MlM)jm`q;RL6`_C#K+@LeiXy@>Of?@^Uid z^)O-hv6`SdK|Xk;c_ZC16tOlP?>!xqi1nA*;SgX|q>Vw414!9mi?~c{r)`|k6aI{f zshlU~a=F47dU{F*=OT{u3~B82VzMfx@mnM0Z&6D#2%1TGIumLm%K=d8QwAyV8BiR? znqtR^ms7P=tSQC-mOf}Ej|3`0BfzoGXg$sbsk--WwO)*&aE3VP$1VIM#R5_LwJUiYDIh=*38|4Fl8D<1z=lHpZI@1avAz z&F1oJ8zcnvaz6s10FvXt|AmEao3r*H2?!sD5SAqm>zUm%#69DG(q?)oO4S@B3UT@C z-t$O$y=ZH;^(3a^An__BOR$C!3A-rzJIO9b7;@w;(^A_0y)Kcg1KS(EU4V9?5Rq;ek_N+^( zDTUrKr5?IdYA~WB6u{V&p@$WLV?`3F0c;t6k?yOAUkjEXEkAnAz%{AzDITx4hh33& zpkBK<-rWlK;7klDvrDKe1;loa`gG^hc%4dS6k_I{R-bkLh8^z2xJ1Ymt!__Ph>M8< zD=`(IF2JqE7we!Xk7}QC0+BetUMWRbwn7s}TTwehfz4&*z9+q@;~GtK2NrvIt1QS(psG|hLgso!UPa!M z$_a7I1$gFxVMjEc(GUxWd$E@Fbb8w6os7>zsS_Qxwk04H*6dOgFf`eS8vv{UcDsm( z2yc1V!|qhz`LYooDmR-CvD?WGG2E%VB%I^@q$L4B&uaU$@cH$$8=> zlq&C9R+Z#z7D`xt$tYzV5euxR{QNf?W|zKt&XL&Mrk(&xQ$@M|=P+-1RHG^2m=z#b zCg(xJGm`%+3XoYJafxbhbJ}`*2AI2UbmfE886a6aXb2twxh%MX_LNFjPw^O^*`!Eb zwP?}L>r(!&fzU#$Op8;!9hHppPEGCikm5tF^*jn1=y}&6`mRSOMun~lyz)DiMD zCw5IcwV&`%Dd5P4-@+pmFl&G!X$8lW=N)tDTgxzvXLYtUfCq6#=gBmqELx?rN=iH| zDx9tRV|~P)2&L@ZDZ#|2XJ*DpIh(t>x+Zob0Mi3FI8h%|g&$c0VqtG#MMr=C@l_?A zd;0k{1GzOd#+POeVcJT8Bm|^{gJJge_U-0*Po-y2i|`J#5_aubxD9UZT%YMV_2A!Q0bCPpN3=JFhs&39(%~cbSm&%6Y(PrF)P|ot zZZa%b)@dj7jEYmqa83h98CV#Ilsv(Yo1d{=IutX4N9NEt2#%c1p*iog)8dI;-d zgWp^P05AkRCML~b1en{vQ-Cri zY2ge)J}#}%K~aOxW<`=)<4T<;LK`CUZ8^P~TI9_;14|1pZ}_#Dx{+pH+W(f3@y(yH zK7?IsIK^=!q15{=3t;Ydc8!;Z({lw9@LD7`Mc`L(9obe=>gUie70_sVI%w$ZHWgZU zjFh+KC>{_u>&#^q4_iWq#K_?r`bJJYtVLv#x2D5ex!!8;F#`7e?XdujSV>2apbh*v zUx0zOK7DHVfx1DT z($j&6$Cp&%9Gx5^ZaY^&zP2(IutgYu15~E@I?h;U%m_oaULLRiXHZ~?;yEHXYaHS_ zKK^6W;LcX43KR9Y>p@Ef954QH{sR)k+Mo+t*`eVT|-lTRU@kJRnM$&Oz59zpR z(v@6frbK*R3L}{GU|?%W-)TZfYkC%J^1$e$!oi{n$rUqoaA}3g2QY^6+|l9T@=vCV zp%$}40Dui(?9I^Ay3yq2X*E_K#7ZN6i6Sif%t`qPH6}RYM((NH4PG3GNNb2?1H1_P zkipsHf%tiNxal08w5a>=!a$yebath<|DYyZ3mt2l#gHiFFCXCHivyy6MAcTs zQkP_Sn^(({@xWLNqVQOW?KT18cEB~rnm@oUo^yv zA?Ze*xmGn zm%DdKE*df!4-K_qMh1H^3IiRnw)6wjF`QofqVgMY$jVFppPiwHETUrG2*mmfEIjY4BDFjS$%KJ`<#_Ma#t`JfYNmlWr|(x2 zV)3_l*G~wJn%`!!q|F*AX*V{piqs<=9h&;vhM$3#$LSu-om7(2TDC|6%9o9}7trOI zb{-v3{wL~ne5va!R+g~myy8E;u7YHY6p3$+^WC7M&3^ZVhhnP+3QcSBI zsNc2h)>P%a6lKOC?>{#u0t9T2fvXs~l7|y-qQq#}iQ$ZHz1yheX^BeT`Wyqrk?kB; z4Y{3elK?fA?kMV>z0#(klY`bY5|1t!uIgfr`xaa40!04U43x zM(0~LemKNLAb|49R`%BHMtvYl)e%h|MO;#Tcfn+ML5~l<{%Ea<^D|%4VOtKWdr*m$%>^uXy}^bd{|oqp61$tiEu zw4jCV9Udm6X&4b&?0seC^Ce70xRZn8BwgpaK?ecxo^uQ#FvrbP_G~RL+RCvSPoxN~ zO9c26sdDcriCeS1^+8I#0Tf4$A8C%)k2G_N9j#wr`+A~s?NC(-CK0w-G9;B3g7pcI zV28VoHl!!0RBCEjH_* zaXL%X=Qrh1+2OqBf~UY${AUnqNxyX*+7#Mr=|_Y|V=-3WPM=X8XWD2JoC`DPPU&kC zym=)OU+J@>=d(yKV!cq5s$q9a%Cl7E1(_9^*#$-=duX;*{>Tnbqk@p_OdH1vlQJk( zbid=5@AW zG!q+ye-Yn=3gelbI=i$)4f(BZkQQ0|vM4=B9RYVA4j-*D3QXBk+WC0b!M+P#c&cnp zii>Ge@lVG61%|gU$}L`8{{-{}>!!Hp@t=53fw%+c%c$BfDg&GXzi7lEdJxu?0XC3w z{obwZgDV{iApP|5f~fpoOcUro|RttZzrt zZ-E3``kzWMbY*u(G?53vW>I$VFief1pY>b9z#1W&6T-{=iQTqI|Fx}OALUbEHF&AQHHs9;~&uF-#gd-ufvR& zO=G)$#PZ%Vjqx(N(by8_J$}FYGXG310X3D~G?jQIH!wv7z5EyH_!~h||EB;;c&*$X zh}wF(B`PT$BpAE&f`3CMtq{WHPpr?jzvp>RR4-(N1=`WZs_Z_wTW zq@jMLPM4q8vR-qJ|j%2(|cw zJpJ>jG=aP@f_xpj;nTl@85k<7=pw$}w|Czq*b@gxCUoLy_g+Jq4`0uH?f+mB+uQ$x zg_*jH_ko58M}5@MsvvPT zvN`Ph4$96x8Od}g}GYo|=a)l~y4TwIJ5N7D|{c4E(&P#NYzqkX3ko!SId~_%M@6_|y#ZvP;77MAK|UU^(>lAj+<8I`{V1j!gYkaw?!uRK z;~*%U!*_cu)1Zi+p5Avg48?Zp)OP@hcU38np_8DWxd~!lwzlBF?gHDcX0R(Bdsbat z9b;eV;qHDHbZ5MWvWa&eMMXsoDtA~#-}Bk%K@+hEN_>Z;q&rA^%+xQqNx4z}=Zy~w z^v^R{(@86b&S#ZjAJK4Z&s>M;^M~7&uhb_da#1$9RB`keI8N!tmKN0l{Y;&7y@(#t zg`$6%-9SHQN@AxPglL$Y~K2NNIFU)I1x06F5uPG=iz2gh-E{j}+*S z6j=9xpd*m6M_HAI&zu%`?}>fR-SW*rYh6lUsugm#jdtyW5(JBBxqza(H@k9YEMFr;O}f<7)bjX&c3=Dux-HiR#5LL2fNYCY&E?AoJgw z?9omS`EA~30Q;N3RS+bG1xaj&gv%X($3;2RdnFJ~SPp;@`X7YIt+j}NIIL!`$ADI_ z|HQj5Ulc-lB8}Yi((Zh!n*r%Jz(+#`+xZ?MzxpP`(79Etwzk%z{RkKU&@k{K@7eiz zl~7Z#eiC2O?O8Aq2uA`jG~g~GkyxSzW>UyQxsmUjTG#EgJN)6R@!$w;o38$h?)%?D zS^qwn0eAjWX#4+1GkEEtK^aM1Ihupqo|jRu20=_mC@gB`FyFz}Anyubt|{5&S(XR^ zwRV-^Cr9a^%-#L(sAN%Y=X==HKpfqlay2E5hYotQOZo@DJFcy~yuU&azW3WT>kf$V zI)%t21no1r^_jG&^49Vc3ly!s`wTqJ*w z1EOrJ_BSs|AkTV!-G)@NezYGm%#d4GejmiL%Ryhq>`yDIoAszxYL865BnpW>0t$ZV zfVXA;UBi(Yl2#w;#juotDdkC0u_UN;yNxk^*yFd^H@#6}SDn@YeNhH`QhG%YyiD~m zx6$eYt=={i4)U|>KDSX~?)#*WQ?$`(DQgdfyv{fs6#>=xrPJm6EIV3?*2}CtSd1@v zQKm6C4J7Tw^J+4=@_j9+%rCT%z%B|fT3^mz%-AvfaGB~$W6pjWg@*Jy{2O5jMu6K| zI>+5bSufP>f%qOQ|xj$v=^=4szK(Zz7iQxFU0Fd!CCbhd<+ zjYb@i4hs4SM8TfEiv>Hw)-Yj6^d|@$h$R?2VE@H8{Iq(W7{G)(Vqjv>d6iudv2yAE zs@wrx2%!x9<=l4z>#vAkd@Difc>m79=c|yWR?9AsOsWb7X&PwX;_&16HNJvXW)3}c z0rk6hw!1$U8V1VL-k7uv{US}aGsQWMCS^{p4@AA-7D!+RG zdcdlFNg#fA)yKLLfi!*^QoeHe&SUBXi(LR_tVyEXqd@5wT>2^3jV;vdQLx|L?9(re zB%zs~Cjmh}5HIyJv>|Zwbc=={?y>?~+T}sG!HY z`wD?~oKV0Og8qv%{@Z2n;>R0ZhL}Cjo4>Wne^|q=gi9Iu*Ma@MO|L`F$CSOP(r_;HF_ zp3euwbT3Kbt~wx4_yd|b%J-?=%EzL5qR~bIa7#La-zEML;}_1TGv zPypRhba(qP=bs&e|()E)mGvHjCgq{EQh)m_@tn?t7l_4$v7a}0eM z{o>F3h1w_x!DtnPcbArMVxek}0>dsn`5n>}K((=b2@QY2l+*sr414`{w=ReBMEhT# zSSul%hIoD*@#Vi@Mu+Jp8uDZLpVEu})w%t#ir*=~e}p}Mg7yD=5C2c?pyLccnNckY za!6kLse6BN=d`@1LS+${nnlq~$R2K`}w*hPAZ1>hX$ph=7y@;klD>i#K* zQ&-LhmT;b1Z7sg~mR|3*usu)$5iF6iFp6z}@)zO)Gz5a!xtpEh6{4ppI-T@$k%a#a zJ^vQ~Qug!zWVPcjwB~;&8Tdz@39to598m-0SR+un%ca6~_8zEVfc9Pr|739hVG0dAzB@2;gz0eE2V!` z&J$GNmi;byHT-6sGaW@Y_7h6kh8DkA-{O9#VwxG;oO|B7dip@beDR#XgBKCTjuGa7 z8HWSTT>6#Zvs2C|pEV(Lq+{>BK*_80T+wxp_Cc9^fGfYK<4pB7gnxOcAXLr-CJm;F zypnI&V5SF*TD!`TZR_;Wj!W_PA<;On*j5|q>T^y}?9nyW-*aXABwd5wo9X=)UGr$) z=i6zKDu7~L28H18#4R+6A{@+|e7ehD0X0GVE+zcmDKAmT*e~7oPjbUQjamF_6aS3^ z{+Ef+-&VzEH~QeHEfAXn+Kgh|;t-sL6Jj9QeouVw{KP{alJY;-a@sa z+WTef-|@7T7p;TBO(hSTVXXX{VpC~idLZ+7g|A(9ttELF7~DF}wMk(X%2O2jJQoYt zJv1I=6@XI5ygElx!@f-eYq${{Wua&BE-sE@-hT}99vT`gocfgBF1_|@Y;3IZK0NyP zL7%7h;TM~82L=Xsl!1pOIVFWJg%`2lK9NnH69xr5MaV;eNw=*wjI9VR#0%lWZ(^^UQ5FqI45m6iS6OT&ufAgB(pJ7D5?fncG@0CLdjZ|p)?@mUptzNA9y&vMe z2|34zJxhf2kgS<3^+E7@mMNh_ps^gIL%inXQWnXh9Ly#KGRcuSlH?)uPSb;c zDsj&rC5?$I+)p!#AD&(06ikxQjf>=ITzHeV@a@jnH&V^m$}yj8un#7cwNahRW7^h- zdS*LGBL$I*ZtucPm>C-C+gOLU#@iI(+;AJq;_eHAcdqx@7ylSvYcmvy;enxZFhPsL z$G4U&XT0dV-0$MG5iW0e`A%lZ`xuc2MbeeHl_QR)Y;q11dEH;a9`ngbG_iGWdE>Tl zSyE1}!fH(>?AUhQrY=2)to!>VG11tvIHqYgS+!6exy{p}qOv#>cqmVvt%E-XmI13$ zZ+h13JB1b5M6Y#Qm+_`(O!-C&HRrB(`{tjJ)3N3_s~jq-miK^j-J;zdUS__WUohV$ zvox%zPj2Vei#VefVWym^+uqjFa>wG6mR1-I1N&eV@;)gxB<)Mrhq5g9_Uk&?@{2sY z5-ZcAoLExZum^))M622AdAZUhm&MKak+PH!Hs!F7RaHok9v4{URJ|PgsFszQo?JId zr);%Yb&?RkUPg~1KVr>0FC*#Ujt`XC0^a@UQHz6l6ZUQN=RI_vM0$^_Y!T{9MU{hP zx2{)tEj{qEi0~LrHIXh~DP^ei+Q}y2KggKtzhbol2>6HyAfgp-(NXk4%0+OBM8$QRpt0MFr+6!jEpK$3V5o`TtzMtLX)@QgjQCO zO!wjQi!tu_h`hEcOQnlZO`&~!^z`c|I(ek>$*luM!VFcLhSCPMKeDJtNsVNe^_iU= zBjj=)K6h9*x1iQda=S}Rchso?Ur*j{Mi2RZ%w)3o?%=c?3tvJR4k5QS(w{7fqziE} zJrgRn8=A`YCMQ~Kwdc2IsC&4Z_LL%Qe77RtqaC_uRohIJqwVC$uVz>4`QklhaaH89 zn)RWuE+A0uHe>t(uymR&`Zr3>O;1miE}i4(%r zmWm%Xi(Mz@yPY&wj%YR2gD1W=(N0RhejcV0!yEZXdpFv9Z@+9LZ^Sj+%NEc7 z{DHM-)V;1dq-t#@n0wJ-^0*G1^k`mqg!l~wD@Y&IX>jgu?lW+GFuEvAXn+~sI$5%W zMa!7WlI`fc93{8PE^cv`vp?gj*t7m+`CAgkX1l`{HdMBhk0#?c%Q(I~MlK~X^n9Y3 z1HQ)G2f?F{NTj&3QEw}Jy3&&*=k^hYQIIg{yK{=0oBK&pHIEvodBB@Ozv%q{BMd6b zh3mqjVq-a2SlCaUQV%WiMKA*bcnB2UR1RI}FRAE{WS9MH(P+g6&v^~L%c-^B4CRsW ztZ|O!bslmw)r;oM8(8R{>j4FSL1kJE7;)W3Jcjq_ez+7;Qd}y^T+hnZO=KG$=KFe!sqsqva@PfSJa>;paD>kBS*nQwxEhs>e z^dv8n_(Ev3*b$U)E7*Ewb!kacPgFJ329&-KE?*`b!161X+ES6yhA+ zZdXfk;RSuw9hYlsX3Bgq5v+OS)o)t{>%|k1HXfKd=Aey~0;<{A&Y5Ay0=u5CB^GkX zF_}=LdwKp+&lHwl?Pb#B6s!c(j4ZQv^8QU`l$n9h*?=vxXg);7ZINb>OYN)oFj=zO zf$8Gq&Rz8wQigCdeNoRin@p^7XKhkoR>^WeWR=&(F$Qh)jYEqU(z7@>QRHeQ{snj= zgs=~+xfHYE;kJmAAWnN}aGsMZu-`;&s_9zfDvHZoh%qY6A`hxD8vGubQCbHH)3(YAaY~y|?{)-11MZ|rvvqgX$L~UK|t?qTv zo8%+Tca;JjS&eiRU9uHrx``kVU-%6AE4RxJgg+sx+}`G-9(wy&p7-)XXnmRoPG$DKE)3!?i@G=4IQRH(y&Z=drLcE43QQ+XEYW9@#<;W{H zR@$_!w{%`ce%CcoA4r~BV!tSMpu6Cl7DsWqfVGUi@1-FgBF&vQKh(RJ+BlEgE_dD- ztO$BBVHejMUclZ;z_}1nk99B2xRgR^hi`wW_Y51c3l7m>kv8o=;#B-Gb=ah+;Kkg{ z>l=nQLROAlpW;A!C)_TMt$dZ!u&w;TNyBCk&P96TsAucafL+0sJ6{!_4N-kW$9chO z^@~zKAqUwWJD5fH&2Q<>9qOp!6WRpD^$>EE`MyNo;T=)8R%`tiQD*;K)a@Qqym z1B{S@W?LFaq%eA4i*=ETRaUMbh&`f$_A1aqq0xh@x+KyzC)dSGl1vKnT-WzOH~mhZ zK7BdiO47Ke%lwJuP{gjqr`Igp@F1kXN=Bq$J)B)@T)0)ghW9uQtnaSEEs8!p$7s6`LE6XMSFoicTM(fe{(%d04h zJm&qx-+y09aM@a0+LN*?5BKn>cuXg5SXau9`l+RjE8<%{w?|)8yuYKkSYm|A)I*lW zdp7zp(yD%1EZTvXq<(?5 z<#%eV9P|x@c_w@97zB>WnQZ2Lnfp4MO5dB)}oTv zWMD*x+9=P#^M_A5;XkHkXxx*tjd*Titf2Dk>L$t!6U*8)$Vyuf5j#xE4k@xQSiWuQ z$2q>rnTH>(lWyF1=x<@oW}xr&cDm(I@|E?dHv20!h$-r;9B#9PAKM$h zvm)_o$8ks}e3Jgv%@<`4`T1Fnyg7NjoRFh?X0pDTh$2qTKF{X#dgxWqeXabFZ3F8! zgHP$_ABJlen?D;DUOt*oPN#Xaa*`~UAkNb=c(L`{XZ0g~FS#BIs32IN7tAs1y$%0 zX?qE7Cbd`7_3GvOH@w!9h6btbEcvcwqWJeG>5Ta9}eiDkSo*f7TdIZeLW}` zL3LyC&B^lYPVZBu+x;KTpSBm=&@SNmAg)l~ahZ#k%NTT=F@7480x+Mcxrzj zfe?7`vR@keZR`0@z)90;zBH02eA?7v%J4QSJe7mHzLKN|No)rHzt5CJqVU<*j z5EpZN54f^jG?}M{)JIJOg>qjaEnPoE#*%INuH~;hNok3e%!N1H5Y)8xnEpNz zBTkBau=C)K5j&#-eZiyC3Arg+_m`kEnm2j5e#{RY)=p30cU)Iua1$V}!dcf09n$KN zkI2@iFC?5CgN4_SB^3P0lTh@OJjb$`cu*wiIppn6<&EF zc|q5V@RM)rcdjAPtseCA2N+MjlNpLuDt{s8i{B~6F^>2F_A0LO#{wADIZK;_q1{q&qalg#t z6z>mLL3Vsw{*rbo^V-og9>%@>LR`-o&lkzq+TA>G5l zCAr&addM~E31_TWQrDYyr`YFT{MTbd&g75+%wXEfnQx9Tew+@oj?|r}ea!Dz!@ad$ zzKkoAx#@A`VfF>(}Nh>|Eu6pvpi(TsRh$AU(YHLnmB7sEyG(oHnK@ld$9C*o^KE{$O;m*>bWIyO>FJd> z9scmuH3ur?wZH~*;8W-oGOG6WfbD-(-r90nvW|vQaujx9v z>fn`E{(vBB?Ej#wt{;3}Xk(3W9Xo2wE{c?)C<}oC{Hd7Z2fBk3q?lf*hqoBi(dr=R z$I+b#;wAcmHX>XF!GrFcl6>~de3w!SVQII|Xa`^k4`KlYPh%!_~!69nqms}GNGN!ZP*6zAhM;f;P-qOPm z-=$A==Fv7Wd%`YC2%{XrD4;Asr9m638!tEBK-oxK3q$c2cu5fCvrhuVoAl%X-6{Q$ z5UWU45SIQ4V0zW$&bd6n;8yYjG!YCBB1XOQJg zRUHCb89ZqAyj|PNVZtN#${RfaQL>ktWLI-JOQZD=f*1$29w9SVD(vyZkAE<9QS7j6y?^(v4Rk%` zEq`HBLiMQ~>sMc_6ZP%-jh%eYOGA2o@a)P1)xw{^UNw_G0pdLv0g%&Hm47V@X)7hu zudhEUV(5PV^AnSr;HlZU;>(k~rLLx-(M6Sab6e|sRY0PO8$9LS$%X@OeQ~&+Ub}Js z{{jIBA_hK8`2Pt6{JjUz`axq1!)}PFFY9__XH1lu)UrY0ctfP_Pjvw8?|!fTSoG43 zu25kZMtOMZk!C|fqh7DS?e^PjR%>c%YF%x0uueB(#4v{C$}1{ELPH`Vq9Veh2qB$P zJ8&wlq_n)gvBBSWa{xdHBQFzC6bwNzbDr(Rs@R&0#|FYf+X@%m?qpLmJzSfMg;JM4&L979XncIR|f{ioVd;)aQIk6D&g-NjD>hwpA>qRNOuzSz328}s;;cBm`|D3O9Iq7#5Ot^fSHIeG z&y!EAFBJ)}Q4`)+bz9YI^Z$7^!$bg3?M!n6Uw ziN5)b_iw9ub^cq~008=)cf7QwLx*rqYRKLD?b@#nmbv6WP!&F4#{AoF9*__O_Oj#K zK3%&l(@F!+NC5%BUYeJiW1#?<>1S&bK6;;mlVrzxktl4++G~d~4lI0Dudt=D+Z(nYKOOD+G$7h={z~J$(4E>ZD2ig+m;n^S6VzqFs^y~$zGdq=vi$ycGll4Ji(k0Y$CFiG z9;p?9kg)6Sdu;BY)HtIW09=))c6_pCQ-)bWB-*%Pw>>a#T=xhSH5XrWg?bg34S>qn z=QclCfA-X?Kh;GB5GWfqWa0C3;*Y(v=##A2iSy?#yDmu&;A+VGW%aUClb?U+x!Wq1 ze0jk|QIk4r(X{r}+h2U{>m0j;KnNT=v>ONj36ZBKn_2<@w$j|3>_!P7^URrY{i+wP zpV;}!i;n(Xwa&vUKHQk$A_U;n(F4TCD}|FmsS|Hbw(Vd3;r0wCkX(CWx1|#&b=tH3%xvXlaRh+JBt}_XIkjh)+-rg zc&Cj5Cm|3>V1zC);iXRLw;G-y&^Ltb7#&_wWiJw_L8q}LbkUV$w1GVHD{+mN6-et@j5n=)O9K*(?V5rBa5ngjs?NP;`x zwtV?S=Qq#2x+mnp50`e``^=-C7w`y#OPKoRJJU;EUGUC1$5l69OwAq(4Mj1e;w?y7JZ}84n57bjc08Q%TxqWG^9m1pZjIVJ} zr%5-*=6~_ZFKJ8fossr!*8WC`Z!FAg1VB_f2fBf;yO?=C&kcw`m=J|$1c0^}w-r<# zZ3h5*>4ltZ3jk!EK2wwM@pBz}MzJTXQY%y78%r)U0s;_VqQ=e{8FAsO*M2DDWs!Lr zGp(8t)O46Q(PMUy{n1B2v`&)s;(Q~#Q)n0*_1OBX2{%RPq@E=21g7%8DdpB;{U+AC! z0E7$aGIrX{!+Iwg7|~pGX8-o@4;DC}i3CB@ULhvy?01#u5nb!zzDi)*mJkq;U_t&x z#o{1zNa+wC8*8?hSe8|*wfO~kmt!cD&O|U3WS9s5s2u~{z}H{QIG4u*03aJa;MV!K zPf$$O_n)lYo^7Rwz_i#F{(|*S% zYc`!X)4+C;uwngY`iR-~$=Yq%R!RW;cUY_EFFRmH|M>>o4+5i&8|n$elzkuitoQHP zwQ0NDx^Plr>E@+NH{>~y0Kl=s`wnJ5{pJJn?;m>ZwS(r6fp;Uul*~X#jk@hG;b!*~|O4^aoWc2?-*CC=wDK0sw@qWf-uToWQ$yt5rf$QA=2$oR-r_ zJVgwI%9FJV6akPg-MM!AnAc|CJ?zli`Pct#g7NH!Uz{>aG^oREFFrR-b#&dz&kLla zk+kpfcBn`sm& zaQeuuBX8`kww*a$B~U~H`joW(3AGznu0C&Obxe7KfZEhKFFrd>b#&dzFAAiDk+AtZ?}2l?Mv=@a|(~ zw*v`*A|R?$X1%ad+)mkJ9ZO!+}fubl$5~Y-0xNxyouU-tx78Vv# zN-3q1r*ItSa5yDOMNy&{2vrm5I}5&f;mMsW1B8tkyJ+!E%z?c{JVi8R@AmL^>I3s0 zS#TkH`N0OtX%l*Mi>&zWomDvwb?kuYch6k%VDTew9yM#b-}Ta?V}&E@R_-qn!n=*0 z-5x|)fe8(OO4lK+e?pxiYf@e>(f|TQP3qKn{q4J+|MaBl##f%dRd?vq*FMM*doFx& zUP@5e7ax4o|NVz;hKJ z5m!S|EsX9tW?*dIp-Ojivmk>^Sl6ybDgRiZogx9S6&%Z#uJ6({g2^hY+PUn$ofI|Q zADb`p7bpP+fiQ$&yvzC0;$|ZIkL@U&e_P%hTwT4AL6xPp!R}ym!2pmL!~%fR;k4K( zuhQ&W9a#^(f8I$ju&oQIld4M^ zsQ(4!Z%cx%dPQAhE*!3OQD3c+g^iSp9sB5WQ&f9ie{oxp6Aedl#i4zAhO_yVB0_Nc zzHzi*W0A}27p}UR0U}k0P<2XV-WhN?63fp;gaorZLtR8`VEUTGJWAR`#RNs_pnUVk znKwQ;dt&<|pXa-Q4NaRhE+li^>bhBvCWUDUbw~sNiLvjrp{i5w9?Z=yJ2r3Wl##K& zZY~p>dRX)=Nve!Ru_ZSGqw-i4L_|;pM|T-@Q+KuP%;{r%#|j-1AVGV=MKuhQmtk%J-t+T=FfBH64`Q6VEJv3ER{ zZ7p#jMX%p)#S&7a==M-WwBbe-tuAR4fQbo6M&+($_wA5S+vBmnc~ z97~j#kmFcI+^Fq5clp!f8n=J^^6F|`?-`1DIpXj)Zb1pXdq!7%xAKELhc>47_*-v# zez{@k;xBXElqh}}J@UNo!I@By>{}!3go8`f4|3SS0Y22sIZMs#iDK z>Ls;==r%^uhT4Dw$$bzIfiKwl?#grhZ=N&0vkqMOKd;|$>SC#bl8_!_duaj6epchjve70YG#MoQ7Cz0{G@iY4cux<4a%NTp|QyP7}M~c{DCPMa_w{NO%KqGH%{otoHQH{LpT>2l-J#b4yQ0D@YduU#A8Q;r^U z9qO0?)9;@7)B`08|M{Dolk&&Z3;}=;^gh)D-w6gZEtgEm#1?%OP>yt*wdF-cHt?%u zqX$2{Xkf|OHCu}X0KyoOhD{nhv}cTt0J^Q!MQ68ubEJX?Rl6IXc(DD6_deZMEg>>t zL+^V0M$_l7Zz+h4oR1I&8AB3APl*^kB_zbC zI{Kq<=;x5CqUd1GsN>KDL{0QTWL%`~%RkKlv>X9ko3Z4x)@xH3hEZOp@krBRwXz%= z7n|Vbd5v0=l#nb+l1i0CpQkstSu(hM>qxNJ2EcM(uD5 z0!otNJ6b`dP(+a;1p+{wc^g02GyJ9d9)Gx|V?y2+|2$YHAu;rtGcm4a%aV7u6bb-m zGE2CW_inp2ZPSbU8$5TPD4QGwqJB||JX#aj8?1Sbqm^pw$&DL#=ehw-pDn=T_wShA z|I5?+8ufi=-x!(u$+9o^*HeJ}YW;O<=a20YboiJ}o)OUm2dNkW04TZ~E_nltayopM zM?cx{s7Fs453l_4j735KIX~Z7cKYPmTzPd8!=O16M zzcJ?efzx|NtM}KrjeTc~4y)Mk_^QpNA^>L9CygAK28fgrL+?406RWo@d3${!4?ea# zN=_av0Rn(Bo?g+a19cD-p$A9y`7>D;?Fbi@#s~-?G?tYVmHMucZ%rAlI4B5$B#ELR z2!bGrq96zkhl5IzC?88x3IIr{D2j3|QW^l2a`>XeF0T60;(8$3j&mLzA9eAoC7++M zhydU!Id!xIK;iBC(|g6J4%E945x`Y`COtjZ31>4Z!u!2FacClUoGLtje@m$VKr-uO zp3AXc8>pP6r%#>Db$S@tsz9Bp_`zvmJ?E+o690y5_XMVZB zJe-!u?Kd0SB?duNMzNJrgr=PIyoQH}o`@6?k#M@O#O`D7yfI45-gwb2)$LiiDtXC+ zkG_{S^UQ%gKkoWDz08RStR`6NQD&-()PpVC!Yk|7-B@FT4$;93qJSa*c(PHHfvD-| zn|IIjD$8Yvn+fkfHkCj3W`@PPdAfRf(JBB+~D7IzmTGz>A(j^QO!vBKudl<{8gM(G_eQcuB;i41>fK=6rx>B#U zGm(QIeEh!T^mm{6Og6B#oc%d;(a_#u+be1$O}n%drsSJEizr{kebv>B3ys(G7#AGf zjor4E&p#_g#wAErgFb?#1bNUJnI1sZNIZA+wu*`k4%MQ)GIQI8tnv5F>bvEQqmH<7 zGy6F9E&H{!??loyQb({35eO4AWJ+(>uP>aph_0+{`|4JVzoGq(&n~(ZREkSPkB3F@ zctT1&m7a0iKqd3JsXIHg^2;+8N#|YM5<|n40+PtBNTd#^eQ?m1QAUZ9tF|P)1xLgp_mfR%^nygiFDJEO4eF-gYi^G z%>+cF-e|N{1TtSfZXkNV)caR|bw4yIrHF|7KC{RB%*(|!`ZU?Rq$^MR{fyh0lj*rG zfYT>)-S50P{ni0nmme}w@H{30>L#ouZSAv%?z_g#-qO$zPYA%rQelYB&yppYN{fYQCY=)_4W*NyG=lSwzOrv@y? zcV%~-KFFvI(E^1)sH~`rzH<47B7x-?%63tBgh;`e|Md$?zSZcxy+S+$1Yi#MvyS>9 zFEI{Zv~*@WZPx*Ds>};w*ucqyLjYXozWB!%>9(j*4=U&&nxy*eeCrW1VNh;=_(ZM? zAluXaXmRUy@Wau+vbXetv z$5(AB6#yWsK2hHHd5&mX-L% z|IVA!ZXK|7`5|*4zi>+N1*g(;-2ig*I3$TXa^7`?)J&ccX$E?yd)X`@^X)A~p~1n1U8d7r2#$p)8bD@(WO^e0ia5xK#^3^I0$Eqo%8nwErri$nJ*q8)h z2=8=BRN^>J5QK)t8i5zB7K^{{mOnyBkRbp>6h$gX5>NB;@+eP<8-P$Gh=f4~2*WZQ zsyK#KF@yy|04Y!b1c6}~kp)H+MM4M?0KX@PNB}?q8IBz^cyQ-Vod7_u*O!!*ND^(e zr3_NSIIR+6V4~Y`wHHL5Wwvp{7*2H)|Em-dy4#sbkt9c&!Q~Xq4)lAL(h`-Q%U`R8sy|i}qAg0N|p!wNu&7pQ#jN`o+3)=j!K;>=C0m&_J7FZa$C)ZaQ~> zrl4Hw-uqh`%r^act~zIV(-TTtbhW0jIo;hm#; ze}hXEGic5|vq$xAAF39dE)_sIhXlB&j&Y3b-1%Ce#T3}+ZtYdJ^QSBMCfwj!;sF3x z(T>gM2jBhT>Xbu2?%esq;frRm6;1|JhAR$=q9{p{)ox2`*DgIhy|lCx&8tbMBet$CC=KxiSC|8jBi08>&~a z{yg_+tfyQSDnJBiU7(L-5tuIu2DW5-W>X64+1?QgvHWloZxGJp6+ zDJV7#yqHPz89`I9g%n8mkO!779Gk?{X71audE0@au!)Oi@Sm;NS}HJ6X=6L^=Qd>9 z@mi{x1VoSk0Gtx6F64WKaP5OM$>G{WmJFoQ)?!L{o(s280B$0MLf56Z}#6bdtM-f%4M)=T?5DR@;5S+LwlV%;~+$ z4q0T~5L#2$GiCq)FpPZcVPe|7bj{&!$v#^5C|l6I&wmYNJlY0HFe?4Q!-}bmrlFE6nkLj5aiw z;cK0I{n>B#&wFXs=-8}+X%k}dzkBV115#ZO92Ud^2moB-$eUB@_kCF40sy{v&rhY3 zr%vv=e&ty^LaQFOb)f)U5)}ZHeDl)#C#}GG-5Kytym#D812Tt896hO{M3y4UfGtKRNxq=QiZK@N&oq%*m?+ z3X%u}Sc!sLanT&1llq1O#jg*Pmz!xvQ;{K-#MR~lbf6+_8#91B1rJ!Ub;X*KW=R2q zy4}03DEBPHQ%#G&MYqF;hfOH@t74X%;{I}Sv2E7|pfAQ`*n?<;bm zDq-mGSO6jCK$e*b_M<22M~sW;G_r4K`u=*!uQLIV%tZxeXcwvt)iKR5OD1m>3xI4` z-)VQx8j-B?HBkkre6|9B0IM}>0YJ1>q#rsr?t#ADZ<-`(+7qbUzwe?t1pqYqAQhn0 z7AgQ`;Ygwc{)u8tSf6Qk&$_;8FEweCIZasSE7VjhPc0;v1z@7P#sPpk=TN3e1gtvw zYw@&{6gIwNlGq#*8~Z_9WXCwhb}pk< zXt|{g$hv^LRK(>d454KDf|I&T>{5rs&?5HAP-eC@^jf?`Fc`fkJ0LMnECy#K5P0jjqD)ai-Iz1s!lWILNsC8GiW zzq?2NEKcPA5J>*wtrtYNWCnEA`BUPn3py@buGJfeC^Vi+@0@!)#-NHas`PpS2!tU) zVWf+e?G_fC*R1%m8L7kDbCu`qCztTg#4V$lAz9SPoL1`(_=?1p85JDBLBlya<|#UDbFmK#_juP#fP~j zh#m9zk}>j(JbP*&V%QT4|B~VcFdwBW8@aDk{z$yf1=mu z$G@e-b!rm(R%{wk>$}@`<-Znz)*vFTQL5~#V-tzQCQ3Bu? z1j&!*0N&5WAR>F2s1Vqs00ISI2o)tn(b4FFE^!7H>m+#^h=8Dvi&fhS%mPa0vMM+9 z?i#1vS!Hj|Ntp-KC;hSwudX^_QKe2>@?cth#?fq(@3I>;^U$V8LEiI*>PMLgbmou;2{hX!y zlzZ-7w(1@LcjmvIuH-4Q(F4YGaG(7)$L6^_|JtY-=d$oBEdT^jJ`tQJ>J29d0{{a; zm`QP}{$c8jDp1-YA<*1r`uL1qJ_EQ*wlBZ`L~q|BdVXDw%ipSacI?X1m#BBS0+l4{ z(b%?ZT>zcDgn?3M{Fx_)x}s5KB{xr7nd$5@ZA%C)uWM4Flr!BT4j zU8J5>REL$#j!pD4GNRSfMt1a^Fy;LI6lk zPC0n4#*5?nUeTb0I)QeJtw;49SjLX1G;yb|UiRe$-a{AVO*J&yvn}!`61^c7A^=FD z1fXIlL8Prj$Pax~(A3t6B9)_~Kz1?~{KGRs0Fb&GYb6Xp@X*|HzRGl3dzV+<_kqWH zdrhvlF}B>bwZ2Q0%%$Zn=-VSP=%*UHAD`9H-P)<3Howk|>L$#QS?e|It^0*ouSgJq zhyn#*8HLb4YQE%C5$`FI-*M9KP01w9j-q@ZAplT!VAJDIb>FjYrGKgK3?#n+4~z8(k_By3{#(4GSx>#ZDR4SE3C9}mG86GVNf?lsnj87^q zDP*;(^BLzj6&I}4^Sl@y7C{&^nRvU+=I<6cM3i3+AONB$)|Z-YTsD}~urpA~F&X zii(QlMcBNhA6O-7jgrM$NAwY_F;?xiqRlQenyAyo{~R>DznIafix32YtIpL}PZ}h> z-w{qTRDQl3{cAme0YxOh*JPClle+bbDQ0?j1qPv#MQ48C#7F!rDh`pg^GD zc`F_~Q}CWJqjK||;AxKqZUDyTyS;8+K5L8X-#3hwWLEJg)a6yuN$uK0_1+RW*?@rm z6eL?o_SsS&b3C=avv1`^J4O?%0uZU>H(R92+i}Vq`^K`Fg#rR_k-d9I(6X#b9#yeD z6CwYjZ+4%qqsUr1$|sJ9e05fdFsaADL`}v87bu^=l&2up=2b|Oy7iA!Wft%rPIGZS zJcO~7c<>+qh|bD0yFNOzcmJI$|1oJ==Z&wQmUt%*YQ07Y=>jXm6$b@D5Jf>0MSXJNhv&ZN5HA-M0c`+*B5$g&X;R~K$36nGlHL0@op59{Z8`@697`cwrpJ>oxFsB0>J+Tr$-f6hV?+;%rr>4RB7)ONaQL`3D**H<2{S5jA*kn0{g;UTS5}l zW#04iJDpv*c-5gg(L>dx$!HccTz&K9tSYRt_3xjkO)sfFQA3z}@Z+F*{)`uipZfPj2eP9=0t z&~zKtBlN;)ThxHz?E!#R=GTeXL<;`Qp&|ufgTg}91bON|5ToxjcKYac_Vn%BPS%J( zG>OBeP3R6eKYV|<)J1`d8aQQge_iQ;Ex+U{pP_6n!N6b}n@3L&fbEl$y`VshsC0+)h`iy1ce<{0t(>JxPm#x-G%*RTtL{Q}q2+BdoF=?|MPK+ly4%|pcB zosvmfaKpFe0Nf4LWo35ss|CNt3`0B*znb+UfE0k?l;di@e*XQH-v|hW-+#KVOz`#< zY!>R@um$!&H2($uK@jC5vTM5){gZ-l=?rMOxNX*~Z$AcrPI1Otl0!PQ(?o=^oQ|3G zdG+qBdH{H}&N?GGGE}R+9BkL3_&L_AJI*z}1KNLH2m*?3H!q4T0kGMvJkN8S%FVlV zIz7V>li6f6cp5YOFyti(j=)_n~7@m49fu*%ox-` z7Kd9ErJe%ELv&Ioatwu_nQwPgzA@w3M;-tE`65jkeaF0Z zb-Q0YR!;$FP5P1YJ7&*+c&7U23ucH(H3C!yMg@}6{-LJr<%Zh$Uj6#0?CO}_H{U)Z zwra=wC(IPkoW6c{-7AxyTOzF6eXho>jZ6xwKCmatifz%XcpiQGu04C$K&_ANFksyD zvEkN(YqA`zal<>u$C}QuQ39?yuLfpJoH?#$U%nZ` z5bAPEg(=t1o|Lh@03*UJr}k%=l}t5Y!iPQe&Z3l~FF(HKyaPd%H2t*|vtZ*>&u=Of zkOcR7;O!TO6u3zv;b2p~}8%a3Qf$K5zQ_Lr?^ zKYsJO@FjQu*$4A7o(5v z&^@Vs+h@CN0memV?)taZ_kU~7W7Cf>*@P1pPSNF^$*VBA8AH;5DN|#hcu$e{v(Eqk z;JUbNThW+#k1iYFe1+!cb< zSzlJJv}R)lk8STv|02ik9rNnymA3?y=h%~aQcjcM;`%u0@OI&SZ8k~b4Tcb#i2Lh( z`yZ;(wvEcmt5z4$rCm@9RBhQhrfEN~P6jYgcB4-m&Og8Mx5;N!%8UT&YLuHxb%#HA zB`$u&?N86me)8YhP6TjX*#1-H*jpCPVe5CkFMkM%2_N26!Djdr44Nv~39oL!aLCMr2oP2av-j~4DMAUBADe{?} z?=QHT^mVpkn!cAGizxpb#b?XO^nMANgN-iVMzEzN{HDk-;;xBN`QNOqGPdg=tCL_Z zFRKqgMsrU3z^iV)Sl;@}$?Vx9`wmOhAI`A=$3z)b35j}FR-M#PMS~3*H3vvIH4#gT zIVvh|QeP4?X}wnd6E&xA*j@L^l;;+UP2(TQ*QmKc4AL~3n=3f0RYrG zHCz%^Ym@~f;P)`6^*PhV6FZLG@W3EL@4H@p`wk@%s6VhH!%CYA)LX=ck_PA%!*#p+ z`MX@N{9E33AOHXpHemYXv;=@;_u2Db<-3ErOq)5Z69e7VIq9p;+BNNOxMfry9rOt= zJ-uRoonIgqi00o578Mo(Kyq@*mBs93f!Aa$X=bv_t!#^ql(fn|P$$oLxUG<*`M4O! zP6i@IY`()fP~PovuY(J`AM}P{&otflA+)_~Yn64*>~$-5_IPRb8~<$g{mvtKb&|e) z|MAnuc9Kqg_`!iX3LJoJ*vBb`{SMi%OM(4DD*}Kk^{|o$sV3R`zXs&qwUx9FW8PNVE-7Z{zyPjczSmA|D7Wwp9 zE}M($*x!lm>XpEL_Pw^0Nje9@-AziWK-&N+*&V*se5uIOxDqbDzb_#S9^@bg8R{{C)A)AlA2 zC3|T`cKy&6FHmH^E0 zfRe4IPUtfIsh6h%0PyCrykqNL`*zz&i$nmtS%v6Lj&SCE-E@v`to`rriPft|=NY<*{V}(``5y6r9ujgL1&A;pB zh0o3caMv9FEc3|En^vq1Uog8V9z@pIVerhU_eH1ysI}zyx__?Ab0HG*iH|wAGu2LJ09BKlrn}HI=&*C`pni3Jk-TOr|zgol2#W52`&% z(h`+Ny8scH@V>*60AR|q|D2*=$bI*o=MHRI@lp7KS$;ezRbH}I6w{juCd&()dFz(G zV88dyiOqRVi!TpV8v>=)^XtDadwcGqQ_sJ&`P|pf-?b$&K1$=LsWj1$*cgqyy2i{? z&l;>h^v>h;S###xJpR6+x3B<`wLIs;qT9)>T_at+-|WxFerftcKy! z9-JPo0RVU1h2!g%|7&ZhfK52bmu!9I^`OV*-|_5ll~8+b^WWdwQo{Q!XYUVrngju4 zJ!rC=TNO7KJ7jc{h`cfTpsB|gH?W@-^NB%Bj7G~bV75v5b}INC znsD}dpLR7z<(k<`}2YC-Sg<2!;Aiv=K|zQf7)%jd2GY2sAnk@D5%=L;~)jU2ER+)>5}afFOg0HFlmbKwGx{XqD)b5|L_7?k~LW z-W!L;{%;m%(Dc3Zkd3djvrcxp7U zp02P<3QnS|TjG~B4B+=e{v+W_wlp<6?`|m(kZIDlwKlxJBM@tjA1u3L%BT+7^I6X3 zNw_{sICk%ArM=oQOwha+R?PG8!@T>M2R_Vmw@QZJ1bqexua)_#yLk3g`^0fQBRPVn ziyMvMhP)aFbm+x8ObN~!V|WMyz!{hjiwFXG+1>(`U)%-If;oO70Yn-g{t;^reYJJO zqRD@oawL6?1pz?R_nEVJdN@lPa`wFc#=pF}(bV+)XUpm@@JfMg7`MoxQu@kE{?AQpi?}9-ips^l~><72YX<&W1w=JW3ukv`~TW>9E=rW?43V`Mx&bD|h zgS+f#x@p9yP(z<#?Y0z@0{Ad^RnfPKJT>4q5_RTn_-GrMIi^oSY;pCO{xzk?e#dlVxV!EN^Dsa{I?DnM^X1ge)@|?)Q&5^Lh8a`|i8% zckjFJ_wKvbV{5LzdXtxe&lf8a6aWB%Tf!Ji_RG6BRrpUq@^~ZwTB%V$_@p}L3wP8L z0GYoUJ_EfeoQ|aYH>bc^OZM`e?`*LyoI8D7x`vV+4P{5a`0PlvOYU-g0|`jY$Ns)M zeeoj`vjmIX_bH(_j>KI*SIKAQ*~%`MJQ!nFLrJ-H^berC^ahVH18nZ8000aKNklx3Qxi#};E_^#Fg~^O;J-g+@uUBZQWlKiMvqr^2)0V7R{fv^fyl^K(;qloGe>S7IbS%S; zNXyXc^xkja>Tld&8Ahd4SS=Qdg;S}mI=znLxPT>~f`Wqia~@07X#k*gOS^PO`ogsj zF)CVeOQo+=s9B{#!6;Zppu36D9z_`Sa)Pc6)D%F9F=6DK15URWzkiQV0R25uyOgvQxwk0hMqGPNa^Q zhHC7JEo*bPEqmo_b4-{hQS_COk#WyGlX~+Fh%8GbB_)lGjbXxvqjj?F;n@KtPnT)*yc%)6U06N(?+1R#in;YAi_R*y5@W@s=EdIEFO8&98C0ePYw6_Z0GY-5P56*H8tUY#~u=H3+Sw z-(Wn%gzs)n*m~27)YZFxVPGbL93o6FcF+Y4%@|lkrP1ac(1N z8L{J_)9diKJ(&3QaZ9jGEc1(r#YEaJOsce?dcj3t{dRPBGep{IW-8T!b$$K_b+O!`(e@@&!RE+Dy)nD^oKD1b-msV9( zRFy)2K|@2USNmLUtsgWMNt^D2yqW_7@_h7zq4JKdTih%DR09KfXjSGeRE#K~=J_Pe z8eLo`3Gg7@OEWAw-}QVth5l9dRTG})`Eo2;?KC(pX6^AKALFc_+pX#$Vi)BZ zt(dm&P4lNy-1uX`ffRL?dw#`LC!;dE`f*ie#2+6xiPJW|5L7DngJYpA{iiov&W@L~ zZ&6_>iiCuS?hDi9{$T!mbQhHFF7Rl(Q?H2HiCXfg=bBF!bMmsh?brTIEv4DX!)i`v z(+iiwG8s;fL6PEs5_P#8h7}iX@xt7AwtOpC29TsE2M*&>AMB$hnkzU7i|B8q_YjRz z#d+_>J1?Gud44oEYzC{?H#%_#ncv_WK>#2h0LJ>?_AyvUDA%E; zd@pa(%9TIdeLIR5SqxeU9$@gN;I7s2yef9QeBG5b?ufu=FR_I~ZU5vK1*ntOdEq8v z@gp7X+gj6u3XTZ7Dy<$*Sj#~+dCOot+I6;I00N}4yY4)LC-&u)ZL0sMtJGF2=Rt8_ zUPvdEb2rtWXcR}Rn~W*H{&KbM3qytrQ4qygJ}=Un1i`CV8Pa%^->Q<1hd-zdI}n_9 zg$L&Esc~*n%y-I=iN!kpi8`UgS~z3jk^MvT8!A~mAu$XVN)o4byHcT@9lNP%Y$mVg zY-v)l9XqR5B`zo+P*p`t(E-{Hoh4?RF`@`bj3^`oUca7nG>=jl^@b-$2$)B2D38(* zBr*?CBWywYBhVh0UNlB1Su}>JpK$|B?AsZ;*-=~oZCWi$Bz1g_D|7`dXh23@ChDn~ z29hGu!?_^9YQ9bYgD&{@#8d_MAM$%2ouKPjcAQ~Hoy$jXD1s>N3hU*vU|T1dOapmJ z9_x7jx{D`@hy^liSLg-{j%F=gCx)^;2O+b)3P@%fbAgr%;efm!aEN^AFVX33(On8T zU?OGXB^nYq=XHqu!U3vJilhOKfaVGEW?0VLs6!ywvaNaZoEJ z#r(1A7EC=q8`iW)i&wNf(=AO^I+-B=CL<&41zL@$2}RY?+fQSZW=?ER7R_98+1&6* zrp;Vla#k7-C*c$jXRKv-_W&JWm&oIqp9=sICDs1EaG$rE$>Y*i%u6@Zr~M%(!nZM&-(&o=_XK( z5QV85)9bawguno)h7)Gis{1MJ6f!RDnZ~!nZR_;Kx1__4B{?};k)F4AT5N#%4*gT+ z6U1_v993+s(jrwWt-_)dE2&=QuOiBK_in&CJ^>+M=T2|I}FHGK@CAJE-VcTvqKG$ zeF+;roVfQA6BgF@^JTjeJKcQguRMey+ndz8J9?7I`T|%>O|P?$j@P+iAj2dv6|0fA zaK6&R(42QhhHbG~u7ks2?@_R{DB8MW$mKxY9G_;T8@ri~3D)cKd0wp44sIqCEG1zs zno9egl9id9Rxoqq;*}*;%8nfa+(K^g>_2x?UsRcmsCxTYIa`j8!QvYtX-SG14Ua;& zSZ(eut4jv~3@Ak8fe`L*Cm;C6)@i$e+_|Rl5{@{8XRAm-jTDm?SnfIrF z{nYh@=K_<>XaFk4(bcwa*{If$ePg?BISae;p{jNlS1w$y_=hPM9o>Xc697US5sWmr zIsNE@8Th)Rh?dKK>{{&wKpFOYaNrAliUdLZ-CO_aHcZGP-`^KdWRecRo$-n+175=# z^^gb)!ca;fM8Iwu^HT{S;h6(xqLNY@x{|v5H686a(iu2AW-M20YqLY^j&>k%)D;Ec zk1=p4P{Cq5{M0z~%dzp_SU*!L=T6JZ(h#qOSKu^%BxVUOdxg@DRcIbIw z&8ck%40ayA2+(6Eal%iop0~HRFJ{f`Y`0WqWo0!EF5w!`Fkq18Qb;YUWq5w&iC)o? zwl+5{rY&;a?ynO|cr)QPGPIqaPD?GpUXQbO9ys>^x2k}Kv%U?Zao=dfdwxxZo7a~>E?kBxjMJ>jX<624IPEbwI6Y4c-9ZSrZo z(YqL(!!BqU8bwEoP3Iiun+_9q!Ty{CGj?$`|NnWTTic{ z%0hE1hvm=}h$MKWTryZjou@E)Mt{pZ`g^R1(%=yURu`A=KEX464hj+O z>tOJ$@VO@+w%p|@Te!@Ct$FPrcwk-LNW_oriMPceds$YJ9Ll+Dm;;m&(gZ*J@cH-} zT?MHpDqC1eoJO$045%^-m8LIk7#i$1JM2Bh;Z1Wv;X|foo`v(`=Wn5)=q<+z(QLM6 zo-_LlO2G%!kv`(II;{NHkvK^RRrc*#!2w<4zh!kVxAU&mv9FE}g^8Z?X8bx97#M>t z5$2E{QyVm&G-hI;_aaBk-6XoMsqWQ5TND_<6UQ54_cM$SKxc%`Lo}+&J~?=yb2pULHXz$w#MoxO=S|^14Z5_~S+Fhw> z15Z=Ks-|Jla&(1iO?gv0_19fp4#j84b-PWw=ln_ccqDJ7Vo-n2_lVyi4%)Oq%YX~1 zA|6Z_T2(9Y9ebw_;NE8~pq>MRx4dN?ve_t&y-E}PU?(mo%T5QeutXoCugUcBJzP1` zyw^7OngXv%l;gTzW3pJLf*UqS0j~+|DhF8V(t(3#N-_L=fkhnsVcIkMNaZ75zWM|3 zyPD`W9hU4)$p6azcMkoq7R#PHtV3T0bAs{`1_Ajvo_x=MtJeGE&(m@e8*ohY+YT|G z-g06>cmaeGI?!>sa^tVA1IQhC*NCVZbH;Iiin-&jI=@8MoNaG@#64AO&?l2zwN6D< z8{^NH^6farOddofmOXiNviR7*4bZ&jd~14gS_R`yn^DJDQo4gJ;BpbG{wzh~Lp(AN_eYHCa<%0R zioc5<0ge16tNG}BI2BRW984m(ko0?gDX|IGLUNc0j(i!?BM?PcEZHZ%)P8O{n<#93 zSS)!@bLh6C)eN(h9q{#Y`DIH6WFWEQ5?{2^Tu~QTgu1{N?JVZu14nGf2~0!3UbE=x zp|IsUf`gG%3+Q^hgSyCq3-bj>e3eJo2{_S3!>26mt_( z*RYB+ImrZxfeaoy0iwUod`jT^WszN@xibnypC z(lbQ#Gq~9sU!>8JC&2G+c=`hGNeBZj+-qhRStEf=S5oNVf?hTuuEkuPlJdBw0-4@d zvBhL;*?jp!Do{(6hk@{fX8Q2BldRG{3l5T+k-xLO;MaX7l5(IUo&UE1tnTBi&5e?7ScUh!P_H>AZ~OLmfAhcz9pn`)~zzyUluRsvZEl&D)+H$p*4mL ztZZsh?Q7@^Ak!<6$xf}8U(NyLmRg&N(Bk)vGIvb@p(Ao)J$H>~3rzudTB*PjMb)M) zO<)ylh$bm9Zgmg zF?cwQp_y5`HaH)6tF2s5;cQm|M;0V#PwqA^sM|}fV@xWzp2qQu3Iu;hM2NCk#%Pdo znz41OdFISg%@@&lR5CeO#*~+$@J@8a(e|cjOA+?r7mC)$6FbV?N;s9xQo&RjMFu?? z>UoQ+{up*Iai~~m+#sp)6zKM2QoCr}+$Qs=NY`cTmxfAs)5^0_xfB+eq>fuw;$AkH zeIIv{^QK_LUzktVl*_w>rz#K62UMq@@X2PU?^k@F7@l!SMW<`(1dUA~Hljf*)i=m< zhMj-^HqoSh+bq0frRK*sqvSBSpc8<-Wz)+5^s82J+=75oV1K#S)CPfJqtR9upa#@9 z4&;@zp$V>XHZ7;9E=xkD)W6E}RWuT*vmnMBD(B8k^w;js^rdgv6bv;R6_1rZ&@oM` z1t=?TuNqA_CHhF~)UgZV6lbGi9H;HiH(JFJ?G)1+weabq%M%XwMIk;v=)Sk;1+#AIn8Bgr6d`srcfqpZHILg>#(FNWZj%24% z$d+Ivd%ACWCkv&-4efErYr9t}rOl;vs=7`lZX63mWv5}|7ey;7uGjI)>na&#Ra;E$ z8CJ>*DXnJi%C=?PxfXwODc6{}_X~Cd4;P>ZoY-PsEPau3JWg2FUR75CNI6C(EGsM9 zoq`m$5RaePCX<&yC+{Q@)E5^K1NmHaJC|2AqN(T1jOof~IdH5MO>_SMVV0#L{?d~? z#`hk8iv$8It@dL;G+TF#KP00?;-^EacXD^_@{Ti7WQ z2C7AZx#MJ`?6PTMQ6Jf*sd$W(Rjhppcd6luQBbSa>;6P54@}aKU&}=;$i;;5Xl_3y z9i#s=3earTY9AD4GD-2q4~qH?JWjic@S|%UHd2O3@9ZJvJR`;UG24nmU){co<)^46 z+p=Fj?MDxNzAt_5y{R2__BcfcfXuac3|XOst)TMg&GmY_fc+Y@Q{?@0)vSjVn{67D z4)@0IHge5sXil&P68ij;ptBX_ zXKEZq7r!_RxbQKB@O}t(iIUY-K>@Q|fz-D`s+n)zQpB$EqQ4_pNhD;sM<3{_%G=~aP$(4^%%&U_2#|dbqaZdFRccgxL0K)SM=5nuDpfl9 z1|7P}AeRgbYhzJxcaB-wcgXsS$R7A5oUN5OhdAsqiAH>kf*%>b)EkqVeIB>Y*_R0+ z3=GXa_J$mCV+wg3j?_$W0~w;6#~7)>vLm&P00PgT?UUOfB?A zdO7i2JWzrloi}ojIxv}zr7C&%{mAnhYWU6r7Vfxs?9XL5#Eixuu1Q%OPnv2)lsnDX z#vIxl4_3`Zcfofj5R@+e=B+Wf*h=U-p}XdRy)~Tc*9wM`w0j)hMO9Pz^K({bOF2wU za8^eW;Ye_$30f1-KD@W6!P{B6ooM?OVQx~Q`{5W)%0Z&YG=ghJtM9G70ub>VEpM18 zG9gM!!(AlPnbW03C?~O=M?*=LqF)ST-VSzt^PbHfC_4>{Op)*KBV*q>H83QT*vd zjD8u>K=bpe-P2@u?DAKYT}>zfbp#t}F8I8A)!}y}63WCsL``H`5GY&R2v47TRkV}3&iM`_a1@;ZfDiZ|t0Bk5>om2b|-w2@+Lav0$AQcNyl2G$m zw&u6-y(e3Ujy_VU!B{Q+d%@GNNHhwfBYk*%3*LUtrtd+Kmxv{csnE8`V%AFZ4}prA ztb)#2h1D1haw%u7I6p31!kvu5V(732_Pz^Yro{e$2yBA4r{EceCiI!+Hea`!I=cT+ zn2XN*WH(+EA23%5C|+vuIe)QDM7PqHA_stVAqyv8yHR&NQJRoy$*n#B-A=)d?l(6Z zp5AFB0{y&gDvys;38=2gX<9cVa znGYZ;$B25;t97Vd!!}mcb>Uk4?%mi5N#=S9gq$Zc>a$tCfOA97NIHR#%;7Ee9jh<+ zI}|qne$9jL;5}{CHJJ$h1YgAUazL}67*uujLo{UGc#f)nvjGOb7@dcE(EOTH#Z?ri zI?+bKXGaQqUchSJz4h)Gkpt!Em%R-3L6oL$1LgDnuJ67?;L^T4cIL*Fzi?{l>qGPM zDY6H7!BikEgkFvr(USM#gUk#Wwku`3y2u^+331aCF09m8L}Ie zqRC|*w4U-fp-XeUI%+z^udTe^clpfUVn{6GwxPZd3uh>vE^QEzzrvcUr?ta92)DMT zM3Pg(%yX`9dr0nY>JSAX&ac3RkX!ld9I~RH(5D7zQM_orpbvOZ6ObS#a29kIBNKmN zd}H2MkR5@54!|IGitPng3mc)1ncFhNCas+|f%!f!D6WLqGChzHeGL|~{Kt%B>bzSw z;#E{LI-GKaSu!1tII@68rU!+L1iO~?px=vN(?>PdJv-*E*eu(U<(6x>$?24*_OX9x zKcm$`bYdmr>WlZVjrbB4 z{d}{LFJWaU7{-l<8DE(;>&iNnKEI+)pKLTPFYL`rYgnxcr5GOEAr*0zOK}DbAHU?d z`HO@~&_^`=j?@WI1%FxM3C4fl4$w^VYs+AqUfmuS4V=Jno?o*tN&7+dkQhr1pQi4H zVRk^+%(iU4RqwW zY^%xojD>1o(04QKL#5f0yb9?1;UKXs8Ui8-T0&7STV2RBFJ*97iTxHsl`C)2CjPzb z@khDRPeH)B9b)Y~Ng|aVlSyC#dh0fkHl>O~2CvIf9yAEreAYY4@#5#6?KF@9Ph9cJQQAh* zylCWMhwp}HcMQofvzFzw-jG-LINOb7jH>|GC$b1_#O8}Wk!7kid*)bL;_XnC@|<^E zp?vgbwBYd>&`7~Y;6Y!XUx^rj!-OxSM9!oPP;3A%E}uwezlxklWtm38c?bd3l{R9; z(8oI0TrNb~+9%N-l_T#!x?<=O-{~VIG9iGi(4I|kRVrg4h+K@vpurpX0gj`>_^vyH zs+p8f+lNqpIBWEG1f=%9;|&|Ndd71eqqLu#+(?*ewX=dzfUQgMHnp+TXNmO1=308B7L`t zHx&F60L#wmE3fCU>0MH7(Uvtt_PRaKfI!qyHQ%PIe+-fh)mcbiwX3~#Oaa!11#Gj2T77(6W+cK(9{(Q+F}y21Tl?ko-hT2(&8u`XBE1v8NX(iN5+Yi-gQOz*qTN5q zk%Ny4vR>DH$n=h`$wifj9Xnp@Q0m$S{YJ|L5yZbrRSOjd=za_h56jh;V)iN=b25$s zR5BfS_Gzy+W|+bWFREF$W~P)p_UymzI>d4%Co|~KVET4{@BT&AE5bgsHge6Sczc)H z5A`_@R;@L*aV{0~nSHSdl>}Ok?FvB{LA65m=_ha&Wn(J3g#jJCOYDXWBVbzusU$8q zc5^+k%Xotyq;S3EwEbIntA_e4lqfq26IjP9qaw4)Zpi9qpP_(6lil&_ymn z?|v#lMBt5|8B9z@s(O)wmeSh_$|7FfRx_FA z>tUQqUJ1a$SmEQ=@`nij%3@2SR)xo*TBn)+63l~IH!d3xO%(7XE2af^-;`E9ZuZ7o z8}ubqWzxFHOHJ}%+*}&(N+xcwlhf&t-Vu3zZu(nv2L{bEuf=-CFKhp*Ga}s7eB*dp zw2Y~M;#CK*^FcQhhk4{`;GyWBR)ZYzvODS%epa2k8~n9a1T&>Sm06~0e!{uGmQ1YN zg~ID;yfurO^EF+nmygRWpnC3ynZ49@q)6(wrbw!T@CE;(FT0CTII&lf?KUZ_9#NZZ_Q)JKHo1zgd-I_ zDHY8*L_641h0i=Phi>(+$P=OOJkaZzbhZO=r~cq&)=W+8Dc{D~EL zEsK-fyu??I5AH|xms`b;^2Lv`So^+_;0B66yz{Bq(wcuB``1x7mBh%m! z+@7jpRp{AsAa>)~L_eQK*!vy`L|&pqT^74fT)8$vbm#4q;{r&0iR5%*lxlAkWJSELL@ih92JWMe}6w!x-@4F^<(p7=BT-6x5 z>)68KMuE@{{MlMC@KGotdQn$^Z?sKJ&xAsK+f(%> zqAcZbw>UFss-B|5z~RBfnWp!&FgJQ`Cje;c2qu4tfP~7itqJ4rU$C`Tu9=N!o0CgO)aDE-^o6!h;LB=>1Jk*q99i9n zw7%G+S1~6*K?h%B4@tN0o*Bpc4US^?1Xe%Q7g3zQ-JE^h-lJB&f~iEvB6e|lVw3$? z#Cd@^yz+grB3{^~xSPK}peOmH4tA|id1hjBDLgU#*-$F=soQuzZuq{#>;?IM_XiBn zILEyZi$h@wxy@AM44jCT!djrI6)U*D%T&Q8Dz~a#r~hcENm`_T z)M79dZ>XuAW2@)Uo`Ga@W4>`aNIZW^0m3{;dyGK%OVPz09s$p@klA)zIMbALo`!sM zcnq38yHJ@*YxV?uc>kRP+rE)Nv|9Osj51E>Zdi4NXi;mThd4`A^r$9ltXV2Yaa7C# zf~Rgu6!oSO6E=3vyjwgZNeqqRQh+PaEM&~dWoM>+rl&VuA{(b!k4E%^T{YpwT8!j9}iSAU7H1Y@9 zd~3Q;Nnn2XV+_6@6!*{D4iGgy&?8nC%P2FeKEdBK`*Wwa_ACXVY3M{(r{iat5CK6O z^IFhKk}kYZ%%lr-04}bDh_I4SbgKeEdd+b{`%)}{54G3c@WG=8Ej~z^F8?v6 zk_lh6X?{nku=7Ozrxr3@M~kD}2m;=hwiNCu$(4_`KV!mESnS%w)$7R5na1O;jUwon zu{~#HGjQV2*v-eo($heR-~8BrR$%mUVGjlDl8_jF3W{}4kvd&Kf`USU0BL+`$&x+U z>%>ucR%;!ljYL%qqfO1Ja#t^b+0@a%^K{dYiVzxRu&1|XO?xbv^bIk{=bP$N4fMer zrIz^Dj!_YT>{&DwQX*6b)?~8CT%WJX^ncX<07k#MyZFYv;x|U}bvPWvrh50CCTW2t z_qxH$6y{R>b|+1W6G#?Cj7^D7CL;2eq#krcazIX1u+#XaRy07LbN>fMDrYMNCZ?Je zoaHNZxFr~KJk&zdF5;FHYP?rRM@xIHlosCh7FfouQr_^_^7Weh4T|%p6feR9)m$)-!ES!G)nb@gizjeoV3I8; z7q>`e5zaX1?2X%F+2t+cLG6>Jd8HFq+U@mW9Py;_FiyV&l4OwnyL! zx74MQ!5lCPj2k8g(tLDy z#48+zJ~WMBo+OFUi2!t)LYaby+G^%M;M+ZfWQ)v^)EY5E!W^(ST7xW7kwvN&%s- z1g=ckQ$RV9bDNhE9wL26{YbsAC&CqrzrTkd`k6pG8`sxL_qQ1rQi2trPU%-;b2GPV zLkOKZO!fHfv5%*zCAO(A6vhi8`EnrceqqNBX>1$)d12HkyGb_sxgJP7oDS-2+c>!T zbuU6|%lOC))ARtD!0?-V(;0Yv+{UFhr+IeWcNV+!+!3Vgx!OCUio*PBZ*K`Hne@$R zxf(1Xe5@e3G`O&w3#JVCk}6hHXGx2wDh!)o7k=DSP>M#Zek_GL!B+UCgc&!+oOzY0 zJr*}?_f+n3);q7PnXnIVtGjS}3$vpAEVf&YxqLExtJZL(BY7SNV0}ttKWm2!4pc$xVD1~Z_jX1YFIx)59n zP4c;je{N`xIrE4#1n`JCvdgf(KidlbZGvPr_1cLw1MU;5^{ zY}x)gHtI|}A>NJ$Ok?*RZ#kcJW*{meFM+SKqf9S+X5rp>=hy950?YOGIP;+`&0-t8 z2gTh0X@|EVNn?r3-nVC2{KeB?Gju1#f;o_ASxra7$wfym8xhRw>6>8VIX_E%Mh32x z{R{LgHad=caloe)rBJ*gq~3&`k`@CpNY=EE?Fv||Y7Kg2@dMm$VvEU(7u;d0-HqZZ z3aK{XsYwjMtB;X4ZKDleOK>~0KtN%Wgfdz`8HWS9eLA0;FD-QSiIyTH_}s*>EMn8H zE8NeSthzJ0zK!!Qu27@jF&Xr*hL%7v^eK20}pE7)Zvqz+6oWp=y+g_CgY8;;{1(*v8qjeS+~Gk!qMI1)(~Fkk5xQRMOLn(3^-Toov}r8P zIkekXJmRD?orC_7wEW80i_d~W3OEH;ZsQqVYrfXz`CZSz@7TS|Rv^Y)KzK`IF-SsO z3DIQ=(e8WTn(*<_+~%4M|Fl(F*ua=$X^l-|l>5umSo4rKe0Fw4UExU)oxpP_m+TMv z8veL!oWaH<+^5J-iNNsEC6>8lQ0XhxySi46qnU<_bg`MO1FK^|HcYEQP2>v}HUAY_ zNa@y{^2GX0l{>hNz(VG1dg5gWaf}GTuY*in}9Qa6uuOdCpJy z!&@Fm$X6%QHKhO%`Dq#T^1|@Qv#Y?cAK|2b9M}&yr^3G`&$@^B5?}@TAr!SlcCA&NX29($D*2_WN4NSd#F7Kaz zg8Vh@EP<7W0LIV*(6ap3v=i|6v~z5Ttz8fdnEw-aL;W@FG<7ljYhJ}G0Q^JfX6oc@ zX>Z3z%tX&fOlWFnY;R&|XU<2gsv=IuNi4t*!)tG5W@&87ZDMciY76usqWh~pA+V~Q zGxxtpe8jF!cHH)c&X&&Hc80d5&fG4>-1ZKpcK=l2{_ot}e+&8-#>2+a&YF+d!o|gb zn}NaI-JRZ@h2GxDoPi1GLdEbmmH6MHOpO1-=iusO^Ea1?F@vd%>0e4YGceIJG5o{( zmxlj;mHLbKzpDhym+&7!f3@LXP5fINGb1A_!@uBvB$+z_|6Tou|GzZ=$jicD&R}Bb zVo2v^Y3fcwO!#j*{x^d&6BGXZ0B#p2Lpx_Pdna2%m%sbs-v$BN&zb&jyVCuo%HK-- z>nXtIGBbEM+qf{;o0u^J*AMhSfSCWEzWS?C{JeiR&C<@&#nRA*&e+M+(8b<~-^ z#L&r*keA{All-qre=`F$p#$n~%FoQm#6-u$Ovl8o!oXx#-{vl>Hmx6 zf7UeiGzP99_*vQiU+n+J|BKn&)Xo$r0B9I_poQiC9_oMFnT4L2RMpPXgYbXi^&On- ztxSzw7+9DYZJD@8gIF9H922I!n!fbDR0u{0+9+wd-iMmDB& z#`dmuE_}eyIsZah+8Ubw2g&qrq`lE!0{>wJen|hW@9^I^S^tA`GBkHGbg=k`nTdtt z-z0Z?CzF4$9PBLr#u{4yhk~(-snb6sRyJm4Vg`QRe-Cr+zeN2P^q;o;jsCyaZ@>jN zK*hq<*2vD#(#Dy=<)0$GgPr+8yQ%}O_EHhms!fo|um}YO1xi6Ks%e2k z5XusHg{xnmk+fpY4!JoY9sx=_*x8!q%MK6$p|3*HzhZ?`zaInr{J(I;U`r{JDHmEN z*oP(mG`jP9@eNR&skbcnfl-d!f;V**w2pU>)WE^+0#Z&_em*>SL?yi{iBGFDJ)I$N zbM#~t9!<76yQ(qPTSkf@^p$%aVTWJQzJwqzWF1_=Lmw5%zvlB22EQz@kUcBgdqrG~ zT~XN1dDW(WoCb^`M0Yu1C4{1|o;nA|ns#9Iy@E)JOlJL;^S%b@%Gt_h_$bSZl5at709gRervTmVPYyxF@g zO>^yoP;$|((wCl>E3ilKKbRpxHkr465?qouL_3ZNpACkinsYK~1Rr$@4jC9&U__(m zKH3KwQJt@pwOpz`OwFgd$3aW=$Lm+#_LkL7db3xT4F)2RaYShw26BVC1$1kUx0aLM4YdJ;JKye-pCc^1 zEWkjdzmzbR2WN+TZSO6FV8Pxt){*3ZhnOM|kq6;aHM5(N@8b0ySPfJ&{B2K6KP-r9TNb1E0o?eLG^@s6vlL zzwf9VKew-M%0TI!J4Naxy@-Xvbu9-3?Y8>1f#H{3+9_pN&)QE%EV+|4I`51VnbL`m zi`lZ_J)gA66Zv_$T;VBczC22D=w8w=z%g)UW>7ao!uo*ArSKV(ID?$y%k!_;XT9Bv z7*;)Rw=eS^r)MKWnoW5m%b|rywE4S1uyWqU%+<>$|GDs+-l!;g~)~eP1asF91eYtrIJuf~-D!`SE z|NRS-h`eo`Tbwzqx^Ah<;^hg_yzU>;CHDrr`KI{-L%nGg0K{+!HRoE@RIP@L2=&UH zru^BIQeQBa6vmIF@-{mHP()ztLZ*fGuS^3wwLHJt!tpv*$DRqNdUW+jfzetz#*R_n zZXI-e0&Sf1Ovj-ds*f-0(priUlO)O{F+)M4UJM2(+fA61TyKA@vnyZSg=&7V z@B1tzgc~&0T$lN(&VVi2qWcyJMSJ~ZaHvSve5BBmmzF>_H~oIcX+GJ2>WMbSg)N#+ zkd_s)3pcT)8Nctfn}45u?xNM~#mni`mDO%9Mja%hQlEqk*6iG>nPYCt{Q7jX#Qs~_ zsbhgt^~^7ZvmV=ExF*C&+HldB>|3*gKK7gaTDtUhfVhs{~b?c{as}-k->ZRJn~9R zXdO9?65Gybo@EO&s-I>B!#Mr!!s+w_BP?G|&+nWKnyVWWo1CqwR9gmHm4UW3LA0yB zblf(L65)2%>vx{7W!uQl)5DGCj_}2E*+#!|Q^5A!dmQeH%y~jxM^BqctrrArdjaWV-QP@5U14Kt2nD%~`(^J={R3WGQNODG< zO_^#JS|5{@-!|Oxu`Vo6C-O>4IpoGXXcnsq7qq^2COU`?m9)Ox8bxVUWVgjNB;JSK zi9%}^j_Yfb`pWXlzb%idqwX`QGdH_c8*IX94HH}S#jBXSXufqJ+^%rnIl`PX_CjMzOwCuO*h>I`x=f}#aZ1S&qrh5s_?`O*w4h42qW3CHpm8xPKC=t?e9w3q{xn0 zzj=HPwcf`R8LBitwaZ(X-`a&Hp#Kcc9KMGk687{b8RoE1`E|$U_E?32??AE*VmImy0ZEIp44gN?75I$4i&$$K)%plval#)D~-#UNwJW)j)33$IHW1I-&@3 z#GlU`>p~NIJk1LV@jM3Ljs;Q4#*VC{t_T8C`^E8`vC&8cW!6BGHLuVn_Zj^)K=vG1CsgI`z#x=@!lE$C#WqB1Hgvkg~PWWP4d zdNBM^DKIlrEDulK&twX8X4zv0m*O}<*oNYw9jm1cWmbk&eKj6wq0-6Fr-YN%VnLG5 zV0n}u8o6NlLEpXU$hy>HIZ3H7hBRiA53f49T00;4V>`eld&>CTUEsAf{fp|?U}36B zkFTR=QVB0Ld*g~T=xUa<JGe)9`fUWzGgV!4KKn6>w4Sc8usE1b2pif3xhi8ip$$yhBMJ2zstk7Uz)9}jm#4z zmYqgb@mF)xJQ8!bn)ut}U6{Qh7~99gGvN1wnraO%2;8J7TK+cD2yjfH#x)l?)_-=S ze;iXwK)M)+ZtLQHmRcdX{&VV!HYqIN{S`b7l4meC6M;a(RGbTih;%_ocFj3~J{>lr zDazOUO4Zpw_pQst;q{rGTO$b(7QL?Jj%`DnU_T$Uz4XCOe7T+w9!7ceKYs-nvyN0aSR6@9<)w!Bu~mN&y8b+ zt<#OqkIvTzs%|Up!Eg|TLh5R&OfsWnR5NW8`aV+eiC!%^Va z%*k!u*9*)g(+^5n-N}Um$g3ICsk{4LNK%rMbwp2E#OOZL?}({W#$8^a$Sn_YXw%9T zO4chnv@N3&-#PoxP;&p!X*=vYQ3!E?LK~^o?R{VrrWwystetK+_1QZex|MwG+kd|u zQ6LA?deC0oSE@nsm{TN5;^@0>AbO4V8KGhiFghQyl`ei5W_=eP@|tRja%@st&CtoB zw!@BuevzyzZ5;}kt^xyu7o<>O%?M9tNe4Uepwg(o4@*{wwnAPbh)2i>G?5?Uol^1*yK0uPYxLG3EQ@O84a8d-6*!f z7PYgQe~F_$kMpzki#m^=PXG!s?HyZ_N}$({mk@uzWPa6rR2lw~-qpjo_{*1`hFbNc zJC?+ua|vd2K12A{Yj0Zc2f1`dvI_59bG19ewN`{mNIz`w;y5FCWV-~sW#f_1ei#+C2gYUpQxa)fn{p;j48!p!$7(1g z9IyH@cC_V|lt9~ZnsBg?(w05*43jUI>s=0*-6)-2NbCZpuy`?!sFg1B5@xffdP9Yc zXWgACFJbun=P^&x%65&S_qbQ zv9;TUwLApCr~KzMkC_iLPtHalBE9i3qUL_%H2LTSOOy+2B6%_rLdB7;ea`5RgeQgN zfXg%@ICvIo6;@(zQVxV&ithM&#p5A3cp$NEM~&PG6}3*z28wIdvQ^Hx)P#v^wg`#j zPnPK=93y9zD&6h>j=5HBDW{x)=)9E2@?(*Q14 zi$x&t_U%@6WbpZ$knQwgXQu>MlJFQB8>UFVri$dq@x$O@Y7JNc_1gyDFY%D%_=AxL z0Mrl2t5-!;`$z+;28x~?%_`&DHgSckBQq$RK7>!Jf$l{n4Tq|`)YN>1x|@Wyu-_VnY)dF2c+*%RXd zigU|vIwi%vQ#sthuV)!h@%rzi%63dqukeC$-q-yU(l9tzWckJMup>IK;x!=3tDffZ zjM{5NUr9635*^4U+3GlEY+%e&+`gzpT+Ytq?sKh$2u9L}-@y-BRy5*~9V8HVUvt^7 zz5`d0*6;j=?%w4YfV-tb$L>Y7EjXRZVp)PTXpY{0bz1^(gHW1pbSbZoBoj}jH^c9I z-7{iwj#AUH^?9K2Hq&D*W9o#vLcQtut*qF;zw0WrE!BGLxD$c_ZsoFd=TTqOSpL?b zlImz)EbFHzREb8++$8o!*JG8@BVP7IRK4v~4clFFziQ}hmvv)AZ5qppS zBOUtV+V2mEDZgY3j=Yegk=bFi8ArxjlTpwVzqATuyGQe?jLLM4c?`stgRrvq8BOU z3+I_Wwn_U4A$9B2vFFa4#6FV>I0{FcWU*PT;_+VaTVzHS;%XpdZ46j+wMND`vD~Z} zV1-lFOUBTgpDUb#ie?{95^k+;zo#GUrD<9E zwwU7gK93moz1ghI*^%pyQNIQGtXa%m`Gu$vm<{9ylSDk2uU<#$Hfq^;aZh5zDD{}D zr06`4Op;CWUo(1J`$H*U^{KHY_-)$xFlqD`>ryOLqd7XaVsar3hc=u!2rTeyspb{e zvh|9l@^AIY4`VaU`OIjqwY0!6XQb`;{y&wy1yCg0x-^QzAcMo;?(QcL`nBZ9c(;9VC!cbl>+$@O6$-LVHME(y+dlL)*eSnLTL|EQ5PJq z%PchENB$y?2dW!zFrMY_KMyYl9*C1i4kqk&Mc9=&x2iq%^=YI!$rtDaw;nu=@+H3! zIm5P0!!Nx%FbNd;(rn)E{F$Ieh!tT4q`PDA$jaoZwlNH>6)vy2k!jSa$Js*{5<_#p8Xd+) z?KZE&jFo($O>OGN&MO2KJ9HS_&&Q>ggtPj_av&%j@r>K!>G6|(`Xp@_S_pL)5bBOB$yH>Ab}&qbfnH1>3CEEc%baf^+F8?}5IJBcs3 z2%S+X7%H31W@&r!i$lc_4L7TODEhYi3pd}H#M_F3Re@m({8`$)#(yR-1|!~3XY2Ob z;d4IuX3O^#s9&5vHy9=eHaEm;(IC{%|2&ZA z5uZ5uONK<=sI^uA18pGH)MhnF2i^AH5o)R1-`{#6)CvXBM&~iC` zVr_wIW_jP?H;}GX6l#gkuX;;FMME0X>xLEuY`! zl4-e|5*&7WeS_IH0KZOd(djym-5^ZgtfN41pTxpxB9WHVpnW&tmR|;E^FcVk3zuaz z$=eVO9HtaKd`V8s#`iCH%}yZW)yQhgbqbm4IPp&y@xw{f3cIU~;eN=TY~dSA<=;6b z+8vxh8`g95=_al3x{VY35^S{FKnZ4&ToZRV(9g^b`&t1e6ZmO2fmOV_psCMd#!(QD#5xqVoeGD8LMRaWVxrZXN8jM zlw`qGyjeZRTKq^@UFDx>Bg|?$`_*Ecib^AQsX|vWn=FVl&DPwe^%CyM+qQJO$mOe- z-oobYadql4I;tuftPWidePt!I>YiQiREd&HJ^Ee1I`-3OibWhLf^L86URyH z0!)hd?+93|`Z?sJT{enLp{YNNj<%tkmAr&u8TuV)Q+Z75Vxp0@s*@iSPDu*%n7|95 zF9YCuVmo@Oz$R@AHMS^(SldGM@>Ngz4}y$wO#wTNFgk!qU{z-O-@sCAR+$-7FWz1c zaZ3mG$+~(Q=_O8;tENTJ7D8R@7Arr|#t&^c>l*(?x_J9jH?jddjfH#`Med`d;w+ z?2bT(gM#bf*k>>;CiWAuwij`SWw5k894Vb5eV#7DuDE+++&)10h+nx9zK%wXm z-zsc3lGIvCr5owu?&pRmrvP}!)tjL*$M(*S0e}2~{ubyYVZxaFBTf26;%&6GKYA)T z&{zB@@E+FktY2!za=}UL8&=FINfR;26*hNln^HGrs>=?r@W%BQ!Bw>c5i5%u;*7WO zI6g@0v?KGL1&-&mUmN0?$+zo82NSTPxK7v>Q;%DDA5}T+SAk)e>J)t#IkucYb(U(( zjRFW}>85`VM^Qk{p;4%Etmiq&fzeHr%)ugKyn^ zm9f*Cd>F6BUcBPIi0{-0wcc!uKXUoFtW96!ewS4xftjHK=>iY_ez|13zt%&O(EBEJ z?f`b=l?~Np3L`x_#owXeF4iIyi|S!xNg)Ncfh!T=92%loeafsybpW9D08^2SnX6mh zn2fQQRhC(To%P^qLr?iB)VWz+eufJAX3aFK{UdWb$a;~3b^h$8DC>|YyyolmR9O|g zBaZDEJ~mP%8x`}Lg`3VifM>ma<}hL=WMhRh1ismbtK`H^3=oSJ0O{XPH_Qhs572By zmPSU;_-sgppmOds&(p&U)NsG#d-7u73z+Y^u97GS5womgXYh%~hsj#pn6raUl;b#- zGi5{etv_;CW}zE6-U&K06sOrZ;_EmA3;Y$se%80PPdoLEKe~zEV{Ba(GW@iuFF@_d zkvwoH>r`>sO^rTQ8VZ_LRcTWyeM=7(tRYC19=XKAp)>J-XxDBT$p}J?QJw{-aEmj{LPx zh$R9YW%4FOM+<<7M6uSE4ejuKfZz?lwWCGj1{(yQt(WGH`iVY8`0IJl*(d`@cu}2+ zJ4_EJ3@wg0SpifbI8bSF;M{_+t9C)@nE9emE8yB1<#y@H9X zt|wjK?TD2bXoFV*|4fzIUx*qF9WXS~grGec1XXo7??iSAu0D7JY?gSG!eC#U!)1;k zsT87sOPas6h#D(J(n((-+oFhu&Csyf34wW#V6@FDWfb99g7LXEcGcpx)xIG19$${q ziurpcv~`DR)t$87SXovs05k`+(kqin(cu_zb}lW(pp=dF8#{$#4i+6i*#Xr5u}88R zN$F0zPDY-sa>(88D}r#R;dZrd>jJjAtpbS(11~KF2XD{6Otf8LcsX^!H@vo=zk>ilcOg&M+*PmXw@+ zW*fcrjlzvt+n(^}eRkLAS2CpB=yOnKk3q9B$f-(l=Mtwq`PocI3wB$kXzkun;@))= zA=#OdA?C_)8ePSS!V|NvZUm@zbHq=HQHEnWf}c+mX%dlYibY#T;7)G-TGLG~5HKU(J;GBeCpdJPu-u?_+D$U=57JW5d(@Cdm- z_MHH`yqvvOV@Ga5sQ{+Q*bA*P-6d7nZv=|Gv!k>%)*N|^xsks|OHHEi^>bKlphR2^ z9_Lv|a79dJI0O8$=eh}4qY@kmyZZ_9MYZ3wt{uezw#^yU&>6^=Y|uM0!Br|5w0y@Y z{?!p^*IQtiZ#kS07$NZ&qg=jwGLiPMTY?>+1$BOAo{3MMXAv*PLK@s+ghl`c{Hh(8 z)|Y*H{Jwi5SVS7nL!F7&pMg+;zs5>6A7FxMEFc+_{b_VFBA9wk7IJ;z$E!{D$t&$={>CDb$gNt=j zQkzy#%69w`e+Z-OpcF~KhlG;Pn+!opvXpr*bz0Tx$@JdOw^TD{11C4bh>Viz-w`~| zFwp#N^h#_K2Fx~H1%9{P_4E<{Vwm9pzwcfQ;e zez}}QbXxgt62@wALX6dDFdD8zTQ^mzb+3TOWUyQn6+CGOKh4{=4&gWxDHI-~AHlU% z?d o|^^q3aZ$%zS*lZC5%eXH+aUe8|s zCSu7gD(rvtDeE04+PB$fV&Wz{)tbK5GdU6N<{Y|n2a-&`!Ks{^XLEW!V(c3X-sMvm zQki4Ub!16%unMCLMPV{>;;S5I>So$f!f557Hn``y;;r}emi6AZL%R&3+o#5@N!HKg z&xQ}^IFJ z!Ld<$PjMxa%mb-}sJ#Q%Wkb{mHHw=ti~6~e{L3_Ltjt|15^I3L$ZaxJxuP$Z+GNYA ziETxvE=PfhQr2OWb{RAdW%E5a-3hGwUJ42V_~3TamM!;pf*7!Ub*%jb-qm91)8*%- zr#j{gG-24(Zn@f2E{7_faD8wiw$0MGdcj@nI!_XIL!ZPs2Y8KsmH-u*yzr({usx-V zGfy(b3Hvt4neG5ahBe^=2B+BUu+;|0o}k}SpR{nNAR@#Nl~F63jLySFC&CaUMn@N+ z_j!xrAp7?%w#bH;W(1t}bA@47C5%w-tu$m(l{k)ev^Amb!Qml7J^icgxwXp$Uwd&Z zZBqS_%q$_qtcum*88VcOLhV+D34XO(DV@Q;Tvf=2sO!Hr;t^gy6JP16LBAa+y_q}+ zrm{l}s(Eo?JA^_Zq205nNeu--=x5z{2jaH6J*eUUH2HS)Q+N!5th23?bcmSVk_ zp`Hs1*$}yUJ-5F=!UGSTL&>dvBtl8VVxM47cYPoipXZ7RTxQr)igXccJb*1qQDXHJ+Q-Sd-+jjm+ zI!c{8DMocAE<{CkTQL} zixW2ZjP8nv7h7#31pPk0J*i)ep1hQEHLax6Eg zO49K&CbL=XR8v0dsR~?#myzU-ZP^nOTyC43#a+9?bTXL^cyd}`aS2oLqemR<0OJ7B0Z#?(0Uk(z0k4c51;j+4a0IZKMP5MHtCZd+dVu-UfflWfm}n%5e8 zQFN=X`iD0AAC2VRww56Uziu?i-L@}4EGgGWiNC_v*s$%t#gfz9OX7DYOv;A-yjy5< zLN=JNF2~g+J(o|?WJp*!0Bph<2TSEd?=-~sr|W{<3PHEnjB;Mr)h-k$Kq(f<3S=X? z$5?YBAG?0@tQgHW;G{i9ygMQQN}mt|oxjK|O-U6F{A3COuFKIr96OQNo~rg`6LgV; zgbFUL;t2_kaQe)4I>x)WjskKX^=a#M&;?O-%7JlmyP<|^ncxk3;f{}lrs5{AzT`^Y z_Zr1o>Wdm^koYn1zb$b!f=Q;O#)b0f&>b2M?BO%ITv?$=$ z+g=(Gc6w1nyD*kq`gI6hi5FL~{+-2){*OOk*EunY&PB*_lWJ-pB$iNBv{YKx+{&7q z9whOn#FFn4b^&)B1UmM z6+kf13IrK5OE0=-UH#+(>*J;bbn0526GF zWUD3wzJKx@f|7FxO*z`=49R}a@M({CPZvQw+|-ntDo)*L2cZdwAS9kF^EiF3@i$px zLq0lo$r(6c@}73tb1-&ns&v1>;uY3EgGNOu8QbEmJo;r$C_447jO$bTHMM=Cm}E3_ z*8RbOg%`{PB>{`S9vm(3z%P@`d|_wOPsD9XdZx;(_9Dn>1^^1iX8TsZerP89)xK!` z39;6H1RBQw#9nA0VjBTT4QRE$|vMmjgvyW_=DBwMysI~#hl`qn}Iy6-zFIkUcnL^g6h90^946?y zQfE7@q+G)-F4s|qs*yVOBW>~{*u}Yx_^b4NII8n)_M;y;ZS-r4b*9u#Op)M%En46&5*W!SEb@Z zQ!U|+T6e0+c_Y31m-mhYAv(6UEg{fC8I=LY@Lc&Mx;!?caq3;)P(P3|>Lk)t3R^;YIk5suR@T8ORAM43vn($^ia za2T9A!5>j*Sg0xpLnS~d^VaA2#-{b~@MVS+8T3Ha6g^qr%F=hC$ZOy}8<{vE1<0BX z3@tuHH;>~JB2HO3(A|Y^AO}}j3@%HD&kb7G0r4+z|5ys%`VlSlR4tsK6^wdy&;|15`@8D@t`VLJ0;A_HV_8U8=vQw?=f~?Ru zDQ{;`KA03{8IUj2o~8WA*25UVl*bnjl4DT4o_AYa)8J#ji&K?CRS+f8I;kD#1J&%F zd-C+`9=#;_Uhm3EM=T%MMus?lZ_cmi3tR9rICV;Q>=}?XyJ1b^5@G!#tW9dCKI?1( z=r+jZ;#1R{)2;YN);(9ul4`={8cU{=cB$Qdumrn6irgJb!eGqy^!C_U)T31>=N4%U z(k@D~&cWQ9H#e_|TmIZD_JAxOoOPXMaC9lpKmkme%2~06n8cv+C}GNynlX_u%}-(Y zC8k+xd^2w#V@o#T#!Fz_Q4`?O#sdZ91SYs9@}_MOt@4~r&TL7EHz_L}>&OL9Zm;9xh`)z?6mK@tvUb!+9B5JS+B{h45Hxgye zBpc<~TSr^=Etqy+WKzZGx-xJYMYlAg_keUtpm^kt*DrH$v%kKTBXf?V;J(T}@7gU_ zYBbTqCo&M*UiM_7&v8Hcu|01RLecy0m8kc=ap0&Wf~S+@*2zSMuE=zzBEH4pu%L#p z#!-ijzPcn2DG+f}Oc`pim0anM%OifYB7Q2d_v-Q&ck;H}eD!q3!|-AFnp_#@Tlf8Y z*%-UWmKd-cQj&OX#0AW}St5RcVu|XY8M|d+}b#;9B3K;n&}#!ot_#!^v~i z#4LS>!TX5yZ)B3bR~y2W5mP!UKB#E^T;NaPW2wnvWojV`hYFrpA9|H_*={;Tme|Al zm8wR*BnvtDmKW#TS~)OBNGs-PR@1;3O)WR7U_)Lw(gY2HTFUR=_kLD6RsBRoVOEo{ zgwM)+TruEI$@Uc%ywtd{ZnN~wN)Org6$6OC1RmJyQX7Kt`TMz61Ab{ebxhOKBCNyxzzCLQn|*Sc>_Mx$`9&Rv0|Rr;IVj&b9gi*o8lQ8clIUbDp#p0(~0QKN2*fr;N+i z4k-v4OA@o%VxYp1nx)ak8(lHu1oNO-Q2}YVht+!Q=%Q47FMu*xJxPdrPKr!)IrUe7 zmbd=bj7eI-fFTAc#rc}pH-AQ@C~wW`(ucYv?tRELii3$I#ZCFFozkYY4nMKwO2@+- zP;(S}lj!d2#d6rr=DuPoTKBwO*1H5vuZ9OyXPpY(i?G914$cL@Av2syg`)b-Nz|T+E z!7?+E`U)_o!mp^xXcSPG8^hZ>L#jgg62&~(HKHG>?@k1E<`F`mt2!qU6BUa@qTlaG zzoZe1v?_TfI!_hQi3jWOr}QawyzTBCS##IHSu7z+Xg!sFp|J#ch?H<$;yePe7p#Ho zAsjk>7P|sLCCS77v1*3V@*@SOvl%k{q}!{+r87H!uuk3Nogfy6FK%Zx*guyv#Rt!f zBl&x}vs}Aycx}lvI*=`se=Ni}sxKJlw19kSE8SNn3X@MG!r}s~fdbTj2t0NYh7udg*#%(17n}`~Z>Cwv$I&HxMD$%Zt^oq{qfQfl^SLcP{>OD>12V?Kv=w7T`q#2c0>^w?@!t89t>} z%8q|be=JdZm*T^16e6v4m?{N}c&+*d)sj9j+83<3VQvBKt8IvZ-TqUJR>ZWi(LE_n zwDYub!%CkUCWN0$6pgrD*FO_F(CBV+%09A!+AL;~b?^ z+~ViP@SN9kmS2L2jMP`498kxE_dZfv~m!3zdu6bXiyvz#|GvdGaLAPJ9rFUO!MdU z%t%(A`>r=(hlmv2tsgnV$Vk;Ulf{kwTE~UCj7_kR6=^qU3w#E_yv?B=S8t(u}193;6P%cWU{ zt7L~1e8!!jY6BZv*Sks&-=Ob{L*nFSkvj_E;l;q2rQ0Ogcza+bDYA+cWF8JPd?GX4 zH>g*Nh?feP7-r(?hOb|Sf@pTaAhI$kt5A2dFwT@y9j@&|CkN1kWdxJ$v3y&I^ryF` zOx1s$uXWH1$DWGJ@bkHi!+`Pj4~X5D-w=TAwMod`pJ;mfSpzl9h%CUb5y;Ki8ithL zvQKCk({^3VMV)w$J;Jk*2u3neFtK5g_TC4wJ&`{J=#uBTX?g9L&wEylwr?yZTyHrf zHNIZG0m)?RuI4lk)&4*P^(8GY(0s>CtnxKCvD1>PNj;@2R%^_b%sA|d$z0+DSwgL| z3GC@`d|E|2I~o{?r?2boLp7OgG-TZ0oPWA)%rQ8ONR?2J^d~|d0}$kwsRT<1ft#UooE`59$ufr#Nd+8br-Jd95Q~~^>QdS58BZD z4r@(iRKH`4)Dz2yY|pT#%H%~w2l(g?2(d>2k1aIknu&R#OF@#~^ER30>t#K-ZRV#V z=OnAE*;Cx|qCMRK?hB4b1Mql<%d#lU0-E-n%knS9bHnK6Yr3wMHUQZ4P;Q+;DuNqq za}wM}4;Z{W1@Nq}E9ACdQTdrmuCh7ZO%4y2xpyjC?&~nYdh%xj7`Dh27K)?z8tZXO z^vpJKZDJfR^?30bnwZc?7D;u~(~b;Yk~;vncI6M_nahiPOj4szZ8rHlb1R>ZfCqTH z`ri8c@`8j?cniBw)Fy$5ZIz-_t)q$Z&~L5j+cV}Umj`x+J+2HRZG;P|;~1_wS>P^F zDujcxcrbBt=Q}9|iHEhFVOVgh_DOG4K=QtO#O3hT~&eGObAEc@>hi&m9uAhD-uu`C-O2}~4`WYm6F zRXYq?a#cm37^JA9IfQWPYyA&Cd0Lt|+=nR}jh*gO_}t4xFH@x8E*XBF8q0~*{H zVLl{(f0_)Bdzs)#E;EfB5{jXN!8}|m_++^UOCA4Q;_Kp=#<{2okhp1M*e2L)47Wvz z>z&0x*kd%44(i?H+lZVy^`lc=uGKA0C68Mfq`92EqRcb&U0RvG7J2G;KVh+JXWmui z4B-17@4|K(u(!@z(aM+evT;=CTP=QRQGzr1f=?l~HVT@(}eaV;)pzM2yghPDUlw28s#B#oY8#@bXr?@Yyrw-r6)&XU509 zt4h((h(BpwOxEz{q;u&>Gv=i>XLJb$1qgUui_U&O+XdhO}uci_0DN z-L`wNFgf9gK}LObAQTZV#1hm#2^P@6#bKsLI+J1Mz;241!2H=w ze?>H&Sc0o3H`OF>ihhw3E4PYS)!FE2ooAl zA*JgR!{k?ZL}9pfNB9%rFQ2xAB2rT6aona0XNcn8HrOKfTK&zMj~Sd+kw`UOnm2&q zf|~sT-=`zM%bRS5r9`EJbxz3won*|HnU3TuU*aMpzjqA|#RH(uYp4*U27u}n2%-y! z&FGx)bh66>_>j~L>@Lr(+ACn|JifnBiZR+MfUM32(U6T zHU4k=vK=Fxo`J!KTm4^VV>$x|TigHc#{v33DN+Ar*Z*&Zf4yP<1I>p4+1S?5&Q4#? zmhO*yseecQw&4En0T@{TKJ4887Yg$qP}Trz2kQ@yy~F?G)_z0&-^XYV09gFrD7=3d znbvN9bv$ix zI!@Bz@>N6(ajU#5IMVt7MIY9?z5dL4s>RysblP!d%@YY?;{PR&0m`G8&s;r+_P{1v zed*MVE30Z#;*wBo+M#@@f1|L%krg z837oCkfg2k2^7C(rqFSMslavU{S>D6!%A!Vbo!~A!i1pAq2R+ExCT#{;&5&4GKQYF z)|KNEq8aLg_dzPSFx=`I#?-wVdE#luj*nB1TlJNgcVs>5RjZ`0RwQsUrBGr6`W)Bz z%NcmiI*%OMmPg56A@#XP)Sej{goeqiR!DbcMVuu3hD+m5c<( z+zoD9uv;#uB<&=aM7Sf?8mzh{Lf83l+O23Ta<=~%{`Kh3(h?<6P%QN>?tjZMFc&l_ z<<+g07!2h6m<8C4R0XLfW61a+eLvF{_HfZN>ecg@25VjIh&rh|p@(nXn}#w_6>>)f zM{%>P6282Wy5hQi@CGaYK&h`E7s6M_Dx#@o3>!ez{l*{>%WDYM`dNxMrsxxhEBQTe zTUa@KP_vh$KtbeXhW?~^`Lr+y<}c|Jfo*==F|~rtyJqB1q-@%|QXfmW5CA(rQi+~m zbB9lN4+N;WIL_}7UW&=?iMF2snQU+i1C5|TuHQGUTb)-T0k+TjZURc{WR-8^i%ll9 znsZYz8zx8YgbY&qV*wv#Y}~n=&kbViDCsM)o_Ce`p+wO_^s)?#5yny0z8X~AWN6+r z7}#WC37x%;`?np8K^^SkRz8*vWGO@IAKbP9^ljtWRGH0`&(D4AXIJJHaeed;j=PKi zyZgmQ;@w2R|Bja53ny?p_hak_uLW7Wl{Dk8t%Oc@;490EcN-rfxYqJI4m*9f5CMi2 zbfK|g{zR104vGTdGTM0*Uf>s?;PZu9nySh0oU-D$BBbrp@nsAq9$O(5xN}`#mv{lv z=&Vt{wwtC%?mkF~on5+|4j}%WDFFms8D>%;&0L-MG@sr#0~C22mZD$KCIeY~SZLek zG~Z}Q0_Fu(7UQzZ_#K>v8gW{wvwD0A{d;e+)F=Y;6mon~!rn<&u8+SIEVm|n>14X- zfb&qGUAhi+D#9VMUIMSjGDNA-B<)pkEIMts}b@}r?dCAiyav6CkU8MT`22Ub3F0bew~aN9w6cWdNm6sJ3i0t zM$(Wj8=W*^J?Z_rMNXP4LaQ){8IlsSIfP#X6AAd*Pva~-RS(yHT|p`dA5(s!*k*rO z|75S0qwq-trYiyy+IQviiHPIXDZTruMfH)-lZ#2hx;>6W=#rHrG=zas>9U*Yo_D zrjl#X{pCau4xWvjM$m+XiExRGNf%T5u7V{$k|KYK{0f-62kS)I#oHr`r5_2XuES2@ z#N#t>Z5lr}?X5Z`GtEsV_la!{` zsFvmdg?L_(X&Gshm(fKd@*uBFoRyo+V^#Rb7lSXOgfa09LN4)zuO235*P#p=%dT>$Irhzm}&Pj!v0oKzn{NTi~e4grXLX` zF4l(s8}%9`jx}@w1oZn!^Eb#xD_bC0Qv-X45A8(9K>wFwWB;#Tn3>p_Ss7SG*cn-W z2bKIm{l{ecYn15kmTG^N-s#^Oto?7`UjspZ>xe(g7XtWixubs||5IS-pUArph*=|0|2V4w{eKaAD%=17 literal 0 HcmV?d00001 diff --git a/TechSheet.pdf b/TechSheet.pdf new file mode 100644 index 0000000000000000000000000000000000000000..b0b12ca4200218bca112f8bc7648ec9d1dc657a1 GIT binary patch literal 228377 zcmeEv1zeR&^Y8&dx;vz#q&oye=|;M{J0%4a1Zkujq#LABO1itGyF?nK;X8y^y;tvj z@4avRf8V=2zkOzB=h>Z|ot>F|&YlCwV?F_DdKxBZl7`fVs)n+LL}&&AIsyxAV`xrJ zS}{Fy16xA^Mj%F-R=~*AR?nJNz*Ngtk55m>LRas>185stYdtM9Xot9PW#NKx7Q~tp zg%#ZQ@WNKX0N!UK$<=8N4-xn!&H9sDO@dXA4>%a(W{E^10su}C!te$)qb*VplV{~8 z!&&A9Gbtmt6SNwa4|*nP@!z=_B3-y#*x0*{uS(tVqA_&anjFWUY^mJv|2S21>Llyb zFQ}K!#p$e<;o>GMag^0E@=&9tNoz-tS;E=oh1!;D%xCoSgdsim&56`WDYXow2=k9D zbscwDO*~^QlqGAc&q^w**B^8#+TgM6^0Ms(GY%NtjRR3jIW|IIRjB|O-4ddi(RsE> z#7(%P<1m!T=!*BO(B*xo!VKAt8E(k_ExL@2_t;w*vmUbn4iX1e7OBQ}g{N}Txe!;# zX6N}fZO7ve-=-ws0^JS9I#xcS%;1#rih#5s z`7-up1PDYr4Mt{iu*y)?_RBjVeh${fw}-5FwxHGmlUqZ&=WPmb#}JU;3PeVvePdyL zOB41rmo0!5ub#pfTp^>MPcrC|VMqn#)1pH(*F%TVrLx{gRIaO${yiM|@v*A^Bf^LV?dJRay{uq!fl0CRPn(Cy zq0ra7CMdxZYKfKL7Z()r8T#AH+SyD9Sz1u~Y;CdJ_-UC#OTn8mqhZ@tL@c4n=icRf zT$d`$6I1!PPLI(uNNF|lyfYOE{l)v+6Q$Nj_oTp{nu$p~dbBk^lem80(Z~qvR?~wD zf78l`oRrcpQ)4J}d8$%GXZP6o6Q1|#uoCfjX;WtF-V5}ICdJ_jKgQ^yCZ3_F3_aG_ z2pU=JgvqxSDFXx+rgPz-hZm-Cag_`St-qMd2r=#0x*yb0l;&-}B_K@XrJ=&ck=e1> zC|TC80JWX4ih=jQqr$2XSt;W6c3{MI`xf3H6l4sHJdft|qok~yosiu4A|zK^MRg}% z6~eKP3+_*O{mj>CD!?~D3z)WPD6xkqu|2aZ?!RI(CMJkUAXF_XHr+$!3Q!Zv%0g~b z&J})S=JVP>SBY}h^!6S;k=7?ap@mymP!I}OlG|P)t`CVF+4)>?m|L>A#!R)G`ylWY z@(}bMr&NrGGA!IGg?$#2!AF3-sQQszv)kl#+=Zy)i}jac<9#*FwpOq8-?jGnSY(Be z)I7%3n4=#$dCNEPaNKKKQ$w2*zttZhjJG1aQpju;rG^MfVfy_?BW26bFBOqzTNAGHrpJQs!Yq7DN;#jb zt`(w4#D!*yT`b-}J25ngl*C2@9Vazga9i4(dQKQGGUnMbXMQ=YNHoF%Q-X<(i?Bcp z5g=+!uZdmBLx{XW-tq531d>E;O%OuuEjCLIIDUKKT#eR z5%G`?(TpF4cWlL#CTsmX*CYWLdc!lV(-Pu^t;&32jM=KT;?E{OT(~-JzVDz&v|(VL8Ak6br%u`B36Lf}zj3`;Oy6rv`o~WOmlw6x@EIgpbV( z62b2ga_q>1PV^gC82vir991Qt{b8Y#`-frHTk?`YUXH*>o0a(9PS`Cs4c+p03-9>a>fwiW@JZdwvKRBc^Sqd9U_F{t=GooF=K9-3h0I&)Gxf zd*p%@2ifvrg~p5k#nAp&<~Zr)se|HCON9W4K_UiPCi0-$USm;Ip)M;s}uLs zlWwMP(IjmYQ3D|f&dZeQ0bEhhG|O&!Bxm(usgBi~t_|64W^$PqoM0b>px`@F>uXPQMk$m|nUCfrrWXgA%m;Rb&1hpK%ZEspGXhi5Ja+b#Cv zY8UH|!Ja^N`>^Z_rtzqMQkWKnss*)Bd?Zh6g3lI8 z$>)7PL<&iI!lXMhM-nJY?=7}16_=7Jd*o6%>uJJP_#qu7WgS)ak;{?7UtYk?$Q*Dh zb55v1>zV7`cw!)Q>AayYefA|l&rZ*NS$&0M`s&(c9WC`}6(zNe^>l1$Wu9o;UY3Q; zv<&oU6^wL&PM@BgfsTMyNYBW?(3XInjtyAgF|xIJtY^(@VP2TU!#VL4;M24L=gdD0RRXH0008~ z16+&)cmcOSK`$?00s~&)kl^58VBpXY5Vs*=p*p^910Z<1`hR4r;B<35+oQK#Isu2G$3k(zl5?G53M1$V?S_LElxdjS#8~kDn01L!}BHls-qFSoA zIg)P0_gGV|aYbX(YI=2NK6^^9gkge7vyWyTUoI5nza4a$` z=6QKUUH~MQpVk!Y#(E`=ZuC86Kmwnxo)f`(qHqjxCr*1lZ$K)-ym(OJj5DUQ=mHSS z#kcv%KaunFWS?-6kO@`lOy(?|a(m388*3?^hMs*MX5aC71KWdPnzyqO`(wI<{EPN` z0!k->pZtptdOk*`1bDIe3acU|lp=V?T>zTWj>YnxcoCeRmMIB3Nejf_vOdX^GG7bIiWF5B zIDDDUi55y(Q_xDE9QL?7)&yO>YM`+X$u-LBcs0Ngc8D#GhmCGbs*0uJ#Rx_A|y3M&9o6Q^410JDIUJYhwUNr z`nM~wsI^?%(D>;~;gp_RkI%}sWfv_`ACy}O9l!IDY6k+MDhl(%5yyp4aGM{^X%f8XQr2tSc_=;L%Pnc7pz}c9-05Q`rBgps{hO zqeb}fh})xwCg+BFly2#=PsaGVu}_H^OQ%1}J{eeN^6ZxjG#xoTTnr;6E%r4&!h$Y3 zp+<^eJ{~jlbH10!BwW!#V6qbTyukl|*R=#Zb~Q0izPXOq=!AP|z1uU`33Ltw-z6J! zn)@eOc+m@gRt{zIX-iCX8CjNMUKtM>?2<6&YuW4?g_cYoi_YDD6ZCG#2lkj}E-KCW zQ)3@h$ZAk6t3H+M1%P{>QD#SNfEkBdBeP{A$AGWbbzaGvWkj*3L!TlkF&ScW>`FsDrKf0DB!O21A9>$CkK`SHQ=EMKCc)#iS^PXUu< z3{TSCRZr2}u!oT^R!E%cgEZ2qobEd`2Bo7qEzAg-lS-|HKG)%5kLDY)7D8Y zCeO&^`Dk~qjmc9xhQ}wB{9l!&$v^y8HT@@<-huOf_$Q}>FkLJC&pz`HcL9p6kFEO; zQ~z1Q|9aN(tU0QkwJ9k-B-3hpSNjruCinZ0%wycgujh=)|49m?kWak{$r)8hT$wz# z;41~KwhvSeKU(-#3Lr5dvte@4mo|VD5f=b~*{L&zLKT1ce5D3eES$>=^J!Z9(2E5# zt2kXN6@RHX=DOfvx7Wpzym-hvWft)5M4yM-uQ*|du4Kx(?*4|8W*Juf@Jj~2^4hNd zsXqa(i^E`gJZ3N}c`SWjOrcasJ@K z{ct+bAb8~yK;l=T{|S=>-DTXNyh736!fkjdL=d@BIkSh~iWAdr{-ybYGr}3wD=cF* z3dgtAe*ojQ-ibvTfA$-ye{r|+rVXqgGM(uc_J7kgh*F-Ci5j@F;_~_xS2sHOmBU-` z6PD{O)2Wxs1(=}EaFn{=Uqb(2%K!ACyYsiS@+R`iBah@G*iRl_R^BYcZ|EAzaXG9G zRuMpFE|LGP>BaBD*2{^M;WyGV5jPP%w?Fr;=dCXRGH}~I*+BD>lYcNN*qIvW|CW}L zIfvThb&e?R+3U+naQPv*((kEm->H}#52_V%z_`SB?}>f-zW#Sh&>LO;pWgq51YEyV zFV7auNGI`fC;&kASa#X#E9jT(J8@%8`NTi_rRom>5Vg_5_^98KZ9Jv1gaiFX{W`WZ*<<_e<3u1K{@h&32leE*gFZ zT~gRHc2y^5Ca3cFdn&E*z&jNudoQ0`0`ah$e1BsJb56x{^(jAE;kv_|7M{nP7A7Ad zrK|*8F=*d`1_KdFgU5nDv!j4ZH*7b**^dYQ9>~(4w41cRxTI=8 zbi9!>=NI%P@Y^Zij?2IaGRS+$iV>Z-8@Xqc@yhXP4*u3s`;B+~hKYZ&Z3De8*eJuC zTj|W1@a9V|XtB%&CfLJ~F-Fb~FT*vzps((~(T49W3;^wzrTB-d9R!y2alPI@DdQYx zK}wHElT{9vGM3}dl0*M7nBBuh9p|{}u(+&GEt1pKJ(%q$olPy0SxS-okAdj98;1Wd z@m0eqY!jAn3gg*>*;S%uKj=x`o>}alA&8%BNq%KKdv4@+bvwTiO#@AW9IHW6n;hVM@1G7Yp|psCp>hC9w&{`+)8f*h9) z0hoSj1+J@qlYdp)OLH;_E-)v6JMs0p(_x1eOFyTKCiLBt-`JuXn_atj;wC9E@9*d` z#6$_zF@@F&pzjK(_SEk>-8@4o1)0AI>i@D1(XF?@uTV_fIHK>k0elmYP_h_z4|8{M zk0iFbjl`>gG?_jFMxEg*pV;jAB|fp=#O!}LFuE3!iuX;F{T&@mxtp;2vaPS_Ygh=PR4i{- zcy4rFbGEB^3r;N%MZR|6sLusa1m)}x{f6OasYD~`PrhG-@S=QzcK-YzHyjW z%Nl8d#yYF*<9H}Ai?)R)Ss0G0(UZb{-%RKjPmuex@>L3J+JX5O#P0PZ#IC} zX7F_2%<=T)nd49|u6*ky>35M8Fkt@_rY-wj6Te^hWuNf3W(I<$#KqspSr7VTMH`Rn z*=3i1($oOHzBstt3cSv-y4j+9&;G9$@CAVB0-)|j2+W??LZB60c8oGkm*^T_-tWK) zcJ=UoMp+>{^9pR4js>}{4cvcB9M{p-qsL#g>n}&Tmm8LM(B()sLH>91^36W-%0C3{ zO71W3;O{RhJhW0F0Fqv5=^h%ybvXWKl>;KNmjWjNBs(G_bojF0zoH?lE>rn)4RJ%` z-@ns=YdPvTW_fR8f7vc@A5ckx8Ga4^L;2TtZI>hLm8-bfd42mZ!PcgB?aDCjhZZZX z2EkHIypPLPS{-+Rg&$nGO9>{)(`kd$=W@@Nzumz93+C;c-QqW%`&-uktZ4)M1LowL z-QqWD^IflhJzo5|xBX^E{?*TX-|N4?yMFUx?HgzRUAKSIGC({CALuD}WBYzt`3_r}ee?SAn=tBo5&nf3<%#w~U>l&E zufDZ(LByBs-fg?gA$J}Eb5Ks7U*4WxcMs-B0K@|zk1li22M;fAL6ekOEh3`#u~s2#Jw{t2w_z>VQG&``PmI=^b)w!a)l`vJxYY1 z_o$fJVB-jcpZ9d_nd7(wZZK#k+}=LR28uB}RVJjbQUctg8N>PAuq_2{1GEO_=7g=9;yW|p&n>FS7{nH-|?WP)BVU+U{^bNfF zyaKD{N=(ySwlzQ~#X%~`j828~Gai)`}$e5UUD+UO`R0%w66c0%M^1Md6+_fw;**%SV!B<@!H_}x zUIIGYMy|-7>%FJj?C;J#ML0IdcFr!_F49g--P0LeK04*BsbyZrnit$frPY5vMn+|% zMi}q9Qj?p)a#||M%WU|VkvadqDkpi5Mwz-KT1btJs})C=LW#;g1s=?~4GPy>g?);_ z1LFt62GkHA^hGZKF$}3Cn^g0>NBuQ2Q3c3R-3A+nh0Fe^cIDvyz3TZ84en}ZuCXK> zVXjkZ=Yrm3{f9TXvZ;3fS{g{~?D(+nVM9kBrj^?}bnVYLYdl6WzDrd)G3Fzb^ zn+Jl1GR)(F%#tN~aI_^7W=E;5dYBi00=X4``@X!#p`hWF^Xv+ha%m#a_t)eEg9%Ze zSB097p=O_GS_a0Xm7$=vh~O?*1)4&#H?g386Gqy!kY}e z@O3{Kc8Ytu^Os|$q-D5~$L{zIC^e(;;*)Oh=Vj+YePxdk1Yp?YlzYEpXuxsX4sdoN?Vkm?ETHAnfp=omPstGcVlSf~;lH z^4@+1ujYAA=bgHxeZ;_5B+Jo|8gF)7-BlaN1JfPOKsJF6E#M4c`aQ5QgvTzY5TSc7 zB?-wMhxrldo`SV#yboL8ffYjN=><8&e|>9r5M`=?;{1UaO}H=8^nzf^;~4}=y4BvO z?K7mvO+tiZ4Rs#qRu#_19GfJZ-aEVl(g=7R2pC26qxIx{NVIzIuJa9Xc?WEea+V$ej~BBb z0zg2)z(H>V4>tpk8DE|=1_IznXdt)IF<6jMAW-R;A3oM3V6sFcyh}{PKtj(-`UrUR z7!pVd0uGwl7V6{Mxns_jzJZDS1dN^%*RnD02?cuun+Y1mUN2U8iy3S zO6C+0P7_#5snu;Hfb@0cz+g+W=Y@F15EzW=r@7#pTP!Y z+jdY5g79=5?XB%2=flG(w8t@VWO*sYa0ydM75x^CJZzE01)3CopfEnKIkCF zD-au|R4pGWHMtS0IUSKS`PE>TXVlFr~(X$Zg3 zDI(9DYz-3YZXrtpIs2MDtreJPspDkRLOWRxTFYtazjp$!6h;Qttty%osXf99Rk2c~ zqjRa^oij^3M+54D3>u#J6yF6P2aLqvWkrUi+@57abetB>Fmq#TAf1R63{#{ieZ}(~ zG?LbtwzwffG&gBnQhL$FEpaP*#^+ znq;Dwn_Rdb1c-*gXbyFBswyv3G*D``<1(CDru$c?2^6{|qGt4~w-sIhIJ3N^Y?(euPxWoA`TOy7KtC)uR=RZJlXwv9p7 z!xwcOiUd(_#liJyA_Y>6JR*3pYD0bQ`K2O1-+l3_S^FdDHm5SDtU9xhybqOhEUTSg zTzv>!Om#qZ?k=by&xV!ae_-Z;7Q_EkNPjeSp4D+Eo9Czz`fb!ZXHWig^9d3u2#QiU zu{9PcT81>`d|zGzDd3WF67#$O%cg>nKP|dD28u%9fZFa$Me-`+;A%A?6m=%wZ~_}# z%*PUj}S0DcLLtH>X`R_vKDX7hXFpo=$?5@tk7iLM|6;#R&~k2uoB5%Zo51fhGs~ZjmR-r#sdn*&#*{+f9iL|UgLk!6%aDT+?fQP@ zp0Uv$ZU5>pBlFlew+_o(E08s`stqFcNsKBxawq)b=uUkFG)!^SNUeXdKqzzqk#sHq zw_q*bw_s{FzrxheBhRqo0?tqf#QVuVqw26P&b48!+I&qKY@)irZ-psMa`gOh|D zi6I~8?tr7}$0e3Mir5NZk;bQl(RV1bW0xmJ(v9(Bt`{iEx=R`aP9h!kfsGN=B#%bl zuVn+Pl}LM(Dm#cjf?3<1*daxpN{-*Ey^z2cxf~Ks_gtwRnSYclP+1y1$a(^Pg4qH_ zps}7LBxWrCkfURJY_ua%yl4BgS@p$GNtyE4gJHmARU}swX|kXT#Kpaz(vb z5eC`a1Ijcn;;MH(LY?m!Fw{M1yZ|_}I^zgoiVsP(-2eft>x!?;p4v?4o|m6e~h-g>*+q;O;1T#=*45GfX~XZhOlfpwVey|aS*_tGC#qr z8b(ZdPSOndDpBwSoLpj;dg;~<6%mhKAY$t5Ct>OcHS)y}w`$z_RQmFdpU6%|KD}{> zDQbbWjO4D{ksQ_BW+v$Sek3$jNDH|ZeEJ4>z{wNZ$D@jT(5F>qx z?wXtD?q3+!dDj5c990xbe|9n`ZnyT>73i z!Z4^@)tL<)}TbaKRBK@b*&jI?=SXS-WoZ5$Gf?;nAUlddPz zzUmJN_Rq^LhPBMcT+GrHNe_dpyN!8NXiH!nMmGyarxg@??p-EazRzDIUYjaiR@elF zNd{)glgdI$B+TQhONH}3Mp)CIMz3qKxtJK2$HR8)J@-q@4}uo}d*tTsWM8p->& z9{~e38ht1KD|tS@<#tlAbZ`N9cW&{!5M?}@RBDXyU3EQ@{=OJxMo|GH!!By9v=pWi z{RsaXlnAj&t;zSOZ>hBIstx4*`k7w{w56Hn<9{iAZS)3(7F+Er0NLdUH@?w;eucBA zLbHxK0j_UFaa0QiA_~h{_eKkM0=bwm4@g5dc#mQ|t19igwDm%ByPGqcClM;bE&%(9 zJT@B-Ui#qDQdt$`JfXInwf{6^6y?U$`P6wMjr+`?H|``TX-*KP1J`7YY~tmvSW8RI zY)9B=S!eVPyT?Rccn0!m+lqusOYinw&&5FI{mY)xb>^q7q7O%2TH8S{S_pjYAve() zzl#wc@5H~Sxr%kx)qi=y1p%2&U2$GR>A?bbjQ#_z1FXaV2aEbMte%QvJYvhL!sePH zm)2{B7`d^xrag}t6Rq|*ug2=4d-CesDC+RwLybLHb;PD{*Hmb1H5sdug!x|FZv%x zd${nI@QhMIQWAEvWR5yNYK^hef!{<){jZecEE{mYRhSLWGTm_w9At7FdRZLTn> z`oT13tZhrzGRD1Q|;05!wwly(NFG^?lT^XesE=J1>0xM5U`rJDfMNr5q~n2dqSFKf^uwAl8j{L7s06kjc-Gfv=~2N76nc;4v3pTS4h z+AoY(s2O1KN|#{2eLjbO_j=X_x?NgUb|6R&X)*#v^Urm|TIkO8S3~XFI`sk;*;N+x zuU6Hu86MDlP@f>2W+z{*{A>%f}zXPI7G*LJcw=sGln->0^ zcK#$M;s(}>vO^)f^1^UR#LT*{{v{xT$5Mav)Ss4;c!u!a{Pm)~#k6z3AFP}Q*48Ov8rrE{CjuWB&9)ek3sWL@rj+CKp=h4PVS`#4 zLe;${yZ!rMn6H9c47c%{*VH)L5>rP`i zO~x^qEB+&tsh(7fwELO}NwZCQMydcSbj%nWyl_ZZ_ z-mG1CiOD*`OzLXv0pKmw05? zL!%o3K2@~DVwFWo17X+V$HW#S`%i^KwU|5DjV)Y*PuXu=oeKAkM#;?#knU*6$-Y+N ze2&f_Jl*iaE`hBS`#u)73HoVumAny7Tn?3^RmADNANQfcOkb}Pamr@1WbO7Pn)!ZU zz+O&CE&!;Zvo~twG3*O)mg#uKhp<30x7mv28A<8@sJ~ zoL>Db?1DcX)f5#a9s_AtsQ&m*Nd_DWJ0INr+4RZl(FsU5G~A|8d4PKpj^!mD(T)LQ zw99oA>PVY2XA+LCJ6dEq{`4m*pa2}d&&{G=GdLIh9D1#wx>KSbm(NAFZ?+G=86fbz zMw=_~PkIG1ajBM_6SOe8`rSz`VLyM|aQ0mQ15Jz}v)L^sF0pT1SYJXDzd2h8x^qKxNZ(hjgU=RUG}Dh5DsC$1J%pyo2l9#y^6~&0DQoeGWNwB0d;@v zIu1Mc0R}@~;`B?!7`ZX7z^%v0m*DqOwezp-@iB18e7+3FV|;mhZOWlII`Dop5cY%5 zk#D8Z782C$m^~dH?=u38)VC}euVST&){5^qUhO;ouZ$VE`|Uq?BNpXks{Z~A1t=8$ z;tzMISKohzoIbXyc855RU4BC+_;u=5qPX0Z8wMk*-E|uVv_C_v9GfM)U$u~6ee>}A zW-6$bjP7Gu{C*hJaTFur2aJ;#2hWwOHm~+(KiLyd_w=K0+W+AWBANY|7xn`1do8`i zy6l2FF_m8O;1AnN-Q^ZiXGmB+f@vdt)X7lBF8OspJbF=j=6D~!MydLyU1S>6-ynV* z!Rp0zk@5Zk8I~KPaG%!ihpi$uO!2J8zZ)mJAH_%p>bkM$Fm-arkf)NTPu5HH?afG| zS?={ZP4*`wK=b-0#7U~WUmLL9uSC57$Xoy>UBd=Fbo4D6R6Y|cdT|MlKEh1Pr?kF-LuJ){aDz^2_J2%|g zvyokA;1h1F{}|+|VyCSZ6{$}$>hnL~r$PwQH=VQj|584}3%LnNX(V1@5hxLDjIJ@p z)EMV4WSR`$=wR?_=cq;gti?vpD@kdq{nP8M!{7pU}BUlw?P zE}LR3jZZqD9(d?2GYp9;EqaIVJ&vch! zS5u@1;wf147iUqJHI1$+NT-;kvfQoaBF#Ca*YAzii_*7%qevm6h6S*6-r@9O)0QCJ zS+*dMW9yYBo_9wZ%OAN7JjUlw*M#fPjyIPrHo+#xl9O9*B(W#=5<8rND9$8HJE$bv z>UGL(DKRuT%zjh}CT-zdG^oYqU1qIxzsjPu2@<={k%Hv1*J=Mw>m zgrwy7w)n-QGKaDYBK=^se2Smo(SjmnF190;SV`qDC(NyfZaMI z7nO#FH_*ls8X0PlSfH{*c%~&887eEI{~;iOojQCUIBi#>B3YSP9rn`KVzmimMBt*a zNQw^H_DQ(_Fk&s~aCYj$>#Xk5C+IVFHakYOPP7bNA`PPbML1=7c>IK71p4C1w#pbT z01^~UeSYCsZuO2qC;bl&yCo%kuBt*N#A5ajT0W z;O!LBN1AJb#bh-7sE0z59_87{i;*{@66KR#CWp2cdC7)m3PjW%O>$I;X9{_!<9TlE zm*y!%RQY{noaQ`*hWN>>7*k#l2|$jQ^JrH=@$AE*STIW4FBVEjqKYXAE0G~He3BNw zZK*{Qs6Ga%M%_mp@vw=UBw)q__+h_GGwfn7ald>wRJMd=z!NLe5)u*r*fJ${S++-% zLFY)q3gY*0atTF*7d4y6(PdzZx!l=aN0NrJbPd#nMKTYs5?la$KQ_PnFt^r~J%=IE z@v5#9nTo?4{!UtTjh6i$@)9M1y09BghxX``UHwmSrJDyC_14BAS!^?K+fQIa#=Uop74EDYLLwO_f*_@ex*TWutZhn$75t+B+=X ze5q}>AK#@^I1@o7f1JeQ34M9v1+s1U%ekyzH%A|`I=>giqUm-!Hk2!>Q1f!Ri!d z#V${d5?#r!yCe~;%<7kUm(gyNG}Le`d9{-URm5-|?CHlj+Bnv_j-e^O&}B#x(Ct*d zcfobidh|^v6Y8|+^!dpZvY8&GIKE=C^4NaePW=HVo~XNiPaHkTbL%=hpSD-b;`Yr& zk+41Wg6dMgdmBFD1&{uE=8LXzX)+2y@UAJikF3#^Mi9{oY3waV<XI6l?Xx7islBN9+THqyDa?lRVR;v=rWukDC#x$9kf4E5SIvL;r3LKmx%IHMm?<4Q`Jrat7|vW zK5LZfG6*E~6C=HM-{KqPrjRuYI?8s;byBcR@mICyT#o-7aKMooZ4JFF$Kpp#>JN09H+oVfTfMNmQDGeZgo= z6hlXA9fclPA926CVHgx;G}aOEE`|{rX&a?q^e38G5yKER$KjTE)8EhZnFkbr?(W+3 z6R&Cd#Ty{L$nuj*4iVLEQRd?k7nFP&^D^}DjxnADV_T*pGXiPS!XRgy!4*oJxBP=% z7#UoeUl*flX+5L2o{4BJ@LLS`k$^vn4gv-abNTb`*T0nx0*Z7CnUJ0Vk?5h6CKw9A z-ABBSwX~ma(Mk7NB^846yb4A|n`ONECIj$m58NOFyGN&n7l586sCWEAC_EayqUTSi ztT!I^91oAAj=HU#Mi@(-CyW&|@fGOf`Z+{qfb2MlXXcnmsI(z_DDSQJ#vtJ?dJzY{ zg)yFTpLiK5-T|3S-H}n?=e#VnQYQPG)v>_8dVo#-B3{n-~ zUHrZv`#9Lha)r5lC}L$vrA*5Bez9Ydmt9=arz|-QN$_pgQH}?W&~_Ptqalz)LIf4= z9FPR=z-tznPZ(J0%*D6|hWO~8a3Lk3$%w7QGmfX*O89Wx73@j;6y6^wD{D7PGlw0n zqJ@RANuI%u{2)B6guORKUYW&G4JwH#J&a7OzH?=1ZLIvvLrpwqm^h#}j@#P1@VT+5 z4{?a_mupVcFYGPUm(nljzXrIyUzez_M@M|)`GP7*3kFj0(YjZ*&r1yM6E}_IQ(sni zYSr7o)7_h2U|q>8h(e*XB5sWpc=^c%dw2TmsfP?b!pn@hXVITlD-v-=p|Y3tLojnh zS$KA8cibc`@}+Z+S$>52olUN?cS;WSRiDRJ^{%P}s>=z79pMy2QYtK}K98%!Pq0oo zo8@(H-~dU7m+j#W62?tzD!op6%u3%*Hn=R8jZJLK?7!#F%woM`5IpSoqYQFHIr6d% z1l8NRlDBoth_mCSV*<2UJw`yHQR=evX3^g3^KBU`Iq+Lzg>z!ZcU>XITa1`!!F)hW|h_k214M-Rz6RVZw-=bi~_NL7HSi%?R5o<)C zlSbR(L9w{S|2`=){ar!)DV74QI}JE&_5EmrU+`J?z&Sp|VO-bL@+2VDJ+VF{nt9x@ zcTaET-P@-~mgIV?+4aKWgjru|2C^bpxAatNAS+NA_IA1VX4-Ga${8yd@-@Izp|PCI zIyYc*BI__fLvzz1*cURszfx%B)PdE8HnZ9=uHn?jvgHduo_ETPIsw(e+52zp0(Zx~ zDHg{#ddvxc4{mZ*xaX^-RDGU<{=+KVapBu#yDE-(Rl`#z7R==_<6UM;e}HH-9xJI` zpa>#UuE0$RwV^7#e@DQPt4tqnoTm5~#Wr|%3T~S9oXM^^D6wZgt@gvw={ZxX zQp20o9b?>mjA7SY)zGn8kw^*`;vt@L3KzRkJ1;qBhRpJHcoX$k3s~Y|8`>^>Ho<6_ z<+MZmvSu$DKC%Z`Cl_cQP>VJ0Sye>Wt~uJ`9&qWk3)5X&5Qzq$GY|(l15n_1+TdYs zgWbM$`CYin|Db^c29AtENYC`h3XS=pl(dYjW^hPhQU4YqDghDQV=Zf&zJc2e&qI@n zd6Mhui5a!sU(Mdt*}n1*pqKt(3m2wVunU$vajlOzLBw!I5R{EKN2g(o>biC>Pba$U^!^dM&kv<30qR36>OT}OywE)=yWegXw(nX$$7Z2cf z##`1T)@-3KUjSd<8TWiopidh6qV3i*wCVj2i+EzXteM2%>5PR!TK)PUK3y9Vq;Y5; zK|Pq$T%*C(7oy&8W{OZh!MZ*I%=1A&p&C2gV~ptZoonTLBR(2_C+HTNXa5ArJk~NED$Xpm%b`4-r7hYjyxM6 z-hN3bWAB0@ABR?y7jBxPr$R9*-(Dv7UsWjCyp84qD(Jx!*5i8Kp9@UJ7K*hvEIqJR^ggCMY)w`YT<2i=_uAyWp+ppuR=$L@%1|aU%|8!uHK<28-W(A*$DkrBxI$2}iVqH7-Uf6UrvwYMbmr1hTp+213vA2nN32^%zWu3}!h4LNJce zjT9RxkMIwmoH&$C>bo^r$c?jv=h>>!bC>^p0-r5RB7PHiYc$e3k^+H;;Dwz1f>Y)? z)a|7*=0v-(qw?8ijp{be6hq<_e%%&&%30+|j$NN}qkRHlMWMo`;=!{AsicVANs#H`!ptB+GpE%K1J5Y;O!(Rl2=+Eyjhug98;9x z5KcOWifIi}@nK>e+_>YwTO(pfESG3NxtS{uq&Pg7$~u7>S29@-aW61j#l-XOw7u{? z^Ua(Dppd^zwap$^y@z-5PBJU!TJordjVv5c<98^Eowa3Bs=YL;C&>nf1_k)j-}ZJk zMh{w`8ba(@5`LHqE8lg(`vP4G_%0&(r3Z;4#??j%blyTh?{CwK)Xb@`5)x7pVw<4i>i+ zf-f0mh4*@%=*uLQj2GOWV`iD^ys^r`;uFT(e_!rK(f$5%Wdn3ZDtlzG_lwE$nS|&5 zJJjMS_{_ksYyeNAza7@iVpgN^gFjl2Wqs(*F1SCJ_N|;Gy1oL5D z@#SW)?No-rQ`-EtJ>_+L=`BeQCNM-CK6{Vdy`LsEE2D+22UQd~tv_=7^4RGeTkPXC z4MQQhrBW3DSUSfk37l9TW{9Fz)K^M^0#6&4^ zyk0k}b5+{yCpxoks3?Zc8MkVc@JMCZoo%@@H>IVn^zlR~qWsZW%h3~@G)Y}n?WbdI z6U1@-E$Ee)b85Vi<-Es%`y^K~EGUgOvBjToi@bf$1F29YApNQDJ1;_c`Y~3%o!Otz zzRo}WD5Jd*)o|+J^W=xzxmVSRRbs{rBP`&GW?Fw2~ znxu~Lq%%RMkSo{&N3L5qY>t)?+anq+COW-HaeS&0jeI8RKTQ3WrWk zyb-Os*sfEiJYfSE&-RV>!3fI=)%;I?s+fsp%4Vg!EAsN0|Hh~6_Pg2xI(%ADG*)L0 z8Q0P-go30(4s~)cfy01s+@z2 z-IjrW^P>S`&kPgpfnwoV+IBo|gzOv!2IQ@>_bHQS;X`}`==gJFwpeHn{SZ#Fan>8k z(?l5(-Rezh1$!C->{pHoEJAEtx%+@n(#x8lm;|U4k6M?s7?*AQ$Y(Q&^T!U1hlF$w zjPFtj`LA4-B^Qc2YTQD4XpU+Ov3zoOcwy6~krQClw#9`F7S0o}MFa3KU2S%i(m5d@ z+;scu9vm&WW9p(j-5l7D?1Ck7*>&LF+}mPOwuENHfgSA*d~iN!sktYnNli=HMXbaN z8G}{1ei~tP(6uW#I%AFu!V3veV-YjH!{6EcnQlei+a;$LatNf>sN7SP#VCEpCTgg zr>;Q2Az@&kZbMxD(?sA;lz;;N*&`A%Qz&rj$G~jcH%r9$NGc>OIi;w7;5H&E9q^}3 zw5*;N6WrzT2u><2sV8QU*0!`kat=mH1b`PH;k-hn~liS>&807k4yP?>b84i#5 zYY`OtFD8y@*NGj5f)vgYbO=!t!m(OdrADp+FI*_b_YKhpT7LqDIM5-$h8ry65E@z?6iE zR_kyt^uyy8t{DY3A_xDa^pD6UD=Drl(ou%X-0Jk~Xf1s6;Y?i1lF&=bu25S`mnNNa=*2J_Zrg6bxl_M2OXp9SReaupQV zy;@~3T{m|E9=@o%bE|IB{eZctnyHL2d2Yt0y!@lxexuaS*YeD><@=rcKAjzXS^r+! zc5169TjBr&5=CZ2gxi(O#&#h2$!NS|XYM%&(w&v{fP3UM5|7y3Fg!CrV~hSsO*F8A zbjHhtTx&eYfxRvjWZgVc-u(jq;MqpPU@;>I4bijq&6yyNH3+s&5DL?DPS$_3BH`S{ zxr2pO5M%_w)|6Ui+qCh%AhzPM;oG4dSTEP9h80?!dt@V16QNwsF7>{s^V2Kk28gRu zHspo7X}As!vpletyuis88sbze-*zoW=qb{f?jl3Mt(`lP_)hW-bsiMl9e9GQKJDJT z^W)`BEwA36!`-WZBjMbGx-i4+%am$&Yv-M>(>ho(mOh)S%zWa2i|K|;JgEy3%rtgZ zL8po|W=b$yRA`r3rieQgC(uSbW@>Ct_f4!+yu%gz;=_=4Kdi+a2NSFNLedivyHMGb z+PR%#WMi1)$+ISZF@ydbS%aXwGv3(v;arv=CmNyHt1#&uYs}q496$mRoB&aB3Ws%! z=iC&+;WJqcbcmG-?R#!~B}vlS!^X-Zase)u@rMU;1hz)Wgl9dAZZIV+bW%CPK#n+R zB0*=Ysq&5mBg1r`(+fiU#e*40f5kl8Mjz<>zMOb&jN_J&AX{5b|L>LRWhz?uFmXb4IQE5z7^hUxVL^>TpZ+LGOCrXlvc?kIck8O1)}8FyL4 z!6$l!Wz%i$Oo`q$y$yue>Av^-klot(yWC$$3J*ToR7k0XkQ2@CXjKw{=;R_36He!S z$}2tAxXB8%Q8I^_>>);)0p1huPrD8>2uNuAr5tUOo zFhZ}ooVBi3&X{XN6IY_pVztM}s%M&eETs;JH>-ur7-2M}ydLI-iQ}|lb-TO@)+rE@ z)1dj#3$QLP4&CeG=NTT`j43ZM4BaMU-hg{)pBoyON>k0)|{6lS(Vk}U-Dn*1LJpfm^FLrttp z3`2t*_No~9Dx6UnfgZA3jwOUIBs7)SYrOz_m)ZIaby>B(h$?ncL3Y*DFd^!Lli@!XBwNe#*y1_(}faDqtca_pwOf2I}5A&Hpo^)%_YghoKnKg&B zHpq>>8sZIw1=bG>EFRu3Pc<|t$h3}FC2MVxcTE56rINYr6|dx=3Lq{_Dj3%ox3HyK-f#ElR*S}98M#o04q-X6o4X-2vKJrV{Ooh?F!ouel=q% zsn{ox%3dgKb6hNepq`h+(UP-Zj7K5#{#Ys5*f4uQ@$bo>+=^Fn*f_IrYlh@hlv z_oilr8c|NxV&T`3ke8A(uHZAG4`C_3FuY6LI^PDyN7LYbfk;d6QJCar+V1 zWB2v4UjF&9`YTum2~JE)xG&q@=_~#6TY}{JGoprtYWJ@!0<)fZzkUR;7HkAj_<- zP0~w)naZLlTkqgENHB7m=upF3%doXH;ntvoG6?gGdqOT*1g?MBp_0#K&=`!P3ov4^ zM zJeabOddRR&8Lx#b+hq}3nf<2T#B1&8jUz;nM5NfIam0*ag?nNZ_$J2lCdw`kZivIV zkS$idD4)@KYJOewnW}uLZum=S8!N!fOqy|6lTnJe7+J2@n1y=Ks9_e%m{E%1G%DKt z@+a;@=#HOdz#DP@dTdfHKn3Hi&cZr7j3GK1^#j^Ux0Uz%mNR=LBK7?HO}^U%PLYv1H(3ESe#HI&V1l zP?==X6K0Wn?!{t^w&W(csGtx>-7MgxV@DoCz*0N%z+%*phAX#|`oy zbUinp_Y6ONU-qf~=TytujupluTZ6yO{>6>Iu<>6k3+8uFMzhyDro+~Ujh($7YUsu= zudYu`2!KT5hAW(^67WuWc|y1Q^81rL>P?kcY*V86>SH;QS`H5vuZ2repP2s}#dWeI_cJMSn0+qQg~;NE){pa(*G9O@Lnng3(W|r=j4%kZu`^^u^TA#B<>r zw#nCt<@p|$Z~HRX=+8>h21?rn_-LERU$nxvdf1yM&DT|4UW{;Zb*}f8ZDp6uO4$b} z5^QTHbVZ6K*Je^w#rAoG8x8PgOv*wA}2VZInh|EdDt)CfOmD^Z@z~lmh@DAA# zkk=AhRX|jNURxrqqkMlqO1(4BI8<`l@6ni+T)ucPwTznWIbWi-IhKE5+SEm^`A>_g97{E2y9{>*c6!G?8NCpy-kSb}=Z1#WEbqv@WK$c37v z-s-MPT%--X+Jf2uRNL8-m%Z|Q$?H8o8B;3Idpf4!UVwW7$7MQdR%YlH4BVQ8*hFcP zIrR)-YbguzL#{}#KtbLVu8PkM@vCQQ?mcCp6FSPD9^++bCiYrjlMkR#UdHOP-(uicHn^fp0?S-!PHBfQc zO!nEU5oB`qD>vGp=|3%7yphSvw28! zOP>kguQT}EcgvgiX1L8O0zPJ1>z(3LHTihdMlceVOj}x}mbtToO&Idrs+4>z(3@^a z(C}~Osod#_M+C&fqNBsp9Db~*Yi@}U1VV$aa1=zUcx1~EO!Qi?^-xauJ zD$~P0T5lKnCoN-ddtF@suLzS-9Qr!ORna&&+X>=-vN z^nIxk1nyIS^l$WgCrJ4WsCs1VVb6ZHB}n)S$$Z-xHw^l9v17zyj?)ySZ2l+9}zo*ZF>jyEgoA zYbb%dI(^_&D28~P@X=Y0QOsS$Ai3UK(}KL?cxqcazW-25|o$+Bg!pc)&yLCN}} zzqY@4@c%6aSRXwW@4nm|H?e*gWQy8K1K7#LJbyh=S98H>z+{G{YUTtTUB3E#PI89P z&})pQUr1K`-&2@&B4YT$P~bc>{&&mgm6In|>BmQfZ>kT{k*(M}j_M-Ry<5PlA6-JI zKNn}~7~(9~*^egIq5e*_O@0f^hB|Kix;&qc7aRF@$bxiyR=;oJEY~x<)?%lq*F(gF zgrDG4P<}f>&%-9EvP(Ls4+XzFbBmMCvKPNoj7)BZq8~aeL}xosC843lkP4)S4S-9zqx?v5FlwY4eo}0o4{4yy71rF9j5d^BFVWhXfeud4td2*+RHX4N@c{I`8-& zDt>%lr0|;7m+({5b{Kl(!7CLYw|$PREypW#R`lfFS-PfO~*A)Pi#B;4_U9^1WE65 zA^d=Eb?hXWGUc+x2qxIFeH(f@hT1PAsn22qv@?&ACv&Q~ zfnv8V*v!3tf1X~v5wWKXdQaWpIydXV6xs<_Ox5Zq-bZZz3WNahv>T2@nQEJ#l0Vl{ zZ#rr-+@BDuJN{l1IWQWQ6>KKPcuzOjgf&BxYK!5bl5~prPfsoZSYbf9MykY79wT|M zsIJTUIo@k9tQm9A>z`Re$;LTky6>)%hV*GDU5CRhX;R)sQBCd?R5_KQA04`%JU+zV z2xXTkU}3FYH}}(r0KgX(r2g%%_kh!+c*?dKAZX&aA%h+krS00>+a{u+Y?oC?=N0wB zngNU=Sj!-}`P7fzvDDIPdR91q@ILjZSu1&-vaBac4+QYp-Y^Pk(-_~ue=`-H~7biZ!|0Ad{yIKo7FN&C1p&$rEu ztRGD5Mg9jV{Ju+Pb5#;Jo;j%$FR?N4q{3Yv9MsPJpi|#p@(*zVXhC5RU0zAOLi8|5 z^T>N4{NW2pO7q|5+@C!}`Le2|p<6=62oLP7#i&vA2VpbN5pci#3&{x~veTqK*QydzH@4k@y9L2Gdcyto-;Jf1iPQ%V?Wub_Y#Hlbu zlODrkwDyakY*_N=oH6#Q+6}s_d&l&?Qj7Wa5cPH815$gsnDm2?rK#2Yk&C1O+0dNi zvwyrh=(yzh>$M#|MX4z}t*zV$nkpq#83A=Jm%>mzn5H-#PWve7pFqP2rn%=Akc!l! zk2$l$av(9sg#!PKNq>sRKVA?pcFQ1|i$UeRBdVEHHtYQ7yRH8OzW;c?Zvvr=azZt_ zakE^)Lt8CBn<0&v4|;2 zvv`9y=U!*g8|&AF`P1#$LPTuYLB}4{Hbd3WY0K?SI?1Q$ z4ZGX-tE}X$Wu9r3y&*5`gIqXxP}`(wNbo1CY;j2Kis+P+@1K#5?`-{ii^;p7z52tA zOG{%1!1$L)`^7$R&S$`bHwVx#9>~o{W*}1 zU8NPA#{Xg68>hZ}1cJumC3=J>3w$Jrv%YyDXX=2lh5YaN9}I061(*)jj5l~>CFBU zZ`-9yhonn(l+0m(HHb3Ij}H@KQvc6NoL*y{wF#kJ@uj;DORW2BzJMshz#7e)m; zUld943oeO8(L?5OwV2=0-t#MSqPueUqH;(%DVW|!@t7UYtUEK zhw`iZ_{3JMu74_@gG<}BNaFT*Z_{b&-dzjcJxfE03H9GL2$v$GDlgHRPIXBCKf|py z+>>hht+;%=T#qP^pKYW5!xqDtTA>}BT<*qhMDU=%q9a4zywSodv*&R>iL#lzTSA^{ zy7MN>k(nLkZ#F;toq-0W+|mj(j*x|;l3=*>Au%l)Cbj$6>)tOQl(DCpVjiiKpF2%M zIo-s?O*Dcd9rz~x?j50$x6=X+e3S8w9=MN#dYz_==_%Qcy%YDtJaXsUe+6L> z6pIJmFu!uyVc-1;_;o2_1Z@NqcXd8Y|Ibj#l)!$>DXuC2lHnHIDU^; zW_6tVW3dNf3wqWUt-EsD_=AjCC0!JVI>q^F)5?yS`%DDKl~xgXLl!dF{h5}Y|K!G+ zBJY7AvG=KnVdpD<)viB6zy5EkdOqx%M&~S|;ek7f3|vr-QJwD(z!LY)9B}-5TU2i` zn=O5Q1fGX=bJ5j09pY9a z4DD-LP*vZSZuiw~IvTF{cfRZ9Atf3G z4aS`M)A?BbjlovN-J4e72NEw-@39PnYN;G!{%1FU-laT#Lt?G4ShC+45ciS=X$cw^ zO|UwGL~#hT9@w*}8OHARK_ZKIHnZ%RZM0`K`oucYn`5%9af(qK$HBZmi(1ToTXSdz z4Gp-imUdMxs-#~#8*UWE4TYj7MbDgJXQMjBG%o9S{F413ZqNoDGx_Sq|9$lT7ci4U zm2jD@#Y3HiYH%zIuhtL@d3QAPe|(_+Q#-mA4w3}sN_)3NWQjjN$H_Ik$xJWbH*GVl?{H(X~bY&?ZAKWaI{;?Z1gRG zzHG1t!9DJ%D4q59(bymCXBA*M8CI4;Ay#9i`kncg`Q9b`feF9xn@vmloiF!?2OLL@ zzqyJxzV{A)VDVzUB?`VqTG8M8o4*^?Z!WR=e~jEVNsEMADHf>=;uLy$%<=CNXs#|D z-q)ox;neVd)V@^X)$Paaei*RCHZC?jE*Qp58yz{ZEOpqh&Fln-fPHRo7AVnFoUT<^ znQSzhD%f;hNJVs%xKTct&)JiES7k7AqMG!*tTskaZc5%T*rp)HacK z!vrV)HgZ~SUu0Xb*iR3ZOMb>>d_GRSytZ7Dxp1w6B_zZSXYm5ZtKF`WFI;CWDf}Fe zJTd#}9((v4&hX+nK~W#r)s=#$)&7U^s~0uD4+m%W(}RMd@13bnMZ=Bx#p>ud)c z|Dsf(oxSvbDjC$%iw8tT=99{w9$CMYy%lHIEgQLU_n`L=c$~B?FBjF&bF^NFPI=2| zQTrZzIGWfZPFAmZ-B{wd{>!!H2aQDIAYFoPY)cA9m6biLZ+xQ0B6koT%-`XHQXgjKiWla z?jG#-qY?9q2Yb)+TVT%dH5QPI>%6$s=>1M|2IllM_ZQXNqN&i8M+WT=rR|HpA&!`- zx!1<-kA;1gm;6!pqu+_ESGZlgtdu~DRqbsLQh5SS2Gz-=9WwIMZ0uEGztF*dzH`o4Zz`!YbuTu>!c^%%0kC~ zs8-kn0|%KyGAU+Iqvx`)2%7DOEq(rsYCk7f;GEgwW=i_(vZ^Pf{uGQ4z0kB#DRMD) z&!GMIh6UEyt7;}e=w30$3o32RZi9eq_8N;5Q?9NXanqJy6BbU?uy}|I;%lP&Z;XHf zQktM*#|KA6TY_j`K#&Hj&$;$`%$V40C}g#6Gx^U)@^WD$qB^V0JP zU3|pmhu_)OKWF3X`ugHbDFO(KwLbI0m8j4;`tjb>fE@TyKaBJE-4KI+beuOoc$~P@ ziW8^;HpMl%Pv}>byx2+qM}%>L_JxXf|s2W&j8l#c^n#3_M1Ej!c<{12j}r2=(UiGc{t zhe7Lf*FV6%_c#6qPrSa0al$_++T$$?{`s|B-#ZqI4A4i_#3lEvEQfJBx8uO&kL+y>AQScZBt zhzZb8Rw~6@_qHCPj5yUG)MI6Ek@S+XP7gOJ7Rg_zX%ljtN7RvPAh5ABO4D{#-5u)SI^Csce*9@3TEEE>rz2 zFFU*c1J(a^wqWZ=TD3+8W39*Lz}A#u+zgWm6z%y?#m92nC#<<&VfJ{v0_}kx&x=?x zirz}$e}3*(BnuXcE49n*R4=S^=Q($KFJi>-zhkzfp>SQ3mfwB=&xDuO*!V({0K9I= z5M1#F+*wrZr{=#i|08Ml{mpxB{bm{Eeqig9e~V;lzD?WtO#A@&lfU`FAHI19UB64; zf3p|!pMUcuW4~EJuOG;T)Nc;BFYa3ku<8dku;#aP+1xitO8s3F@J(zwKm2dc-<om%97UFjQH-M|YwQ1UB;LCZ!9{J4t4$rU{o zc6TSfx>`SA|FAk0Gg-f)Ds?^BLD;XS_QZ+l7~1utb~?4Gx>Cv36!CoYuhWsgn%)1& zWAklV`B(M!cMy%SD`|y}zRw=O7Is)GMxvvzVVtv3j-@?CN{36wYhwN4o9P+ba{)3^ z%7y&;AH`4X@K)XE34&KZM%cv1aB2jh`@78+l<^*(FDul`#Sol+Vw+)Z^}1POPWjP$ zgBrrs%71`lYgmM9oKxXQsxA*@BYJn}Z_%(BgQvxq8cV3!W-Ic4YYvQtk#V7Ct2XhbC-P z>>OmV;oNMk&oo~pKFl1O-~a^4i4Kh4W@`jWF;0rZI?FB@hQ&Rao;U~A; z{v`hk(2RoXIA>sD-QEB{;v|zJ!m^(1gk!O0{lGuF;)zpRp^$W$Ix3DENM_?fy-oKg zE&s_3CjjjXFBB}psA04CfTUI)6#x2-Ls4^2P6m6Mz=?Xw6q_|g91T_fa%Nm05A~D+ zRaTrw=5}_Nv5t^ZupV<5_^;-%X@wY`oO0LVR7TvR4a0Ogb{P1nZ=a<(6&V_lagGi? zwImdMtYz;wWamSJICD^O?sT-8Ph~^#*G?ZBg#)WaU4b02RGtd`2hFk}=1y;l-KFCM zB75#tum5D1rPqS#6rTi*9nds`n6*oig5#E~2K73s(`Gk=$`VIIl~M*$4LNgm4$9^B z7d8?6DXFsQ;JKsI4y!L^6s47DxICtOc0Q}GRaLRFnPocN8Ay6u%E^k5 zK0ZrD+xu|y94IXUiH&ygiiC=#JBgkH_q9IF!`)SQy;M}tM%&Y(tQStN2!ljHp$_6O zU64%l-RZXm;m)E+q1XZl0fL_gjn#RF@63iaRSjrC6a$!<%{&GjgpCk#6NOno&LD2? zOW3%$M%CNqkug2NfJ7>uyqrFDzwCp-Cga!7ug*>=GV_k%RrH3e9UkHG_+m@D-_BkL zbldg5o7^G+$;O)a4P)(hm|l-qOP(C|E?HaOrAUYe+>ku2ymwpt@yCVggPf{M+j_?s(0@i^WKiom8$WCpxnEXU;7T|msXh&eyNh6 zA`OGX-Ngx(y#0W&Tf9>HXe(SEvb;ylIM{n$rXz27EweoBm4n7m{aye`Wtr=$XpM z!@st_c<{4WxK*n#b4nNEda}V^fcSq85Dv?(tIVbFM61P$eru$96!?UO@>m@qQFZ^M)=DA=1H; zFY4nNnoU&0D$R z0UVQ}Mf{`hCM@2_4=1-05yU%CbE~>`oVF6=jlX+iNuTH-0d~6Nsa4AB57bi)1)U-D zfqJiI7|@qe%C)?0li!ssYg&#CRJOT3%yi-reSEW#s*R#NrsF35-p>HA?a*1MRQ4h1Sy@}RAoDF{i5{BKJ6MNuVZ?@0HpEkA4RKB?t$N`QlrJ3U@X~TA( zpwRa~?R0Jj1Fco<>y=nCbwZ9LJ7UAuW%qk{h^2@EOP!FOjAOtz+Y0 zcCmp8_w6iT0^px1R7s#U% zh+Tc08U<3)UTCYE=s<NKpt?1_r(CP`xNSuLo35zt@$%KzgMAtt`}VZjtga``Cw)B* zsJ|*}`nM))X zDx>g;kg~HA&=PafK~0pKcHvU{!}r;wkI}uayh+o7i>5h=+fPRMn`+oNORwamHSUn^ zgpT%!m>2H~uDagk^V%$}TgPc#X$!Y!*2QtMf)^l!8=|aux!p5}?X}0TZELTq=93Q5 z9hOMc_0cFj%Qs$bjVN(h1D>&}iyI!M&Teh0V&JaGw?T#d{*ohxcdWQK8K)9qm6_R$ zVu_GteAM($7&|pn;muPpNg@nt%vKM0+pm3L|y8{?i1!>dY?N1Xi`<2>Uii;VU7iMyL&odvq$B>40woR!nB zk&VN;Ub3V!ny1v8b1?W)oL;O(VWLmq(RDmT#~!h#8VPD2oIJ$1tnq9DGP@-ErkOqA zx01nzsarTM8Rvj-zY?7eEkp-S3n=8G{~>;3ua~-ksM~gPA(?B{rp{OO8Gx?z&9u$% zIBSJTKV%L~juP||vwxYvtxla53VV}O8q4V!2KYm=u%@b9S?v?(#HSiu zq&erQJg#!lAue|v9@mGU6KE(Y7tgxu|{e6P}CIH@&gVJj~ z44d5PxRep$h*3Oi=+}DuNo=vKT6>U_Q}P9E=M7jsg%WQbcZ`M5;T(ROU7h^gyhB#l zysnn@1okRzZ`IoXrp8#!-Ga^v(7>{R8{d6&KC5GouoGJY?Zy!~W!8n$6P^&gyk7A% ze^;#8Ew&fvmfNvT#BdXoZKih+Yp+ODLxo$IgO${kgw)ZA4BF9-!Zw2_6|0hm8bi!D z<_ilkMJJ4>9*Z%sDJhdVtZVTyW#$ew?=GSr2He!!N9csnFpp z_UPz!lerQvCz)` zEp;%%s$URWYIc0Pxs+#48kV$OIAUC3M&(UfUh>|=mk6LYz)|5T2) z^1X723yT-!QJ7!RsR%=`UZP(=e{;sz%5#St+31)w@@D~hT5uC|C_{if-*Lr!`H87M zi?GHj-4G-|G$$#ydnZ^o*;!n82a&~JZom}f)mPXmHkqqyYGHC0_Qb7sSM#WOgr(u+}6TT%5{P*siY$ZA-=Q+WTr z=lxKQ{cc3~VTiIM!m8Db#yzY06HAcMwFc-%8IUbYX!+sRY7m412<+**t%sdx**+5_ zmN;ZR0A-mM3r%SsDPKQpIbj%5gk2(sIam-=LNT1NQN$Z!pD-=%_nperGL}l2U^NcO zyQNSkPAoCla1zM*>_)~^E1z*}h)LPwz&1kQ5eH3t9s0Rn_q)%uZ|8@Yk4r90zvjm2 zwG?_~^}}%N0X%1l8%;;Ngmm)U&SoMH84j7(gDV;(`hZDj(@sZ|zL}hcD-Z5&9M=H0 z2r8H_A*<7=_g5$JBm}#O^g4+xcb&hXgx0?MW*JbttOe_+$IP&%ZLY_xVUjAbsLBW- z9Zk|DP>f^r{iFxs_L;Dfgl)bMuHJNJeVJoe%v`;7`oTwAr$Gw)^)3(L5MN;6qjwTq z+<4CE7`F9drAlV%F;M(ZS_dOS9x;`7-_&r~ygfSxlw7fbhUBiLk6eV~m5n+0__;BW z>n>{{Ycyp`X)=Np;(qfi8~pk0H>OI#i@nLNYKmvhHbOUxCADG`bEky8m&qQlyCi95P`jnHtU`To!20Z6YD#cN)5F z*0q(s{)OaGkPx`cK8a)4LT>pLN6RAU!ieP3#Z#Xq>8R3secRW77oWTss)|asmYJsO zVqLpz?G7?dEcMEv>sh*CO+7ZZl)E6FQe79MGsMTT=+#X{Pb<=Dht0tTq6eby9v3b% zgg9l-30cCaY-R2&057~Db>j9Q4qFJb(;Pp%fT??jWjA+SvV&jf3kjycqs!mU29dL` zC!@X0F|n?`K<)8hjy>$|;o<=URW_ZrXY=?MlD7@9M9U_woD)GhAkN-B#r=Ux7lZv? z_+hB!vcMquRBOC3)98h%JQW-#RQ=9>06KJ&xq;@PJ1JyQ@Y%# z!Oc-~(j&=`Y!Bpd-sA=~J>1yo*(VkQP4AuhhXX44K<`%N4oqE&+*+aSaiw0OYpKu^ zVtFe(+jr-?gGJ7QA6dFQbzR#Mi%nU4oMaB{^qQ$&U&~?g)+g3%5@4(dO3c|AVu8K}y3IaQz=)z;= znkJ-Lr9f30N*BR&T1x#=DdJv^!#DQ5>R~rgt*2EX4^DV(b5lm&+^=3#u@-7*5*e1d ztx^U*nwzNJc=-(YM$lRnE|Ibt4f^om`_5xIUFyE6{ao|jOVbVy(u$b?{SP*mcdM;2NZGcNk*ubT>Q--dC)tA zK6ptd?c@G5IcmUO4a_`6g`Y7qEX6n&rm|l*8o_wJEMSHi#ylCJ{lF-Wxe8J=xb1?s zzgb~|x(g89a(htf9sAs)ZSy*C`Xa-p$qrps^U z1L+P?EQPKtQ*U&-lshdT41LP4%>NAP`L;)nNKk%LK5rEkm6N&380uzsBTZ{4wuF zKhSp8orA+~&Qm1^u)tFGFMi~6=2Pi-l$_&{5?LMVKg9E_%QWItT#30I&s)H98uAR4 ztw!~a*Am~#KCjLY0s#lgumnx6lsV&9 zGZb8Xz8jTVOMV&JMDSeIQz80Ljl*e%G^1Q|t>stb+BLIn0%S8$SMayN3oHU>jYF(x zM<0~U+BGRB*ub{EqK+A$q8?maW|dy(&411<*wi^>#aVX@Z{cab!0NTzlNTw2*g5a< zI;%+5tjGHHts96_!xqS$LP>&8J=*J`$AcTI>(ZK@XJx$^@=8x$v=L(jb1$xreM$-m z)^kh?F@!O++;Xt3@d_BZC&cC;(KjShE^0?DnM4euMrhEUV@`KL z9>~$Oe#mV47ZRY*i&7RhX(pT!r*&tbxCAb3>2*#Kqqnov7m^Pk_Rm@4CaC#J9*<_G zd~}wD!y8$(f94;Rplnb~87_>UW9-_7c@V;%bP7BAKJ7g~q_m$YltY+VGNS)Z^^M~t zkwj>Z*S2TgiA^5|SU`L{gR~@p$~X`1<9F6|F0+;_&-3-0)ix|A)F=2H zz!$FwXqN6>LY6cb1&jFJ+3;N+{cL0-4EtdJaM7grd97G2>G50UfBV<4?$XHU{nzxT zMQ>g;^d2ixGD*q^W74YtHqT%59D0&TDMYTpQgO;5nTX4y3);&G*?E12Bc)^NllcL)gt*Few&2$1iRnVp@Potd54 z-S@rseeXg2Zdac={jXE!oa(Obt~xEU5rGER?Z|`VUQnsY4K;ze!H_D!l}TdSe>IF& zhdextZY`vQ$oRU<$R&EJA5Bvqzj-vFhuv;;bFP?j`UPii7u)+}_x-Vys|CDbirdz+;i-gLl4o zO1@R5RRub15niAq^cpl|O>c3}koIo?7{5Puts7gjEHQVKJuR;$mt(-R5UoeVD(e-E z7gsE5gG-M4FloSh@SEP=4^eJ*n3=G>OmItl#-~{=1(7p$G(sf-uVTQ}U02o>88iu{ z*^FnSciRb;b5&`jL)v`ULxr#7`4xzI(rxT}1dmHHO61=RvvU=`rbiQM6L@ib%|=H` z5suvuh~HDWGyF)M`+Cgxrp4ru^;6LtCDnN~Z(g@N8@o036u4ZW5kZ%eOWf2utFt`% zaojmUwx*cuGuFoBgF8O@i3`pvIWIN z^;4`BtZ#Yz<)yMLQ^F9?;yMpif|V{aHO)(#*#e^SyJr@9od%0QZY?&Ez7p_4CmXRH z3;aum^J%zNs%R&jnbptLN|^En72fvh}kWtlZ6+lvl3Z=(dNq3S z!2AGsWSFgG7=a_KW43O;XnSXfQc-mD_25j!{GnQaW-JQ>w}*<&^ICGO-?RNPP$OS16L?j(V=g+V9PtFVS0xt zYj#&^r8y-Jyvb}6m;y&GAyHla`2UuJ&z@V;zEGYqET!dqV~v&@lM>j7oIaO#-d|71 zCJFS7Tw7~Xk%^*YM{s%ISItpX++&>erXxx2L`L2G;F7YO0}U5;c>qFK&31?eUjw)?9{V&EBQN;PA0GJh54+k=ZaQSkA!Z zGAef!mSo*WH>I}}K*Ne5s#>oWThfc%E6gO=aQqfg;d-E4(ecGHG5U__*pt|(L^Wjjxgkdec zWtFVRe)3hq>eN#r>61IpyUhuPlaCTS4#6Um#O}o>FhOcPUJ_Z)I#oLGj?6-G&nVUd z-K)-z_qX#ys{@x|``(`4pgX*+w;Idj{eO4U@%}$upBVxdPm!d(r!QLXBE!z+UEHT6 zdcoKm@n=;=Ez4^ML}V1tqv>*At;nRiHRE9j4O>n%n6v2uJFq@|rd9$xtAgUE-fG%O zVM>DLW&m#TO&tA^Yj$mbk@aSD;uyV7HopO$BkI!O42YGG4Q(sxXw|agr8YyEun7cH zBv?DiXIRNaXl&M89ejEI`-r!VgffCqB?!jG7>kcKv`6$ezl4Q}4aAAm#`L%wco;wD zd^7|uGG6i?lQoCeUFi{}|w0fg|VV>_eqLPU)U#wo`^uQlVrG)i!+*>R71Nh@B|% zhj2TRfa}7_(CdJ_CoMTy{$` zi;8U}-%>xNO32pbDwUGHSew9%t+SqS2xtt+)EqRh#xkHUAKwQRE6mcNDG|EYz3OMm zl;`WJ>|e#n)1A?aBCXYx2(llQ=cjmy%%_bGcG41SxEhq1?uAqL=h1{>(8V`t?DM9N z31lRVHoNVEo<^ik;^gh$H^U0A(tT^28s6JohpSIyc57vfMy<9ZH#M%q`q_Fw97mm= zXtj{?!$=awxY;7NTosYiUQT?MN2mgHCC$F6;RCuo5)G^uMD^V?guKa^-2Gdy=O~UQ z)XwX{QivlQ+dx=@qF^3Y<5@UFiu?wl0^=bJ%=vUDqck^ad>kxrRJfa{Bnz4B>RZKE z7nYLdfx-qvFuWQC$kXmTr!N_u9gT8#s)M5o%4tj2j4WVQiR&)=t*U9V=n$6zy7rM% z&EZ?ROG51YYeFK_9pl~}&s_4- zCu&7Q@5I1pba2@qVZ(7>*9rC@zewGuHx*FDE}x_@#G;EsYTDKmu4q*9z{5SNIBrYk z7Ve_-adCAt-^H+@L25DeDhjMFa=`lJSq~ux{sW#tl%1jDjZnW;OvmuZUa%RI{4hP- zjsj{WUDx_naJPEdj#?NsyZ0O_u$0GRteIpWTPIIEn1xa<;1H{a5sx4d9fQjTdR>e8 z%QLO5l5FwD`KZa};&%ae7#ad^?vJ;DtHBf)cBHr?{RR6^*#E^DzvDN*TSM0S1}@?p z{05`!p+)!x3X*1@m<%osW~QXug#AW*7`WJTjer?-T&x-Hu*e#jzsb4Q}-0u$7M?e*?Hg%LE7Pt~Xzjqe7XFopbHs?jSXm-+T}`;>I)2 z(4C0InWnE)!Vf7plGmMz^JM;nps0l1Vt;T*udm^uhH)MR5SoOQl4Y4BiQ$;(^+&Vs z9Xq|d-5;=tqfg?tuMX**5LSDrLFD>T@9iL7wwQ6K91GGAPxZco(AgHP@vslk&Q|zF z@`Hg`{^Qo?trggxRAOL-kkW)=%ED{TCXztnuW-Ri48!X236WD|frtGaMHp57$=n8! zo7EBHRkq6s>q`bt&u)q%j);pBbj^?RH|jGyA%`jgJiKI{r}5@zFL-g`m(ip-`MDg( zZCHxtjCJqTl)6%t_U;k5jqZum09qERwGbmoi+|H9vo(5_^>>Ek_gJR5^^El(Aju~_kIu*0Nj zn1zSZk$ODU$f9_n>1oR$FbR_@&I51P{n`8S z>kh@I(AW+a^h*z;!?DuJHq#TJK>*_%<>FZEm#+6-t3s>+6}F@3@zen;uFLG6)65*Z zSEF{?3ZLb=U&9P`=Mtf4Eyhf~v8fJKmW=i)kbS#a{TSt}bP^2?>>zDp(J(@$gg*69q<5$*_<|TaS4)3B%Jkv|JwO{V{dRKwuJnD z!|oHMc6=MJiP8TbwHWf?@A=0!iL?sooYt63c@1d%vq3HW5um0>-x72Zey6qGP;?Tc zb&zEEOE=?}w@QB_@pG-pzcJ7J7`Xof3xu<$C|p~TOs@cZ@#A5TjZ0s7ekhCX^1=MM`Wfsf8EGJcoupB+1n?d6=2cUTQ~*)M3!Cw zGuCHd~}`llK|FTsR@I8ZxLp{x8t51 zJc3;wLDFa~#vHb%rskXRrT3GZ8cVvp4v!a>+@^wu|D5fBH`^{h3%HVP-^*5L8$p3B z+DN!Wv9V_T!FrpM)Ah@XHkUIWo|uXk!q+PT9#upWQod>AhICn7_QA)mCuTX0I&)X+ z_Rl-Ph{~3?2yl(=MYqa|km9}I6Y${d-m6z6;zHUn=;Z1hO)*AHA@x%^?I>63eCv=) z2KJSa0*X)E|d{$E08>MR1C9?BCskhzdQP^q}w;ZS0^*f;5FEw z`-6~>!o&!IuyHVh*bo;@sglYsH?FouQdtd3@*ao<&qqOC${+=s5^6;gt*-g7?oDgt zHruMaI>I=O%M%Mm#-&&JcaxCa(H5GzQ4t3SdhWLA|P)5NAVb!fY%IzNk5*zX+?Y9 zIn5|?-^+kAtp27^o8}G?lfs7`PajNV@)`1tn5Z_2xHOOvri=pco{8eSM!qWt?p{yO zJ1DEQ6JWHAu`V47dEi2WAj4FDG&9NP5WVrdD*R3#2oJLLW4Mr$T-*4gpKyHjX&K@x{C=XFe4#2N#4h|PvpKg>gAm7+4 z&anMOVq=V^&{t>MD!rGIHa6o5%ccq77vm2N3uz7T=Nr8Na3$|#ce3A&-1lnO@s-vH zv9JmvFDb=>{8NfyMeE#*Gk?!U#(}o{(1AS%tROA8RM%^Nsd`CtrZM;91Dn$d%y}8x zzOQNb#=M~o-|_^saW4c4oKiIH;RMeMXWCX?Zl8bREAec9G`6?$RO!s?dFKBpBB9L|klH2}zH=*IA{_gnOgz=EHh!O3 zpeiB&_O6liauy~v^4!WdLI240t3xq~O7T#*$t=ww)5(4d^zZC?JJZti$Yh5H&*UcWDuQWdD&lah~bsl&7P(&7ETKxAiu&(hRiNMY` z8TZ{3DORqI9C4N!1h2p%g{SGrL3sB zJNP&q*I<4awLw3BziM9CQe{Kc{d}JSl^j+JRgj2*Uj zr7SaOhWV2p_-?(n_aWj2+mQDq&PzI2Fvr-~lu&cGcx6Pa)yyK(SD%z-cs^imG)KIi z;CGSLlB&wF{-h+$8U|o))HJnaew}tY-z*tMA~tp|{O-OzQ-)uMNt~A60100?Q7gl= z?pJ9xT^*$O*6rcVRH`=L7@yrafaZeiJ+nMZ00&UfO-~l3PblOuVs7_$bXfg z3o411i4hlZtV60o^YHtWhJ;q>$eI@+fW$dBEG>v)RX8tTpD9U%FvaTLw@VZDTAnGB@ov!0c_Ih%I!BV!XW#>oI%ijBUWQyyut3Urcs2|>*>8@mHdd2Qf1ti660SCKCv8_K5ef&wfc#^nK3JgF2v zm=rx}t`g+jZhL9c#uoS0lpPbmwiuw{4A#*jL-_%%$Akios=yJvY+U5}t~c~eBzWPt z<-Qt70lZ>mjYm|f?0~B|ZE1&Q)FsmoMZ+c(7+m%jq4KaRfz0|eulDOnHD`vw{;Lt)q2iXWLQug67rx?$NnJqlk`Zox9~W21+JW#A z|Aa^_CxpV&(1*z;td(EOPgsR(7sfLUzLWfgqt;!E(OilIDT zR>EiK1ZO~>7LH$*!;wug@DWy_-u=x4P#~9+UKEDZPy%Vr;Qk`9_st@x1i`1|_QlYE zD~w)-AMJ<{cf*QJml4<)v^N|hsl-xa!lD$>Y=0PUy_T{6|4Pna#pi8-Xn=j0tsJI(a@z3Pmet0i8 z>jXU(RMD7-svt9fmzI>dn(_DwqcGJZkAc~;q?NqPS~9me@8y&6lG%d%Ne$KUCTZz? z1W6En-$)U`mY)g)qJEe~hJ{Od?jMn^gBR)L0P*)Y=Y=ob`xY4MP{NlwG% zr|#YV`^9oH@1YB|Sdd5fYDDo<6Se+j!K7OjZpcV%Mv)6a=b&*UGTT`Fe(ye&DJAhV zW@GqO&;GDKqsXdWgyv|S_-^mwN0u3RgeztCo#2&3f@G;Uma-H(L;ex`3R809b;!k2 zG3sv9#u<7&rR+u(BEyG-dH@WK#|w?WUzqteE4uQ!$;efIy8J2yBT>f_;ZpZ0m;QA5 zrNqx2O!F7Jsxtl=-?>;Tt{a2u_Lk7v8ya1r))zVLMt9kpBwSSM%Zs05LzZ~K?ILW@QK>Cb z7MHO{2W}GAon*KU0K@JM*%zP$nn~(f1Dl|;xJNuDW;mVavu#31hwOU7aiw!v0EDch zO5EF7FCLwKO^w;GkV1wfUr^WZ1Os+cbvBVXJ>@$lmnRIP$A8)_;#H;N2f5d-J zAyjQi(_q)AynV(0)9oMl)fN;s2!+IRXTY|4lPpCS>UF;XG*K*4K|)}bBy%Mx|Cr0D zOzGhZqGu5ozpDV$lP*ucY$!dCy|@b0y*v$SAXmvifu7-!?9GR16c#Pm%zG#=;J383qr1lBfK=xuroI8nX=iL2^R);9UME~nK54mSDt!=5wFu-;>0?8B5%W@@!*1+=q>v|`B&M?Wkl`3JLX-?JzR(qOfj}B4xYC zw5Wy-mbU}?&pS!tUODUtGA-HYpUJLG(G0&xhRg;*HOkFWqcIt-tK=_X=r_9DZ<@ob zS;XJlEkt)IJZiL`j~@COR)C!m=l`i#x9O!0Om}}!sVceegDz3se5|^41&f@3g6Yz{7ZVw4-N7}C-+NZn|1>$H_>zK*X z6XEb~n~^0)*K;4(O}vzsV6xC*u<|M!MZZEyV(_b{Uw*GLr&((sQpXtM>YZpH5KJ%H zPDIQqcIJ}W1(V&SNCh<9GTIUF@JIF>#PR9_qsqI7%k4wy@gy76yeT&@wY^Gi-)Y2h08u&~$*=3FRt9 z&}sUmAOcapfNYosiN=n2JhtkdbcOH>^VyuB2Sd8P$NtEYGlX>yHY!J-Pmre6(45*9 z_Nvg^EkMsl8&WHR3em9)4P1kQ*o5q^h&;u;M1$=nD=(@R%AmA?-|w(t zQY!Ord#u_agD=7rv*3w#))@0Y;<#;wWSl177BSYa6QN4AE$!CL?nuQ`Inh$t5Cjr+ zwP;js(&?-EI-k_@NyF`qK&|#L4i#zjvpVnK36p{5L`%Oq2NGxcD{+{Ej7M>=4VYpP zg$ON(XyfsE5b!E;$CzN4Y?aJ~CuhI-ql!hlhcxC`V{z7VCmT(Y6(|P|yfjjfs(-Ho zszfKF66YhLFHW3D1V?83N}KAS;V=c^tV;5B;B#0ZPU?&LXdbzqjHcm#U=t3~TPCbo zghg&GPphT2z1wZ47O`ijXFNd(NKiy4IMd#eH{eMaCmH`hNn4wD$*fl?gsW$1d9gtm zWVf0{Ksq(c>0-G-pWZ+shyw;Sbtvnk7F|g*;FYS%Rl*G89r`kEGn^k`B*@R>YoC||C`G&fJz9(jM^rKi| zM^dU}ejjenlI)7!@gjdFrP7qEme4PAmTY@Q00%9%)aVrrfePM4TWVkjL1FbbK=&hF z*2V4Ws7h{Zds7~w;)_i6$Qz9NGZDkBDBcc}=*CT?w4wQ#`m`CJHtZynfmMv!g-y8j zyR;6mh^9hXOr{a0ZoGq2&GrE?F}cT*a0Q&q1mO*+e<=$>4*rop)I9v$>kA zRY8{Tq2h}AqF-{Ovzhi$o)h%0J?q<<%JBKvMYCApSC7e^Tdy89rkGYOwkpZ!8m=K> z$ih=FaY;;C7IxbCki+T=hVau+wP8(V-5s9RgD6S6ZE`H23@)L(6k%_Wott*(dAf%J z-4^!n4hhhSwA^}zxVZ^L zM%kMDl>z8Unf^w4^A;w@vPLbe^PY336j&yo_cc3wYc z9hG#ON{7csYSKh)DB2;O(!42lM(v&nOI3rs&VJMSn<%a;?6llzCIlB*C|ffJAgDuQ z$by(#m>%u|>p-jIh$=jnE`x_V_-6}OoFd`XH|(-R7XEf{#tmGJrubw}E3na=rphJR zrdJz?7dbW|$X@gAg993Z_Wnag&D-s*M}kZe%{`r6~Q@Yh2})DqR##HhW@ zVM)@4>J7T82GH9h?@mi=0@($}7~Uya9DwJp$npv}uM$}``y_sf;t zb*V3^XEkta;vT)U*`n%8d4@IRP(h_eE?HVh@qM0NjX|d{^I8u1OD?S;eWLeE-1{#*Aci#XS%Zn}EB}fr zJ-o9c44e$Lx1Qj@>~)y)CO#Y!n%iN6(+b_?;%QGD4O5C5=&9D`Vw->HGq*5l=U4aW zeM3^yN9@LeXlMWTxI*X^SBa8@_Q(|T0$W0Fyj5Bp=VLH=jg;zwJv|oSHvDPY>)!ldMAF4K%RBh`_FkxA*<#=kPLGT!M zu1|MyRD(!p+}OhgQ~5zb%46!!%>sIrf&;q|qk@Y98C&<$5@*M0^Mnhp8n*5E_m#)9 zMktQ^Jq>Qw)QMa$n8CgAFcj_Z-tQS9Z>F;HtAXtn3}9e+mts}#8+3Hkfdrz+^-k}c z<8zf=ce@GoH4QVnR}J7QCp8w8MPk+#0nn_EH`r(7EETHQ$ij6bUD88YDA`>#(@^i& zWQ1A4K|WhIvg)X7zh@Y@i&Zx+kk+hdEmt$WDq*3?`c4{kOnZ>7Jy!NC{vBXjIA6f& z{WGS|d>%pH0MS}`lTBo(|A>j_(wfj8$Q2RSvxl4Fmj}lrcq!MO=-X7bSG+t1epugRDCSijsBLB zTk3H3IMHA!v}weZr1SxIevBh+Q_L5!1wy&G1E8zA+6k$Wcpel&U3W{t*Z~rgVkC&K zBx$-xRK&fw^f~{ykuL$i>C=W2#R*AT$p(|RWd#<@V?&-shh6kH%iEIE?>99RI{??HOXw?son{sFH6&g5PL70>jElgFA4hys#7mBM=xs4;Gfxj z&p9~pwU@bWB&%kSgrk?uWe}BX0!@xjxXJu|z zal|utp)MGbI)y(~%^%k>2vI3-4~hQ(*=Yg4xID8NJRV4!Iev%4dGJ4wTnl*OG3Ji3 z&dQJqiyOZI`n~~LKjbO3{bfu3mgSKFTw)L7wEE7%`Y_glz9 zXWsz-E1<`j|4}o4q>%dmynp z9~67@?_U6Zg?y&`1M=MON%H6Eze4_-Z~RvxkADMv`4>9&$Uk+Cxl;t8i=R%V^?$jJ z#}Gq(1AO{Lclet^-giRi%WZ#NvHz6wZ^%EW`sSbV|9OUAB6t4z1OF-KFOWyb|0(~U zXZQv3#9u#k3={rdUB8LE@t2+B7hds0UB5scpZz@n{ssCkkVjVj1%%%_lRf92(_{N3CmdEy#L1Jy%%q^~bB$UmJrEM&t88(-f)Av>FZ#AOyb z`x<-y+piKv0quoI5ja8Of+nr$kLm5j(1|?Y=^gXkM=NdAmj!7} z+lw$v6Kt&-Yd2H#s4h+{0DwgbY$06Gd|xlS#yOBL)Hn=CWB=sc^-RM<;xDL^0O&H! zELM4Nz#K@ZHzl~kkL%U$h6)qN(oqC@rsn|zW(pE7X_g4|LoPe5@j~=FUH-d=OR!QO zVY|#&AG(~rG!^7WyvBSqvu$`b$?u1=d#3x|q_xsZXu7WyF0l=vD~|AL)GN|>P_6AQ z44R|S{f^WM`x{_LN4B$mVZkhhB|E94@-YSpFDfc`VeOPDfEnNf+LVO`9nnuZ!SW-? z4o+oItS|>bV+9r6;Z@wZHQhX;hb-p__QTTI*I@<&k*~?()Z(OKMy~JPBhMNReukHE z1h^W{%4_>4cu^voEJ}a*lHct};geb@&;M>Gbs3NW%$FKJP#ra*l57e+Q(b!KtARSS zO@Xb!sN<>7O&EwL!YF0Zh5bgtk$O|dlWZXj$i^F=#*V~DJGB#JD(-+Gy2iwrFTIm$ z3=Kr90d&1C^^{beU?J&Wr!~@u>INxN02zJGVB8SReq&Y1Pcu;e#WKubC}Knr>LX33 z8lWZx`(;(Q#bYRBr(K07bX;<3EIi#|qX=|LiZ8-_L`%sqzVzP)= zwgQPz(0y>!{WouOx1f+cBF7hxg3RQ* z12GZBiI^G(y$?6jL?g1|9XjisHKGA5Zp5oTW#mB%93(Lutd}c1h@j@6@}Oi105kNx zHYFjt)qyPIiD<;un5)SMfB{NGV(-?P^+)0}Ltfkcr#9aJ2zn!By{1{>qX+z<>GRGQ zTovnN);fv}XM*FD+wOm(!$L8`G#s#mKUy{p#<)B$VUR{Ud?>|0qY7hE_vvI$NiBx> zoiWmMROeukfE@o*)g(J7^?(peEC{6!#3%{^oHxdv+mr9ki%eDc73h2eNY&m&o8J-* zUi18%4uyV{!D01ynMBF@xgudb*-;D~*X!aa-Pq^SIO)qXQp|&|3&KZ__6qRLmm^Gc zNX^=Z+(Q{_)ng9t5BntX=;$q^XyshISlcRJBOEy0wN%OosM+Wp8v>PchHq+{88Gx^ZSANm~Eb{=NuK#fasQ^ zXdgN`&M%F}FX=~~*{=zzrnXlH?Mdm^->bL%@V@W@cGl^YqbyrgmSD)c=*+Uemz*=t zK_aztfp8JM0HS0F&Yk`n+1qoMqe@5+Q{q5OOqV|Iq|8;Znr*Gyz3&bJ;F5jrz^vf~ z@eG8V8*#-1o!HQG;5_D73D0XML)0lh11XzO9LtM^W@`Xj*lY3%@%x| zRn#2Ua+mU`$zaoFi92|@V-6Aj@EcU+RWm;r)MCo=xAm@!>s(~3(56Dv-@PsfRG^SV1C_Qga-k5!MrTLt6O-K1w%w6&yC|9Vo>- za12q`<8Fxid3>*FDw3lO5270-!8QT$0EuwYipYa%;U1<9z_Q& z?+9b+O)%3n*yyjrZc;7zG(2im1lxU7HotHHKJo*|W1#IZI-K?#1+lR{Bq^R-@6dZl z&QLAx_Kb}+_i79d9GkA+n*|TK5_4|z4!?!VCH^{-)-4e4V$x+t1*W+)+wNBj_m2Mpktv~i0CVY~OkjU-PX zu`CPa;1f)cjTM|qkoQ~|pj*W=Z)bu(jnuX@JtkHh33Ry)+^6(|&j`1EC44{+y><5* zNJfRgQ&EezN*2|>+pEHvg|v_0X@S1t6{jF_Gu# zc75zMAe`^BSB_y73iEl-uZvYGC741i3%y{O7sl)TdJVdz=Y%0wQ-~66Cp{+e# zV?8C#fSrqR;pkvuO75m08$^8zG!eJWR34QGuNoxsA1q|p$btn4h6!^Y8uLCf z=8wZhN>dJ1(fGV$OcVdwZVGniDd5_PN?h*uaU*1qDS~-C`N?lVtwHot&4Idqf2{cz z6=Wv@ipP-23Lz`?WOX^DE1*}9C*gWa^K|7tN^aorCP<_~O(%yzjTfyMM$lI2CVu4nb%c1?fYs~$r_YYPd| z<)($c)|_{+F66Eq6JYqTU19R+>*rS&g_FzF&wWYuX2W9Cl;tuA1E25^!+(7jcy3t9 zj6MB{((~<+z3bj&#)Fi{5n%?RJ0g`{DtqOIeXg>HD<9%gL`c24B5IS|!?%RrC9QYH zU)Y^) z5_#;XO0Jc9yupJX^W~cF8pT$%ch{L0AWf`X+%P%>{fYeIDAF(Vx)hqf+VDmDf#%sb z)Vv-oUD(aw;O#7hEwuHM!mr$%2*$J1G7aK$D*JSfmU+S1pAHGY@xt#V1p?CODFpMz zB7oUn-+3)`1RYPmzu?+`p^IALaGo{1mLtdl&kaT<-zQqH@2Jb?1~)-lx~Tj=J_NmKBXD zb@O^7|6SQeE{P0{9=az%r1W@n&tk0#^VWrm1QaEW;p{#bADkcBB%vdowsX!Ev9?i9 zf5^`-mJ_^jtWQXGUL$)M3{BXjIX3xX<8d~7FG@4~O%>~StB?t45Ydt8tcoHlG4tr@nIa}}&78}g zpaQ4U=y+q>)a}`ukLB$_qqZD$>gNHr}a+E}o#?rZIlaRjD%QhJQQiX~u zN5bAvkucrb(CAd0QgM-xI#=XwLrOFzzdtW$bfA?FiW7F!i|G2C(yV!@XJ8dQpR%DV zL@W13mx;!rA~~T>>%SulnIt`l6-ER|vk2y1zvVFNk1%OV(FW<5P%w*f_LX1uN8%1A ziBg19%95c@#R7mf*jF1&Px@bvwKDlfn~KH4%jwKiNCXkXf3R&bTi#w-s6Tw=xY*=b zdWDR;x^QKE)Hx#JMn#!|20O9j$UI04r9D~td{SW&{KZzSNG_RCqYScP?3U921S5!c zb>1;UCn2ja0E?hO(D(%Kkx!fjFQKE^BPeVJu~@7_v!R|l-kiWKH=XqnJo_+#Y52}h zNc2b2sh^}1M+LolYgHj*!cDh%`;Mjzv{V8uIkKSF0EPeJHbg-qdDAr15bFZ4b zER*9$%Elh)eRErFolPuv=q+@-q@Dv9-o?yk2T4ijNYv3lW`9LN=@d%D5vSBq*U=?; z8%^x+s_N*tnx)Y|+h{zDGUey^%=V$v_x9`OO(@uOdni`HY}G?=k*s%(A5UkVEYx3( zy*8R);r)?xO)SqnMZBc*yfDbfURPh`nxIRA{tbE1Q68scBu!fxL_~om`?u?hzyZtv zv@Q#^ht&Ga>SixA!26d}brgK-SSBAQEv&?zZZLw~(X`xz6@$Xqn_%NnIN1~Z%J0>Q zj_nfEjm0ciWCBWFl|J(}8cxsXe*@dq#-9{i(<_qhBxa5cZDu>pqB&=1oWFVwO#+O+N8enu@dnXR%XUe?!p zdj_gnsGop$jJP+G)ne5`UC=-XJsy^|sA1mOuaacYE`7M$u`(wzR80nU5d6@ynSMqs zN;#T<-bVy;sKgR%i$6MF=nGUuXr92C@O&0>b;wsCYJtJfxQHs0f7|48!=WY`hDk07 z`cdB-`OBy8w*K4ZsDUV$3xL^NeC@ZZHH}kvqId$-+~$r=?GX?~Gx`P?(^psEXB8(| zFQM2tE2$TbuY@8%8S=?bf4K3adLS{c^Q%&Vc{#3)631L%HCnYgFHd4q?k6?ORh@mZ zz2#C(#G{KGV!`a3x8uP!fnZYA<09%4AxK2P#K8&$7qEZam)lOxrNGJK!omhAM zd6`u<#Y+mbY+t*DWy{A0kzFaXyIF#tn zODiL)Tk@jsx*6jhy?udn+marA<<)n0TW^53_7rkQV{6`V2&{QZYMd%RRbg`%x*blt z{pfP#s|Hfn8N!7}Ub|+B znsqvXFP(8Sg7xlXQ6>tY{+JyxmX(CKmkW)&Fvy*RCI^5~Cd{u@9D1IkfA;};N!5`_ z8O*bZ)S;t#qv~2H5hTBE{;)d&DZbkTSUa@_ipy2`dsueNEmHKlNwrsiN2U0A#5ln$WzgkV zf<^lYD+Pu%?a^@#kKeHXhN}S4!UtovQWA1fE&)IR^Jat~c<2;EllikG~*0%)_%Xyy~<){FP+@ zt>C!Imr4Z!6tC{9`%*o|oh?nC*TD`(?zB)@0q@1Ej!86LFS$_I!BA;!cRS2E>f+{Z z_fg?B*owN*ytU=TnzRs1$0<=*vRG*QUhV(>F1KqNwf4+SK`y#T-RH6MgS0G*k3Jr*exJ>+F_$W!V!AlI@;nuJPWnh>zrncN~*#93NtW{yLe}G zO91#3MDPfyc7K0QKs*qUot5I9A~}Xktqx+tJ|FFN-6yaEUalm64DF$#Hc&Iq$&npu z&C)GQZwx01qFfbvtTar#D^YkI)@L6fW`>=O+XOKb$d{d0g)>mUfmO9?l5&Yzn|0t0 zgwL|agHy=0#hM~5$Rmk^4#81Csg&UQ?6v!lc}qad&E0_;hVeBfqY-dIkMDT|IN6ig zKR^e`Oo-hsS5H(9AEdeXz;4`s7p`CnB+HFMyAQ6o{3LNbtxlV`lk(9&Af3}ah|ACf zA1QxCht=XBx%3rLn0}-T!%_b#y^01)MVTZ44Vy<=`c)$G`D zj1}CsMqNwXR$NmLLAUfp$dn}GZr_#&f4E6wrv4NWO)d=yP%D{Edy#K$Mq)bG&%w|2 z#+9Ut0&ai64J=uKrfT>mr-B{7U7^Q1$r@nFLqV)>ZlOh}mLAD1rLFK71**~x z`zn$w+-(%Dz>P5E(m5)_Mjm=7oE@CDe@M};ozfD4B+YP86)A?Q+Po;@H_TxL{Qz-T zL_fO+U~{NesXR_8qT6I8!^`l7*a3#KiDEhx7=lTQ*PhUTY_<>E7876unxb7$)mhO; zDp6Dv$9f%+cFM>yV^;@{$m(*!rftCPsjExhiiM5DgH(!JM+`lx+ef=hixMLb`+{e8 z>ZvW3&Ae8Gh5h2;Rt-abGHX=7Aa0OcS0MdpSZma=)@-*&1{!9;lwa4g^igwg-PTGv ziF%P(1&m$!$W-(I_eL(-BNgJ-kE+UPqlV!hSf9FSmYNy6a%^{9qFEQ}6f1IrHMTZ( zx7K}~1w69aUXZjYkJGW?q{bGchF|qjOf_NrP*-K!bkf4zXesbty3h3#&ZKiuN)fD5S=G$LNDo>j+Ov{DUM4Q zOSqqDli+J@D7*6okiY4mGdN~0=~G@r9C(xE z#rs7~E_QG2zr2%wiK0$(Kv=_6y4ZqWzM-|l#&Mp>>K~xDY#LZ=1DM1OpUr@hFLjq} zabLlD41@+_nnrbe}tdugB?LE3^f>;L7uB^F8mF!9G6ec$ZdYZ-L1Wf9vlT=5~V;?SKWjrSJWe_ ziWU^j!&Ssewq^FQRX|>MIb$RIf7pBLsJNbGQFs_&fWZka!QCxLLU4iy8{FL;f&_PW zhu}82JHZL=F2REbl0c9^-jLt9_uO;Ox$C=ct#{Y={&?(J?7h3Yx~i+IcUSLSU6s1j zG{ONXKZQ(>eYp*ERI8VzbJ&*oR+{|6=;n4n5&`?-m#jaW_zYN0oyn7vU5y;4nJ{+L z>r0h|RXqkF7|z-sP8%i@W8=lwFI5af6R_MPD0Rx62#OQRS;crt$oA8Is$7NfB4#mm zvYfh(;h7X@RTpvppx(}dHa&G|zX;x%OsQ0_4@;aJ{8~X4VsVqkQ+n?WS7e|f%iQ6e zqd^cuIrt#*E7bWrzAGPM%t0k$l?}HVF@+Xg<$jgc+`WW+8danNt{g9N@jM~X^s>uU z0HIszUHt{v8jUmkgGU21JQZz3ks~PKv@%rZ3x_GbZW8JzwMxiHxWY0e76P$~kR)wu zBy}a%R4C3dDAu3H&?MUiiI|jMPYYisH=|%bQytYR2uT`?!ynJK6)!)!R_bDY=9#p5 zrWQ2%K@A={V%f)U!Ol_rXymO=i;;rIP+|1OWQRuxaYqG%C-Z=DyR*A2?;OpP!HP^b zHzh7y{1v617MYbUSM~QPYUuac-Wmk|yd&fS40B6pK%+Y2CA)qxpBfWP=+m#jWY`Fb zX88L)$09ngk)V>s-56krSoSWL z>u~Du*Lqo|;V~d+Oj>TmIcB9=1G2S!#i@3t9xUB;d*n!$SkZ5t{kdu_o44>X5X;|j zOD;x!%StGV-F<|;9Qdz}2Z=>w)L_;P7c0K@Yium^i>;dU*UnRpKo|M#p$nCBzUAnl zocWtn`Itf#Lpy0@rw<&oqSC?0ahd|3p^^Az4$sBYgNVFQjU@{dHYa8%#rLDb1@=8#kK~2i&d-{I5w^_;MPW z-6vPpURv0m{oizH)K^QSZi_(zcgfR2ZJx*Hln-;rIt@Y`J%$L3y&RQCuW1gcUTc@^ zGKw(WJuy`eQ|FCUVi-N!W8ZfYtSQ$KHGJ`c z&kh16leGTVX>?}&R>gD;15Tpj9$8fhLFUj2DhYpr;#$OlIc+MQLLu5{;Eeqbd0M*V z#6&iKlzz7e%QLdVM)%m6)t7Jn-{t=%A2weOdweC^?R*z|x%twE8d2v*eHOFRSd;9I1bLmwX>6j zDTH)^saeUNc2sk{M5!H(%C@&iO&m#AXTg8v*(YMVGV=o|;-Cp&aNaVxc#uN!m2@WJ zCv!xmPHY)9**q=Jmtt@Xrpl=1$G?7Qv{Z;)&WL;{2*mcRB9z~TR( z>g|Nk-w%1TPxfNo> z!a9-IIe@_3LS8m4Sz=tmJ+=7e48tQh8N_q291_AaS{%s{s#Rt+s2$Ah#_PbrUIp=l zW>!azb~Nl@`y@H3m7#j(PmCUO*AFh#!@-oQR!R)W!&f=?(v$*bs1m+gU4~|dWFoAP zAPSp|MZ`d5o|xS6-fse)8`*j|yVn)aEmXtM3gX&0cd%Gkw0NNW?BwDhq1-BDuh!l1y=kG*f-9?{Dh{Zj`Oc zSLmcq918;46(i0@>3uQ7BmAX=(B>!7SHGRQsK-EMqMpN+ppFcl?^EMgkBwmhAuuQ- zdkj<7C${zwQDdns9@4Ry(WoR6cn1PQ@uHQQl;G%%FL}TTriibuYNt<+p^k)Vh+asy z+#|iVxaQU`-SbG$#j0>BEOmS3tDO`h9MGetZtp?r-53=!sxo7)h(kZRW-6V>MEG%$ zfJ1CDbq>}x3*|03dbVZxagxgKMSqx~EzM#XYhw}DQ@6A)4e%F7)t0B4WHWnd!dr^D zit*WMMhZEmLK%Fwm^DU+l>o{5EK1cosT58vX4Z#;&o)c{ z?4ayHO9m=XEq=9+{#pLtIXZG@!kbt{xSf=!r7P%RNJ`HAq^Rbb026~EF;tbgUjKGv zwLK6wS*@-|_z+2W&x_j-t%P4LtMA5+Tb&z}|dWeHr7vloyHdj^ey# z$WHxSQc5cJCUok9e_50wliTzpI&Ez<*QKpdUEwNR4~9+HIS}br4)( zl_qxCrnyv8u0V$FKu=)sW+KsCcRIz6>rkH$E+n6~;Ejm=MQi&V|3sueuVAXuSU@UH z4~i{YYpPBmKrFf8_>0Bbz46 zX-HLdkq4tC1K{BNcxcMGcS8x#AqcAEK>$YXtr0o>_maV`Vgd}lu15GB)fV)a_!c|+twL;Dp zdB>EuhPY*@mUpnQrs%c#{)AMCF$?1fKzb^8T7Ii}I3O?n;*JYcNXcQfj>MyXc!f7Kay(P9qPPkW;>8R)7x&4388fp$u&TatE05~o(l zifhLLW{JUEsfjAsI=3k!YIQd1=Q$=N7n&+0g@xn1WI>EsyR?Wyq%SoeSR`syaFrlh z+StmD=TMB|Nv%M>J`9G47mwWn5E03y|0zkt^00;>L#!x;A(KsauI9Wgz_)f(5=F>!JVT%AM^J(X5SATNd=iCkC&*GQzd9P_%i|VDY$W*m z)sG>zfLd9OCUZLv8)@KrE4n%Jw4&Rd<6b0lTRZb3T?{@H>H56A!ePczH$o)~9!&|Z zv6+lDF$fvN)4o)JT1%wKgbS*;4Rf*5Dr3Ncz+FDiZw#(QO*!--!z_{ZCxQ%$I8vb$ zruR4UYDEiM4%&(_Wxlt*V(0sZ>%IqCUfft`A@aDqLq;T%X$H(7 zEP|CXh47g0um1p)JFN^BrF8(nUEqoWaZKO-5;~*^ME;-sg;&f*tP)!tTx`3-BEJ9| zu|yI6&;Fiwd0m|>)wnD9X&R}aQ}HGQlgVLP1M-*_v`>Ein-^d%(>RHEE94cXBWYmJ zS^ZzGfw@dSMMVIWae36?q$)N3Ln=;{2NQ=_5Atd=(%Qcj9Z96szECry4TcmmlGpzg zzs7X5J0Bi!`Uw$1QsapHJN`2pdfDSDi{n`S-B_XJyINBsOi&a3518HhZ@{$t>7g@Z zL(n&TH*PWYW15zS@hJxe7+nMmrvRz`X!Iu_j;ZK;?R6d)SOrUQ`YQ#7N&1HjC_W~! zI@zD$_1aV1oIf;$x5m?Ai%S~fS`YgEJvD)8DXt+jbjAM9q-X96Fq5~ zmp{!6zY%P+P-x;#d{xEf&jK8SZG@~8Y^7y6TuGyO{NcH-L@Z<1Y zZ-qU%)2G45*B=ygIPRki&KOuL$!rRWh3)*FpdIxfnGXw=hREhrGfL2~od%US&2LJzB$-_d9v7G9uLR^FiOW_+Xm|{1E|ga3^Ur zOnA@3d4OjypGJI*T{ePl5rTMgwN+wJ27wOMuPVKMcx}G{NzgNP?WrrTc~bTTV(dKi zT24$!)usZ_0IOa!0FBW*2KK^vk;Gv0>O||YdW7}@aVSS;$&9J&H(;ahs`58L!_4d{ zXLM|_;XyAYt;*uAL6`s|F+eO|?0D`fN6__X4l0PTUZq4et=&%kBZ~c*GtY@ORvv>` zlnJHdZ-A>3BD5(5W~peu(cyl!*7A=hTa2@*`uWH?S3=g`fYpyz?&=2gcOA^^Nl&;& z9?7EOL%0CoYCz^G6Q(!>t`s@T_veb%``ja`BCzAsLER^0-0~w!y~uRrRia@ZCDe22x|%~Oem~b-~1}->u-e@Rhk-B0}h zdxXP?|MEmUjEWN_$B$~3QUTgMzX7FMW~u-1vrA;foL@#PBS?N&{X^|n!q=;@{;WNq z&qt^KysIKc6ST%e8?Kz9_Zx7L@>lBw2xB!4;Pj4CKhMB=?$>$-TNEg@p#<}x{6F#V zeePZ4n3Ed4QrV0uD1U4&1g&p9U4bbwSyTHvHqkHLQ1ejbYz{EA^6>XDNqO zU~bgu^uH}6{Tjb_ok73_0I!M{@k=;XBM}*_Pca`ISt`5V{v{Lt6v*fw!y5e;cja*a zx-BCn5j+^NumD;A9&hU3!%T&Nu^_?|5y54E%9{y3lvOEKDeT{XZybLw@Gqe-NNKlD z&CSKtx76EU?g0CrqHq0+%Kw&#JR7>*`OTG)4K|!4|5Hg+30B!>3(>S}`|Sb%(T3BE$#bMY zk?HJJ-R2BMY-!oP&M&Qh31bcH3~6$13Q2r zLb_0x(dMUO{kK7w5Ad(-^-XJ}(S!cnkpY>pv!PT#l{HlnR_aq7>f~2d3u^Gni$XTZ zPP5)bIP&m6!|vLcPd{J&xZ2JWFW4G8;WDiLS{zu3jPFnD(`3a<_UWWXDSYPP!cT+N zCnK|MM?>`GLb@A=mW$7>vsv++jjF|0zbj{rmGr1fqwPIlcyc6HYs+>qQ)G+)fE;E+ zzA$27Zb;T9iku0W6PJ1T`dBS{&y9W-kA{d~$0!zgigagY(PGro-p=#4JRm`Q4yj1I zp1*|hw>J6I7W)Mz@@}Lj6d9ZSAe*aC`AG{Jj}EXg2O}uauOhK~d@2J+`))ii&C2U@ z?dM~KwN&67*EANFw8W<$veAmg$}rFB^iRb;J(9HQyz?{q+^4k&FnQ(HoENCxa~Q&m z6mN|yP+;^V1W?ToBGrmV1z6p=gCLTMIpaxkQNKgiy8x{AGT=I3I`$$I;H5ys=wt%gzxeL!*SH{A zf=iImhS+l&5Kk}0nKOxutvB|C;*PU<+rEd>f6a&6)KA?yp7aNm%pac9{2Rv( zf=qv)E7Gn=Rsn;SSCIP{0Gf>6AG>j245okVz&zm#QNd?eBskRJG#06|iGd_5DZxE5 z)B5Cy0SFqQSTWjWMfZz;pKEPl4{?G61oV^8sUPVTIA-t)#)r*|3;Ke|KX19NQ#;9T zC1737?59C*93*)~3|Ayo&ut@woC|J4U|n~cmMehz*p5!(svTVViJfUTUA1&O8XRx zN{TPflsFs<#oej4tR5X;#je{aM9j?l%CJUtc4*eF&f6C+gMI^QjsLqe82fl*S^}uR zdb1}#@hra~Qlir8l~XMHv{8|I06)jS7XhpoM8Q zhQA)P{Dw{erqv4oPCvk&h|f-zy+w-QU<#5!JN;7awLg%ec!TYulX@J53RXR2H4!~* z4QsatUz8OuZw88ZZ+~Ui>lB{~hcln&H}SDT=tRNI7|4K)eV_hU5JlM3BEY;Z3}|Pg z(2ScyZ${IrptS~fQr0xqkHNfuvNX4v#&k&?$_gGQ_^$p*sUKraNhS}*Q-9%qq*l82 zZ(2cZebBeS_Mu*M%jSEVC&(w@aN8QcfobAaVBTqhrEf_Ot4L?j+A*#q#9u33P`DH} zi5Z|kxdHr>%wBH~VKW`UX5*4N0{~GCv*YiR8GV;kb)|?TkV@F)-={))TJNwe0?7f? z|JV>R6NsHuh-`gq+3U@JpELEDZjJmCl^a#g4E_w>;|ti#kQrvYM=9?QtnjC62P38O zDiJ$Iw5+pohvYtwT8d9OLuDQTNS>0y}-tpbO zJ}E$^z@@d3E8Xyd>5~3j;Z$_k_gga@-5&r&l~6=RaYr7Q@WniC&Imj^wx;UDOPDo?7Z zyhMk0;=)b`zrLI(k z5VNc&sw;~t_+ikFAuJxkJ-1+3T~Xlc^=O>+zVC1n3&P>14mIAwLXDu!> zr`x8d3K9vnnjENWj1^esG2c+UDPqXXOIsyZu2Zie;+RxnK~ED!>{mv0N_5UHOs}A| zR2YTKaQ`vb{Eq%>%PNqj#|wQktzmqhpwpBCwTCsCPhNaF>sn}@K;5uq$N7(G?$bZQ zJjC8gcUD@1D!WuXAr=^U&Y76h;bdGDo!a3h=91~~L=$I!%qRsEm2Fe|jfE$L6qV~{ z+?qilB_R_+)IUcl@l%7rsCL}&FE8Va^0TWOK+jQ(Uq(mL(CldH;wOwmb1Qi)U+GqG zXzLX9MCXLe8roFaaM-&S)C;3&wbCFnDe=U-Pudbht_2FC*}K|~u_%%9dS*Ir#G5_Y ze5RfrKKIvtt?7g;S)50vfrpijhzSYFoo{C_BtS1voezf#+HOUzx(sKe3ou_OEsv~- zSOrn=gdfJv!T6yt$<_4eKiUd3tNwhgvMH$l-RU)}pZU*D7CKLN9rI?~8KI*ATy$I7 zR169lx(g$K6@sgTu}g{mk!(YXW4n0?N~Mx&G|mvhAA9A$^X<=-uL%?#puH20ASz;c zTSZwg(!L#b7T*Y5CwiA*yjy3OuU2)cpWq&zeUZn2Zz2DfRyoXWfM}*;W3*lP?6_=Z zB&0J}@p8x-UkMjAm1{n?j3;rvIMcPskdRDM5Qm7MO1o+E7-NqHzKZUnB9&L=UEmdm z*>a#)>$6ImH@mP8#0;%3KEM$#4X5{w7$Zt>arvt4gY*2a;L7WX?d3%zRcg-wQKG~B z0wP)hY9Zwg390=w-=Qfr=}ACSX=}2UN|<@9frB}>SaYLIC`^!k@y+5EcdI${5h#F? zY-#q{QdoIUjKBwfYj*MHFrMj1tF+so%@K7Yi2L;sqF!bwe17e_h}8(3JuK+UtK1k^ zysSWJJ+CsQ#=#R|lE>0rzxiJdcTef67axy(`5xX5{RWiZe7<_h-2Qv!9sg z5O)_wFDVn^;FQ{Qw3r`bsPe_fbFyo<%mcR2mkdFaQGpf{1ULvIybP0CM%^9g*p_bL zq$q%QeIpo+5Vq zy_w3^T}ya1U_Gjf4&3Z>Nkj_lJXEa+c~bQO4r_Sh=9Uc8pg4(kbKp!nn#e&sCP*f+ z$P`(=hHkePkfgay*yolyN*s|{j4ooPjpU6jx*Hxl4S_?cEB`p zO%qQWgcCU9&h{hL7EW@l)L8R5k%_H>ugC_fEJ^{JC$_TFz$cx+Ii(f(x5)@>F-pHi zlh^w>BppZwa7b*_gOIjDf@@h#wAgu*pASx(N9$Ak)W~MDr-?>H79*U5>3(WzTJd}! zbf?gCP^V}LTQQp`kpbL-KFR)b=_Njm5hPgy75v?SVJG0^H>wFaieO%q>1bZ%NINj# z7%NMe*;(S|op}i%1m9A-!U&_&&^DJ~E0dErV3%QBjp5LKMr_-KO=NZHGw8ihkvB1J zUsgaNtJpivs4_uo6myPA{nwGUJ-|k6w>N|-Q~+kFiVkz4u>lR#6c0U$L>_Fkn=OVJ z-hzA|Tzk$CRPnNT|6;KeQbZ{uBkWk!w&dujVPsIxW{Q>PRqln)HS}&W^W#is3|sc?L#}5j%-SmJK2v+a>8k6g^6YA1eHQ(wy_5C zdxff+coy?@HW`Gnaxv<%ByJNCmhYi8_Zv&t8qBkYX@Ga(r9$lljx<+~I^d3xY>a$VJ6{{UPv|1q)BNe;SILshkJCr=l``5UF z%G!lozX2v&PPVm~nTjzX)(F)(kN?#1zbZ=E2&qw&EqgB9oyfi$-7MP!1qUhLvw4mn z=vh{qRLTyDPhHpUSIxx?JHyX%5xZo0%^I?BH$|(YG>%8cAq1Qix-F3Ao@R@Z$y8 z4+a{uv*^FwXC9sqqKy5J^x_RGGk+&)(U%|JE?J%VDGC&Tg72>blo&J;SKniFm9pc+ zf@>?FQcSMi=JAjZpsHG_q`Hy|8U`D7?f8m##y6A`-#Y{c5HVU!SqghxWdo&kkV8`d zQb(MPj=5va(XLTyb6iRt@tla{Wy_>!;i+DoVu+219c=OX)uc*>R8y)FJgy?UhN#m7 z#cE;ry!D2?W$2Nsn@00qn+bdva&&Js<)&FDCy;cJ8y*UcruH%y4Kk3chZZIY+0}?9 zGg$Gtjy!Q<*Ym)6)6JpLYBX8zROU6)Yv9GL+!bPc7*Ssk*ga1GFcjlP<3B1_I#SB< z5~~r_ewF4lx6~L_*{EIwM=-v1rKR3x6o`uY4bW1S(mUJ^MF#%LvlcOBRgi7-)s^>5 zkNiQ*%rz5r+u})>$r%X^e@6kYO!nq2gM3FsLMB#qp_TTV0myk{L=~zd>gHjE+<*L& z4;%h+_)mKw2HtOPJwE#1x+kL&vZ4lo=2;tYRP7H1hrt!1Z^)r0m%eE>-|zS%{Ddzi zmQCQ1hfc`$Yfj|1R|nTYZ_|UV zz!F7raISs!gPg|EVTycVLZGb<{2NY8J_;#7{-CMB&$umCIiq+CI7ff1$uLl|s;mDN#&0x3fr3%gy2kIHaO9!Nbc#_K zgsuysE3+x9A{-O&40y!{M7ivoB&(?sTBE71TB>|}p<|o8u%O2o2+=Knvy7lBpu7C? zT0bOr%!wexNLJg*obUYZX-7kj5vKxVOw%D2He^`dGNQFkcT z<-MU?oZvE?vZgjHt083jQ#7W2UJF)8?KlY+u4;M!CyU{UHR(*2HSkUGwtPjcU_BbI zBVlHKbwQ&E%aaW|4?@-=QhD2|`r&@1>*%Dup&qe3D%GSyBvXS^EP9sp^r~`7I)ws% zb7PRZl3g(~r{XDU7M$yPX61naAO*bQF*BlA_fQ3lWFE+CPCelv=HClSUMY}t4{xkP zFJ;=#-1DmSM>ubvP-XSKcitD9A92x~N}RA_{WhedLHQ?cu+EIgMcb2WsR4loJinPxe)s&OI zSx^44!LPJ?ZnD960T6d(J<1Pnfp`zTWW>jcITRt#5RvfR2h!KJ!+B9;gp61tegl;_ zcZ@fs(UhbSTjNja9vJ3jJ43BYn4sA2!NmAfTYy72j`3|1bwoA9=K1CTjt}K22gH^v zhA4S(pd&VxCDDNcv+2QwMM_%bVUY<$W{8cGWq2X2$_DWZOSuw|4W1oJY{A@0$yt%AX%>T| zE|+nA`c6k`qF7a<(lyvleIdKEC*Hb1+?rvfx_Bvb-i|$Z=R$cd+z8%ZmjXesOsX=m zPaK~|yQiJvIwA(h0015WR`p#&jC8UFjE9iV3OKcCwdY(Hx%~I!dMKC>`8WT+wl8$> zeY+Jai+MHNN!3V5bi6c;OP+Z=wc~d%qM!$vZ)M_%JnEvw#S4$ck593*H0;$DN#ky= zC|e1bi=jqxrexq8TrVCpRBLaldo!5fSx40@0S_=;$XH!$4rQyx%R-VgBra%rr=&@U zTbJ*pENRS=Y^!XEUozo3pjqpZS%&z|n)QHXT z!dhc^Sq0(n?HiI-u?2j7YeC2x);HE`GwIloNVaxu>gJ_+M4I!A2*PXIQ=+2W( zQKG`7@+R7T2S5Kpff65cHl5j4q$?Q*Vn#>Uqo{8v&{&^i3%-c~ZR={urjOk?;D5=_ zDjU-I!U^*}WV7MWz_iLEq{A@ZxrGmA7^tbf?rx6Bk}avx+{13tQGZz$xLqM@Y{Bex z8jq;_xtcQ6e>A+gzc;1AovcGvjkVb8 zwYhgp-NH;{{^gDrW>3-DRP=haOXhI9YYpj00*+FTn4%p8Xj?_+5B@G4#1@&%z8N2M zVM$^hQK^A0;ntO_rp5dBve~@= z-ie;}bt$6XD_}EwQx{ifGb6jFD+gn1F!%)p2gTFHt5@vuX7(1YmK1!j6cu(UD_d7H zXLc!DBUdvCGZP0>v)8Y|e}nbN&LJLw(zIRb8z_GX_{qYUqC)oy47ClvJ^tIL@ck@gj;;sIw@4=ksv(?PEoIcX_SrYH;c2?b{RKYQKl=JA?Nq zzkMH;_AfoPHtyZsI(!VObNxO(ZfjPrB5qCjJ^si~%eqxMxHcDVY&*iVb8xs5-*9j^ z5U;sF!h1ft(dQlJH%}z8jpD)JcYpi+Z5n%ST1{GwM&9|0JR(hQjZ*nMQZ4GC$6v#8 zI~=uNY9^}!iXGA<1@wPAr`W)$***Gr{-TKW>8Ly*KBQeX<`avPmVPURQj<)y1g?Z&FMbEMS8q7v$l7uuz7t<$fsVV6A>B3j zFMuxgh<{O0AT~KGf;!rMk|@uZ{H8^RAw%6cC9Z_V{W+DEE;nfntn!gPBE)AgwS)G- z`W!gf?_#32GATV-{hx+ZW5Q|vDl7`(d+o%UCbSL=dP|J`T+hLJ zeCNHZ>$<5%B_AwN?uW{{sW6mJ=oe7VwJw$~nsZFhLM2xMxy_m>NqII`owmM!koLri zmy$U^IWb2ajw>iGahJz?Z2YCH^;{)0w>i6j0=L=ZyTW9y@poJrF|$29Z%0VE!Jp@T zQMrhd#C{bnr24}RUUMvU^U50H4RQ~ zV0f_h7mbcPA>A3WT%ckKYeez!z+ux)SgqCWI}qxw$$ z`wv4owv186z8%g(gaqft8-Yoakiq|0RklqLyz5oS)pn} zjt(CQyP%z!A>HZHf=|Fh+dS9WqSWc>CPFYqsw3@fL}SUhg*OuKcM zjBK0S&*K5daW2`+80^c>$wYZP-yk5KFruERT@Pf1X6l1-UvcKScl|+KamMZ=n73kj z2({~$c)DmkQF~T~5tL>Ex$8{0GFVg7JaL5i$FuMefjaow348P}T3l&fEkrC-A-xtr ztPApwg5%Q;5TK#j|N_8_r3-dGQ^((W@l`~xOq-=llU@>JIX&z-GZ*0U@OO5fflj5;!@iQCLD%|>;NJ7=m+lVNqzAo_bXJC)!-D8$7Xhh1b>>xf{ z-ac`ro-M~owgH+M?i0})O&jhTmYLg`?UT3pVRdz!D!MF>P{;7!0fxz^tSga9yerwS zOCUc)khUwK)l$cE-UPcDcjNY%%XdPBdEb0PP6lb%LPgEW-uJ4wU=n1R^y}#or{@%z z?WPS$Lc@QK$4qfPhp%rIJnW`t)}U2=zNiu)T6Z9q5M66he36{!K1JeWB;hEA5;keR z_2pR%Pw8U)c^4BBm(!v}D)!I3XA>(pkByEb*;&rlvW8w-4s94TR)d{|Eyp%FkKodH=QCJ#y3_};WaD(y{uhcOt^jK%~yRC&g9)P zG80~uU6iBgNABr#ua4Fn=@>9~;;h24OH-`&1CDj5$YvpDPf01w+^fqjrR~PptM3l) zHLXEj-*E1^`i<8rbZ`Deqt!=Lv?eKcg~eLh+&*Yk&GntwQ6ZcNB5mYXG)6;l|F;b{ z*2H!N7)rv|d(JXP2eE|==LccM?C;gAAWkK(x*Qa~O1WSZmo@-D3xm0J9E3+DsxECA zh6ma{`gHWLT<}Rne`NBTyFht_Zl4HN`?Wn5)5aFYrt-fZc`i+?czBMYV@Y4fxJ7T< zjuIDj{JrcUd|1+lJP!Ey^V?zjt4CaBln>T2sKq1rE5ntTK|SR@FP*D^7AZ|?3o{L$ z>l!+7-g@(tE=Z8}jL3F<9ufJ-WwwAT&csRLQGD%f^@%`f{?64+UJ6#Ppt zfL?7PX1`Y&dUe}f0Kyk07=^myZqH7H4>(w;3$*RQN{bk>T_X zVXvhYrW}EFfB;PC)AFtjVDx(A5Y>dYWliTqN#~cjt21Q>_4oDYxn{b}x zB~EyxM=q7VPkMiOr$6(&g?7-tSAu{n6Fq8`GWzpvWT2(J_uAt7e*JI5_=o!@=mKWN zL3D)aQMtI^=||cRyguD5W+9hADc%yz*0%m4REbe&f3E#zo&Z#b`b)Yn;p;eBmmT%* zzJ18gCE5+5uwTCda{ZPSzu5*5YYQFbt_@Y+Ex>#OB%p7|PzT}Oom~C)ORf9zFO=;k z)%r=<=Hln&`F9Om;GZ;bPLBVE3O=DJ7e&K|*8Q`l?0cW9t;PxjU7_D~N{ylg9bSON zdn_s*{Z#+kPimb+R$-e&Rb=@})+>uH7N2IplcTS5OEL;?!KsQQHy!%FK0ADr7xqbi z6!gjdW#LlyMefGF#H>3z?M6@G@gw=i(!rL`iiJTabNO2?D3EE%C2(@$t*@#z1Q?qT%l*x1_fKNG`_7J%PU`_Dv9g;sK8lcKTk zAp8@ZNt8~;h?En_Du=N``Em@#6u4r~g!2cqu!HAe5wb(rm0D+zD4TXvEVof{&u8_q zd>XMbZ%}BPmnn+qG{LOz1Nt5~k;{Wp#83_L99_N#S_+3u54>fluyNQKYK#2MVd(d++Xs=yDfhX-asR`YB(g+Q~{^zc^FY5i%h)N zx`}iwP2G`GC}nvul=p_iGl6uRQMLYvCw{XjL}Ur zVhkC9Ew2WXSL*{1i}&}0x(SLnOdX#ZzhVa2(Wp7$S#1J9Yo4HY3-Ll3oniG@+SnV5TdSBo`DVh~t3q0kNTP0y* zG>_zQIjsw=>cPC)I@C{js&iIxs)fF)+FDdEKAIr#92AvjePI!$-a_ws~3N zOky#4)D!x6Yj=^b8@&nIL_$nLEa4fEh~j6tKra}|4=r;6yz6r2d8bs1fY%55n67}; zf+beS2=8??2TPW&FI$>W5VVqe1Y|o~LuPFBW_m zxe^I}eR&_!ulGvClb&)Xv;JZj-mjs+0hQ+(5~x?g>Jm#>udOZdmf%J4hS`%5e2G&- z6dBG%rx))YEUT%stpJZXhF+vNm~>H$7BzMZ5p0Z=)9WjHYDynuLXkc zr{N~v=aN&#a2F|;q*CMabZVLGy7WJML993GbvmAlv)?+9o6G9A_V-lDNez?b%i?J)8lP2}(vkRig*{A%Ii=}qI&vZ*j1K3XJ z$J=)eYuFI2W;uvwK6?l>kULv7jQYiFAa_l$xSqsI195J1P#A_HO?1ny@P`+{@`HXL z<0JL2%AbZO{O<*)_{f5g^bky-I!B4Wul3pOJmjmoUXAY6uR_D4w2Qw#b0|qLZ2fd@ zMVFC&@kJ9T<%H!#rCNifRax-$o{O}=`*azr>4KYdEIwOAogAsCKND5=%o+((e8&~C z4)4wR(W6khGw4CylP}7`Dqg((VkrStPeFgw{Q--Bfoc4kDCK5;(@_$(-$>PiVDv!! zB5pIEPlGp_-q+c-{Ca~4pZBnrMZBNBs5c}e#|H&{78{6n=8Pf)e(;?-eM>_nw6RrL zU`(KB5~h?-G0U>|k^$s!CM9``q4&}$U9fBZ6<1Cft9DRZBTqO%dj)>?;Hq52m07OY zVy+h6SCVy9Os~rB1A5Ep&%ZpXy9A?yRIZp)8ODvqJznp>vlZ)_ z-H6u?VXbyES<5a*D)mDMFvnpN)NREJc*)Ti5Vf)&Mnc7KDZukpb)iK5YuS$*%bRn* zxr+3>ZIQEmlF7(h>(3E4oNhhdZ0K>ZP%HgrFYR;Oiy}mdsP~rLj zckAYY(BVjzDd?bpg0v|rOq3Iv8OO23XAdv`ZMP%!{O7HsYfAMfvv4~KW9<=a(;_}a zHgn4$bdMl;`NH6rOL+n#clk0+2&#neX7%$U;%9W0P8+=%I~p|~@6p+a-cq2AIS9;@ z#?1+DCmzTD3pM7Ce)30;;rtJJ3@6t=D>B^w4Mk>9cP#d_6@6n#W8TZZTC?7)!(R$+ z!>0pi%GZlnPiUG(5V#J=Q1BIoZ8v}ArF3qJjiyAct{k?5%_>)Pr^x5>@N?f_>FAE@ z_Q1OG_xkzv_a$M2-xqUNxj#&9zAk;#-~4E>?zL8vg+BWEx9h`%OLr!3<8{k$pY`S1!q_@mdlISwW}PE9(`lTr@9&NM@Avy( z#-^4yN-{!mq-<*%^c-BCJCT0<>gPQI{}a0AwqYSWOHQy`98&D3tBe!Lp!?|V`5UGE zu~m~m_w6bJqZC!L+Ha?foQ{nKLD|R*WZDHkU3~T`Ei&3c`QXH3VYW%R>pXZG*iGad zcTM-(gX|)CBsno`#I5XrWuHYU8BSm}H*Uw&F$1Hyze}j`&nHMU$4RAe=K>g5^xpeS z%psqbs-kC=K!p(=H;c5RH!J78vc4$(WX0rW9Y`!%v;g-w-{iCBwR?$x^o~QQ(K9~sIp6acchu{x%nTKNy8oxZLpEJ}x)B#P8hboDPvPsnCJH(YPw#+#pH znogEiednI>L{H^j8em!ZPPy5;ClLZP{CNl?98Qi6#lJ18I?^3elf$yVB||*1zJmot z5|_f7D4fj9E*X~^s|jt8?TML0;o70bU;gkzf>oUJV+2M#E;pBTDWn>g#!OHX)){~= zXanaBFJvRJx3cq>ebnE`aKirsH&dUHQ81b#d>QNZdKU4WfmSe)23I*gn?CQ*cNWHa zDuK63GY>_viN;dSAMy(B|L|q+m8fJY_Y)k=mlL|hREGa8Tr^+C*e02kM_p@Sm;jSR z;TslDjp`8)ue+~=Ap-GRFu3O~D65Rah#zF37Zgz$m!3RhTf$G|B70(lzY7Q^1SGVB z$S@)JJEu1++>&v*VbySs{ozC4_uKOU6nfc+SQ;1*%@-7g_S{Bgm1yb7XKJp(<*@!q z$fhql^i_fJgc{b&gnfA==}4lHTYS=~?46&j^39ka7q<9dYB>#X6#`U5W`z|; zxCnT;ZsFMYS+Fd%(0XJS00vaqD|%)RExzwb+;UhN-5qSZ!gQ zi%zYHWt$^E(z8$hgDkf)ZExeRxJ=~!)40@GLUTWk^8B3nFd6h$6*frI+gfCCd8=*K z9teX;Dek(2A1W_;^3DRokU;B5Hd!6puL)b;b)AKn1LdNEyjJqV&ZMd;HJWC&DiY(> z5tYZpG~s4&?)$;Wbl2XD<`WVr9~V`ctuaXEnzZqq)2jzric)n+%qcnBO7^2eM|j7H ztjgn!CXrPsS5f@Z!M)GA-5tq3TwHEJbB05aG+xDBDS|RlI#D?o9Lc?w?QvVY5jD`e zp$FDxa!mzMv~pEAd@NeVwAC79v+`=i^i@Lzskyb?1u5vES9mNWmMa%VD}aV=?ZkKK z=)*KMsU4(ro}BN>i}ym?-IoK{6IcQV%3 zbK)?!8Nu%8KzG=t`M29x;FM|aP~9m}Rkr2G0;?lTm;$YAdkw^4?p(DV?B%yQpJG+h zup^qPaf{(FLsf_iOB=n?N1a=KIb z4RQW0zm{v1Gz*d8kZRTQQ(g`*u+k>8)wYMCtYmBC1j|w~!c<5|!i#u@JC+v&DU@D> zMal^p$}YOf`-ZCdaC8Xuuhl7|3L}WTI{J+&Bx*CDUlTD?^Qr$9@*bfI`{gsFiEe3DZ1iv^qz8os-&Y_nBKA-32}_yn63!?Y!C8w~zezH!qqL2u9M+ z?)C?&uN(}$wMtq6zS&jw(|eg7#T)VpwPu58w>~nRrn4R13!9(OuN#lwbv85^Uptpm zb_m4*VPc&NbPvg}UY#)51|AsKb)74m6(>y>%jovDmLD%t%UML$--9eD;WQGy%*9s9xV0Or%2EPaSZsFxFKRC~J-t0Q5;*P&&qnlssMj?WN z8vb{^pN|I>ztPpnx*Pt*I49&a^Z}Z?AE)J2G&+ktgEjdyL*q@U`0s=ln8L`T5Ll1Y zO9^04wtFtekM6Xv&aX#=U$VkT9;&&XSEqpTn&z0b)bbT$?F#(uPE)2mzNSAthj!ky z3+N^28%!t%r(Glouh_jVpzTjEPipjSFMwQJFPPPt91dCMrR=f`xv7rUkF-|3I9tnS zPNCMd)|7JkEmTNb_eaTBdx+8c+t58nR#phka6lLwAMvJ2cTnkvv2SxqE)O4Z#dWYm zZ9V~IF59@a2(OfY~36N6PlA@~Cs4%gh4 zxZ?vhwi(65psKxHtN&CNi(r+j+8EW|ki#_R@hb`cuAMjJkKu~okMt^AM=oq<#R=gh zH>fUpZ-P$8MB7%WwsqQ|SkbkknG;So|KeD7>bpk|JpvELJxg})NfFWE_I1R<`Y{*eloz2il|tOEsouO9mlZy7mE1bu z?U~MUcnLrAB#gi5se|4oUJfl~b1u?a%U>-1?2p=8GogOQdBL;x?In1@`#xDjU?;}S zs#+tXtvtsh0M}I9oTy)xj2Lhe{OgM^gXmq>nAlweS|T*+??IysoCl`}y2n=FrKtm5 z??1|9gvXUrDJ;vxOXT-$OfadxAn#9(-H!{CvEg|%6SN<6jccQO6cm1>dg&1w-)GW( z>&4Z`NDkqNlbjBm`HwUbWQDTEB3DyUtorIt;Wd0lO^@Wede@+uxGIc)U{uW^t_fUF zMF#6T6DRqFv-E3Knx9F@4tw6`IQcdIJ#gPSn%)#qw5H#2h{9$!t8GQK&+@ljs^w(C zk>PJY|D!7Gx7u?c!0||cy!MX!|0(_d6!8D0^tssB{%2yz#P)xc{(t%i{y*2if4d5p z2wAzTZ-CN5g$d{m1y?265gOlIS|a6{kh#`2D<5FNBs@ctvzYzr*XQ+Vz7D zYOA1+?R;5eShtvt_fIXr$>d9QMXz_*Q3K}4zK?Af4risRWtd=+(B;B#)&E7cvX;O@ zv@KEOv_*|lM!mM@d|p+c)$SAKAB;Rj?GJ>ILtWrITAr{^0M zVp!;Rbo|mRv!RCkptY-4@SI5tlv?K2o=dw}+{#HU6js+W(zIY&&6K~+E3*kS(yvzUg3rc7_G$ONU$K0 zgs%R^a5X`82h$6SobtJV3@bvugSPa$uFnEFaGOc<3$96{Vxw2_*hTof5^!TdI2ze}r#2z!p!Bl$%Ol2W^ha?@b`}Oy1k$O0?iV7tt5n z30n4m$rjBu0Hxox-={EqGG3jny~9-)ks7rW*a@e6FCZT=uIvfMOG%7v`NcqR9zy-KWC5(g6Id zRpWMV<}uiOigU{O&c!=Qkhdiu^fRPd_nF`mYLltO|Ma@(eM_r_Dhg+Tq!q#buP~bX zkzls6HqZF9_7!%nKf%61;LvVYUTzE27Dvo^cdc{xblBlacUA^+4wKftUt6ml%ziF_ zp$}u<)q|slAa2k7mPb@X*d+cl9m{mvz;E;)_D_XGXDk%NVTVyGc3A|$2VCm|-(zQV z4q@(MHKMQ$d;64*^GBIExM|?t_MZF=)S0I-^DC>f(52u}@oaKYekl0QRH)l(NKSy- zD~ww|3+caX753-O?IFn_qp=wn`6O!E+9YxbpgpNQ)idS|i)V;`>^m%TrusPNP(8JV z{>!Jr_?%r-Q5bXvO@!)?#1o-FY9OmF^b7~k1Y!sDRV7|<_k!Gmz%sj(-(b~K+`k|Jjtc#^8fCOw#B2PoYHWqyUCRR|#EkJ1Cb{BC})@*f0qCcGnt4`waO zTgkZ7;U!3s@A{WOt)m^KG1QD<=)W*r3j^HIy{h@(d{g%*=;DXCdxQC72*6A@zf#in z4&3a&+~wZwKtVAX3TBuhlVB-@aM%(#OJTGlrns7r(1MzT+l1kJMdK1n{MC-mPDr;h zV`KSyY1lK_(#9C5!~D$OBNGg*{^NF*Qzwg;8>bZ!?-^qEed^=XOT9;Y6mtL3(%Y{9Y=L2z zRjBqf)VrjBwj|B~@>@dez84~JfjIxTQN)ormbkspA*gOPY1uY>l^un2#BZ`f3BE@x z$3L8o99&{4%mTBx=D#pOr9Uw`fLS1%pq;|2M8yWdZbvj63b`ySGU~y!?Lfau&0_X| znAQQ$&cn^Lz&(aWseqtA_v`*Zzy25te!+hVoO09l|HYPYOu254vIR9chy2_EpU_%_ zDYnn^W}R{971UjgL8M834*K&0uM5k4(!2Mqw+J@VitHOD*RL!SFd6JH!Xf<-4>Eyw z3MV_O=5egaINiDC8PN5jpbKp8!F5^|nunqjzwzPu=Afv3g$ z#JPKOTpnBoZ+qhPmzjR-7}!{UbX1Vw3;hOUGY8-Y>OpFZ^o5mjGl(*^pCiGurVr}I zuCT@SL~|JOTo!`f0oHUX<%0H3VA1M1$vK`(PY7AEo(??w`=&rlIZh6|9=|$w9q8`| z=p)n&@H1>7&`Wp$MrdeHkvadRUP`-oVX+muTyR zVXG#1)S3zFsjHUWU_ER<gS%RP^s4| z?eT~Njq|?2pGS#`pgfBQ8Ab`kszP4M*3j~?wA#=TwpOPJU=Q==W2idM&ML$~@@)6=MHX+A3JOXatwWh`t zqPzA1a8MJuN1391TeIyVie0e+HsN-e9GL$sBT&ml&Uxb@}627t2zF1K2*5#*h) z;jI?Ezp)pF?2GU&f7IH^x+!k`UK=m&y2rKm#8Z0;h`qbU6~nND#(VTiCQ+t{Z0;7{15Z(pe91tFa6i++7LYtD0+`B}k1s*LrvV74w^6_CbP6 z6H10hKxp=7FCwm2r_rlJoCGG9Fr!$>Nq*MgXYJF1nM+Gm3_Qd>5I@vBt2hN_seH=V zgq4?2GG6c@-_BxVi7y7r;`4X7+f5A%(^lBZV=BvwGyNy^f zed^>Fj;sxi?7<_v+1^Q((gUMU$qj_Z#+ zG3?~MIyra6_o?_ZZApQUW`#w@W8>L8Px|XC@CMD!=H41sJ8QkpPpgZ!WQI~uH@pK? z25OhM78e2=U&hK3?r-87H|K$=8gV$4Le&dBdj`N0X%g9xP<}aGP+bWW40xirJmPx1 zlZ1Q2Pu!c{}3e!(n>r*nwFI$!Ij7>iaZf$!O~T!tTGF%V-`6Ujl9% zuN4Ut9K`z29-;H4b!W|?Lp_M#0ols1peGQsIr;2z_}j7h(<)YQ-1 zz>@;&%N>Vh-3tQ{Phl<*>H-T%;kms&mmu9!G7oDV-cu%iDfaa03rBu{XOgzC^)Fk81J zu2{U*Wq+lRS=B+V(0PtwT#d{2Wa*+i`gStp==Vv0+ZmoElO>G2vy`?%RGM%xz}1U4 z`VNYDpda0lpXT%i$7ln-i%qcqR0Q?p?<{JYC(zC9g(X0^H5qp8d4MJ@?p}v|hXHF9 zvl>Xzkb^8UjTC6mRH6i~21)x86VgHfkONOKjn(G&glYMB2q>G3sMG+z#UhXP-m2Av zZjeYikofSIjHCPndli04a+cx}Fi_jELy6A}T-{y$p9I->boi9y#4DL9&?`nO&h9ouTVk6<9emqlOj8`A2g}_tcYY*9thh{NyVWd=&*w?u_*= zj%YqH*U_*!JRhb!9H6vX+!3|bwbWB9UE6i#*L!N5K9=VorbPN z+MEyQ8W}Ke*QVa9lroM~te#}+*tDI8LCovB7dcrj5$S|#S&T>6udl~s>@2k167EIM zAScM?2qK0G2Ef~%zN1*)Y7&3;T^Nz;FK|+rSbDi;XhI+jaA~u+$7s#O6F^nt(T9yM zP`vrR%69J12R%vs=in-3?l6K?pq}Vp?$-@qTyW{|BS#;so+>I_woC6$)m9nYp4!zT zE4z5jT(I>@*_@`|GVl)Rr`!(Y_nsr|lPHGGFMCOEH2g@i$o%`pxMKZe zKcekaen{>qoCZ9!Zf5aMnD*lyME50Lzdk8v?zJ@`$2+nfHLqFrJO3+czc&8x2w>C#hK27bxb0 zj5D-B=|FM@b4ZgX76?a=|noL)JtM0Ra%h)pH>d!g^#^g7y$?voJ01m& zVnssjShvO==W(>7x)Ioao#N*{lvac2hWZF$a3rAV*FrnPr`I{dHj)oq;av4bMxP^m zbNKCZ6$aBeSt_vYO={V6tUH#w)|e47q2Ns*8hD!AFMbD;GA}q8Y8q)a5Qhg3 zrNW^5xXKKX<&bUXtVcOv95GPqsI)e0Ph!GCclM`3;S`NvMc5p~+y3n#xhMQFFjIKG zoAmfRs!&flh$+R!!?3{du^_qQi`f|hoAil}3xaZ`EEy1C^V1(5$zwp|)Dp73&jH}7 zj+rFQT{2=%Nf{@n$DA*UqKqU#S?;sRa$e6Sl-41aO&a4y@1J7A(+H2eCnk4U4<|lZ zUUaIBaErGHVE9nSqq=DP!_QsI9Hh?sODPnmH!-*7743w81kWpRXW}a~o)PT2sSI5i zNR|g}Y`_$3a51@f1rZ+|{$@%lGTPt;X0~Z=k4m#^$y|_nP}@rj4~bJyFfiPI49U0{ z9w0naMXio9@$}}kl00*wG>kZQ7=p*)hY|B_W&*&DsQml;II0`YDoeE8!<{!{<$|4; z2yE}_uYd(Pel8$Vy{Kl=G1H=`l4|JY>joNOw^N(6>16K+y zS%#9ghDRt58UD{l`&P$>K}fg|$}8zeicNP2JTjp)%}cf6mD^$Ql|80O(9>3Vz#x#8 zfbd7&=j8zcOhGN^^VG2nu!r>vSS zVfbsvZow(`^-5-Sc++SK*)P;SG*^Gr_Bd?L;k51sFzz^Y~pi?)22(M?RbR zx%O%Lr)Bn*Spv@OhS_^=Tej~Tuqk9U+Pb8xn=7*K&0K%%r70>NW{e4#i4PWE8#@Y^ z>?pCzi#%~)g6cOUQCA;vu%zVJaWsvSB)ypoDcvG`_r(*&!Z$>T{m3W{gy3)2$crOb zi&q2B_pFq3C5swV^s1_e`1E2bKTQ9yV8y)oTzhuGi^aooIHg5N!jBK$hfh-9Q>DXB zX!zwc0f{&Wn}bD7Gj+*BzcLSF>oH-F_28U>klena#&vp5woY2Lcfd;KNhM_mP)j9u zmX9}d`K-gRUOyg==@}X!qS9Q-X+?EcbW8%Nq3zX{3D0c~!1|{(u5c`}A;!p8t{}^m zDOHeW1m#l_S%+@L*|^Ef9*!pOVd>1pVK;d~W86bz>fahWli6Ui$@x)pn5rnGE-G#3f5F%fi}n9|ULlWbfV}#>|!jO6lxTd&Vn z3EL;bGPsJd+{jAJ&E54{*9>aTD5o5nRsyDW<236?q{#KoG?rbIEhp~tB$_d4bzeKG zgIoVld+DO}NSa)0bsZ*n>dBY{Jj;_gMD+%4WezjY5NL?nh-Iu3Sf`Y=3Oy!FTmTsTH`wnymX%{T>EM+Y!-0rrH@_tp8ksU z=yDtQGQXS2L21q{@NfS~NIuznyMCUO-aODtX&>dJ-e54ph8pljq)OA&%ALH}dq6lBCw^?n8c*}E8)0X;Gzz39N&8qR%f#=v;qV?oMAYey5(A^`D&L#al z+P6eO6q8Aw3Bq>|l}`_1A!e|ZfMuGNC(JjJfJIZdZbW0Ygcf2Y)8PJVc#M<5_0=y3 z*7((!j0~%uDZ3oAnh*`jIOT{q){Y0i#n=SnvhdmQ!|?;Ub7cF*-wAMzLK$b>@tAX+ zjYuxI(RZ7}3(W^Xw(5$ky(I=lLx#Obi-QY7{VT;Y+RcisvX~fx` z1T=bhhdL(#>N*ZqRGj#Oq?9-d^(+=K)Q)&jw6=Px(o)2jt9m+Rp{}Y#di`K3Dk8vF zsF(_=jt&Z7l(VKpxM=l=c}0R8bUCu*OQejQ1$Yw0As=5$e>tSVuJ|ftWC})sjljS6}-6H>L6Hq!Q1A_ACp_tk0szwz6G)dj~=Ib-D5H=DJ9mn)R-< zJZd>D=RNue+nO7#=6XM67lS1`HxBp84|Yd)!;>oQ4LA4AIoI3|en*8BOz#k$OMcp( z9RF@(pQSrh3bkylqX+5ftltic;OdTXmKcj{AMsJ;Fm$&>3KJM}1le2HvzCF^t0wDg zj3Jg4m^NX@zV{3FGR!1#d)<+qVosbPwjaU@0KH#K`DY?asIjnfA~qy2w;!T(r%}1WpAaLHiQ~wVCe;={_W?CT{x+7@P6n#XrtjrB#0x3YrK=2 zuehW(UGlR4vqR$>Fl9VePj}zhiQTX!#b{}R(6R5p#8|3)=9_A3C!cU(Daz~*N5T{? zzKAa+R#-6KlRdsqu8iHk=m(l72vAJ>WR-fVl-?l^lm9g)gjjWmi1?XQ>J2zeqxF^} z-znw)V`gY1$q7f31r`Q`STkrJwhfoCjnAmz9rz)l>8D;|%3W1nEv-pdl~!U7y$6Rk zfwMsj?jNc>q@z+(J+^dcjX8B`q0UODE|xkb^SLTOvMYfXDi1x?KZTqz#`dA;?pjKq zo2Em&r&hlempYR2ao2E!7Q@fE$pK1NrAfFK3m(cH+9bJHI?4Wa2c9nG*Th*bDQtU< zE}$f}srljN#bSFR|5Fh_yIFNVu7~ERLSBBl0P+)gFk~@}#YTrUPBpka4OHuC2BG#klOu(fo%J8-`AG}Tk;JZ=y;YKG zXXXCi%L2EJ>D@TBuOsQH_BV_gFh2VHnnTmz*4htSFbtLx$=~T?>@~i7w@K}vBK|`d zq&?ftCrqIWr<;0wrRf}upZ|IRT{OsYOgXbHcL(~}UTABpQN%oB?d0mnF?-T%c6PdHB0{;vJ4vVFLx$EXZsmjll9Nz{%5Y13^F#R%kz`cC&$42%KT(E_K^ z^mE)jyxrVmi)jtlEY>*HqSh|zS$8i#!wqswc~>c5n?p zxI}r$AYfq8abb~{D~*?n?z`d?rGK+Ycee$fg#c8e#Hy-QmVgn3ny_dc9-F_)tEs%{ zHUt5fbA?6W+1C%Gc4U6La^*T|Jcon8@9<>}JkF0DM!&((vlGL)n1wkfnBenzD$Wj@ z_x_wgV!8Sw&Rn__J0!zu*48kjjjoSgmoj8qMGea*ERrYWBSas1)6B5Zxbrs8H}1i8 z=K`%4wW(m~xD1N13&s9r8;j7fn*3BGUz-1v1=t_PGsv-VO_bL$6~^*QPM1B}CP=Qq z)HX7vaY}uQa{D+4c|-{lA>;+15R&fj7p}_O{^*?T#F_QCcJ4F0>wF0^`ROlOzl@%b zZr~{8c)YoQ;a*tp?7H$gh*ZPetw!0Wn2K#mFj>Dccg7d+v0tELi5i*8yKMQG(L^R_ zh?GqZ@EXYi%jqPJS;H*)e&aO^29%E<9WFgEn)1f?5xExC9G z603@8gMKze72-_SEXW63=IqVAmY<|+OxXIHOzYhiN5IAg-+_cYKLg1$dU^B^Lj%}Z z*!C6{rdg+Uu|NF=cfMD&ZyVl2T>+-mpqg(BL@cW0?S6$$!mnHv!w(m6Sbnirb z^Wa+9ZbYuxz}Y*eMB3O<9KRDGV|m%Nvwr{D?WGfPljQo#fzVzS>=itj6KQs6AU4gi zg>Towm3YYd5lTXKq_4EQF5fLUU8+=JqofVTfPg z3(LXcAIy+EGW~XshbJi+nG1T(zaK}cSI4(}&7K>=ekI#8jSo6638D}yo0qu^Xm_0Y zsG3G04)4tIR(gj0IL9CT%hea3lAE!3LpEmn{_#|rm^pGRe7>ZKup=?@r6&z>CF5nLOnz#^=ibsq0FWO==#-yzpu1047VS*=_j0?MMx@-<*=27KX zQ>VQjsh5Z_{8@JYMO~kJiAU{S3&;o@s;thF*4rdug5alS0*3Z}Dj@I_4W9cKrHhA? zS~b3XQw$xUC_l$eeQM%#eUOb7iTxqYw9uamAPu~#4Uq85=&%+J8#e2r9bh3vyhVP zYSWQ!jh;~rH+Ss8lxv%1e~R2ubrtwF7kU@lHQzSbD0^+UIb_dbgVLhe+TPNBbMONF zQ56?4BLQkQO{`IsqVgL`E4Vy_e{Y)gJanYEBiVH;#zqB|Pmt2+%}2aObUxw7E-Oi)G8eC_W zQ}6lKr-+c}7x>y~ z!ufCbJCCh?VQz@Scf|3%&4)u%gYlT6hu^2A)M8iJoP0#G}R-6{(e5C<;NCY_!aOp;=3!D8Bu*5aQUoNF)uQ~7zgs_t~XGNi=yn$r}J7PG(p4p2&h z?Fc{*Ra4}{6s%EA=)YK1FYTOJBocBk43#CEV_a~BGQzb_m*n}Zsm2qF%S!qEVBoNG z0*X?&ndIsQiS-sbOy@4T=)OH*ReGi#t(QVv;04os_lNoj<@msZ>^Fb4&Sw z+vJ7KQ9CN|*py}~Wv z92+MX-`5V>i)~AF&-4)Z3EUfw8(7OfmDJR1XROD#Cv8pEIj{0u;ymA!ZD+I3rB&t3 zVX^u7o;Q!L3~C7+IIVux{8AW5T`$}y^w)Uzdc8mjrn8JJMK9`b@@>CBSt-3{6u|f6 zegS^y*-4vS2AhQ;4G888hQ=Qp3qB2f#C)b+re@mp?8mr;=+?NE*4r=U;bUk!otDm| zLuSX)kdc*Z1NugiHm|S>kR4a;dZEFOlN>~VS0^Xjfc;n35N*2%`w7!rhy^ghtlRaQ z2rwNQ&=_&Z-jDF)JIh12R{k(*6JSTgiDgov#UZIL)DUjg;GT74Ulb(CttfR* zZfh+~3v_gI_$88Zo|sfhWvlXqPcZ`%0rD^_F|Zdtw#%3fYYtk(cLZg5Gn;BuWG+(l z?LkZltp-QX6!xi+H2a=F8n84r$|XSmAh(35B9d$Vst~GBZN6_~kASaqkVAty!`~Hm zY5a_Z&m6-k(M8gO^pi6VIq*>t)90DE=u$*pZ}c~WB|*0wK!Q-(wv9g?Qsp?Nyg1A! zR&^Q$rl#*{7mx~4ETJzeqAIQHDpm-jVN}v*R3fsdu7aY#q?BWTL{HHt$?vhOCmZp6 z-)wVa4J>GYc>}hL3M3W+Zw(XeJ*Q*>hO#*-%HSYfpT5ga&IzG5S5foBukk}5(hB)| z7F*4@<4DMxJxrp!mCaoZMZ)Mr!edO5(rZiXYNE(shtIyN&T=d)e!=!v=%+V>rFKup zBjEfTU|)4HsR^E$;MwuGzEZ34dkaya=%nT?uG$;Kiy_B*Wh*Mu79)-oJr#lfv?Wc9 zo4Hd^VW)2kyhSCH**djpw4GwSiMe{jGiM`xBW6>Cc3Q~YFciD$Cue%hP0RGlES`9i3^aF$eT(S>4VT*H(&VWcexlWT!OON5T&K_3Z z!-9jdlj;6UNGBP0Fvuv9qHf9v?YdsWAw*t+YjG!6ePLw8X?;^io`lV+qU|&PGaPF{ ziq`iS_@V8sP1E?fD3%#JEVqSj;z6EUXi=duqXs(h#akhT&sR<^NyNZZOUOSv#`iB1 zfQFiOc4hW;yVEV&X)DXgP~7{GW)y{g zZura{E^pg@djEtv>oMr{W{?46hT10q3k(IpKh&NW@!hh_-*Hbl9=**)tt~{#keY@K z>*a1XbNojb;4p6JPbu+UzyD*w7BQL{?^yA zhnYVB+g6&y(=_#wqITksF$>0)5Sy}JzSN_BIIl2nvve*4j>Z6h&I@$p?;tPYRP(Qd6G717 zIrl#?$O*K~Mau<7sdYlD5)2qA_Y5sXiGf1q%UJ30QaQ^7?oOBV?V9qFf=#JV z$-~IKs;?WOK(kNW@e2~z&9~OtdZb5t!>i^DPT$yP2CQ2aXC@>DCbHShU6<`LSStV6 z)IOeaDX-#0pVcRMzMBhJOAVi|<3>cuJYbCYBuEaCg;Z~OOns;Oo%@rQxt^0w1oEMZ zE^+#TnHtnhWoORJ_Onx@PrsPk8QJf> z-+!l^t5!4zE_q<}wX+jiNVKDpp%Dul%h#hWyDj7|A!4k^+nS)`-@2_R@+y>4!$~S! z2g5w4wb}2Y-J_;2URUy151bdIumz%oRkDyIHy&77uu?BK$rS*d)8uvhUCk?z`su5PKRUV!i= zRiT3LZTVeHaXOe^M}OKFHo#6pq(S3F?FzbXcm++zB6dRJX8qADo9m|ItutYvE(4ms zHyeSNq-uW5$Vw=+cuNuu!_9emn%43%!6as}`XCZ$DzJo+AlPT*^mX{KMy5Ddgiz9; zIj`2pdCqiJY@(KOgnb5KHrNqAN?!3cbEEq3xFsLhv*2*ifA25oNNLpDrq4^%AgLnS zfVCAE?%9H;A8Ss6w;e}~RCwGRk1#m)BZ}_BRGm@c28CKPn4` zF?#?;__-uXx~R9m_{6l97w|RLI&DK>1%;?dj>g6jzq6%H1|IZ?R}E#^5a2rDSh+v* zFzWriZxrTT*9@>iif>QAr?n&t_;(h+?Tx>S=0Pw=<~tb@Gr9*x1{g=bIdG;!nT%pC z?A>09=HW4qSKZEQoLrIx7FVx}`j#rm4mF3Tp3rMga};xOnJ(gZz)jpaZ&)`3#PQj+ zs@I7NYeFLc;UiY%(t$FeO7SK{Sx6H0|e!KiT5ZXU|Ezi8kH81tMbXJPn za&mZY-2J)s6pMrgArya$Ja(_Sl_Civ>x7(3ZZMBxLTUlrHnnAb?C|kl@vE!+?`~E( zml?Llb}EF}@$fmL#KF06B+05ZCCikVTspoGN~Eh{ptkcOz)vdG!h%87xnGXE|6U5W z`cy7Q>bnf^AnvuEb){aewp)_())w9aqzHXdkDXch7(dD3a%0bG!$2|&v?F^?a>=FCESd{Jv{y6QVGM!zU*&<426(y2=!_CVZboI{nnyJ6Sc^Dn zsK+psA$V%fv8Xu-RylJCTFD&)USj9iSLhchi?-_$uL#Y#XR>QpG){bPaGq-Efh=0& zIdyA7*ZE(g&Jiv=z`TlDbXx8T4jT-e2eP2DN_(}7B$t-=Ms-a`T|w{S~I5t ziQ(D8-3o~vBA`UfxC1U$`ku_780Ypy^o>rqU6W#Et{mr_&6Q7M7&iM&kJa_hV=6_pD*bCWl!Mv@Hsk}S zfbI1LuR+`1EDw*&1ts10+kG9ul=_L^jVbL#6f_yF$vLZ-b~=vzrV`)Qw;*#ReiMwY zi9E2hOv5Vpb;ac!&m0&!OoW7LT;#EOteZxJ?*~2YL_S5BVNOEFPbH&%An{g3wV$Bfk{7M8D!Z3~$0{T20^Y|Ch?&Q~pfKbB= z^&_4Gck-|W1n8If;G>gJlKX^LnX{F{mHnH)H$!oy!QsZh)tasH$HpCPPX^oB&i8&S07R&cT!}>SsW+Y$|bHQ zt?W}&zUgp1x6>ylPUNvjd;p~v@?0HcpETq%bR-~Wlz;>p{B{%&v}#mL2AqC38o6zR z1V-qAOx%QAh>9DOAnt>&4~(iCMW9OS2#$}=*)8BJv#Rv-m+IQEur_^T0Lk8Q_+`EdWBA*gr=*3UuxE~}49Gq*<>5xWFyN_H9{?IfF`{-CdEpFsIy#rM+R2`ftNO5KS}F`m`q$IE<-CQGU%xCQvx9_AhWSy7St84jz;HH=>bng^ayIcQzH9Dx&r>*^aiGD;O?ETm+(IfR`48`GY#*|- zG)w|Lb7v*_51OBo@~F;~ObGmhPv-i>5oD>6qR*Zy`YrJw@&5CpUdeKsX61ip4h5YZe!S@kR z#G`TsaQzOJ~dXax`_ga|b|CdTL!MS(RsB76*zftp_|0+mSyWh3BzU%*kGG zS=}_e0BZOi{Or;%RYZbPZ8hkjN7l7hd=6+X?p%E=MeX8iOlykv1hLeqwE6w59B0oY z?Lj7l1;cTUYDE1|hZLyztJjQ}px*=NSMlaK>D>CQpWc1^>V-wj<8c4LoRcPAMs{Tc zo#m@hj4vrQ>0H76sob)95-QE0M1Xqk8Z!!r!k~kpy&CuwR4*HC(9t;8Kx!Lkkk=qs zrG;(WnBt+^2)$~-3*-q#(8fo9)*yh#t;a>uyTym@GJLDQ)f$ci?QQO&^D%i7d?mb1 zKgu02N6Q6Q;+6r#fq`N|f`X}nf%s)>-Ym3#+U3f~c;uq5kDqk$8z7aTOzK;r;VuKy zlss5Xx(wtbgW-mN1);?evIYQD{r6cS6=^guODDj0i=N{4{HeHMvPAHiXqK50`4-Xo z0-5gXjZ#iIcuFUu-M7il_Iv&e~JUdb=gwK1GPtKkyw!Q5x>VJGM zZ82K#t@kr`;~oDY&Cr+;p-DwxI?xGpDQ zjsGdeq(#mY@4J0*-ST}$8VtWPoDf+C*dlgeJs-JzK2U}TiBzf`XS2OSM9Lo?HepuilpOBs4}S7cOe4Zf63g1 z4j@Os!A#saO?(VcfN<<^GrUG#qkZ{baJ5d8fzbr@vonttFlo@aC9P-}LZ=013{NV# zF|Cl$*3;Kp^QJ^b3OXEbg=EF(3&Meefs)-s+tjE@odB9+fYNEyj%X2YtTq?C$Z7btQXb@GOB<>V*S! z^!7=xpcb7f5W-I63ej*M#4vpNqiARP5uV+bNSb=r+d>B*&j<1jODhfT{IK?~-7i5f znCSqrT1dTnmBGV#8SD4{4^}%e_zBSbM{=fV8A4!XP%4(v$>Uv@L92B#>!0{PMYu-?JnKh z!}N#gf&}TZsngj*_sl~fI6Tk@XER@HGRxQ5clj15TSU*wTpm=Sav0|ig{uqsA1+V* zpw$PXqsh{p;^OZ+&oSjOoDeFB$wQ zE`(y{o-MD>nr~FHbI|u*AjO^as*CxjGt4cuD@=A8w6RuNFLE;G_jL?UTbX6V1kN7am zB)4-q&eH9Pq=<5lfY_C+ml)tDrsadOaj585DRWoX)2&=BPg_&u{{Vf*;|4PJdI)e%DO$?cvKfn6+J6ls# zIhSQt&pvV}m<}5SBPi)gK)d%r>jzL0HIN=9yArJo>_B%0cI5VWzZrYV`&8^h@1J9T z$`~8a4Y4h`hidoa9*I1Wd)xbV?CrRoZ5$(iI%HW_M}wV*{FQl{D*x?t=W|LZ1shKw zU#o=Uu<=#oTO+OV4(~hot;lb3@5wwH!Lq4VV4NDC*AZ|m{hEHhKf76TjrSVgK;YWQHL)Aww`uP1-4WQG8*cn@<~Ny-BL9kbdbH*F?&aC- zxd>v|6e=Wx5vDTHkjgRX%6Ormb0y+#w~M6XR4tCsPX%H0Hx|@n4o!v+HP6n|;-Qvy zHB}r?zp&4b^?|IP_?NQDU_+{g?hx7vHig~K5(JwQETc{#(wnS##A1nlZi|2ZY^qYQ z>@7C_0RJ3?&}#KtLJ-oXPC6G2STX&Yy@sj&(5EI7JxOX$(g?$Z3PD3(OvM7QoKR=2 z!CIE!X*a{{OggZ(m5no zROV2imMP?5Gzv}0rt@RIRpr>)AmHl|8X7z|tq4)h?3$hw^ly}|R-H?!k=5AJ^fV9a zbeu_bUoZ#1+P-@D-S;Mkb4z7+1toe5zO&~3y*EwXDEAfb`T7N~99`Re?cg&nE_-G7 ztQ9_TI56j;+b=z`RIUpTF&n-TO37m6x$7?bmWAh=Z|S`5ao3rRzHeXObK%BZi^>LBW8=a8PkbOqS_q2AKWK2}Z*_kH1IW zfX>=@;9W|8rYABqH&Md_fT1Rv#K79gtdg@Ef5hGWuVvg5^p01Tp2QEL*N_`6R$@J* z$9>Gj=-mCD;~qwb5zn#~tqnbEQ;Y_-(c%m`hn7B~o6}FFlELZ>oUpisnqM@N4G3>To5+pk&Dkx58|q%tA2n&OLfFQ(rLAl0 z>c~=ZDH$d^lo7H=Ib=F)KAJn)@+;G?YfUx-X3RuGxY}K4d+lB@$M&0lUaK{Lq$9!^ zgSvpJ3duO5qe}BG^lJVc^tZf4XYd%ZxIl8s9HqPMQT%Q4i1I9XRzEuDr|5U+IQ}Jh zi}?tBgiqm5^rwt}Gl{O8E1$2;=9l0-=w8$PwfE(98WZo+1cE*>RGCq~Bx;&H)7QkY ztd9fKb%(-1^=qOT`9|Lij8LW7e-n&)h+ak?4&wTy-swQ zn)Q;SJXuI6DuS7QfcwEga}iM=gZb>|xahpkamxXHH6+xFr>&&&7ut9_RFV5%LK$FU z*VUu)d>vY6CUo-hT73G(?=PRcu`y>alu~L)BR%=zd7+Lu=}ampIyT|NEMHY^5dSST zZ~a2oGvrjsa$^sSkeDb&^Z3nD#}^S0X2ZHlk>)}C*Ope>H(+5^Jm3yf2V--);|Pp` z5a1F0pP$tj9f)Vfa6qY-*VgG+oqjaKJeWM1d^PzF^NZw%?1%a@>>0gofE&qib?C^E2!L6Y5<~WO@i}j%&O)GDUF80$y8jgHL@H5;4+#_ z?rIc{qPRdET7X+}EEXj$w>FlHKZU3}E1OX0=Z=XHjV8$BE*`(cW1d>kdStE)RP0Tk zH&a@Two+f$ z_CJ<(0r$hPw$^_@XQi?=lSijJ3-FHlt10YlyYvdcqmMc^mI_@9z17zf zr&?j&)E~JgxGPZ}%gPcx+ny$#m^V%YQScC{PO0m!yIwmW4tTDw8WBf4qvEK?P@UOq z*lu9Ny0o{uZlLZi_IvD!I@ZYCVHmGt=4mM={JqkqVnE&p>Xkc0d>9|ahY(^qlw{3A zDf)?_CZ1x<@k$*|1_PB=YqxF0Ml7~28=-U6x7ns_tj#t?{;3H1rtT=VM1xThonKhH zb9%l-=j5lEX#dvd+L#P_XhzeKj(L zVIxT98Aun?RAYl|t}f4FJv)uHO7+aElL*yM<_F9FCnzjYXNB$TjDhGEu|nWM(%ElE z)aVU8g#8e@Z^@PyOOx9N@BjO7$6d37vo9nj&jl5ZYfkJaUH6kc%Pu>3-x~|IY;3UG zeGGSHY03UYn|}Jezr0c!zb`7|J1=VvMWcE7s?zFN|95-e8W7cWExh+RXWk6+fPr}c zb7qENfPtBj_aKoo3L=6+)To#k9bkAQz`)F)A<;_Oq(+-m+dP}3VjfzpNkyoLn%1=a z(l&j?_O`dVY443~O={a~Yg>~(631`teGU(e&F#JS`|qDj-@+2@4h}T3{ULIO;`TR;Uvd31y(5^pEBrv@vL#&xDBRxpk>GST~{(sE)n$ewz zRuuCBVdx4!_3Bwcm1C@B17Oj4w-x}MK)!M_$kP7jQyD=nKb?TDPo)RBK_db8P8oxo z*+jq#Qzig)6g;N8_F^P`Hwu#l3<3s(rmNCn!VWrxU~kjB|2nfd`Qva+nvKhsv*^l` zcx7)>re1?B$zSKPg%(%I@}&FyAG?b9Zqn!0B$qSWU}lxBevgog^M`*Z&UR`N~hmcON7*FkC&)UIf17nQB30T&-SjzSY87 z=&jgtC!t^1RGw~0x4KkLwZp(0bIrVkw^ply>S`l-C0%V@u547*sq4-4mPTu@@@eJg zRG+s#X&ZAMMu(IKRL@$TwH~s4Re4-BrJgdMu$;1WdKy-5oM7NV4$ubShC7=&TS}$W6Ypz8-K6te#0K8s#;m5E;9#h z-;}@Xc-^XaK)uVn)52C7mzkL~Gua`r@zM^-^wm!aMJlU>H(M+Ll{#IeQroOnx=$i% z#pLL=inu#swP?)KI9o7oQe(Y3S3RblQvX;jQ*T$<$RUhgko)&5Pbz<)WR=@hmSHPV zXu%_ucyn7NJu9U9S|#PgIgwJ1R4=O-)igdcrFY^{r-%)RMcgS}TA70!;8^rt?@;VA z-Kn)+F~4mA*(;skY>b)3Rgri=Ltg*xlr~Ug(th2IHiGG43sDIs^(%5K%Ztgoz2j;g zWw+H(_W;;m?PT2RTS1#MuY9XTwXvD{$Sg>1UeQMv zSt~gz40&?$>nv$jRiVYk7o;dM$i9_PFFi6&QP_T<*NvrNdi-2eC_Xn98p>IpH#5_k zlA_JE+xL@5Pw@;zlGMOp~1gadRLz za~(mvBbSwvs>^m~>FilJC)GxtTH=}8hk#m`m0bG(dEN$=xSF5$4msajQ{yeHsTuwD z{#(~Ca9C}Iu*2-jn0t-9hXxmVYm)r?UB5fO*ySqGD%QEzeTI4Paj$q00HftF57EIq zBp1CWELPbA(zfNj?-BO(IGX(KPk)4V(~Qd=5d2}T#xnBZ0PtN?Up@&QKFB~;syP?*=7_34b&QsO!W~?*Y&5XIx-PtiS_aM&*-zCa+E)PK$xH5Ek zrVg0Tc2s-9bIN- za*-+L#QB6VSKK{>0f+mc-M;AUXLi1^eapos?)%P&y(@X?bn@uQ-BY;sD-Z7}G}_Xw z8kj?rzBaWh`QtaIlYiPfbSVA!p}(Jc|GRk2>17#dwg6)2{-BrMx+=mePa+?1^uyKV ze&4V;VM}Cf%OCVTnxz;qpU6Fx|1;aqvfjv*TReJSeqPWW^eoH|_%?WYJ%hecpXQqg zTeAwXnzDXs`I$|2C?9_(_un$#$o)6Z>-oRSm1haA?0jV^Q8VGhcALWCg5{yf;X>Jb zQDJs|t*gZalU{|XFh3*1#3&U?BeLqP0jppgw92fFKIyR{@?pU@?qkO2K?Cb6!cJZ4 zzGB}r-a6?ZQ*migH}hz51=&Z-6Zg^b$Ud5M2a@cXZM;a|H_zP4$;vIr%rd+4^9nMF z>a8rWJeI;z?6$%DYVIXt;~MBUIZlV&wUBe>@C%WHwy3=i*6{Yx(Q$O1Kvu`hmh00fD_dLGrzBQ$-%st8B zQG1cwU1d+OeFSQnxclJQ)CRTsnbGx+H^Gp7^$*FVoH6+%ViA85F=U$y=~z@DV{kXu zunflUcgW>rdL<7#${n2Xy?^Ii$)#(^>&fL;FR&+MJxCAz@6<$C$xmbXgiK~4j#g`( z#yY{MvZ6d8j}h`V=k3osm&fH9hsYl=X+Fz0vjy)QVy?}!ko{9$? zH?2-S4HzTqrn^91r63zRCM7q~MxEWtK{D{Ed_UTTV= z7}>D77^f=$%8&u1S8I#0g#pW`G!>&v&QyHuE6{sk{KY(ZcIzN^Wd(WoYoLfS8uT!y zyoi(l*{Xa2Tp0E9|3`QzkwlRfy;xCSrz-m%7=hmtQP zzdZQ>F2o^x+mQ#z{8n@Ioa`7#79TdBL;;vp6#B~o^gunAy26@_tg?K0wY*tAqT}2y zx2MEa;;DDldk%OM1)iXXX$>Sa_voJVobmiMPhOKM-UPMVZ5D^KklqAMBO6aHi`fjT zdB&ZuRTUPHeI+AIorIL2QUJ^<14>3|rRDLp<@x{~4D1h#2hIh!fSq?5jXTnC6b4<3 z|MIhvHa}?`-AGXG_0lhb_8Ms|2-2eI=8BVTQBI$RD$u5)|bxv4y?BMo^e z1+a)zJM`{il&^6U#A8YS71=_kI*rf}p#+}0TuM)eJoFy={5z*q?Vcn%*dO9^rLA7m z%@@D-A8!Zvda_eqwkFq--Mpvg{vWN#@~n_~-0mgzp(}5E?-$QJ`SG@Q7~}SpZg+WZ z?8>p0@5PoUj=#>hcko4oI!55!FA=$qUla^#o!rhC^$wh&)C3P_V0yv9EM9*oEf=pp z6#2`<3lF@j?9ayc?3Wzusmzn+)7Ejv-xRXLmKUrq$)@Bd6|yhNz92uW_@e0x8M3Do zdv$w_PiE|O$a+nknF(%0J?fBc$XJ)z>WIjD6|!3uZOU8Kx2Lw5WP+m=tzp;6R?B6) zql~LEEk%u~GPk@yk*~}*YGoFHLA%F5(&!Va)LA!o66oh~&MF$%fry-b|>>p+H(u{YyV2A6RmU=mENrg76H zlZ-cQHVv9aO`K_(`P~%nc+5fWK6rsGEU-S^h|Dun-PzkmA2!RKo$C&5ocj~!yW(V5 zP^rzZ{uGQ<`RbHDs8ptrcY?1^!F)Gg)Wte)4NA_PXXmv<^Io}hxd>xXH>0Oq_N8U+ z@`9wto#Z@vOXDJ@@b)SnZo`7Vx?YwdYj$fL3nK4*jQh-nbh}IDcB_23C3n347Hdcp zXP0ZB7lBk8zj_|;WyfKzn~4@5gVh*z3)$N4ioKToAeaTDAWz_%qvBUrQ272;)42Z^{o^jtUtybU}*=TlW#SR(S+D79nix@p_#hdY7ydR&%ay*U8j-yd-%?47FD;vp5)lc4@ z(W<~g!Lgc=U3rhme4LDUw_SaUT?q2qiG1k7ag|aRU>LH@s0x&q`2)3qmcXXKHr1z8 z2T~4LpR=j*lquR&R-eJj9Ci-tQq$DrXAATShIO!cMLX!^ed@S+sf9&&nq4K>Tta~c z_aKy4q&W6wHr8e1?45b~{*1Sa0@JwvM4rGG1&WxW(~K7RQ2LHrby<@(GYn@keQ`SvAH#7Z8lO_71=l?Q=N^KSzyh+ zBOufm!Y-nANj!@qmu*b*C-(3`R9(*}(>Dtg3s%(R?rdMzS>tmQ)_?ur55mhrw|*vM zlIVtoj~#e;Xk^ij$5sX0eOb8+Rxcm=(p~HCY<#lk`CXSoetO5BTwg~zgk(oS>eMpN z3i=TP)>>3~)%Y&7)gd$53F1?ALBBpds1FHVA;gA=z8Q&m&7-`2lkdxigY@a51?j6dQ3rnwZ#GWzwr(aGm} z^L>8*P5OW_z%O)Ecmm!Aq&1=t9}0xdhPZDQ<3#Z`|NY)wer}1kEv3!Ds$3RV#-iF# zlXZzlZcO*#Twla-z;VFG`FKArcJO|^)1Y@6YY{Fs*2?t?o7~FVY*tTkzReS?t+oYa zn6t?&MzhV5=XBfhs{ECtNBh?5tbn5ekbFcX1*~c zge1`gy>a7}jYA;2X0O>udVcT?o?QP^_a*1sI`|`7QRML_Ackmigg-pAWc`#M7dZWk z*o|%UHD<4)(;M|~4$)%|9D~_1k$<`_Pj3}dNDe?;dM&w#b3sT;3A)Uhpbv~|Sw=7~ zkU=l#Ol1U}`58nco}0)Brjw%*4b}0+kmryZW3X6h3_5vZu!6F^bQzdC~ET z&Z2AO>iqpZdi9ekhnqQ75jzVAR~F0XT~4d0#CVMG&0O)6E7&^h8X4YjlfE5E!6 zgfrHt+-aGah6a3F@{wwfi7#Z`ZkgV?{#N`5-Z|Dmu6_El+zrWB@q&tclSywfVDQpq zuIwef?Pk`6t#o#JvtU!Fj^=b%Xv~G>Whqr)O>4fbd0lfs!==D7dCdtqo0^$vRl!2J zTu`T^sQTGa?Han@cM;@|xUE30%h31qfTJ{a_Uu`~?yJZwD=uDWt$+ga@9_%N3j^-Y zB>#;_*l=>GqDb~>v>2_y|1R8d5FHHtXNW}>l%=$rk9me~;d)Sn>CWDXxDm7)y%%DSDMO1Fmo7#vs}?K{ zF^JP}*20!hDZ?#+wLiO9Q0W)pMLmeB{#txuv2JM|TCB)A%`S%WZEVBw=6fr0mS#?~ zHwzVtrM|KX^{Q^JYQchA)@qg()>>cWZ2_BLV{O*8!75$jXd}~jD6N`z2AqPk)yX+6 zx2&DU{{|xacJnl@q)$XPlKUHgEm(P8yi7&+mA8n-RiX^xZT(xdwHNj8KyQ4Dc0XfK zQa7Z3b*Da6d@QiAuBq}SS)gIr(q;9_Soy;0nrcQ~l&5l=@_4twotu~M*5=jS)R;g` zm5td*?&q?RvRISA8Fml>J7=f% zm$c~}UvFA5)Vi>tsCx84zN(S^YI5k@UAcVGXYsO$BiQ-WJIS)YzMXvFYuJi)$=@em z_yzv_Ut!(zD#k~WCr+M7e(w2Yp{iRa@7Qt2XK>HZ>iYVDw3fsxd)L>ttUqzvSm)xE zFeljttLELXYPO&}REqBwPPJjIDz%pudZMLwJ4ZF6DMUlS$K9iOyGsw74_Lq8o=o|& z^+euj&nxOzG(Xj5D3BVm&sA5$4al^-rd(!u0Ji$Gte8ljW@u`f1c`tfi zES=`2l)usZHsv>|zw!L0q(Grnd8$zmSCz=>lqkjO;i#q0r0adTte7}y+U$@D>yQcy z)C2(5<1Rsu%7t==gbz{>|(b0Pz3D zp?5EyzbsA*rO`)R$a?)bPD2K#G`Srz7cBV|+3mO}y|5kmjDdFSE&AGaJ8m`1x=59T^Wczj5qq zeQQVXg1dw*U0tK)3o2H(KGfg$@w^7+$bI|Q-gjmq*1Ye|2Ud0s?fLFVIKJW7zxHi! z>D{`erL4!F{PofUo9}(O~oDwOF8g;L3K3VDhW*|k`E8b6KXNQ1`&tqjXmO1WGq z<5E(%)3_1%PYG1fs8l)@kFiII_CWj>!HjFEk~E^#VZZL2j@8Kp1y)#6=V?M4s-arY zYF1n(w;$f7%iLO+eTfe(uEc5#(w^~7pO=#^LW+ZZj2u_Tu*qd`864%f6dcT+oH}sj z>&)=Lfn+Ye{ORPAxNDUC`1=ns&s?E8jfq~f&%?~G9(_v~8f@6#aJJ!G1DDpYFH5Ls z1&}e?Z5oHuX}4uLoMm>K&*7}M+ZIvPv}(H2w>eyHyRF#aD!1Eia=0K%S8lFt(M>mL zG-}3IT%46pbU_!;FX*tonw{4KFvad;@N%cRR~l%QlxW zW&4(r2cawUm+4bbJvrsLMrW~jHDX==va%DFST5#Qy(}34*W`2tA>k$w1Gxed)_aB`*-N&C#cFa5LzjIzR zpv2X;uKpI*=*`H3cMDCsjo7#cV@7Bx-@{;IHiJD(aavW{U1?7+Z!%XIMVixT)Dsdo z9fSl3*LeQmi=fcXNvzu;T}Q;?QU<2f7wCw|$9otFHtv>7u*w_j#~u1dT0dxp5QPV2q+WjCUKK!iVu`eBAh5 z{C8~p1B1z4Z5tYbg%LeLX09G0s({Fha}s8yMzSskX(eQlndF5D$+1yq$`Zs%Go4}* zy&c93n2hR8U@SyO4^iyHke^ne&R>tG2N_{bNyp6G6H&DstOL(Jf)TfVT-R?O(+g=f zJjmWe)C}X-i2~x>_djmSYY8OtiT2@zH!aLwC~JOS!KTjW7k+OSSNHx`X0;6;tuIPb zAxX<%5AZ-fD#K@l>77n&yg%<1*DJ+rW9}D<8M8gp*Okkvu*#k1Zb0jCl!@lvgYRMD z_Bg-Ad6#=P-pM~vd;}kHpU8Wu_-d|6&fkY0%)QU^WbOg{JaaJjXz`ii*8_hjzFG{+ ztPE^rjQQ4pbwRbSI?$Ed>sJ>lnXD{qBEJRYMDBbWQrc~)4p#>0{w{$ja_8nc8Jx~A zIQIpHXB3462NZfmt75Z)CBJFID3I-itg>nR89`T)pPik>=u%Tbuqlmn>uSCDHpkIY zjvPlFOp9Z`gK-?!S74!Hu;Oe5TT!NTW|;PshfmU*VAA~^{YKLHiT;<9$SbNW6g2fAT_lR~b1U7AP_=96_!I%O#UFCEth1A6j<9n$G1EL0p0 zEb3JIkkbYQS<2v&IZbP_p5$Z6@={)#qtD7~E~n=X^pzF<{p;V~^V|_^-n={d{!M9F zs;|E?_KE5a=57Y#}kAYQm-w@ zKaG1(I#Mye5;U#+D9`MrIzp$}(#i1@oHC8~f^!O%nGgUFoyd|iG zGQ7X+Wf&M>RkY}Ykai}GNwbua1C0x_3PkC+d2!>=Wn!Hdk5jxv!2>x`@=KZ;eg2$0 zwT_dM?crR9+ri0Y?$o?IHOyrETrtdPbO1Rto^}wJJ~_;E?Aq*h`dcoNXrQ;P>D&Am zNGEUF>~mmIIjv*vEOwp}T0-+hj7a02>SOmkw{G#N?W2RAP5y3Ihu>kb8t%$;7j`}F zvf90mt>jzAmfgGgsUB|mu1D`^+3?7|1ylEo-+QRelU<~g)yg&d`dXT*vhzbZ>f7&Y z>E3Y=(cklZh`W{h2GXE0!AeeNc_pvn{D_zFa=#{uq8j;TIhKcX?9E8QdXO4i4PpUe z076I!kcz!|N~1xWWmxtV@Z?~!1UBnAfj29$^0Zt-u4h|io1wVVJQH99W{^3{$an@b zx23EXZ^8ibVMso|5&18kC)(g@=zbaezjA(~cs0fia}$T5)Zj2V3`}M+75}LfUrwex zg#Ww>|1qgsl_bhp$gj(A9`+HWLpHQU$XI1*w=lceou)6CPN}$CwRdU<6)gSTESRU2 z!Dy$~zh=OOX|_{HMLIdr5X$J&5ysYOwnWhEkzql$S;oj@>F_((^TVx|yJDJ_q-C57Ehg3uXtPZ&_{q1>EMy4`d%djk& zk{WnNI?r;v!)nW76=qHbzGIU-??^j^|Ax$LngDZG{7c0A(%}GVo`ThY4L2MgrGk1I zzXQF>g$37U7vqBN`nM@Pv)_RoBojf!3;O+fkZ#UX_)%%WaDF3sB({;vLTN`kkEEwz z7issxhTzOQ9hvwVjNXkOl2n#z?&PAz&YVHnV783x<6wnD!HI7Z=G~Z=x(wMe$Lz?; zWKSDbJ$3)O`)-bp+#U6~tR8>Uis57XKGpvc=48!Zobv2@VEWFfQBURSk}R*@QFd&{ zJwK@~Rxmo!C&(|z9RQqb(2FM#clF$4nr)Gc>}Glauqd&NEmSQ=f_AfZzxF%$Rpxd4 zI&)5|u_CO&NGoVrM#gc|_+i1yGU+VKaI97)G?dGJ2{dddm;aLd`ru+x zrUCAA-^Hfw_CJ%36Sn`Um7RU~tq{}!4LMF{& z!Y=F}i*C;kn4czV2JxfGPYngulxEAC^Zx!-?iHJFvxfG1<|+0mkm+*tgy6x-^{Jf| zuQTKqx|yZiQdt{rW7cwOWxcR6?c%y*DJ%z#Ae|UiIO#H(obVAnipJ@PlLJ@Ln~HXymE#7wg5EceRA<^_7@?fN8V&Q zPBgSaQ^@~6;lsBjpG`h{J3cDwxl+n3zcR&qpZ>@+80Lrn*)j2M-X>kmJ4%a^{;2e` zzsmml%nwYHTRw^+*&YB@G`^Hh;Yik_b+f-ijnTD#OXWeF#r+B`0!gN2A41Ixcye(x z3h+`ejU2h(lZ~8A5T%2uf$y8Z6v@7YT;SG&AADbZ4`3UcjXIexq85M|aJAspL4KbB zxD?<^@*pxm+`mCuEtFXRt{(giqzu_wS_TX82Oz8v!kkbK2e`|i9tDuc8ZbT*p9x;= zfcVQH+zI*p0sNMecwjVOjF8Vb#9a;PZi8Mr}%)wduI(uURC zCy<+KMN0v02h+@c4aN$D7NQ^FG3HOqpSY+jBEO(Gq5OsFE_GIlF6C|Q{i)&9AM5Tn zJeby&?oR){>9&l@jE;;SW-c*@EmgKO+pes~bJX@!{?q)o92t(Exo(GXlyUzoFFWsc z&qRKtAg54Xm|OT2Z&Q)AIKOzX`18J70x5x&3(l0hTDqz%v)oeAP+3vAuS#C^a&STL zVTp1}(FRyGlRxrK+>b&q7yiD^41R=c4eSPlr0^uQS(+|~ez7!FD$bH%7Wz@41al|{ z*6AWFLuPzRg5}7Get5%ibly8WdChK!Q_Hmr9Ue zt^CWXgA$yIZqsj=<;flTDGA0%XDF9o1}O}663n8Yp-F-{h;yF=%TS8pVF{KagJDd9 z6{ycJF2PEaX3Uac6K~-AlERG?RuLLm+9VigWL+o0KqKq01Ott%pO9dnk@X!3 z1{&GYBp7I9^GYz#$hJa)fks(w2?iQvHAyhgDC>t33^a0jBp7Jq{G0>>jht5{7-&>5 zNntfc4}+ z7Mk``6t>YgV%@T7oWm5hQ}{TAb7}mqQMi!8uTi*|!v9WSrC9&pQCKYNF9c4Z{$7^C zv@KrRMjhcF z+d@La*Wqy{_nGe*YR#v(NXKSc-%g1}0~C{S$TLjyjnS5LNp09l>)HV}$u~iL!Zfc= znkVUD1GFB5LZok!I6YFFIP`#aN~nit720si56a;23@>j8=r zu|M}-!_A_M9HBMqqbcIFrF|0D^-wy7rE+7Eyd$lPQ_2rt(|<^9NspVUBjE;~a-~QQ z((Vo_>-uIX+$YJ=b})Ukb|X@YhpDU;bJ|KndT4G@_$SKD4v9AXkVeGyI*8XvOBj)G zG+&;y)3kR=brms0FwhO=ZW@Ph%6wTtcwHO=5>iO?8X4M2`D^$ZD$e8_rZPP`FSkyL zzd=fqk!xahQVhi?ZfEG~qvL!6;#b4`!4Ehf|9n(x%;!j-Bz62Wem~^y2RlI}0jU@H zila?5Z?V^j@-a3$M*3z`{=Y48E2YGYOq?raCG?Ut(Ce3iSpst#0=IxK={-xS^dSEA z;IkTRGW%(O5xpM%t)RYZkd~f#s^^V69~@z3{Cnoff&x5 z={G|8b*4ULw*m+|Kd@ZKLvE+kX@xhXNr9R37=E8IyJ~vAV({Um~<irHRCiu^M)=Otd8k>lO-$|bU+lVVyNkHyhHHgBC! zG#fiMk8kJZB_EEdJ}NbOuaoI>`OS=%ams&lvw|6!Hy1Msam5+*FvU2@zh|~RV!h^b zcfTYhMf{7sYEa^jIca(=4}DmR=J=zLw*7;Yn;8qmnO%HdYs8VJgHm$J!z0m=Fu&=|%*M$?||00r~WX7+K_6-xCINvw` z=@tZofnu;qe0->{k6+c>-IIv(t0M78Y)hnbO>ciB&aaGY!RPU7R`4xdT|PcM(8)*o;*qUA5Y0E6 z6JRLX9SaZkjLi3n@O829*4}|`lC~F!QOvJSga`T}BT$D}Z#+6s#INn`NJL|Nb2!!+ z8At$HOH0=D^v0nsq{`v;z6hU~p=DQZES})QgM*QxTINP!=a7M(Sc5A(LiKt zd@wv1i52l(P{P)p-i{u=H^FZW$NA1kytjKG(&^**#suF3KJnr9cw}f8>d-gBw?{gn z{SiJo5Fz;yL~M=5`a0u$e-tVZAMWUg#N%DVeY9$PM=U~d7>C?QMWAWjy#wJszEf;{ zoZkw_;`@ON{O~|$Bz8R(JW!$DSfqmzqJ89goB#o4+b32SYBT_u4iG379q#S{y6};^ z5|M#;@0JKOGD4^d&|oY|stPFD67AbUDA6?>gVZt7pgRfXX6OZVxB`EJsV$Xy+gzCP06KH$;z-oD<%2oyX# z*awle_9lAx?r0P^6Y3J}AAwr0>+OsHA&284pR`A#cgAUj`orDfyL$&Bagi%x5$FL4 z_=t)e+8ON_7Mn;Shx_7D8m+T8KG+u?5j{J%L}H0v(pFzjA~9I)_ix?0)z>c}(g&>L z?@9Fc`TG+C;r@ufKfWnJNC*5FBR$eb0{_W`TO)mh6KRr_Eo&N=HZBRRX>3`^w=CtG z8<*6tTwTwH8dlZUuc%+SMyuAUY2TUcFa++QTmYm4i~%#>pv%%$^+J;YTZH{aqQfL* zM|2An1tQCme1J~IK+NM7tv*lM>3#rUN1*V!gm3kTq0O>Yei#SgI8Cl37&DCI&dn zZwdDeQ;`ym$0LdPseSw{&?BLzjLftG+9XLrV8SpTAB=SL0*idG(8}97v zCH)w>e~gX^MZ`NsaZ6>%b(QMt?eCR1oyH^mGM*4+GT}Vx8Qls3dbqu>H{L_afxJX? z_XA5p{ee`2BfQ9_5{9lVh~lHMYp!vm7lN>h(_*00bVOnUQVU{Iy=mllPjnbMYb>&* z7X}o<10QTYi3gO7KtGlGB8fNKMyM7Pkbqu3M`O~kuvF`=8)r+aJe!~cd}4(i{&U?W|CzAStO2NElfS`fGNmt=)`3vY-aPXQ{1>?pJb1*Ut!OHImw=4 zU-;-?<)ed@j}BHoI#@v;9jttGu=3Ht%0~w)A04cGbg=T#!OBMmD<2)Kd~~q#(ZR|` z2P^+y9;{rm`!NTHY5W_7{o=Ybk!$uv#F_Vv^X#J>KJS~$;TCXBTmyF#*uiT`ATszL z<+76Y86tbc*7x9X{0xg|zX(CB7#;ISo&IqSH-=~Lo1?1^$o>X@C$GND{$jGezC@S? z*XyJ1M1DaD4VbWIm3)Q$1@i)o6L#=;b0Wh=L;ho8@nQ+6s1kw6!s3!QLu&Rv&>z4s z?0>Lt0-w=Tlli`qOCc?IVD@83hcU9F{p`P^aWD)L*f%C~^Ge3fu-}95@35~zO-Zs> zC$xqV$nab231mcec8WbNg&dzuHI$%GoP7vk^fK6I!JGqg2@Hp#>=)1uFnhop1;Zg7 z*mf{}FfGJ~J;EM=dLAHG*}?XMiGtY!hC^%EFM|J_#AXk%cOWOEeUK=?2KQ6!!_<8q zTr0TGf`1OU&%ifv$E5G4z$M{NO8%dN?+kFCkle??-v;hu@J-xDrSC25Fin?`-2H5P zBFC-|qtSn^l9VQ58!RCk9l4s#PV6 zvOCy4EK8Iv*0L?^CRRpX1S_gb$s@i7d3EVt&3?_e=4H)Ujci>0viz+4ocxkpCTh8E zm2Z|0%17mU<@@ETz4E;ZX0v8cGpb?r8eS972%1)n%&x%uL-(`oPXKx3y38=9NP;onb;dnckO%Qt%VE%TX$0j6$FCE0wK|CG!>cCeAz6fj*(h6oX zm_aF2J{vNVhQz-_Lcn++G!=Zw>pYwXRxsZGPqP30Nro{ss*zN%pe#UdW~I& z-UI`=gKY;h2<9jlj@iKU&mnf6+xl54a!{NIh0lY=6qN!Tvcnd{5Z#EqaMm z1NB3T*Ftbx(IR_u%@O-j_cVrJp@wLpRNJdvF?$fatEO?|hXuzh<`nqFIrx@w$xRFEx_W3%E)Gg+f3q?R)t;B(7Ob*JWdF3)WBpUF8 zn3-;VW+@2kXlA+;L^I1Gk&e0D5T(dXq8zJqLxf}QN*a7*F4*N3tJ8+uv>Z2Xqvc>c z7bjng16&}*VU!T(dL@DXtG~$N|3+jy8UA@^M}5Rqzu8qE0ke7hQ(Jn>07f2{N8 zB!C}h^ES74^bj{38UMK}Qa9e|s^gD^J8lr%L4w1sx?`xLe$D!09YUmTA}oaKUE#X6 z$%8wVG+k5HuGzAd?6^T@J4j|rNLdG)ZV=o=f)A3inn+nqq^yI&L0VSRs>Qgeb^S3V zTHLneHqo7AG-~J%n{AG^#Toj+MYMA)beKP8JH;Uwwlv9(>Hs;30#!l|o&c@!D z8{4*R+qP}nwr%^%KJWYBkLUNR`o6pB)R~#nJv~!3-KXk4ef70HMcU7_Yv%{qf@EEZ)-R-yicZEmm#$tI zgtYxc2dn_Z*X1oT$08L2i{yB4`3dZf!o>aGsKNg-e*dKgXP{?h2K<{FoaNu#;0n%G z`n1B97IuRAwz@XPR(6&)|L98TnCsID@csQ!Dj1vV+e+y>%2}H0SjfmrDnQW+o9P(Z z;xm3VzEt`7EuGY;nV4DesTt^)@d0!I7JPa-I%W-8Q9B(oV_iNALoGVdXZ_OoYe3l8Odo*%4@z^n zFM4u)3%$Raf}&^r_dX=_Ee!39@RO(8{=D&j(9JaHth%WR(CI0 z@5CR>aaUutX2f~yYC9t%o9~a8wO$G6na^Jwf|XvDH&V!l`7e*1wcgHI)dyOnYA&~1 zUjaO`iIOZC?}bFK?2aB6fu=7vx~@=*g@cUmH!M6<#u;Uc#)q^z>67Dk`OVJ~t(uWv ztS?#_mD&4R-GjC_#q*{DVjK=1+)U+}soN*9?30Ve?yO}2H7e?9IO>|{T7JgT4mjF9m3zo&kO!xiuBZ)3jTS*X6_c%punTFlg?Lu`TW#`SK<_6HE@D|W9u+}O**u)yVuV)L@lhxd)s^*W|TUb>D**_fJOLk9>ezU zBlR!Wpq01(WB1oj6m0DE|1rw1W2^u7VE+Gkh_Q{Woq&;!&EJDBq4U3OW;Q5VC1X81 zBU?2V78ZPFdOCb2Iy!ttMh1KU(^s2``5*1S{lDX^tZevzFS)~gZwuqU{`@uXKSur|7JM1`pHRoiN&COC06_m&=)Z)6{}rIe z2e2{GvHoXpIB$n`QtOXr<+iQNNFde_^%#PG*5rDR(tjrC#Cd%YMI4L}0A3=npjx$| z+Ib>ahJ=etQu5{%S5?K$ruLT0>S7Lo=_bf`63=X{6J0gW_9r$TcF69Jx&V8kMmTje zxVR_eg>HTQ?D+hA{Wvl(m>5fAvpq;-Fq}|dp3u{y@Qay-`4i1rMafEY&!7sCLd@+Js-LePdBn7%EOxd^tnh&WcreF9RGUm9d6 z<-a}y?vLmw_&f*9k5tcuv8ETJLF{uF=Yl%I;I=~%D|1b!E3MJFz;f}re~t)YU9I$y z*FoA7XZWxoln6Oy`!0Do9f|f#^ilE>gjn!P=WgbFUkSNT#093%IQqkw;sj+b(AlSU zy&r5=2N#}WI<z@W9vs zg=YX846efzZv=EMPfqF zgimu0iecTW!>a>2=JgvVPoJ>a;HlRQ&yLa+-jeKtH1D%co!186C-~tuT}Obic7=RG zMkn(%(M=bWP*ydZMu70t?vk&N8(Y{rdu?EI!15rjzLw%1`G%MRafSfJ9{AenRBly; zR+d&S8n2dl6Ub*mb3poU4+3kHtJ7b-RUueYjnnjZ%!+@gr3T2AyY!n76&+AA20gXS z!eWmNUYC1ERuN}|X@7DoiP1uJ4^d9t9|_+fvwMF8efXeSk%Zm#4uP%tcLrGw^5{ub zC04I4aiHYvNcHQ6;E{35q{Q(kzwPfBN3mzihp}%R3kev}4!OW(98O)*ht^ub$*B4+ zg5;k&L1=};Z&hwvZUdfr2xDL!5wzg)gek-VH|h4oOn7wc!E5`KD{Ql;L-bt7~q_slAsK@ zYQvi&oPD3YB>$wF5xNq)qP}}j?-EKznMB0SEc_;or`C#Jq*Xp7Sb5om!RcbP5ySEdZ2g!>EzM@XA-pZi_*IbJfx;u_iMY(I+$Pf(|i_Kb%?t3Og>3-S>J z+}DVMVUWAmZTAG^3~zZb`4mB}Zt8+*t3Tt{robenXSvAljsTAyy&RTqJdb!-z^37a zPFa&r`R}zNC)~CP#;a%QgD=WpIFJD?UaZw=Vpfp%z#X|e)S*v58=j%MaA#1Sfwv?T z_=LBgE>Ua|7kq&-5K6#~zDJ^8AV2{30MrxN(mJ;tV!O9grB&%wrsW?@1x16#5->(P z7)!A?zhUV&P=|NCtMDV@2d=7jack0t+`y^QtozL^paYc95UM-zD>t?y&8ij8U$s@> zpQ777ICsqK17K5%{l-(xpKp_>YH_h=_fI$lKjxC9xE_Uf!n?sYVLL*eWI@U6bP~^vu4l0lJ0M}GHa}X$jh%aOx$l8z4oIs?6CVd`je2^~{OwiDP zZXuTfX|wB}6d7AtgATUUtN*ajfSRhxe8oW-?h-ftXZQWd-^52^OZAUFsGmU--Ngi3 z-=g|6r(3m8C97PabUqn@(M-2tuykCO3}qN51b4SUZo4d5ykQ!ANOF1i+3m2-3N3pv zZlLW@ZJn@QxLtlYNU@*YeoN8Ddk)HM zM|wNsyt)ItAUQ>R-!Da2+(Dh%0CGt@(@)9tSZWIHUK zPh-R~G-pYsv!J4(prxdSQ$|r$?u_;Z#RdB2)`rU>aaITI30uuU(I9|{i<+tb53{wf zFSkl!-h9z-G0;(Mg{2d-Q=~r`9Z5aA>a7dEPA2dfzbi4_^?x@qX+T=+KK~OF4 zrRIluge{Mb=GSy8eSl04JkoE;{C zgrctnN(A!6Wv6+Bz`Og#9^aw1m6~H1>_Zfrgl1ABaYDyqn0-D}-}v}COZmn4pT|bk zUf|-3P0v^aj=Ex3`^xk^E-qe>0l-B=$GbV*y*Ym0!#MERz}nI*PA^MO9Iy(cN_<^M zRBe3Sod!W)-2}3IaoLD{@RQ=BQ-D>XE8y3=85GE%8ItpR7FhIShTjb8sya{ zl^_ZGlDL($h($?;Pm*H+mJS?wU#EFNxyCiPc8GoWos8G52wD&8toYjZ>D(b_OYKl_ z4nBTqtiYm}ChMb&74dx|1AX?F%R!=x94!GCgE-e{?`xX&u=Es%q>03?%7@|J3{`nA z6Mfo)V~c>C=c|F?YeD%s|LF&}O2fGtO_AvP?@@OEaxJ4{tx6aSy$aPiXzp4=m;}Z8 zFugz4M{$uX{6sO6c6=Quv^jl+WqI5q(einw&6N5M6^2k~ybd6>8uQZseg*mjttzoC z6Epw?x4?aL;r%t1=ExKTzRuG#Ro-ZIvyq22G3nS5D$VwjK1k8#F|zT2+Srcf`mS81 zx?$Bt`$uon0Qe&=qIegGbuuOsSNGPT!U>xqp=y-eHFjvsT*x8a$W&3>B>mZ=72!;e zjy-veOS9w2#G<{;;R*Z1Vm6ppGdw;ec|+(qDmakw<9g- zRa8_6uVJ+xqPoyGT3PVG4u52QEDo!rrV=4y^S$_ri!jZEA-~)%~QC3 zFAKKoRt-z0S;^yOTzqn_(uI1N>s?<(sGF2$wEO7X)fF19JI8E%OyXn~n$~)2vD0b6 zz^GwI^Sd{Lx=n6XfKds9b-?-rH~8aLXGrUYqD~^}qY>W7^N(A?!_cS6r^qmNQD5j} zpSaN{f^#^JMh8bZqC=X|-Go8;gQPc{t5G5pkwefY*{!}7RcqxL-be=dtI%?10(IQ+z^IjqdB{Koq;UZwl(2m1a@KNb zvyg4(0l~O5Q|GLaeG1ihQ-CDBsdDK?udDZ;kEl)uYIzs5llxbU(xgjR^zt_QR{JrR z%=@Z)mC~2Ed)QNO#RU@-SoN-vD-&Tusy*m;e^~#Jo`g4p+*I)QPYi<22EJD5*D^%{ z-fn#ALj5}1r>3FcdxrZk1Q2P>N=;f*J*K_H}RieHCK) zGURxT5$=5cNw z>qFEg(2KWbhP1j}LW3A@$?5XRcZuG4jFS_YO0-{MA*qG^GbJfzhKPd{VHT-_7YoiM zTmjhfaAK%+Y)ji^I+V5uiRF{sqHsec$n|Ux&h45o^3M5_^;h3^&xE3`{Jz*IUDx{< zyIGooQ=`ThG?!L%R^mb#m^OSHIu~q{ZM%t)_Lq2pMrpT}~Nsy%kesi7RjK zFwZI8s_3r_c9<~N*j)QyQg@#&2pRcli0Ty0+(iG|C*2jJpFU5l?IEv1wX}yjC!yrw zh6YcOBA%i#sGEG+# zQ4%gPw2=cxlxAbg&Y;x^Ld}D=fZbN3ZSlAc+GpBGWCUHV2FiW`dxP5*v9~zdNv>^9 zHKLHOs)R^0v>vp$m^`Pwnm|Zh-a?|Ae0M98e2Eb1q7Vqwrc6HuHIucA6|@*=p3 z1+E?(njRacUK)8<{uc5dSK$q`TY8VprpW8y`+dFeT3+u2UcUt1zyyCGImkI9$hsnl zu!K1;+SMQ@s~@1v&A2lLKwy(DRhtZ#Y{xpHvu65|UI`zFsv)YWlv~4OXKji$&YHB! zMjGHtW;mp)5s6FHi>-3EF2RzqzF1%qRE%@;e88m%Vd48-4%W2anFphQkdp0Tfz!!| zo4d*Qg-sI}x#nhIqx8`UMt3m7rf?ez8L1@aKnyj$m!Uig2X*hv{bo+iFrlq1DkEbN zEpXKlw_x+IIETfe=>+(XmTQesOCOGHjEOHECeR)%7$n)8E(aEJ&_dNK@l-v{N88ko z<~ta|Yza)Wzy1zwu|i=i@~it1(E2mWI(C){kKn$_2Bf45t6v1Jkb&jMri%Hn1iPTA zq&}zgM5if}a(x-a34SHEeeeO)PX0l%!PP8s>BG!*GJ#Xmz9nS)tFW2$wtAYHF(VfB z0P-Hcg}Fb4Xcax&uL5lb4s0PN=E&Y`Br}r0*vjf=9H~xiN=Md1M3&I;p#*B)s`$k8 z_?Yx`e+mkOn`Ik0d>6i^o`mrmE7wXgL%Ln-)tYeCyDOnT#r zbb%3w_q|HddbQhQY*oD}R(-!AV7u0~`e|Z8aYMl~`Hmn)3EHM8TD+Ha7Jes{rI{mV zCQTdxo(6T#pr0jR#jFs%6f8+jWJF+Yj50dnkg&QO>y$Q3S!Sz`Zf{II?|*no^z`;w zoxxK&r?t?kY<6XTaEyL*2LOFSJ7c;~@Lk7z*=pJ5t-#U_v=7p7+AMYY@LlM>n*nRY zy8YI9VdHQbQKMdBE4{;$%z<(!3)Z5>xK`21m7JYSgP}jRo%;t&F)y#Gc#%Pw_$b6a zCt_Zxj=(oU=dg;ej@1w@&>Ho0%R} z0Dyxw+ZFx+-K@>l&a&I!WDlRtkHUo2vmmi=NG~+Kw3l%yi5I~+E}uPdogo}BA~|6b zg`DBKkCGc`S~k3FS9eGuKeJ&P!AL$q%gt`}u`KDa%DeK`g*ll1)S1S`lO|k~Sq}V4 z(^nXj$%VD}Ti*AoF%i5g7ogFHJGW&ztAHEdQ}hc?Vd#_%T&W0v$INM}T|-ynMH}x% z=_DICn{9vXa-w$L*9gaYtL~OP8(gbRRs%;q2iLl;Jw<9JA5WpQCEfC8tieFf=L^DHV)iH|mb?_^nV9Z3qheM~*d}&Q+dJ z4JmaDmr?W?Td3Zo3X!Wqi#CYU)K4P+AW-M#Q8-4lSKC8d=^k?thpgrrkbGjgcuF=o zSA3S<#vKu2>6Ve`9t`+z-GX}ki7uylJW%j{2}EUYLGbnB+;bg!0c^<2i+ly=D&v`{rbf zswQY$bHNlWX`c4-w_B2dsFzEk=|x-&ewUeL^-2viCtwWNm?y%HHNT3n{0SlXl?rXX zAImVih)Rqm)916GOAdwAI3pPz)ciiC+29P^bnD4>ye0u=EvMmrW%WR+_(8Fg35??1 zOHqdSLux`XSyEK>r>~h&{2$inacL>$6KAem+Ny$3UhZ^=+=4XZS|0{padCEPq0UNH z1o6SfLfyEDe1>DCU^Wdln9SKkTQbsw+^a*zy*0Qlw~1ry_;Op^in6RGN0;Gk(o$HT zCQaHSxC~GiPB~du>tit({pX?PJ>xZ(W-^QQ7dP44oEC1<{31~{ek{T3oBows6_9wS z#8B%zcsjVR^pDC5WiwqZAF?9!az3mp-yIT`LI!5!7R#B9vsS`S(SCY!&QGIhmBm$R zQBnOBo8R>%ibo12Yl@B?%$b!ti#LtN7F112uT_`Z2I)-;^LmSiYHWQAMH=My&n}*T zc1pd2B&-u5Tl@B-Vc5A?E=Ae`RgnJsB)iF!a_<*5MS_BYyr3#kIkR|2V$#(&7|I?R z?ZYKAdJIC#VGR%a(m?}t@qjLRQ89YRCX|Jpded?y+u+zg6l*PaOicxIn6eD4J*Cx| zYwgPMNm@%pJ@`XmupcC5W;>}#1@#UQj5c-Ay}}WP=oS&bL&!6D>7)JtFQIRNNbuIV zw)yLE-smM)oA#WLMQid;UE;bvbiMRW)!R8S3mL79!X71K1#XC zd_nTE#x-f%&(cddF3Sl6*MNgJ(Awni1$*p7lxai(zhg_;0uWZkrI=g@N%?GPn)3?d ze<#d+6V5wUhDXKZVZb7#_# z-2o}R5v2X&ym$p)drJw-P*x3jTlf>X2@K%h1CNeu#yF7sf#{yJ=1<_!u^OP5C$^`cmb`LN^ETwou+? zn}Af3FDF)hwSIk681c#V$0j>8BIEtZHAda$-E@o32#Nm<4+17N4flp*_&HiT8EjfA z7O+fg*WlC;IFoAXc4+EWUm=lO@tN>4I>R*r8>+RzV!+b*c^HqN9IaS;EX65IW#_&F z^ZEtscF`MD6Z}f{v9gJBip!y##7c6^umcQ{v%jW&R|uJSO$(bfIv5XA;VkZDe|E>F_GToHB*6l1 zfwB2acK+GeW&%uua0B|=g_Lwq_A-ja7FwjmZ-IL1k(hdEq8WoFlW>o&1X-LyxqKux zK0P@-db}je#g?XQCc$^M?f_fw_o_`b`EpdKt+D`I15y5AmYKc_P0lZNCRIAM%16~D z31a6a%*x)=renSRCKIh2ZVOIbVSCq`^EHV9>14cH(bu`%7wcW_)AJLI_pob+(txJ8 zVB9I(1TIK-CL<}wq9fNhMp17f=Bus#_RbZQmDiri$u^H1GmRF$#)hH9*iz~1CWht1 z3TN|A>5eO)3D*n5C0E(O($Biu$DDhN2&UEQ_i6VAg`Lv&EMc@I#z1^BK? zsEDG`a(4V=D6`wE+xSz~R=sT@EO>yyv9irI?T&R$OKx+;6i5$IeBP1VjvTLGFgYTG z4*ZxL#ZF{iK8_EOCB{}n8gnPlN1Ld=kGjn7qS~DseHcQhW!yOXJCZrmYX#XlI1L6# z>Tc{R+6l9^Af*%6M|J^0(?iEIKX$N}2uc{npJQMTXIaGSO@TF@NpF*P+~_+gJvl#R zJg7XmwbCu=@O8m5yY-Nh#(oBdvPGZn95lzK#f%P}rbX>Qxd2;N9l^MQ{6e)vF~X2O z{9;29P*giV-1>Q*U-XP<(h-)@33Z?DuStP+0Rt(}R_0B}0(@+*U7(h&)S{75Hg+Z< zoPQ5t(by9mYCep?CSti23*=D3*~}`R8S8tNlC}n|367{svT*j+W~0tQb7s3@5Cm_- zU2QYh-8Hk*qxa&(73YM=5~G7JzAn4LOhZ)2Vm9B(khRlsXK(zm&C%UjNaXrlGU1GB z!2P4Uzr%fE#hxiL(<8eyV5tUgC*mu?x+uVz*KD$Wy_X?*Sshl_gB5)7sFI3Xu{Q;l zye)9Ecg>hp`G&!*M5?!#4K{H?1!ci5JVg+^twr2jalB-{B#gItaJ;!{RA>wVSX9Yh zUdWD1ci4zk`PvRtey`U<4X*NW6{f9YAjqw0plHF=$v%KOcGBv3t*tbtAg0m@vjR_f zxU_7*ZsTMRr>%AkZ)-Hv21X=1rGd$-<+1w{Rl|X7p-V%R4Mn zWX^;8w#-V|xhMCw@uES@c|e@3?1fDUpv>g=Om@pNdjXuX%ZL%wTz;V;A_mSlJ#(7Z zZ=P^ih~2cNPp@-~G+(zdT!qk+qz08&PhOF8uEwj0XWRCTi3OR_%(nJBdj!z~yP)cs zQc{vO77X@~w)Q;+0u^qlG?HzM48nYA3i+G7nIwTxry94&^JmUS0mQ6LJ+hiFa=MTq zG+Oi+6l>_D)K4S0hF2Dsu&ZO@${c*4JCBV`)ACYR|L@OAudeNiK-RR*d_q0rV>Xth ziOsbugK!r(YBPJO49lR+cjk&AcN^?gJq3!LV!G96G#>f-fAV%e!qfZ1C@8aJYcNhZ zHKgz?v}}ctY7MTRU)ztvM)M_V^%mE0Oyp^PBd^t1ZDaq|7;jdI&9&_nu<*T17vS@%*vR5cw0nTqzg$=+sN0s4lv=k@Ddu(If3lTK=w$QFwkXBdc0~3M0SgLSh$G9QY$V zq~cG?j948Miat_(<#5eDyrC*E=OoW)9qJe0+YA^|a$0>uJ>|fkDVrl!0L+<+c%AkW z$a6>iC9ekS_tBd5L3gbHdO*s;5S=e_0W4rF2TDE>hEgWv7#dI}I9%WhDC0LS2-H&! z7nazJ)q$-^y46z-O&PBTr1fzcbO;C77qK&WK9dfO$|X9@)~d;OlJtTp=($6)c0>KN z7YL9HKq(^Y71trIBh|kfsscvQgOO1NJueBP9jO9#23%xSr5q9OCc*g906wC7FpZ=q zoHV{v=yuJaOoKfb&H*ql<2(B3M|$PaI#ZU4r7$4lWrTkRU(#WGJ!(HFdb2joQvWs+ z4}jlSYV^?WOk1@A_&OsT_0jK6VMghW7M=OO`k;&QtieMiTX8VHWl*SOWxfuzEaG35 z=1z0104kx5tuP>;8#u_!2&0c1G`&y?XYG+a*!i~n0g&lDGE4k=nnuw&QO-JG{_+q! z{;&X;H)IDHR+JTasLB`}P$*bW_2%`lq@^1kDOpJRQO@lOIxC_fF z)=wj=i_Wx;KcM8Bzd|O=go&S75Jsm5-tVU~0KcLW{iOkOmlHKKsTnwhKlVeo)hN}A zJ$p^SdLWA$;x6^5%@=RG{Qzm;3v_Ebm2cN@baMjX_&W-k4E`uU2lmHNFh3^n%X4PR z&1Oz3Kohea&;Y%6q*h-*ok_RoSo{!V0hS$ECp0BxM40w5c~8M>3c-jNph9|#8HD@2 z2q{f8`GmNu6bL)^8u(`FnH_%})j&|D^+p6%B zL@{WEu6tFWd8L}uQh9@b+axwNF7Ss@a>71BcIi(A=wemCRo?m`q(pjbi?cMOh*1<1w?xJd?Of<*VAp4Fo3)5ur zddNm#y5<4xSn```f;s&}(aHgfJKRePzIJRwOgxufQNwGy(Iq(d8oeieDG7I7w~EcqA-kJ&_o#pY=d8Zjk$LaoJzOYLo> zL1lzDYtj0!=jsj3@RziraG4g7qNS!e353h{OMuT94KW+^`1coo#6&ZTC=;!k3vvpb z;2C6%Rz^d#d?7L^epjn_tz(~hF!J}Rw#;5M<=Q>MLUKbwJHfaLJHdt)osz*UPD{ZD zM?HJllILRR2l(x;baG3LT6w5KtK{a!i=rR&HEzPF)RAwV4g*{YNaEbq`Uz4? z#A@1$jeI5n=z(2W+3{x?%qLtTu9S6V0cnDKXx+gZP}1P$>ppj6!3FWQhqa66=r6fsnS29e?R*7=${Qyv}y#4e{|#t7cD|~4i#2Tsj<`M&V4B6_z-6{ z`mljD$$jS%B9=OWr>*?8V_I*28PlFeYz9#w1>!oz$dkZQr_D%2H4(HxmHE_0U1%IU#!&wl`Q^ap)k?9 z!p)W@I&ZvHBQid%cM814=r@u6pf-Y*@Aace{ZnedT;DJ%{5S-OaejM`Yzy-tW`AsQ zoPHdV?#g9;3e2c+!#7Gq1_t5qB zwp=v8OE(@36z2MA7W^Gtpw%Zjy=Q4^Q==3C@*Lt9sX+8p76L8`$+!EMaegH$xhn`Y zcE4}|0U>@XI~j;@K>^o*Z{TzhgkhL-5a`bf<(U?L*? z3cLeFSlO9X$<#4%ldwY}uzaGsr;dn%8p`ZOtyrPj5E6Xf2?=y;L^B1pMeO7tA|eBQ z0FmF*hx286+NgmSaFrmwf0$mGaix?x-cdO>>mo#d8~!k~P!jhqIJjH*Ns z79^p1rx4RmA_GVYyaJ<*vw0Zl8Yqdv!UG_5;gujrr}Xc*vPu#mxc*W^KBy%5;zc z>T=}9WovZfbhmF$%P7=@h(I`W5h)B1LE~U!CB-C&MEMd<(}yslM?^$^FU%k4Ue6xV zODd~3abh79sPFDOr@oSjtV1BXw1^ZEF=sYeN|G!Td&3s?#LKzU58KSyB`zOQ8!MsOb|qw z$dR{BFQ3eU@~0jG5zI=f7sk%)L)QQbzmAD_%&>1v0527#n|B@Qpo1R~A#sWZF1T1y zm|wS`7h@Iq<-~6=DhAM2Jr(3p-*0eGt-gfvbNSzMr>FK#riFTls|a-pC1}fm7l>T> z>3XsdaNuFTG`U-BzU7ISxq;(gVPnI@y2GsU@K5^l&tPZQ0T=7+l|Xc%p&ir3Q)J?%8a1YnFy)UV>s*GVs)D`jzjc;l zhq=TEl08(^<`Xo=A`}4^DcNlh?&lD=%}c<1HNB6-Ii&~jP~3(Fxp9N(O`W8T=0pHC^P z%OOJWYN{xbD=5Y(B`Tf9ehgH+wx&kUy#ZY9@MaQs<8xzzHRtRK0AG&vi@FTsR~S1VJeUH4LVa8ph=E8NfL6aG))c0^j@ zoFj$l&iZ}%$S--;hzqJGD2}s^a+#b5{AV<5h)Fp=O7?Vkf_nzTRm$t7H7`MzpfqJG zi+(dwQXv0^lC2h=t|pm1(ksxs6r7_v$Gtg}|0==u9F=DfFzM4DGQuyEe zc)*Mkn)X0r5xIS%$xxD6N$TSs)0C9WIs2Z*$;r`k5tewS1_-@ zD-glo4z$uF8xtaM3_ArAZL}9sguvd!)l*?|TE%8SVb;X$u3$KNHGdIXQ^)h3y~^|t z4QWb)lN!WI=D+g3s%5c*J)cKO@w-6B08ce^VK!u)M=A4OrZAz4hYv27+g-IZWrukN zXh4f=iRafdXPjhlYB55073`XEpbrZ-FmMln_@?gACEYnpD9*>B;u&%r`*7^G$(ux| z0m!CM7sUoesDCg-`lAf8{6*UA z5*U1g;cN2a&gJI7j}h|Fi9pF}kf+4?qVta!&FO1UstTauy!w2ADW_CKVCoXHz(g7a zTu^6>MZ{LVPM`O~pQLQ~8;~gUtfN47@Mef&_!*cyfv5w5bpJeIQf0g`0T0t6^cxHj zdXYlqn^pl`GlDBw5s%sslqqLee$JJ_HNu50cO$%w!86^*uab$6)~^zn)oU8c6qxZHHS>cjmR{9Zxfo z2mske&TGW4KKg!;HNY2+Wcf#WF0;h5G=po}x@5N;?xByeP}BJo<-zYV?vfgh(m`)N zPLbwaNEI~lz3H1}UxdTV5R`tPft0~Opm`xe_bvf-xDp-sb~89WVJ({KV+cD~etz`k0+aEW?bz&fRdS|oM$n