diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..05609e1
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,71 @@
+cmake_minimum_required (VERSION 2.8.3)
+project (lidar_tag)
+
+# CMAKE TWEAKS
+#========================================================================
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed")
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ tf
+ sensor_msgs
+ visualization_msgs
+ message_filters
+ std_msgs
+ velodyne_pointcloud
+ roslib
+ lidartag_msgs
+)
+# CHECK THE DEPENDENCIES
+# PCL
+find_package(PCL 1.2 REQUIRED)
+find_package(Boost REQUIRED
+ COMPONENTS filesystem system signals regex date_time program_options thread
+)
+find_package(Eigen3)
+if(NOT EIGEN3_FOUND)
+ # Fallback to cmake_modules
+ find_package(cmake_modules REQUIRED)
+ find_package(Eigen REQUIRED)
+ set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+ set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only
+ # Possibly map additional variables to the EIGEN3_ prefix.
+else()
+ set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif()
+
+catkin_package(
+ INCLUDE_DIRS include
+ CATKIN_DEPENDS
+ message_filters
+ roscpp
+ sensor_msgs
+ std_msgs
+ tf
+
+ DEPENDS
+ Eigen3
+)
+
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+ ${PCL_INCLUDE_DIRS}
+ ${Boost_INCLUDE_DIRS}
+ ${EIGEN3_INCLUDE_DIRS}
+)
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+# COMPILE THE SOURCE
+#========================================================================
+add_executable(lidar_tag_main src/main.cc src/lidar_tag.cc src/apriltag_utils.cc src/utils.cc src/tag49h14.cc src/tag16h5.cc)
+add_dependencies(lidar_tag_main ${${PROJECT_NAME}_EXPORTED_TARGETS}
+ ${catkin_EXPORTED_TARGETS})
+target_link_libraries(lidar_tag_main
+ ${catkin_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ${PCL_LIBRARIES}
+)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..f288702
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,674 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc.
+ 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.
+
+
+ Copyright (C)
+
+ 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 .
+
+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:
+
+ Copyright (C)
+ 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
+.
+
+ 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
+.
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..5cf59ac
--- /dev/null
+++ b/README.md
@@ -0,0 +1,42 @@
+# LiDARTag
+## Overview
+
+This is a ROS package for detecting the LiDARTag via ROS.
+
+* Author: Bruce JK Huang
+* Maintainer: [Bruce JK Huang](https://www.brucerobot.com/), brucejkh[at]gmail.com
+* Affiliation: [The Biped Lab](https://www.biped.solutions/), the University of Michigan
+
+This package has been tested under [ROS] Kinetic and Ubuntu 16.04.
+More detail introduction will be update shortly. Sorry for the inconvenient!
+
+## Quick View
+
+
+
+# Why LiDAR?
+Robust to lighting!!
+
+
+
+
+
+
+## Presentation and Video
+https://www.brucerobot.com/lidartag
+
+## Installation
+TODO
+
+## Parameters
+TODO
+
+## Examples
+TODO
+
+## Citations
+The LiDARTag is described in:
+
+*Jiunn-Kai Huang, Maani Ghaffari, Ross Hartley, Lu Gan, Ryan M. Eustice and Jessy W. Grizzle “LiDARTag: A Real-Time Fiducial Tag using Point Clouds” (under review)
+
+
diff --git a/figure/figure1-2.png b/figure/figure1-2.png
new file mode 100644
index 0000000..895a9cb
Binary files /dev/null and b/figure/figure1-2.png differ
diff --git a/figure/figure1.png b/figure/figure1.png
new file mode 100644
index 0000000..8d17536
Binary files /dev/null and b/figure/figure1.png differ
diff --git a/figure/lighting1.jpg b/figure/lighting1.jpg
new file mode 100644
index 0000000..87d2693
Binary files /dev/null and b/figure/lighting1.jpg differ
diff --git a/figure/lighting2.jpg b/figure/lighting2.jpg
new file mode 100644
index 0000000..20dadd3
Binary files /dev/null and b/figure/lighting2.jpg differ
diff --git a/figure/lighting3.jpg b/figure/lighting3.jpg
new file mode 100644
index 0000000..3622b38
Binary files /dev/null and b/figure/lighting3.jpg differ
diff --git a/include/apriltag_utils.h b/include/apriltag_utils.h
new file mode 100644
index 0000000..2651857
--- /dev/null
+++ b/include/apriltag_utils.h
@@ -0,0 +1,59 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+#ifndef APRILTAG_UTILS_H
+#define APRILTAG_UTILS_H
+
+#include "lidar_tag.h"
+
+namespace BipedAprilLab{
+
+ static inline int imax(int a, int b);
+
+
+ /** if the bits in w were arranged in a d*d grid and that grid was
+ * rotated, what would the new bits in w be?
+ * The bits are organized like this (for d = 3):
+ *
+ * 8 7 6 2 5 8 0 1 2
+ * 5 4 3 ==> 1 4 7 ==> 3 4 5 (rotate90 applied twice)
+ * 2 1 0 0 3 6 6 7 8
+ **/
+ uint64_t rotate90(uint64_t w, uint32_t d);
+
+ void QuickDecodeAdd(BipedLab::QuickDecode_t *qd, uint64_t code, int id, int hamming);
+
+ void QuickDecodeInit(BipedLab::GrizTagFamily_t *family, int maxhamming);
+ void QuickDecodeCodeword(BipedLab::GrizTagFamily_t *tf, uint64_t rcode,
+ BipedLab::QuickDecodeEntry_t *entry);
+} // namespace
+#endif
+
+
diff --git a/include/lidar_tag.h b/include/lidar_tag.h
new file mode 100644
index 0000000..5a44afd
--- /dev/null
+++ b/include/lidar_tag.h
@@ -0,0 +1,482 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+
+
+#pragma once
+#ifndef LIDARTAG_H
+#define LIDARTAG_H
+
+#include // std::queue
+#include
+
+#include
+#include
+#include
+#include
+#include // Marker
+#include // Marker
+
+#include
+
+// To trasnform to pcl format
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include "lidartag_msgs/LiDARTagDetection.h"
+#include "lidartag_msgs/LiDARTagDetectionArray.h"
+#include "types.h"
+
+// #include "tensorflow_ros_test/lib.h"
+
+namespace BipedLab{
+ class LiDARTag{
+ public:
+ LiDARTag();
+
+ private:
+ /*****************************************************
+ * Variables
+ *****************************************************/
+ // Flags for functioning
+ int _adaptive_thresholding; // Use adaptive thresholding or not
+ int _collect_dataset; // To collect dataset (only publish one tag)
+ int _sleep_to_display; // Sleep for visualization
+ double _sleep_time_for_vis; // Sleep for how long?
+ int _valgrind_check; // Debugging with Valgrind
+ int _fake_tag;
+ int _decode_method; // Which decode methods to use?
+ int _grid_viz; // visualize remapping grid
+ bool _id_decoding; // decode id or not
+ bool _write_CSV; // Write CSV files
+ std::string _assign_id; // Directly assign Id, mainly for calibration usage
+
+ // ROS
+ ros::NodeHandle _nh;
+ ros::Subscriber _LiDAR1_sub;
+ ros::Publisher _original_pc_pub;
+ ros::Publisher _edge_pub;
+ ros::Publisher _cluster_pub;
+ ros::Publisher _payload_pub;
+
+ ros::Publisher _cluster_marker_pub; // cluster markers
+ ros::Publisher _boundary_marker_pub; // cluster boundary
+ ros::Publisher _payload_marker_pub; // payload boundary
+ ros::Publisher _payload_grid_pub; // grid visualization
+ ros::Publisher _payload_grid_line_pub; // grid visualization
+ ros::Publisher _lidartag_pose_pub; // Publish LiDAR pose
+
+ // Flag
+ int _point_cloud_received; // check if a scan of point cloud has received or not
+ int _stop; // Just a switch for exiting this program while using valgrind
+
+ // ros::Publisher DebugPointCheckPub_; // Debug
+ // ros::Publisher DebugBoundaryPointPub_; // Debug
+ // ros::Publisher DebugNoEdgePub_; // Debug
+ // ros::Publisher DebugExtractPayloadPub_; // Debug
+
+ //ros::Publisher TextPub_; // publish text
+ // visualization_msgs::MarkerArray _markers;
+
+
+ // Queue for pc data
+ std::queue _point_cloud1_queue;
+
+ // lock
+ boost::mutex _refine_lock;
+ boost::mutex _buff_to_pcl_vector_lock;
+ boost::mutex _point_cloud1_queue_lock;
+
+ // LiDAR parameters
+ ros::Time _current_scan_time; // store current time of the lidar scan
+ std::string _pointcloud_topic; // subscribe channel
+ std::string _pub_frame; // publish under what frame?
+
+ // Overall LiDAR system parameters
+ LiDARSystem_t _LiDAR_system;
+ int _beam_num;
+ int _vertical_fov;
+
+ // PointCould data (for a single scan)
+ int _point_cloud_size;
+ std_msgs::Header _point_cloud_header;
+
+ // Edge detection parameters
+ double _intensity_threshold;
+ double _depth_threshold;
+
+ // If on-board processing is limited, limit range of points
+ double _distance_bound;
+
+ // fiducial marker parameters
+ double _payload_size; // physical payload size
+ int _tag_family; // what tag family ie tag16h5, this should be 16
+ int _tag_hamming_distance; // what tag family ie tag16h5, this should be 5
+ int _max_decode_hamming; // max hamming while decoding
+ int _black_border; // black boarder of the fiducial marker
+
+ // Cluster parameters
+ double _threshold;
+ double _RANSAC_threshold;
+ int _fine_cluster_threshold; // TODO: REPLACE WITH TAG PARAMETERS
+ int _filling_gap_max_index; // TODO: CHECK
+ int _filling_max_points_threshold; //TODO: REMOVE
+ double _points_threshold_factor; // TODO: CHECK
+
+ // Payload
+ double _line_intensity_bound; // TODO: not sure but may okay to remove it for releasing
+ double _payload_intensity_threshold;
+ int _min_returns_per_grid;
+ GrizTagFamily_t *tf;
+ lidartag_msgs::LiDARTagDetectionArray _lidartag_pose_array; // an array of apriltags
+
+
+ // NN
+ // LoadWeight *NNptr_;
+ std::string _latest_model;
+ std::string _weight_path;
+ int _max_point_on_payload;
+ int _XYZRI; // input channels
+
+ // Debug
+ Debug_t _debug_cluster;
+ Timing_t _timing;
+
+ // Statistics
+ Statistics_t _result_statistics;
+
+
+ /*****************************************************
+ * Functions
+ *****************************************************/
+
+ /* [Main loop]
+ * main loop of process
+ */
+ void _mainLoop();
+
+
+ /* [basic ros]
+ * A function to get all parameters from a roslaunch
+ * if not get all parameters then it will use hard-coded parameters
+ */
+ void _getParameters();
+
+
+ /* [basic ros]
+ * A function of ros spin
+ * reason: in order to put it into background as well as able to run other tasks
+ */
+ inline void _rosSpin();
+
+
+ /* [basic ros]
+ * A function to make sure the program has received at least one pointcloud
+ * from subscribed channel
+ * at the very start of this program
+ */
+ inline void _waitForPC();
+
+
+ /* [basic ros]
+ * A function to push the received pointcloud into a queue in the class
+ */
+ inline void _pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &pc);
+
+
+ /* [Transform/Publish]
+ * A function to transform pcl msgs to ros msgs and then publish
+ * WhichPublisher should be a string of "organized" or "original" regardless
+ * lowercase and uppercase
+ */
+ void _publishPC(const pcl::PointCloud::Ptr &t_source_PC,
+ const std::string &t_frame_name, std::string t_which_publisher);
+
+ /* [Type transformation]
+ * A function to transform from a customized type (LiDARpoints_t) of vector of
+ * vector (EdgeBuff) into a standard type (PointXYZRI) of pcl vector (out)
+ */
+ void _buffToPclVector(const std::vector> &t_edge_buff,
+ pcl::PointCloud::Ptr t_out);
+
+ /* [Pre-Processing]
+ * A function to slice the Veloydyne full points to sliced pointed
+ * based on ring number
+ */
+ inline void _fillInOrderedPC(const pcl::PointCloud::Ptr &t_pcl_pointcloud,
+ std::vector< std::vector > &t_ordered_buff);
+
+
+ /* [Type transformation]
+ * A function to get pcl OrderedBuff from a ros sensor-msgs form of
+ * pointcould queue
+ */
+ std::vector> _getOrderBuff();
+ // void GetOrderBuff(std::vector>& OrderedBuff);
+
+
+ /* [LiDAR analysis]
+ * A function to get a LiDAR system parameters such as max, min points per scan
+ * and how many points per ring
+ */
+ void _analyzeLiDARDevice();
+
+
+ /* [LiDAR analysis]
+ * A function to find maximum points and minimum points in a single scan, i.e. to
+ * find extrema within 32 rings
+ */
+ void _maxMinPtsInAScan(std::vector &t_point_count_table,
+ std::vector &t_max_min_table,
+ std::vector &t_ring_average_table,
+ const std::vector> &t_ordered_buff);
+
+
+ /* [LiDAR analysis]
+ * A function to calculate how many points are supposed to be on a cluster at 1
+ * meter away
+ */
+ void _pointsPerSquareMeterAtOneMeter();
+
+
+ /*
+ * A function to get a number of points on a given-distance tag or object
+ * from LiDAR analysis
+ */
+ int _areaPoints(const double &t_distance, const double &t_obj_width, const double &t_obj_height);
+
+
+
+ /* [LiDARTag detection]
+ * Given lidar pointcloud, this function performs
+ * lidartag detection and decode the corresponding id
+ */
+ pcl::PointCloud::Ptr
+ _lidarTagDetection(const std::vector> &t_ordered_buff,
+ std::vector &t_cluster_buff);
+
+
+ /* [Edge detection and clustering]
+ * A function to
+ * (1) calculate the depth gradient and the intensity gradient at a point of a pointcloud
+ * (2) group detected 'edge' into different group
+ */
+ void _gradientAndGroupEdges(const std::vector> &t_ordered_buff,
+ std::vector> &t_edge_buff,
+ std::vector &t_cluster_buff);
+
+
+ /* [Clustering-Linkage]
+ * A function to cluster a single edge point into a new cluster or an existing cluster
+ */
+ void _clusterClassifier(const LiDARPoints_t &point, std::vector &t_cluster_buff);
+
+
+ /* [Clustering-Update]
+ * A function update some information about a cluster if this point belongs to
+ * this cluster; if not belonging to this cluster then return and create a new
+ * one
+ */
+ void _updateCluster(const LiDARPoints_t &t_point,
+ ClusterFamily_t &t_old_cluster,
+ TestCluster_t *t_new_cluster);
+
+
+
+ /* [Clustering-Validation]
+ * A function to
+ * (1) remove invalid cluster based on the index is too far or not
+ * (2) fill in the points between index of edges
+ * (3) after filling, if the points are too less (based on the analyzed system
+ * and given distant of the cluster), then remove this cluster
+ * (4) Adaptive thresholding (Maximize and minimize intensity) by comparing
+ * with the average value
+ */
+ void _fillInCluster(const std::vector> &t_ordered_buff,
+ std::vector &t_cluster_buff);
+
+
+ /* [Clustering-Validation]
+ * A valid cluster, valid tag, the points from the original point cloud that belong to the cluster
+ * could be estimated from the LiDAR system
+ * Therefore, if the points on the tag is too less, which means it is not a valid
+ * tag where it might just a shadow of a valid tag
+ */
+ bool _clusterPointsCheck(ClusterFamily_t &t_cluster);
+
+
+ /* [Clustering-Validation] TODO:RENAME
+ * A function to
+ * (1) do adaptive thresholding (Maximize and minimize intensity) by comparing
+ * with the average value and
+ * (2) sort points with ring number and re-index with current cluster into
+ * tag_edges vector so as to do regression boundary lines
+ * (3) It will *remove* if linefitting fails
+ */
+ bool _adaptiveThresholding(ClusterFamily_t &t_cluster);
+
+
+ /* [Clustering-Validation]
+ * A function to fit 4 lines of a payload in a cluster by
+ * (1) finding the edges of the payload (how to find is stated below)
+ * (2) rejecting and removing the cluster if one of the line is too short
+ */
+ bool _detectPayloadBoundries(ClusterFamily_t &t_cluster);
+
+
+ /* [Payload extraction]
+ * A function to extract the payload points from a valid cluster.
+ * Let's say we have 10 points on the left boundary (line) of the tag and 9 points on the right boundary
+ * (line) of the tag.
+ * It is separated into two parts.
+ * TODO: should use medium instead of max points
+ * (i) Find the max points that we have on a ring in this cluster by
+ * exmaming the average points on the first 1/2 rings int((10+9)/4)
+ * (ii) For another half of the rings, we just find the start index and add the
+ * average number of points to the payload points
+ */
+ void _extractPayloadWOThreshold(ClusterFamily_t &t_cluster);
+
+
+
+
+ /* [Type transformation]
+ * A function to transform an eigen type of vector to pcl point type
+ */
+ void _eigenVectorToPointXYZRI(const Eigen::Vector4f &t_vector, PointXYZRI &t_point);
+
+
+ /* [Normal vector]
+ * A function to estimate the normal vector of a potential payload
+ */
+ Eigen::MatrixXf _estimateNormalVector(ClusterFamily_t &cluster);
+
+
+ /* [Pose: tag to robot]
+ * A function to publish pose of tag to the robot
+ */
+ void _tagToRobot(const int &t_cluster_id, const Eigen::Vector3f &t_normal_vec,
+ Homogeneous_t &t_pose,
+ tf::Transform &t_transform, const PointXYZRI &t_ave);
+
+
+ /* [Payload decoding]
+ * A function to decode payload with different means
+ * 0: Naive decoding
+ * 1: Weighted Gaussian
+ * 2: Deep learning
+ * 3: Gaussian Process
+ * 4: ?!
+ */
+ bool _decodPayload(ClusterFamily_t &t_cluster);
+
+
+ /* [Decoder]
+ * A function to determine a codeword on a payload using equal weight
+ * methods
+ */
+ void _getCodeNaive(std::string &t_code, pcl::PointCloud t_payload);
+
+
+ /* [Decoder]
+ * Decode using Weighted Gaussian weight
+ * return 0: normal
+ * return -1: not enough return
+ * return -2: fail corner detection
+ */
+ int _getCodeWeightedGaussian(std::string &Code, Homogeneous_t &t_pose,
+ int &t_payload_points,
+ const PointXYZRI &ave,
+ const pcl::PointCloud &t_payload,
+ const std::vector &t_payload_boundary_ptr);
+
+ /* [Decoder]
+ * Create hash table of chosen tag family
+ */
+ void _initDecoder();
+
+
+ /* [Decoder]
+ * Feed a code to test if initialized correctly
+ */
+ void _testInitDecoder();
+
+
+ /* [ros-visualization]
+ * A function to prepare for visualization results in rviz
+ */
+ void _clusterToPclVectorAndMarkerPublisher(const std::vector &t_cluster,
+ pcl::PointCloud::Ptr t_out_cluster,
+ pcl::PointCloud::Ptr t_out_payload,
+ visualization_msgs::MarkerArray &t_marker_array);
+
+
+ /* [Drawing]
+ * A function to draw lines in rviz
+ */
+ void _assignLine(visualization_msgs::Marker &marker,
+ visualization_msgs::MarkerArray t_mark_array,
+ const uint32_t shape, const std::string ns,
+ const double r, const double g, const double b,
+ const PointXYZRI t_point1, const PointXYZRI t_point2,
+ const int t_count);
+
+
+ /* [Drawing]
+ * A function to assign markers in rviz
+ */
+ void _assignMarker(visualization_msgs::Marker &t_marker, const uint32_t t_shape,
+ const std::string t_namespace,
+ const double r, const double g, const double b,
+ const PointXYZRI &t_point, const int t_count,
+ const double t_size, const std::string Text="");
+
+
+ /*****************************************************
+ * not used
+ *****************************************************/
+
+ // Clean up
+ void _freeUp(std::vector &ClusterBuff);
+ void _freeCluster(ClusterFamily_t &Cluster);
+ template
+ void _freeVec(Container& c);
+ void _freePCL(pcl::PointCloud &vec);
+ void _freeTagLineStruc(TagLines_t &TagEdges);
+ }; // GrizTag
+} // namespace
+#endif
diff --git a/include/tag16h5.h b/include/tag16h5.h
new file mode 100644
index 0000000..e4302a8
--- /dev/null
+++ b/include/tag16h5.h
@@ -0,0 +1,39 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+
+#ifndef _TAG16H5
+#define _TAG16H5
+
+#include "lidar_tag.h"
+
+BipedLab::GrizTagFamily_t *tag16h5_create();
+void tag16h5_destroy(BipedLab::GrizTagFamily_t *tf);
+#endif
diff --git a/include/tag49h14.h b/include/tag49h14.h
new file mode 100644
index 0000000..6c1fa29
--- /dev/null
+++ b/include/tag49h14.h
@@ -0,0 +1,38 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+#ifndef _TAG49H14
+#define _TAG49H14
+
+#include "lidar_tag.h"
+
+BipedLab::GrizTagFamily_t *tag49h14_create();
+void tag49h14_destroy(BipedLab::GrizTagFamily_t *tf);
+#endif
diff --git a/include/types.h b/include/types.h
new file mode 100644
index 0000000..64d1a53
--- /dev/null
+++ b/include/types.h
@@ -0,0 +1,246 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+namespace BipedLab {
+ typedef velodyne_pointcloud::PointXYZIR PointXYZRI;
+ typedef struct QuickDecodeEntry {
+ uint64_t rcode; // the queried code
+ uint16_t id; // the tag id (a small integer)
+ uint16_t hamming; // how many errors corrected?
+ uint16_t rotation; // number of rotations [0, 3]
+ }QuickDecodeEntry_t;
+
+ typedef struct QuickDecode {
+ int nentries;
+ QuickDecodeEntry_t *entries;
+ }QuickDecode_t;
+
+ typedef struct PayloadVoting{
+ PointXYZRI *p;
+ float weight;
+ int cell;
+ PointXYZRI centroid;
+ }PayloadVoting_t;
+
+
+ // velodyne_pointcloud::PointXYZIR operator+ (const PointXYZRI& p1, const PointXYZRI p2) {
+ // PointXYZRI tmp;
+ // tmp.x = p1.x + p2.x;
+ // tmp.y = p1.y + p2.y;
+ // tmp.z = p1.z + p2.z;
+ // tmp.intensity = p1.intensity + p2.intensity;
+ // return tmp;
+ // };
+ typedef struct MaxMin {
+ int min;
+ int average;
+ int max;
+ } MaxMin_t;
+
+ // Structure for LiDAR system
+ typedef struct LiDARSystem {
+ std::vector> point_count_table; // point per ring PointCountTable[Scan][ring]
+ std::vector max_min_table; // max min points in a scan
+ std::vector ring_average_table; // max, min, average points in a ring, examed through out a few seconds
+ double points_per_square_meter_at_one_meter; // TODO: only assume place the tag at dense-point area
+ double beam_per_vertical_radian;
+ double point_per_horizontal_radian;
+ } LiDARSystem_t;
+
+ // Struture for LiDAR PointCloud with index
+ typedef struct LiDARPoints {
+ PointXYZRI point;
+ int index;
+ double depth_gradient; // only take abs value due to uncertain direction
+ double intensity_gradient; // Also account for direction by knowing tag is white to black
+ double threshold_intensity;
+ } LiDARPoints_t;
+
+ typedef struct TagLines{
+ int upper_ring;
+ int lower_ring;
+ std::vector upper_line; // basically just a specific ring, just point to it should be fine
+ std::vector lower_line; // same above
+ std::vector left_line; // same
+ std::vector right_line; // same above
+
+
+ std::vector bottom_left; // basically just a specific ring, just point to it should be fine
+ std::vector bottom_right; // same above
+ std::vector top_left; // same
+ std::vector top_right; // same above
+ } TagLines_t;
+
+ typedef struct TagBoundaries{
+ int status; // 0 is up right, 1 is tilted
+ std::vector line_one; // basically just a specific ring, just point to it should be fine
+ std::vector line_two; // same above
+ std::vector line_three; // same
+ std::vector line_four; // same above
+ } TagBoundaries_t;
+
+ typedef struct Homogeneous{
+ // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ float roll;
+ float pitch;
+ float yaw;
+ Eigen::Matrix translation;
+ Eigen::Matrix rotation;
+ Eigen::Matrix homogeneous;
+ } Homogeneous_t;
+
+ typedef struct Grid{
+ float cx;
+ float cz;
+ float cy;
+ } Grid_t;
+
+ typedef struct ClusterFamily {
+ // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ int cluster_id;
+ int valid;
+ PointXYZRI top_most_point;
+ PointXYZRI bottom_most_point;
+
+ PointXYZRI front_most_point;
+ PointXYZRI back_most_point;
+
+ PointXYZRI right_most_point;
+ PointXYZRI left_most_point;
+
+ PointXYZRI average; // Average point
+ PointXYZRI max_intensity; // Maximux intensity point
+ PointXYZRI min_intensity; // Minimum intensity point
+ pcl::PointCloud data;
+
+ std::vector max_min_index_of_each_ring; // to fill in points between end points in this cluster
+ std::vector> ordered_points_ptr; // of the cluster (to find black margin of the tag)
+ std::vector accumulate_intensity_of_each_ring; // to find the upper/lower lines of the tag
+ TagLines_t tag_edges; // store line segment from points
+ TagBoundaries_t tag_boundaries;
+
+ std::vector payload_right_boundary_ptr; // of the cluster (to find black margin of the tag)
+ std::vector payload_left_boundary_ptr; // of the cluster (to find black margin of the tag)
+ std::vector payload_boundary_ptr; // of the cluster (to find black margin of the tag)
+
+ pcl::PointCloud payload; // payload points with boundary
+ int payload_without_boundary; // size of payload points without boundary
+ // Eigen::Vector3f NormalVector; // Normal vectors of the payload
+ Eigen::Matrix normal_vector;
+ QuickDecodeEntry_t entry;
+ Homogeneous_t pose;
+ tf::Transform transform;
+
+
+
+ /* VectorXf:
+ * point_on_line.x : the X coordinate of a point on the line
+ * point_on_line.y : the Y coordinate of a point on the line
+ * point_on_line.z : the Z coordinate of a point on the line
+ * line_direction.x : the X coordinate of a line's direction
+ * line_direction.y : the Y coordinate of a line's direction
+ * line_direction.z : the Z coordinate of a line's direction
+ */
+ std::vector line_coeff; // Upper, left, bottom, right line (count-clockwise)
+ } ClusterFamily_t;
+
+ typedef struct GrizTagFamily {
+ // How many codes are there in this tag family?
+ uint32_t ncodes;
+
+ // The codes in the family.
+ uint64_t *codes;
+
+ // how wide (in bit-sizes) is the black border? (usually 1)
+ uint32_t black_border;
+
+ // how many bits tall and wide is it? (e.g. 36bit tag ==> 6)
+ uint32_t d;
+
+ // minimum hamming distance between any two codes. (e.g. 36h11 => 11)
+ uint32_t h;
+
+ // a human-readable name, e.g., "tag36h11"
+ char *name;
+
+ // some detector implementations may preprocess codes in order to
+ // accelerate decoding. They put their data here. (Do not use the
+ // same apriltag_family instance in more than one implementation)
+ void *impl;
+ }GrizTagFamily_t;
+
+ typedef struct ClusterRemoval {
+ int removed_by_point_check;
+ int boundary_point_check;
+ int no_edge_check;
+ int minimum_return;
+ int decode_fail;
+ int decoder_not_return;
+ int decoder_fail_corner;
+ }ClusterRemoval_t;
+
+ typedef struct Statistics {
+ ClusterRemoval_t cluster_removal;
+ int original_cluster_size;
+ int remaining_cluster_size;
+ int point_cloud_size;
+ int edge_cloud_size;
+ }Statistics_t;
+
+ typedef struct Timing{
+ // in us
+ clock_t start_total_time;
+ clock_t start_computation_time;
+ clock_t timing;
+
+ double total_time;
+ double computation_time;
+ double edgingand_clustering_time;
+ double to_pcl_vector_time;
+ double point_check_time;
+ double line_fitting_time;
+ double payload_extraction_time;
+ double normal_vector_time;
+ double payload_decoding_time;
+ double tag_to_robot_time;
+ }Timing_t;
+
+ typedef struct TestCluster {
+ int flag;
+ ClusterFamily_t new_cluster;
+ }TestCluster_t;
+
+ typedef struct Debug{
+ std::vector point_check;
+ std::vector boundary_point;
+ std::vector no_edge;
+ std::vector extract_payload;
+ }Debug_t;
+} // namespace
diff --git a/include/utils.h b/include/utils.h
new file mode 100644
index 0000000..389b217
--- /dev/null
+++ b/include/utils.h
@@ -0,0 +1,171 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+#pragma once
+#ifndef UTILS_H
+#define UTILS_H
+
+#include
+#include // to use to_lower function
+#include // for variadic functions
+#include
+
+
+#include
+#include
+#include "lidar_tag.h"
+
+
+namespace BipedLab {
+
+ //typedef velodyne_pointcloud::PointXYZIR PointXYZRI;
+ namespace utils {
+ double spendTime(const std::clock_t &t_end, const std::clock_t &t_start);
+ std::string tranferToLowercase(std::string &t_data);
+ void pressEnterToContinue();
+ double deg2Rad(double t_degree);
+ double rad2Deg(double t_radian);
+ bool isRotationMatrix(Eigen::Matrix3f &t_R);
+ Eigen::Vector3f rotationMatrixToEulerAngles(Eigen::Matrix3f &t_R);
+
+
+ bool checkParameters(int t_n, ...);
+ void COUT(const velodyne_pointcloud::PointXYZIR& t_p);
+ bool compareIndex(LiDARPoints_t *A, LiDARPoints_t *B);
+ uint64_t bitShift(std::string const& t_value);
+
+
+
+ void normalizeByAve(std::vector &t_x, std::vector &t_y,
+ std::vector &t_z, std::vector &t_I,
+ const pcl::PointCloud payload);
+ void normalize(std::vector &t_x, std::vector &t_y,
+ std::vector &t_z, std::vector &t_I,
+ const pcl::PointCloud t_payload);
+
+ velodyne_pointcloud::PointXYZIR pointsAddDivide (
+ const velodyne_pointcloud::PointXYZIR& t_p1,
+ const velodyne_pointcloud::PointXYZIR& t_p2, float t_d=1);
+
+ velodyne_pointcloud::PointXYZIR vectorize (
+ const velodyne_pointcloud::PointXYZIR& t_p1,
+ const velodyne_pointcloud::PointXYZIR& t_p2);
+
+ float dot (const velodyne_pointcloud::PointXYZIR& t_p1,
+ const velodyne_pointcloud::PointXYZIR& t_p2);
+
+ float Norm (const velodyne_pointcloud::PointXYZIR& t_p);
+
+ // a function to determine the step of given two points
+ float getStep(const velodyne_pointcloud::PointXYZIR &t_p1,
+ const velodyne_pointcloud::PointXYZIR &t_p2, const int t_d);
+
+ void getProjection(const velodyne_pointcloud::PointXYZIR &t_p1,
+ const velodyne_pointcloud::PointXYZIR &t_p2,
+ const velodyne_pointcloud::PointXYZIR &t_p,
+ float &t_k, Eigen::Vector2f &t_v);
+
+ double MVN(const float &t_tag_size, const int &t_d,
+ const Eigen::Vector2f &t_X, const Eigen::Vector2f t_mean);
+
+ void assignCellIndex(const float &t_tag_size,
+ const Eigen::Matrix3f &t_R,
+ velodyne_pointcloud::PointXYZIR &t_p_reference,
+ const velodyne_pointcloud::PointXYZIR &t_average,
+ const int t_d, PayloadVoting_t &t_vote);
+
+ void sortPointsToGrid(std::vector> &t_grid,
+ std::vector &t_votes);
+
+ Eigen::Vector2f pointToLine(const velodyne_pointcloud::PointXYZIR &t_p1,
+ const velodyne_pointcloud::PointXYZIR &t_p2,
+ const velodyne_pointcloud::PointXYZIR &t_p);
+
+ void formGrid(Eigen::MatrixXf &t_vertices,
+ float t_x, float t_y, float t_z, float t_tag_size);
+
+ void fitGrid(Eigen::MatrixXf &t_vertices,
+ Eigen::Matrix3f &t_R,
+ const velodyne_pointcloud::PointXYZIR &t_p1,
+ const velodyne_pointcloud::PointXYZIR &t_p2,
+ const velodyne_pointcloud::PointXYZIR &t_p3,
+ const velodyne_pointcloud::PointXYZIR &t_p4);
+
+ float distance(
+ const velodyne_pointcloud::PointXYZIR &t_p1,
+ const velodyne_pointcloud::PointXYZIR &t_p2);
+ template
+ float getAngle (T a, U b);
+
+
+ int checkCorners(
+ const float t_tag_size,
+ const velodyne_pointcloud::PointXYZIR &t_p1,
+ const velodyne_pointcloud::PointXYZIR &t_p2,
+ const velodyne_pointcloud::PointXYZIR &t_p3,
+ const velodyne_pointcloud::PointXYZIR &t_p4);
+
+ velodyne_pointcloud::PointXYZIR toVelodyne(const Eigen::Vector3f &t_p);
+ Eigen::Vector3f toEigen(const velodyne_pointcloud::PointXYZIR &t_point);
+ void minus(velodyne_pointcloud::PointXYZIR &t_p1,
+ const velodyne_pointcloud::PointXYZIR &t_p2);
+
+ template
+ T blockMatrix(int t_n, ...);
+
+
+ // template
+ Eigen::Matrix4d poseToEigenMatrix(const geometry_msgs::Pose &t_pose);
+ // Eigen::Matrix4d poseToEigenMatrix(const T &pose);
+
+ template
+ Eigen::Matrix3d qToR(const T &t_pose);
+
+ Eigen::Matrix3d qToR(const Eigen::Vector3f &t_pose);
+
+ // q1q2 = q2q1q2^-1
+ Eigen::Matrix3d qMultiplication(const double &t_q1_w, const Eigen::Vector3f &t_q1,
+ const double &t_q2_w, const Eigen::Vector3f &t_q2);
+
+
+ template
+ bool goodNumber(T t_number){
+ if (std::isinf(t_number) || std::isnan(t_number))
+ return false;
+ else return true;
+ }
+
+
+
+ // std::ostream& operator<<(std::ostream& os, const velodyne_pointcloud::PointXYZIR& p);
+
+ }
+} // Bipedlab
+#endif
diff --git a/launch/LiDARTag.launch b/launch/LiDARTag.launch
new file mode 100644
index 0000000..c2d58b5
--- /dev/null
+++ b/launch/LiDARTag.launch
@@ -0,0 +1,76 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..b58fce2
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,74 @@
+
+
+ lidar_tag
+ 0.0.0
+ A package for lidar_tag
+ brucebot
+
+
+
+
+
+ BSD
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ tf
+ std_msgs
+ visualization_msgs
+ sensor_msgs
+ message_filters
+ apriltag_msgs
+ lidartag_msgs
+
+
+
+ roscpp
+ rospy
+ tf
+ std_msgs
+ visualization_msgs
+ sensor_msgs
+ message_filters
+ apriltag_msgs
+ lidartag_msgs
+
+
+ roslib
+ rosunit
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/apriltag_utils.cc b/src/apriltag_utils.cc
new file mode 100644
index 0000000..3f91315
--- /dev/null
+++ b/src/apriltag_utils.cc
@@ -0,0 +1,305 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+ All rights reserved.
+
+ This software was developed in the APRIL Robotics Lab under the
+ direction of Edwin Olson, ebolson@umich.edu. This software may be
+ available under alternative licensing terms; contact the address above.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+ 2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ IS" AND
+ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ IMPLIED
+ WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ LIABLE FOR
+ ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ DAMAGES
+ (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ THIS
+ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ The views and conclusions contained in the software and documentation are
+ those
+ of the authors and should not be interpreted as representing official
+ policies,
+ either expressed or implied, of the Regents of The University of
+ Michigan.
+ */
+
+#include "apriltag_utils.h"
+
+// Adapted/modified from apriltags code
+namespace BipedAprilLab{
+
+ static inline int imax(int a, int b) {
+ return (a > b) ? a : b;
+ }
+
+ /** if the bits in w were arranged in a d*d grid and that grid was
+ * rotated, what would the new bits in w be?
+ * The bits are organized like this (for d = 3):
+ *
+ * 8 7 6 2 5 8 0 1 2
+ * 5 4 3 ==> 1 4 7 ==> 3 4 5 (rotate90 applied twice)
+ * 2 1 0 0 3 6 6 7 8
+ **/
+
+ uint64_t rotate90(uint64_t w, uint32_t d) {
+ uint64_t wr = 0;
+
+ for (int32_t r = d-1; r >=0; r--) {
+ for (int32_t c = 0; c < d; c++) {
+ int32_t b = r + d*c;
+
+ wr = wr << 1;
+
+ if ((w & (((uint64_t) 1) << b))!=0)
+ wr |= 1;
+ }
+ }
+
+ return wr;
+ }
+
+ void QuickDecodeAdd(BipedLab::QuickDecode_t *qd, uint64_t code, int id, int hamming) {
+ uint32_t bucket = code % qd->nentries;
+
+ while (qd->entries[bucket].rcode != UINT64_MAX) {
+ bucket = (bucket + 1) % qd->nentries;
+ }
+
+ qd->entries[bucket].rcode = code;
+ qd->entries[bucket].id = id;
+ qd->entries[bucket].hamming = hamming;
+ }
+
+ void QuickDecodeInit(BipedLab::GrizTagFamily_t *family, int maxhamming){
+ assert(family->impl == NULL);
+ assert(family->ncodes < 65535);
+
+ printf("Requesting memory\n");
+ BipedLab::QuickDecode_t *qd = new BipedLab::QuickDecode_t;
+
+ int capacity = family->ncodes;
+
+ int nbits = family->d * family->d;
+
+ if (maxhamming >= 1)
+ capacity += family->ncodes * nbits;
+
+ if (maxhamming >= 2)
+ capacity += family->ncodes * nbits * (nbits-1);
+
+ if (maxhamming >= 3)
+ capacity += family->ncodes * nbits * (nbits-1) * (nbits-2);
+
+ if (maxhamming >= 4)
+ capacity += family->ncodes * nbits * (nbits-1) * (nbits-2) * (nbits-3);
+
+ if (maxhamming >= 5)
+ capacity += family->ncodes * nbits * (nbits-1) * (nbits-2) * (nbits-3) * (nbits-4);
+
+ if (maxhamming >= 6)
+ capacity += family->ncodes * nbits * (nbits-1) * (nbits-2) * (nbits-3) * (nbits-4) * (nbits-5);
+
+ if (maxhamming >= 7)
+ capacity += family->ncodes * nbits * (nbits-1) * (nbits-2) * (nbits-3) * (nbits-4) * (nbits-5) * (nbits-6);
+
+ qd->nentries = capacity * 3;
+
+ qd->entries = new BipedLab::QuickDecodeEntry_t[qd->nentries];
+ if (qd->entries == NULL) {
+ printf("apriltag_utils.c: failed to allocate hamming decode table. Reduce max hamming size.\n");
+ exit(-1);
+ }
+
+ printf("Adding codes..\n");
+ for (int i = 0; i < qd->nentries; i++)
+ qd->entries[i].rcode = UINT64_MAX;
+
+ for (int i = 0; i < family->ncodes; i++) {
+ uint64_t code = family->codes[i];
+
+ // add exact code (hamming = 0)
+ QuickDecodeAdd(qd, code, i, 0);
+
+ if (maxhamming >= 1) {
+ // add hamming 1
+ for (int j = 0; j < nbits; j++)
+ QuickDecodeAdd(qd, code ^ (1L << j), i, 1);
+ }
+
+ if (maxhamming >= 2) {
+ // add hamming 2
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ QuickDecodeAdd(qd, code ^ (1L << j) ^ (1L << k), i, 2);
+ }
+
+ if (maxhamming >= 3) {
+ // add hamming 3
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ for (int m = 0; m < k; m++)
+ QuickDecodeAdd(qd, code ^ (1L << j) ^ (1L << k) ^ (1L << m), i, 3);
+ }
+
+ if (maxhamming >= 4) {
+ // add hamming 3
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ for (int m = 0; m < k; m++)
+ for (int n = 0; n < m; n++)
+ QuickDecodeAdd(qd,
+ code ^ (1L << j) ^ (1L << k) ^ (1L << m) ^ (1L << n),
+ i, 4);
+ }
+
+ if (maxhamming >= 5) {
+ // add hamming 3
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ for (int m = 0; m < k; m++)
+ for (int n = 0; n < m; n++)
+ for (int o = 0; o < n; o++)
+ QuickDecodeAdd(qd,
+ code ^ (1L << j) ^ (1L << k) ^ (1L << m) ^ (1L << n) ^ (1L << o),
+ i, 5);
+ }
+
+ if (maxhamming >= 6) {
+ // add hamming 3
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ for (int m = 0; m < k; m++)
+ for (int n = 0; n < m; n++)
+ for (int o = 0; o < n; o++)
+ for (int p = 0; p < o; p++)
+ QuickDecodeAdd(qd,
+ code ^ (1L << j) ^ (1L << k) ^ (1L << m) ^ (1L << n) ^ (1L << o) ^ (1L << p),
+ i, 6);
+ }
+
+ if (maxhamming >= 7) {
+ // add hamming 3
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ for (int m = 0; m < k; m++)
+ for (int n = 0; n < m; n++)
+ for (int o = 0; o < n; o++)
+ for (int p = 0; p < o; p++)
+ for (int q = 0; q < p; q++)
+ QuickDecodeAdd(qd,
+ code ^ (1L << j) ^ (1L << k) ^ (1L << m) ^ (1L << n) ^ (1L << o) ^ (1L << p) ^ (1L << q),
+ i, 7);
+ }
+
+
+ if (maxhamming > 7) {
+ printf("apriltag_utils.c: maxhamming beyond 7 not supported\n");
+ }
+ }
+
+ // printf("done adding..\n");
+ family->impl = qd;
+
+ if (1) {
+ int longest_run = 0;
+ int run = 0;
+ int run_sum = 0;
+ int run_count = 0;
+
+ // This accounting code doesn't check the last possible run that
+ // occurs at the wrap-around. That's pretty insignificant.
+ for (int i = 0; i < qd->nentries; i++) {
+ if (qd->entries[i].rcode == UINT64_MAX) {
+ if (run > 0) {
+ run_sum += run;
+ run_count ++;
+ }
+ run = 0;
+ } else {
+ run ++;
+ longest_run = imax(longest_run, run);
+ }
+ }
+
+ printf("quick decode: longest run: %d, average run %.3f\n", longest_run, 1.0 * run_sum / run_count);
+ }
+ }
+
+
+ void QuickDecodeCodeword(BipedLab::GrizTagFamily_t *tf, uint64_t rcode,
+ BipedLab::QuickDecodeEntry_t *entry) {
+ BipedLab::QuickDecode_t *qd = (BipedLab::QuickDecode_t*) tf->impl;
+
+ for (int ridx = 0; ridx < 4; ridx++) {
+
+ for (int bucket = rcode % qd->nentries;
+ qd->entries[bucket].rcode != UINT64_MAX;
+ bucket = (bucket + 1) % qd->nentries) {
+ // std::cout << "bucket: " << bucket << std::endl;
+
+ if (qd->entries[bucket].rcode == rcode) {
+ *entry = qd->entries[bucket];
+ entry->rotation = ridx;
+ //printf("rotation: %i\n", entry->rotation);
+ return;
+ }
+ }
+
+ rcode = rotate90(rcode, tf->d);
+ }
+
+ entry->rcode = 0;
+ entry->id = 9999;
+ entry->hamming = 255;
+ entry->rotation = 0;
+ }
+}
+
diff --git a/src/ctags_file b/src/ctags_file
new file mode 100644
index 0000000..464b2f6
--- /dev/null
+++ b/src/ctags_file
@@ -0,0 +1,125 @@
+!_TAG_FILE_FORMAT 2 /extended format; --format=1 will not append ;" to lines/
+!_TAG_FILE_SORTED 1 /0=unsorted, 1=sorted, 2=foldcase/
+!_TAG_PROGRAM_AUTHOR Darren Hiebert /dhiebert@users.sourceforge.net/
+!_TAG_PROGRAM_NAME Exuberant Ctags //
+!_TAG_PROGRAM_URL http://ctags.sourceforge.net /official site/
+!_TAG_PROGRAM_VERSION 5.9~svn20110310 //
+AdaptiveThresholding_ griz_tag.cc /^ bool GrizTag::AdaptiveThresholding_(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+AnalyzeSystem_ griz_tag.cc /^ void GrizTag::AnalyzeSystem_(){$/;" f class:BipedLab::GrizTag
+AreaPoints griz_tag.cc /^ int GrizTag::AreaPoints(const double Distance, const double ObjWidth, const double ObjHeight){$/;" f class:BipedLab::GrizTag
+AssignLine griz_tag.cc /^ void GrizTag::AssignLine(visualization_msgs::Marker &Marker, visualization_msgs::MarkerArray MarkArray,$/;" f class:BipedLab::GrizTag
+AssignMarker griz_tag.cc /^ void GrizTag::AssignMarker(visualization_msgs::Marker &Marker, const uint32_t Shape, const string NameSpace,$/;" f class:BipedLab::GrizTag
+BipedAprilLab apriltag_utils.cc /^namespace BipedAprilLab{$/;" n file:
+BipedLab griz_tag.cc /^namespace BipedLab {$/;" n file:
+BipedLab utils.cc /^namespace BipedLab{$/;" n file:
+BitShift utils.cc /^ uint64_t BitShift(std::string const& value) {$/;" f namespace:BipedLab::utils
+BuffToPclVector griz_tag.cc /^ void GrizTag::BuffToPclVector(const std::vector> &EdgeBuff,$/;" f class:BipedLab::GrizTag
+COUT utils.cc /^ void COUT(const velodyne_pointcloud::PointXYZIR& p) {$/;" f namespace:BipedLab::utils
+ClusterClassifier_ griz_tag.cc /^ void GrizTag::ClusterClassifier_(const LiDARPoints_t &Point, vector &ClusterBuff){$/;" f class:BipedLab::GrizTag
+ClusterPointsCheck_ griz_tag.cc /^ bool GrizTag::ClusterPointsCheck_(ClusterFamily_t &Cluster){ $/;" f class:BipedLab::GrizTag
+ClusterToPclVectorAndMarkerPublisher_ griz_tag.cc /^ void GrizTag::ClusterToPclVectorAndMarkerPublisher_(const std::vector &Cluster,$/;" f class:BipedLab::GrizTag
+Codewords_ lib.cc /^ vector Codewords_;$/;" m class:LoadWeight::impl file:
+CompareIndex utils.cc /^ bool CompareIndex(LiDARPoints_t *A, LiDARPoints_t *B){$/;" f namespace:BipedLab::utils
+ComputeRANSAC_ griz_tag.cc /^ Eigen::VectorXf GrizTag::ComputeRANSAC_(std::vector &Line){ $/;" f class:BipedLab::GrizTag
+Decoder griz_tag.cc /^ void GrizTag::Decoder(){$/;" f class:BipedLab::GrizTag
+DecoderTest griz_tag.cc /^ void GrizTag::DecoderTest(){$/;" f class:BipedLab::GrizTag
+DegToRad utils.cc /^ double DegToRad(double Degree){$/;" f namespace:BipedLab::utils
+Dot utils.cc /^ float Dot$/;" f namespace:BipedLab::utils
+EdgeDetection_ griz_tag.cc /^ GrizTag::EdgeDetection_(const std::vector>& OrderedBuff, $/;" f class:BipedLab::GrizTag
+EigenVectorToPointXYZRI_ griz_tag.cc /^ void GrizTag::EigenVectorToPointXYZRI_(const Eigen::Vector4f &Vector, PointXYZRI &Point){$/;" f class:BipedLab::GrizTag
+ExtractPayload griz_tag.cc /^ void GrizTag::ExtractPayload(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+ExtractPayloadWOThreshold_ griz_tag.cc /^ void GrizTag::ExtractPayloadWOThreshold_(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+FillInCluster_ griz_tag.cc /^ void GrizTag::FillInCluster_(const std::vector>& OrderedBuff, $/;" f class:BipedLab::GrizTag
+FillInOrderedPc_ griz_tag.cc /^ void GrizTag::FillInOrderedPc_(const pcl::PointCloud::Ptr PclPointcloud, $/;" f class:BipedLab::GrizTag
+FitGrid utils.cc /^ void FitGrid(Eigen::MatrixXf &GridVertices,$/;" f namespace:BipedLab::utils
+FormGrid utils.cc /^ void FormGrid(Eigen::MatrixXf &vertices, $/;" f namespace:BipedLab::utils
+Forward lib.cc /^void LoadWeight::Forward(const float *x, const float *y, const float *z, $/;" f class:LoadWeight
+ForwardInternally lib.cc /^ void ForwardInternally(const float *x, const float *y, const float *z, const float *I, $/;" f class:LoadWeight::impl
+FreeCluster griz_tag.cc /^ void GrizTag::FreeCluster(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+FreePCL griz_tag.cc /^ void GrizTag::FreePCL(pcl::PointCloud &vec){$/;" f class:BipedLab::GrizTag
+FreeTagLineStruc griz_tag.cc /^ void GrizTag::FreeTagLineStruc(TagLines_t &TagEdges){$/;" f class:BipedLab::GrizTag
+FreeUp griz_tag.cc /^ void GrizTag::FreeUp(vector &ClusterBuff){$/;" f class:BipedLab::GrizTag
+FreeVec griz_tag.cc /^ void GrizTag::FreeVec(Container& c) { $/;" f class:BipedLab::GrizTag
+GetCode griz_tag.cc /^ bool GrizTag::GetCode(string &Code, const pcl::PointCloud Payload){$/;" f class:BipedLab::GrizTag
+GetCodeNaive griz_tag.cc /^ void GrizTag::GetCodeNaive(string &Code, pcl::PointCloud Payload){$/;" f class:BipedLab::GrizTag
+GetInnerOutputs lib.cc /^ float* GetInnerOutputs(){$/;" f class:LoadWeight::impl
+GetOrderBuff griz_tag.cc /^ std::vector> GrizTag::GetOrderBuff(){$/;" f class:BipedLab::GrizTag
+GetOutput lib.cc /^float* LoadWeight::GetOutput(){$/;" f class:LoadWeight
+GetParameters_ griz_tag.cc /^ void GrizTag::GetParameters_(){$/;" f class:BipedLab::GrizTag
+GetProjection utils.cc /^ void GetProjection(const velodyne_pointcloud::PointXYZIR &p1, $/;" f namespace:BipedLab::utils
+GetStep utils.cc /^ float GetStep(const velodyne_pointcloud::PointXYZIR &p1, const velodyne_pointcloud::PointXYZIR &p2, const int d){$/;" f namespace:BipedLab::utils
+GetWeightAssignGrid utils.cc /^ void GetWeightAssignGrid(const int d, PayloadVoting_t &Vote, float &SumWeight, $/;" f namespace:BipedLab::utils
+GotAllParameters utils.cc /^ bool GotAllParameters(int n, ...){$/;" f namespace:BipedLab::utils
+GradientAndGroupEdges_ griz_tag.cc /^ void GrizTag::GradientAndGroupEdges_(const std::vector>& OrderedBuff, $/;" f class:BipedLab::GrizTag
+GrizTag griz_tag.cc /^ GrizTag::GrizTag():$/;" f class:BipedLab::GrizTag
+GroupEdges_ griz_tag.cc /^ void GrizTag::GroupEdges_(std::vector> &EdgeBuff){$/;" f class:BipedLab::GrizTag
+LineFittingWOThreshold_ griz_tag.cc /^ bool GrizTag::LineFittingWOThreshold_(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+LineFitting_ griz_tag.cc /^ bool GrizTag::LineFitting_(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+LoadInternally lib.cc /^ void LoadInternally(const char *ModelPath, const char *CkptPath, const int TagFamily, float *result = nullptr) {$/;" f class:LoadWeight::impl
+LoadWeight lib.cc /^LoadWeight::LoadWeight(const char *model_path, const char *ckpt_path, const int TagFamily)$/;" f class:LoadWeight
+LoadWeight_ lib.cc /^int LoadWeight_(const char *ModelPath, const char *CheckpointPath, float *result) {$/;" f
+MAX_INTENSITY griz_tag.cc 35;" d file:
+MIN_INTENSITY griz_tag.cc 36;" d file:
+MVN utils.cc /^ double MVN(const Eigen::Vector2f &X, const Eigen::Vector2f Mean){$/;" f namespace:BipedLab::utils
+MaxMinPtsInAScan_ griz_tag.cc /^ void GrizTag::MaxMinPtsInAScan_(std::vector &PointCountTable, $/;" f class:BipedLab::GrizTag
+MergeCluster_ griz_tag.cc /^ void GrizTag::MergeCluster_(std::vector &ClusterBuff) {$/;" f class:BipedLab::GrizTag
+Norm utils.cc /^ float Norm $/;" f namespace:BipedLab::utils
+NormalEstimation griz_tag.cc /^ GrizTag::NormalEstimation(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+PayloadDecoder griz_tag.cc /^ void GrizTag::PayloadDecoder(ClusterFamily_t &Cluster){$/;" f class:BipedLab::GrizTag
+PointCloudCallBack_ griz_tag.cc /^ void GrizTag::PointCloudCallBack_(const sensor_msgs::PointCloud2ConstPtr &pc){$/;" f class:BipedLab::GrizTag
+PointXYZRI test.cc /^typedef velodyne_pointcloud::PointXYZIR PointXYZRI;$/;" t file:
+PointsPerSquareMeterAtOneMeter griz_tag.cc /^ void GrizTag::PointsPerSquareMeterAtOneMeter(){$/;" f class:BipedLab::GrizTag
+PressEnterToContinue utils.cc /^ void PressEnterToContinue() {$/;" f namespace:BipedLab::utils
+PublishPC_ griz_tag.cc /^ void GrizTag::PublishPC_(const pcl::PointCloud::Ptr SourcePC, $/;" f class:BipedLab::GrizTag
+QuickDecodeAdd apriltag_utils.cc /^ void QuickDecodeAdd(BipedLab::QuickDecode_t *qd, uint64_t code, int id, int hamming) {$/;" f namespace:BipedAprilLab
+QuickDecodeCodeword apriltag_utils.cc /^ void QuickDecodeCodeword(BipedLab::GrizTagFamily_t *tf, uint64_t rcode,$/;" f namespace:BipedAprilLab
+QuickDecodeInit apriltag_utils.cc /^ void QuickDecodeInit(BipedLab::GrizTagFamily_t *family, int maxhamming){$/;" f namespace:BipedAprilLab
+ROSToDecoder griz_tag.cc /^ void GrizTag::ROSToDecoder(std::vector &ClusterBuff){$/;" f class:BipedLab::GrizTag
+RadToDeg utils.cc /^ double RadToDeg(double Radian){$/;" f namespace:BipedLab::utils
+RefineCluster_ griz_tag.cc /^ void GrizTag::RefineCluster_(std::vector &ClusterBuff) { $/;" f class:BipedLab::GrizTag
+RosSpin_ griz_tag.cc /^ void GrizTag::RosSpin_(){$/;" f class:BipedLab::GrizTag
+SQRT2 griz_tag.cc 34;" d file:
+SortNormalizeWeight utils.cc /^ void SortNormalizeWeight(std::vector> &Grid, $/;" f namespace:BipedLab::utils
+Spin_ griz_tag.cc /^ void GrizTag::Spin_(){$/;" f class:BipedLab::GrizTag
+TagFamily_ lib.cc /^ int TagFamily_;$/;" m class:LoadWeight::impl file:
+TagToRobot griz_tag.cc /^ void GrizTag::TagToRobot(const int ClusterID, const Eigen::Vector3f NormalVec, Homogeneous_t &Pose, $/;" f class:BipedLab::GrizTag
+ToVelodyne utils.cc /^ velodyne_pointcloud::PointXYZIR ToVelodyne(const Eigen::Vector3f &p){$/;" f namespace:BipedLab::utils
+TranferToLowercase utils.cc /^ std::string TranferToLowercase(std::string &data){$/;" f namespace:BipedLab::utils
+UpdateCluster_ griz_tag.cc /^ void GrizTag::UpdateCluster_(const LiDARPoints_t &Point, ClusterFamily_t &OldCluster, TestCluster_t *NewCluster){$/;" f class:BipedLab::GrizTag
+Vectorize utils.cc /^ velodyne_pointcloud::PointXYZIR Vectorize$/;" f namespace:BipedLab::utils
+WaitForPC_ griz_tag.cc /^ void GrizTag::WaitForPC_(){$/;" f class:BipedLab::GrizTag
+WeightedDecode griz_tag.cc /^ bool GrizTag::WeightedDecode(string &Code, const PointXYZRI &Average, $/;" f class:BipedLab::GrizTag
+_Known_bound lib.cc /^ typedef void _Known_bound;$/;" t struct:std::_Unique_if file:
+_Single_object lib.cc /^ typedef unique_ptr _Single_object;$/;" t struct:std::_Unique_if file:
+_Unique_if lib.cc /^ template struct _Unique_if {$/;" s namespace:std file:
+_Unique_if lib.cc /^ template struct _Unique_if {$/;" s namespace:std file:
+_Unique_if lib.cc /^ template struct _Unique_if {$/;" s namespace:std file:
+_Unknown_bound lib.cc /^ typedef unique_ptr _Unknown_bound;$/;" t struct:std::_Unique_if file:
+imax apriltag_utils.cc /^ static inline int imax(int a, int b) {$/;" f namespace:BipedAprilLab
+impl lib.cc /^class LoadWeight::impl{$/;" c class:LoadWeight file:
+main example.cc /^int main()$/;" f
+main load_weight.cc /^int main() {$/;" f
+main main.cc /^int main(int argc, char **argv){$/;" f
+main normal.cc /^int main()$/;" f
+main random_sample_consensus.cpp /^main(int argc, char** argv)$/;" f
+main test.cc /^int main(int argc, char* argv[]) {$/;" f
+main test_.cc /^int main( int argc, char** argv )$/;" f
+main worker_pool.cc /^int main (int argc, char *argv[]) {$/;" f
+make_unique lib.cc /^ make_unique(Args&&... args) {$/;" f namespace:std
+make_unique lib.cc /^ make_unique(size_t n) {$/;" f namespace:std
+normalize test.cc /^void normalize(std::vector &x, std::vector &y, $/;" f
+normalize utils.cc /^ void normalize(std::vector &x, std::vector &y, $/;" f namespace:BipedLab::utils
+normalize_by_ave utils.cc /^ void normalize_by_ave(std::vector &x, std::vector &y, $/;" f namespace:BipedLab::utils
+options_ lib.cc /^ tensorflow::SessionOptions options_;$/;" m class:LoadWeight::impl file:
+outputs lib.cc /^ std::vector outputs;$/;" m class:LoadWeight::impl file:
+points_add_divide utils.cc /^ velodyne_pointcloud::PointXYZIR points_add_divide$/;" f namespace:BipedLab::utils
+rotate90 apriltag_utils.cc /^ uint64_t rotate90(uint64_t w, uint32_t d) {$/;" f namespace:BipedAprilLab
+session_ lib.cc /^ Session* session_;$/;" m class:LoadWeight::impl file:
+simpleVis random_sample_consensus.cpp /^simpleVis (pcl::PointCloud::ConstPtr cloud)$/;" f
+std lib.cc /^namespace std {$/;" n file:
+tag16h5_create tag16h5.cc /^BipedLab::GrizTagFamily_t *tag16h5_create()$/;" f
+tag16h5_destroy tag16h5.cc /^void tag16h5_destroy(BipedLab::GrizTagFamily_t *tf)$/;" f
+tag49h14_create tag49h14.cc /^BipedLab::GrizTagFamily_t *tag49h14_create()$/;" f
+tag49h14_destroy tag49h14.cc /^void tag49h14_destroy(BipedLab::GrizTagFamily_t *tf)$/;" f
+tensor_dict lib.cc /^typedef std::vector> tensor_dict;$/;" t file:
+utils utils.cc /^ namespace utils {$/;" n namespace:BipedLab file:
diff --git a/src/lidar_tag.cc b/src/lidar_tag.cc
new file mode 100644
index 0000000..b410c88
--- /dev/null
+++ b/src/lidar_tag.cc
@@ -0,0 +1,2790 @@
+/* Copyright (C) 2013-2020, The Regents of The University of Michigan.
+ * All rights reserved.
+ * This software was developed in the Biped Lab (https://www.biped.solutions/)
+ * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
+ * be available under alternative licensing terms; contact the address above.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * The views and conclusions contained in the software and documentation are those
+ * of the authors and should not be interpreted as representing official policies,
+ * either expressed or implied, of the Regents of The University of Michigan.
+ *
+ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
+ * WEBSITE: https://www.brucerobot.com/
+ */
+
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include // SVD
+#include // matrix exponential
+#include // tensor output
+
+#include // package
+#include "lidar_tag.h"
+#include "apriltag_utils.h"
+#include "utils.h"
+#include "tag49h14.h"
+#include "tag16h5.h"
+
+#include
+#include // std::sort
+#include /* sqrt, pow(a,b) */
+#include /* clock_t, clock, CLOCKS_PER_SEC */
+#include /* srand, rand */
+#include // log files
+
+
+/* CONSTANT */
+#define SQRT2 1.41421356237
+#define MAX_INTENSITY 255
+#define MIN_INTENSITY 0
+
+using namespace std;
+
+namespace BipedLab {
+ LiDARTag::LiDARTag():
+ _nh("~"),
+ _point_cloud_received(0),
+ _pub_frame("velodyne"), // what frame of the published pointcloud should be
+ _stop(0) // Just a switch for exiting this program while using valgrind
+ {
+ LiDARTag::_getParameters();
+ cout << "\033[1;32m\n\n===== loading tag family ===== \033[0m\n";
+ LiDARTag::_initDecoder();
+
+ if (_decode_method!=0 && _decode_method!=1){
+ ROS_ERROR("Please use 0 or 1 for decode_method in the launch file");
+ ROS_INFO_STREAM("currently using: "<< _decode_method);
+ }
+ cout << "\033[1;32m=========================== \033[0m\n";
+ cout << "\033[1;32m=========================== \033[0m\n";
+
+ ROS_INFO("ALL INITIALIZED!");
+ _LiDAR1_sub = _nh.subscribe(_pointcloud_topic, 50,
+ &LiDARTag::_pointCloudCallback, this);
+ _edge_pub = _nh.advertise("EdgedPC", 10);
+ _cluster_pub = _nh.advertise("DetectedPC", 10);
+ _payload_pub = _nh.advertise("Payload", 10);
+ _boundary_marker_pub = _nh.advertise( "BoundaryMarker", 10);
+ _cluster_marker_pub = _nh.advertise("ClusterMarker", 10);
+ _payload_marker_pub = _nh.advertise("PayloadEdges", 10);
+ _payload_grid_pub = _nh.advertise("Grid", 10);
+ _payload_grid_line_pub = _nh.advertise("GridLine", 10);
+ _lidartag_pose_pub = _nh.advertise("LiDARTagPose", 1);
+
+ boost::thread RosSpin(&LiDARTag::_rosSpin, this); // put ros spin into a background thread
+
+ ROS_INFO("Waiting for pointcloud data");
+ LiDARTag::_waitForPC();
+
+ // Exam the minimum distance of each point in a ring
+ ROS_INFO("Analyzing system");
+ LiDARTag::_analyzeLiDARDevice();
+ boost::thread ExtractionSpin(&LiDARTag::_mainLoop, this);
+ ExtractionSpin.join();
+ RosSpin.join();
+ }
+
+
+ /*
+ * A function to get all parameters from a roslaunch
+ * if not get all parameters then it will use hard-coded parameters
+ */
+ void LiDARTag::_getParameters(){
+ bool GotSleepToDisplay = ros::param::get("sleep_to_display", _sleep_to_display);
+ bool GotSleepTimeForVis = ros::param::get("sleep_time_for_visulization", _sleep_time_for_vis);
+ bool GotValgrindCheck = ros::param::get("valgrind_check", _valgrind_check);
+ bool GotFakeTag = ros::param::get("fake_data", _fake_tag);
+
+ bool GotCSV = ros::param::get("write_csv", _write_CSV);
+ bool GotDecodeId = ros::param::get("decode_id", _id_decoding);
+ bool GotAssignId = ros::param::get("assign_id", _assign_id);
+
+ bool GotAdaptiveThresholding = ros::param::get("adaptive_thresholding", _adaptive_thresholding);
+ bool GotCollectData = ros::param::get("collect_data", _collect_dataset);
+
+ bool GotLidarTopic = ros::param::get("pointcloud_topic", _pointcloud_topic);
+ bool GotBeamNum = ros::param::get("beam_number", _beam_num);
+ bool GotSize = ros::param::get("tag_size", _payload_size);
+
+ bool GotTagFamily = ros::param::get("tag_family", _tag_family);
+ bool GotTagHamming = ros::param::get("tag_hamming_distance", _tag_hamming_distance);
+ bool GotMaxDecodeHamming = ros::param::get("max_decode_hamming", _max_decode_hamming);
+ bool GotBlackBorder = ros::param::get("black_border", _black_border);
+
+ bool GotDistanceBound = ros::param::get("distance_bound", _distance_bound);
+ bool GotIntensityBound = ros::param::get("intensity_bound", _intensity_threshold);
+ bool GotDepthBound = ros::param::get("depth_bound", _depth_threshold);
+ bool GotFineClusterThreshold = ros::param::get("fine_cluster_threshold", _fine_cluster_threshold);
+ bool GotVerticalFOV = ros::param::get("vertical_fov", _vertical_fov);
+ bool GotFillInGapThreshold = ros::param::get("fill_in_gap_threshold", _filling_gap_max_index);
+ bool GotFillInMaxPointsThreshold = ros::param::get("fill_in_max_points_threshold", _filling_max_points_threshold);
+ bool GotPointsThresholdFactor = ros::param::get("points_threshold_factor", _points_threshold_factor);
+ bool GotLineIntensityBound = ros::param::get("line_intensity_bound", _line_intensity_bound);
+ bool GotPayloadIntensityThreshold = ros::param::get("payload_intensity_threshold", _payload_intensity_threshold);
+
+ bool GotLatestModel = ros::param::get("latest_model", _latest_model);
+ bool GotWeightPath = ros::param::get("weight_path", _weight_path);
+
+ bool GotMaxPointsOnPayload = ros::param::get("max_points_on_payload", _max_point_on_payload);
+ bool GotXYZRI = ros::param::get("xyzri", _XYZRI);
+ bool GotMinPerGrid = ros::param::get("min_retrun_per_grid", _min_returns_per_grid);
+ bool GotDecodeMethod = ros::param::get("decode_method", _decode_method);
+ bool GotGridViz = ros::param::get("grid_viz", _grid_viz);
+
+ bool Pass = utils::checkParameters(33, GotFakeTag, GotLidarTopic, GotBeamNum,
+ GotDecodeId, GotAssignId, GotCSV,
+ GotDistanceBound, GotIntensityBound, GotDepthBound,
+ GotSize, GotTagFamily, GotTagHamming, GotMaxDecodeHamming,
+ GotFineClusterThreshold, GotVerticalFOV,
+ GotFillInGapThreshold, GotFillInMaxPointsThreshold,
+ GotPointsThresholdFactor, GotLineIntensityBound,
+ GotAdaptiveThresholding, GotCollectData, GotSleepToDisplay,
+ GotSleepTimeForVis, GotValgrindCheck,
+ GotLatestModel, GotWeightPath,
+ GotMaxPointsOnPayload, GotXYZRI, GotMinPerGrid, GotDecodeMethod,
+ GotGridViz);
+
+ if (!Pass){
+ // TODO: check compleness
+ cout << "\033[1;32m=========================== \033[0m\n";
+ cout << "use hard-coded parameters\n";
+ cout << "\033[1;32m=========================== \033[0m\n";
+
+ // Default value
+ _pointcloud_topic = "/velodyne_points";
+ _beam_num = 32;
+ _distance_bound = 7; // for edge gradient
+ _intensity_threshold = 2; // for edge gradient
+ _depth_threshold = 0.5; // for edge gradient
+ _payload_intensity_threshold = 30;
+ _payload_size = 0.15;
+
+ _tag_family = 16;
+ _tag_hamming_distance = 5;
+ _max_decode_hamming = 2;
+
+ _fine_cluster_threshold = 20; // if the points in a cluster is small than this, it'd get dropped
+ _vertical_fov = 40;
+
+ // When fill in the cluster, if it the index is too far away then drop it
+ // TODO:Need a beteer way of doing it!
+ _filling_gap_max_index = 200;
+ _filling_max_points_threshold = 4500;
+
+ _line_intensity_bound = 1000; // To determine the payload edge
+
+ // if the points on a "valid" tag is less than this factor, then remove
+ // it (the higher, the looser)
+ _points_threshold_factor = 1.3;
+ _adaptive_thresholding = 0;
+ _collect_dataset = 1;
+
+ _sleep_to_display = 1;
+ _sleep_time_for_vis = 0.05;
+ _valgrind_check = 0;
+ _black_border = 2;
+ _fake_tag = 0;
+
+ _latest_model = "-337931";
+ _weight_path = "/weight/";
+ _max_point_on_payload = 150;
+ _XYZRI = 4 ;
+ _min_returns_per_grid = 3;
+ _decode_method = 2;
+ _grid_viz = 1;
+ }
+ else{
+ cout << "\033[1;32m=========================== \033[0m\n";
+ cout << "use parameters from the launch file\n";
+ cout << "\033[1;32m=========================== \033[0m\n";
+ }
+
+ _threshold = _payload_size/4; // point association threshold (which cluster the point belongs to?)
+ _RANSAC_threshold = _payload_size/10;
+
+ ROS_INFO("Subscribe to %s\n", _pointcloud_topic.c_str());
+ ROS_INFO("Use %i-beam LiDAR\n", _beam_num);
+ ROS_INFO("_intensity_threshold: %f \n", _intensity_threshold);
+ ROS_INFO("_depth_threshold: %f \n", _depth_threshold);
+ ROS_INFO("_payload_size: %f \n", _payload_size);
+ ROS_INFO("_vertical_fov: %i \n", _vertical_fov);
+ ROS_INFO("_fine_cluster_threshold: %i \n", _fine_cluster_threshold);
+ ROS_INFO("_filling_gap_max_index: %i \n", _filling_gap_max_index);
+ ROS_INFO("_filling_max_points_threshold: %i \n", _filling_max_points_threshold);
+ ROS_INFO("_points_threshold_factor: %f \n", _points_threshold_factor);
+ ROS_INFO("_adaptive_thresholding: %i \n", _adaptive_thresholding);
+ ROS_INFO("_collect_dataset: %i \n", _collect_dataset);
+ ROS_INFO("_decode_method: %i \n", _decode_method);
+ ROS_INFO("Threshold_: %f \n", _threshold);
+ ROS_INFO("_RANSAC_threshold: %f \n", _RANSAC_threshold);
+
+ usleep(2e6);
+ }
+
+
+ /*
+ * Main loop
+ */
+ void LiDARTag::_mainLoop(){
+ ROS_INFO("Start edge extraction");
+ //ros::Rate r(10); // 10 hz
+ ros::Duration duration(_sleep_time_for_vis);
+ int Count = 0; // just to caculate average speed
+ clock_t StartAve = clock();
+ pcl::PointCloud::Ptr ClusterPC(new pcl::PointCloud);
+ pcl::PointCloud::Ptr PayloadPC(new pcl::PointCloud);
+ ClusterPC->reserve(_point_cloud_size);
+
+ // XXX Tunalbe
+ PayloadPC->reserve(_point_cloud_size);
+ int valgrind_check = 0;
+ while (ros::ok()) {
+ _lidartag_pose_array.detections.clear();
+ _timing = {clock(), clock(), clock(), 0,0,0,0,0,0,0,0,0};
+
+ // Try to take a pointcloud from the buffer
+ std::vector> OrderedBuff = LiDARTag::_getOrderBuff();
+ if (OrderedBuff.empty()){
+ continue;
+ }
+
+ // A vector of clusters
+ vector ClusterBuff;
+
+ pcl::PointCloud::Ptr ExtractedEdgesPC = LiDARTag::_lidarTagDetection(OrderedBuff,
+ ClusterBuff);
+ ClusterPC->clear();
+ PayloadPC->clear();
+
+ // Prepare results for rviz
+ visualization_msgs::MarkerArray ClusterMarkers;
+ LiDARTag::_clusterToPclVectorAndMarkerPublisher(ClusterBuff, ClusterPC, PayloadPC, ClusterMarkers);
+
+ // publish lidartag poses
+ _lidartag_pose_pub.publish(_lidartag_pose_array);
+
+ // publish results for rviz
+ LiDARTag::_publishPC(ExtractedEdgesPC, _pub_frame, string("edge"));
+ LiDARTag::_publishPC(ClusterPC, _pub_frame, string("Cluster"));
+ if (_collect_dataset){
+ if (_result_statistics.remaining_cluster_size==1)
+ LiDARTag::_publishPC(PayloadPC, _pub_frame, string("Payload"));
+ else if (_result_statistics.remaining_cluster_size>1)
+ cout << "More than one!! " << endl;
+ else
+ cout << "Zero!! " << endl;
+ }
+ else
+ LiDARTag::_publishPC(PayloadPC, _pub_frame, string("Payload"));
+
+ //exit(0);
+ if (_sleep_to_display) duration.sleep();
+ Count++;
+ _timing.total_time = utils::spendTime(clock(), _timing.start_total_time);
+ ROS_INFO_STREAM("Hz (total): " << 1e3/_timing.total_time);
+ cout << "\033[1;31m====================================== \033[0m\n";
+ if (_valgrind_check){
+ valgrind_check++;
+ if (valgrind_check > 0) {
+ _stop = 1;
+ break;
+ }
+ }
+ }
+ }
+
+
+ /*
+ * A function to get pcl OrderedBuff from a ros sensor-msgs form of pointcould queue
+ * */
+ std::vector> LiDARTag::_getOrderBuff(){
+ _point_cloud1_queue_lock.lock();
+ if (_point_cloud1_queue.size()==0) {
+ _point_cloud1_queue_lock.unlock();
+ //ros::spinOnce();
+ //cout << "Pointcloud queue is empty" << endl;
+ //cout << "size: " << Empty.size() << endl;
+ vector> Empty;
+ return Empty;
+ }
+ ROS_INFO_STREAM("Queue size: " << _point_cloud1_queue.size());
+
+ sensor_msgs::PointCloud2ConstPtr msg = _point_cloud1_queue.front();
+ _current_scan_time = msg->header.stamp;
+
+ // Convert to sensor_msg to pcl type
+ pcl::PointCloud::Ptr PclPointcloud(new pcl::PointCloud);
+ pcl::fromROSMsg(*msg, *PclPointcloud);
+ _point_cloud1_queue.pop();
+ _point_cloud1_queue_lock.unlock();
+
+ std::vector> OrderedBuff(_beam_num);
+
+ // Ordered pointcloud with respect to its own ring number
+ _fillInOrderedPC(PclPointcloud, OrderedBuff);
+ _point_cloud_size = PclPointcloud->size();
+
+ return OrderedBuff;
+ }
+
+
+ /*
+ * A function to get a LiDAR system parameters such as max, min points per scan
+ * and how many points per ring
+ * The data format is:
+ *
+ * (A) PointCountTable:
+ * PointCountTable[Scan][Ring]
+ * -----------------------------------------
+ * 1 2 3 4 5 6 7 ... scan
+ * -----------------------------------------
+ * 0 17xx ...
+ * -----------------------------------------
+ * 1 16xx ...
+ * -----------------------------------------
+ * 2
+ * .
+ * .
+ * .
+ * 31
+ * ring
+ *
+ *
+ * (B) MaxMinTable:
+ * ----------------------------------------
+ * 1 2 3 .... scan
+ * Max Max Max
+ * Min Min Min
+ * ---------------------------------------
+ */
+ void LiDARTag::_analyzeLiDARDevice(){
+ clock_t begin = clock();
+ int NumberOfScan = 0;
+ _LiDAR_system.point_count_table.resize(50);
+ _LiDAR_system.ring_average_table.reserve(_beam_num);
+
+ // Initialize the table
+ MaxMin_t max_min{100000, -1, -1}; // min, ave, max
+
+ for (int j=0; j<_beam_num; ++j){
+ _LiDAR_system.ring_average_table.push_back(max_min);
+ }
+
+ // Calulate for each scan with a few seconds
+ while (ros::ok()){
+ std::vector> OrderedBuff = LiDARTag::_getOrderBuff();
+ if (OrderedBuff.empty()){
+ continue;
+ }
+ LiDARTag::_maxMinPtsInAScan(_LiDAR_system.point_count_table[NumberOfScan],
+ _LiDAR_system.max_min_table,
+ _LiDAR_system.ring_average_table,
+ OrderedBuff);
+ NumberOfScan++;
+ clock_t end = clock();
+ if (((double) (end-begin)/ CLOCKS_PER_SEC) > 3){
+ break;
+ }
+ }
+ for (auto i=_LiDAR_system.ring_average_table.begin(); i!=_LiDAR_system.ring_average_table.end(); ++i){
+ (*i).average /= NumberOfScan;
+ }
+
+ LiDARTag::_pointsPerSquareMeterAtOneMeter();
+
+
+
+
+ // Check values of pointtable and maxmintable
+ // int k = 0;
+ // for (auto i=_LiDAR_system.PointCountTable.begin(); i!=_LiDAR_system.PointCountTable.end(); ++i, ++k){
+ // cout << "Vector[" << k << "]:" << endl;
+ // for (auto j=(*i).begin(); j!=(*i).end(); ++j){
+ // cout << "points: " << *j << endl;
+ // }
+ // }
+
+ // k=0;
+ // for (auto i=_LiDAR_system.MaxMinTable.begin(); i!=_LiDAR_system.MaxMinTable.end(); ++i, ++k){
+ // cout << "At scan [" << k << "]" << endl;
+ // cout << "Max: " << (*i).Max << endl;
+ // cout << "Min: " << (*i).Min << endl;
+ // }
+
+ // k=0;
+ // for (auto i=_LiDAR_system.ring_average_table.begin(); i!=_LiDAR_system.ring_average_table.end(); ++i, ++k){
+ // cout << "At ring [" << k << "]" << endl;
+ // cout << "Max: " << (*i).Max << endl;
+ // cout << "Ave: " << (*i).average << endl;
+ // cout << "Min: " << (*i).Min << endl;
+ // }
+ // exit(0);
+ }
+
+
+ /*
+ * A function to calculate how many points are supposed to be on a cluster at 1
+ * meter away
+ */
+ void LiDARTag::_pointsPerSquareMeterAtOneMeter(){
+ double system_average;
+
+ for (auto i=_LiDAR_system.ring_average_table.begin();
+ i!=_LiDAR_system.ring_average_table.end(); ++i){
+ system_average += (*i).average;
+ }
+ system_average /= _LiDAR_system.ring_average_table.size();
+ _LiDAR_system.beam_per_vertical_radian = _beam_num / utils::deg2Rad(_vertical_fov);
+ _LiDAR_system.point_per_horizontal_radian = system_average / utils::deg2Rad(360);
+ }
+
+
+ /*
+ * A function to get a number of points on a given-distance tag or object
+ */
+ int LiDARTag::_areaPoints(const double &Distance, const double &ObjWidth, const double &ObjHeight){
+ double WAngle = ObjWidth * (1 + SQRT2) / abs(Distance);
+
+ if (WAngle>=1) return (int) 1e6; // return big number to reject the cluster
+
+ double HAngle = ObjHeight * (1 + SQRT2) / abs(Distance);
+ if (HAngle>=1) return (int) 1e6; // return big number to reject the cluster
+
+ double HorizontalAngle = asin(WAngle); // in radian
+ double VerticalAngle = asin(HAngle); // in radian
+ int NumOfVerticalRing = floor(VerticalAngle * _LiDAR_system.beam_per_vertical_radian);
+ int NumOfHorizontalPoints = floor(HorizontalAngle * _LiDAR_system.point_per_horizontal_radian);
+
+ // use 3 instead of 2 becasue of we assume the tag would be put in the dense
+ // region of LiDAR (LiDAR is denser in the middle)
+ int Area = floor(3 * (NumOfVerticalRing * NumOfHorizontalPoints) / (1 + SQRT2));
+
+ //cout << "distance: " << abs(Distance) << endl;
+ //cout << "HorizontalAngle: " << HorizontalAngle << endl;
+ //cout << "VerticalAngle: " << VerticalAngle << endl;
+
+ //cout << "NumOfVerticalRing: " << NumOfVerticalRing << endl;
+ //cout << "NumOfHorizontalPoints: " << NumOfHorizontalPoints << endl;
+ //cout << "Area: " << Area << endl;
+
+ return Area;
+ }
+
+
+ /*
+ * A function to find maximum points and minimum points in a single scan, i.e. to
+ * find extrema within 32 rings
+ */
+ void LiDARTag::_maxMinPtsInAScan(std::vector &PointCountTable,
+ std::vector &MaxMinTable,
+ std::vector &RingAverageTable,
+ const std::vector>& OrderedBuff){
+
+ // every scan should have different largest/ smallest numbers
+ int Largest = -1;
+ int Smallest = 100000;
+ MaxMin_t max_min;
+
+ int i = 0;
+ for (auto Ring=OrderedBuff.begin(); Ring!=OrderedBuff.end(); ++Ring){
+ int RingSize = (*Ring).size();
+ PointCountTable.push_back(RingSize);
+
+ // first time (we had initialized with -1)
+ if (RingAverageTable[i].average<0) {
+ RingAverageTable[i].average = RingSize;
+ }
+ else{
+ RingAverageTable[i].average = (RingAverageTable[i].average + RingSize);
+ }
+
+ if (RingAverageTable[i].maxRingSize){
+ RingAverageTable[i].min = RingSize;
+ }
+
+ if (RingSize > Largest) {
+ Largest = RingSize;
+ max_min.max = RingSize;
+ }
+
+ if (RingSize < Smallest) {
+ Smallest = RingSize;
+ max_min.min = RingSize;
+ }
+ i++;
+ }
+ MaxMinTable.push_back(max_min);
+ }
+
+
+ /*
+ * A function to transfer pcl msgs to ros msgs and then publish
+ * WhichPublisher should be a string of "organized" or "original" regardless
+ * lowercase and uppercase
+ */
+ void LiDARTag::_publishPC(const pcl::PointCloud::Ptr &SourcePC,
+ const std::string &Frame, string WhichPublisher){
+ utils::tranferToLowercase(WhichPublisher); // check letter cases
+ sensor_msgs::PointCloud2 PCsWaitedToPub;
+ pcl::toROSMsg(*SourcePC, PCsWaitedToPub);
+ PCsWaitedToPub.header.frame_id = Frame;
+
+ try {
+ if (WhichPublisher=="edge") _edge_pub.publish(PCsWaitedToPub);
+ else if (WhichPublisher=="original") _original_pc_pub.publish(PCsWaitedToPub);
+ else if (WhichPublisher=="cluster") _cluster_pub.publish(PCsWaitedToPub);
+ else if (WhichPublisher=="payload") _payload_pub.publish(PCsWaitedToPub);
+ else {
+ throw "No such Publisher exists";
+ }
+ }
+ catch (const char* msg){
+ cout << "\033[1;31m========================= \033[0m\n";
+ cerr << msg << endl;
+ cout << "\033[1;31m========================= \033[0m\n";
+ exit(-1);
+ }
+ }
+
+
+ /* [basic ros]
+ * A function to push the received pointcloud into a queue in the class
+ */
+ void LiDARTag::_pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &pc){
+ _point_cloud_received = 1; // flag to make sure it receives a pointcloud at the very begining of the program
+ _point_cloud_header = pc->header;
+ boost::mutex::scoped_lock(_point_cloud1_queue_lock);
+ _point_cloud1_queue.push(pc);
+ }
+
+
+ /*
+ * A function to make sure the program has received at least one pointcloud at
+ * the very start of this program
+ */
+ inline
+ void LiDARTag::_waitForPC(){
+ while (ros::ok()){
+ if (_point_cloud_received) {
+ ROS_INFO("Got pointcloud data");
+ return;
+ }
+ ros::spinOnce();
+ }
+ }
+
+
+ /*
+ * A function to slice the Veloydyne full points to sliced pointed based on ring
+ * number
+ * */
+ inline
+ void LiDARTag::_fillInOrderedPC(const pcl::PointCloud::Ptr &PclPointcloud,
+ std::vector>& OrderedBuff) {
+ LiDARPoints_t LiDARPoint;
+ int index [_beam_num] = {0};
+ for (auto && p : *PclPointcloud) {
+ LiDARPoint.point = p;
+ LiDARPoint.index = index[p.ring];
+ index[p.ring] += 1;
+ OrderedBuff[p.ring].push_back(LiDARPoint);
+ }
+ }
+
+
+ /*
+ * Main function
+ */
+ pcl::PointCloud::Ptr
+ LiDARTag::_lidarTagDetection(const std::vector>& OrderedBuff,
+ std::vector &ClusterBuff){
+
+ _timing.start_computation_time = clock();
+ pcl::PointCloud::Ptr Out(new pcl::PointCloud);
+ Out->reserve(_point_cloud_size);
+ std::vector> EdgeBuff(_beam_num); // Buff to store all detected edges
+
+ // calculate gradient for depth and intensity as well as group them into diff groups
+ _result_statistics = {{0, 0, 0, 0, 0, 0}, 0, 0, 0, 0};
+ _timing.timing = clock();
+ LiDARTag::_gradientAndGroupEdges(OrderedBuff, EdgeBuff, ClusterBuff);
+ _timing.edgingand_clustering_time = utils::spendTime(clock(), _timing.timing);
+
+ _timing.timing = clock();
+ // transform from a vector of vector (EdgeBuff) into a pcl vector (out)
+ boost::thread BuffToPclVectorThread(&LiDARTag::_buffToPclVector, this, boost::ref(EdgeBuff), Out);
+ _timing.to_pcl_vector_time = utils::spendTime(clock(), _timing.timing);
+
+
+ LiDARTag::_fillInCluster(OrderedBuff, ClusterBuff);
+ BuffToPclVectorThread.join();
+ _result_statistics.point_cloud_size = _point_cloud_size;
+ _result_statistics.edge_cloud_size = Out->size();
+
+ _timing.computation_time = utils::spendTime(clock(), _timing.start_computation_time);
+ ROS_DEBUG_STREAM("--------------- summary ---------------");
+ ROS_DEBUG_STREAM("Original cloud size: " << _result_statistics.point_cloud_size);
+ ROS_DEBUG_STREAM("--Edge cloud size: " << _result_statistics.edge_cloud_size);
+ ROS_DEBUG_STREAM("Original cluster: " << _result_statistics.original_cluster_size);
+ ROS_DEBUG_STREAM("--Point check Removed: " << _result_statistics.cluster_removal.removed_by_point_check);
+ ROS_DEBUG_STREAM("--Minimum return removed: " << _result_statistics.cluster_removal.minimum_return);
+ ROS_DEBUG_STREAM("--Boundary point Removed: " << _result_statistics.cluster_removal.boundary_point_check);
+ ROS_DEBUG_STREAM("--No Edge Removed: " << _result_statistics.cluster_removal.no_edge_check);
+ ROS_DEBUG_STREAM("--Not Enough for decode Removed: " << _result_statistics.cluster_removal.decoder_not_return);
+ ROS_DEBUG_STREAM("--Corners Removed: " << _result_statistics.cluster_removal.decoder_fail_corner);
+ ROS_DEBUG_STREAM("--Decode-Fail Removed: " << _result_statistics.cluster_removal.decode_fail);
+ ROS_DEBUG_STREAM("--Remaining Cluster: " << _result_statistics.remaining_cluster_size);
+ ROS_DEBUG_STREAM("computation_time Hz: " << 1/_timing.computation_time*1e3);
+ ROS_DEBUG_STREAM("--------------- Timing ---------------");
+ ROS_DEBUG_STREAM("computation_time: " << _timing.computation_time);
+ ROS_DEBUG_STREAM("edgingand_clustering_time: " << _timing.edgingand_clustering_time);
+ ROS_DEBUG_STREAM("PointCheck: " << _timing.point_check_time);
+ ROS_DEBUG_STREAM("LineFitting: " << _timing.line_fitting_time);
+ ROS_DEBUG_STREAM("ExtractPayload: " << _timing.payload_extraction_time);
+ ROS_DEBUG_STREAM("NormalVector: " << _timing.normal_vector_time);
+ ROS_DEBUG_STREAM("PayloadDecoder: " << _timing.payload_decoding_time);
+ ROS_DEBUG_STREAM("_tagToRobot: " << _timing.tag_to_robot_time);
+
+ return Out;
+ }
+
+
+ /*
+ * A function to
+ * (1) calculate the depth gradient and the intensity gradient at a point of a pointcloud
+ * (2) group detected 'edge' into different group
+ */
+ void LiDARTag::_gradientAndGroupEdges(const std::vector>& OrderedBuff,
+ std::vector>& EdgeBuff,
+ std::vector &ClusterBuff) {
+
+ // clock_t start = clock();
+ // TODO: if suddently partial excluded, it will cause errors
+ for(int i=0; i<_beam_num; i++) {
+ for(int j=2; j _distance_bound ||
+ std::abs(point.y) > _distance_bound ||
+ std::abs(point.z) > _distance_bound) continue;
+ const auto& PointL = OrderedBuff[i][j-2].point;
+ const auto& PointR = OrderedBuff[i][j+2].point;
+
+ double DepthGrad = std::max((PointL.getVector3fMap()-point.getVector3fMap()).norm(),
+ (point.getVector3fMap()-PointR.getVector3fMap()).norm());
+
+ // double IntenstityGrad = std::max(std::abs(PointL.intensity - point.intensity),
+ // std::abs(point.intensity - PointR.intensity));
+
+ // if (IntenstityGrad > _intensity_threshold &&
+ // DepthGrad > _depth_threshold) {
+ if (DepthGrad > _depth_threshold) {
+
+ // Cluster the detected 'edge' into different groups
+ _clusterClassifier(OrderedBuff[i][j], ClusterBuff);
+
+ // push the detected point that is an edge into a buff
+ LiDARPoints_t LiDARPoints = {OrderedBuff[i][j].point, OrderedBuff[i][j].index,
+ DepthGrad, 0};
+ EdgeBuff[i].push_back(LiDARPoints);
+ }
+ }
+ }
+ // clock_t end = clock();
+ // cout << "extraction hz: " << 1/ (((double) (end - start))/clocks_per_sec) << endl;
+ }
+
+
+ /*
+ * A function to
+ * (1) remove invalid cluster based on the index is too far or not
+ * (2) fill in the points between index of edges
+ * (3) after filling, if the points are too less (based on the analyzed system
+ * and given distant of the cluster), then remove this cluster
+ * (4) Adaptive thresholding (Maximize and minimize intensity) by comparing
+ * with the average value
+ */
+ void LiDARTag::_fillInCluster(const std::vector>& OrderedBuff,
+ std::vector &ClusterBuff){
+
+ _result_statistics.original_cluster_size = ClusterBuff.size();
+ _result_statistics.remaining_cluster_size = ClusterBuff.size();
+ for (int i=0; i_filling_gap_max_index) continue;
+
+ // push points between index into the cluster
+ for (int k=MinIndex+1; k < MaxIndex; ++k){ // remove minimum index itself (it has been in the cloud already)
+ ClusterBuff[i].data.push_back(OrderedBuff[j][k]);
+ //cout << "point added:" << k << endl;
+ }
+ }
+
+ // If the points in this cluster after filling are still less than the
+ // factored threshold, then remove it
+ _timing.timing = clock();
+ if(!_clusterPointsCheck(ClusterBuff[i])){
+ _timing.point_check_time += utils::spendTime(clock(), _timing.timing);
+ //cout << "cluster removed" << endl;
+ ClusterBuff[i].valid = 0;
+ //ClusterBuff.erase(ClusterBuff.begin()+i);
+ // _debug_cluster.point_check.push_back(&ClusterBuff[i]);
+ //i--;
+ _result_statistics.remaining_cluster_size -= 1;
+ _result_statistics.cluster_removal.removed_by_point_check ++;
+ }
+ // Adaptive thresholding (Maximize and minimize intensity) by comparing
+ // with the average value
+ else {
+ _timing.point_check_time += utils::spendTime(clock(), _timing.timing);
+ //boost::thread BuffToPclVectorThread(&LiDARTag::AdaptiveThresholding, this, boost::ref(ClusterBuff[i]));
+ if(!LiDARTag::_adaptiveThresholding(ClusterBuff[i])) {
+ ClusterBuff[i].valid = 0;
+ _result_statistics.remaining_cluster_size -= 1;
+ // ClusterBuff.erase(ClusterBuff.begin()+i);
+ // i--;
+ }
+ }
+ }
+ }
+
+
+ /*
+ * A function to
+ * (1) do adaptive thresholding (Maximize and minimize intensity) by comparing
+ * with the average value and
+ * (2) sort points with ring number and re-index with current cluster into
+ * tag_edges vector so as to do regression boundary lines
+ * (3) It will *remove* if linefitting fails
+ */
+ bool LiDARTag::_adaptiveThresholding(ClusterFamily_t &Cluster){
+ Cluster.ordered_points_ptr.resize(_beam_num);
+ Cluster.payload_right_boundary_ptr.resize(_beam_num);
+ Cluster.payload_left_boundary_ptr.resize(_beam_num);
+
+ for (int k=0; kpoint.intensity = Cluster.data[k].point.intensity/Cluster.max_intensity.intensity;
+ // Calucate "biased" average intensity value
+ // if (ClusterPointPtr->point.intensity > MIN_INTENSITY)
+ // Cluster.accumulate_intensity_of_each_ring[ClusterPointPtr->point.ring] += ClusterPointPtr->point.intensity;
+ // else
+ // Cluster.accumulate_intensity_of_each_ring[ClusterPointPtr->point.ring] -= MAX_INTENSITY; // Punish if it is black
+
+ Cluster.ordered_points_ptr[ClusterPointPtr->point.ring].push_back(ClusterPointPtr);
+ }
+
+ // Fit line of the cluster
+ _timing.timing = clock();
+ if (!LiDARTag::_detectPayloadBoundries(Cluster)){
+ _timing.line_fitting_time += utils::spendTime(clock(), _timing.timing);
+
+ return false;
+ }
+ else {
+ _timing.line_fitting_time += utils::spendTime(clock(), _timing.timing);
+ _timing.timing = clock();
+ _extractPayloadWOThreshold(Cluster);
+ _timing.payload_extraction_time += utils::spendTime(clock(), _timing.timing);
+
+ // 2 is because this payload points is actually includes black
+ // boundary
+ if (Cluster.payload.size() < _min_returns_per_grid*std::pow((std::sqrt(_tag_family)+2*_black_border), 2)) {
+ //_debug_cluster.ExtractPayload.push_back(&Cluster);
+ _result_statistics.cluster_removal.minimum_return ++;
+ return false;
+ }
+
+ // return true;
+ if (_id_decoding){
+ _timing.timing = clock();
+ if (LiDARTag::_decodPayload(Cluster)){
+ _timing.payload_decoding_time += utils::spendTime(clock(), _timing.timing);
+
+ _timing.timing = clock();
+ Cluster.normal_vector = _estimateNormalVector(Cluster);
+ _timing.normal_vector_time += utils::spendTime(clock(), _timing.timing);
+
+ _timing.timing = clock();
+ LiDARTag::_tagToRobot(Cluster.cluster_id, Cluster.normal_vector,
+ Cluster.pose, Cluster.transform, Cluster.average);
+ _timing.tag_to_robot_time += utils::spendTime(clock(), _timing.timing);
+ return true;
+ }
+ else {
+ return false;
+ }
+ }
+ else {
+ LiDARTag::_decodPayload(Cluster);
+
+ // directly assign ID
+ string Code(_assign_id);
+ uint64_t Rcode = stoull(Code, nullptr, 2);
+ BipedAprilLab::QuickDecodeCodeword(tf, Rcode, &Cluster.entry);
+ Cluster.cluster_id = Cluster.entry.id;
+
+ Cluster.normal_vector = _estimateNormalVector(Cluster);
+ LiDARTag::_tagToRobot(Cluster.cluster_id, Cluster.normal_vector,
+ Cluster.pose, Cluster.transform, Cluster.average);
+ return true;
+ }
+ }
+ }
+
+ /* A function to publish pose of tag to the robot
+ */
+ void LiDARTag::_tagToRobot(const int &cluster_id, const Eigen::Vector3f &NormalVec,
+ Homogeneous_t &pose,
+ tf::Transform &transform, const PointXYZRI &Ave){
+ Eigen::Vector3f x(1, 0, 0);
+ Eigen::Vector3f y(0, 1, 0);
+ Eigen::Vector3f z(0, 0, 1);
+ pose.rotation = utils::qToR(NormalVec).cast ();
+ pose.translation << Ave.x, Ave.y, Ave.z;
+
+ pose.yaw = utils::rad2Deg(acos(NormalVec.dot(y)));
+ pose.pitch = -utils::rad2Deg(acos(NormalVec.dot(x)));
+ pose.roll = utils::rad2Deg(acos(NormalVec.dot(z)));
+
+ pose.homogeneous.topLeftCorner(3,3) = pose.rotation;
+ pose.homogeneous.topRightCorner(3,1) = pose.translation;
+ pose.homogeneous.row(3) << 0,0,0,1;
+
+ static tf::TransformBroadcaster Broadcaster_;
+ transform.setOrigin(tf::Vector3(Ave.x, Ave.y, Ave.z));
+
+ // rotate to fit iamge frame
+ Eigen::Vector3f qr(0, std::sqrt(2)/2, 0);
+ float qr_w = std::sqrt(2)/2;
+ // Eigen::Vector3f qr(-0.5, 0.5, -0.5);
+ // float qr_w = 0.5;
+
+ // Eigen::Vector3f qr(-0.5, -0.5, -0.5);
+ // float qr_w = 0.5;
+ // Eigen::Vector3f qr(std::sqrt(2)/2, 0, 0);
+ // float qr_w = std::sqrt(2)/2;
+ // Eigen::Vector3f qiCameraFrame = qr_w*NormalVec + 0*qr + qr.cross(NormalVec); // 0 is q.w of normalvec
+ // float qwCameraFrame = qr_w*0 - qr.dot(NormalVec); // 0 is q.w of normalvec
+ Eigen::Vector3f qiCameraFrame = NormalVec + 2*qr_w*(qr.cross(NormalVec)) + 2*qr.cross(qr.cross(NormalVec)); // 0 is q.w of normalvec
+ float qwCameraFrame = 0; // 0 is q.w of normalvec
+
+
+ Eigen::Vector3f q_i = qiCameraFrame;
+ double q_w = qwCameraFrame;
+ double norm = std::sqrt(std::pow(q_i(0), 2) + std::pow(q_i(1), 2) + std::pow(q_i(2), 2) + std::pow(q_w, 2));
+ q_i = (q_i/norm).eval();
+ q_w = q_w/norm;
+ tf::Quaternion q(q_i(0), q_i(1), q_i(2), q_w);
+ transform.setRotation(q);
+ Broadcaster_.sendTransform(tf::StampedTransform(transform, _point_cloud_header.stamp,
+ _pub_frame, to_string(cluster_id)+"_rotated"));
+
+
+ tf::Quaternion q2(NormalVec(0), NormalVec(1), NormalVec(2), 0);
+ transform.setRotation(q2);
+ Broadcaster_.sendTransform(tf::StampedTransform(transform, _point_cloud_header.stamp,
+ _pub_frame, "LiDARTag-ID" + to_string(cluster_id)));
+
+ // publish lidar tag pose
+ lidartag_msgs::LiDARTagDetection lidartag_msg; //single message
+ lidartag_msg.id = cluster_id;
+ lidartag_msg.size = _payload_size;
+ geometry_msgs::Quaternion geo_q;
+ geo_q.x = q_i(0);
+ geo_q.y = q_i(1);
+ geo_q.z = q_i(2);
+ geo_q.w = q_w;
+ // cout << "R: \n" << pose.rotation << endl;
+ // cout << "det(R): \n" << pose.rotation.determinant() << endl;
+ // cout << "q: " << q_i(0) << ", "
+ // << q_i(1) << ", "
+ // << q_i(2) << ", "
+ // << q_w << endl;
+ lidartag_msg.pose.position.x = Ave.x;
+ lidartag_msg.pose.position.y = Ave.y;
+ lidartag_msg.pose.position.z = Ave.z;
+ lidartag_msg.pose.orientation = geo_q;
+ lidartag_msg.header = _point_cloud_header;
+ lidartag_msg.header.frame_id = std::string("lidartag_") + to_string(cluster_id);
+ _lidartag_pose_array.header = _point_cloud_header;
+ _lidartag_pose_array.detections.push_back(lidartag_msg);
+ // cout << "R.T*NV: " << endl << pose.rotation.transpose()*NormalVec << endl;
+ // cout << "H: " << endl << pose.homogeneous << endl;
+
+ /*
+ Eigen::Vector3f x(1, 0, 0);
+ Eigen::Vector3f y(0, 1, 0);
+ Eigen::Vector3f z(0, 0, 1);
+ Eigen::Matrix3f zSkew;
+ zSkew << 0, -z(2), z(1),
+ z(2), 0, -z(0),
+ -z(1), z(0), 0;
+ Eigen::Vector3f u = zSkew*NormalVec;
+ //u = x.cross(NormalVec);
+ //u = z.cross(NormalVec);
+ //u = -z.cross(NormalVec);
+ //u = -x.cross(NormalVec);
+ //u = -y.cross(NormalVec);
+ //u = x.cross(NormalVec);
+
+ u = (u.normalized()).eval();
+ float theta = acos(z.dot(NormalVec));
+ u = (u*theta).eval();
+ Eigen::Matrix3f uSkew;
+ uSkew << 0, -u(2), u(1),
+ u(2), 0, -u(0),
+ -u(1), u(0), 0;
+
+ pose.rotation = uSkew.exp();
+ pose.translation << Ave.x, Ave.y, Ave.z;
+ pose.yaw = utils::rad2Deg(acos(NormalVec.dot(y)));
+ pose.pitch = -utils::rad2Deg(acos(NormalVec.dot(x)));
+ pose.roll = utils::rad2Deg(acos(NormalVec.dot(z)));
+
+ pose.homogeneous.topLeftCorner(3,3) = pose.rotation;
+ pose.homogeneous.topRightCorner(3,1) = pose.translation;
+ pose.homogeneous.row(3) << 0,0,0,1;
+
+ static tf::TransformBroadcaster Broadcaster_;
+ transform.setOrigin(tf::Vector3(Ave.x, Ave.y, Ave.z));
+ Eigen::Vector3f q_i = sin(theta/2)*u;
+ double q_w = std::cos(theta/2);
+ double norm = std::sqrt(std::pow(q_i(0), 2) + std::pow(q_i(1), 2) + std::pow(q_i(2), 2) + std::pow(q_w, 2));
+ q_i = (q_i/norm).eval();
+ q_w = q_w/norm;
+ tf::Quaternion q(q_i(0), q_i(1), q_i(2), q_w);
+ transform.setRotation(q);
+ Broadcaster_.sendTransform(tf::StampedTransform(transform, _point_cloud_header.stamp,
+ _pub_frame, to_string(cluster_id)));
+
+ // publish lidar tag pose
+ lidartag_msgs::LiDARTagDetection lidartag_msg; //single message
+ lidartag_msg.id = cluster_id;
+ lidartag_msg.size = _payload_size;
+ geometry_msgs::Quaternion geo_q;
+ geo_q.x = q_i(0);
+ geo_q.y = q_i(1);
+ geo_q.z = q_i(2);
+ geo_q.w = q_w;
+ // cout << "R: \n" << pose.rotation << endl;
+ // cout << "det(R): \n" << pose.rotation.determinant() << endl;
+ // cout << "q: " << q_i(0) << ", "
+ // << q_i(1) << ", "
+ // << q_i(2) << ", "
+ // << q_w << endl;
+ lidartag_msg.pose.position.x = Ave.x;
+ lidartag_msg.pose.position.y = Ave.y;
+ lidartag_msg.pose.position.z = Ave.z;
+ lidartag_msg.pose.orientation = geo_q;
+ lidartag_msg.header = _point_cloud_header;
+ lidartag_msg.header.frame_id = std::string("lidartag_") + to_string(cluster_id);
+ _lidartag_pose_array.header = _point_cloud_header;
+ _lidartag_pose_array.detections.push_back(lidartag_msg);
+ // cout << "R.T*NV: " << endl << pose.rotation.transpose()*NormalVec << endl;
+ // cout << "H: " << endl << pose.homogeneous << endl;
+ */
+ }
+
+
+
+ /*
+ * A function to extract the payload points from a valid cluster.
+ * Let's say we have 10 points on the left boundary (line) of the tag and 9 points on the right boundary
+ * (line) of the tag.
+ * It is seperated into two parts.
+ * TODO: should use medium instead of max points
+ * (i) Find the max points that we have on a ring in this cluster by
+ * exmaming the average points on the first 1/2 rings int((10+9)/4)
+ * (ii) For another half of the rings, we just find the start index and add the
+ * average number of points to the payload points
+ */
+ void LiDARTag::_extractPayloadWOThreshold(ClusterFamily_t &Cluster){
+ int LastRoundLength = 0; // Save to recover a missing ring
+ PointXYZRI average{0,0,0,0};
+ for(int Ring=0; Ring<_beam_num; ++Ring){
+
+ // if (Cluster.payload_right_boundary_ptr[Ring]!=0)
+ // Cluster.payload_boundary_ptr.push_back(Cluster.payload_right_boundary_ptr[Ring]);
+
+ // if (Cluster.payload_left_boundary_ptr[Ring]!=0)
+ // Cluster.payload_boundary_ptr.push_back(Cluster.payload_left_boundary_ptr[Ring]);
+
+ if (Cluster.payload_right_boundary_ptr[Ring]==0 &&
+ Cluster.payload_left_boundary_ptr[Ring]==0) continue;
+ // cout << "Ring" << Ring << endl;
+ else if (Cluster.payload_right_boundary_ptr[Ring]!=0 &&
+ Cluster.payload_left_boundary_ptr[Ring]!=0){
+ Cluster.payload_boundary_ptr.push_back(Cluster.payload_right_boundary_ptr[Ring]);
+ Cluster.payload_boundary_ptr.push_back(Cluster.payload_left_boundary_ptr[Ring]);
+ int StartIndex = Cluster.payload_left_boundary_ptr[Ring]->index;
+ int EndIndex = Cluster.payload_right_boundary_ptr[Ring]->index;
+ LastRoundLength = EndIndex - StartIndex;
+
+
+ for (int j=0; jindex == StartIndex){
+ // Remove index itself because itself is not the part of a
+ // payload
+ for (int k=j+1; k=Cluster.ordered_points_ptr[Ring].size()) break; // make sure the index is valid
+ // cout << "j: " << j << endl;
+ // cout << "k: " << k << endl;
+ // cout << "validation1: " << endl;
+ // utils::COUT(Cluster.ordered_points_ptr[Ring][k]->point);
+ //
+ Cluster.payload.push_back(Cluster.ordered_points_ptr[Ring][k]);
+ average.x += Cluster.ordered_points_ptr[Ring][k]->point.x;
+ average.y += Cluster.ordered_points_ptr[Ring][k]->point.y;
+ average.z += Cluster.ordered_points_ptr[Ring][k]->point.z;
+ }
+ break;
+ }
+ }
+ }
+ Cluster.average.x = average.x/ Cluster.payload.size();
+ Cluster.average.y = average.y/ Cluster.payload.size();
+ Cluster.average.z = average.z/ Cluster.payload.size();
+ // else if (LastRoundLength!=0 && Cluster.payload_right_boundary_ptr[Ring]!=0){
+ // int EndIndex = Cluster.payload_right_boundary_ptr[Ring]->index;
+
+ // for (int j=Cluster.ordered_points_ptr[Ring].size()-1; j>0; --j){
+ // if (Cluster.ordered_points_ptr[Ring][j]->index == EndIndex){
+ // Cluster.payload.push_back(Cluster.ordered_points_ptr[Ring][j]);
+
+ // for (int k=j-1; k>j-LastRoundLength; --k){
+ // if (k<0) break; // make sure the index is valid
+ // Cluster.payload.push_back(Cluster.ordered_points_ptr[Ring][k]);
+ // }
+ // break;
+ // }
+ // }
+
+ // }
+ // else if (LastRoundLength!=0 && Cluster.payload_left_boundary_ptr[Ring]!=0){
+ // int StartIndex = Cluster.payload_left_boundary_ptr[Ring]->index;
+
+ // for (int j=0; jindex == StartIndex){
+ // Cluster.payload.push_back(Cluster.ordered_points_ptr[Ring][j]);
+
+ // for (int k=j-1; k=Cluster.ordered_points_ptr[Ring].size()) break; // make sure the index is valid
+ // Cluster.payload.push_back(Cluster.ordered_points_ptr[Ring][k]);
+ // }
+ // break;
+ // }
+ // }
+
+ // }
+ }
+ }
+
+
+ bool LiDARTag::_detectPayloadBoundries(ClusterFamily_t &Cluster){
+ // return true;
+ bool FitOkay = false; // Meaning reject this cluster and will be removed
+
+ // Initialization
+ Cluster.tag_edges.upper_ring = _beam_num;
+ Cluster.tag_edges.lower_ring = 0;
+ double AverageIntensity = ((Cluster.max_intensity.intensity +
+ Cluster.min_intensity.intensity)/2);
+ double DetectionThreshold_ = (AverageIntensity - Cluster.min_intensity.intensity)/
+ (_payload_intensity_threshold*Cluster.max_intensity.intensity);
+ //cout << "Detection Threshold: " << DetectionThreshold_ << endl;
+
+ /*
+ * Compare current point's gradient intensity with next points
+ * gradient intensity
+ * Example:
+ * o: white, x: black
+ *
+ * a9876543210
+ * ooooxxxxxxxooooo
+ * ^^^^ ^^^^
+ * |||| ||||
+ * Gradient from 1-2 sould be large; the same as the gradient from 0-2
+ * Gradient from 9-8 sould be large; the same as the gradient from a-8
+ */
+ int BoundaryPointCount = 0; // Check there're sufficient points
+ float RightMin = 1000;
+ float RightMax = -1000;
+ float LeftMin = 1000;
+ float LeftMax = -1000;
+ PointXYZRI PTOP;
+ PointXYZRI PDown;
+ PointXYZRI PLeft;
+ PointXYZRI PRight;
+ for (int Ring=0; Ring<_beam_num; ++Ring){
+ // skip this ring if doesn't exist
+ if (Cluster.ordered_points_ptr[Ring].size()floor((Cluster.ordered_points_ptr[Ring].size()-2)/2);
+ //P>1;
+ --P){
+ /*
+ * (1) By knowing it from white to black on the left calucate the
+ * intensity gradient
+ * (2) Since have thresholded already, we * could also choose > 255
+ */
+ // cout << "p intensity: " << Cluster.ordered_points_ptr[Ring][P]->point.intensity << endl;
+ // cout << "PR: " << P << endl;
+ // cout << "PointR: " << Cluster.ordered_points_ptr[Ring][P]->index << endl;
+ // utils::COUT(Cluster.ordered_points_ptr[Ring][P]->point);
+ // Cluster.payload_right_boundary_ptr[Ring] = Cluster.ordered_points_ptr[Ring][P];
+ // FitOkay = true;
+ // break;
+ if ((Cluster.ordered_points_ptr[Ring][P]->point.intensity -
+ Cluster.ordered_points_ptr[Ring][P-1]->point.intensity>DetectionThreshold_) &&
+ (Cluster.ordered_points_ptr[Ring][P+1]->point.intensity -
+ Cluster.ordered_points_ptr[Ring][P-1]->point.intensity>DetectionThreshold_)) {
+
+ Cluster.payload_right_boundary_ptr[Ring] = Cluster.ordered_points_ptr[Ring][P];
+ RightMin = (RightMin < Cluster.ordered_points_ptr[Ring][P]->point.y) ? RightMin : Cluster.ordered_points_ptr[Ring][P]->point.y;
+ RightMax = (RightMax > Cluster.ordered_points_ptr[Ring][P]->point.y) ? RightMax : Cluster.ordered_points_ptr[Ring][P]->point.y;
+ BoundaryPointCount ++;
+
+ break;
+ }
+ }
+
+
+ /* [Left]
+ * Find edges from the left of a ring
+ * Start from 1 becasue we take another point on the left into account
+ * size -1 because we compare with next point
+ */
+ // for (int P=2; P 255
+ * (3) To determin if p if the edge:
+ * 1. compare with p+1 (to its right)
+ */
+ //cout << "PL: " << P << endl;
+ //Cluster.payload_left_boundary_ptr[Ring] = Cluster.ordered_points_ptr[Ring][P];
+ //cout << "PointL: " << Cluster.ordered_points_ptr[Ring][P]->index << endl;
+ //// utils::COUT(Cluster.ordered_points_ptr[Ring][P]->point);
+ //FitOkay = true;
+
+ //break;
+ if ((Cluster.ordered_points_ptr[Ring][P]->point.intensity -
+ Cluster.ordered_points_ptr[Ring][P+1]->point.intensity>DetectionThreshold_) &&
+ (Cluster.ordered_points_ptr[Ring][P-1]->point.intensity -
+ Cluster.ordered_points_ptr[Ring][P+1]->point.intensity>DetectionThreshold_)) {
+
+ Cluster.payload_left_boundary_ptr[Ring] = Cluster.ordered_points_ptr[Ring][P];
+ LeftMin = (LeftMin < Cluster.ordered_points_ptr[Ring][P]->point.y) ? LeftMin : Cluster.ordered_points_ptr[Ring][P]->point.y;
+ LeftMax = (LeftMax > Cluster.ordered_points_ptr[Ring][P]->point.y) ? LeftMax : Cluster.ordered_points_ptr[Ring][P]->point.y;
+ BoundaryPointCount ++;
+
+ break;
+ }
+ }
+ }
+
+ // reject if points are too less (can't even get decoded!)
+ if (BoundaryPointCount < int(sqrt(_tag_family))*2) {
+ _result_statistics.cluster_removal.boundary_point_check++;
+ // _debug_cluster.BoundaryPoint.push_back(&Cluster);
+ return false;
+ } else FitOkay = true;
+
+ if (!FitOkay) {
+ _result_statistics.cluster_removal.no_edge_check++;
+ // _debug_cluster.NoEdge.push_back(&Cluster);
+ }
+
+ return FitOkay;
+ }
+
+
+
+
+
+ /* [Normal vector]
+ * A function to estimate the normal vector of a potential payload
+ */
+ Eigen::MatrixXf
+ LiDARTag::_estimateNormalVector(ClusterFamily_t &Cluster){
+ Eigen::MatrixXf EigenPC(3, Cluster.payload.size());
+
+ for (int i=0; ipoint.x - Cluster.average.x;
+ EigenPC(1,i) = Cluster.payload[i]->point.y - Cluster.average.y;
+ EigenPC(2,i) = Cluster.payload[i]->point.z - Cluster.average.z;
+ }
+
+ Eigen::JacobiSVD svd(EigenPC, Eigen::ComputeFullU);
+ Eigen::Matrix normal_vector_robot = svd.matrixU().col(2); // Take the last column of U in SVD
+
+ // flip through xy plane
+ // Eigen::Matrix3f m;
+ // m << 0, 1, 0,
+ // 0, 0, 1,
+ // 1, 0, 0;
+
+ // m << 0,1,0,
+ // 1,0,0,
+ // 0,0,1;
+
+ // m << 0,0,1,
+ // 1,0,0,
+ // 0,1,0;
+ // m << 0,0,1,
+ // 0,1,0,
+ // 1,0,0;
+
+ // m << 1,0,0,
+ // 0,0,1,
+ // 0,1,0;
+ // cout << "Normal Vector1: \n" << normal_vector_robot << endl;
+ // normal_vector_robot = (m*normal_vector_robot).eval();
+ // cout << "Normal Vector2: \n" << normal_vector_robot << endl;
+
+ // Make sure consistency
+ if (normal_vector_robot(0)>=0) normal_vector_robot = (-normal_vector_robot).eval();
+ // cout << "Normal Vector3: \n" << normal_vector_robot << endl;
+
+ // Coordinate transform
+ // Eigen::Matrix normal_vector_tag;
+ // normal_vector_tag << normal_vector_robot(2), normal_vector_robot(0), normal_vector_robot(1);
+ // normal_vector_tag << normal_vector_robot(0), normal_vector_robot(2), normal_vector_robot(1);
+
+
+ return normal_vector_robot;
+
+ // pcl::PointCloud::Ptr Cloud (new pcl::PointCloud);
+
+ // for (int i=0; ipoint.x,
+ // Cluster.payload[i]->point.y,
+ // Cluster.payload[i]->point.z};
+ // cout << "point: " << point << endl;
+ // Cloud->push_back(point);
+ // }
+ // pcl::PointCloud::Ptr normals (new pcl::PointCloud);
+
+ // // pcl::IntegralImageNormalEstimation ne;
+ // // ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
+ // // ne.setMaxDepthChangeFactor(0.02f);
+ // // ne.setNormalSmoothingSize(10.0f);
+ // // ne.setInputCloud(Cloud);
+ // // ne.compute(*normals);
+ // // cout << "NV: " << *normals << endl;
+
+ // pcl::NormalEstimation NE;
+ // NE.setInputCloud (Cloud);
+ // pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ());
+ // NE.setSearchMethod (tree);
+
+ // // Output datasets
+ // pcl::PointCloud::Ptr CloudNormals (new pcl::PointCloud);
+ // NE.setRadiusSearch (1);
+ // NE.compute (*CloudNormals);
+ // cout << "normals: " << *CloudNormals << endl;
+ // std::cout << "cloud_normals->points.size (): " << CloudNormals->points.size () << std::endl;
+ //
+ // return CloudNormals;
+ }
+
+ /*
+ * A function of ros spin
+ * reason: in order to put it into background as well as able to run other tasks
+ */
+ void LiDARTag::_rosSpin(){
+ while (ros::ok() && !_stop){
+ ros::spinOnce();
+ }
+ }
+
+
+ /*
+ * A function to cluster a single point into a new cluster or an existing cluster
+ */
+ void LiDARTag::_clusterClassifier(const LiDARPoints_t &point, vector &ClusterBuff){
+ // The first time to cluster the point cloud
+ int ValidCluster = 1; // Marker every cluster is valid and will be checked again later
+ if (ClusterBuff.size()==0){
+ PointXYZRI top_most_point = point.point;
+ top_most_point.z = top_most_point.z + _threshold;
+ PointXYZRI bottom_most_point = point.point;
+ bottom_most_point.z -= _threshold;
+
+ PointXYZRI front_most_point = point.point;
+ front_most_point.x += _threshold;
+ PointXYZRI back_most_point = point.point;
+ back_most_point.x -= _threshold;
+
+ PointXYZRI right_most_point = point.point;
+ right_most_point.y -= _threshold;
+ PointXYZRI left_most_point = point.point;
+ left_most_point.y += _threshold;
+ //cout << "_threshold:" << _threshold << endl;
+ //
+ //cout << "\033[1;31m============== \033[0m\n";
+ //cout << "First created" << endl;
+ //cout << "TopMost: " << top_most_point.x << ", " << top_most_point.y << ", " << top_most_point.z << endl;
+ //cout << "bottom_most_point: " << bottom_most_point.x << ", " << bottom_most_point.y << ", " << bottom_most_point.z << endl;
+ //cout << "Front: " << front_most_point.x << ", " << front_most_point.y << ", " << front_most_point.z << endl;
+ //cout << "back: " << back_most_point.x << ", " << back_most_point.y << ", " << back_most_point.z << endl;
+ //cout << "Right: " << right_most_point.x << ", " << right_most_point.y << ", " << right_most_point.z << endl;
+ //cout << "Left: " << left_most_point.x << ", " << left_most_point.y << ", " << left_most_point.z << endl;
+
+ ClusterFamily_t CurrentCluster = {0, ValidCluster, top_most_point, bottom_most_point,
+ front_most_point, back_most_point,
+ right_most_point, left_most_point,
+ point.point};
+ MaxMin_t InitialValue = {(int)1e8, 0, -1};
+ CurrentCluster.max_min_index_of_each_ring.resize(_beam_num, InitialValue);
+ CurrentCluster.max_min_index_of_each_ring[point.point.ring].max = point.index;
+ CurrentCluster.max_min_index_of_each_ring[point.point.ring].min = point.index;
+
+ CurrentCluster.max_intensity = point.point;
+ CurrentCluster.min_intensity = point.point;
+
+ CurrentCluster.data.push_back(point);
+ ClusterBuff.push_back(CurrentCluster);
+ return ;
+ }
+ else {
+ // Take a point to go through all the existing cluster to see if this
+ // point belongs to any of them
+ // Once it is found belonging to one of the clusters then return.
+ // After looping though and couldn't find a belonging group then add it
+ // to a new cluster
+ TestCluster_t *new_cluster = new TestCluster_t{0};
+ //cout << "\033[1;31m============== \033[0m\n";
+ //cout << "ClusterBuff size: " << ClusterBuff.size() << endl;
+ for (int i=0; iflag)) {
+ delete new_cluster;
+ return;
+ }
+ }
+ // Add a new cluster
+ if (new_cluster->flag){
+ //cout << "new cluster added" << endl;
+ int Newcluster_id = ClusterBuff.size();
+ PointXYZRI top_most_point = point.point;
+ top_most_point.z += _threshold;
+ PointXYZRI bottom_most_point = point.point;
+ bottom_most_point.z -= _threshold;
+
+ PointXYZRI front_most_point = point.point;
+ front_most_point.x += _threshold;
+ PointXYZRI back_most_point = point.point;
+ back_most_point.x -= _threshold;
+
+ PointXYZRI right_most_point = point.point;
+ right_most_point.y -= _threshold;
+ PointXYZRI left_most_point = point.point;
+ left_most_point.y += _threshold;
+
+ //cout << "TopMost: " << top_most_point.x << ", " << top_most_point.y << ", " << top_most_point.z << endl;
+ //cout << "bottom_most_point: " << bottom_most_point.x << ", " << bottom_most_point.y << ", " << bottom_most_point.z << endl;
+ //cout << "Front: " << front_most_point.x << ", " << front_most_point.y << ", " << front_most_point.z << endl;
+ //cout << "back: " << back_most_point.x << ", " << back_most_point.y << ", " << back_most_point.z << endl;
+ //cout << "Right: " << right_most_point.x << ", " << right_most_point.y << ", " << right_most_point.z << endl;
+ //cout << "Left: " << left_most_point.x << ", " << left_most_point.y << ", " << left_most_point.z << endl;
+
+
+ // Check every time when new marker added
+ // visualization_msgs::MarkerArray CheckArray;
+ // visualization_msgs::Marker CheckMarker;
+ // LiDARTag::_assignMarker(CheckMarker, visualization_msgs::Marker::CUBE,
+ // "Check0",
+ // 1, 0, 0,
+ // top_most_point, 0, 0.05);
+ // CheckArray.markers.push_back(CheckMarker);
+
+ // LiDARTag::_assignMarker(CheckMarker, visualization_msgs::Marker::CUBE,
+ // "Check1",
+ // 1, 0, 0,
+ // bottom_most_point, 1, 0.05);
+ // CheckArray.markers.push_back(CheckMarker);
+
+ // LiDARTag::_assignMarker(CheckMarker, visualization_msgs::Marker::CUBE,
+ // "Check2",
+ // 1, 0, 0,
+ // front_most_point, 2, 0.05);
+ // CheckArray.markers.push_back(CheckMarker);
+
+ // LiDARTag::_assignMarker(CheckMarker, visualization_msgs::Marker::CUBE,
+ // "Check3",
+ // 1, 0, 0,
+ // back_most_point, 3, 0.05);
+ // CheckArray.markers.push_back(CheckMarker);
+
+ // LiDARTag::_assignMarker(CheckMarker, visualization_msgs::Marker::CUBE,
+ // "Check4",
+ // 1, 0, 0,
+ // left_most_point, 4, 0.05);
+ // CheckArray.markers.push_back(CheckMarker);
+
+ // LiDARTag::_assignMarker(CheckMarker, visualization_msgs::Marker::CUBE,
+ // "Check5",
+ // 1, 0, 0,
+ // right_most_point, 5, 0.05);
+ // CheckArray.markers.push_back(CheckMarker);
+
+ // LiDARTag::_assignMarker(CheckMarker, visualization_msgs::Marker::SPHERE,
+ // "Check6",
+ // 0, 1, 0,
+ // Point, 5, 0.1);
+ // CheckArray.markers.push_back(CheckMarker);
+
+ // _boundary_marker_pub.publish(CheckArray);
+ //utils::pressEnterToContinue();
+
+
+ new_cluster->new_cluster = {Newcluster_id, ValidCluster, top_most_point, bottom_most_point,
+ front_most_point, back_most_point,
+ right_most_point, left_most_point,
+ point.point};
+
+ // To fill in points between initial points and end points later
+ MaxMin_t InitialValue = {(int)1e8, 0, -1};
+ new_cluster->new_cluster.max_min_index_of_each_ring.resize(_beam_num, InitialValue);
+ new_cluster->new_cluster.max_min_index_of_each_ring[point.point.ring].max = point.index;
+ new_cluster->new_cluster.max_min_index_of_each_ring[point.point.ring].min = point.index;
+
+ new_cluster->new_cluster.max_intensity = point.point;
+ new_cluster->new_cluster.min_intensity = point.point;
+
+ new_cluster->new_cluster.data.push_back(point);
+ ClusterBuff.push_back(new_cluster->new_cluster);
+ }
+ delete new_cluster;
+ }
+ }
+
+
+ /*
+ * A function update some information about a cluster if this point belongs to
+ * this cluster; if not belonging to this cluster then return and create a new
+ * one
+ */
+ void LiDARTag::_updateCluster(const LiDARPoints_t &point, ClusterFamily_t &OldCluster, TestCluster_t *new_cluster){
+ // This point is outside of the current cluster
+ if (!(
+ (point.point.z < OldCluster.top_most_point.z + _threshold) &&
+ (point.point.x < OldCluster.front_most_point.x + _threshold) &&
+ (point.point.y < OldCluster.left_most_point.y + _threshold) &&
+ (OldCluster.back_most_point.x - _threshold< point.point.x) &&
+ (OldCluster.right_most_point.y - _threshold< point.point.y) &&
+ (OldCluster.bottom_most_point.z - _threshold< point.point.z))){
+
+ // cout << "\033[1;31m============== \033[0m\n";
+ // cout << "New flag" << endl;
+ // cout << "point: " << point.point.x << ", " << point.point.y << ", " << point.point.z << endl;
+ // cout << "TOP.z: " << OldCluster.top_most_point.z << ", " << point.point.z << endl;
+ // cout << "Front.x: " << OldCluster.front_most_point.x << ", " << point.point.x << endl;
+ // cout << "Left.y: " << OldCluster.left_most_point.y << ", " << point.point.y << endl;
+ // cout << "Right.y: " << OldCluster.right_most_point.y << ", " << point.point.y << endl;
+ // cout << "Bottom.z: " << OldCluster.bottom_most_point.z << ", " << point.point.z << endl;
+ // cout << "Back.x: " << OldCluster.back_most_point.x << ", " << point.point.x << endl;
+ // utils::pressEnterToContinue();
+ new_cluster->flag = 1;
+ return ;
+ }
+ else{
+ new_cluster->flag = 0;
+ }
+
+ // This point is inside this cluster
+ if (!new_cluster->flag){
+ // update the boundary of the old cluster
+ if (point.point.x + _threshold > OldCluster.front_most_point.x) {
+ OldCluster.front_most_point = point.point;
+ OldCluster.front_most_point.x += _threshold;
+ }
+ if (point.point.x - _threshold < OldCluster.back_most_point.x) {
+ OldCluster.back_most_point = point.point;
+ OldCluster.back_most_point.x -= _threshold;
+ }
+
+ if (point.point.y + _threshold > OldCluster.left_most_point.y) {
+ OldCluster.left_most_point = point.point;
+ OldCluster.left_most_point.y += _threshold;
+ }
+ if (point.point.y - _threshold< OldCluster.right_most_point.y) {
+ OldCluster.right_most_point = point.point;
+ OldCluster.right_most_point.y -= _threshold;
+ }
+
+ if (point.point.z + _threshold > OldCluster.top_most_point.z) {
+ OldCluster.top_most_point = point.point;
+ OldCluster.top_most_point.z += _threshold;
+ }
+ if (point.point.z - _threshold < OldCluster.bottom_most_point.z) {
+ OldCluster.bottom_most_point = point.point;
+ OldCluster.bottom_most_point.z -= _threshold;
+ }
+
+ // update the average // spend around 5-6 HZ
+ OldCluster.average.getVector3fMap() = ((OldCluster.average.getVector3fMap() * OldCluster.data.size() +
+ point.point.getVector3fMap()) / (OldCluster.data.size()+1)).eval();
+
+ // update the max/min index of each ring in this cluster
+ if (OldCluster.max_min_index_of_each_ring[point.point.ring].max < point.index)
+ OldCluster.max_min_index_of_each_ring[point.point.ring].max = point.index;
+
+ if (OldCluster.max_min_index_of_each_ring[point.point.ring].min > point.index)
+ OldCluster.max_min_index_of_each_ring[point.point.ring].min = point.index;
+
+ // update the max/min intensity of this cluster
+ if (OldCluster.max_intensity.intensity < point.point.intensity)
+ OldCluster.max_intensity = point.point;
+
+ if (OldCluster.min_intensity.intensity > point.point.intensity)
+ OldCluster.min_intensity = point.point;
+
+ OldCluster.data.push_back(point);
+ }
+ }
+
+
+
+
+ /*
+ * A function to transform from a customized type (LiDARpoints_t) of vector of vector (EdgeBuff)
+ * into a standard type (PointXYZRI) of pcl vector (out)
+ */
+ void LiDARTag::_buffToPclVector(const std::vector> &EdgeBuff,
+ pcl::PointCloud::Ptr Out){
+ for (int RingNumber=0; RingNumber<_beam_num; ++RingNumber){
+ if (EdgeBuff.at(RingNumber).size()!=0){
+ for (int i=0; ipush_back(EdgeBuff[RingNumber][i].point);
+ }
+ }
+ }
+ }
+
+
+ /*
+ * A function to prepare for displaying results in rviz
+ */
+ void LiDARTag::_clusterToPclVectorAndMarkerPublisher(const std::vector &Cluster,
+ pcl::PointCloud::Ptr OutCluster,
+ pcl::PointCloud::Ptr OutPayload,
+ visualization_msgs::MarkerArray &ClusterArray){
+
+ /* initialize random seed for coloring the marker*/
+ srand (time(NULL));
+ visualization_msgs::MarkerArray BoundMarkArray;
+ visualization_msgs::MarkerArray PayloadMarkArray;
+
+
+ int PointsInClusters = 0;
+ for (int Key=0; Key0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+
+ /*
+ LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::ARROW,
+ "NormalVector1" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ Cluster[Key].average, 1, 0.01);
+ BoundaryMarker.scale.x = 0.15;
+ // BoundaryMarker.pose.orientation.x = (Cluster[Key].normal_vector(0)>0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+
+ LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::ARROW,
+ "NormalVector2" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ Cluster[Key].average, 1, 0.01);
+ BoundaryMarker.scale.x = 0.15;
+ // BoundaryMarker.pose.orientation.x = (Cluster[Key].normal_vector(0)>0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+
+ LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::ARROW,
+ "NormalVector3" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ Cluster[Key].average, 1, 0.01);
+ BoundaryMarker.scale.x = 0.15;
+ // BoundaryMarker.pose.orientation.x = (Cluster[Key].normal_vector(0)>0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+
+ LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::ARROW,
+ "NormalVector4" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ Cluster[Key].average, 1, 0.01);
+ BoundaryMarker.scale.x = 0.15;
+ // BoundaryMarker.pose.orientation.x = (Cluster[Key].normal_vector(0)>0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+
+ LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::ARROW,
+ "NormalVector5" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ Cluster[Key].average, 1, 0.01);
+ BoundaryMarker.scale.x = 0.15;
+ // BoundaryMarker.pose.orientation.x = (Cluster[Key].normal_vector(0)>0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+
+ LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::ARROW,
+ "NormalVector6" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ Cluster[Key].average, 1, 0.01);
+ BoundaryMarker.scale.x = 0.15;
+ // BoundaryMarker.pose.orientation.x = (Cluster[Key].normal_vector(0)>0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+
+ LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::ARROW,
+ "NormalVector7" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ Cluster[Key].average, 1, 0.01);
+ BoundaryMarker.scale.x = 0.15;
+ // BoundaryMarker.pose.orientation.x = (Cluster[Key].normal_vector(0)>0) ?
+ // -Cluster[Key].normal_vector(0) : Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.x = Cluster[Key].normal_vector(0);
+ BoundaryMarker.pose.orientation.y = Cluster[Key].normal_vector(1);
+ BoundaryMarker.pose.orientation.z = Cluster[Key].normal_vector(2);
+ BoundaryMarker.pose.orientation.w = 0;
+ BoundMarkArray.markers.push_back(BoundaryMarker);
+ */
+
+
+
+ // LiDARTag::_assignMarker(BoundaryMarker, visualization_msgs::Marker::SPHERE,
+ // "Average" + to_string(Cluster[Key].cluster_id),
+ // r, g, b,
+ // Cluster[Key].average, 5, 0.04);
+ // BoundMarkArray.markers.push_back(BoundaryMarker);
+
+
+
+ //_cluster_pub.publish(BoundaryMarkerList);
+ //LiDARTag::_assignLine(BoundaryMarkerList, visualization_msgs::Marker::LINE_STRIP,
+ // r, g, b,
+ // Cluster[Key], 1);
+ //_cluster_pub.publish(BoundaryMarkerList);
+
+ // _boundary_marker_pub.publish(BoundMarkArray);
+
+ // Draw payload boundary marker
+ visualization_msgs::Marker PayloadMarker;
+
+ if (_adaptive_thresholding){
+ // Upper boundary
+ for (int i=0; ipoint, i, 0.015);
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+ }
+
+ // Lower boundary
+ for (int i=0; ipoint, i, 0.015);
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+ }
+
+ // Left boundary (green)
+ for (int i=0; ipoint, i, 0.015);
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+ }
+
+ // Right boundary (red)
+ for (int i=0; ipoint, i, 0.015);
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+ }
+ }
+ else {
+
+ // cout << "size1: " << Cluster[Key].payload_boundary_ptr.size() << endl;
+ int count = 0;
+ for (int i=0; ipoint, count, 0.015);
+ // PayloadMarkArray.markers.push_back(PayloadMarker);
+ // count ++;
+
+ // LiDARTag::_assignMarker(PayloadMarker, visualization_msgs::Marker::SPHERE,
+ // "PayloadBoundary_" + to_string(Cluster[Key].cluster_id),
+ // 1, 0, 0,
+ // Cluster[Key].payload_boundary_ptr[i][1]->point, count, 0.015);
+ // count ++;
+ // PayloadMarkArray.markers.push_back(PayloadMarker);
+ // }
+ // for (int k=0; kpoint, i, 0.015);
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+ count ++;
+ // }
+ }
+ }
+
+ // Text
+ /*
+ LiDARTag::_assignMarker(PayloadMarker, visualization_msgs::Marker::TEXT_VIEW_FACING,
+ "PayloadUpperBoundary_text_" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 1,
+ Cluster[Key].tag_edges.upper_line[0]->point, 5, 0.05,
+ string("Ring: " + to_string(Cluster[Key].tag_edges.upper_line[0]->point.ring))
+ );
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+
+ LiDARTag::_assignMarker(PayloadMarker, visualization_msgs::Marker::TEXT_VIEW_FACING,
+ "PayloadLowerBoundary_text_" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 1,
+ Cluster[Key].tag_edges.lower_line[0]->point, 5, 0.05,
+ string("Ring: " + to_string(Cluster[Key].tag_edges.lower_line[0]->point.ring))
+ );
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+ */
+
+
+ // corner points and RANSAC line
+ if (_adaptive_thresholding){
+ Eigen::Vector4f EigenPoint;
+ PointXYZRI point; // just for conversion
+
+ for (int i=0; i<4; ++i){ // 4 corners
+ // Corners
+ if (i!=3){
+ pcl::lineWithLineIntersection(Cluster[Key].line_coeff[i], Cluster[Key].line_coeff[i+1],
+ EigenPoint, 1e-2);
+ }
+ else {
+ pcl::lineWithLineIntersection(Cluster[Key].line_coeff[i], Cluster[Key].line_coeff[0],
+ EigenPoint, 1e-2);
+ }
+
+ LiDARTag::_eigenVectorToPointXYZRI(EigenPoint, point);
+ LiDARTag::_assignMarker(PayloadMarker, visualization_msgs::Marker::SPHERE,
+ "Corner_" + to_string(Cluster[Key].cluster_id),
+ 0, 1, 1,
+ point, i, 0.02);
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+
+ // RANSAC
+ LiDARTag::_assignMarker(PayloadMarker, visualization_msgs::Marker::ARROW,
+ "RANSAC" + to_string(Cluster[Key].cluster_id),
+ 1, 1, 0,
+ point, i, 0.01);
+ double Length = sqrt(pow(Cluster[Key].line_coeff[i][3],2) +
+ pow(Cluster[Key].line_coeff[i][4],2) +
+ pow(Cluster[Key].line_coeff[i][5],2));
+ PayloadMarker.scale.x = 0.15;
+ PayloadMarker.pose.orientation.x = Cluster[Key].line_coeff[i][3]/Length;
+ PayloadMarker.pose.orientation.y = Cluster[Key].line_coeff[i][4]/Length;
+ PayloadMarker.pose.orientation.z = Cluster[Key].line_coeff[i][5]/Length;
+ PayloadMarkArray.markers.push_back(PayloadMarker);
+ }
+ }
+
+ // Add lines to the tag
+ //LiDARTag::_assignLine(PayloadMarker, PayloadMarkArray,
+ // visualization_msgs::Marker::LINE_STRIP,
+ // "left_line_" + to_string(Cluster[Key].cluster_id),
+ // r, g, b,
+ // PointTL, PointTR, 1);
+ //PayloadMarkArray.markers.push_back(PayloadMarker);
+
+
+ // Draw all detected-filled cluster points
+ for (int i=0; ipush_back(Cluster[Key].data[i].point);
+ double Intensity = Cluster[Key].data[i].point.intensity;
+ LiDARTag::_assignMarker(Marker, visualization_msgs::Marker::SPHERE,
+ to_string(Cluster[Key].cluster_id),
+ Intensity, Intensity, Intensity,
+ Cluster[Key].data[i].point, i, 0.01);
+ ClusterArray.markers.push_back(Marker);
+ }
+
+ // Add all payload points to a vector and will be publish to another
+ // channel for both visualizatoin and creating training datasets
+ // cout << "payload size2: " << Cluster[Key].payload.size() << endl;
+ for (int i=0; i