Commit e1c93f59 by Lars

### Rotation matrices working!!!

parent fc128316
 ... ... @@ -80,8 +80,8 @@ from imutils.video import VideoStream #PARAMS MARKER_SIZE = 200 #size in mm CAMERA = 1 #which Port is the Reciever connected to MARKER_SIZE = 180.0 #size in mm CAMERA = 0 #which Port is the Reciever connected to PMKF = True #Do you want a PointMassKalmanFilter TEXT_OUTPUT = False #Do you want text output of the raw data ... ... @@ -141,12 +141,18 @@ if PMKF: #PointMassKalmanFilter Kinf_r = np.matrix([[0.0686292493],[0.0683245284]]) def pmkf(state_old, input, type): #type = True for trans, False for rot if type = True: return Ahat_t*state_old_a+ Kinf_t*input else return Ahat_r*state_old_a+ Kinf_r*input if type == True: return Ahat_t*state_old+ Kinf_t*input else: return Ahat_r*state_old+ Kinf_r*input state_t = [np.matrix([[0],[0]]),np.matrix([[0],[0]]),np.matrix([[0],[0]])] state_r = [np.matrix([[0],[0]]),np.matrix([[0],[0]]),np.matrix([[0],[0]])] def fixRot(rot): if rot<= 0: rot = rot + math.pi else: rot = rot -math.pi return rot ... ... @@ -225,11 +231,14 @@ while(cam.isOpened()): #PointMassKalmanFilter if PMKF: print('before: ' + str(rvec[0])) rvec[0] = fixRot(rvec[0]) print('after: ' + str(rvec[0])) for i in range(0,3): state_t[i] = pmkf(state_t[i], tvec[i], True) #translation state_r[i] = pmkf(state_r[i], tvec[i], False) #rotation tvec[i] = state_t[i,0] rvec[i] = state_r[i,0] state_r[i] = pmkf(state_r[i], rvec[i], False) #rotation tvec[i] = state_t[i][0] rvec[i] = state_r[i][0] if TEXT_OUTPUT: outt0.write(str(tvec[0])) outt1.write(str(tvec[1])) ... ... @@ -238,7 +247,7 @@ while(cam.isOpened()): outr1.write(str(rvec[1])) outr2.write(str(rvec[2])) #publish cfcamera_pub.publish(rvec[0],rvec[1],rvec[2],tvec[0],tvec[1],tvec[2]) cfcamera_pub.publish(rvec[0],rvec[1],rvec[2],tvec[0]/1100,tvec[1]/1000,tvec[2]/2050) #print to screen outputr = 'r: '+str(rvec) outputt = 't: '+str(tvec) ... ... @@ -254,19 +263,12 @@ while(cam.isOpened()): else: print('Wrong Marker detected at Frame: ' + str(framecount)) mask[0] = 1 break else: print('No Marker detected at Frame: ' + str(framecount)) mask[0] = 1 #kalman filter----------------------------------------------------------------------------------------------------------- masked_data = np.ma.masked_array(data, mask) #mask missing measurements #filtered, covariance = (kf.filter_update(filtered, covariance, masked_data[0])) data_out = filtered output = 'flt' + str(np.round(data_out, decimals = 2)) #cv2.putText(QueryImg, output, (0,50), fontface, fontscale, fontcolor) # Display our image cv2.imshow('QueryImage', QueryImg) # Exit at the end of the video on the 'q' keypress ... ...
 repo: 8a21fd850624c931e448cbcfb38168cb2717c790 node: 323c052e17310984a40dda9426e0435bed7178fa branch: 3.3 tag: 3.3.7
 [patterns] *.sh = LF *.MINPACK = CRLF scripts/*.in = LF debug/msvc/*.dat = CRLF debug/msvc/*.natvis = CRLF unsupported/test/mpreal/*.* = CRLF ** = native [repository] native = LF
 syntax: glob qrc_*cxx *.orig *.pyc *.diff diff *.save save *.old *.gmo *.qm core core.* *.bak *~ *build* *.moc.* *.moc ui_* CMakeCache.txt tags .*.swp activity.png *.out *.php* *.log *.orig *.rej log patch a a.* lapack/testing lapack/reference
 2db9468678c6480c9633b6272ff0e3599d1e11a3 2.0-beta3 375224817dce669b6fa31d920d4c895a63fabf32 2.0-beta1 3b8120f077865e2a072e10f5be33e1d942b83a06 2.0-rc1 19dfc0e7666bcee26f7a49eb42f39a0280a3485e 2.0-beta5 7a7d8a9526f003ffa2430dfb0c2c535b5add3023 2.0-beta4 7d14ad088ac23769c349518762704f0257f6a39b 2.0.1 b9d48561579fd7d4c05b2aa42235dc9de6484bf2 2.0-beta6 e17630a40408243cb1a51ad0fe3a99beb75b7450 before-hg-migration eda654d4cda2210ce80719addcf854773e6dec5a 2.0.0 ee9a7c468a9e73fab12f38f02bac24b07f29ed71 2.0-beta2 d49097c25d8049e730c254a2fed725a240ce4858 after-hg-migration 655348878731bcb5d9bbe0854077b052e75e5237 actual-start-from-scratch 12a658962d4e6dfdc9a1c350fe7b69e36e70675c 3.0-beta1 5c4180ad827b3f869b13b1d82f5a6ce617d6fcee 3.0-beta2 7ae24ca6f3891d5ac58ddc7db60ad413c8d6ec35 3.0-beta3 c40708b9088d622567fecc9208ad4a426621d364 3.0-beta4 b6456624eae74f49ae8683d8e7b2882a2ca0342a 3.0-rc1 a810d5dbab47acfe65b3350236efdd98f67d4d8a 3.1.0-alpha1 304c88ca3affc16dd0b008b1104873986edd77af 3.1.0-alpha2 920fc730b5930daae0a6dbe296d60ce2e3808215 3.1.0-beta1 8383e883ebcc6f14695ff0b5e20bb631abab43fb 3.1.0-rc1 bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2 da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1 a8e0d153fc5e239ef8b06e3665f1f9e8cb8d49c8 before-evaluators 09a8e21866106b49c5dec1d6d543e5794e82efa0 3.3-alpha1 ce5a455b34c0a0ac3545a1497cb4a16c38ed90e8 3.3-beta1 69d418c0699907bcd0bf9e0b3ba0a112ed091d85 3.3-beta2 bef509908b9da05d0d07ffc0da105e2c8c6d3996 3.3-rc1 04ab5fa4b241754afcf631117572276444c67239 3.3-rc2 26667be4f70baf4f0d39e96f330714c87b399090 3.3.0 f562a193118d4f40514e2f4a0ace6e974926ef06 3.3.1 da9b4e14c2550e0d11078a3c39e6d56eba9905df 3.3.2 67e894c6cd8f5f1f604b27d37ed47fdf012674ff 3.3.3 5a0156e40feb7c4136680b493c6e433d91a6f355 3.3.4 b3f3d4950030e3fa2e8fde6b68405106ae5685e1 3.3.5 b70bf4fad46787bce88d17d6f80d22f70ac882a1 3.3.6
This diff is collapsed.
 /* Copyright (c) 2011, Intel Corporation. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of Intel Corporation nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ \ No newline at end of file
This diff is collapsed.
This diff is collapsed.
 Minpack Copyright Notice (1999) University of Chicago. All rights reserved Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. The end-user documentation included with the redistribution, if any, must include the following acknowledgment: "This product includes software developed by the University of Chicago, as Operator of Argonne National Laboratory. Alternately, this acknowledgment may appear in the software itself, if and wherever such third-party acknowledgments normally appear. 4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL BE CORRECTED. 5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT (INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE POSSIBILITY OF SUCH LOSS OR DAMAGES.