alperen.isik
/
It_Robotics_Pong_Robot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Inital commit -Lab2

This commit is contained in:
Alperen ISIK 2025-03-21 10:58:07 +01:00
commit 65061f49a8
19 changed files with 2943 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
bin/CameraCalibration Executable file
View File

Binary file not shown.

BIN
bin/RobotServoing Executable file
View File

Binary file not shown.

8
data/color_params.yaml Normal file
View File

@ -0,0 +1,8 @@
%YAML:1.0
---
lowH: 13
highH: 28
lowS: 145
highS: 255
lowV: 166
highV: 255

46
data/default.xml Normal file
View File

@ -0,0 +1,46 @@
<?xml version="1.0"?>
<opencv_storage>
<Settings>
<!-- Number of inner corners per a item row and column. (square, circle) -->
<BoardSize_Width> 9</BoardSize_Width>
<BoardSize_Height>6</BoardSize_Height>
<!-- The size of a square in some user defined metric system (pixel, millimeter)-->
<Square_Size>24</Square_Size>
<!-- The type of input used for camera calibration. One of: CHESSBOARD CIRCLES_GRID ASYMMETRIC_CIRCLES_GRID -->
<Calibrate_Pattern>"CHESSBOARD"</Calibrate_Pattern>
<!-- The input to use for calibration.
To use an input camera -> give the ID of the camera, like "1"
To use an input video -> give the path of the input video, like "/tmp/x.avi"
To use an image list -> give the path to the XML or YAML file containing the list of the images, like "/tmp/circles_list.xml"
-->
<Input>"0"</Input>
<!-- If true (non-zero) we flip the input images around the horizontal axis.-->
<Input_FlipAroundHorizontalAxis>0</Input_FlipAroundHorizontalAxis>
<!-- Time delay between frames in case of camera. -->
<Input_Delay>200</Input_Delay>
<!-- How many frames to use, for calibration. -->
<Calibrate_NrOfFrameToUse>25</Calibrate_NrOfFrameToUse>
<!-- Consider only fy as a free parameter, the ratio fx/fy stays the same as in the input cameraMatrix.
Use or not setting. 0 - False Non-Zero - True-->
<Calibrate_FixAspectRatio> 1 </Calibrate_FixAspectRatio>
<!-- If true (non-zero) tangential distortion coefficients are set to zeros and stay zero.-->
<Calibrate_AssumeZeroTangentialDistortion>1</Calibrate_AssumeZeroTangentialDistortion>
<!-- If true (non-zero) the principal point is not changed during the global optimization.-->
<Calibrate_FixPrincipalPointAtTheCenter> 0 </Calibrate_FixPrincipalPointAtTheCenter>
<!-- The name of the output log file. -->
<Write_outputFileName>"./data/microsoft_livecam_hd3000.xml"</Write_outputFileName>
<!-- If true (non-zero) we write to the output file the feature points.-->
<Write_DetectedFeaturePoints>1</Write_DetectedFeaturePoints>
<!-- If true (non-zero) we write to the output file the extrinsic camera parameters.-->
<Write_extrinsicParameters>1</Write_extrinsicParameters>
<!-- If true (non-zero) we show after calibration the undistorted images.-->
<Show_UndistortedImage>1</Show_UndistortedImage>
</Settings>
</opencv_storage>

801
data/microsoft_livecam_hd3000.xml Normal file
View File

