Repository: hovren/crisp
Branch: master
Commit: 65cae19e7cfa
Files: 27
Total size: 169.7 KB
Directory structure:
gitextract_22n3_8sv/
├── .gitignore
├── CITATION
├── LICENSE
├── MANIFEST.in
├── README.md
├── conda.recipe/
│ ├── bld.bat
│ ├── build.sh
│ └── meta.yaml
├── crisp/
│ ├── __init__.py
│ ├── calibration.py
│ ├── camera.py
│ ├── cli.py
│ ├── fastintegrate/
│ │ └── fastintegrate.pyx
│ ├── imu.py
│ ├── l3g4200d.py
│ ├── pose.py
│ ├── ransac.py
│ ├── remove_slp.py
│ ├── rotations.py
│ ├── stream.py
│ ├── timesync.py
│ ├── tracking.py
│ ├── videoslice.py
│ └── znccpyr.py
├── examples/
│ └── gopro_dataset_example.py
├── setup.cfg
└── setup.py
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
build/
*.pyc
*.mat
*~
*.m
*.c
*.so
*.hdf
*.ipynb
.eggs/
.idea/
.ipynb_checkpoints/
crisp.egg-info/
dist/
crisp/fastintegrate/fastint.c
================================================
FILE: CITATION
================================================
To cite this software please use the following BibTex information
@inproceedings{Ovren2015,
title = {{Gyroscope-based video stabilisation with auto-calibration}},
author = {Ovrén, Hannes and Forssén, Per-Erik},
booktitle = {2015 IEEE International Conference on Robotics and Automation (ICRA)},
year = {2015},
month = may,
address = {Seattle, WA},
pages = {2090--2097},
doi = {10.1109/ICRA.2015.7139474},
}
================================================
FILE: LICENSE
================================================
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The GNU General Public License is a free, copyleft license for
software and other kinds of works.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
the GNU General Public License is intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
software for all its users. We, the Free Software Foundation, use the
GNU General Public License for most of our software; it applies also to
any other work released this way by its authors. You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
To protect your rights, we need to prevent others from denying you
these rights or asking you to surrender the rights. Therefore, you have
certain responsibilities if you distribute copies of the software, or if
you modify it: responsibilities to respect the freedom of others.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must pass on to the recipients the same
freedoms that you received. You must make sure that they, too, receive
or can get the source code. And you must show them these terms so they
know their rights.
Developers that use the GNU GPL protect your rights with two steps:
(1) assert copyright on the software, and (2) offer you this License
giving you legal permission to copy, distribute and/or modify it.
For the developers' and authors' protection, the GPL clearly explains
that there is no warranty for this free software. For both users' and
authors' sake, the GPL requires that modified versions be marked as
changed, so that their problems will not be attributed erroneously to
authors of previous versions.
Some devices are designed to deny users access to install or run
modified versions of the software inside them, although the manufacturer
can do so. This is fundamentally incompatible with the aim of
protecting users' freedom to change the software. The systematic
pattern of such abuse occurs in the area of products for individuals to
use, which is precisely where it is most unacceptable. Therefore, we
have designed this version of the GPL to prohibit the practice for those
products. If such problems arise substantially in other domains, we
stand ready to extend this provision to those domains in future versions
of the GPL, as needed to protect the freedom of users.
Finally, every program is threatened constantly by software patents.
States should not allow patents to restrict development and use of
software on general-purpose computers, but in those that do, we wish to
avoid the special danger that patents applied to a free program could
make it effectively proprietary. To prevent this, the GPL assures that
patents cannot be used to render the program non-free.
The precise terms and conditions for copying, distribution and
modification follow.
TERMS AND CONDITIONS
0. Definitions.
"This License" refers to version 3 of the GNU General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
"The Program" refers to any copyrightable work licensed under this
License. Each licensee is addressed as "you". "Licensees" and
"recipients" may be individuals or organizations.
To "modify" a work means to copy from or adapt all or part of the work
in a fashion requiring copyright permission, other than the making of an
exact copy. The resulting work is called a "modified version" of the
earlier work or a work "based on" the earlier work.
A "covered work" means either the unmodified Program or a work based
on the Program.
To "propagate" a work means to do anything with it that, without
permission, would make you directly or secondarily liable for
infringement under applicable copyright law, except executing it on a
computer or modifying a private copy. Propagation includes copying,
distribution (with or without modification), making available to the
public, and in some countries other activities as well.
To "convey" a work means any kind of propagation that enables other
parties to make or receive copies. Mere interaction with a user through
a computer network, with no transfer of a copy, is not conveying.
An interactive user interface displays "Appropriate Legal Notices"
to the extent that it includes a convenient and prominently visible
feature that (1) displays an appropriate copyright notice, and (2)
tells the user that there is no warranty for the work (except to the
extent that warranties are provided), that licensees may convey the
work under this License, and how to view a copy of this License. If
the interface presents a list of user commands or options, such as a
menu, a prominent item in the list meets this criterion.
1. Source Code.
The "source code" for a work means the preferred form of the work
for making modifications to it. "Object code" means any non-source
form of a work.
A "Standard Interface" means an interface that either is an official
standard defined by a recognized standards body, or, in the case of
interfaces specified for a particular programming language, one that
is widely used among developers working in that language.
The "System Libraries" of an executable work include anything, other
than the work as a whole, that (a) is included in the normal form of
packaging a Major Component, but which is not part of that Major
Component, and (b) serves only to enable use of the work with that
Major Component, or to implement a Standard Interface for which an
implementation is available to the public in source code form. A
"Major Component", in this context, means a major essential component
(kernel, window system, and so on) of the specific operating system
(if any) on which the executable work runs, or a compiler used to
produce the work, or an object code interpreter used to run it.
The "Corresponding Source" for a work in object code form means all
the source code needed to generate, install, and (for an executable
work) run the object code and to modify the work, including scripts to
control those activities. However, it does not include the work's
System Libraries, or general-purpose tools or generally available free
programs which are used unmodified in performing those activities but
which are not part of the work. For example, Corresponding Source
includes interface definition files associated with source files for
the work, and the source code for shared libraries and dynamically
linked subprograms that the work is specifically designed to require,
such as by intimate data communication or control flow between those
subprograms and other parts of the work.
The Corresponding Source need not include anything that users
can regenerate automatically from other parts of the Corresponding
Source.
The Corresponding Source for a work in source code form is that
same work.
2. Basic Permissions.
All rights granted under this License are granted for the term of
copyright on the Program, and are irrevocable provided the stated
conditions are met. This License explicitly affirms your unlimited
permission to run the unmodified Program. The output from running a
covered work is covered by this License only if the output, given its
content, constitutes a covered work. This License acknowledges your
rights of fair use or other equivalent, as provided by copyright law.
You may make, run and propagate covered works that you do not
convey, without conditions so long as your license otherwise remains
in force. You may convey covered works to others for the sole purpose
of having them make modifications exclusively for you, or provide you
with facilities for running those works, provided that you comply with
the terms of this License in conveying all material for which you do
not control copyright. Those thus making or running the covered works
for you must do so exclusively on your behalf, under your direction
and control, on terms that prohibit them from making any copies of
your copyrighted material outside their relationship with you.
Conveying under any other circumstances is permitted solely under
the conditions stated below. Sublicensing is not allowed; section 10
makes it unnecessary.
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological
measure under any applicable law fulfilling obligations under article
11 of the WIPO copyright treaty adopted on 20 December 1996, or
similar laws prohibiting or restricting circumvention of such
measures.
When you convey a covered work, you waive any legal power to forbid
circumvention of technological measures to the extent such circumvention
is effected by exercising rights under this License with respect to
the covered work, and you disclaim any intention to limit operation or
modification of the work as a means of enforcing, against the work's
users, your or third parties' legal rights to forbid circumvention of
technological measures.
4. Conveying Verbatim Copies.
You may convey verbatim copies of the Program's source code as you
receive it, in any medium, provided that you conspicuously and
appropriately publish on each copy an appropriate copyright notice;
keep intact all notices stating that this License and any
non-permissive terms added in accord with section 7 apply to the code;
keep intact all notices of the absence of any warranty; and give all
recipients a copy of this License along with the Program.
You may charge any price or no price for each copy that you convey,
and you may offer support or warranty protection for a fee.
5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to
produce it from the Program, in the form of source code under the
terms of section 4, provided that you also meet all of these conditions:
a) The work must carry prominent notices stating that you modified
it, and giving a relevant date.
b) The work must carry prominent notices stating that it is
released under this License and any conditions added under section
7. This requirement modifies the requirement in section 4 to
"keep intact all notices".
c) You must license the entire work, as a whole, under this
License to anyone who comes into possession of a copy. This
License will therefore apply, along with any applicable section 7
additional terms, to the whole of the work, and all its parts,
regardless of how they are packaged. This License gives no
permission to license the work in any other way, but it does not
invalidate such permission if you have separately received it.
d) If the work has interactive user interfaces, each must display
Appropriate Legal Notices; however, if the Program has interactive
interfaces that do not display Appropriate Legal Notices, your
work need not make them do so.
A compilation of a covered work with other separate and independent
works, which are not by their nature extensions of the covered work,
and which are not combined with it such as to form a larger program,
in or on a volume of a storage or distribution medium, is called an
"aggregate" if the compilation and its resulting copyright are not
used to limit the access or legal rights of the compilation's users
beyond what the individual works permit. Inclusion of a covered work
in an aggregate does not cause this License to apply to the other
parts of the aggregate.
6. Conveying Non-Source Forms.
You may convey a covered work in object code form under the terms
of sections 4 and 5, provided that you also convey the
machine-readable Corresponding Source under the terms of this License,
in one of these ways:
a) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by the
Corresponding Source fixed on a durable physical medium
customarily used for software interchange.
b) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by a
written offer, valid for at least three years and valid for as
long as you offer spare parts or customer support for that product
model, to give anyone who possesses the object code either (1) a
copy of the Corresponding Source for all the software in the
product that is covered by this License, on a durable physical
medium customarily used for software interchange, for a price no
more than your reasonable cost of physically performing this
conveying of source, or (2) access to copy the
Corresponding Source from a network server at no charge.
c) Convey individual copies of the object code with a copy of the
written offer to provide the Corresponding Source. This
alternative is allowed only occasionally and noncommercially, and
only if you received the object code with such an offer, in accord
with subsection 6b.
d) Convey the object code by offering access from a designated
place (gratis or for a charge), and offer equivalent access to the
Corresponding Source in the same way through the same place at no
further charge. You need not require recipients to copy the
Corresponding Source along with the object code. If the place to
copy the object code is a network server, the Corresponding Source
may be on a different server (operated by you or a third party)
that supports equivalent copying facilities, provided you maintain
clear directions next to the object code saying where to find the
Corresponding Source. Regardless of what server hosts the
Corresponding Source, you remain obligated to ensure that it is
available for as long as needed to satisfy these requirements.
e) Convey the object code using peer-to-peer transmission, provided
you inform other peers where the object code and Corresponding
Source of the work are being offered to the general public at no
charge under subsection 6d.
A separable portion of the object code, whose source code is excluded
from the Corresponding Source as a System Library, need not be
included in conveying the object code work.
A "User Product" is either (1) a "consumer product", which means any
tangible personal property which is normally used for personal, family,
or household purposes, or (2) anything designed or sold for incorporation
into a dwelling. In determining whether a product is a consumer product,
doubtful cases shall be resolved in favor of coverage. For a particular
product received by a particular user, "normally used" refers to a
typical or common use of that class of product, regardless of the status
of the particular user or of the way in which the particular user
actually uses, or expects or is expected to use, the product. A product
is a consumer product regardless of whether the product has substantial
commercial, industrial or non-consumer uses, unless such uses represent
the only significant mode of use of the product.
"Installation Information" for a User Product means any methods,
procedures, authorization keys, or other information required to install
and execute modified versions of a covered work in that User Product from
a modified version of its Corresponding Source. The information must
suffice to ensure that the continued functioning of the modified object
code is in no case prevented or interfered with solely because
modification has been made.
If you convey an object code work under this section in, or with, or
specifically for use in, a User Product, and the conveying occurs as
part of a transaction in which the right of possession and use of the
User Product is transferred to the recipient in perpetuity or for a
fixed term (regardless of how the transaction is characterized), the
Corresponding Source conveyed under this section must be accompanied
by the Installation Information. But this requirement does not apply
if neither you nor any third party retains the ability to install
modified object code on the User Product (for example, the work has
been installed in ROM).
The requirement to provide Installation Information does not include a
requirement to continue to provide support service, warranty, or updates
for a work that has been modified or installed by the recipient, or for
the User Product in which it has been modified or installed. Access to a
network may be denied when the modification itself materially and
adversely affects the operation of the network or violates the rules and
protocols for communication across the network.
Corresponding Source conveyed, and Installation Information provided,
in accord with this section must be in a format that is publicly
documented (and with an implementation available to the public in
source code form), and must require no special password or key for
unpacking, reading or copying.
7. Additional Terms.
"Additional permissions" are terms that supplement the terms of this
License by making exceptions from one or more of its conditions.
Additional permissions that are applicable to the entire Program shall
be treated as though they were included in this License, to the extent
that they are valid under applicable law. If additional permissions
apply only to part of the Program, that part may be used separately
under those permissions, but the entire Program remains governed by
this License without regard to the additional permissions.
When you convey a copy of a covered work, you may at your option
remove any additional permissions from that copy, or from any part of
it. (Additional permissions may be written to require their own
removal in certain cases when you modify the work.) You may place
additional permissions on material, added by you to a covered work,
for which you have or can give appropriate copyright permission.
Notwithstanding any other provision of this License, for material you
add to a covered work, you may (if authorized by the copyright holders of
that material) supplement the terms of this License with terms:
a) Disclaiming warranty or limiting liability differently from the
terms of sections 15 and 16 of this License; or
b) Requiring preservation of specified reasonable legal notices or
author attributions in that material or in the Appropriate Legal
Notices displayed by works containing it; or
c) Prohibiting misrepresentation of the origin of that material, or
requiring that modified versions of such material be marked in
reasonable ways as different from the original version; or
d) Limiting the use for publicity purposes of names of licensors or
authors of the material; or
e) Declining to grant rights under trademark law for use of some
trade names, trademarks, or service marks; or
f) Requiring indemnification of licensors and authors of that
material by anyone who conveys the material (or modified versions of
it) with contractual assumptions of liability to the recipient, for
any liability that these contractual assumptions directly impose on
those licensors and authors.
All other non-permissive additional terms are considered "further
restrictions" within the meaning of section 10. If the Program as you
received it, or any part of it, contains a notice stating that it is
governed by this License along with a term that is a further
restriction, you may remove that term. If a license document contains
a further restriction but permits relicensing or conveying under this
License, you may add to a covered work material governed by the terms
of that license document, provided that the further restriction does
not survive such relicensing or conveying.
If you add terms to a covered work in accord with this section, you
must place, in the relevant source files, a statement of the
additional terms that apply to those files, or a notice indicating
where to find the applicable terms.
Additional terms, permissive or non-permissive, may be stated in the
form of a separately written license, or stated as exceptions;
the above requirements apply either way.
8. Termination.
You may not propagate or modify a covered work except as expressly
provided under this License. Any attempt otherwise to propagate or
modify it is void, and will automatically terminate your rights under
this License (including any patent licenses granted under the third
paragraph of section 11).
However, if you cease all violation of this License, then your
license from a particular copyright holder is reinstated (a)
provisionally, unless and until the copyright holder explicitly and
finally terminates your license, and (b) permanently, if the copyright
holder fails to notify you of the violation by some reasonable means
prior to 60 days after the cessation.
Moreover, your license from a particular copyright holder is
reinstated permanently if the copyright holder notifies you of the
violation by some reasonable means, this is the first time you have
received notice of violation of this License (for any work) from that
copyright holder, and you cure the violation prior to 30 days after
your receipt of the notice.
Termination of your rights under this section does not terminate the
licenses of parties who have received copies or rights from you under
this License. If your rights have been terminated and not permanently
reinstated, you do not qualify to receive new licenses for the same
material under section 10.
9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or
run a copy of the Program. Ancillary propagation of a covered work
occurring solely as a consequence of using peer-to-peer transmission
to receive a copy likewise does not require acceptance. However,
nothing other than this License grants you permission to propagate or
modify any covered work. These actions infringe copyright if you do
not accept this License. Therefore, by modifying or propagating a
covered work, you indicate your acceptance of this License to do so.
10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically
receives a license from the original licensors, to run, modify and
propagate that work, subject to this License. You are not responsible
for enforcing compliance by third parties with this License.
An "entity transaction" is a transaction transferring control of an
organization, or substantially all assets of one, or subdividing an
organization, or merging organizations. If propagation of a covered
work results from an entity transaction, each party to that
transaction who receives a copy of the work also receives whatever
licenses to the work the party's predecessor in interest had or could
give under the previous paragraph, plus a right to possession of the
Corresponding Source of the work from the predecessor in interest, if
the predecessor has it or can get it with reasonable efforts.
You may not impose any further restrictions on the exercise of the
rights granted or affirmed under this License. For example, you may
not impose a license fee, royalty, or other charge for exercise of
rights granted under this License, and you may not initiate litigation
(including a cross-claim or counterclaim in a lawsuit) alleging that
any patent claim is infringed by making, using, selling, offering for
sale, or importing the Program or any portion of it.
11. Patents.
A "contributor" is a copyright holder who authorizes use under this
License of the Program or a work on which the Program is based. The
work thus licensed is called the contributor's "contributor version".
A contributor's "essential patent claims" are all patent claims
owned or controlled by the contributor, whether already acquired or
hereafter acquired, that would be infringed by some manner, permitted
by this License, of making, using, or selling its contributor version,
but do not include claims that would be infringed only as a
consequence of further modification of the contributor version. For
purposes of this definition, "control" includes the right to grant
patent sublicenses in a manner consistent with the requirements of
this License.
Each contributor grants you a non-exclusive, worldwide, royalty-free
patent license under the contributor's essential patent claims, to
make, use, sell, offer for sale, import and otherwise run, modify and
propagate the contents of its contributor version.
In the following three paragraphs, a "patent license" is any express
agreement or commitment, however denominated, not to enforce a patent
(such as an express permission to practice a patent or covenant not to
sue for patent infringement). To "grant" such a patent license to a
party means to make such an agreement or commitment not to enforce a
patent against the party.
If you convey a covered work, knowingly relying on a patent license,
and the Corresponding Source of the work is not available for anyone
to copy, free of charge and under the terms of this License, through a
publicly available network server or other readily accessible means,
then you must either (1) cause the Corresponding Source to be so
available, or (2) arrange to deprive yourself of the benefit of the
patent license for this particular work, or (3) arrange, in a manner
consistent with the requirements of this License, to extend the patent
license to downstream recipients. "Knowingly relying" means you have
actual knowledge that, but for the patent license, your conveying the
covered work in a country, or your recipient's use of the covered work
in a country, would infringe one or more identifiable patents in that
country that you have reason to believe are valid.
If, pursuant to or in connection with a single transaction or
arrangement, you convey, or propagate by procuring conveyance of, a
covered work, and grant a patent license to some of the parties
receiving the covered work authorizing them to use, propagate, modify
or convey a specific copy of the covered work, then the patent license
you grant is automatically extended to all recipients of the covered
work and works based on it.
A patent license is "discriminatory" if it does not include within
the scope of its coverage, prohibits the exercise of, or is
conditioned on the non-exercise of one or more of the rights that are
specifically granted under this License. You may not convey a covered
work if you are a party to an arrangement with a third party that is
in the business of distributing software, under which you make payment
to the third party based on the extent of your activity of conveying
the work, and under which the third party grants, to any of the
parties who would receive the covered work from you, a discriminatory
patent license (a) in connection with copies of the covered work
conveyed by you (or copies made from those copies), or (b) primarily
for and in connection with specific products or compilations that
contain the covered work, unless you entered into that arrangement,
or that patent license was granted, prior to 28 March 2007.
Nothing in this License shall be construed as excluding or limiting
any implied license or other defenses to infringement that may
otherwise be available to you under applicable patent law.
12. No Surrender of Others' Freedom.
If conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot convey a
covered work so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you may
not convey it at all. For example, if you agree to terms that obligate you
to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
under version 3 of the GNU Affero General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
but the special requirements of the GNU Affero General Public License,
section 13, concerning interaction through a network will apply to the
combination as such.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
the GNU General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the
Program specifies that a certain numbered version of the GNU General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
GNU General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
versions of the GNU General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
Later license versions may give you additional or different
permissions. However, no additional obligations are imposed on any
author or copyright holder as a result of your choosing to follow a
later version.
15. Disclaimer of Warranty.
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGES.
17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided
above cannot be given local legal effect according to their terms,
reviewing courts shall apply local law that most closely approximates
an absolute waiver of all civil liability in connection with the
Program, unless a warranty or assumption of liability accompanies a
copy of the Program in return for a fee.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
state the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
Also add information on how to contact you by electronic and paper mail.
If the program does terminal interaction, make it output a short
notice like this when it starts in an interactive mode:
<program> Copyright (C) <year> <name of author>
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, your program's commands
might be different; for a GUI interface, you would use an "about box".
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
For more information on this, and how to apply and follow the GNU GPL, see
<http://www.gnu.org/licenses/>.
The GNU General Public License does not permit incorporating your program
into proprietary programs. If your program is a subroutine library, you
may consider it more useful to permit linking proprietary applications with
the library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License. But first, please read
<http://www.gnu.org/philosophy/why-not-lgpl.html>.
================================================
FILE: MANIFEST.in
================================================
include README.md
recursive-include crisp/fastintegrate *
================================================
FILE: README.md
================================================
# Camera-to-IMU calibration toolbox
This toolbox provides a python library to perform joint calibration of a rolling shutter camera-gyroscope system.
Given gyroscope and video data, this library can find the following parameters
* True gyroscope rate
* Time offset
* Rotation between camera and gyroscope coordinate frames
* Gyroscope measurement bias
If you use the package for your work, please cite the following paper
> Ovrén, H and Forssén, P.-E. "Gyroscope-based video stabilisation with auto-calibration." In 2015 IEEE International Conference on Robotics and Automation (ICRA) (pp. 2090–2097). Seattle, WA
## Can I use these methods for my application?
The calibration methods in this package assumes the following
- Your camera is calibrated, including known readout time
- The camera frame rate is constant, and known
- The gyroscope frame rate is constant, and approximately known (within a few Hz, or percent)
If the video and gyroscope data are *not uniformly sampled*, but you have access
to somewhat reliable timestamps, then you can still use the method
if you resample the data to be uniform.
By "reliable" we mean timestamps without drift, and no (or negligble) jitter.
## Changes from 1.0
The 2.0 version of crisp features a new fully automatic calibrator.
This means that there is no compelling reason to use the semi-manual methods in the previous version of crisp.
Therefore the old example scripts have been removed, and the old functions are not imported into the module namespace.
No old functions have been removed, so if you want to use them they are still available in submodules.
## Installation
To use the package you need the following Python packages:
* NumPy
* SciPy
* OpenCV
* matplotlib
The easiest way is to install from PyPI:
$ pip install crisp
If you want to build the package from source, you also need the *Cython* package.
To build and install the `crisp` module just run the following commands:
$ python setup.py build
$ python setup.py install
For a user-only installation add `--user` to the install command.
## Usage
The gyroscope and video data are first loaded into a stream object (`GyroStream`, and a subclass of `VideoStream` respectively).
To be able to understand how points are mapped from the real world to the image, the video stream also need a `CameraModel` (-subclass) instance.
import crisp
gyro = crisp.GyroStream.from_data(some_data_array)
camera_model = crisp.AtanCameraModel(...) # One specific choice of camera model
video = crisp.VideoStream.from_file(camera_model, video_file_path)
We then tie the streams together using a `AutoCalibrator` instance.
Since the calibration proces need to have estimates of the time offset and relative rotation,
these are first estimated using the `initialize()` member. This initialization only requires that
you give an approximate gyroscope sample rate (in Hz).
calibrator = crisp.AutoCalibrator(video, gyro)
calibrator.initialize(guessed_gyro_rate)
result = calibrator.calibrate() # Dict of calibrated parameters
Initialization and calibration errors can be caught by handling `InitializationError` and `CalibrationError`.
### Example scripts
We bundle one example script `gopro_dataset_example.py` which shows how to use the
library with the data in our dataset (http://www.cvl.isy.liu.se/research/datasets/gopro-gyro-dataset/).
This is the same dataset that was used to produce the above mentioned ICRA 2015 paper.
## Feedback
* For any questions regarding the method and paper, please send an e-mail to hannes.ovren@liu.se.
* For issues about the code, you are welcome to either use the tools (issue reporting, etc.) provided by GitHub, or send an e-mail.
## License
All code in this repository is licensed under the GPL version 3.
================================================
FILE: conda.recipe/bld.bat
================================================
"%PYTHON%" setup.py install
if errorlevel 1 exit 1
:: Add more build steps here, if they are necessary.
:: See
:: http://docs.continuum.io/conda/build.html
:: for a list of environment variables that are set during the build process.
================================================
FILE: conda.recipe/build.sh
================================================
#!/bin/bash
$PYTHON setup.py install
# Add more build steps here, if they are necessary.
# See
# http://docs.continuum.io/conda/build.html
# for a list of environment variables that are set during the build process.
================================================
FILE: conda.recipe/meta.yaml
================================================
package:
name: crisp
version: "2.2.1"
source:
git_url: https://github.com/hovren/crisp.git
git_rev: v2.2.1
# build:
# noarch_python: True
# preserve_egg_dir: True
# entry_points:
# Put any entry points (scripts to be generated automatically) here. The
# syntax is module:function. For example
#
# - crisp = crisp:main
#
# Would create an entry point called crisp that calls crisp.main()
# If this is a new build for the same version, increment the build
# number. If you do not include this key, it defaults to 0.
# number: 1
requirements:
build:
- python
- setuptools
- numpy
- mock
- nose
- scipy
- matplotlib
run:
- python
- numpy
- scipy
- opencv
- matplotlib
test:
# Python imports
imports:
- crisp
# commands:
# You can put test commands to be run here. Use this to test that the
# entry points work.
# You can also put a file called run_test.py in the recipe that will be run
# at test time.
# requires:
# Put any additional test requirements here. For example
# - nose
about:
home: https://github.com/hovren/crisp
license: GNU General Public License (GPL)
summary: 'Camera-to-IMU calibration and synchronization toolkit'
# See
# http://docs.continuum.io/conda/build.html for
# more information about meta.yaml
================================================
FILE: crisp/__init__.py
================================================
# -*- coding: utf-8 -*-
"""
========================================
Camera-to-IMU calibration toolbox
========================================
This package solves the task of finding the parameters
that relate gyroscope data with video data.
To run, please see the README or the class AutoCalibrator.
"""
from __future__ import absolute_import
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2015, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
from .camera import CameraModel, AtanCameraModel, OpenCVCameraModel
from .stream import GyroStream, VideoStream, OpenCvVideoStream
from .calibration import AutoCalibrator, CalibrationError, InitializationError
================================================
FILE: crisp/calibration.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Camera-gyro calibration module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2015, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import time
import warnings
import logging
logger = logging.getLogger('crisp')
import numpy as np
import scipy.optimize
from . import videoslice, rotations, ransac, timesync, fastintegrate
PARAM_SOURCE_ORDER = ('user', 'initialized', 'calibrated') # Increasing order of importance
PARAM_ORDER = ('gyro_rate', 'time_offset', 'gbias_x', 'gbias_y', 'gbias_z', 'rot_x', 'rot_y', 'rot_z')
MAX_OPTIMIZATION_TRACKS = 1500
MAX_OPTIMIZATION_FEV = 800
DEFAULT_NORM_C = 3.0
class CalibrationError(Exception):
pass
class InitializationError(Exception):
pass
class AutoCalibrator(object):
"""Class that handles auto calibration of a camera-gyroscope system.
This calibrator uses the method described in [1]_.
The parameters which are calibrated for are
* Gyroscope sample rate
* Time offset
* Gyroscope bias
* Rotation between camera and gyroscope
Notes
---------------------
Given time offset d, and gyro rate F, the time relation is such that we
calculate the corresponding gyroscope sample n from video time t as
n = F ( t + d )
The rotation between camera and gyroscope, R, is expressed such that it transfers points from the gyroscope
coordinate frame to the camera coordinate frame as
p_camera = R * p_gyro
The bias is applied to the gyroscope measurements, w, before integration
w_adjusted = w - bias
References
----------------------
.. [1] Ovrén, H and Forssén, P.-E. "Gyroscope-based video stabilisation with auto-calibration."
In 2015 IEEE International Conference on Robotics and Automation (ICRA) (pp. 2090–2097). Seattle, WA
"""
def __init__(self, video, gyro):
"""Create calibrator
Parameters
---------------
video : VideoStream
A video stream object that provides frames and camera information
gyro : GyroStream
A gyroscope stream that provides angular velocity measurements
"""
self.video = video
self.gyro = gyro
self.slices = None
# Parameters can be supplied from different sources, and it can be useful to track that
self.params = {
'user' : {}, # Supplied by the user
'initialized' : {}, # Estimated automatically by running initialize()
'calibrated' : {} # Final calibrated values
}
def initialize(self, gyro_rate, slices=None, skip_estimation=False):
"""Prepare calibrator for calibration
This method does three things:
1. Create slices from the video stream, if not already provided
2. Estimate time offset
3. Estimate rotation between camera and gyroscope
Parameters
------------------
gyro_rate : float
Estimated gyroscope sample rate
slices : list of Slice, optional
Slices to use for optimization
skip_estimation : bool
Do not estimate initial time offset and rotation.
Raises
--------------------
InitializationError
If the initialization fails
"""
self.params['user']['gyro_rate'] = gyro_rate
for p in ('gbias_x', 'gbias_y', 'gbias_z'):
self.params['initialized'][p] = 0.0
if slices is not None:
self.slices = slices
if self.slices is None:
self.slices = videoslice.Slice.from_stream_randomly(self.video)
logger.debug("Number of slices: {:d}".format(len(self.slices)))
if len(self.slices) < 2:
logger.error("Calibration requires at least 2 video slices to proceed, got %d", len(self.slices))
raise InitializationError("Calibration requires at least 2 video slices to proceed, got {:d}".format(len(self.slices)))
if not skip_estimation:
time_offset = self.find_initial_offset()
# TODO: Detect when time offset initialization fails, and raise InitializationError
R = self.find_initial_rotation()
if R is None:
raise InitializationError("Failed to calculate initial rotation")
def video_time_to_gyro_sample(self, t):
"""Convert video time to gyroscope sample index and interpolation factor
Parameters
-------------------
t : float
Video timestamp
Returns
--------------------
n : int
Sample index that precedes t
tau : float
Interpolation factor [0.0-1.0]. If tau=0, then t falls on exactly n. If tau=1 then t falls exactly on n+1
"""
f_g = self.parameter['gyro_rate']
d_c = self.parameter['time_offset']
n = f_g * (t + d_c)
n0 = int(np.floor(n))
tau = n - n0
return n0, tau
@property
def parameter(self):
"""Return the current best value of a parameter"""
D = {}
for source in PARAM_SOURCE_ORDER:
D.update(self.params[source])
return D
def calibrate(self, max_tracks=MAX_OPTIMIZATION_TRACKS, max_eval=MAX_OPTIMIZATION_FEV, norm_c=DEFAULT_NORM_C):
"""Perform calibration
Parameters
----------------------
max_eval : int
Maximum number of function evaluations
Returns
---------------------
dict
Optimization result
Raises
-----------------------
CalibrationError
If calibration fails
"""
x0 = np.array([self.parameter[param] for param in PARAM_ORDER])
available_tracks = np.sum([len(s.inliers) for s in self.slices])
if available_tracks < max_tracks:
warnings.warn("Could not use the requested {} tracks, since only {} were available in the slice data.".format(max_tracks, available_tracks))
max_tracks = available_tracks
# Get subset of available tracks such that all slices are still used
slice_sample_idxs = videoslice.fill_sampling(self.slices, max_tracks)
func_args = (self.slices, slice_sample_idxs, self.video.camera_model, self.gyro, norm_c)
self.slice_sample_idxs = slice_sample_idxs
logger.debug("Starting optimization on {:d} slices and {:d} tracks".format(len(self.slices), max_tracks))
start_time = time.time()
# TODO: Check what values of ftol and xtol are required for good results. The current setting is probably pessimistic.
leastsq_result = scipy.optimize.leastsq(optimization_func, x0, args=func_args, full_output=True, ftol=1e-10, xtol=1e-10, maxfev=max_eval)
elapsed = time.time() - start_time
x, covx, infodict, mesg, ier = leastsq_result
self.__debug_leastsq = leastsq_result
logger.debug("Optimization completed in {:.1f} seconds and {:d} function evaluations. ier={}, mesg='{}'".format(elapsed, infodict['nfev'], ier, mesg))
if ier in (1,2,3,4):
for pname, val in zip(PARAM_ORDER, x):
self.params['calibrated'][pname] = val
return self.parameter
else:
raise CalibrationError(mesg)
def find_initial_offset(self, pyramids=6):
"""Estimate time offset
This sets and returns the initial time offset estimation.
Parameters
---------------
pyramids : int
Number of pyramids to use for ZNCC calculations.
If initial estimation of time offset fails, try lowering this value.
Returns
---------------
float
Estimated time offset
"""
flow = self.video.flow
gyro_rate = self.parameter['gyro_rate']
frame_times = np.arange(len(flow)) / self.video.frame_rate
gyro_times = np.arange(self.gyro.num_samples) / gyro_rate
time_offset = timesync.sync_camera_gyro(flow, frame_times, self.gyro.data.T, gyro_times, levels=pyramids)
logger.debug("Initial time offset: {:.4f}".format(time_offset))
self.params['initialized']['time_offset'] = time_offset
return time_offset
def find_initial_rotation(self):
"""Estimate rotation between camera and gyroscope
This sets and returns the initial rotation estimate.
Note that the initial time offset must have been estimated before calling this function!
Returns
--------------------
(3,3) ndarray
Estimated rotation between camera and gyroscope
"""
if 'time_offset' not in self.parameter:
raise InitializationError("Can not estimate rotation without an estimate of time offset. Please estimate the offset and try again.")
dt = float(1.0 / self.parameter['gyro_rate']) # Must be python float for fastintegrate
q = self.gyro.integrate(dt)
video_axes = []
gyro_axes = []
for _slice in self.slices:
# Estimate rotation here
_slice.estimate_rotation(self.video.camera_model, ransac_threshold=7.0) # sets .axis and .angle memebers
if _slice.axis is None:
continue
assert _slice.angle > 0
t1 = _slice.start / self.video.frame_rate
n1, _ = self.video_time_to_gyro_sample(t1)
t2 = _slice.end / self.video.frame_rate
n2, _ = self.video_time_to_gyro_sample(t2)
try:
qx = q[n1]
qy = q[n2]
except IndexError:
continue # No gyro data -> nothing to do with this slice
Rx = rotations.quat_to_rotation_matrix(qx)
Ry = rotations.quat_to_rotation_matrix(qy)
R = np.dot(Rx.T, Ry)
v, theta = rotations.rotation_matrix_to_axis_angle(R)
if theta < 0:
v = -v
gyro_axes.append(v)
video_axes.append(_slice.axis)
if len(gyro_axes) < 2:
logger.warning("Rotation estimation requires at least 2 rotation axes, got {}".format(len(gyro_axes)))
return None
logger.debug("Using {:d} slices (from initial {:d} for rotation estimation".format(len(gyro_axes), len(self.slices)))
model_func = lambda data: rotations.procrustes(data[:3], data[3:6], remove_mean=False)[0]
def eval_func(model, data):
X = data[:3].reshape(3,-1)
Y = data[3:6].reshape(3,-1)
R = model
Xhat = np.dot(R, Y)
costheta = np.sum(Xhat*X, axis=0)
theta = np.arccos(costheta)
return theta
inlier_selection_prob = 0.99999
model_points = 2 # Set to 3 to use non-minimal case
inlier_ratio = 0.5
threshold = np.deg2rad(10.0)
ransac_iterations = int(np.log(1 - inlier_selection_prob) / np.log(1-inlier_ratio**model_points))
data = np.vstack((np.array(video_axes).T, np.array(gyro_axes).T))
assert data.shape == (6, len(gyro_axes))
R, ransac_conseus_idx = ransac.RANSAC(model_func, eval_func, data,
model_points, ransac_iterations,
threshold, recalculate=True)
n, theta = rotations.rotation_matrix_to_axis_angle(R)
logger.debug("Found rotation: n={} theta={}; r={}".format(n, theta, n*theta))
logger.debug(R)
rx, ry, rz = theta * n
self.params['initialized']['rot_x'] = rx
self.params['initialized']['rot_y'] = ry
self.params['initialized']['rot_z'] = rz
return R
def print_params(self):
"""Print the current best set of parameters"""
print("Parameters")
print("--------------------")
for param in PARAM_ORDER:
print(' {:>11s} = {}'.format(param, self.parameter[param]))
def sample_at_time(t, rate):
s = t * rate - 0.5 # Shift half sample due to rectangular integration
n = int(np.floor(s))
tau = s - n
return n, tau
def robust_norm(r, c):
return r / (1 + (np.abs(r)/c))
def optimization_func(x, slices, slice_sample_idxs, camera, gyro, norm_c):
# Unpack parameters and convert representations
Fg, offset, gbias_x, gbias_y, gbias_z, rot_x, rot_y, rot_z, = x
gyro_bias = np.array([gbias_x, gbias_y, gbias_z])
# Construct coordinate frame rotation matrix
v = np.array([rot_x, rot_y, rot_z])
theta = np.linalg.norm(v)
v /= theta
R_g2c = rotations.axis_angle_to_rotation_matrix(v, theta)
Tg = float(1. / Fg) # Must be python float for fastintegrate to work
row_delta = camera.readout / camera.rows
errors = [] # Residual vector
# Margin of integration is amount of gyro samples per frame
integration_margin = int(np.ceil(Fg * camera.readout))
for _slice, sample_idxs in zip(slices, slice_sample_idxs):
if len(sample_idxs) < 1:
continue
t_start = _slice.start / camera.frame_rate + offset
t_end = _slice.end / camera.frame_rate + offset
slice_start, _ = sample_at_time(t_start, Fg)
slice_end, _ = sample_at_time(t_end, Fg)
slice_end += 1 # sample_at_time() gives first sample
# Gyro samples to integrate within
integration_start = slice_start
integration_end = slice_end + integration_margin
# Handle out of bounds cases by padding left and right using
# first and last gyroscope sample respectively
if integration_start < 0 or integration_end >= gyro.num_samples:
num_local_samples = integration_end - integration_start + 1
gyro_part = np.empty((num_local_samples, 3))
if integration_start < 0:
# Pad left
part_start = -integration_start
data_start = 0
gyro_part[:part_start] = gyro.data[0]
else:
part_start = 0
data_start = integration_start
if integration_end >= gyro.num_samples:
# Pad right
rpad_len = integration_end - gyro.num_samples + 1
if rpad_len < num_local_samples:
gyro_part[-rpad_len:] = gyro.data[-1]
else: # integration range outside data
gyro_part[:] = gyro.data[-1]
part_end = -rpad_len # Not inclusive
data_end = gyro.num_samples
else:
part_end = num_local_samples
data_end = integration_end + 1
try:
gyro_part[part_start:part_end] = gyro.data[data_start:data_end]
except ValueError:
pass # Completely out of bounds. This is OK.
else: # No pad required (default case)
gyro_part = gyro.data[integration_start:integration_end+1]
# TODO: Decide what to do if integration_end - integration_start < 1. This
gyro_part_corrected = gyro_part + gyro_bias
q = fastintegrate.integrate_gyro_quaternion_uniform(gyro_part_corrected, Tg)
for track in _slice.points[sample_idxs]:
x = track[0] # Points in first frame
y = track[-1] # Points in last frame
# Get row time
tx = t_start + x[1] * row_delta
ty = t_end + y[1] * row_delta
# Sample index and interpolation value for point correspondences
nx, taux = sample_at_time(tx, Fg)
ny, tauy = sample_at_time(ty, Fg)
# Interpolate rotation using SLERP
a = nx - integration_start
b = ny - integration_start
qx = rotations.slerp(q[a], q[a+1], taux)
qy = rotations.slerp(q[b], q[b+1], tauy)
Rx = rotations.quat_to_rotation_matrix(qx)
Ry = rotations.quat_to_rotation_matrix(qy)
R1 = np.dot(Rx.T, Ry) # Note: Transpose order is "wrong", but this is because definition of Rx
R = R_g2c.dot(R1).dot(R_g2c.T)
Y = camera.unproject(y)
Xhat = np.dot(R, Y)
xhat = camera.project(Xhat)
err = x - xhat.flatten()
errors.extend(err.flatten())
# Symmetric errors, so let's do this again
R1 = np.dot(Ry.T, Rx) # Note: Transpose order is "wrong", but this is because definition of Rx
R = R_g2c.dot(R1).dot(R_g2c.T)
X = camera.unproject(x)
Yhat = np.dot(R, X)
yhat = camera.project(Yhat)
err = y - yhat.flatten()
errors.extend(err.flatten())
if not errors:
raise ValueError("No residuals!")
# Apply robust norm
robust_errors = robust_norm(np.array(errors), norm_c)
return robust_errors
================================================
FILE: crisp/camera.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Camera module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import os
import glob
import logging
logger = logging.getLogger()
import numpy as np
import cv2
import scipy.interpolate
from . import remove_slp
class CameraModel(object):
"""Class that describes a camera model
This encapsulates knowledge of a specific camera,
i.e. its parameters and how the image is formed.
Note that all cameras are assumed to be rolling shutter cameras.
"""
def __init__(self, image_size, frame_rate, readout):
"""Create camera model
Parameters
-----------------
image_size : tuple (rows, columns)
The size of the image in pixels
frame_rate : float
The frame rate of the camera
readout : float
Rolling shutter readout time. Set to 0 for global shutter cameras.
"""
self.image_size = image_size
self.frame_rate = frame_rate
self.readout = readout
@property
def rows(self):
return self.image_size[1]
@property
def columns(self):
return self.image_size[0]
def project(self, points):
"""Project 3D points to image coordinates.
This projects 3D points expressed in the camera coordinate system to image points.
Parameters
--------------------
points : (3, N) ndarray
3D points
Returns
--------------------
image_points : (2, N) ndarray
The world points projected to the image plane
"""
raise NotImplementedError("Class {} does not implement project()".format(self.__class__.__name__))
def unproject(self, image_points):
"""Find (up to scale) 3D coordinate of an image point
This is the inverse of the `project` function.
The resulting 3D points are only valid up to an unknown scale.
Parameters
----------------------
image_points : (2, N) ndarray
Image points
Returns
----------------------
points : (3, N) ndarray
3D coordinates (valid up to scale)
"""
raise NotImplementedError("Class {} does not implement unproject()".format(self.__class__.__name__))
class AtanCameraModel(CameraModel):
"""atan camera model
This implements the camera model of Devernay and Faugeras ([1]_) using the simplified form in [2]_.
References
-----------------------
.. [1] F. Devernay and O. Faugeras, “Straight lines have to be straight: Au- tomatic calibration and removal of
distortion from scenes of structured environments,” Machine Vision and Applications, vol. 13, 2001.
.. [2] Johan Hedborg and Björn Johansson. "Real time camera ego-motion compensation and lens undistortion on GPU."
Technical report, Linköping University, Department of Electrical Engineering, Sweden, 2007
"""
def __init__(self, image_size, frame_rate, readout, camera_matrix, dist_center, dist_param):
"""Create model
Parameters
------------------------
image_size : tuple (rows, columns)
The size of the image in pixels
frame_rate : float
The frame rate of the camera
readout : float
Rolling shutter readout time. Set to 0 for global shutter cameras.
camera_matrix : (3, 3) ndarray
The internal camera calibration matrix
dist_center : (2, ) ndarray
Distortion center in pixels
dist_param : float
Distortion parameter
"""
super(AtanCameraModel, self).__init__(image_size, frame_rate, readout)
self.camera_matrix = camera_matrix
self.inv_camera_matrix = np.linalg.inv(self.camera_matrix)
self.wc = dist_center
self.lgamma = dist_param
@classmethod
def from_hdf(cls, filename):
"""Load camera model params from a HDF5 file
The HDF5 file should contain the following datasets:
wc : (2,) float with distortion center
lgamma : float distortion parameter
readout : float readout value
size : (2,) int image size
fps : float frame rate
K : (3, 3) float camera matrix
Parameters
--------------------
filename : str
Path to file with parameters
Returns
---------------------
AtanCameraModel
Camera model instance
"""
import h5py
with h5py.File(filename, 'r') as f:
wc = f["wc"].value
lgamma = f["lgamma"].value
K = f["K"].value
readout = f["readout"].value
image_size = f["size"].value
fps = f["fps"].value
instance = cls(image_size, fps, readout, K, wc, lgamma)
return instance
def invert(self, points):
"""Invert the distortion
Parameters
------------------
points : ndarray
Input image points
Returns
-----------------
ndarray
Undistorted points
"""
X = points if not points.ndim == 1 else points.reshape((points.size, 1))
wx, wy = self.wc
# Switch to polar coordinates
rn = np.sqrt((X[0,:] - wx)**2 + (X[1,:] - wy)**2)
phi = np.arctan2(X[1,:] - wy, X[0,:]-wx)
# 'atan' method
r = np.tan(rn * self.lgamma) / self.lgamma;
# Switch back to rectangular coordinates
Y = np.ones(X.shape)
Y[0,:] = wx + r * np.cos(phi)
Y[1,:]= wy + r * np.sin(phi)
return Y
def apply(self, points):
"""Apply the distortion
Parameters
---------------------
points : ndarray
Input image points
Returns
-----------------
ndarray
Distorted points
"""
X = points if not points.ndim == 1 else points.reshape((points.size, 1))
wx, wy = self.wc
# Switch to polar coordinates
rn = np.sqrt((X[0,:] - wx)**2 + (X[1,:] - wy)**2)
phi = np.arctan2(X[1,:] - wy, X[0,:] - wx)
r = np.arctan(rn * self.lgamma) / self.lgamma
# Switch back to rectangular coordinates
Y = np.ones(X.shape)
Y[0,:] = wx + r * np.cos(phi)
Y[1,:] = wy + r * np.sin(phi)
return Y
def project(self, points):
"""Project 3D points to image coordinates.
This projects 3D points expressed in the camera coordinate system to image points.
Parameters
--------------------
points : (3, N) ndarray
3D points
Returns
--------------------
image_points : (2, N) ndarray
The world points projected to the image plane
"""
K = self.camera_matrix
XU = points
XU = XU / np.tile(XU[2], (3,1))
X = self.apply(XU)
x2d = np.dot(K, X)
return from_homogeneous(x2d)
def unproject(self, image_points):
"""Find (up to scale) 3D coordinate of an image point
This is the inverse of the `project` function.
The resulting 3D points are only valid up to an unknown scale.
Parameters
----------------------
image_points : (2, N) ndarray
Image points
Returns
----------------------
points : (3, N) ndarray
3D coordinates (valid up to scale)
"""
Ki = self.inv_camera_matrix
X = np.dot(Ki, to_homogeneous(image_points))
X = X / X[2]
XU = self.invert(X)
return XU
class OpenCVCameraModel(CameraModel):
"""OpenCV camera model
This implements the camera model as defined in OpenCV.
For details, see the OpenCV documentation.
"""
def __init__(self, image_size, frame_rate, readout, camera_matrix, dist_coefs):
"""Create camera model
Parameters
-------------------
image_size : tuple (rows, columns)
The size of the image in pixels
frame_rate : float
The frame rate of the camera
readout : float
Rolling shutter readout time. Set to 0 for global shutter cameras.
camera_matrix : (3, 3) ndarray
The internal camera calibration matrix
dist_coefs : ndarray
Distortion coefficients [k1, k2, p1, p2 [,k3 [,k4, k5, k6]] of 4, 5, or 8 elements.
Can be set to None to use zero parameters
"""
super(OpenCVCameraModel, self).__init__(image_size, frame_rate, readout)
self.camera_matrix = camera_matrix
self.inv_camera_matrix = np.linalg.inv(self.camera_matrix)
self.dist_coefs = dist_coefs
def project(self, points):
"""Project 3D points to image coordinates.
This projects 3D points expressed in the camera coordinate system to image points.
Parameters
--------------------
points : (3, N) ndarray
3D points
Returns
--------------------
image_points : (2, N) ndarray
The world points projected to the image plane
"""
rvec = tvec = np.zeros(3)
image_points, jac = cv2.projectPoints(points.T.reshape(-1,1,3), rvec, tvec, self.camera_matrix, self.dist_coefs)
return image_points.reshape(-1,2).T
def unproject(self, image_points):
"""Find (up to scale) 3D coordinate of an image point
This is the inverse of the `project` function.
The resulting 3D points are only valid up to an unknown scale.
Parameters
----------------------
image_points : (2, N) ndarray
Image points
Returns
----------------------
points : (3, N) ndarray
3D coordinates (valid up to scale)
"""
undist_image_points = cv2.undistortPoints(image_points.T.reshape(1,-1,2), self.camera_matrix, self.dist_coefs, P=self.camera_matrix)
world_points = np.dot(self.inv_camera_matrix, to_homogeneous(undist_image_points.reshape(-1,2).T))
return world_points
@classmethod
def from_hdf(cls, filepath):
import h5py
with h5py.File(filepath, 'r') as f:
dist_coef = f["dist_coef"].value
K = f["K"].value
readout = f["readout"].value
image_size = f["size"].value
fps = f["fps"].value
instance = cls(image_size, fps, readout, K, dist_coef)
return instance
def to_homogeneous(X):
if X.ndim == 1:
return np.append(X, 1)
else:
_, N = X.shape
Y = np.ones((3, N))
return np.vstack((X, np.ones((N, ))))
def from_homogeneous(X):
Y = X / X[2]
return Y[:2]
# Below is legacy code (pre-ICRA2015)
class Camera(object):
def __init__(self):
self.K = None
self.readout_time = 0.0
self.timestamps = []
self.files = [] # Filenames, same index corresponds to timestamps list
self._images = []
def load_image(self, filename):
return cv2.imread(filename, cv2.CV_LOAD_IMAGE_GRAYSCALE)
@property
def images(self):
if len(self._images) < 1:
self._images = [self.load_image(f) for f in self.files]
return self._images
def image_sequence(self, first=0, last=-1):
file_slice = self.files[first:] if last == -1 else self.files[first:last+1]
for filename in file_slice:
img = self.load_image(filename)
if img is None:
raise IOError("Failed to load file %s" % filename)
yield(img)
class DepthCamera(Camera):
pass
class Kinect(object):
DEFAULT_DEPTH_NIR_SHIFT = (1.5, -3.5)
DEFAULT_OPARS = [0, 0]
DEFAULT_NIR_K = np.array([[ 582.67750309, 0. , 314.96824757],
[ 0. , 584.65055308, 248.16240365],
[ 0. , 0. , 1. ]])
DEFAULT_RGB_K = np.array([[ 519.83879135, 0. , 313.55797842],
[ 0. , 520.71387 , 267.59027502],
[ 0. , 0. , 1. ]])
class NirCamera(Camera):
def load_image(self, filename):
img = cv2.imread(filename, cv2.CV_LOAD_IMAGE_UNCHANGED).astype('float32')
img = remove_slp.remove_slp(img)
img = Kinect.NirCamera.convert_nbit_float32_to_uint8(img, 10)
return img
@staticmethod
def convert_nbit_float32_to_uint8(img, nbit):
"Converts a float32 image to a uint8 under the assumption that the float32 image has a bit depth of nbit. Will copy input buffer before making changes to it"
img = img.copy()
if nbit != 8:
img *= 255.0 / (2**nbit - 1)
return img.astype('uint8')
def __init__(self, depth_camera, video_camera, mode):
self.depth_camera = depth_camera
self.video_camera = video_camera
self.video_mode = mode
self.set_default_params()
def set_default_params(self):
self.opars = Kinect.DEFAULT_OPARS # Depth conversion parameters
self.depth_nir_shift = Kinect.DEFAULT_DEPTH_NIR_SHIFT # NIR to depth shift params
self.depth_camera.K = Kinect.DEFAULT_NIR_K
self.video_camera.K = Kinect.DEFAULT_RGB_K if self.video_mode == 'rgb' else Kinect.DEFAULT_NIR_K
@classmethod
def from_directory(cls, datadir, video_mode='any'):
# ) Load list of NIR files
nir_file_list = glob.glob(os.path.join(datadir, 'i-*.pgm'))
nir_file_list.sort()
# ) Load list of RGB files
rgb_file_list = glob.glob(os.path.join(datadir, 'r-*.ppm'))
rgb_file_list.sort()
if video_mode == 'any':
video_mode = 'rgb' if len(rgb_file_list) > len(nir_file_list) else 'nir'
video_files = rgb_file_list if video_mode == 'rgb' else nir_file_list
depth_camera = DepthCamera()
# FIXME: KinectNirCamera only handles 10-bit NIR right now
video_camera = Kinect.NirCamera() if video_mode == 'nir' else Camera()
#) Load list of depth files
depth_files = glob.glob(os.path.join(datadir, 'd-*.pgm'))
depth_files.sort()
# Get a consistent set of files
video_files = Kinect.purge_bad_timestamp_files(video_files)
depth_files = Kinect.purge_bad_timestamp_files(depth_files)
if video_mode == 'nir':
(video_files, depth_files, _, _) = Kinect.find_nir_file_with_missing_depth(video_files, depth_files)
depth_timestamps = Kinect.timestamps_from_file_list(depth_files)
video_timestamps = Kinect.timestamps_from_file_list(video_files)
depth_camera.timestamps = depth_timestamps
depth_camera.files = depth_files
video_camera.timestamps = video_timestamps
video_camera.files = video_files
instance = cls(depth_camera, video_camera, video_mode)
return instance
@staticmethod
def timestamp_from_filename(fname):
"Extract timestamp from filename"
ts = int(fname.split('-')[-1].split('.')[0])
return ts
@staticmethod
def timestamps_from_file_list(file_list):
"Take list of Kinect filenames (without path) and extracts timestamps while accounting for timestamp overflow (returns linear timestamps)."
timestamps = np.array([Kinect.timestamp_from_filename(fname) for fname in file_list])
# Handle overflow
diff = np.diff(timestamps)
idxs = np.flatnonzero(diff < 0)
ITEM_SIZE = 2**32
for i in idxs:
timestamps[i+1:] += ITEM_SIZE
return timestamps.flatten()
@staticmethod
def detect_bad_timestamps(ts_list):
EXPECTED_DELTA = 2002155 # Expected time between IR frames
MAX_DIFF = EXPECTED_DELTA / 4
bad_list = []
for frame_num in range(1, len(ts_list)):
diff = ts_list[frame_num] - ts_list[frame_num-1]
if abs(diff - EXPECTED_DELTA) > MAX_DIFF:
bad_list.append(frame_num)
return bad_list
@staticmethod
def purge_bad_timestamp_files(file_list):
"Given a list of image files, find bad frames, remove them and modify file_list"
MAX_INITIAL_BAD_FRAMES = 15
bad_ts = Kinect.detect_bad_timestamps(Kinect.timestamps_from_file_list(file_list))
# Trivial case
if not bad_ts:
return file_list
# No bad frames after the initial allowed
last_bad = max(bad_ts)
if last_bad >= MAX_INITIAL_BAD_FRAMES:
raise Exception('Only 15 initial bad frames are allowed, but last bad frame is %d' % last_bad)
# Remove all frames up to the last bad frame
for i in range(last_bad + 1):
os.remove(file_list[i])
# Purge from the list
file_list = file_list[last_bad+1:]
return file_list # Not strictly needed since Python will overwrite the list
@staticmethod
def depth_file_for_nir_file(video_filename, depth_file_list):
"""Returns the corresponding depth filename given a NIR filename"""
(root, filename) = os.path.split(video_filename)
needle_ts = int(filename.split('-')[2].split('.')[0])
haystack_ts_list = np.array(Kinect.timestamps_from_file_list(depth_file_list))
haystack_idx = np.flatnonzero(haystack_ts_list == needle_ts)[0]
depth_filename = depth_file_list[haystack_idx]
return depth_filename
@staticmethod
def depth_file_for_rgb_file(rgb_filename, rgb_file_list, depth_file_list):
"""Returns the *closest* depth file from an RGB filename"""
(root, filename) = os.path.split(rgb_filename)
rgb_timestamps = np.array(Kinect.timestamps_from_file_list(rgb_file_list))
depth_timestamps = np.array(Kinect.timestamps_from_file_list(depth_file_list))
needle_ts = rgb_timestamps[rgb_file_list.index(rgb_filename)]
haystack_idx = np.argmin(np.abs(depth_timestamps - needle_ts))
depth_filename = depth_file_list[haystack_idx]
return depth_filename
@staticmethod
def find_nir_file_with_missing_depth(video_file_list, depth_file_list):
"Remove all files without its own counterpart. Returns new lists of files"
new_video_list = []
new_depth_list = []
for fname in video_file_list:
try:
depth_file = Kinect.depth_file_for_nir_file(fname, depth_file_list)
new_video_list.append(fname)
new_depth_list.append(depth_file)
except IndexError: # Missing file
pass
# Purge bad files
bad_nir = [f for f in video_file_list if f not in new_video_list]
bad_depth = [f for f in depth_file_list if f not in new_depth_list]
return (new_video_list, new_depth_list, bad_nir, bad_depth)
def disparity_image_to_distance(self, dval_img):
"Convert image of Kinect disparity values to distance (linear method)"
dist_img = dval_img / 2048.0
dist_img = 1 / (self.opars[0]*dist_img + self.opars[1])
return dist_img
def align_depth_to_nir(self, depth_img):
vpad = np.zeros((4,640))
depth_new = np.vstack((vpad, depth_img, vpad))
x, y = np.mgrid[0:np.size(depth_new,1), 0:np.size(depth_new,0)]
xs = x + self.depth_nir_shift[1]
ys = y + self.depth_nir_shift[0]
points = np.dstack((x,y)).reshape([-1,2])
depth_new = scipy.interpolate.griddata(points, depth_new[points[:,1],points[:,0]].flatten(), (xs.T, ys.T), method='nearest')
return depth_new
def depthmap_for_nir(self, nir_filename):
if not self.video_mode == 'nir':
raise Exception("Tried to get depth map from NIR, but capture used RGB")
depth_filename = Kinect.depth_file_for_nir_file(nir_filename, self.video_camera.files, self.depth_camera.files)
depth_img = cv2.imread(depth_filename, cv2.CV_LOAD_IMAGE_UNCHANGED)
depth_img = self.disparity_image_to_distance(depth_img)
depth_img = self.align_depth_to_nir(depth_img)
return depth_img
================================================
FILE: crisp/cli.py
================================================
# -*- coding: utf-8 -*-
"""
Command line interface helpers
"""
from __future__ import absolute_import
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import os
import csv
import logging
logger = logging.getLogger()
from scipy.io import loadmat
from .imu import ArduIMU, IMU
def load_imu_from_file(imu_file):
try:
imu = ArduIMU(imu_file)
logger.debug("Loaded IMU data from ArduIMU logfile %s" % imu_file)
return imu
except (IOError, ValueError):
logger.debug("%s did not load as ArduIMU file" % imu_file)
try:
imu = IMU.from_mat_file(imu_file)
logger.debug("Loaded IMU data from .mat-file %s" % imu_file)
return imu
except IOError:
logger.debug("%s did not load as MAT file" % imu_file)
return None
#--------------------------------------------------------------------------
def load_vars_from_mat(filename, var_dict):
result_dict = {}
M = loadmat(filename)
for var_name, possible_names in list(var_dict.items()):
val = None
for key in possible_names:
logger.debug("Trying %s for variable %s in %s" % (key, var_name, filename))
try:
val = M[key]
break
except KeyError:
pass
if val is None:
raise ValueError("Could not find a candidate for requested variable %s" % var_name)
result_dict[var_name]= val
return result_dict
#--------------------------------------------------------------------------
def load_images_timestamps_from_csv(image_csv):
(root, _) = os.path.split(image_csv)
timestamps = []
files = []
with open(image_csv, 'rb') as f:
reader = csv.reader(f)
for row in reader:
filename, timestamp = row[:2]
_, filename = os.path.split(filename)
timestamp = float(timestamp)
image_path = os.path.join(root, filename)
timestamps.append(timestamp)
files.append(image_path)
return files, timestamps
================================================
FILE: crisp/fastintegrate/fastintegrate.pyx
================================================
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 3 18:10:40 2014
@author: hannes
"""
cimport cython
import numpy as np
cimport numpy as np
cdef extern from "math.h":
double sqrt(double x)
DTYPE = np.float
ctypedef np.double_t DTYPE_t
@cython.boundscheck(False)
def integrate_gyro_quaternion_uniform(np.ndarray[DTYPE_t,ndim=2] gyro_data, np.float dt,
initial=None):
#NB: Quaternion q = [a, n1, n2, n3], scalar first
cdef unsigned int N = gyro_data.shape[0]
cdef np.ndarray[DTYPE_t, ndim=2] q_list = np.empty((N, 4)) # Nx4 quaternion list
# Iterate over all (except first)
cdef unsigned int i, j
cdef DTYPE_t wx, wy, wz
cdef DTYPE_t q0, q1, q2, q3
#cdef np.ndarray[DTYPE_t, ndim=1] qnew = np.zeros((4,))
cdef DTYPE_t qnorm
cdef DTYPE_t dt_half = dt / 2.0
# Initial rotation
if initial is None:
q0 = 1.0
q1 = q2 = q3 = 0.0
else:
q0, q1, q2, q3 = initial
for i in range(N):
wx = gyro_data[i,0]
wy = gyro_data[i,1]
wz = gyro_data[i,2]
q_list[i, 0] = q0 + dt_half * (-wx*q1 -wy*q2 -wz*q3)
q_list[i, 1] = q1 + dt_half * (q0*wx + q2*wz - wy*q3)
q_list[i, 2] = q2 + dt_half * (wy*q0 -wz*q1 + wx*q3)
q_list[i, 3] = q3 + dt_half * (wz*q0 + wy*q1 -wx*q2)
# Normalize
qnorm = sqrt(q_list[i, 0]**2 + q_list[i, 1]**2 + q_list[i, 2]**2 + q_list[i, 3]**2)
for j in range(4):
q_list[i, j] /= qnorm
# New prev values
q0 = q_list[i, 0]
q1 = q_list[i, 1]
q2 = q_list[i, 2]
q3 = q_list[i, 3]
return q_list
================================================
FILE: crisp/imu.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
IMU module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
#--------------------------------------------------------------------------
# Includes
#--------------------------------------------------------------------------
import numpy as np
import re
import scipy.io
from . import rotations
from . import fastintegrate
from . import l3g4200d
#--------------------------------------------------------------------------
# Classes
#--------------------------------------------------------------------------
class IMU(object):
"""
Defines an IMU (currently only gyro)
"""
def __init__(self):
self.integrated = []
self.gyro_data = []
self.timestamps = []
@classmethod
def from_mat_file(cls, matfilename):
"""Load gyro data from .mat file
The MAT file should contain the following two arrays
gyro : (3, N) float ndarray
The angular velocity measurements.
timestamps : (N, ) float ndarray
Timestamps of the measurements.
Parameters
---------------
matfilename : string
Name of the .mat file
Returns
----------------
A new IMU class instance
"""
M = scipy.io.loadmat(matfilename)
instance = cls()
instance.gyro_data = M['gyro']
instance.timestamps = M['timestamps']
return instance
@property
def rate(self):
"""Get the sample rate in Hz.
Returns
---------
rate : float
The sample rate, in Hz, calculated from the timestamps
"""
N = len(self.timestamps)
t = self.timestamps[-1] - self.timestamps[0]
rate = 1.0 * N / t
return rate
def zero_level_calibrate(self, duration, t0=0.0):
"""Performs zero-level calibration from the chosen time interval.
This changes the previously lodaded data in-place.
Parameters
--------------------
duration : float
Number of timeunits to use for calibration
t0 : float
Starting time for calibration
Returns
----------------------
gyro_data : (3, N) float ndarray
The calibrated data (note that it is also changed in-place!)
"""
t1 = t0 + duration
indices = np.flatnonzero((self.timestamps >= t0) & (self.timestamps <= t1))
m = np.mean(self.gyro_data[:, indices], axis=1)
self.gyro_data -= m.reshape(3,1)
return self.gyro_data
def gyro_data_corrected(self, pose_correction=np.eye(3)):
"""Get relative pose corrected data.
Parameters
-------------
pose_correction : (3,3) ndarray, optional
Rotation matrix that describes the relative pose between the IMU and something else (e.g. camera).
Returns
---------------
gyro_data : (3, N) ndarray
The relative pose corrected data.
"""
return pose_correction.dot(self.gyro_data)
def integrate(self, pose_correction=np.eye(3), uniform=True):
"""Integrate angular velocity measurements to rotations.
Parameters
-------------
pose_correction : (3,3) ndarray, optional
Rotation matrix that describes the relative pose between the IMU and something else (e.g. camera).
uniform : bool
If True (default), assume uniform sample rate. This will use a faster integration method.
Returns
-------------
rotations : (4, N) ndarray
Rotations as unit quaternions with scalar as first element.
"""
if uniform:
dt = float(self.timestamps[1]-self.timestamps[0]) # Must be python float for fastintegrate to work
return fastintegrate.integrate_gyro_quaternion_uniform(self.gyro_data_corrected, dt)
else:
N = len(self.timestamps)
integrated = np.zeros((4, N))
integrated[:,0] = np.array([1, 0, 0, 0]) # Initial rotation (no rotation)
# Iterate over all
for i in range(1, len(self.timestamps)):
w = pose_correction.dot(self.gyro_data[:, i]) # Change to correct coordinate frame
dt = float(self.timestamps[i] - self.timestamps[i - 1])
qprev = integrated[:, i - 1].flatten()
A = np.array([[0, -w[0], -w[1], -w[2]],
[w[0], 0, w[2], -w[1]],
[w[1], -w[2], 0, w[0]],
[w[2], w[1], -w[0], 0]])
qnew = (np.eye(4) + (dt/2.0) * A).dot(qprev)
qnorm = np.sqrt(np.sum(qnew ** 2))
qnew = qnew / qnorm if qnorm > 0 else 0
integrated[:, i] = qnew
#print "%d, %s, %s, %s, %s" % (i, w, dt, qprev, qnew)
return integrated
@staticmethod
def rotation_at_time(t, timestamps, rotation_sequence):
"""Get the gyro rotation at time t using SLERP.
Parameters
-----------
t : float
The query timestamp.
timestamps : array_like float
List of all timestamps
rotation_sequence : (4, N) ndarray
Rotation sequence as unit quaternions with scalar part as first element.
Returns
-----------
q : (4,) ndarray
Unit quaternion representing the rotation at time t.
"""
idx = np.flatnonzero(timestamps >= (t - 0.0001))[0]
t0 = timestamps[idx - 1]
t1 = timestamps[idx]
tau = (t - t0) / (t1 - t0)
q1 = rotation_sequence[:, idx - 1]
q2 = rotation_sequence[:, idx]
q = rotations.slerp(q1, q2, tau)
return q
class ArduIMU(IMU):
def __init__(self, filename):
super(ArduIMU, self).__init__()
self.filename = filename
ts, acc, gyro = self.__load(filename)
self.timestamps = ts
self.gyro_data = gyro
def __load(self, gyro_data_filename):
f = open(gyro_data_filename, 'r')
# Read header
if not f.readline().strip().startswith('RPY'):
raise ValueError("This is not a ArduIMU log file")
data = np.array([[]], dtype='float32')
data.shape = 0,7
for line in f.readlines():
row = [int(s) for s in re.findall('\d+', line)]
if len(row) == 8:
if row[7] != 1:
pass # "Checksum error, skipping"
else:
data = np.append(data, [row[0:7]], axis=0)
timestamps = data[:,0]
timestamps -= timestamps[0] # Start at 0
timestamps /= 1000.0 # Milliseconds -> seconds
# Map from 10-bit value to voltage
# The arduino has mapped the range [0,Vref] to [0,1023] before removing the offset
Vref = 3.3
data[:,1:] *= (Vref / 1023.0);
accelerometer = data[:,1:4]
gyroscope = data[:,4:7]
# Scale gyro output
gyro_scale = 3.33 / 1000 # (V/(degrees/s)) From datasheet (not ratiometric)
gyroscope = np.deg2rad(gyroscope / gyro_scale) # rad / s
# Scale accelerometer
gravity = 9.81
gravity_scale = 0.330 # V/g
accelerometer /= gravity_scale; # Scale to acceleration in g's
accelerometer *= gravity # Scale to acceleration in m/s2
return (timestamps, accelerometer.T, gyroscope.T)
class L3G4200DGyro(IMU):
def __init__(self, filename, post_process=True):
super(L3G4200DGyro, self).__init__()
self.filename = filename
ts, gyro = self.__load(filename, post_process)
self.timestamps = ts
self.gyro_data = gyro
def __load(self, filename, post_process=True):
data, ts, T = l3g4200d.load_L3G_arduino(filename)
# Our L3G4200D rig has some issues
if post_process:
print("Post processing L3G4200D data")
data = l3g4200d.post_process_L3G4200D_data(data)
assert data.shape[0] == 3, "Expected gyro to have 3 elements in first dim, got {0:d}".format(data.shape[0])
return ts, data
================================================
FILE: crisp/l3g4200d.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Created on Wed Mar 5 10:24:38 2014
@author: hannes
"""
import numpy as np
import struct
class ParserException(Exception):
pass
class GyroParserBase(object):
def __init__(self):
self.fs = None # Full scale resolution. Max rate. (dps)
self.data_scale = None # mdps per digit
self.data = None
def parse(self, data):
raise NotImplemented()
class L3GArduinoParser(GyroParserBase):
COMMAND_START = 0xAB
COMMAND_DATA = 0xC8
COMMAND_SAMPLE_RATE = 0xDB
COMMAND_TIME_SYNC = 0xBB
REG_CTRL1 = 0x20
REG_CTRL4 = 0x23
FS_RATE_FACTOR = { 250 : 8.75e-3, # From L3G datasheet
500 : 17.50e-3,
2000 : 70e-3 }
def __init__(self):
GyroParserBase.__init__(self)
self.reg = {} # Register -> value map
self.ndata = 0
self.actual_data_rate = None
self.fs = None
self.data_scale = None
self.sync_times = []
def parse(self, input_data):
total_bytes = len(input_data)
self.data = np.empty((3, total_bytes // 6)) # Will be lower than this
temp_data = input_data
num_bytes = 0
while num_bytes < total_bytes - 1:
command = ord(temp_data[num_bytes])
length = ord(temp_data[num_bytes + 1])
data = temp_data[num_bytes+2:num_bytes+2+length]
#data_str = " ".join(["%x" % ord(x) for x in data])
#print "Command: %x, Length: %x, Data: %s" % (command, length, data_str)
self.__handle(command, data)
num_bytes += length + 2
try:
self.data *= self.data_scale
except RuntimeWarning:
print("Scale warning! res=", self.data, "*", self.data_scale)
self.data = self.data[:,0:self.ndata]
def __handle(self, command, data):
if command == L3GArduinoParser.COMMAND_DATA:
if not self.data_scale:
raise ParserException("No data scale loaded before first data packet")
raw_str = data #b''.join([chr(x) for x in data])
sfmt = "<hhh"
n = len(raw_str) // 6
for i in range(n):
data_str = raw_str[6*i:6*i+6]
x, y, z = struct.unpack(sfmt, data_str)
arr= np.array([x, y, z])#.reshape(3, 1)
self.data[:, self.ndata] = arr
self.ndata += 1
elif command == L3GArduinoParser.COMMAND_START:
for reg, val in zip(data[::2], data[1::2]):
reg = ord(reg)
val = ord(val)
self.reg[reg] = val
#print "Checking reg %x with val %x" % (reg, val)
if reg == L3GArduinoParser.REG_CTRL4:
fsbits = (val & 0x30) >> 4
fs = {0 : 250, 1 : 500, 2 : 2000, 3 : 2000}[fsbits]
self.fs = fs
self.data_scale = L3GArduinoParser.FS_RATE_FACTOR[self.fs]
if reg == L3GArduinoParser.REG_CTRL1:
drbits = (val & 0xC0) >> 6
self.data_rate = {0 : 100, 1 : 200, 2 : 400 , 3 : 800}[drbits]
elif command == L3GArduinoParser.COMMAND_SAMPLE_RATE:
sfmt = '<L'
#print "Sample rate byte (%d) %s" % (len(data), data.__repr__())
#T = struct.unpack(sfmt, data)
#print "Got sample rate", T
print("Got sample rate byte, but until implementation is changed in Arduino code, the value should not be used as it is unstable as hell.")
# Note: Reimplement the Arduino code to emit timestamps on a regular basis that can be used to fix the time rate.
self.actual_data_rate = None#1000000. / T[0] # Hz
elif command == L3GArduinoParser.COMMAND_TIME_SYNC:
sfmt = '<L'
ts = struct.unpack(sfmt, data)[0] / 1000.0 # msec -> seconds
self.sync_times.append((self.ndata, ts))
def load_L3G_arduino(filename, remove_begin_spurious=False, return_parser=False):
"Load gyro data collected by the arduino version of the L3G logging platform, and return the data (in rad/s), a time vector, and the sample rate (seconds)"
file_data = open(filename, 'rb').read()
parser = L3GArduinoParser()
parser.parse(file_data[7:]) # Skip first "GYROLOG" header in file
data = parser.data
if parser.actual_data_rate:
T = 1. / parser.actual_data_rate
print("Found measured data rate %.3f ms (%.3f Hz)" % (1000*T, 1. / T))
else:
T = 1. / parser.data_rate
print("Using data rate provided by gyro (probably off by a few percent!) %.3f ms (%.3f Hz)" % (1000*T, 1. / T))
N = parser.data.shape[1]
t = np.linspace(0, T*N, num=data.shape[1])
print(t.shape, data.shape)
print("Loaded %d samples (%.2f seconds) with expected sample rate %.3f ms (%.3f Hz)" % (N, t[-1], T*1000.0, 1./T))
try:
print("Actual sample rate is %.3f ms (%.3f Hz)" % (1000. / parser.actual_data_rate, parser.actual_data_rate, ))
except TypeError:
pass
if remove_begin_spurious:
to_remove = int(0.3/T) # Remove first three tenth of second
data[:,:to_remove] = 0.0
if return_parser:
return np.deg2rad(data), t, T, parser
else:
return np.deg2rad(data), t, T
def post_process_L3G4200D_data(data, do_plot=False):
def notch(Wn, bandwidth):
f = Wn/2.0
R = 1.0 - 3.0*(bandwidth/2.0)
K = ((1.0 - 2.0*R*np.cos(2*np.pi*f) + R**2)/(2.0 -
2.0*np.cos(2*np.pi*f)))
b,a = np.zeros(3),np.zeros(3)
a[0] = 1.0
a[1] = - 2.0*R*np.cos(2*np.pi*f)
a[2] = R**2
b[0] = K
b[1] = -2*K*np.cos(2*np.pi*f)
b[2] = K
return b,a
# Remove strange high frequency noise and bias
b,a = notch(0.8, 0.03)
data_filtered = np.empty_like(data)
from scipy.signal import filtfilt
for i in range(3):
data_filtered[i] = filtfilt(b, a, data[i])
if do_plot:
from matplotlib.pyplot import subplot, plot, specgram, title
# Plot the difference
ax = None
for i in range(3):
if ax is None:
ax = subplot(5,1,i+1)
else:
subplot(5,1,i+1, sharex=ax, sharey=ax)
plot(data[i])
plot(data_filtered[i])
title(['x','y','z'][i])
subplot(5,1,4)
specgram(data[0])
title("Specgram of biased X")
subplot(5,1,5)
specgram(data_filtered[0])
title("Specgram of filtered unbiased X")
return data_filtered
================================================
FILE: crisp/pose.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Relative pose calibration module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import logging
logger = logging.getLogger()
import numpy as np
import matplotlib.pyplot as plt
import cv2
from . import timesync
from . import tracking
from . import rotations
def estimate_pose(image_sequences, imu_sequences, K):
"""Estimate sync between IMU and camera based on gyro readings and optical flow.
The user should first create at least two sequences of corresponding image and
gyroscope data.
From each sequence we calculate the rotation axis (one from images, one from IMU/gyro).
The final set of len(image_sequences) corresponding rotation axes are then used to calculate
the relative pose between the IMU and camera.
The returned rotation is such that it transfers vectors in the gyroscope coordinate
frame to the camera coordinate frame:
X_camera = R * X_gyro
Parameters
------------
image_sequences : list of list of ndarrays
List of image sequences (list of ndarrays) to use. Must have at least two sequences.
imu_sequences : list of (3, N) ndarray
Sequence of gyroscope measurements (angular velocities).
K : (3,3) ndarray
Camera calibration matrix
Returns
-----------
R : (3,3) ndarray
The relative pose (gyro-to-camera) such that X_camera = R * X_gyro
"""
assert len(image_sequences) == len(imu_sequences)
assert len(image_sequences) >= 2
# Note: list(image_sequence) here makes sure any generator type input is expanded to an actual list
sync_correspondences = [_get_point_correspondences(list(image_sequence)) for image_sequence in image_sequences]
# ) Procrustes on corresponding pairs
PROCRUSTES_MAX_POINTS = 15 # Number of tracks/points to use for procrustes
logger.debug("Running procrustes on track-retrack results")
image_rotation_axes = []
for i, points in enumerate(sync_correspondences):
if points.size < 1:
logger.error('Shape of points are %s', str(points.shape))
raise Exception("Did not get enough points when tracking")
num_points_to_use = min(PROCRUSTES_MAX_POINTS, points.shape[0])
logger.debug("Using %d tracks to calculate procrustes", num_points_to_use)
idxs_to_use = np.random.permutation(points.shape[0])[:num_points_to_use]
assert points.shape[-1] == 2
x = points[idxs_to_use,0,:].T.reshape(2,-1)
y = points[idxs_to_use,-1,:].T.reshape(2,-1)
x = np.vstack((x, np.ones((1, x.shape[1]))))
y = np.vstack((y, np.ones((1, y.shape[1]))))
K_inv = np.linalg.inv(K)
X = K_inv.dot(x)
Y = K_inv.dot(y)
# Depth must be positive
(R, t) = rotations.procrustes(X, Y, remove_mean=False) # X = R * Y + t
(v, theta) = rotations.rotation_matrix_to_axis_angle(R)
image_rotation_axes.append(v) # Save rotation axis
# Check the quality via the mean reprojection error
mean_error = np.mean(np.sqrt(np.sum((X - R.dot(Y))**2, axis=0)))
MEAN_ERROR_LIMIT = 0.1 # Arbitrarily chosen limit (in meters)
logger.debug('Image sequence %d: Rotation axis %s, degrees %.2f, mean error %.3f',
i, v, np.rad2deg(theta), mean_error)
if mean_error > MEAN_ERROR_LIMIT:
logger.warning("Procrustes solution mean error %.3f > %.3f", mean_error, MEAN_ERROR_LIMIT)
# ) Gyro principal rotation axis
gyro_rotation_axes = []
for i, gyro_seq in enumerate(imu_sequences):
assert gyro_seq.shape[0] == 3
v = principal_rotation_axis(gyro_seq)
logger.debug('Gyro sequence %d: Rotation axis %s', i, v)
gyro_rotation_axes.append(v)
# ) Procrustes to get rotation between coordinate frames
X = np.vstack(image_rotation_axes).T
Y = np.vstack(gyro_rotation_axes).T
(R,t) = rotations.procrustes(X, Y, remove_mean=False)
return (R, t)
#--------------------------------------------------------------------------
def pick_manual(image_sequence, imu_gyro, num_sequences=2):
"""Select N matching sequences and return data indices.
Parameters
---------------
image_sequence : list_like
A list, or generator, of image data
imu_gyro : (3, N) ndarray
Gyroscope data (angular velocities)
num_sequences : int
The number of matching sequences to pick
Returns
----------------
sync_sequences : list
List of (frame_pair, gyro_pair) tuples where each pair contains
(a, b) which are indices of the (inclusive) range [a, b] that was chosen
"""
assert num_sequences >= 2
# Create optical flow for user to select parts in
logger.info("Calculating optical flow")
flow = tracking.optical_flow_magnitude(image_sequence)
# ) Prompt user for sync slices
logger.debug("Prompting user for %d sequences" % num_sequences)
imu_fake_timestamps = np.linspace(0,1,num=imu_gyro.shape[1])
sync_sequences = [timesync.manual_sync_pick(flow, imu_fake_timestamps, imu_gyro) for i in range(num_sequences)]
return sync_sequences
#--------------------------------------------------------------------------
def principal_rotation_axis(gyro_data):
"""Get the principal rotation axis of angular velocity measurements.
Parameters
-------------
gyro_data : (3, N) ndarray
Angular velocity measurements
Returns
-------------
v : (3,1) ndarray
The principal rotation axis for the chosen sequence
"""
N = np.zeros((3,3))
for x in gyro_data.T: # Transpose because samples are stored as columns
y = x.reshape(3,1)
N += y.dot(y.T)
(eig_val, eig_vec) = np.linalg.eig(N)
i = np.argmax(eig_val)
v = eig_vec[:,i]
# Make sure v has correct sign
s = 0
for x in gyro_data.T: # Transpose because samples are stored as columns
s += v.T.dot(x.reshape(3,1))
v *= np.sign(s)
return v
#--------------------------------------------------------------------------
def _get_point_correspondences(image_list, max_corners=200, min_distance=5, quality_level=0.07):
max_retrack_distance = 0.5
initial_points = cv2.goodFeaturesToTrack(image_list[0], max_corners, quality_level, min_distance)
(tracks, status) = tracking.track_retrack(image_list, initial_points=initial_points, max_retrack_distance=max_retrack_distance) # Status is ignored
return tracks[:,(0,-1),:] # First and last frame only
================================================
FILE: crisp/ransac.py
================================================
from __future__ import division, print_function, absolute_import
import numpy as np
def RANSAC(model_func, eval_func, data, num_points, num_iter, threshold, recalculate=False):
"""Apply RANSAC.
This RANSAC implementation will choose the best model based on the number of points in the consensus set. At evaluation time the model is created using num_points points. Then it will be recalculated using the points in the consensus set.
Parameters
------------
model_func: Takes a data parameter of size DxK where K is the number of points needed to construct the model and returns the model (Mx1 vector)
eval_func: Takes a model parameter (Lx1) and one or more data points (DxC, C>=1) and calculates the score of the point(s) relative to the selected model
data : array (DxN) where D is dimensionality and N number of samples
"""
M = None
max_consensus = 0
all_idx = list(range(data.shape[1]))
final_consensus = []
for k in range(num_iter):
np.random.shuffle(all_idx)
model_set = all_idx[:num_points]
x = data[:, model_set]
m = model_func(x)
model_error = eval_func(m, data)
assert model_error.ndim == 1
assert model_error.size == data.shape[1]
consensus_idx = np.flatnonzero(model_error < threshold)
if len(consensus_idx) > max_consensus:
M = m
max_consensus = len(consensus_idx)
final_consensus = consensus_idx
# Recalculate using current consensus set?
if recalculate and len(final_consensus) > 0:
final_consensus_set = data[:, final_consensus]
M = model_func(final_consensus_set)
return (M, final_consensus)
================================================
FILE: crisp/remove_slp.py
================================================
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Kinect NIR Structured Light Pattern removal
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
# Adapted from MATLAB code written by Per-Erik Forssén (perfo@isy.liu.se)
#--------------------------------------------------------------------------
# Includes
#--------------------------------------------------------------------------
import cv2
import numpy as np
#--------------------------------------------------------------------------
# Default parameters
#--------------------------------------------------------------------------
GSTD1 = 0.575
GSTD2 = 2.3
GSTD3 = 3.4
W = 9.0
KSIZE = 19 # MATLAB equivalent of -9:9
EPS = 2.2204E-16
#--------------------------------------------------------------------------
# Public functions
#--------------------------------------------------------------------------
def remove_slp(img, gstd1=GSTD1, gstd2=GSTD2, gstd3=GSTD3, ksize=KSIZE, w=W):
"""Remove the SLP from kinect IR image
The input image should be a float32 numpy array, and should NOT be a square root image
Parameters
------------------
img : (M, N) float ndarray
Kinect NIR image with SLP pattern
gstd1 : float
Standard deviation of gaussian kernel 1
gstd2 : float
Standard deviation of gaussian kernel 2
gstd3 : float
Standard deviation of gaussian kernel 3
ksize : int
Size of kernel (should be odd)
w : float
Weighting factor
Returns
------------------
img_noslp : (M,N) float ndarray
Input image with SLP removed
"""
gf1 = cv2.getGaussianKernel(ksize, gstd1)
gf2 = cv2.getGaussianKernel(ksize, gstd2)
gf3 = cv2.getGaussianKernel(ksize, gstd3)
sqrtimg = cv2.sqrt(img)
p1 = cv2.sepFilter2D(sqrtimg, -1, gf1, gf1)
p2 = cv2.sepFilter2D(sqrtimg, -1, gf2, gf2)
maxarr = np.maximum(0, (p1 - p2) / p2)
minarr = np.minimum(w * maxarr, 1)
p = 1 - minarr
nc = cv2.sepFilter2D(p, -1, gf3, gf3) + EPS
output = cv2.sepFilter2D(p*sqrtimg, -1, gf3, gf3)
output = (output / nc) ** 2 # Since input is sqrted
return output
================================================
FILE: crisp/rotations.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Rotation handling module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import numpy as np
from numpy.testing import assert_almost_equal
from . import ransac
#------------------------------------------------------------------------------
def procrustes(X, Y, remove_mean=False):
"""Orthogonal procrustes problem solver
The procrustes problem finds the best rotation R, and translation t
where
X = R*Y + t
The number of points in X and Y must be at least 2.
For the minimal case of two points, a third point is temporarily created
and used for the estimation.
Parameters
-----------------
X : (3, N) ndarray
First set of points
Y : (3, N) ndarray
Second set of points
remove_mean : bool
If true, the mean is removed from X and Y before solving the
procrustes problem. Can yield better results in some applications.
Returns
-----------------
R : (3,3) ndarray
Rotation component
t : (3,) ndarray
Translation component (None if remove_mean is False)
"""
assert X.shape == Y.shape
assert X.shape[0] > 1
# Minimal case, create third point using cross product
if X.shape[0] == 2:
X3 = np.cross(X[:,0], X[:,1], axis=0)
X = np.hstack((X, X3 / np.linalg.norm(X3)))
Y3 = np.cross(Y[:,0], Y[:,1], axis=0)
Y = np.hstack((Y, Y3 / np.linalg.norm(Y3)))
D, N = X.shape[:2]
if remove_mean:
mx = np.mean(X, axis=1).reshape(D, 1)
my = np.mean(Y, axis=1).reshape(D, 1)
Xhat = X - mx
Yhat = Y - my
else:
Xhat = X
Yhat = Y
(U, S, V) = np.linalg.svd((Xhat).dot(Yhat.T))
Dtmp = np.eye(Xhat.shape[0])
Dtmp[-1,-1] = np.linalg.det(U.dot(V))
R_est = U.dot(Dtmp).dot(V)
# Now X=R_est*(Y-my)+mx=R_est*Y+t_est
if remove_mean:
t_est= mx - R_est.dot(my)
else:
t_est = None
return (R_est, t_est)
#--------------------------------------------------------------------------
def rotation_matrix_to_axis_angle(R):
"""Convert a 3D rotation matrix to a 3D axis angle representation
Parameters
---------------
R : (3,3) array
Rotation matrix
Returns
----------------
v : (3,) array
(Unit-) rotation angle
theta : float
Angle of rotations, in radians
Note
--------------
This uses the algorithm as described in Multiple View Geometry, p. 584
"""
assert R.shape == (3,3)
assert_almost_equal(np.linalg.det(R), 1.0, err_msg="Not a rotation matrix: determinant was not 1")
S, V = np.linalg.eig(R)
k = np.argmin(np.abs(S - 1.))
s = S[k]
assert_almost_equal(s, 1.0, err_msg="Not a rotation matrix: No eigen value s=1")
v = np.real(V[:, k]) # Result is generally complex
vhat = np.array([R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1]])
sintheta = 0.5 * np.dot(v, vhat)
costheta = 0.5 * (np.trace(R) - 1)
theta = np.arctan2(sintheta, costheta)
return (v, theta)
#--------------------------------------------------------------------------
def axis_angle_to_rotation_matrix(v, theta):
"""Convert rotation from axis-angle to rotation matrix
Parameters
---------------
v : (3,) ndarray
Rotation axis (normalized)
theta : float
Rotation angle (radians)
Returns
----------------
R : (3,3) ndarray
Rotation matrix
"""
if np.abs(theta) < np.spacing(1):
return np.eye(3)
else:
v = v.reshape(3,1)
np.testing.assert_almost_equal(np.linalg.norm(v), 1.)
vx = np.array([[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]])
vvt = np.dot(v, v.T)
R = np.eye(3)*np.cos(theta) + (1 - np.cos(theta))*vvt + vx * np.sin(theta)
return R
#--------------------------------------------------------------------------
def quat_to_rotation_matrix(q):
"""Convert unit quaternion to rotation matrix
Parameters
-------------
q : (4,) ndarray
Unit quaternion, scalar as first element
Returns
----------------
R : (3,3) ndarray
Rotation matrix
"""
q = q.flatten()
assert q.size == 4
assert_almost_equal(np.linalg.norm(q), 1.0, err_msg="Not a unit quaternion!")
qq = q ** 2
R = np.array([[qq[0] + qq[1] - qq[2] - qq[3], 2*q[1]*q[2] -
2*q[0]*q[3], 2*q[1]*q[3] + 2*q[0]*q[2]],
[2*q[1]*q[2] + 2*q[0]*q[3], qq[0] - qq[1] + qq[2] -
qq[3], 2*q[2]*q[3] - 2*q[0]*q[1]],
[2*q[1]*q[3] - 2*q[0]*q[2], 2*q[2]*q[3] + 2*q[0]*q[1],
qq[0] - qq[1] - qq[2] + qq[3]]])
return R
#--------------------------------------------------------------------------
def integrate_gyro_quaternion(gyro_ts, gyro_data):
"""Integrate angular velocities to rotations
Parameters
---------------
gyro_ts : ndarray
Timestamps
gyro_data : (3, N) ndarray
Angular velocity measurements
Returns
---------------
rotations : (4, N) ndarray
Rotation sequence as unit quaternions (first element scalar)
"""
#NB: Quaternion q = [a, n1, n2, n3], scalar first
q_list = np.zeros((gyro_ts.shape[0], 4)) # Nx4 quaternion list
q_list[0,:] = np.array([1, 0, 0, 0]) # Initial rotation (no rotation)
# Iterate over all (except first)
for i in range(1, gyro_ts.size):
w = gyro_data[i]
dt = gyro_ts[i] - gyro_ts[i - 1]
qprev = q_list[i - 1]
A = np.array([[0, -w[0], -w[1], -w[2]],
[w[0], 0, w[2], -w[1]],
[w[1], -w[2], 0, w[0]],
[w[2], w[1], -w[0], 0]])
qnew = (np.eye(4) + (dt/2.0) * A).dot(qprev)
qnorm = np.sqrt(np.sum(qnew ** 2))
qnew /= qnorm
q_list[i] = qnew
return q_list
#--------------------------------------------------------------------------
def slerp(q1, q2, u):
"""SLERP: Spherical linear interpolation between two unit quaternions.
Parameters
------------
q1 : (4, ) ndarray
Unit quaternion (first element scalar)
q2 : (4, ) ndarray
Unit quaternion (first element scalar)
u : float
Interpolation factor in range [0,1] where 0 is first quaternion
and 1 is second quaternion.
Returns
-----------
q : (4,) ndarray
The interpolated unit quaternion
"""
q1 = q1.flatten()
q2 = q2.flatten()
assert q1.shape == q2.shape
assert q1.size == 4
costheta = np.dot(q1, q2)
if np.isclose(u, 0.):
return q1
elif np.isclose(u, 1.):
return q2
elif u > 1 or u < 0:
raise ValueError("u must be in range [0, 1]")
# Shortest path
if costheta < 0:
costheta = -costheta
q2 = -q2
# Almost the same, we can return any of them?
if np.isclose(costheta, 1.0):
return q1
theta = np.arccos(costheta)
f1 = np.sin((1.0 - u)*theta) / np.sin(theta)
f2 = np.sin(u*theta) / np.sin(theta)
q = f1*q1 + f2*q2
q = q / np.sqrt(np.sum(q**2)) # Normalize
return q
#--------------------------------------------------------------------------
def estimate_rotation_procrustes_ransac(x, y, camera, threshold, inlier_ratio=0.75, do_translation=False):
"""Calculate rotation between two sets of image coordinates using ransac.
Inlier criteria is the reprojection error of y into image 1.
Parameters
-------------------------
x : array 2xN image coordinates in image 1
y : array 2xN image coordinates in image 2
camera : Camera model
threshold : float pixel distance threshold to accept as inlier
do_translation : bool Try to estimate the translation as well
Returns
------------------------
R : array 3x3 The rotation that best fulfills X = RY
t : array 3x1 translation if do_translation is False
residual : array pixel distances ||x - xhat|| where xhat ~ KRY (and lens distorsion)
inliers : array Indices of the points (in X and Y) that are RANSAC inliers
"""
assert x.shape == y.shape
assert x.shape[0] == 2
X = camera.unproject(x)
Y = camera.unproject(y)
data = np.vstack((X, Y, x))
assert data.shape[0] == 8
model_func = lambda data: procrustes(data[:3], data[3:6], remove_mean=do_translation)
def eval_func(model, data):
Y = data[3:6].reshape(3,-1)
x = data[6:].reshape(2,-1)
R, t = model
Xhat = np.dot(R, Y) if t is None else np.dot(R, Y) + t
xhat = camera.project(Xhat)
dist = np.sqrt(np.sum((x-xhat)**2, axis=0))
return dist
inlier_selection_prob = 0.99999
model_points = 2
ransac_iterations = int(np.log(1 - inlier_selection_prob) / np.log(1-inlier_ratio**model_points))
model_est, ransac_consensus_idx = ransac.RANSAC(model_func, eval_func, data, model_points, ransac_iterations, threshold, recalculate=True)
if model_est is not None:
(R, t) = model_est
dist = eval_func((R, t), data)
else:
dist = None
R, t = None, None
ransac_consensus_idx = []
return R, t, dist, ransac_consensus_idx
================================================
FILE: crisp/stream.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Input streams module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2015, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import os
import collections
import logging
logger = logging.getLogger('crisp')
import cv2
import numpy as np
from . import fastintegrate, tracking, rotations
# Handle OpenCV 2.4.x -> 3.0
try:
CV_CAP_PROP_POS_MSEC = cv2.cv.CV_CAP_PROP_POS_MSEC
except AttributeError:
CV_CAP_PROP_POS_MSEC = cv2.CAP_PROP_POS_MSEC
class GyroStream(object):
def __init__(self):
self.__last_dt = None
self.__last_q = None
self.data = None # Arranged as Nx3 where N is number of samples
@classmethod
def from_csv(cls, filename):
"""Create gyro stream from CSV data
Load data from a CSV file.
The data must be formatted with three values per line: (x, y, z)
where x, y, z is the measured angular velocity (in radians) of the specified axis.
Parameters
-------------------
filename : str
Path to the CSV file
Returns
---------------------
GyroStream
A gyroscope stream
"""
instance = cls()
instance.data = np.loadtxt(filename, delimiter=',')
return instance
@classmethod
def from_data(cls, data):
"""Create gyroscope stream from data array
Parameters
-------------------
data : (N, 3) ndarray
Data array of angular velocities (rad/s)
Returns
-------------------
GyroStream
Stream object
"""
if not data.shape[1] == 3:
raise ValueError("Gyroscope data must have shape (N, 3)")
instance = cls()
instance.data = data
return instance
@property
def num_samples(self):
return self.data.shape[0]
def integrate(self, dt):
"""Integrate gyro measurements to orientation using a uniform sample rate.
Parameters
-------------------
dt : float
Sample distance in seconds
Returns
----------------
orientation : (4, N) ndarray
Gyroscope orientation in quaternion form (s, q1, q2, q3)
"""
if not dt == self.__last_dt:
self.__last_q = fastintegrate.integrate_gyro_quaternion_uniform(self.data, dt)
self.__last_dt = dt
return self.__last_q
class VideoStream(object):
"""Video stream representation
This is the base class for all video streams, and should normally not be used directly.
Instead you should use a VideoStream subclass that can work on the data you have.
The concept of a video stream can be summarized as something that
"provides frames of video data captured by a single camera".
A VideoStream object is iterable to allow reading frames easily::
stream = VideoStreamSubClass(SOME_PARAMETER)
for frame in stream:
do_stuff(frame)
"""
def __init__(self, camera_model, flow_mode='optical'):
"""Create a VideoStream object
Parameters
----------------
camera_model : CameraModel
Camera model used by this stream
"""
self._flow = None
self.flow_mode = flow_mode
self.camera_model = camera_model
def __iter__(self):
return self._frames()
def _frames(self):
raise NotImplementedError("{} does not implement the _frames() method used to extract frames".format(self.__class__.__name__))
@classmethod
def from_file(cls, camera_model, filename):
"""Create stream automatically from filename.
Note
--------------------
This currently only works with video files that are readable by OpenCV
Parameters
--------------------
camera_model : CameraModel
Camera model to use with this stream
filename : str
The filename to load the stream data from
Returns
--------------------
VideoStream
Video stream of a suitable sub class
"""
# TODO: Other subclasses
return OpenCvVideoStream(camera_model, filename)
def project(self, points):
"""Project 3D points to image coordinates.
This projects 3D points expressed in the camera coordinate system to image points.
Parameters
--------------------
points : (3, N) ndarray
3D points
Returns
--------------------
image_points : (2, N) ndarray
The world points projected to the image plane of the camera used by the stream
"""
return self.camera_model.project(points)
def unproject(self, image_points):
"""Find (up to scale) 3D coordinate of an image point
This is the inverse of the `project` function.
The resulting 3D points are only valid up to an unknown scale.
Parameters
----------------------
image_points : (2, N) ndarray
Image points
Returns
----------------------
points : (3, N) ndarray
3D coordinates (valid up to scale)
"""
return self.camera_model.unproject(image_points)
@property
def frame_rate(self):
return self.camera_model.frame_rate
@property
def flow(self):
if self._flow is None:
logger.debug("Generating flow. This can take minutes depending on video length")
if self.flow_mode == 'rotation':
self._generate_frame_to_frame_rotation()
elif self.flow_mode == 'optical':
self._flow = tracking.optical_flow_magnitude(self)
else:
raise ValueError("No such flow mode '{}'".format(self.flow_mode))
#self.__generate_flow()
return self._flow
def _generate_frame_to_frame_rotation(self):
rotation = []
weights = []
step = 1
maxlen = step + 1
frame_queue = collections.deque([], maxlen)
for frame in self:
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
frame_queue.append(frame)
if len(frame_queue) == maxlen:
max_corners = 500
quality_level = 0.07 # Why??
min_distance = 10
initial_pts = cv2.goodFeaturesToTrack(frame_queue[0], max_corners, quality_level, min_distance)
pts, status = tracking.track_retrack(list(frame_queue), initial_pts)
X = pts[:, 0, :].T
Y = pts[:, 1, :].T
threshold = 2.0
R, _, err, inliers = rotations.estimate_rotation_procrustes_ransac(X, Y, self.camera_model, threshold)
if R is None:
weight = 0
angle = 0
r = np.zeros(3)
else:
weight = (1.0 * len(inliers)) / len(pts)
axis, angle = rotations.rotation_matrix_to_axis_angle(R)
r = axis * angle
rotation.append(r.reshape(3, 1))
weights.append(weight)
rotation = np.hstack(rotation)
weights = np.array(weights)
# Scale from rad/frame to rad/s
rotation *= self.camera_model.frame_rate
# Remove and interpolate bad values
threshold = 0.2
mask = weights > threshold
x = np.arange(rotation.shape[1])
for i in range(3):
rotation[i, ~mask] = np.interp(x[~mask], x[mask], rotation[i, mask])
self._frame_rotations = rotation
self._flow = np.linalg.norm(rotation, axis=0)
class OpenCvVideoStream(VideoStream):
"""Video stream that uses OpenCV to extract image data.
This stream class uses the OpenCV VideoCapture class and can thus handle any
video type that is supported by the installed version of OpenCV.
It can only handle video files, and not live streams.
"""
def __init__(self, camera_model, filename, start_time=0.0, duration=None):
"""Create video stream
Parameters
---------------
camera_model : CameraModel
Camera model
filename : str
Path to the video file
start_time : float
The time in seconds where to start capturing (USE WITH CAUTION)
duration : float
Duration in seconds to capture (USE WITH CAUTION)
Notes
-------------------
You can specify the start time and duration you want to use for the capture.
However, be advised that this may or may not work depending on the type of video data
and your installation of OpenCV. Use with caution!
"""
super(OpenCvVideoStream, self).__init__(camera_model)
self.filename = filename
self.start_time = start_time
self.duration = duration
self.step = 1
def _frames(self):
vc = cv2.VideoCapture(self.filename)
if not vc.isOpened():
raise IOError("Failed to open '{}'. Either there is something wrong with the file or OpenCV does not have the correct codec".format(self.filename))
# OpenCV does something really stupid: to set the frame we need to set it twice and query in between
t = self.start_time * 1000. # turn to milliseconds
t2 = t + self.duration*1000.0 if self.duration is not None else None
for i in range(2): # Sometimes needed for setting to stick
vc.set(CV_CAP_PROP_POS_MSEC, t)
vc.read()
t = vc.get(CV_CAP_PROP_POS_MSEC)
counter = 0
retval = True
while retval and (t2 is None or (t2 is not None and t < t2)):
retval, im = vc.read()
if retval:
if np.mod(counter, self.step) == 0:
yield im
elif t2 is not None:
raise IOError("Failed to get frame at time %.2f" % t)
else:
pass # Loop will end normally
t = vc.get(CV_CAP_PROP_POS_MSEC)
counter += 1
================================================
FILE: crisp/timesync.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Time synchronization module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
#--------------------------------------------------------------------------
# Includes
#--------------------------------------------------------------------------
import logging
logger = logging.getLogger()
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt
import scipy.signal as ssig
import scipy.optimize
from matplotlib.mlab import normpdf
from . import tracking
from .imu import IMU
from . import rotations
from . import znccpyr
#--------------------------------------------------------------------------
# Public functions
#--------------------------------------------------------------------------
def sync_camera_gyro(image_sequence_or_flow, image_timestamps, gyro_data, gyro_timestamps, levels=6, full_output=False):
"""Get time offset that aligns image timestamps with gyro timestamps.
Given an image sequence, and gyroscope data, with their respective timestamps,
calculate the offset that aligns the image data with the gyro data.
The timestamps must only differ by an offset, not a scale factor.
This function finds an approximation of the offset *d* that makes this transformation
t_gyro = t_camera + d
i.e. your new image timestamps should be
image_timestamps_aligned = image_timestamps + d
The offset is calculated using zero-mean cross correlation of the gyroscope data magnitude
and the optical flow magnitude, calculated from the image sequence.
ZNCC is performed using pyramids to make it quick.
The offset is accurate up to about +/- 2 frames, so you should run
*refine_time_offset* if you need better accuracy.
Parameters
---------------
image_sequence_or_flow : sequence of image data, or ndarray
This must be either a list or generator that provides a stream of
images that are used for optical flow calculations.
image_timestamps : ndarray
Timestamps of the images in image_sequence
gyro_data : (3, N) ndarray
Gyroscope measurements (angular velocity)
gyro_timestamps : ndarray
Timestamps of data in gyro_data
levels : int
Number of pyramid levels
full_output : bool
If False, only return the offset, otherwise return extra data
Returns
--------------
time_offset : float
The time offset to add to image_timestamps to align the image data
with the gyroscope data
flow : ndarray
(Only if full_output=True)
The calculated optical flow magnitude
"""
# If input is not flow, then create from iamge sequence
try:
assert image_sequence_or_flow.ndim == 1
flow_org = image_sequence_or_flow
except AssertionError:
flow_org = tracking.optical_flow_magnitude(image_sequence_or_flow)
# Gyro from gyro data
gyro_mag = np.sum(gyro_data**2, axis=0)
flow_timestamps = image_timestamps[:-2]
# Resample to match highest
rate = lambda ts: len(ts) / (ts[-1] - ts[0])
freq_gyro = rate(gyro_timestamps)
freq_image = rate(flow_timestamps)
if freq_gyro > freq_image:
rel_rate = freq_gyro / freq_image
flow_mag = znccpyr.upsample(flow_org, rel_rate)
else:
flow_mag = flow_org
rel_rate = freq_image / freq_gyro
gyro_mag = znccpyr.upsample(gyro_mag, rel_rate)
ishift = znccpyr.find_shift_pyr(flow_mag, gyro_mag, levels)
if freq_gyro > freq_image:
flow_shift = int(-ishift / rel_rate)
else:
flow_shift = int(-ishift)
time_offset = flow_timestamps[flow_shift]
if full_output:
return time_offset, flow_org # Return the orginal flow, not the upsampled version
else:
return time_offset
#--------------------------------------------------------------------------
def sync_camera_gyro_manual(image_sequence, image_timestamps, gyro_data, gyro_timestamps, full_output=False):
"""Get time offset that aligns image timestamps with gyro timestamps.
Given an image sequence, and gyroscope data, with their respective timestamps,
calculate the offset that aligns the image data with the gyro data.
The timestamps must only differ by an offset, not a scale factor.
This function finds an approximation of the offset *d* that makes this transformation
t_gyro = t_camera + d
i.e. your new image timestamps should be
image_timestamps_aligned = image_timestamps + d
The offset is calculated using correlation. The parts of the signals to use are
chosen by the user by picking points in a plot window.
The offset is accurate up to about +/- 2 frames, so you should run
*refine_time_offset* if you need better accuracy.
Parameters
---------------
image_sequence : sequence of image data
This must be either a list or generator that provides a stream of
images that are used for optical flow calculations.
image_timestamps : ndarray
Timestamps of the images in image_sequence
gyro_data : (3, N) ndarray
Gyroscope measurements (angular velocity)
gyro_timestamps : ndarray
Timestamps of data in gyro_data
full_output : bool
If False, only return the offset, otherwise return extra data
Returns
--------------
time_offset : float
The time offset to add to image_timestamps to align the image data
with the gyroscope data
flow : ndarray
(Only if full_output=True)
The calculated optical flow magnitude
frame_pair : (int, int)
The frame pair that was picked for synchronization
"""
flow = tracking.optical_flow_magnitude(image_sequence)
flow_timestamps = image_timestamps[:-2]
# Let user select points in both pieces of data
(frame_pair, gyro_idx) = manual_sync_pick(flow, gyro_timestamps, gyro_data)
# Normalize data
gyro_abs_max = np.max(np.abs(gyro_data), axis=0)
gyro_normalized = (gyro_abs_max / np.max(gyro_abs_max)).flatten()
flow_normalized = (flow / np.max(flow)).flatten()
rate = lambda ts: len(ts) / (ts[-1] - ts[0])
# Resample to match highest
freq_gyro = rate(gyro_timestamps)
freq_image = rate(flow_timestamps)
logger.debug("Gyro sampling frequency: %.2f Hz, Image sampling frequency: %.2f Hz", freq_gyro, freq_image)
gyro_part = gyro_normalized[gyro_idx[0]:gyro_idx[1]+1] # only largest
flow_part = flow_normalized[frame_pair[0]:frame_pair[1]+1]
N = flow_part.size * freq_gyro / freq_image
flow_part_resampled = ssig.resample(flow_part, N).flatten()
# ) Cross correlate the two signals and find time diff
corr = ssig.correlate(gyro_part, flow_part_resampled, 'full') # Find the flow in gyro data
i = np.argmax(corr)
t_0_f = flow_timestamps[frame_pair[0]]
t_1_f = flow_timestamps[frame_pair[1]]
t_off_g = gyro_timestamps[gyro_idx[0] + i]
t_off_f = t_1_f
time_offset = t_off_g - t_off_f
if full_output:
return time_offset, flow, frame_pair
else:
return time_offset
#--------------------------------------------------------------------------
def manual_sync_pick(flow, gyro_ts, gyro_data):
# First pick good points in flow
plt.clf()
plt.plot(flow)
plt.title('Select two points')
selected_frames = [int(round(x[0])) for x in plt.ginput(2)]
# Now pick good points in gyro
plt.clf()
plt.subplot(211)
plt.plot(flow)
plt.plot(selected_frames, flow[selected_frames], 'ro')
plt.subplot(212)
plt.plot(gyro_ts, gyro_data.T)
plt.title('Select corresponding sequence in gyro data')
plt.draw()
selected = plt.ginput(2) #[int(round(x[0])) for x in plt.ginput(2)]
gyro_idxs = [(gyro_ts >= x[0]).nonzero()[0][0] for x in selected]
plt.plot(gyro_ts[gyro_idxs], gyro_data[:, gyro_idxs].T, 'ro')
plt.title('Ok, click to continue to next')
plt.draw()
plt.waitforbuttonpress(timeout=10.0)
plt.close()
return (tuple(selected_frames), gyro_idxs)
#--------------------------------------------------------------------------
def refine_time_offset(image_list, frame_timestamps, rotation_sequence, rotation_timestamps, camera_matrix, readout_time):
"""Refine a time offset between camera and IMU using rolling shutter aware optimization.
To refine the time offset using this function, you must meet the following constraints
1) The data must already be roughly aligned. Only a few image frames of error
is allowed.
2) The images *must* have been captured by a *rolling shutter* camera.
This function finds a refined offset using optimization.
Points are first tracked from the start to the end of the provided images.
Then an optimization function looks at the reprojection error of the tracked points
given the IMU-data and the refined offset.
The found offset *d* is such that you want to perform the following time update
new_frame_timestamps = frame_timestamps + d
Parameters
------------
image_list : list of ndarray
A list of images to perform tracking on. High quality tracks are required,
so make sure the sequence you choose is easy to track in.
frame_timestamps : ndarray
Timestamps of image_list
rotation_sequence : (4, N) ndarray
Absolute rotations as a sequence of unit quaternions (first element is scalar).
rotation_timestamps : ndarray
Timestamps of rotation_sequence
camera_matrix : (3,3) ndarray
The internal camera calibration matrix of the camera.
readout_time : float
The readout time of the camera.
Returns
------------
offset : float
A refined offset that aligns the image data with the rotation data.
"""
# ) Track points
max_corners = 200
quality_level = 0.07
min_distance = 5
max_tracks = 20
initial_points = cv2.goodFeaturesToTrack(image_list[0], max_corners, quality_level, min_distance)
(points, status) = tracking.track_retrack(image_list, initial_points)
# Prune to at most max_tracks number of tracks, choose randomly
track_id_list = np.random.permutation(points.shape[0])[:max_tracks]
rows, cols = image_list[0].shape[:2]
row_delta_time = readout_time / rows
num_tracks, num_frames, _ = points.shape
K = np.matrix(camera_matrix)
def func_to_optimize(td, *args):
res = 0.0
N = 0
for frame_idx in range(num_frames-1):
for track_id in track_id_list:
p1 = points[track_id, frame_idx, :].reshape((-1,1))
p2 = points[track_id, frame_idx + 1, :].reshape((-1,1))
t1 = frame_timestamps[frame_idx] + (p1[1] - 1) * row_delta_time + td
t2 = frame_timestamps[frame_idx + 1] + (p2[1] - 1) * row_delta_time +td
t1 = float(t1)
t2 = float(t2)
q1 = IMU.rotation_at_time(t1, rotation_timestamps, rotation_sequence)
q2 = IMU.rotation_at_time(t2, rotation_timestamps, rotation_sequence)
R1 = rotations.quat_to_rotation_matrix(q1)
R2 = rotations.quat_to_rotation_matrix(q2)
p1_rec = K.dot(R1.T).dot(R2).dot(K.I).dot(np.vstack((p2, 1)))
if p1_rec[2] == 0:
continue
else:
p1_rec /= p1_rec[2]
res += np.sum((p1 - np.array(p1_rec[0:2]))**2)
N += 1
return res / N
# Bounded Brent optimizer
t0 = time.time()
tolerance = 1e-4 # one tenth millisecond
(refined_offset, fval, ierr, numfunc) = scipy.optimize.fminbound(func_to_optimize, -0.12, 0.12, xtol=tolerance, full_output=True)
t1 = time.time()
if ierr == 0:
logger.info("Time offset found by brent optimizer: %.4f. Elapsed: %.2f seconds (%d function calls)", refined_offset, t1-t0, numfunc)
else:
logger.error("Brent optimizer did not converge. Aborting!")
raise Exception("Brent optimizer did not converge, when trying to refine offset.")
return refined_offset
def good_sequences_to_track(flow, motion_threshold=1.0):
"""Get list of good frames to do tracking in.
Looking at the optical flow, this function chooses a span of frames
that fulfill certain criteria.
These include
* not being too short or too long
* not too low or too high mean flow magnitude
* a low max value (avoids motion blur)
Currently, the cost function for a sequence is hard coded. Sorry about that.
Parameters
-------------
flow : ndarray
The optical flow magnitude
motion_threshold : float
The maximum amount of motion to consider for sequence endpoints.
Returns
------------
sequences : list
Sorted list of (a, b, score) elements (highest scpre first) of sequences
where a sequence is frames with frame indices in the span [a, b].
"""
endpoints = []
in_low = False
for i, val in enumerate(flow):
if val < motion_threshold:
if not in_low:
endpoints.append(i)
in_low = True
else:
if in_low:
endpoints.append(i-1) # Previous was last in a low spot
in_low = False
def mean_score_func(m):
mu = 15
sigma = 8
top_val = normpdf(mu, mu, sigma)
return normpdf(m, mu, sigma) / top_val
def max_score_func(m):
mu = 40
sigma = 8
if m <= mu:
return 1.
else:
top_val = normpdf(mu, mu, sigma)
return normpdf(m, mu, sigma) / top_val
def length_score_func(l):
mu = 30
sigma = 10
top_val = normpdf(mu, mu, sigma)
return normpdf(l, mu, sigma) / top_val
min_length = 5 # frames
sequences = []
for k, i in enumerate(endpoints[:-1]):
for j in endpoints[k+1:]:
length = j - i
if length < min_length:
continue
seq = flow[i:j+1]
m_score = mean_score_func(np.mean(seq))
mx_score = max_score_func(np.max(seq))
l_score = length_score_func(length)
logger.debug("%d, %d scores: (mean=%.5f, max=%.5f, length=%.5f)" % (i,j,m_score, mx_score, l_score))
if min(m_score, mx_score, l_score) < 0.2:
continue
score = m_score + mx_score + l_score
sequences.append((i, j, score))
return sorted(sequences, key=lambda x: x[2], reverse=True)
================================================
FILE: crisp/tracking.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Tracking module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2013, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
#--------------------------------------------------------------------------
# Includes
#--------------------------------------------------------------------------
import cv2
import numpy as np
#--------------------------------------------------------------------------
# Parameters
#--------------------------------------------------------------------------
GFTT_DEFAULTS = {'max_corners' : 40,
'quality_level' : 0.07,
'min_distance' : 10}
#--------------------------------------------------------------------------
# Public functions
#--------------------------------------------------------------------------
def track_points(img1, img2, initial_points=None, gftt_params={}):
"""Track points between two images
Parameters
-----------------
img1 : (M, N) ndarray
First image
img2 : (M, N) ndarray
Second image
initial_points : ndarray
Initial points. If empty, initial points will be calculated from
img1 using goodFeaturesToTrack in OpenCV
gftt_params : dict
Keyword arguments for goodFeaturesToTrack
Returns
-----------------
points : ndarray
Tracked points
initial_points : ndarray
Initial points used
"""
params = GFTT_DEFAULTS
if gftt_params:
params.update(gftt_params)
if initial_points is None:
initial_points = cv2.goodFeaturesToTrack(img1, params['max_corners'], params['quality_level'], params['min_distance'])
[_points, status, err] = cv2.calcOpticalFlowPyrLK(img1, img2, initial_points, np.array([]))
# Filter out valid points only
points = _points[np.nonzero(status)]
initial_points = initial_points[np.nonzero(status)]
return (points, initial_points)
#--------------------------------------------------------------------------
def optical_flow_magnitude(image_sequence, max_diff=60, gftt_options={}):
"""Return optical flow magnitude for the given image sequence
The flow magnitude is the mean value of the total (sparse) optical flow
between two images.
Crude outlier detection using the max_diff parameter is used.
Parameters
----------------
image_sequence : sequence
Sequence of image data (ndarrays) to calculate flow magnitude from
max_diff : float
Distance threshold for outlier rejection
gftt_options : dict
Keyword arguments to the OpenCV goodFeaturesToTrack function
Returns
----------------
flow : ndarray
The optical flow magnitude
"""
flow = []
prev_img = None
for img in image_sequence:
if img.ndim == 3 and img.shape[2] == 3:
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
if prev_img is None:
prev_img = img
continue
(next_points, prev_points) = track_points(prev_img, img, gftt_params=gftt_options)
distance = np.sqrt(np.sum((next_points - prev_points)**2, 1))
distance2 = distance[np.nonzero(distance < max_diff)] # Crude outlier rejection
dm = np.mean(distance2)
if np.isnan(dm):
dm = 0
flow.append(dm)
prev_img = img
return np.array(flow)
#--------------------------------------------------------------------------
def track(image_list, initial_points, remove_bad=True):
"""Track points in image list
Parameters
----------------
image_list : list
List of images to track in
initial_points : ndarray
Initial points to use (in first image in image_list)
remove_bad : bool
If True, then the resulting list of tracks will only contain succesfully
tracked points. Else, it will contain all points present in initial_points.
Returns
-----------------
tracks : (N, M, 2) ndarray
N tracks over M images with (x,y) coordinates of points
status : (N,) ndarray
The status of each track. 1 means ok, while 0 means tracking failure
"""
# Precreate track array
tracks = np.zeros((initial_points.shape[0], len(image_list), 2), dtype='float32') # NxMx2
tracks[:,0,:] = np.reshape(np.array(initial_points), [-1,2])
track_status = np.ones([np.size(initial_points,0),1]) # All initial points are OK
empty = np.array([])
window_size = (5,5)
for i in range(1, len(image_list)):
img1 = image_list[i-1]
img2 = image_list[i]
prev_ok_track = np.flatnonzero(track_status)
prev_points = tracks[prev_ok_track,i-1,:]
[points, status, err] = cv2.calcOpticalFlowPyrLK(img1, img2, prev_points, empty, empty, empty, window_size)
if status is None:
track_status[:] = 0 # All tracks are bad
break
valid_set = np.flatnonzero(status)
now_ok_tracks = prev_ok_track[valid_set] # Remap
tracks[now_ok_tracks,i,:] = points[valid_set]
track_status[prev_ok_track] = status
if remove_bad:
final_ok = np.flatnonzero(track_status)
tracks = tracks[final_ok] # Only rows/tracks with nonzero status
track_status = track_status[final_ok]
return (tracks, track_status)
#--------------------------------------------------------------------------
def track_retrack(image_list, initial_points, max_retrack_distance=0.5, keep_bad=False):
"""Track-retracks points in image list
Using track-retrack can help in only getting point tracks of high quality.
The point is tracked forward, and then backwards in the image sequence.
Points that end up further than max_retrack_distance from its starting point
are marked as bad.
Parameters
----------------
image_list : list
List of images to track in
initial_points : ndarray
Initial points to use (in first image in image_list)
max_retrack_distance : float
The maximum distance of the retracked point from its starting point to
still count as a succesful retrack.
remove_bad : bool
If True, then the resulting list of tracks will only contain succesfully
tracked points. Else, it will contain all points present in initial_points.
Returns
-----------------
tracks : (N, M, 2) ndarray
N tracks over M images with (x,y) coordinates of points
Note that M is the number of image in the input, and is the track in
the forward tracking step.
status : (N,) ndarray
The status of each track. 1 means ok, while 0 means tracking failure
"""
(forward_track, forward_status) = track(image_list, initial_points, remove_bad=False)
# Reverse the order
(backward_track, backward_status) = track(image_list[::-1], forward_track[:,-1,:], remove_bad=False)
# Prune bad tracks
ok_track = np.flatnonzero(forward_status * backward_status) # Only good if good in both
forward_first = forward_track[ok_track,0,:]
backward_last = backward_track[ok_track,-1,:]
# Distance
retrack_distance = np.sqrt(np.sum((forward_first - backward_last)**2, 1))
# Allowed
retracked_ok = np.flatnonzero(retrack_distance <= max_retrack_distance)
final_ok = ok_track[retracked_ok]
if keep_bad: # Let caller check status
status = np.zeros(forward_status.shape)
status[final_ok] = 1
return (forward_track, status)
else: # Remove tracks with faulty retrack
return (forward_track[final_ok], forward_status[final_ok])
================================================
FILE: crisp/videoslice.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
Video slice module
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2015, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import logging
logger = logging.getLogger(__name__)
import cv2
import numpy as np
from . import rotations
from . import tracking
class Slice(object):
def __init__(self, start, end, points):
self.points = points
self.start = start
self.end = end
self.axis = None
self.angle = None
self.inliers = []
def estimate_rotation(self, camera, ransac_threshold=7.0):
"""Estimate the rotation between first and last frame
It uses RANSAC where the error metric is the reprojection error of the points
from the last frame to the first frame.
Parameters
-----------------
camera : CameraModel
Camera model
ransac_threshold : float
Distance threshold (in pixels) for a reprojected point to count as an inlier
"""
if self.axis is None:
x = self.points[:, 0, :].T
y = self.points[:, -1, :].T
inlier_ratio = 0.5
R, t, dist, idx = rotations.estimate_rotation_procrustes_ransac(x, y,
camera,
ransac_threshold,
inlier_ratio=inlier_ratio,
do_translation=False)
if R is not None:
self.axis, self.angle = rotations.rotation_matrix_to_axis_angle(R)
if self.angle < 0: # Constrain to positive angles
self.angle = -self.angle
self.axis = -self.axis
self.inliers = idx
return self.axis is not None
@staticmethod
def from_stream_randomly(video_stream, step_bounds=(5, 15), length_bounds=(2, 15), max_start=None, min_distance=10, min_slice_points=10):
"""Create slices from a video stream using random sampling
Parameters
-----------------
video_stream : VideoStream
A video stream
step_bounds : tuple
Range bounds (inclusive) of possible step lengths
length_bounds : tuple
Range bounds (inclusive) of possible slice lengths
max_start : int
Maximum frame number to start from
min_distance : float
Minimum (initial) distance between tracked points
min_slice_points : int
Minimum number of points to keep a slice
Returns
-------------------
list of Slice
List of slices
"""
new_step = lambda: int(np.random.uniform(low=step_bounds[0], high=step_bounds[1]))
new_length = lambda: int(np.random.uniform(low=length_bounds[0], high=length_bounds[1]))
seq_frames = []
slices = []
seq_start_points = None
next_seq_start = new_step() if max_start is None else min(new_step(), max_start)
next_seq_length = new_length()
for i, im in enumerate(video_stream):
if next_seq_start <= i < next_seq_start + next_seq_length:
im = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
seq_frames.append(im)
if len(seq_frames) == 1:
max_corners = 400
quality_level = 0.07
seq_start_points = cv2.goodFeaturesToTrack(im, max_corners, quality_level, min_distance)
elif len(seq_frames) == next_seq_length:
points, status = tracking.track_retrack(seq_frames, seq_start_points)
if points.shape[0] >= min_slice_points:
s = Slice(next_seq_start, i, points)
slices.append(s)
logger.debug('{0:4d} {1:3d} {2:5d} {3:>5d}-{4:<5d}'.format(len(slices)-1, points.shape[1], points.shape[0], next_seq_start, i))
seq_frames = []
next_seq_start = i + new_step()
next_seq_length = new_length()
return slices
def fill_sampling(slice_list, N):
"""Given a list of slices, draw N samples such that each slice contributes as much as possible
Parameters
--------------------------
slice_list : list of Slice
List of slices
N : int
Number of samples to draw
"""
A = [len(s.inliers) for s in slice_list]
N_max = np.sum(A)
if N > N_max:
raise ValueError("Tried to draw {:d} samples from a pool of only {:d} items".format(N, N_max))
samples_from = np.zeros((len(A),), dtype='int') # Number of samples to draw from each group
remaining = N
while remaining > 0:
remaining_groups = np.flatnonzero(samples_from - np.array(A))
if remaining < len(remaining_groups):
np.random.shuffle(remaining_groups)
for g in remaining_groups[:remaining]:
samples_from[g] += 1
else:
# Give each group the allowed number of samples. Constrain to their max size.
to_each = max(1, int(remaining / len(remaining_groups)))
samples_from = np.min(np.vstack((samples_from + to_each, A)), axis=0)
# Update remaining count
remaining = int(N - np.sum(samples_from))
if not remaining == 0:
raise ValueError("Still {:d} samples left! This is an error in the selection.")
# Construct index list of selected samples
samples = []
for s, a, n in zip(slice_list, A, samples_from):
if a == n:
samples.append(np.array(s.inliers)) # all
elif a == 0:
samples.append(np.arange([]))
else:
chosen = np.random.choice(s.inliers, n, replace=False)
samples.append(np.array(chosen))
return samples
================================================
FILE: crisp/znccpyr.py
================================================
# -*- coding: utf-8 -*-
from __future__ import division, print_function, absolute_import
"""
ZNCC using Pyramids
"""
__author__ = "Per-Erik Forssén"
__copyright__ = "Copyright 2013, Per-Erik Forssén"
__license__ = "GPL"
__email__ = "perfo@isy.liu.se"
import logging
logger = logging.getLogger()
import numpy as np
def gaussian_kernel(gstd):
"""Generate odd sized truncated Gaussian
The generated filter kernel has a cutoff at $3\sigma$
and is normalized to sum to 1
Parameters
-------------
gstd : float
Standard deviation of filter
Returns
-------------
g : ndarray
Array with kernel coefficients
"""
Nc = np.ceil(gstd*3)*2+1
x = np.linspace(-(Nc-1)/2,(Nc-1)/2,Nc,endpoint=True)
g = np.exp(-.5*((x/gstd)**2))
g = g/np.sum(g)
return g
def subsample(time_series, downsample_factor):
"""Subsample with Gaussian prefilter
The prefilter will have the filter size $\sigma_g=.5*ssfactor$
Parameters
--------------
time_series : ndarray
Input signal
downsample_factor : float
Downsampling factor
Returns
--------------
ts_out : ndarray
The downsampled signal
"""
Ns = np.int(np.floor(np.size(time_series)/downsample_factor))
g = gaussian_kernel(0.5*downsample_factor)
ts_blur = np.convolve(time_series,g,'same')
ts_out = np.zeros((Ns,1), dtype='float64')
for k in range(0,Ns):
cpos = (k+.5)*downsample_factor-.5
cfrac = cpos-np.floor(cpos)
cind = np.floor(cpos)
if cfrac>0:
ts_out[k]=ts_blur[cind]*(1-cfrac)+ts_blur[cind+1]*cfrac
else:
ts_out[k]=ts_blur[cind]
return ts_out
def upsample(time_series, scaling_factor):
"""Upsample using linear interpolation
The function uses replication of the value at edges
Parameters
--------------
time_series : ndarray
Input signal
scaling_factor : float
The factor to upsample with
Returns
--------------
ts_out : ndarray
The upsampled signal
"""
Ns0 = np.size(time_series)
Ns = np.int(np.floor(np.size(time_series)*scaling_factor))
ts_out = np.zeros((Ns,1), dtype='float64')
for k in range(0,Ns):
cpos = int(np.min([Ns0-1,np.max([0.,(k+0.5)/scaling_factor-0.5])]))
cfrac = cpos-np.floor(cpos)
cind = int(np.floor(cpos))
#print "cpos=%f cfrac=%f cind=%d", (cpos,cfrac,cind)
if cfrac>0:
ts_out[k]=time_series[cind]*(1-cfrac)+time_series[cind+1]*cfrac
else:
ts_out[k]=time_series[cind]
return ts_out
def do_binning(time_series,factor):
Ns = np.size(time_series) // factor
ts_out = np.zeros((Ns,1), dtype='float64')
for k in range(0,Ns):
ts_out[k]=0
for l in range(0,factor):
ts_out[k] += time_series[k*factor+l]
ts_out[k] /= factor
return ts_out
def create_pyramid(time_series,octaves):
pyr_out = [time_series ]
for k in range(0,octaves):
pyr_out.append(do_binning(pyr_out[-1],2))
return pyr_out
def zncc(ts1,ts2):
"""Zero mean normalised cross-correlation (ZNCC)
This function does ZNCC of two signals, ts1 and ts2
Normalisation by very small values is avoided by doing
max(nmin,nvalue)
Parameters
--------------
ts1 : ndarray
Input signal 1 to be aligned with
ts2 : ndarray
Input signal 2
Returns
--------------
best_shift : float
The best shift of *ts1* to align it with *ts2*
ts_out : ndarray
The correlation result
"""
# Output is the same size as ts1
Ns1 = np.size(ts1)
Ns2 = np.size(ts2)
ts_out = np.zeros((Ns1,1), dtype='float64')
ishift = int(np.floor(Ns2/2)) # origin of ts2
t1m = np.mean(ts1)
t2m = np.mean(ts2)
for k in range(0,Ns1):
lstart = np.int(ishift-k)
if lstart<0 :
lstart=0
lend = np.int(ishift-k+Ns2)
imax = np.int(np.min([Ns2,Ns1-k+ishift]))
if lend>imax :
lend=imax
csum = 0
ts1sum = 0
ts1sum2 = 0
ts2sum = 0
ts2sum2 = 0
Nterms = lend-lstart
for l in range(lstart,lend):
csum += ts1[k+l-ishift]*ts2[l]
ts1sum += ts1[k+l-ishift]
ts1sum2 += ts1[k+l-ishift]*ts1[k+l-ishift]
ts2sum += ts2[l]
ts2sum2 += ts2[l]*ts2[l]
ts1sum2 = np.max([t1m*t1m*100,ts1sum2])-ts1sum*ts1sum/Nterms
ts2sum2 = np.max([t2m*t2m*100,ts2sum2])-ts2sum*ts2sum/Nterms
#ts_out[k]=csum/np.sqrt(ts1sum2*ts2sum2)
ts_out[k]=(csum-2.0*ts1sum*ts2sum/Nterms+ts1sum*ts2sum/Nterms/Nterms)/np.sqrt(ts1sum2*ts2sum2)
best_shift = np.argmax(ts_out)-ishift
return best_shift, ts_out
def refine_correlation(ts1,ts2,shift_guess):
"""Refine a rough guess of shift by evaluating ZNCC for similar values
Shifts of *ts1* are tested in the range [-2:2]
Refine a rough guess of shift, by trying neighbouring ZNCC values
in the range [-2:2]
Parameters
----------------
ts1 : list_like
The first timeseries
ts2 : list_like
The seconds timeseries
shift_guess : float
The guess to start from
Returns
---------------
best_shift : float
The best shift of those tested
ts_out : ndarray
Computed correlation values
"""
Ns1 = np.size(ts1)
Ns2 = np.size(ts2)
ts_out = np.zeros((5,1))
ishift = int(np.floor(Ns2/2)) # origin of ts2
k_offset = shift_guess-2+ishift # Try shifts starting with this one
t1m = np.mean(ts1)
t2m = np.mean(ts2)
for k in range(0,5):
km = k+k_offset
lstart = np.int(ishift-km)
if lstart<0 :
lstart=0
lend = np.int(ishift-km+Ns2)
imax = np.int(np.min([Ns2,Ns1-km+ishift]))
if lend>imax :
lend=imax
csum = 0
ts1sum = 0
ts1sum2 = 0
ts2sum = 0
ts2sum2 = 0
Nterms = lend-lstart
for l in range(lstart,lend):
csum += ts1[km+l-ishift]*ts2[l]
ts1sum += ts1[km+l-ishift]
ts1sum2 += ts1[km+l-ishift]*ts1[km+l-ishift]
ts2sum += ts2[l]
ts2sum2 += ts2[l]*ts2[l]
ts1sum2 = np.max([t1m*t1m*100,ts1sum2])-ts1sum*ts1sum/Nterms
ts2sum2 = np.max([t2m*t2m*100,ts2sum2])-ts2sum*ts2sum/Nterms
#ts_out[k]=csum/np.sqrt(ts1sum2*ts2sum2)
ts_out[k]=(csum-2.0*ts1sum*ts2sum/Nterms+ts1sum*ts2sum/Nterms/Nterms)/np.sqrt(ts1sum2*ts2sum2)
best_shift = np.argmax(ts_out)+k_offset-ishift
return best_shift, ts_out
def find_shift_pyr(ts1,ts2,nlevels):
"""
Find shift that best aligns two time series
The shift that aligns the timeseries ts1 with ts2.
This is sought using zero mean normalized cross correlation (ZNCC) in a coarse to fine search with an octave pyramid on nlevels levels.
Parameters
----------------
ts1 : list_like
The first timeseries
ts2 : list_like
The seconds timeseries
nlevels : int
Number of levels in pyramid
Returns
----------------
ts1_shift : float
How many samples to shift ts1 to align with ts2
"""
pyr1 = create_pyramid(ts1,nlevels)
pyr2 = create_pyramid(ts2,nlevels)
logger.debug("pyramid size = %d" % len(pyr1))
logger.debug("size of first element %d " % np.size(pyr1[0]))
logger.debug("size of last element %d " % np.size(pyr1[-1]))
ishift, corrfn = zncc(pyr1[-1],pyr2[-1])
for k in range(1,nlevels+1):
ishift, corrfn = refine_correlation(pyr1[-k-1],pyr2[-k-1],ishift*2)
return ishift
================================================
FILE: examples/gopro_dataset_example.py
================================================
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import print_function, absolute_import
"""
This is an example script that shows how to run the calibrator on our dataset.
The dataset can be found here:
http://www.cvl.isy.liu.se/research/datasets/gopro-gyro-dataset/
To run, simply point the script to one of the video files in the directory
$ python gopro_gyro_dataset_example.py /path/to/dataset/video.MP4
"""
__author__ = "Hannes Ovrén"
__copyright__ = "Copyright 2015, Hannes Ovrén"
__license__ = "GPL"
__email__ = "hannes.ovren@liu.se"
import os
import sys
import argparse
import numpy as np
import crisp
from crisp.l3g4200d import post_process_L3G4200D_data
import crisp.rotations
from crisp.calibration import PARAM_ORDER
CAMERA_MATRIX = np.array(
[[ 853.12703455, 0. , 988.06311256],
[ 0. , 873.54956631, 525.71056312],
[ 0. , 0. , 1. ]]
)
CAMERA_DIST_CENTER = (0.00291108, 0.00041897)
CAMERA_DIST_PARAM = 0.8894355
CAMERA_FRAME_RATE = 30.0
CAMERA_IMAGE_SIZE = (1920, 1080)
CAMERA_READOUT = 0.0316734
GYRO_RATE_GUESS = 853.86
def to_rot_matrix(r):
"Convert combined axis angle vector to rotation matrix"
theta = np.linalg.norm(r)
v = r/theta
R = crisp.rotations.axis_angle_to_rotation_matrix(v, theta)
return R
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('video')
args = parser.parse_args()
gyro_file = os.path.splitext(args.video)[0] + '_gyro.csv'
reference_file = os.path.splitext(args.video)[0] + '_reference.csv'
camera = crisp.AtanCameraModel(CAMERA_IMAGE_SIZE, CAMERA_FRAME_RATE, CAMERA_READOUT, CAMERA_MATRIX,
CAMERA_DIST_CENTER, CAMERA_DIST_PARAM)
print('Creating video stream from {}'.format(args.video))
video = crisp.VideoStream.from_file(camera, args.video)
print('Creating gyro stream from {}'.format(gyro_file))
gyro = crisp.GyroStream.from_csv(gyro_file)
print('Post processing L3G4200D gyroscope data to remove frequency spike noise')
gyro.data = post_process_L3G4200D_data(gyro.data.T).T
print('Creating calibrator')
calibrator = crisp.AutoCalibrator(video, gyro)
print('Estimating time offset and camera to gyroscope rotation. Guessing gyro rate = {:.2f}'.format(GYRO_RATE_GUESS))
try:
calibrator.initialize(gyro_rate=GYRO_RATE_GUESS)
except crisp.InitializationError as e:
print('Initialization failed. Reason "{}"'.format(e.message))
sys.exit(-1)
print('Running calibration. This can take a few minutes.')
try:
calibrator.calibrate()
calibrator.print_params()
except crisp.CalibrationError as e:
print('Calibration failed. Reason "{}"'.format(e.message))
sys.exit(-2)
# Compare with reference data
reference_data = np.loadtxt(reference_file, delimiter=',')
reference_data[[2,3,4,5,6,7]] = reference_data[[5,6,7,2,3,4]] # Swap order of bias and rot
param_data = np.array([calibrator.parameter[p] for p in PARAM_ORDER])
print('\nCompare with reference data')
print()
print('{:^15s} {:^12s} {:^12s} {:^12s}'.format('Parameter', 'Reference', 'Optimized', 'Difference'))
for param, ref, data in zip(PARAM_ORDER, reference_data, param_data):
print("{:>15s} {:E} {:E} {:E}".format(param, ref, data, ref-data))
R_ref = to_rot_matrix(reference_data[5:])
R_data = to_rot_matrix(param_data[5:])
dR = np.dot(R_ref.T, R_data)
v, theta = crisp.rotations.rotation_matrix_to_axis_angle(dR)
print('Reference rotation')
print(R_ref)
print('Optimized rotation')
print(R_data)
print("Angle difference: {:.4f} degrees".format(np.rad2deg(theta)))
================================================
FILE: setup.cfg
================================================
[[metadata]
description-file = README.md
================================================
FILE: setup.py
================================================
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import absolute_import, print_function
#from distutils.core import setup
#from distutils.extension import Extension
#from setuptools.command.sdist import sdist as _sdist
from setuptools import setup, Extension
from setuptools.command.build_ext import build_ext as _build_ext
import sys
import codecs
try:
import numpy as np
except ImportError:
print("Please install numpy before building this package")
raise
try:
#from Cython.Distutils import build_ext
from Cython.Build import cythonize
USE_CYTHON = True
except ImportError:
USE_CYTHON = False
try:
from pypandoc import convert
read_md = lambda f: convert(f, 'rst')
except ImportError:
print("warning: pypandoc module not found, could not convert Markdown to RST")
read_md = lambda f: codecs.open(f, encoding='utf-8').read()
# Fast quaternion integration module
file_ext = 'pyx' if USE_CYTHON else 'c'
fastint_sources = ["crisp/fastintegrate/fastintegrate.{}".format(file_ext),]
ext_modules = [Extension("crisp.fastintegrate", fastint_sources, include_dirs=[np.get_include()]),]
if USE_CYTHON:
ext_modules = cythonize(ext_modules)
classifiers = [
'Development Status :: 4 - Beta',
# Indicate who your project is intended for
'Intended Audience :: Science/Research',
'Topic :: Scientific/Engineering',
# Pick your license as you wish (should match "license" above)
'License :: OSI Approved :: GNU General Public License (GPL)',
# Specify the Python versions you support here. In particular, ensure
# that you indicate whether you support Python 2, Python 3 or both.
'Programming Language :: Python :: 2.7',
'Programming Language :: Python :: 3.4'
]
keywords = 'gyroscope gyro camera imu calibration synchronization'
requires = [ 'numpy',
'scipy',
'matplotlib'
]
setup(name='crisp',
version='2.2.1',
author="Hannes Ovrén",
author_email="hannes.ovren@liu.se",
url="https://github.com/hovren/crisp",
description="Camera-to-IMU calibration and synchronization toolkit",
long_description=read_md('README.md'),
license="GPL",
packages=['crisp'],
ext_modules=ext_modules,
# cmdclass={'build_ext' : build_ext},
classifiers=classifiers,
install_requires=requires,
requires=requires,
keywords=keywords,
# cmdclass={'build_ext' : build_ext}
)
gitextract_22n3_8sv/ ├── .gitignore ├── CITATION ├── LICENSE ├── MANIFEST.in ├── README.md ├── conda.recipe/ │ ├── bld.bat │ ├── build.sh │ └── meta.yaml ├── crisp/ │ ├── __init__.py │ ├── calibration.py │ ├── camera.py │ ├── cli.py │ ├── fastintegrate/ │ │ └── fastintegrate.pyx │ ├── imu.py │ ├── l3g4200d.py │ ├── pose.py │ ├── ransac.py │ ├── remove_slp.py │ ├── rotations.py │ ├── stream.py │ ├── timesync.py │ ├── tracking.py │ ├── videoslice.py │ └── znccpyr.py ├── examples/ │ └── gopro_dataset_example.py ├── setup.cfg └── setup.py
SYMBOL INDEX (139 symbols across 15 files)
FILE: crisp/calibration.py
class CalibrationError (line 29) | class CalibrationError(Exception):
class InitializationError (line 32) | class InitializationError(Exception):
class AutoCalibrator (line 35) | class AutoCalibrator(object):
method __init__ (line 68) | def __init__(self, video, gyro):
method initialize (line 90) | def initialize(self, gyro_rate, slices=None, skip_estimation=False):
method video_time_to_gyro_sample (line 137) | def video_time_to_gyro_sample(self, t):
method parameter (line 160) | def parameter(self):
method calibrate (line 167) | def calibrate(self, max_tracks=MAX_OPTIMIZATION_TRACKS, max_eval=MAX_O...
method find_initial_offset (line 211) | def find_initial_offset(self, pyramids=6):
method find_initial_rotation (line 238) | def find_initial_rotation(self):
method print_params (line 328) | def print_params(self):
function sample_at_time (line 335) | def sample_at_time(t, rate):
function robust_norm (line 341) | def robust_norm(r, c):
function optimization_func (line 345) | def optimization_func(x, slices, slice_sample_idxs, camera, gyro, norm_c):
FILE: crisp/camera.py
class CameraModel (line 23) | class CameraModel(object):
method __init__ (line 31) | def __init__(self, image_size, frame_rate, readout):
method rows (line 48) | def rows(self):
method columns (line 52) | def columns(self):
method project (line 55) | def project(self, points):
method unproject (line 72) | def unproject(self, image_points):
class AtanCameraModel (line 90) | class AtanCameraModel(CameraModel):
method __init__ (line 103) | def __init__(self, image_size, frame_rate, readout, camera_matrix, dis...
method from_hdf (line 128) | def from_hdf(cls, filename):
method invert (line 160) | def invert(self, points):
method apply (line 189) | def apply(self, points):
method project (line 219) | def project(self, points):
method unproject (line 241) | def unproject(self, image_points):
class OpenCVCameraModel (line 264) | class OpenCVCameraModel(CameraModel):
method __init__ (line 270) | def __init__(self, image_size, frame_rate, readout, camera_matrix, dis...
method project (line 292) | def project(self, points):
method unproject (line 311) | def unproject(self, image_points):
method from_hdf (line 332) | def from_hdf(cls, filepath):
function to_homogeneous (line 344) | def to_homogeneous(X):
function from_homogeneous (line 352) | def from_homogeneous(X):
class Camera (line 358) | class Camera(object):
method __init__ (line 359) | def __init__(self):
method load_image (line 366) | def load_image(self, filename):
method images (line 370) | def images(self):
method image_sequence (line 375) | def image_sequence(self, first=0, last=-1):
class DepthCamera (line 383) | class DepthCamera(Camera):
class Kinect (line 386) | class Kinect(object):
class NirCamera (line 396) | class NirCamera(Camera):
method load_image (line 397) | def load_image(self, filename):
method convert_nbit_float32_to_uint8 (line 404) | def convert_nbit_float32_to_uint8(img, nbit):
method __init__ (line 411) | def __init__(self, depth_camera, video_camera, mode):
method set_default_params (line 417) | def set_default_params(self):
method from_directory (line 424) | def from_directory(cls, datadir, video_mode='any'):
method timestamp_from_filename (line 466) | def timestamp_from_filename(fname):
method timestamps_from_file_list (line 472) | def timestamps_from_file_list(file_list):
method detect_bad_timestamps (line 486) | def detect_bad_timestamps(ts_list):
method purge_bad_timestamp_files (line 498) | def purge_bad_timestamp_files(file_list):
method depth_file_for_nir_file (line 522) | def depth_file_for_nir_file(video_filename, depth_file_list):
method depth_file_for_rgb_file (line 532) | def depth_file_for_rgb_file(rgb_filename, rgb_file_list, depth_file_li...
method find_nir_file_with_missing_depth (line 543) | def find_nir_file_with_missing_depth(video_file_list, depth_file_list):
method disparity_image_to_distance (line 561) | def disparity_image_to_distance(self, dval_img):
method align_depth_to_nir (line 567) | def align_depth_to_nir(self, depth_img):
method depthmap_for_nir (line 579) | def depthmap_for_nir(self, nir_filename):
FILE: crisp/cli.py
function load_imu_from_file (line 21) | def load_imu_from_file(imu_file):
function load_vars_from_mat (line 40) | def load_vars_from_mat(filename, var_dict):
function load_images_timestamps_from_csv (line 60) | def load_images_timestamps_from_csv(image_csv):
FILE: crisp/imu.py
class IMU (line 29) | class IMU(object):
method __init__ (line 33) | def __init__(self):
method from_mat_file (line 39) | def from_mat_file(cls, matfilename):
method rate (line 66) | def rate(self):
method zero_level_calibrate (line 79) | def zero_level_calibrate(self, duration, t0=0.0):
method gyro_data_corrected (line 104) | def gyro_data_corrected(self, pose_correction=np.eye(3)):
method integrate (line 119) | def integrate(self, pose_correction=np.eye(3), uniform=True):
method rotation_at_time (line 160) | def rotation_at_time(t, timestamps, rotation_sequence):
class ArduIMU (line 187) | class ArduIMU(IMU):
method __init__ (line 188) | def __init__(self, filename):
method __load (line 195) | def __load(self, gyro_data_filename):
class L3G4200DGyro (line 235) | class L3G4200DGyro(IMU):
method __init__ (line 236) | def __init__(self, filename, post_process=True):
method __load (line 243) | def __load(self, filename, post_process=True):
FILE: crisp/l3g4200d.py
class ParserException (line 13) | class ParserException(Exception):
class GyroParserBase (line 16) | class GyroParserBase(object):
method __init__ (line 17) | def __init__(self):
method parse (line 22) | def parse(self, data):
class L3GArduinoParser (line 25) | class L3GArduinoParser(GyroParserBase):
method __init__ (line 36) | def __init__(self):
method parse (line 45) | def parse(self, input_data):
method __handle (line 65) | def __handle(self, command, data):
function load_L3G_arduino (line 105) | def load_L3G_arduino(filename, remove_begin_spurious=False, return_parse...
function post_process_L3G4200D_data (line 136) | def post_process_L3G4200D_data(data, do_plot=False):
FILE: crisp/pose.py
function estimate_pose (line 24) | def estimate_pose(image_sequences, imu_sequences, K):
function pick_manual (line 110) | def pick_manual(image_sequence, imu_gyro, num_sequences=2):
function principal_rotation_axis (line 142) | def principal_rotation_axis(gyro_data):
function _get_point_correspondences (line 175) | def _get_point_correspondences(image_list, max_corners=200, min_distance...
FILE: crisp/ransac.py
function RANSAC (line 5) | def RANSAC(model_func, eval_func, data, num_points, num_iter, threshold,...
FILE: crisp/remove_slp.py
function remove_slp (line 37) | def remove_slp(img, gstd1=GSTD1, gstd2=GSTD2, gstd3=GSTD3, ksize=KSIZE, ...
FILE: crisp/rotations.py
function procrustes (line 21) | def procrustes(X, Y, remove_mean=False):
function rotation_matrix_to_axis_angle (line 88) | def rotation_matrix_to_axis_angle(R):
function axis_angle_to_rotation_matrix (line 124) | def axis_angle_to_rotation_matrix(v, theta):
function quat_to_rotation_matrix (line 153) | def quat_to_rotation_matrix(q):
function integrate_gyro_quaternion (line 181) | def integrate_gyro_quaternion(gyro_ts, gyro_data):
function slerp (line 220) | def slerp(q1, q2, u):
function estimate_rotation_procrustes_ransac (line 270) | def estimate_rotation_procrustes_ransac(x, y, camera, threshold, inlier_...
FILE: crisp/stream.py
class GyroStream (line 29) | class GyroStream(object):
method __init__ (line 30) | def __init__(self):
method from_csv (line 36) | def from_csv(cls, filename):
method from_data (line 58) | def from_data(cls, data):
method num_samples (line 80) | def num_samples(self):
method integrate (line 83) | def integrate(self, dt):
class VideoStream (line 101) | class VideoStream(object):
method __init__ (line 117) | def __init__(self, camera_model, flow_mode='optical'):
method __iter__ (line 129) | def __iter__(self):
method _frames (line 132) | def _frames(self):
method from_file (line 136) | def from_file(cls, camera_model, filename):
method project (line 158) | def project(self, points):
method unproject (line 175) | def unproject(self, image_points):
method frame_rate (line 194) | def frame_rate(self):
method flow (line 198) | def flow(self):
method _generate_frame_to_frame_rotation (line 210) | def _generate_frame_to_frame_rotation(self):
class OpenCvVideoStream (line 256) | class OpenCvVideoStream(VideoStream):
method __init__ (line 263) | def __init__(self, camera_model, filename, start_time=0.0, duration=No...
method _frames (line 289) | def _frames(self):
FILE: crisp/timesync.py
function sync_camera_gyro (line 36) | def sync_camera_gyro(image_sequence_or_flow, image_timestamps, gyro_data...
function sync_camera_gyro_manual (line 124) | def sync_camera_gyro_manual(image_sequence, image_timestamps, gyro_data,...
function manual_sync_pick (line 214) | def manual_sync_pick(flow, gyro_ts, gyro_data):
function refine_time_offset (line 243) | def refine_time_offset(image_list, frame_timestamps, rotation_sequence, ...
function good_sequences_to_track (line 336) | def good_sequences_to_track(flow, motion_threshold=1.0):
FILE: crisp/tracking.py
function track_points (line 32) | def track_points(img1, img2, initial_points=None, gftt_params={}):
function optical_flow_magnitude (line 71) | def optical_flow_magnitude(image_sequence, max_diff=60, gftt_options={}):
function track (line 114) | def track(image_list, initial_points, remove_bad=True):
function track_retrack (line 163) | def track_retrack(image_list, initial_points, max_retrack_distance=0.5, ...
FILE: crisp/videoslice.py
class Slice (line 22) | class Slice(object):
method __init__ (line 23) | def __init__(self, start, end, points):
method estimate_rotation (line 31) | def estimate_rotation(self, camera, ransac_threshold=7.0):
method from_stream_randomly (line 64) | def from_stream_randomly(video_stream, step_bounds=(5, 15), length_bou...
function fill_sampling (line 117) | def fill_sampling(slice_list, N):
FILE: crisp/znccpyr.py
function gaussian_kernel (line 18) | def gaussian_kernel(gstd):
function subsample (line 41) | def subsample(time_series, downsample_factor):
function upsample (line 73) | def upsample(time_series, scaling_factor):
function do_binning (line 105) | def do_binning(time_series,factor):
function create_pyramid (line 116) | def create_pyramid(time_series,octaves):
function zncc (line 123) | def zncc(ts1,ts2):
function refine_correlation (line 182) | def refine_correlation(ts1,ts2,shift_guess):
function find_shift_pyr (line 245) | def find_shift_pyr(ts1,ts2,nlevels):
FILE: examples/gopro_dataset_example.py
function to_rot_matrix (line 42) | def to_rot_matrix(r):
Condensed preview — 27 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (181K chars).
[
{
"path": ".gitignore",
"chars": 135,
"preview": "build/\n*.pyc\n*.mat\n*~\n*.m\n*.c\n*.so\n*.hdf\n*.ipynb\n.eggs/\n.idea/\n.ipynb_checkpoints/\ncrisp.egg-info/\ndist/\ncrisp/fastinteg"
},
{
"path": "CITATION",
"chars": 409,
"preview": "To cite this software please use the following BibTex information\n\n@inproceedings{Ovren2015,\ntitle = {{Gyroscope-based v"
},
{
"path": "LICENSE",
"chars": 35146,
"preview": " GNU GENERAL PUBLIC LICENSE\n Version 3, 29 June 2007\n\n Copyright (C) 2007 Free "
},
{
"path": "MANIFEST.in",
"chars": 58,
"preview": "include README.md\nrecursive-include crisp/fastintegrate *\n"
},
{
"path": "README.md",
"chars": 3812,
"preview": "# Camera-to-IMU calibration toolbox\nThis toolbox provides a python library to perform joint calibration of a rolling shu"
},
{
"path": "conda.recipe/bld.bat",
"chars": 236,
"preview": "\"%PYTHON%\" setup.py install\nif errorlevel 1 exit 1\n\n:: Add more build steps here, if they are necessary.\n\n:: See\n:: http"
},
{
"path": "conda.recipe/build.sh",
"chars": 219,
"preview": "#!/bin/bash\n\n$PYTHON setup.py install\n\n# Add more build steps here, if they are necessary.\n\n# See\n# http://docs.continuu"
},
{
"path": "conda.recipe/meta.yaml",
"chars": 1376,
"preview": "package:\n name: crisp\n version: \"2.2.1\"\n\nsource:\n git_url: https://github.com/hovren/crisp.git\n git_rev: v2.2.1\n"
},
{
"path": "crisp/__init__.py",
"chars": 686,
"preview": "# -*- coding: utf-8 -*-\n\"\"\"\n========================================\nCamera-to-IMU calibration toolbox\n================="
},
{
"path": "crisp/calibration.py",
"chars": 17249,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nCamera-gyro calibration mo"
},
{
"path": "crisp/camera.py",
"chars": 20727,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nCamera module\n\"\"\"\n__author"
},
{
"path": "crisp/cli.py",
"chars": 2156,
"preview": "# -*- coding: utf-8 -*-\n\"\"\"\nCommand line interface helpers\n\"\"\"\nfrom __future__ import absolute_import\n\n__author__ = \"Han"
},
{
"path": "crisp/fastintegrate/fastintegrate.pyx",
"chars": 1696,
"preview": "# -*- coding: utf-8 -*-\n\"\"\"\nCreated on Mon Feb 3 18:10:40 2014\n\n@author: hannes\n\"\"\"\ncimport cython\n\nimport numpy as np\n"
},
{
"path": "crisp/imu.py",
"chars": 8739,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nIMU module\n\"\"\"\n\n__author__"
},
{
"path": "crisp/l3g4200d.py",
"chars": 6794,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nCreated on Wed Mar 5 10:2"
},
{
"path": "crisp/pose.py",
"chars": 6829,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nRelative pose calibration "
},
{
"path": "crisp/ransac.py",
"chars": 1717,
"preview": "from __future__ import division, print_function, absolute_import\n\nimport numpy as np\n\ndef RANSAC(model_func, eval_func, "
},
{
"path": "crisp/remove_slp.py",
"chars": 2324,
"preview": "#!/usr/bin/env python\n# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nKine"
},
{
"path": "crisp/rotations.py",
"chars": 9646,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nRotation handling module\n\""
},
{
"path": "crisp/stream.py",
"chars": 10257,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nInput streams module\n\"\"\"\n\n"
},
{
"path": "crisp/timesync.py",
"chars": 15246,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nTime synchronization modul"
},
{
"path": "crisp/tracking.py",
"chars": 7832,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nTracking module\n\"\"\"\n\n__aut"
},
{
"path": "crisp/videoslice.py",
"chars": 6305,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nVideo slice module\n\"\"\"\n\n__"
},
{
"path": "crisp/znccpyr.py",
"chars": 7932,
"preview": "# -*- coding: utf-8 -*-\nfrom __future__ import division, print_function, absolute_import\n\n\"\"\"\nZNCC using Pyramids\n\"\"\"\n\n_"
},
{
"path": "examples/gopro_dataset_example.py",
"chars": 3767,
"preview": "#!/usr/bin/env python\n# -*- coding: utf-8 -*-\nfrom __future__ import print_function, absolute_import\n\"\"\"\nThis is an exam"
},
{
"path": "setup.cfg",
"chars": 41,
"preview": "[[metadata]\ndescription-file = README.md\n"
},
{
"path": "setup.py",
"chars": 2469,
"preview": "#!/usr/bin/env python\n# -*- coding: utf-8 -*-\nfrom __future__ import absolute_import, print_function\n\n#from distutils.co"
}
]
About this extraction
This page contains the full source code of the hovren/crisp GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 27 files (169.7 KB), approximately 41.9k tokens, and a symbol index with 139 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.