SLAM A 04.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from math import sin, cos, pi\n",
    "from pylab import *\n",
    "from lego_robot import *\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# This function takes the old (x, y, heading) pose and the motor ticks\n",
    "# (ticks_left, ticks_right) and returns the new (x, y, heading).\n",
    "def filter_step(old_pose, motor_ticks, ticks_to_mm, robot_width):\n",
    "    \n",
    "    x,y,theta = old_pose\n",
    "    \n",
    "    left_ticks  = motor_ticks[0]*ticks_to_mm\n",
    "    right_ticks = motor_ticks[1]*ticks_to_mm\n",
    "    \n",
    "    # Find out if there is a turn at all.\n",
    "    if motor_ticks[0] == motor_ticks[1]:\n",
    "        # No turn. Just drive straight.\n",
    "\n",
    "        # --->>> Implement your code to compute x, y, theta here.\n",
    "      \n",
    "        x = x + left_ticks*cos(theta)\n",
    "        y = y + left_ticks*sin(theta)\n",
    "\n",
    "        return (x, y, theta)\n",
    "\n",
    "    else:\n",
    "        # Turn. Compute alpha, R, etc.\n",
    "               \n",
    "        # --->>> Implement your code to compute x, y, theta here.\n",
    "        \n",
    "        alpha = (right_ticks-left_ticks)/robot_width\n",
    "        bigR = left_ticks/alpha\n",
    "        \n",
    "        c_x,c_y = np.array([x,y])-(bigR+(robot_width/2))*np.array([sin(theta),-cos(theta)])\n",
    "             \n",
    "        theta_prime = (theta+alpha)%(2*pi)\n",
    "        \n",
    "        x_prime, y_prime = np.array([c_x,c_y])+(bigR+(robot_width/2))*np.array([sin(theta_prime),-cos(theta_prime)])\n",
    " \n",
    "        return (x_prime, y_prime, theta_prime)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "ticks_to_mm = 0.349\n",
    "\n",
    "# Measured width of the robot (wheel gauge), in mm.\n",
    "robot_width = 150.0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Read data.\n",
    "logfile = LegoLogfile()\n",
    "logfile.read(\"robot4_motors.txt\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "pose = (0.0, 0.0, 0.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Loop over all motor tick records generate filtered position list.\n",
    "filtered = []\n",
    "for ticks in logfile.motor_ticks:\n",
    "    pose = filter_step(pose, ticks, ticks_to_mm, robot_width)\n",
    "    filtered.append(pose)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(0.0, 0.0, 0.0)\n",
      "(24.779, 0.0, 0.0)\n",
      "(69.102, 0.0, 0.0)\n",
      "(113.94845953817779, -0.052171404797263676, 6.280858640512919)\n",
      "(158.9693376803923, -0.15692017028978156, 6.280858640512919)\n",
      "(158.9693376803923, -0.15692017028978156, 6.280858640512919)\n",
      "(203.6412167672408, -0.26085692984824893, 6.280858640512919)\n",
      "(248.48767630541857, -0.31302833464724245, 0.0)\n",
      "(293.33413584359636, -0.3651997394445061, 6.280858640512919)\n",
      "(338.18059538177414, -0.4173711442417698, 0.0)\n",
      "(338.18059538177414, -0.4173711442417698, 0.0)\n",
      "(383.0270549199519, -0.4695425490390335, 6.280858640512919)\n",
      "(427.8735144581297, -0.5217139538362972, 0.0)\n",
      "(427.8735144581297, -0.5217139538362972, 0.0)\n",
      "(472.7199739963075, -0.5738853586335608, 6.280858640512919)\n",
      "(517.5664335344852, -0.6260567634308245, 0.0)\n",
      "(562.412893072663, -0.6782281682280882, 6.280858640512919)\n",
      "(607.2593526108408, -0.7303995730253519, 0.0)\n",
      "(652.1058121490186, -0.7825709778226155, 6.280858640512919)\n",
      "(652.1058121490186, -0.7825709778226155, 6.280858640512919)\n",
      "(696.9522716871963, -0.8347423826198792, 0.0)\n",
      "(741.7987312253741, -0.8869137874171429, 6.280858640512919)\n",
      "(786.8196093675887, -0.9916625529096608, 6.280858640512919)\n",
      "(786.8196093675887, -0.9916625529096608, 6.280858640512919)\n",
      "(831.4914884544372, -1.0955993124681283, 6.280858640512919)\n",
      "(876.337947992615, -1.1477707172671217, 0.0)\n",
      "(921.1844075307928, -1.1999421220643853, 6.280858640512919)\n",
      "(966.2052856730073, -1.3046908875569032, 6.280858640512919)\n",
      "(966.2052856730073, -1.3046908875569032, 6.280858640512919)\n",
      "(1010.8771647598559, -1.4086276471153707, 6.280858640512919)\n",
      "(1055.7236242980337, -1.460799051914364, 0.0)\n",
      "(1100.5700838362113, -1.5129704567116278, 6.280858640512919)\n",
      "(1100.5700838362113, -1.5129704567116278, 6.280858640512919)\n",
      "(1145.416543374389, -1.5651418615088915, 0.0)\n",
      "(1190.2630029125667, -1.6173132663061551, 6.280858640512919)\n",
      "(1235.2838810547812, -1.722062031798673, 6.280858640512919)\n",
      "(1280.6537582523617, -1.8276228032252415, 6.280858640512919)\n",
      "(1280.6537582523617, -1.8276228032252415, 6.280858640512919)\n",
      "(1325.5002177905394, -1.879794208023668, 0.0)\n",
      "(1370.346677328717, -1.9319656128209317, 6.280858640512919)\n",
      "(1415.1931368668947, -1.9841370176181954, 0.0)\n",
      "(1415.1931368668947, -1.9841370176181954, 0.0)\n",
      "(1460.0395964050724, -2.036308422415459, 6.280858640512919)\n",
      "(1504.88605594325, -2.0884798272127227, 0.0)\n",
      "(1549.7325154814278, -2.1406512320099864, 6.280858640512919)\n",
      "(1549.7325154814278, -2.1406512320099864, 6.280858640512919)\n",
      "(1594.5789750196054, -2.19282263680725, 0.0)\n",
      "(1639.425434557783, -2.2449940416045138, 6.280858640512919)\n",
      "(1684.270680252878, -1.9841412539412886, 0.013959999999999972)\n",
      "(1728.6586102573704, 0.8598284716808848, 0.11400666666666663)\n",
      "(1728.6586102573704, 0.8598284716808848, 0.11400666666666663)\n",
      "(1773.0661516073922, 8.15697133623172, 0.2117266666666666)\n",
      "(1816.3670532147673, 19.757120907572187, 0.31177333333333324)\n",
      "(1858.1488321992847, 35.51411974830671, 0.40949333333333326)\n",
      "(1898.1939541378306, 55.272365533420725, 0.5072133333333333)\n",
      "(1898.1939541378306, 55.272365533420725, 0.5072133333333333)\n",
      "(1935.9441113136663, 78.79474056409418, 0.6072599999999999)\n",
      "(1971.7084324656485, 106.39625026390331, 0.7073066666666665)\n",
      "(2003.7765704672984, 136.7129018214648, 0.8073533333333331)\n",
      "(2033.1928253471913, 170.54140793552847, 0.9027466666666665)\n",
      "(2033.1928253471913, 170.54140793552847, 0.9027466666666665)\n",
      "(2057.97039210388, 208.67984307162823, 1.0865533333333333)\n",
      "(2074.669421483258, 249.84687865974914, 1.28432)\n",
      "(2083.126033698585, 292.75942835957085, 1.4681266666666666)\n",
      "(2083.126033698585, 292.75942835957085, 1.4681266666666666)\n",
      "(2084.1388689756545, 333.71289727487977, 1.6240133333333333)\n",
      "(2078.547805089071, 375.3485280900152, 1.7845533333333332)\n",
      "(2066.7578883528718, 414.94777760042797, 1.9357866666666665)\n",
      "(2066.7578883528718, 414.94777760042797, 1.9357866666666665)\n",
      "(2049.168209123536, 452.13958148376247, 2.0893466666666667)\n",
      "(2026.718254215487, 485.7891554112931, 2.228946666666667)\n",
      "(1998.288181658648, 516.9513281008549, 2.3918133333333333)\n",
      "(1965.8446819214103, 542.813589658971, 2.5453733333333335)\n",
      "(1965.8446819214103, 542.813589658971, 2.5453733333333335)\n",
      "(1930.1298001858388, 563.2359870231759, 2.6989333333333336)\n",
      "(1891.3537628505355, 577.9888845439078, 2.857146666666667)\n",
      "(1850.75498165005, 586.543194167464, 3.010706666666667)\n",
      "(1850.75498165005, 586.543194167464, 3.010706666666667)\n",
      "(1806.5457092628562, 591.5797381176783, 3.045606666666667)\n",
      "(1761.9006832792377, 595.8258316030442, 3.047933333333334)\n",
      "(1717.2556572956194, 600.0719250884104, 3.045606666666667)\n",
      "(1673.1366813413033, 604.319782164924, 3.045606666666667)\n",
      "(1673.1366813413033, 604.319782164924, 3.045606666666667)\n",
      "(1628.4916553576847, 608.565875650289, 3.047933333333334)\n",
      "(1583.6679746956513, 612.7763498406925, 3.047933333333334)\n",
      "(1538.496823640889, 617.0194633659055, 3.047933333333334)\n",
      "(1493.8517976572705, 621.265556851271, 3.045606666666667)\n",
      "(1493.8517976572705, 621.265556851271, 3.045606666666667)\n",
      "(1449.206771673652, 625.5116503366371, 3.047933333333334)\n",
      "(1405.0780317970764, 629.656845857422, 3.047933333333334)\n",
      "(1360.254351135043, 633.8673200478256, 3.047933333333334)\n",
      "(1360.254351135043, 633.8673200478256, 3.047933333333334)\n",
      "(1315.7781408657386, 638.0451549034199, 3.047933333333334)\n",
      "(1270.9544602037051, 642.2556290938235, 3.047933333333334)\n",
      "(1226.4782499344008, 646.4334639494177, 3.047933333333334)\n",
      "(1181.6545692723673, 650.6439381398213, 3.047933333333334)\n",
      "(1181.6545692723673, 650.6439381398213, 3.047933333333334)\n",
      "(1137.3117623079613, 654.3413192434955, 3.068873333333334)\n",
      "(1092.146743912543, 655.3665599529293, 3.1689200000000004)\n",
      "(1047.8314085607471, 650.2144466042558, 3.345746666666667)\n",
      "(1047.8314085607471, 650.2144466042558, 3.345746666666667)\n",
      "(1005.0980823462834, 636.8531296100091, 3.5435133333333333)\n",
      "(968.2348392804383, 617.4495671350057, 3.7087066666666666)\n",
      "(934.9926465551358, 592.6182186635655, 3.857613333333333)\n",
      "(934.9926465551358, 592.6182186635655, 3.857613333333333)\n",
      "(906.204896497751, 563.4689844676583, 4.008846666666666)\n",
      "(882.1866767074047, 530.7051982201101, 4.1507733333333325)\n",
      "(862.8496284237096, 493.99667316427883, 4.304333333333332)\n",
      "(851.3226921947943, 458.8977588884075, 4.485813333333332)\n",
      "(851.3226921947943, 458.8977588884075, 4.485813333333332)\n",
      "(846.5365586646793, 422.6226901775544, 4.676599999999999)\n",
      "(848.6313249943371, 386.0905748918205, 4.862733333333332)\n",
      "(857.4505569497534, 350.57713179005157, 5.048866666666665)\n",
      "(857.4505569497534, 350.57713179005157, 5.048866666666665)\n",
      "(873.4727447894983, 316.33867296290697, 5.251286666666665)\n",
      "(895.567347534239, 286.5254365186581, 5.449053333333332)\n",
      "(922.6186027875641, 261.8877159795202, 5.639839999999999)\n",
      "(955.5731375838882, 240.27219398192335, 5.765479999999998)\n",
      "(955.5731375838882, 240.27219398192335, 5.765479999999998)\n",
      "(994.8200320128362, 217.85984077055036, 5.763153333333332)\n",
      "(1033.4607967980937, 195.79362428831155, 5.765479999999998)\n",
      "(1072.278847527764, 173.68601198963157, 5.765479999999998)\n",
      "(1072.278847527764, 173.68601198963157, 5.765479999999998)\n",
      "(1111.828806778558, 151.10059041369095, 5.763153333333332)\n",
      "(1150.7726363856618, 128.86130556688659, 5.765479999999998)\n",
      "(1189.7164659927657, 106.62202072008222, 5.763153333333332)\n",
      "(1189.7164659927657, 106.62202072008222, 5.763153333333332)\n",
      "(1228.1801113069591, 84.59757252849471, 5.763153333333332)\n",
      "(1267.1239409140635, 62.358287681690854, 5.765479999999998)\n",
      "(1306.0677705211674, 40.11900283488649, 5.763153333333332)\n",
      "(1345.0116001282713, 17.87971798808212, 5.765479999999998)\n",
      "(1383.9554297353752, -4.359566858722246, 5.763153333333332)\n",
      "(1383.9554297353752, -4.359566858722246, 5.763153333333332)\n",
      "(1423.02480174743, -26.730856754114274, 5.763153333333332)\n",
      "(1461.7913104105542, -48.928725797604045, 5.763153333333332)\n",
      "(1500.860682422609, -71.30001569299607, 5.763153333333332)\n",
      "(1500.860682422609, -71.30001569299607, 5.763153333333332)\n",
      "(1540.033590761982, -93.4891811462594, 5.772459999999998)\n",
      "(1580.7278116068424, -111.77506666294761, 5.949286666666665)\n",
      "(1620.7030153181552, -119.3519959285511, 6.242446666666665)\n",
      "(1653.6707511051889, -116.70070655026188, 0.20123469282041206)\n",
      "(1653.6707511051889, -116.70070655026188, 0.20123469282041206)\n",
      "(1685.675198493732, -105.93298482419048, 0.4478613594870787)\n",
      "(1713.1657790074723, -88.49189260680382, 0.6828546928204121)\n",
      "(1736.117467711594, -64.67759177893589, 0.9248280261537454)\n",
      "(1736.117467711594, -64.67759177893589, 0.9248280261537454)\n",
      "(1752.903750494513, -35.38204884791239, 1.1761080261537453)\n",
      "(1761.8236147836926, -3.8919244107487785, 1.4134280261537453)\n",
      "(1763.1213505151761, 28.638670368459525, 1.6484213594870787)\n",
      "(1757.0885604958635, 59.928170950400585, 1.8741080261537453)\n",
      "(1757.0885604958635, 59.928170950400585, 1.8741080261537453)\n",
      "(1743.8942080568381, 89.50218285017965, 2.106774692820412)\n",
      "(1724.025030105259, 115.51377846501202, 2.3394413594870787)\n",
      "(1692.9287812992904, 145.85248345110983, 2.3976080261537454)\n",
      "(1692.9287812992904, 145.85248345110983, 2.3976080261537454)\n",
      "(1660.1535651301438, 175.94919121286694, 2.399934692820412)\n",
      "(1627.2146837375851, 206.12563362424063, 2.399934692820412)\n",
      "(1594.018467334147, 236.53782949195318, 2.399934692820412)\n",
      "(1561.3369209524678, 266.478518446988, 2.399934692820412)\n",
      "(1561.3369209524678, 266.478518446988, 2.399934692820412)\n",
      "(1528.1407045490298, 296.8907143147006, 2.399934692820412)\n",
      "(1494.9444881455918, 327.30291018241314, 2.399934692820412)\n",
      "(1462.0056067530331, 357.4793525937868, 2.399934692820412)\n",
      "(1428.809390349595, 387.8915484614994, 2.399934692820412)\n",
      "(1428.809390349595, 387.8915484614994, 2.399934692820412)\n",
      "(1393.9789883157475, 416.9396925819013, 2.493001359487079)\n",
      "(1356.9448000148802, 442.19836331068194, 2.5930480261537454)\n",
      "(1317.572968731011, 463.6317590156, 2.693094692820412)\n",
      "(1317.572968731011, 463.6317590156, 2.693094692820412)\n",
      "(1276.4374083451423, 481.0060428661219, 2.790814692820412)\n",
      "(1233.4699019052846, 494.3877995923645, 2.8885346928204116)\n",
      "(1189.9045915667898, 503.3566472883954, 2.9885813594870783)\n",
      "(1189.9045915667898, 503.3566472883954, 2.9885813594870783)\n",
      "(1145.466430214796, 507.68840338086835, 3.100261359487078)\n",
      "(1100.767108374667, 505.1130222042094, 3.2980280261537445)\n",
      "(1057.7822207215718, 493.8933147758615, 3.4957946928204113)\n",
      "(1020.1287549376349, 476.47362559539835, 3.6540080261537446)\n",
      "(1020.1287549376349, 476.47362559539835, 3.6540080261537446)\n",
      "(986.0508209883072, 454.0387800944851, 3.793608026153745)\n",
      "(955.8505766266101, 427.3915404367727, 3.9355346928204114)\n",
      "(929.119494325939, 395.6600657289169, 4.089094692820411)\n",
      "(905.1316457576605, 360.06865423333545, 4.149588026153745)\n",
      "(905.1316457576605, 360.06865423333545, 4.149588026153745)\n",
      "(881.2965770031933, 322.2866865573419, 4.149588026153745)\n",
      "(857.4615082487261, 284.50471888134837, 4.149588026153745)\n",
      "(833.8126509689032, 247.0179228278235, 4.149588026153745)\n",
      "(833.8126509689032, 247.0179228278235, 4.149588026153745)\n",
      "(809.742754835992, 208.7651791492408, 4.151914692820411)\n",
      "(786.1811807663655, 171.22346160247014, 4.151914692820411)\n",
      "(762.2485582861938, 133.09053590535666, 4.151914692820411)\n",
      "(738.315935806022, 94.95761020824318, 4.151914692820411)\n",
      "(738.315935806022, 94.95761020824318, 4.151914692820411)\n",
      "(714.5688375311229, 57.12028858630112, 4.151914692820411)\n",
      "(692.1278002316806, 17.904162775869708, 4.233348026153744)\n",
      "(673.5054119473032, -22.68163180996848, 4.331068026153744)\n",
      "(673.5054119473032, -22.68163180996848, 4.331068026153744)\n",
      "(659.0377297137518, -64.74185844598328, 4.431114692820411)\n",
      "(648.6834505310344, -108.71592930344558, 4.531161359487077)\n",
      "(642.8359653282739, -152.6337211130442, 4.628881359487077)\n",
      "(642.8359653282739, -152.6337211130442, 4.628881359487077)\n",
      "(642.951933057368, -197.598984376982, 4.8010546928204105)\n",
      "(651.2168623731883, -241.42745140791016, 4.99649469282041)\n",
      "(666.4938511389378, -280.9287433962919, 5.166341359487077)\n",
      "(687.0045501280167, -317.4152950916473, 5.28267469282041)\n",
      "(687.0045501280167, -317.4152950916473, 5.28267469282041)\n",
      "(711.3101530247052, -355.311576028284, 5.28267469282041)\n",
      "(735.2389248687318, -392.62031772559294, 5.28267469282041)\n",
      "(760.7901121246213, -429.46422687514826, 5.354801359487077)\n",
      "(789.1074278110009, -463.7666479494404, 5.45019469282041)\n",
      "(789.1074278110009, -463.7666479494404, 5.45019469282041)\n",
      "(821.0423473721216, -495.47282669057546, 5.552568026153743)\n",
      "(856.153602190658, -523.9004504083359, 5.65261469282041)\n",
      "(893.4632798985535, -548.4358945027664, 5.7503346928204095)\n",
      "(893.4632798985535, -548.4358945027664, 5.7503346928204095)\n",
      "(933.1666247849286, -569.2487740469962, 5.850381359487076)\n",
      "(974.2463182797208, -585.8446862847036, 5.948101359487076)\n",
      "(1017.9346263851371, -598.6471701849199, 6.048148026153743)\n",
      "(1061.1315201422622, -606.7836775206599, 6.145868026153742)\n",
      "(1061.1315201422622, -606.7836775206599, 6.145868026153742)\n",
      "(1105.9586563309522, -610.7593389574783, 6.243588026153742)\n",
      "(1151.3058012481713, -610.2337698767078, 0.0627760523074894)\n",
      "(1196.0287534687066, -605.2202314636417, 0.16049605230748937)\n",
      "(1196.0287534687066, -605.2202314636417, 0.16049605230748937)\n",
      "(1239.5257144839893, -595.9255622216015, 0.26054271897415604)\n",
      "(1282.2089845316839, -582.2262918715196, 0.3605893856408227)\n",
      "(1323.492125599068, -564.3103691671197, 0.4583093856408227)\n",
      "(1362.2204590729482, -542.7910358372114, 0.5560293856408226)\n",
      "(1362.2204590729482, -542.7910358372114, 0.5560293856408226)\n",
      "(1399.3683625915776, -517.9935528895861, 0.6211760523074893)\n",
      "(1435.867623139351, -491.9357012047258, 0.6188493856408226)\n",
      "(1472.539319547395, -465.81909492526717, 0.6188493856408226)\n",
      "(1472.539319547395, -465.81909492526717, 0.6188493856408226)\n",
      "(1508.9267392390975, -439.90494295805246, 0.6188493856408226)\n",
      "(1545.4259997868703, -413.84709127319184, 0.6211760523074893)\n",
      "(1581.925260334643, -387.78923958833184, 0.6188493856408226)\n",
      "(1618.8812334590284, -361.4701789966294, 0.6188493856408226)\n",
      "(1618.8812334590284, -361.4701789966294, 0.6188493856408226)\n",
      "(1654.1811748457276, -333.834276676194, 0.7095893856408226)\n",
      "(1686.0893512687599, -303.6011382475496, 0.8073093856408227)\n",
      "(1715.655506766817, -269.4430871580119, 0.9073560523074893)\n",
      "(1715.655506766817, -269.4430871580119, 0.9073560523074893)\n",
      "(1741.6049922399723, -232.6748641057881, 1.0050760523074893)\n",
      "(1764.395333939598, -194.2615926778656, 1.0655693856408226)\n",
      "(1786.1469446245355, -155.04329226115806, 1.063242718974156)\n",
      "(1807.7292820745715, -116.13019262590569, 1.0655693856408226)\n",
      "(1807.7292820745715, -116.13019262590569, 1.0655693856408226)\n",
      "(1829.3507999367343, -77.0393043098777, 1.0655693856408226)\n",
      "(1851.1024106216719, -37.8210038931702, 1.063242718974156)\n",
      "(1872.854021306608, 1.3972965235370793, 1.0655693856408226)\n",
      "(1872.854021306608, 1.3972965235370793, 1.0655693856408226)\n",
      "(1894.9441784613482, 41.22599850315055, 1.063242718974156)\n",
      "(1916.4869609767884, 79.96147127027243, 1.063242718974156)\n",
      "(1938.2385716617246, 119.17977168698053, 1.0655693856408226)\n",
      "(1938.2385716617246, 119.17977168698053, 1.0655693856408226)\n",
      "(1960.0290076321855, 158.5760575679775, 1.0655693856408226)\n",
      "(1981.7806183171233, 197.79435798468512, 1.063242718974156)\n",
      "(2003.5322290020595, 237.0126584013924, 1.0655693856408226)\n",
      "(2024.4374735124857, 274.7049549108342, 1.063242718974156)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n",
      "(2027.5690283581766, 280.3511693677142, 1.0655693856408226)\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Draw result.\n",
    "for pose in filtered:\n",
    "    print (pose)\n",
    "    plot([p[0] for p in filtered], [p[1] for p in filtered], 'bo')\n",
    "show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}