diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Data/measurement.txt b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Data/measurement.txt new file mode 100644 index 0000000..dc4315c --- /dev/null +++ b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Data/measurement.txt @@ -0,0 +1,413 @@ +1686487 5110 5110 2320 2360 5110 5110 2160 1190 +1686915 5110 5110 2320 2360 5110 5110 2160 1190 +1687132 5110 5110 2320 2360 5110 5110 2160 1190 +1687555 5110 5110 2320 2360 5110 5110 2160 1190 +1687770 5110 5110 2320 2360 5110 5110 2160 1190 +1688194 5110 5110 2320 2360 5110 5110 2170 1190 +1688411 5110 5110 2320 2360 5110 5110 2160 1180 +1688627 5110 5110 1040 2360 5110 5110 2160 1180 +1688843 5110 5110 1040 2360 5110 5110 2170 1190 +1689054 5110 5110 2320 2370 5110 5110 2170 1190 +1689273 5110 5110 2320 2360 5110 5110 2160 1180 +1689479 5110 5110 2320 2360 5110 5110 2170 1190 +1689698 5110 5110 2320 2370 5110 5110 2170 1190 +1689906 5110 5110 2320 2370 5110 5110 2160 1180 +1690116 5110 5110 2320 2360 5110 5110 2160 1180 +1690338 5110 5110 2320 2360 5110 5110 2160 1190 +1690549 5110 5110 2320 2360 5110 5110 2160 1190 +1690759 5110 5110 2320 2360 5110 5110 2160 1190 +1690970 5110 5110 2320 2350 5110 5110 2160 1190 +1691192 5110 5110 2320 2350 5110 5110 2160 1190 +1691400 5110 5110 2320 2360 5110 5110 2160 1190 +1691822 5110 5110 2320 2350 5110 5110 2160 1190 +1692039 5110 5110 2320 2350 5110 5110 2160 1190 +1692258 5110 5110 2320 400 5110 5110 2160 1190 +1692470 5110 5110 2320 2360 5110 5110 2170 1190 +1692681 5110 5110 2320 2360 5110 5110 2160 1180 +1692900 5110 5110 2320 2350 5110 5110 2160 1180 +1693105 5110 5110 2320 2350 5110 5110 2170 1190 +1693319 5110 5110 2320 2360 5110 5110 2160 1190 +1693530 5110 5110 2320 2360 5110 5110 2160 1180 +1693751 5110 5110 2320 2360 5110 5110 2170 1180 +1693959 5110 5110 2320 2360 5110 5110 2170 1190 +1694178 5110 5110 2320 2360 5110 5110 2160 1190 +1694387 5110 5110 2320 2360 5110 5110 2160 1190 +1694811 5110 5110 2320 2350 5110 5110 2160 1190 +1695028 5110 5110 2320 2360 5110 5110 2160 1190 +1695243 5110 5110 2320 2360 5110 5110 2160 1190 +1695451 5110 5110 2320 2360 5110 5110 2160 1190 +1695665 5110 5110 2320 2360 5110 5110 2170 1190 +1695888 5110 5110 2320 2350 5110 5110 2170 1190 +1696098 5110 5110 2320 2350 5110 5110 2160 1180 +1696314 5110 5110 2320 2360 5110 5110 2160 1180 +1696524 5110 5110 2320 2360 5110 5110 2160 1190 +1696740 5110 5110 2320 2360 5110 5110 2160 1190 +1696952 5110 5110 2320 2360 5110 5110 2160 1180 +1697168 5110 5110 2320 2360 5110 5110 2160 1180 +1697377 5110 5110 2320 2360 5110 5110 2170 1190 +1697590 5110 5110 2310 2360 5110 5110 2170 1190 +1697802 5110 5110 2310 2360 5110 5110 2150 1180 +1698013 5110 5110 2290 2310 5060 5110 2150 1180 +1698233 5110 5110 2290 2310 5060 5110 2100 1190 +1698442 5110 2240 2210 2240 4970 5110 2100 1190 +1698652 5110 2240 2210 2240 4970 4950 2000 1260 +1698868 860 2120 2100 4260 4860 4830 1910 1260 +1699087 860 2010 2000 4140 5110 4830 1910 1260 +1699293 870 2010 2000 4140 5110 4700 1830 1260 +1699515 870 1900 1890 4020 5110 4700 1830 1260 +1699721 860 1900 1890 4020 5110 4590 1740 1260 +1699939 860 1800 1810 3910 5110 4480 1740 1260 +1700150 870 1700 1810 3910 5110 4480 5110 1220 +1700362 870 1700 2700 3800 5110 4370 5110 1220 +1700578 870 1600 2700 3800 5110 4370 5080 1180 +1700788 870 1600 2600 3680 5110 4250 5080 1180 +1701000 860 1500 2600 3680 5110 4250 5110 1180 +1701218 860 1500 2480 3570 5110 4140 5110 1180 +1701435 830 1410 2480 3570 5110 4140 5110 1180 +1701645 830 1410 2380 3450 5110 4030 5110 1180 +1701857 920 1320 2380 3450 5110 4030 5110 1260 +1702070 920 1320 2270 2290 5110 3900 5110 1260 +1702289 920 5010 2150 2170 5110 3790 5110 1270 +1702500 920 2070 2050 2170 5110 3790 5110 1270 +1702713 920 2070 2050 2170 5110 2270 2390 1270 +1702928 920 4800 1960 2170 5110 2270 2390 1270 +1703144 920 4800 1960 1990 5110 3620 1670 1270 +1703349 910 4720 1880 1990 5110 3620 1670 1220 +1703566 910 4720 1880 1910 5110 3470 5110 1220 +1703775 900 1810 1880 1910 5110 3470 5110 1210 +1703990 900 1810 1880 2800 5110 3410 5110 1210 +1704210 890 1700 1680 2800 5110 3410 5110 1210 +1704423 890 1700 1680 2800 5110 3340 5110 1220 +1704637 880 1590 1580 2600 5110 3340 5110 1220 +1704845 880 1590 1580 2600 5110 3210 3220 1220 +1705065 860 1480 2480 3550 5110 3210 3220 1220 +1705272 860 1480 2480 3550 5110 3130 3110 1230 +1705488 820 1380 2370 3450 5110 3020 3000 1230 +1705695 840 1300 2260 3320 5110 3020 3000 1230 +1705912 840 1300 2260 3320 5110 2900 2890 1240 +1706122 860 1200 2150 3220 5110 2900 2890 1240 +1706339 860 1200 2150 3220 5110 2820 2780 1260 +1706549 860 1140 2030 2070 5110 2820 2780 1260 +1706767 850 1140 2030 2070 5110 2820 2680 1250 +1706977 850 5110 1930 1960 5110 2820 2680 1250 +1707189 850 5110 1930 1960 5110 5110 2570 1260 +1707409 850 1850 1820 2870 5110 5110 2570 1260 +1707616 840 1850 1820 2870 5110 5110 2470 1260 +1707834 840 1740 1730 2780 5110 5110 2470 1260 +1708047 840 1740 1730 2780 5110 5110 2390 1260 +1708261 840 1640 1630 2680 5110 5110 2390 1260 +1708478 840 1550 1630 2580 5110 5110 2290 1260 +1708688 790 1440 1630 2580 5110 5110 2200 1270 +1708902 790 1440 2440 3120 5110 5010 2200 1270 +1709118 780 1340 2440 3120 5110 5010 2090 1270 +1709329 780 1340 2330 3000 5110 4900 2090 1270 +1709540 820 1250 2330 3000 5110 4900 2020 1270 +1709759 820 1250 2230 3610 5110 4780 2020 1270 +1709968 830 1170 2230 3610 5110 4780 1950 1280 +1710181 830 1170 2120 3490 5110 4670 1950 1280 +1710391 820 5110 2120 3490 5110 4670 1900 1280 +1710611 820 5110 2010 2030 5110 4550 1900 1280 +1710820 820 5110 2010 2030 5110 4550 5110 1280 +1711032 820 5110 1900 1920 5110 4450 5110 1280 +1711247 820 1850 1820 1920 5110 4450 5110 1280 +1711464 820 1850 1820 3190 5110 4340 5110 1280 +1711882 810 1630 1620 3110 5110 4230 5110 1280 +1712100 810 1630 1620 3000 5110 4120 5110 1280 +1712317 810 1520 1620 3000 5110 4120 5110 1330 +1712525 810 1520 1620 2910 5110 4020 5110 1330 +1712739 770 1410 2010 2910 5110 4020 5110 1370 +1712952 770 1410 2010 2790 5110 3910 5110 1370 +1713170 760 1310 1360 2690 5110 3910 5110 1380 +1713385 760 1310 1360 2690 5110 3800 5110 1380 +1713599 810 1230 1800 5110 5110 3800 5110 1380 +1713813 810 1230 1800 5110 5110 3690 5110 1360 +1714021 800 1140 1710 5110 5110 3690 5110 1360 +1714235 800 1140 1710 5110 5110 3590 5110 1320 +1714452 800 1080 1610 5110 5110 3590 5110 1320 +1714657 800 1080 1610 5110 5110 3480 5110 1310 +1714872 800 1520 1610 5110 5110 3370 3390 1310 +1715087 800 1520 2040 4990 5110 3370 3390 1310 +1715303 790 1520 2040 4990 5110 3270 3390 1310 +1715516 790 1310 1940 4880 5110 3270 3390 1310 +1715731 790 1310 1940 4880 5110 3160 3170 1320 +1715939 790 1220 1840 4780 5110 3160 3170 1320 +1716156 790 1220 1840 4780 5110 3080 3060 1320 +1716366 790 1130 1730 1830 5110 3080 3060 1320 +1716584 730 1130 1730 1830 5110 2970 2950 1330 +1716800 730 1050 1620 4540 5110 2970 2950 1330 +1717008 730 1050 1620 4540 5110 2890 2840 1330 +1717218 730 1550 1530 4540 5110 2890 2840 1330 +1717436 760 1550 1530 4540 5110 2890 2740 1340 +1717655 760 1430 1440 4310 5110 5110 2740 1340 +1718071 750 1240 1390 4190 5110 5110 2540 1350 +1718293 750 1240 5110 4090 5110 5110 2540 1350 +1718503 750 1150 5110 4090 5110 5110 2440 1350 +1718716 750 1150 5110 3970 5110 5110 2440 1350 +1718928 730 1070 5110 3970 5110 5110 2350 1350 +1719139 730 1070 5110 3850 5110 5110 2350 1350 +1719359 720 1020 5110 3850 5110 5110 2260 1360 +1719567 720 1020 5110 3730 5110 5110 2260 1360 +1719787 720 980 5110 3730 5110 5110 2190 1360 +1719994 720 980 5110 3620 5110 5110 2090 1360 +1720215 730 5110 5110 3620 5110 5110 2090 1370 +1720429 730 5110 5110 3510 5110 4850 2030 1370 +1720641 740 5110 3440 3510 5110 4850 2030 1360 +1720852 740 5110 3440 3390 5110 4730 5110 1360 +1721063 730 5110 3300 3390 5110 4730 5110 1370 +1721275 730 5110 3180 3270 5110 4610 5110 1370 +1721699 670 5110 3070 3150 5110 4500 5110 1380 +1721921 670 5110 3070 3030 5110 4400 5110 1390 +1722339 660 5110 2940 2910 5110 4400 5110 1400 +1722561 660 5010 2820 2800 5110 4400 5110 1400 +1722769 660 5010 2820 2800 5110 4180 5110 1430 +1722982 660 4890 2700 2680 5110 4180 5110 1430 +1723203 660 4890 2700 2680 5110 4070 5110 1470 +1723416 650 4780 2590 2570 5110 4070 5110 1470 +1723627 650 4780 2590 2570 5110 3960 5110 1490 +1723839 650 4690 2470 2570 5110 3960 5110 1490 +1724051 650 4690 2470 2570 5110 3860 5110 1490 +1724271 630 4600 2350 2360 5110 3730 5110 1440 +1724480 630 4500 2230 2240 5110 3730 5110 1440 +1724696 630 4500 2230 2240 5110 3730 5110 1420 +1724905 630 4400 2110 2140 5110 3730 5110 1420 +1725117 620 4400 2110 2140 5110 3550 5110 1420 +1725337 620 2080 2020 5110 5110 3550 5110 1420 +1725544 620 2080 2020 5110 5110 3430 5110 1430 +1725755 620 4260 1910 1950 5110 3430 5110 1430 +1725975 620 4260 1910 1950 5110 3340 5110 1420 +1726185 620 1880 1840 1900 5110 3340 5110 1420 +1726403 640 1780 1840 1900 5110 3260 5110 1420 +1726612 640 1780 1840 5110 5110 3190 5110 1420 +1726824 650 1700 1840 5110 5110 3190 5110 1400 +1727247 690 1620 5110 5110 5110 3120 5110 1390 +1727467 700 1520 1640 5110 5110 3000 5110 1370 +1727680 700 1520 1560 5110 5110 2880 5110 1370 +1727890 710 1440 1560 5110 5110 2880 5110 1350 +1728104 710 1440 5110 5110 5110 2790 5110 1350 +1728322 730 1350 5110 5110 5110 2790 5110 1330 +1728531 730 1350 5110 5110 5110 2680 5110 1330 +1728748 740 1290 5110 5110 5110 2680 5110 900 +1728962 740 1290 5110 5110 5110 2570 3140 900 +1729173 750 5110 5110 5110 5110 2570 3140 740 +1729390 750 5110 5110 5110 5110 890 3030 740 +1729808 780 5110 5110 5110 5110 840 2920 1290 +1730032 860 5110 5110 5110 5110 840 2920 1290 +1730238 860 5110 5110 5110 5110 2270 2240 1290 +1730456 870 5110 5110 5110 5110 2270 2240 1270 +1730664 870 5110 5110 1770 1290 5110 2120 1270 +1730878 890 5110 5110 1640 4930 5110 2040 1250 +1731094 890 5020 5110 2220 4930 5110 2040 1250 +1731307 890 5020 5110 2220 4820 5110 1920 1240 +1731521 880 3610 2800 5110 4820 5110 1920 1240 +1731731 880 3610 2800 5110 4710 5110 1830 1230 +1731943 850 4860 2880 5110 4710 5110 1830 1230 +1732160 850 4860 2880 5110 4710 4580 1750 1220 +1732378 850 4760 5110 5110 4710 4580 1750 1220 +1732589 850 4760 5110 5110 5110 4460 1650 1210 +1732800 870 4680 5110 5110 5110 4460 1650 1210 +1733019 2850 4680 5110 5110 5110 4350 5110 1200 +1733232 2850 4560 5110 5110 5110 4350 5110 1200 +1733443 2860 4560 5110 5110 5110 4220 5110 1200 +1733651 2860 4520 5110 5110 5110 4220 5110 1200 +1734076 2880 4430 5110 5110 5110 3940 5110 1350 +1734300 2880 5110 5110 5110 5110 3940 5110 1350 +1734717 2880 5110 5110 5110 5110 3830 5110 2320 +1734932 2890 5110 5110 5110 5110 3700 5110 2280 +1735146 2890 5110 5110 5110 5110 3580 5110 2280 +1735364 2890 5110 5110 5110 5110 3580 5110 2030 +1735577 2890 5110 5110 5110 5110 3460 5110 2030 +1735788 2890 5110 5110 5110 5110 3460 5110 1170 +1736006 2890 5110 5110 5110 5110 3340 5110 1170 +1736214 2900 5110 5110 5110 5110 3340 5110 1150 +1736425 2900 5110 5110 5110 5110 3240 5110 1150 +1736647 2900 5110 5110 5110 5110 3240 5110 1120 +1736854 2900 5110 5110 5110 5110 3130 5110 1120 +1737069 2910 5110 5110 5110 5110 3130 5110 1150 +1737280 2900 5110 5110 5110 5110 3010 5110 1160 +1737495 2900 5110 5110 5110 5110 2900 5110 1160 +1737708 2910 5110 5110 5110 5110 2900 5110 1160 +1737924 2910 5110 5110 5110 5110 2800 5110 1160 +1738140 2900 5110 5110 5110 5110 2800 5110 1160 +1738350 2900 5110 5110 5110 5110 2690 5110 1160 +1738567 2910 5110 5110 5110 5110 2690 5110 1160 +1738775 2910 5110 5110 5110 5110 2580 2570 1160 +1738986 2900 5110 5110 5110 5110 2580 2570 1150 +1739208 2900 5110 5110 5110 5110 1200 2460 1150 +1739419 2910 5040 5110 5110 5110 1200 2460 1160 +1739631 2910 5040 5110 5110 5110 2370 2340 1160 +1739843 2910 4920 5110 5110 5110 2370 2340 1160 +1740058 2910 4920 5110 5110 5110 2370 2230 1160 +1740271 2900 4810 5110 5110 5110 5110 2130 1160 +1740483 2900 4710 5110 5110 5110 5110 2130 1160 +1740693 2900 4710 5110 5110 5110 5110 2030 1160 +1740912 2890 4600 5110 5110 5110 5110 2030 1160 +1741123 2890 4600 5110 5110 5110 5110 1920 1170 +1741338 2900 4520 5110 5110 5110 5110 1920 1170 +1741554 2900 4520 5110 5110 5110 5110 1840 1160 +1741763 2900 4450 5110 5110 5110 5110 1840 1160 +1741976 2920 4450 5110 5110 5110 5110 1770 1170 +1742188 2920 4330 5110 5110 5110 5110 1770 1170 +1742405 2970 4330 5110 5110 5110 4560 5110 1170 +1742615 2970 4240 5110 5110 5110 4560 5110 1170 +1742834 2970 4240 5110 5110 5110 4560 5110 1170 +1743043 2970 4140 5110 5110 5110 4560 5110 1170 +1743254 2950 4140 5110 5110 5110 5110 5110 1170 +1743467 2950 4080 5110 5110 5110 5110 5110 1170 +1743688 2910 4000 5110 5110 5110 5110 5110 1190 +1743896 2890 3920 5110 5110 5110 5110 5110 1240 +1744112 2890 3920 5110 5110 5110 4000 5110 1240 +1744329 2880 3810 5110 5110 5110 4000 5110 1280 +1744535 2880 3810 5110 5110 5110 4000 5110 1280 +1744757 2940 3760 5110 5110 5110 4000 5110 1320 +1744967 2940 3760 5110 5110 5110 5110 5110 1320 +1745180 3040 3720 5110 5110 5110 5110 3820 1330 +1745392 3040 3720 5110 5110 5110 5110 3820 1330 +1745605 3020 5110 5110 5110 5110 5110 3790 5110 +1745822 3020 5110 3650 5110 2420 5110 3790 5110 +1746031 3010 5110 3650 5110 2420 5110 3790 5110 +1746251 3010 5110 3580 5110 2340 5110 5110 5110 +1746459 3000 5110 3510 5110 2340 5110 5110 5110 +1746677 3000 5110 4900 3500 5110 5110 5110 5110 +1746887 3000 5110 4900 3400 5110 5110 5110 5110 +1747096 5110 5110 4830 3400 5110 5110 5110 1720 +1747313 5110 5110 4830 3300 5110 5110 5110 1720 +1747522 5110 4320 5110 3300 5110 5110 5110 1720 +1747736 5110 4320 5110 3220 5110 5110 5110 1720 +1747952 5110 4250 5110 3220 5110 5110 5110 5110 +1748171 5110 4250 5110 3110 5110 5110 1860 5110 +1748379 4940 4160 5110 4540 5110 5110 1860 3500 +1748598 4940 4160 5110 4540 3010 5110 1780 3460 +1748805 4950 4090 5110 4430 3010 5110 1780 3460 +1749024 4950 4090 5110 4430 2870 4710 1700 3460 +1749234 4950 4000 5110 4330 2870 4710 1700 3460 +1749442 4950 4000 5110 4330 2760 5110 1700 3470 +1749655 4960 3930 5110 4330 2760 5110 1700 3470 +1749872 4960 3830 5110 5070 2660 5110 5110 3530 +1750093 4960 3830 5110 5070 2550 5110 5110 3530 +1750300 4960 3750 3760 4930 2550 5110 5110 3530 +1750517 4980 3750 3760 4930 2450 2460 4300 5110 +1750727 4980 3670 3660 4830 2330 2460 4300 5110 +1750945 4990 3670 3660 4830 2330 2460 5110 5110 +1751156 4990 3610 3550 4740 3730 2460 5110 5110 +1751369 4990 3610 3550 4740 3730 2220 5100 5110 +1751579 4990 3540 3460 5110 3610 2220 5100 5110 +1751799 5000 3540 3460 5110 3610 2080 5110 5110 +1752004 5000 3440 5110 5110 3500 2080 5110 5110 +1752218 5010 3440 5110 5110 3500 1970 1430 5110 +1752430 5010 3360 4380 5110 3370 1970 1430 5110 +1752653 5020 3360 4380 5110 3370 1860 4970 2900 +1752863 5020 3250 4270 5110 3250 1740 4970 2900 +1753078 2470 3160 4160 5110 3120 1630 4840 1160 +1753291 2430 3070 4160 5110 3120 1630 1670 1120 +1753497 2430 3070 3110 5110 3010 1560 1670 1120 +1753712 2410 2980 3110 5110 3010 1560 1530 2540 +1753931 2410 2980 2990 5110 2900 5110 1530 2540 +1754147 2390 2890 2990 5110 2900 5110 1410 1040 +1754359 2390 2890 2740 5110 2800 2790 1410 1040 +1754564 2380 2810 2740 5110 2800 2790 1320 2060 +1754779 2380 2810 3740 5110 2700 2680 1320 2060 +1754997 2350 2720 3640 5110 2700 2680 1220 980 +1755208 2350 2720 3640 5110 3540 2570 1150 980 +1755418 2350 2630 3540 5110 3540 2570 1150 980 +1755633 2350 2630 3540 5110 3440 2450 4020 980 +1755846 1340 2550 3450 5110 3440 2450 4020 1000 +1756067 1340 2550 3450 5110 3250 2330 3950 1000 +1756278 1320 2460 3320 5110 3130 2210 3860 1000 +1756488 1050 2370 3210 5110 3130 2210 3860 960 +1756702 1050 2370 3210 5110 3000 2110 3790 960 +1756913 1330 2050 3110 5110 3000 2110 3790 950 +1757128 1330 2050 3110 5110 2890 2010 2330 880 +1757348 1000 2230 3010 5110 2890 2010 2330 880 +1757559 1000 2230 3010 5110 2780 640 5110 800 +1757771 1350 1860 2900 5110 2780 640 5110 800 +1757985 1350 1860 2900 5110 2670 1820 1810 790 +1758404 1360 1070 2810 5110 2670 2580 1680 770 +1758623 1370 5110 1250 5110 2670 2580 1680 770 +1759045 1380 5110 1250 5110 5110 2470 1580 760 +1759260 1380 5110 1250 5110 5110 2350 1490 750 +1759475 1400 1470 1600 5110 5110 2230 1430 750 +1759692 1400 2200 2040 5110 5110 2230 1430 750 +1759904 1420 2200 2040 5110 5110 2090 1350 990 +1760117 1420 2410 2180 2310 3040 2090 1350 990 +1760331 1420 2410 2180 2310 3040 1960 2050 990 +1760544 1420 2180 2160 3060 2920 1960 2050 990 +1760754 1430 2180 2160 3060 2920 1850 1920 980 +1761179 1450 1560 5110 2970 2800 1740 1820 960 +1761606 1460 1970 5110 2970 2680 1620 1710 960 +1761821 1460 1860 5080 5110 2560 1500 1710 960 +1762041 1470 1770 5080 5110 2560 1500 1710 950 +1762253 1470 1770 4950 5110 2460 1400 1710 950 +1762463 1480 1680 4840 5110 3570 1300 1510 940 +1762886 910 1590 4730 5110 3440 5110 1420 930 +1763108 900 1520 4730 5110 3440 5110 1310 920 +1763315 900 1520 4640 5110 3470 4300 1310 920 +1763530 910 1430 4640 5110 3470 4300 1080 930 +1763742 910 1430 4530 5110 3470 1990 1080 930 +1763960 910 4650 4440 5110 3470 1990 980 920 +1764170 910 4650 4440 5110 5110 1880 980 920 +1764388 910 4650 4320 5110 5110 1880 980 920 +1764603 910 4650 4320 5110 5110 1770 3140 920 +1764811 910 5110 4210 5110 5110 1770 3140 920 +1765025 910 5110 4210 4870 5110 1680 3030 920 +1765242 900 5110 4100 4870 5110 1680 3030 920 +1765450 900 5110 4100 4750 5110 3280 2930 920 +1765669 900 5110 3980 4630 5110 3150 2860 940 +1765881 890 5110 3880 4630 5110 3150 2860 940 +1766097 890 5110 3880 4520 5110 3050 1350 650 +1766308 900 5110 3770 4390 5110 3050 1350 650 +1766518 900 5110 3770 4390 5110 2930 1240 640 +1766735 890 5110 3660 4280 5110 2930 1240 640 +1766948 890 5110 3660 4280 5110 2810 1140 650 +1767161 890 5110 3540 4170 5110 2810 1140 650 +1767372 890 5110 3540 4170 5110 2700 1040 4160 +1767582 890 5110 3440 4050 5110 2700 1040 4160 +1767796 890 5110 3440 4050 5110 2670 970 3120 +1768222 930 3370 3330 3930 5110 2500 2520 4230 +1768443 940 3260 3230 3820 5110 2500 2520 4230 +1768861 1490 3140 3130 3710 5110 2260 2210 4240 +1769085 1490 2990 3020 3590 5110 2260 2210 4240 +1769298 1490 2990 3020 3590 5110 2160 2020 4220 +1769504 1490 2920 2940 3500 5110 2160 2020 4220 +1769724 1490 2920 2940 3500 5110 2050 1880 660 +1769933 1490 2820 3350 3500 5110 2050 1880 660 +1770148 1490 2820 3350 3500 5110 1940 1930 640 +1770361 1490 2720 3240 4220 5110 1940 1930 640 +1770569 1490 2620 3240 4220 5110 1830 1930 650 +1770787 1490 2620 3110 4110 5110 1730 1930 650 +1771003 1490 2530 3110 4110 5110 1730 1720 640 +1771213 1490 2530 3000 5110 5110 1630 1720 640 +1771427 1490 2470 3000 5110 5110 1630 1720 650 +1771643 1490 2470 2880 5110 5110 1530 1720 650 +1771852 1500 2350 2880 5110 5110 1530 1520 1980 +1772067 1500 2270 2730 5110 5110 1430 1520 1210 +1772282 1500 2270 2600 5110 5110 1320 1520 1210 +1772500 1510 2210 2600 5110 5110 1320 1300 1190 +1772710 1510 2210 2570 5110 5110 1320 1220 1190 +1772921 1510 2740 2380 5110 5110 1320 1220 1210 +1773131 1510 2740 2380 5110 5110 5110 1100 1210 +1773344 1510 2640 2340 5110 5110 5110 1100 1240 +1773567 1510 2640 2340 5110 5110 5110 1000 1240 +1773777 1510 2540 2240 5110 5110 5110 1000 1240 +1773986 1510 2540 2240 5110 5110 4370 910 1240 +1774197 1520 2440 2130 5110 5110 4370 910 1250 +1774410 1520 2440 2130 5110 5110 4240 3590 1250 +1774632 1510 2060 2030 5110 5110 4240 3590 1240 +1774838 1510 2060 2030 5110 5110 4120 3500 1240 +1775264 1510 1830 1820 5110 5110 3990 3410 1190 +1775486 1510 1830 1820 5110 5110 3870 3390 1200 +1775696 1510 1730 2410 5110 5110 3870 3390 1200 +1775904 1510 1730 2410 5110 5110 3750 3250 630 +1776119 1520 1620 2290 5110 5110 3750 3250 630 +1776339 1520 1620 2290 5110 5110 3630 4780 620 +1776545 1520 1530 2190 5110 5110 3630 4780 620 +1776760 1520 1530 2190 5110 5110 3530 4700 620 +1776979 1510 1430 2100 5110 5110 3530 4700 620 +1777188 1510 1430 2100 5110 5110 3440 4620 620 +1777405 1510 1360 2010 5110 5110 3440 4620 620 +1777618 1500 1360 2010 5110 5110 3370 4570 640 +1777829 1500 1280 1930 3820 5110 3370 4570 640 +1778252 1400 1210 1830 3710 5110 3380 3170 680 +1778473 1400 1120 420 3610 5110 3380 3170 680 +1778892 1390 1060 1670 2570 5110 3290 3090 1280 +1779109 1380 1060 1670 2570 5110 3250 3050 5110 +1779324 1380 1040 1650 2550 5110 3240 3050 5110 diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Data/poses.txt b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Data/poses.txt new file mode 100644 index 0000000..881ec39 --- /dev/null +++ b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Data/poses.txt @@ -0,0 +1,413 @@ +1686487 0 0 0 +1686915 0 0 0 +1687132 0 0 0 +1687555 0 0 0 +1687770 0 0 0 +1688194 0 0 0 +1688411 0 0 0 +1688627 0 0 0 +1688843 0 0 0 +1689054 0 0 0 +1689273 0 0 0 +1689479 0 0 0 +1689698 0 0 0 +1689906 0 0 0 +1690116 0 0 0 +1690338 0 0 0 +1690549 0 0 0 +1690759 0 0 0 +1690970 0 0 0 +1691192 0 0 0 +1691400 0 0 0 +1691822 0 0 0 +1692039 0 0 0 +1692258 0 0 0 +1692470 0 0 0 +1692681 0 0 0 +1692900 0 0 0 +1693105 0 0 0 +1693319 0 0 0 +1693530 0 0 0 +1693751 0 0 0 +1693959 0 0 0 +1694178 0 0 0 +1694387 0 0 0 +1694811 0 0 0 +1695028 0 0 0 +1695243 0 0 0 +1695451 0 0 0 +1695665 0 0 0 +1695888 0 0 0 +1696098 0 0 0 +1696314 0 0 0 +1696524 0 0 0 +1696740 0 0 0 +1696952 0 0 0 +1697168 0 0 0 +1697377 0 0 0 +1697590 10 0 3600 +1697802 33 0 0 +1698013 71 0 0 +1698233 122 0 1 +1698442 184 0 0 +1698652 249 0 0 +1698868 379 0 0 +1699087 445 0 0 +1699293 511 0 3600 +1699515 574 0 3599 +1699721 636 0 3599 +1699939 701 0 3597 +1700150 767 0 3596 +1700362 829 0 3595 +1700578 888 -1 3595 +1700788 951 -1 3594 +1701000 1015 -2 3594 +1701218 1077 -2 3594 +1701435 1142 -3 3593 +1701645 1208 -4 3592 +1701857 1274 -5 3592 +1702070 1340 -6 3592 +1702289 1470 -7 3591 +1702500 1530 -8 3591 +1702713 1581 -10 3570 +1702928 1624 -13 3549 +1703144 1675 -18 3547 +1703349 1733 -23 3549 +1703566 1792 -28 3550 +1703775 1855 -34 3551 +1703990 1918 -39 3553 +1704210 1981 -44 3555 +1704423 2039 -49 3556 +1704637 2103 -54 3557 +1704845 2167 -58 3558 +1705065 2223 -62 3562 +1705272 2283 -66 3563 +1705488 2411 -74 3567 +1705695 2477 -78 3569 +1705912 2540 -81 3570 +1706122 2601 -84 3572 +1706339 2668 -87 3574 +1706549 2731 -90 3577 +1706767 2792 -92 3578 +1706977 2857 -95 3580 +1707189 2919 -97 3582 +1707409 2982 -99 3584 +1707616 3039 -100 3586 +1707834 3097 -102 3585 +1708047 3161 -103 3585 +1708261 3216 -105 3582 +1708478 3338 -109 3581 +1708688 3401 -111 3581 +1708902 3468 -113 3580 +1709118 3531 -115 3581 +1709329 3595 -117 3583 +1709540 3657 -119 3584 +1709759 3718 -121 3585 +1709968 3781 -122 3586 +1710181 3845 -124 3588 +1710391 3906 -125 3590 +1710611 3970 -126 3592 +1710820 4029 -127 3592 +1711032 4080 -127 3592 +1711247 4136 -128 3592 +1711464 4202 -129 3591 +1711882 4388 -132 3590 +1712100 4451 -134 3589 +1712317 4514 -135 3588 +1712525 4579 -136 3587 +1712739 4646 -138 3588 +1712952 4711 -139 3589 +1713170 4772 -140 3591 +1713385 4836 -141 3592 +1713599 4900 -142 3592 +1713813 4957 -143 3593 +1714021 5010 -143 3592 +1714235 5068 -144 3591 +1714452 5133 -145 3590 +1714657 5199 -147 3589 +1714872 5329 -150 3586 +1715087 5395 -151 3585 +1715303 5460 -153 3584 +1715516 5518 -155 3583 +1715731 5576 -156 3582 +1715939 5639 -159 3581 +1716156 5704 -161 3580 +1716366 5768 -163 3579 +1716584 5831 -165 3579 +1716800 5894 -167 3581 +1717008 5957 -170 3582 +1717218 6022 -172 3583 +1717436 6081 -173 3583 +1717655 6143 -175 3585 +1718071 6322 -180 3587 +1718293 6383 -181 3589 +1718503 6444 -182 3589 +1718716 6507 -183 3590 +1718928 6571 -184 3592 +1719139 6637 -185 3592 +1719359 6699 -186 3593 +1719567 6760 -187 3594 +1719787 6825 -187 3594 +1719994 6890 -188 3593 +1720215 6953 -189 3592 +1720429 7013 -190 3591 +1720641 7077 -191 3591 +1720852 7143 -192 3590 +1721063 7206 -193 3589 +1721275 7336 -196 3587 +1721699 7462 -199 3586 +1721921 7528 -200 3585 +1722339 7658 -204 3584 +1722561 7721 -206 3585 +1722769 7785 -207 3586 +1722982 7849 -209 3586 +1723203 7914 -210 3586 +1723416 7978 -212 3585 +1723627 8042 -214 3585 +1723839 8104 -215 3584 +1724051 8168 -217 3583 +1724271 8297 -221 3581 +1724480 8362 -224 3580 +1724696 8425 -226 3580 +1724905 8485 -228 3584 +1725117 8540 -229 3591 +1725337 8595 -230 0 +1725544 8648 -229 12 +1725755 8697 -227 28 +1725975 8740 -224 42 +1726185 8783 -221 56 +1726403 8832 -215 70 +1726612 8875 -209 87 +1726824 8920 -202 103 +1727247 9026 -180 118 +1727467 9148 -155 112 +1727680 9208 -143 106 +1727890 9268 -132 101 +1728104 9328 -122 94 +1728322 9391 -111 89 +1728531 9455 -102 82 +1728748 9517 -93 77 +1728962 9578 -85 75 +1729173 9637 -77 72 +1729390 9700 -69 72 +1729808 9821 -54 71 +1730032 9887 -45 69 +1730238 9952 -37 68 +1730456 10018 -29 68 +1730664 10083 -22 66 +1730878 10215 -7 61 +1731094 10278 0 58 +1731307 10341 6 57 +1731521 10407 12 56 +1731731 10473 19 55 +1731943 10539 25 54 +1732160 10605 31 53 +1732378 10671 38 51 +1732589 10738 44 51 +1732800 10804 50 50 +1733019 10870 55 50 +1733232 10936 61 49 +1733443 11002 67 48 +1733651 11068 73 47 +1734076 11264 88 43 +1734300 11329 94 43 +1734717 11461 103 41 +1734932 11527 108 41 +1735146 11593 113 39 +1735364 11658 118 38 +1735577 11724 122 36 +1735788 11790 126 36 +1736006 11854 130 34 +1736214 11920 134 33 +1736425 11986 138 32 +1736647 12052 142 31 +1736854 12118 145 30 +1737069 12184 149 29 +1737280 12315 156 28 +1737495 12381 159 27 +1737708 12447 162 26 +1737924 12514 165 25 +1738140 12580 168 25 +1738350 12646 171 24 +1738567 12712 174 24 +1738775 12777 177 23 +1738986 12843 180 22 +1739208 12909 182 21 +1739419 12974 185 21 +1739631 13040 187 21 +1739843 13105 190 20 +1740058 13170 192 19 +1740271 13299 196 17 +1740483 13364 198 16 +1740693 13429 200 17 +1740912 13493 202 16 +1741123 13559 204 15 +1741338 13624 206 14 +1741554 13689 208 14 +1741763 13756 209 14 +1741976 13822 211 13 +1742188 13887 213 12 +1742405 13953 214 11 +1742615 14019 215 11 +1742834 14084 217 10 +1743043 14150 218 9 +1743254 14216 219 8 +1743467 14281 220 9 +1743688 14408 223 12 +1743896 14468 224 0 +1744112 14516 223 3572 +1744329 14556 220 3542 +1744535 14593 215 3512 +1744757 14631 208 3481 +1744967 14668 199 3450 +1745180 14707 187 3422 +1745392 14750 173 3396 +1745605 14789 157 3368 +1745822 14827 139 3344 +1746031 14867 119 3321 +1746251 14906 97 3298 +1746459 14946 73 3275 +1746677 15023 20 3235 +1746887 15062 -9 3217 +1747096 15099 -40 3198 +1747313 15139 -75 3181 +1747522 15180 -112 3165 +1747736 15220 -152 3150 +1747952 15259 -191 3136 +1748171 15298 -233 3123 +1748379 15334 -274 3109 +1748598 15366 -312 3097 +1748805 15401 -354 3083 +1749024 15436 -401 3072 +1749234 15472 -448 3060 +1749442 15503 -493 3048 +1749655 15536 -542 3038 +1749872 15599 -639 3018 +1750093 15632 -694 3008 +1750300 15662 -746 2998 +1750517 15693 -800 2990 +1750727 15723 -855 2981 +1750945 15751 -909 2974 +1751156 15779 -963 2967 +1751369 15805 -1016 2960 +1751579 15831 -1071 2954 +1751799 15857 -1127 2948 +1752004 15883 -1183 2942 +1752218 15908 -1240 2938 +1752430 15935 -1301 2938 +1752653 15961 -1361 2932 +1752863 15987 -1422 2927 +1753078 16036 -1542 2918 +1753291 16058 -1599 2913 +1753497 16080 -1656 2908 +1753712 16102 -1715 2902 +1753931 16123 -1773 2897 +1754147 16143 -1830 2893 +1754359 16163 -1888 2887 +1754564 16182 -1945 2883 +1754779 16201 -2004 2878 +1754997 16219 -2062 2872 +1755208 16237 -2119 2868 +1755418 16254 -2178 2863 +1755633 16271 -2238 2858 +1755846 16288 -2298 2853 +1756067 16304 -2358 2850 +1756278 16336 -2478 2843 +1756488 16351 -2539 2840 +1756702 16366 -2600 2835 +1756913 16380 -2660 2830 +1757128 16393 -2719 2826 +1757348 16406 -2779 2821 +1757559 16419 -2841 2814 +1757771 16431 -2902 2810 +1757985 16443 -2965 2805 +1758404 16463 -3087 2792 +1758623 16472 -3148 2785 +1759045 16490 -3272 2776 +1759260 16497 -3332 2775 +1759475 16514 -3460 2777 +1759692 16523 -3526 2778 +1759904 16532 -3591 2778 +1760117 16540 -3654 2779 +1760331 16549 -3716 2780 +1760544 16558 -3780 2783 +1760754 16567 -3844 2785 +1761179 16586 -3969 2788 +1761606 16606 -4098 2792 +1761821 16616 -4162 2793 +1762041 16626 -4224 2789 +1762253 16634 -4281 2783 +1762463 16652 -4408 2785 +1762886 16669 -4527 2771 +1763108 16675 -4586 2764 +1763315 16682 -4647 2755 +1763530 16687 -4708 2749 +1763742 16692 -4769 2742 +1763960 16696 -4830 2736 +1764170 16699 -4889 2727 +1764388 16701 -4948 2720 +1764603 16703 -5011 2718 +1764811 16705 -5076 2716 +1765025 16706 -5139 2713 +1765242 16707 -5202 2714 +1765450 16709 -5267 2716 +1765669 16712 -5391 2719 +1765881 16714 -5455 2720 +1766097 16717 -5516 2721 +1766308 16719 -5581 2723 +1766518 16722 -5645 2725 +1766735 16724 -5707 2727 +1766948 16727 -5772 2727 +1767161 16730 -5839 2726 +1767372 16733 -5903 2726 +1767582 16735 -5964 2727 +1767796 16738 -6029 2726 +1768222 16744 -6159 2728 +1768443 16747 -6223 2730 +1768861 16756 -6419 2730 +1769085 16759 -6483 2730 +1769298 16763 -6549 2729 +1769504 16766 -6613 2730 +1769724 16769 -6679 2730 +1769933 16772 -6745 2730 +1770148 16775 -6809 2730 +1770361 16779 -6876 2729 +1770569 16782 -6941 2729 +1770787 16785 -7005 2731 +1771003 16788 -7065 2734 +1771213 16791 -7123 2738 +1771427 16795 -7185 2739 +1771643 16800 -7248 2740 +1771852 16804 -7310 2741 +1772067 16813 -7442 2741 +1772282 16817 -7504 2740 +1772500 16821 -7567 2741 +1772710 16826 -7632 2739 +1772921 16830 -7696 2737 +1773131 16834 -7760 2735 +1773344 16838 -7827 2734 +1773567 16842 -7893 2734 +1773777 16845 -7959 2734 +1773986 16849 -8025 2733 +1774197 16853 -8091 2732 +1774410 16856 -8157 2732 +1774632 16860 -8222 2731 +1774838 16863 -8288 2730 +1775264 16873 -8482 2728 +1775486 16876 -8547 2727 +1775696 16879 -8613 2727 +1775904 16882 -8678 2727 +1776119 16884 -8742 2725 +1776339 16886 -8802 2720 +1776545 16888 -8859 2713 +1776760 16889 -8916 2705 +1776979 16889 -8968 2695 +1777188 16888 -9018 2683 +1777405 16885 -9068 2667 +1777618 16881 -9117 2649 +1777829 16876 -9167 2632 +1778252 16854 -9340 2630 +1778473 16849 -9384 2630 +1778892 16842 -9433 2630 +1779109 16842 -9437 2631 +1779324 16842 -9437 2631 diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Images/Map.png b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Images/Map.png new file mode 100644 index 0000000..1eb9bd6 Binary files /dev/null and b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/Images/Map.png differ diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/LICENSE b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/LICENSE new file mode 100644 index 0000000..ee6ffb9 --- /dev/null +++ b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 Udacity + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/README.md b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/README.md new file mode 100644 index 0000000..a59aeb6 --- /dev/null +++ b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/README.md @@ -0,0 +1,47 @@ +[![Udacity - Robotics NanoDegree Program](https://s3-us-west-1.amazonaws.com/udacity-robotics/Extra+Images/RoboND_flag.png)](https://www.udacity.com/robotics) + +# RoboND-OccupancyGridMappingAlgorithm +You will visualize the mapped environment through the generated image + +### Instruction +Code the visualization function which will plot the state of each grid cell using the matplotlib python library +``` C++ +void visualization() +{ + //TODO: Initialize a plot named Map of size 300x150 + + //TODO: Loop over the log odds values of the cells and plot each cell state. + //Unkown state: green color, occupied state: black color, and free state: red color + + //TODO: Save the image and close the plot +} +``` +Here are some helpful commands you can use to generate plots with the `matplotlib` library: +* *Set Title*: `plt::title("Your Title");` +* *Set Limits*: `plt::xlim(x-axis lower limit, x-axis upper limit );` +* *Plot Data*:`plt::plot({ x-value }, { y-value }, "Color and Shape");` +* *Save Plot*: `plt::save("File name and directory")`; +* *Close Plot*: `plt::clf()`; + +Check out this [link](https://github.com/lava/matplotlib-cpp) for more information on the `matplotlib` C++ library. For information regarding the plot color and shape refer to the LineSpec and LineColor section of the [MATLAB](https://www.mathworks.com/help/matlab/ref/plot.html?requestedDomain=true) documentation. + +### Compiling +```sh +$ cd /home/workspace/ +$ git clone https://github.com/udacity/RoboND-OccupancyGridMappingAlgorithm +$ cd RoboND-OccupancyGridMappingAlgorithm/ +$ rm -rf Images/* #Delete the folder content and not the folder itself! +$ g++ main.cpp -o app -std=c++11 -I/usr/include/python2.7 -lpython2.7 +``` + +### Running +```sh +$ ./app +``` + +Now, wait for the program to generate the map! + +### Generated Map + +![alt text](Images/Map.png) + diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/app b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/app new file mode 100755 index 0000000..df543dc Binary files /dev/null and b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/app differ diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/main.cpp b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/main.cpp new file mode 100644 index 0000000..04e3676 --- /dev/null +++ b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/main.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include "src/matplotlibcpp.h" //Graph Library + +using namespace std; +namespace plt = matplotlibcpp; + +// Sensor characteristic: Min and Max ranges of the beams +double Zmax = 5000, Zmin = 170; +// Defining free cells(lfree), occupied cells(locc), unknown cells(l0) log odds values +double l0 = 0, locc = 0.4, lfree = -0.4; +// Grid dimensions +double gridWidth = 100, gridHeight = 100; +// Map dimensions +double mapWidth = 30000, mapHeight = 15000; +// Robot size with respect to the map +double robotXOffset = mapWidth / 5, robotYOffset = mapHeight / 3; +// Defining an l vector to store the log odds values of each cell +vector< vector > l(mapWidth/gridWidth, vector(mapHeight/gridHeight)); + +double inverseSensorModel(double x, double y, double theta, double xi, double yi, double sensorData[]) +{ + // Defining Sensor Characteristics + double Zk, thetaK, sensorTheta; + double minDelta = -1; + double alpha = 200, beta = 20; + + //******************Compute r and phi**********************// + double r = sqrt(pow(xi - x, 2) + pow(yi - y, 2)); + double phi = atan2(yi - y, xi - x) - theta; + + //Scaling Measurement to [-90 -37.5 -22.5 -7.5 7.5 22.5 37.5 90] + for (int i = 0; i < 8; i++) { + if (i == 0) { + sensorTheta = -90 * (M_PI / 180); + } + else if (i == 1) { + sensorTheta = -37.5 * (M_PI / 180); + } + else if (i == 6) { + sensorTheta = 37.5 * (M_PI / 180); + } + else if (i == 7) { + sensorTheta = 90 * (M_PI / 180); + } + else { + sensorTheta = (-37.5 + (i - 1) * 15) * (M_PI / 180); + } + + if (fabs(phi - sensorTheta) < minDelta || minDelta == -1) { + Zk = sensorData[i]; + thetaK = sensorTheta; + minDelta = fabs(phi - sensorTheta); + } + } + + //******************Evaluate the three cases**********************// + if (r > min((double)Zmax, Zk + alpha / 2) || fabs(phi - thetaK) > beta / 2 || Zk > Zmax || Zk < Zmin) { + return l0; + } + else if (Zk < Zmax && fabs(r - Zk) < alpha / 2) { + return locc; + } + else if (r <= Zk) { + return lfree; + } +} + +void occupancyGridMapping(double Robotx, double Roboty, double Robottheta, double sensorData[]) +{ + //******************Code the Occupancy Grid Mapping Algorithm**********************// + for (int x = 0; x < mapWidth / gridWidth; x++) { + for (int y = 0; y < mapHeight / gridHeight; y++) { + double xi = x * gridWidth + gridWidth / 2 - robotXOffset; + double yi = -(y * gridHeight + gridHeight / 2) + robotYOffset; + if (sqrt(pow(xi - Robotx, 2) + pow(yi - Roboty, 2)) <= Zmax) { + l[x][y] = l[x][y] + inverseSensorModel(Robotx, Roboty, Robottheta, xi, yi, sensorData) - l0; + } + } + } +} + +void visualization() +{ + //Graph Format + plt::title("Map"); + plt::xlim(0, (int)(mapWidth / gridWidth)); + plt::ylim(0, (int)(mapHeight / gridHeight)); + + // Draw every grid of the map: + for (double x = 0; x < mapWidth / gridWidth; x++) { + cout << "Remaining Rows= " << mapWidth / gridWidth - x << endl; + for (double y = 0; y < mapHeight / gridHeight; y++) { + if (l[x][y] == 0) { //Green unkown state + plt::plot({ x }, { y }, "g."); + } + else if (l[x][y] > 0) { //Black occupied state + plt::plot({ x }, { y }, "k."); + } + else { //Red free state + plt::plot({ x }, { y }, "r."); + } + } + } + + //Save the image and close the plot + plt::save("./Images/Map.png"); + plt::clf(); +} + +int main() +{ + double timeStamp; + double measurementData[8]; + double robotX, robotY, robotTheta; + + FILE* posesFile = fopen("Data/poses.txt", "r"); + FILE* measurementFile = fopen("Data/measurement.txt", "r"); + + // Scanning the files and retrieving measurement and poses at each timestamp + while (fscanf(posesFile, "%lf %lf %lf %lf", &timeStamp, &robotX, &robotY, &robotTheta) != EOF) { + fscanf(measurementFile, "%lf", &timeStamp); + for (int i = 0; i < 8; i++) { + fscanf(measurementFile, "%lf", &measurementData[i]); + } + occupancyGridMapping(robotX, robotY, (robotTheta / 10) * (M_PI / 180), measurementData); + } + + // Visualize the map at the final step + cout << "Wait for the image to generate" << endl; + visualization(); + cout << "Done!" << endl; + + return 0; +} + diff --git a/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/src/matplotlibcpp.h b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/src/matplotlibcpp.h new file mode 100644 index 0000000..638a272 --- /dev/null +++ b/mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/src/matplotlibcpp.h @@ -0,0 +1,1024 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include // requires c++11 support + +#if __cplusplus > 199711L || _MSC_VER > 1800 +#include +#endif + +#include + +#ifndef WITHOUT_NUMPY + #define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION + #include +#endif // WITHOUT_NUMPY + +#if PY_MAJOR_VERSION >= 3 +#define PyString_FromString PyUnicode_FromString +#endif + + +namespace matplotlibcpp { + + namespace detail { + static std::string s_backend; + + struct _interpreter { + PyObject *s_python_function_show; + PyObject *s_python_function_draw; + PyObject *s_python_function_pause; + PyObject *s_python_function_save; + PyObject *s_python_function_figure; + PyObject *s_python_function_plot; + PyObject *s_python_function_semilogx; + PyObject *s_python_function_semilogy; + PyObject *s_python_function_loglog; + PyObject *s_python_function_fill_between; + PyObject *s_python_function_hist; + PyObject *s_python_function_subplot; + PyObject *s_python_function_legend; + PyObject *s_python_function_xlim; + PyObject *s_python_function_ylim; + PyObject *s_python_function_title; + PyObject *s_python_function_axis; + PyObject *s_python_function_xlabel; + PyObject *s_python_function_ylabel; + PyObject *s_python_function_grid; + PyObject *s_python_function_clf; + PyObject *s_python_function_errorbar; + PyObject *s_python_function_annotate; + PyObject *s_python_function_tight_layout; + PyObject *s_python_empty_tuple; + + /* For now, _interpreter is implemented as a singleton since its currently not possible to have + multiple independent embedded python interpreters without patching the python source code + or starting a separate process for each. + http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program + */ + + static _interpreter& get() { + static _interpreter ctx; + return ctx; + } + + private: + _interpreter() { + + // optional but recommended +#if PY_MAJOR_VERSION >= 3 + wchar_t name[] = L"plotting"; +#else + char name[] = "plotting"; +#endif + Py_SetProgramName(name); + Py_Initialize(); + +#ifndef WITHOUT_NUMPY + import_array(); // initialize numpy C-API +#endif + + PyObject* matplotlibname = PyString_FromString("matplotlib"); + PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); + PyObject* pylabname = PyString_FromString("pylab"); + if (!pyplotname || !pylabname || !matplotlibname) { + throw std::runtime_error("couldnt create string"); + } + + PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); + if(!matplotlib) { throw std::runtime_error("Error loading module matplotlib!"); } + + // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, + // or matplotlib.backends is imported for the first time + if (!s_backend.empty()) { + PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); + } + + PyObject* pymod = PyImport_Import(pyplotname); + Py_DECREF(pyplotname); + if(!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } + + + PyObject* pylabmod = PyImport_Import(pylabname); + Py_DECREF(pylabname); + if(!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } + + s_python_function_show = PyObject_GetAttrString(pymod, "show"); + s_python_function_draw = PyObject_GetAttrString(pymod, "draw"); + s_python_function_pause = PyObject_GetAttrString(pymod, "pause"); + s_python_function_figure = PyObject_GetAttrString(pymod, "figure"); + s_python_function_plot = PyObject_GetAttrString(pymod, "plot"); + s_python_function_semilogx = PyObject_GetAttrString(pymod, "semilogx"); + s_python_function_semilogy = PyObject_GetAttrString(pymod, "semilogy"); + s_python_function_loglog = PyObject_GetAttrString(pymod, "loglog"); + s_python_function_fill_between = PyObject_GetAttrString(pymod, "fill_between"); + s_python_function_hist = PyObject_GetAttrString(pymod,"hist"); + s_python_function_subplot = PyObject_GetAttrString(pymod, "subplot"); + s_python_function_legend = PyObject_GetAttrString(pymod, "legend"); + s_python_function_ylim = PyObject_GetAttrString(pymod, "ylim"); + s_python_function_title = PyObject_GetAttrString(pymod, "title"); + s_python_function_axis = PyObject_GetAttrString(pymod, "axis"); + s_python_function_xlabel = PyObject_GetAttrString(pymod, "xlabel"); + s_python_function_ylabel = PyObject_GetAttrString(pymod, "ylabel"); + s_python_function_grid = PyObject_GetAttrString(pymod, "grid"); + s_python_function_xlim = PyObject_GetAttrString(pymod, "xlim"); + s_python_function_save = PyObject_GetAttrString(pylabmod, "savefig"); + s_python_function_annotate = PyObject_GetAttrString(pymod,"annotate"); + s_python_function_clf = PyObject_GetAttrString(pymod, "clf"); + s_python_function_errorbar = PyObject_GetAttrString(pymod, "errorbar"); + s_python_function_tight_layout = PyObject_GetAttrString(pymod, "tight_layout"); + + if( !s_python_function_show + || !s_python_function_draw + || !s_python_function_pause + || !s_python_function_figure + || !s_python_function_plot + || !s_python_function_semilogx + || !s_python_function_semilogy + || !s_python_function_loglog + || !s_python_function_fill_between + || !s_python_function_subplot + || !s_python_function_legend + || !s_python_function_ylim + || !s_python_function_title + || !s_python_function_axis + || !s_python_function_xlabel + || !s_python_function_ylabel + || !s_python_function_grid + || !s_python_function_xlim + || !s_python_function_save + || !s_python_function_clf + || !s_python_function_annotate + || !s_python_function_errorbar + || !s_python_function_errorbar + || !s_python_function_tight_layout + ) { throw std::runtime_error("Couldn't find required function!"); } + + if ( !PyFunction_Check(s_python_function_show) + || !PyFunction_Check(s_python_function_draw) + || !PyFunction_Check(s_python_function_pause) + || !PyFunction_Check(s_python_function_figure) + || !PyFunction_Check(s_python_function_plot) + || !PyFunction_Check(s_python_function_semilogx) + || !PyFunction_Check(s_python_function_semilogy) + || !PyFunction_Check(s_python_function_loglog) + || !PyFunction_Check(s_python_function_fill_between) + || !PyFunction_Check(s_python_function_subplot) + || !PyFunction_Check(s_python_function_legend) + || !PyFunction_Check(s_python_function_annotate) + || !PyFunction_Check(s_python_function_ylim) + || !PyFunction_Check(s_python_function_title) + || !PyFunction_Check(s_python_function_axis) + || !PyFunction_Check(s_python_function_xlabel) + || !PyFunction_Check(s_python_function_ylabel) + || !PyFunction_Check(s_python_function_grid) + || !PyFunction_Check(s_python_function_xlim) + || !PyFunction_Check(s_python_function_save) + || !PyFunction_Check(s_python_function_clf) + || !PyFunction_Check(s_python_function_tight_layout) + || !PyFunction_Check(s_python_function_errorbar) + ) { throw std::runtime_error("Python object is unexpectedly not a PyFunction."); } + + s_python_empty_tuple = PyTuple_New(0); + } + + ~_interpreter() { + Py_Finalize(); + } + }; + } + + // must be called before the first regular call to matplotlib to have any effect + void backend(const std::string& name) + { + detail::s_backend = name; + } + + bool annotate(std::string annotation, double x, double y) + { + PyObject * xy = PyTuple_New(2); + PyObject * str = PyString_FromString(annotation.c_str()); + + PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); + PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "xy", xy); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; + } + +#ifndef WITHOUT_NUMPY + // Type selector for numpy array conversion + template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default + template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; + template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; + + template + PyObject* get_array(const std::vector& v) + { + detail::_interpreter::get(); //interpreter needs to be initialized for the numpy commands to work + NPY_TYPES type = select_npy_type::type; + if (type == NPY_NOTYPE) + { + std::vector vd(v.size()); + npy_intp vsize = v.size(); + std::copy(v.begin(),v.end(),vd.begin()); + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, NPY_DOUBLE, (void*)(vd.data())); + return varray; + } + + npy_intp vsize = v.size(); + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); + return varray; + } + +#else // fallback if we don't have numpy: copy every element of the given vector + + template + PyObject* get_array(const std::vector& v) + { + PyObject* list = PyList_New(v.size()); + for(size_t i = 0; i < v.size(); ++i) { + PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); + } + return list; + } + +#endif // WITHOUT_NUMPY + + template + bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) + { + assert(x.size() == y.size()); + + // using numpy arrays + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; + } + + template< typename Numeric > + bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) + { + assert(x.size() == y1.size()); + assert(x.size() == y2.size()); + + // using numpy arrays + PyObject* xarray = get_array(x); + PyObject* y1array = get_array(y1); + PyObject* y2array = get_array(y2); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, y1array); + PyTuple_SetItem(args, 2, y2array); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; + } + + template< typename Numeric> + bool hist(const std::vector& y, long bins=10,std::string color="b", double alpha=1.0) + { + + PyObject* yarray = get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + + PyTuple_SetItem(plot_args, 0, yarray); + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; + } + + template< typename Numeric> + bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) + { + PyObject* yarray = get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; + } + + template + bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") + { + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") + { + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") + { + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") + { + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::string &s = "") + { + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + PyObject* yerrarray = get_array(yerr); + + PyObject *kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "yerr", yerrarray); + + PyObject *pystring = PyString_FromString(s.c_str()); + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if (res) + Py_DECREF(res); + else + throw std::runtime_error("Call to errorbar() failed."); + + return res; + } + + template + bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") + { + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(2); + + PyTuple_SetItem(plot_args, 0, yarray); + PyTuple_SetItem(plot_args, 1, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") + { + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") + { + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") + { + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") + { + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + + template + bool plot(const std::vector& y, const std::string& format = "") + { + std::vector x(y.size()); + for(size_t i=0; i + void ylim(Numeric left, Numeric right) + { + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(args); + Py_DECREF(res); + } + + template + void xlim(Numeric left, Numeric right) + { + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(args); + Py_DECREF(res); + } + + + inline double* xlim() + { + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(res); + return arr; + } + + + inline double* ylim() + { + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(res); + return arr; + } + + inline void subplot(long nrows, long ncols, long plot_number) + { + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); + if(!res) throw std::runtime_error("Call to subplot() failed."); + + Py_DECREF(args); + Py_DECREF(res); + } + + inline void title(const std::string &titlestr) + { + PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pytitlestr); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_title, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + // if PyDeCRFF, the function doesn't work on Mac OS + } + + inline void axis(const std::string &axisstr) + { + PyObject* str = PyString_FromString(axisstr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + // if PyDeCRFF, the function doesn't work on Mac OS + } + + inline void xlabel(const std::string &str) + { + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlabel, args); + if(!res) throw std::runtime_error("Call to xlabel() failed."); + + // if PyDeCRFF, the function doesn't work on Mac OS + } + + inline void ylabel(const std::string &str) + { + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylabel, args); + if(!res) throw std::runtime_error("Call to ylabel() failed."); + + // if PyDeCRFF, the function doesn't work on Mac OS + } + + inline void grid(bool flag) + { + PyObject* pyflag = flag ? Py_True : Py_False; + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyflag); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); + if(!res) throw std::runtime_error("Call to grid() failed."); + + // if PyDeCRFF, the function doesn't work on Mac OS + } + + inline void show(const bool block = true) + { + PyObject* res; + if(block) + { + res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_show, + detail::_interpreter::get().s_python_empty_tuple); + } + else + { + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "block", Py_False); + res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); + } + + + if (!res) throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); + } + + inline void draw() + { + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_draw, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to draw() failed."); + + Py_DECREF(res); + } + + template + void pause(Numeric interval) + { + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); + if(!res) throw std::runtime_error("Call to pause() failed."); + + Py_DECREF(args); + Py_DECREF(res); + } + + inline void save(const std::string& filename) + { + PyObject* pyfilename = PyString_FromString(filename.c_str()); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyfilename); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args); + if (!res) throw std::runtime_error("Call to save() failed."); + + Py_DECREF(args); + Py_DECREF(res); + } + + inline void clf() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_clf, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to clf() failed."); + + Py_DECREF(res); + } + + // Actually, is there any reason not to call this automatically for every plot? + inline void tight_layout() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_tight_layout, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to tight_layout() failed."); + + Py_DECREF(res); + } + +#if __cplusplus > 199711L || _MSC_VER > 1800 + // C++11-exclusive content starts here (variadic plot() and initializer list support) + + namespace detail { + template + using is_function = typename std::is_function>>::type; + + template + struct is_callable_impl; + + template + struct is_callable_impl + { + typedef is_function type; + }; // a non-object is callable iff it is a function + + template + struct is_callable_impl + { + struct Fallback { void operator()(); }; + struct Derived : T, Fallback { }; + + template struct Check; + + template + static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match + + template + static std::false_type test( Check* ); + + public: + typedef decltype(test(nullptr)) type; + typedef decltype(&Fallback::operator()) dtype; + static constexpr bool value = type::value; + }; // an object is callable iff it defines operator() + + template + struct is_callable + { + // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not + typedef typename is_callable_impl::value, T>::type type; + }; + + template + struct plot_impl { }; + + template<> + struct plot_impl + { + template + bool operator()(const IterableX& x, const IterableY& y, const std::string& format) + { + // 2-phase lookup for distance, begin, end + using std::distance; + using std::begin; + using std::end; + + auto xs = distance(begin(x), end(x)); + auto ys = distance(begin(y), end(y)); + assert(xs == ys && "x and y data must have the same number of elements!"); + + PyObject* xlist = PyList_New(xs); + PyObject* ylist = PyList_New(ys); + PyObject* pystring = PyString_FromString(format.c_str()); + + auto itx = begin(x), ity = begin(y); + for(size_t i = 0; i < xs; ++i) { + PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); + PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); + } + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xlist); + PyTuple_SetItem(plot_args, 1, ylist); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } + }; + + template<> + struct plot_impl + { + template + bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) + { + //std::cout << "Callable impl called" << std::endl; + + if(begin(ticks) == end(ticks)) return true; + + // We could use additional meta-programming to deduce the correct element type of y, + // but all values have to be convertible to double anyways + std::vector y; + for(auto x : ticks) y.push_back(f(x)); + return plot_impl()(ticks,y,format); + } + }; + } + + // recursion stop for the above + template + bool plot() { return true; } + + template + bool plot(const A& a, const B& b, const std::string& format, Args... args) + { + return detail::plot_impl::type>()(a,b,format) && plot(args...); + } + + /* + * This group of plot() functions is needed to support initializer lists, i.e. calling + * plot( {1,2,3,4} ) + */ + bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { + return plot(x,y,format); + } + + bool plot(const std::vector& y, const std::string& format = "") { + return plot(y,format); + } + + bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { + return plot(x,y,keywords); + } + + bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { + return named_plot(name,x,y,format); + } + +#endif + +}