Something went wrong on our end
-
abhishek-0802 authoredabhishek-0802 authored
navipy_arenatools_cam_calib_py.html 46.00 KiB
<!DOCTYPE html>
<html>
<head>
<meta http-equiv="Content-Type" content="text/html; charset=utf-8">
<meta http-equiv="X-UA-Compatible" content="IE=emulateIE7" />
<title>Coverage for navipy/arenatools/cam_calib.py: 0%</title>
<link rel="stylesheet" href="style.css" type="text/css">
<script type="text/javascript" src="jquery.min.js"></script>
<script type="text/javascript" src="jquery.hotkeys.js"></script>
<script type="text/javascript" src="jquery.isonscreen.js"></script>
<script type="text/javascript" src="coverage_html.js"></script>
<script type="text/javascript">
jQuery(document).ready(coverage.pyfile_ready);
</script>
</head>
<body class="pyfile">
<div id="header">
<div class="content">
<h1>Coverage for <b>navipy/arenatools/cam_calib.py</b> :
<span class="pc_cov">0%</span>
</h1>
<img id="keyboard_icon" src="keybd_closed.png" alt="Show keyboard shortcuts" />
<h2 class="stats">
82 statements
<span class="run hide_run shortkey_r button_toggle_run">0 run</span>
<span class="mis shortkey_m button_toggle_mis">82 missing</span>
<span class="exc shortkey_x button_toggle_exc">0 excluded</span>
</h2>
</div>
</div>
<div class="help_panel">
<img id="panel_icon" src="keybd_open.png" alt="Hide keyboard shortcuts" />
<p class="legend">Hot-keys on this page</p>
<div>
<p class="keyhelp">
<span class="key">r</span>
<span class="key">m</span>
<span class="key">x</span>
<span class="key">p</span> toggle line displays
</p>
<p class="keyhelp">
<span class="key">j</span>
<span class="key">k</span> next/prev highlighted chunk
</p>
<p class="keyhelp">
<span class="key">0</span> (zero) top of page
</p>
<p class="keyhelp">
<span class="key">1</span> (one) first highlighted chunk
</p>
</div>
</div>
<div id="source">
<table>
<tr>
<td class="linenos">
<p id="n1" class="pln"><a href="#n1">1</a></p><p id="n2" class="pln"><a href="#n2">2</a></p>
<p id="n3" class="pln"><a href="#n3">3</a></p>
<p id="n4" class="stm mis"><a href="#n4">4</a></p>
<p id="n5" class="stm mis"><a href="#n5">5</a></p>
<p id="n6" class="stm mis"><a href="#n6">6</a></p>
<p id="n7" class="stm mis"><a href="#n7">7</a></p>
<p id="n8" class="stm mis"><a href="#n8">8</a></p>
<p id="n9" class="stm mis"><a href="#n9">9</a></p>
<p id="n10" class="stm mis"><a href="#n10">10</a></p>
<p id="n11" class="pln"><a href="#n11">11</a></p>
<p id="n12" class="pln"><a href="#n12">12</a></p>
<p id="n13" class="stm mis"><a href="#n13">13</a></p>
<p id="n14" class="pln"><a href="#n14">14</a></p>
<p id="n15" class="stm mis"><a href="#n15">15</a></p>
<p id="n16" class="stm mis"><a href="#n16">16</a></p>
<p id="n17" class="stm mis"><a href="#n17">17</a></p>
<p id="n18" class="stm mis"><a href="#n18">18</a></p>
<p id="n19" class="stm mis"><a href="#n19">19</a></p>
<p id="n20" class="stm mis"><a href="#n20">20</a></p>
<p id="n21" class="stm mis"><a href="#n21">21</a></p>
<p id="n22" class="stm mis"><a href="#n22">22</a></p>
<p id="n23" class="pln"><a href="#n23">23</a></p>
<p id="n24" class="pln"><a href="#n24">24</a></p>
<p id="n25" class="stm mis"><a href="#n25">25</a></p>
<p id="n26" class="pln"><a href="#n26">26</a></p>
<p id="n27" class="pln"><a href="#n27">27</a></p>
<p id="n28" class="pln"><a href="#n28">28</a></p>
<p id="n29" class="pln"><a href="#n29">29</a></p>
<p id="n30" class="stm mis"><a href="#n30">30</a></p>
<p id="n31" class="stm mis"><a href="#n31">31</a></p>
<p id="n32" class="stm mis"><a href="#n32">32</a></p>
<p id="n33" class="pln"><a href="#n33">33</a></p>
<p id="n34" class="pln"><a href="#n34">34</a></p>
<p id="n35" class="pln"><a href="#n35">35</a></p>
<p id="n36" class="stm mis"><a href="#n36">36</a></p>
<p id="n37" class="pln"><a href="#n37">37</a></p>
<p id="n38" class="pln"><a href="#n38">38</a></p>
<p id="n39" class="pln"><a href="#n39">39</a></p>
<p id="n40" class="pln"><a href="#n40">40</a></p>
<p id="n41" class="stm mis"><a href="#n41">41</a></p>
<p id="n42" class="stm mis"><a href="#n42">42</a></p>
<p id="n43" class="stm mis"><a href="#n43">43</a></p>
<p id="n44" class="pln"><a href="#n44">44</a></p>
<p id="n45" class="pln"><a href="#n45">45</a></p>
<p id="n46" class="stm mis"><a href="#n46">46</a></p>
<p id="n47" class="pln"><a href="#n47">47</a></p>
<p id="n48" class="pln"><a href="#n48">48</a></p>
<p id="n49" class="stm mis"><a href="#n49">49</a></p>
<p id="n50" class="stm mis"><a href="#n50">50</a></p>
<p id="n51" class="stm mis"><a href="#n51">51</a></p>
<p id="n52" class="stm mis"><a href="#n52">52</a></p>
<p id="n53" class="pln"><a href="#n53">53</a></p>
<p id="n54" class="pln"><a href="#n54">54</a></p>
<p id="n55" class="stm mis"><a href="#n55">55</a></p>
<p id="n56" class="pln"><a href="#n56">56</a></p>
<p id="n57" class="pln"><a href="#n57">57</a></p>
<p id="n58" class="pln"><a href="#n58">58</a></p>
<p id="n59" class="pln"><a href="#n59">59</a></p>
<p id="n60" class="stm mis"><a href="#n60">60</a></p>
<p id="n61" class="stm mis"><a href="#n61">61</a></p>
<p id="n62" class="pln"><a href="#n62">62</a></p>
<p id="n63" class="stm mis"><a href="#n63">63</a></p>
<p id="n64" class="stm mis"><a href="#n64">64</a></p>
<p id="n65" class="stm mis"><a href="#n65">65</a></p>
<p id="n66" class="pln"><a href="#n66">66</a></p>
<p id="n67" class="stm mis"><a href="#n67">67</a></p>
<p id="n68" class="pln"><a href="#n68">68</a></p>
<p id="n69" class="pln"><a href="#n69">69</a></p>
<p id="n70" class="stm mis"><a href="#n70">70</a></p>
<p id="n71" class="pln"><a href="#n71">71</a></p><p id="n72" class="pln"><a href="#n72">72</a></p>
<p id="n73" class="stm mis"><a href="#n73">73</a></p>
<p id="n74" class="pln"><a href="#n74">74</a></p>
<p id="n75" class="pln"><a href="#n75">75</a></p>
<p id="n76" class="pln"><a href="#n76">76</a></p>
<p id="n77" class="pln"><a href="#n77">77</a></p>
<p id="n78" class="pln"><a href="#n78">78</a></p>
<p id="n79" class="pln"><a href="#n79">79</a></p>
<p id="n80" class="pln"><a href="#n80">80</a></p>
<p id="n81" class="pln"><a href="#n81">81</a></p>
<p id="n82" class="pln"><a href="#n82">82</a></p>
<p id="n83" class="pln"><a href="#n83">83</a></p>
<p id="n84" class="pln"><a href="#n84">84</a></p>
<p id="n85" class="pln"><a href="#n85">85</a></p>
<p id="n86" class="pln"><a href="#n86">86</a></p>
<p id="n87" class="pln"><a href="#n87">87</a></p>
<p id="n88" class="pln"><a href="#n88">88</a></p>
<p id="n89" class="stm mis"><a href="#n89">89</a></p>
<p id="n90" class="stm mis"><a href="#n90">90</a></p>
<p id="n91" class="stm mis"><a href="#n91">91</a></p>
<p id="n92" class="stm mis"><a href="#n92">92</a></p>
<p id="n93" class="stm mis"><a href="#n93">93</a></p>
<p id="n94" class="stm mis"><a href="#n94">94</a></p>
<p id="n95" class="stm mis"><a href="#n95">95</a></p>
<p id="n96" class="pln"><a href="#n96">96</a></p>
<p id="n97" class="pln"><a href="#n97">97</a></p>
<p id="n98" class="stm mis"><a href="#n98">98</a></p>
<p id="n99" class="stm mis"><a href="#n99">99</a></p>
<p id="n100" class="stm mis"><a href="#n100">100</a></p>
<p id="n101" class="pln"><a href="#n101">101</a></p>
<p id="n102" class="stm mis"><a href="#n102">102</a></p>
<p id="n103" class="stm mis"><a href="#n103">103</a></p>
<p id="n104" class="stm mis"><a href="#n104">104</a></p>
<p id="n105" class="stm mis"><a href="#n105">105</a></p>
<p id="n106" class="pln"><a href="#n106">106</a></p>
<p id="n107" class="stm mis"><a href="#n107">107</a></p>
<p id="n108" class="pln"><a href="#n108">108</a></p>
<p id="n109" class="pln"><a href="#n109">109</a></p>
<p id="n110" class="stm mis"><a href="#n110">110</a></p>
<p id="n111" class="pln"><a href="#n111">111</a></p>
<p id="n112" class="pln"><a href="#n112">112</a></p>
<p id="n113" class="pln"><a href="#n113">113</a></p>
<p id="n114" class="pln"><a href="#n114">114</a></p>
<p id="n115" class="pln"><a href="#n115">115</a></p>
<p id="n116" class="pln"><a href="#n116">116</a></p>
<p id="n117" class="pln"><a href="#n117">117</a></p>
<p id="n118" class="stm mis"><a href="#n118">118</a></p>
<p id="n119" class="stm mis"><a href="#n119">119</a></p>
<p id="n120" class="stm mis"><a href="#n120">120</a></p>
<p id="n121" class="stm mis"><a href="#n121">121</a></p>
<p id="n122" class="stm mis"><a href="#n122">122</a></p>
<p id="n123" class="stm mis"><a href="#n123">123</a></p>
<p id="n124" class="stm mis"><a href="#n124">124</a></p>
<p id="n125" class="pln"><a href="#n125">125</a></p>
<p id="n126" class="stm mis"><a href="#n126">126</a></p>
<p id="n127" class="stm mis"><a href="#n127">127</a></p>
<p id="n128" class="stm mis"><a href="#n128">128</a></p>
<p id="n129" class="stm mis"><a href="#n129">129</a></p>
<p id="n130" class="stm mis"><a href="#n130">130</a></p>
<p id="n131" class="pln"><a href="#n131">131</a></p>
<p id="n132" class="pln"><a href="#n132">132</a></p>
<p id="n133" class="stm mis"><a href="#n133">133</a></p>
<p id="n134" class="stm mis"><a href="#n134">134</a></p>
<p id="n135" class="stm mis"><a href="#n135">135</a></p>
<p id="n136" class="stm mis"><a href="#n136">136</a></p>
<p id="n137" class="pln"><a href="#n137">137</a></p>
<p id="n138" class="pln"><a href="#n138">138</a></p>
<p id="n139" class="stm mis"><a href="#n139">139</a></p>
<p id="n140" class="stm mis"><a href="#n140">140</a></p>
<p id="n141" class="stm mis"><a href="#n141">141</a></p><p id="n142" class="stm mis"><a href="#n142">142</a></p>
<p id="n143" class="stm mis"><a href="#n143">143</a></p>
<p id="n144" class="pln"><a href="#n144">144</a></p>
<p id="n145" class="stm mis"><a href="#n145">145</a></p>
<p id="n146" class="stm mis"><a href="#n146">146</a></p>
<p id="n147" class="stm mis"><a href="#n147">147</a></p>
<p id="n148" class="pln"><a href="#n148">148</a></p>
<p id="n149" class="pln"><a href="#n149">149</a></p>
<p id="n150" class="stm mis"><a href="#n150">150</a></p>
<p id="n151" class="stm mis"><a href="#n151">151</a></p>
<p id="n152" class="stm mis"><a href="#n152">152</a></p>
<p id="n153" class="stm mis"><a href="#n153">153</a></p>
</td>
<td class="text">
<p id="t1" class="pln"><span class="str">"""</span><span class="strut"> </span></p>
<p id="t2" class="pln"><span class="str">Function for camera calibrations</span><span class="strut"> </span></p>
<p id="t3" class="pln"><span class="str">"""</span><span class="strut"> </span></p>
<p id="t4" class="stm mis"><span class="key">import</span> <span class="nam">pandas</span> <span class="key">as</span> <span class="nam">pd</span><span class="strut"> </span></p>
<p id="t5" class="stm mis"><span class="key">import</span> <span class="nam">numpy</span> <span class="key">as</span> <span class="nam">np</span><span class="strut"> </span></p>
<p id="t6" class="stm mis"><span class="key">import</span> <span class="nam">cv2</span><span class="strut"> </span></p>
<p id="t7" class="stm mis"><span class="key">import</span> <span class="nam">os</span><span class="strut"> </span></p>
<p id="t8" class="stm mis"><span class="key">from</span> <span class="nam">navipy</span><span class="op">.</span><span class="nam">io</span> <span class="key">import</span> <span class="nam">ivfile</span><span class="strut"> </span></p>
<p id="t9" class="stm mis"><span class="key">import</span> <span class="nam">matplotlib</span><span class="op">.</span><span class="nam">pyplot</span> <span class="key">as</span> <span class="nam">plt</span><span class="strut"> </span></p>
<p id="t10" class="stm mis"><span class="key">from</span> <span class="nam">mpl_toolkits</span><span class="op">.</span><span class="nam">mplot3d</span> <span class="key">import</span> <span class="nam">Axes3D</span> <span class="com"># noqa F401</span><span class="strut"> </span></p>
<p id="t11" class="pln"><span class="strut"> </span></p>
<p id="t12" class="pln"><span class="strut"> </span></p>
<p id="t13" class="stm mis"><span class="key">def</span> <span class="nam">concatenate_manhattan_2d3d</span><span class="op">(</span><span class="nam">manhattan_3d</span><span class="op">,</span> <span class="nam">manhattan_2d</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t14" class="pln"> <span class="str">""" Join 3D and 2D points for pose estimation """</span><span class="strut"> </span></p>
<p id="t15" class="stm mis"> <span class="key">if</span> <span class="key">not</span> <span class="nam">isinstance</span><span class="op">(</span><span class="nam">manhattan_2d</span><span class="op">,</span> <span class="nam">pd</span><span class="op">.</span><span class="nam">DataFrame</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t16" class="stm mis"> <span class="key">raise</span> <span class="nam">TypeError</span><span class="op">(</span><span class="str">'manhattan_2d should be a DataFrame'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t17" class="stm mis"> <span class="key">if</span> <span class="key">not</span> <span class="nam">isinstance</span><span class="op">(</span><span class="nam">manhattan_3d</span><span class="op">,</span> <span class="nam">pd</span><span class="op">.</span><span class="nam">DataFrame</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t18" class="stm mis"> <span class="key">raise</span> <span class="nam">TypeError</span><span class="op">(</span><span class="str">'manhattan_3d should be a DataFrame'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t19" class="stm mis"> <span class="nam">d</span> <span class="op">=</span> <span class="op">{</span><span class="str">'three_d'</span><span class="op">:</span> <span class="nam">manhattan_3d</span><span class="op">,</span> <span class="str">'two_d'</span><span class="op">:</span> <span class="nam">manhattan_2d</span><span class="op">}</span><span class="strut"> </span></p>
<p id="t20" class="stm mis"> <span class="nam">manhattan_3d2d</span> <span class="op">=</span> <span class="nam">pd</span><span class="op">.</span><span class="nam">concat</span><span class="op">(</span><span class="nam">d</span><span class="op">.</span><span class="nam">values</span><span class="op">(</span><span class="op">)</span><span class="op">,</span> <span class="nam">axis</span><span class="op">=</span><span class="num">1</span><span class="op">,</span> <span class="nam">keys</span><span class="op">=</span><span class="nam">d</span><span class="op">.</span><span class="nam">keys</span><span class="op">(</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t21" class="stm mis"> <span class="nam">manhattan_3d2d</span> <span class="op">=</span> <span class="nam">manhattan_3d2d</span><span class="op">.</span><span class="nam">astype</span><span class="op">(</span><span class="nam">float</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t22" class="stm mis"> <span class="key">return</span> <span class="nam">manhattan_3d2d</span><span class="strut"> </span></p>
<p id="t23" class="pln"><span class="strut"> </span></p>
<p id="t24" class="pln"><span class="strut"> </span></p>
<p id="t25" class="stm mis"><span class="key">def</span> <span class="nam">objects_points_from_manhattan</span><span class="op">(</span><span class="nam">manhattan_3d2d</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t26" class="pln"> <span class="str">"""return objects points from a join manhatta</span><span class="strut"> </span></p>
<p id="t27" class="pln"><span class="strut"> </span></p>
<p id="t28" class="pln"><span class="str"> .. seealso: concatenate_manhattan_2d3d</span><span class="strut"> </span></p>
<p id="t29" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t30" class="stm mis"> <span class="key">if</span> <span class="key">not</span> <span class="nam">isinstance</span><span class="op">(</span><span class="nam">manhattan_3d2d</span><span class="op">,</span> <span class="nam">pd</span><span class="op">.</span><span class="nam">DataFrame</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t31" class="stm mis"> <span class="key">raise</span> <span class="nam">TypeError</span><span class="op">(</span><span class="str">'manhattan_3d2d should be a DataFrame'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t32" class="stm mis"> <span class="key">return</span> <span class="nam">manhattan_3d2d</span><span class="op">.</span><span class="nam">dropna</span><span class="op">(</span><span class="op">)</span><span class="op">.</span><span class="nam">loc</span><span class="op">[</span><span class="op">:</span><span class="op">,</span> <span class="str">'three_d'</span><span class="op">]</span><span class="xx">\</span><span class="strut"> </span></p>
<p id="t33" class="pln"> <span class="op">.</span><span class="nam">loc</span><span class="op">[</span><span class="op">:</span><span class="op">,</span> <span class="op">[</span><span class="str">'x'</span><span class="op">,</span> <span class="str">'y'</span><span class="op">,</span> <span class="str">'z'</span><span class="op">]</span><span class="op">]</span><span class="op">.</span><span class="nam">values</span><span class="strut"> </span></p>
<p id="t34" class="pln"><span class="strut"> </span></p>
<p id="t35" class="pln"><span class="strut"> </span></p>
<p id="t36" class="stm mis"><span class="key">def</span> <span class="nam">image_points_from_manhattan</span><span class="op">(</span><span class="nam">manhattan_3d2d</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t37" class="pln"> <span class="str">"""return image points from a joined manhattan</span><span class="strut"> </span></p>
<p id="t38" class="pln"><span class="strut"> </span></p>
<p id="t39" class="pln"><span class="str"> .. seelso: concatenate_manhattan_2d3d</span><span class="strut"> </span></p>
<p id="t40" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t41" class="stm mis"> <span class="key">if</span> <span class="key">not</span> <span class="nam">isinstance</span><span class="op">(</span><span class="nam">manhattan_3d2d</span><span class="op">,</span> <span class="nam">pd</span><span class="op">.</span><span class="nam">DataFrame</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t42" class="stm mis"> <span class="key">raise</span> <span class="nam">TypeError</span><span class="op">(</span><span class="str">'manhattan_3d2d should be a DataFrame'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t43" class="stm mis"> <span class="key">return</span> <span class="nam">manhattan_3d2d</span><span class="op">.</span><span class="nam">dropna</span><span class="op">(</span><span class="op">)</span><span class="op">.</span><span class="nam">loc</span><span class="op">[</span><span class="op">:</span><span class="op">,</span> <span class="str">'two_d'</span><span class="op">]</span><span class="op">.</span><span class="nam">loc</span><span class="op">[</span><span class="op">:</span><span class="op">,</span> <span class="op">[</span><span class="str">'x'</span><span class="op">,</span> <span class="str">'y'</span><span class="op">]</span><span class="op">]</span><span class="op">.</span><span class="nam">values</span><span class="strut"> </span></p>
<p id="t44" class="pln"><span class="strut"> </span></p>
<p id="t45" class="pln"><span class="strut"> </span></p>
<p id="t46" class="stm mis"><span class="key">def</span> <span class="nam">compose_from_vectors</span><span class="op">(</span><span class="nam">rvec</span><span class="op">,</span> <span class="nam">tvec</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t47" class="pln"> <span class="str">"""return camera pose from rotation and translation vectors</span><span class="strut"> </span></p>
<p id="t48" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t49" class="stm mis"> <span class="nam">r</span> <span class="op">=</span> <span class="nam">cv2</span><span class="op">.</span><span class="nam">Rodrigues</span><span class="op">(</span><span class="nam">rvec</span><span class="op">)</span><span class="op">[</span><span class="num">0</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t50" class="stm mis"> <span class="nam">camera_pose</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">hstack</span><span class="op">(</span><span class="op">(</span><span class="nam">r</span><span class="op">,</span> <span class="nam">tvec</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t51" class="stm mis"> <span class="nam">camera_pose</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">vstack</span><span class="op">(</span><span class="op">(</span><span class="nam">camera_pose</span><span class="op">,</span> <span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t52" class="stm mis"> <span class="key">return</span> <span class="nam">camera_pose</span><span class="strut"> </span></p>
<p id="t53" class="pln"><span class="strut"> </span></p>
<p id="t54" class="pln"><span class="strut"> </span></p>
<p id="t55" class="stm mis"><span class="key">def</span> <span class="nam">camera_pose</span><span class="op">(</span><span class="nam">manhattan_2d</span><span class="op">,</span> <span class="nam">manhattan_3d</span><span class="op">,</span> <span class="nam">camera_intrinsics</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p><p id="t56" class="pln"> <span class="str">"""solvePnP with 3D-2D points correspondance.</span><span class="strut"> </span></p>
<p id="t57" class="pln"><span class="strut"> </span></p>
<p id="t58" class="pln"><span class="str"> The camera intrinsics have to be known appriori</span><span class="strut"> </span></p>
<p id="t59" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t60" class="stm mis"> <span class="nam">camera_matrix</span> <span class="op">=</span> <span class="nam">camera_intrinsics</span><span class="op">[</span><span class="str">'intrinsic_matrix'</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t61" class="stm mis"> <span class="nam">distcoeffs</span> <span class="op">=</span> <span class="nam">camera_intrinsics</span><span class="op">[</span><span class="str">'distortion'</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t62" class="pln"><span class="strut"> </span></p>
<p id="t63" class="stm mis"> <span class="nam">manhattan_2d3d</span> <span class="op">=</span> <span class="nam">concatenate_manhattan_2d3d</span><span class="op">(</span><span class="nam">manhattan_3d</span><span class="op">,</span> <span class="nam">manhattan_2d</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t64" class="stm mis"> <span class="nam">objects_points</span> <span class="op">=</span> <span class="nam">objects_points_from_manhattan</span><span class="op">(</span><span class="nam">manhattan_2d3d</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t65" class="stm mis"> <span class="nam">image_points</span> <span class="op">=</span> <span class="nam">image_points_from_manhattan</span><span class="op">(</span><span class="nam">manhattan_2d3d</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t66" class="pln"><span class="strut"> </span></p>
<p id="t67" class="stm mis"> <span class="nam">retval</span><span class="op">,</span> <span class="nam">rvec</span><span class="op">,</span> <span class="nam">tvec</span> <span class="op">=</span> <span class="nam">cv2</span><span class="op">.</span><span class="nam">solvePnP</span><span class="op">(</span><span class="strut"> </span></p>
<p id="t68" class="pln"> <span class="nam">objects_points</span><span class="op">,</span> <span class="nam">image_points</span><span class="op">,</span> <span class="nam">camera_matrix</span><span class="op">,</span> <span class="nam">distcoeffs</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t69" class="pln"><span class="strut"> </span></p>
<p id="t70" class="stm mis"> <span class="key">return</span> <span class="nam">compose_from_vectors</span><span class="op">(</span><span class="nam">rvec</span><span class="op">,</span> <span class="nam">tvec</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t71" class="pln"><span class="strut"> </span></p>
<p id="t72" class="pln"><span class="strut"> </span></p>
<p id="t73" class="stm mis"><span class="key">def</span> <span class="nam">calibrates_extrinsic</span><span class="op">(</span><span class="nam">filenames</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t74" class="pln"> <span class="nam">manhattan_3d</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t75" class="pln"> <span class="nam">cameras_intrinsics</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t76" class="pln"> <span class="nam">corner_th</span><span class="op">=</span><span class="op">-</span><span class="num">1</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t77" class="pln"> <span class="nam">manhattan_3d_key</span><span class="op">=</span><span class="str">'manhattan'</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t78" class="pln"> <span class="str">"""Calibrate from files.</span><span class="strut"> </span></p>
<p id="t79" class="pln"><span class="strut"> </span></p>
<p id="t80" class="pln"><span class="str"> :param filenames: a list of ivfile name with manhattan markers</span><span class="strut"> </span></p>
<p id="t81" class="pln"><span class="str"> :param file_manhattan_3d: hdf file with manhattan_3d_key to load \</span><span class="strut"> </span></p>
<p id="t82" class="pln"><span class="str">containing a pandas dataframe with x,y,z coordinates of each points.</span><span class="strut"> </span></p>
<p id="t83" class="pln"><span class="str"> :param cameras_intrinsics: a list of camera intrinsics</span><span class="strut"> </span></p>
<p id="t84" class="pln"><span class="str"> :param corner_th: a threshold for points to be ignored on the top\</span><span class="strut"> </span></p>
<p id="t85" class="pln"><span class="str"> right image corner (useful when certain points are not visible)</span><span class="strut"> </span></p>
<p id="t86" class="pln"><span class="str"> :param manhattan_3d_key: the key to load the manhattan_3d \</span><span class="strut"> </span></p>
<p id="t87" class="pln"><span class="str">(default: manhattan)</span><span class="strut"> </span></p>
<p id="t88" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t89" class="stm mis"> <span class="nam">cameras_extrinsics</span> <span class="op">=</span> <span class="nam">dict</span><span class="op">(</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t90" class="stm mis"> <span class="key">for</span> <span class="nam">cam_i</span><span class="op">,</span> <span class="nam">cfile</span> <span class="key">in</span> <span class="nam">filenames</span><span class="op">.</span><span class="nam">items</span><span class="op">(</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t91" class="stm mis"> <span class="nam">_</span><span class="op">,</span> <span class="nam">ext</span> <span class="op">=</span> <span class="nam">os</span><span class="op">.</span><span class="nam">path</span><span class="op">.</span><span class="nam">splitext</span><span class="op">(</span><span class="nam">cfile</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t92" class="stm mis"> <span class="key">if</span> <span class="nam">ext</span> <span class="op">==</span> <span class="str">'.tra'</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t93" class="stm mis"> <span class="nam">manhattan_2d</span> <span class="op">=</span> <span class="nam">ivfile</span><span class="op">.</span><span class="nam">manhattan</span><span class="op">(</span><span class="nam">cfile</span><span class="op">,</span> <span class="nam">corner_th</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t94" class="stm mis"> <span class="key">elif</span> <span class="nam">ext</span> <span class="op">==</span> <span class="str">'.csv'</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t95" class="stm mis"> <span class="nam">manhattan_2d</span> <span class="op">=</span> <span class="nam">pd</span><span class="op">.</span><span class="nam">read_csv</span><span class="op">(</span><span class="nam">cfile</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t96" class="pln"> <span class="nam">names</span><span class="op">=</span><span class="op">[</span><span class="str">'x'</span><span class="op">,</span> <span class="str">'y'</span><span class="op">]</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t97" class="pln"> <span class="nam">header</span><span class="op">=</span><span class="num">0</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t98" class="stm mis"> <span class="nam">idx</span> <span class="op">=</span> <span class="op">(</span><span class="nam">manhattan_2d</span><span class="op">.</span><span class="nam">x</span> <span class="op"><</span> <span class="nam">corner_th</span><span class="op">)</span> <span class="op">&</span> <span class="op">(</span><span class="nam">manhattan_2d</span><span class="op">.</span><span class="nam">y</span> <span class="op"><</span> <span class="nam">corner_th</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t99" class="stm mis"> <span class="nam">manhattan_2d</span><span class="op">.</span><span class="nam">x</span><span class="op">[</span><span class="nam">idx</span><span class="op">]</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">nan</span><span class="strut"> </span></p>
<p id="t100" class="stm mis"> <span class="nam">manhattan_2d</span><span class="op">.</span><span class="nam">y</span><span class="op">[</span><span class="nam">idx</span><span class="op">]</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">nan</span><span class="strut"> </span></p>
<p id="t101" class="pln"> <span class="key">else</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t102" class="stm mis"> <span class="nam">msg</span> <span class="op">=</span> <span class="str">'FileType not understood (only .tra, .csv are supported)'</span><span class="strut"> </span></p>
<p id="t103" class="stm mis"> <span class="key">raise</span> <span class="nam">ValueError</span><span class="op">(</span><span class="nam">msg</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t104" class="stm mis"> <span class="nam">cameras_extrinsics</span><span class="op">[</span><span class="nam">cam_i</span><span class="op">]</span> <span class="op">=</span> <span class="nam">dict</span><span class="op">(</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t105" class="stm mis"> <span class="nam">cameras_extrinsics</span><span class="op">[</span><span class="nam">cam_i</span><span class="op">]</span><span class="op">[</span><span class="str">'pose'</span><span class="op">]</span> <span class="op">=</span> <span class="nam">camera_pose</span><span class="op">(</span><span class="strut"> </span></p>
<p id="t106" class="pln"> <span class="nam">manhattan_2d</span><span class="op">,</span> <span class="nam">manhattan_3d</span><span class="op">,</span> <span class="nam">cameras_intrinsics</span><span class="op">[</span><span class="nam">cam_i</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t107" class="stm mis"> <span class="key">return</span> <span class="nam">cameras_extrinsics</span><span class="strut"> </span></p>
<p id="t108" class="pln"><span class="strut"> </span></p>
<p id="t109" class="pln"><span class="strut"> </span></p>
<p id="t110" class="stm mis"><span class="key">def</span> <span class="nam">plot_manhattan</span><span class="op">(</span><span class="nam">manhattan_3d</span><span class="op">,</span> <span class="nam">figsize</span><span class="op">=</span><span class="op">(</span><span class="num">15</span><span class="op">,</span> <span class="num">15</span><span class="op">)</span><span class="op">,</span> <span class="nam">unit</span><span class="op">=</span><span class="key">None</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t111" class="pln"> <span class="str">"""Plot a manhattan dataframe</span><span class="strut"> </span></p>
<p id="t112" class="pln"><span class="strut"> </span></p>
<p id="t113" class="pln"><span class="str"> :param manhattan_3d: x,y,z coordinates of each tower</span><span class="strut"> </span></p>
<p id="t114" class="pln"><span class="str"> :param figsize: figure size</span><span class="strut"> </span></p>
<p id="t115" class="pln"><span class="str"> :returns: figure and axis handles</span><span class="strut"> </span></p>
<p id="t116" class="pln"><span class="strut"> </span></p>
<p id="t117" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t118" class="stm mis"> <span class="key">if</span> <span class="nam">unit</span> <span class="key">is</span> <span class="key">None</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t119" class="stm mis"> <span class="key">try</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t120" class="stm mis"> <span class="nam">unit</span> <span class="op">=</span> <span class="nam">manhattan</span><span class="op">.</span><span class="nam">unit</span><span class="strut"> </span></p>
<p id="t121" class="stm mis"> <span class="key">except</span> <span class="nam">AttributeError</span> <span class="key">as</span> <span class="nam">e</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t122" class="stm mis"> <span class="key">pass</span><span class="strut"> </span></p>
<p id="t123" class="stm mis"> <span class="key">if</span> <span class="nam">unit</span> <span class="key">is</span> <span class="key">None</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t124" class="stm mis"> <span class="nam">unit</span> <span class="op">=</span> <span class="str">'no-unit'</span><span class="strut"> </span></p>
<p id="t125" class="pln"><span class="strut"> </span></p><p id="t126" class="stm mis"> <span class="nam">fig</span> <span class="op">=</span> <span class="nam">plt</span><span class="op">.</span><span class="nam">figure</span><span class="op">(</span><span class="nam">figsize</span><span class="op">=</span><span class="nam">figsize</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t127" class="stm mis"> <span class="nam">ax0</span> <span class="op">=</span> <span class="nam">fig</span><span class="op">.</span><span class="nam">add_subplot</span><span class="op">(</span><span class="num">221</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t128" class="stm mis"> <span class="nam">ax1</span> <span class="op">=</span> <span class="nam">fig</span><span class="op">.</span><span class="nam">add_subplot</span><span class="op">(</span><span class="num">223</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t129" class="stm mis"> <span class="nam">ax3</span> <span class="op">=</span> <span class="nam">fig</span><span class="op">.</span><span class="nam">add_subplot</span><span class="op">(</span><span class="num">224</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t130" class="stm mis"> <span class="key">for</span> <span class="nam">ax</span><span class="op">,</span> <span class="nam">col_i</span><span class="op">,</span> <span class="nam">col_j</span> <span class="key">in</span> <span class="nam">zip</span><span class="op">(</span><span class="op">[</span><span class="nam">ax1</span><span class="op">,</span> <span class="nam">ax0</span><span class="op">,</span> <span class="nam">ax3</span><span class="op">]</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t131" class="pln"> <span class="op">[</span><span class="str">'x'</span><span class="op">,</span> <span class="str">'x'</span><span class="op">,</span> <span class="str">'z'</span><span class="op">]</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t132" class="pln"> <span class="op">[</span><span class="str">'y'</span><span class="op">,</span> <span class="str">'z'</span><span class="op">,</span> <span class="str">'y'</span><span class="op">]</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t133" class="stm mis"> <span class="nam">ax</span><span class="op">.</span><span class="nam">plot</span><span class="op">(</span><span class="nam">manhattan_3d</span><span class="op">.</span><span class="nam">loc</span><span class="op">[</span><span class="op">:</span><span class="op">,</span> <span class="nam">col_i</span><span class="op">]</span><span class="op">,</span> <span class="nam">manhattan_3d</span><span class="op">.</span><span class="nam">loc</span><span class="op">[</span><span class="op">:</span><span class="op">,</span> <span class="nam">col_j</span><span class="op">]</span><span class="op">,</span> <span class="str">'ko'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t134" class="stm mis"> <span class="nam">ax</span><span class="op">.</span><span class="nam">set_xlabel</span><span class="op">(</span><span class="str">'{} [{}]'</span><span class="op">.</span><span class="nam">format</span><span class="op">(</span><span class="nam">col_i</span><span class="op">,</span> <span class="nam">unit</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t135" class="stm mis"> <span class="nam">ax</span><span class="op">.</span><span class="nam">set_ylabel</span><span class="op">(</span><span class="str">'{} [{}]'</span><span class="op">.</span><span class="nam">format</span><span class="op">(</span><span class="nam">col_j</span><span class="op">,</span> <span class="nam">unit</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t136" class="stm mis"> <span class="nam">ax</span><span class="op">.</span><span class="nam">axis</span><span class="op">(</span><span class="str">'equal'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t137" class="pln"><span class="strut"> </span></p>
<p id="t138" class="pln"> <span class="com"># Annotate</span><span class="strut"> </span></p>
<p id="t139" class="stm mis"> <span class="key">for</span> <span class="nam">row_i</span><span class="op">,</span> <span class="nam">row_val</span> <span class="key">in</span> <span class="nam">manhattan_3d</span><span class="op">.</span><span class="nam">iterrows</span><span class="op">(</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t140" class="stm mis"> <span class="nam">shift</span> <span class="op">=</span> <span class="op">[</span><span class="num">10</span><span class="op">,</span> <span class="num">10</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t141" class="stm mis"> <span class="nam">xy</span> <span class="op">=</span> <span class="nam">row_val</span><span class="op">.</span><span class="nam">loc</span><span class="op">[</span><span class="op">[</span><span class="str">'x'</span><span class="op">,</span> <span class="str">'y'</span><span class="op">]</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t142" class="stm mis"> <span class="nam">xytext</span> <span class="op">=</span> <span class="nam">xy</span> <span class="op">-</span> <span class="num">2</span> <span class="op">*</span> <span class="op">(</span><span class="op">(</span><span class="nam">xy</span> <span class="op">></span> <span class="num">0</span><span class="op">)</span> <span class="op">-</span> <span class="num">0.5</span><span class="op">)</span> <span class="op">*</span> <span class="nam">shift</span><span class="strut"> </span></p>
<p id="t143" class="stm mis"> <span class="nam">ax1</span><span class="op">.</span><span class="nam">annotate</span><span class="op">(</span><span class="str">'{}'</span><span class="op">.</span><span class="nam">format</span><span class="op">(</span><span class="nam">row_i</span><span class="op">)</span><span class="op">,</span> <span class="nam">xy</span><span class="op">,</span> <span class="nam">xytext</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t144" class="pln"><span class="strut"> </span></p>
<p id="t145" class="stm mis"> <span class="nam">ax4</span> <span class="op">=</span> <span class="nam">fig</span><span class="op">.</span><span class="nam">add_subplot</span><span class="op">(</span><span class="num">222</span><span class="op">,</span> <span class="nam">projection</span><span class="op">=</span><span class="str">'3d'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t146" class="stm mis"> <span class="key">for</span> <span class="nam">i</span><span class="op">,</span> <span class="nam">row</span> <span class="key">in</span> <span class="nam">manhattan_3d</span><span class="op">.</span><span class="nam">iterrows</span><span class="op">(</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t147" class="stm mis"> <span class="nam">ax4</span><span class="op">.</span><span class="nam">plot</span><span class="op">(</span><span class="op">[</span><span class="nam">row</span><span class="op">.</span><span class="nam">x</span><span class="op">,</span> <span class="nam">row</span><span class="op">.</span><span class="nam">x</span><span class="op">]</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t148" class="pln"> <span class="op">[</span><span class="nam">row</span><span class="op">.</span><span class="nam">y</span><span class="op">,</span> <span class="nam">row</span><span class="op">.</span><span class="nam">y</span><span class="op">]</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t149" class="pln"> <span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="nam">row</span><span class="op">.</span><span class="nam">z</span><span class="op">]</span><span class="op">,</span> <span class="str">'k-.'</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t150" class="stm mis"> <span class="nam">ax4</span><span class="op">.</span><span class="nam">set_xlabel</span><span class="op">(</span><span class="str">'{} [{}]'</span><span class="op">.</span><span class="nam">format</span><span class="op">(</span><span class="str">'x'</span><span class="op">,</span> <span class="nam">unit</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t151" class="stm mis"> <span class="nam">ax4</span><span class="op">.</span><span class="nam">set_ylabel</span><span class="op">(</span><span class="str">'{} [{}]'</span><span class="op">.</span><span class="nam">format</span><span class="op">(</span><span class="str">'y'</span><span class="op">,</span> <span class="nam">unit</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t152" class="stm mis"> <span class="nam">ax4</span><span class="op">.</span><span class="nam">set_zlabel</span><span class="op">(</span><span class="str">'{} [{}]'</span><span class="op">.</span><span class="nam">format</span><span class="op">(</span><span class="str">'z'</span><span class="op">,</span> <span class="nam">unit</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t153" class="stm mis"> <span class="key">return</span> <span class="nam">fig</span><span class="op">,</span> <span class="op">[</span><span class="nam">ax0</span><span class="op">,</span> <span class="nam">ax1</span><span class="op">,</span> <span class="nam">ax3</span><span class="op">,</span> <span class="nam">ax4</span><span class="op">]</span><span class="strut"> </span></p>
</td>
</tr>
</table>
</div>
<div id="footer">
<div class="content">
<p>
<a class="nav" href="index.html">« index</a> <a class="nav" href="https://coverage.readthedocs.io">coverage.py v4.5.2</a>,
created at 2019-01-16 18:11
</p>
</div>
</div>
</body>
</html>