@ -0,0 +1,801 @@
<?xml version="1.0"?>
<opencv_storage>
<calibration_time>"Fri 21 Mar 2025 09:27:47 AM CET"</calibration_time>
<nr_of_frames>25</nr_of_frames>
<image_width>640</image_width>
<image_height>480</image_height>
<board_width>9</board_width>
<board_height>6</board_height>
<square_size>24.</square_size>
<fix_aspect_ratio>1.</fix_aspect_ratio>
<!-- flags: +fix_aspectRatio +zero_tangent_dist -->
<flags>10</flags>
<fisheye_model>0</fisheye_model>
<camera_matrix type_id="opencv-matrix">
<rows>3</rows>
<cols>3</cols>
<dt>d</dt>
<data>
6.4255337987856933e+02 0. 3.0599103303969883e+02 0.
6.4255337987856933e+02 2.4890050657835533e+02 0. 0. 1.</data></camera_matrix>
<distortion_coefficients type_id="opencv-matrix">
<rows>5</rows>
<cols>1</cols>
<dt>d</dt>
<data>
-3.6862222382996651e-01 5.4039239357051927e-01 0. 0.
5.7490593833122294e-01</data></distortion_coefficients>
<avg_reprojection_error>1.1800776344446415e+00</avg_reprojection_error>
<per_view_reprojection_errors type_id="opencv-matrix">
<rows>25</rows>
<cols>1</cols>
<dt>f</dt>
<data>
4.46368992e-01 4.74848151e-01 4.66854095e-01 4.78679091e-01
5.68649769e-01 5.13346672e-01 4.26975638e-01 3.42881858e-01
3.47389936e-01 3.62793386e-01 3.83918703e-01 3.97116661e-01
1.28075409e+00 3.74014401e+00 1.58068585e+00 6.05669498e-01
4.64526922e-01 4.59366083e-01 4.01410282e-01 3.85575354e-01
8.09818804e-01 7.70252466e-01 5.83547652e-01 5.22580028e-01
3.37711191e+00</data></per_view_reprojection_errors>
<!-- a set of 6-tuples (rotation vector + translation vector) for each view -->
<extrinsic_parameters type_id="opencv-matrix">
<rows>25</rows>
<cols>6</cols>
<dt>d</dt>
<data>
-5.7431593890464989e-02 1.3029467717610399e-01
-1.4674693486203274e+00 -9.7360966536635260e+01
8.2171905259136608e+01 4.2477091656427456e+02
-3.6666920664906962e-02 1.4771675025327061e-01
-1.4688457238091308e+00 -9.7525321015099493e+01
8.3198172952024464e+01 4.2256384356837935e+02
-2.7725946511208202e-02 1.8923765639016457e-01
-1.4648211155455908e+00 -9.8838991853909192e+01
8.2301717330212369e+01 4.2005733220270088e+02 3.7217939713951577e-02
2.4849598007105811e-01 -1.4488158383018686e+00
-1.0045226355901009e+02 8.0251429726818088e+01
4.2967580575910512e+02 1.6224699828105241e-01 9.9278771306062080e-02
-1.3815434180674284e+00 -9.8246062273019461e+01
5.7641587036254577e+01 4.0893849239280036e+02 2.1445855231256339e-01
-6.4614842737479763e-02 -1.3381671809185784e+00
-9.6533423266991079e+01 4.7344387309822082e+01
4.0521488218214540e+02 1.5590437982300218e-01
-1.9808581393367394e-01 -1.3481557447295740e+00
-9.3796679145560958e+01 4.7524625702831848e+01
4.1286585489046820e+02 2.7633307463230481e-02
-2.6614547692078699e-01 -1.3682886085837740e+00
-9.4955963203558582e+01 4.6825309461115275e+01
4.1829054980203750e+02 -1.6736587438844255e-01
-3.7725702383945248e-01 -1.4115242723659911e+00
-8.8249405676594250e+01 4.3340856493635606e+01
4.0527189231004417e+02 -3.8103359635426648e-01
-4.9640956454034507e-01 -1.4498868121599970e+00
-6.5620322752815795e+01 3.3078140790670126e+01
3.6995519937881068e+02 -6.4434103607262161e-01
-3.8200151836118840e-01 -1.4362214423957094e+00
-1.0121792720624782e+02 3.2245126093586713e+01
3.8968355803903808e+02 -7.5335329190473399e-01
-5.0169060031450763e-01 -1.3878441123012553e+00
-1.0288236774541983e+02 1.3466789207972456e+01
3.6424977042487927e+02 -8.1842846420309145e-01
-6.4275168986938969e-01 -1.3766809444474220e+00
-8.9526689993302412e+01 1.0015135300032092e+01
3.4276689054605635e+02 -8.8566426454787128e-01
-7.0024785779959886e-01 -1.3538627645664303e+00
-8.9620448385779554e+01 -1.4482701604047463e+00
3.2657095561039921e+02 -6.6030028251874506e-01
-7.8040947256473192e-01 -1.3309285551353995e+00
-7.7062986353048544e+01 -1.1310681881154341e+00
3.1478470080437398e+02 -5.2614624171681224e-01
-8.1808682951495881e-01 -1.3391916669553690e+00
-6.9672769114042410e+01 7.6192937873454083e+00
3.0796067436087378e+02 -3.8898869459659235e-01
-8.2209847296860683e-01 -1.3441477130264459e+00
-6.7628805040922472e+01 2.1900313178984515e+01
3.0662483207720618e+02 -1.4395492125176648e-01
-9.0148557547095820e-01 -1.3334332792599211e+00
-4.8703196547039006e+01 3.5217847091826037e+01
2.9295734414984918e+02 7.2152965562305799e-02
-9.6473892383226756e-01 -1.2956643029275301e+00
-3.4558440460790322e+01 5.0665576820870818e+01
2.9548593800380354e+02 2.1714418421075021e-01
-1.0538093068041965e+00 -1.2345598365049912e+00
-1.9392435357549946e+01 5.0099099908107299e+01
2.9053945595138788e+02 2.6960072121530176e-01
-8.5080553892233401e-01 -1.3358384908793999e+00
-4.2729723616700170e+01 7.5848342552128955e+01
3.1809378173438790e+02 5.5145854649796280e-02
-4.4498149523556108e-01 -1.5915032100212350e+00
-5.7818789579809746e+01 8.9212220015625931e+01
3.5634117396846824e+02 -4.6521582579577492e-01
1.6145602023653166e-01 -1.6279849784386189e+00
-7.7636578986063867e+01 1.0058152225323030e+02
4.0488418585013170e+02 -8.3381742582532392e-01
4.8292857110365672e-01 -1.5202940606577400e+00
-1.0071140738562387e+02 1.0348178243466556e+02
4.3162226501772841e+02 -9.6675154343092073e-01
5.7304649775913852e-01 -1.5399281913858436e+00
-9.3102251581267780e+01 1.0660578243153054e+02
4.3266592036354433e+02</data></extrinsic_parameters>
<image_points type_id="opencv-matrix">
<rows>25</rows>
<cols>54</cols>
<dt>"2f"</dt>
<data>
1.62141739e+02 3.69541840e+02 1.65150879e+02 3.35476776e+02
1.68121063e+02 3.00471252e+02 1.71282425e+02 2.64747345e+02
1.74327072e+02 2.28714584e+02 1.77460510e+02 1.92765808e+02
1.80659042e+02 1.56494324e+02 1.84204590e+02 1.20872429e+02
1.87486267e+02 8.57541122e+01 1.95518509e+02 3.74826935e+02
1.98597733e+02 3.40434235e+02 2.01640091e+02 3.05163635e+02
2.04939056e+02 2.68773956e+02 2.08154388e+02 2.32539429e+02
2.11461960e+02 1.95817429e+02 2.15061798e+02 1.59304886e+02
2.18423630e+02 1.23118767e+02 2.22158615e+02 8.75667038e+01
2.30240524e+02 3.79904205e+02 2.33411240e+02 3.45333191e+02
2.36525711e+02 3.09553406e+02 2.40046799e+02 2.73113800e+02
2.43286896e+02 2.36257965e+02 2.46623337e+02 1.99265198e+02
2.50326965e+02 1.62164902e+02 2.54038010e+02 1.25600159e+02
2.57655182e+02 8.96587753e+01 2.65955231e+02 3.84903381e+02
2.69301331e+02 3.49869995e+02 2.72679321e+02 3.14067719e+02
2.76204498e+02 2.77092834e+02 2.79507629e+02 2.40119812e+02
2.83118744e+02 2.02719254e+02 2.86954926e+02 1.65315811e+02
2.90568848e+02 1.28412079e+02 2.94451965e+02 9.22128601e+01
3.02443695e+02 3.89360016e+02 3.05757965e+02 3.54356140e+02
3.09384552e+02 3.18092194e+02 3.12920685e+02 2.81165283e+02
3.16428497e+02 2.43721375e+02 3.20115540e+02 2.06322571e+02
3.23856506e+02 1.68550827e+02 3.27650421e+02 1.31473999e+02
3.31478577e+02 9.50669708e+01 3.39424805e+02 3.93652557e+02
3.42886444e+02 3.58407593e+02 3.46521210e+02 3.22160492e+02
3.50259918e+02 2.84860321e+02 3.53832977e+02 2.47547531e+02
3.57548096e+02 2.09866333e+02 3.61391113e+02 1.72106476e+02
3.65170898e+02 1.34781601e+02 3.69134460e+02 9.81592712e+01
1.61127258e+02 3.71561859e+02 1.63810181e+02 3.37570007e+02
1.66579102e+02 3.02485596e+02 1.69437836e+02 2.66571442e+02
1.72268845e+02 2.30359146e+02 1.75222198e+02 1.93911880e+02
1.78184067e+02 1.57185699e+02 1.81323212e+02 1.20913063e+02
1.84408157e+02 8.50969620e+01 1.94683304e+02 3.76976196e+02
1.97632965e+02 3.42537628e+02 2.00448883e+02 3.07279602e+02
2.03512039e+02 2.70689056e+02 2.06398346e+02 2.34157562e+02
2.09544647e+02 1.97080597e+02 2.12813751e+02 1.59917084e+02
2.16090225e+02 1.23195274e+02 2.19479828e+02 8.68218994e+01
2.29581909e+02 3.81932404e+02 2.32533264e+02 3.47453552e+02
2.35597977e+02 3.11639526e+02 2.38829727e+02 2.74992157e+02
2.41917999e+02 2.37783493e+02 2.45187088e+02 2.00525284e+02
2.48562607e+02 1.62805954e+02 2.52162247e+02 1.25658905e+02
2.55623352e+02 8.90625687e+01 2.65498230e+02 3.86981628e+02
2.68702087e+02 3.52060577e+02 2.72117462e+02 3.16195923e+02
2.75402405e+02 2.79074310e+02 2.78595276e+02 2.41803162e+02
2.82106262e+02 2.04099152e+02 2.85697693e+02 1.66134933e+02
2.89312073e+02 1.28520386e+02 2.93080688e+02 9.15561218e+01
3.02265045e+02 3.91422089e+02 3.05492859e+02 3.56438843e+02
3.08904144e+02 3.20300354e+02 3.12434753e+02 2.83137421e+02
3.15857056e+02 2.45535477e+02 3.19467316e+02 2.07615692e+02
3.23232605e+02 1.69443497e+02 3.26960785e+02 1.31644531e+02
3.30697937e+02 9.43880615e+01 3.39427612e+02 3.95708466e+02
3.42848358e+02 3.60546234e+02 3.46483032e+02 3.24319489e+02
3.50131104e+02 2.86845917e+02 3.53696136e+02 2.49372299e+02
3.57401489e+02 2.11304108e+02 3.61242584e+02 1.72882721e+02
3.65093475e+02 1.35035355e+02 3.69009949e+02 9.75952530e+01
1.58476852e+02 3.70913025e+02 1.60703796e+02 3.37039703e+02
1.63317139e+02 3.01805023e+02 1.65747223e+02 2.65802612e+02
1.68386627e+02 2.29242279e+02 1.71091202e+02 1.92389679e+02
1.73731949e+02 1.54939438e+02 1.76691635e+02 1.17982697e+02
1.79613174e+02 8.13849030e+01 1.91978058e+02 3.76582123e+02
1.94524994e+02 3.42354401e+02 1.97246933e+02 3.06892029e+02
1.99860367e+02 2.70245148e+02 2.02594482e+02 2.33295135e+02
2.05593033e+02 1.95598038e+02 2.08704666e+02 1.57709564e+02
2.11845154e+02 1.20261978e+02 2.15242889e+02 8.30658188e+01
2.26714661e+02 3.82024658e+02 2.29490280e+02 3.47529388e+02
2.32422134e+02 3.11643524e+02 2.35476501e+02 2.74741425e+02
2.38449875e+02 2.37208862e+02 2.41545731e+02 1.99303253e+02
2.45030701e+02 1.60811951e+02 2.48451035e+02 1.22749809e+02
2.51954987e+02 8.51955948e+01 2.62699097e+02 3.87398560e+02
2.65781860e+02 3.52518585e+02 2.69143463e+02 3.16451050e+02
2.72430969e+02 2.79060913e+02 2.75525452e+02 2.41416901e+02
2.78962616e+02 2.02924149e+02 2.82625916e+02 1.64182785e+02
2.86281464e+02 1.25608902e+02 2.90129059e+02 8.76016235e+01
2.99641479e+02 3.92288849e+02 3.02965057e+02 3.57327606e+02
3.06399628e+02 3.20843292e+02 3.09903839e+02 2.83437378e+02
3.13372467e+02 2.45366531e+02 3.17033356e+02 2.06695374e+02
3.20856476e+02 1.67596664e+02 3.24700073e+02 1.28872787e+02
3.28629944e+02 9.05050049e+01 3.37080658e+02 3.96909698e+02
3.40639374e+02 3.61704132e+02 3.44413666e+02 3.25298401e+02
3.48134369e+02 2.87508209e+02 3.51801117e+02 2.49391602e+02
3.55697418e+02 2.10547089e+02 3.59671539e+02 1.71322708e+02
3.63761139e+02 1.32350510e+02 3.67943695e+02 9.36866379e+01
1.59014786e+02 3.65344360e+02 1.61139313e+02 3.32689056e+02
1.63258728e+02 2.98770844e+02 1.65411957e+02 2.63726746e+02
1.67579163e+02 2.27896515e+02 1.69864471e+02 1.91478271e+02
1.72238770e+02 1.54286346e+02 1.74785431e+02 1.17002388e+02
1.77395340e+02 7.98523331e+01 1.91923996e+02 3.71364227e+02
1.94285858e+02 3.38517883e+02 1.96646713e+02 3.04441864e+02
1.99269928e+02 2.68775909e+02 2.01698853e+02 2.32629715e+02
2.04443115e+02 1.95496506e+02 2.07362396e+02 1.57713348e+02
2.10295212e+02 1.20040215e+02 2.13356415e+02 8.23711929e+01
2.25867233e+02 3.77332977e+02 2.28666733e+02 3.44295074e+02
2.31551712e+02 3.09702911e+02 2.34496292e+02 2.74066498e+02
2.37365982e+02 2.37366638e+02 2.40375488e+02 1.99867096e+02
2.43678513e+02 1.61508423e+02 2.47156891e+02 1.23385735e+02
2.50524841e+02 8.51628799e+01 2.61237640e+02 3.83066132e+02
2.64403137e+02 3.49697662e+02 2.67601196e+02 3.15152893e+02
2.71058533e+02 2.78837891e+02 2.74324432e+02 2.42228882e+02
2.77664886e+02 2.04346512e+02 2.81503815e+02 1.65612411e+02
2.85239380e+02 1.27014969e+02 2.89173706e+02 8.84094238e+01
2.97234863e+02 3.88307983e+02 3.00681366e+02 3.55046082e+02
3.04413727e+02 3.20142853e+02 3.08175079e+02 2.83931763e+02
3.11776611e+02 2.46785553e+02 3.15709869e+02 2.08804077e+02
3.19786530e+02 1.69848251e+02 3.23976257e+02 1.30871445e+02
3.28179810e+02 9.20611420e+01 3.33642670e+02 3.93506287e+02
3.37703033e+02 3.59919495e+02 3.41684326e+02 3.25059540e+02
3.45835785e+02 2.88657379e+02 3.50015350e+02 2.51553543e+02
3.54340424e+02 2.13313644e+02 3.58753326e+02 1.74302597e+02
3.63327148e+02 1.35203079e+02 3.67882385e+02 9.59293823e+01
1.54637390e+02 3.37283386e+02 1.60331009e+02 3.02368683e+02
1.66013168e+02 2.66267120e+02 1.71800903e+02 2.29251587e+02
1.77610596e+02 1.91568253e+02 1.83563034e+02 1.53736771e+02
1.89598969e+02 1.15474762e+02 1.96036758e+02 7.75704803e+01
2.02351013e+02 4.02310333e+01 1.90354202e+02 3.44371185e+02
1.96334778e+02 3.09416870e+02 2.02429749e+02 2.73483673e+02
2.08586304e+02 2.36187790e+02 2.14804337e+02 1.98607147e+02
2.21266373e+02 1.60519974e+02 2.27767395e+02 1.22126869e+02
2.34358871e+02 8.40565033e+01 2.41324844e+02 4.64100342e+01
2.26565964e+02 3.50927551e+02 2.32896301e+02 3.16412903e+02
2.39348419e+02 2.80385132e+02 2.46022507e+02 2.43326660e+02
2.52521423e+02 2.05586853e+02 2.59365631e+02 1.67613297e+02
2.66251221e+02 1.29062332e+02 2.73329193e+02 9.08523407e+01
2.80320374e+02 5.32898636e+01 2.63260681e+02 3.57621918e+02
2.69893738e+02 3.22868500e+02 2.76684998e+02 2.87258850e+02
2.83700714e+02 2.50152420e+02 2.90723328e+02 2.12843094e+02
2.97714874e+02 1.74695221e+02 3.05086609e+02 1.36333130e+02
3.12294434e+02 9.82309875e+01 3.19978912e+02 6.04985695e+01
2.99794067e+02 3.63547424e+02 3.06692810e+02 3.29340912e+02
3.13889465e+02 2.93583771e+02 3.21159393e+02 2.57060699e+02
3.28470428e+02 2.19703430e+02 3.35916473e+02 1.81955292e+02
3.43347961e+02 1.43671814e+02 3.51040680e+02 1.05731987e+02
3.58570007e+02 6.81856461e+01 3.36183380e+02 3.69435516e+02
3.43429352e+02 3.35275146e+02 3.50717560e+02 2.99893372e+02
3.58270752e+02 2.63460266e+02 3.65836975e+02 2.26688370e+02
3.73436188e+02 1.89231949e+02 3.81211121e+02 1.51208267e+02
3.88923401e+02 1.13500938e+02 3.96970978e+02 7.60492325e+01
1.56335419e+02 3.22453827e+02 1.64307861e+02 2.86653442e+02
1.72328064e+02 2.50033829e+02 1.80431335e+02 2.12955124e+02
1.88573471e+02 1.75786743e+02 1.97016479e+02 1.38898834e+02
2.05311325e+02 1.02001434e+02 2.14059021e+02 6.60117493e+01
2.22693253e+02 3.05561428e+01 1.92234802e+02 3.30321747e+02
2.00352112e+02 2.94529205e+02 2.08532303e+02 2.58422791e+02
2.16775177e+02 2.21333160e+02 2.25315796e+02 1.84489044e+02
2.33663193e+02 1.47553497e+02 2.42437668e+02 1.10787392e+02
2.51024384e+02 7.47540054e+01 2.60087494e+02 3.97407990e+01
2.27969986e+02 3.37521362e+02 2.36328918e+02 3.02403046e+02
2.44599731e+02 2.66373657e+02 2.53227448e+02 2.29694901e+02
2.61587067e+02 1.93091690e+02 2.70431915e+02 1.56471283e+02
2.78891235e+02 1.19792152e+02 2.87813660e+02 8.41376495e+01
2.96584839e+02 4.91905212e+01 2.63784668e+02 3.44622803e+02
2.72209381e+02 3.09660431e+02 2.80604950e+02 2.74244507e+02
2.89244537e+02 2.37840424e+02 2.98022766e+02 2.01769882e+02
3.06490570e+02 1.65370575e+02 3.15294464e+02 1.29230103e+02
3.23909882e+02 9.36158066e+01 3.33005432e+02 5.89208488e+01
2.98958557e+02 3.50937836e+02 3.07311676e+02 3.16746582e+02
3.15691467e+02 2.81535583e+02 3.24343689e+02 2.45790771e+02
3.33045013e+02 2.09996094e+02 3.41716705e+02 1.74245331e+02
3.50288574e+02 1.38434982e+02 3.59152130e+02 1.03289574e+02
3.67761169e+02 6.87346954e+01 3.33408966e+02 3.57382782e+02
3.41733856e+02 3.23323547e+02 3.50124268e+02 2.88674408e+02
3.58667816e+02 2.53405029e+02 3.67412903e+02 2.18289978e+02
3.75834167e+02 1.82872879e+02 3.84442657e+02 1.47516296e+02
3.93098816e+02 1.12851669e+02 4.01786469e+02 7.86234741e+01
1.63499817e+02 3.20954987e+02 1.71276367e+02 2.85565918e+02
1.78897171e+02 2.49654846e+02 1.86843567e+02 2.13900192e+02
1.94516403e+02 1.78505142e+02 2.02474182e+02 1.43739243e+02
2.10350525e+02 1.09467323e+02 2.18367752e+02 7.64693222e+01
2.26355591e+02 4.44115372e+01 1.98550049e+02 3.28643250e+02
2.06227737e+02 2.93212708e+02 2.13927216e+02 2.57777435e+02
2.21602676e+02 2.21922775e+02 2.29385605e+02 1.86863892e+02
2.37085999e+02 1.52174744e+02 2.44981094e+02 1.18218887e+02
2.52628006e+02 8.51456528e+01 2.60583282e+02 5.32245216e+01
2.33498489e+02 3.35686584e+02 2.41139084e+02 3.00788208e+02
2.48539749e+02 2.65507355e+02 2.56266327e+02 2.30248138e+02
2.63738495e+02 1.95196793e+02 2.71528412e+02 1.60746994e+02
2.78966125e+02 1.26773857e+02 2.86658447e+02 9.41014786e+01
2.94203827e+02 6.22885170e+01 2.68346008e+02 3.42670563e+02
2.75619568e+02 3.07837433e+02 2.83110535e+02 2.73241364e+02
2.90504303e+02 2.37967484e+02 2.98085999e+02 2.03551666e+02
3.05316071e+02 1.69390228e+02 3.12703522e+02 1.35773315e+02
3.19968781e+02 1.03191162e+02 3.27509583e+02 7.15301132e+01
3.02434601e+02 3.48807648e+02 3.09459534e+02 3.14783569e+02
3.16586304e+02 2.80313202e+02 3.23868713e+02 2.45755814e+02
3.31197144e+02 2.11541916e+02 3.38351074e+02 1.77852234e+02
3.45297638e+02 1.44612213e+02 3.52437439e+02 1.12314491e+02
3.59442383e+02 8.09711456e+01 3.35810699e+02 3.54959503e+02
3.42667816e+02 3.21142609e+02 3.49419098e+02 2.87271362e+02
3.56436493e+02 2.53122177e+02 3.63532928e+02 2.19494339e+02
3.70261871e+02 1.86261505e+02 3.77073364e+02 1.53335022e+02
3.83844482e+02 1.21423225e+02 3.90709625e+02 9.02605362e+01
1.63202469e+02 3.19132782e+02 1.70751221e+02 2.84222748e+02
1.78180115e+02 2.49341721e+02 1.85608566e+02 2.14931519e+02
1.92750793e+02 1.81393906e+02 2.00107193e+02 1.48947769e+02
2.07104401e+02 1.17210365e+02 2.14265015e+02 8.68953400e+01
2.21158279e+02 5.77642784e+01 1.98090027e+02 3.26609772e+02
2.05272583e+02 2.91402496e+02 2.12482376e+02 2.56772217e+02
2.19487915e+02 2.22255997e+02 2.26532455e+02 1.88795135e+02
2.33328323e+02 1.56242935e+02 2.40255402e+02 1.24639313e+02
2.46788513e+02 9.42751236e+01 2.53534302e+02 6.52088470e+01
2.32874023e+02 3.33506104e+02 2.39856522e+02 2.98706787e+02
2.46599686e+02 2.63947357e+02 2.53426102e+02 2.29769913e+02
2.59940582e+02 1.96272720e+02 2.66627197e+02 1.63697937e+02
2.72890625e+02 1.32216110e+02 2.79405487e+02 1.01916771e+02
2.85521851e+02 7.28525848e+01 2.67942902e+02 3.40407379e+02
2.74377014e+02 3.05564362e+02 2.80832275e+02 2.71275909e+02
2.87239471e+02 2.37001053e+02 2.93548981e+02 2.03778275e+02
2.99601776e+02 1.71400970e+02 3.05630524e+02 1.39999207e+02
3.11524353e+02 1.09752525e+02 3.17487701e+02 8.06951599e+01
3.02362671e+02 3.46536957e+02 3.08379028e+02 3.12267334e+02
3.14377930e+02 2.77959106e+02 3.20343872e+02 2.44294083e+02
3.26305695e+02 2.11184753e+02 3.32049896e+02 1.79096222e+02
3.37482666e+02 1.47818649e+02 3.43113373e+02 1.17680855e+02
3.48450623e+02 8.87770844e+01 3.36375366e+02 3.52508301e+02
3.41914673e+02 3.18421875e+02 3.47349304e+02 2.84702881e+02
3.52849670e+02 2.51188599e+02 3.58406342e+02 2.18477402e+02
3.63619019e+02 1.86657898e+02 3.68736359e+02 1.55567825e+02
3.73842255e+02 1.25688362e+02 3.78937408e+02 9.67554550e+01
1.68754608e+02 3.15769012e+02 1.75585297e+02 2.80992004e+02
1.82197266e+02 2.46927017e+02 1.88570038e+02 2.14206085e+02
1.94619797e+02 1.82722992e+02 2.00557297e+02 1.52914551e+02
2.06271606e+02 1.24262093e+02 2.11941254e+02 9.73741684e+01
2.17229797e+02 7.16535110e+01 2.05102921e+02 3.22190491e+02
2.11252182e+02 2.86822327e+02 2.17192581e+02 2.52845764e+02
2.22824463e+02 2.19672409e+02 2.28375519e+02 1.88218536e+02
2.33560272e+02 1.58065308e+02 2.38828186e+02 1.29496994e+02
2.43692719e+02 1.02311966e+02 2.48651779e+02 7.66723251e+01
2.41715103e+02 3.27910461e+02 2.47147797e+02 2.92769958e+02
2.52278580e+02 2.58596558e+02 2.57390442e+02 2.25632339e+02
2.62193909e+02 1.93775757e+02 2.67018829e+02 1.63536148e+02
2.71294739e+02 1.34885406e+02 2.75711700e+02 1.07693069e+02
2.79742737e+02 8.20798798e+01 2.78631378e+02 3.33733948e+02
2.83238037e+02 2.98507416e+02 2.87650604e+02 2.64507538e+02
2.91936981e+02 2.31302994e+02 2.96239655e+02 1.99482559e+02
3.00147369e+02 1.69331802e+02 3.04001160e+02 1.40605103e+02
3.07570984e+02 1.13379639e+02 3.11315399e+02 8.75219498e+01
3.15243530e+02 3.38929779e+02 3.18882385e+02 3.04041473e+02
3.22551636e+02 2.69961182e+02 3.26153564e+02 2.37053452e+02
3.29627167e+02 2.05235046e+02 3.32982422e+02 1.75176575e+02
3.36051880e+02 1.46445404e+02 3.39196716e+02 1.19207199e+02
3.42034515e+02 9.33343124e+01 3.51393158e+02 3.43819885e+02
3.54330994e+02 3.09068390e+02 3.57085632e+02 2.75462250e+02
3.59897308e+02 2.42539124e+02 3.62728119e+02 2.11008102e+02
3.65270477e+02 1.81111816e+02 3.67717377e+02 1.52403412e+02
3.70143982e+02 1.25165863e+02 3.72558624e+02 9.92170410e+01
1.93610336e+02 3.05545410e+02 1.99474792e+02 2.70389069e+02
2.04775558e+02 2.37321396e+02 2.09960556e+02 2.06168015e+02
2.14627914e+02 1.76969666e+02 2.19247406e+02 1.50043884e+02
2.23458389e+02 1.24578026e+02 2.27607666e+02 1.01300583e+02
2.31481583e+02 7.93229828e+01 2.34216110e+02 3.09622925e+02
2.38552155e+02 2.74129364e+02 2.42697922e+02 2.40715103e+02
2.46585876e+02 2.09282639e+02 2.50324265e+02 1.79869568e+02
2.53687683e+02 1.52713593e+02 2.57147888e+02 1.27502655e+02
2.60230591e+02 1.03933601e+02 2.63298279e+02 8.20093536e+01
2.75100311e+02 3.13403076e+02 2.78167511e+02 2.77781830e+02
2.80902161e+02 2.44276978e+02 2.83608795e+02 2.12852768e+02
2.86162689e+02 1.83276001e+02 2.88583954e+02 1.55664124e+02
2.90786163e+02 1.30608566e+02 2.92950958e+02 1.06892830e+02
2.95078400e+02 8.51289368e+01 3.16370605e+02 3.16851135e+02
3.17833832e+02 2.81363129e+02 3.19394501e+02 2.47891235e+02
3.20767273e+02 2.16311432e+02 3.22260345e+02 1.86583435e+02
3.23456604e+02 1.59341766e+02 3.24672546e+02 1.34019806e+02
3.25785126e+02 1.10366837e+02 3.27034607e+02 8.83629303e+01
3.57108673e+02 3.20106964e+02 3.57221008e+02 2.84776398e+02
3.57329865e+02 2.51419769e+02 3.57504700e+02 2.19844498e+02
3.57686005e+02 1.90276154e+02 3.58068176e+02 1.62966843e+02
3.58184906e+02 1.37670029e+02 3.58407135e+02 1.14055473e+02
3.58575836e+02 9.19702377e+01 3.97300140e+02 3.22735413e+02
3.95931274e+02 2.87688202e+02 3.94737549e+02 2.54736038e+02
3.93727356e+02 2.23405884e+02 3.92922791e+02 1.93998917e+02
3.92009979e+02 1.66841202e+02 3.91262299e+02 1.41524002e+02
3.90448975e+02 1.17706207e+02 3.89737122e+02 9.55622253e+01
1.43116730e+02 3.00558441e+02 1.53245193e+02 2.69747162e+02
1.62525818e+02 2.40821014e+02 1.71375793e+02 2.13544281e+02
1.79320114e+02 1.88098862e+02 1.87190918e+02 1.64250504e+02
1.94379761e+02 1.41709778e+02 2.01332825e+02 1.21261070e+02
2.07769867e+02 1.01497513e+02 1.78465729e+02 3.02098511e+02
1.87414673e+02 2.70521393e+02 1.95832687e+02 2.40909073e+02
2.03657715e+02 2.13067581e+02 2.11054459e+02 1.87102463e+02
2.17792618e+02 1.62869751e+02 2.24405838e+02 1.40582367e+02
2.30580460e+02 1.19516129e+02 2.36360291e+02 1.00241722e+02
2.15371780e+02 3.03477509e+02 2.23344955e+02 2.71341278e+02
2.30593811e+02 2.41185684e+02 2.37551697e+02 2.12973648e+02
2.43737335e+02 1.86439102e+02 2.49576523e+02 1.61832596e+02
2.55448318e+02 1.39427292e+02 2.60640259e+02 1.18435623e+02
2.65713318e+02 9.88674774e+01 2.54166000e+02 3.04612823e+02
2.60807251e+02 2.71877289e+02 2.66944122e+02 2.41501877e+02
2.72592834e+02 2.12718460e+02 2.77825470e+02 1.85854126e+02
2.82709778e+02 1.61316727e+02 2.87486694e+02 1.38671616e+02
2.91805511e+02 1.17472549e+02 2.96048920e+02 9.79637833e+01
2.94244293e+02 3.05653931e+02 2.99307129e+02 2.72655670e+02
3.04198914e+02 2.41710068e+02 3.08591003e+02 2.12766754e+02
3.12605896e+02 1.85551666e+02 3.16421753e+02 1.60869308e+02
3.20165863e+02 1.38113754e+02 3.23444977e+02 1.16810020e+02
3.26628815e+02 9.72017593e+01 3.35082916e+02 3.06253693e+02
3.38851685e+02 2.72836182e+02 3.42178467e+02 2.41912430e+02
3.45337189e+02 2.12703629e+02 3.48147217e+02 1.85544815e+02
3.50797211e+02 1.60785889e+02 3.53349213e+02 1.37685501e+02
3.55613434e+02 1.16368996e+02 3.57842102e+02 9.65044327e+01
1.29686584e+02 2.71508972e+02 1.42632080e+02 2.43754288e+02
1.54532303e+02 2.18400070e+02 1.65662323e+02 1.94669861e+02
1.75706436e+02 1.72912415e+02 1.85322006e+02 1.53034439e+02
1.94160675e+02 1.34437408e+02 2.02539902e+02 1.17679214e+02
2.10197250e+02 1.01829529e+02 1.66971573e+02 2.72925537e+02
1.78573807e+02 2.44366104e+02 1.89307419e+02 2.18154984e+02
1.99243073e+02 1.93942551e+02 2.08301620e+02 1.71694229e+02
2.16749695e+02 1.51484192e+02 2.24717575e+02 1.33270401e+02
2.32059189e+02 1.16129395e+02 2.39148270e+02 1.00470116e+02
2.06206589e+02 2.74263611e+02 2.16347565e+02 2.44988968e+02
2.25663788e+02 2.18265366e+02 2.34396637e+02 1.93728806e+02
2.42190033e+02 1.70962723e+02 2.49359436e+02 1.50377579e+02
2.56311554e+02 1.32078232e+02 2.62688965e+02 1.14925850e+02
2.68557343e+02 9.92424850e+01 2.47472626e+02 2.75411285e+02
2.56121460e+02 2.45520554e+02 2.63788483e+02 2.18577026e+02
2.70919952e+02 1.93494308e+02 2.77289398e+02 1.70371323e+02
2.83376831e+02 1.49661957e+02 2.89138947e+02 1.31232742e+02
2.94314972e+02 1.13843384e+02 2.99244019e+02 9.81558456e+01
2.90429291e+02 2.76493683e+02 2.96874878e+02 2.46323380e+02
3.02948578e+02 2.18781357e+02 3.08433197e+02 1.93497589e+02
3.13362671e+02 1.70096970e+02 3.17960297e+02 1.49356506e+02
3.22375488e+02 1.30529358e+02 3.26387909e+02 1.13289413e+02
3.30095428e+02 9.73427200e+01 3.34362549e+02 2.76913086e+02
3.38878052e+02 2.46585983e+02 3.43044250e+02 2.19113968e+02
3.46725250e+02 1.93529694e+02 3.50205719e+02 1.70102539e+02
3.53347992e+02 1.49358337e+02 3.56318604e+02 1.30297348e+02
3.59021423e+02 1.12675056e+02 3.61570099e+02 9.66533813e+01
1.42234360e+02 2.66385254e+02 1.54873672e+02 2.41835754e+02
1.66318192e+02 2.19568985e+02 1.76754547e+02 1.99290833e+02
1.86235855e+02 1.80494659e+02 1.95091232e+02 1.63634399e+02
2.02956726e+02 1.48125549e+02 2.13519531e+02 1.28507324e+02
2.17517090e+02 1.20462318e+02 1.83331238e+02 2.67195251e+02
1.94150528e+02 2.41897095e+02 2.03889130e+02 2.19161148e+02
2.12743546e+02 1.98264496e+02 2.20774567e+02 1.79398041e+02
2.28171753e+02 1.62273132e+02 2.35159821e+02 1.47104309e+02
2.41383713e+02 1.32672485e+02 2.48767944e+02 1.16342857e+02
2.26444626e+02 2.67753967e+02 2.35168045e+02 2.42103806e+02
2.42884872e+02 2.18814301e+02 2.50178848e+02 1.98056305e+02
2.56416840e+02 1.78611664e+02 2.62289276e+02 1.61256088e+02
2.67741272e+02 1.46049789e+02 2.74772797e+02 1.26265335e+02
2.77745483e+02 1.17610947e+02 2.71398560e+02 2.68201935e+02
2.77705933e+02 2.42251465e+02 2.83459686e+02 2.18980453e+02
2.88604401e+02 1.97543335e+02 2.93273041e+02 1.77853012e+02
2.97521393e+02 1.60666443e+02 3.01549438e+02 1.45421967e+02
3.05184357e+02 1.31228790e+02 3.09588715e+02 1.14770149e+02
3.17451630e+02 2.68667023e+02 3.21243134e+02 2.42460403e+02
3.24629120e+02 2.18993896e+02 3.27825623e+02 1.97485580e+02
3.30582855e+02 1.77646744e+02 3.33187164e+02 1.60436249e+02
3.35583435e+02 1.45056732e+02 3.37913483e+02 1.30610596e+02
3.40079987e+02 1.15590462e+02 3.64258942e+02 2.68294952e+02
3.65437012e+02 2.42278488e+02 3.66458160e+02 2.18885437e+02
3.67465271e+02 1.97413467e+02 3.68379761e+02 1.77640808e+02
3.69255829e+02 1.60556442e+02 3.70041382e+02 1.44838867e+02
3.70728882e+02 1.30531738e+02 3.71820312e+02 1.14657974e+02
1.34330566e+02 2.45708557e+02 1.48402420e+02 2.24235168e+02
1.60924866e+02 2.04976471e+02 1.72554474e+02 1.87530762e+02
1.82790848e+02 1.71663727e+02 1.95580963e+02 1.52599136e+02
2.06069946e+02 1.37857407e+02 2.12169693e+02 1.28911362e+02
2.12169800e+02 1.28911209e+02 1.77195084e+02 2.45990112e+02
1.89154053e+02 2.23589539e+02 1.99869873e+02 2.03828629e+02
2.09638840e+02 1.85983414e+02 2.18386124e+02 1.70060425e+02
2.31220078e+02 1.47868271e+02 2.36334518e+02 1.38724823e+02
2.44110229e+02 1.25520493e+02 2.46376465e+02 1.21646156e+02
2.22136856e+02 2.45922882e+02 2.31731720e+02 2.23226730e+02
2.40337540e+02 2.03086456e+02 2.48233490e+02 1.85414185e+02
2.54880157e+02 1.68902725e+02 2.63810577e+02 1.49096085e+02
2.70186066e+02 1.35424896e+02 2.74643951e+02 1.26105766e+02
2.74643951e+02 1.26105629e+02 2.69259674e+02 2.45734146e+02
2.76208771e+02 2.22799957e+02 2.82481659e+02 2.02704712e+02
2.88011688e+02 1.84409821e+02 2.92936951e+02 1.67695755e+02
2.99789124e+02 1.46374985e+02 3.03544830e+02 1.35700104e+02
3.07226654e+02 1.23756950e+02 3.08596100e+02 1.19976395e+02
3.17587341e+02 2.45777527e+02 3.21624817e+02 2.22591660e+02
3.25272522e+02 2.02302200e+02 3.28510742e+02 1.83958786e+02
3.31356812e+02 1.67213196e+02 3.35186523e+02 1.47160309e+02
3.37503174e+02 1.33820297e+02 3.39788971e+02 1.23864548e+02
3.39789001e+02 1.23864449e+02 3.66761841e+02 2.44830307e+02
3.67808838e+02 2.21950775e+02 3.68707092e+02 2.01882004e+02
3.69541351e+02 1.83492416e+02 3.70338531e+02 1.66845810e+02
3.71252655e+02 1.45831833e+02 3.72062042e+02 1.33967987e+02
3.72348480e+02 1.22994377e+02 3.72348480e+02 1.22994148e+02
1.52511719e+02 2.45328049e+02 1.63713562e+02 2.19529907e+02
1.73801407e+02 1.96601166e+02 1.83203217e+02 1.76105362e+02
1.91438049e+02 1.57561691e+02 1.99254639e+02 1.41228668e+02
2.06317673e+02 1.26211838e+02 2.15539017e+02 1.07865303e+02
2.19254196e+02 9.97283936e+01 1.98730560e+02 2.52405365e+02
2.07528717e+02 2.25460648e+02 2.15509811e+02 2.01776764e+02
2.22635040e+02 1.80555832e+02 2.29290146e+02 1.61606827e+02
2.35259628e+02 1.44806976e+02 2.40714249e+02 1.30143524e+02
2.48388062e+02 1.09505959e+02 2.51866592e+02 1.01432892e+02
2.46323120e+02 2.59148743e+02 2.52411957e+02 2.31655090e+02
2.57896667e+02 2.07372818e+02 2.63091919e+02 1.86007812e+02
2.67522125e+02 1.66304688e+02 2.71737885e+02 1.48892975e+02
2.75587250e+02 1.34068298e+02 2.80798401e+02 1.14607689e+02
2.82852478e+02 1.05963882e+02 2.94768707e+02 2.65796600e+02
2.98139648e+02 2.37921143e+02 3.01096863e+02 2.13434662e+02
3.03819763e+02 1.91246750e+02 3.06424469e+02 1.71101669e+02
3.08749573e+02 1.53786118e+02 3.10910706e+02 1.38570450e+02
3.13780090e+02 1.17691261e+02 3.15416992e+02 1.09487663e+02
3.43276123e+02 2.72538483e+02 3.43546234e+02 2.44347412e+02
3.44058105e+02 2.19424347e+02 3.44501862e+02 1.96883759e+02
3.45172546e+02 1.76482193e+02 3.45517548e+02 1.58815750e+02
3.45938538e+02 1.43336563e+02 3.46429199e+02 1.28897247e+02
3.46743195e+02 1.13517105e+02 3.91160736e+02 2.78328949e+02
3.88719574e+02 2.50171265e+02 3.86575409e+02 2.25326721e+02
3.84862976e+02 2.02599075e+02 3.83453064e+02 1.82034592e+02
3.82098389e+02 1.64392349e+02 3.80744629e+02 1.48386078e+02
3.79273529e+02 1.27495476e+02 3.78863495e+02 1.18765305e+02
1.63909958e+02 2.64001190e+02 1.72790283e+02 2.33814987e+02
1.80739868e+02 2.06760635e+02 1.88243668e+02 1.82507309e+02
1.94726532e+02 1.60572845e+02 2.00948425e+02 1.41194931e+02
2.06504257e+02 1.23394531e+02 2.11774734e+02 1.07685921e+02
2.16827362e+02 9.17628479e+01 2.11325653e+02 2.73455475e+02
2.17708817e+02 2.42365845e+02 2.23431656e+02 2.14588013e+02
2.28671051e+02 1.89632965e+02 2.33492584e+02 1.67304749e+02
2.37844376e+02 1.47443878e+02 2.41918579e+02 1.29795105e+02
2.45529312e+02 1.13612984e+02 2.49064529e+02 9.90277405e+01
2.59318451e+02 2.82433746e+02 2.62873688e+02 2.50871796e+02
2.66364716e+02 2.22731583e+02 2.69415985e+02 1.97692368e+02
2.72291534e+02 1.74642792e+02 2.74842224e+02 1.54145447e+02
2.77174561e+02 1.36380295e+02 2.79376221e+02 1.19999329e+02
2.81514221e+02 1.03259514e+02 3.07264557e+02 2.91124908e+02
3.08326294e+02 2.59379395e+02 3.09208832e+02 2.31165024e+02
3.10138489e+02 2.05432236e+02 3.11087555e+02 1.81961029e+02
3.11838623e+02 1.61556427e+02 3.12504059e+02 1.43390701e+02
3.13213684e+02 1.26702057e+02 3.13742767e+02 1.11545509e+02
3.54367584e+02 2.99465973e+02 3.52564758e+02 2.67655945e+02
3.51193268e+02 2.39295746e+02 3.50022614e+02 2.13376648e+02
3.49162567e+02 1.89605408e+02 3.48133545e+02 1.69054901e+02
3.47146851e+02 1.50550964e+02 3.46331451e+02 1.33613281e+02
3.45447693e+02 1.14952110e+02 4.00186768e+02 3.06701935e+02
3.96080505e+02 2.75287537e+02 3.92203308e+02 2.46988831e+02
3.89089142e+02 2.21072845e+02 3.86360809e+02 1.97446457e+02
3.83662994e+02 1.76749512e+02 3.81122650e+02 1.57773392e+02
3.79000916e+02 1.40653732e+02 3.76988373e+02 1.25006622e+02
1.67264984e+02 2.93756348e+02 1.74796082e+02 2.58745117e+02
1.81625992e+02 2.27183365e+02 1.87993088e+02 1.98503937e+02
1.93616089e+02 1.72479141e+02 1.98969604e+02 1.49214523e+02
2.03721619e+02 1.27906677e+02 2.08283218e+02 1.09029572e+02
2.12409531e+02 9.14054031e+01 2.14444519e+02 3.04668915e+02
2.19656998e+02 2.68936340e+02 2.24325165e+02 2.36900681e+02
2.28579636e+02 2.07655609e+02 2.32551239e+02 1.81322556e+02
2.36179001e+02 1.57570633e+02 2.39357239e+02 1.36439224e+02
2.42373581e+02 1.17064072e+02 2.45252960e+02 9.94084396e+01
2.61463776e+02 3.14747101e+02 2.64202789e+02 2.79063934e+02
2.66735046e+02 2.46579895e+02 2.68976837e+02 2.17432617e+02
2.71158691e+02 1.90450607e+02 2.73079315e+02 1.66282669e+02
2.74614471e+02 1.44965164e+02 2.76259430e+02 1.25341171e+02
2.77631012e+02 1.07530609e+02 3.07909027e+02 3.24341553e+02
3.08381592e+02 2.88564270e+02 3.08488373e+02 2.56460083e+02
3.08840729e+02 2.26710510e+02 3.09361420e+02 1.99566513e+02
3.09588318e+02 1.75476532e+02 3.09563019e+02 1.53820526e+02
3.09727814e+02 1.33959442e+02 3.09894928e+02 1.15741348e+02
3.53003967e+02 3.33111328e+02 3.50887512e+02 2.97898560e+02
3.49131958e+02 2.65655457e+02 3.47595825e+02 2.36086090e+02
3.46419830e+02 2.08737152e+02 3.44957611e+02 1.84543198e+02
3.43564026e+02 1.62740250e+02 3.42368042e+02 1.42615952e+02
3.41285156e+02 1.24367348e+02 3.96391724e+02 3.40922729e+02
3.92307770e+02 3.06173340e+02 3.88346069e+02 2.74458923e+02
3.85169159e+02 2.44890869e+02 3.82321136e+02 2.17886475e+02
3.79395172e+02 1.93738373e+02 3.76611816e+02 1.71580124e+02
3.74269501e+02 1.51295776e+02 3.72033478e+02 1.32675217e+02
2.01081436e+02 3.24746216e+02 2.04208435e+02 2.83331696e+02
2.07104126e+02 2.45203064e+02 2.09888504e+02 2.10535324e+02
2.12477722e+02 1.78826340e+02 2.15046310e+02 1.50506210e+02
2.17248947e+02 1.24390984e+02 2.19272995e+02 1.01206505e+02
2.21356949e+02 7.96316376e+01 2.47435333e+02 3.37411652e+02
2.48483337e+02 2.95947906e+02 2.49496155e+02 2.58316345e+02
2.50370178e+02 2.23367584e+02 2.51356781e+02 1.91764297e+02
2.52223511e+02 1.63111191e+02 2.52761536e+02 1.37269836e+02
2.53447861e+02 1.13463715e+02 2.54217010e+02 9.19815369e+01
2.91990723e+02 3.48579529e+02 2.90849701e+02 3.08119568e+02
2.90096619e+02 2.70571960e+02 2.89174255e+02 2.36384369e+02
2.88601593e+02 2.04545532e+02 2.88172974e+02 1.75671494e+02
2.87122864e+02 1.49716034e+02 2.86474792e+02 1.25950745e+02
2.85840576e+02 1.04235306e+02 3.34297821e+02 3.58909637e+02
3.31447388e+02 3.19162201e+02 3.28590576e+02 2.82628662e+02
3.26352325e+02 2.48408554e+02 3.24415771e+02 2.16797806e+02
3.22435638e+02 1.88290924e+02 3.20179718e+02 1.62335342e+02
3.18400208e+02 1.38387817e+02 3.16645905e+02 1.16362068e+02
3.73898651e+02 3.67997681e+02 3.69229126e+02 3.29574707e+02
3.65082062e+02 2.93520599e+02 3.61318268e+02 2.60041870e+02
3.58236725e+02 2.28680786e+02 3.54840912e+02 2.00371307e+02
3.51563751e+02 1.74408203e+02 3.48685852e+02 1.50460052e+02
3.46009979e+02 1.28410889e+02 4.11186432e+02 3.76094940e+02
4.05084961e+02 3.38583679e+02 3.99354340e+02 3.03764648e+02
3.94455048e+02 2.70707214e+02 3.90048737e+02 2.40139923e+02
3.85647522e+02 2.12124771e+02 3.81380188e+02 1.86167343e+02
3.77645508e+02 1.62213242e+02 3.74087830e+02 1.40003403e+02
2.32172699e+02 3.56989410e+02 2.32763107e+02 3.12442505e+02
2.33520691e+02 2.70579224e+02 2.34422089e+02 2.31860962e+02
2.35326462e+02 1.96110733e+02 2.36160416e+02 1.63591675e+02
2.37014053e+02 1.33474411e+02 2.37614227e+02 1.06363846e+02
2.38507675e+02 8.11914520e+01 2.72691772e+02 3.69162262e+02
2.72083344e+02 3.25456268e+02 2.71411194e+02 2.84854584e+02
2.70765503e+02 2.46547714e+02 2.70518402e+02 2.11531433e+02
2.70121704e+02 1.79087692e+02 2.69414886e+02 1.49426987e+02
2.69039215e+02 1.22023537e+02 2.68663635e+02 9.70199738e+01
3.10627014e+02 3.79470520e+02 3.08425354e+02 3.37543335e+02
3.06560486e+02 2.97801788e+02 3.04533630e+02 2.60783112e+02
3.03271942e+02 2.26036209e+02 3.01935547e+02 1.93949524e+02
3.00032013e+02 1.64510193e+02 2.98587433e+02 1.37380005e+02
2.97242004e+02 1.12312775e+02 3.45784973e+02 3.88817352e+02
3.42412689e+02 3.48333862e+02 3.39163239e+02 3.10267181e+02
3.36394867e+02 2.73720734e+02 3.34139038e+02 2.39753998e+02
3.31602844e+02 2.08341782e+02 3.28759827e+02 1.79273132e+02
3.26541931e+02 1.52222519e+02 3.24369873e+02 1.27117119e+02
3.77898956e+02 3.96863708e+02 3.73389160e+02 3.58232513e+02
3.69293335e+02 3.21130463e+02 3.65501953e+02 2.85896790e+02
3.62513458e+02 2.52581085e+02 3.59181091e+02 2.21863602e+02
3.55571899e+02 1.93033905e+02 3.52600128e+02 1.66367416e+02
3.49687164e+02 1.41390610e+02 4.07737366e+02 4.04110626e+02
4.02382538e+02 3.66668152e+02 3.97158783e+02 3.31271790e+02
3.92707581e+02 2.96847748e+02 3.88794403e+02 2.64638702e+02
3.84693756e+02 2.34572922e+02 3.80574097e+02 2.06266708e+02
3.77172913e+02 1.79694901e+02 3.73617188e+02 1.54938705e+02
2.63544922e+02 3.58018890e+02 2.62853790e+02 3.11977936e+02
2.62210541e+02 2.68514038e+02 2.61521545e+02 2.28286392e+02
2.61313782e+02 1.91214264e+02 2.60809631e+02 1.57386597e+02
2.60537903e+02 1.26074959e+02 2.60103912e+02 9.78533707e+01
2.59988251e+02 7.17247086e+01 2.98334229e+02 3.71367615e+02
2.96450562e+02 3.26700409e+02 2.94700012e+02 2.85055511e+02
2.93076447e+02 2.45673203e+02 2.91839569e+02 2.09525894e+02
2.90424622e+02 1.76062073e+02 2.88753571e+02 1.45347610e+02
2.87470795e+02 1.16968506e+02 2.86265228e+02 9.10214767e+01
3.30040009e+02 3.82655212e+02 3.27071045e+02 3.40317078e+02
3.24359497e+02 2.99857330e+02 3.21556427e+02 2.62098694e+02
3.19572357e+02 2.26530151e+02 3.17566650e+02 1.93550659e+02
3.14791199e+02 1.63303818e+02 3.12745392e+02 1.35280655e+02
3.10638031e+02 1.09307442e+02 3.58759979e+02 3.92862762e+02
3.54984833e+02 3.52247589e+02 3.51251373e+02 3.13751404e+02
3.47974335e+02 2.76782104e+02 3.45383392e+02 2.42323532e+02
3.42355042e+02 2.10225388e+02 3.39047272e+02 1.80409042e+02
3.36287170e+02 1.52591049e+02 3.33557037e+02 1.26725632e+02
3.84657776e+02 4.01683929e+02 3.80056183e+02 3.63201813e+02
3.75659607e+02 3.25871094e+02 3.71769348e+02 2.90498047e+02
3.68626251e+02 2.56873932e+02 3.65042694e+02 2.25658051e+02
3.61168213e+02 1.96289719e+02 3.57855774e+02 1.68932541e+02
3.54650269e+02 1.43245087e+02 4.08570984e+02 4.09489471e+02
4.03372345e+02 3.72473297e+02 3.98224854e+02 3.37130341e+02
3.93733582e+02 3.02700653e+02 3.89867493e+02 2.70308350e+02
3.85678070e+02 2.39907669e+02 3.81554993e+02 2.11191589e+02
3.77900513e+02 1.84193832e+02 3.74370361e+02 1.58807205e+02
2.20197845e+02 3.99139465e+02 2.22223526e+02 3.53644958e+02
2.24292618e+02 3.09264069e+02 2.26446060e+02 2.66515717e+02
2.28544266e+02 2.25704315e+02 2.30653305e+02 1.87531174e+02
2.32616852e+02 1.51278305e+02 2.34421021e+02 1.18028862e+02
2.36391785e+02 8.67394485e+01 2.57412811e+02 4.04088409e+02
2.58497681e+02 3.60414368e+02 2.59665100e+02 3.17886200e+02
2.60770294e+02 2.76467712e+02 2.62072845e+02 2.37236084e+02
2.63186707e+02 1.99922943e+02 2.63876373e+02 1.64915039e+02
2.64780823e+02 1.32045853e+02 2.65697906e+02 1.01581474e+02
2.92221527e+02 4.07998474e+02 2.92223389e+02 3.66465698e+02
2.92427124e+02 3.25502075e+02 2.92467865e+02 2.85916168e+02
2.93066010e+02 2.47773163e+02 2.93483246e+02 2.11693634e+02
2.93164124e+02 1.77589096e+02 2.93277496e+02 1.45624313e+02
2.93221405e+02 1.15574692e+02 3.24325897e+02 4.11632721e+02
3.23559662e+02 3.71651886e+02 3.22718658e+02 3.32680328e+02
3.22291443e+02 2.94288452e+02 3.22168060e+02 2.57698669e+02
3.21481628e+02 2.22733704e+02 3.20468445e+02 1.89665680e+02
3.19762390e+02 1.58375092e+02 3.19229095e+02 1.29009079e+02
3.53801178e+02 4.14409515e+02 3.52224335e+02 3.76320221e+02
3.50696625e+02 3.38705261e+02 3.49397675e+02 3.02123749e+02
3.48646973e+02 2.66677124e+02 3.47380829e+02 2.33076721e+02
3.45761505e+02 2.00739258e+02 3.44508057e+02 1.70398361e+02
3.43245270e+02 1.41527206e+02 3.81359406e+02 4.16682617e+02
3.79076416e+02 3.80089142e+02 3.76564117e+02 3.44351837e+02
3.74770447e+02 3.08931580e+02 3.73274414e+02 2.75078766e+02
3.71321320e+02 2.42490143e+02 3.69307770e+02 2.11261978e+02
3.67611664e+02 1.81530960e+02 3.65863037e+02 1.53381958e+02
2.03271561e+02 4.05306244e+02 2.01220276e+02 3.64373993e+02
1.99398407e+02 3.23446930e+02 1.97734756e+02 2.82735046e+02
1.96373260e+02 2.43072266e+02 1.95085846e+02 2.04505661e+02
1.94005615e+02 1.66894836e+02 1.92815384e+02 1.31367111e+02
1.92087311e+02 9.72688599e+01 2.44407867e+02 4.02632996e+02
2.41880188e+02 3.62378998e+02 2.39401642e+02 3.22014832e+02
2.37227478e+02 2.81899384e+02 2.35273697e+02 2.42766815e+02
2.33402100e+02 2.04719101e+02 2.31535095e+02 1.68107452e+02
2.30010803e+02 1.32824677e+02 2.28471436e+02 9.94363785e+01
2.84815491e+02 3.99616150e+02 2.81520660e+02 3.59927246e+02
2.78572418e+02 3.20445160e+02 2.75641510e+02 2.81121857e+02
2.73263763e+02 2.42588669e+02 2.70897980e+02 2.05285660e+02
2.68312866e+02 1.69221802e+02 2.66206787e+02 1.34698090e+02
2.63967407e+02 1.01804161e+02 3.24013855e+02 3.96299164e+02
3.20286743e+02 3.57463135e+02 3.16583252e+02 3.18607208e+02
3.13370544e+02 2.80106995e+02 3.10359039e+02 2.42433289e+02
3.07316620e+02 2.05793625e+02 3.04208344e+02 1.70551270e+02
3.01421722e+02 1.36701950e+02 2.98761505e+02 1.04385239e+02
3.61547974e+02 3.92556427e+02 3.57230896e+02 3.54575134e+02
3.53121185e+02 3.16656494e+02 3.49212250e+02 2.78962616e+02
3.45746277e+02 2.42282471e+02 3.42191711e+02 2.06536880e+02
3.38568970e+02 1.71989212e+02 3.35318359e+02 1.38881042e+02
3.32100037e+02 1.07213776e+02 3.97583954e+02 3.88582764e+02
3.92690918e+02 3.51503723e+02 3.88011322e+02 3.14575012e+02
3.83644104e+02 2.77841125e+02 3.79651459e+02 2.42017029e+02
3.75559082e+02 2.07285019e+02 3.71579376e+02 1.73444916e+02
3.67787994e+02 1.41047668e+02 3.64284332e+02 1.09959122e+02
1.85824356e+02 4.03379486e+02 1.85505005e+02 3.67301880e+02
1.85317917e+02 3.31212097e+02 1.84902725e+02 2.94854675e+02
1.84422989e+02 2.59382599e+02 1.84103333e+02 2.24445160e+02
1.83634384e+02 1.90018433e+02 1.83435303e+02 1.57134369e+02
1.82874420e+02 1.24934097e+02 2.17389816e+02 4.03227142e+02
2.16644775e+02 3.66091888e+02 2.15888657e+02 3.28812500e+02
2.15325562e+02 2.91600677e+02 2.14436203e+02 2.55106445e+02
2.13761780e+02 2.19278915e+02 2.13326736e+02 1.84321075e+02
2.12626450e+02 1.50440338e+02 2.12159256e+02 1.18040031e+02
2.50856705e+02 4.02721039e+02 2.49711929e+02 3.64606171e+02
2.48664413e+02 3.26337097e+02 2.47723007e+02 2.88160980e+02
2.46449005e+02 2.50604660e+02 2.45374680e+02 2.13938980e+02
2.44615646e+02 1.78203568e+02 2.43710388e+02 1.43864319e+02
2.42818466e+02 1.10759201e+02 2.86474030e+02 4.01798828e+02
2.85076721e+02 3.62619995e+02 2.83628754e+02 3.23433472e+02
2.82272888e+02 2.84266937e+02 2.80462646e+02 2.45818497e+02
2.79202271e+02 2.08456009e+02 2.78053680e+02 1.72166290e+02
2.76687805e+02 1.37101883e+02 2.75480438e+02 1.03564873e+02
3.24063782e+02 4.00369843e+02 3.22099304e+02 3.60295959e+02
3.20329498e+02 3.20050812e+02 3.18307648e+02 2.80062927e+02
3.16261169e+02 2.40866486e+02 3.14478210e+02 2.02824677e+02
3.12850952e+02 1.65703491e+02 3.11209442e+02 1.30373413e+02
3.09444733e+02 9.64232941e+01 3.63104767e+02 3.98561584e+02
3.60744720e+02 3.57466583e+02 3.58522949e+02 3.16338989e+02
3.56188812e+02 2.75569824e+02 3.53691284e+02 2.35672104e+02
3.51464935e+02 1.96981171e+02 3.49313690e+02 1.59428024e+02
3.47081604e+02 1.23429016e+02 3.44975311e+02 8.93195724e+01
1.60794510e+02 3.97535675e+02 1.63410736e+02 3.63729034e+02
1.66071671e+02 3.30061890e+02 1.68596802e+02 2.96286682e+02
1.70933746e+02 2.63073242e+02 1.73468185e+02 2.30464584e+02
1.75922684e+02 1.98395767e+02 1.78644104e+02 1.67485947e+02
1.80884506e+02 1.37415939e+02 1.78408340e+02 3.99380127e+02
1.80698959e+02 3.63889862e+02 1.83193527e+02 3.28631195e+02
1.85588028e+02 2.93341217e+02 1.87742126e+02 2.58656494e+02
1.90329788e+02 2.24669235e+02 1.93187103e+02 1.91507233e+02
1.95563461e+02 1.59361130e+02 1.98268173e+02 1.28425476e+02
1.97556488e+02 4.01071014e+02 1.99807510e+02 3.64110138e+02
2.02208496e+02 3.27119293e+02 2.04658279e+02 2.90236298e+02
2.06593246e+02 2.53922668e+02 2.08779236e+02 2.18554199e+02
2.11721878e+02 1.84005325e+02 2.14414001e+02 1.50903229e+02
2.16888947e+02 1.18782341e+02 2.18787109e+02 4.02992889e+02
2.21118591e+02 3.64131042e+02 2.23454269e+02 3.25395172e+02
2.25675079e+02 2.86656952e+02 2.27394775e+02 2.48788971e+02
2.29673279e+02 2.11930115e+02 2.32725327e+02 1.76133591e+02
2.35173767e+02 1.41500183e+02 2.37641327e+02 1.08592697e+02
2.42512558e+02 4.04677460e+02 2.44596313e+02 3.63957489e+02
2.46888535e+02 3.23409027e+02 2.48939240e+02 2.82792877e+02
2.50532867e+02 2.43212311e+02 2.52849091e+02 2.04721161e+02
2.55543243e+02 1.67362213e+02 2.58031830e+02 1.31733505e+02
2.60302887e+02 9.75951538e+01 2.68299927e+02 4.06514984e+02
2.70401855e+02 3.63684906e+02 2.72738342e+02 3.20893921e+02
2.74715851e+02 2.78440002e+02 2.76332001e+02 2.36969910e+02
2.78547180e+02 1.96798798e+02 2.80980469e+02 1.58147141e+02
2.82904907e+02 1.21096489e+02 2.85263580e+02 8.60775833e+01
1.69868454e+02 4.01302826e+02 1.80481323e+02 3.66703400e+02
1.76497589e+02 3.33976532e+02 1.75277054e+02 3.01770294e+02
1.79896164e+02 2.68199615e+02 1.78845917e+02 2.38260742e+02
1.92686005e+02 1.98944901e+02 1.82780273e+02 1.77649658e+02
1.96812073e+02 1.38254776e+02 1.85613266e+02 4.01543457e+02
1.87646423e+02 3.66097046e+02 1.83616318e+02 3.32292969e+02
1.91781158e+02 2.95653931e+02 1.87757492e+02 2.64345886e+02
1.87791595e+02 2.33138382e+02 1.92686050e+02 1.98944839e+02
1.92292374e+02 1.70613846e+02 1.96812195e+02 1.38254608e+02
2.00652023e+02 4.01540649e+02 2.02492752e+02 3.64202118e+02
2.04476990e+02 3.27167755e+02 2.06504028e+02 2.90444244e+02
2.07842468e+02 2.54570251e+02 2.09426254e+02 2.19712631e+02
2.12159149e+02 1.85730423e+02 2.14101913e+02 1.53478455e+02
2.16252899e+02 1.22222687e+02 2.17426895e+02 4.01445770e+02
2.19307449e+02 3.62181610e+02 2.21147781e+02 3.22995453e+02
2.22743317e+02 2.84449402e+02 2.23802948e+02 2.46844559e+02
2.25629395e+02 2.10442978e+02 2.28052094e+02 1.75287888e+02
2.29833603e+02 1.41501633e+02 2.31697632e+02 1.09423561e+02
2.36401352e+02 4.01163788e+02 2.37942261e+02 3.59644684e+02
2.39569534e+02 3.18583344e+02 2.40992065e+02 2.77891968e+02
2.41777527e+02 2.38491760e+02 2.43452255e+02 2.00421021e+02
2.45519257e+02 1.63613235e+02 2.47254837e+02 1.28812103e+02
2.48998474e+02 9.53958511e+01 2.57023773e+02 4.00693359e+02
2.58434753e+02 3.56828674e+02 2.60114471e+02 3.13396393e+02
2.61339691e+02 2.70552155e+02 2.62112335e+02 2.29098145e+02
2.63492035e+02 1.89299179e+02 2.65199677e+02 1.51142349e+02
2.66458557e+02 1.14716064e+02 2.68024933e+02 8.04716110e+01</data></image_points>
</opencv_storage>

