To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit e1c93f59 authored by Lars's avatar Lars
Browse files

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,13 +141,19 @@ 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,20 +231,23 @@ 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]))
outt2.write(str(tvec[2]))
outr0.write(str(rvec[0]))
outr1.write(str(rvec[1]))
outr2.write(str(rvec[2]))
outt0.write(str(tvec[0]))
outt1.write(str(tvec[1]))
outt2.write(str(tvec[2]))
outr0.write(str(rvec[0]))
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
/*
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
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.
Mozilla Public License Version 2.0
==================================
1. Definitions
--------------
1.1. "Contributor"
means each individual or legal entity that creates, contributes to
the creation of, or owns Covered Software.
1.2. "Contributor Version"
means the combination of the Contributions of others (if any) used
by a Contributor and that particular Contributor's Contribution.
1.3. "Contribution"
means Covered Software of a particular Contributor.
1.4. "Covered Software"
means Source Code Form to which the initial Contributor has attached
the notice in Exhibit A, the Executable Form of such Source Code
Form, and Modifications of such Source Code Form, in each case
including portions thereof.
1.5. "Incompatible With Secondary Licenses"
means
(a) that the initial Contributor has attached the notice described
in Exhibit B to the Covered Software; or
(b) that the Covered Software was made available under the terms of
version 1.1 or earlier of the License, but not also under the
terms of a Secondary License.
1.6. "Executable Form"
means any form of the work other than Source Code Form.
1.7. "Larger Work"
means a work that combines Covered Software with other material, in
a separate file or files, that is not Covered Software.
1.8. "License"
means this document.
1.9. "Licensable"
means having the right to grant, to the maximum extent possible,
whether at the time of the initial grant or subsequently, any and
all of the rights conveyed by this License.
1.10. "Modifications"
means any of the following:
(a) any file in Source Code Form that results from an addition to,
deletion from, or modification of the contents of Covered
Software; or
(b) any new file in Source Code Form that contains any Covered
Software.
1.11. "Patent Claims" of a Contributor
means any patent claim(s), including without limitation, method,
process, and apparatus claims, in any patent Licensable by such
Contributor that would be infringed, but for the grant of the
License, by the making, using, selling, offering for sale, having
made, import, or transfer of either its Contributions or its
Contributor Version.
1.12. "Secondary License"
means either the GNU General Public License, Version 2.0, the GNU
Lesser General Public License, Version 2.1, the GNU Affero General
Public License, Version 3.0, or any later versions of those
licenses.
1.13. "Source Code Form"
means the form of the work preferred for making modifications.
1.14. "You" (or "Your")
means an individual or a legal entity exercising rights under this
License. For legal entities, "You" includes any entity that
controls, is controlled by, or is under common control with You. For
purposes of this definition, "control" means (a) the power, direct
or indirect, to cause the direction or management of such entity,
whether by contract or otherwise, or (b) ownership of more than
fifty percent (50%) of the outstanding shares or beneficial
ownership of such entity.
2. License Grants and Conditions
--------------------------------
2.1. Grants
Each Contributor hereby grants You a world-wide, royalty-free,
non-exclusive license:
(a) under intellectual property rights (other than patent or trademark)
Licensable by such Contributor to use, reproduce, make available,
modify, display, perform, distribute, and otherwise exploit its
Contributions, either on an unmodified basis, with Modifications, or
as part of a Larger Work; and
(b) under Patent Claims of such Contributor to make, use, sell, offer
for sale, have made, import, and otherwise transfer either its
Contributions or its Contributor Version.
2.2. Effective Date
The licenses granted in Section 2.1 with respect to any Contribution
become effective for each Contribution on the date the Contributor first
distributes such Contribution.
2.3. Limitations on Grant Scope
The licenses granted in this Section 2 are the only rights granted under
this License. No additional rights or licenses will be implied from the
distribution or licensing of Covered Software under this License.
Notwithstanding Section 2.1(b) above, no patent license is granted by a
Contributor:
(a) for any code that a Contributor has removed from Covered Software;
or
(b) for infringements caused by: (i) Your and any other third party's
modifications of Covered Software, or (ii) the combination of its
Contributions with other software (except as part of its Contributor
Version); or
(c) under Patent Claims infringed by Covered Software in the absence of
its Contributions.
This License does not grant any rights in the trademarks, service marks,
or logos of any Contributor (except as may be necessary to comply with
the notice requirements in Section 3.4).
2.4. Subsequent Licenses
No Contributor makes additional grants as a result of Your choice to
distribute the Covered Software under a subsequent version of this
License (see Section 10.2) or under the terms of a Secondary License (if
permitted under the terms of Section 3.3).
2.5. Representation
Each Contributor represents that the Contributor believes its
Contributions are its original creation(s) or it has sufficient rights
to grant the rights to its Contributions conveyed by this License.
2.6. Fair Use
This License is not intended to limit any rights You have under
applicable copyright doctrines of fair use, fair dealing, or other
equivalents.
2.7. Conditions
Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
in Section 2.1.
3. Responsibilities
-------------------
3.1. Distribution of Source Form
All distribution of Covered Software in Source Code Form, including any
Modifications that You create or to which You contribute, must be under
the terms of this License. You must inform recipients that the Source
Code Form of the Covered Software is governed by the terms of this
License, and how they can obtain a copy of this License. You may not
attempt to alter or restrict the recipients' rights in the Source Code
Form.
3.2. Distribution of Executable Form
If You distribute Covered Software in Executable Form then:
(a) such Covered Software must also be made available in Source Code
Form, as described in Section 3.1, and You must inform recipients of
the Executable Form how they can obtain a copy of such Source Code
Form by reasonable means in a timely manner, at a charge no more
than the cost of distribution to the recipient; and
(b) You may distribute such Executable Form under the terms of this
License, or sublicense it under different terms, provided that the
license for the Executable Form does not attempt to limit or alter
the recipients' rights in the Source Code Form under this License.
3.3. Distribution of a Larger Work
You may create and distribute a Larger Work under terms of Your choice,
provided that You also comply with the requirements of this License for
the Covered Software. If the Larger Work is a combination of Covered
Software with a work governed by one or more Secondary Licenses, and the
Covered Software is not Incompatible With Secondary Licenses, this
License permits You to additionally distribute such Covered Software
under the terms of such Secondary License(s), so that the recipient of
the Larger Work may, at their option, further distribute the Covered
Software under the terms of either this License or such Secondary
License(s).
3.4. Notices
You may not remove or alter the substance of any license notices
(including copyright notices, patent notices, disclaimers of warranty,
or limitations of liability) contained within the Source Code Form of
the Covered Software, except that You may alter any license notices to
the extent required to remedy known factual inaccuracies.
3.5. Application of Additional Terms
You may choose to offer, and to charge a fee for, warranty, support,
indemnity or liability obligations to one or more recipients of Covered
Software. However, You may do so only on Your own behalf, and not on
behalf of any Contributor. You must make it absolutely clear that any
such warranty, support, indemnity, or liability obligation is offered by
You alone, and You hereby agree to indemnify every Contributor for any
liability incurred by such Contributor as a result of warranty, support,
indemnity or liability terms You offer. You may include additional
disclaimers of warranty and limitations of liability specific to any
jurisdiction.
4. Inability to Comply Due to Statute or Regulation
---------------------------------------------------
If it is impossible for You to comply with any of the terms of this
License with respect to some or all of the Covered Software due to
statute, judicial order, or regulation then You must: (a) comply with
the terms of this License to the maximum extent possible; and (b)
describe the limitations and the code they affect. Such description must
be placed in a text file included with all distributions of the Covered
Software under this License. Except to the extent prohibited by statute
or regulation, such description must be sufficiently detailed for a
recipient of ordinary skill to be able to understand it.
5. Termination
--------------
5.1. The rights granted under this License will terminate automatically
if You fail to comply with any of its terms. However, if You become
compliant, then the rights granted under this License from a particular
Contributor are reinstated (a) provisionally, unless and until such
Contributor explicitly and finally terminates Your grants, and (b) on an
ongoing basis, if such Contributor fails to notify You of the
non-compliance by some reasonable means prior to 60 days after You have
come back into compliance. Moreover, Your grants from a particular
Contributor are reinstated on an ongoing basis if such Contributor
notifies You of the non-compliance by some reasonable means, this is the
first time You have received notice of non-compliance with this License
from such Contributor, and You become compliant prior to 30 days after
Your receipt of the notice.
5.2. If You initiate litigation against any entity by asserting a patent
infringement claim (excluding declaratory judgment actions,
counter-claims, and cross-claims) alleging that a Contributor Version
directly or indirectly infringes any patent, then the rights granted to
You by any and all Contributors for the Covered Software under Section
2.1 of this License shall terminate.
5.3. In the event of termination under Sections 5.1 or 5.2 above, all
end user license agreements (excluding distributors and resellers) which
have been validly granted by You or Your distributors under this License
prior to termination shall survive termination.
************************************************************************
* *
* 6. Disclaimer of Warranty *
* ------------------------- *
* *
* Covered Software is provided under this License on an "as is" *
* basis, without warranty of any kind, either expressed, implied, or *
* statutory, including, without limitation, warranties that the *
* Covered Software is free of defects, merchantable, fit for a *
* particular purpose or non-infringing. The entire risk as to the *
* quality and performance of the Covered Software is with You. *
* Should any Covered Software prove defective in any respect, You *
* (not any Contributor) assume the cost of any necessary servicing, *
* repair, or correction. This disclaimer of warranty constitutes an *
* essential part of this License. No use of any Covered Software is *
* authorized under this License except under this disclaimer. *
* *
************************************************************************
************************************************************************
* *
* 7. Limitation of Liability *
* -------------------------- *
* *
* Under no circumstances and under no legal theory, whether tort *
* (including negligence), contract, or otherwise, shall any *
* Contributor, or anyone who distributes Covered Software as *
* permitted above, be liable to You for any direct, indirect, *
* special, incidental, or consequential damages of any character *
* including, without limitation, damages for lost profits, loss of *
* goodwill, work stoppage, computer failure or malfunction, or any *
* and all other commercial damages or losses, even if such party *
* shall have been informed of the possibility of such damages. This *
* limitation of liability shall not apply to liability for death or *
* personal injury resulting from such party's negligence to the *
* extent applicable law prohibits such limitation. Some *
* jurisdictions do not allow the exclusion or limitation of *
* incidental or consequential damages, so this exclusion and *
* limitation may not apply to You. *
* *
************************************************************************
8. Litigation
-------------
Any litigation relating to this License may be brought only in the
courts of a jurisdiction where the defendant maintains its principal
place of business and such litigation shall be governed by laws of that
jurisdiction, without reference to its conflict-of-law provisions.
Nothing in this Section shall prevent a party's ability to bring
cross-claims or counter-claims.
9. Miscellaneous
----------------
This License represents the complete agreement concerning the subject
matter hereof. If any provision of this License is held to be
unenforceable, such provision shall be reformed only to the extent
necessary to make it enforceable. Any law or regulation which provides
that the language of a contract shall be construed against the drafter
shall not be used to construe this License against a Contributor.
10. Versions of the License
---------------------------