├── README.md ├── april_tag_detector ├── .DS_Store ├── CMakeLists.txt ├── package.xml └── src │ ├── LICENSE │ ├── Makefile │ ├── april_tag_detector.cpp │ ├── apriltag.c │ ├── apriltag.h │ ├── common.h │ ├── g2d.c │ ├── g2d.h │ ├── graymodel.c │ ├── graymodel.h │ ├── gridder.h │ ├── homography.c │ ├── homography.h │ ├── image_f32.c │ ├── image_f32.h │ ├── image_u32.c │ ├── image_u32.h │ ├── image_u8.c │ ├── image_u8.h │ ├── line_fit.h │ ├── matd.c │ ├── matd.h │ ├── segment2.c │ ├── tag36h10.c │ ├── tag36h10.h │ ├── tag36h11.c │ ├── tag36h11.h │ ├── tagtest.c │ ├── timeprofile.h │ ├── unionfind.c │ ├── unionfind.h │ ├── workerpool.c │ ├── workerpool.h │ ├── zarray.c │ ├── zarray.h │ ├── zhash.c │ └── zhash.h ├── calibration ├── CMakeLists.txt ├── calibration │ ├── extrinsics.yaml │ └── extrinsics_example.yaml ├── include │ └── calibration │ │ └── camera_to_velodyne.h ├── package.xml └── src │ └── camera_to_velodyne.cpp ├── ccicp2d ├── CMakeLists.txt ├── README ├── include │ └── ccicp2d │ │ ├── icp.h │ │ ├── icpPointToPlane.h │ │ ├── icpPointToPoint.h │ │ ├── icpTools.h │ │ ├── kdtree.h │ │ └── matrix.h ├── package.xml └── src │ ├── icp.cpp │ ├── icpPointToPlane.cpp │ ├── icpPointToPoint.cpp │ ├── icpTools.cpp │ ├── kdtree.cpp │ └── matrix.cpp ├── ekf ├── .DS_Store ├── CMakeLists.txt ├── package.xml └── src │ ├── nasa_ekf.cpp │ └── nasa_ekf.h ├── global_matching ├── .cproject ├── .project ├── .pydevproject ├── CATKIN_IGNORE ├── CMakeLists.txt ├── global_generate_rviz.vcg ├── global_match_rviz.vcg ├── global_matching.csv ├── mainpage.dox ├── matlab │ ├── calcTransform.m │ └── rigid_transform_3D.m ├── msg │ └── GlocCloud.msg ├── package.xml └── src │ ├── global_generate.cpp │ ├── global_match.cpp │ └── global_transform.cpp ├── graph_slam ├── CMakeLists.txt ├── msg │ └── Edge.msg ├── package.xml └── src │ ├── global_mapping_viz.cpp │ ├── global_mapping_viz.h │ ├── graphSlamTools.cpp │ ├── graphSlamTools.h │ ├── graph_slam.cpp │ ├── graph_slam.h │ ├── graph_slam │ ├── __init__.py │ ├── __init__.pyc │ └── msg │ │ ├── _Edge.py │ │ ├── _Node.py │ │ ├── _NodeArray.py │ │ └── __init__.py │ └── gs_test.cpp ├── ground_segmentation ├── CMakeLists.txt ├── README ├── include │ └── ground_segmentation │ │ ├── PointcloudXYZGD.h │ │ └── groundSegmentation.h ├── package.xml └── src │ ├── groundSegmentation.cpp │ └── pointcloud_filter.cpp ├── local_mapper ├── CMakeLists.txt ├── package.xml └── src │ └── local_mapper.cpp ├── mapping.rviz ├── mls ├── CMakeLists.txt ├── README ├── include │ └── mls │ │ └── mls.h ├── package.xml └── src │ └── mls.cpp ├── nasa_mapping ├── CMakeLists.txt ├── launch │ ├── drivers.launch │ └── nasa_mapping.launch └── package.xml ├── sample_mapping ├── CMakeLists.txt ├── hand_cal ├── include │ └── sample_mapping │ │ └── sample_mapping.h ├── package.xml └── src │ ├── sample_conversion_node.cpp │ ├── sample_mapping.cpp │ └── sample_mapping_node.cpp └── scan_registration ├── CMakeLists.txt ├── README ├── package.xml └── src ├── scan_registration.cpp └── scan_registration.h /README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/servos/SLAM/696e26e6924ed2edd3f1997b7516cdbce3e79f59/README.md -------------------------------------------------------------------------------- /april_tag_detector/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/servos/SLAM/696e26e6924ed2edd3f1997b7516cdbce3e79f59/april_tag_detector/.DS_Store -------------------------------------------------------------------------------- /april_tag_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(april_tag_detector) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs nav_msgs 8 | geometry_msgs tf tf_conversions ) 9 | 10 | catkin_package() 11 | 12 | ########### 13 | ## Build ## 14 | ########### 15 | set(CMAKE_BUILD_TYPE Release) 16 | include_directories( 17 | ${catkin_INCLUDE_DIRS} 18 | ) 19 | #common commands for building c++ executables and libraries 20 | add_executable(april_tag_detector src/april_tag_detector.cpp) 21 | target_link_libraries(april_tag_detector 22 | ${catkin_LIBRARIES} ) 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /april_tag_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | april_tag_detector 4 | 0.0.0 5 | april_tag_detector 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | tf 15 | tf_conversions 16 | nav_msgs 17 | 18 | roscpp 19 | std_msgs 20 | sensor_msgs 21 | geometry_msgs 22 | tf 23 | tf_conversions 24 | nav_msgs 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /april_tag_detector/src/LICENSE: -------------------------------------------------------------------------------- 1 | All of the code provided in this project is Copyright 2013 by the 2 | Regents of the University of Michigan. It is distributed under the 3 | terms of the GNU Lesser General Public License version 3. 4 | 5 | As the Copyright holders, we reserve the right to provide the software 6 | under alternative licenses in the future. Contact ebolson@umich.edu if 7 | you would like to negotiate an alternative licensing option. 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /april_tag_detector/src/Makefile: -------------------------------------------------------------------------------- 1 | CC=gcc -pthread -g -std=gnu99 -Wall -Wno-unused-parameter -Wno-format-zero-length -O4 2 | 3 | # For most x86_64 platforms this will be a bit faster. 4 | # 5 | # CC += -msse4.2 6 | 7 | TAGTEST_OBJS = apriltag.o tagtest.o image_f32.o image_u8.o image_u32.o unionfind.o zhash.o zarray.o matd.o homography.o graymodel.o tag36h11.o tag36h10.o segment2.o workerpool.o g2d.o 8 | 9 | all: tagtest 10 | 11 | 12 | tagtest: $(TAGTEST_OBJS) 13 | $(CC) -o $@ $(TAGTEST_OBJS) -lm -lpthread 14 | 15 | 16 | clean: 17 | rm -f $(TAGTEST_OBJS) *~ tagtest cachegrind.out* callgrind.out* gmon.out 18 | -------------------------------------------------------------------------------- /april_tag_detector/src/april_tag_detector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //definitions 4 | using namespace std; 5 | 6 | //global variables 7 | 8 | int main(int argc, char **argv) 9 | { 10 | 11 | ros::init(argc, argv, "april_tag_detector"); 12 | ros::NodeHandle n; 13 | ros::Rate loop_rate(20); 14 | 15 | //subscriber 16 | //FIXME camera image: ros::Subscriber imu_sub = n.subscribe("/imu/data", 1, imuCallback); 17 | 18 | //publisher 19 | //pose_pub = n.advertise ("mapping/apriltag/pose", 1); 20 | 21 | while (ros::ok()) 22 | { 23 | 24 | ros::spinOnce(); //check for new sensor messages 25 | loop_rate.sleep(); //sleep 26 | } 27 | 28 | return 0; 29 | } 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /april_tag_detector/src/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _COMMON_H 21 | #define _COMMON_H 22 | 23 | #include 24 | 25 | static inline int iclamp(int a, int min, int max) 26 | { 27 | if (a < min) 28 | return min; 29 | if (a > max) 30 | return max; 31 | return a; 32 | } 33 | 34 | static inline double dclamp(double a, double min, double max) 35 | { 36 | if (a < min) 37 | return min; 38 | if (a > max) 39 | return max; 40 | return a; 41 | } 42 | 43 | static inline int isq(int v) 44 | { 45 | return v*v; 46 | } 47 | 48 | static inline float sq(float v) 49 | { 50 | return v*v; 51 | } 52 | 53 | static inline int imin(int a, int b) 54 | { 55 | return a < b ? a : b; 56 | } 57 | 58 | static inline int imax(int a, int b) 59 | { 60 | return a < b ? b : a; 61 | } 62 | 63 | static inline float mod2pi(float v) 64 | { 65 | // XXX Dumb version 66 | while (v < -M_PI) 67 | v += 2*M_PI; 68 | 69 | while (v >= M_PI) 70 | v -= 2*M_PI; 71 | 72 | return v; 73 | } 74 | 75 | #endif 76 | 77 | 78 | -------------------------------------------------------------------------------- /april_tag_detector/src/g2d.h: -------------------------------------------------------------------------------- 1 | #ifndef _G2D_H 2 | #define _G2D_H 3 | 4 | #include "zarray.h" 5 | 6 | // This library tries to avoid needless proliferation of types. 7 | // 8 | // A point is a double[2]. (Note that when passing a double[2] as an 9 | // argument, it is passed by pointer, not by value.) 10 | // 11 | // A polygon is a zarray_t of double[2]. (Note that in this case, the 12 | // zarray contains the actual vertex data, and not merely a pointer to 13 | // some other data. IMPORTANT: A polygon must be specified in CCW 14 | // order. It is implicitly closed (do not list the same point at the 15 | // beginning at the end. 16 | // 17 | // Where sensible, it is assumed that objects should be allocated 18 | // sparingly; consequently "init" style methods, rather than "create" 19 | // methods are used. 20 | 21 | //////////////////////////////////////////////////////////////////// 22 | // Lines 23 | 24 | typedef struct 25 | { 26 | // Internal representation: a point that the line goes through (p) and 27 | // the direction of the line (u). 28 | double p[2]; 29 | double u[2]; // always a unit vector 30 | } g2d_line_t; 31 | 32 | // initialize a line object. 33 | void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2]); 34 | 35 | // The line defines a one-dimensional coordinate system whose origin 36 | // is p. Where is q? (If q is not on the line, the point nearest q is 37 | // returned. 38 | double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2]); 39 | 40 | // Intersect two lines. The intersection, if it exists, is written to 41 | // p (if not NULL), and 1 is returned. Else, zero is returned. 42 | int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p); 43 | 44 | //////////////////////////////////////////////////////////////////// 45 | // Line Segments. line.p is always one endpoint; p1 is the other 46 | // endpoint. 47 | typedef struct 48 | { 49 | g2d_line_t line; 50 | double p1[2]; 51 | } g2d_line_segment_t; 52 | 53 | void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2]); 54 | 55 | // Intersect two segments. The intersection, if it exists, is written 56 | // to p (if not NULL), and 1 is returned. Else, zero is returned. 57 | int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p); 58 | 59 | void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p); 60 | double g2d_line_segment_closest_point_distance(const g2d_line_segment_t *seg, const double *q); 61 | 62 | //////////////////////////////////////////////////////////////////// 63 | // Polygons 64 | 65 | zarray_t *g2d_polygon_create_data(double v[][2], int sz); 66 | 67 | // Takes a polygon in either CW or CCW and modifies it (if necessary) 68 | // to be CCW. 69 | void g2d_polygon_make_ccw(zarray_t *poly); 70 | 71 | // Return 1 if point q lies within poly. 72 | int g2d_polygon_contains_point(const zarray_t *poly, double q[2]); 73 | 74 | // Do the edges of the polygons cross? (Does not test for containment). 75 | int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb); 76 | 77 | // Does polya completely contain polyb? 78 | int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb); 79 | 80 | // Is there some point which is in both polya and polyb? 81 | int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb); 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /april_tag_detector/src/graymodel.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "graymodel.h" 25 | #include "matd.h" 26 | 27 | graymodel_t *graymodel_create() 28 | { 29 | graymodel_t *gm = calloc(1, sizeof(graymodel_t)); 30 | gm->A = matd_create(4,4); 31 | gm->b = matd_create(4,1); 32 | return gm; 33 | } 34 | 35 | void graymodel_add_observation(graymodel_t *gm, double x, double y, double v) 36 | { 37 | double xy = x*y; 38 | 39 | // update only upper-right elements. A'A is symmetric, 40 | // we'll fill the other elements in later. 41 | MAT_EL(gm->A, 0, 0) += x*x; 42 | MAT_EL(gm->A, 0, 1) += x*y; 43 | MAT_EL(gm->A, 0, 2) += x*xy; 44 | MAT_EL(gm->A, 0, 3) += x; 45 | MAT_EL(gm->A, 1, 1) += y*y; 46 | MAT_EL(gm->A, 1, 2) += y*xy; 47 | MAT_EL(gm->A, 1, 3) += y; 48 | MAT_EL(gm->A, 2, 2) += xy*xy; 49 | MAT_EL(gm->A, 2, 3) += xy; 50 | MAT_EL(gm->A, 3, 3) += 1; 51 | 52 | MAT_EL(gm->b, 0, 0) += x*v; 53 | MAT_EL(gm->b, 1, 0) += y*v; 54 | MAT_EL(gm->b, 2, 0) += xy*v; 55 | MAT_EL(gm->b, 3, 0) += v; 56 | 57 | gm->n++; 58 | } 59 | 60 | void graymodel_solve(graymodel_t *gm) 61 | { 62 | if (gm->n > 6) { 63 | // make symmetric 64 | for (int i = 0; i < 4; i++) 65 | for (int j = i+1; j < 4; j++) 66 | MAT_EL(gm->A, j, i) = MAT_EL(gm->A, i, j); 67 | 68 | gm->x = matd_solve(gm->A, gm->b); 69 | } else { 70 | // use flat model 71 | gm->x = matd_create(4, 1); 72 | MAT_EL(gm->x, 3, 0) = MAT_EL(gm->b, 3, 0) / gm->n; 73 | } 74 | } 75 | 76 | double graymodel_interpolate(graymodel_t *gm, double x, double y) 77 | { 78 | assert(gm->x != NULL); 79 | return MAT_EL(gm->x, 0, 0)*x + MAT_EL(gm->x, 1, 0)*y + 80 | MAT_EL(gm->x, 2, 0)*x*y + MAT_EL(gm->x, 3, 0); 81 | } 82 | 83 | void graymodel_destroy(graymodel_t *gm) 84 | { 85 | matd_destroy(gm->A); 86 | matd_destroy(gm->b); 87 | if (gm->x != NULL) 88 | matd_destroy(gm->x); 89 | free(gm); 90 | } 91 | 92 | -------------------------------------------------------------------------------- /april_tag_detector/src/graymodel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _GRAYMODEL_H 21 | #define _GRAYMODEL_H 22 | 23 | #include "matd.h" 24 | 25 | typedef struct graymodel graymodel_t; 26 | struct graymodel 27 | { 28 | matd_t *A; 29 | matd_t *b; 30 | matd_t *x; 31 | 32 | int n; 33 | }; 34 | 35 | graymodel_t *graymodel_create(); 36 | 37 | void graymodel_add_observation(graymodel_t *gm, double x, double y, double v); 38 | 39 | void graymodel_solve(graymodel_t *gm); 40 | 41 | double graymodel_interpolate(graymodel_t *gm, double x, double y); 42 | 43 | void graymodel_destroy(graymodel_t *gm); 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /april_tag_detector/src/gridder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _GRIDDER_H 21 | #define _GRIDDER_H 22 | 23 | #include 24 | 25 | #define GRIDDER_MIN_ALLOC 8 26 | 27 | struct gridder_cell 28 | { 29 | int alloc; 30 | int size; 31 | void **p; 32 | }; 33 | 34 | typedef struct gridder gridder_t; 35 | struct gridder 36 | { 37 | int x0, y0; 38 | int cell_size; 39 | int width, height; 40 | 41 | struct gridder_cell *cells; 42 | }; 43 | 44 | typedef struct gridder_iterator gridder_iterator_t; 45 | struct gridder_iterator 46 | { 47 | gridder_t *g; 48 | int ix0, iy0, ix1, iy1; 49 | int ix, iy; 50 | int pos; 51 | }; 52 | 53 | static inline gridder_t *gridder_create(int x0, int y0, int x1, int y1, int cell_size) 54 | { 55 | gridder_t *g = calloc(1, sizeof(gridder_t)); 56 | g->x0 = x0; 57 | g->y0 = y0; 58 | g->cell_size = cell_size; 59 | 60 | g->width = (int) (x1 - x0 + cell_size - 1) / cell_size; 61 | g->height = (int) (y1 - y0 + cell_size - 1) / cell_size; 62 | 63 | g->cells = calloc(g->width * g->height, sizeof(struct gridder_cell)); 64 | return g; 65 | } 66 | 67 | static inline void gridder_destroy(gridder_t *g) 68 | { 69 | for (int idx = 0; idx < g->width * g->height; idx++) 70 | free(g->cells[idx].p); 71 | 72 | free(g->cells); 73 | free(g); 74 | } 75 | 76 | static inline void gridder_add(gridder_t *g, int x, int y, void *p) 77 | { 78 | int ix = (x - g->x0) / g->cell_size; 79 | if (ix < 0 || ix >= g->width) 80 | return; 81 | 82 | int iy = (y - g->y0) / g->cell_size; 83 | if (iy < 0 || iy >= g->height) 84 | return; 85 | 86 | int idx = iy*g->width + ix; 87 | 88 | struct gridder_cell *cell = &g->cells[idx]; 89 | 90 | if (cell->size == cell->alloc) { 91 | int newalloc = cell->alloc * 2; 92 | if (newalloc < GRIDDER_MIN_ALLOC) 93 | newalloc = GRIDDER_MIN_ALLOC; 94 | 95 | cell->p = realloc(cell->p, sizeof(void*) * newalloc); 96 | cell->alloc = newalloc; 97 | } 98 | 99 | cell->p[cell->size++] = p; 100 | } 101 | 102 | static inline int clamp(int x, int x0, int x1) 103 | { 104 | x = x < x0 ? x0 : x; 105 | x = x > x1 ? x1 : x; 106 | return x; 107 | } 108 | 109 | static inline void gridder_iterator_init(gridder_t *g, gridder_iterator_t *gi, int cx, int cy, int r) 110 | { 111 | gi->g = g; 112 | gi->ix0 = clamp((cx - r - g->x0) / g->cell_size, 0, g->width - 1); 113 | gi->ix1 = clamp((cx + r - g->x0) / g->cell_size, 0, g->width - 1); 114 | gi->iy0 = clamp((cy - r - g->y0) / g->cell_size, 0, g->height - 1); 115 | gi->iy1 = clamp((cy + r - g->y0) / g->cell_size, 0, g->height - 1); 116 | 117 | gi->ix = gi->ix0; 118 | gi->iy = gi->iy0; 119 | gi->pos = 0; 120 | } 121 | 122 | // returns null if no more. 123 | static inline void *gridder_iterator_next(gridder_iterator_t *gi) 124 | { 125 | gridder_t *g = gi->g; 126 | 127 | while (1) { 128 | int idx = gi->iy*g->width + gi->ix; 129 | 130 | if (gi->pos < g->cells[idx].size) 131 | return g->cells[idx].p[gi->pos++]; 132 | 133 | if (gi->ix < gi->ix1) { 134 | gi->ix++; 135 | gi->pos = 0; 136 | continue; 137 | } 138 | 139 | if (gi->iy < gi->iy1) { 140 | gi->ix = gi->ix0; 141 | gi->iy++; 142 | gi->pos = 0; 143 | continue; 144 | } 145 | 146 | return NULL; 147 | } 148 | } 149 | 150 | #endif 151 | -------------------------------------------------------------------------------- /april_tag_detector/src/homography.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _HOMOGRAPHY_H 21 | #define _HOMOGRAPHY_H 22 | 23 | // correspondences is a list of float[4]s, consisting of the points x 24 | // and y concatenated. We will compute a homography such that y = Hx 25 | matd_t *homography_compute(zarray_t *correspondences); 26 | 27 | void homography_project(const matd_t *H, double x, double y, double *ox, double *oy); 28 | 29 | matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy); 30 | matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D); 31 | 32 | /* 33 | static inline void homography_project01(const matd_t *H, double x, double y, double *ox, double *oy) 34 | { 35 | homography_project(H, 2*x - 1, 2*y - 1, ox, oy); 36 | } 37 | */ 38 | #endif 39 | -------------------------------------------------------------------------------- /april_tag_detector/src/image_f32.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #include 21 | #include 22 | #include "image_f32.h" 23 | 24 | image_f32_t *image_f32_create(int width, int height) 25 | { 26 | image_f32_t *fim = (image_f32_t*) calloc(1, sizeof(image_f32_t)); 27 | 28 | fim->width = width; 29 | fim->height = height; 30 | fim->stride = width; // XXX do better alignment 31 | 32 | fim->buf = calloc(fim->height * fim->stride, sizeof(float)); 33 | 34 | return fim; 35 | } 36 | 37 | // scales by 1/255u 38 | image_f32_t *image_f32_create_from_u8(image_u8_t *im) 39 | { 40 | image_f32_t *fim = image_f32_create(im->width, im->height); 41 | 42 | for (int y = 0; y < fim->height; y++) 43 | for (int x = 0; x < fim->width; x++) 44 | fim->buf[y*fim->stride + x] = im->buf[y*im->stride + x] / 255.0f; 45 | 46 | return fim; 47 | } 48 | 49 | void image_f32_destroy(image_f32_t *im) 50 | { 51 | free(im->buf); 52 | free(im); 53 | } 54 | -------------------------------------------------------------------------------- /april_tag_detector/src/image_f32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _IMAGE_F32_H 21 | #define _IMAGE_F32_H 22 | 23 | #include 24 | 25 | typedef struct image_f32 image_f32_t; 26 | 27 | struct image_f32 28 | { 29 | int width, height; 30 | int stride; // in units of floats 31 | 32 | float *buf; // indexed as buf[y*stride + x] 33 | }; 34 | 35 | #include "image_u8.h" 36 | 37 | image_f32_t *image_f32_create(int width, int height); 38 | 39 | image_f32_t *image_f32_create_from_u8(image_u8_t *im); 40 | 41 | void image_f32_destroy(image_f32_t *im); 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /april_tag_detector/src/image_u32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _IMAGE_U32_H 21 | #define _IMAGE_U32_H 22 | 23 | #include 24 | #include "image_u8.h" 25 | #include "image_f32.h" 26 | 27 | typedef struct image_u32 image_u32_t; 28 | struct image_u32 29 | { 30 | int width, height; 31 | int stride; 32 | 33 | uint32_t *buf; 34 | }; 35 | 36 | image_u32_t *image_u32_create(int width, int height); 37 | image_u32_t *image_u32_create_from_pnm(const char *path); 38 | image_u32_t *image_u32_create_from_u8(image_u8_t *in); 39 | image_u32_t *image_u32_copy(image_u32_t *src); 40 | 41 | // returns an array of 3 channels. 42 | image_u8_t **image_u32_split_channels(image_u32_t *src); 43 | 44 | void image_u32_clear(image_u32_t *im); 45 | void image_u32_destroy(image_u32_t *im); 46 | void image_u32_draw_line(image_u32_t *im, float x0, float y0, float x1, float y1, uint32_t v, int width); 47 | void image_u32_draw_circle(image_u32_t *im, float x0, float y0, float r, uint32_t v); 48 | 49 | int image_u32_write_pnm(image_u32_t *im, const char *path); 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /april_tag_detector/src/image_u8.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _IMAGE_U8_H 21 | #define _IMAGE_U8_H 22 | 23 | #include 24 | 25 | typedef struct image_u8 image_u8_t; 26 | struct image_u8 27 | { 28 | int width, height; 29 | int stride; 30 | 31 | uint8_t *buf; 32 | }; 33 | 34 | #include "image_f32.h" 35 | 36 | image_u8_t *image_u8_create(int width, int height); 37 | image_u8_t *image_u8_create_from_rgb3(int width, int height, uint8_t *rgb, int stride); 38 | image_u8_t *image_u8_create_from_f32(image_f32_t *fim); 39 | image_u8_t *image_u8_create_from_pnm(const char *path); 40 | 41 | image_u8_t *image_u8_copy(const image_u8_t *src); 42 | void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width); 43 | void image_u8_draw_circle(image_u8_t *im, float x0, float y0, float r, int v); 44 | 45 | void image_u8_clear(image_u8_t *im); 46 | void image_u8_destroy(image_u8_t *im); 47 | 48 | int image_u8_write_pgm(const image_u8_t *im, const char *path); 49 | 50 | void image_u8_darken(image_u8_t *im); 51 | void image_u8_gaussian_blur(image_u8_t *im, double sigma, int k); 52 | 53 | // 1.5, 2, 3, 4, ... supported 54 | image_u8_t *image_u8_decimate(image_u8_t *im, float factor); 55 | 56 | // rotate the image by 'rad' radians. (Rotated in the "intuitive 57 | // sense", i.e., if Y were up. When input values are unavailable, the 58 | // value 'pad' is inserted instead. The geometric center of the output 59 | // image corresponds to the geometric center of the input image. 60 | image_u8_t *image_u8_rotate(const image_u8_t *in, double rad, uint8_t pad); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /april_tag_detector/src/line_fit.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _LINE_FIT_H 21 | #define _LINE_FIT_H 22 | 23 | #include "common.h" 24 | 25 | #include 26 | 27 | #define LINE_FIT_TYPE float 28 | 29 | typedef struct line_fit line_fit_t; 30 | struct line_fit 31 | { 32 | LINE_FIT_TYPE mXX, mYY, mXY, mX, mY; // 1st and 2nd moments 33 | LINE_FIT_TYPE n; // total weight of points 34 | uint16_t x0, x1, y0, y1; // bounding box 35 | }; 36 | 37 | static inline void line_fit_init(line_fit_t *lf) 38 | { 39 | memset(lf, 0, sizeof(line_fit_t)); 40 | lf->x0 = 65535; 41 | lf->y0 = 65535; 42 | 43 | // since we only use pixel values, 0 is small enough for x1, y1. 44 | } 45 | 46 | static inline void line_fit_init_nozero_update(line_fit_t *lf, LINE_FIT_TYPE x, LINE_FIT_TYPE y, LINE_FIT_TYPE w) 47 | { 48 | lf->mX = w*x; 49 | lf->mY = w*y; 50 | lf->mXX = w*x*x; 51 | lf->mXY = w*x*y; 52 | lf->mYY = w*y*y; 53 | 54 | lf->n = w; 55 | 56 | lf->x0 = x; 57 | lf->x1 = x; 58 | lf->y0 = y; 59 | lf->y1 = y; 60 | } 61 | 62 | static inline void line_fit_init_nozero_update_w1(line_fit_t *lf, uint32_t x, uint32_t y) 63 | { 64 | lf->mX += x; 65 | lf->mY += y; 66 | lf->mXX += x*x; 67 | lf->mXY += x*y; 68 | lf->mYY += y*y; 69 | 70 | lf->n++; 71 | 72 | lf->x0 = x; 73 | lf->x1 = x; 74 | lf->y0 = y; 75 | lf->y1 = y; 76 | } 77 | 78 | // make this line fit object include all the data that was included in the other one. 79 | static inline void line_fit_combine(line_fit_t *lf, line_fit_t *other) 80 | { 81 | lf->mX += other->mX; 82 | lf->mY += other->mY; 83 | lf->mXX += other->mXX; 84 | lf->mXY += other->mXY; 85 | lf->mYY += other->mYY; 86 | 87 | lf->n += other->n; 88 | 89 | lf->x0 = imin(other->x0, lf->x0); 90 | lf->x1 = imax(other->x1, lf->x1); 91 | lf->y0 = imin(other->y0, lf->y0); 92 | lf->y1 = imax(other->y1, lf->y1); 93 | } 94 | 95 | static inline void line_fit_update(line_fit_t *lf, uint32_t x, uint32_t y, uint32_t w) 96 | { 97 | lf->mX += w*x; 98 | lf->mY += w*y; 99 | lf->mXX += w*x*x; 100 | lf->mXY += w*x*y; 101 | lf->mYY += w*y*y; 102 | 103 | lf->n += w; 104 | 105 | lf->x0 = imin(x, lf->x0); 106 | lf->x1 = imax(x, lf->x1); 107 | lf->y0 = imin(y, lf->y0); 108 | lf->y1 = imax(y, lf->y1); 109 | } 110 | 111 | static inline void line_fit_update_w1(line_fit_t *lf, uint32_t x, uint32_t y) 112 | { 113 | lf->mX += x; 114 | lf->mY += y; 115 | lf->mXX += x*x; 116 | lf->mXY += x*y; 117 | lf->mYY += y*y; 118 | 119 | lf->n += 1; 120 | 121 | lf->x0 = imin(x, lf->x0); 122 | lf->x1 = imax(x, lf->x1); 123 | lf->y0 = imin(y, lf->y0); 124 | lf->y1 = imax(y, lf->y1); 125 | } 126 | 127 | // writes a point that lies on the line in p0, the unit vector in u. 128 | static inline void line_fit_compute(line_fit_t *lf, float *p0, float *u) 129 | { 130 | float Ex = (float) (lf->mX / lf->n); 131 | float Ey = (float) (lf->mY / lf->n); 132 | float Cxx = (float) (lf->mXX / lf->n - Ex*Ex); 133 | float Cxy = (float) (lf->mXY / lf->n - Ex*Ey); 134 | float Cyy = (float) (lf->mYY / lf->n - Ey*Ey); 135 | 136 | float phi = 0.5f*atan2(-2*Cxy, (Cyy - Cxx)); 137 | 138 | // the direction of the line is -sin(phi), cos(phi), and it goes 139 | // through (Ex, Ey) 140 | p0[0] = Ex; 141 | p0[1] = Ey; 142 | u[0] = -sin(phi); 143 | u[1] = cos(phi); 144 | } 145 | 146 | #endif 147 | -------------------------------------------------------------------------------- /april_tag_detector/src/matd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | 20 | #ifndef _MATD_H 21 | #define _MATD_H 22 | 23 | #define MATD_EPS 1e-8 24 | 25 | typedef struct 26 | { 27 | int nrows, ncols; 28 | 29 | // data in row-major order (index = rows*ncols + col) 30 | double *data; 31 | } matd_t; 32 | 33 | #define MAT_EL(m, row,col) (m)->data[((row)*(m)->ncols + (col))] 34 | 35 | typedef struct 36 | { 37 | // was the input matrix singular? When a near-zero pivot is found, 38 | // it is replaced with a value of MATD_EPS so that an approximate inverse 39 | // can be found. This flag is set to indicate that this has happened. 40 | int singular; 41 | 42 | int *piv; // permutation indices 43 | int pivsign; 44 | matd_t *lu; // combined L and U matrices, permuted. 45 | } matd_lu_t; 46 | 47 | ////////////////////////////////// 48 | // Creating and Destroying 49 | matd_t *matd_create(int rows, int cols); 50 | matd_t *matd_create_data(int rows, int cols, const double *data); 51 | 52 | // NB: Scalars are different than 1x1 matrices (implementation note: 53 | // they are encoded as 0x0 matrices). For example: for matrices A*B, A 54 | // and B must both have specific dimensions. However, if A is a 55 | // scalar, there are no restrictions on the size of B. 56 | matd_t *matd_create_scalar(double v); 57 | 58 | matd_t *matd_copy(const matd_t *m); 59 | matd_t *matd_identity(int dim); 60 | void matd_destroy(matd_t *m); 61 | 62 | 63 | ////////////////////////////////// 64 | // Basic Operations 65 | 66 | // fmt is printf specifier for each individual element. Each row will 67 | // be printed on a separate newline. 68 | void matd_print(const matd_t *m, const char *fmt); 69 | 70 | matd_t *matd_add(const matd_t *a, const matd_t *b); 71 | matd_t *matd_subtract(const matd_t *a, const matd_t *b); 72 | matd_t *matd_scale(const matd_t *a, double s); 73 | 74 | matd_t *matd_multiply(const matd_t *a, const matd_t *b); 75 | matd_t *matd_transpose(const matd_t *a); 76 | 77 | // matd_inverse attempts to compute an inverse. Fast (and less 78 | // numerically stable) methods are used for 2x2, 3x3, and 4x4 79 | // matrices. Nearly singular matrices are "patched up" by replacing 80 | // near-zero pivots or determinants with MATD_EPS. 81 | matd_t *matd_inverse(const matd_t *a); 82 | 83 | matd_t *matd_solve(matd_t *A, matd_t *b); 84 | 85 | double matd_det(const matd_t *a); 86 | 87 | matd_t *matd_op(const char *expr, ...); 88 | 89 | //////////////////////////////// 90 | // LU Decomposition 91 | 92 | /** The LU decomposition exists for square, full-rank matrices. This 93 | * function computes the factorization; actually doing something with 94 | * the factorization requires another call. The caller should call 95 | * matd_lu_destroy on the return value. Note that for rank-deficient 96 | * matrices, MATD_EPS will be substituted for the zero pivot, which 97 | * allows the factorization to continue (albeit with poor numerical 98 | * stability). 99 | **/ 100 | matd_lu_t *matd_lu(const matd_t *a); 101 | void matd_lu_destroy(matd_lu_t *mlu); 102 | 103 | /** Compute the determinant of the matrix. **/ 104 | double matd_lu_det(const matd_lu_t *lu); 105 | 106 | /** Retrieve the L factor. **/ 107 | matd_t *matd_lu_l(const matd_lu_t *lu); 108 | 109 | /** Retrieve the U factor. **/ 110 | matd_t *matd_lu_u(const matd_lu_t *lu); 111 | 112 | /** Compute an answer to Ax = b. **/ 113 | matd_t *matd_lu_solve(const matd_lu_t *mlu, const matd_t *b); 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /april_tag_detector/src/tag36h10.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #ifndef _TAG36H10 20 | #define _TAG36H10 21 | 22 | april_tag_family_t *tag36h10_create(); 23 | void tag36h10_destroy(april_tag_family_t *tf); 24 | #endif 25 | -------------------------------------------------------------------------------- /april_tag_detector/src/tag36h11.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #ifndef _TAG36H11 20 | #define _TAG36H11 21 | 22 | april_tag_family_t *tag36h11_create(); 23 | void tag36h11_destroy(april_tag_family_t *tf); 24 | #endif 25 | -------------------------------------------------------------------------------- /april_tag_detector/src/tagtest.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "apriltag.h" 26 | #include "image_u8.h" 27 | #include "tag36h11.h" 28 | #include "tag36h10.h" 29 | #include "zarray.h" 30 | 31 | // Invoke: 32 | // 33 | // tagtest [options] input.pnm 34 | 35 | int main(int argc, char *argv[]) 36 | { 37 | april_tag_family_t *tf = tag36h11_create(); 38 | 39 | april_tag_detector_t *td = april_tag_detector_create(tf); 40 | td->small_tag_refinement = 0; 41 | 42 | int maxiters = 1; 43 | 44 | zarray_t *inputs = zarray_create(sizeof(char*)); 45 | int waitsec = 0; 46 | 47 | for (int i = 1; i < argc; i++) { 48 | if (!strcmp(argv[i], "-d")) 49 | td->debug = 1; 50 | else if (!strcmp(argv[i], "-t")) 51 | td->nthreads = atoi(argv[++i]); 52 | else if (!strcmp(argv[i], "-f")) 53 | td->seg_decimate = (i+1 < argc && isdigit(argv[i+1][0])) ? atof(argv[++i]) : 2; 54 | else if (!strcmp(argv[i], "-i")) 55 | maxiters = atoi(argv[++i]); 56 | else if (!strcmp(argv[i], "-r")) 57 | td->small_tag_refinement = 1; 58 | else if (!strcmp(argv[i], "-w")) 59 | waitsec = atoi(argv[++i]); 60 | else if (!strcmp(argv[i], "-b")) 61 | td->seg_sigma = atof(argv[++i]); 62 | /* else if (!strcmp(argv[i], "--family")) { 63 | char *fam = argv[++i]; 64 | if (!strcmp(fam, "36h11")) 65 | td->tag_family = tag36h11_create(); 66 | else if (!strcmp(fam, "36h10")) 67 | td->tag_family = tag36h10_create(); 68 | } */ 69 | else 70 | zarray_add(inputs, &argv[i]); 71 | } 72 | 73 | for (int iter = 0; iter < maxiters; iter++) { 74 | 75 | if (maxiters > 1) 76 | printf("iter %d / %d\n", iter + 1, maxiters); 77 | 78 | for (int input = 0; input < zarray_size(inputs); input++) { 79 | 80 | char *path; 81 | zarray_get(inputs, input, &path); 82 | printf("loading %s\n", path); 83 | image_u8_t *im = image_u8_create_from_pnm(path); 84 | if (im == NULL) { 85 | printf("couldn't find %s\n", path); 86 | continue; 87 | } 88 | 89 | zarray_t *detections = april_tag_detector_detect(td, im); 90 | 91 | for (int i = 0; i < zarray_size(detections); i++) { 92 | april_tag_detection_t *det; 93 | zarray_get(detections, i, &det); 94 | 95 | printf("detection %3d: id %4d, hamming %d, goodness %f\n", i, det->id, det->hamming, det->goodness); 96 | april_tag_detection_destroy(det); 97 | } 98 | 99 | zarray_destroy(detections); 100 | 101 | timeprofile_display(td->tp); 102 | printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads); 103 | 104 | image_u8_destroy(im); 105 | 106 | if (zarray_size(inputs) > 1 || iter > 0) 107 | sleep(waitsec); 108 | } 109 | } 110 | 111 | april_tag_detector_destroy(td); 112 | 113 | tag36h11_destroy(tf); 114 | return 0; 115 | } 116 | -------------------------------------------------------------------------------- /april_tag_detector/src/timeprofile.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #ifndef _TIME_PROFILE_H 20 | #define _TIME_PROFILE_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "zarray.h" 27 | 28 | struct timeprofile_entry 29 | { 30 | char name[32]; 31 | int64_t utime; 32 | }; 33 | 34 | typedef struct timeprofile timeprofile_t; 35 | struct timeprofile 36 | { 37 | int64_t utime; 38 | zarray_t *stamps; 39 | }; 40 | 41 | static inline int64_t utime_now() 42 | { 43 | struct timeval tv; 44 | gettimeofday (&tv, NULL); 45 | return (int64_t) tv.tv_sec * 1000000 + tv.tv_usec; 46 | } 47 | 48 | static inline timeprofile_t *timeprofile_create() 49 | { 50 | timeprofile_t *tp = calloc(1, sizeof(timeprofile_t)); 51 | tp->stamps = zarray_create(sizeof(struct timeprofile_entry)); 52 | 53 | tp->utime = utime_now(); 54 | 55 | return tp; 56 | } 57 | 58 | static inline void timeprofile_destroy(timeprofile_t *tp) 59 | { 60 | zarray_destroy(tp->stamps); 61 | free(tp); 62 | } 63 | 64 | static inline void timeprofile_clear(timeprofile_t *tp) 65 | { 66 | zarray_clear(tp->stamps); 67 | tp->utime = utime_now(); 68 | } 69 | 70 | static inline void timeprofile_stamp(timeprofile_t *tp, const char *name) 71 | { 72 | struct timeprofile_entry tpe; 73 | 74 | strncpy(tpe.name, name, sizeof(tpe.name)); 75 | tpe.name[sizeof(tpe.name)-1] = 0; 76 | tpe.utime = utime_now(); 77 | 78 | zarray_add(tp->stamps, &tpe); 79 | } 80 | 81 | static inline void timeprofile_display(timeprofile_t *tp) 82 | { 83 | int64_t lastutime = tp->utime; 84 | 85 | for (int i = 0; i < zarray_size(tp->stamps); i++) { 86 | struct timeprofile_entry *stamp; 87 | 88 | zarray_get_volatile(tp->stamps, i, &stamp); 89 | 90 | double cumtime = (stamp->utime - tp->utime)/1000000.0; 91 | 92 | double parttime = (stamp->utime - lastutime)/1000000.0; 93 | 94 | printf("%2d %32s %15f ms %15f ms\n", i, stamp->name, parttime*1000, cumtime*1000); 95 | 96 | lastutime = stamp->utime; 97 | } 98 | } 99 | 100 | static inline uint64_t timeprofile_total_utime(timeprofile_t *tp) 101 | { 102 | if (zarray_size(tp->stamps) == 0) 103 | return 0; 104 | 105 | struct timeprofile_entry *first, *last; 106 | zarray_get_volatile(tp->stamps, 0, &first); 107 | zarray_get_volatile(tp->stamps, zarray_size(tp->stamps) - 1, &last); 108 | 109 | return last->utime - first->utime; 110 | } 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /april_tag_detector/src/unionfind.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #include "unionfind.h" 20 | #include 21 | #include 22 | 23 | unionfind_t *unionfind_create(uint32_t maxid) 24 | { 25 | unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t)); 26 | uf->maxid = maxid; 27 | uf->data = (struct ufrec*) calloc(maxid+1, sizeof(struct ufrec)); 28 | 29 | return uf; 30 | } 31 | 32 | void unionfind_destroy(unionfind_t *uf) 33 | { 34 | free(uf->data); 35 | free(uf); 36 | } 37 | -------------------------------------------------------------------------------- /april_tag_detector/src/unionfind.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #ifndef _UNIONFIND_H 20 | #define _UNIONFIND_H 21 | 22 | #include 23 | 24 | typedef struct unionfind unionfind_t; 25 | 26 | struct unionfind 27 | { 28 | uint32_t maxid; 29 | struct ufrec *data; 30 | }; 31 | 32 | struct ufrec 33 | { 34 | // a parent of zero means that the parent is its own parent. (this 35 | // speeds initialization). This is slightly interesting for node 36 | // 0, but it is initialized to its own parent, so it works out 37 | // okay. 38 | uint32_t parent; 39 | 40 | // actual size - 1 (so that on initialization it can be zero) 41 | uint32_t size; 42 | }; 43 | 44 | unionfind_t *unionfind_create(uint32_t maxid); 45 | void unionfind_destroy(unionfind_t *uf); 46 | 47 | static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) 48 | { 49 | // base case: a node is its own parent 50 | if (uf->data[id].parent == 0) 51 | return id; 52 | 53 | // otherwise, recurse 54 | uint32_t root = unionfind_get_representative(uf, uf->data[id].parent); 55 | 56 | // short circuit the path 57 | uf->data[id].parent = root; 58 | 59 | return root; 60 | } 61 | 62 | static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id) 63 | { 64 | uint32_t repid = unionfind_get_representative(uf, id); 65 | return uf->data[repid].size + 1; 66 | } 67 | 68 | static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid) 69 | { 70 | uint32_t aroot = unionfind_get_representative(uf, aid); 71 | uint32_t broot = unionfind_get_representative(uf, bid); 72 | 73 | if (aroot == broot) 74 | return aroot; 75 | 76 | uint32_t asize = uf->data[aroot].size; 77 | uint32_t bsize = uf->data[broot].size; 78 | 79 | if (asize > bsize) { 80 | uf->data[broot].parent = aroot; 81 | uf->data[aroot].size += bsize + 1; 82 | return aroot; 83 | } else { 84 | uf->data[aroot].parent = broot; 85 | uf->data[broot].size += asize + 1; 86 | return broot; 87 | } 88 | } 89 | #endif 90 | -------------------------------------------------------------------------------- /april_tag_detector/src/workerpool.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #define __USE_GNU 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "workerpool.h" 25 | #include "timeprofile.h" 26 | #include 27 | 28 | struct workerpool { 29 | int nthreads; 30 | zarray_t *tasks; 31 | int taskspos; 32 | 33 | pthread_t *threads; 34 | int *status; 35 | 36 | pthread_mutex_t mutex; 37 | pthread_cond_t startcond; // used to signal the availability of work 38 | pthread_cond_t endcond; // used to signal completion of all work 39 | 40 | int end_count; // how many threads are done? 41 | }; 42 | 43 | struct task 44 | { 45 | void (*f)(void *p); 46 | void *p; 47 | }; 48 | 49 | void *worker_thread(void *p) 50 | { 51 | workerpool_t *wp = (workerpool_t*) p; 52 | 53 | int cnt = 0; 54 | 55 | while (1) { 56 | struct task *task; 57 | 58 | pthread_mutex_lock(&wp->mutex); 59 | while (wp->taskspos == zarray_size(wp->tasks)) { 60 | wp->end_count++; 61 | // printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt); 62 | pthread_cond_broadcast(&wp->endcond); 63 | pthread_cond_wait(&wp->startcond, &wp->mutex); 64 | cnt = 0; 65 | // printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self()); 66 | } 67 | 68 | zarray_get_volatile(wp->tasks, wp->taskspos, &task); 69 | wp->taskspos++; 70 | cnt++; 71 | pthread_mutex_unlock(&wp->mutex); 72 | // pthread_yield(); 73 | sched_yield(); 74 | 75 | // we've been asked to exit. 76 | if (task->f == NULL) 77 | return NULL; 78 | 79 | task->f(task->p); 80 | } 81 | 82 | return NULL; 83 | } 84 | 85 | workerpool_t *workerpool_create(int nthreads) 86 | { 87 | assert(nthreads > 0); 88 | 89 | workerpool_t *wp = calloc(1, sizeof(workerpool_t)); 90 | wp->nthreads = nthreads; 91 | wp->tasks = zarray_create(sizeof(struct task)); 92 | 93 | if (nthreads > 1) { 94 | wp->threads = calloc(wp->nthreads, sizeof(pthread_t)); 95 | 96 | pthread_mutex_init(&wp->mutex, NULL); 97 | pthread_cond_init(&wp->startcond, NULL); 98 | pthread_cond_init(&wp->endcond, NULL); 99 | 100 | for (int i = 0; i < nthreads; i++) { 101 | int res = pthread_create(&wp->threads[i], NULL, worker_thread, wp); 102 | if (res != 0) { 103 | perror("pthread_create"); 104 | exit(-1); 105 | } 106 | } 107 | } 108 | 109 | return wp; 110 | } 111 | 112 | void workerpool_destroy(workerpool_t *wp) 113 | { 114 | if (wp == NULL) 115 | return; 116 | 117 | // force all worker threads to exit. 118 | if (wp->nthreads > 1) { 119 | for (int i = 0; i < wp->nthreads; i++) 120 | workerpool_add_task(wp, NULL, NULL); 121 | 122 | pthread_mutex_lock(&wp->mutex); 123 | pthread_cond_broadcast(&wp->startcond); 124 | pthread_mutex_unlock(&wp->mutex); 125 | 126 | for (int i = 0; i < wp->nthreads; i++) 127 | pthread_join(wp->threads[i], NULL); 128 | 129 | pthread_mutex_destroy(&wp->mutex); 130 | pthread_cond_destroy(&wp->startcond); 131 | pthread_cond_destroy(&wp->endcond); 132 | free(wp->threads); 133 | } 134 | 135 | zarray_destroy(wp->tasks); 136 | free(wp); 137 | } 138 | 139 | int workerpool_get_nthreads(workerpool_t *wp) 140 | { 141 | return wp->nthreads; 142 | } 143 | 144 | void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p) 145 | { 146 | struct task t; 147 | t.f = f; 148 | t.p = p; 149 | 150 | zarray_add(wp->tasks, &t); 151 | } 152 | 153 | void workerpool_run_single(workerpool_t *wp) 154 | { 155 | for (int i = 0; i < zarray_size(wp->tasks); i++) { 156 | struct task *task; 157 | zarray_get_volatile(wp->tasks, i, &task); 158 | task->f(task->p); 159 | } 160 | 161 | zarray_clear(wp->tasks); 162 | } 163 | 164 | // runs all added tasks, waits for them to complete. 165 | void workerpool_run(workerpool_t *wp) 166 | { 167 | if (wp->nthreads > 1) { 168 | wp->end_count = 0; 169 | 170 | pthread_mutex_lock(&wp->mutex); 171 | pthread_cond_broadcast(&wp->startcond); 172 | 173 | while (wp->end_count < wp->nthreads) { 174 | // printf("caught %d\n", wp->end_count); 175 | pthread_cond_wait(&wp->endcond, &wp->mutex); 176 | } 177 | 178 | pthread_mutex_unlock(&wp->mutex); 179 | 180 | wp->taskspos = 0; 181 | 182 | zarray_clear(wp->tasks); 183 | 184 | } else { 185 | workerpool_run_single(wp); 186 | } 187 | } 188 | -------------------------------------------------------------------------------- /april_tag_detector/src/workerpool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the AprilTag library. 3 | * 4 | * AprilTag is free software; you can redistribute it and/or modify it 5 | * under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation; either version 2.1 of 7 | * the License, or (at your option) any later version. 8 | * 9 | * AprilTag is distributed in the hope that it will be useful, but 10 | * WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public 15 | * License along with Libav; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 17 | * 02110-1301 USA 18 | */ 19 | #ifndef _WORKERPOOL_H 20 | #define _WORKERPOOL_H 21 | 22 | #include "zarray.h" 23 | 24 | typedef struct workerpool workerpool_t; 25 | 26 | // as a special case, if nthreads==1, no additional threads are 27 | // created, and workerpool_run will run synchronously. 28 | workerpool_t *workerpool_create(int nthreads); 29 | void workerpool_destroy(workerpool_t *wp); 30 | 31 | void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p); 32 | 33 | // runs all added tasks, waits for them to complete. 34 | void workerpool_run(workerpool_t *wp); 35 | 36 | // same as workerpool_run, except always single threaded. (mostly for debugging). 37 | void workerpool_run_single(workerpool_t *wp); 38 | 39 | int workerpool_get_nthreads(workerpool_t *wp); 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /april_tag_detector/src/zarray.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "zarray.h" 7 | 8 | #define MIN_ALLOC 8 9 | 10 | struct zarray 11 | { 12 | size_t el_sz; // size of each element 13 | 14 | int size; // how many elements? 15 | int alloc; // we've allocated storage for how many elements? 16 | char *data; 17 | }; 18 | 19 | zarray_t *zarray_create(size_t el_sz) 20 | { 21 | assert(el_sz > 0); 22 | 23 | zarray_t *za = calloc(1, sizeof(zarray_t)); 24 | za->el_sz = el_sz; 25 | return za; 26 | } 27 | 28 | void zarray_destroy(zarray_t *za) 29 | { 30 | if (za == NULL) 31 | return; 32 | 33 | if (za->data != NULL) 34 | free(za->data); 35 | free(za); 36 | } 37 | 38 | zarray_t *zarray_copy(const zarray_t *za) 39 | { 40 | zarray_t *zb = calloc(1, sizeof(zarray_t)); 41 | zb->el_sz = za->el_sz; 42 | zb->size = za->size; 43 | zb->alloc = za->alloc; 44 | zb->data = malloc(zb->alloc * zb->el_sz); 45 | memcpy(zb->data, za->data, za->size * za->el_sz); 46 | return zb; 47 | } 48 | 49 | void zarray_clear(zarray_t *za) 50 | { 51 | assert(za != NULL); 52 | za->size = 0; 53 | } 54 | 55 | int zarray_size(const zarray_t *za) 56 | { 57 | assert(za != NULL); 58 | 59 | return za->size; 60 | } 61 | 62 | inline void zarray_ensure_capacity(zarray_t *za, int capacity) 63 | { 64 | assert(za != NULL); 65 | 66 | if (za->alloc <= capacity) { 67 | 68 | capacity = za->alloc * 2; 69 | 70 | if (capacity < MIN_ALLOC) 71 | capacity = MIN_ALLOC; 72 | 73 | za->data = realloc(za->data, za->el_sz * capacity); 74 | za->alloc = capacity; 75 | } 76 | } 77 | 78 | // pass a pointer to the value you wish to store. 79 | void zarray_add(zarray_t *za, const void *p) 80 | { 81 | assert(za != NULL); 82 | assert(p != NULL); 83 | 84 | zarray_ensure_capacity(za, za->size + 1); 85 | 86 | memcpy(&za->data[za->size*za->el_sz], p, za->el_sz); 87 | za->size++; 88 | } 89 | 90 | // pass a pointer to storage for the value you wish to retrieve 91 | void zarray_get(const zarray_t *za, int idx, void *p) 92 | { 93 | assert(za != NULL); 94 | assert(p != NULL); 95 | assert(idx >= 0); 96 | assert(idx < za->size); 97 | 98 | memcpy(p, &za->data[idx*za->el_sz], za->el_sz); 99 | } 100 | 101 | void zarray_get_volatile(const zarray_t *za, int idx, void *p) 102 | { 103 | assert(za != NULL); 104 | assert(p != NULL); 105 | assert(idx >= 0); 106 | assert(idx < za->size); 107 | 108 | *((void**) p) = &za->data[idx*za->el_sz]; 109 | } 110 | 111 | // remove the entry whose value is equal to the value pointed to by p. 112 | // if shuffle is true, the last element in the array will be placed in 113 | // the newly-open space; if false, the zarray is compacted. 114 | int zarray_remove_value(zarray_t *za, const void *p, int shuffle) 115 | { 116 | assert(za != NULL); 117 | assert(p != NULL); 118 | 119 | for (int idx = 0; idx < za->size; idx++) { 120 | if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) { 121 | zarray_remove_index(za, idx, shuffle); 122 | return 1; 123 | } 124 | } 125 | 126 | return 0; 127 | } 128 | 129 | void zarray_remove_index(zarray_t *za, int idx, int shuffle) 130 | { 131 | assert(za != NULL); 132 | assert(idx >= 0); 133 | assert(idx < za->size); 134 | 135 | if (shuffle) { 136 | if (idx < za->size-1) 137 | memcpy(&za->data[idx*za->el_sz], &za->data[(za->size-1)*za->el_sz], za->el_sz); 138 | za->size--; 139 | return; 140 | } else { 141 | // size = 10, idx = 7. Should copy 2 entries (at idx=8 and idx=9). 142 | // size = 10, idx = 9. Should copy 0 entries. 143 | int ncopy = za->size - idx - 1; 144 | if (ncopy > 0) 145 | memmove(&za->data[idx*za->el_sz], &za->data[(idx+1)*za->el_sz], ncopy*za->el_sz); 146 | za->size--; 147 | return; 148 | } 149 | } 150 | 151 | void zarray_insert(zarray_t *za, int idx, const void *p) 152 | { 153 | assert(za != NULL); 154 | assert(p != NULL); 155 | assert(idx >= 0); 156 | assert(idx <= za->size); 157 | 158 | zarray_ensure_capacity(za, za->size + 1); 159 | // size = 10, idx = 7. Should copy three entries (idx=7, idx=8, idx=9) 160 | int ncopy = za->size - idx; 161 | 162 | memmove(&za->data[(idx+1)*za->el_sz], &za->data[idx*za->el_sz], ncopy*za->el_sz); 163 | memcpy(&za->data[idx*za->el_sz], p, za->el_sz); 164 | 165 | za->size++; 166 | } 167 | 168 | void zarray_set(zarray_t *za, int idx, const void *p, void *outp) 169 | { 170 | assert(za != NULL); 171 | assert(p != NULL); 172 | assert(idx >= 0); 173 | assert(idx < za->size); 174 | 175 | if (outp != NULL) 176 | memcpy(outp, &za->data[idx*za->el_sz], za->el_sz); 177 | 178 | memcpy(&za->data[idx*za->el_sz], p, za->el_sz); 179 | } 180 | 181 | void zarray_map(zarray_t *za, void (*f)()) 182 | { 183 | assert(za != NULL); 184 | assert(f != NULL); 185 | 186 | for (int idx = 0; idx < za->size; idx++) 187 | f(&za->data[idx*za->el_sz]); 188 | } 189 | 190 | void zarray_vmap(zarray_t *za, void (*f)()) 191 | { 192 | assert(za != NULL); 193 | assert(f != NULL); 194 | assert(za->el_sz == sizeof(void*)); 195 | 196 | for (int idx = 0; idx < za->size; idx++) { 197 | void *pp = &za->data[idx*za->el_sz]; 198 | void *p = *(void**) pp; 199 | f(p); 200 | } 201 | } 202 | 203 | void zarray_sort(zarray_t *za, int (*compar)(const void*, const void*)) 204 | { 205 | assert(za != NULL); 206 | assert(compar != NULL); 207 | 208 | qsort(za->data, za->size, za->el_sz, compar); 209 | } 210 | 211 | 212 | int zarray_contains(const zarray_t *za, const void *p) 213 | { 214 | assert(za != NULL); 215 | assert(p != NULL); 216 | 217 | for (int idx = 0; idx < za->size; idx++) { 218 | if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) { 219 | return 1; 220 | } 221 | } 222 | 223 | return 0; 224 | } 225 | 226 | int zstrcmp(const void * a_pp, const void * b_pp) 227 | { 228 | assert(a_pp != NULL); 229 | assert(b_pp != NULL); 230 | 231 | char * a = *(void**)a_pp; 232 | char * b = *(void**)b_pp; 233 | 234 | return strcmp(a,b); 235 | } 236 | 237 | // returns -1 if not in array. Remember p is a pointer to the item. 238 | int zarray_index_of(const zarray_t *za, const void *p) 239 | { 240 | for (int i = 0; i < za->size; i++) { 241 | if (!memcmp(p, &za->data[i*za->el_sz], za->el_sz)) 242 | return i; 243 | } 244 | 245 | return -1; 246 | } 247 | -------------------------------------------------------------------------------- /april_tag_detector/src/zarray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ZARRAY_H 2 | #define _ZARRAY_H 3 | 4 | #include 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | /** 11 | * Defines a structure which acts as a resize-able array ala Java's ArrayList. 12 | */ 13 | typedef struct zarray zarray_t; 14 | 15 | /** 16 | * Creates and returns a variable array structure capable of holding elements of 17 | * the specified size. It is the caller's responsibility to call zarray_destroy() 18 | * on the returned array when it is no longer needed. 19 | */ 20 | zarray_t *zarray_create(size_t el_sz); 21 | 22 | /** 23 | * Frees all resources associated with the variable array structure which was 24 | * created by zarray_create(). After calling, 'za' will no longer be valid for storage. 25 | */ 26 | void zarray_destroy(zarray_t *za); 27 | 28 | /** Allocate a new zarray that contains a copy of the data in the argument. **/ 29 | zarray_t *zarray_copy(const zarray_t *za); 30 | 31 | /** 32 | * Retrieves the number of elements currently being contained by the passed 33 | * array, which may be different from its capacity. The index of the last element 34 | * in the array will be one less than the returned value. 35 | */ 36 | int zarray_size(const zarray_t *za); 37 | 38 | /** 39 | * Allocates enough internal storage in the supplied variable array structure to 40 | * guarantee that the supplied number of elements (capacity) can be safely stored. 41 | */ 42 | void zarray_ensure_capacity(zarray_t *za, int capacity); 43 | 44 | /** 45 | * Adds a new element to the end of the supplied array, and sets its value 46 | * (by copying) from the data pointed to by the supplied pointer 'p'. 47 | * Automatically ensures that enough storage space is available for the new element. 48 | */ 49 | void zarray_add(zarray_t *za, const void *p); 50 | 51 | /** 52 | * Retrieves the element from the supplied array located at the zero-based 53 | * index of 'idx' and copies its value into the variable pointed to by the pointer 54 | * 'p'. 55 | */ 56 | void zarray_get(const zarray_t *za, int idx, void *p); 57 | 58 | /** 59 | * Similar to zarray_get(), but returns a "live" pointer to the internal 60 | * storage, avoiding a memcpy. This pointer is not valid across 61 | * operations which might move memory around (i.e. zarray_remove_value(), 62 | * zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()). 63 | * 'p' should be a pointer to the pointer which will be set to the internal address. 64 | */ 65 | void zarray_get_volatile(const zarray_t *za, int idx, void *p); 66 | 67 | /** 68 | * Remove the entry whose value is equal to the value pointed to by 'p'. 69 | * If shuffle is true, the last element in the array will be placed in 70 | * the newly-open space; if false, the zarray is compacted. At most 71 | * one element will be removed. 72 | * 73 | * Note that objects will be compared using memcmp over the full size 74 | * of the value. If the value is a struct that contains padding, 75 | * differences in the padding bytes can cause comparisons to 76 | * fail. Thus, it remains best practice to bzero all structs so that 77 | * the padding is set to zero. 78 | * 79 | * Returns the number of elements removed (0 or 1). 80 | */ 81 | int zarray_remove_value(zarray_t *za, const void *p, int shuffle); 82 | 83 | /** 84 | * Removes the entry at index 'idx'. 85 | * If shuffle is true, the last element in the array will be placed in 86 | * the newly-open space; if false, the zarray is compacted. 87 | */ 88 | void zarray_remove_index(zarray_t *za, int idx, int shuffle); 89 | 90 | /** 91 | * Creates a new entry and inserts it into the array so that it will have the 92 | * index 'idx' (i.e. before the item which currently has that index). The value 93 | * of the new entry is set to (copied from) the data pointed to by 'p'. 'idx' 94 | * can be one larger than the current max index to place the new item at the end 95 | * of the array, or zero to add it to an empty array. 96 | */ 97 | void zarray_insert(zarray_t *za, int idx, const void *p); 98 | 99 | /** 100 | * Sets the value of the current element at index 'idx' by copying its value from 101 | * the data pointed to by 'p'. The previous value of the changed element will be 102 | * copied into the data pointed to by 'outp' if it is not null. 103 | */ 104 | void zarray_set(zarray_t *za, int idx, const void *p, void *outp); 105 | 106 | /** 107 | * Calls the supplied function for every element in the array in index order. 108 | * The map function will be passed a pointer to each element in turn and must 109 | * have the following format: 110 | * 111 | * void map_function(element_type *element) 112 | */ 113 | void zarray_map(zarray_t *za, void (*f)()); 114 | 115 | /** 116 | * Calls the supplied function for every element in the array in index order. 117 | * HOWEVER values are passed to the function, not pointers to values. In the 118 | * case where the zarray stores object pointers, zarray_vmap allows you to 119 | * pass in the object's destroy function (or free) directly. Can only be used 120 | * with zarray's which contain pointer data. The map function should have the 121 | * following format: 122 | * 123 | * void map_function(element_type *element) 124 | */ 125 | void zarray_vmap(zarray_t *za, void (*f)()); 126 | 127 | /** 128 | * Removes all elements from the array and sets its size to zero. Pointers to 129 | * any data elements obtained i.e. by zarray_get_volatile() will no longer be 130 | * valid. 131 | */ 132 | void zarray_clear(zarray_t *za); 133 | 134 | /** 135 | * Determines whether any element in the array has a value which matches the 136 | * data pointed to by 'p'. 137 | * 138 | * Returns 1 if a match was found anywhere in the array, else 0. 139 | */ 140 | int zarray_contains(const zarray_t *za, const void *p); 141 | 142 | /** 143 | * Uses qsort() to sort the elements contained by the array in ascending order. 144 | * Uses the supplied comparison function to determine the appropriate order. 145 | * 146 | * The comparison function will be passed a pointer to two elements to be compared 147 | * and should return a measure of the difference between them (see strcmp()). 148 | * I.e. it should return a negative number if the first element is 'less than' 149 | * the second, zero if they are equivalent, and a positive number if the first 150 | * element is 'greater than' the second. The function should have the following format: 151 | * 152 | * int comparison_function(const element_type *first, const element_type *second) 153 | * 154 | * zstrcmp() can be used as the comparison function for string elements, which 155 | * will call strcmp() internally. 156 | */ 157 | void zarray_sort(zarray_t *za, int (*compar)(const void*, const void*)); 158 | 159 | /** 160 | * A comparison function for comparing strings which can be used by zarray_sort() 161 | * to sort arrays with char* elements. 162 | */ 163 | int zstrcmp(const void * a_pp, const void * b_pp); 164 | 165 | /** 166 | * Find the index of an element, or return -1 if not found. Remember that p is 167 | * a pointer to the element. 168 | **/ 169 | int zarray_index_of(const zarray_t *za, const void *p); 170 | 171 | #ifdef __cplusplus 172 | } 173 | #endif 174 | 175 | #endif 176 | -------------------------------------------------------------------------------- /calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(camera_to_velodyne) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs 8 | geometry_msgs pcl_ros tf tf_conversions nav_msgs eigen_conversions visualization_msgs) 9 | 10 | ################################### 11 | ## catkin specific configuration ## 12 | ################################### 13 | catkin_package(INCLUDE_DIRS include) 14 | 15 | ########### 16 | ## Build ## 17 | ########### 18 | set(CMAKE_BUILD_TYPE Release) 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | 25 | add_library(camera_to_velodyne src/camera_to_velodyne.cpp) 26 | target_link_libraries(camera_to_velodyne ${catkin_LIBRARIES} yaml-cpp) 27 | -------------------------------------------------------------------------------- /calibration/calibration/extrinsics.yaml: -------------------------------------------------------------------------------- 1 | - cam_id: 0 2 | projection_matrix: [11162.85801, 0.000000000000e+00, 1938.03850, 0, 0.000000000000e+00, 11109.93803, 1270.60007, 0, 0.000000000000e+00, 0.000000000000e+00, 1.000000000000e+00,0] 3 | extrinsics: [0.0607901, -0.205867, 0.97669, 0.313862, -0.99711, 0.0321484, 0.0688373, -0.302954,-0.0455703, -0.978052, -0.203318, -0.291717, 0, 0, 0, 1] 4 | distortion: [-0.13870, -0.00000, -0.00929, -0.00027, 0.00000] 5 | 6 | #note: this is the intrinsics for the 100mm zoom setting on the point and shoot camera!!!! 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /calibration/calibration/extrinsics_example.yaml: -------------------------------------------------------------------------------- 1 | - cam_id: 0 2 | projection_matrix: [1.1e-2,2.1,0000e0, 0, 1,2,3,4,6,7,8,9] 3 | extrinsics: [1, 2, 3, 4, 5,6,7,8,9,10,11,12,13,14,15,16] 4 | - cam_id: 1 5 | projection_matrix: [1.1e+3, 2.1e-2, 3.3, 0, 1,2,3,4,6,7,8,9] 6 | extrinsics: [1, 2, 3, 4, 5,6,7,8,9,10,11,12,13,14,15,16] 7 | - cam_id: 2 8 | projection_matrix: [1.1, 2.1, 3.3, 0, 1,2,3,4,6,7,8,9] 9 | extrinsics: [1, 2, 3, 4, 5,6,7,8,9,10,11,12,13,14,15,16] 10 | 11 | -------------------------------------------------------------------------------- /calibration/include/calibration/camera_to_velodyne.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_TO_VELODNYE_H 2 | #define CAMERA_TO_VELODNYE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace Eigen 20 | { 21 | typedef Eigen::Matrix Matrix34d; 22 | typedef Eigen::Matrix Vector5d; 23 | } 24 | 25 | struct camera_parameters 26 | { 27 | int cam_id; 28 | Eigen::Matrix34d projection_matrix; 29 | Eigen::Matrix4d extrinsics; //from camera frame to velodyne frame 30 | Eigen::Vector5d distortion_coefficients; 31 | Eigen::Matrix3d camera_matrix; 32 | 33 | 34 | }; 35 | 36 | void operator >> (const YAML::Node& node, Eigen::Matrix34d& v) { 37 | 38 | node[0] >> v(0,0); node[1] >> v(0,1); node[2] >> v(0,2); node[3] >> v(0,3); 39 | node[4] >> v(1,0); node[5] >> v(1,1); node[6] >> v(1,2); node[7] >> v(1,3); 40 | node[8] >> v(2,0); node[9] >> v(2,1); node[10] >> v(2,2); node[11] >> v(2,3); 41 | } 42 | 43 | void operator >> (const YAML::Node& node, Eigen::Matrix4d& v) { 44 | node[0] >> v(0,0); node[1] >> v(0,1); node[2] >> v(0,2); node[3] >> v(0,3); 45 | node[4] >> v(1,0); node[5] >> v(1,1); node[6] >> v(1,2); node[7] >> v(1,3); 46 | node[8] >> v(2,0); node[9] >> v(2,1); node[10] >> v(2,2); node[11] >> v(2,3); 47 | node[12] >> v(3,0); node[13] >> v(3,1); node[14] >> v(3,2); node[15] >> v(3,3); 48 | } 49 | 50 | void operator >> (const YAML::Node& node, Eigen::Vector5d& v) { 51 | node[0] >> v(0,0); node[1] >> v(1,0); node[2] >> v(2,0); node[3] >> v(3,0); node[4] >> v(4,0); 52 | } 53 | 54 | void operator >> (const YAML::Node& node, camera_parameters& cam) { 55 | node["cam_id"] >> cam.cam_id; 56 | node["projection_matrix"] >> cam.projection_matrix; 57 | node["extrinsics"] >> cam.extrinsics; 58 | node["distortion"] >> cam.distortion_coefficients; 59 | 60 | } 61 | 62 | 63 | 64 | class CameraToVelodyne { 65 | private: 66 | 67 | std::string fp; 68 | std::vector camera_vector; 69 | void read_yaml_file(std::string file_name); 70 | 71 | 72 | public: 73 | 74 | 75 | CameraToVelodyne(); //constructor 76 | Eigen::Vector3d rangexyz_to_cameraxyz(Eigen::Vector3d rangexyz, int cam_id); 77 | Eigen::Vector2d cameraxyz_to_camerauv(Eigen::Vector3d cameraxyz, int cam_id); 78 | Eigen::Vector3d camerauv_to_cameraray(Eigen::Vector2d camerauv, int cam_id); 79 | Eigen::Vector3d cameraray_to_rangeflatground(Eigen::Vector3d cameraray, int cam_id,double range_height); 80 | 81 | 82 | 83 | }; 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | camera_to_velodyne 4 | 0.0.0 5 | mls 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | nav_msgs 16 | tf 17 | eigen_conversions 18 | tf_conversions 19 | visualization_msgs 20 | 21 | roscpp 22 | std_msgs 23 | sensor_msgs 24 | geometry_msgs 25 | pcl_ros 26 | nav_msgs 27 | tf 28 | eigen_conversions 29 | tf_conversions 30 | visualization_msgs 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /calibration/src/camera_to_velodyne.cpp: -------------------------------------------------------------------------------- 1 | #include "calibration/camera_to_velodyne.h" 2 | 3 | using namespace std; 4 | 5 | void CameraToVelodyne::read_yaml_file(std::string file_name) 6 | { 7 | std::ifstream fin(file_name.c_str()); 8 | YAML::Parser parser(fin); 9 | YAML::Node doc; 10 | parser.GetNextDocument(doc); 11 | for(unsigned i=0;i> cam; 14 | 15 | //manually make camera matrix from projection matrix 16 | cam.camera_matrix = cam.projection_matrix.block<3,3>(0,0); 17 | camera_vector.push_back(cam); 18 | } 19 | 20 | //std::cout< 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "ccicp2d/matrix.h" 31 | #include "ccicp2d/kdtree.h" 32 | 33 | class Icp { 34 | 35 | public: 36 | 37 | // constructor 38 | // input: M_GA ....... pointer to first model point (ground adjacent) 39 | // M_NGA ....... pointer to first model point (non ground adjacent) 40 | // M_num ... number of model points 41 | // dim ... dimensionality of model points (2 or 3) 42 | Icp (double *M_GA, double *M_NGA ,const int32_t M_GA_num,const int32_t M_NGA_num,const int32_t dim); 43 | 44 | // deconstructor 45 | virtual ~Icp (); 46 | 47 | // set subsampling step size of coarse matching (1. stage) 48 | void setSubsamplingStep (int32_t val) { sub_step = val; } 49 | 50 | // set maximum number of iterations (1. stopping criterion) 51 | void setMaxIterations (int32_t val) { max_iter = val; } 52 | 53 | // set minimum delta of rot/trans parameters (2. stopping criterion) 54 | void setMinDeltaParam (double val) { min_delta = val; } 55 | 56 | // fit template to model yielding R,t (M = R*T + t) 57 | // input: T ....... pointer to first template point 58 | // T_num ... number of template points 59 | // R ....... initial rotation matrix 60 | // t ....... initial translation vector 61 | // indist .. inlier distance (if <=0: use all points) 62 | // output: R ....... final rotation matrix 63 | // t ....... final translation vector 64 | // h_thresh height threshold 65 | void fit(double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,Matrix &R,Matrix &t,double indist, double h_dist); 66 | 67 | void getEdgeWeight(double* eW); 68 | int getNumberCorrespondences(void); 69 | 70 | //for getting out the covariance (information) of a match 71 | 72 | 73 | private: 74 | 75 | // iterative fitting 76 | void fitIterate(double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,Matrix &R,Matrix &t,double indist); 77 | 78 | // inherited classes need to overwrite these functions 79 | virtual double fitStep(double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,Matrix &R,Matrix &t, double inDist) = 0; 80 | // virtual std::vector getInliers(double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,const Matrix &R,const Matrix &t,const double indist) = 0; 81 | 82 | protected: 83 | 84 | // kd tree of model points 85 | kdtree::KDTree* M_tree_GA; 86 | kdtree::KDTreeArray M_data_GA; 87 | kdtree::KDTree* M_tree_NGA; 88 | kdtree::KDTreeArray M_data_NGA; 89 | 90 | int M_GA_SIZE; 91 | int M_NGA_SIZE; 92 | 93 | int32_t dim; // dimensionality of model + template data (2 or 3) 94 | int32_t sub_step; // subsampling step size 95 | int32_t max_iter; // max number of iterations 96 | double min_delta; // min parameter delta 97 | Matrix cModelPoints; 98 | Matrix cScenePoints; 99 | int numCorr; 100 | 101 | 102 | }; 103 | 104 | #endif // ICP_H 105 | -------------------------------------------------------------------------------- /ccicp2d/include/ccicp2d/icpPointToPlane.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | libicp is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #ifndef ICP_POINT_TO_PLANE_H 22 | #define ICP_POINT_TO_PLANE_H 23 | 24 | #include "ccicp2d/icp.h" 25 | 26 | class IcpPointToPlane : public Icp { 27 | 28 | public: 29 | 30 | IcpPointToPlane (double *M,const int32_t M_num,const int32_t dim,const int32_t num_neighbors=10,const double flatness=5.0) : Icp(M,M_num,dim) { 31 | M_normal = computeNormals(num_neighbors,flatness); 32 | } 33 | 34 | virtual ~IcpPointToPlane () { 35 | delete M_normal; 36 | } 37 | 38 | private: 39 | 40 | double fitStep (double *T,const int32_t T_num,Matrix &R,Matrix &t,const std::vector &active); 41 | std::vector getInliers (double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist); 42 | 43 | // utility functions to compute normals from the model tree 44 | void computeNormal (const kdtree::KDTreeResultVector &neighbors,double *M_normal,const double flatness); 45 | double* computeNormals (const int32_t num_neighbors,const double flatness); 46 | 47 | // normals of model points 48 | double *M_normal; 49 | }; 50 | 51 | #endif // ICP_POINT_TO_PLANE_H 52 | -------------------------------------------------------------------------------- /ccicp2d/include/ccicp2d/icpPointToPoint.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | libicp is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #ifndef ICP_POINT_TO_POINT_H 22 | #define ICP_POINT_TO_POINT_H 23 | 24 | #include "ccicp2d/icp.h" 25 | 26 | class IcpPointToPoint : public Icp { 27 | 28 | public: 29 | 30 | IcpPointToPoint (double *M_GA, double *M_NGA ,const int32_t M_GA_num,const int32_t M_NGA_num, const int32_t dim) : Icp(M_GA,M_NGA,M_GA_num,M_NGA_num,dim) {} 31 | virtual ~IcpPointToPoint () {} 32 | void getEdgeWeight(double* eW); 33 | int getNumberCorrespondences(void); 34 | 35 | private: 36 | 37 | double fitStep(double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,Matrix &R,Matrix &t, double inDist); 38 | 39 | // std::vector getInliers (double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,const Matrix &R,const Matrix &t,const double indist); 40 | }; 41 | 42 | #endif // ICP_POINT_TO_POINT_H 43 | -------------------------------------------------------------------------------- /ccicp2d/include/ccicp2d/icpTools.h: -------------------------------------------------------------------------------- 1 | #ifndef ICPTOOLS_H 2 | #define ICPTOOLS_H 3 | 4 | #include 5 | 6 | #include "ground_segmentation/PointcloudXYZGD.h" 7 | #include "ground_segmentation/groundSegmentation.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | //ICP includes 19 | //#include "scan_registration2D.h" 20 | 21 | #define ICP_MAX_PTS 20000 22 | 23 | //ground adjacentcy 24 | #define NUMBINSGA 1200 25 | #define RESOLUTION 0.5 26 | #define GRD_ADJ_THRESH 2 27 | 28 | using namespace std; 29 | 30 | enum RegistrationType { 31 | SCAN_TO_SCAN, 32 | SCAN_TO_MAP 33 | }; 34 | 35 | class CCICP { 36 | 37 | private: 38 | double* refPts_GA; 39 | double* refPts_NGA; 40 | double* tarPts_GA; 41 | double* tarPts_NGA; 42 | 43 | pcl::PointCloud::Ptr seg_target; 44 | pcl::PointCloud::Ptr seg_scene; 45 | 46 | pcl::PointCloud::Ptr ground_target; 47 | pcl::PointCloud::Ptr ground_scene; 48 | 49 | RegistrationType type; 50 | 51 | groundSegmentation gSeg; 52 | 53 | 54 | void classifyPoints(pcl::PointCloud::Ptr outcloud); 55 | void segmentGround(pcl::PointCloud::Ptr incloud, pcl::PointCloud::Ptr outcloud, pcl::PointCloud::Ptr ground_cloud); 56 | 57 | geometry_msgs::PoseStamped doICPMatch(vector >& model_GA, 58 | vector >& model_NGA, 59 | vector >& scene_GA, 60 | vector >& scene_NGA, 61 | geometry_msgs::PoseStamped& initPose); 62 | 63 | geometry_msgs::PoseStamped doRPHRegistration(pcl::PointCloud::Ptr target_cloud, pcl::PointCloud::Ptr scene_cloud,geometry_msgs::PoseStamped& initPose); 64 | geometry_msgs::PoseStamped doHeightOnlyReg(pcl::PointCloud::Ptr target, pcl::PointCloud::Ptr scene,geometry_msgs::PoseStamped& initPose); 65 | geometry_msgs::PoseStamped doHeightInterpolate(pcl::PointCloud::Ptr ground,geometry_msgs::PoseStamped& initPose); 66 | 67 | public: 68 | CCICP(RegistrationType type_ = SCAN_TO_SCAN); 69 | 70 | //Setters 71 | void setTargetCloud(pcl::PointCloud::Ptr target, geometry_msgs::PoseStamped& initPose); 72 | void setTargetGndCloud( pcl::PointCloud::Ptr target); 73 | void setSceneCloud(pcl::PointCloud::Ptr scene); 74 | 75 | //returns the residual of the ICP 76 | double getResidual(); 77 | 78 | //Calculated the ICP of the target and scene clouds 79 | 80 | //Calls setter functions fisrt 81 | geometry_msgs::PoseStamped doICPMatch(pcl::PointCloud::Ptr target_cloud, pcl::PointCloud::Ptr scene_cloud, geometry_msgs::PoseStamped& initPose, RegistrationType type = SCAN_TO_SCAN); 82 | //Assumes setters have been used 83 | geometry_msgs::PoseStamped doICPMatch(geometry_msgs::PoseStamped& initPose); 84 | 85 | void getSegmentedClouds(pcl::PointCloud& target, pcl::PointCloud& scene,pcl::PointCloud& ground_target,pcl::PointCloud& ground_scene); 86 | 87 | }; 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /ccicp2d/include/ccicp2d/kdtree.h: -------------------------------------------------------------------------------- 1 | #ifndef __KDTREE_HPP 2 | #define __KDTREE_HPP 3 | 4 | // (c) Matthew B. Kennel, Institute for Nonlinear Science, UCSD (2004) 5 | // 6 | // Licensed under the Academic Free License version 1.1 found in file LICENSE 7 | // with additional provisions in that same file. 8 | // 9 | // Implement a kd tree for fast searching of points in a fixed data base 10 | // in k-dimensional Euclidean space. 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace kdtree { 19 | 20 | typedef boost::multi_array KDTreeArray; 21 | typedef boost::const_multi_array_ref KDTreeROArray; 22 | 23 | typedef struct { 24 | float lower, upper; 25 | } interval; 26 | 27 | // let the compiler know that this is a names of classes. 28 | class KDTreeNode; 29 | class SearchRecord; 30 | 31 | struct KDTreeResult { 32 | public: 33 | float dis; // square distance 34 | int idx; // neighbor index 35 | }; 36 | 37 | class KDTreeResultVector : public std::vector { 38 | // inherit a std::vector 39 | // but, optionally maintain it in heap form as a priority 40 | // queue. 41 | public: 42 | 43 | // 44 | // add one new element to the list of results, and 45 | // keep it in heap order. To keep it in ordinary, as inserted, 46 | // order, then simply use push_back() as inherited 47 | // via std::vector<> 48 | 49 | void push_element_and_heapify(KDTreeResult&); 50 | float replace_maxpri_elt_return_new_maxpri(KDTreeResult&); 51 | 52 | float max_value(); 53 | // return the distance which has the maximum value of all on list, 54 | // assuming that ALL insertions were made by 55 | // push_element_and_heapify() 56 | }; 57 | 58 | class KDTree { 59 | public: 60 | const KDTreeArray& the_data; 61 | // "the_data" is a reference to the underlying multi_array of the 62 | // data to be included in the tree. 63 | // 64 | // NOTE: this structure does *NOT* own the storage underlying this. 65 | // Hence, it would be a very bad idea to change the underlying data 66 | // during use of the search facilities of this tree. 67 | // Also, the user must deallocate the memory underlying it. 68 | 69 | 70 | const int N; // number of data points 71 | int dim; 72 | bool sort_results; // sorting result? 73 | const bool rearrange; // are we rearranging? 74 | 75 | public: 76 | 77 | // constructor, has optional 'dim_in' feature, to use only 78 | // first 'dim_in' components for definition of nearest neighbors. 79 | KDTree(KDTreeArray& data_in, bool rearrange_in = true, int dim_in=-1); 80 | 81 | // destructor 82 | ~KDTree(); 83 | 84 | 85 | public: 86 | 87 | void n_nearest_brute_force(std::vector& qv, int nn, KDTreeResultVector& result); 88 | // search for n nearest to a given query vector 'qv' usin 89 | // exhaustive slow search. For debugging, usually. 90 | 91 | void n_nearest(std::vector& qv, int nn, KDTreeResultVector& result); 92 | // search for n nearest to a given query vector 'qv'. 93 | 94 | void n_nearest_around_point(int idxin, int correltime, int nn, 95 | KDTreeResultVector& result); 96 | // search for 'nn' nearest to point [idxin] of the input data, excluding 97 | // neighbors within correltime 98 | 99 | void r_nearest(std::vector& qv, float r2, KDTreeResultVector& result); 100 | // search for all neighbors in ball of size (square Euclidean distance) 101 | // r2. Return number of neighbors in 'result.size()', 102 | 103 | void r_nearest_around_point(int idxin, int correltime, float r2, 104 | KDTreeResultVector& result); 105 | // like 'r_nearest', but around existing point, with decorrelation 106 | // interval. 107 | 108 | int r_count(std::vector& qv, float r2); 109 | // count number of neighbors within square distance r2. 110 | 111 | int r_count_around_point(int idxin, int correltime, float r2); 112 | // like r_count, c 113 | 114 | friend class KDTreeNode; 115 | friend class SearchRecord; 116 | 117 | private: 118 | 119 | KDTreeNode* root; // the root pointer 120 | 121 | const KDTreeArray* data; 122 | // pointing either to the_data or an internal 123 | // rearranged data as necessary 124 | 125 | std::vector ind; 126 | // the index for the tree leaves. Data in a leaf with bounds [l,u] are 127 | // in 'the_data[ind[l],*] to the_data[ind[u],*] 128 | 129 | KDTreeArray rearranged_data; 130 | // if rearrange is true then this is the rearranged data storage. 131 | 132 | static const int bucketsize = 12; // global constant. 133 | 134 | private: 135 | void set_data(KDTreeArray& din); 136 | void build_tree(); // builds the tree. Used upon construction. 137 | KDTreeNode* build_tree_for_range(int l, int u, KDTreeNode* parent); 138 | void select_on_coordinate(int c, int k, int l, int u); 139 | int select_on_coordinate_value(int c, float alpha, int l, int u); 140 | void spread_in_coordinate(int c, int l, int u, interval& interv); 141 | }; 142 | 143 | class KDTreeNode { 144 | public: 145 | KDTreeNode(int dim); 146 | ~KDTreeNode(); 147 | 148 | private: 149 | // visible to self and KDTree. 150 | friend class KDTree; // allow kdtree to access private data 151 | 152 | int cut_dim; // dimension to cut; 153 | float cut_val, cut_val_left, cut_val_right; //cut value 154 | int l, u; // extents in index array for searching 155 | 156 | std::vector box; // [min,max] of the box enclosing all points 157 | 158 | KDTreeNode *left, *right; // pointers to left and right nodes. 159 | 160 | void search(SearchRecord& sr); 161 | // recursive innermost core routine for searching.. 162 | 163 | bool box_in_search_range(SearchRecord& sr); 164 | // return true if the bounding box for this node is within the 165 | // search range given by the searchvector and maximum ballsize in 'sr'. 166 | 167 | void check_query_in_bound(SearchRecord& sr); // debugging only 168 | 169 | // for processing final buckets. 170 | void process_terminal_node(SearchRecord& sr); 171 | void process_terminal_node_fixedball(SearchRecord& sr); 172 | }; 173 | 174 | } // namespace kdtree 175 | 176 | #endif 177 | -------------------------------------------------------------------------------- /ccicp2d/include/ccicp2d/matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | matrix is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | matrix is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | matrix; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #ifndef MATRIX_H 22 | #define MATRIX_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #ifndef _MSC_VER 31 | #include 32 | #else 33 | typedef __int8 int8_t; 34 | typedef __int16 int16_t; 35 | typedef __int32 int32_t; 36 | typedef __int64 int64_t; 37 | typedef unsigned __int8 uint8_t; 38 | typedef unsigned __int16 uint16_t; 39 | typedef unsigned __int32 uint32_t; 40 | typedef unsigned __int64 uint64_t; 41 | #endif 42 | 43 | #define endll endl << endl // double end line definition 44 | 45 | typedef double FLOAT; // double precision 46 | //typedef float FLOAT; // single precision 47 | 48 | class Matrix { 49 | 50 | public: 51 | 52 | // constructor / deconstructor 53 | Matrix (); // init empty 0x0 matrix 54 | Matrix (const int32_t m,const int32_t n); // init empty mxn matrix 55 | Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val' 56 | Matrix (const Matrix &M); // creates deepcopy of M 57 | ~Matrix (); 58 | 59 | // assignment operator, copies contents of M 60 | Matrix& operator= (const Matrix &M); 61 | 62 | // copies submatrix of M into array 'val', default values copy whole row/column/matrix 63 | void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 64 | 65 | // set or get submatrices of current matrix 66 | Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1); 67 | void setMat(const Matrix &M,const int32_t i,const int32_t j); 68 | 69 | // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix 70 | void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 71 | 72 | // set (part of) diagonal to scalar, -1 as end replaces whole diagonal 73 | void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1); 74 | 75 | // clear matrix 76 | void zero(); 77 | 78 | // extract columns with given index 79 | Matrix extractCols (std::vector idx); 80 | 81 | // create identity matrix 82 | static Matrix eye (const int32_t m); 83 | void eye (); 84 | 85 | // create matrix with ones 86 | static Matrix ones(const int32_t m,const int32_t n); 87 | 88 | // create diagonal matrix with nx1 or 1xn matrix M as elements 89 | static Matrix diag(const Matrix &M); 90 | 91 | // returns the m-by-n matrix whose elements are taken column-wise from M 92 | static Matrix reshape(const Matrix &M,int32_t m,int32_t n); 93 | 94 | // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix) 95 | static Matrix rotMatX(const FLOAT &angle); 96 | static Matrix rotMatY(const FLOAT &angle); 97 | static Matrix rotMatZ(const FLOAT &angle); 98 | 99 | // simple arithmetic operations 100 | Matrix operator+ (const Matrix &M); // add matrix 101 | Matrix operator- (const Matrix &M); // subtract matrix 102 | Matrix operator* (const Matrix &M); // multiply with matrix 103 | Matrix operator* (const FLOAT &s); // multiply with scalar 104 | Matrix operator/ (const Matrix &M); // divide elementwise by matrix (or vector) 105 | Matrix operator/ (const FLOAT &s); // divide by scalar 106 | Matrix operator- (); // negative matrix 107 | Matrix operator~ (); // transpose 108 | FLOAT l2norm (); // euclidean norm (vectors) / frobenius norm (matrices) 109 | FLOAT mean (); // mean of all elements in matrix 110 | 111 | // complex arithmetic operations 112 | static Matrix cross (const Matrix &a, const Matrix &b); // cross product of two vectors 113 | static Matrix inv (const Matrix &M); // invert matrix M 114 | bool inv (); // invert this matrix 115 | FLOAT det (); // returns determinant of matrix 116 | bool solve (const Matrix &M,FLOAT eps=1e-20); // solve linear system M*x=B, replaces *this and M 117 | bool lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20); // replace *this by lower upper decomposition 118 | void svd(Matrix &U,Matrix &W,Matrix &V); // singular value decomposition *this = U*diag(W)*V^T 119 | 120 | // print matrix to stream 121 | friend std::ostream& operator<< (std::ostream& out,const Matrix& M); 122 | 123 | // direct data access 124 | FLOAT **val; 125 | int32_t m,n; 126 | 127 | private: 128 | 129 | void allocateMemory (const int32_t m_,const int32_t n_); 130 | void releaseMemory (); 131 | inline FLOAT pythag(FLOAT a,FLOAT b); 132 | 133 | }; 134 | 135 | #endif // MATRIX_H 136 | -------------------------------------------------------------------------------- /ccicp2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ccicp2d 4 | 0.0.0 5 | ccicp2d 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | tf 16 | tf_conversions 17 | ground_segmentation 18 | eigen_conversions 19 | 20 | roscpp 21 | std_msgs 22 | sensor_msgs 23 | geometry_msgs 24 | pcl_ros 25 | tf 26 | tf_conversions 27 | ground_segmentation 28 | eigen_conversions 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /ccicp2d/src/icp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | libicp is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #include "ccicp2d/icp.h" 22 | #include 23 | 24 | using namespace std; 25 | 26 | Icp::Icp (double *M_GA, double *M_NGA ,const int32_t M_GA_num,const int32_t M_NGA_num,const int32_t dim) : 27 | dim(dim), sub_step(10), max_iter(20), min_delta(1e-6) { 28 | 29 | // check for correct dimensionality 30 | if (dim!=2 && dim!=3) { 31 | ROS_ERROR_STREAM("LIBICP works only for data of dimensionality 2 or 3" ); 32 | M_tree_GA = 0; 33 | M_tree_NGA = 0; 34 | return; 35 | } 36 | 37 | // check for minimum number of points 38 | if ((M_GA_num + M_NGA_num)<5) { 39 | ROS_ERROR_STREAM("LIBICP works only with at least 5 model points"); 40 | M_tree_GA = 0; 41 | M_tree_NGA = 0; 42 | return; 43 | } 44 | 45 | //copy the number of points to the class 46 | 47 | M_GA_SIZE =M_GA_num; 48 | M_NGA_SIZE =M_NGA_num; 49 | 50 | // copy model points to M_data 51 | M_data_GA.resize(boost::extents[M_GA_num][dim]); 52 | for (int32_t m=0; m0) 65 | M_tree_GA = new kdtree::KDTree(M_data_GA); 66 | 67 | // build a kd tree from the model point cloud 68 | //if(M_NGA_num>0) 69 | M_tree_NGA = new kdtree::KDTree(M_data_NGA); 70 | } 71 | 72 | Icp::~Icp () { 73 | if (M_tree_GA) 74 | delete M_tree_GA; 75 | 76 | if (M_tree_NGA) 77 | delete M_tree_NGA; 78 | } 79 | 80 | void Icp::fit (double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,Matrix &R,Matrix &t,double indist, double h_dist) { 81 | 82 | // make sure we have a model tree 83 | /*if (!M_tree_GA || !M_tree_GA) { 84 | ROS_ERROR_STREAM( "ERROR: Model not initialized"); 85 | return; 86 | }*/ 87 | 88 | // check for minimum number of points 89 | if (T_NGA_num<5) { 90 | ROS_WARN_STREAM("Template has " << T_NGA_num <<" NGA points" ); 91 | //return; 92 | } 93 | 94 | // check for minimum number of points 95 | if (T_GA_num<5) { 96 | ROS_WARN_STREAM("Warning: Template has "<< T_GA_num << " GA points"); 97 | //return; 98 | } 99 | 100 | if (T_GA_num + T_NGA_num < 5) { 101 | ROS_ERROR_STREAM("ERROR: Total template has " << T_GA_num + T_NGA_num <<" points"); 102 | return; 103 | } 104 | 105 | 106 | 107 | //vector active_GA; 108 | //active_GA.clear(); 109 | 110 | //vector active_NGA; 111 | //active_NGA.clear(); 112 | 113 | fitIterate(T_GA,T_NGA,T_GA_num,T_NGA_num,R,t,indist); 114 | } 115 | 116 | void Icp::fitIterate(double *T_GA,double *T_NGA,const int32_t T_GA_num,const int32_t T_NGA_num,Matrix &R,Matrix &t,double inDist) { 117 | 118 | // iterate until convergence 119 | for (int32_t iter=0; iter 2 | 3 | ekf 4 | 0.0.0 5 | efk 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | tf 16 | tf_conversions 17 | nav_msgs 18 | 19 | roscpp 20 | std_msgs 21 | sensor_msgs 22 | geometry_msgs 23 | pcl_ros 24 | tf 25 | tf_conversions 26 | nav_msgs 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ekf/src/nasa_ekf.h: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define NUMINITIMUSAMPLES 200 14 | #define LOOPRATE 100 //hz 15 | #define PUBRATE 1.0 //hz 16 | #define PUBITER (int)(((double)LOOPRATE)/((double)PUBRATE)) 17 | 18 | //motion noise 19 | #define MOTNOISE 0.05 20 | #define MEASNOISEYAW 0.01 21 | #define MEASNOISESLAM 10 22 | 23 | struct imuMeasurement 24 | { 25 | bool newMeas; 26 | double dt; 27 | double ax; 28 | double ay; 29 | double az; 30 | double gx; 31 | double gy; 32 | double gz; 33 | 34 | //bias terms 35 | 36 | double bias_gz; 37 | }; 38 | 39 | struct slamMeasurement 40 | { 41 | bool newMeas; 42 | double x; 43 | double y; 44 | double z; 45 | double pitch; 46 | double roll; 47 | double yaw; 48 | 49 | 50 | }; 51 | 52 | struct globalMeasurement 53 | { 54 | bool newMeas; 55 | double x; 56 | double y; 57 | double yaw; 58 | 59 | 60 | 61 | }; 62 | 63 | 64 | struct cmdPacket 65 | { 66 | bool newCmd; 67 | double dt; 68 | double ux; 69 | double uy; 70 | double uw; 71 | }; 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /global_matching/.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | global_matching-RelWithDebInfo@global_matching 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.cdt.make.core.makeBuilder 10 | clean,full,incremental, 11 | 12 | 13 | org.eclipse.cdt.make.core.cleanBuildTarget 14 | clean 15 | 16 | 17 | org.eclipse.cdt.make.core.enableCleanBuild 18 | true 19 | 20 | 21 | org.eclipse.cdt.make.core.append_environment 22 | true 23 | 24 | 25 | org.eclipse.cdt.make.core.stopOnError 26 | true 27 | 28 | 29 | org.eclipse.cdt.make.core.enabledIncrementalBuild 30 | true 31 | 32 | 33 | org.eclipse.cdt.make.core.build.command 34 | /usr/bin/make 35 | 36 | 37 | org.eclipse.cdt.make.core.contents 38 | org.eclipse.cdt.make.core.activeConfigSettings 39 | 40 | 41 | org.eclipse.cdt.make.core.build.target.inc 42 | all 43 | 44 | 45 | org.eclipse.cdt.make.core.build.arguments 46 | 47 | 48 | 49 | org.eclipse.cdt.make.core.buildLocation 50 | /home/jdservos/wavelab/wavelab/projects/servos/global_matching 51 | 52 | 53 | org.eclipse.cdt.make.core.useDefaultBuildCmd 54 | false 55 | 56 | 57 | org.eclipse.cdt.make.core.environment 58 | VERBOSE=1|ROS_ROOT=/opt/ros/fuerte/share/ros|ROS_PACKAGE_PATH=/home/jdservos/ros_workspace:/home/jdservos/wavelab:/opt/ros/fuerte/share:/opt/ros/fuerte/stacks:/home/jdservos/UWRT|PYTHONPATH=/opt/ros/fuerte/lib/python2.7/dist-packages:|PATH=/opt/ros/fuerte/bin:/usr/local/cuda/bin:/usr/local/MATLAB/R2012b/bin:/home/jdservos/eclipse:/usr/lib/lightdm/lightdm:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games| 59 | 60 | 61 | org.eclipse.cdt.make.core.enableFullBuild 62 | true 63 | 64 | 65 | org.eclipse.cdt.make.core.build.target.auto 66 | all 67 | 68 | 69 | org.eclipse.cdt.make.core.enableAutoBuild 70 | false 71 | 72 | 73 | org.eclipse.cdt.make.core.build.target.clean 74 | clean 75 | 76 | 77 | org.eclipse.cdt.make.core.fullBuildTarget 78 | all 79 | 80 | 81 | org.eclipse.cdt.make.core.buildArguments 82 | 83 | 84 | 85 | org.eclipse.cdt.make.core.build.location 86 | /home/jdservos/wavelab/wavelab/projects/servos/global_matching 87 | 88 | 89 | org.eclipse.cdt.make.core.autoBuildTarget 90 | all 91 | 92 | 93 | org.eclipse.cdt.core.errorOutputParser 94 | org.eclipse.cdt.core.MakeErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GLDErrorParser; 95 | 96 | 97 | 98 | 99 | org.eclipse.cdt.make.core.ScannerConfigBuilder 100 | 101 | 102 | 103 | 104 | 105 | org.eclipse.cdt.core.ccnature 106 | org.eclipse.cdt.make.core.makeNature 107 | org.eclipse.cdt.make.core.ScannerConfigNature 108 | org.eclipse.cdt.core.cnature 109 | 110 | 111 | 112 | [Subprojects] 113 | 2 114 | virtual:/virtual 115 | 116 | 117 | [Targets] 118 | 2 119 | virtual:/virtual 120 | 121 | 122 | [Targets]/[exe] global_matching 123 | 2 124 | virtual:/virtual 125 | 126 | 127 | [Targets]/[exe] global_matching/ 128 | 2 129 | virtual:/virtual 130 | 131 | 132 | [Targets]/[exe] global_matching/Source Files 133 | 2 134 | virtual:/virtual 135 | 136 | 137 | [Targets]/[exe] global_matching/Source Files/global_match.cpp 138 | 1 139 | /home/jdservos/wavelab/wavelab/projects/servos/global_matching/src/global_match.cpp 140 | 141 | 142 | [Targets]/[exe] global_matching/Header Files 143 | 2 144 | virtual:/virtual 145 | 146 | 147 | [Targets]/[exe] global_matching/CMake Rules 148 | 2 149 | virtual:/virtual 150 | 151 | 152 | [Targets]/[exe] global_matching/Resources 153 | 2 154 | virtual:/virtual 155 | 156 | 157 | 158 | -------------------------------------------------------------------------------- /global_matching/.pydevproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Default 6 | python 2.6 7 | 8 | /opt/ros/fuerte/stacks/nodelet_core/nodelet_topic_tools/src 9 | /opt/ros/fuerte/stacks/nodelet_core/nodelet_topic_tools/lib 10 | /opt/ros/fuerte/stacks/perception_pcl/pcl_ros/src 11 | /opt/ros/fuerte/stacks/perception_pcl/pcl_ros/lib 12 | /opt/ros/fuerte/stacks/bond_core/smclib/src 13 | /opt/ros/fuerte/stacks/perception_pcl_fuerte_unstable/flann/lib 14 | /opt/ros/fuerte/stacks/geometry/tf/src 15 | /opt/ros/fuerte/stacks/geometry/tf/lib 16 | /opt/ros/fuerte/stacks/nodelet_core/nodelet/src 17 | /opt/ros/fuerte/stacks/nodelet_core/nodelet/lib 18 | /opt/ros/fuerte/stacks/geometry/angles/lib 19 | /opt/ros/fuerte/stacks/dynamic_reconfigure/src 20 | /opt/ros/fuerte/stacks/dynamic_reconfigure/lib 21 | /opt/ros/fuerte/stacks/bond_core/bondcpp/src 22 | /opt/ros/fuerte/stacks/bond_core/bondcpp/lib 23 | /opt/ros/fuerte/stacks/bullet/src 24 | /opt/ros/fuerte/stacks/bullet/lib 25 | /opt/ros/fuerte/stacks/pluginlib/src 26 | /opt/ros/fuerte/stacks/pluginlib/lib 27 | /opt/ros/fuerte/stacks/perception_pcl_fuerte_unstable/pcl16/src 28 | /opt/ros/fuerte/stacks/perception_pcl_fuerte_unstable/pcl16/lib 29 | /opt/ros/fuerte/stacks/bond_core/bond/src 30 | /home/jdservos/wavelab/wavelab/projects/servos/global_matching/src 31 | /opt/ros/fuerte/share/ros/core/mk 32 | /usr/local/lib/python2.7/dist-packages/rosdep-0.10.3-py2.7.egg 33 | /opt/ros/fuerte/lib/python2.7/dist-packages 34 | /home/jdservos/wavelab/wavelab/projects/servos/global_matching 35 | /usr/lib/python2.7 36 | /usr/lib/python2.7/plat-linux2 37 | /usr/lib/python2.7/lib-tk 38 | /usr/lib/python2.7/lib-dynload 39 | /usr/local/lib/python2.7/dist-packages 40 | /usr/lib/python2.7/dist-packages 41 | /usr/lib/python2.7/dist-packages/PIL 42 | /usr/lib/python2.7/dist-packages/gst-0.10 43 | /usr/lib/python2.7/dist-packages/gtk-2.0 44 | /usr/lib/pymodules/python2.7 45 | /usr/lib/python2.7/dist-packages/ubuntu-sso-client 46 | /usr/lib/python2.7/dist-packages/ubuntuone-client 47 | /usr/lib/python2.7/dist-packages/ubuntuone-control-panel 48 | /usr/lib/python2.7/dist-packages/ubuntuone-couch 49 | /usr/lib/python2.7/dist-packages/ubuntuone-installer 50 | /usr/lib/python2.7/dist-packages/ubuntuone-storage-protocol 51 | /usr/lib/python2.7/dist-packages/wx-2.8-gtk2-unicode 52 | 53 | 54 | -------------------------------------------------------------------------------- /global_matching/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/servos/SLAM/696e26e6924ed2edd3f1997b7516cdbce3e79f59/global_matching/CATKIN_IGNORE -------------------------------------------------------------------------------- /global_matching/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(global_matching) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs graph_slam 8 | geometry_msgs pcl_ros tf tf_conversions ) 9 | 10 | 11 | 12 | add_message_files(FILES GlocCloud.msg) 13 | generate_messages(DEPENDENCIES sensor_msgs geometry_msgs) 14 | 15 | ################################### 16 | ## catkin specific configuration ## 17 | ################################### 18 | catkin_package() 19 | 20 | ########### 21 | ## Build ## 22 | ########### 23 | 24 | include_directories( 25 | include 26 | ${catkin_INCLUDE_DIRS} 27 | ) 28 | 29 | #rosbuild_add_executable(cylinder_segmentation src/cyl_segmentation.cpp) 30 | #rosbuild_add_executable(save_laser_file src/save_laser_file.cpp) 31 | add_executable(global_matching src/global_match.cpp) 32 | add_executable(global_transform src/global_transform.cpp) 33 | add_executable(global_generate src/global_generate.cpp) 34 | target_link_libraries(global_matching 35 | ${catkin_LIBRARIES} ) 36 | target_link_libraries(global_transform 37 | ${catkin_LIBRARIES} ) 38 | target_link_libraries(global_generate 39 | ${catkin_LIBRARIES} ) 40 | -------------------------------------------------------------------------------- /global_matching/global_generate_rviz.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=0 2 | Background\ ColorG=0 3 | Background\ ColorR=0 4 | Camera\ Config=1.5698 3.18539 51.0045 -0.884551 -1.48495 0 5 | Camera\ Type=rviz::XYOrbitViewController 6 | Fixed\ Frame=/velodyne 7 | PointCloud2..AxisColorAutocompute\ Value\ Bounds=0 8 | PointCloud2..AxisColorAxis=2 9 | PointCloud2..AxisColorMax\ Value=3 10 | PointCloud2..AxisColorMin\ Value=-7.90325 11 | PointCloud2..AxisColorUse\ Fixed\ Frame=1 12 | PointCloud2..FlatColorColorB=0 13 | PointCloud2..FlatColorColorG=1 14 | PointCloud2..FlatColorColorR=0 15 | PointCloud2..IntensityAutocompute\ Intensity\ Bounds=1 16 | PointCloud2..IntensityChannel\ Name=intensity 17 | PointCloud2..IntensityMax\ ColorB=1 18 | PointCloud2..IntensityMax\ ColorG=0 19 | PointCloud2..IntensityMax\ ColorR=0 20 | PointCloud2..IntensityMax\ Intensity=255 21 | PointCloud2..IntensityMin\ ColorB=0 22 | PointCloud2..IntensityMin\ ColorG=0 23 | PointCloud2..IntensityMin\ ColorR=1 24 | PointCloud2..IntensityMin\ Intensity=55 25 | PointCloud2..IntensityUse\ full\ RGB\ spectrum=1 26 | PointCloud2.Alpha=1 27 | PointCloud2.Billboard\ Size=0.01 28 | PointCloud2.Color\ Transformer=FlatColor 29 | PointCloud2.Decay\ Time=0 30 | PointCloud2.Enabled=1 31 | PointCloud2.Position\ Transformer=XYZ 32 | PointCloud2.Queue\ Size=10 33 | PointCloud2.Selectable=1 34 | PointCloud2.Style=0 35 | PointCloud2.Topic=/output3 36 | PointCloud22..AxisColorAutocompute\ Value\ Bounds=1 37 | PointCloud22..AxisColorAxis=2 38 | PointCloud22..AxisColorMax\ Value=6.34596 39 | PointCloud22..AxisColorMin\ Value=-3.32802 40 | PointCloud22..AxisColorUse\ Fixed\ Frame=1 41 | PointCloud22..FlatColorColorB=0 42 | PointCloud22..FlatColorColorG=1 43 | PointCloud22..FlatColorColorR=0 44 | PointCloud22..IntensityAutocompute\ Intensity\ Bounds=1 45 | PointCloud22..IntensityChannel\ Name=intensity 46 | PointCloud22..IntensityMax\ ColorB=0 47 | PointCloud22..IntensityMax\ ColorG=0 48 | PointCloud22..IntensityMax\ ColorR=1 49 | PointCloud22..IntensityMax\ Intensity=255 50 | PointCloud22..IntensityMin\ ColorB=0 51 | PointCloud22..IntensityMin\ ColorG=1 52 | PointCloud22..IntensityMin\ ColorR=0 53 | PointCloud22..IntensityMin\ Intensity=75 54 | PointCloud22..IntensityUse\ full\ RGB\ spectrum=1 55 | PointCloud22.Alpha=1 56 | PointCloud22.Billboard\ Size=0.01 57 | PointCloud22.Color\ Transformer=AxisColor 58 | PointCloud22.Decay\ Time=0 59 | PointCloud22.Enabled=1 60 | PointCloud22.Position\ Transformer=XYZ 61 | PointCloud22.Queue\ Size=10 62 | PointCloud22.Selectable=1 63 | PointCloud22.Style=0 64 | PointCloud22.Topic=/output_small 65 | Property\ Grid\ Splitter=769,78 66 | Property\ Grid\ State=expanded=.Global Options,.Global Status,PointCloud2.Enabled,PointCloud22.Enabled,PointCloud22.Status;splitterratio=0.521739 67 | QMainWindow=000000ff00000000fd000000030000000000000347000003a1fc0200000001fc0000001d000003a1000000ee00fffffffa000000010100000002fb0000000c00430061006d006500720061020000035b000000e1000002e50000025afb000000100044006900730070006c00610079007301000000000000011d0000011d00ffffff0000000100000131000003a1fc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000001d0000011d0000006700fffffffb0000000a005600690065007700730100000140000001a2000000bb00fffffffb0000001200530065006c0065006300740069006f006e01000002e8000000d60000006700ffffff000000030000073f0000005cfc0100000001fb0000000800540069006d006501000000000000073f000002bf00ffffff000002bb000003a100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 68 | Target\ Frame=/velodyne 69 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 70 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 71 | [Display0] 72 | ClassName=rviz::PointCloud2Display 73 | Name=PointCloud2 74 | [Display1] 75 | ClassName=rviz::PointCloud2Display 76 | Name=PointCloud22 77 | [Window] 78 | Height=1092 79 | Width=1871 80 | X=1912 81 | Y=-28 82 | -------------------------------------------------------------------------------- /global_matching/global_match_rviz.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=0 2 | Background\ ColorG=0 3 | Background\ ColorR=0 4 | Camera\ Config=1.5698 3.11538 85.4602 1.5079 -9.45407 0 5 | Camera\ Type=rviz::XYOrbitViewController 6 | Fixed\ Frame=/nasa 7 | Grid.Alpha=0.5 8 | Grid.Cell\ Size=2 9 | Grid.ColorB=0.5 10 | Grid.ColorG=0.5 11 | Grid.ColorR=0.5 12 | Grid.Enabled=1 13 | Grid.Line\ Style=0 14 | Grid.Line\ Width=0.03 15 | Grid.Normal\ Cell\ Count=0 16 | Grid.OffsetX=0 17 | Grid.OffsetY=0 18 | Grid.OffsetZ=0 19 | Grid.Plane=0 20 | Grid.Plane\ Cell\ Count=600 21 | Grid.Reference\ Frame= 22 | Property\ Grid\ Splitter=769,78 23 | Property\ Grid\ State=expanded=.Global Options,.Global Status,Grid.Enabled,global map.Enabled,matched pointcloud.Enabled;splitterratio=0.521739 24 | QMainWindow=000000ff00000000fd000000030000000000000198000003a1fc0200000001fc0000001d000003a1000000ee00fffffffa000000010100000002fb0000000c00430061006d006500720061020000035b000000e1000002e50000025afb000000100044006900730070006c00610079007301000000000000011d0000011d00ffffff0000000100000164000003a1fc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000001d0000011d0000006700fffffffb0000000a005600690065007700730100000140000001a2000000bb00fffffffb0000001200530065006c0065006300740069006f006e01000002e8000000d60000006700ffffff000000030000073f0000005cfc0100000001fb0000000800540069006d006501000000000000073f000002bf00ffffff00000437000003a100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 25 | Target\ Frame=/nasa 26 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 27 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 28 | global\ map..AxisColorAutocompute\ Value\ Bounds=0 29 | global\ map..AxisColorAxis=2 30 | global\ map..AxisColorMax\ Value=3 31 | global\ map..AxisColorMin\ Value=-7.90325 32 | global\ map..AxisColorUse\ Fixed\ Frame=1 33 | global\ map..FlatColorColorB=0 34 | global\ map..FlatColorColorG=1 35 | global\ map..FlatColorColorR=0 36 | global\ map..IntensityAutocompute\ Intensity\ Bounds=1 37 | global\ map..IntensityChannel\ Name=intensity 38 | global\ map..IntensityMax\ ColorB=1 39 | global\ map..IntensityMax\ ColorG=0 40 | global\ map..IntensityMax\ ColorR=0 41 | global\ map..IntensityMax\ Intensity=255 42 | global\ map..IntensityMin\ ColorB=0 43 | global\ map..IntensityMin\ ColorG=0 44 | global\ map..IntensityMin\ ColorR=1 45 | global\ map..IntensityMin\ Intensity=55 46 | global\ map..IntensityUse\ full\ RGB\ spectrum=1 47 | global\ map.Alpha=1 48 | global\ map.Billboard\ Size=0.01 49 | global\ map.Color\ Transformer=FlatColor 50 | global\ map.Decay\ Time=0 51 | global\ map.Enabled=1 52 | global\ map.Position\ Transformer=XYZ 53 | global\ map.Queue\ Size=10 54 | global\ map.Selectable=1 55 | global\ map.Style=0 56 | global\ map.Topic=/mapping/gloc/map 57 | matched\ pointcloud..AxisColorAutocompute\ Value\ Bounds=1 58 | matched\ pointcloud..AxisColorAxis=2 59 | matched\ pointcloud..AxisColorMax\ Value=10 60 | matched\ pointcloud..AxisColorMin\ Value=-10 61 | matched\ pointcloud..AxisColorUse\ Fixed\ Frame=1 62 | matched\ pointcloud..FlatColorColorB=1 63 | matched\ pointcloud..FlatColorColorG=0 64 | matched\ pointcloud..FlatColorColorR=0 65 | matched\ pointcloud..IntensityAutocompute\ Intensity\ Bounds=1 66 | matched\ pointcloud..IntensityChannel\ Name=intensity 67 | matched\ pointcloud..IntensityMax\ ColorB=1 68 | matched\ pointcloud..IntensityMax\ ColorG=1 69 | matched\ pointcloud..IntensityMax\ ColorR=1 70 | matched\ pointcloud..IntensityMax\ Intensity=4096 71 | matched\ pointcloud..IntensityMin\ ColorB=0 72 | matched\ pointcloud..IntensityMin\ ColorG=0 73 | matched\ pointcloud..IntensityMin\ ColorR=0 74 | matched\ pointcloud..IntensityMin\ Intensity=0 75 | matched\ pointcloud..IntensityUse\ full\ RGB\ spectrum=0 76 | matched\ pointcloud.Alpha=1 77 | matched\ pointcloud.Billboard\ Size=0.01 78 | matched\ pointcloud.Color\ Transformer=FlatColor 79 | matched\ pointcloud.Decay\ Time=0 80 | matched\ pointcloud.Enabled=1 81 | matched\ pointcloud.Position\ Transformer=XYZ 82 | matched\ pointcloud.Queue\ Size=10 83 | matched\ pointcloud.Selectable=1 84 | matched\ pointcloud.Style=0 85 | matched\ pointcloud.Topic=/mapping/gloc/match 86 | [Display0] 87 | ClassName=rviz::GridDisplay 88 | Name=Grid 89 | [Display1] 90 | ClassName=rviz::PointCloud2Display 91 | Name=global map 92 | [Display2] 93 | ClassName=rviz::PointCloud2Display 94 | Name=matched pointcloud 95 | [Window] 96 | Height=1056 97 | Width=1855 98 | X=1920 99 | Y=0 100 | -------------------------------------------------------------------------------- /global_matching/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b FeatureDetection is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /global_matching/matlab/calcTransform.m: -------------------------------------------------------------------------------- 1 | %number of features 2 | n=4; 3 | 4 | %features from wpi localization spreadsheet (ENU Frame) 5 | wpi1003 = [51.12 -16.44]; 6 | wpi1002 = [60.88 -29.33]; 7 | wpi1020 = [60.33 40.72]; 8 | wpi1021 = [70.97 40.23]; 9 | 10 | %features measured from generated pointcloud 11 | map1003 = [-20.92 11.776]; 12 | map1002 = [-14.318 -1.7205]; 13 | map1020 = [-2.818 66.86]; 14 | map1021 = [7.32 64.65]; 15 | 16 | %concat corresponding features 17 | B = [wpi1003;wpi1002;wpi1020;wpi1021]; 18 | A = [map1003;map1002;map1020;map1021]; 19 | 20 | [ret_R, ret_t] = rigid_transform_3D(A, B); 21 | 22 | A2 = (ret_R*A') + repmat(ret_t, 1, n); 23 | A2 = A2' 24 | A2 25 | B 26 | 27 | ret_R 28 | ret_t 29 | 30 | % Find the error 31 | err = A2 - B; 32 | err = err .* err; 33 | err = sum(err(:)); 34 | rmse = sqrt(err/n); 35 | 36 | disp(sprintf("RMSE: %f", rmse)); 37 | 38 | %} 39 | 40 | 41 | -------------------------------------------------------------------------------- /global_matching/matlab/rigid_transform_3D.m: -------------------------------------------------------------------------------- 1 | % expects row data 2 | function [R,t] = rigid_transform_3D(A, B) 3 | if nargin != 2 4 | error("Missing parameters"); 5 | end 6 | 7 | assert(size(A) == size(B)) 8 | 9 | centroid_A = mean(A); 10 | centroid_B = mean(B); 11 | 12 | N = size(A,1); 13 | 14 | H = (A - repmat(centroid_A, N, 1))' * (B - repmat(centroid_B, N, 1)); 15 | 16 | [U,S,V] = svd(H); 17 | 18 | R = V*U'; 19 | 20 | %if det(R) < 0 21 | % printf("Reflection detected\n"); 22 | % V(:,2) *= -1; 23 | % R = V*U'; 24 | % end 25 | 26 | t = -R*centroid_A' + centroid_B' 27 | end 28 | 29 | 30 | -------------------------------------------------------------------------------- /global_matching/msg/GlocCloud.msg: -------------------------------------------------------------------------------- 1 | #Tagged Pointcloud 2 | uint16 id 3 | sensor_msgs/PointCloud2 cloud 4 | geometry_msgs/PoseStamped pose 5 | -------------------------------------------------------------------------------- /global_matching/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | global_matching 4 | 0.0.0 5 | global_matching 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | tf 16 | tf_conversions 17 | graph_slam 18 | 19 | roscpp 20 | std_msgs 21 | sensor_msgs 22 | geometry_msgs 23 | pcl_ros 24 | tf 25 | tf_conversions 26 | graph_slam 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /global_matching/src/global_transform.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | // PCL specific includes 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | //use this file to transform a point cloud to proper global co-ordinates 19 | 20 | ros::Publisher pub_map; 21 | sensor_msgs::PointCloud2 ros_map; 22 | 23 | pcl::GeneralizedIterativeClosestPoint gicp; 24 | pcl::PointCloud::Ptr cloud_map (new pcl::PointCloud); 25 | pcl::PointCloud::Ptr tcloud_map (new pcl::PointCloud); 26 | 27 | 28 | Eigen::Matrix4f transformation; 29 | 30 | int main(int argc, char** argv) 31 | { 32 | ros::init(argc, argv, "global_transform"); 33 | ros::NodeHandle nh; 34 | pub_map=nh.advertise("/mapping/global_transform_map",1); 35 | 36 | //read map cloud 37 | ROS_INFO_STREAM("Loading Map Point Cloud..."); 38 | if (pcl::io::loadPCDFile ("maps/WPI_FINAL.pcd", *cloud_map) == -1) // load the file 39 | { 40 | ROS_ERROR_STREAM("No map cloud found"); 41 | return (-1); 42 | } 43 | ROS_INFO_STREAM("Done Loading Map"); 44 | 45 | //eye transform 46 | transformation.block(0,0,2,2) << 1,0,0,1; 47 | transformation.block(0,3,3,1) << 0,0,0; 48 | transformation(2,2) = 1; 49 | transformation(3,3) = 1; 50 | 51 | 52 | //put in the transform (WPI) 53 | //transformation.block(0,0,2,2) << 0.98627,-0.16516,0.16516,0.98627; 54 | //transformation.block(0,3,3,1) << 74.249,-24.841,0; 55 | //transformation(2,2) = 1; 56 | //transformation(3,3) = 1; 57 | 58 | //put in the transform (SDC) 59 | //transformation.block(0,0,2,2) << 0.98415,-0.17736,0.17736,0.98415; 60 | //transformation.block(0,3,3,1) << 5,-10,0; 61 | //transformation(2,2) = 1; 62 | //transformation(3,3) = 1; 63 | 64 | std::cout<<"Transform is : " <size () << " data points to WPI_Transform.pcd." << std::endl; 70 | 71 | ros::Rate r(1); 72 | while(ros::ok()) { 73 | pcl::toROSMsg(*tcloud_map,ros_map); 74 | ros_map.header.frame_id = "/nasa"; 75 | ROS_INFO_STREAM("Publishing Map"); 76 | pub_map.publish(ros_map); 77 | ros::spinOnce(); 78 | r.sleep(); 79 | } 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /graph_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(graph_slam) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs nav_msgs 8 | geometry_msgs pcl_ros tf tf_conversions message_generation visualization_msgs mls ccicp2d 9 | eigen_conversions ground_segmentation actionlib nasa_msgs) 10 | 11 | 12 | 13 | add_message_files(FILES Edge.msg ) 14 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 15 | 16 | ################################### 17 | ## catkin specific configuration ## 18 | ################################### 19 | catkin_package() 20 | 21 | ########### 22 | ## Build ## 23 | ########### 24 | set(CMAKE_BUILD_TYPE Release) 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | 31 | #find_package(libg2o REQUIRED) 32 | #include_directories(/usr/include/suitesparse) 33 | set(libg2o_INCLUDE_DIRS /usr/local/include) 34 | 35 | add_executable(graph_slam src/graph_slam.cpp src/global_mapping_viz.cpp src/graphSlamTools.cpp) 36 | #add_executable(gs_test src/gs_test.cpp) 37 | 38 | target_link_libraries (graph_slam mls ccicp2d 39 | g2o_ext_csparse g2o_csparse_extension g2o_stuff g2o_core g2o_solver_csparse g2o_types_slam3d 40 | ${catkin_LIBRARIES}) 41 | #target_link_libraries (gs_test ${catkin_LIBRARIES}) 42 | -------------------------------------------------------------------------------- /graph_slam/msg/Edge.msg: -------------------------------------------------------------------------------- 1 | #delta is in the frame of node "from" 2 | #Node delta 3 | geometry_msgs/Pose delta 4 | uint16 from 5 | uint16 to 6 | float64[9] covariance 7 | -------------------------------------------------------------------------------- /graph_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graph_slam 4 | 0.0.0 5 | graph_slam 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | tf 16 | tf_conversions 17 | nav_msgs 18 | mls 19 | ccicp2d 20 | visualization_msgs 21 | eigen_conversions 22 | ground_segmentation 23 | actionlib 24 | nasa_msgs 25 | 26 | roscpp 27 | std_msgs 28 | sensor_msgs 29 | geometry_msgs 30 | pcl_ros 31 | tf 32 | tf_conversions 33 | nav_msgs 34 | mls 35 | ccicp2d 36 | visualization_msgs 37 | eigen_conversions 38 | ground_segmentation 39 | actionlib 40 | nasa_msgs 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /graph_slam/src/global_mapping_viz.cpp: -------------------------------------------------------------------------------- 1 | #include "global_mapping_viz.h" 2 | 3 | std_msgs::ColorRGBA red, green, blue, white, black; 4 | visualization_msgs::MarkerArray marker_array; 5 | visualization_msgs::Marker msg_arrow; 6 | visualization_msgs::Marker msg_dots; 7 | geometry_msgs::Point msg_point; 8 | 9 | void graphVizInit() { 10 | // colours 11 | red.r = 1; red.g = 0; red.b = 0; red.a = 1; 12 | green.r = 0; green.g = 1; green.b = 0; green.a = 1; 13 | blue.r = 0; blue.g = 0; blue.b = 1; blue.a = 1; 14 | white.r = 1; white.g = 1; white.b = 1; white.a = 1; 15 | black.r = 0; black.g = 0; black.b = 0; black.a = 0; 16 | 17 | msg_arrow.header.frame_id = "/global"; 18 | msg_arrow.type = visualization_msgs::Marker::ARROW; 19 | msg_arrow.action = visualization_msgs::Marker::ADD; 20 | msg_arrow.lifetime = ros::Duration(0); 21 | 22 | msg_dots.header.frame_id = "/global"; 23 | msg_dots.type = visualization_msgs::Marker::SPHERE_LIST; 24 | msg_dots.action = visualization_msgs::Marker::ADD; 25 | msg_dots.lifetime = ros::Duration(0); 26 | msg_dots.pose.orientation.w = 1; 27 | 28 | } 29 | 30 | visualization_msgs::MarkerArray getVisualMsg (NodeList& pG, EdgeList& edgeList) 31 | { 32 | marker_array.markers.clear(); 33 | msg_arrow.points.clear(); 34 | msg_dots.points.clear(); 35 | int id = 0; 36 | 37 | // start vertex_list - Dots for graph nodes 38 | msg_dots.id = id; 39 | msg_dots.ns = "nodes"; 40 | msg_dots.scale.x = 1; 41 | msg_dots.scale.y = 1; 42 | msg_dots.scale.z = 1; 43 | msg_dots.color = red; 44 | for(unsigned int i=0; i < pG.size(); i++) { 45 | msg_point.x = pG[i].pose.pose.position.x; 46 | msg_point.y = pG[i].pose.pose.position.y; 47 | msg_point.z = pG[i].pose.pose.position.z; 48 | msg_dots.points.push_back(msg_point); 49 | } 50 | marker_array.markers.push_back(msg_dots); 51 | id++; 52 | 53 | // start vertex_list -arrows for graph edges 54 | 55 | for(unsigned int i=0; i < edgeList.size(); i++) { 56 | 57 | //grab the edge 58 | Edge pE = edgeList[i]; 59 | 60 | msg_arrow.id = id; 61 | msg_arrow.points.clear(); 62 | msg_arrow.ns = "edges"; 63 | msg_arrow.scale.x = 0.5; 64 | msg_arrow.scale.y = 1; 65 | msg_arrow.scale.z = 1; 66 | if(pE.from == 0) 67 | msg_arrow.color = white; 68 | else 69 | msg_arrow.color = blue; 70 | 71 | 72 | 73 | //fill in the point 74 | msg_point.x = pG[pE.from].pose.pose.position.x; 75 | msg_point.y = pG[pE.from].pose.pose.position.y; 76 | msg_point.z = pG[pE.from].pose.pose.position.z; 77 | //ROS_INFO("start adding: %f, %f", msg_point.x,msg_point.y); 78 | msg_arrow.points.push_back(msg_point); 79 | msg_point.x = pG[ pE.to].pose.pose.position.x; 80 | msg_point.y = pG[ pE.to].pose.pose.position.y; 81 | msg_point.z = pG[ pE.to].pose.pose.position.z; 82 | //ROS_INFO("end adding: %f, %f", msg_point.x,msg_point.y); 83 | msg_arrow.points.push_back(msg_point); 84 | //we have filled in the arrow stuff, push it back to the marker array 85 | marker_array.markers.push_back(msg_arrow); 86 | id++; 87 | } 88 | 89 | 90 | return marker_array; 91 | 92 | } 93 | -------------------------------------------------------------------------------- /graph_slam/src/global_mapping_viz.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOBAL_MAPPING_VIZ_H 2 | #define GLOBAL_MAPPING_VIZ_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "graph_slam.h" 19 | 20 | 21 | void graphVizInit(); 22 | visualization_msgs::MarkerArray getVisualMsg(NodeList& pG, EdgeList& edgeList); 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /graph_slam/src/graphSlamTools.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPHSLAMTOOLS_H 2 | #define GRAPHSLAMTOOLS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "ccicp2d/icpTools.h" 21 | 22 | #include "graph_slam/Edge.h" 23 | 24 | #include "graph_slam.h" 25 | 26 | #define KNN_DIST_THRESH 5.0 //distance between kf's 27 | #define GSLAM_KNN 3 //number of nearest neighbours to add in pose graph 28 | 29 | //FIXME: this should be an enum 30 | #define CTYPE_HOME 0 31 | #define CTYPE_ODOM 1 32 | #define CTYPE_NN 2 33 | 34 | //edge rejection based on ICP allowed movement 35 | #define DIST_MOVE_THRESH 10 //in meters 36 | #define ROT_MOVE_THRESH 0.2 //in radians 37 | 38 | namespace Eigen 39 | { 40 | typedef Eigen::Matrix Vector6d; 41 | typedef Eigen::Matrix Matrix6d; 42 | } 43 | 44 | double graphSlamGetNearestKF(PoseGraph& pG, Node& gN); 45 | void graphSlamAddEdgeToPrevious(PoseGraph& pG); 46 | void graphSlamAddEdgeToHome(PoseGraph& pG); 47 | bool graphSlamAddEdgeToX(PoseGraph& pG, int from, int to); 48 | vector graphSlamGetKNN(PoseGraph& pG, Node& gN, int K); 49 | Eigen::Matrix6d computeEdgeInformationLUM(pcl::PointCloud::Ptr& source_trans, pcl::PointCloud::Ptr& target, double max_corr_dist); 50 | 51 | void setup_gicp(); 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /graph_slam/src/graph_slam.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_SLAM_H 2 | #define GRAPH_SLAM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct Edge 10 | { 11 | int from; 12 | int to; 13 | geometry_msgs::PoseStamped edge; 14 | Eigen::MatrixXd edgeCov; 15 | Eigen::MatrixXd edgeInf; 16 | int ctype; 17 | 18 | Edge():edgeCov(6,6) {} 19 | }; 20 | 21 | struct Node 22 | { 23 | int idx; 24 | geometry_msgs::PoseStamped pose; 25 | pcl::PointCloud::Ptr keyframe; 26 | 27 | Node():keyframe(new pcl::PointCloud) {} 28 | }; 29 | 30 | typedef std::vector EdgeList; 31 | typedef std::vector NodeList; 32 | 33 | struct PoseGraph 34 | { 35 | NodeList nodes; 36 | EdgeList edges; 37 | }; 38 | 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /graph_slam/src/graph_slam/__init__.py: -------------------------------------------------------------------------------- 1 | #autogenerated by ROS python message generators -------------------------------------------------------------------------------- /graph_slam/src/graph_slam/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/servos/SLAM/696e26e6924ed2edd3f1997b7516cdbce3e79f59/graph_slam/src/graph_slam/__init__.pyc -------------------------------------------------------------------------------- /graph_slam/src/graph_slam/msg/_Edge.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from graph_slam/Edge.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | import graph_slam.msg 8 | 9 | class Edge(genpy.Message): 10 | _md5sum = "725b40920ff64ae7ec6c42f719a033e1" 11 | _type = "graph_slam/Edge" 12 | _has_header = False #flag to mark the presence of a Header object 13 | _full_text = """#delta is in the frame of node "from" 14 | #Node delta 15 | Node delta 16 | uint16 from 17 | uint16 to 18 | float64[9] covariance 19 | 20 | ================================================================================ 21 | MSG: graph_slam/Node 22 | float64 x 23 | float64 y 24 | float64 theta 25 | 26 | 27 | """ 28 | __slots__ = ['delta','from_','to','covariance'] 29 | _slot_types = ['graph_slam/Node','uint16','uint16','float64[9]'] 30 | 31 | def __init__(self, *args, **kwds): 32 | """ 33 | Constructor. Any message fields that are implicitly/explicitly 34 | set to None will be assigned a default value. The recommend 35 | use is keyword arguments as this is more robust to future message 36 | changes. You cannot mix in-order arguments and keyword arguments. 37 | 38 | The available fields are: 39 | delta,from_,to,covariance 40 | 41 | :param args: complete set of field values, in .msg order 42 | :param kwds: use keyword arguments corresponding to message field names 43 | to set specific fields. 44 | """ 45 | if args or kwds: 46 | super(Edge, self).__init__(*args, **kwds) 47 | #message fields cannot be None, assign default values for those that are 48 | if self.delta is None: 49 | self.delta = graph_slam.msg.Node() 50 | if self.from_ is None: 51 | self.from_ = 0 52 | if self.to is None: 53 | self.to = 0 54 | if self.covariance is None: 55 | self.covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.] 56 | else: 57 | self.delta = graph_slam.msg.Node() 58 | self.from_ = 0 59 | self.to = 0 60 | self.covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.] 61 | 62 | def _get_types(self): 63 | """ 64 | internal API method 65 | """ 66 | return self._slot_types 67 | 68 | def serialize(self, buff): 69 | """ 70 | serialize message into buffer 71 | :param buff: buffer, ``StringIO`` 72 | """ 73 | try: 74 | _x = self 75 | buff.write(_struct_3d2H.pack(_x.delta.x, _x.delta.y, _x.delta.theta, _x.from_, _x.to)) 76 | buff.write(_struct_9d.pack(*self.covariance)) 77 | except struct.error as se: self._check_types(se) 78 | except TypeError as te: self._check_types(te) 79 | 80 | def deserialize(self, str): 81 | """ 82 | unpack serialized message in str into this message instance 83 | :param str: byte array of serialized message, ``str`` 84 | """ 85 | try: 86 | if self.delta is None: 87 | self.delta = graph_slam.msg.Node() 88 | end = 0 89 | _x = self 90 | start = end 91 | end += 28 92 | (_x.delta.x, _x.delta.y, _x.delta.theta, _x.from_, _x.to,) = _struct_3d2H.unpack(str[start:end]) 93 | start = end 94 | end += 72 95 | self.covariance = _struct_9d.unpack(str[start:end]) 96 | return self 97 | except struct.error as e: 98 | raise genpy.DeserializationError(e) #most likely buffer underfill 99 | 100 | 101 | def serialize_numpy(self, buff, numpy): 102 | """ 103 | serialize message with numpy array types into buffer 104 | :param buff: buffer, ``StringIO`` 105 | :param numpy: numpy python module 106 | """ 107 | try: 108 | _x = self 109 | buff.write(_struct_3d2H.pack(_x.delta.x, _x.delta.y, _x.delta.theta, _x.from_, _x.to)) 110 | buff.write(self.covariance.tostring()) 111 | except struct.error as se: self._check_types(se) 112 | except TypeError as te: self._check_types(te) 113 | 114 | def deserialize_numpy(self, str, numpy): 115 | """ 116 | unpack serialized message in str into this message instance using numpy for array types 117 | :param str: byte array of serialized message, ``str`` 118 | :param numpy: numpy python module 119 | """ 120 | try: 121 | if self.delta is None: 122 | self.delta = graph_slam.msg.Node() 123 | end = 0 124 | _x = self 125 | start = end 126 | end += 28 127 | (_x.delta.x, _x.delta.y, _x.delta.theta, _x.from_, _x.to,) = _struct_3d2H.unpack(str[start:end]) 128 | start = end 129 | end += 72 130 | self.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 131 | return self 132 | except struct.error as e: 133 | raise genpy.DeserializationError(e) #most likely buffer underfill 134 | 135 | _struct_I = genpy.struct_I 136 | _struct_3d2H = struct.Struct("<3d2H") 137 | _struct_9d = struct.Struct("<9d") 138 | -------------------------------------------------------------------------------- /graph_slam/src/graph_slam/msg/_Node.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from graph_slam/Node.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | 8 | class Node(genpy.Message): 9 | _md5sum = "938fa65709584ad8e77d238529be13b8" 10 | _type = "graph_slam/Node" 11 | _has_header = False #flag to mark the presence of a Header object 12 | _full_text = """float64 x 13 | float64 y 14 | float64 theta 15 | 16 | 17 | """ 18 | __slots__ = ['x','y','theta'] 19 | _slot_types = ['float64','float64','float64'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | x,y,theta 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(Node, self).__init__(*args, **kwds) 37 | #message fields cannot be None, assign default values for those that are 38 | if self.x is None: 39 | self.x = 0. 40 | if self.y is None: 41 | self.y = 0. 42 | if self.theta is None: 43 | self.theta = 0. 44 | else: 45 | self.x = 0. 46 | self.y = 0. 47 | self.theta = 0. 48 | 49 | def _get_types(self): 50 | """ 51 | internal API method 52 | """ 53 | return self._slot_types 54 | 55 | def serialize(self, buff): 56 | """ 57 | serialize message into buffer 58 | :param buff: buffer, ``StringIO`` 59 | """ 60 | try: 61 | _x = self 62 | buff.write(_struct_3d.pack(_x.x, _x.y, _x.theta)) 63 | except struct.error as se: self._check_types(se) 64 | except TypeError as te: self._check_types(te) 65 | 66 | def deserialize(self, str): 67 | """ 68 | unpack serialized message in str into this message instance 69 | :param str: byte array of serialized message, ``str`` 70 | """ 71 | try: 72 | end = 0 73 | _x = self 74 | start = end 75 | end += 24 76 | (_x.x, _x.y, _x.theta,) = _struct_3d.unpack(str[start:end]) 77 | return self 78 | except struct.error as e: 79 | raise genpy.DeserializationError(e) #most likely buffer underfill 80 | 81 | 82 | def serialize_numpy(self, buff, numpy): 83 | """ 84 | serialize message with numpy array types into buffer 85 | :param buff: buffer, ``StringIO`` 86 | :param numpy: numpy python module 87 | """ 88 | try: 89 | _x = self 90 | buff.write(_struct_3d.pack(_x.x, _x.y, _x.theta)) 91 | except struct.error as se: self._check_types(se) 92 | except TypeError as te: self._check_types(te) 93 | 94 | def deserialize_numpy(self, str, numpy): 95 | """ 96 | unpack serialized message in str into this message instance using numpy for array types 97 | :param str: byte array of serialized message, ``str`` 98 | :param numpy: numpy python module 99 | """ 100 | try: 101 | end = 0 102 | _x = self 103 | start = end 104 | end += 24 105 | (_x.x, _x.y, _x.theta,) = _struct_3d.unpack(str[start:end]) 106 | return self 107 | except struct.error as e: 108 | raise genpy.DeserializationError(e) #most likely buffer underfill 109 | 110 | _struct_I = genpy.struct_I 111 | _struct_3d = struct.Struct("<3d") 112 | -------------------------------------------------------------------------------- /graph_slam/src/graph_slam/msg/_NodeArray.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from graph_slam/NodeArray.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | import graph_slam.msg 8 | 9 | class NodeArray(genpy.Message): 10 | _md5sum = "4b009d3ef060038e61c7be91101a1af0" 11 | _type = "graph_slam/NodeArray" 12 | _has_header = False #flag to mark the presence of a Header object 13 | _full_text = """#Array of nodes returned from the graph slam optimization 14 | Node[] nodes 15 | 16 | ================================================================================ 17 | MSG: graph_slam/Node 18 | float64 x 19 | float64 y 20 | float64 theta 21 | 22 | 23 | """ 24 | __slots__ = ['nodes'] 25 | _slot_types = ['graph_slam/Node[]'] 26 | 27 | def __init__(self, *args, **kwds): 28 | """ 29 | Constructor. Any message fields that are implicitly/explicitly 30 | set to None will be assigned a default value. The recommend 31 | use is keyword arguments as this is more robust to future message 32 | changes. You cannot mix in-order arguments and keyword arguments. 33 | 34 | The available fields are: 35 | nodes 36 | 37 | :param args: complete set of field values, in .msg order 38 | :param kwds: use keyword arguments corresponding to message field names 39 | to set specific fields. 40 | """ 41 | if args or kwds: 42 | super(NodeArray, self).__init__(*args, **kwds) 43 | #message fields cannot be None, assign default values for those that are 44 | if self.nodes is None: 45 | self.nodes = [] 46 | else: 47 | self.nodes = [] 48 | 49 | def _get_types(self): 50 | """ 51 | internal API method 52 | """ 53 | return self._slot_types 54 | 55 | def serialize(self, buff): 56 | """ 57 | serialize message into buffer 58 | :param buff: buffer, ``StringIO`` 59 | """ 60 | try: 61 | length = len(self.nodes) 62 | buff.write(_struct_I.pack(length)) 63 | for val1 in self.nodes: 64 | _x = val1 65 | buff.write(_struct_3d.pack(_x.x, _x.y, _x.theta)) 66 | except struct.error as se: self._check_types(se) 67 | except TypeError as te: self._check_types(te) 68 | 69 | def deserialize(self, str): 70 | """ 71 | unpack serialized message in str into this message instance 72 | :param str: byte array of serialized message, ``str`` 73 | """ 74 | try: 75 | if self.nodes is None: 76 | self.nodes = None 77 | end = 0 78 | start = end 79 | end += 4 80 | (length,) = _struct_I.unpack(str[start:end]) 81 | self.nodes = [] 82 | for i in range(0, length): 83 | val1 = graph_slam.msg.Node() 84 | _x = val1 85 | start = end 86 | end += 24 87 | (_x.x, _x.y, _x.theta,) = _struct_3d.unpack(str[start:end]) 88 | self.nodes.append(val1) 89 | return self 90 | except struct.error as e: 91 | raise genpy.DeserializationError(e) #most likely buffer underfill 92 | 93 | 94 | def serialize_numpy(self, buff, numpy): 95 | """ 96 | serialize message with numpy array types into buffer 97 | :param buff: buffer, ``StringIO`` 98 | :param numpy: numpy python module 99 | """ 100 | try: 101 | length = len(self.nodes) 102 | buff.write(_struct_I.pack(length)) 103 | for val1 in self.nodes: 104 | _x = val1 105 | buff.write(_struct_3d.pack(_x.x, _x.y, _x.theta)) 106 | except struct.error as se: self._check_types(se) 107 | except TypeError as te: self._check_types(te) 108 | 109 | def deserialize_numpy(self, str, numpy): 110 | """ 111 | unpack serialized message in str into this message instance using numpy for array types 112 | :param str: byte array of serialized message, ``str`` 113 | :param numpy: numpy python module 114 | """ 115 | try: 116 | if self.nodes is None: 117 | self.nodes = None 118 | end = 0 119 | start = end 120 | end += 4 121 | (length,) = _struct_I.unpack(str[start:end]) 122 | self.nodes = [] 123 | for i in range(0, length): 124 | val1 = graph_slam.msg.Node() 125 | _x = val1 126 | start = end 127 | end += 24 128 | (_x.x, _x.y, _x.theta,) = _struct_3d.unpack(str[start:end]) 129 | self.nodes.append(val1) 130 | return self 131 | except struct.error as e: 132 | raise genpy.DeserializationError(e) #most likely buffer underfill 133 | 134 | _struct_I = genpy.struct_I 135 | _struct_3d = struct.Struct("<3d") 136 | -------------------------------------------------------------------------------- /graph_slam/src/graph_slam/msg/__init__.py: -------------------------------------------------------------------------------- 1 | from ._Node import * 2 | from ._NodeArray import * 3 | from ._Edge import * 4 | -------------------------------------------------------------------------------- /graph_slam/src/gs_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | void nodeList_cb(const graph_slam::NodeArrayConstPtr& nodeList) 10 | { 11 | for (size_t i = 0; i < nodeList->nodes.size(); ++i) { 12 | std::cout << nodeList->nodes[i].x << ", " 13 | << nodeList->nodes[i].y << ", " 14 | << nodeList->nodes[i].theta << std::endl; 15 | } 16 | 17 | } 18 | 19 | graph_slam::Edge newEdge(float x, float y, float theta, int from, int to) 20 | { 21 | graph_slam::Edge edge; 22 | edge.delta.x = x; 23 | edge.delta.y = y; 24 | edge.delta.theta = theta; 25 | edge.to = to; 26 | edge.from = from; 27 | edge.covariance[0] = 1; 28 | edge.covariance[4] = 1; 29 | edge.covariance[8] = 1; 30 | 31 | return edge; 32 | } 33 | 34 | int main(int argc, char **argv) 35 | { 36 | // Initialize ROS 37 | ros::init (argc, argv, "gs_test"); 38 | ros::NodeHandle nh; 39 | 40 | ros::Subscriber sub = nh.subscribe("/graph_slam/nodeArray", 10, nodeList_cb); 41 | ros::Publisher pub = nh.advertise ("/graph_slam/edges", 100); 42 | 43 | //generate graph 44 | graph_slam::Edge edge; 45 | 46 | ros::Duration(0.5).sleep(); 47 | 48 | edge = newEdge(1,0,1.5, 0,1); 49 | pub.publish(edge); 50 | 51 | ros::Duration(0.5).sleep(); 52 | 53 | edge = newEdge(1,0,1.5, 1,2); 54 | pub.publish(edge); 55 | 56 | ros::Duration(0.5).sleep(); 57 | 58 | edge = newEdge(1,0,1.5, 2,3); 59 | pub.publish(edge); 60 | 61 | ros::Duration(0.5).sleep(); 62 | 63 | edge = newEdge(1,0,1.5, 3,0); 64 | pub.publish(edge); 65 | 66 | ros::Duration(0.5).sleep(); 67 | 68 | edge = newEdge(0,-1,0, 3,6); 69 | pub.publish(edge); 70 | 71 | ros::Duration(0.5).sleep(); 72 | 73 | edge = newEdge(0,2,-1.5, 6,2); 74 | pub.publish(edge); 75 | 76 | std::cout << "Published edges... spinning" << std::endl; 77 | ros::spin(); 78 | 79 | } 80 | -------------------------------------------------------------------------------- /ground_segmentation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ground_segmentation) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs 8 | geometry_msgs pcl_ros tf tf_conversions visualization_msgs 9 | nav_msgs microstrain_3dmgx2_imu 10 | nasa_msgs) 11 | 12 | ################################### 13 | ## catkin specific configuration ## 14 | ################################### 15 | catkin_package(INCLUDE_DIRS include) 16 | 17 | ########### 18 | ## Build ## 19 | ########### 20 | set(CMAKE_BUILD_TYPE Release) 21 | include_directories( 22 | include 23 | ${catkin_INCLUDE_DIRS} 24 | ) 25 | 26 | 27 | #add_executable(ground_segmentation src/ground_segmentation.cpp src/groundSegmentation.cpp) 28 | add_library(ground_segmentation src/groundSegmentation.cpp) 29 | 30 | set_target_properties(ground_segmentation PROPERTIES COMPILE_FLAGS "-ftemplate-depth-256" ) 31 | 32 | target_link_libraries(ground_segmentation ${catkin_LIBRARIES} ) 33 | -------------------------------------------------------------------------------- /ground_segmentation/README: -------------------------------------------------------------------------------- 1 | -- Ground Segmentation Library -- 2 | 3 | This library segments the3 ground for a pointcloud using gaussian processes 4 | 5 | 6 | Classes: 7 | 8 | GroundSegmentation 9 | Functions: 10 | 11 | segment_ground(...) 12 | classify_points(...) 13 | 14 | Members: 15 | 16 | -------------------------------------------------------------------------------- /ground_segmentation/include/ground_segmentation/PointcloudXYZGD.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef POINTCLOUDXYZGD_H 3 | #define POINTCLOUDXYZGD_H 4 | #define PCL_NO_PRECOMPILE 5 | #include 6 | #include 7 | 8 | struct PointXYZGD 9 | { 10 | PCL_ADD_POINT4D; // quad-word XYZ 11 | uint16_t ground_adj; ///< ground adjacent flag 12 | uint16_t drivable; ///< driveable flag 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 14 | } EIGEN_ALIGN16; 15 | 16 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZGD, 17 | (float, x, x) 18 | (float, y, y) 19 | (float, z, z) 20 | (uint16_t, ground_adj, ground_adj) 21 | (uint16_t, drivable, drivable)) 22 | 23 | 24 | // Helper functions 25 | inline bool isDR(float in) { 26 | return (in > 0.7) ; 27 | } 28 | 29 | inline bool isGA(float in) { 30 | return (in > 0.5) ; 31 | } 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /ground_segmentation/include/ground_segmentation/groundSegmentation.h: -------------------------------------------------------------------------------- 1 | #ifndef NDTDATASTRUCTS_H 2 | #define NDTDATASTRUCTS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "ground_segmentation/PointcloudXYZGD.h" 12 | //#include "HLBFGS/HLBFGS.h" 13 | 14 | //Defines 15 | #define INVALID 1000 16 | 17 | #define NUMBINSA 72 //number of angular sectors 18 | #define NUMBINSL 200 //number of linear bins 19 | 20 | using namespace std; 21 | 22 | 23 | struct signalPoint 24 | { 25 | double range; 26 | double height; 27 | int idx; 28 | bool isGround; 29 | }; 30 | 31 | struct linCell 32 | { 33 | 34 | vector binPoints; //all the points 35 | vector obsPoints; //just the obs points 36 | vector drvPoints; 37 | vector groundPoints; //just the ground points 38 | PointXYZGD prototypePoint; 39 | int cAssigned; //what cluster is it assigned to 40 | Eigen::Vector3d obsMean; //mean of obstacle points 41 | 42 | 43 | 44 | }; 45 | 46 | struct angCell 47 | { 48 | 49 | linCell lCell[NUMBINSL]; //the linear cells within that angle sector 50 | pcl::PointXY rangeHeightSignal[NUMBINSL]; 51 | vector sigPoints; //range height signal for that sector 52 | 53 | 54 | }; 55 | 56 | 57 | 58 | struct polarBinGrid 59 | { 60 | 61 | angCell aCell[NUMBINSA]; 62 | 63 | }; 64 | 65 | 66 | 67 | class groundSegmentation { 68 | 69 | public: 70 | 71 | //variables 72 | double RMAX; //max radius of point to consider 73 | int MAXBINPOINTS; //max number of points to consider per bin 74 | int NUMSEEDPOINTS; 75 | 76 | //for GP model 77 | double P_L; // length parameter, so how close the points have to be in the GP model to consider them correlated 78 | double P_SF; // scaling on the whole covariance function 79 | double P_SN; // the expected noise for the mode 80 | double P_TMODEL; // the required confidence required in order to consider something ground 81 | double P_TDATA; // scaled value that is required for a query point to be considered ground 82 | double P_TG; // ground height threshold 83 | 84 | double ROBOT_HEIGHT; 85 | 86 | //seeding parameters 87 | double MAXSEEDRANGE;//meters 88 | double MAXSEEDHEIGHT; //meters 89 | 90 | //data structures 91 | polarBinGrid* pBG; 92 | int test; 93 | pcl::PointCloud::Ptr refCloud; 94 | pcl::PointCloud::Ptr gCloud; //ground points 95 | pcl::PointCloud::Ptr oCloud; //obstacle points 96 | pcl::PointCloud::Ptr dCloud; //obstacle points 97 | 98 | //constructor 99 | groundSegmentation(); 100 | void setupGroundSegmentation(pcl::PointCloud::Ptr , pcl::PointCloud::Ptr , pcl::PointCloud::Ptr, pcl::PointCloud::Ptr); 101 | void genPolarBinGrid(pcl::PointCloud::Ptr ); 102 | void initializePolarBinGrid(void); 103 | Eigen::MatrixXd genGPModel(vector& , vector& ,float ,float ); 104 | void segmentGround(void); 105 | void sectorINSAC(int); 106 | 107 | //setters 108 | void set_rmax(double a){ RMAX = a;} 109 | void set_num_maxbinpoints(int a){ MAXBINPOINTS = a;} 110 | void set_num_seedpoints(int a){ NUMSEEDPOINTS = a;} 111 | 112 | void set_gp_lengthparameter(double a){ P_L = a;} 113 | void set_gp_covariancescale(double a){ P_SF = a;} 114 | void set_gp_modelnoise(double a){ P_SN = a;} 115 | void set_gp_groundmodelconfidence(double a){ P_TMODEL = a;} 116 | void set_gp_grounddataconfidence(double a){ P_TDATA = a;} 117 | void set_gp_groundthreshold(double a){ P_TG = a;} 118 | 119 | void set_robotheight(double a){ ROBOT_HEIGHT = a;} 120 | 121 | void set_seeding_maxrange(double a){ MAXSEEDRANGE = a;} 122 | void set_seeding_maxheight(double a){ MAXSEEDHEIGHT = a;} 123 | 124 | 125 | 126 | 127 | 128 | }; 129 | 130 | 131 | #endif 132 | -------------------------------------------------------------------------------- /ground_segmentation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ground_segmentation 4 | 0.0.0 5 | ground_segmentation 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | tf 16 | tf_conversions 17 | opencv2 18 | visualization_msgs 19 | nav_msgs 20 | microstrain_3dmgx2_imu 21 | nasa_msgs 22 | 23 | roscpp 24 | std_msgs 25 | sensor_msgs 26 | geometry_msgs 27 | pcl_ros 28 | tf 29 | tf_conversions 30 | opencv2 31 | visualization_msgs 32 | nav_msgs 33 | microstrain_3dmgx2_imu 34 | nasa_msgs 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /local_mapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(local_mapper) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs 8 | geometry_msgs pcl_ros tf tf_conversions visualization_msgs 9 | nav_msgs graph_slam ekf nasa_msgs mls eigen_conversions ground_segmentation) 10 | 11 | find_package(OpenCV) 12 | include_directories(${OpenCV_INCLUDE_DIRS}) 13 | 14 | ################################### 15 | ## catkin specific configuration ## 16 | ################################### 17 | #catkin_package(INCLUDE_DIRS include) 18 | catkin_package() 19 | 20 | ########### 21 | ## Build ## 22 | ########### 23 | set(CMAKE_BUILD_TYPE Release) 24 | include_directories( 25 | include 26 | ${catkin_INCLUDE_DIRS} 27 | ) 28 | 29 | 30 | add_executable(local_mapper src/local_mapper.cpp ) 31 | 32 | target_link_libraries(local_mapper ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} mls) 33 | -------------------------------------------------------------------------------- /local_mapper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | local_mapper 4 | 0.0.0 5 | local_mapper 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | tf 16 | tf_conversions 17 | opencv2 18 | visualization_msgs 19 | nav_msgs 20 | graph_slam 21 | ekf 22 | global_matching 23 | nasa_msgs 24 | mls 25 | eigen_conversions 26 | ground_segmentation 27 | 28 | roscpp 29 | std_msgs 30 | sensor_msgs 31 | geometry_msgs 32 | pcl_ros 33 | tf 34 | tf_conversions 35 | opencv2 36 | visualization_msgs 37 | nav_msgs 38 | graph_slam 39 | ekf 40 | global_matching 41 | nasa_msgs 42 | mls 43 | eigen_conversions 44 | ground_segmentation 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /local_mapper/src/local_mapper.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | // PCL Includes 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | //ROS includes 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | //Our Includes 26 | #include 27 | 28 | 29 | MLS local_map(200, 200, 0.2, true); 30 | 31 | geometry_msgs::PoseStamped curPose; 32 | 33 | ros::Publisher localCloudPub; 34 | ros::Publisher drivablilityPub; 35 | 36 | pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); 37 | 38 | ros::Time pose_time; 39 | ros::Time cloud_time; 40 | bool newCloud = false; 41 | 42 | void pose_cb(const geometry_msgs::PoseStamped& pose) 43 | { 44 | curPose = pose; 45 | pose_time = pose.header.stamp; 46 | } 47 | 48 | void poseOffet_cb(const geometry_msgs::PoseStamped& pose) 49 | { 50 | local_map.offsetMap(pose); 51 | } 52 | 53 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 54 | { 55 | //Update Map 56 | pcl::fromROSMsg(*input, *input_cloud); 57 | 58 | cloud_time = input->header.stamp; 59 | newCloud = true; 60 | 61 | //shoudl we setPose() here? 62 | 63 | } 64 | 65 | int 66 | main (int argc, char** argv) 67 | { 68 | // Initialize ROS 69 | ros::init (argc, argv, "local_mapper"); 70 | ros::NodeHandle nh; 71 | 72 | // Create a ROS subscriber for the input point cloud 73 | ros::Subscriber poseSub = nh.subscribe("/mapping/ekf/pose", 1, pose_cb); 74 | //ros::Subscriber poseSub = nh.subscribe("/mapping/scan_reg/pose", 1, pose_cb); 75 | ros::Subscriber poseOffsetSub = nh.subscribe("/mapping/graph_slam/pose_offset", 1, poseOffet_cb); 76 | ros::Subscriber sub = nh.subscribe ("/velodyne_points", 1, cloud_cb); 77 | 78 | localCloudPub = nh.advertise("/mapping/local_poiuntcloud", 1, true); 79 | drivablilityPub = nh.advertise("/mapping/local_drivability", 1, true); 80 | ros::Publisher mlsPub = nh.advertise("/mapping/local/mls_viz", 1, true); 81 | 82 | 83 | //Set map options 84 | //local_map.setDisablePointCloud(true); 85 | //local_map.setMaxClusterPoints(100); 86 | local_map.setMinClusterPoints(20); 87 | 88 | //local_map.setClusterDistTheshold(1.0); 89 | //local_map.setClusterSigmaFactor(5); 90 | //local_map.setNormalTheshold(0.2); 91 | //local_map.setHeightTheshold(0.3); 92 | 93 | local_map.clearMap(); 94 | 95 | ros::Rate loop_rate(50); //FIXME should this be higer? 96 | 97 | int count = 0; 98 | while(ros::ok()) { 99 | loop_rate.sleep(); 100 | ros::spinOnce(); 101 | 102 | if(newCloud && (pose_time-cloud_time).toSec() >= 0) { 103 | //FIXME this only syncs to the scan regisration... roll and pitch will be off.... 104 | newCloud = false; 105 | count++; 106 | 107 | local_map.addToMap(input_cloud, curPose); 108 | 109 | //publish cloud and map 110 | pcl::PointCloud::Ptr local_cloud; 111 | sensor_msgs::PointCloud2 cloud_msg; 112 | local_map.filterPointCloud(0.1,0.1); //filter gloibal cloud so it won't kill rviz 113 | local_cloud = local_map.getGlobalCloud(); 114 | pcl::toROSMsg(*local_cloud,cloud_msg); 115 | cloud_msg.header.frame_id = "/local_oriented"; 116 | cloud_msg.header.stamp = ros::Time::now(); 117 | localCloudPub.publish(cloud_msg); 118 | 119 | nav_msgs::OccupancyGrid::Ptr drivability; 120 | drivability = local_map.getDrivability(); 121 | drivability->header.frame_id = "/local_oriented"; 122 | drivablilityPub.publish(drivability); 123 | 124 | local_map.visualize(mlsPub, "/local_oriented"); 125 | 126 | } 127 | 128 | } 129 | 130 | } 131 | 132 | 133 | -------------------------------------------------------------------------------- /mls/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mls) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs 8 | geometry_msgs pcl_ros tf tf_conversions nav_msgs eigen_conversions visualization_msgs ground_segmentation) 9 | 10 | ################################### 11 | ## catkin specific configuration ## 12 | ################################### 13 | catkin_package(INCLUDE_DIRS include) 14 | 15 | ########### 16 | ## Build ## 17 | ########### 18 | set(CMAKE_BUILD_TYPE Release) 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | 25 | add_library(mls src/mls.cpp) 26 | target_link_libraries(mls ${catkin_LIBRARIES} ground_segmentation) 27 | -------------------------------------------------------------------------------- /mls/README: -------------------------------------------------------------------------------- 1 | -- Ground Segmentation Library -- 2 | 3 | This library segments the3 ground for a pointcloud using gaussian processes 4 | 5 | 6 | Classes: 7 | 8 | GroundSegmentation 9 | Functions: 10 | 11 | segment_ground(...) 12 | classify_points(...) 13 | 14 | Members: 15 | 16 | -------------------------------------------------------------------------------- /mls/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mls 4 | 0.0.0 5 | mls 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | nav_msgs 16 | tf 17 | eigen_conversions 18 | tf_conversions 19 | visualization_msgs 20 | ground_segmentation 21 | 22 | roscpp 23 | std_msgs 24 | sensor_msgs 25 | geometry_msgs 26 | pcl_ros 27 | nav_msgs 28 | tf 29 | eigen_conversions 30 | tf_conversions 31 | visualization_msgs 32 | ground_segmentation 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /nasa_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nasa_mapping) 3 | find_package(catkin REQUIRED) 4 | catkin_package() 5 | -------------------------------------------------------------------------------- /nasa_mapping/launch/drivers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /nasa_mapping/launch/nasa_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /nasa_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nasa_mapping 4 | 0.0.0 5 | nasa mapping top-level package 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | 11 | scan_registration2D 12 | ekf 13 | graph_slam 14 | local_mapper 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /sample_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sample_mapping) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs nav_msgs 8 | geometry_msgs pcl_ros tf tf_conversions message_generation visualization_msgs 9 | eigen_conversions camera_to_velodyne OpenCV cv_bridge image_transport nasa_msgs) 10 | ## System dependencies are found with CMake's conventions 11 | # find_package(Boost REQUIRED COMPONENTS system) 12 | 13 | 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 31 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 32 | ## pulled in transitively but can be declared for certainty nonetheless: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | #generate_messages(DEPENDENCIES std_msgs geometry_msgs) 73 | 74 | ################################### 75 | ## catkin specific configuration ## 76 | ################################### 77 | ## The catkin_package macro generates cmake config files for your package 78 | ## Declare things to be passed to dependent projects 79 | ## INCLUDE_DIRS: uncomment this if you package contains header files 80 | ## LIBRARIES: libraries you create in this project that dependent projects also need 81 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 82 | ## DEPENDS: system dependencies of this project that dependent projects also need 83 | catkin_package( 84 | INCLUDE_DIRS include 85 | # LIBRARIES sample_mapping 86 | # CATKIN_DEPENDS roscpp rospy 87 | # DEPENDS system_lib 88 | ) 89 | 90 | ########### 91 | ## Build ## 92 | ########### 93 | 94 | ## Specify additional locations of header files 95 | ## Your package locations should be listed before other locations 96 | # include_directories(include) 97 | set(CMAKE_BUILD_TYPE Release) 98 | include_directories( 99 | include 100 | ${catkin_INCLUDE_DIRS} 101 | ${OpenCV_INCLUDE_DIRS} 102 | ) 103 | 104 | 105 | 106 | ## Declare a cpp library 107 | # add_library(sample_mapping 108 | # src/${PROJECT_NAME}/sample_mapping.cpp 109 | # ) 110 | 111 | ## Declare a cpp executable 112 | add_executable(sample_mapping_node src/sample_mapping_node.cpp src/sample_mapping.cpp) 113 | add_executable(sample_conversion_node src/sample_conversion_node.cpp) 114 | 115 | ## Add cmake target dependencies of the executable/library 116 | ## as an example, message headers may need to be generated before nodes 117 | # add_dependencies(sample_mapping_node sample_mapping_generate_messages_cpp) 118 | 119 | ## Specify libraries to link a library or executable target against 120 | # target_link_libraries(sample_mapping_node 121 | # ${catkin_LIBRARIES} 122 | # ) 123 | 124 | target_link_libraries(sample_mapping_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} camera_to_velodyne) 125 | target_link_libraries(sample_conversion_node ${catkin_LIBRARIES} camera_to_velodyne) 126 | 127 | ############# 128 | ## Install ## 129 | ############# 130 | 131 | # all install targets should use catkin DESTINATION variables 132 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 133 | 134 | ## Mark executable scripts (Python etc.) for installation 135 | ## in contrast to setup.py, you can choose the destination 136 | # install(PROGRAMS 137 | # scripts/my_python_script 138 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 139 | # ) 140 | 141 | ## Mark executables and/or libraries for installation 142 | # install(TARGETS sample_mapping sample_mapping_node 143 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 144 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 145 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 146 | # ) 147 | 148 | ## Mark cpp header files for installation 149 | # install(DIRECTORY include/${PROJECT_NAME}/ 150 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 151 | # FILES_MATCHING PATTERN "*.h" 152 | # PATTERN ".svn" EXCLUDE 153 | # ) 154 | 155 | ## Mark other files for installation (e.g. launch and bag files, etc.) 156 | # install(FILES 157 | # # myfile1 158 | # # myfile2 159 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 160 | # ) 161 | 162 | ############# 163 | ## Testing ## 164 | ############# 165 | 166 | ## Add gtest based cpp test target and link libraries 167 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sample_mapping.cpp) 168 | # if(TARGET ${PROJECT_NAME}-test) 169 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 170 | # endif() 171 | 172 | ## Add folders to be run by python nosetests 173 | # catkin_add_nosetests(test) 174 | -------------------------------------------------------------------------------- /sample_mapping/hand_cal: -------------------------------------------------------------------------------- 1 | #Hand Calibration Numbers 2 | -------------------------------------------------------------------------------- /sample_mapping/include/sample_mapping/sample_mapping.h: -------------------------------------------------------------------------------- 1 | #ifndef SAMPLEMAPPING_H 2 | #define SAMPLEMAPPING_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | struct cell 28 | { 29 | double probability; 30 | 31 | }; 32 | 33 | struct intparameters 34 | { 35 | double prob_max; 36 | double prob_min; 37 | double rmin; 38 | double rmax; 39 | }; 40 | 41 | class SampleGaussian 42 | { 43 | private: 44 | double bound; //3 sigma bound 45 | 46 | public: 47 | double amplitude; 48 | double cov; //sigma squared 49 | double x; 50 | double y; 51 | 52 | SampleGaussian(double amp, double var,double xpos, double ypos) 53 | { 54 | amplitude = amp; 55 | cov = var; 56 | bound = 3*sqrt(var); 57 | x = xpos; 58 | y = ypos; 59 | } 60 | 61 | double getProbabilityValue(double x, double y) //gets the probability value for the gaussian (assume centered at 0,0) 62 | { 63 | return amplitude*exp( -1.0*( (x*x)/(2*cov) + (y*y)/(2*cov)) ); 64 | } 65 | 66 | double getBound(){return bound;} 67 | 68 | 69 | }; 70 | 71 | class SampleMap { 72 | private: 73 | 74 | cell* data; //grid data 75 | int size_x; //in bins 76 | int size_y; //in bins 77 | double size_x_meters; 78 | double size_y_meters; 79 | double xmin_meters; 80 | double ymin_meters; 81 | double resolution; 82 | nav_msgs::OccupancyGrid::Ptr sample_grid; 83 | intparameters integration_parameters; 84 | double fov_cone; 85 | 86 | //opencv variables 87 | cv::Mat cv_map; 88 | double pixel_prob_threshold; // min probability in order to make it into the cv map (normalize 0 - 1) 89 | std::vector sample_points; //vector of sample locations in global xy 90 | 91 | //functions 92 | 93 | int computeSampleGaussian(double cov); 94 | int mapPixelToXY(int camera, int u, int v); //maps a pixel uv co-ordinate to a XY (meters) location in the map 95 | int mapXYToLinear(double x, double y); //converts global XY co-ordinates to a linear index for map cell 96 | int mapIJToXY(double i, double j, double& x, double& y); //converts map i-j coordinates to global XY 97 | int mapIJToLinear(double i, double j); //maps 2 index in cells to linear index 98 | int mapProbabilityToOccupancy(double _input_range_min, double _input_range_max, double _input_value_tobe_converted); 99 | void copyMaptoCV(); 100 | void processMapCV(); 101 | 102 | public: 103 | 104 | SampleMap(int xbins, int ybins, double res); 105 | void initalizeSampleMap(); 106 | void addSampleToMap(double x, double y, SampleGaussian sample); //adds a sample at global x y to the map with a mean prob and cov 107 | void removeConeFromMap(double x, double y, double theta, double dec_value); 108 | void setIntegrationParameters(intparameters ip){ integration_parameters = ip;} 109 | nav_msgs::OccupancyGrid::Ptr getSampleOccupancy() { return sample_grid; } 110 | cv::Mat getCVMap(){ copyMaptoCV(); processMapCV(); return cv_map;} 111 | 112 | }; 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /sample_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sample_mapping 4 | 0.0.0 5 | The sample_mapping package 6 | 7 | 8 | 9 | 10 | adas 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | roscpp 46 | rospy 47 | 48 | std_msgs 49 | sensor_msgs 50 | geometry_msgs 51 | pcl_ros 52 | tf 53 | tf_conversions 54 | nav_msgs 55 | visualization_msgs 56 | eigen_conversions 57 | camera_to_velodyne 58 | vision_opencv 59 | cv_bridge 60 | image_transport 61 | nasa_msgs 62 | 63 | 64 | 65 | sensor_msgs 66 | geometry_msgs 67 | pcl_ros 68 | tf 69 | tf_conversions 70 | nav_msgs 71 | visualization_msgs 72 | eigen_conversions 73 | camera_to_velodyne 74 | vision_opencv 75 | cv_bridge 76 | image_transport 77 | nasa_msgs 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /sample_mapping/src/sample_conversion_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | //Our Includes 6 | #include 7 | 8 | using namespace std; 9 | 10 | //globals 11 | // camera to velodnye 12 | CameraToVelodyne CtoV; 13 | geometry_msgs::PoseStamped curPose; 14 | ros::Publisher sampleConversionPub; 15 | ros::Time pose_time; 16 | bool first_pose=false; 17 | 18 | 19 | void pose_cb(const geometry_msgs::PoseStamped& pose) 20 | { 21 | curPose = pose; 22 | pose_time = pose.header.stamp; 23 | 24 | } 25 | 26 | 27 | Eigen::Vector3d convert_uv_to_xyz(int cam_id, int u, int v){ 28 | 29 | 30 | Eigen::Vector2d camuv(u,v); 31 | Eigen::Vector3d cray = CtoV.camerauv_to_cameraray(camuv,cam_id); 32 | 33 | Eigen::Vector3d rpoint; 34 | rpoint = CtoV.cameraray_to_rangeflatground(cray,cam_id,-1.3);//-1.464); //last param is the height of the sensor from the ground 35 | 36 | 37 | //now convert the point in velodyne frame to global frame 38 | tf::Quaternion q_robot; 39 | tf::quaternionMsgToTF(curPose.pose.orientation, q_robot); 40 | Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity(); 41 | tf::Matrix3x3 matq(q_robot); 42 | transformation << matq[0][0], matq[0][1], matq[0][2], curPose.pose.position.x, 43 | matq[1][0], matq[1][1], matq[1][2], curPose.pose.position.y, 44 | matq[2][0], matq[2][1], matq[2][2], curPose.pose.position.z, 45 | 0,0,0,1; 46 | 47 | Eigen::Vector4d rpoint_h(rpoint(0),rpoint(1),rpoint(2),1); 48 | Eigen::Vector4d transformed_rpoint = transformation*rpoint_h; 49 | 50 | Eigen::Vector3d xyz_location(transformed_rpoint(0),transformed_rpoint(1),transformed_rpoint(2)); 51 | 52 | return xyz_location; 53 | 54 | } 55 | 56 | void camera_cb(const nasa_msgs::ObjectMapCam& cam) 57 | { 58 | if(cam.state >= 1) { 59 | nasa_msgs::ObjectMapCam xyzObjectCam; 60 | Eigen::Vector3d xyz_location = convert_uv_to_xyz(cam.camera_id, (int)cam.x, (int)cam.y); 61 | //copy all the info 62 | xyzObjectCam = cam; 63 | xyzObjectCam.global_pose.position.x = xyz_location(0); 64 | xyzObjectCam.global_pose.position.y = xyz_location(1); 65 | xyzObjectCam.global_pose.position.z = xyz_location(2); 66 | 67 | sampleConversionPub.publish(xyzObjectCam); //publish message with xyz global info 68 | } 69 | } 70 | 71 | int main (int argc, char** argv) 72 | { 73 | // Initialize ROS 74 | ros::init (argc, argv, "sample_mapper"); 75 | ros::NodeHandle nh; 76 | 77 | // Create a ROS subscriber for the current pose 78 | ros::Subscriber poseSub = nh.subscribe("/mapping/ekf/pose", 1, pose_cb); 79 | //ros publisher for sample xyz location 80 | sampleConversionPub = nh.advertise("/mapping/cam_center", 1, true); 81 | //ros subscriber for cameara uv info 82 | ros::Subscriber cameraSub = nh.subscribe("/sample_detection/cam_center", 1, camera_cb); //FIXME: change me to the right topic! 83 | 84 | ros::Rate loop_rate(50); 85 | 86 | curPose.pose.position.x=0;curPose.pose.position.y=0;curPose.pose.position.z=0; 87 | curPose.pose.orientation.x=0;curPose.pose.orientation.y=0;curPose.pose.orientation.z=0;curPose.pose.orientation.w=1; 88 | 89 | 90 | while(ros::ok()) { 91 | loop_rate.sleep(); 92 | 93 | ros::spinOnce(); 94 | /*double curr_x = 0; 95 | double curr_y = 0; 96 | double curr_theta = 0; 97 | cout<<"converting"< 5 | 6 | //globals 7 | 8 | geometry_msgs::PoseStamped curPose; 9 | ros::Publisher sampleMapPub; 10 | ros::Time pose_time; 11 | bool first_pose=false; 12 | 13 | //opencv 14 | 15 | static const std::string OPENCV_WINDOW = "sample map window"; 16 | 17 | //new sample map 18 | 19 | SampleMap SM(400,400,0.5); 20 | 21 | void pose_cb(const geometry_msgs::PoseStamped& pose) 22 | { 23 | curPose = pose; 24 | pose_time = pose.header.stamp; 25 | first_pose=true; 26 | } 27 | 28 | 29 | int main (int argc, char** argv) 30 | { 31 | // Initialize ROS 32 | ros::init (argc, argv, "sample_mapper"); 33 | ros::NodeHandle nh; 34 | 35 | // Create a ROS subscriber for the input point cloud 36 | ros::Subscriber poseSub = nh.subscribe("/mapping/ekf/pose", 1, pose_cb); 37 | sampleMapPub = nh.advertise("/mapping/sample_map", 1, true); 38 | ros::Rate loop_rate(10); //FIXME should this be higer? 39 | int loopcounter = 0; 40 | 41 | //set the integration params 42 | intparameters ip; 43 | ip.prob_max = 5; 44 | ip.prob_min = 0; 45 | ip.rmax = 10; 46 | ip.rmin = 1; 47 | SM.setIntegrationParameters(ip); 48 | //init the map 49 | SM.initalizeSampleMap(); 50 | 51 | std::vector sampleVec; 52 | 53 | SampleGaussian sample1 = SampleGaussian(1,0.1, 5,0); 54 | SampleGaussian sample2 = SampleGaussian(1,0.1, 20,-10); 55 | SampleGaussian sample3 = SampleGaussian(5,0.2, 20,1); 56 | 57 | sampleVec.push_back(sample1); 58 | sampleVec.push_back(sample2); 59 | sampleVec.push_back(sample3); 60 | 61 | SM.addSampleToMap(sample3.x,sample3.y,sample3); 62 | 63 | 64 | //test camera to velodnye 65 | CameraToVelodyne CtoV; 66 | 67 | cv::Mat src; 68 | //src = cv::Mat::zeros(600, 600, CV_8UC1); 69 | 70 | while(ros::ok()) { 71 | loop_rate.sleep(); 72 | 73 | double curr_x = 0; 74 | double curr_y = 0; 75 | double curr_theta = 0; 76 | 77 | if(first_pose) 78 | { 79 | 80 | curr_x = curPose.pose.position.x; 81 | curr_y = curPose.pose.position.y; 82 | curr_theta = tf::getYaw(curPose.pose.orientation); 83 | } 84 | 85 | 86 | 87 | 88 | if(loopcounter%10 == 0) 89 | { 90 | //pretend we see a new sample 91 | int ridx = (rand() % (int)(1+ 1)); 92 | SampleGaussian rsample = sampleVec[ridx]; 93 | //add it to the map 94 | SM.addSampleToMap(rsample.x,rsample.y,rsample); 95 | } 96 | 97 | //remove from map 98 | 99 | SM.removeConeFromMap(curr_x,curr_y,curr_theta, 0.008); 100 | 101 | //publish the occupancy grid 102 | 103 | nav_msgs::OccupancyGrid::Ptr sample_occupancy; 104 | sample_occupancy = SM.getSampleOccupancy(); 105 | sampleMapPub.publish(sample_occupancy); 106 | 107 | //std::cout<<"published occupancy"< 2 | 3 | scan_registration 4 | 0.0.0 5 | scan_registration 6 | jdservos 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | pcl_ros 15 | tf 16 | tf_conversions 17 | opencv2 18 | visualization_msgs 19 | nav_msgs 20 | microstrain_3dmgx2_imu 21 | actionlib 22 | actionlib_msgs 23 | nasa_msgs 24 | ground_segmentation 25 | ccicp2d 26 | eigen_conversions 27 | 28 | roscpp 29 | std_msgs 30 | sensor_msgs 31 | geometry_msgs 32 | pcl_ros 33 | tf 34 | tf_conversions 35 | opencv2 36 | visualization_msgs 37 | nav_msgs 38 | microstrain_3dmgx2_imu 39 | actionlib 40 | actionlib_msgs 41 | nasa_msgs 42 | ground_segmentation 43 | ccicp2d 44 | eigen_conversions 45 | graph_slam 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /scan_registration/src/scan_registration.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // scan_registration.cpp 3 | // 4 | // This node warps the scan registration library to subscribe and 5 | // publish ros messages. ` 6 | // 7 | // 8 | 9 | //C includes 10 | 11 | //ros includes 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | //pcl includes 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | //libraries 26 | #include "ccicp2d/icpTools.h" 27 | 28 | //local includes 29 | #include "scan_registration.h" 30 | 31 | 32 | using namespace std; 33 | using namespace pcl; 34 | 35 | //ros 36 | 37 | #define DEBUG 1 38 | 39 | ros::Publisher posePub; 40 | ros::Publisher targetPub; 41 | ros::Publisher scenePub; 42 | 43 | //ros occupancy data struct 44 | pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); 45 | pcl::PointCloud::Ptr target_obs_cloud (new pcl::PointCloud); 46 | pcl::PointCloud::Ptr target_gnd_cloud (new pcl::PointCloud); 47 | 48 | #if DEBUG 49 | pcl::PointCloud::Ptr target (new pcl::PointCloud); 50 | pcl::PointCloud::Ptr scene (new pcl::PointCloud); 51 | pcl::PointCloud::Ptr target_ground (new pcl::PointCloud); 52 | pcl::PointCloud::Ptr scene_ground (new pcl::PointCloud); 53 | #endif 54 | 55 | geometry_msgs::PoseStamped poseOut; 56 | 57 | CCICP icp(SCAN_TO_MAP); 58 | 59 | ////////////////////// 60 | //Callbacks 61 | // 62 | void pose_cb(const geometry_msgs::PoseStamped& input) 63 | { 64 | poseOut = input; 65 | } 66 | 67 | bool obs_flag = false; 68 | bool gnd_flag = false; 69 | 70 | bool first_gnd = false; 71 | bool first_obs = false; 72 | 73 | void target_obs_cb(const sensor_msgs::PointCloud2ConstPtr& target_input) 74 | { 75 | 76 | //ROS_INFO_STREAM("Got obs Cloud"); 77 | //This is in the global frame 78 | pcl::fromROSMsg (*target_input, *target_obs_cloud); 79 | obs_flag = true; 80 | 81 | if(obs_flag && gnd_flag) { 82 | icp.setTargetCloud(target_obs_cloud, poseOut); 83 | icp.setTargetGndCloud(target_gnd_cloud); 84 | obs_flag = false; 85 | gnd_flag = false; 86 | } 87 | 88 | first_obs = true; 89 | } 90 | 91 | void target_gnd_cb(const sensor_msgs::PointCloud2ConstPtr& target_input) 92 | { 93 | //This is in the global frame 94 | pcl::fromROSMsg (*target_input, *target_gnd_cloud); 95 | gnd_flag = true; 96 | if(obs_flag && gnd_flag) { 97 | icp.setTargetCloud(target_obs_cloud, poseOut); 98 | icp.setTargetGndCloud(target_gnd_cloud); 99 | obs_flag = false; 100 | gnd_flag = false; 101 | } 102 | 103 | first_gnd = true; 104 | } 105 | 106 | //////////////////////////////////// 107 | //MAIN CALLBACK 108 | // most work happens in this callback 109 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 110 | { 111 | 112 | //wait for first ground and obs targets 113 | 114 | if(!(first_gnd && first_obs)) 115 | return; 116 | 117 | input_cloud->clear(); 118 | pcl::fromROSMsg (*input, *input_cloud); 119 | //note: this is in the local frame 120 | 121 | if(target_obs_cloud->empty()) return; 122 | if(input_cloud->size() < 20000) { 123 | ROS_WARN_STREAM("Input Cloud is to small!! Size: " << input_cloud->size()); 124 | return; 125 | } 126 | 127 | //Compensate for roll and pitch 128 | Eigen::Affine3d trans; 129 | pcl::PointCloud::Ptr temp(new pcl::PointCloud); 130 | geometry_msgs::Pose cpose; 131 | double roll, pitch, yaw; 132 | tf::Quaternion q; 133 | tf::quaternionMsgToTF(poseOut.pose.orientation, q); 134 | tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll,1); //get angles 135 | cpose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,0); //remove yaw 136 | cpose.position.z = poseOut.pose.position.z; 137 | tf::poseMsgToEigen(cpose, trans); //generate transform 138 | pcl::transformPointCloud(*input_cloud,*temp, trans); //transform cloud 139 | icp.setSceneCloud(temp); 140 | 141 | #if DEBUG 142 | icp.getSegmentedClouds(*target, *scene,*target_ground,*scene_ground); 143 | sensor_msgs::PointCloud2 cloud_msg; 144 | pcl::toROSMsg(*scene,cloud_msg); 145 | cloud_msg.header.frame_id = "/local"; 146 | cloud_msg.header.stamp = ros::Time::now(); 147 | scenePub.publish(cloud_msg); 148 | #endif 149 | 150 | 151 | /////////////////////////////////////////// 152 | //MAIN FUNCTIONS 153 | /////////////////////////////////////////// 154 | 155 | //Perform scan registration! 156 | // output should be in the glbal frame given initial position poseOut 157 | // and traget_cloud are in global frame 158 | geometry_msgs::PoseStamped result; 159 | result = icp.doICPMatch(poseOut); 160 | 161 | if(result.pose.orientation.w == 9999) { 162 | //An error occured 163 | ROS_ERROR_STREAM("ICP could not complete registration, skipping this scan"); 164 | return; 165 | } 166 | 167 | poseOut = result; 168 | 169 | //publish pose 170 | poseOut.header.stamp = input->header.stamp; 171 | poseOut.header.frame_id = "/global"; 172 | posePub.publish(poseOut); 173 | } 174 | 175 | int main (int argc, char** argv) 176 | { 177 | // Initialize ROS 178 | ros::init (argc, argv, "scan_registration"); 179 | ros::NodeHandle nh; 180 | 181 | ros::Subscriber poseSub = nh.subscribe ("/mapping/ekf/pose", 1, pose_cb); 182 | 183 | // Create a ROS subscriber for the input point cloud 184 | ros::Subscriber subScene = nh.subscribe ("/velodyne_points", 1, cloud_cb); 185 | ros::Subscriber subObsTarget = nh.subscribe ("/mapping/global/obstacle_pointcloud", 1, target_obs_cb); 186 | ros::Subscriber subGndTarget = nh.subscribe ("/mapping/global/ground_pointcloud", 1, target_gnd_cb); 187 | 188 | //ros publisher for pose 189 | posePub = nh.advertise("mapping/scan_reg/pose", 1); 190 | 191 | //debugging topics 192 | targetPub = nh.advertise("mapping/scan_reg/target", 1); 193 | scenePub = nh.advertise("mapping/scan_reg/scene", 1); 194 | 195 | //init Pose 196 | poseOut.pose.orientation.w = 1; //normalized quaternion 197 | 198 | // Spin 199 | ros::spin (); 200 | 201 | } 202 | 203 | 204 | -------------------------------------------------------------------------------- /scan_registration/src/scan_registration.h: -------------------------------------------------------------------------------- 1 | // 2 | // scan_registration.h 3 | // 4 | // Definitions for the scan registration node 5 | // 6 | 7 | #ifndef GLOBAL_MAPPING_NODE_H 8 | #define GLOBAL_MAPPING_NODE_H 9 | 10 | //moving average smoothing 11 | //#define USE_MV_AVG 12 | #define AVG_FILT_NUM 3 13 | 14 | 15 | #endif 16 | --------------------------------------------------------------------------------