88
include/DynamixelHandler.h Normal file
View File

@ -0,0 +1,88 @@
#if defined(__linux__) || defined(__APPLE__)
#include <fcntl.h>
#include <getopt.h>
#include <termios.h>
#define STDIN_FILENO 0
#elif defined(_WIN32) || defined(_WIN64)
#include <conio.h>
#endif
#define _USE_MATH_DEFINES
#include <cmath>
// standard includes
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <iostream>
#include <vector>
// dynamixel sdk include
#include "dynamixel_sdk/dynamixel_sdk.h"
// addresses of variables in the register
#define ADDR_XL320_CONTROL_MODE 11
#define ADDR_XL320_TORQUE_ENABLE 24
#define ADDR_XL320_GOAL_POSITION 30
#define ADDR_XL320_GOAL_VELOCITY 32
#define ADDR_XL320_PRESENT_POSITION 37
#define ADDR_XL320_PRESENT_VELOCITY 39
#define ADDR_XL320_HARDWARE_ERROR_STATUS 50
// rotation direction
#define ROT_DIRECTION_Q1 1
#define ROT_DIRECTION_Q2 -1
#define ROT_DIRECTION_QPEN 1
// nb of joints
#define NB_JOINTS 3
class DynamixelHandler
{
public:
DynamixelHandler();
~DynamixelHandler();
public:
bool openPort();
void closePort();
bool setBaudRate(int);
void setDeviceName(std::string);
void setProtocolVersion(float);
bool enableTorque(bool);
bool setControlMode(int iControlMode);
bool readCurrentJointPosition(std::vector<uint16_t>& vCurrentJointPosition);
bool readCurrentJointPosition(std::vector<float>& vCurrentJointPosition);
bool sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition);
bool sendTargetJointPosition(std::vector<float>& vTargetJointPosition);
bool sendTargetJointVelocity(std::vector<uint16_t>& vTargetJointVelocity);
bool sendTargetJointVelocity(std::vector<float>& vTargetJointVelocity);
int convertAngleToJointCmd(float fJointAngle);
float convertJointCmdToAngle(int iJointCmd);
int convertJointVelocityToJointCmd(float fJointVelocity);
private:
std::string m_sDeviceName;
float m_fProtocolVersion;
int m_i32BaudRate;
dynamixel::PortHandler* m_pPortHandler;
dynamixel::PacketHandler* m_pPacketHandler;
bool m_bIsDeviceNameSet;
bool m_bIsProtocolVersionSet;
bool m_bIsPortOpened;
bool m_bIsBaudRateSet;
int m_i32DxlCommunicationResult; // Communication result
uint8_t m_ui8DxlError; // Dynamixel error
std::vector<uint16_t> m_vDxlCurrentPosition; // Present position
float m_fMinJointCmd = 0;
float m_fMaxJointCmd = 1023;
float m_fMinJointAngle = -150.0f/180.0f*M_PI;
float m_fMaxJointAngle = 150.0f/180.0f*M_PI;
};

