diff options
Diffstat (limited to 'ftnoir_tracker_pt')
| -rw-r--r-- | ftnoir_tracker_pt/FTNoIR_PT_Controls.ui | 629 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/camera.cpp | 174 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/camera.h | 14 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 165 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt.h | 28 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 97 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h | 9 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h | 17 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_extractor.cpp | 25 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_extractor.h | 11 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_tracker.cpp | 160 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_tracker.h | 100 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/trans_calib.cpp | 2 |
13 files changed, 595 insertions, 836 deletions
diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui index 73b1f767..0e6048c3 100644 --- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui +++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui @@ -9,12 +9,12 @@ <rect> <x>0</x> <y>0</y> - <width>453</width> - <height>621</height> + <width>397</width> + <height>588</height> </rect> </property> <property name="sizePolicy"> - <sizepolicy hsizetype="Minimum" vsizetype="Minimum"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> <horstretch>0</horstretch> <verstretch>0</verstretch> </sizepolicy> @@ -36,82 +36,10 @@ <property name="sizeConstraint"> <enum>QLayout::SetFixedSize</enum> </property> - <item row="1" column="0"> - <widget class="QGroupBox" name="groupBox_5"> - <property name="title"> - <string>Status</string> - </property> - <layout class="QGridLayout" name="gridLayout_10"> - <item row="2" column="2"> - <widget class="QDialogButtonBox" name="buttonBox"> - <property name="standardButtons"> - <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set> - </property> - </widget> - </item> - <item row="1" column="0"> - <widget class="QLabel" name="label_3"> - <property name="text"> - <string>Extracted Points:</string> - </property> - </widget> - </item> - <item row="0" column="0"> - <widget class="QLabel" name="label_38"> - <property name="text"> - <string>Camera Info:</string> - </property> - </widget> - </item> - <item row="2" column="1"> - <widget class="QDialogButtonBox" name="buttonBox_2"> - <property name="standardButtons"> - <set>QDialogButtonBox::Apply</set> - </property> - <property name="centerButtons"> - <bool>true</bool> - </property> - </widget> - </item> - <item row="0" column="2"> - <widget class="QLabel" name="caminfo_label"> - <property name="sizePolicy"> - <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> - <horstretch>0</horstretch> - <verstretch>0</verstretch> - </sizepolicy> - </property> - <property name="minimumSize"> - <size> - <width>120</width> - <height>0</height> - </size> - </property> - <property name="text"> - <string/> - </property> - </widget> - </item> - <item row="1" column="2"> - <widget class="QLabel" name="pointinfo_label"> - <property name="minimumSize"> - <size> - <width>50</width> - <height>0</height> - </size> - </property> - <property name="text"> - <string/> - </property> - </widget> - </item> - </layout> - </widget> - </item> <item row="0" column="0"> <widget class="QTabWidget" name="tabWidget"> <property name="sizePolicy"> - <sizepolicy hsizetype="Minimum" vsizetype="Minimum"> + <sizepolicy hsizetype="Preferred" vsizetype="MinimumExpanding"> <horstretch>0</horstretch> <verstretch>0</verstretch> </sizepolicy> @@ -155,21 +83,30 @@ </property> </widget> </item> - <item row="0" column="1"> - <widget class="QComboBox" name="camdevice_combo"> + <item row="1" column="1"> + <widget class="QSpinBox" name="res_x_spin"> <property name="sizePolicy"> <sizepolicy hsizetype="Preferred" vsizetype="Fixed"> <horstretch>0</horstretch> <verstretch>0</verstretch> </sizepolicy> </property> - <property name="minimumContentsLength"> + <property name="toolTip"> + <string>Desired capture width</string> + </property> + <property name="suffix"> + <string> px</string> + </property> + <property name="maximum"> + <number>2000</number> + </property> + <property name="singleStep"> <number>10</number> </property> </widget> </item> - <item row="1" column="0"> - <widget class="QLabel" name="label_36"> + <item row="2" column="0"> + <widget class="QLabel" name="label_41"> <property name="sizePolicy"> <sizepolicy hsizetype="Expanding" vsizetype="Preferred"> <horstretch>0</horstretch> @@ -177,34 +114,38 @@ </sizepolicy> </property> <property name="text"> - <string>Width</string> + <string>Height</string> </property> </widget> </item> - <item row="1" column="1"> - <widget class="QSpinBox" name="res_x_spin"> + <item row="0" column="1"> + <widget class="QComboBox" name="camdevice_combo"> <property name="sizePolicy"> <sizepolicy hsizetype="Preferred" vsizetype="Fixed"> <horstretch>0</horstretch> <verstretch>0</verstretch> </sizepolicy> </property> - <property name="toolTip"> - <string>Desired capture width</string> - </property> - <property name="suffix"> - <string> px</string> + <property name="minimumContentsLength"> + <number>10</number> </property> - <property name="maximum"> - <number>2000</number> + </widget> + </item> + <item row="1" column="0"> + <widget class="QLabel" name="label_36"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Expanding" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> </property> - <property name="singleStep"> - <number>10</number> + <property name="text"> + <string>Width</string> </property> </widget> </item> - <item row="2" column="0"> - <widget class="QLabel" name="label_41"> + <item row="4" column="0"> + <widget class="QLabel" name="label_4"> <property name="sizePolicy"> <sizepolicy hsizetype="Expanding" vsizetype="Preferred"> <horstretch>0</horstretch> @@ -212,7 +153,7 @@ </sizepolicy> </property> <property name="text"> - <string>Height</string> + <string>Field of view</string> </property> </widget> </item> @@ -273,116 +214,39 @@ </property> </widget> </item> - </layout> - </widget> - </item> - <item> - <widget class="QGroupBox" name="groupBox_4"> - <property name="title"> - <string>Camera orientation</string> - </property> - <layout class="QGridLayout" name="gridLayout_3"> - <item row="0" column="0"> - <widget class="QLabel" name="label_18"> - <property name="text"> - <string>Roll</string> - </property> - <property name="buddy"> - <cstring>camroll_combo</cstring> - </property> - </widget> - </item> - <item row="0" column="1"> - <widget class="QComboBox" name="camroll_combo"> - <property name="sizePolicy"> - <sizepolicy hsizetype="Minimum" vsizetype="Fixed"> - <horstretch>0</horstretch> - <verstretch>0</verstretch> - </sizepolicy> - </property> - <property name="minimumSize"> - <size> - <width>0</width> - <height>0</height> - </size> - </property> - <property name="toolTip"> - <string>Rotation of the camera image</string> - </property> - </widget> - </item> - <item row="1" column="0"> - <widget class="QLabel" name="label_4"> - <property name="text"> - <string>Pitch</string> - </property> - <property name="buddy"> - <cstring>campitch_spin</cstring> - </property> - </widget> - </item> - <item row="1" column="1"> - <widget class="QSpinBox" name="campitch_spin"> - <property name="contextMenuPolicy"> - <enum>Qt::DefaultContextMenu</enum> - </property> - <property name="toolTip"> - <string>The angle the camera is facing upwards</string> - </property> + <item row="4" column="1"> + <widget class="QSpinBox" name="fov"> <property name="suffix"> - <string> deg</string> + <string>°</string> + </property> + <property name="prefix"> + <string/> </property> <property name="minimum"> - <number>-180</number> + <number>10</number> </property> <property name="maximum"> - <number>180</number> + <number>90</number> </property> </widget> </item> - <item row="1" column="2"> + <item row="5" column="0"> <widget class="QLabel" name="label_5"> - <property name="text"> - <string>positive = upwards</string> + <property name="sizePolicy"> + <sizepolicy hsizetype="Expanding" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> </property> - </widget> - </item> - <item row="2" column="0"> - <widget class="QLabel" name="label_20"> <property name="text"> - <string>Yaw</string> - </property> - <property name="buddy"> - <cstring>camyaw_spin</cstring> - </property> - </widget> - </item> - <item row="2" column="1"> - <widget class="QSpinBox" name="camyaw_spin"> - <property name="contextMenuPolicy"> - <enum>Qt::DefaultContextMenu</enum> - </property> - <property name="toolTip"> - <string>The angle the camera is facing leftwards</string> - </property> - <property name="suffix"> - <string> deg</string> - </property> - <property name="prefix"> - <string/> - </property> - <property name="minimum"> - <number>-180</number> - </property> - <property name="maximum"> - <number>180</number> + <string>Dynamic pose resolution</string> </property> </widget> </item> - <item row="2" column="2"> - <widget class="QLabel" name="label_21"> + <item row="5" column="1"> + <widget class="QCheckBox" name="dynamic_pose"> <property name="text"> - <string>positve = left</string> + <string/> </property> </widget> </item> @@ -520,14 +384,20 @@ <attribute name="title"> <string>Model</string> </attribute> - <layout class="QVBoxLayout" name="verticalLayout_16"> - <item> + <layout class="QGridLayout" name="gridLayout_3"> + <item row="0" column="0"> <widget class="QTabWidget" name="model_tabs"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Minimum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> <property name="tabShape"> <enum>QTabWidget::Rounded</enum> </property> <property name="currentIndex"> - <number>1</number> + <number>0</number> </property> <property name="usesScrollButtons"> <bool>false</bool> @@ -545,6 +415,18 @@ <layout class="QGridLayout" name="gridLayout_6"> <item row="0" column="0"> <widget class="QGroupBox" name="groupBox_8"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Minimum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="minimumSize"> + <size> + <width>331</width> + <height>208</height> + </size> + </property> <property name="title"> <string>Model Dimensions</string> </property> @@ -558,7 +440,7 @@ </rect> </property> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -570,14 +452,14 @@ <widget class="QSpinBox" name="clip_bheight_spin"> <property name="geometry"> <rect> - <x>110</x> - <y>115</y> + <x>150</x> + <y>130</y> <width>100</width> <height>22</height> </rect> </property> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -589,7 +471,7 @@ <widget class="QLabel" name="label_44"> <property name="geometry"> <rect> - <x>40</x> + <x>65</x> <y>55</y> <width>71</width> <height>111</height> @@ -605,8 +487,8 @@ <widget class="QLabel" name="label_50"> <property name="geometry"> <rect> - <x>10</x> - <y>35</y> + <x>20</x> + <y>40</y> <width>46</width> <height>13</height> </rect> @@ -619,13 +501,13 @@ <property name="geometry"> <rect> <x>50</x> - <y>165</y> + <y>160</y> <width>100</width> <height>22</height> </rect> </property> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -637,14 +519,14 @@ <widget class="QSpinBox" name="clip_theight_spin"> <property name="geometry"> <rect> - <x>110</x> - <y>75</y> + <x>150</x> + <y>70</y> <width>100</width> <height>22</height> </rect> </property> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -656,7 +538,7 @@ <widget class="QLabel" name="label_51"> <property name="geometry"> <rect> - <x>255</x> + <x>290</x> <y>40</y> <width>46</width> <height>13</height> @@ -669,8 +551,8 @@ <widget class="QLabel" name="label_45"> <property name="geometry"> <rect> - <x>265</x> - <y>60</y> + <x>300</x> + <y>70</y> <width>21</width> <height>111</height> </rect> @@ -693,14 +575,20 @@ <layout class="QVBoxLayout" name="verticalLayout_14"> <item> <widget class="QGroupBox" name="groupBox_9"> + <property name="minimumSize"> + <size> + <width>331</width> + <height>208</height> + </size> + </property> <property name="title"> <string>Model Dimensions</string> </property> <widget class="QLabel" name="label_46"> <property name="geometry"> <rect> - <x>30</x> - <y>90</y> + <x>100</x> + <y>60</y> <width>111</width> <height>81</height> </rect> @@ -716,7 +604,7 @@ <property name="geometry"> <rect> <x>20</x> - <y>35</y> + <y>40</y> <width>46</width> <height>13</height> </rect> @@ -728,14 +616,14 @@ <widget class="QSpinBox" name="cap_length_spin"> <property name="geometry"> <rect> - <x>20</x> - <y>70</y> + <x>90</x> + <y>40</y> <width>101</width> <height>22</height> </rect> </property> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -744,37 +632,11 @@ <number>65535</number> </property> </widget> - <widget class="QLabel" name="label_54"> - <property name="geometry"> - <rect> - <x>140</x> - <y>90</y> - <width>16</width> - <height>16</height> - </rect> - </property> - <property name="text"> - <string>R</string> - </property> - </widget> - <widget class="QLabel" name="label_55"> - <property name="geometry"> - <rect> - <x>290</x> - <y>85</y> - <width>16</width> - <height>16</height> - </rect> - </property> - <property name="text"> - <string>R</string> - </property> - </widget> <widget class="QLabel" name="label_47"> <property name="geometry"> <rect> - <x>270</x> - <y>85</y> + <x>220</x> + <y>100</y> <width>81</width> <height>81</height> </rect> @@ -789,14 +651,14 @@ <widget class="QSpinBox" name="cap_width_spin"> <property name="geometry"> <rect> - <x>255</x> - <y>55</y> - <width>100</width> + <x>240</x> + <y>70</y> + <width>81</width> <height>22</height> </rect> </property> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -808,8 +670,8 @@ <widget class="QLabel" name="label_49"> <property name="geometry"> <rect> - <x>290</x> - <y>35</y> + <x>240</x> + <y>40</y> <width>46</width> <height>13</height> </rect> @@ -821,14 +683,14 @@ <widget class="QSpinBox" name="cap_height_spin"> <property name="geometry"> <rect> - <x>60</x> - <y>120</y> - <width>116</width> + <x>20</x> + <y>90</y> + <width>81</width> <height>22</height> </rect> </property> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -868,7 +730,7 @@ <item row="2" column="5"> <widget class="QSpinBox" name="m2y_spin"> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -894,7 +756,7 @@ <item row="1" column="2"> <widget class="QSpinBox" name="m1x_spin"> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -907,7 +769,7 @@ <item row="2" column="2"> <widget class="QSpinBox" name="m1y_spin"> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -920,7 +782,7 @@ <item row="3" column="5"> <widget class="QSpinBox" name="m2z_spin"> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -933,7 +795,7 @@ <item row="1" column="5"> <widget class="QSpinBox" name="m2x_spin"> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -945,15 +807,21 @@ </item> <item row="0" column="0" colspan="6"> <widget class="QLabel" name="label_56"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> <property name="text"> - <string><html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p><p>Use any units you want, e.g. millimeters, inches, parsecs...</p></body></html></string> + <string><html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p><p>Use any units you want, not necessarily centimeters.</p></body></html></string> </property> </widget> </item> <item row="3" column="2"> <widget class="QSpinBox" name="m1z_spin"> <property name="suffix"> - <string> units</string> + <string> mm</string> </property> <property name="minimum"> <number>-65535</number> @@ -1011,7 +879,7 @@ </sizepolicy> </property> <property name="text"> - <string><html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">3</span></p></body></html></string> + <string><html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">2</span></p></body></html></string> </property> </widget> </item> @@ -1048,86 +916,122 @@ </widget> </widget> </item> - <item> + <item row="1" column="0"> <widget class="QGroupBox" name="groupBox_10"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Maximum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> <property name="title"> <string>Model position</string> </property> <layout class="QGridLayout" name="gridLayout_4"> - <item row="1" column="1"> - <widget class="QSpinBox" name="tx_spin"> - <property name="suffix"> - <string> units</string> - </property> - <property name="minimum"> - <number>-65535</number> - </property> - <property name="maximum"> - <number>65536</number> - </property> + <item row="0" column="0"> + <widget class="QFrame" name="frame_2"> + <property name="frameShape"> + <enum>QFrame::NoFrame</enum> + </property> + <property name="frameShadow"> + <enum>QFrame::Raised</enum> + </property> + <layout class="QGridLayout" name="gridLayout_11"> + <item row="0" column="0"> + <widget class="QLabel" name="label_61"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Maximum" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="text"> + <string>x:</string> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QSpinBox" name="tx_spin"> + <property name="suffix"> + <string> mm</string> + </property> + <property name="minimum"> + <number>-65535</number> + </property> + <property name="maximum"> + <number>65536</number> + </property> + </widget> + </item> + <item row="1" column="0"> + <widget class="QLabel" name="label_62"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Maximum" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="text"> + <string>y:</string> + </property> + </widget> + </item> + <item row="1" column="1"> + <widget class="QSpinBox" name="ty_spin"> + <property name="suffix"> + <string> mm</string> + </property> + <property name="minimum"> + <number>-65535</number> + </property> + <property name="maximum"> + <number>65536</number> + </property> + </widget> + </item> + <item row="2" column="0"> + <widget class="QLabel" name="label_66"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Maximum" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="text"> + <string>z:</string> + </property> + </widget> + </item> + <item row="2" column="1"> + <widget class="QSpinBox" name="tz_spin"> + <property name="suffix"> + <string> mm</string> + </property> + <property name="minimum"> + <number>-65535</number> + </property> + <property name="maximum"> + <number>65536</number> + </property> + </widget> + </item> + </layout> </widget> </item> - <item row="0" column="0" colspan="3"> + <item row="0" column="1"> <widget class="QLabel" name="label_59"> <property name="text"> - <string><html><head/><body><p>Translation from head center to model reference point<br/> in default pose</p></body></html></string> + <string><html><head/><body><p>Only pitch and yaw during calibration.</p><p>Don't roll and don't translate.</p></body></html></string> </property> </widget> </item> <item row="2" column="1"> - <widget class="QSpinBox" name="ty_spin"> - <property name="suffix"> - <string> units</string> - </property> - <property name="minimum"> - <number>-65535</number> - </property> - <property name="maximum"> - <number>65536</number> - </property> - </widget> - </item> - <item row="2" column="0"> - <widget class="QLabel" name="label_62"> - <property name="text"> - <string>y:</string> - </property> - </widget> - </item> - <item row="3" column="0"> - <widget class="QLabel" name="label_66"> - <property name="text"> - <string>z:</string> - </property> - </widget> - </item> - <item row="1" column="0"> - <widget class="QLabel" name="label_61"> - <property name="text"> - <string>x:</string> - </property> - </widget> - </item> - <item row="3" column="1"> - <widget class="QSpinBox" name="tz_spin"> - <property name="suffix"> - <string> units</string> - </property> - <property name="minimum"> - <number>-65535</number> - </property> - <property name="maximum"> - <number>65536</number> - </property> - </widget> - </item> - <item row="4" column="2"> <widget class="QPushButton" name="tcalib_button"> <property name="enabled"> <bool>false</bool> </property> <property name="text"> - <string>Calibrate</string> + <string>Toggle calibration</string> </property> <property name="checkable"> <bool>true</bool> @@ -1174,17 +1078,89 @@ </widget> </widget> </item> + <item row="1" column="0"> + <widget class="QGroupBox" name="groupBox_5"> + <property name="title"> + <string>Status</string> + </property> + <layout class="QGridLayout" name="gridLayout_10"> + <item row="1" column="0"> + <widget class="QLabel" name="label_3"> + <property name="text"> + <string>Extracted Points:</string> + </property> + </widget> + </item> + <item row="2" column="1"> + <widget class="QDialogButtonBox" name="buttonBox"> + <property name="standardButtons"> + <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set> + </property> + </widget> + </item> + <item row="0" column="0"> + <widget class="QLabel" name="label_38"> + <property name="text"> + <string>Camera Info:</string> + </property> + </widget> + </item> + <item row="1" column="1"> + <widget class="QLabel" name="pointinfo_label"> + <property name="minimumSize"> + <size> + <width>50</width> + <height>0</height> + </size> + </property> + <property name="text"> + <string/> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QLabel" name="caminfo_label"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="minimumSize"> + <size> + <width>120</width> + <height>0</height> + </size> + </property> + <property name="text"> + <string/> + </property> + </widget> + </item> + </layout> + </widget> + </item> </layout> </widget> <tabstops> <tabstop>tabWidget</tabstop> <tabstop>camdevice_combo</tabstop> <tabstop>res_x_spin</tabstop> - <tabstop>camroll_combo</tabstop> - <tabstop>campitch_spin</tabstop> - <tabstop>camyaw_spin</tabstop> + <tabstop>res_y_spin</tabstop> + <tabstop>fps_spin</tabstop> + <tabstop>fov</tabstop> <tabstop>threshold_slider</tabstop> + <tabstop>mindiam_spin</tabstop> + <tabstop>threshold_secondary_slider</tabstop> + <tabstop>maxdiam_spin</tabstop> <tabstop>model_tabs</tabstop> + <tabstop>clip_tlength_spin</tabstop> + <tabstop>clip_theight_spin</tabstop> + <tabstop>clip_bheight_spin</tabstop> + <tabstop>clip_blength_spin</tabstop> + <tabstop>cap_length_spin</tabstop> + <tabstop>cap_width_spin</tabstop> + <tabstop>cap_height_spin</tabstop> <tabstop>m1x_spin</tabstop> <tabstop>m1y_spin</tabstop> <tabstop>m1z_spin</tabstop> @@ -1194,6 +1170,7 @@ <tabstop>tx_spin</tabstop> <tabstop>ty_spin</tabstop> <tabstop>tz_spin</tabstop> + <tabstop>tcalib_button</tabstop> </tabstops> <resources> <include location="ftnoir_tracker_pt.qrc"/> diff --git a/ftnoir_tracker_pt/camera.cpp b/ftnoir_tracker_pt/camera.cpp index fec503e1..432e0158 100644 --- a/ftnoir_tracker_pt/camera.cpp +++ b/ftnoir_tracker_pt/camera.cpp @@ -119,13 +119,12 @@ void Camera::set_fps(int fps) void Camera::set_res(int x_res, int y_res) { - if (cam_desired.res_x != x_res || cam_desired.res_y != y_res) - { - cam_desired.res_x = x_res; - cam_desired.res_y = y_res; - _set_res(); - _set_fps(); - } + if (cam_desired.res_x != x_res || cam_desired.res_y != y_res) + { + cam_desired.res_x = x_res; + cam_desired.res_y = y_res; + _set_res(); + } } bool Camera::get_frame(float dt, cv::Mat* frame) @@ -143,20 +142,20 @@ bool Camera::get_frame(float dt, cv::Mat* frame) return new_frame; } -// ---------------------------------------------------------------------------- -#ifdef OPENTRACK_API void CVCamera::start() { + if (cap) + delete cap; cap = new VideoCapture(desired_index); - // extract camera info + _set_res(); + _set_fps(); + // extract camera info if (cap->isOpened()) - { - _set_fps(); - _set_res(); - active = true; - active_index = desired_index; - cam_info.res_x = cap->get(CV_CAP_PROP_FRAME_WIDTH); - cam_info.res_y = cap->get(CV_CAP_PROP_FRAME_HEIGHT); + { + active = true; + active_index = desired_index; + cam_info.res_x = 0; + cam_info.res_y = 0; } else { delete cap; cap = nullptr; @@ -169,25 +168,28 @@ void CVCamera::stop() { cap->release(); delete cap; + cap = nullptr; } - active = false; + active = false; } bool CVCamera::_get_frame(Mat* frame) { if (cap && cap->isOpened()) - { + { Mat img; for (int i = 0; i < 100 && !cap->read(img); i++) ;; - if (img.empty()) - return false; + if (img.empty()) + return false; - *frame = img; - return true; - } - return false; + *frame = img; + cam_info.res_x = img.cols; + cam_info.res_y = img.rows; + return true; + } + return false; } void CVCamera::_set_fps() @@ -197,13 +199,11 @@ void CVCamera::_set_fps() void CVCamera::_set_res() { - if (cap) - { + if (cap) + { cap->set(CV_CAP_PROP_FRAME_WIDTH, cam_desired.res_x); cap->set(CV_CAP_PROP_FRAME_HEIGHT, cam_desired.res_y); - cam_info.res_x = cap->get(CV_CAP_PROP_FRAME_WIDTH); - cam_info.res_y = cap->get(CV_CAP_PROP_FRAME_HEIGHT); - } + } } void CVCamera::_set_device_index() { @@ -214,117 +214,3 @@ void CVCamera::_set_device_index() } cap = new VideoCapture(desired_index); } - -#else -// ---------------------------------------------------------------------------- -VICamera::VICamera() : frame_buffer(NULL) -{ - VI.listDevices(); -} - -void VICamera::start() -{ - if (desired_index >= 0) - { - if (cam_desired.res_x == 0 || cam_desired.res_y == 0) - VI.setupDevice(desired_index); - else - VI.setupDevice(desired_index, cam_desired.res_x, cam_desired.res_y); - - active = true; - active_index = desired_index; - - cam_info.res_x = VI.getWidth(active_index); - cam_info.res_y = VI.getHeight(active_index); - new_frame = cv::Mat(cam_info.res_y, cam_info.res_x, CV_8UC3); - // If matrix is not continuous we have to copy manually via frame_buffer - if (!new_frame.isContinuous()) { - unsigned int size = VI.getSize(active_index); - frame_buffer = new unsigned char[size]; - } - } -} - -void VICamera::stop() -{ - if (active) - { - VI.stopDevice(active_index); - } - if (frame_buffer) - { - delete[] frame_buffer; - frame_buffer = NULL; - } - active = false; -} - -bool VICamera::_get_frame(Mat* frame) -{ - if (active && VI.isFrameNew(active_index)) - { - if (new_frame.isContinuous()) - { - VI.getPixels(active_index, new_frame.data, false, true); - } - else - { - // If matrix is not continuous we have to copy manually via frame_buffer - VI.getPixels(active_index, frame_buffer, false, true); - new_frame = cv::Mat(cam_info.res_y, cam_info.res_x, CV_8UC3, frame_buffer).clone(); - } - *frame = new_frame; - return true; - } - return false; -} - -void VICamera::_set_device_index() -{ - if (active) restart(); -} - -void VICamera::_set_f() -{ - cam_info.f = cam_desired.f; -} - -void VICamera::_set_fps() -{ - bool was_active = active; - if (active) stop(); - VI.setIdealFramerate(desired_index, cam_desired.fps); - if (was_active) start(); -} - -void VICamera::_set_res() -{ - if (active) restart(); -} -#endif - -// ---------------------------------------------------------------------------- -Mat FrameRotation::rotate_frame(Mat frame) -{ - switch (rotation) - { - case CLOCKWISE: - { - Mat dst; - transpose(frame, dst); - flip(dst, dst, 1); - return dst; - } - - case COUNTER_CLOCKWISE: - { - Mat dst; - transpose(frame, dst); - flip(dst, dst, 0); - return dst; - } - - default: - return frame; - } -} diff --git a/ftnoir_tracker_pt/camera.h b/ftnoir_tracker_pt/camera.h index 889bf2d3..2bce6f35 100644 --- a/ftnoir_tracker_pt/camera.h +++ b/ftnoir_tracker_pt/camera.h @@ -5,8 +5,7 @@ * copyright notice and this permission notice appear in all copies. */ -#ifndef CAMERA_H -#define CAMERA_H +#pragma once #include <opencv2/core/core.hpp> #ifndef OPENTRACK_API @@ -128,14 +127,3 @@ enum RotationType ZERO = 1, COUNTER_CLOCKWISE = 2 }; - -// ---------------------------------------------------------------------------- -class FrameRotation -{ -public: - RotationType rotation; - - cv::Mat rotate_frame(cv::Mat frame); -}; - -#endif //CAMERA_H diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 9b3795b9..acf4daa0 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -21,10 +21,11 @@ using namespace cv; Tracker::Tracker() : mutex(QMutex::Recursive), commands(0), - video_widget(NULL), + video_widget(NULL), video_frame(NULL), - new_settings(nullptr) + ever_success(false) { + connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings())); } Tracker::~Tracker() @@ -48,6 +49,13 @@ void Tracker::reset_command(Command command) commands &= ~command; } +float Tracker::get_focal_length() +{ + static constexpr float pi = 3.1415926f; + const float fov = static_cast<int>(s.fov) * pi / 180.f; + return 0.5f / tan(0.5f * fov); +} + void Tracker::run() { #ifdef PT_PERF_LOG @@ -57,21 +65,43 @@ void Tracker::run() #endif while((commands & ABORT) == 0) - { - apply_inner(); - const double dt = time.start() * 1e-9; + { + const double dt = time.elapsed() * 1e-9; + time.start(); + cv::Mat frame; const bool new_frame = camera.get_frame(dt, &frame); if (new_frame && !frame.empty()) { QMutexLocker lock(&mutex); - frame = frame_rotation.rotate_frame(frame); - const std::vector<cv::Vec2f>& points = point_extractor.extract_points(frame); - for (auto p : points) + std::vector<cv::Vec2f> points = point_extractor.extract_points(frame); + + bool success = points.size() == PointModel::N_POINTS; + + ever_success |= success; + + if (success) + point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose); + { + Affine X_CM = pose(); + Affine X_MH(Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below + Affine X_GH = X_CM * X_MH; + cv::Vec3f p = X_GH.t; // head (center?) position in global space + float fx = get_focal_length(); + cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen + + points.push_back(p_); + } + + for (int i = 0; i < points.size(); i++) + { + auto& p = points[i]; auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2); cv::Scalar color(0, 255, 0); + if (i == points.size()-1) + color = cv::Scalar(0, 0, 255); cv::line(frame, cv::Point(p2.x - 20, p2.y), cv::Point(p2.x + 20, p2.y), @@ -83,8 +113,7 @@ void Tracker::run() color, 4); } - if (points.size() == PointModel::N_POINTS) - point_tracker.track(points, model); + video_widget->update_image(frame); } #ifdef PT_PERF_LOG @@ -92,64 +121,18 @@ void Tracker::run() if (!frame.empty()) log_stream<<" fps: "<<camera.get_info().fps; log_stream<<"\n"; #endif - } - - qDebug()<<"Tracker:: Thread stopping"; -} -void Tracker::apply(settings& s) -{ - // caller guarantees object lifetime - new_settings = &s; + } + qDebug()<<"Tracker:: Thread stopping"; } -void Tracker::apply_inner() +void Tracker::apply_settings() { - settings* tmp = new_settings.exchange(nullptr); - if (tmp == nullptr) - return; - reset(); - auto& s = *tmp; qDebug()<<"Tracker:: Applying settings"; - - { - cv::Vec3f M01(s.m01_x, s.m01_y, s.m01_z); - cv::Vec3f M02(s.m02_x, s.m02_y, s.m02_z); - model = PointModel(M01, M02); - } + QMutexLocker lock(&mutex); camera.set_device_index(s.cam_index); camera.set_res(s.cam_res_x, s.cam_res_y); camera.set_fps(s.cam_fps); - frame_rotation.rotation = static_cast<RotationType>(static_cast<int>(s.cam_roll)); - point_extractor.threshold_val = s.threshold; - point_extractor.threshold_secondary_val = s.threshold_secondary; - point_extractor.min_size = s.min_point_size; - point_extractor.max_size = s.max_point_size; - t_MH = cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z); - R_GC = Matx33f( cos(deg2rad*s.cam_yaw), 0, sin(deg2rad*s.cam_yaw), - 0, 1, 0, - -sin(deg2rad*s.cam_yaw), 0, cos(deg2rad*s.cam_yaw)); - R_GC = R_GC * Matx33f( 1, 0, 0, - 0, cos(deg2rad*s.cam_pitch), sin(deg2rad*s.cam_pitch), - 0, -sin(deg2rad*s.cam_pitch), cos(deg2rad*s.cam_pitch)); - - FrameTrafo X_MH(Matx33f::eye(), t_MH); - X_GH_0 = R_GC * X_MH; - qDebug()<<"Tracker::apply ends"; -} - -void Tracker::reset() -{ - QMutexLocker lock(&mutex); - point_tracker.reset(); -} - -void Tracker::center() -{ - point_tracker.reset(); // we also do a reset here since there is no reset shortkey yet - QMutexLocker lock(&mutex); - FrameTrafo X_CM_0 = point_tracker.pose(); - FrameTrafo X_MH(Matx33f::eye(), t_MH); - X_GH_0 = R_GC * X_CM_0 * X_MH; + qDebug()<<"Tracker::apply ends"; } void Tracker::start_tracker(QFrame *parent_window) @@ -163,8 +146,7 @@ void Tracker::start_tracker(QFrame *parent_window) video_layout->addWidget(video_widget); video_frame->setLayout(video_layout); video_widget->resize(video_frame->width(), video_frame->height()); - apply(s); - apply_inner(); + apply_settings(); camera.start(); start(); } @@ -172,7 +154,7 @@ void Tracker::start_tracker(QFrame *parent_window) #ifndef OPENTRACK_API void Tracker::StopTracker(bool exit) { - set_command(PAUSE); + set_command(PAUSE); } #endif @@ -182,37 +164,38 @@ void Tracker::StopTracker(bool exit) void Tracker::data(THeadPoseData *data) { - { - QMutexLocker lock(&mutex); - - FrameTrafo X_CM = point_tracker.pose(); - FrameTrafo X_MH(Matx33f::eye(), t_MH); - FrameTrafo X_GH = R_GC * X_CM * X_MH; - Matx33f R = X_GH.R * X_GH_0.R.t(); - Vec3f t = X_GH.t - X_GH_0.t; - + if (ever_success) + { + Affine X_CM = pose(); + + Affine X_MH(Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); + Affine X_GH = X_CM * X_MH; + + Matx33f R = X_GH.R; + Vec3f t = X_GH.t; + + // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame + // -z -> x, y -> z, x -> -y + Matx33f R_EG(0, 0,-1, + -1, 0, 0, + 0, 1, 0); + R = R_EG * R * R_EG.t(); + + // extract rotation angles + float alpha, beta, gamma; + beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) ); + alpha = atan2( R(1,0), R(0,0)); + gamma = atan2( R(2,1), R(2,2)); + + // extract rotation angles + data[Yaw] = rad2deg * alpha; + data[Pitch] = -rad2deg * beta; + data[Roll] = rad2deg * gamma; // get translation(s) data[TX] = t[0] / 10.0; // convert to cm data[TY] = t[1] / 10.0; data[TZ] = t[2] / 10.0; - - // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame - // -z -> x, y -> z, x -> -y - Matx33f R_EG( 0, 0,-1, - -1, 0, 0, - 0, 1, 0); - R = R_EG * R * R_EG.t(); - - // extract rotation angles - float alpha, beta, gamma; - beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) ); - alpha = atan2( R(1,0), R(0,0)); - gamma = atan2( R(2,1), R(2,2)); - - data[Yaw] = rad2deg * alpha; - data[Pitch] = - rad2deg * beta; // FTNoIR expects a minus here - data[Roll] = rad2deg * gamma; - } + } } //----------------------------------------------------------------------------- diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h index 921d61cf..349cf2c8 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h @@ -33,22 +33,20 @@ //----------------------------------------------------------------------------- // Constantly processes the tracking chain in a separate thread -class Tracker : public ITracker, protected QThread +class Tracker : public QThread, public ITracker { + Q_OBJECT public: Tracker(); ~Tracker() override; void start_tracker(QFrame* parent_window) override; void data(double* data) override; - void apply(settings& s); - void apply_inner(); - void center(); - void reset(); // reset the trackers internal state variables - - void pose(FrameTrafo* X_CM) { QMutexLocker lock(&mutex); *X_CM = point_tracker.pose(); } + Affine pose() { QMutexLocker lock(&mutex); return point_tracker.pose(); } int get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); } void get_cam_info(CamInfo* info) { QMutexLocker lock(&mutex); *info = camera.get_info(); } +public slots: + void apply_settings(); protected: void run() override; private: @@ -59,31 +57,25 @@ private: }; void set_command(Command command); void reset_command(Command command); + + float get_focal_length(); + volatile int commands; CVCamera camera; - FrameRotation frame_rotation; PointExtractor point_extractor; PointTracker point_tracker; - FrameTrafo X_GH_0; // for centering - cv::Vec3f t_MH; // translation from model frame to head frame - cv::Matx33f R_GC; // rotation from opengl reference frame to camera frame - - // --- ui --- - cv::Mat frame; // the output frame for display - PTVideoWidget* video_widget; QFrame* video_frame; settings s; - std::atomic<settings*> new_settings; Timer time; + + volatile bool ever_success; static constexpr double rad2deg = 180.0/3.14159265; static constexpr double deg2rad = 3.14159265/180.0; - - PointModel model; }; #undef VideoWidget diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp index a15e97b9..7be05bb7 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp @@ -34,17 +34,10 @@ TrackerDialog::TrackerDialog() ui.camdevice_combo->addItem(iter->c_str()); } - ui.camroll_combo->addItem("-90"); - ui.camroll_combo->addItem("0"); - ui.camroll_combo->addItem("90"); - tie_setting(s.cam_index, ui.camdevice_combo); tie_setting(s.cam_res_x, ui.res_x_spin); tie_setting(s.cam_res_y, ui.res_y_spin); tie_setting(s.cam_fps, ui.fps_spin); - tie_setting(s.cam_roll, ui.camroll_combo); - tie_setting(s.cam_pitch, ui.campitch_spin); - tie_setting(s.cam_yaw, ui.camyaw_spin); tie_setting(s.threshold_secondary, ui.threshold_secondary_slider); tie_setting(s.threshold, ui.threshold_slider); @@ -72,53 +65,20 @@ TrackerDialog::TrackerDialog() tie_setting(s.t_MH_x, ui.tx_spin); tie_setting(s.t_MH_y, ui.ty_spin); tie_setting(s.t_MH_z, ui.tz_spin); + + tie_setting(s.fov, ui.fov); + + tie_setting(s.active_model_panel, ui.model_tabs); + + tie_setting(s.dynamic_pose, ui.dynamic_pose); connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) ); - connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); - ui.model_tabs->setCurrentIndex(s.active_model_panel); - - connect(ui.model_tabs, SIGNAL(currentChanged(int)), this, SLOT(set_model(int))); connect(&timer,SIGNAL(timeout()), this,SLOT(poll_tracker_info())); timer.start(100); - - connect(ui.buttonBox_2, SIGNAL(clicked(QAbstractButton*)), this, SLOT(do_apply_without_saving(QAbstractButton*))); -} - -void TrackerDialog::set_model_clip() -{ - s.m01_x = 0; - s.m01_y = static_cast<double>(s.clip_ty); - s.m01_z = -static_cast<double>(s.clip_tz); - s.m02_x = 0; - s.m02_y = -static_cast<double>(s.clip_by); - s.m02_z = -static_cast<double>(s.clip_bz); - - settings_changed(); -} - -void TrackerDialog::set_model_cap() -{ - s.m01_x = -static_cast<double>(s.cap_x); - s.m01_y = -static_cast<double>(s.cap_y); - s.m01_z = -static_cast<double>(s.cap_z); - s.m02_x = static_cast<double>(s.cap_x); - s.m02_y = -static_cast<double>(s.cap_y); - s.m02_z = -static_cast<double>(s.cap_z); - - settings_changed(); -} - -void TrackerDialog::set_model_custom() -{ - settings_changed(); -} - -void TrackerDialog::set_model(int val) -{ - s.active_model_panel = val; } void TrackerDialog::startstop_trans_calib(bool start) @@ -128,10 +88,13 @@ void TrackerDialog::startstop_trans_calib(bool start) qDebug()<<"TrackerDialog:: Starting translation calibration"; trans_calib.reset(); trans_calib_running = true; + s.t_MH_x = 0; + s.t_MH_y = 0; + s.t_MH_z = 0; } else { - qDebug()<<"TrackerDialog:: Stoppping translation calibration"; + qDebug()<<"TrackerDialog:: Stopping translation calibration"; trans_calib_running = false; { auto tmp = trans_calib.get_estimate(); @@ -139,7 +102,6 @@ void TrackerDialog::startstop_trans_calib(bool start) s.t_MH_y = tmp[1]; s.t_MH_z = tmp[2]; } - settings_changed(); } } @@ -179,24 +141,13 @@ void TrackerDialog::trans_calib_step() { if (tracker) { - FrameTrafo X_CM; - tracker->pose(&X_CM); + Affine X_CM = tracker->pose(); trans_calib.update(X_CM.R, X_CM.t); - cv::Vec3f t_MH = trans_calib.get_estimate(); - s.t_MH_x = t_MH[0]; - s.t_MH_y = t_MH[1]; - s.t_MH_z = t_MH[2]; } } -void TrackerDialog::settings_changed() -{ - if (tracker) tracker->apply(s); -} - void TrackerDialog::save() { - do_apply_without_saving(nullptr); s.b->save(); } @@ -206,28 +157,6 @@ void TrackerDialog::doOK() close(); } -void TrackerDialog::do_apply_without_saving(QAbstractButton*) -{ - switch (s.active_model_panel) { - default: - case 0: - set_model_clip(); - break; - case 1: - set_model_cap(); - break; - case 2: - set_model_custom(); - break; - } - if (tracker) tracker->apply(s); -} - -void TrackerDialog::doApply() -{ - save(); -} - void TrackerDialog::doCancel() { s.b->reload(); @@ -238,8 +167,6 @@ void TrackerDialog::register_tracker(ITracker *t) { qDebug()<<"TrackerDialog:: Tracker registered"; tracker = static_cast<Tracker*>(t); - if (isVisible() & s.b->modifiedp()) - tracker->apply(s); ui.tcalib_button->setEnabled(true); //ui.center_button->setEnabled(true); } diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h index 7f634c04..c6f7f8e1 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h @@ -36,19 +36,10 @@ public: public slots: void doOK(); void doCancel(); - void doApply(); - void do_apply_without_saving(QAbstractButton *); void startstop_trans_calib(bool start); void poll_tracker_info(); - void set_model(int idx); private: - void set_model_clip(); - void set_model_cap(); - void set_model_custom(); - - void settings_changed(); - settings s; Tracker* tracker; QTimer timer; diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h index 9365eb9a..57752ed6 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h @@ -9,7 +9,6 @@ #define FTNOIR_TRACKER_PT_SETTINGS_H #include <opencv2/core/core.hpp> -#include "point_tracker.h" #include "opentrack/options.hpp" using namespace options; @@ -21,9 +20,6 @@ struct settings cam_res_x, cam_res_y, cam_fps, - cam_roll, - cam_pitch, - cam_yaw, threshold, threshold_secondary, min_point_size, @@ -36,8 +32,10 @@ struct settings value<int> clip_ty, clip_tz, clip_by, clip_bz; value<int> active_model_panel, cap_x, cap_y, cap_z; - - // XXX todo red channel only, good for crapola CCD sensors -sh 20140922 + + value<int> fov; + + value<bool> dynamic_pose; settings() : b(bundle("tracker-pt")), @@ -45,9 +43,6 @@ struct settings cam_res_x(b, "camera-res-width", 640), cam_res_y(b, "camera-res-height", 480), cam_fps(b, "camera-fps", 30), - cam_roll(b, "camera-roll", 1), - cam_pitch(b, "camera-pitch", 0), - cam_yaw(b, "camera-yaw", 0), threshold(b, "threshold-primary", 128), threshold_secondary(b, "threshold-secondary", 128), min_point_size(b, "min-point-size", 10), @@ -68,7 +63,9 @@ struct settings active_model_panel(b, "active-model-panel", 0), cap_x(b, "cap-x", 0), cap_y(b, "cap-y", 0), - cap_z(b, "cap-z", 0) + cap_z(b, "cap-z", 0), + fov(b, "camera-fov", 56), + dynamic_pose(b, "dynamic-pose-resolution", false) {} }; diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp index 819bf5e8..94096f9d 100644 --- a/ftnoir_tracker_pt/point_extractor.cpp +++ b/ftnoir_tracker_pt/point_extractor.cpp @@ -20,7 +20,7 @@ PointExtractor::PointExtractor(){ //freopen("CON", "w", stderr); } // ---------------------------------------------------------------------------- -const vector<Vec2f>& PointExtractor::extract_points(Mat& frame) +std::vector<Vec2f> PointExtractor::extract_points(Mat& frame) { const int W = frame.cols; const int H = frame.rows; @@ -37,7 +37,8 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat& frame) Mat frame_gray; cvtColor(frame, frame_gray, CV_RGB2GRAY); - int secondary = threshold_secondary_val; + int secondary = s.threshold_secondary; + int primary = s.threshold; // mask for everything that passes the threshold (or: the upper threshold of the hysteresis) Mat frame_bin; @@ -49,15 +50,15 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat& frame) Mat frame_last_and_low; if(secondary==0){ - threshold(frame_gray, frame_bin, threshold_val, 255, THRESH_BINARY); + threshold(frame_gray, frame_bin, primary, 255, THRESH_BINARY); }else{ // we recombine a number of buffers, this might be slower than a single loop of per-pixel logic // but it might as well be faster if openCV makes good use of SIMD - float t = threshold_val; + float t = primary; //float hyst = float(threshold_secondary_val)/512.; //threshold(frame_gray, frame_bin, (t + ((255.-t)*hyst)), 255, THRESH_BINARY); - float hyst = float(threshold_secondary_val)/256.; - threshold(frame_gray, frame_bin, t, 255, THRESH_BINARY); + float hyst = float(primary)/(256.*8.); + threshold(frame_gray, frame_bin, t, 255, THRESH_BINARY); threshold(frame_gray, frame_bin_low,std::max(float(1), t - (t*hyst)), 255, THRESH_BINARY); frame_bin.copyTo(frame_bin_copy); @@ -71,6 +72,10 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat& frame) frame_last.copyTo(frame_bin); } } + + int min_size = s.min_point_size; + int max_size = s.max_point_size; + unsigned int region_size_min = 3.14*min_size*min_size/4.0; unsigned int region_size_max = 3.14*max_size*max_size/4.0; @@ -118,7 +123,7 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat& frame) if(secondary==0){ val = frame_gray.at<unsigned char>(i,j); - val = float(val - threshold_val)/(256 - threshold_val); + val = float(val - primary)/(256 - primary); val = val*val; // makes it more stable (less emphasis on low values, more on the peak) }else{ //hysteresis point detection gets stability from ignoring pixel noise so we decidedly leave the actual pixel values out of the picture @@ -149,10 +154,10 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat& frame) channels.push_back(frame_gray - frame_bin); }else{ frame_bin_copy.setTo(120, frame_bin_copy); - frame_bin_low.setTo(90, frame_bin_low); + //frame_bin_low.setTo(90, frame_bin_low); channels.push_back(frame_gray + frame_bin_copy); - channels.push_back(frame_gray + frame_last_and_low); - channels.push_back(frame_gray + frame_bin_low); + channels.push_back(frame_gray - frame_bin_copy); + channels.push_back(frame_gray - frame_bin_copy); //channels.push_back(frame_gray + frame_bin); } merge(channels, frame); diff --git a/ftnoir_tracker_pt/point_extractor.h b/ftnoir_tracker_pt/point_extractor.h index 5252b68d..06d148d6 100644 --- a/ftnoir_tracker_pt/point_extractor.h +++ b/ftnoir_tracker_pt/point_extractor.h @@ -11,6 +11,8 @@ #include <opencv2/core/core.hpp> #include <opencv2/imgproc/imgproc.hpp> +#include "ftnoir_tracker_pt_settings.h" + // ---------------------------------------------------------------------------- // Extracts points from an opencv image class PointExtractor @@ -19,14 +21,11 @@ public: // extracts points from frame and draws some processing info into frame, if draw_output is set // dt: time since last call in seconds // WARNING: returned reference is valid as long as object - const std::vector<cv::Vec2f>& extract_points(cv::Mat &frame); + std::vector<cv::Vec2f> extract_points(cv::Mat &frame); const std::vector<cv::Vec2f>& get_points() { return points; } PointExtractor(); - - int threshold_val; - int threshold_secondary_val; - int min_size, max_size; - + + settings s; private: std::vector<cv::Vec2f> points; cv::Mat frame_last; diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp index 8a633c5d..e4c999ad 100644 --- a/ftnoir_tracker_pt/point_tracker.cpp +++ b/ftnoir_tracker_pt/point_tracker.cpp @@ -33,44 +33,6 @@ static void set_row(Matx33f& m, int i, const Vec3f& v) m(i,2) = v[2]; } -PointModel::PointModel() : - M01 { 0, 0, 0 }, - M02 { 0, 0, 0 } -{ -} - -PointModel::PointModel(Vec3f M01, Vec3f M02) - : M01(M01), M02(M02) -{ - // calculate u - u = M01.cross(M02); - u /= norm(u); - - // calculate projection matrix on M01,M02 plane - float s11 = M01.dot(M01); - float s12 = M01.dot(M02); - float s22 = M02.dot(M02); - P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, - -s12, s11); - // calculate d and d_order for simple freetrack-like point correspondence - vector<Vec2f> points; - points.push_back(Vec2f(0,0)); - points.push_back(Vec2f(M01[0], M01[1])); - points.push_back(Vec2f(M02[0], M02[1])); - // fit line to orthographically projected points - // ERROR: yields wrong results with colinear points?! - /* - Vec4f line; - fitLine(points, line, CV_DIST_L2, 0, 0.01, 0.01); - d[0] = line[0]; d[1] = line[1]; - */ - // TODO: fix this - d = Vec2f(M01[0]-M02[0], M01[1]-M02[1]); - - // sort model points - get_d_order(points, d_order); -} - #ifdef OPENTRACK_API static bool d_vals_sort(const pair<float,int> a, const pair<float,int> b) { @@ -78,10 +40,11 @@ static bool d_vals_sort(const pair<float,int> a, const pair<float,int> b) } #endif -void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const +void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[], cv::Vec2f d) const { - // get sort indices with respect to d scalar product + // fit line to orthographically projected points vector< pair<float,int> > d_vals; + // get sort indices with respect to d scalar product for (unsigned i = 0; i<points.size(); ++i) d_vals.push_back(pair<float, int>(d.dot(points[i]), i)); @@ -99,69 +62,85 @@ void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[] } -// ---------------------------------------------------------------------------- PointTracker::PointTracker() { - X_CM.t[2] = 1000; // default position: 1 m away from cam; } -void PointTracker::reset() +PointTracker::PointOrder PointTracker::find_correspondences_previous(const vector<Vec2f>& points, const PointModel& model, float f) { - // enter init phase - X_CM = FrameTrafo(); -} + PointTracker::PointOrder p; + p.points[0] = project(Vec3f(0,0,0), f); + p.points[1] = project(model.M01, f); + p.points[2] = project(model.M02, f); -void PointTracker::track(const vector<Vec2f>& projected_points, const PointModel& model) -{ - const PointOrder& order = find_correspondences(projected_points, model); - int iters = POSIT(model, order); - qDebug()<<"POSIT iterations:"<<iters; -} - -PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel& model) -{ - // ... otherwise we look at the distance to the projection of the expected model points - // project model points under current pose - Vec2f p_exp[3]; - p_exp[0] = project(Vec3f(0,0,0)); - p_exp[1] = project(model.get_M01()); - p_exp[2] = project(model.get_M02()); - // set correspondences by minimum distance to projected model point bool point_taken[PointModel::N_POINTS]; for (int i=0; i<PointModel::N_POINTS; ++i) - point_taken[i] = false; - - PointOrder p; - for (int i=0; i<PointModel::N_POINTS; ++i) - p.points[i] = Vec2f(0, 0); - + point_taken[i] = false; + for (int i=0; i<PointModel::N_POINTS; ++i) { - float min_sdist = 1e4; - int min_idx = 0; - // find closest point to projected model point i - for (int j=0; j<PointModel::N_POINTS; ++j) - { - Vec2f d = p_exp[i]-projected_points[j]; - float sdist = d.dot(d); - if (sdist < min_sdist) + float min_sdist = 0; + int min_idx = 0; + // find closest point to projected model point i + for (int j=0; j<PointModel::N_POINTS; ++j) { - min_idx = j; - min_sdist = sdist; + Vec2f d = p.points[i]-points[j]; + float sdist = d.dot(d); + if (sdist < min_sdist || j==0) + { + min_idx = j; + min_sdist = sdist; + } } - } - // if one point is closest to more than one model point, abort - if (point_taken[min_idx]) return p; - point_taken[min_idx] = true; - p.points[i] = projected_points[min_idx]; + // if one point is closest to more than one model point, fallback + if (point_taken[min_idx]) + { + return find_correspondences(points, model); + } + point_taken[min_idx] = true; + p.points[i] = points[min_idx]; } return p; } +void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f, bool dynamic_pose) +{ + PointOrder order; + + if (!dynamic_pose) + order = find_correspondences(points, model); + else + order = find_correspondences_previous(points, model, f); + + POSIT(model, order, f); +} +PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model) +{ + // We do a simple freetrack-like sorting in the init phase... + // sort points + int point_d_order[PointModel::N_POINTS]; + int model_d_order[PointModel::N_POINTS]; + cv::Vec2f d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]); + model.get_d_order(points, point_d_order, d); + // calculate d and d_order for simple freetrack-like point correspondence + model.get_d_order(std::vector<cv::Vec2f> { + Vec2f{0,0}, + Vec2f(model.M01[0], model.M01[1]), + Vec2f(model.M02[0], model.M02[1]) + }, + model_d_order, + d); + // set correspondences + PointOrder p; + for (int i=0; i<PointModel::N_POINTS; ++i) + p.points[model_d_order[i]] = points[point_d_order[i]]; + + return p; +} -int PointTracker::POSIT(const PointModel& model, const PointOrder& order_) +int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length) { // POSIT algorithm for coplanar points as presented in // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] @@ -169,13 +148,12 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_) // The expected rotation used for resolving the ambiguity in POSIT: // In every iteration step the rotation closer to R_expected is taken - Matx33f R_expected; - R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation + Matx33f R_expected = Matx33f::eye(); // initial pose = last (predicted) pose Vec3f k; - get_row(R_expected, 2, k); - float Z0 = std::abs(X_CM.t[2]) < 1e-3 ? 1e3 : X_CM.t[2]; + get_row(R_expected, 2, k); + float Z0 = 1000.f; float old_epsilon_1 = 0; float old_epsilon_2 = 0; @@ -287,3 +265,9 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_) // //qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n'; } + +cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f) +{ + cv::Vec3f v_C = X_CM * v_M; + return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]); +} diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h index dac99b87..10bd2cef 100644 --- a/ftnoir_tracker_pt/point_tracker.h +++ b/ftnoir_tracker_pt/point_tracker.h @@ -16,34 +16,38 @@ #endif #include <vector> +#include "ftnoir_tracker_pt_settings.h" + +#include <QObject> + // ---------------------------------------------------------------------------- // Affine frame trafo -class FrameTrafo +class Affine { public: - FrameTrafo() : R(cv::Matx33f::eye()), t(0,0,0) {} - FrameTrafo(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {} + Affine() : R(cv::Matx33f::eye()), t(0,0,0) {} + Affine(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {} cv::Matx33f R; cv::Vec3f t; }; -inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) +inline Affine operator*(const Affine& X, const Affine& Y) { - return FrameTrafo(X.R*Y.R, X.R*Y.t + X.t); + return Affine(X.R*Y.R, X.R*Y.t + X.t); } -inline FrameTrafo operator*(const cv::Matx33f& X, const FrameTrafo& Y) +inline Affine operator*(const cv::Matx33f& X, const Affine& Y) { - return FrameTrafo(X*Y.R, X*Y.t); + return Affine(X*Y.R, X*Y.t); } -inline FrameTrafo operator*(const FrameTrafo& X, const cv::Matx33f& Y) +inline Affine operator*(const Affine& X, const cv::Matx33f& Y) { - return FrameTrafo(X.R*Y, X.t); + return Affine(X.R*Y, X.t); } -inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3f& v) +inline cv::Vec3f operator*(const Affine& X, const cv::Vec3f& v) { return X.R*v + X.t; } @@ -59,24 +63,49 @@ class PointModel public: static constexpr int N_POINTS = 3; - PointModel(cv::Vec3f M01, cv::Vec3f M02); - PointModel(); - - inline const cv::Vec3f& get_M01() const { return M01; } - inline const cv::Vec3f& get_M02() const { return M02; } - -private: cv::Vec3f M01; // M01 in model frame cv::Vec3f M02; // M02 in model frame cv::Vec3f u; // unit vector perpendicular to M01,M02-plane cv::Matx22f P; - - cv::Vec2f d; // determinant vector for point correspondence - int d_order[3]; // sorting of projected model points with respect to d scalar product - - void get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const; + + enum Model { Clip = 0, Cap = 1, Custom = 2 }; + + PointModel(settings& s) + { + set_model(s); + // calculate u + u = M01.cross(M02); + u /= norm(u); + + // calculate projection matrix on M01,M02 plane + float s11 = M01.dot(M01); + float s12 = M01.dot(M02); + float s22 = M02.dot(M02); + P = 1.0/(s11*s22-s12*s12) * cv::Matx22f(s22, -s12, -s12, s11); + } + + void set_model(settings& s) + { + switch (s.active_model_panel) + { + case Clip: + M01 = cv::Vec3f(0, static_cast<double>(s.clip_ty), -static_cast<double>(s.clip_tz)); + M02 = cv::Vec3f(0, -static_cast<double>(s.clip_by), -static_cast<double>(s.clip_bz)); + break; + case Cap: + M01 = cv::Vec3f(-static_cast<double>(s.cap_x), -static_cast<double>(s.cap_y), -static_cast<double>(s.cap_z)); + M02 = cv::Vec3f(static_cast<double>(s.cap_x), -static_cast<double>(s.cap_y), -static_cast<double>(s.cap_z)); + break; + case Custom: + M01 = cv::Vec3f(s.m01_x, s.m01_y, s.m01_z); + M02 = cv::Vec3f(s.m02_x, s.m02_y, s.m02_z); + break; + } + } + + void get_d_order(const std::vector<cv::Vec2f>& points, int* d_order, cv::Vec2f d) const; }; // ---------------------------------------------------------------------------- @@ -90,25 +119,26 @@ public: // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5) // f : (focal length)/(sensor width) // dt : time since last call - void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model); - FrameTrafo pose() const { return X_CM; } - void reset(); - + void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose); + Affine pose() const { return X_CM; } + cv::Vec2f project(const cv::Vec3f& v_M, float f); private: // the points in model order - typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder; - static constexpr float focal_length = 1.0f; - - inline cv::Vec2f project(const cv::Vec3f& v_M) + struct PointOrder { - cv::Vec3f v_C = X_CM * v_M; - return cv::Vec2f(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]); - } + cv::Vec2f points[PointModel::N_POINTS]; + PointOrder() + { + for (int i = 0; i < PointModel::N_POINTS; i++) + points[i] = cv::Vec2f(0, 0); + } + }; PointOrder find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel &model); - int POSIT(const PointModel& point_model, const PointOrder& order); // The POSIT algorithm, returns the number of iterations + PointOrder find_correspondences_previous(const std::vector<cv::Vec2f>& points, const PointModel &model, float f); + int POSIT(const PointModel& point_model, const PointOrder& order, float focal_length); // The POSIT algorithm, returns the number of iterations - FrameTrafo X_CM; // trafo from model to camera + Affine X_CM; // trafo from model to camera }; #endif //POINTTRACKER_H diff --git a/ftnoir_tracker_pt/trans_calib.cpp b/ftnoir_tracker_pt/trans_calib.cpp index 729a0b7f..2994eb48 100644 --- a/ftnoir_tracker_pt/trans_calib.cpp +++ b/ftnoir_tracker_pt/trans_calib.cpp @@ -41,4 +41,4 @@ Vec3f TranslationCalibrator::get_estimate() { Vec6f x = P.inv() * y; return Vec3f(-x[0], -x[1], -x[2]); -}
\ No newline at end of file +} |