first commit

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} 
+)

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
+<img src="https://github.com/brucejk/LiDARTag/blob/master/figure/figure1-2.png" width="640">
+<img src="https://github.com/brucejk/LiDARTag/blob/master/figure/figure1.png" width="640">
+
+# Why LiDAR?
+Robust to lighting!!  
+<img src="https://github.com/brucejk/LiDARTag/blob/master/figure/lighting1.jpg" width="240">
+<img src="https://github.com/brucejk/LiDARTag/blob/master/figure/lighting2.jpg" width="240">
+<img src="https://github.com/brucejk/LiDARTag/blob/master/figure/lighting3.jpg" width="240">
+
+
+
+## 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)
+
+

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, [email protected]. 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 ([email protected])
+ * 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
+
+