20
include/Kinematics.h Normal file
View File

@ -0,0 +1,20 @@
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <iostream>
#include "opencv2/opencv.hpp"
float deg2rad(float angle);
float rad2deg(float angle);
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2);
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2);
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2);
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);

BIN
lib/CameraCalibration.o Normal file
View File

Binary file not shown.

BIN
lib/DynamixelHandler.o Normal file
View File

Binary file not shown.

BIN
lib/Kinematics.o Normal file
View File

Binary file not shown.

BIN
lib/RobotServoing.o Normal file
View File

Binary file not shown.

19
makefile Normal file
View File

@ -0,0 +1,19 @@
all: CameraCalibration Kinematics DynamixelHandler RobotServoing
CameraCalibration: src/CameraCalibration.cpp
g++ -c src/CameraCalibration.cpp -o lib/CameraCalibration.o -I./include -I/usr/include/opencv4
g++ lib/CameraCalibration.o -o bin/CameraCalibration -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
Kinematics: src/Kinematics.cpp include/Kinematics.h
g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
DynamixelHandler: src/DynamixelHandler.cpp include/DynamixelHandler.h
g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include
RobotServoing: src/RobotServoing.cpp
g++ -c src/RobotServoing.cpp -o lib/RobotServoing.o -I./include -I/usr/include/opencv4
g++ -o bin/RobotServoing lib/RobotServoing.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
clean:
rm -f lib/*.o
rm -f bin/*

769
src/CameraCalibration.cpp Normal file
View File

@ -0,0 +1,769 @@
#include <iostream>
#include <sstream>
#include <string>
#include <ctime>
#include <cstdio>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
using namespace cv;
using namespace std;
class Settings
{
public:
Settings() : goodInput(false) {}
enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };
void write(FileStorage& fs) const //Write serialization for this class
{
fs << "{"
<< "BoardSize_Width" << boardSize.width
<< "BoardSize_Height" << boardSize.height
<< "Square_Size" << squareSize
<< "Calibrate_Pattern" << patternToUse
<< "Calibrate_NrOfFrameToUse" << nrFrames
<< "Calibrate_FixAspectRatio" << aspectRatio
<< "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
<< "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint
<< "Write_DetectedFeaturePoints" << writePoints
<< "Write_extrinsicParameters" << writeExtrinsics
<< "Write_gridPoints" << writeGrid
<< "Write_outputFileName" << outputFileName
<< "Show_UndistortedImage" << showUndistorted
<< "Input_FlipAroundHorizontalAxis" << flipVertical
<< "Input_Delay" << delay
<< "Input" << input
<< "}";
}
void read(const FileNode& node) //Read serialization for this class
{
node["BoardSize_Width" ] >> boardSize.width;
node["BoardSize_Height"] >> boardSize.height;
node["Calibrate_Pattern"] >> patternToUse;
node["Square_Size"] >> squareSize;
node["Calibrate_NrOfFrameToUse"] >> nrFrames;
node["Calibrate_FixAspectRatio"] >> aspectRatio;
node["Write_DetectedFeaturePoints"] >> writePoints;
node["Write_extrinsicParameters"] >> writeExtrinsics;
node["Write_gridPoints"] >> writeGrid;
node["Write_outputFileName"] >> outputFileName;
node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
node["Calibrate_UseFisheyeModel"] >> useFisheye;
node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
node["Show_UndistortedImage"] >> showUndistorted;
node["Input"] >> input;
node["Input_Delay"] >> delay;
node["Fix_K1"] >> fixK1;
node["Fix_K2"] >> fixK2;
node["Fix_K3"] >> fixK3;
node["Fix_K4"] >> fixK4;
node["Fix_K5"] >> fixK5;
validate();
}
void validate()
{
goodInput = true;
if (boardSize.width <= 0 || boardSize.height <= 0)
{
cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << endl;
goodInput = false;
}
if (squareSize <= 10e-6)
{
cerr << "Invalid square size " << squareSize << endl;
goodInput = false;
}
if (nrFrames <= 0)
{
cerr << "Invalid number of frames " << nrFrames << endl;
goodInput = false;
}
if (input.empty()) // Check for valid input
inputType = INVALID;
else
{
if (input[0] >= '0' && input[0] <= '9')
{
stringstream ss(input);
ss >> cameraID;
inputType = CAMERA;
}
else
{
if (isListOfImages(input) && readStringList(input, imageList))
{
inputType = IMAGE_LIST;
nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size();
}
else
inputType = VIDEO_FILE;
}
if (inputType == CAMERA)
inputCapture.open(cameraID, cv::CAP_V4L2);
if (inputType == VIDEO_FILE)
inputCapture.open(input);
if (inputType != IMAGE_LIST && !inputCapture.isOpened())
inputType = INVALID;
}
if (inputType == INVALID)
{
cerr << " Input does not exist: " << input;
goodInput = false;
}
flag = 0;
if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
if(calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST;
if(aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO;
if(fixK1) flag |= CALIB_FIX_K1;
if(fixK2) flag |= CALIB_FIX_K2;
if(fixK3) flag |= CALIB_FIX_K3;
if(fixK4) flag |= CALIB_FIX_K4;
if(fixK5) flag |= CALIB_FIX_K5;
if (useFisheye)
{
// the fisheye model has its own enum, so overwrite the flags
flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC;
if(fixK1) flag |= fisheye::CALIB_FIX_K1;
if(fixK2) flag |= fisheye::CALIB_FIX_K2;
if(fixK3) flag |= fisheye::CALIB_FIX_K3;
if(fixK4) flag |= fisheye::CALIB_FIX_K4;
if (calibFixPrincipalPoint) flag |= fisheye::CALIB_FIX_PRINCIPAL_POINT;
}
calibrationPattern = NOT_EXISTING;
if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID;
if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
if (calibrationPattern == NOT_EXISTING)
{
cerr << " Camera calibration mode does not exist: " << patternToUse << endl;
goodInput = false;
}
atImageList = 0;
}
Mat nextImage()
{
Mat result;
if( inputCapture.isOpened() )
{
Mat view0;
inputCapture >> view0;
view0.copyTo(result);
}
else if( atImageList < imageList.size() )
result = imread(imageList[atImageList++], IMREAD_COLOR);
return result;
}
static bool readStringList( const string& filename, vector<string>& l )
{
l.clear();
FileStorage fs(filename, FileStorage::READ);
if( !fs.isOpened() )
return false;
FileNode n = fs.getFirstTopLevelNode();
if( n.type() != FileNode::SEQ )
return false;
FileNodeIterator it = n.begin(), it_end = n.end();
for( ; it != it_end; ++it )
l.push_back((string)*it);
return true;
}
static bool isListOfImages( const string& filename)
{
string s(filename);
// Look for file extension
if( s.find(".xml") == string::npos && s.find(".yaml") == string::npos && s.find(".yml") == string::npos )
return false;
else
return true;
}
public:
Size boardSize; // The size of the board -> Number of items by width and height
Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern
float squareSize; // The size of a square in your defined unit (point, millimeter,etc).
int nrFrames; // The number of frames to use from the input for calibration
float aspectRatio; // The aspect ratio
int delay; // In case of a video input
bool writePoints; // Write detected feature points
bool writeExtrinsics; // Write extrinsic parameters
bool writeGrid; // Write refined 3D target grid points
bool calibZeroTangentDist; // Assume zero tangential distortion
bool calibFixPrincipalPoint; // Fix the principal point at the center
bool flipVertical; // Flip the captured images around the horizontal axis
string outputFileName; // The name of the file where to write
bool showUndistorted; // Show undistorted images after calibration
string input; // The input ->
bool useFisheye; // use fisheye camera model for calibration
bool fixK1; // fix K1 distortion coefficient
bool fixK2; // fix K2 distortion coefficient
bool fixK3; // fix K3 distortion coefficient
bool fixK4; // fix K4 distortion coefficient
bool fixK5; // fix K5 distortion coefficient
int cameraID;
vector<string> imageList;
size_t atImageList;
VideoCapture inputCapture;
InputType inputType;
bool goodInput;
int flag;
private:
string patternToUse;
};
static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
{
if(node.empty())
x = default_value;
else
x.read(node);
}
enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, float grid_width, bool release_object);
int main(int argc, char* argv[])
{
const String keys
= "{help h usage ? | | print this message }"
"{@settings |default.xml| input setting file }"
"{d | | actual distance between top-left and top-right corners of "
"the calibration grid }"
"{winSize | 11 | Half of search window for cornerSubPix }";
CommandLineParser parser(argc, argv, keys);
parser.about("This is a camera calibration sample.\n"
"Usage: camera_calibration [configuration_file -- default ./default.xml]\n"
"Near the sample file you'll find the configuration file, which has detailed help of "
"how to edit it. It may be any OpenCV supported file format XML/YAML.");
if (!parser.check())
{
parser.printErrors();
return 0;
}
if (parser.has("help"))
{
parser.printMessage();
return 0;
}
//! [file_read]
Settings s;
const string inputSettingsFile = parser.get<string>(0);
FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
if (!fs.isOpened())
{
cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
parser.printMessage();
return -1;
}
fs["Settings"] >> s;
fs.release(); // close Settings file
//! [file_read]
//FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML
//fout << "Settings" << s;
if (!s.goodInput)
{
cout << "Invalid input detected. Application stopping. " << endl;
return -1;
}
int winSize = parser.get<int>("winSize");
float grid_width = s.squareSize * (s.boardSize.width - 1);
bool release_object = false;
if (parser.has("d"))
{
grid_width = parser.get<float>("d");
release_object = true;
}
vector<vector<Point2f> > imagePoints;
Mat cameraMatrix, distCoeffs;
Size imageSize;
int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
clock_t prevTimestamp = 0;
const Scalar RED(0,0,255), GREEN(0,255,0);
const char ESC_KEY = 27;
//! [get_input]
for(;;)
{
Mat view;
bool blinkOutput = false;
view = s.nextImage();
//----- If no more image, or got enough, then stop calibration and show result -------------
if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames )
{
if(runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
release_object))
mode = CALIBRATED;
else
mode = DETECTION;
}
if(view.empty()) // If there are no more images stop the loop
{
// if calibration threshold was not reached yet, calibrate now
if( mode != CALIBRATED && !imagePoints.empty() )
runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
release_object);
break;
}
//! [get_input]
imageSize = view.size(); // Format input image.
if( s.flipVertical )
flip( view, view, 0 );
//! [find_pattern]
vector<Point2f> pointBuf;
bool found;
int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
if(!s.useFisheye)
{
// fast check erroneously fails with high distortions like fisheye
chessBoardFlags |= CALIB_CB_FAST_CHECK;
}
switch( s.calibrationPattern ) // Find feature points on the input format
{
case Settings::CHESSBOARD:
found = findChessboardCorners( view, s.boardSize, pointBuf, chessBoardFlags);
break;
case Settings::CIRCLES_GRID:
found = findCirclesGrid( view, s.boardSize, pointBuf );
break;
case Settings::ASYMMETRIC_CIRCLES_GRID:
found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
break;
default:
found = false;
break;
}
//! [find_pattern]
//! [pattern_found]
if ( found) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
if( s.calibrationPattern == Settings::CHESSBOARD)
{
Mat viewGray;
cvtColor(view, viewGray, COLOR_BGR2GRAY);
cornerSubPix( viewGray, pointBuf, Size(winSize,winSize),
Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001 ));
}
if( mode == CAPTURING && // For camera only take new samples after delay time
(!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) )
{
imagePoints.push_back(pointBuf);
prevTimestamp = clock();
blinkOutput = s.inputCapture.isOpened();
}
// Draw the corners.
drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found );
}
//! [pattern_found]
//----------------------------- Output Text ------------------------------------------------
//! [output_text]
string msg = (mode == CAPTURING) ? "100/100" :
mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
int baseLine = 0;
Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10);
if( mode == CAPTURING )
{
if(s.showUndistorted)
msg = cv::format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames );
else
msg = cv::format( "%d/%d", (int)imagePoints.size(), s.nrFrames );
}
putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED);
if( blinkOutput )
bitwise_not(view, view);
//! [output_text]
//------------------------- Video capture output undistorted ------------------------------
//! [output_undistorted]
if( mode == CALIBRATED && s.showUndistorted )
{
Mat temp = view.clone();
if (s.useFisheye)
{
Mat newCamMat;
fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize,
Matx33d::eye(), newCamMat, 1);
cv::fisheye::undistortImage(temp, view, cameraMatrix, distCoeffs, newCamMat);
}
else
undistort(temp, view, cameraMatrix, distCoeffs);
}
//! [output_undistorted]
//------------------------------ Show image and check for input commands -------------------
//! [await_input]
imshow("Image View", view);
char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay);
if( key == ESC_KEY )
break;
if( key == 'u' && mode == CALIBRATED )
s.showUndistorted = !s.showUndistorted;
if( s.inputCapture.isOpened() && key == 'g' )
{
mode = CAPTURING;
imagePoints.clear();
}
//! [await_input]
}
// -----------------------Show the undistorted image for the image list ------------------------
//! [show_results]
if( s.inputType == Settings::IMAGE_LIST && s.showUndistorted && !cameraMatrix.empty())
{
Mat view, rview, map1, map2;
if (s.useFisheye)
{
Mat newCamMat;
fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize,
Matx33d::eye(), newCamMat, 1);
fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(), newCamMat, imageSize,
CV_16SC2, map1, map2);
}
else
{
initUndistortRectifyMap(
cameraMatrix, distCoeffs, Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize,
CV_16SC2, map1, map2);
}
for(size_t i = 0; i < s.imageList.size(); i++ )
{
view = imread(s.imageList[i], IMREAD_COLOR);
if(view.empty())
continue;
remap(view, rview, map1, map2, INTER_LINEAR);
imshow("Image View", rview);
char c = (char)waitKey();
if( c == ESC_KEY || c == 'q' || c == 'Q' )
break;
}
}
//! [show_results]
return 0;
}
//! [compute_errors]
static double computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
const vector<vector<Point2f> >& imagePoints,
const vector<Mat>& rvecs, const vector<Mat>& tvecs,
const Mat& cameraMatrix , const Mat& distCoeffs,
vector<float>& perViewErrors, bool fisheye)
{
vector<Point2f> imagePoints2;
size_t totalPoints = 0;
double totalErr = 0, err;
perViewErrors.resize(objectPoints.size());
for(size_t i = 0; i < objectPoints.size(); ++i )
{
if (fisheye)
{
fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], cameraMatrix,
distCoeffs);
}
else
{
projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
}
err = norm(imagePoints[i], imagePoints2, NORM_L2);
size_t n = objectPoints[i].size();
perViewErrors[i] = (float) std::sqrt(err*err/n);
totalErr += err*err;
totalPoints += n;
}
return std::sqrt(totalErr/totalPoints);
}
//! [compute_errors]
//! [board_corners]
static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Point3f>& corners,
Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
{
corners.clear();
switch(patternType)
{
case Settings::CHESSBOARD:
case Settings::CIRCLES_GRID:
for( int i = 0; i < boardSize.height; ++i )
for( int j = 0; j < boardSize.width; ++j )
corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
break;
case Settings::ASYMMETRIC_CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0));
break;
default:
break;
}
}
//! [board_corners]
static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
vector<float>& reprojErrs, double& totalAvgErr, vector<Point3f>& newObjPoints,
float grid_width, bool release_object)
{
//! [fixed_aspect]
cameraMatrix = Mat::eye(3, 3, CV_64F);
if( !s.useFisheye && s.flag & CALIB_FIX_ASPECT_RATIO )
cameraMatrix.at<double>(0,0) = s.aspectRatio;
//! [fixed_aspect]
if (s.useFisheye)
{
distCoeffs = Mat::zeros(4, 1, CV_64F);
} else
{
distCoeffs = Mat::zeros(8, 1, CV_64F);
}
vector<vector<Point3f> > objectPoints(1);
calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern);
objectPoints[0][s.boardSize.width - 1].x = objectPoints[0][0].x + grid_width;
newObjPoints = objectPoints[0];
objectPoints.resize(imagePoints.size(),objectPoints[0]);
//Find intrinsic and extrinsic camera parameters
double rms;
if (s.useFisheye)
{
Mat _rvecs, _tvecs;
rms = fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, _rvecs,
_tvecs, s.flag);
rvecs.reserve(_rvecs.rows);
tvecs.reserve(_tvecs.rows);
for(int i = 0; i < int(objectPoints.size()); i++)
{
rvecs.push_back(_rvecs.row(i));
tvecs.push_back(_tvecs.row(i));
}
} else
{
int iFixedPoint = -1;
if (release_object)
iFixedPoint = s.boardSize.width - 1;
rms = calibrateCameraRO(objectPoints, imagePoints, imageSize, iFixedPoint,
cameraMatrix, distCoeffs, rvecs, tvecs, newObjPoints,
s.flag | CALIB_USE_LU);
}
if (release_object)
{
cout << "New board corners: " << endl;
cout << newObjPoints[0] << endl;
cout << newObjPoints[s.boardSize.width - 1] << endl;
cout << newObjPoints[s.boardSize.width * (s.boardSize.height - 1)] << endl;
cout << newObjPoints.back() << endl;
}
cout << "Re-projection error reported by calibrateCamera: "<< rms << endl;
bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
objectPoints.clear();
objectPoints.resize(imagePoints.size(), newObjPoints);
totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix,
distCoeffs, reprojErrs, s.useFisheye);
return ok;
}
// Print camera parameters to the output file
static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
const vector<Mat>& rvecs, const vector<Mat>& tvecs,
const vector<float>& reprojErrs, const vector<vector<Point2f> >& imagePoints,
double totalAvgErr, const vector<Point3f>& newObjPoints )
{
FileStorage fs( s.outputFileName, FileStorage::WRITE );
time_t tm;
time( &tm );
struct tm *t2 = localtime( &tm );
char buf[1024];
strftime( buf, sizeof(buf), "%c", t2 );
fs << "calibration_time" << buf;
if( !rvecs.empty() || !reprojErrs.empty() )
fs << "nr_of_frames" << (int)std::max(rvecs.size(), reprojErrs.size());
fs << "image_width" << imageSize.width;
fs << "image_height" << imageSize.height;
fs << "board_width" << s.boardSize.width;
fs << "board_height" << s.boardSize.height;
fs << "square_size" << s.squareSize;
if( !s.useFisheye && s.flag & CALIB_FIX_ASPECT_RATIO )
fs << "fix_aspect_ratio" << s.aspectRatio;
if (s.flag)
{
std::stringstream flagsStringStream;
if (s.useFisheye)
{
flagsStringStream << "flags:"
<< (s.flag & fisheye::CALIB_FIX_SKEW ? " +fix_skew" : "")
<< (s.flag & fisheye::CALIB_FIX_K1 ? " +fix_k1" : "")
<< (s.flag & fisheye::CALIB_FIX_K2 ? " +fix_k2" : "")
<< (s.flag & fisheye::CALIB_FIX_K3 ? " +fix_k3" : "")
<< (s.flag & fisheye::CALIB_FIX_K4 ? " +fix_k4" : "")
<< (s.flag & fisheye::CALIB_RECOMPUTE_EXTRINSIC ? " +recompute_extrinsic" : "");
}
else
{
flagsStringStream << "flags:"
<< (s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "")
<< (s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "")
<< (s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "")
<< (s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "")
<< (s.flag & CALIB_FIX_K1 ? " +fix_k1" : "")
<< (s.flag & CALIB_FIX_K2 ? " +fix_k2" : "")
<< (s.flag & CALIB_FIX_K3 ? " +fix_k3" : "")
<< (s.flag & CALIB_FIX_K4 ? " +fix_k4" : "")
<< (s.flag & CALIB_FIX_K5 ? " +fix_k5" : "");
}
fs.writeComment(flagsStringStream.str());
}
fs << "flags" << s.flag;
fs << "fisheye_model" << s.useFisheye;
fs << "camera_matrix" << cameraMatrix;
fs << "distortion_coefficients" << distCoeffs;
fs << "avg_reprojection_error" << totalAvgErr;
if (s.writeExtrinsics && !reprojErrs.empty())
fs << "per_view_reprojection_errors" << Mat(reprojErrs);
if(s.writeExtrinsics && !rvecs.empty() && !tvecs.empty() )
{
CV_Assert(rvecs[0].type() == tvecs[0].type());
Mat bigmat((int)rvecs.size(), 6, CV_MAKETYPE(rvecs[0].type(), 1));
bool needReshapeR = rvecs[0].depth() != 1 ? true : false;
bool needReshapeT = tvecs[0].depth() != 1 ? true : false;
for( size_t i = 0; i < rvecs.size(); i++ )
{
Mat r = bigmat(Range(int(i), int(i+1)), Range(0,3));
Mat t = bigmat(Range(int(i), int(i+1)), Range(3,6));
if(needReshapeR)
rvecs[i].reshape(1, 1).copyTo(r);
else
{
//*.t() is MatExpr (not Mat) so we can use assignment operator
CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
r = rvecs[i].t();
}
if(needReshapeT)
tvecs[i].reshape(1, 1).copyTo(t);
else
{
CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
t = tvecs[i].t();
}
}
fs.writeComment("a set of 6-tuples (rotation vector + translation vector) for each view");
fs << "extrinsic_parameters" << bigmat;
}
if(s.writePoints && !imagePoints.empty() )
{
Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2);
for( size_t i = 0; i < imagePoints.size(); i++ )
{
Mat r = imagePtMat.row(int(i)).reshape(2, imagePtMat.cols);
Mat imgpti(imagePoints[i]);
imgpti.copyTo(r);
}
fs << "image_points" << imagePtMat;
}
if( s.writeGrid && !newObjPoints.empty() )
{
fs << "grid_points" << newObjPoints;
}
}
//! [run_and_save]
bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, float grid_width, bool release_object)
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
vector<Point3f> newObjPoints;
bool ok = runCalibration(s, imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs,
totalAvgErr, newObjPoints, grid_width, release_object);
cout << (ok ? "Calibration succeeded" : "Calibration failed")
<< ". avg re projection error = " << totalAvgErr << endl;
if (ok)
saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, imagePoints,
totalAvgErr, newObjPoints);
return ok;
}
//! [run_and_save]

407
src/DynamixelHandler.cpp Normal file
View File

@ -0,0 +1,407 @@
#include "DynamixelHandler.h"
DynamixelHandler::DynamixelHandler():
m_sDeviceName(""), m_fProtocolVersion(0.0), m_i32BaudRate(0),
m_pPacketHandler(nullptr), m_pPortHandler(nullptr),
m_bIsDeviceNameSet(false), m_bIsProtocolVersionSet(false), m_bIsPortOpened(false), m_bIsBaudRateSet(false),
m_ui8DxlError(0), m_i32DxlCommunicationResult(COMM_TX_FAIL)
{
}
DynamixelHandler::~DynamixelHandler()
{
}
int DynamixelHandler::convertJointVelocityToJointCmd(float fJointVelocity)
{
if (fJointVelocity == 0.0f)
return 0;
float a = 0.0f;
float b = 0.0f;
if (fJointVelocity>0)
{
float l_fMaxJointCmd = 1023;
float l_fMinJointCmd = 0;
float l_fMaxJointVelocity = 114 * 60.0f * 2 * M_PI;
float l_fMinJointAngle = 0.0f;
// y = ax + b
a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle);
b = l_fMinJointCmd - a * l_fMinJointAngle;
}
if (fJointVelocity<0)
{
float l_fMaxJointCmd = 2047;
float l_fMinJointCmd = 1024;
float l_fMaxJointVelocity = 0.0f;
float l_fMinJointAngle = -114 * 60.0f * 2 * M_PI;
// y = ax + b
a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle);
b = l_fMinJointCmd - a * l_fMinJointAngle;
}
float jointCmd = a * fJointVelocity + b;
return (int)jointCmd;
}
int DynamixelHandler::convertAngleToJointCmd(float fJointAngle)
{
// y = ax + b
float a = (m_fMaxJointCmd-m_fMinJointCmd) / (m_fMaxJointAngle - m_fMinJointAngle);
float b = m_fMinJointCmd - a * m_fMinJointAngle;
float jointCmd = a * fJointAngle + b;
return (int)jointCmd;
}
float DynamixelHandler::convertJointCmdToAngle(int iJointCmd)
{
// y = ax + b
float a = (m_fMaxJointAngle - m_fMinJointAngle) / (m_fMaxJointCmd-m_fMinJointCmd);
float b = m_fMinJointAngle - a * m_fMinJointCmd;
float jointAngle = a * iJointCmd + b;
return jointAngle;
}
bool DynamixelHandler::openPort()
{
if (m_pPortHandler == nullptr)
{
std::cout << "[ERROR](DynamixelHandler::openPort) m_pPortHandler is null!" << std::endl;
m_bIsPortOpened = false;
return m_bIsPortOpened;
}
if (!m_bIsDeviceNameSet)
{
std::cout << "[ERROR](DynamixelHandler::openPort) m_sDeviceName is not set!" << std::endl;
m_bIsPortOpened = false;
return m_bIsPortOpened;
}
if (m_bIsPortOpened)
{
std::cout << "[WARNING](DynamixelHandler::openPort) port is already opened!" << std::endl;
return m_bIsPortOpened;
}
if (m_pPortHandler->openPort())
{
std::cout << "[INFO](DynamixelHandler::openPort) Succeeded to open the port!" << std::endl;
m_bIsPortOpened = true;
}
else
{
std::cout << "[ERROR](DynamixelHandler::openPort) Failed to open the port!" << std::endl;
m_bIsPortOpened = false;
}
return m_bIsPortOpened;
}
void DynamixelHandler::closePort()
{
if (m_pPortHandler == nullptr)
{
std::cout << "[ERROR](DynamixelHandler::closePort) m_pPortHandler is null!" << std::endl;
m_bIsPortOpened = false;
return;
}
if (!m_bIsPortOpened)
{
std::cout << "[WARNING](DynamixelHandler::openPort) port is already closed!" << std::endl;
return;
}
m_pPortHandler->closePort();
std::cout << "[INFO](DynamixelHandler::closePort) Succeeded to close the port!" << std::endl;
m_bIsPortOpened = false;
}
bool DynamixelHandler::setBaudRate(int i32BaudRate)
{
m_i32BaudRate = i32BaudRate;
if (nullptr != m_pPortHandler)
{
if (m_pPortHandler->setBaudRate(m_i32BaudRate))
{
std::cout << "[INFO](DynamixelHandler::setBaudRate) Succeeded to change the baudrate!" << std::endl;
m_bIsBaudRateSet = true;
}
else
{
std::cout << "[ERROR](DynamixelHandler::setBaudRate) Failed to change the baudrate!" << std::endl;
m_bIsBaudRateSet = false;
}
}
else
{
std::cout << "[ERROR](DynamixelHandler::setBaudRate) m_pPortHandler is null!" << std::endl;
m_bIsBaudRateSet = false;
}
return m_bIsBaudRateSet;
}
void DynamixelHandler::setDeviceName(std::string sDeviceName)
{
m_sDeviceName = sDeviceName;
m_bIsDeviceNameSet = true;
if (nullptr != m_pPortHandler)
{
delete m_pPortHandler;
m_pPortHandler = nullptr;
}
// Initialize PortHandler instance
m_pPortHandler = dynamixel::PortHandler::getPortHandler(m_sDeviceName.c_str());
}
void DynamixelHandler::setProtocolVersion(float fProtocolVersion)
{
m_fProtocolVersion = fProtocolVersion;
m_bIsProtocolVersionSet = true;
if (nullptr != m_pPacketHandler)
{
delete m_pPacketHandler;
m_pPacketHandler = nullptr;
}
m_pPacketHandler = dynamixel::PacketHandler::getPacketHandler(m_fProtocolVersion);
}
bool DynamixelHandler::readCurrentJointPosition(std::vector<float>& vCurrentJointPosition)
{
// Creates a vector of joint position
std::vector<uint16_t> l_vCurrentJointPosition;
// Reads the current joint positions in motor command unit
bool bIsReadSuccessfull = this->readCurrentJointPosition(l_vCurrentJointPosition);
//std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2]<< std::endl;
// q1
vCurrentJointPosition.push_back(ROT_DIRECTION_Q1*convertJointCmdToAngle(l_vCurrentJointPosition[0]));
// qpen
vCurrentJointPosition.push_back(ROT_DIRECTION_QPEN*convertJointCmdToAngle(l_vCurrentJointPosition[1]));
// q2
vCurrentJointPosition.push_back(ROT_DIRECTION_Q2*convertJointCmdToAngle(l_vCurrentJointPosition[2]));
//std::cout << "vCurrentJointPosition= " << vCurrentJointPosition[0] << ", " << vCurrentJointPosition[1] << ", " << vCurrentJointPosition[2]<< std::endl;
return bIsReadSuccessfull;
}
bool DynamixelHandler::readCurrentJointPosition(std::vector<uint16_t>& vCurrentJointPosition)
{
bool bIsReadSuccessfull = false;
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
uint16_t dxl_present_position = 0;
dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_POSITION, &dxl_present_position, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsReadSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsReadSuccessfull = false;
}
else
{
vCurrentJointPosition.push_back(dxl_present_position);
bIsReadSuccessfull = true;
}
}
return bIsReadSuccessfull;
}
bool DynamixelHandler::sendTargetJointPosition(std::vector<float>& vTargetJointPosition)
{
// Checks if the input vector has the right size
if (vTargetJointPosition.size() != NB_JOINTS)
{
std::cout << "[ERROR] (sendTargetJointPosition) Input vector has not the right size!" << std::endl;
return false;
}
// Creates a vector of motor commands
std::vector<uint16_t> l_vTargetJointPosition;
// q1
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0]));
// qpen
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_QPEN*vTargetJointPosition[1]));
// q2
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q2*vTargetJointPosition[2]));
//std::cout << "l_vTargetJointPosition= " << l_vTargetJointPosition[0] << ", " << l_vTargetJointPosition[1] << ", " << l_vTargetJointPosition[2]<< std::endl;
// call the dxl sendTargetJointPosition
bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition);
return bIsSendSuccessfull;
}
bool DynamixelHandler::sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition)
{
bool bIsSendSuccessfull = false;
// checks if the vector size is correct
if (vTargetJointPosition.size() != NB_JOINTS)
{
std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): Size of command vector is not correct: " << vTargetJointPosition.size() << " instead of " << NB_JOINTS << "!" << std::endl;
bIsSendSuccessfull = false;
}
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
uint16_t dxl_present_position = 0;
dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_POSITION, vTargetJointPosition[l_joint], &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}
bool DynamixelHandler::sendTargetJointVelocity(std::vector<float>& vTargetJointVelocity)
{
// Checks if the input vector has the right size
if (vTargetJointVelocity.size() != NB_JOINTS)
{
std::cout << "[ERROR] (sendTargetJointVelocity) Input vector has not the right size!" << std::endl;
return false;
}
// Creates a vector of motor commands
std::vector<uint16_t> l_vTargetJointVelocity;
// q1
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0]));
// qpen
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_QPEN*vTargetJointVelocity[1]));
// q2
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[2]));
std::cout << "l_vTargetJointVelocity= " << l_vTargetJointVelocity[0] << ", " << l_vTargetJointVelocity[1] << ", " << l_vTargetJointVelocity[2]<< std::endl;
// call the dxl sendTargetJointPosition
bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity);
return bIsSendSuccessfull;
}
bool DynamixelHandler::sendTargetJointVelocity(std::vector<uint16_t>& vTargetJointVelocity)
{
bool bIsSendSuccessfull = false;
// checks if the vector size is correct
if (vTargetJointVelocity.size() != NB_JOINTS)
{
std::cout << "[ERROR] (DynamixelHandler::sendTargetJointVelocity): Size of command vector is not correct: " << vTargetJointVelocity.size() << " instead of " << NB_JOINTS << "!" << std::endl;
bIsSendSuccessfull = false;
}
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
uint16_t dxl_present_position = 0;
dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_VELOCITY, vTargetJointVelocity[l_joint], &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}
bool DynamixelHandler::enableTorque(bool bEnableTorque)
{
bool bIsSendSuccessfull = false;
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_TORQUE_ENABLE, bEnableTorque, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}
bool DynamixelHandler::setControlMode(int iControlMode)
{
bool bIsSendSuccessfull = false;
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_CONTROL_MODE, iControlMode, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}

161
src/Kinematics.cpp Normal file
View File

@ -0,0 +1,161 @@
#include "Kinematics.h"
float deg2rad(float angle)
{
return angle/180.0*M_PI;
}
float rad2deg(float angle)
{
return angle*180.0/M_PI;
}
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
{
float x = L1 * cos(q1) + L2 * cos(q1+q2);
float y = L1 * sin(q1) + L2 * sin(q1+q2);
std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl;
std::vector<float> X;
X.push_back(x);
X.push_back(y);
return X;
}
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
{
std::vector<float> qi;
float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2);
std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl;
if (cos_q2 >1 | cos_q2 <-1)
{
qi.push_back(0.0);
std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
}
else if (cos_q2 == 1)
{
qi.push_back(1.0);
float q1 = atan2(y, x);
float q2 = 0;
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
}
else if (cos_q2 == -1)
{
qi.push_back(1.0);
float q1 = atan2(y, x);
float q2 = M_PI;
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
}
else
{
qi.push_back(2.0);
std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
float q2 = acos(cos_q2);
float q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
q2 = -acos(cos_q2);
q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
}
return qi;
}
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
{
std::vector<float> jacobian;
float j11 = -L2*sin(q1+q2) - L1*sin(q1);
float j12 = -L2*sin(q1+q2);
float j21 = L2*cos(q1+q2) + L1*cos(q1);
float j22 = L2*cos(q1+q2);
jacobian.push_back(j11);
jacobian.push_back(j12);
jacobian.push_back(j21);
jacobian.push_back(j22);
return jacobian;
}
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
{
int rank = -1;
cv::Mat1f oJacobianMatrix(2, 2);
if (vJacobianMatrix.size() == 4)
{
// Converts the Jacobian matrix from std::vector to cv::Mat
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
std::cout << "=====Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
// Computes the determinant of the Jacobian matrix
float determinant = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]);
std::cout << "=====Determinant of the Jacobian matrix=====" << std::endl << determinant << std::endl;
// Computes SVD
cv::Mat1f w, u, vt;
cv::SVD::compute(oJacobianMatrix, w, u, vt);
// Finds non zero singular values
cv::Mat1f nonZeroSingularValues = w/w.at<float>(0,0) > threshold;
// Counts the number of non zero singular values
rank = cv::countNonZero(nonZeroSingularValues);
std::cout << "=====Rank of the Jacobian matrix=====" << std::endl << rank << " / " << oJacobianMatrix.rows << std::endl;
// Determines the inverse of the Jacobian matrix
cv::Mat oJacobianInverse = oJacobianMatrix.inv();
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
}
else
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
return rank;
}
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
{
cv::Mat1f oJacobianMatrix(2, 2);
cv::Mat oJacobianInverse;
if (vJacobianMatrix.size() == 4)
{
// Converts the Jacobian matrix from std::vector to cv::Mat
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
std::cout << "=====Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
// Determines the inverse of the Jacobian matrix
cv::invert(oJacobianMatrix, oJacobianInverse, cv::DECOMP_SVD);
//oJacobianInverse = oJacobianMatrix.inv();
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
}
else
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
return oJacobianInverse;
}

199
src/RedBallDetection.cpp Normal file
View File

@ -0,0 +1,199 @@
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml"
#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml"
#define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5
bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["camera_matrix"] >> camMatrix;
fs["distortion_coefficients"] >> distCoeffs;
return true;
}
bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV)
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open the file storage: " << filename << " !"<< std::endl;
return false;
}
fs << "lowH" << iLowH;
fs << "highH" << iHighH;
fs << "lowS" << iLowS;
fs << "highS" << iHighS;
fs << "lowV" << iLowV;
fs << "highV" << iHighV;
// releases the writer
fs.release();
return true;
}
int main(int argc, char** argv)
{
// initializes main parameters
std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
float fFPS = FPS;
// updates main parameters from arguments
int opt;
while ((opt = getopt (argc, argv, ":i:f:o:s:")) != -1)
{
switch (opt)
{
case 'o':
sColorParamFilename = optarg;
break;
case 'f':
fFPS = atof(optarg);
break;
case 'i':
sCameraParamFilename = optarg;
break;
case 's':
iStructuralElementSize = atoi(optarg);
break;
case '?':
if (optopt == 'o' || optopt == 'f' || optopt == 'i' || optopt == 's')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
return 1;
default:
abort ();
}
}
// distorted/undistorted image
bool bIsImageUndistorted = true;
// reads camera intrinsic parameters
cv::Mat cameraMatrix, distCoeffs;
bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs);
// checks if the camera parameters were successfully read
if (!isCamParamsSet)
{
std::cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << std::endl;
}
// creates a camera grabber
cv::VideoCapture cap(0, cv::CAP_V4L2);
// checks if the camera was successfully opened
if (!cap.isOpened())
{
std::cout << "[ERROR] Cannot open the webcam" << std::endl;
return 1;
}
cv::namedWindow("Control", cv::WINDOW_AUTOSIZE); //create a window called "Control"
// sets min/max value for HSV color representation
int iLowH = 0;
int iHighH = 179;
int iLowS = 0;
int iHighS = 255;
int iLowV = 0;
int iHighV = 255;
// creates trackbars in "Control" window
cv::createTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
cv::createTrackbar("HighH", "Control", &iHighH, 179);
cv::createTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
cv::createTrackbar("HighS", "Control", &iHighS, 255);
cv::createTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
cv::createTrackbar("HighV", "Control", &iHighV, 255);
while (true)
{
cv::Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal); // read a new frame from video
if (!bSuccess) //if not success, break loop
{
std::cout << "[WARNING] Cannot read a frame from video stream" << std::endl;
break;
}
if (bIsImageUndistorted && isCamParamsSet)
{
cv::Mat temp = imgOriginal.clone();
cv::undistort(temp, imgOriginal, cameraMatrix, distCoeffs);
}
//Convert the captured frame from BGR to HSV
cv::Mat imgHSV;
cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV);
//Threshold the image based on the trackbar values
cv::Mat imgThresholded;
inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded);
//morphological opening (remove small objects from the foreground)
cv::erode(imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
cv::dilate( imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
//morphological closing (fill small holes in the foreground)
cv::dilate( imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
cv::erode(imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
cv::imshow("Thresholded Image", imgThresholded); //show the thresholded image
cv::imshow("Original", imgOriginal); //show the original image
// waits for awhile depending on the FPS value
// checks if ESC was pressed to exit
char key = (char)cv::waitKey(1000.0/fFPS);
//wait for 'esc' key press for 30ms. If 'esc' key is pressed, break loop
if (key == 27)
{
std::cout << "[INFO] esc key is pressed by user -> Shuting down!" << std::endl;
break;
}
if (key == 'u')
{
bIsImageUndistorted = !bIsImageUndistorted;
std::cout << "[INFO] Image undistorted: " << bIsImageUndistorted<< std::endl;
}
if (key == 's')
{
writeColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
std::cout << "[INFO] Color parameters saved to file: " << sColorParamFilename << std::endl;
}
}
return 0;
}

227
src/RedBallTracking.cpp Normal file
View File

@ -0,0 +1,227 @@
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/calib3d.hpp>
#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml"
#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml"
#define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5
#define AREA_THRESOLD 1000
using namespace cv;
using namespace std;
bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["camera_matrix"] >> camMatrix;
fs["distortion_coefficients"] >> distCoeffs;
return true;
}
bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open the color paramter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["lowH"] >> iLowH;
fs["highH"] >> iHighH;
fs["lowS"] >> iLowS;
fs["highS"] >> iHighS;
fs["lowV"] >> iLowV;
fs["highV"] >> iHighV;
return true;
}
int main( int argc, char** argv )
{
// initializes main parameters
std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
float fFPS = FPS;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD;
// updates main parameters from arguments
int opt;
while ((opt = getopt (argc, argv, ":c:f:s:a:i:")) != -1)
{
switch (opt)
{
case 'c':
sColorParamFilename = optarg;
break;
case 'f':
fFPS = atof(optarg);
break;
case 's':
iStructuralElementSize = atoi(optarg);
break;
case 'a':
iAreaThresold = atoi(optarg);
break;
case 'i':
sCameraParamFilename = optarg;
break;
case '?':
if (optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'a')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
return 1;
default:
abort ();
}
}
// reads color parameters from the file storage
int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
// checks if the color parameters were successfully read
if (!isColorParamsSet)
{
std::cout << "[ERROR] Color parameters could not be loaded!" << std::endl;
return -1;
}
// distorted/undistorted image
bool bIsImageUndistorted = true;
// reads camera intrinsic parameters
cv::Mat cameraMatrix, distCoeffs;
bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs);
// checks if the camera parameters were successfully read
if (!isCamParamsSet)
{
std::cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << std::endl;
}
// creates a camera grabber
VideoCapture cap(0, cv::CAP_V4L2); //capture the video from webcam
// checks if the camera was successfully opened
if ( !cap.isOpened() ) // if not success, exit program
{
cout << "[ERROR] Could not open the camera!" << endl;
return -1;
}
// inits previous x,y location of the ball
int iLastX = -1;
int iLastY = -1;
// captures a temporary image from the camera
Mat imgTmp;
cap.read(imgTmp);
// creates a black image with the size as the camera output
Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 );
// main loop launched every FPS
while (true)
{
// reads a new frame from video
cv::Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal);
// checks if a new frame was grabbed
if (!bSuccess) //if not success, break loop
{
std::cout << "[WARNING] Could not read a new frame from video stream" << std::endl;
break;
}
if (bIsImageUndistorted && isCamParamsSet)
{
cv::Mat temp = imgOriginal.clone();
cv::undistort(temp, imgOriginal, cameraMatrix, distCoeffs);
}
// converts the captured frame from BGR to HSV
cv::Mat imgHSV;
cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV);
// thresholds the image based on the trackbar values
cv::Mat imgThresholded;
inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded);
// applies morphological opening (removes small objects from the foreground)
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
// applies morphological closing (removes small holes from the foreground)
cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
// calculates the moments of the thresholded image
Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;
// if the area <= iAreaThresold, considers that the there are no object in the image and it's because of the noise, the area is not zero
if (dArea > iAreaThresold)
{
// calculates the position of the ball
int posX = dM10 / dArea;
int posY = dM01 / dArea;
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
{
// draww a red line from the previous point to the current point
line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0,0,255), 2);
}
// stores the current position for enxt frame
iLastX = posX;
iLastY = posY;
}
// displays the thresholded image
imshow("Thresholded Image", imgThresholded);
// shows the original image with the tracking (red) lines
imgOriginal = imgOriginal + imgLines;
imshow("Original", imgOriginal);
// waits for awhile depending on the FPS value
char key = (char)cv::waitKey(1000.0/fFPS);
// checks if ESC was pressed to exit
if (key == 27) // if 'esc' key is pressed, break loop
{
std::cout << "[INFO] esc key is pressed by user -> Shuting down!" << std::endl;
break;
}
if (key == 'u')
{
bIsImageUndistorted = !bIsImageUndistorted;
std::cout << "[INFO] Image undistorted: " << bIsImageUndistorted<< std::endl;
}
}
return 0;
}

31
src/ReferenceTransform.cpp Normal file
View File

@ -0,0 +1,31 @@
#include <opencv2/opencv.hpp>
#include <iostream>
// Gerçek dünya dönüşüm oranlarını tanımla (Bunları ölçüm yaparak ayarla!)
#define RATIO_CM_PER_PIXEL_X 0.1 // X yönünde cm/pixel oranı
#define RATIO_CM_PER_PIXEL_Y 0.1 // Y yönünde cm/pixel oranı
#define IMAGE_WIDTH 640 // Kameranın çözünürlüğünü buraya yaz
#define IMAGE_HEIGHT 480
// Piksellerden Gerçek Dünya Koordinatlarına Dönüşüm Fonksiyonu
cv::Point2f pixelToWorld(int u, int v) {
float x = (v - IMAGE_HEIGHT / 2) * RATIO_CM_PER_PIXEL_Y;
float y = (u - IMAGE_WIDTH / 2) * RATIO_CM_PER_PIXEL_X;
return cv::Point2f(x, y);
}
int main() {
int u, v;
// Kullanıcıdan piksel koordinatlarını al
std::cout << "Enter pixel coordinates (u v): ";
std::cin >> u >> v;
// Koordinat dönüşümünü uygula
cv::Point2f worldCoord = pixelToWorld(u, v);
// Sonucu ekrana yazdır
std::cout << "World coordinates (cm): X = " << worldCoord.x << ", Y = " << worldCoord.y << std::endl;
return 0;
}

167
src/RobotServoing.cpp Normal file
View File

@ -0,0 +1,167 @@
#include <chrono>
#include <thread>
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/calib3d.hpp>
#include "Kinematics.h"
#include "DynamixelHandler.h"
#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml"
#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml"
#define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5
#define AREA_THRESOLD 1000
#define ROBOT_L1 5
#define ROBOT_L2 6
#define RATIO_CM_PER_PIXEL_X 0.1 // X ekseninde dönüşüm oranı
#define RATIO_CM_PER_PIXEL_Y 0.1 // Y ekseninde dönüşüm oranı
using namespace cv;
using namespace std;
DynamixelHandler _oDxlHandler;
std::string _robotDxlPortName = "/dev/ttyUSB0";
float _robotDxlProtocol = 2.0;
int _robotDxlBaudRate = 1000000;
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
{
std::cout << "=== Initialization of the Dynamixel Motor communication ===" << std::endl;
dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol);
dxlHandler.openPort();
dxlHandler.setBaudRate(baudRate);
dxlHandler.enableTorque(true);
std::cout << std::endl;
}
void closeRobot(DynamixelHandler& dxlHandler)
{
dxlHandler.enableTorque(false);
dxlHandler.closePort();
}
// Pikselden dünya koordinatlarına dönüştürme fonksiyonu
cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height)
{
float x = (v - img_height / 2) * RATIO_CM_PER_PIXEL_Y;
float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X;
return cv::Point2f(x, y);
}
int main(int argc, char** argv)
{
float L1 = ROBOT_L1;
float L2 = ROBOT_L2;
float qpen = deg2rad(-90); // in rad
std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
float fFPS = FPS;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD;
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
// Kamera açılıyor
VideoCapture cap(0, cv::CAP_V4L2);
if (!cap.isOpened())
{
cout << "[ERROR] Could not open the camera!" << endl;
return -1;
}
// İlk görüntü alınıyor
Mat imgTmp;
cap.read(imgTmp);
int img_width = imgTmp.size().width;
int img_height = imgTmp.size().height;
int iLastX = -1, iLastY = -1;
while (true)
{
Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal);
if (!bSuccess)
{
cout << "[WARNING] Could not read a new frame from video stream" << endl;
break;
}
// Görüntüyü HSV formatına çevir
cv::Mat imgHSV;
cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV);
// Renk eşikleme
cv::Mat imgThresholded;
inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded);
// Morfolojik işlemler
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
// Moment hesaplama
Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;
int posX = -1, posY = -1;
if (dArea > iAreaThresold)
{
posX = dM10 / dArea;
posY = dM01 / dArea;
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
{
line(imgOriginal, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
}
iLastX = posX;
iLastY = posY;
}
// Piksel koordinatlarını dünya koordinatlarına çevir
cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height);
float x = worldCoord.x;
float y = worldCoord.y;
cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl;
// Ters kinematik hesaplama
std::vector<float> qi = computeInverseKinematics(x, y, L1, L2);
// Robot eklemleri hareket ettir
if (qi.size() >= 3)
{
std::vector<float> vTargetJointPosition;
vTargetJointPosition.push_back(qi[1]);
vTargetJointPosition.push_back(qpen);
vTargetJointPosition.push_back(qi[2]);
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
}
// Görüntü gösterme
imshow("Original", imgOriginal);
// Kullanıcı girişini kontrol et
char key = (char)cv::waitKey(1000.0 / fFPS);
if (key == 27) // 'ESC' tuşu çıkışı sağlıyor
{
cout << "[INFO] ESC key pressed -> Shutting down!" << endl;
break;
}
}
// Robot bağlantısını kapat
closeRobot(_oDxlHandler);
return 0;
}